工业直线运动机构的快速精确预测定位方法及定位系统

技术领域

本发明涉及工业直线运动机构的精确定位技术。

背景技术

在如高精度运动平台等工业设备运动控制中，运动机构能够精确、快速的运动到指定 坐标位置是十分重要的。然而现有的电机驱动器都是根据接收到的位置脉冲数决定自己此 次要到达的目标位置，再根据反馈的位置脉冲来决定自己停止的位置。在一些情况下（如 在台达电机驱动器使用龙门控制方式时），由于位移传感器输出并不能作为反馈传给驱动 器（这就需要通过一些比例运算来确定要发给驱动器的位置脉冲数，由于电机编码器刻度 和位置传感器刻度各自的不均匀性，这种计算必然导致较大的舍入误差），而我们期望运 动的目标位置总是以位移传感器为参考的，也就是说，发送给电机驱动器的位置脉冲就是 此次整个运动过程中期望位移传感器输出脉冲数，但电机驱动器的参考不再是主控制器参 考的位移传感器，这样就导致期望的位移和实际位移总会出现偏差，一般来说，位移传感 器的精度为5微米时，系统定位偏差会达到50微米左右。

要想让系统最终到达期望位置，可以在以上过程结束后，计算当前位置和目标位置的 距离，继续让电机朝着期望位置转动，如此往复，直到设备运动机构到达期望坐标位置。 这样设备的精度是达到的，但是该过程是相当耗时的，因为每次位置脉冲发送结束到电机 真正停下来都需要一段时间（指令结束，驱动器对电机有一个调整过程），这将是一笔巨 大的时间开销。

发明内容

本发明的目的是为了解决高精度运动平台等工业设备运动控制系统不同能时保证快 速定位和精确定位的问题，提供一种耗时短、精度高的工业直线运动机构的快速精确预测 定位方法及定位模块。

本发明所述的工业直线运动机构的快速精确预测定位方法，该方法是基于工业直线运 动机构的快速精确预测定位系统实现的，所述工业直线运动机构的快速精确预测定位系统 包括DSP、位置脉冲减计数器、电机驱动器、位移计数器和位移传感器，所述DSP向位置 脉冲减计数器发送脉冲信号，所述位置脉冲减计数器发送驱动信号给电机驱动器以控制电 机运动，同时，DSP从位置脉冲减计数器接收位置脉冲减计数器状态信号，即剩余脉冲数 量信号，DSP通过位移计数器接收位移传感器发送的当前位置脉冲信号；

所述的快速精确预测定位方法是由嵌入在DSP内部的软件实现的，该方法包括以下步 骤：

目标位置指令接收步骤：用于接收目标位置指令脉冲数N，并在该步骤结束之后执行 目标脉冲计算步骤；

目标脉冲计算步骤：用于计算需要输出的脉冲数，所述的脉冲数等于目标位置指令脉 冲数N；同时对到标志位RchPosCnt进行清零,即RchPosCnt＝0；并在该步骤结束之后执 行计划脉冲写入步骤；

计划脉冲写入步骤：将脉冲数N的绝对值|N|写入位置脉冲减计数器；并在该步骤结束 之后执行脉冲发送步骤；

脉冲发送步骤：用于通过位置脉冲发送端口发送1个位置脉冲信号给位置脉冲减计数 器；并在该步骤结之后执行剩余脉冲信号接收步骤；

剩余脉冲信号接收步骤：用于接收位置脉冲减计数器发来的剩余脉冲数量信号；并在 该步骤结束之后执行第一判断步骤；

第一判断步骤；用于判断剩余脉冲数量信号是否为0，并在判断结果为是时执行当前 位置接受步骤；在判断结果为否时返回执行脉冲发送步骤；

当前位置接受步骤：用于接收位移计数器发来的当前位置脉冲信号M；并在该步骤结 束之后执行第二判断步骤；

第二判断步骤：用于判断|N-M|是否等于2，并在判断结果为为是时执行到位标志递 增步骤；在判断结果为否时执行校正脉冲数计算步骤；

到位标志递增步骤：用于使到位标志增加1，即RchPosCnt＝RchPosCnt+1；并在该步 骤结束之后执行第三判断步骤；

第三判断步骤：判断到位标志是否为p，p为大于10的整数，并在判断结果为为是时 结束定位；在判断结果为否时返回执行第二判断步骤；

校正脉冲数计算步骤：用于计算N的值，N=(ULONG)(θ(N-M))，其中，θ为0.5到 1之间的数，同时对到标志位RchPosCnt进行清零；并在该步骤结束之后执行计划脉冲写 入步骤。

本发明所述的工业直线运动机构的快速精确预测定位系统，其特征在于：它包括DSP、 位置脉冲减计数器、电机驱动器、位移计数器和位移传感器，所述DSP向位置脉冲减计数 器发送脉冲信号，所述位置脉冲减计数器发送驱动信号给电机驱动器以控制电机运动，同 时，DSP从位置脉冲减计数器接收位置脉冲减计数器状态信号，即剩余脉冲数量信号，DSP 通过位移计数器接收位移传感器发送的当前位置脉冲信号；

在DSP内部嵌入有由软件实现的定位模块，所述定位模块包括以下单元：

目标位置指令接收单元：用于接收目标位置指令脉冲数N；

目标脉冲计算单元：用于计算需要输出的脉冲数，所述的脉冲数等于目标位置指令脉 冲数N；同时对到标志位RchPosCnt进行清零,即RchPosCnt＝0；

计划脉冲写入单元：将脉冲数N的绝对值|N|写入位置脉冲减计数器；

脉冲发送单元：用于通过位置脉冲发送端口发送1个位置脉冲信号给位置脉冲减计数 器；

剩余脉冲信号接收单元：用于接收位置脉冲减计数器发来的剩余脉冲数量信号；

第一判断单元；用于判断剩余脉冲数量信号是否为0，并在判断结果为是时启动当前 位置接受单元；在判断结果为否时启动脉冲发送单元；

当前位置接受单元：用于接收位移计数器发来的当前位置脉冲信号M；

第二判断单元：用于判断|N-M|是否等于2，并在判断结果为是时启动到位标志递增 单元；在判断结果为否时启动校正脉冲数计算单元；

到位标志递增单元：用于使到位标志增加1，即RchPosCnt＝RchPosCnt+1；

第三判断单元：判断到位标志是否为p，p为大于10的整数，并在判断结果为是时结 束定位；在判断结果为否时启动第二判断单元；

校正脉冲数计算单元：用于计算N的值，N=(ULONG)(θ(N-M))，其中，θ为0.5到1 之间的数，同时对到标志位RchPosCnt进行清零。

本发明所述的工业直线运动机构的快速精确预测定位方法在定位过程中，当预设脉冲 数发送完成且设备还未停止运行时，预测设备运动机构有没有可能最终停在目标位置，若 有，就等待其停止再做进一步判断。若没有可能，就判断运动机构是根本就到不了目标位 置还是会冲过目标位置，如果是到不了，就立即向位置脉冲减计数器发送若干个脉冲，让 电机继续前行；如果是会过冲，就向位置脉冲减计数器发送若干个反方向脉冲，使电机反 向行进，直到电机到达目标位置为止。位移传感器的精度为5微米时，这种方法能够保证 定位精度在5微米以内，且在定位过程中加入了预测过程，根据预测结果决定电机的下一 步运动方式，与传统的高精度定位方式相比，定位时间缩短了20%以上。

附图说明

图1为实施方式一所述的工业直线运动机构的快速精确预测定位方法的流程图；

图2为实施方式六所述的工业直线运动机构的快速精确预测定位系统的结构框图。

具体实施方式

具体实施方式一：结合图1说明本实施方式，本实施方式所述的工业直线运动机构 的快速精确预测定位方法，该方法是基于工业直线运动机构的快速精确预测定位系统实现 的，所述工业直线运动机构的快速精确预测定位系统包括DSP1、位置脉冲减计数器2、电 机驱动器3、位移计数器4和位移传感器5，所述DSP1向位置脉冲减计数器2发送脉冲信 号，所述位置脉冲减计数器2发送驱动信号给电机驱动器3以控制电机运动，同时，DSP1 从位置脉冲减计数器2接收位置脉冲减计数器状态信号，即剩余脉冲数量信号，DSP1通 过位移计数器4接收位移传感器5发送的当前位置脉冲信号；

所述的快速精确预测定位方法是由嵌入在DSP1内部的软件实现的，该方法包括以下 步骤：

目标位置指令接收步骤：用于接收目标位置指令脉冲数N，并在该步骤结束之后执行 目标脉冲计算步骤；

目标脉冲计算步骤：用于计算需要输出的脉冲数，所述的脉冲数等于目标位置指令脉 冲数N；同时对到标志位RchPosCnt进行清零,即RchPosCnt＝0；并在该步骤结束之后执 行计划脉冲写入步骤；

计划脉冲写入步骤：将脉冲数N的绝对值|N|写入位置脉冲减计数器2；并在该步骤结 束之后执行脉冲发送步骤；

脉冲发送步骤：用于通过位置脉冲发送端口发送1个位置脉冲信号给位置脉冲减计数 器2；并在该步骤结之后执行剩余脉冲信号接收步骤；

剩余脉冲信号接收步骤：用于接收位置脉冲减计数器2发来的剩余脉冲数量信号；并 在该步骤结束之后执行第一判断步骤；

第一判断步骤；用于判断剩余脉冲数量信号是否为0，并在判断结果为是时执行当前 位置接受步骤；在判断结果为否时返回执行脉冲发送步骤；

当前位置接受步骤：用于接收位移计数器4发来的当前位置脉冲信号M；并在该步骤 结束之后执行第二判断步骤；

第二判断步骤：用于判断|N-M|是否等于2，并在判断结果为为是时执行到位标志递 增步骤；在判断结果为否时执行校正脉冲数计算步骤；

到位标志递增步骤：用于使到位标志增加1，即RchPosCnt＝RchPosCnt+1；并在该步 骤结束之后执行第三判断步骤；

第三判断步骤：判断到位标志是否为p，p为大于10的整数，并在判断结果为为是时 结束定位；在判断结果为否时返回执行第二判断步骤；

校正脉冲数计算步骤：用于计算N的值，N=(ULONG)(θ(N-M))，其中，θ为0.5到1 之间的数，同时对到标志位RchPosCnt进行清零；并在该步骤结束之后执行计划脉冲写入 步骤。

本实施方式所述的工业直线运动机构的快速精确预测定位方法是由嵌入在DSP1内部 的软件来实现的，该方法中，所有的位置和距离均以脉冲数来表示。系统运行前先初始化， 初始化后设备运动机构回到原点，位移计数器4发送给DSP1的当前位置脉冲信号为0。 DSP1首先接收目标位置指令脉冲数N，根据目标位置指令脉冲数N计算出需要发送给电机 驱动器3的脉冲数，该脉冲数等于N-0，即：需要发送给电机驱动器3的脉冲数等于目标 脉冲数与当前位置脉冲信号的差值，计算结果仍计为N。同时对到标志位RchPosCnt进行 清零，然后将所述脉冲数N写入位置脉冲减计数器2。打开位置脉冲发送端口，向位置脉 冲减计数器2发送一个脉冲信号，位置脉冲减计数器2将此脉冲作为驱动信号发送给电机 驱动器3，电机驱动器3根据该驱动信号控制电机转动，根据N的符号（正或负）来设定 电机的转动方向。DSP1每发送一个脉冲信号给位置脉冲减计数器2之后，会从位置脉冲 减计数器2接收到一个反应位置脉冲减计数器2状态的信号，该信号为位置脉冲减计数器 2的剩余脉冲数，DSP1判断该剩余脉冲数是否为0，即位置脉冲减计数器2的状态是否为 空，如果位置脉冲减计数器2的状态不为空，则DSP1继续向位置脉冲减计数器2发送脉 冲信号，如此循环，直到位置脉冲减计数器2的状态为空时，停止发送脉冲信号。此时， DSP1接收位移计数器4发来的当前位置脉冲信号M，然后通过目标位置与当前位置的距离 预测设备运动机构有无可能停在目标位置，如果目标位置与当前位置的距离大于2个脉 冲，则认为电机停止转动后，运动机构不可能停在目标位置处，，此时需要采取校正措施， 避免时间浪费。校正措施为先计算再次需要输出的脉冲数，然后将此脉冲数发送给位置脉 冲减计数器2，再次需要输出的脉冲数＝(ULONG)(θ(N-M))，此脉冲数作为新的N值，取 代位置脉冲减计数器2先前接收到的脉冲数N，即对N的数值大小进行更新,此处的系数 θ为大于0.5且小于1的数，将目标位置与当前位置的距离乘上该系数，用向下圆整的方 式方法取整后，N总是大于等于1，也就是说设备定位精度为正负一个位移传感器脉冲。 N-M的符号代表预测最终电机是到不了位还是会冲过目标位置,N-M的符号将决定电机的 转动方向。如果目标位置与当前位置的距离小于2个脉冲，则使到标志位RchPosCnt增加 1，即RchPosCnt＝RchPosCnt+1，然后判断RchPosCnt是否等于p，所述p为大于10的整 数，如果RchPosCnt不等于p,则继续判断目标位置与当前位置的距离是否小于2个脉冲， 如此循环，直到RchPosCnt等于p为止，停止定位。在RchPosCnt从0增加到p的这段时 间内，如果目标位置与当前位置的距离始终保持小于2个脉冲，则认为设备运动机构已经 到达目标位置，可以结束定位。

本实施方式所述的工业直线运动机构的快速精确预测定位方法在定位过程中，当预设 脉冲数发送完成且设备还未停止运行时，预测设备运动机构有没有可能最终停在目标位 置，若有，就等待其停止再做进一步判断。若没有可能，就判断运动机构是根本就到不了 目标位置还是会冲过目标位置，如果是到不了，就立即向位置脉冲减计数器2发送若干个 脉冲，让电机继续前行；如果是会过冲，就向位置脉冲减计数器2发送若干个反方向脉冲， 使电机反向行进，直到电机到达目标位置为止。这种方法能够保证定位精度在5微米以内， 且在定位过程中加入了预测过程，根据预测结果决定电机的下一步运动方式，与传统的高 精度定位方式相比，定位时间缩短了20%以上。

具体实施方式二：本实施方式是对实施方式一所述的工业直线运动机构的快速精确预 测定位方法的进一步限定，本实施方式中，所述的第三判断步骤中，p=20。

具体实施方式三：本实施方式是对实施方式一所述的工业直线运动机构的快速精确预 测定位方法的进一步限定，本实施方式中，所述的位移传感器5采用磁栅尺或光栅尺实现。

具体实施方式四：本实施方式是对实施方式一所述的工业直线运动机构的快速精确预 测定位方法的进一步限定，本实施方式中，所述的位置脉冲减计数器2每接到一个DSP1 发来的脉冲信号后，向电机驱动器3发送一个脉冲信号，然后再反馈给DSP1一个状态信 号，该状态信号表示剩余脉冲数量。

本实施方式中的位置脉冲减计数器2是由嵌入在CPLD中的软件来实现的。位置脉冲 减计数器2的功能为每接到一个DSP1发来的脉冲信号后，向电机驱动器3发送一个脉冲 信号，然后再反馈给DSP1一个状态信号，该状态信号表示剩余脉冲数量。剩余脉冲数量 的初始值为DSP1向其写入的|N|，位置脉冲减计数器2每向电机驱动器3发送完一个脉冲 信号后，剩余脉冲数量减1，当发送完|N|个脉冲给电机驱动器3后，剩余脉冲数量为0， 此时，位置脉冲减计数器2的状态为空。

具体实施方式五：本实施方式是对实施方式一所述的工业直线运动机构的快速精确预 测定位方法的进一步限定，本实施方式中，所述的位移计数器4用于对位移传感器5输出 的脉冲进行计数，其计数值用于确定设备运动机构在系统坐标系中的位置，即当前位置。

本实施方式中的所有的距离和位置均通过脉冲数来表示，位移传感器5输出的脉冲数 反应了设备运动机构的当前位置，位移计数器4对位移传感器5输出的脉冲进行计数，并 将其发送给DSP1，以使DSP1获得设备运动机构的当前位置。

具体实施方式六：结合图2说明本实施方式，本实施方式所述的工业直线运动机构 的快速精确预测定位系统包括DSP1、位置脉冲减计数器2、电机驱动器3、位移计数器4 和位移传感器5，所述DSP1向位置脉冲减计数器2发送脉冲信号，所述位置脉冲减计数 器2发送驱动信号给电机驱动器3以控制电机运动，同时，DSP1从位置脉冲减计数器2 接收位置脉冲减计数器状态信号，即剩余脉冲数量信号，DSP1通过位移计数器4接收位 移传感器5发送的当前位置脉冲信号；

在DSP1内部嵌入有由软件实现的定位模块，所述定位模块包括以下单元：

目标位置指令接收单元：用于接收目标位置指令脉冲数N；

目标脉冲计算单元：用于计算需要输出的脉冲数，所述的脉冲数等于目标位置指令脉 冲数N；同时对到标志位RchPosCnt进行清零,即RchPosCnt＝0；

计划脉冲写入单元：将脉冲数N的绝对值|N|写入位置脉冲减计数器2；

脉冲发送单元：用于通过位置脉冲发送端口发送1个位置脉冲信号给位置脉冲减计数 器2；

剩余脉冲信号接收单元：用于接收位置脉冲减计数器2发来的剩余脉冲数量信号；

第一判断单元；用于判断剩余脉冲数量信号是否为0，并在判断结果为是时启动当前 位置接受单元；在判断结果为否时启动脉冲发送单元；

当前位置接受单元：用于接收位移计数器4发来的当前位置脉冲信号M；

第二判断单元：用于判断|N-M|是否等于2，并在判断结果为是时启动到位标志递增 单元；在判断结果为否时启动校正脉冲数计算单元；

到位标志递增单元：用于使到位标志增加1，即RchPosCnt＝RchPosCnt+1；

第三判断单元：判断到位标志是否为p，p为大于10的整数，并在判断结果为是时结 束定位；在判断结果为否时启动第二判断单元；

校正脉冲数计算单元：用于计算N的值，N=(ULONG)(θ(N-M))，其中，θ为0.5到1 之间的数，同时对到标志位RchPosCnt进行清零。

本实施方式中，所述的工业直线运动机构的快速精确预测定位模块是由嵌入在DSP1 内部的软件来实现的，该模块中，所有的位置和距离均以脉冲数来表示。系统运行前先初 始化，初始化后设备运动机构回到原点，位移计数器4发送给DSP1的当前位置脉冲信号 为0。DSP1首先接收目标位置指令脉冲数N，根据目标位置指令脉冲数N计算出需要发送 给电机驱动器3的脉冲数，该脉冲数等于N-0，即：需要发送给电机驱动器3的脉冲数等 于目标脉冲数与当前位置脉冲信号的差值，计算结果仍计为N。同时对到标志位RchPosCnt 进行清零，然后将所述脉冲数N写入位置脉冲减计数器2。打开位置脉冲发送端口，向位 置脉冲减计数器2发送一个脉冲信号，位置脉冲减计数器2将此脉冲作为驱动信号发送给 电机驱动器3，电机驱动器3根据该驱动信号控制电机转动，根据N的符号（正或负）来 设定电机的转动方向。DSP1每发送一个脉冲信号给位置脉冲减计数器2之后，会从位置 脉冲减计数器2接收到一个反应位置脉冲减计数器2状态的信号，该信号为位置脉冲减计 数器2的剩余脉冲数，DSP1判断该剩余脉冲数是否为0，即位置脉冲减计数器2的状态是 否为空，如果位置脉冲减计数器2的状态不为空，则DSP1继续向位置脉冲减计数器2发 送脉冲信号，如此循环，直到位置脉冲减计数器2的状态为空时，停止发送脉冲信号。此 时，DSP1接收位移计数器4发来的当前位置脉冲信号M，然后通过目标位置与当前位置的 距离预测设备运动机构有无可能停在目标位置，如果目标位置与当前位置的距离大于2 个脉冲，则认为电机停止转动后，运动机构不可能停在目标位置处，，此时需要采取校正 措施，避免时间浪费。校正措施为先计算再次需要输出的脉冲数，然后将此脉冲数发送给 位置脉冲减计数器2，再次需要输出的脉冲数＝(ULONG)(θ(N-M))，此脉冲数作为新的N 值，取代位置脉冲减计数器2先前接收到的脉冲数N，即对N的数值大小进行更新,此处 的系数θ为大于0.5且小于1的数，将目标位置与当前位置的距离乘上该系数，用向下 圆整的方式方法取整后，N总是大于等于1，也就是说设备定位精度为正负一个位移传感 器脉冲。N-M的符号代表预测最终电机是到不了位还是会冲过目标位置,N-M的符号将决 定电机的转动方向。如果目标位置与当前位置的距离小于2个脉冲，则使到标志位 RchPosCnt增加1，即RchPosCnt＝RchPosCnt+1，然后判断RchPosCnt是否等于p，所述 p为大于10的整数，如果RchPosCnt不等于p,则继续判断目标位置与当前位置的距离是 否小于2个脉冲，如此循环，直到RchPosCnt等于p为止，停止定位。在RchPosCnt从0 增加到p的这段时间内，如果目标位置与当前位置的距离始终保持小于2个脉冲，则认为 设备运动机构已经到达目标位置，可以结束定位。

本实施方式所述的工业直线运动机构的快速精确预测定位系统在定位过程中，当预设 脉冲数发送完成且设备还未停止运行时，预测设备运动机构有没有可能最终停在目标位 置，若有，就等待其停止再做进一步判断。若没有可能，就判断运动机构是根本就到不了 目标位置还是会冲过目标位置，如果是到不了，就立即向位置脉冲减计数器2发送若干个 脉冲，让电机继续前行；如果是会过冲，就向位置脉冲减计数器2发送若干个反方向脉冲， 使电机反向行进，直到电机到达目标位置为止。所述快速精确预测定位系统能够保证定位 精度在5微米以内，且在定位过程中加入了预测过程，根据预测结果决定电机的下一步运 动方式，与传统的高精度定位方式相比，定位时间缩短了20%以上。

具体实施方式七：本实施方式是对实施方式六所述的工业直线运动机构的快速精确 预测定位系统的进一步限定，本实施方式中，所述的第三判断单元中,p=20。

具体实施方式八：本实施方式是对实施方式六所述的工业直线运动机构的快速精确预 测定位系统的进一步限定，本实施方式中，所述的位移传感器5采用磁栅尺或光栅尺实现。

具体实施方式九：本实施方式是对实施方式六所述的工业直线运动机构的快速精确预 测定位系统的进一步限定，本实施方式中，所述的位置脉冲减计数器2每接到一个DSP1 发来的脉冲信号后，向电机驱动器3发送一个脉冲信号，然后再反馈给DSP1一个状态信 号，该状态信号表示剩余脉冲数量。

本实施方式中的位置脉冲减计数器2是由嵌入在CPLD中的软件来实现的。位置脉冲 减计数器2的功能为每接到一个DSP1发来的脉冲信号后，向电机驱动器3发送一个脉冲 信号，然后再反馈给DSP1一个状态信号，该状态信号表示剩余脉冲数量。剩余脉冲数量 的初始值为DSP1向其写入的|N|，位置脉冲减计数器2每向电机驱动器3发送完一个脉冲 信号后，剩余脉冲数量减1，当发送完|N|个脉冲给电机驱动器3后，剩余脉冲数量为0， 此时，位置脉冲减计数器2的状态为空。

具体实施方式十：本实施方式是对实施方式六所述的工业直线运动机构的快速精确预 测定位系统的进一步限定，本实施方式中，所述的位移计数器4用于对位移传感器5输出 的脉冲进行计数，其计数值用于确定设备运动机构在系统坐标系中的位置，即当前位置。

本实施方式中的所有的距离和位置均通过脉冲数来表示，位移传感器5输出的脉冲数 反应了设备运动机构的当前位置，位移计数器4对位移传感器5输出的脉冲进行计数，并 将其发送给DSP1，以使DSP1获得设备运动机构的当前位置。