低精度多个传感器融合的原点精确定位系统及定位方法

摘要

低精度多个传感器融合的原点精确定位系统及定位方法，涉及多个传感器融合的原点精确定位技术。它是为了解决现有原点定位精度设备中如采用高精度传感器存在成本高，装配难度大，而如果采用低精度传感器虽然能克服前述问题，但定位精度又不够的矛盾。本发明在采用了低精度传感器降低装配难度和设备成本的同时，保证了定位精度；采用了位移传感器和原点传感器配合实现定位，在达到了采用高精度传感器相同的定位精度的前提下，降低了对传感器安装精度的要求，进而降低了传感器的装配难度，装配难度同比降低了30%以上，设备成本降低了20%。本发明适用于多传感器融合的原点精确定位技术。

说明书

技术领域

本发明涉及多传感器融合的原点精确定位技术。

背景技术

随着工业生产中自动化设备的日益普及，对设备运动控制精度的要求也越来越高，而设 备原点定位精度对设备运动控制精度影响甚大。现行工业设备中，原点定位精度低往往是由 于所采用的传感器的精度低导致的，为了提高精度，往往采用依靠高精度的感应式传感器来 实现，如SONY公司生产的的PK15，其定位精度能够达到3um，但随之而来的是设备研发制造 成本的大幅增加，以及由于高装配精度带来的装配难度。

发明内容

本发明是为了解决现有原点定位精度设备中如采用高精度传感器存在成本高，装配难度 大，而如果采用低精度传感器虽然能克服前述问题，但定位精度又不够的矛盾，进而提供了 一种低精度多个传感器融合的原点精确定位系统及定位方法。

低精度多个传感器融合的原点精确定位系统，它包括工控机1、DSP2、位置脉冲减计数 器3、电机驱动器4、电机5、移动机构、位移传感器6、位移计数器7和原点传感器8；

工控机1的位置指令信号输出端连接DSP2的位置指令信号输入端，DSP2将要发送的脉 冲总数写入位置脉冲减计数器3，所述DSP2的脉冲信号输出端连接位置脉冲减计数器3的脉 冲信号输入端，所述位置脉冲减计数器3的状态信号输出端连接DSP2的状态信号输入端，位 置脉冲减计数器3用于控制发送给电机驱动器4的位置脉冲总数，位置脉冲减计数器3的位 置脉冲信号输出端连接电机驱动器4的位置脉冲信号输入端，所述电机驱动器4的驱动信号 输出端连接电机5的驱动信号输入端，所述电机5用于驱动移动机构移动，位移传感器6用 于检测移动机构的位移信息，位移传感器6输出表示位移信息的脉冲信号给位移计数器7的 脉冲信号输入端，原点传感器8用于检测移动机构是否位于原点范围的状态，原点传感器8 的检测信号输出端连接位移计数器7的原点检测信号输入端，所述位移计数器7的脉冲计数 输出端连接DSP2的位置脉冲信号输入端；

在DSP2内部的软件实现的定位模块，所述定位模块包括以下单元：

初始化单元：用于启动电机5，电机5的转动方向为设备坐标系中轴向的负方向；位移 计数器7清零，位置脉冲减计数器3的脉冲数为maxval，maxval表示该轴整个运动范围所对 应的位移传感器脉冲数；

判断单元一：用于判断移动机构是否到达原点传感器8的感应范围且接收到位移传感器 6的Z相信号，如果是则启动关闭脉冲指令单元，否则重新启动该单元；

关闭脉冲指令单元：用于读取系统每个脉冲周期上位移计数器7的值，读取的当前值与 上个周期的读取的值相同时，则DSP2中的到位标志计数器加1，否则到位标志计数器清零， 判断位标志计数器是否大于等于20，如果是则关闭电机5的脉冲指令，使移动机构停止运动， 然后执行判断单元二；否则重复启动该单元；

判断单元二：用于判断电机5是否在设备坐标系的运动方向沿轴的正方向，如果是则结 束原点定位；如果否则启动校正单元；

校正单元：到位标志计数器清零，DSP2向位置脉冲减计数器3写入正整数rglapls，设 定电机5的运动方向为设备坐标系中轴的正方向，然后启动电机5，然后重复查询位置脉冲 减计数器3的状态，待位置脉冲减计数器3为0时，启动关闭脉冲指令单元。

低精度多个传感器融合的原点精确定位方法，该方法是低精度多个传感器融合的原点精 确定位系统实现的，所述低精度多个传感器融合的原点精确定位系统包括工控机1、DSP2、 位置脉冲减计数器3、电机驱动器4、电机5、移动机构、位移传感器6、位移计数器7和原 点传感器8；

工控机1的位置指令信号输出端连接DSP2的位置指令信号输入端，DSP2将要发送的脉 冲总数写位置脉冲减计数器3，所述DSP2的脉冲信号输出端连接位置脉冲减计数器3的脉冲 信号输入端，所述位置脉冲减计数器3的状态信号输出端连接DSP2的状态信号输入端，位置 脉冲减计数器3用于控制发送给电机驱动器4的位置脉冲总数，位置脉冲减计数器3的位置 脉冲信号输出端连接电机驱动器4的位置脉冲信号输入端，所述电机驱动器4的驱动信号输 出端连接电机5的驱动信号输入端，所述电机5用于驱动移动机构移动，位移传感器6用于 检测移动机构的位移信息，位移传感器6输出表示位移信息的脉冲信号给位移计数器7的脉 冲信号输入端，原点传感器8用于检测移动机构是否位于原点范围的状态，原点传感器8的 检测信号输出端连接位移计数器7的原点检测信号输入端，所述位移计数器7的脉冲计数输 出端连接DSP2的位置脉冲信号输入端；

根据运动设备建立设备坐标系，所述的原点精确定位方法是由嵌入在DSP2内部的软件实 现的，该方法包括以下步骤：

初始化步骤：用于启动电机5，电机5的转动方向为设备坐标系中轴向的负方向；位移 计数器7清零，位置脉冲减计数器3的脉冲数为maxval，maxval表示该轴整个运动范围所对 应的位移传感器6脉冲数，在该步骤结束后执行判断步骤一；

判断步骤一：移动机构到达原点传感器8的感应范围且接收到位移传感器6的Z相信号， 则执行关闭脉冲指令步骤，否则重新执行该步骤；

关闭脉冲指令步骤：读取系统每个脉冲周期上位移计数器7的值，读取的当前值与上个 周期的读取的值相同时，则DSP2中的到位标志计数器加1，否则到位标志计数器清零，判断 位标志计数器是否大于等于20，如果是则关闭电机5的脉冲指令，使移动机构停止运动，然 后执行判断步骤二；否则重复执行该步骤；

判断步骤二：判断电机5在设备坐标系的运动方向，当运动方向沿轴的正方向则结束原 点定位；当运动方向沿轴的负方向则执行校正步骤；

校正步骤：到位标志计数器清零，DSP2向位置脉冲减计数器3写入正整数rglapls，设 定电机5的运动方向为设备坐标系中轴的正方向，然后启动电机5，然后重复查询位置脉冲 减计数器3的状态，待位置脉冲减计数器3为0时，执行步骤关闭脉冲指令步骤。

本发明的有益效果是：本发明在采用了低精度传感器降低装配难度和设备成本的同时， 保证了定位精度。

本发明所述的低精度多个传感器融合的原点精确定位方法中采用了位移传感器和原点传 感器配合实现定位，其中原点传感器用于检测移动机构是否位于原点范围的状态，位移传感 器用于检测移动机构的位移信息，通过对两个传感器采集的数据进行比较和计算进而实现精 确定位。

本发明所述的低精度多个传感器融合的原点精确定位系统采用多个低精度传感器实现联 合定位，在达到了采用高精度传感器相同的定位精度的前提下，降低了对传感器安装精度的 要求，进而降低了传感器的装配难度，装配难度同比降低了30%以上，设备成本降低了20%。

附图说明

图1为低精度多个传感器融合的原点精确定位系统的整体结构图；

图2为低精度多个传感器融合的原点精确定位方法的流程图。

具体实施方式

具体实施方式一：下面结合图1说明本实施方式，本实施方式所述的低精度多个传感器 融合的原点精确定位系统，它包括工控机1、DSP2、位置脉冲减计数器3、电机驱动器4、电 机5、移动机构、位移传感器6、位移计数器7和原点传感器8；

工控机1的位置指令信号输出端连接DSP2的位置指令信号输入端，DSP2将要发送的脉 冲总数写位置脉冲减计数器3，所述DSP2的脉冲信号输出端连接位置脉冲减计数器3的脉冲 信号输入端，所述位置脉冲减计数器3的状态信号输出端连接DSP2的状态信号输入端，位置 脉冲减计数器3用于控制发送给电机驱动器4的位置脉冲总数，位置脉冲减计数器3的位置 脉冲信号输出端连接电机驱动器4的位置脉冲信号输入端，所述电机驱动器4的驱动信号输 出端连接电机5的驱动信号输入端，所述电机5用于驱动移动机构移动，位移传感器6用于 检测移动机构的位移信息，位移传感器6输出表示位移信息的脉冲信号给位移计数器7的脉 冲信号输入端，原点传感器8用于检测移动机构是否位于原点范围的状态，原点传感器8的 检测信号输出端连接位移计数器7的原点检测信号输入端，所述位移计数器7的脉冲计数输 出端连接DSP2的位置脉冲信号输入端；

在DSP2内部的软件实现的定位模块，所述定位模块包括以下单元：

初始化单元：用于启动电机5，电机5的转动方向为设备坐标系中轴向的负方向；位移 计数器7清零，位置脉冲减计数器3的脉冲数为maxval，maxval表示该轴整个运动范围所对 应的位移传感器脉冲数；

判断单元一：用于判断移动机构是否到达原点传感器8的感应范围且接收到位移传感器 6的Z相信号，如果是则启动关闭脉冲指令单元，否则重新启动该单元；

关闭脉冲指令单元：用于读取系统每个脉冲周期上位移计数器7的值，读取的当前值与 上个周期的读取的值相同时，则DSP2中的到位标志计数器加1，否则到位标志计数器清零， 判断位标志计数器是否大于等于20，如果是则关闭电机5的脉冲指令，使移动机构停止运动， 然后执行判断单元二；否则重复启动该单元；

判断单元二：用于判断电机5是否在设备坐标系的运动方向沿轴的正方向，如果是则结 束原点定位；如果否则启动校正单元；

校正单元：到位标志计数器清零，DSP2向位置脉冲减计数器3写入正整数rglapls，设 定电机5的运动方向为设备坐标系中轴的正方向，然后启动电机5，然后重复查询位置脉冲 减计数器3的状态，待位置脉冲减计数器3为0时，启动关闭脉冲指令单元。

具体实施方式二：本实施方式对具体实施方式一所述的低精度多个传感器融合的原点精 确定位系统作进一步限定，本实施方式中，位移传感器6采用磁栅尺或光栅尺实现。

具体实施方式三：下面结合图1和图2说明本实施方式，本实施方式所述的低精度多个 传感器融合的原点精确定位方法，该方法是低精度多个传感器融合的原点精确定位系统实现 的，所述低精度多个传感器融合的原点精确定位系统包括工控机1、DSP2、位置脉冲减计数 器3、电机驱动器4、电机5、移动机构、位移传感器6、位移计数器7和原点传感器8；

工控机1的位置指令信号输出端连接DSP2的位置指令信号输入端，DSP2将要发送的脉 冲总数写位置脉冲减计数器3，所述DSP2的脉冲信号输出端连接位置脉冲减计数器3的脉冲 信号输入端，所述位置脉冲减计数器3的状态信号输出端连接DSP2的状态信号输入端，位置 脉冲减计数器3用于控制发送给电机驱动器4的位置脉冲总数，位置脉冲减计数器3的位置 脉冲信号输出端连接电机驱动器4的位置脉冲信号输入端，所述电机驱动器4的驱动信号输 出端连接电机5的驱动信号输入端，所述电机5用于驱动移动机构移动，位移传感器6用于 检测移动机构的位移信息，位移传感器6输出表示位移信息的脉冲信号给位移计数器7的脉 冲信号输入端，原点传感器8用于检测移动机构是否位于原点范围的状态，原点传感器8的 检测信号输出端连接位移计数器7的原点检测信号输入端，所述位移计数器7的脉冲计数输 出端连接DSP2的位置脉冲信号输入端；

根据运动设备建立设备坐标系，所述的原点精确定位方法是由嵌入在DSP2内部的软件实 现的，该方法包括以下步骤：

初始化步骤：用于启动电机5，电机5的转动方向为设备坐标系中轴向的负方向；位移 计数器7清零，位置脉冲减计数器3的脉冲数为maxval，maxval表示该轴整个运动范围所对 应的位移传感器脉冲数，在该步骤结束后执行判断步骤一；

判断步骤一：判断移动机构是否到达原点传感器8的感应范围且接收到位移传感器6的 Z相信号，如果是则执行关闭脉冲指令步骤，如果否则重新执行该步骤；

关闭脉冲指令步骤：读取系统每个脉冲周期上位移计数器7的值，读取的当前值与上个 周期的读取的值相同时，则DSP2中的到位标志计数器加1，否则到位标志计数器清零，判断 位标志计数器是否大于等于20，如果是则关闭电机5的脉冲指令，使移动机构停止运动，然 后执行判断步骤二；否则重复执行该步骤；

判断步骤二：判断电机5是否在设备坐标系的运动方向沿轴的正方向，如果是则结束原 点定位；如果否则执行校正步骤；

校正步骤：到位标志计数器清零，DSP2向位置脉冲减计数器3写入正整数rglapls，设 定电机5的运动方向为设备坐标系中轴的正方向，然后启动电机5，然后重复查询位置脉冲 减计数器3的状态，待位置脉冲减计数器3为0时，执行步骤关闭脉冲指令步骤。

本实施方式中，初始化步骤中位移计数器清零后不是真正的把该计数器置零，而是根据 设备在该轴方向上行程而定的值midval，该值稍大于整个行程对应的最大位移脉冲计数。这 样避免因为设备上电就反向移动而造成的位移计数器反向溢出。

在设备回原点过程中将位置脉冲减计数器初值设为maxval，该值为走完整个行程所需要 发送给电机驱动器的最大位置脉冲数，这样将起到保护作用因为当减计数器为0的时候，位 置脉冲输出将被关断，一定程度避免了因某设备故障，导致设备不能到原点区域，但是位置 脉冲一直在发送，进而发生危险。

关闭脉冲指令步骤中，当发送给电机的位置脉冲信号关断之后，由于惯性，以及驱动器 根据收到的位置脉冲数调整电机位置，运动设备不可能立即停止。本发明采用计数滤波的方 式，当在20个采样周期内，位置计数器的值都没有变，那么可以认为运动设备确实已经停止。 这样就在尽可能短的时间内可靠的得到运动设备是否停止的信息。

校正步骤中，在原点位置关断脉冲，并等待系统稳定后，向位置脉冲减计数器写入正整 数，并设定运动设备运动方向为坐标轴正方向。即让设备再正向运动正整数个脉冲。正整数 的取值与运动设备惯性和位置脉冲延迟有关，要保证该过程结束后，运动设备停止的位置在 原点位置的正方向一侧。

具体实施方式四：本实施方式对具体实施方式三所述的低精度多个传感器融合的原点精 确定位方法作进一步限定，本实施方式中，位移传感器6采用磁栅尺或光栅尺实现。

本实施方式中，在需要定位操作的设备中，位移传感器是必不可少的，以磁栅尺和光栅 尺最为常见，而且这些位移传感器大多为增量式的，其输出信号都符合统一的标准规范，即 以A/B/Z的方式输出位移增量脉冲。

具体实施方式五：本实施方式对具体实施方式三所述的低精度多个传感器融合的原点精 确定位方法作进一步限定，本实施方式中，DSP2通过位移计数器7接收位移传感器6发送的 表示当前位置的脉冲信息号。

具体实施方式六：本实施方式对具体实施方式三所述的低精度多个传感器融合的原点精 确定位方法作进一步限定，本实施方式中，判断步骤一的位移传感器6的Z相信号为固定间 隔a的脉冲信号，原定传感器的感应区域宽度w大于等于a且小于2a。

本实施方式中，所选用的位移传感器的精度一定是要符合设备的定位精度的，其输出信 号的Z相为固定间隔a大于传感器精度输出的脉冲信号。所选用的低精度传感器应满足其感 应区域宽度w满足a<=w<2a，这样就保证了在原点定位传感器感应区域内有且只有一个位移 传感器的Z相脉冲。