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=egin{bmatrix}x\y\ hetaend{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))-
abla M(S_i(xi))frac{partial S_i(xi)}{partial xi}Delta xi]^2$
求偏导并令为0
$2sum_{i=0}^n[
abla M(S_i(xi))frac{partial S_i(xi)}{partial xi}]^T[1-M(S_i(xi))-
abla M(S_i(xi))frac{partial S_i(xi)}{partial xi}Delta xi]=0$
求得
$Deltaxi=H^{-1}sum_{i=1}^n[
abla M(S_i(xi))frac{partial S_i(xi)}{partial xi}]^T[1-M(S_i(xi))]$
其中
$H=sum_{i=1}^n[
abla M(S_i(xi))frac{partial S_i(xi)}{partial xi}]^T[
abla M(S_i(xi))frac{partial S_i(xi)}{partial xi}]$
更新姿态为(xi=xi+Deltaxi)。
这一部分代码见SLAM.py。
栅格地图上偏微分求法
egin{align*}
abla M(S_i(xi))frac{partial S_i(xi)}{partial xi}
=&egin{bmatrix}
frac{partial M(S_i(xi))}{partial x}\
frac{partial M(S_i(xi))}{partial y}\
frac{partial M(S_i(xi))}{partial heta}
end{bmatrix}\
=&egin{bmatrix}
frac{partial M(S_i(xi))}{partial x}\
frac{partial M(S_i(xi))}{partial y}\
(-xsin heta-ycos heta)frac{partial M(S_i(xi))}{partial x} + (xcos heta-ysin heta)frac{partial M(S_i(xi))}{partial y}
end{bmatrix}\
=&egin{bmatrix}
1 & 0\
0 & 1\
-xsin heta-ycos heta & xcos heta-ysin heta
end{bmatrix}
egin{bmatrix}
frac{partial M(S_i(xi))}{partial x}\
frac{partial M(S_i(xi))}{partial y}
end{bmatrix}
end{align*}
其中的
$$
egin{bmatrix}
frac{partial M(S_i(xi))}{partial x} \
frac{partial M(S_i(xi))}{partial y}
end{bmatrix}
$$
可以用附近的四个点做双线性插值，不妨设(S_i(xi)=egin{bmatrix}x\yend{bmatrix})，
则有
egin{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*}
egin{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}