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

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

Fusion method of navigation data under multi sampling rate for autopilot vehicle

【技术实现步骤摘要】
用于自动驾驶汽车的多采样率下导航数据的融合方法
本专利技术涉及导航定位
，更具体的说是涉及一种用于自动驾驶汽车的多采样率下导航数据的融合方法。
技术介绍
目前，应用于自动驾驶汽车组合导航系统的传感器包括惯性导航系统、卫星定位系统、视觉里程计、高精度地图以及激光雷达等。惯性导航系统具有较高的数据更新率，可以在车辆高速行驶中提供更好的导航参数迭代更新；视觉里程计通过图像处理技术不但可以提供载体的姿态信息，而且可以用来实现车辆的避障功能，频率较低，一般不超过200HZ；RTK(RealTimeKinematic)，即载波相位差分技术，其具有较高的定位精度，但其数据更新率较低，通常为1HZ，由于各个传感器各有优势，在导航系统设计时不得不将多种传感器组合使用，从而获取到更加全方位的导航数据。但是，不同导航系统或者传感器的采样率不一致，导致了高采样率传感器的信息没有被有效利用，进而在精度上受到限制，并不能达到预期的导航效果，导航数据的可靠性得不到保障，进而影响到自动驾驶汽车的安全性能。因此，如何提供一种精度更高的用于自动驾驶汽车的多采样率下导航数据的处理方法是本领域技术人员亟需解决的问题。
技术实现思路
有鉴于此，本专利技术提供了一种用于自动驾驶汽车的多采样率下导航数据的融合方法，该方法可以充分利用各个传感器的采样数据，能够在最高采样率下实现数据的融合，解决了现有的导航系统数据处理方法由于导航系统或者传感器的采样率不一致，高采样率传感器的信息不能被有效利用，进而在精度上受到限制，并不能达到预期的导航效果的问题。为了实现上述目的，本专利技术采用如下技术方案：本专利技术提供了一种用于自动驾驶汽车的多采样率下导航数据的融合方法，该方法包括：步骤1：以导航系统中采样率最高的导航传感器为参考，建立状态方程和量测方程；步骤2：根据各个导航传感器的采样比，建立适用于导航系统中各采样率导航传感器的状态模型；步骤3：对建立好的状态模型进行Kalman滤波，并根据滤波结果在最高采样率下进行数据融合。具体地，所述步骤1具体包括：设惯性导航系统为组合导航系统中采样率最高的导航传感器，则状态方程和量测方程为：其中，式中，为姿态误差角，δVE、δVN、δVU为速度误差，δl、δλ、δh为位置误差，εE、εN、εU为陀螺零偏，为加速度计常值漂移；其中，w(k)＝[wgx,wgy,wgz,wax,way,waz]T式中，各参量表示加速度计和陀螺的白噪声；其中，v1(k1)表示测量噪声，服从正态分布。具体地，步骤2具体包括：步骤201：设组合导航系统中有N个导航传感器，其中第1个传感器采样率最高，第N个传感器采样率最低，第i个传感器的采样率处于N和1之间(2≤i≤N)，设为Si，则各导航传感器的采样率之间的关系满足下式：Si＝S1/ni,1≤i≤N其中，n1＝1，i(i＝1,2,…N)表示导航传感器或导航传感器对应的分辨率；步骤202：根据各导航传感器的采样率之间的关系，得到各个采样率的导航传感器之间的状态量存在如下关系：xi(ki)＝x1(niki)i＝2,…N步骤203：根据得到的状态量间的关系，推导出其他采样率的导航传感器的状态方程和量测方程，并得到适用于导航系统中各采样率导航传感器的状态模型。具体地，所述步骤203中适用于导航系统中各采样率导航传感器的状态模型结构为：式中，i为导航传感器的采样率，满足1≤i≤N；系统噪声wi(ki)和vi(ki)互不相关。具体地，所述步骤3具体包括：步骤301：根据建立好的适用于导航系统中各采样率导航传感器的状态模型，构建估计模型，结构如下：其中，上式表示传感器i的第1到第ki个观测值；步骤302：对任意k＝1,2,…，设融合N个传感器获得的估计值和估计误差方差阵分别为和Pf(k|k)，则有：其中，其中，k＝niki-l；l＝1,2,…,ni-1，和Pi(ki|ki)表示利用建立好的适用于导航系统中各采样率导航传感器的状态模型对导航传感器i进行Kalman滤波的结果；并且当i＝1时，有Pi(ki|ki)＝P1|i(k|k)；步骤303：根据对各导航传感器的滤波结果，实时获得最高采样率下各导航传感器的状态估计值和估计误差协方差Pf(k|k)。经由上述的技术方案可知，与现有技术相比，本专利技术公开提供了一种用于自动驾驶汽车的多采样率下导航数据的融合方法，该方法首先建立组合导航系统的状态模型，然后将状态模型推广到各不同采样率下，得到各采样率下的量测值与最高采样率下的关系，最终能够在最高采样率下实现数据融合，该方法可以充分利用各个传感器的采样数据，能够在最高采样率下实现数据的融合，提高了组合导航系统的导航精度。附图说明为了更清楚地说明本专利技术实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本专利技术的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。图1附图为本专利技术提供的一种用于自动驾驶汽车的多采样率下导航数据的融合方法的流程示意图；图2附图为本专利技术实施例中多传感器采样数据示意图；图3附图为本专利技术实施例中用于自动驾驶汽车的多采样率下导航数据的融合方法的算法流程示意图。具体实施方式下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本专利技术一部分实施例，而不是全部的实施例。基于本专利技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本专利技术保护的范围。参见附图1，本专利技术实施例公开了一种用于自动驾驶汽车的多采样率下导航数据的融合方法，该方法包括：S1：以导航系统中采样率最高的导航传感器为参考，建立状态方程和量测方程；S2：根据各个导航传感器的采样比，建立适用于导航系统中各采样率导航传感器的状态模型；S3：对建立好的状态模型进行Kalman滤波，并根据滤波结果在最高采样率下进行数据融合。具体地，结合附图3所示，上述步骤S1和步骤S2具体包括：假设组合导航系统中有N个导航传感器，依次可以是惯性导航系统、视觉里程计/高精度地图、GPS导航系统，多传感器采样过程可参见附图2所示，其中第1个传感器采样率最高，为最细尺度，第N个传感器采样率最低，为最粗尺度，第i个传感器的采样率处于N和1之间(2≤i≤N)，设为Si，则各传感器的采样率之间的关系满足：Si＝S1/ni,1≤i≤N(1)其中，n1＝1，根据多尺度理论，i(i＝1,2,…N本文档来自技高网...

【技术保护点】
1.一种用于自动驾驶汽车的多采样率下导航数据的融合方法，其特征在于，包括：/n步骤1：以导航系统中采样率最高的导航传感器为参考，建立状态方程和量测方程；/n步骤2：根据各个导航传感器的采样比，建立适用于导航系统中各采样率导航传感器的状态模型；/n步骤3：对建立好的状态模型进行Kalman滤波，并根据滤波结果在最高采样率下进行数据融合。/n

【技术特征摘要】
1.一种用于自动驾驶汽车的多采样率下导航数据的融合方法，其特征在于，包括：
步骤1：以导航系统中采样率最高的导航传感器为参考，建立状态方程和量测方程；
步骤2：根据各个导航传感器的采样比，建立适用于导航系统中各采样率导航传感器的状态模型；
步骤3：对建立好的状态模型进行Kalman滤波，并根据滤波结果在最高采样率下进行数据融合。


2.根据权利要求1所述的一种用于自动驾驶汽车的多采样率下导航数据的融合方法，其特征在于，所述步骤1具体包括：
设惯性导航系统为组合导航系统中采样率最高的导航传感器，则状态方程和量测方程为：



其中，



式中，为姿态误差角，δVE、δVN、δVU为速度误差，δl、δλ、δh为位置误差，εE、εN、εU为陀螺零偏，▽E、▽N、▽U为加速度计常值漂移；
其中，
w(k)＝[wgx,wgy,wgz,wax,way,waz]T
式中，各参量表示加速度计和陀螺的白噪声；
其中，v1(k1)表示测量噪声，服从正态分布。


3.根据权利要求1所述的一种用于自动驾驶汽车的多采样率下导航数据的融合方法，其特征在于，步骤2具体包括：
步骤201：设组合导航系统中有N个导航传感器，其中第1个传感器采样率最高，第N个传感器采样率最低，第i个传感器的采样率处于N和1之间(2≤i≤N)，设为Si，则各导航传感器的采样率之间的关系满足下式：
Si＝S1/ni,1≤i≤N
其中，n1＝1，i(i＝1,2,…N)表示导航传感器或导航传感器对应的分辨率；
步骤202：根据各导航传感器的采样率之间的关系，得到各个采样率的导航传感器之间的状态量存在如下关系：
x...

【专利技术属性】
技术研发人员：张威，
申请(专利权)人：湖南率为控制科技有限公司，
类型：发明
国别省市：湖南;43