一种巡检机器人雷达里程计的速度与精度优化方法

1.本发明属于空间环境感知技术领域，尤其涉及一种巡检机器人雷达里程计的速度与精度优化方法。


背景技术：

2.车底巡检机器人用于代替人工，在停车场或车辆段的检修地沟内对车辆的车底零部件进行数据采集，通过人工智能算法进行分析，自动给出车辆维保意见，以保证车辆的运行安全，降低巡检工人的工作强度。为了保证对车底零部件数据采集的精度、保障机器人与车底零部件不发生碰撞，机器人在检修沟内定位数据源的速度与精度非常重要。
3.基于激光雷达的同步定位与建图技术是机器人领域最常用的定位方法。ji zhang在2014年提出的loam(lidar odometry and mapping)是一种经典的三维雷达里程计，该算法将同步定位与建图分为不同输出频率，以提高效率与精度。然而，由于现有的基于纯三维雷达点云数据的loam算法，在六自由度运动估计中误差达到了2.5％以上，且三维雷达性能受限于点云采集速度与点云输入速度，其运动估计的输出频率通常处于10hz左右，这非常不利于巡检机器人基于该定位数据源的运动控制与传感器数据参考。
4.因此，有必要基于现有loam算法，为车底巡检机器人在检修地沟这类特殊场景下的定位需求，提供一种轮速计耦合高频高精度雷达里程计解决方案。


技术实现要素：

5.为实现上述目的，本发明提供一种巡检机器人雷达里程计的速度与精度优化方法。
6.本发明的一种巡检机器人雷达里程计的速度与精度优化方法，首先在机器人上安装的三维激光雷达、电机轮速计和惯性测量单元imu，其优化步骤具体为：
7.步骤1：用机器人上装载的三维激光雷达进行扫描，获取巡检地沟内的激光点云数据，三维激光雷达在tk时刻扫描组合起来的点云数据记为p。
8.步骤2：计算曲率：计算当前点前后各5个点与当前的点的曲率c，其计算公式为：
[0009][0010]
其中，x表示三维激光雷达单个扫描线上的点，xi表示第i个点，|x|表示向量的模，也就是这个点到三维激光雷达坐标系原点的距离。
[0011]
步骤3：筛选特征点：按照曲率的大小筛选特征点，共分为两类：曲率大的点为线特征点、曲率小的点为面特征点，并对特征点的数量进行限制，这样就在整个三维空间内，将线特征点和面特征点提取出来，用以代替整个点云数据。
[0012]
步骤4：两帧点云位姿转换：第k 1帧与第k帧的相对位姿为：其中，r是旋转矩阵，t是平移向量；第k 1帧中的点pi转换到第k帧坐标系：
[0013]
使用惯性测量单元imu提供的在两帧点云采样时间内的旋转运动估计r
imu
作为点云平移变换的结果r。
[0014]
使用轮速计的位姿变换中的平移变换t
odom
作为点云平移变换的迭代初值，即用于优化的相对位姿中的平移向量t的迭代初值由电机轮速计在这个时间内的变换给出。
[0015]
步骤5：特征关联：
[0016]
步骤5.1：线特征关联：当pi为线特征点时，在上一帧中搜索离最近的线特征点记为点a，并在相邻线上再找一个线特征点记为点b，两点组成直线，其残差函数为点到直线的距离：
[0017][0018]
其中，pa、pb分别表示点a、b在本地坐标系中的坐标，
×
为向量的叉乘运算。
[0019]
线特征残差雅克比：
[0020][0021]
对于巡检地沟内直线运行的机器人，只需要平移运动的雅克比，也就是只估计平移运动t，不估计角度变化，所以不需要考虑旋转项，因此，等号右边第二项有所需要的角度估计数据r由惯性测量单元imu给出。
[0022]
步骤5.2：面特征关联：当pi为面特征点时，在上一帧中搜索离最近的面特征点记为点m，并在相邻线上再找两个面特征点分别记为j，l，三点组成平面，其残差函数为点到面的距离：
[0023][0024]
其中，pj、p
l
、pm分别表示点j、l、m在本地坐标系中的坐标。
[0025]
面特征残差雅克比：
[0026][0027]
同理，其中等号右边第二项有不需要考虑旋转项r。
[0028]
步骤6：位姿优化：结合以上求解公式，使用ceres优化器，进行位姿平移部分的凸优化。
[0029]
步骤7：输出三维雷达数据：将获得的点云数据每10帧与特征地图进行匹配，并把每一帧的特征加入特征地图，特征地图输出频率为1hz，位姿估计输出频率为10hz。
[0030]
步骤8：更新三维雷达里程计数据：如果接收到三维雷达里程计发布信息，则更新三维雷达里程计数据(x
lidar
,y
lidar
,yaw
lidar
)，其中，x
lidar
、y
lidar
、yaw
lidar
分别代表三维雷达里程计x方向位移、y方向位移、偏航角的三自由度运动估计数据，并取当前时间的电机轮速计位姿(x
odom
，y
odom
，yaw
odom
)记录为输出插值基准(x
odombase
,y
odombase
,yaw
odombase
)，其中，x
odom
、y
odom
、yaw
odom
分别是电机轮速计发布的x方向位移、y方向位移、偏航角的三自由度运动估计
数据，此步骤更新频率为10hz。
[0031]
步骤9：进行插值，在平整的直线沟道内充分采信轮速里程计在两帧三维激光雷达里程计输出间隔中的可靠性，当接收到电机轮速计发布的信息时，把接收到的(x
odom
，y
odom
，yaw
odom
)与(x
odombase
，x
odombase
，yaw
odombase
)作差获得轮速计的变换关系，计算公式如下：
[0032][0033]
其中，k
dir
表示向前或者向后运动，向前运动为值为1，向后运动值为-1；计算出行驶距离δd，然后发布更新位姿x
lidar
：x
lidar
＝x
lidar
δd
·
cos(yaw
lidar
)，此步骤更新频率为50hz，再次收到三维雷达里程计发布信息时，将这一时刻的电机轮速计位姿(x
odom
，y
odom
，yaw
odom
)标记为下一次插值的(x
odombase
，y
odombase
，yaw
odombase
)。
[0034]
本发明的有益技术效果为：
[0035]
本发明通过对单自由度的机器人使用改进的loam算法，同时针对loam算法输出频率低、六自由度运动估计下精度低的问题进行了改进。经检验，运用此方案后三维激光雷达里程计发布频率从10hz左右提高到50hz左右，提升了运动控制平顺程度与传感器数值匹配的精度，在巡检地沟内100m导航中，综合定位误差可低至
±
0.0075m，即为0.0075％，定位精度得到优化。
附图说明
[0036]
图1是机器人运行示意图；
[0037]
附图标记：1-惯性测量单元(imu)，2-巡检地沟墙壁，3-三维激光雷达扫描区域示意图，4-三维激光雷达，5-机器人本体；
[0038]
图2是本发明的总技术流程图；
[0039]
图3是基于特征的里程计实现流程图。
具体实施方式
[0040]
下面结合附图和具体实施方法对本发明做进一步详细说明。
[0041]
车底巡检机器人在狭窄的巡检地沟内仅具有前后直线运动的单个自由度。由于巡检地沟内大部分为混凝土柱、墙面等重复特征，基于传统二维雷达的定位算法发生退化的概率极大，导致无法使用二维雷达在检修地沟内进行精确定位，所以本发明使用三维雷达来进行定位，提高定位精度，loam算法是一种常用的同步定位与建图算法，但是现有的loam算法存在输出频率低、六自由度运动估计下精度低的问题，结合巡检地沟巡检机器人在巡检地沟内仅直线运行，因此只需要考虑单轴上的定位的特性，本发明提供一种巡检机器人雷达里程计的速度与精度优化方法。
[0042]
本发明的一种在直线巡检地沟内自动运行的差速驱动轮式巡检机器人，结合图1所示，机器人5采用差速驱动在平整直线沟道内运行，安装有：三维激光雷达4、轮式编码器、惯性测量单元(imu)1等，基于ros系统，机器人通过三维激光雷达，与轮式编码器在巡检地沟内自主定位、导航。1)机器人在巡检地沟内直线运动，只对一个自由度使用loam算法；2)在两帧雷达点云之间匹配时，使用电机轮速计的位姿变换作为点云位姿变换的优化迭代初值约束，以提高定位精度；3)使用电机轮速计插值提高输出频率，提升了控制平顺程度，以及传感器数据与里程计位姿的匹配精度。
[0043]
本发明的一种巡检机器人雷达里程计的速度与精度优化方法，如图2、图3所示，具体为：
[0044]
步骤1：用机器人上装载的三维激光雷达进行扫描，获取巡检地沟内的激光点云数据，三维激光雷达在tk时刻扫描组合起来的点云数据记为p。
[0045]
步骤2：计算曲率：计算当前点前后各5个点与当前的点的曲率c，其计算公式为：
[0046][0047]
其中，x表示三维激光雷达单个扫描线上的点，xi表示第i个点，|x|表示向量的模，也就是这个点到三维激光雷达坐标系原点的距离。
[0048]
步骤3：筛选特征点：按照曲率的大小筛选特征点，共分为两类：曲率大的点为线特征点、曲率小的点为面特征点，并对特征点的数量进行限制，这样就在整个三维空间内，将线特征点和面特征点提取出来，用以代替整个点云数据，减少后期需要处理的数据量。
[0049]
步骤4：两帧点云位姿转换：第k 1帧与第k帧的相对位姿为：其中，r是旋转矩阵，t是平移向量；第k 1帧中的点pi转换到第k帧坐标系：
[0050]
考虑到原始loam算法在对旋转的运动估计上具有较大的误差，在imu具有地磁仪功能时，使用惯性测量单元imu提供的在两帧点云采样时间内的旋转运动估计r
imu
作为点云平移变换的结果r，提升了角度估计的精度。
[0051]
三维激光雷达一个扫描的运动过程中，机器人并非完全的匀速运动，因此，在此时间段内使用轮速计的位姿变换中的平移变换todom作为点云平移变换的迭代初值，即用于优化的相对位姿中的平移向量t的迭代初值由电机轮速计在这个时间内的变换给出。与现有loam算法的匀速插值运动模型相比，提供了更优的优化迭代初值，提升后续平移运动估计的精度。
[0052]
步骤5：特征关联：
[0053]
步骤5.1：线特征关联：当pi为线特征点时，在上一帧中搜索离最近的线特征点记为点a，并在相邻线上再找一个线特征点记为点b，两点组成直线，其残差函数为点到直线的距离：
[0054][0055]
其中，pa、pb分别表示点a、b在本地坐标系中的坐标，
×
为向量的叉乘运算。
[0056]
线特征残差雅克比：
[0057][0058]
现有的loam算法需要六自由度运动估计，因此需要对t中的旋转项r进行优化，但是对于巡检地沟内直线运行的机器人，只需要平移运动的雅克比，也就是只估计平移运动t，不估计角度变化，所以不需要考虑旋转项，因此，等号右边第二项有所需要的角度估计数据r由惯性测量单元imu给出。对比现有loam算法，这一优化减少需要处理的
数据量，并且大大提高了定位精度。
[0059]
步骤5.2：面特征关联：当pi为面特征点时，在上一帧中搜索离最近的面特征点记为点m，并在相邻线上再找两个面特征点分别记为j，l，三点组成平面，其残差函数为点到面的距离：
[0060][0061]
其中，pj、p
l
、pm分别表示点j、l、m在本地坐标系中的坐标。
[0062]
面特征残差雅克比：
[0063][0064]
同理，其中等号右边第二项有不需要考虑旋转项r。
[0065]
步骤6：位姿优化：结合以上求解公式，使用ceres优化器，进行位姿平移部分的凸优化。
[0066]
步骤7：输出三维雷达数据：结合图3所示，将获得的点云数据每10帧与特征地图进行匹配，并把每一帧的特征加入特征地图，特征地图输出频率为1hz，位姿估计输出频率为10hz。
[0067]
步骤8：更新三维雷达里程计数据：如果接收到三维雷达里程计发布信息，则更新三维雷达里程计数据(x
lidar
,y
lidar
,yaw
lidar
)，其中，x
lidar
、y
lidar
、yaw
lidar
分别代表三维雷达里程计x方向位移、y方向位移、偏航角的三自由度运动估计数据，并取当前时间的电机轮速计位姿(x
odom
，y
odom
，yaw
odom
)记录为输出插值基准(x
odombase
,y
odombase
,yaw
odombase
)，其中，x
odom
、y
odom
、yaw
odom
分别是电机轮速计发布的x方向位移、y方向位移、偏航角的三自由度运动估计数据，此步骤更新频率为10hz。
[0068]
步骤9：进行插值，在平整的直线沟道内充分采信轮速里程计在两帧三维激光雷达里程计输出间隔中的可靠性，当接收到电机轮速计发布的信息时，把接收到的(x
odom
，y
odom
，xaw
odom
)与(x
odombase
，y
odombase
，yaw
odombase
)作差获得轮速计的变换关系，计算公式如下：
[0069][0070]
其中，k
dir
表示向前或者向后运动，向前运动为值为1，向后运动值为-1；计算出行驶距离δd，然后发布更新位姿x
lidar
：x
lidar
＝x
lidar
δd
·
cos(yaw
lidar
)，此步骤更新频率为50hz，再次收到三维雷达里程计发布信息时，将这一时刻的电机轮速计位姿(x
odom
，y
odom
，yaw
odom
)标记为下一次插值的(x
odombase
，y
odombase
，yaw
odombase
)。
[0071]
与现有技术相比，本发明通过对单自由度的机器人使用改进的loam算法，同时针对loam算法输出频率低、六自由度运动估计下精度低的问题进行了改进。经检验，运用此方案后三维激光雷达里程计发布频率从10hz左右提高到50hz左右，提升了运动控制平顺程度与传感器数值匹配的精度，在巡检地沟内100m导航中，综合定位误差可低至
±
0.0075m，即为0.0075％，定位精度得到优化。