移动体系统和移动体的到位判定方法.docx

移动体系统和移动体的到位判定方法专利名称：移动体系统和移动体的到位判定方法技术领域：本发明涉及一种移动体系统，特别涉及移动体是否能够停止在容许范围内(到 位)的判定背景技术：对于多轴的移动体，如下情况较多，S卩，以第1轴的位置进入规定范围内(到位范 围内)为条件，使第2轴动作例如在桥式吊车中，当行走方向位置进入规定范围内时，升 降台开始升降或者横向进给在堆垛机和无人搬送车中，当行走方向位置(以及堆垛机中 的升降方向位置)进入规定范围内时，使滑叉等移载装置动作另外在机床等中，当第1轴 的χ方向位置或者xy面内位置进入规定范围内时，沿着第2轴的ζ方向使工具移动，并开 始加工对第1轴和第2轴的顺序动作应用到位判定，当第1轴的位置进入到位范围内时， 开始进行第2轴的动作例如专利文献1(JP2000-231412A)公开如下技术，S卩，当在xy面 内的移动后接着进行ζ轴方向的移动时，进行关于xy面内的合成移动方向的到位判定，对 于二维移动进行一维中的到位判定但是，当只使用当前位置是否在到位范围内来判定时，判定为到位后，移动体可能 由于过冲而从到位范围脱离通过图6、图7说明此情况图6表示移动体不振动而停止的 状况，图7表示移动体由于振动而产生过冲的状况。

图6、图7中的a)表示位置的轨迹，b) 表示速度和位置在相位面上的轨迹，c)表示粗略和精确这2个阶段的到位判定结果在图 6中，移动体不振动地朝向目的位置减速，不发生过冲与之相对，在图7中，位置和速度振 动，相位面中的轨迹为螺旋状，有必要在中途取消暂时成立的到位判定发明内容本发明的课题在于迅速且正确地判定有无由于过冲而从到位范围脱离的可能性本发明是当移动体进入了到位范围内时进行到位判定的系统，其特征在于，设置 有传感器和计算机构，上述传感器用于求得移动体的位置、速度和加速度，上述计算机构用 于基于所求得的位置、速度和加速度而推定移动体的停止位置是否在到位范围内在本发明中，根据当前位置在到位范围内、以及推定停止位置也在到位范围内这 两者进行判定因此，如果移动体有由于过冲等从到位范围脱离的可能性，则不进行到位判 定，可以进行具有可靠性的判定另外，在本发明中，根据移动体的实际位置、速度和加速度 来判定，不需要使用移动体的模型因此，没有基于移动体的模型化的误差上述计算机构为，根据移动体的位置的时间序列数据{Pi}求得速度的时间序列 数据{vi}，并且根据求得的速度的时间序列数据求得加速度的时间序列数据Iai)，在这 里，i为表示时间序列的附注，i表示当前，并且从当前位置Pi到停止位置的距离实质上 以-vi7ai求得。

在这里，“实质上”的意思是，-Vi2Aii乘以0.8 1.2左右的常数亦可， 或者加减到位范围的1/10 1/100左右的补偿亦可实质上，由于-vi7ai的距离相当于到停止位置的距离的上限，所以，如果从当前位置实质上只前进-Vi2Aii的位置在到位范围 内，则可以判定移动体由于过冲等从到位范围脱离的可能性实质上是没有的而且此判定 可以通过简单的计算而迅速进行优选地，上述传感器作为求得移动体的位置的线性传感器，在短周期内正确测定 第1轴方向的位置优选地，当通过上述传感器求得的当前位置和通过上述计算机构推定的停止位置 都在到位范围内时，判定为到位本发明的移动体的到位判定方法是当移动体进入了到位范围内时判定到位的方 法，其特征在于，设置有求得步骤，通过传感器求得移动体的位置、速度和加速度；推定步骤，用于通过计算机构基于所求得的位置、速度和加速度来推定移动体的 停止位置；以及，判定步骤，用于通过判定机构判定所推定的停止位置是否在到位范围内本说明书中，关于移动体系统的记载直接完全适合于移动体的到位判定方法，反 过来，关于移动体的到位判定方法的记载直接完全适合于移动体系统优选地，在上述判定步骤中，当通过上述传感器求得的当前位置和通过上述计算 机构推定的停止位置都在到位范围内时，判定为到位。

图1是实施例的移动体系统的主要部分的框图图2是实施例中的到位判定部的框图图3是表示实施例中的到位判定算法的流程图图4是表示实施例中的停止位置的推定算法的流程图图5是表示实施例中的、应用了相位面的停止位置的推定的附图图6表示现有例中的到位判定，a)表示移动体不振动地接近目的位置时的轨迹，b)表示移动体的相位面上的轨迹，c)表示粗略到位的判定信号和精确到位的判定信号图7表示现有例中的到位判定，a)表示移动体一边过冲一边接近目的位置时的轨迹，b)表示移动体的相位面上的轨迹，c)表示粗略到位的判定信号和精确到位的判定信号附图标记说明2移动体系统4、10 控制器6、12伺服放大器8、14线性传感器16到位判定部20位置数据存储部21速度数据存储部22加速度数据存储部23计算部24停止推定位置存储部M1、M2 马达具体实施例方式下面示出实施本发明的最佳实施例本发明的范围应该基于所要求保护的范围的 记载，参考说明书的记载和本领域的公知技术，根据本领域技术人员的理解来确定在图1 图5中，表示实施例的移动体系统2在各图中，4为第1轴控制器，10为 第2轴控制器，经由各个伺服放大器6、12驱动马达M1、M2。

线性传感器8、14分别求得第1 轴方向的移动体的位置和第2轴方向的移动体的位置，并输入到控制器4、10到位判定部16以第1轴方向的根据线性传感器8得到的位置的时间序列数据为 基础，生成速度的时间序列数据，并根据速度的时间序列数据生成加速度的时间序列数据 而且通过当前位置和当前速度、以及当前加速度来推定停止位置，并判定其是否在到位范 围内在当前位置在到位范围内并且推定停止位置也在到位范围内时，到位判定部16判定 为到位，基于此，第2轴控制器10启动马达M2控制器4 到位判定部16设置在移动体上，例如在马达Ml、M2为地面1次的线性 马达并且在移动体上设置马达Ml、M2的2次侧的情况下，在地面侧设置控制器4 到位判 定部16线性传感器8、14可以设置在移动体上，亦可设置在地面侧，线性传感器8、14例如 由多个线圈构成，根据线圈的电感变化检测出与被检测用的磁标记相对应的位置在图2中，表示到位判定部16的结构位置数据存储部20存储根据线性传感器 8得到的位置的时间序列数据{Pi}，计算部23根据位置的时间序列数据的差分生成速度的 时间序列数据Ivi}，速度数据存储部21存储时间序列数据{vi}。

计算部23根据速度的时 间序列数据生成加速度的时间序列数据{ai}，并由加速度数据存储部22存储计算部23利 用当前位置Pi和当前速度vi以及当前加速度ai，以Pi-vi7ai来求得停止位置在这里， 负号对应于减速时加速度为负的情况推定停止位置不需要严格地为Pi_vi2/ai，实质上为 Pi-vi2/ai即可例如在vi2/ai的项中，乘以0. 8 1. 2左右的系数亦可，或者在Pi_vi2/ ai的项中加减到位范围的幅度的1/10 1/100左右的补偿亦可计算部23存储停止推定 位置当当前位置和停止推定位置都在到位范围内时，计算部23判定为到位在图3 图5中表示到位判定的方法在这里，由粗略到位和精确到位这2个阶 段来判定到位，但是1个阶段亦可，或者3个阶段以上亦可当当前位置在粗略到位范围内 并且推定停止位置也在粗略到位范围内时，判定为粗略到位然后，当当前位置在精确到位 范围内并且推定停止位置也在精确到位范围内时，判定为精确到位表示停止位置的推定机制根据位置Pi的时间序列数据得到速度的时间序列数 据vi，并据此得到加速度的时间序列数据ai停止推定位置定为Pi_vi2/ai，如上述那样， vi2/ai的项乘以0. 8 1. 2的系数亦可，或者加减到位范围的1/10 1/100左右的补偿亦 可。

另外，虽然位置Pi的时间序列数据可以在短周期内正确地从线性传感器得到，但也可 以从测定周期长的激光距离传感器得到5在图5中表示Pi-Vi2Aii的项的意思图5表示位置Pi和速度vi的相位面，相 位面上的坐标按照Q0、QU Q2、Q3的顺序接近目标停止位置另外，在目标停止位置上，位 置和速度都为0在这里，在进入了粗略到位范围内的Ql时刻，例如求得连接其与紧之前的 点QO的轨迹的连线(图5的虚线)与位置轴的截距，来判定其是否在粗略到位范围内另 外，在当前位置进入了精确到位范围内的Q3时刻，求得连接紧之前的点Q2与点Q3的连线 与位置轴的截距，来判定其是否在精确到位范围内此处，从当前位置与紧之前的两个点产 生连线，但是，例如也可以以点Q3、Q2的中点作为当前位置，以点Q1、Q0的中点作为一个之 前的位置，从四个点产生连线对这样得到的截距的意义进行说明QO Q3等的点由线性传感器得到，而不是根 据移动体的控制上的模型得到另外，由于对移动体赋予加速度从而使之停止在目的位置， 所以，实际上如图5的白圆的排列那样，停止在比图5的接线更接近目的位置的一侧例如 在等加速度运动中使移动体减速的情况下，停止位置为Pi-vi72ai，-vi2/ai的项假定为从 当前位置仅以等加速度运动情况的2倍前进并停止。

这样，在图5中与位置轴的截距为在 最坏的情况下估算的移动体的停止位置在图5的评价中，根据现实的移动体的位置的时间序列数据推定停止位置，不包 含移动体的控制上的模型因此，不受对移动体模型化时的误差的影响因此，可以推定距 目的停止位置的偏差的上限另外，由于在进行图5的推定的时刻，移动体的位置在粗略到 位范围内，或者在精确到位范围内，并进行减速运动，因此不可能停止在到位范围的近前一 侧(到位范围之外、且靠近该到位范围的位置)由于这些原因，可以通过正确并且简单的 计算迅速地判定移动体是否停止在到位范围内当正确并且迅速地判定移动体是否停止在到位范围内时，不仅可以更正确地定 位，而且还能够更迅速地使接下来的第2轴的运动开始权利要求1.一种移动体系统，是在移动体进入了到位范围内时进行到位判定的系统，其特征在 于，具有传感器，用于求得移动体的位置、速度和加速度；以及，计算机构，用于基于所求得的位置、速度和加速度来推定移动体的停止位置是否在到 位范围内2.如权利要求1所述的移动体系统，其特征在于，上述计算机构根据移动体的位置的 时间序列数据{Pi}求得速度的时间序列数据Ivi}，并且，根据所求得的速度的时间序列数 据求得加速度的时间序列数据lai}，在这里i为表示时间序列的附注，并且i表示当前，从当前位置Pi到停止位置的距离实质上以-Vi2Aii求得。

3.如权利要求1所述的移动体系统，其特征在于，上述传感器为求得移动体的位置的 线性传感器4.如权利要求2所述的移动体系统，其特征在于，还设置有判定机构，在通过上述传感 器求得的当前位置和通过上述计算机构推定的停止位置都在到位范围内时，该判定机构判 定为到位5.一种移动体的到位判定方法，是在移动体进入了到位范围内时判定为到位的方法， 其特征在于，具有求得步骤，通过传感器求得移动体的位置、速度和加速度；推定步骤，用于通过计算机构基于所求得的位置、速度和加速度来推定移动体的停止 位置；以及，判定步骤，用于通过判定机构判定所推定的停止位置是否在到位范围内6.权利要求5所述的移动体的到位判定方法，其特征在于，在上述判定步骤中，当通过 上述传感器求得的当前位置和通过上述计算机构推定的停止位置都在到位范围内时，判定 为到位全文摘要本发明提供一种移动体系统和移动体的到位判定方法设置有传感器和计算机构，上述传感器用于求得移动体的位置、速度和加速度，上述计算机构基于所求得的位置、速度和加速度，判定移动体的停止位置是否在到位范围内。