带式输送机巡检机器人系统高精度定位方法

579 编辑：中冶有色技术网来源：中国矿业大学

2023-11-23 13:52:57

权利要求书： 1.一种带式输送机巡检机器人系统高精度定位方法，其特征在于，所述定位方法包括以下步骤：

S1，对编码器增量进行累加计算，得到机器人沿轨道运动距离；

S2，结合步骤S1获得机器人沿轨道运动距离，采用NFC读到的IC卡位置信息对机器人连续位置进行融合修正，得到机器人连续位置的融合修正公式为：

IC为NFC读到的IC卡位置信息；c表示NFC读到对应IC卡时，编码器开始计数的采样点对应的位置；编码器累加系数k是机器人运行实际距离与对应编码器累加距离之比，与摩擦轮直径成正比；i为机器人从轨道位置a到轨道位置b对应的编码器的第i个采样周期，取值由编码器采样频率决定；Δli表示编码器在第i个采样周期中得到增量；

S3，利用NFC读到的IC卡间数据，和编码器累加得到的对应的IC卡间数据，对步骤S2中编码器累加系数k的取值进行分段、分方向的预测修正，获得表示机器人从ICj卡正向运动到ICj+1卡测得的k值

和表示机器人从ICj卡反向运动到ICj-1卡测得的k值/>

j表示IC卡在轨道上安装的序列号，ICj是NFC读到第j个IC卡的编码器的采样点位置；判断修正后的编码器累加系数k的取值有效性；

S4，对步骤S3修正后的编码器累加系数k的取值进行卡尔曼滤波，减小修正后的编码器累加系数k的随机误差，获得最优估计修正k值；

S5，依据步骤S4获得最优估计修正k值和步骤S2获得的融合修正位置公式，根据轨道与定位模块相对位置模型计算机器人所在轨道的高精度位置；

步骤S3中，利用NFC读到的IC卡间数据，和编码器累加得到的对应的IC卡间数据，对步骤S2中编码器累加系数k的取值进行分段、分方向的预测修正的过程包括以下步骤：

利用NFC读到的IC卡间数据，和编码器累加得到的对应的IC卡间数据，对k值进行分段、分方向的预测修正，则修正k值为：

其中，j表示IC卡在轨道上安装的序列号，

表示机器人从ICj卡正向运动到ICj+1卡测得的k值，/>

表示机器人从ICj卡反向运动到ICj-1卡测得的k值；

步骤S4中，对步骤S3修正后的编码器累加系数k的取值进行卡尔曼滤波，减小修正后的编码器累加系数k的随机误差，获得最优估计修正k值的过程包括以下步骤：

S41，令zjt＝kjt，kjt表示在第t次修正的kj值，则系统模型为：

其中，xjt，表示状态真实值，zjt表示观测值，vjt表示测量噪声，且满足p(vj)∈N(0,R)；

S42，预测方程为：

其中，

为状态预测值，/>

为最优估计值，/>

状态预测协方差，pjt为状态估计协方差；

S43，更新方程为：

其中，Kjt为卡尔曼增益，且Kjt∈[0,1]，第t次修正的最优估计值为

依据下述高精度融合修正位置公式计算机器人所在轨道的高精度位置：

式中，

表示第t次修正的/>

值，/>

表示第t次修正的/>

值。

2.根据权利要求1所述的带式输送机巡检机器人系统高精度定位方法，其特征在于，步骤S1中，对编码器增量进行累加计算，得到机器人沿轨道运动距离的过程包括以下步骤：

S11，假设编码器采样周期内最大旋转角度小于半圈，编码器在每个采样周期中的旋转增量为：

其中，n表示编码器脉冲，y表示编码器原始数据，i表示编码器的采样序号，由编码器采样频率决定；

S12，机器人沿轨道运动的距离表示为：

3.根据权利要求1所述的带式输送机巡检机器人系统高精度定位方法，其特征在于，步骤S3中，判断有效性的过程包括以下步骤：

采用e表示有效性误差，当

时，判断修正kj值不存在粗大误差，认为kj有效，更新kj值；当/>

时，判断修正kj值存在粗大误差，认为kj无效，不更新kj值。

说明书：一种带式输送机巡检机器人系统高精度定位方法技术领域

本发明涉及煤矿安全生产巡检技术领域以及机器人融合定位技术领域，具体而言涉及一种带式输送机巡检机器人系统高精度定位方法。

背景技术

机器人一般选用里程计进行轨迹位置解算，实现机器人轨迹位置反馈。而牵引式轨道机器人将机器人本体与驱动机构进行了分离，之间通过绳索牵引的方式实现间接驱动，这种驱动方式的机器人位置信息采集方案有间接测量和直接测量两种。

间接测量是通过采集驱动电机旋转的圈数，间接计算机器人位置，该种方案的优点是简单易部署，但缺点是牵引绳索与电机的驱动轮之间存在打滑现象，更重要的是牵引绳索存在弹性，应用于煤矿中带式输送机巡检机器人牵引绳索往往很长，其轴向弹性形变不可忽略，因此，间接测量误差很大。

直接测量是通过在机器人本体上安装的编码器与轨道相对运动，实现直接测量，需要机器人本体与运动单元间进行无线通信，传递传感器测量数据，部署难度大。但这种编码器测量数据的方式与牵引绳索的打滑和弹性形变无关，可以比较准确测量出机器人位置信息，适用于较高精度定位的闭环控制。

带式输送机巡检机器人主要部署在煤矿上，用于带式输送机的安全生产巡检。煤矿环境恶劣，空气中携带的煤粉比较多，容易落在轨道上，从而导致与编码器连接的摩擦轮打滑，以及摩擦轮外圈包胶长时间运行磨损会导致编码器的定位不准确。因此，现有的轨道机器人的位置测量存在不准确、不稳定等问题，使得轨道式器机人的高精度定位控制比较困难。

发明内容

本发明针对现有技术中的不足，提供一种带式输送机巡检机器人系统高精度定位方法，为轨道机器人故障定位、自动充电等需要高精度定位功能的实现，提供了高精度位置反馈条件。

为实现上述目的，本发明采用以下技术方案：

本发明实施例提出了一种带式输送机巡检机器人系统高精度定位方法，所述定位方法包括以下步骤：

S1，对编码器增量进行累加计算，得到机器人沿轨道运动距离；

S2，结合步骤S1获得机器人沿轨道运动距离，采用NFC读到的IC卡位置信息对机器人连续位置进行融合修正，得到机器人连续位置的融合修正公式为：

IC为NFC读到的IC卡位置信息；c表示NFC读到对应IC卡时，编码器开始计数的采样点对应的位置；编码器累加系数k是机器人运行实际距离与对应编码器累加距离之比，与摩擦轮直径成正比；i为机器人从轨道位置a到轨道位置b对应的编码器的第i个采样周期，取值由编码器采样频率决定；Δli表示编码器在第i个采样周期中得到增量；

S3，利用NFC读到的IC卡间数据，和编码器累加得到的对应的IC卡间数据，对步骤S2中编码器累加系数k的取值进行分段、分方向的预测修正，获得表示机器人从ICj卡正向运动到ICj+1卡测得的k值kj+和表示机器人从ICj卡反向运动到ICj-1卡测得的k值kj-，j表示IC卡在轨道上安装的序列号，ICj是NFC读到第j个IC卡的编码器的采样点位置；判断修正后的编码器累加系数k的取值有效性；

S4，对步骤S3修正后的编码器累加系数k的取值进行卡尔曼滤波，减小修正后的编码器累加系数k的随机误差，获得最优估计修正k值；

S5，依据步骤S4获得最优估计修正k值和步骤S2获得的融合修正位置公式，根据轨道与定位模块相对位置模型计算机器人所在轨道的高精度位置。

进一步地，步骤S1中，对编码器增量进行累加计算，得到机器人沿轨道运动距离的过程包括以下步骤：

S11，假设编码器采样周期内最大旋转角度小于半圈，编码器在每个采样周期中的旋转增量为：

其中，n表示编码器脉冲，y表示编码器原始数据，i表示编码器的采样序号，由编码器采样频率决定；

S12，机器人沿轨道运动的距离表示为：

进一步地，步骤S3中，利用NFC读到的IC卡间数据，和编码器累加得到的对应的IC卡间数据，对步骤S2中编码器累加系数k的取值进行分段、分方向的预测修正的过程包括以下步骤：

利用NFC读到的IC卡间数据，和编码器累加得到的对应的IC卡间数据，对k值进行分段、分方向的预测修正，则修正k值为：

其中，j表示IC卡在轨道上安装的序列号，

表示机器人从ICj卡正向运动到ICj+1卡测得的k值，/>

表示机器人从ICj卡反向运动到ICj-1卡测得的k值。

进一步地，步骤S3中，判断有效性的过程包括以下步骤：

采用e表示有效性误差，当

时，判断修正kj值不存在粗大误差，认为kj有效，更新kj值；当/>

时，判断修正kj值存在粗大误差，认为kj无效，不更新kj值。

进一步地，步骤S4中，对步骤S3修正后的编码器累加系数k的取值进行卡尔曼滤波，减小修正后的编码器累加系数k的随机误差，获得最优估计修正k值的过程包括以下步骤：

S41，令zjt＝kjt，kjt表示在第t次修正的kj值，则系统模型为：

其中，xjt，表示状态真实值，zjt表示观测值，vjt表示测量噪声，且满足p(vj)∈N(0,R)；

S42，预测方程为：

其中，

为状态预测值，/>

为最优估计值，/>

状态预测协方差，pjt为状态估计协方差；

S43，更新方程为：

其中，Kjt为卡尔曼增益，且Kjt∈[0,1]，第t次修正的最优估计值为

进一步地，依据下述高精度融合修正位置公式计算机器人所在轨道的高精度位置：

式中，

表示第t次修正的/>

值，/>

表示第t次修正的/>

值。

本发明的有益效果是：

本发明提出的带式输送机巡检机器人系统高精度定位方法，在轨道机器人运行过程中不需要工作人员测量修正参数，利用两个自身定位传感器测得的数据，通过传感器融合相互修正的思想，实现带式输送机巡检机器人系统的高精度定位。

附图说明

图1为本发明实施例的带式输送机巡检机器人系统结构示意图。

图2为本发明实施例的两个定位传感器安装示意图。

图3为本发明实施例的轨道与定位模块相对位置模型示意图。

图4为本发明实施例的修正k值与最优估计值曲线图。

图5为本发明实施例的带式输送机巡检机器人系统高精度定位方法流程图。

图中序号为：1、工字钢轨道；2、NFC读卡器；3、编码器；4、机器人本体；5、IC卡片。

具体实施方式

现在结合附图对本发明作进一步详细的说明。

需要注意的是，发明中所引用的如“上”、“下”、“左”、“右”、“前”、“后”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。

图5为本发明实施例的带式输送机巡检机器人系统高精度定位方法流程图。参见图5，该高精度定位方法包括以下步骤：

S1，对编码器增量进行累加计算，得到机器人沿轨道运动距离。

S2，结合步骤S1获得机器人沿轨道运动距离，采用NFC读到的IC卡位置信息对机器人连续位置进行融合修正，得到机器人连续位置的融合修正公式为：

IC为NFC读到的IC卡位置信息；c表示NFC读到对应IC卡时，编码器开始计数的采样点对应的位置；编码器累加系数k是机器人运行实际距离与对应编码器累加距离之比，与摩擦轮直径成正比；i为机器人从轨道位置a到轨道位置b对应的编码器的第i个采样周期，取值由编码器采样频率决定；Δli表示编码器在第i个采样周期中得到增量。

S3，利用NFC读到的IC卡间数据，和编码器累加得到的对应的IC卡间数据，对步骤S2中编码器累加系数k的取值进行分段、分方向的预测修正，获得表示机器人从ICj卡正向运动到ICj+1卡测得的k值

和表示机器人从ICj卡反向运动到ICj-1卡测得的k值/>

j表示IC卡在轨道上安装的序列号，ICj是NFC读到第j个IC卡的编码器的采样点位置；判断修正后的编码器累加系数k的取值有效性。

S4，对步骤S3修正后的编码器累加系数k的取值进行卡尔曼滤波，减小修正后的编码器累加系数k的随机误差，获得最优估计修正k值。

S5，依据步骤S4获得最优估计修正k值和步骤S2获得的融合修正位置公式，根据轨道与定位模块相对位置模型计算机器人所在轨道的高精度位置。

图1为本发明带式输送机巡检机器人系统结构图，由悬挂于胶带正上方的轨道机器人本体、运动单元和监控室中的客户端组成，巡检机器人可以实现数据采集、客户端监控和运动控制。图2为本发明两个定位传感器安装示意图，IC卡中保存有较为精确的轨道点位置信息，通过NFC(Near Field Communication)读取，编码器可以获得连续位置信息，通过感器融合相互修正的思想，实现带式输送机巡检机器人系统高精度定位。图3为本发明实施例的轨道与定位模块相对位置模型示意图。本实施例的带式输送机巡检机器人系统高精度定位方法具体包括以下步骤：

一、得到机器人沿轨道运动距离

步骤S1中，对编码器增量进行累加计算，得到机器人沿轨道运动距离的过程包括以下步骤：

S11，假设编码器采样周期内最大旋转角度小于半圈，编码器在每个采样周期中的旋转增量为：

其中，n表示编码器脉冲，y表示编码器原始数据，i表示编码器的采样序号，由编码器采样频率决定；

S12，机器人沿轨道运动的距离表示为：

二、得到机器人连续位置的融合修正公式

IC卡中保存的位置信息是工作人员通过测量后存入的，可以认为IC卡中的位置信息是相对准确的。当NFC读到一个IC卡中数据时，将剔除之前编码器的累加值，在此位置基础上重新累加，则有机器人连续位置的融合修正公式为：

其中，c表示NFC读到对应IC卡时，编码器开始计数的采样点。比例系数k是机器人运行实际距离与对应编码器累加距离之比，也称编码器累加系数，与摩擦轮直径成正比。i为机器人从a到b，编码器的采样过程，由编码器采样频率决定。Δl表示编码器在每个采样周期中得到增量。

当然，在轨道上存在着布置一个IC卡，此时只需要将IC卡作为节点，但同样适用前述连续位置的融合修正公式。

三、对编码器累加系数k的取值预测修正

步骤S3中，利用NFC读到的IC卡间数据，和编码器累加得到的对应的IC卡间数据，对步骤S2中编码器累加系数k的取值进行分段、分方向的预测修正的过程包括以下步骤：

利用NFC读到的IC卡间数据，和编码器累加得到的对应的IC卡间数据，对k值进行分段、分方向的预测修正，则修正k值为：

其中，j表示IC卡在轨道上安装的序列号，

表示机器人从ICj卡正向运动到ICj+1卡测得的k值，/>

表示机器人从ICj卡反向运动到ICj-1卡测得的k值。

步骤S3中，判断有效性的过程包括以下步骤：

采用e表示有效性误差，当

时，判断修正kj值不存在粗大误差，认为kj有效，更新kj值；当/>

时，判断修正kj值存在粗大误差，认为kj无效，不更新kj值。

四、获得最优估计修正k值

步骤S4中，对步骤S3修正后的编码器累加系数k的取值进行卡尔曼滤波，减小修正后的编码器累加系数k的随机误差，获得最优估计修正k值的过程包括以下步骤：

S41，令zjt＝kjt，kjt表示在第t次修正的kj值，则系统模型为：

其中，xjt，表示状态真实值，zjt表示观测值，vjt表示测量噪声，且满足p(vj)∈N(0,R)；

S42，预测方程为：

其中，

为状态预测值，/>

为最优估计值，/>

状态预测协方差，pjt为状态估计协方差；

S43，更新方程为：

其中，Kjt为卡尔曼增益，且Kjt∈[0,1]，第t次修正的最优估计值为

在实例中，摩擦轮直径取60mm，编码器采样频率1000Hz，则编码器累加系数k为0.184，最优估计修正k值将趋近于0.183附近。

五、计算机器人所在轨道的高精度位置

依据下述高精度融合修正位置公式计算机器人所在轨道的高精度位置：

式中，

表示第t次修正的/>

值，/>

表示第t次修正的/>

值。

综上所述，本实施例的一种带式输送机巡检机器人系统高精度定位方法，其通过对编码器累加的机器人行走距离，与NFC读取IC卡中轨道位置信息相融合修正，得到机器人在轨道上连续的融合修正位置公式，根据历史数据，再对编码器累加系数进行修正、有效性判断和卡尔曼滤波，最后得到带式输送机巡检机器人在轨道上的高精度位置信息。

实例应用

应用时，参考图1，带式输送机巡检机器人系统，通过机器人本体获取带式输送机工作环境数据和运行图像，上传到客户端，进行监控，通过客户端选择机器人运行模式，运动单元结合定位信息实现机器人本体的运动控制。

定位传感器相对安装位置，参考图2，定位传感器包括安装于机器人本体4上侧的NFC模块2，用于读取轨道1下侧保存有轨道位置信息的IC卡5的点位置数据，以及通过与轨道摩擦驱动的编码器组成，用于读取连续位置信息。通过定位融合与参数修正，实现带式输送机巡检机器人的高精度定位。

机器人第t次从a运动到b，以4m/s的匀速前进，如图3所示轨道与定位模块相对位置模型示意图。

根据定位传感器参数，可得编码器采样频率为1000Hz，对应i的更新周期1ms，Δli＝22；编码器脉冲为1024，使用60mm直径的摩擦轮安装编码器，则

IC0＝0，IC1＝10m，IC2＝20m，a对应-5m，b对应15m。计算过程和计算结果如下：

(1)由以上参数可得，初始值：取z10＝k10＝0.1841，e1＝5％，

p10＝1，R＝0.5，则：/>

(2)令编码器的累加值

则一组随机数数据A1～A9分别是：54325，55906，52706，55813，55482，54243，55661，53358，56161，54363。

(3)第一次k值计算与有效性判断。

及为有效。

(4)对第一次状态与协方差进行预测，由预测方程得：

(5)对第一次最优状态值进行更新，由更新方程得：

(6)因此，可以得到第一次机器人从a到达b点时，对应轨道b点的位置为

(7)同理迭代计算，可以得到k2～k9，分别为：0.1897，0.1792，0.1802，0.1844，0.1797，0.1874，0.1781，0.1839。可以得到

分别为：0.1842，0.1828，0.1822，0.1826，0.1822，0.1829，0.1823，0.1825。可以得到/>

分别为：15.07，15.03，15.01，15.02，15.01，15.03，15.01，15.02。

(8)由图4可以看出，最优估计修正k值波动小于修正k值，趋近于0.183附近，可以为高精度的位置控制提供位置反馈信息。

步骤(3)～(8)为从IC1卡到IC2卡轨道间的机器人位置融合修正计算，其他轨道区间位置融合修正计算方式同上步骤，最终获得带式输送机巡检机器人在整个轨道上的高精度位置信息，可用于带式输送机巡检机器人高精度的位置控制。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。