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

技术特征：
1.一种巡检机器人雷达里程计的速度与精度优化方法，其特征在于，在机器人(5)上安装的三维激光雷达(4)、电机轮速计和惯性测量单元imu(1)，优化步骤具体为：步骤1：用机器人上装载的三维激光雷达进行扫描，获取巡检地沟内的激光点云数据，三维激光雷达在t
k
时刻扫描组合起来的点云数据记为p；步骤2：计算曲率：计算当前点前后各5个点与当前的点的曲率c，其计算公式为：其中，x表示三维激光雷达单个扫描线上的点，x
i
表示第i个点，|x|表示向量的模，也就是这个点到三维激光雷达坐标系原点的距离；步骤3：筛选特征点：按照曲率的大小筛选特征点，共分为两类：曲率大的点为线特征点、曲率小的点为面特征点，并对特征点的数量进行限制，这样就在整个三维空间内，将线特征点和面特征点提取出来，用以代替整个点云数据；步骤4：两帧点云位姿转换：第k 1帧与第k帧的相对位姿为：其中，r是旋转矩阵，t是平移向量；第k 1帧中的点p
i
转换到第k帧坐标系：使用惯性测量单元imu提供的在两帧点云采样时间内的旋转运动估计r
imu
作为点云平移变换的结果r；使用轮速计的位姿变换中的平移变换t
odom
作为点云平移变换的迭代初值，即用于优化的相对位姿中的平移向量t的迭代初值由电机轮速计在这个时间内的变换给出；步骤5：特征关联：步骤5.1：线特征关联：当p
i
为线特征点时，在上一帧中搜索离最近的线特征点记为点a，并在相邻线上再找一个线特征点记为点b，两点组成直线，其残差函数为点到直线的距离：其中，p
a
、p
b
分别表示点a、b在本地坐标系中的坐标，
×
为向量的叉乘运算；线特征残差雅克比：对于巡检地沟内直线运行的机器人，只需要平移运动的雅克比，也就是只估计平移运动t，不估计角度变化，所以不需要考虑旋转项，因此，等号右边第二项有所需要的角度估计数据r由惯性测量单元imu给出；步骤5.2：面特征关联：当p
i
为面特征点时，在上一帧中搜索离最近的面特征点记为点m，并在相邻线上再找两个面特征点分别记为j，l，三点组成平面，其残差函数为点到面的距离：
其中，p
j
、p
l
、p
m
分别表示点j、l、m在本地坐标系中的坐标；面特征残差雅克比：同理，其中等号右边第二项有不需要考虑旋转项r；步骤6：位姿优化：结合以上求解公式，使用ceres优化器，进行位姿平移部分的凸优化；步骤7：输出三维雷达数据：将获得的点云数据每10帧与特征地图进行匹配，并把每一帧的特征加入特征地图，特征地图输出频率为1hz，位姿估计输出频率为10hz；步骤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
oaom
、yaw
odom
分别是电机轮速计发布的x方向位移、y方向位移、偏航角的三自由度运动估计数据，此步骤更新频率为10hz；步骤9：进行插值，在平整的直线沟道内充分采信轮速里程计在两帧三维激光雷达里程计输出间隔中的可靠性，当接收到电机轮速计发布的信息时，把接收到的(x
odom
，y
odom
，yaw
odom
)与(x
odombase
，y
odombase
，yaw
odombase
)作差获得轮速计的变换关系，计算公式如下：其中，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
)。

技术总结
本发明公开了一种巡检机器人雷达里程计的速度与精度优化方法，具体为：使用三维激光雷达采集点云数据，计算曲率，按照曲率的大小筛选特征点；进行两帧点云位姿转换，再进行线特征关联和面特征关联；使用Ceres优化器进行位姿平移部分的凸优化，输出三维雷达数据，更新三维雷达里程计数据，再进行插值。本发明提升了运动控制平顺程度与传感器数值匹配的精度，同时提高了定位的效率与精度。同时提高了定位的效率与精度。同时提高了定位的效率与精度。


技术研发人员：田发存 张目华 马磊 沈楷 孙永奎
受保护的技术使用者：西南交通大学
技术研发日：2022.09.16
技术公布日：2022/11/11