CN111220145A - 用于自动驾驶汽车的多采样率下导航数据的融合方法 - Google Patents

Abstract

本发明公开了一种用于自动驾驶汽车的多采样率下导航数据的融合方法，该方法首先建立组合导航系统的状态模型，然后将状态模型推广到各不同采样率下，得到各采样率下的量测值与最高采样率下的关系，最终能够在最高采样率下实现数据融合，该方法可以充分利用各个传感器的采样数据，能够在最高采样率下实现数据的融合，提高了组合导航系统的导航精度。

Description

用于自动驾驶汽车的多采样率下导航数据的融合方法

技术领域

本发明涉及导航定位技术领域，更具体的说是涉及一种用于自动驾驶汽车的多采样率下导航数据的融合方法。

背景技术

目前，应用于自动驾驶汽车组合导航系统的传感器包括惯性导航系统、卫星定位系统、视觉里程计、高精度地图以及激光雷达等。惯性导航系统具有较高的数据更新率，可以在车辆高速行驶中提供更好的导航参数迭代更新；视觉里程计通过图像处理技术不但可以提供载体的姿态信息，而且可以用来实现车辆的避障功能，频率较低，一般不超过200HZ；RTK(Real Time Kinematic)，即载波相位差分技术，其具有较高的定位精度，但其数据更新率较低，通常为1HZ，由于各个传感器各有优势，在导航系统设计时不得不将多种传感器组合使用，从而获取到更加全方位的导航数据。

但是，不同导航系统或者传感器的采样率不一致，导致了高采样率传感器的信息没有被有效利用，进而在精度上受到限制，并不能达到预期的导航效果，导航数据的可靠性得不到保障，进而影响到自动驾驶汽车的安全性能。

因此，如何提供一种精度更高的用于自动驾驶汽车的多采样率下导航数据的处理方法是本领域技术人员亟需解决的问题。

发明内容

有鉴于此，本发明提供了一种用于自动驾驶汽车的多采样率下导航数据的融合方法，该方法可以充分利用各个传感器的采样数据，能够在最高采样率下实现数据的融合，解决了现有的导航系统数据处理方法由于导航系统或者传感器的采样率不一致，高采样率传感器的信息不能被有效利用，进而在精度上受到限制，并不能达到预期的导航效果的问题。

为了实现上述目的，本发明采用如下技术方案：

本发明提供了一种用于自动驾驶汽车的多采样率下导航数据的融合方法，该方法包括：

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

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

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

具体地，所述步骤1具体包括：

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

其中，

式中，

为姿态误差角，δV_E、δV_N、δV_U为速度误差，δl、δλ、δh为位置误差，ε_E、ε_N、ε_U为陀螺零偏，

为加速度计常值漂移；

其中，

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

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

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

具体地，步骤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：根据得到的状态量间的关系，推导出其他采样率的导航传感器的状态方程和量测方程，并得到适用于导航系统中各采样率导航传感器的状态模型。

具体地，所述步骤203中适用于导航系统中各采样率导航传感器的状态模型结构为：

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

具体地，所述步骤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)。

经由上述的技术方案可知，与现有技术相比，本发明公开提供了一种用于自动驾驶汽车的多采样率下导航数据的融合方法，该方法首先建立组合导航系统的状态模型，然后将状态模型推广到各不同采样率下，得到各采样率下的量测值与最高采样率下的关系，最终能够在最高采样率下实现数据融合，该方法可以充分利用各个传感器的采样数据，能够在最高采样率下实现数据的融合，提高了组合导航系统的导航精度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1附图为本发明提供的一种用于自动驾驶汽车的多采样率下导航数据的融合方法的流程示意图；

图2附图为本发明实施例中多传感器采样数据示意图；

图3附图为本发明实施例中用于自动驾驶汽车的多采样率下导航数据的融合方法的算法流程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参见附图1，本发明实施例公开了一种用于自动驾驶汽车的多采样率下导航数据的融合方法，该方法包括：

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

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

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

具体地，结合附图3所示，上述步骤S1和步骤S2具体包括：

假设组合导航系统中有N个导航传感器，依次可以是惯性导航系统、视觉里程计/高精度地图、GPS导航系统，多传感器采样过程可参见附图2所示，其中第1个传感器采样率最高，为最细尺度，第N个传感器采样率最低，为最粗尺度，第i个传感器的采样率处于N和1之间(2≤i≤N)，设为S_i，则各传感器的采样率之间的关系满足：

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

其中，n₁＝1，根据多尺度理论，i(i＝1,2,…N)既表示传感器也表示尺度，在最高采样率下(即最细尺度)建立系统方程，可描述为：

式中，i为导航传感器的采样率，满足1≤i≤N；式中，

表示第i个传感器的第k次量测值，

表示第i个传感器的量测转移矩阵。系统噪声w(k)∈R^n×1为零均值，系统噪声w_i(k_i)和v_i(k_i)互不相关；方差为Q(k)∈R^n×n的高斯白噪声，满足

E[w(k)]＝0 (3)

E[w(k)w^T(l)]＝Q(k)δ_kl (4)

对应第i个传感器的零均值噪声，其对应的协方差矩阵为

满足

E[v_i(k)]＝0 (5)

E[v_i(k)w^T(l)]＝0 (7)

初始状态向量x(0)是一随机变量，满足：

E[x(0)]＝x₀ (8)

E{[x(0)-x₀][x(0)-x₀]^T}＝P₀ (9)

假设x(0)、w(k)、v_i(k)彼此相互独立，且不同采样率下的观测噪声相互独立。

由式(1)可得：

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

根据上述内容，可以获得各个采样率下的状态方程以及量测方程，所以可以得到如下推导：

式中，

w_i(k_i)＝[w^T(n_ik_i) w^T(n_ik_i+1)…w^T(n_ik_i+n_i-1)] (14)

则，

Q_i(k_i)＝E[w_i(k_i)w_i ^T(l_i)]＝diag{Q(n_ik_i),Q(n_ik_i+1),…,Q(n_ik_i+n_i-1)} (15)

最终可以得到基于尺度i(1≤i≤N)的状态模型，描述如下：

特别的，当i＝1时，为最细尺度上，即最高采样率下的状态模型。且各尺度上的系统噪声w_i(k_i)和v_i(k_i)互不相关。

具体地，步骤S3具体包括：

步骤301：为了能够在最细尺度上获得粗尺度的状态估计值，最终获得全局状态的最优估计，首先将Z_i(k_i)与x(k)联系起来，根据建立好的适用于导航系统中各采样率导航传感器的状态模型，构建估计模型，结构如下：

其中，

上式表示传感器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)；

下面推导式(21)，由系统方程(1)可知，

上式可以写成：

其中，k＝n_ik_i-l；l＝1,2,…,n_i-1；

又由式(29)和(30)可得

所以：

由式(26)、(27)、(32)可得式(25)成立。

根据定义式(19)有

由式(23)和式(33)可得式(21)成立。且

步骤303：根据对各导航传感器的滤波结果，实时获得最高采样率下各导航传感器的状态估计值

和估计误差协方差P_f(k|k)。

具体地，融合算法步骤如下：

1)对于任意的i＝2,3,…N，都有mod(k,n_i)≠0，则

P_f(k|k)＝P₁(k|k) (35)

2)若存在i₁,i₂,…,i_j，(1＜i₁,i₂,…,i_j≤N)，使得mod(k,n_i)＝0，则

式中，

综上所述，本发明实施例公开的用于自动驾驶汽车的多采样率下导航数据的融合方法，与现有技术相比，具有如下优点：

该方法首先建立组合导航系统的状态模型，然后将状态模型推广到各不同采样率下，得到各采样率下的量测值与最高采样率下的关系，最终能够在最高采样率下实现数据融合，该方法可以充分利用各个传感器的采样数据，能够在最高采样率下实现数据的融合，提高了组合导航系统的导航精度。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (5)

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)。

Images