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\\ \theta\end{bmatrix}\)，使得點云盡可能分布在障礙物上，使用最小二乘，即求
\(\xi=argmin_{\xi}\sum_{i=0}^n[1-M(S_i(\xi))]^2\), 其中\(S_i(\xi)\)表示第i個點在姿態\(\xi\)下的坐標，\(M\)為已有的地圖。設姿態變化量為\(\Delta\xi\)，優化目標為
\(\sum_{i=0}^n[1-M(S_i(\xi+\Delta\xi))]^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
\(2\sum_{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\) 求得
\(\Delta\xi=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+\Delta\xi\)。
這一部分代碼見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}\\ (-xsin\theta-ycos\theta)\frac{\partial M(S_i(\xi))}{\partial x} + (xcos\theta-ysin\theta)\frac{\partial M(S_i(\xi))}{\partial y} \end{bmatrix}\\ =&\begin{bmatrix} 1 & 0\\ 0 & 1\\ -xsin\theta-ycos\theta & xcos\theta-ysin\theta \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\\y\end{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*}
