移动机器人建图与导航代码实现——1.Hector SLAMHector SLAM

Hector SLAM

这一部分利用hector slam完成建图和定位，暂无全局定位功能，使用2D激光雷达和里程计。测试中使用的GUI改编自https://www.cnblogs.com/scomup/p/7074847.html 。

整体分为4步：
1.motion prediction，即估计当前姿态和上一时刻的变化量
2.scan matching，通过激光点云修正上一步的估计，也是该算法的重点
3.map update，更新地图
4.pose update，更新姿态

地图

首先我们需要确定地图的格式，采用栅格地图。设一个二维数组(overline{M})，值域为([-infty, +infty])，数值越大，表示该点为障碍物的概率越大，反之越小。将其映射到((0, 1))上，可以看作概率，因此设(M_{ij} = frac{e^{overline{M}_{ij}}}{1+e^{overline{M}_{ij}}})，即可看作是概率栅格地图。源码在GridMap.py中，地图更新部分有很慢的Python实现，在注释部分中，也有较快的C++实现(c_extern/map_update.cpp)，测试效率差30倍。

地图更新

考虑激光点云中的一个点(P)和当前机器人位置(Q)，(P)附近的点是障碍物的概率应该增大，从(Q)到(P)线的点是障碍物的概率应该减小。

motion prediction

这一步是为了将激光点云粗略地从机器人坐标系映射到世界坐标系。设k时刻的里程计对应变换矩阵为(T_k)，经过修正的姿态为(widetilde{xi}_k)，则可以初步估计k+1时刻姿态为(widetilde{xi}_kT^{-1}_kT_{k+1})。

scan matching

设激光点云有n个点，我们希望估计一个姿态(xi=begin{bmatrix}x\y\ thetaend{bmatrix})，使得点云尽可能分布在障碍物上，使用最小二乘，即求

(xi=argmin_{xi}sum_{i=0}^n[1-M(S_i(xi))]^2),

其中(S_i(xi))表示第i个点在姿态(xi)下的坐标，(M)为已有的地图。设姿态变化量为(Deltaxi)，优化目标为

(sum_{i=0}^n[1-M(S_i(xi+Deltaxi))]^2)，

对其泰勒展开

(sum_{i=0}^n[1-M(S_i(xi))-nabla M(S_i(xi))frac{partial S_i(xi)}{partial xi}Delta xi]^2)

求偏导并令为0

(2sum_{i=0}^n[nabla M(S_i(xi))frac{partial S_i(xi)}{partial xi}]^T[1-M(S_i(xi))-nabla M(S_i(xi))frac{partial S_i(xi)}{partial xi}Delta xi]=0)

求得

(Deltaxi=H^{-1}sum_{i=1}^n[nabla M(S_i(xi))frac{partial S_i(xi)}{partial xi}]^T[1-M(S_i(xi))])

其中

(H=sum_{i=1}^n[nabla M(S_i(xi))frac{partial S_i(xi)}{partial xi}]^T[nabla M(S_i(xi))frac{partial S_i(xi)}{partial xi}])

更新姿态为(xi=xi+Deltaxi)。

这一部分代码见SLAM.py。

栅格地图上偏微分求法

begin{align*} nabla M(S_i(xi))frac{partial S_i(xi)}{partial xi} =&begin{bmatrix} frac{partial M(S_i(xi))}{partial x}\ frac{partial M(S_i(xi))}{partial y}\ frac{partial M(S_i(xi))}{partial theta} end{bmatrix}\ =&begin{bmatrix} frac{partial M(S_i(xi))}{partial x}\ frac{partial M(S_i(xi))}{partial y}\ (-xsintheta-ycostheta)frac{partial M(S_i(xi))}{partial x} + (xcostheta-ysintheta)frac{partial M(S_i(xi))}{partial y} end{bmatrix}\ =&begin{bmatrix} 1 & 0\ 0 & 1\ -xsintheta-ycostheta & xcostheta-ysintheta end{bmatrix} begin{bmatrix} frac{partial M(S_i(xi))}{partial x}\ frac{partial M(S_i(xi))}{partial y} end{bmatrix} end{align*}

其中的

[ begin{bmatrix} frac{partial M(S_i(xi))}{partial x} \ frac{partial M(S_i(xi))}{partial y} end{bmatrix} ]

可以用附近的四个点做双线性插值，不妨设(S_i(xi)=begin{bmatrix}x\yend{bmatrix})，
则有

begin{align*} frac{partial M(S_i(xi))}{partial x} approx&([y]+1-y)(M_{[x]+1,[y]}-M_{[x],[y]})+(y-[y])(M_{[x]+1,[y+1]}-M_{[x],[y+1]}) end{align*} begin{align*} frac{partial M(S_i(xi))}{partial y} approx&([x]+1-x)(M_{[x],[y]+1}-M_{[x],[y]})+(x-[x])(M_{[x]+1,[y+1]}-M_{[x+1],[y]}) end{align*}