State Estimation and Localization for Self-Driving Cars 第五周作业 组合导航

  本文采用(GNSS+IMU+LIDAR)的组合导航方案实现车辆的导航。这里采用松耦合的导航方式，即通过对GNSS和LIDAR的测量数据进行预处理以直接获取车辆的位置，方案图如下：

1 系统模型

  汽车的运动状态包括位置，速度和姿态
$${\mathbf{x}}_k=\left[\begin{array}{c}{\mathbf{p}}_k\\ {\mathbf{v}}_k\\ {\mathbf{q}}_k\end{array}\right]\in R^{10}$$
其中，四元素${\mathbf{q}}_k$采用Hamilton约定。
  运动模型的输入是IMU测量的比力和角速度
$${\mathbf{u}}_k=\left[\begin{array}{c}{\mathbf{f}}_k\\ {\omega }_k\end{array}\right]\in R^6$$

1.1 状态运动模型

  利用欧拉法进行离散化得到的运动模型如下：
$$\begin{array}{rl}& {\mathbf{p}}_k={\mathbf{p}}_{k-1}+\Delta t{\mathbf{v}}_{k-1}+\frac{\Delta t^2}{2}\left({\mathbf{C}}_{ns}{\mathbf{f}}_{k-1}+\mathbf{g}\right)\\ & {\mathbf{v}}_k={\mathbf{v}}_{k-1}+\Delta t\left({\mathbf{C}}_{ns}{\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{array}$$

其中，
Cns=Cns(qk−1)Ω([qwqv])=qw1+[0−qvTqv−{qv}×]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​)Ω([qw​qv​​])=qw​1+[0qv​​−qvT​−{qv​}×​​]q(θ)=[sin2∣θ∣​∣θ∣θ​cos2∣θ∣​​]

1.2 误差状态运动模型

  误差状态定义如下：
$$\delta {\mathbf{x}}_k=\left[\begin{array}{c}\delta {\mathbf{p}}_k\\ \delta {\mathbf{v}}_k\\ \delta {\mathbf{\varphi }}_k\end{array}\right]\in R^9$$
其中，误差轴角$\delta {\mathbf{\varphi }}_k$定义在global上。
  误差状态运动方程为
$$\delta {\mathbf{x}}_k={\mathbf{F}}_{k-1}\delta {\mathbf{x}}_{k-1}+{\mathbf{L}}_{k-1}{\mathbf{n}}_{k-1}$$
其中
$${\mathbf{F}}_{k-1}=\left[\begin{array}{ccc}1& 1\Delta t& 0\\ 0& 1& -{\left[{\mathbf{C}}_{ns}{\mathbf{f}}_{k-1}\right]}_{\times }\Delta t\\ 0& 0& 1\end{array}\right]{\mathbf{L}}_{k-1}=\left[\begin{array}{ll}0& 0\\ 1& 0\\ 0& 1\end{array}\right]$$
  IMU测量噪声为
$$\begin{array}{rl}{\mathbf{n}}_k& \sim \mathcal{N}\left(0,{\mathbf{Q}}_k\right)\\ & \sim \mathcal{N}\left(0,\Delta t^2\left[\begin{array}{cc}{\sigma }_{\mathrm{a}\mathrm{c}\mathrm{c}}^2& \\ & {\sigma }_{\mathrm{g}\mathrm{y}\mathrm{r}\mathrm{o}}^2\end{array}\right]\right)\end{array}$$

1.3 测量模型

在松耦合的假设下，GNSS和LIDAR 有相同的测量模型。
$$\begin{array}{rl}{\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 \mathcal{N}\left(0,{\mathbf{R}}_{\mathrm{G}\mathrm{N}\mathrm{S}\mathrm{S}}\right)\end{array}$$

2. 滤波方程

2.1 利用IMU数据进行预测

$${\check{\mathbf{x}}}_k=\left[\begin{array}{c}{\check{\mathbf{p}}}_k\\ {\check{\mathbf{v}}}_k\\ {\tilde{\mathbf{q}}}_k\end{array}\right]\begin{array}{rl}{\check{\mathbf{p}}}_k& ={\mathbf{p}}_{k-1}+\Delta t{\mathbf{v}}_{k-1}+\frac{\Delta t^2}{2}\left({\mathbf{C}}_{ns}{\mathbf{f}}_{k-1}+{\mathbf{g}}_n\right)\\ {\check{\mathbf{v}}}_k& ={\mathbf{v}}_{k-1}+\Delta t\left({\mathbf{C}}_{ns}{\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{array}$$

2.2 协方差传播

$${\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$$

2.3 量测更新

1. 计算增益
   $${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\check{\mathbf{P}}}_k{\mathbf{H}}_k^T+\mathbf{R}\right)}^{-1}$$
2. 计算误差状态
   $$\delta {\mathbf{x}}_k={\mathbf{K}}_k\left({\mathbf{y}}_k-{\check{\mathbf{p}}}_k\right)$$
3. 状态更新
   $$\begin{array}{rl}& {\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 \mathbf{\varphi })\otimes {\check{\mathbf{q}}}_k\end{array}$$
4. 协方差更新
   $${\hat{\mathbf{P}}}_k=\left(1-{\mathbf{K}}_k{\mathbf{H}}_k\right){\check{\mathbf{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 仿真结果

个人心得，评注

1. 程序用到了作业中自带的Rotate.py包，里面定义了四元数这个类，以及相关函数。
2. 滤波循环中第四行

C_ns = Quaternion(*q_est[k-1]).to_mat()

记得不要丢掉$\ast$号。
3. 滤波循环中判断是否有可用的GNSS/LIDAR测量数据的方法其核心思想如下：设置一个即将被使用的测量数据的序号，如果该序号对应的测量数据的时间戳在已使用的IMU测量数据的时间戳之前，则说明GNSS/LIDAR有数据可用。
4. 文中的角度误差按照global来定义。