基于似然场的全局定位-Toy模板网

这篇具有很好参考价值的文章主要介绍了基于似然场的全局定位。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

似然场法定位检测

似然场最小二乘问题构建

机器人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$同理。