目录
1.原理详解
2.算法解析
1.原理详解
Hector整体算法很直接,就是将激光点与已有的地图“对齐”,即扫描匹配。扫描匹配就是使用当前帧与已经有的地图数据构建误差函数,使用高斯牛顿法得到最优解和偏差量。其工作是实现激光点到栅格地图的转换,t时刻所有的激光点都能变换到栅格地图中,也就意味着匹配成功。
具体流程为:
首先初始时刻激光自身的坐标系与栅格地图坐标系重合,即激光在地图中的初始位姿(estimate)已知,激光的第一帧扫描数据在地图中的坐标已知。接着,获取到第二帧激光扫描数据,第二帧数据在激光雷达坐标系下的坐标是可以测出的(根据激光雷达的range、angle便可得到在激光坐标系下的坐标),但是不知道与第一帧的相对位置关系。
下一步就是实现这两帧数据的匹配,我们假设两帧数据无限接近,即激光点在栅格地图占用值接近1(占用值越大,匹配的效果越好)。然后构造最小二乘法,对函数先对括号内部展开,然后对使误差偏导为0,求解高斯牛顿方程(其中有用到地图求偏导的方程(双线性插值法),带入即可求出位姿增量(在hector位姿增量用变量searchDir)。
接下来就可求出第二帧激光在地图坐标系下的位姿(第一帧位姿加上位姿增量即可得到,estimate += searchDir),slam中定位完成。
接下来建图,后一帧的激光位姿求出,因为已知后一帧激光点在激光坐标系下的坐标,所以可根据后一帧激光位姿得后一帧激光点在地图中坐标,即映射到地图中,完成slam建图过程。
cartographer中的前端匹配使用了双三次线性插值+ceres库求解非线性优化问题(构造最小二乘,优化匹配),而hector slam中使用了双线性插值+高斯牛顿求解非线性优化问题
hector存在的问题:
1、其中对于双线性差值,在理论上存在不连续的可能,Pm可能在计算的时候迭代的过程中跑出P00->P11围成的正方形。这个问题也被google的cartographer改进为三线性差值。
https://github.com.cnpmjs.org/googlecartographer/cartographer
2、没有对地图的修正能力,一旦地图出错,之后的匹配也都会出现问题。
2.算法解析
算法解析参考