一种应用于机器人的RTK-GNSS外参标定方法

技术领域

本发明涉及定位领域，尤其涉及一种应用于机器人的RTK-GNSS外参标定方 法。

背景技术

RTK广泛应用于室外建图与定位系统中，其不会累计误差，并且在开阔场景可以达到1-3厘米的精度，可以有效消除各种里程计的累计误差。缺点是在周围由遮挡物时定位不准确，无法应对室内、隧道、周围由高大建筑物等场景。

SLAM(同步定位与建图)过程通过传感器(激光、摄像头、编码器、IMU 等)可以同步估计周围环境和机器人自身状态，广泛应用在无人驾驶、无人仓储、智能巡检等场景中，目前室外SLAM可以达到厘米级定位精度，缺点是在大型超大型应用场景中，在缺少回环的情况下仍然会累计误差，如果误差累计过大, 回环检测也难以修正。

针对上述定位方案的优缺点，可以发展融合RTK与SLAM的解决方案，只需要在RTK信息足够准确时利用RTK信息对SLAM过程的累计误差进行消除，可以提高SLAM过程的稳定性，使其可以在室外空旷，大型应用场景中使用。

为了融合RTK与SLAM过程，必须给出RTK所在坐标系与SLAM全局坐标系的转换关系，在目前的方案中存在以下缺点：

1.由于机器人定位需要给出位姿信息，大多数辅助里程计的方案都要求给出带有姿态的RTK信息，这就要求使用尺寸较大的双天线RTK，且双天线距离越远，给出方向越准确，但在体型较小的机器人(比如无人机)上是不可能实现的。

2.现有的离线标定算法需要首先给出地图与同步采集的RTK信息，才能进一步处理得到RTK与地图之间的外参，这样的方法无法应用RTK辅助建图过程，只能辅助进行定位。

3.现有标定方法在标定过程中没有考虑RTK天线与机器人车体坐标系不重合的问题，这样会带来无法消除的外参误差(最大为RTK天线到机器人中心距离 *2)。大多数情况下该外参误差已经超出了RTK本身的精度范围。

发明内容

发明目的：本发明针对上述不足，提供了一种应用于移动机器人的外参标定方法。

技术方案：

一种应用于机器人的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数据的序号；

(4)以

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

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

所述步骤(2)中，机器人接收RTK移动站的定位数据，从该数据中提取得到置信度协方差、经纬度、程高信息，并据此得到其协方差和前后时刻位置增量，并分别与协方差设定阈值及位置增量设定阈值进行比较，筛选得到协方差和前后时刻位置增量均小于协方差设定阈值及位置增量设定阈值的定位数据作为有效定位数据；

所述协方差设定阈值根据经验值得到，通过观察RTK数据的波动情况确定；所述位置增量阈值＝机器人最大运行速度*RTK移动站定位数据发送间隔*经验系数，经验系数设为3。

所述步骤(3)中，机器人对步骤(2)得到的有效定位数据进行坐标转换，将经纬度转换到地心坐标系下，机器人根据RTK移动站发送的定位数据以及 SLAM传感器获取的数据计算得到各个时刻其自身位姿数据，并根据时间顺序存储自身位姿数据，以机器人各位置

有益效果：本发明使用单天线RTK，深度嵌入到SLAM算法中，在SLAM 过程在线标定了所需外参，可以充分利用RTK定位高精度的优点。

附图说明

图1是本发明的工作流程图。

图2是本发明的前后端架构图。

图3是本发明的坐标系关系图。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明。

经典的基于优化的SLAM算法由前端后端组成，前端负责构建约束，后端则利用优化方法，在前端构建的约束下，对待优化项(一般为环境状态，机器人状态)进行优化。图1是本发明的工作流程图。如图1所示，本发明一种应用于移动机器人的外参标定方法包括如下步骤：

(1)在机器人上分别配置有RTK移动站和SLAM传感器；RTK移动站通过串口与机器人进行通信，并配置一RTK基站通过无线调频信号与RTK移动站进行通信，通过RTK移动站完成RTK差分定位；RTK基站需要部署在开阔的区域，自此该RTK基站不应该再移动；上电后，RTK移动站同时接收RTK基站数据和卫星数据，并据此进行差分定位，从而输出绝对误差在厘米级的定位数据至机器人；

(2)机器人通过串口接收RTK移动站的定位数据，从该数据中提取得到置信度协方差、经纬度、程高信息；机器人据此得到其协方差和前后时刻位置增量，并分别与协方差设定阈值及位置增量设定阈值进行比较，筛选得到协方差和前后时刻位置增量均小于协方差设定阈值及位置增量设定阈值的定位数据作为有效定位数据，这一步是为了保证RTK信息准确性，规避掉RTK定位数据在特定场景下位置漂移、跳动带来的风险；

在本发明中，协方差设定阈值根据经验值得到，通过观察RTK数据的波动情况确定；位置增量阈值＝机器人最大运行速度*RTK移动站定位数据发送间隔* 经验系数，经验系数一般为3。

(3)机器人对步骤(2)得到的有效定位数据进行坐标转换，将经纬度转换到地心坐标系下；如果是接收到第一帧RTK数据，则根据该信息在原地创建ENU 坐标系，即在程高为0的机器人上的RTK天线所在的位置创建ENU坐标系；并将后续接收到的RTK数据转换到该坐标系之后发送到机器人；地心坐标系是在地球内建立的O-XYZ坐标系；

(4)机器人根据接收到的RTK数据与对应时刻的机器人位姿联合构建残差方程：

机器人根据RTK移动站发送的定位数据以及SLAM传感器获取的数据计算得到各个时刻其自身位姿数据，并根据时间顺序存储自身位姿数据，以机器人各位姿

其中，u表示ENU坐标系，g表示机器人建立的全局地图坐标系，base表示机器人本体坐标系；ni:表示机器人状态队列的第i个序号，fni表示与第i个机器人状态同时刻的RTK信息在RTK信息队列中的序号，f0表示第一帧RTK数据的序号；

(5)以

其中，

可以将式(2)作为比较准确的初始位姿；

由于机器人是根据RTK数据的第一帧信息中的定位信息原地创建ENU坐标系，则

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

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

本发明将RTK信息作为一种传感器添加到前端中,并且构建RTK与机器人状态之间的位姿约束关系,将RTK信息所在的ENU坐标系与地图坐标系之间的位姿转换,RTK天线在机器人车体坐标系中的位置作为待优化项添加到后端的残差方程中。

这样在一个过程中就完成了在线外参标定与辅助SLAM过程两个功能.在 SLAM过程完成后,可以同步输出对应的地图和外参数据,直接应用在定位业务中。

以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等)，这些等同变换均属于本发明的保护范围。