本文采用(GNSS+IMU+LIDAR)的组合导航方案实现车辆的导航。这里采用松耦合的导航方式,即通过对GNSS和LIDAR的测量数据进行预处理以直接获取车辆的位置,方案图如下:
1 系统模型
汽车的运动状态包括位置,速度和姿态
x
k
=
[
p
k
v
k
q
k
]
∈
R
10
\mathbf{x}_{k}=\left[\begin{array}{c} \mathbf{p}_{k} \\ \mathbf{v}_{k} \\ \mathbf{q}_{k} \end{array}\right] \in R^{10}
xk=⎣⎡pkvkqk⎦⎤∈R10
其中,四元素
q
k
\mathbf{q}_{k}
qk采用Hamilton约定。
运动模型的输入是IMU测量的比力和角速度
u
k
=
[
f
k
ω
k
]
∈
R
6
\mathbf{u}_{k}=\left[\begin{array}{c} \mathbf{f}_{k} \\ \omega_{k} \end{array}\right] \in R^{6}
uk=[fkωk]∈R6
1.1 状态运动模型
利用欧拉法进行离散化得到的运动模型如下:
p
k
=
p
k
−
1
+
Δ
t
v
k
−
1
+
Δ
t
2
2
(
C
n
s
f
k
−
1
+
g
)
v
k
=
v
k
−
1
+
Δ
t
(
C
n
s
f
k
−
1
+
g
)
q
k
=
q
k
−
1
⊗
q
(
ω
k
−
1
Δ
t
)
=
Ω
(
q
(
ω
k
−
1
Δ
t
)
)
q
k
−
1
\begin{aligned} &\mathbf{p}_{k}=\mathbf{p}_{k-1}+\Delta t \mathbf{v}_{k-1}+\frac{\Delta t^{2}}{2}\left(\mathbf{C}_{n s} \mathbf{f}_{k-1}+\mathbf{g}\right)\\ &\mathbf{v}_{k}=\mathbf{v}_{k-1}+\Delta t\left(\mathbf{C}_{n s} \mathbf{f}_{k-1}+\mathbf{g}\right)\\ &\mathbf{q}_{k}=\mathbf{q}_{k-1} \otimes \mathbf{q}\left(\omega_{k-1} \Delta t\right)=\Omega\left(\mathbf{q}\left(\omega_{k-1} \Delta t\right)\right) \mathbf{q}_{k-1} \end{aligned}
pk=pk−1+Δtvk−1+2Δt2(Cnsfk−1+g)vk=vk−1+Δt(Cnsfk−1+g)qk=qk−1⊗q(ωk−1Δt)=Ω(q(ωk−1Δt))qk−1
其中,
C
n
s
=
C
n
s
(
q
k
−
1
)
Ω
(
[
q
w
q
v
]
)
=
q
w
1
+
[
0
−
q
v
T
q
v
−
{
q
v
}
×
]
q
(
θ
)
=
[
sin
∣
θ
∣
2
θ
∣
θ
∣
cos
∣
θ
∣
2
]
\mathbf{C}_{n s}=\mathbf{C}_{n s}\left(\mathbf{q}_{k-1}\right) \quad \mathbf{\Omega}\left(\left[\begin{array}{c} q_{w} \\ \mathbf{q}_{v} \end{array}\right]\right)=q_{w} \mathbf{1}+\left[\begin{array}{cc} 0 & -\mathbf{q}_{v}^{T} \\ \mathbf{q}_{v} & -\left\{\mathbf{q}_{v}\right\}_{\times} \end{array}\right] \quad \mathbf{q}(\boldsymbol{\theta})=\left[\begin{array}{c} \sin \frac{|\boldsymbol{\theta}|}{2} \\ \frac{\boldsymbol{\theta}}{| \boldsymbol{\theta}|} \cos \frac{|\boldsymbol{\theta}|}{2} \end{array}\right]
Cns=Cns(qk−1)Ω([qwqv])=qw1+[0qv−qvT−{qv}×]q(θ)=[sin2∣θ∣∣θ∣θcos2∣θ∣]
1.2 误差状态运动模型
误差状态定义如下:
δ
x
k
=
[
δ
p
k
δ
v
k
δ
ϕ
k
]
∈
R
9
\delta \mathbf{x}_{k}=\left[\begin{array}{c} \delta \mathbf{p}_{k} \\ \delta \mathbf{v}_{k} \\ \delta \boldsymbol{\phi}_{k} \end{array}\right] \in R^{9}
δxk=⎣⎡δpkδvkδϕk⎦⎤∈R9
其中,误差轴角
δ
ϕ
k
\delta \boldsymbol{\phi}_{k}
δϕk定义在global上。
误差状态运动方程为
δ
x
k
=
F
k
−
1
δ
x
k
−
1
+
L
k
−
1
n
k
−
1
\delta \mathbf{x}_{k}=\mathbf{F}_{k-1} \delta \mathbf{x}_{k-1}+\mathbf{L}_{k-1} \mathbf{n}_{k-1}
δxk=Fk−1δxk−1+Lk−1nk−1
其中
F
k
−
1
=
[
1
1
Δ
t
0
0
1
−
[
C
n
s
f
k
−
1
]
×
Δ
t
0
0
1
]
L
k
−
1
=
[
0
0
1
0
0
1
]
\mathbf{F}_{k-1}=\left[\begin{array}{cccc} 1 & 1 \Delta t & 0 \\ 0 & 1 & -\left[\mathbf{C}_{n s} \mathbf{f}_{k-1}\right]_{\times} \Delta t \\ 0 & 0 & 1 \end{array}\right] \quad \mathbf{L}_{k-1}=\left[\begin{array}{ll} 0 & 0 \\ 1 & 0 \\ 0 & 1 \end{array}\right]
Fk−1=⎣⎡1001Δt100−[Cnsfk−1]×Δt1⎦⎤Lk−1=⎣⎡010001⎦⎤
IMU测量噪声为
n
k
∼
N
(
0
,
Q
k
)
∼
N
(
0
,
Δ
t
2
[
σ
a
c
c
2
σ
g
y
r
o
2
]
)
\begin{aligned} \mathbf{n}_{k} & \sim \mathscr{N}\left(\mathbf{0}, \mathbf{Q}_{k}\right) \\ & \sim \mathcal{N}\left(\mathbf{0}, \Delta t^{2}\left[\begin{array}{cc} \sigma_{\mathrm{acc}}^{2} & \\ & \sigma_{\mathrm{gyro}}^{2} \end{array}\right]\right) \end{aligned}
nk∼N(0,Qk)∼N(0,Δt2[σacc2σgyro2])
1.3 测量模型
在松耦合的假设下,GNSS和LIDAR 有相同的测量模型。
y
k
=
h
(
x
k
)
+
ν
k
=
H
k
x
k
+
ν
k
=
[
1
,
0
,
0
]
x
k
+
ν
k
=
p
k
+
ν
k
ν
k
∼
N
(
0
,
R
G
N
S
S
)
\begin{aligned} \mathbf{y}_{k} &=\mathbf{h}\left(\mathbf{x}_{k}\right)+\nu_{k} \\ &=\mathbf{H}_{k} \mathbf{x}_{k}+\nu_{k}=[1,0,0] \mathbf{x}_{k}+\nu_{k} \\ &=\mathbf{p}_{k}+\nu_{k} \\ \nu_{k} & \sim \mathscr{N}\left(\mathbf{0}, \mathbf{R}_{\mathrm{GNSS}}\right) \end{aligned}
ykνk=h(xk)+νk=Hkxk+νk=[1,0,0]xk+νk=pk+νk∼N(0,RGNSS)
2. 滤波方程
2.1 利用IMU数据进行预测
x ˇ k = [ p ˇ k v ˇ k q ~ k ] p ˇ k = p k − 1 + Δ t v k − 1 + Δ t 2 2 ( C n s f k − 1 + g n ) v ˇ k = v k − 1 + Δ t ( C n s f k − 1 + g n ) q ˇ k = Ω ( q ( ω k − 1 Δ t ) ) q k − 1 \check{\mathbf{x}}_{k}=\left[\begin{array}{c} \check{\mathbf{p}}_{k} \\ \check{\mathbf{v}}_{k} \\ \mathbf{\tilde { q }}_{k} \end{array}\right] \quad \begin{aligned} \check{\mathbf{p}}_{k} &=\mathbf{p}_{k-1}+\Delta t \mathbf{v}_{k-1}+\frac{\Delta t^{2}}{2}\left(\mathbf{C}_{n s} \mathbf{f}_{k-1}+\mathbf{g}_{n}\right) \\ \check{\mathbf{v}}_{k} &=\mathbf{v}_{k-1}+\Delta t\left(\mathbf{C}_{n s} \mathbf{f}_{k-1}+\mathbf{g}_{n}\right) \\ & \check{\mathbf{q}}_{k}=\mathbf{\Omega}\left(\mathbf{q}\left(\omega_{k-1} \Delta t\right)\right) \mathbf{q}_{k-1} \end{aligned} xˇk=⎣⎡pˇkvˇkq~k⎦⎤pˇkvˇk=pk−1+Δtvk−1+2Δt2(Cnsfk−1+gn)=vk−1+Δt(Cnsfk−1+gn)qˇk=Ω(q(ωk−1Δt))qk−1
2.2 协方差传播
P ˇ k = F k − 1 P k − 1 F k − 1 T + L k − 1 Q k − 1 L k − 1 T \check{\mathbf{P}}_{k}=\mathbf{F}_{k-1} \mathbf{P}_{k-1} \mathbf{F}_{k-1}^{T}+\mathbf{L}_{k-1} \mathbf{Q}_{k-1} \mathbf{L}_{k-1}^{T} Pˇk=Fk−1Pk−1Fk−1T+Lk−1Qk−1Lk−1T
2.3 量测更新
- 计算增益
K k = P ˇ k H k T ( H k P ˇ k H k T + R ) − 1 \mathbf{K}_{k}=\mathbf{\check { P }}_{k} \mathbf{H}_{k}^{T}\left(\mathbf{H}_{k} \check{\mathbf{P}}_{k} \mathbf{H}_{k}^{T}+\mathbf{R}\right)^{-1} Kk=PˇkHkT(HkPˇkHkT+R)−1 - 计算误差状态
δ x k = K k ( y k − p ˇ k ) \delta \mathbf{x}_{k}=\mathbf{K}_{k}\left(\mathbf{y}_{k}-\check{\mathbf{p}}_{k}\right) δxk=Kk(yk−pˇk) - 状态更新
p ^ k = p ˇ k + δ p k v ^ k = v ˇ k + δ v k q ^ k = q ( δ ϕ ) ⊗ q ˇ k \begin{aligned} &\hat{\mathbf{p}}_{k}=\check{\mathbf{p}}_{k}+\delta \mathbf{p}_{k}\\ &\hat{\mathbf{v}}_{k}=\check{\mathbf{v}}_{k}+\delta \mathbf{v}_{k}\\ &\hat{\mathbf{q}}_{k}=\mathbf{q}(\delta \boldsymbol{\phi}) \otimes \check{\mathbf{q}}_{k} \end{aligned} p^k=pˇk+δpkv^k=vˇk+δvkq^k=q(δϕ)⊗qˇk - 协方差更新
P ^ k = ( 1 − K k H k ) P ˇ k \hat{\mathbf{P}}_{k}=\left(\mathbf{1}-\mathbf{K}_{k} \mathbf{H}_{k}\right) \check{\mathbf{P}}_{k} P^k=(1−KkHk)Pˇk
3.程序
附上作业所要求提交的子程序,仅供参考。
3.1 测量更新
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check, q_check):
# 3.1 Compute Kalman Gain
r_cov = np.eye(3)*sensor_var
K = p_cov_check@h_jac.T@np.linalg.inv(h_jac@p_cov_check@h_jac.T+r_cov)
# 3.2 Compute error state
delta_x = K @ (y_k - p_check)
# 3.3 Correct predicted state
p_hat = p_check + delta_x[0:3]
v_hat = v_check + delta_x[3:6]
q_hat = Quaternion(axis_angle = delta_x[6:9]).quat_mult_left(q_check)
# 3.4 Compute corrected covariance
p_cov_hat = (np.eye(9)- K @ h_jac)@ p_cov_check
return p_hat, v_hat, q_hat, p_cov_hat
3.2 滤波循环
for k in range(1, imu_f.data.shape[0]): # start at 1 b/c we have initial prediction from gt
delta_t = imu_f.t[k] - imu_f.t[k - 1]
# 1. Update state with IMU inputs
C_ns = Quaternion(*q_est[k-1]).to_mat()
C_ns_dot_f_km = np.dot(C_ns,imu_f.data[k-1])
p_est[k] = p_est[k-1] + delta_t*v_est[k-1] + ((delta_t**2)/2)*(C_ns_dot_f_km+g)
v_est[k] = v_est[k-1] + delta_t*(C_ns_dot_f_km+g)
q_est[k] = Quaternion(axis_angle = delta_t*imu_w.data[k-1]).quat_mult_left(q_est[k-1])
# 1.1 Linearize the motion model and compute Jacobians
F_k_1 = np.eye(9)
F_k_1[0:3,3:6] = np.eye(3) * delta_t
F_k_1[3:6,6:9] = -skew_symmetric(C_ns_dot_f_km) * delta_t
# 2. Propagate uncertainty
Q_k_1 = np.eye(6)
Q_k_1[0:3,0:3] = Q_k_1[0:3,0:3]*var_imu_f
Q_k_1[3:6,3:6] = Q_k_1[3:6,3:6]*var_imu_w
Q_k_1 = (delta_t**2) * Q_k_1
p_cov[k] = F_k_1@p_cov[k-1]@F_k_1.T + l_jac@Q_k_1@l_jac.T
# 3. Check availability of GNSS and LIDAR measurements
if gnss_i < gnss.data.shape[0] and imu_f.t[k] >= gnss.t[gnss_i]:
p_est[k], v_est[k], q_est[k], p_cov[k] = measurement_update(var_gnss, p_cov[k], gnss.data[gnss_i], p_est[k], v_est[k], q_est[k])
gnss_i += 1
if lidar_i < lidar.data.shape[0] and imu_f.t[k] >= lidar.t[lidar_i]:
p_est[k], v_est[k], q_est[k], p_cov[k] = measurement_update(var_lidar, p_cov[k], lidar.data[lidar_i], p_est[k], v_est[k], q_est[k])
lidar_i += 1
# Update states (save)
3.3 仿真结果
个人心得,评注
- 程序用到了作业中自带的Rotate.py包,里面定义了四元数这个类,以及相关函数。
- 滤波循环中第四行
C_ns = Quaternion(*q_est[k-1]).to_mat()
记得不要丢掉
∗
*
∗号。
3. 滤波循环中判断是否有可用的GNSS/LIDAR测量数据的方法其核心思想如下:设置一个即将被使用的测量数据的序号,如果该序号对应的测量数据的时间戳在已使用的IMU测量数据的时间戳之前,则说明GNSS/LIDAR有数据可用。
4. 文中的角度误差按照global来定义。