基于多传感器融合的组合导航方法及系统技术方案

本发明专利技术公开了基于多传感器融合的组合导航方法及系统，该方法包括：在GPS信号有效时，获取初始GPS导航信息；获取里程计的数据，并根据里程计的数据判断载体是否为静止状态；若载体为运动状态，则判断GPS信号是否有效；若GPS信号有效，使用GPS导航信息修正惯性测量元件误差并输出导航结果；若GPS信号失效，使用里程计的数据修正惯性测量元件误差并输出导航结果。本发明专利技术提供的基于多传感器融合的组合导航方法及系统，在GPS失效时，使用里程计进行补偿以抑制误差发散，从而提供准确的位置和方位信息，符合自动驾驶车辆的导航需求，在GPS失效时，保持稳定可靠的导航，提高导航精度和抗干扰能力，提高导航系统的应用范围。

【技术实现步骤摘要】
基于多传感器融合的组合导航方法及系统
本申请涉及车载导航
，尤其涉及基于多传感器融合的组合导航方法及系统。
技术介绍
近年来，自动驾驶技术不断发展。准确定位是自动驾驶汽车的重要组成部分，全球定位系统(GPS)和惯性导航系统(INS)是两种常用的导航系统。全球定位系统(GPS)具有全天候实时导航能力，具有数据输出稳定、精度高、成本低等优点，但同时存在数据输出率低，易丢失信号、无法提供导航信息等缺点。GPS定位需要接收至少四颗卫星的信号，但是当车辆行驶到道路、隧道、地下停车场等被树木或者建筑物严重遮挡的区域时，GPS无法接收到足够的卫星信号进行定位，也没有定位输出。在这种情况下，可以使用惯性导航设备进行定位计算。捷联惯性导航系统(INS)不依赖任何外界信息，具有很强的自主性。不受外界环境影响，可提供完整的载体实时导航参数。包括载体位置、速度和姿态在内的全自主导航系统具有抗干扰能力和保密性，但在导航误差累计方面存在一定的缺陷。全球定位系统(GPS)和惯性导航系统(INS)通常被集成在一起以克服惯性导航系统(INS)的误差累计问题，但是在GPS/INS进行实时的组合导航解算过程中，GPS失效时会导致系统无法正常工作或行精度降低，如果GPS长时间不能恢复，加速度计和陀螺仪的误差会随时间累计，对姿态和定位精度有极大的影响，影响导航结果。
技术实现思路
本专利技术的目的是要提供一种基于多传感器融合的组合导航方法及系统，可以解决上述现有技术问题中的一个或者多个。根据本专利技术的一个方面，提供基于多传感器融合的组合导航方法，包括以下步骤：在GPS信号有效时，获取初始GPS导航信息；获取里程计的数据，并根据里程计的数据判断载体是否为静止状态；若载体为运动状态，则判断GPS信号是否有效；若GPS信号有效，使用GPS导航信息修正惯性测量元件误差并输出导航结果；若GPS信号失效，使用里程计的数据修正惯性测量元件误差并输出导航结果。在一些实施方式中，采集加速度计和磁力计输出的数据，确定载体坐标系包括：若GPS信号有效，使用GPS导航信息修正惯性测量元件误差并输出导航结果包括：建立状态模型如下：式中，F1(t)为状态转移矩阵，状态转移矩阵计算式如下：以东北天地理坐标系为基准坐标系，确定系统状态量如下：式中，δL、δλ和δh分别代表纬度误差、经度误差和高度误差；δVx、δVy和δVz为东、北、天三个方向上的速度误差；фx、фy和фz为姿态角误差；εx、εy和εz为东北天坐标系三个轴向的陀螺仪的随机漂移；和为东北天坐标系三个轴向的加速度计随机误差；计算误差协方差：P(t,t-1)＝F(t,t-1)P(t,t-1)FT(t,t-1)+Q(t)计算卡尔曼增益矩阵：K(t)＝P(t,t-1)HT(t)[H(t)P(t,t-1)HT(t)+R(t)]-1计算t时刻状态量的估计值：x(t)＝x(t,t-1)+K(t)[z(t)-H(t)x(t,t-1)]更新误差协方差矩阵，再将其代回时间更新：P(t)＝[I-K(t)H(t)]P(t,t-1)H(t)为空间映射矩阵，H(t)表达式如下：其中，系统观测方程由惯性测量元件与GPS导航信息输出的位置差构成，观测方程如下：式中，LINS为惯性测量元件输出的纬度，LGPS为GPS输出的纬度；λINS为惯性测量元件输出的经度，λGPS为GPS输出的经度；hINS为惯性测量元件输出的高度，hGPS为GPS输出的高度；选择旋转椭球体模型中的WGS-84标准，Rm为子午圈曲率半径，Rn为卯酉圈上曲率半径；h为高度，L为纬度。在一些实施方式中，若GPS信号失效，使用里程计的数据修正惯性测量元件误差并输出导航结果包括：建立状态模型如下：式中，F1(t)为状态转移矩阵，状态转移矩阵计算式如下：以东北天地理坐标系为基准坐标系，确定系统状态量如下：式中，δL、δλ和δh分别代表纬度误差、经度误差和高度误差；δVx、δVy和δVz为东、北、天三个方向上的速度误差；фx、фy和фz为姿态角误差；εx、εy和εz为东北天坐标系三个轴向的陀螺仪的随机漂移；和为东北天坐标系三个轴向的加速度计随机误差；计算误差协方差：P(t,t-1)＝F(t,t-1)P(t,t-1)FT(t,t-1)+Q(t)计算卡尔曼增益矩阵：K(t)＝P(t,t-1)HT(t)[H(t)P(t,t-1)HT(t)+R(t)]-1计算t时刻状态量的估计值：x(t)＝x(t,t-1)+K(t)[z(t)-H(t)x(t,t-1)]更新误差协方差矩阵，再将其代回时间更新：P(t)＝[I-K(t)H(t)]P(t,t-1)H(t)为空间映射矩阵，H(t)表达式如下：其中，系统观测方程由惯性测量元件与GPS导航信息输出的位置差构成，观测方程如下：式中，n表示大地坐标系，b表示载体坐标系，为大地坐标系下惯性测量元件输出的速度，为大地坐标系下里程计输出的速度；为转换矩阵，为载体坐标系下里程计输出的速度。在一些实施方式中，以东北天地理坐标系为基准坐标系，确定系统状态量如下：具体包括：以东北天地理坐标系为基准坐标系，选择旋转椭球体模型中的WGS84标准，其中，Re为赤道平面半径，f为椭球体的扁率，Rm为子午圈曲率半径，Rn为为卯酉圈上曲率半径，δL、δλ和δh分别代表纬度误差、经度误差和高度误差；δVx、δVy和δVz为东、北、天三个方向上的速度误差；фx、фy和фz为姿态角误差；εx、εy和εz为东北天坐标系三个轴向的陀螺仪的随机漂移；和为东北天坐标系三个轴向的加速度计随机误差建立姿态误差模型如下：式中，ωie为跟随地球的旋转角速度；建立速度误差模型如下：Rm＝Re(1-2f+3fsin2L)Rn＝Re(1+fsin2L)式中，fx、fy和fz为比力；建立位置误差模型如下：建立陀螺仪误差模型如下：式中，Tgi为εgi的相关时间常数。建立加速度计误差模型如下：式中，Tai为相关时间常数。第二方面，本专利技术实施例提供了基于多传感器融合的组合导航系统，用于执行上述任一项的的基于多传感器融合的组合导航方法，该系统包括：惯性测量元件、GPS接收装置、里程计和处理器，惯性测量元件、GPS接收装置和里程计的数据输出端与处理器的输入端相连，处理器用于获取惯性测量元件、GPS接收装置和里程计输出的数据进行处理并输出最终的导航信息。在一些实施方式中，惯性测量元件包括加速本文档来自技高网...

【技术保护点】
1.基于多传感器融合的组合导航方法，其特征在于，包括以下步骤：/n在GPS信号有效时，获取初始GPS导航信息；/n获取里程计的数据，并根据所述里程计的数据判断载体是否为静止状态；/n若载体为运动状态，则判断GPS信号是否有效；/n若GPS信号有效，使用GPS导航信息修正惯性测量元件误差并输出导航结果；/n若GPS信号失效，使用里程计的数据修正惯性测量元件误差并输出导航结果。/n

【技术特征摘要】
1.基于多传感器融合的组合导航方法，其特征在于，包括以下步骤：
在GPS信号有效时，获取初始GPS导航信息；
获取里程计的数据，并根据所述里程计的数据判断载体是否为静止状态；
若载体为运动状态，则判断GPS信号是否有效；
若GPS信号有效，使用GPS导航信息修正惯性测量元件误差并输出导航结果；
若GPS信号失效，使用里程计的数据修正惯性测量元件误差并输出导航结果。


2.根据权利要求1所述的基于多传感器融合的组合导航方法，其特征在于，所述若GPS信号有效，使用GPS导航信息修正惯性测量元件误差并输出导航结果包括：
建立状态模型如下：



式中，F1(t)为状态转移矩阵，状态转移矩阵计算式如下：



以东北天地理坐标系为基准坐标系，确定系统状态量如下：



式中，δL、δλ和δh分别代表纬度误差、经度误差和高度误差；δVx、δVy和δVz为东、北、天三个方向上的速度误差；фx、фy和фz为姿态角误差；εx、εy和εz为东北天坐标系三个轴向的陀螺仪的随机漂移；和为东北天坐标系三个轴向的加速度计随机误差；
计算误差协方差：
P(t,t-1)＝F(t,t-1)P(t,t-1)FT(t,t-1)+Q(t)
计算卡尔曼增益矩阵：
K(t)＝P(t,t-1)HT(t)[H(t)P(t,t-1)HT(t)+R(t)]-1
计算t时刻状态量的估计值：
x(t)＝x(t,t-1)+K(t)[z(t)-H(t)x(t,t-1)]
更新误差协方差矩阵，再将其代回时间更新：
P(t)＝[I-K(t)H(t)]P(t,t-1)
H(t)为空间映射矩阵，H(t)表达式如下：



其中，系统观测方程由惯性测量元件与GPS导航信息输出的位置差构成，观测方程如下：



式中，LINS为惯性测量元件输出的纬度，LGPS为GPS输出的纬度；λINS为惯性测量元件输出的经度，λGPS为GPS输出的经度；hINS为惯性测量元件输出的高度，hGPS为GPS输出的高度；选择旋转椭球体模型中的WGS-84标准，Rm为子午圈曲率半径，Rn为卯酉圈上曲率半径；h为高度，L为纬度。


3.根据权利要求1所述的基于多传感器融合的组合导航方法，其特征在于，所述若GPS信号失效，使用里程计的数据修正惯性测量元件误差并输出导航结果包括：
建立状态模型如下：



式中，F1(t)为状态转移矩阵，状态转移矩阵计算式如下：



以东北天地理坐标系为基准坐标系，确定系统状态量如下：



式中，δL、δλ和δh分别代表纬度误差、经度误差和高度误差；δVx、δVy和δVz为东、北、天三个方向上的速度误差；фx、фy和фz为姿态角误差；εx、εy和εt为东北天坐标系三个轴向的陀螺仪的随机漂移；和为东北天坐标系三个轴向的加速度计随机误差；
计算误差协方差：
P(t,t-1)＝F(t,t-1)P(t,t-1)FT(t,t-1)+Q(t)
计算卡尔曼增益矩阵：
K(t)＝P(t,t-1)HT(t)[H(t)P(...

【专利技术属性】
技术研发人员：冼承钧，徐铭鸿，时广轶，王春波，吴志刚，徐开明，金玉丰，
申请(专利权)人：无锡北微传感科技有限公司，北京大学深圳研究生院，
类型：发明
国别省市：江苏;32