[发明专利]一种应用于机器人的RTK-GNSS外参标定方法

权利要求书

1.一种应用于机器人的RTK-GNSS外参标定方法，其特征在于：包括步骤：

(1)在机器人上分别配置有RTK移动站和SLAM传感器，RTK移动站与机器人进行通信，并配置一个RTK基站通过调频信号与RTK移动站进行通信，通过RTK移动站完成RTK差分定位；

(2)机器人接收RTK移动站的定位数据，如果接收到的是第一帧RTK数据，则根据该信息在原地创建ENU坐标系，并将后续接收到的RTK数据转换到该坐标系下；

(3)机器人根据RTK移动站发送的定位数据以及SLAM传感器获取的数据计算得到各个时刻其自身位姿数据，并构建残差方程：

其中，u表示ENU坐标系，g表示机器人建立的全局地图坐标系，base表示机器人本体坐标系；ni:表示机器人状态队列的第i个序号，fni表示与第i个机器人状态同时刻的RTK信息在RTK信息队列中的序号，f0表示第一帧RTK数据的序号；是第fni个RTK数据的定位信息在ENU坐标系下的表示，是第一帧RTK数据的定位信息在ENU坐标系下的表示，是第i个机器人位姿在全局地图坐标系下的表示，是RTK移动站天线在机器人本体坐标系下的位姿，是第一帧RTK数据的定位信息在全局地图坐标系下的表示；代表ENU坐标系与全局地图坐标之间的旋转矩阵，表示机器人本体坐标系与全局地图坐标系之间的旋转矩阵；

(4)以作为优化变量，其中，初始值为0；表示在全局地图坐标系下第i个机器人位置；表示ENU坐标系变换至全局地图坐标系的位置信息，由第一帧RTK数据决定；得到初始位姿：

(5)使用LM算法计算得到残差方程的最优值，进而得到各优化变量的估计结果；使用估计结果更新机器人位姿、机器人建立的全局地图信息和RTK外参；

(6)以得到的各优化变量的最优估计结果作为初始值，重复步骤(5)进行迭代，并计算前后两次迭代的最终残差值的变化量，若该变化量小于设定阈值，则结束迭代，得到最终机器人建立的全局地图信息和RTK外参；若该变化量大于设定阈值，则重复本步骤，直至达到最大迭代次数，输出最后一次迭代得到的机器人建立的全局地图信息和RTK外参。

2.根据权利要求1所述的应用于机器人的RTK-GNSS外参标定方法，其特征在于：所述步骤(2)中，机器人接收RTK移动站的定位数据，从该数据中提取得到置信度协方差、经纬度、程高信息，并据此得到其协方差和前后时刻位置增量，并分别与协方差设定阈值及位置增量设定阈值进行比较，筛选得到协方差和前后时刻位置增量均小于协方差设定阈值及位置增量设定阈值的定位数据作为有效定位数据。

3.根据权利要求2所述的应用于机器人的RTK-GNSS外参标定方法，其特征在于：所述协方差设定阈值根据经验值得到，通过观察RTK数据的波动情况确定；所述位置增量阈值＝机器人最大运行速度*RTK移动站定位数据发送间隔*经验系数，经验系数设为3。

4.根据权利要求1所述的应用于机器人的RTK-GNSS外参标定方法，其特征在于：所述步骤(3)中，机器人对步骤(2)得到的有效定位数据进行坐标转换，将经纬度转换到地心坐标系下，机器人根据RTK移动站发送的定位数据以及SLAM传感器获取的数据计算得到各个时刻其自身位姿数据，并根据时间顺序存储自身位姿数据，以机器人各位姿的时间戳为准，对其接收到的RTK数据进行插值，找到相应时刻的RTK位置最终得到构建残差方程。