[发明专利]用于自动驾驶汽车的多采样率下导航数据的融合方法

权利要求书

1.一种用于自动驾驶汽车的多采样率下导航数据的融合方法，其特征在于，包括：

步骤1：以导航系统中采样率最高的导航传感器为参考，建立状态方程和量测方程；

步骤2：根据各个导航传感器的采样比，建立适用于导航系统中各采样率导航传感器的状态模型；

步骤3：对建立好的状态模型进行Kalman滤波，并根据滤波结果在最高采样率下进行数据融合。

2.根据权利要求1所述的一种用于自动驾驶汽车的多采样率下导航数据的融合方法，其特征在于，所述步骤1具体包括：

设惯性导航系统为组合导航系统中采样率最高的导航传感器，则状态方程和量测方程为：

其中，

式中，为姿态误差角，δV_E、δV_N、δV_U为速度误差，δl、δλ、δh为位置误差，ε_E、ε_N、ε_U为陀螺零偏，▽_E、▽_N、▽_U为加速度计常值漂移；

其中，

w(k)＝[w_gx,w_gy,w_gz,w_ax,w_ay,w_az]^T

式中，各参量表示加速度计和陀螺的白噪声；

其中，v₁(k₁)表示测量噪声，服从正态分布。

3.根据权利要求1所述的一种用于自动驾驶汽车的多采样率下导航数据的融合方法，其特征在于，步骤2具体包括：

步骤201：设组合导航系统中有N个导航传感器，其中第1个传感器采样率最高，第N个传感器采样率最低，第i个传感器的采样率处于N和1之间(2≤i≤N)，设为S_i，则各导航传感器的采样率之间的关系满足下式：

S_i＝S₁/n_i,1≤i≤N

其中，n₁＝1，i(i＝1,2,…N)表示导航传感器或导航传感器对应的分辨率；

步骤202：根据各导航传感器的采样率之间的关系，得到各个采样率的导航传感器之间的状态量存在如下关系：

x_i(k_i)＝x₁(n_ik_i)i＝2,…N

步骤203：根据得到的状态量间的关系，推导出其他采样率的导航传感器的状态方程和量测方程，并得到适用于导航系统中各采样率导航传感器的状态模型。

4.根据权利要求3所述的一种用于自动驾驶汽车的多采样率下导航数据的融合方法，其特征在于，所述步骤203中适用于导航系统中各采样率导航传感器的状态模型结构为：

式中，i为导航传感器的采样率，满足1≤i≤N；系统噪声w_i(k_i)和v_i(k_i)互不相关。

5.根据权利要求4所述的一种用于自动驾驶汽车的多采样率下导航数据的融合方法，其特征在于，所述步骤3具体包括：

步骤301：根据建立好的适用于导航系统中各采样率导航传感器的状态模型，构建估计模型，结构如下：

其中，

上式表示传感器i的第1到第k_i个观测值；

步骤302：对任意k＝1,2,…，设融合N个传感器获得的估计值和估计误差方差阵分别为和P_f(k|k)，则有：

其中，

其中，k＝n_ik_i-l；l＝1,2,…,n_i-1，和P_i(k_i|k_i)表示利用建立好的适用于导航系统中各采样率导航传感器的状态模型对导航传感器i进行Kalman滤波的结果；并且当i＝1时，有P_i(k_i|k_i)＝P_1|i(k|k)；

步骤303：根据对各导航传感器的滤波结果，实时获得最高采样率下各导航传感器的状态估计值和估计误差协方差P_f(k|k)。