为什么用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