一起做激光反光板（六）-基于滑窗的EKF-SLAM及外参自动标定公式推导

在第四篇中已经提到，如果场景中反光板不够多，容易造成EKF系统效果不好的问题，且我们还想用上其他的点云信息，保证在反光板不够的情况下仍能够正确的收敛。

我们考虑扩充观测信息：

（1）角点和线段特征，加法和反光板加法类似，因此，不再详细展开，其中线段特征的提取，我们打算采用黄国权老师实验室github上的工程；

（2）激光前后帧的ICP：我们打算使用黄国权老师实验室改进的 libPointMatcher或者ROS自带的点线ICP，做帧间ICP，能够拿到当期帧和上一帧的相对位姿及协方差矩阵；

（3）当前激光帧的绝对位姿：我们可以通过利用cartographer的CSM方法来得到，即历史帧构建submap地图，然后，当前帧和submap地图做CSM匹配，得到绝对位姿，由于cartographer的CSM方法不能得到协方差矩阵，所以，你可以手动给定协方差矩阵，或者自己去求解该协方差矩阵。求解cartographer的CSM匹配的协方差矩阵的方法，有论文已经提及过，后续考虑加上，暂时还是手动给协方差矩阵。

因此，我们的观测共包含以上三类，此外，我们统称反光板、角点和线段特征为特征。由于我们使用ICP的相对观测，所以，我们需要在状态空间内维护至少2帧的位姿，所以这种方法很类似MSCKF。我们暂时只考虑前后两帧的滑窗。

滑窗EKF公式推导

状态空间至少包含9个自由度：机器人的前后实时位姿和激光雷达坐标系到车体坐标系的转换。

状态空间

假如当前的状态空间中有$N$个新增的特征（$N>=0$），此时，状态空间为：

$X_k={\left[\begin{array}{cccccccccccccc}{x}_{k-1}& y_{k-1}& {\theta }_{k-1}& x_k& y_k& {\theta }_k& {}_l^b{x}_k& {}_l^b{y}_k& {}_l^b{\theta }_k& m_x^1& m_y^1& ...& m_x^N& m_y^N\end{array}\right]}_{9+2N}^T$

运动模型-预测

（1）位姿预测：

第一帧和第二帧位姿直接填充状态空间。

运动方程可以写为：

$X_t=A{X}_{t-1}+B$

$A={\left[\begin{array}{ccccccccccc}0& 0& 0& 1& 0& 0& 0& 0& ...& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& ...& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& ...& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& ...& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& ...& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& ...& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0& ...& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& ...& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& ...& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& ...& 0& 1\end{array}\right]}_{(2N+9)\ast (2N+9)}$

$B=A\ast {\left[\begin{array}{c}0\\ 0\\ 0\\ v_t\Delta t\cos ({\theta }_{t-1}+{\omega }_t\Delta t/2)\\ v_t\Delta t\sin ({\theta }_{t-1}+{\omega }_t\Delta t/2)\\ {\omega }_t\Delta t\\ 0\\ 0\\ 0\\ 0\\ 0\\ ...\\ 0\\ 0\end{array}\right]}_{2\ast N+9}$

需要注意的是，我们在运动过程中是需要做边缘化操作的，因此，在当前更新中$t-2$到$t-1$时刻的预测，我们不再使用轮速计模型来预测。

（2）协方差预测

状态量在$t$时刻的协方差预测方程为：${\Sigma }_t=G_t{\Sigma }_{t-1}{G}_t^T+G_u{\Sigma }_u{G}_u^T$

其中：

$G_t$是运动模型关于状态$X_{t-1}$的雅克比：

$G_t=\frac{\partial X_t}{\partial X_{t-1}}={\left[\begin{array}{ccc}{0}_{3\ast 3}& 1_{3\ast 3}& 0_{3\ast (2N+3)}\\ 0_{3\ast 3}& G_{\xi }^{{}^{\prime }}& 0_{3\ast (2N+3)}\\ 0_{(2N+3)\ast 3}& 0_{(2N+3)\ast 3}& 1_{(2N+3)\ast (2N+3)}\end{array}\right]}_{(2\ast N+9)\ast (2\ast N+9)}$

其中：$G_{\xi }^{{}^{\prime }}$和第一篇文章中的$G_{\xi }$一样。

$G_u$是运动模型关于控制（轮速计线速度和角速度）$u={\left[\begin{array}{cc}{v}_t& {\omega }_t\end{array}\right]}^T$的雅克比：

$G_u=\frac{\partial X_t}{\partial u}={\left[\begin{array}{c}{0}_{3\ast 2}\\ G_u^{{}^{\prime }}\\ 0_{(2N+3)\ast 2}\end{array}\right]}_{(2\ast N+9)\ast 2}$

其中：$G_u^{{}^{\prime }}$和第一篇文章中的$G_u$一样。

观测模型

我们的观测模型可以写为：

$h={\left[\begin{array}{cccccccccccccc}{x}_c& y_c& {\theta }_c& x_r& y_r& {\theta }_r& r_x^1& r_y^1& ...& r_x^i& r_y^i& ...& r_x^K& r_y^K\end{array}\right]}_{6+2K}^T$

其中，前三项为scan-submap得到的绝对全局位姿，第二个三项为帧间ICP得到的当前帧和上一帧的相对位姿，后面的$2K$项为特征。

特征的H矩阵求解

假设当前激光帧中检测到$K$个特征${\left[\begin{array}{cccccccc}{r}_x^1& r_y^1& ...& r_x^i& r_y^i& ...& r_x^K& r_y^K\end{array}\right]}_{2K\ast 1}$，需要分为三种情况讨论：

（1）通过数据关联（欧式距离或马氏距离等手段），我们找到老地图中对应的$k1$个特征；

（2）通过数据关联，我们找到状态空间中对应的$k2$个特征；

（3）无法关联上的特征有$k3=K-k1-k2$个。

单个特征的观测方程可以写为相同的形式：一起做激光反光板（五）

关联上老地图的特征：

和之前一样，不再细述。

关联上状态空间上的特征

和上一个公式基本一样，需要注意的是，由于匹配的是状态空间上的特征，$C_i$是不一样的。

假如匹配的是状态空间内第$j$个特征，则有：

$C_i={\left[\begin{array}{ccc}{0}_{2\ast (j-1)}& {}_W^{l}R& 0_{2\ast (N-j)}\end{array}\right]}_{2\ast 2N}$

绝对位姿的H矩阵求解

观测：$z=\left[\begin{array}{ccc}{x}_t& y_t& {\theta }_t\end{array}\right]$

$H_t^c={\left[\begin{array}{ccc}{0}_{3\ast 3}& 1_{3\ast 3}& 0_{3\ast (2N+3)}\end{array}\right]}_{2\ast (2N+9)}$

相对位姿的H矩阵求解

观测：$$z{=}_2^{1}T{=}_1^W{T}^{-1}\ast {}_2^{W}T=\left[\begin{array}{cc}{R}_1^{-1}{R}_2& R_1^{-1}(t_2-t_1)\\ 0& 1\end{array}\right]$$
$H_t^r={\left[\begin{array}{cc}{H}_{3\ast 6}^p& 0_{3\ast (2N+3)}\end{array}\right]}_{3\ast (2N+9)}$
其中：$${}_1^{W}T=\left[\begin{array}{ccc}\cos {\theta }_{t-1}& -\sin {\theta }_{t-1}& x_{t-1}\\ \sin {\theta }_{t-1}& \cos {\theta }_{t-1}& y_{t-1}\\ 0& 0& 1\end{array}\right]$$
$${}_2^{W}T=\left[\begin{array}{ccc}\cos {\theta }_t& -\sin {\theta }_t& x_t\\ \sin {\theta }_t& \cos {\theta }_t& y_t\\ 0& 0& 1\end{array}\right]$$

$H^p=\left[\begin{array}{cccccc}-\cos {\theta }_{t-1}& -\sin {\theta }_{t-1}& -(x_t-x_{t-1})\sin {\theta }_{t-1}+(y_t-y_{t-1})\cos {\theta }_{t-1}& \cos {\theta }_{t-1}& \sin {\theta }_{t-1}& 0\\ \sin {\theta }_{t-1}& -\cos {\theta }_{t-1}& -(x_t-x_{t-1})\cos {\theta }_{t-1}-(y_t-y_{t-1})\sin {\theta }_{t-1}& -\sin {\theta }_{t-1}& \cos {\theta }_{t-1}& 0\\ 0& 0& -1& 0& 0& 1\end{array}\right]$

所以，总的$H$矩阵为：

$H={\left[\begin{array}{c}{H}_t^c\\ H_t^r\\ H_1\\ H_2\\ ...\\ H_K\end{array}\right]}_{(6+2K)\ast (9+2N)}$

其余的在之前的定位EKF公式推导都有所体现，所以，不再详细说明。

这样我们就通过EKF完成了以下的EKF公式推导：

（1）在线轮速计外参标定

（2）反光板、角点、线段等特征观测

（3）绝对位姿观测

（4）相对位姿观测
另外，需要强调的是：
（1）关于FEJ（First-Estimates Jacobian）的问题，请自行搜索相关文献，简单来说包括两个部分：一是线性化点的方式不同：在计算预测的雅克比矩阵$G_u$和$G_{\xi ,t}$使用的是预测的值，而不是更新的值；二是使用第一次观测：在计算观测的雅克比矩阵$H_t$时，使用的是第一次观测值；
（2）关于栅格地图的构建，我们直接调用cartographer相关的类及函数来做；
（3）关于CSM方法的实现，同样是基于cartographer来做；
（4）畸变校正的原理比较简单，不再介绍，我们直接在代码中实现。
关于维护多帧位姿的滑窗，在本文就不再详细推导。后面就不再进行相关的公式推导，该专栏的相关内容后续会在github上更新相应的代码。