hector slam原理详解

hector slam 原理详解

个人理解，不当之处请指出，谢谢！
首先是进行scan matching过程
激光自身的坐标系与全局坐标系（可以理解为栅格地图坐标系）原点已知且重合。即激光在地图中初始位置已知，此时激光的第一帧扫描数据在地图中坐标已知。现在激光第二帧扫描数据相对于本身坐标系的坐标也可测出。但不知与第一帧的相对位置关系。（激光/机器人的前后位姿是通过前后帧激光点之间的R、T变换得来的）
后一帧的激光点数据通过R、T（未知）变换到第一帧坐标系中即全局坐标系中。我们要将两帧的数据进行匹配，此时我们假设两帧数据无限接近，即概率格子地图中接近1.（涉及到栅格地图的原理，概率叠加）。
然后构造最小二乘法，对函数先对括号内部展开，然后对使误差偏导为0，求解高斯牛顿方程（为什么用高斯牛顿，可看视觉slam14讲），其中有用到地图求偏导的方程（双线性插值法），带入即可。求出旋转矩阵R与T。
接下来就可求出激光（机器人）在后一帧时刻的位姿（前一帧位姿乘以旋转矩阵得出后一时刻位姿），slam中定位完成。
接下来建图，后一帧的激光位姿求出，因为已知后一帧激光点在激光坐标系下的坐标，所以可根据后一帧激光位姿得后一帧激光点在地图中坐标，即映射到地图中，完成slam建图过程。