[发明专利]基于多层次地图匹配的移动机器人位姿纠正算法

摘要

本发明公开了一种基于多层次地图匹配的移动机器人位姿纠正算法，属于机器人和计算机图形学技术领域。本发明所述算法利用同步定位与构图算法建立全局栅格地图，结合当前观测与栅格地图的匹配关系利用AMCL算法纠正航迹推算出的位姿，从而得到相对准确的全局位姿信息；同时，本发明把栅格地图转换为之对应的全局目标点云地图，将机器人实时观测的激光点云与目标点云地图配准，以进一步修正全局位姿。本发明所述方法可以获得准确的全局位姿信息，减少长距离定位误差积累，避免现有粒子滤波技术中因粒子空间有限导致求解的位姿不够准确以及ICP算法求解的精度与效率对初始位姿过于依赖的缺点，实现了高效且准确的位姿求解。

主权项

1.一种基于多层次地图匹配的移动机器人位姿纠正算法，其特征在于，包括以下步骤：步骤1.利用同步定位与构图算法建立机器人二维栅格地图；在机器人每次开机后，采用开源的计算机视觉的opencv函数库读取已经建好的栅格地图；步骤2.栅格地图上的黑色像素点代表激光扫描到的物体，把相应的黑色像素点转化为图像标系下的激光点云数据格式；再将图像坐标系下的激光点云P转换至地图坐标系下，得到地图坐标系下的点云P′，即全局目标点云地图；图像坐标系和地图坐标系之间的转换关系为：P′＝RP+t；其中R是图像坐标系到地图坐标系的旋转矩阵，t是地图坐标系原点在图像坐标系下的坐标；步骤3.根据机器人运动学模型，利用里程计和惯性测量单元的数据融合，计算出机器人的航迹，从而得到一个没有全局信息的初始位姿估计；步骤4.采用自适应蒙特卡洛定位算法将激光雷达扫描到的当前观测点云与栅格地图进行匹配，纠正机器人航迹推算出的位姿，从而得到机器人在栅格地图中的全局位姿；步骤5.设定激光雷达的扫描间距和旋转角度差，激光雷达依次扫描并采集激光点云数据S_k，该点云位于机器人坐标系下；再采用AMCL算法输出的全局位姿作为激光点云S_k与全局目标点云地图P′的初始配准参数初始旋转矩阵和初始平移向量步骤6.根据初始配准参数，将点云S_k和全局目标点云地图P′进行配准，得到最终纠正的机器人位姿，为旋转矩阵和平移向量步骤6的具体步骤为：步骤6‑1.令当前迭代次数i＝1，根据和把机器人坐标系下的点云S_k旋转平移变换得到地图坐标系下的点云Q，并把点云Q和点云地图P′按照kd‑tree的数据结构存储，Q＝{q_j,j＝1...N}，P′＝{p′_j,j＝1...N}，N是激光单次扫描得到点云总数；步骤6‑2.根据最近距离原则在Q中搜索p′j的距离最近点qj得到搜索关联点对(qj,p′j)；根据最近互邻原则在点云地图P′中搜索qj的最近点p″j，判断p′j与p″j的距离是否小于预设条件dm；若搜索关联点对(q_j,p′_j)满足预设条件，则确定关联点对为(q_j,p′_j)；否则返回步骤6‑2重新搜索；最终得到N_t对关联点对(q_m,p′_m)，m＝1…N_t，N_t为满足预设条件的关联点对的个数，每个关联点对的距离为步骤6‑3.对Nt对关联点对的距离进行排序，选择距离较小的No对关联点对(qn,p′n)，其中n＝1…No，No＝Nt×η，η为固定重叠率；步骤6‑4.基于奇异分解的方法，利用N_o对关联点对(q_n,p′_n)，计算并更新旋转矩阵和平移向量步骤6‑5.判断当前误差是否达到预设精度要求，如达到要求则进行下一步骤，end＝i；如未达到要求，令i＝i+1，返回至步骤6‑1；步骤7.把旋转矩阵转换为对应的四元数，最终得到纠正后的机器人在地图坐标系下的位置和角度。