基於似然場的全域性定位

似然場法定位檢測

似然場最小二乘問題構建

機器人Robot在地圖World中的位姿表示為\(\boldsymbol{x}\),鐳射雷達掃描得到的點雲表示為\(\{p_i^R\}\),其中\(^R\)表示在機器人座標系下的座標，\(_i\)表示點雲中第i個點。

\[\boldsymbol{x}=[x, y, \theta]^{\rm{T}} \]

那麼，點雲中機器人座標系下的某個掃描點\(\boldsymbol{p}_i^B\)的距離與角度為\(r_i\),\(\rho_i\)，那麼根據當前鐳射的位姿，可以將它轉換到世界座標系下：

\[p_i^W=[x+r_i\cos(\rho_i+\theta),y+r_i\sin(\rho_i+\theta)]^\top \]

下面思考這樣一個問題：

理論上我們應該已知點雲\(\{p_i^R\}\)，如何求解機器人在地圖World中的位姿\(x\)，使得點雲上所有的點經過變換後，都在地圖上對應位置的黑線上？
對於機器人一幀掃描得到的點雲，我們能否建立一個最小二乘的最佳化問題，使得點雲中的每個點與地圖中的對應點之間的距離最小？透過這樣子就可以讓掃描得到的點雲都儘量的落到地圖黑線上。

採用高斯似然場法將掃描資料與柵格地圖進行配準。

似然場圍繞每一個 地圖邊界點 產生，隨距離增大而逐漸增大，其範圍和衰減過程可自行定義。（從圖中的表現為影像灰度代表值，黑色為0，白色為1,其餘為0-1），我們暫且先把這個場定義為似然場\(\pi\)。

似然場中的讀數可直接作為配準時的目標函式，也即要求某個掃描點\(p_i^R\),經機器人位姿\(\boldsymbol{x}\)變換後，得到世界座標系上的點\(p_i^W\),同時，存在一個世界座標系下的似然場\(\pi\)。這個點落在似然場\(\pi\)中的讀數為\(\pi(p_i^W)\)

於是機器人位姿\(x\)，可透過最佳化問題得到，最小二乘問題構建如下，其中n為點雲中點的個數，\(\boldsymbol{p}_i^W\)為點雲中第i個點在世界座標系下的座標，\(\|\cdot\|_2\)為L2範數,歐氏距離.

\[\boldsymbol{x}^*=\arg\min_{x}\sum_{i=1}^n\|\pi(\boldsymbol{p}_i^W)\|_2^2 \]

π 函式對位姿\(x\)的雅可比矩陣可由鏈式求導法則

\[\frac{\partial\pi}{\partial\boldsymbol{x}}=\frac{\partial\pi}{\partial\boldsymbol{p}_i^W}\frac{\partial\boldsymbol{p}_i^W}{\partial\boldsymbol{x}} \]

---

對於：\(\frac{\partial\pi}{\partial p_{i}^{W}}\)
似然場是以影像形式儲存，因此必須對\(\boldsymbol{p}^W_i\)按照某種解析度進行取樣，設\(\boldsymbol{p}^W_i\)到影像座標\(\boldsymbol{p}^f_i\)的轉換關係為
​$$p_i^f=\alpha p_i^W+c$$
α 表示縮放係數，\(\boldsymbol{c}\)表示距離影像中心的偏移量。

座標系轉換為：Robot 座標系 --> World 座標系 --> 似然場座標系 那麼

\[\frac{\partial\pi}{\partial\boldsymbol{p}_i^W}=\frac{\partial\pi}{\partial\boldsymbol{p}_i^f}\frac{\partial\boldsymbol{p}_i^f}{\partial\boldsymbol{p}_i^W}=\alpha[\Delta\pi_x,\Delta\pi_y]^\top \]

其中\(\frac{\partial\pi}{\partial p_{i}^{f}}=[\Delta\pi_{x},\Delta\pi_{y}]^{\top}\)為似然場在影像上的梯度。

---

對於：\(\frac{\partial\boldsymbol{p}_i^W}{\partial\boldsymbol{x}}\)
這裡\(\boldsymbol{x}\)描述了世界座標系 W 下機器人位姿。

\[\boldsymbol{x}=[x, y, \theta]^{\rm{T}} \]

我們設機器人座標系下的某個掃描點\(\boldsymbol{p}_i^B\)的距離與角度為\(r_i\),\(\rho_i\)，那麼根據當前鐳射的位姿，可以將它轉換到世界座標系下：

\[p_i^W=[x+r_i\cos(\rho_i+\theta),y+r_i\sin(\rho_i+\theta)]^\top \]

那麼

\[\frac{\partial\boldsymbol{p}_i^W}{\partial\boldsymbol{x}}=\begin{bmatrix}1&0\\0&1\\-r_i\sin(\rho_i+\theta)&r_i\cos(\rho_i+\theta)\end{bmatrix}^T\in\mathbb{R}^{2\times3} \]

---

從而得到最終的偏導公式

\(\left.\frac{\partial\pi}{\partial\boldsymbol{x}}=\frac{\partial\pi}{\partial\boldsymbol{p}_i^W}\frac{\partial\boldsymbol{p}_i^W}{\partial\boldsymbol{x}}=\alpha\left[\begin{matrix}\Delta\pi_x,\Delta\pi_y\end{matrix}\right.\right]\begin{bmatrix}1&0&-R_i\sin(\rho_i+\theta)\\0&1&R_i\cos(\rho_i+\theta)\end{bmatrix}=[\alpha\Delta\pi_x,\alpha\Delta\pi_y,-\alpha\Delta\pi_xR_i\sin(\rho_i+\theta)+\alpha\Delta\pi_y,R_i\cos(\rho_i+\theta)]\)

在實際程式碼中，\(\Delta\pi_x\)只需要對似然場上x軸相鄰兩個點座標的值做差*0.5即可（如果為了更精確的梯度，可以多階線性差值），\(\Delta\pi_y\)同理。

---

最小二乘問題的高斯牛頓求解

高斯—牛頓迭代法的基本思想是使用泰勒級數展開式去近似地代替非線性迴歸模型，然後透過多次迭代，多次修正迴歸係數，使迴歸係數不斷逼近非線性迴歸模型的最佳迴歸係數，最後使原模型的殘差平方和達到最小。

對於一個非線性最小二乘問題：

\[x^{*} = arg \, \min\limits_{x} \frac{1}{2} || f(x) ||^2 \]

高斯牛頓的思想是把\(f(x)\)利用泰勒展開，取一階線性項近似。

\[f(x + \Delta x) = f(x) + f^\prime (x) \Delta x = f(x) +J(x) \Delta x \]

帶入到上式，得,其中\(J(x)\)代表雅可比也就是一階導：

\[\frac{1}{2} || f(x + \Delta x) ||^2 = \frac{1}{2}(f(x)^Tf(x) + 2f(x)^TJ(x) \Delta x + \Delta x^TJ(x)^TJ(x) \Delta x) \]

對上式求導，令導數為0：

\[J(x)^TJ(x) \Delta x = -J(x)^Tf(x) \]

令\(H = J^TJ \quad B = -J^Tf\)

\[H\Delta x = B \]

求解，便可以獲得調整增量\(Δx\)。這要求\(H\)可逆（正定），但實際情況並不一定滿足這個條件，因此可能發散，另外步長\(Δx\)可能太大，也會導致發散,這個也就是config中的移動限制