[发明专利]一种带式输送机巡检机器人系统高精度定位方法

权利要求书

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

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

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

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

S3，利用NFC读到的IC卡间数据，和编码器累加得到的对应的IC卡间数据，对步骤S2中编码器累加系数k的取值进行分段、分方向的预测修正，获得表示机器人从IC_j卡正向运动到IC_j+1卡测得的k值和表示机器人从IC_j卡反向运动到IC_j-1卡测得的k值j表示IC卡在轨道上安装的序列号，IC_j是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卡在轨道上安装的序列号，表示机器人从IC_j卡正向运动到IC_j+1卡测得的k值，表示机器人从IC_j卡反向运动到IC_j-1卡测得的k值；

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

S41，令z_jt＝k_jt，k_jt表示在第t次修正的k_j值，则系统模型为：

其中，x_jt，表示状态真实值，z_jt表示观测值，v_jt表示测量噪声，且满足p(v_j)∈N(0,R)；

S42，预测方程为：

其中，为状态预测值，为最优估计值，状态预测协方差，p_jt为状态估计协方差；

S43，更新方程为：

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

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

式中，表示第t次修正的值，表示第t次修正的值。

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

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

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

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

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

采用e表示有效性误差，当时，判断修正k_j值不存在粗大误差，认为k_j有效，更新k_j值；当时，判断修正k_j值存在粗大误差，认为k_j无效，不更新k_j值。