CN111721290B - 一种多源传感器信息融合定位切换方法 - Google Patents

Abstract

本发明公开了一种多源传感器融合定位切换方法，包括：利用GNSS接收机获取初始位姿信息；当无法使用GNSS卫星信号时，利用SINS/里程计融合导航，并利用组合导航***动态误差模型对SINS/里程计融合导航结果进行修正；当GNSS卫星信号恢复后，将融合导航输出预切换至GNSS/SINS松耦合输出，并将GNSS卫星信号输出与松耦合输出进行加权平均，获得融合定位结果，按设定值减小松耦合输出权值，直至完全切换至GNSS卫星信号输出。本发明能够避有效利用多源传感器融合信息，避免切换过程中出现跳变，保证融合定位输出在不同的场景下的平稳过渡。

Description

一种多源传感器信息融合定位切换方法

技术领域

本发明属于多源传感器信息融合定位领域，具体为一种多源传感器融合定位切换方法。

背景技术

目前，卫星导航定位***在室外定位领域获得了广泛的应用，但受建筑物、树木和地形的影响，还存在一些存在无法接收足够卫星星历数据进行定位的区域。当运动体运动到此区域内时，需要使用非卫星导航***进行补充，如惯性导航***。当运动体从卫星信号良好区域进入无卫星信号区域时，需要将导航定位信号从卫星导航***切换至非卫星导航***；当运动体从无卫星导航区域进入卫星信号良好区域时，需要将导航定位信号从非卫星导航***切换至卫星导航***。传统的切换方法采用硬切换技术。由于非卫星导航技术通常基于积分的，因此会存在累计误差，当从卫星导航***切换至非卫星导航***时，卫星导航***最后一次输出作为非卫星导航***的初值，因此不会产生位置信号的突变，但当从非卫星导航***切换至卫星导航***时，由于有累积误差的存在且卫星导航***的输出不依赖于非卫星导航***的输出，此时位置信号会产生突变，该突变会对采用此位置信号作为反馈信号的控制***造成不利影响，使得控制性能恶化。

发明内容

本发明的目的在于提供一种多源传感器融合定位切换方法，能够有效利用多传感器量测信息减小多传感器信息切换误差。

实现本发明目的的技术解决方案为：一种多源传感器融合定位切换方法，包括：

利用GNSS接收机获取初始位姿信息；

当无法使用GNSS卫星信号时，利用SINS/里程计融合导航，并利用组合导航***动态误差模型对SINS/里程计融合导航结果进行修正；

当GNSS卫星信号恢复后，将融合导航输出预切换至GNSS/SINS松耦合输出，并将GNSS卫星信号输出与松耦合输出进行加权平均，获得融合定位结果，按设定值减小松耦合输出权值，直至完全切换至GNSS卫星信号输出。

优选地，利用组合导航***动态误差模型对SINS/里程计融合导航结果进行修正的具体过程为：

建立SINS动态误差模型，具体为：

其中，X_INS表示惯导***动态误差模型的状态向量，δP表示位置误差，δVⁿ表示速度误差，/>表示失准角，δε_g表示陀螺零偏误差，/>表示加速度计零偏误差，F_INS表示捷联惯导动态误差矩阵，W_INS表示捷联惯导动态误差模型的***噪声；

建立里程计动态误差模型，具体为：

其中，X_OD＝[δα δβ δτ]^T，X_OD表示里程计动态误差模型的状态向量，δα表示航向安装夹角误差，δβ表示俯仰安装夹角误差，δτ表示里程计刻度因子误差，W_OD表示里程计动态误差模型的***噪声；F_OD表示里程计动态误差矩阵；

综合惯性导航***动态误差模型和里程计动态误差模型，构成如下组合导航***动态误差模型：

其中，0_3×15、0_15×3表示零矩阵；

采用组合导航***动态误差模型对SINS/里程计输出位置进行校正，利用误差估计结果对K时刻SINS的位置输出速度输出/>和姿态矩阵输出C_k进行校正，同时对陀螺仪零偏ε_g、加速度计零偏/>航向安装夹角α、俯仰安装夹角β和里程计刻度因子τ进行更新。

优选地，获得GNSS/SINS松耦合输出的具体方法为：

根据GNSS测量协方差信息，建立GNSS状态方程与量测方程；

根据GNSS与SINS分别测算得到的位置、速度误差量测方程解算松耦合量测方程，得到松耦合输出。

优选地，根据GNSS与SINS分别测算得到的位置、速度误差量测方程解算松耦合量测方程的具体过程为：

载体真实位置为(x,y,z)、速度为(v_x,v_y,v_z)，SINS解算得到的位置、速度分别为(x₁,y₁,z₁)，GNSS得到的位置、速度分别为(x_G,y_G,z_G)、(v_xG,v_yG,v_zG)，其中：

SINS解算得到的位置具体为：

式中，x₁、y₁、z₁分别是捷联惯导***输出位置，δx、δy、δz沿地理坐标系三个轴上的位置误差噪声；

GNSS得到的位置具体为：

式中，x_G、y_G、z_G分别是GNSS***输出位置，n_x、n_y、n_z分别是接收机沿地理坐标系三个轴上的位置误差噪声；

得到位置误差方程如下：

将位置误差转换到大地坐标系中，得到位置误差的量测方程为：

其中，V_p＝[-n_x,-n_y,-n_z]^T，，R为地球赤道半径，L为地理纬度；

得到速度量测方程：

式中n_vx、n_vy、n_vz分别是接收机沿地理坐标系轴向上的速度误差噪声，δv_x、δv_y、δv_z分别是接收机沿地理坐标系轴向上的速度。

H_v＝[O_3×3 I₃ O_3×9],V_v＝[-n_vx-n_vy-n_vz]。

将位置量测方程和速度量测方程合并，得到松耦合量测方程为：

优选地，融合定位结果具体为：

P＝wP_A+(1-w)P_B

式中，P_A为GNSS/SINS松耦合输出；P_B为GNSS的输出，w为GNSS/SINS松耦合输出的权值。

本发明与现有技术相比，其显著优点在于：本发明在切换GNSS输出前，进行SINS/GNSS松耦合模式预切换，并且采用软切换算法，避免***切换时产生的跳变，保证了定位输出的平滑过渡。

附图说明

图1是本发明的原理示意图。

图2是本发明中SINS/里程计融合定位原理示意图。

图3是本发明中SINS/GNSS松耦合原理示意图。

图4是本发明的流程示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步描述。

一种多源传感器融合定位切换方法，包括以下步骤：

通过GNSS接收机判断GNSS卫星信号是否满足定位条件，具体如下：

根据NMEA 0183协议，GNSS接收机状态帧GPGSV反映卫星状态信息，通过该帧信息判断可接收卫星数量小于4颗时，GNSS卫星数据为不可用。

当可接收卫星数量≥4颗时，GNSS卫星数据获取初始位姿信息；

当无法使用GNSS卫星信号时，利用SINS/里程计融合导航，建立SINS/里程计误差模型并对导航结果进行修正，具体如下：

建立SINS动态误差模型，模型为Φ角误差方程，包括位置误差、速度误差、失准角和惯性器件漂移误差；

建立里程计动态误差模型，模型包含安装夹角误差(航向/俯仰安装夹角误差)和刻度因子误差；

对SINS/里程计误差进行估计和校正，计算组合定位***状态向量的估计值、预测方差、估计方差和滤波增益，利用误差估计结果对SINS***位置输出、速度输出和姿态矩阵输出进行比较，并对陀螺仪零偏，安装夹角和刻度因子进行更新。

当GNSS信号恢复后，进行GNSS/SINS松耦合模式初始化、滤波收敛、趋势预测等工作，并利用软切换算法，将SINS/里程计融合导航输出预切换至GNSS/SINS松耦合输出，再通过软切换将GNSS的输出与松耦合输出进行加权平均，保证定位输出平滑过渡，直至完全切换至GNSS卫星信号输出，具体如下：

根据GNSS测量协方差信息，建立GNSS状态方程与量测方程，选择误差状态，建立SINS平台/速度/位置误差方程；

GNSS与SINS分别测算得到位置、速度误差量测方程，并由此解算松耦合量测方程。

利用软切换算法，保持原有SINS/里程计融合导航输出的情况下，通过对融合导航和松耦合导航输出结果加权平均，逐步减小原输出的权值，预切换至GNSS/SINS松耦合输出，避免切换过程中出现跳变，保证定位输出平滑过渡，直至完全切换至GNSS卫星信号输出。

实施例1

如图1所示，GNSS接收机通过星历数据及伪距推算时间差，利用最小二乘原理进行解算得到定位输出。当GNSS信号由于建筑物遮挡等原因不可用时，定位***切换至SINS/里程计组合定位模式，以上一时刻GNSS输出为初始状态进行SINS初始对准，确定初始坐标基准与初始姿态矩阵f,ω,同时根据里程计的速度信息和SINS输出的实时姿态矩阵f,ω进行航位推算，将SINS解算P,V,A与航位推算信息作为量测进行滤波，经过计算得到最优估计值，最后利用***误差估计值进行实时误差校正并得到整个定位***的输出。当GNSS信号逐步恢复，在保持原有输出情况下，进行GNSS/SINS松耦合模式初始化、滤波收敛、趋势预测等工作，利用软切换算法，保证持续修正SINS误差并预切换至松耦合定位输出，再将GNSS的输出与松耦合输出进行加权平均，并逐步减小原输出的权值，直至完全切换到GNSS输出。

图1中SINS/里程计融合定位结构具体如图2所示，滤波器输入为里程计输出的速度和惯导解算的速度、加速度约束条件，输出为SINS状态估计误差，状态模型包括SINS的速度误差模型、位置误差模型、姿态误差模型和里程计的误差模型，利用最优估计误差实现惯导子***的校正，输出校正后的载体速度、位姿信息。

图1中SINS/GNSS松耦合结构具体如图3所示，SINS与GPS结构上是两个单独的导航***，需要较为准确的GNSS测量协方差信息，GNSS测量得到的位置与速度观测量用于SINS的滤波器中。

如图4所示，一种多源传感器融合定位切换方法，包括以下步骤：

通过GNSS接收机判断GNSS卫星信号是否满足定位条件，根据NMEA 0183协议，GNSS接收机状态帧GPGSV反映卫星状态信息，通过该帧信息判断可接收卫星数量小于4颗时，GNSS卫星数据为不可用。

信号良好时默认使用GNSS卫星数据获取初始位姿信息，建立伪距观测以及误差方程，通过星历数据及伪距推算时间差，利用最小二乘原理进行解算得到定位输出。

无法使用GNSS卫星信号时，利用SINS/里程计融合导航，建立SINS/里程计误差模型并对融合导航输出进行修正，具体为：

建立SINS动态误差模型，模型表示如下：

其中，X_INS表示惯导***动态误差模型的状态向量，由位置误差δP、速度误差δVⁿ、失准角/>陀螺零偏误差δε_g和加速度计零偏误差/>组成；

建立里程计动态误差模型，模型表示如下：

其中，X_OD＝[δα δβ δτ]^T，X_OD表示里程计动态误差模型的状态向量，由航向安装夹角误差δα，俯仰安装夹角误差δβ和里程计刻度因子误差δτ组成；W_OD表示里程计动态误差模型的***噪声；F_OD表示里程计动态误差矩阵。

综合惯性导航***动态误差模型和里程计动态误差模型，构成如下组合导航***动态误差模型：

其中，0_3×15、0_15×3表示零矩阵。

采用组合导航***动态误差模型对SINS/里程计误差进行估计和校正，利用误差估计结果对K时刻捷联惯导***(SINS)的位置输出速度输出/>和姿态矩阵输出C_k进行校正，同时对陀螺仪零偏ε_g、加速度计零偏/>航向安装夹角α、俯仰安装夹角β和里程计刻度因子τ进行更新。

当GNSS信号恢复后，进行预切换，进行GNSS/SINS松耦合模式初始化、滤波收敛、趋势预测等工作，具体为：

根据GNSS测量协方差信息，建立GNSS状态方程与量测方程，状态方程与量测方程如下：

Z₁＝H₁X₁+V₁

式中，W₁为***噪声，G₁为噪声分配阵，F₁为状态转移矩阵，X₁为***状态向量、H₁为***观测矩阵、V₁为观测噪声。

步骤4.2、解算松耦合量测方程：

设载体真实位置为(x,y,z)、速度为(v_x,v_y,v_z)，SINS解算得到的位置、速度分别为(x₁,y₁,z₁)，GNSS得到的位置、速度分别为(x_G,y_G,z_G)、(v_xG,v_yG,v_zG)。

SINS解算的位置可以表示为

式中，x₁、y₁、z₁分别是捷联惯导***输出位置，δx、δy、δz沿地理坐标系三个轴上的位置误差噪声。

GNSS得到的位置可以表示为

式中，x_G、y_G、z_G分别是GNSS***输出位置，n_x、n_y、n_z分别是接收机沿地理坐标系三个轴上的位置误差噪声。

可以得到位置误差方程如下：

将位置误差转换到大地坐标系中，可以得到位置误差的量测方程：

其中V_p＝[-n_x,-n_y,-n_z]^T。

同理，可得到速度量测方程：

式中n_vx、n_vy、n_vz分别是接收机沿地理坐标系轴向上的速度误差噪声，

H_v＝[O_3×3 I₃ O_3×9],V_v＝[-n_vx-n_vy-n_vz]。

将位置量测方程和速度量测方程合并，得到松耦合量测方程为：

进入切换区可以同时得到两个***的定位结果，但选择任意一个都会造成另一个定位***所带有的信息量浪费以及输出的跳变，采用软切换算法可以有效利用两个定位***的信息并保证定位输出平滑过渡，首先将融合导航输出预切换至GNSS/SINS松耦合输出，再将GNSS输出与GNSS/SINS松耦合输出进行加权平均，对两个***进行定位结果的融合。将GNSS/SINS松耦合输出赋予权值w，则GNSS的输出赋予权值为1-w，则融合结果P可以表示为P＝wP_A+(1-w)P_B

式中P_A——GNSS/SINS松耦合输出；P_B——GNSS的输出。

逐步减小原输出的权值，直至完全切换至GNSS卫星信号输出。

Claims (5)

1.一种多源传感器融合定位切换方法，其特征在于，包括以下步骤：

利用GNSS接收机获取初始位姿信息；

当无法使用GNSS卫星信号时，利用SINS/里程计融合导航，并利用组合导航***动态误差模型对SINS/里程计融合导航结果进行修正；

当GNSS卫星信号恢复后，将融合导航输出预切换至GNSS/SINS松耦合输出，并将GNSS卫星信号输出与松耦合输出进行加权平均，获得融合定位结果，按设定值减小松耦合输出权值，直至完全切换至GNSS卫星信号输出；

利用组合导航***动态误差模型对SINS/里程计融合导航结果进行修正的具体过程为：

建立SINS动态误差模型，具体为：

其中，X_INS表示惯导***动态误差模型的状态向量，δP表示位置误差，δVⁿ表示速度误差，/>表示失准角，δε_g表示陀螺零偏误差，/>表示加速度计零偏误差，F_INS表示捷联惯导动态误差矩阵，W_INS表示捷联惯导动态误差模型的***噪声；

建立里程计动态误差模型，具体为：

其中，X_OD＝[δα δβ δτ]^T，X_OD表示里程计动态误差模型的状态向量，δα表示航向安装夹角误差，δβ表示俯仰安装夹角误差，δτ表示里程计刻度因子误差，W_OD表示里程计动态误差模型的***噪声；F_OD表示里程计动态误差矩阵；

综合惯性导航***动态误差模型和里程计动态误差模型，构成如下组合导航***动态误差模型：

其中，0_3×15、0_15×3表示零矩阵；

采用组合导航***动态误差模型对SINS/里程计输出位置进行校正，利用误差估计结果对K时刻SINS的位置输出速度输出/>和姿态矩阵输出C_k进行校正，同时对陀螺仪零偏ε_g、加速度计零偏/>航向安装夹角α、俯仰安装夹角β和里程计刻度因子τ进行更新。

2.根据权利要求1所述的多源传感器融合定位切换方法，其特征在于，获得GNSS/SINS松耦合输出的具体方法为：

根据GNSS测量协方差信息，建立GNSS状态方程与量测方程；

根据GNSS与SINS分别测算得到的位置、速度误差量测方程解算松耦合量测方程，得到松耦合输出。

3.根据权利要求1所述的多源传感器融合定位切换方法，其特征在于，根据GNSS与SINS分别测算得到的位置、速度误差量测方程解算松耦合量测方程的具体过程为：

载体真实位置为(x,y,z)、速度为(v_x,v_y,v_z)，SINS解算得到的位置、速度分别为(x₁,y₁,z₁)，GNSS得到的位置、速度分别为(x_G,y_G,z_G)、(v_xG,v_yG,v_zG)，其中：

SINS解算得到的位置具体为：

式中，x₁、y₁、z₁分别是捷联惯导***输出位置，δx、δy、δz沿地理坐标系三个轴上的位置误差噪声；

GNSS得到的位置具体为：

式中，x_G、y_G、z_G分别是GNSS***输出位置，n_x、n_y、n_z分别是接收机沿地理坐标系三个轴上的位置误差噪声；

得到位置误差方程如下：

将位置误差转换到大地坐标系中，得到位置误差的量测方程为：

其中，V_p＝[-n_x,-n_y,-n_z]^T，R为地球赤道半径，L为地理纬度；

得到速度量测方程：

式中n_vx、n_vy、n_vz分别是接收机沿地理坐标系轴向上的速度误差噪声，δv_x、δv_y、δv_z分别是接收机沿地理坐标系轴向上的速度；

H_v＝[O_3×3 I₃ O_3×9],V_v＝[-n_vx -n_vy -n_vz]

将位置量测方程和速度量测方程合并，得到松耦合量测方程为：

4.根据权利要求1所述的多源传感器融合定位切换方法，其特征在于，融合定位结果具体为：

P＝wP_A+(1-w)P_B

式中，P_A为GNSS/SINS松耦合输出；P_B为GNSS的输出，w为GNSS/SINS松耦合输出的权值。

5.根据权利要求1所述的多源传感器融合定位切换方法，其特征在于，当可接收卫星数量≥4颗时，GNSS接收机获取初始位姿信息。