基于多传感器融合的组合导航方法及系统与流程

本申请涉及车载导航技术领域，尤其涉及基于多传感器融合的组合导航方法及系统。

背景技术：

近年来，自动驾驶技术不断发展。准确定位是自动驾驶汽车的重要组成部分，全球定位系统(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)f^t(t,t-1)+q(t)

计算卡尔曼增益矩阵：

k(t)＝p(t,t-1)h^t(t)[h(t)p(t,t-1)h^t(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)f^t(t,t-1)+q(t)

计算卡尔曼增益矩阵：

k(t)＝p(t,t-1)h^t(t)[h(t)p(t,t-1)h^t(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+3fsin²l)

rn＝re(1+fsin²l)

式中，fx、fy和fz为比力；

建立位置误差模型如下：

建立陀螺仪误差模型如下：

式中，tgi为εgi的相关时间常数。

建立加速度计误差模型如下：

式中，tai为相关时间常数。

第二方面，本发明实施例提供了基于多传感器融合的组合导航系统，用于执行上述任一项的的基于多传感器融合的组合导航方法，该系统包括：惯性测量元件、gps接收装置、里程计和处理器，惯性测量元件、gps接收装置和里程计的数据输出端与处理器的输入端相连，处理器用于获取惯性测量元件、gps接收装置和里程计输出的数据进行处理并输出最终的导航信息。

在一些实施方式中，惯性测量元件包括加速度计和陀螺仪，加速度计和陀螺仪的输出端均与处理器的输入端相连。

在一些实施方式中，处理器包括滤波器。

在一些实施方式中，输出最终的导航信息包括载体位置信息，载体速度信息和载体姿态信息。

本申请技术方案与现有技术相比，具有以下有益效果：

本发明提供的导航方法，在gps有效时，使用gps导航信息修正惯性测量元件误差，在gps失效时，使用里程计进行补偿以抑制误差发散，从而提供准确的位置和方位信息，符合自动驾驶车辆的导航需求，在gps失效时，可以保持稳定可靠的导航，有效地提高导航精度和抗干扰能力，提高导航系统的应用范围。

另外，在本发明技术方案中，凡未作特别说明的，均可通过采用本领域中的常规手段来实现本技术方案。

附图说明

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

图1是本申请一实施例提供的基于多传感器融合的组合导航方法的流程图。

图2是本申请另一实施例提供的基于多传感器融合的组合导航系统的结构示意图。

图3是直线路段实验结果比较图。

图4是地形复杂的直线路段实验结果比较图。

图5是s型桥梁实验结果比较图。

具体实施方式

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

实施例1：

如图1所示为本发明一实施例提供的基于多传感器融合的组合导航方法，包括以下步骤：

s11：在gps信号有效时，获取初始gps导航信息；

s12：获取里程计的数据，并根据里程计的数据判断载体是否为静止状态；

s13：若载体为运动状态，则判断gps信号是否有效；

s14：若gps信号有效，使用gps导航信息修正惯性测量元件误差并输出导航结果；

s15：若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)f^t(t,t-1)+q(t)

计算卡尔曼增益矩阵：

k(t)＝p(t,t-1)h^t(t)[h(t)p(t,t-1)h^t(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输出的纬度；hins为惯性测量元件输出的经度，λ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)f^t(t,t-1)+q(t)

计算卡尔曼增益矩阵：

k(t)＝p(t,t-1)h^t(t)[h(t)p(t,t-1)h^t(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+3fsin²l)

rn＝re(1+fsin²l)

式中，fx、fy和fz为比力；

建立位置误差模型如下：

建立陀螺仪误差模型如下：

式中，tgi为εgi的相关时间常数。

建立加速度计误差模型如下：

式中，tai为相关时间常数。

在可选的实施例中，f1(t)为15*15的状态转移矩阵，矩阵中部分元素表达式如下：

f(1,5)＝-fz

f(1,6)＝fy

f(2,4)＝fz

f(2,6)＝-fx

f(3,4)＝-fy

f(3,5)＝fx

f(3,7)＝-2ωievxsinl

f(5,7)＝-ωiesinl

f(9,3)＝1

矩阵中的其他元素均为0。

本发明提供的导航方法，在gps有效时，使用gps导航信息修正惯性测量元件误差，在gps失效时，使用里程计进行补偿以抑制误差发散，从而提供准确的位置和方位信息，符合自动驾驶车辆的导航需求，在gps失效时，可以保持稳定可靠的导航，有效地提高导航精度和抗干扰能力，提高导航系统的应用范围。

实施例2：

如图2所示为本发明实施例2提供的基于多传感器融合的组合导航系统，用于执行上述方法实施例中任一基于多传感器融合的组合导航方法，包括惯性测量元件1、gps接收装置2、里程计3和处理器4，惯性测量元件1、gps接收装置2和里程计3的数据输出端与处理器4的输入端相连，处理器4用于获取惯性测量元件1、gps接收装置2和里程计3输出的数据进行处理并输出最终的导航信息。

在一些实施方式中，惯性测量元件1包括加速度计和陀螺仪，加速度计和陀螺仪的输出端均与处理器4的输入端相连。

在一些实施方式中，处理器4包括滤波器。

在一些实施方式中，输出最终的导航信息包括载体位置信息，载体速度信息和载体姿态信息。

上述实施例2的基于多传感器融合的组合导航系统可用于执行本发明实施例1的基于多传感器融合的组合导航方法，并相应的达到上述本发明实施例1的基于多传感器融合的组合导航方法所达到的技术效果，这里不再赘述。

实施例3：

本次实验载体采用福斯的休旅车，惯性测量元件选用高精度动态倾角传感器bw-vg500，其包括三轴加速度计和三轴陀螺仪，里程计为汽车自带里程表，并且选用了和芯星通的gps。惯性测量元件的采样频率为20hz，里程计的频率为20hz。

实验分为三个运动场景，分别为(1)直线路段，总距离约0.35km；(2)地形复杂的直线路段(含上坡与下坡)，总距离约为1.7km；(3)s型桥梁(含弯道)，总距离约为2.8km。

在每个运动场景中，分别设置了三种情况，第一种使用惯性导航系统(惯性测量元件)输出导航结果，第二种使用惯性导航系统(惯性测量元件)和里程计结合输出导航结果，第三种使用惯性导航系统(惯性测量元件)、gps和里程计融合输出导航结果。其中，第一种情况模拟gps失效时，gps与惯性导航系统(惯性测量元件)的组合导航方法输出的导航结果。

图3至图5分别为三个不同运动场景下的实验结果，每个运动场景中分别有三条曲线代表三种不同导航方法的距离误差，其中黑色实线代表第一种导航方法，虚线代表第二种导航方法，灰色实线代表第三种导航方法。

从实验结果中可以看出，当gps失效并且无里程计辅助补偿时，惯性导航系统(惯性测量元件)的误差会累计，并且随着道路情况复杂程度的增加，误差会增大。

当gps失效但是有里程计辅助补偿时，惯性导航系统(惯性测量元件)的误差会被校正，从而准确定位，提高导航精度。

实验结果证明了，在gps失效时，使用里程计进行补偿以抑制误差发散，从而提供准确的位置和方位信息，符合自动驾驶车辆的导航需求，在gps失效时，可以保持稳定可靠的导航，有效地提高导航精度和抗干扰能力，提高导航系统的应用范围。

以上所描述的装置实施例仅仅是示意性的，其中作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如rom/ram、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。