車載多傳感器融合定位方案Pipeline：IMU，CNSS，LIDAR

為什麼用IMU, CNSS, LIDAR的組合

IMU可以提供連續的位置資訊，以補充GNSS可能因為地形等原因缺失的資料資訊

GNSS可以提供絕對的位置資訊，減輕IMU累積的資料漂移問題

LIDAR可以根據準确的周邊資訊，确定自己在地圖中的位置

三者可互相補充共同确定車輛的狀态和所處位置

整體流程圖

整個方案的大體思路為：将IMU資料作為輸入帶入運動學方程計算nominal state，當其他觀測資料available時，将其帶入計算車輛error state,然後兩者相加得出最終的更正狀态，用于後續疊代。當沒有觀測資料時，就将預測資料作為結果進行疊代。

采用這種方式的好處是：将最終的true state的确定分成了兩個，nominal state隻和狀态方程和輸入有關，這樣一來我們就能很友善的在方程上增加想要的限制。

另一個好處在于能很好地處理傳感器資料暫時丢失的問題。有觀測資料的時候就拿來修正狀态，沒有的時候就用預測狀态，應用起來比較flexible。當然隻用IMU時，資料漂移現象還是沒法避免，個人感覺在這種情況下，可能還需要camera輔助。

1. 同步傳感器坐标系

通常情況下，IMU和GNSS資料用的是車身坐标系，而LIDAR坐标系與傳感器on board位置相關，是以要将LIDAR資料轉換到車身坐标系，完成傳感器資料坐标系的統一

C_li = np.array([
     [ 0.9975 , -0.04742,  0.05235],
     [ 0.04992,  0.99763, -0.04742],
     [-0.04998,  0.04992,  0.9975 ]
])

t_i_li = np.array([0.5, 0.1, 0.5])

# Transform from the LIDAR frame to the vehicle (IMU) frame.
lidar.data = (C_li @ lidar.data.T).T + t_i_li

           

2. 設定預估傳感器方差和噪音常量

這是濾波器最重要的方面之一，如果出現标定錯誤或者資料丢失時，通常需要調整傳感器方差進行補償。

var_imu_f = 0.10
var_imu_w = 0.001
var_gnss  = 0.01
var_lidar = 100

g = np.array([0, 0, -9.81])  # gravity 1x3
l_jac = np.zeros([9, 6])
l_jac[3:, :] = np.eye(6)  # motion model noise jacobian
h_jac = np.zeros([3, 9])
h_jac[:, :3] = np.eye(3)  # measurement model jacobian

           

3. 設定ES-EKF初始變量值

車輛狀态包括：位置P_k(3,1), 速度V_k(3,1)，姿态q_k(4,1)

IMU資料作為Motion model 的輸入：比力f_k(3,1), 角速度(3,1)

同時分别設定Gnss/LIDAR資料索引，因為這些資料并不能100%随時獲得，和帶有各自的時間戳

p_est = np.zeros([imu_f.data.shape[0], 3])  # position estimates
v_est = np.zeros([imu_f.data.shape[0], 3])  # velocity estimates
q_est = np.zeros([imu_f.data.shape[0], 4])  # orientation estimates as quaternions
p_cov = np.zeros([imu_f.data.shape[0], 9, 9])  # covariance matrices at each timestep

# Set initial values.
p_est[0] = gt.p[0]  #p_est.shape (10918, 3)
v_est[0] = gt.v[0]
q_est[0] = Quaternion(euler=gt.r[0]).to_numpy()
p_cov[0] = np.zeros(9)  # covariance of estimate
gnss_i  = 0  #index of gnss/lidar data
lidar_i = 0

           

4. 根據IMU資料預估車輛狀态-prediction

IMU資料作為輸入，根據motion model 預測車輛狀态

# 1. Update state with IMU inputs
    #orientation of THE sensor why*? Cns
    rotation_matrix = Quaternion(*q_est[k - 1]).to_mat()  #.shape = 3x3

    # 1.1 Linearize the motion model and compute Jacobians  3x1; 3x1, 4x1
    p_est[k] = p_est[k-1] + delta_t*v_est[k-1] + (delta_t**2/2)*(rotation_matrix.dot(imu_f.data[k-1])+g)
    v_est[k] = v_est[k-1] + delta_t*(rotation_matrix.dot(imu_f.data[k-1])+g)
    q_est[k] = Quaternion(axis_angle=imu_w.data[k-1]*delta_t).quat_mult_right(q_est[k-1])
    #predicted data used when external data is not available
    
           

同時需要計算雅可比矩陣

雅可比矩陣的含義及表達：

計算結果

F = np.identity(9)
    #given in C2M5L2
    F[:3,3:6] = delta_t*np.identity(3)
    F[3:6,6:] = -(rotation_matrix.dot(skew_symmetric(imu_f.data[k-1].reshape((3,1)))))

    #measurement noise given in C2M5L2
    Q = np.identity(6)
    Q[:,:3] *= (delta_t**2)*var_imu_f
    Q[:,-3:] *= (delta_t**2)*var_imu_w
    # 2. Propagate uncertainty
    p_cov[k] = F.dot(p_cov[k-1]).dot(F.T) + l_jac.dot(Q).dot(l_jac.T)

           

在其他傳感器資料non available的情況下，在這裡計算出來的predicted state就是下一輪更新用的true state

5. 檢測GNSS或LIDAR資料可用性

當與IMU資料時間戳對應的LIDAR/GNSS資料存在時，對車輛狀态進行更新。

# 3. Check availability of GNSS and LIDAR measurements : only part value get corrected
    #don't forget to check for time consistance
    if lidar_i<lidar.t.shape[0] and lidar.t[lidar_i] == imu_f.t[k-1]:
        #y_k need to be a 3x1 vector, lidar.data[lidar_i].shape (,3)
        y_k = lidar.data[lidar_i].T
        p_est[k], v_est[k], q_est[k], p_cov[k] = measurement_update(var_lidar,p_cov[k],y_k,p_est[k],v_est[k],q_est[k])
        lidar_i += 1

    if gnss_i<gnss.t.shape[0] and gnss.t[gnss_i] == imu_f.t[k-1]:
        y_k = gnss.data[gnss_i].T
        p_est[k], v_est[k], q_est[k], p_cov[k] = measurement_update(var_gnss, p_cov[k], y_k, p_est[k], v_est[k], q_est[k])
        gnss_i += 1

           

6. 利用GNSS/LIDAR的測量資料對車輛狀态更正-correction

計算卡爾曼增益和error state,然後更正預測狀态(predicted state)，最後更新協方差

# 3.1 Compute Kalman Gain
    #R need to be 3x3 : h_jac.dot(p_cov_check).dot(h_jac.T) 3x9 9x9 9x3 shape = 3x3
    I = np.identity(3)
    R = I*sensor_var

    #K.shape = 9x9 9x3 3x3 = 9x3
    K = p_cov_check.dot(h_jac.T).dot(np.linalg.inv(h_jac.dot(p_cov_check).dot(h_jac.T)+R))

    # 3.2 Compute error state
    #error.shape = 9x3 (3x1-3x1) = 9x1
    error = K.dot(y_k - p_check) #shape(9,)

    p_del = error[:3]
    v_del = error[3:6]
    phi_del = error[6:]

    # 3.3 Correct predicted state
    p_hat = p_check + p_del
    v_hat = v_check + v_del
    q_hat = Quaternion(euler=phi_del).quat_mult_right(q_check) #4x1

    # 3.4 Compute corrected covariance
    p_cov_hat = (np.identity(9) - K.dot(h_jac)).dot(p_cov_check)

    #Output: p_hat 3x1; v_hat 3x1; q_hat 4x1; p_cov_hat 9x9
    return p_hat, v_hat, q_hat, p_cov_hat

           

7. 結果

從效果來看位置誤差在5之間，姿态誤差0.5左右，較好的預測了車輛狀态。