amcl与icp融合算法的探究

因为amcl定位算法精度不够高,而且必须有一定的移动距离或角度变化才能有新的位姿发布,会影响导航最终精度。 所以需要对amcl的定位位姿做一定的矫正,输出相对更准确的位姿。
自然而然想到了icp的方法,icp在匹配时,受到初始值的影响,如果直接将地图与激光帧匹配消耗时间较长,且匹配结果差。只有两片点云“足够近”后,使用icp匹配位姿才会更为准确。
下面的任务就是找到地图点云和激光点云以及初始值。
地图点云:通过map topic 传过来的占据栅格地图,提取其中值为100的点,x、y存入点云cloud_map
激光点云:根据scan topic 通过运算

cloud_in->points[i].x = cos(laser_scan->angle_min+i*laser_scan->angle_increment+tf::getYaw(p.pose.pose.orientation))*laser_scan->ranges[i]+p.pose.pose.position.x;
 cloud_in->points[i].y = sin(laser_scan->angle_min+i*laser_scan->angle_increment+tf::getYaw(p.pose.pose.orientation))*laser_scan->ranges[i]+p.pose.pose.position.y;

其中p.pose就是amcl输出的位姿。在这里已经对激光雷达点云操作了平移和旋转,也就是icp的初始值。
将点云cloud_in和cloud_map做icp,此时如果amcl估计位姿校准的话,icp后的结果也较为准确。
把icp后的旋转矩阵作用到amcl估计位姿p点上,即可求出更为精确的位姿。
我不是大神,只是分享记录一下,肯定还存在许多不足,如果有什么地方出现错误,请批评指正。

你可能感兴趣的:(amcl与icp融合算法的探究)