[发明专利]一种基于四轮全向全驱移动机器人的二维激光雷达建图方法

权利要求书

1.一种基于四轮全向全驱移动机器人的二维激光雷达建图方法，其特征在于，具体包括以下步骤：

S1，对四轮全向全驱移动机器人进行正运动学解算，实时获得移动机器人的里程计信息；

获取编码器数据{V_LF，V_RF，V_LB，V_RB，A_LF，A_RF，A_LB，A_RB}，对四轮全向全驱移动机器人进行正运动学解算；其中V_LF、V_RF、V_LB和V_RB分别为四个驱动电机短时间t的平均速度，A_LF、A_RF、A_LB、A_RB分别为四个转向电机的转角；

机器人具有三种运动模式：直线行驶模式、四轮转向模式和零半径转向模式；计算在Δt时间内机器人的X方向平移量ΔX、Y方向平移量ΔY和旋转量Δθ；

S2，基于移动机器人的里程计信息，对激光雷达数据进行去除运动畸变处理；

S3，根据里程计信息和激光雷达对周围环境的感知信息，构建概率栅格地图；

每帧去除运动畸变的激光雷达数据为一个节点的数据，一定数量的节点组成一个子图，若干个子图构成一张完整的地图；一帧去除运动畸变后的激光雷达数据传输时与当前活跃的子图进行扫描匹配，并根据里程计信息提供的一个初始位姿，估计节点在子图的位姿；

移动机器人相对于子图的位姿用T_ε＝(ε_x，ε_y，ε_θ)表示，一帧激光点云包括一个起点和若干终点，记H＝{h₁，…，h_k，…，h_K}为激光雷达扫描到的K个激光点的集合，h_k表示第k个激光束在移动机器人坐标系下的位置坐标，h_k在子图坐标系下的坐标表示为：

起点栅格的状态为空闲，从起点到终点之间的栅格状态也为空闲，存放于集合misses，misses中的栅格被赋予初值p_miss；每个终点所在的栅格表示为被占有，存放于集合hits，hits中的栅格被赋予初值p_hit；

当一个栅格点先前已有p值，用以下公式进行更新：

M_new(x)＝clamp(odds^-1(odds(M_old(x))·odds(p_hit))) (3)

其中，M_new(x)为更新后的p值，odds(p)为栅格被占有和空闲概率的比值，odds^-1()函数根据被占有和空闲概率比值求解被占有的概率，clamp是区间限定函数，M_old(x)为栅格已存在的p值；

3.1插入子图；

每次获得更新后的节点插入至子图最优位置，通过扫描匹配对T_εh_k进行优化，扫描匹配过程为求解最小二乘问题：

其中，M_smooth是线性评价函数，该函数输出结果为(0，1)以内的数值；

求解最小二乘问题过程中，构造多分辨率子图；将节点扫描数据先在低分辨率地图中进行扫描匹配得到初始位姿估计，然后根据得到的位姿估计在高分辨率地图中再次进行扫描匹配，得到准确的位姿，最后把这个节点插入到子图中，当子图中插入的节点数到达规定数量，将子图设置为完成状态；

3.2闭环检测添加约束；

通过稀疏位姿调整方法优化所有节点和子图的位姿，减少累积误差；

式(7)描述了全局位姿估计的偏差，其中是第i个子图在世界坐标系下的坐标，是第j个节点的扫描位姿，和分别是子图位姿和节点位姿的位置矢量，二者求差得到世界坐标系下节点相对于子图的位置矢量；矩阵是世界坐标系下的矢量在子图局部坐标系下的转换关系；和分别是子图位姿和节点位姿中的方向角，二者求差为子图局部坐标系下的相对方向角；ε_ij是前端中节点和子图间的相对位姿；根据世界坐标系下的位姿估计计算出的相对变换关系与前端输出的相对变换关系求差，得到全局估计的偏差量；表示一个关于全局位姿估计偏差的二次型，用作评价每个子图和节点对的位姿估计代价函数，将其带入式(5)得到后端优化的目标函数；表示前端扫描匹配结果的不确定性；

构建运动滤波器进行简化闭环检测过程并保证位姿优化结果精度；定义运动滤波器的平移运动阈值和角度运动阈值；当前准备参与闭环检测的节点和上一个构建约束的节点之间的相对运动变化量大于平移运动阈值或者大于角度运动阈值，则参与闭环检测，否则，抛弃该节点；

3.3子图和节点位姿调整；

根据优化结果修正各个子图和节点的全局位姿；

3.4子图拼接；

根据子图的全局位姿，将所有子图进行拼接，得到完整的地图。