在第四篇中已经提到,如果场景中反光板不够多,容易造成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 N个新增的特征( N > = 0 N>=0 N>=0),此时,状态空间为:
X k = [ x k − 1 y k − 1 θ k − 1 x k y k θ k l b x k l b y k l b θ k m x 1 m y 1 . . . m x N m y N ] 9 + 2 N T X_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)位姿预测:
第一帧和第二帧位姿直接填充状态空间。
运动方程可以写为:
X t = A X t − 1 + B X_t = A X_{t-1} + B Xt=AXt−1+B
A = [ 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 ] ( 2 N + 9 ) ∗ ( 2 N + 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 ∗ [ 0 0 0 v t Δ t cos ( θ t − 1 + ω t Δ t / 2 ) v t Δ t sin ( θ t − 1 + ω t Δ t / 2 ) ω t Δ t 0 0 0 0 0 . . . 0 0 ] 2 ∗ N + 9 B = 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 − 2 t-2 t−2到 t − 1 t-1 t−1时刻的预测,我们不再使用轮速计模型来预测。
(2)协方差预测
状态量在 t t t时刻的协方差预测方程为: Σ t = G t Σ t − 1 G t T + G u Σ u G u T \Sigma_{t} = G_{t}\Sigma_{t-1}G_{t}^T+G_u\Sigma_uG_u^T Σt=GtΣt−1GtT+GuΣuGuT
其中:
G t G_{t} Gt是运动模型关于状态 X t − 1 X _{t-1} Xt−1的雅克比:
G t = ∂ X t ∂ X t − 1 = [ 0 3 ∗ 3 1 3 ∗ 3 0 3 ∗ ( 2 N + 3 ) 0 3 ∗ 3 G ξ ′ 0 3 ∗ ( 2 N + 3 ) 0 ( 2 N + 3 ) ∗ 3 0 ( 2 N + 3 ) ∗ 3 1 ( 2 N + 3 ) ∗ ( 2 N + 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ξ一样。
G u G_u Gu是运动模型关于控制(轮速计线速度和角速度) u = [ v t ω t ] T u = \begin{bmatrix}v_t & \omega_t\end{bmatrix}^T u=[vtωt]T的雅克比:
G u = ∂ X t ∂ u = [ 0 3 ∗ 2 G u ′ 0 ( 2 N + 3 ) ∗ 2 ] ( 2 ∗ N + 9 ) ∗ 2 G_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
其中: G u ′ G_{u}^{'} Gu′和第一篇文章中的 G u G_{u} Gu一样。
观测模型
我们的观测模型可以写为:
h = [ x c y c θ c x r y r θ r r x 1 r y 1 . . . r x i r y i . . . r x K r y K ] 6 + 2 K T h = \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得到的当前帧和上一帧的相对位姿,后面的 2 K 2K 2K项为特征。
特征的H矩阵求解
假设当前激光帧中检测到 K K K个特征 [ r x 1 r y 1 . . . r x i r y i . . . r x K r y K ] 2 K ∗ 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)通过数据关联(欧式距离或马氏距离等手段),我们找到老地图中对应的 k 1 k1 k1个特征;
(2)通过数据关联,我们找到状态空间中对应的 k 2 k2 k2个特征;
(3)无法关联上的特征有 k 3 = K − k 1 − k 2 k3=K-k1-k2 k3=K−k1−k2个。
单个特征的观测方程可以写为相同的形式:一起做激光反光板(五)
关联上老地图的特征:
和之前一样,不再细述。
关联上状态空间上的特征
和上一个公式基本一样,需要注意的是,由于匹配的是状态空间上的特征, C i C_i Ci是不一样的。
假如匹配的是状态空间内第 j j j个特征,则有:
C i = [ 0 2 ∗ ( j − 1 ) W l R 0 2 ∗ ( N − j ) ] 2 ∗ 2 N C_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 = [ x t y t θ t ] z = \begin{bmatrix} x_t & y_t&\theta_t \end{bmatrix} z=[xtytθt]
H t c = [ 0 3 ∗ 3 1 3 ∗ 3 0 3 ∗ ( 2 N + 3 ) ] 2 ∗ ( 2 N + 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
=
2
1
T
=
1
W
T
−
1
∗
2
W
T
=
[
R
1
−
1
R
2
R
1
−
1
(
t
2
−
t
1
)
0
1
]
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]
H
t
r
=
[
H
3
∗
6
p
0
3
∗
(
2
N
+
3
)
]
3
∗
(
2
N
+
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)
其中:
1
W
T
=
[
cos
θ
t
−
1
−
sin
θ
t
−
1
x
t
−
1
sin
θ
t
−
1
cos
θ
t
−
1
y
t
−
1
0
0
1
]
^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⎦⎤
2
W
T
=
[
cos
θ
t
−
sin
θ
t
x
t
sin
θ
t
cos
θ
t
y
t
0
0
1
]
^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⎦⎤
H p = [ − cos θ t − 1 − sin θ t − 1 − ( x t − x t − 1 ) sin θ t − 1 + ( y t − y t − 1 ) cos θ t − 1 cos θ t − 1 sin θ t − 1 0 sin θ t − 1 − cos θ t − 1 − ( x t − x t − 1 ) cos θ t − 1 − ( y t − y t − 1 ) sin θ t − 1 − sin θ t − 1 cos θ t − 1 0 0 0 − 1 0 0 1 ] 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⎦⎤
所以,总的 H H H矩阵为:
H = [ H t c H t r H 1 H 2 . . . H K ] ( 6 + 2 K ) ∗ ( 9 + 2 N ) 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)的问题,请自行搜索相关文献,简单来说包括两个部分:一是线性化点的方式不同:在计算预测的雅克比矩阵
G
u
G_u
Gu和
G
ξ
,
t
G_{\xi,t}
Gξ,t使用的是预测的值,而不是更新的值;二是使用第一次观测:在计算观测的雅克比矩阵
H
t
H_t
Ht时,使用的是第一次观测值;
(2)关于栅格地图的构建,我们直接调用cartographer相关的类及函数来做;
(3)关于CSM方法的实现,同样是基于cartographer来做;
(4)畸变校正的原理比较简单,不再介绍,我们直接在代码中实现。
关于维护多帧位姿的滑窗,在本文就不再详细推导。后面就不再进行相关的公式推导,该专栏的相关内容后续会在github上更新相应的代码。