似然场法定位检测

似然场最小二乘问题构建

机器人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中的移动限制