一起做激光反光板（六）-基于滑窗的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个自由度：机器人的前后实时位姿和激光雷达坐标系到车体坐标系的转换。

状态空间

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

Xk=[xk−1yk−1θk−1xkykθklbxklbyklbθkmx1my1...mxNmyN]9+2NTX_k=\begin{bmatrix}x_{k-1} & y_{k-1} &\theta_{k-1} & x_k & y_k &\theta_k &^b_lx_k&^b_ly_k&^b_l\theta_k & m_x^1 & m_y^1 &...& m_x^N & m_y^N\end{bmatrix}^T_{9+2N}Xk=[xk−1yk−1θk−1xkykθklbxklbyklbθkmx1my1...mxNmyN]9+2NT

运动模型-预测

（1）位姿预测：

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

运动方程可以写为：

Xt=AXt−1+BX_t = A X_{t-1} + BXt=AXt−1+B

A=[00010000...0000001000...0000000100...0000010000...0000001000...0000000100...0000000010...0000000001...0000000000...1000000000...01](2N+9)∗(2N+9)A = \begin{bmatrix}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{bmatrix}_{(2N+9)*(2N+9)}A=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡00000000000000000000000000000010010000000100100000001001000000000010000000000100..............................00000000100000000001⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤(2N+9)∗(2N+9)

B=A∗[000vtΔtcos⁡(θt−1+ωtΔt/2)vtΔtsin⁡(θt−1+ωtΔt/2)ωtΔt00000...00]2∗N+9B = A * \begin{bmatrix}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{bmatrix}_{2*N+9}B=A∗⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡000vtΔtcos(θt−1+ωtΔt/2)vtΔtsin(θt−1+ωtΔt/2)ωtΔt00000...00⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤2∗N+9

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

（2）协方差预测

状态量在ttt时刻的协方差预测方程为：Σt=GtΣt−1GtT+GuΣuGuT\Sigma_{t} = G_{t}\Sigma_{t-1}G_{t}^T+G_u\Sigma_uG_u^TΣt=GtΣt−1GtT+GuΣuGuT

其中：

GtG_{t}Gt是运动模型关于状态Xt−1X _{t-1}Xt−1的雅克比：

Gt=∂Xt∂Xt−1=[03∗313∗303∗(2N+3)03∗3Gξ′03∗(2N+3)0(2N+3)∗30(2N+3)∗31(2N+3)∗(2N+3)](2∗N+9)∗(2∗N+9)G_{t} = \frac{\partial X_t}{\partial X_{t-1}}=\begin{bmatrix}0_{3*3} & 1_{3*3} & 0_{3*(2N+3)} \\ 0_{3*3} & G_\xi^{'} & 0_{3*(2N+3)} \\0_{(2N+3)*3} & 0_{(2N+3)*3}& 1_{(2N+3) * (2N+3)} \end{bmatrix}_{(2*N+9)*(2*N+9)}Gt=∂Xt−1∂Xt=⎣⎡03∗303∗30(2N+3)∗313∗3Gξ′0(2N+3)∗303∗(2N+3)03∗(2N+3)1(2N+3)∗(2N+3)⎦⎤(2∗N+9)∗(2∗N+9)

其中：Gξ′G_{\xi}^{'}Gξ′和第一篇文章中的GξG_{\xi}Gξ一样。

GuG_uGu是运动模型关于控制（轮速计线速度和角速度）u=[vtωt]Tu = \begin{bmatrix}v_t & \omega_t\end{bmatrix}^Tu=[vtωt]T的雅克比：

Gu=∂Xt∂u=[03∗2Gu′0(2N+3)∗2](2∗N+9)∗2G_u = \frac{\partial X_t}{\partial u}=\begin{bmatrix}0_{3*2} \\ G_u^{'} \\0_{(2N+3)*2} \end{bmatrix}_{(2*N+9)*2}Gu=∂u∂Xt=⎣⎡03∗2Gu′0(2N+3)∗2⎦⎤(2∗N+9)∗2

其中：Gu′G_{u}^{'}Gu′和第一篇文章中的GuG_{u}Gu一样。

观测模型

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

h=[xcycθcxryrθrrx1ry1...rxiryi...rxKryK]6+2KTh = \begin{bmatrix} 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{bmatrix}^T_{6+2K}h=[xcycθcxryrθrrx1ry1...rxiryi...rxKryK]6+2KT

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

特征的H矩阵求解

假设当前激光帧中检测到KKK个特征[rx1ry1...rxiryi...rxKryK]2K∗1\begin{bmatrix}r_x^1 & r_y^1 & ...&r_x^i& r_y^i&... &r_x^K & r_y^K\end{bmatrix}_{2K*1}[rx1ry1...rxiryi...rxKryK]2K∗1，需要分为三种情况讨论：

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

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

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

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

关联上老地图的特征：

和之前一样，不再细述。

关联上状态空间上的特征

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

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

Ci=[02∗(j−1)WlR02∗(N−j)]2∗2NC_i = \begin{bmatrix} 0_{2*{(j-1)}}& ^l_WR & 0_{2 * (N-j)}\end{bmatrix}_{2*2N}Ci=[02∗(j−1)WlR02∗(N−j)]2∗2N

绝对位姿的H矩阵求解

观测：z=[xtytθt]z = \begin{bmatrix} x_t & y_t&\theta_t \end{bmatrix}z=[xtytθt]

Htc=[03∗313∗303∗(2N+3)]2∗(2N+9)H_t^c = \begin{bmatrix} 0_{3*3} & 1_{3*3} &0_{3*(2N+3)}\end{bmatrix}_{2*(2N+9)}Htc=[03∗313∗303∗(2N+3)]2∗(2N+9)

相对位姿的H矩阵求解

观测：z=21T=1WT−1∗2WT=[R1−1R2R1−1(t2−t1)01]z = ^1_2T = ^W_1T^{-1} * {}^W_2T =\begin{bmatrix} R_1^{-1}R_2 & R_1^{-1} (t_2-t_1) \\ 0 & 1\end{bmatrix} z=21T=1WT−1∗2WT=[R1−1R20R1−1(t2−t1)1]
Htr=[H3∗6p03∗(2N+3)]3∗(2N+9)H_t^r = \begin{bmatrix}H_{3*6}^p & 0_{3*(2N+3)}\end{bmatrix}_{3*(2N+9)}Htr=[H3∗6p03∗(2N+3)]3∗(2N+9)
其中：1WT=[cos⁡θt−1−sin⁡θt−1xt−1sin⁡θt−1cos⁡θt−1yt−1001]^W_1T = \begin{bmatrix} \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{bmatrix}1WT=⎣⎡cosθt−1sinθt−10−sinθt−1cosθt−10xt−1yt−11⎦⎤
2WT=[cos⁡θt−sin⁡θtxtsin⁡θtcos⁡θtyt001]^W_2T = \begin{bmatrix} \cos\theta_{t} & -\sin\theta_{t} & x_{t} \\ \sin\theta_{t} & \cos\theta_{t} & y_{t} \\ 0&0&1 \end{bmatrix}2WT=⎣⎡cosθtsinθt0−sinθtcosθt0xtyt1⎦⎤

Hp=[−cos⁡θt−1−sin⁡θt−1−(xt−xt−1)sin⁡θt−1+(yt−yt−1)cos⁡θt−1cos⁡θt−1sin⁡θt−10sin⁡θt−1−cos⁡θt−1−(xt−xt−1)cos⁡θt−1−(yt−yt−1)sin⁡θt−1−sin⁡θt−1cos⁡θt−1000−1001]H^p =\begin{bmatrix}-\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{bmatrix}Hp=⎣⎡−cosθt−1sinθt−10−sinθt−1−cosθt−10−(xt−xt−1)sinθt−1+(yt−yt−1)cosθt−1−(xt−xt−1)cosθt−1−(yt−yt−1)sinθt−1−1cosθt−1−sinθt−10sinθt−1cosθt−10001⎦⎤

所以，总的HHH矩阵为：

H=[HtcHtrH1H2...HK](6+2K)∗(9+2N)H = \begin{bmatrix} H_t^c \\ H_t^r \\ H_1 \\ H_2 \\ ...\\ H_K \end{bmatrix}_{(6+2K) * (9+2N)}H=⎣⎢⎢⎢⎢⎢⎢⎡HtcHtrH1H2...HK⎦⎥⎥⎥⎥⎥⎥⎤(6+2K)∗(9+2N)

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

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

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

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

（3）绝对位姿观测

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