[发明专利]基于多传感器紧耦合的移动机器人位姿估计方法和系统

权利要求书

1.一种基于多传感器紧耦合的移动机器人位姿估计方法，所述多传感器包括相机、IMU和激光雷达，其特征在于，所述方法包括：

对相机采集的当前帧RGB-D图像与上一帧RGB-D图像进行特征匹配，计算匹配过程的视觉重投影误差；

对IMU测量的数据进行积分，构建IMU积分残差；

从激光雷达采集的点云中提取边缘特征点和平面特征点，计算边缘特征点到边缘线的距离以及平面特征点到平面的距离，构建激光点云几何残差；

以视觉重投影误差、IMU积分残差和激光点云几何残差最小为目标，进行位姿估计，得到局部位姿；

使用局部位姿更新激光点云地图后在激光点云地图上进行全局优化，得到全局位姿。

2.如权利要求1所述的一种基于多传感器紧耦合的移动机器人位姿估计方法，其特征在于，所述激光点云几何残差的具体构建方式为：

通过计算点云的局部曲率值，从激光雷达采集的点云中筛选边缘特征点和平面特征点，得到边缘特征点集合和平面特征点集合；

从当前次扫描得到的边缘特征点集合中选取一个边缘特征点，从上一次扫描得到的边缘特征点集合中选取两个边缘特征点组成边缘线；

从当前次扫描得到的平面特征点集合中选取一个平面特征点，从上一次扫描得到的平面特征点集合中选取三个平面特征点组成平面；

通过连续两次扫描的点云位姿变换关系计算边缘特征点和平面特征点的坐标，进而计算边缘特征点到边缘线的距离以及平面特征点到平面的距离，得到激光点云几何残差。

3.如权利要求2所述的一种基于多传感器紧耦合的移动机器人位姿估计方法，其特征在于，所述边缘特征点集合与平面特征点集合的构建包括：

将激光雷达在某一时刻垂直切面扫描得到的点云作为线扫集合，计算线扫集合中点云的局部曲率值，将线扫集合中局部曲率值最大的两个点作为边缘特征点，将线扫集合中局部曲率值最小的四个点作为平面特征点；

将激光雷达在某一次扫描中所有线扫集合中的边缘特征点组合，构建边缘特征点集合，将激光雷达在某一次扫描中所有线扫集合中的平面特征点组合，构建平面特征点集合。

4.如权利要求2所述的一种基于多传感器紧耦合的移动机器人位姿估计方法，其特征在于，所述边缘特征点到边缘线的距离通过如下方式计算：

从当前次扫描得到的边缘特征点集合中选取一个边缘特征点i₀，在选上一次扫描得到的边缘特征点集合中选取与i₀最近的边缘特征点j₀，在上一次扫描得到的边缘特征点集合中选取与j₀相邻扫描线中最近的边缘特征点l₀，将j₀和l₀的连线作为边缘线，计算边缘特征点i₀到边缘线的距离。

5.如权利要求2所述的一种基于多传感器紧耦合的移动机器人位姿估计方法，其特征在于，所述平面特征点到平面的距离通过如下方式计算：

从当前次扫描得到的平面特征点集合中选取一个平面特征点i，在选上一次扫描得到的平面特征点集合中选取与i最近的平面特征点j，在上一次扫描得到的平面特征点集合中选取与j相同线扫中最近的平面特征点l，找寻相邻帧中与点j最近的平面特征点m，计算平面特征点i到j、l和m形成的平面的距离。

6.如权利要求1-5任一所述的一种基于多传感器紧耦合的移动机器人位姿估计方法，其特征在于，所述激光雷达每次扫描一周得到的点云为得到一帧图像的点云，激光雷达每次扫描时，将得到的所有点重投影到每一帧的初始时刻。

7.如权利要求1-5任一所述的一种基于多传感器紧耦合的移动机器人位姿估计方法，其特征在于，所述方法还包括：

采用如下方式对视觉重投影误差进行优化：

其中，r为视觉重投影误差，ρ(r)为优化后的视觉重投影误差。