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

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

【技术实现步骤摘要】
一种多源传感器信息融合定位切换方法
本专利技术属于多源传感器信息融合定位领域，具体为一种多源传感器融合定位切换方法。
技术介绍
目前，卫星导航定位系统在室外定位领域获得了广泛的应用，但受建筑物、树木和地形的影响，还存在一些存在无法接收足够卫星星历数据进行定位的区域。当运动体运动到此区域内时，需要使用非卫星导航系统进行补充，如惯性导航系统。当运动体从卫星信号良好区域进入无卫星信号区域时，需要将导航定位信号从卫星导航系统切换至非卫星导航系统；当运动体从无卫星导航区域进入卫星信号良好区域时，需要将导航定位信号从非卫星导航系统切换至卫星导航系统。传统的切换方法采用硬切换技术。由于非卫星导航技术通常基于积分的，因此会存在累计误差，当从卫星导航系统切换至非卫星导航系统时，卫星导航系统最后一次输出作为非卫星导航系统的初值，因此不会产生位置信号的突变，但当从非卫星导航系统切换至卫星导航系统时，由于有累积误差的存在且卫星导航系统的输出不依赖于非卫星导航系统的输出，此时位置信号会产生突变，该突变会对采用此位置信号作为反馈信号的控制系统造成不利影响，使得控制性能恶化。
技术实现思路
本专利技术的目的在于提供一种多源传感器融合定位切换方法，能够有效利用多传感器量测信息减小多传感器信息切换误差。实现本专利技术目的的技术解决方案为：一种多源传感器融合定位切换方法，包括：利用GNSS接收机获取初始位姿信息；当无法使用GNSS卫星信号时，利用SINS/里程计融合导航，并利用组合导航系统动态误差模型对SINS/里程计融合导航结果进行修正；当GNSS卫星信号恢复后，将融合导航输出预切换至GNSS/SINS松耦合输出，并将GNSS卫星信号输出与松耦合输出进行加权平均，获得融合定位结果，按设定值减小松耦合输出权值，直至完全切换至GNSS卫星信号输出。优选地，利用组合导航系统动态误差模型对SINS/里程计融合导航结果进行修正的具体过程为：建立SINS动态误差模型，具体为：其中，XINS表示惯导系统动态误差模型的状态向量，δP表示位置误差，δVn表示速度误差，表示失准角，δεg表示陀螺零偏误差，表示加速度计零偏误差，FINS表示捷联惯导动态误差矩阵，WINS表示捷联惯导动态误差模型的系统噪声；建立里程计动态误差模型，具体为：其中，XOD＝[δαδβδτ]T，XOD表示里程计动态误差模型的状态向量，δα表示航向安装夹角误差，δβ表示俯仰安装夹角误差，δτ表示里程计刻度因子误差，WOD表示里程计动态误差模型的系统噪声；FOD表示里程计动态误差矩阵；综合惯性导航系统动态误差模型和里程计动态误差模型，构成如下组合导航系统动态误差模型：其中，03×15、015×3表示零矩阵；采用组合导航系统动态误差模型对SINS/里程计输出位置进行校正，利用误差估计结果对K时刻SINS的位置输出速度输出和姿态矩阵输出Ck进行校正，同时对陀螺仪零偏εg、加速度计零偏航向安装夹角α、俯仰安装夹角β和里程计刻度因子τ进行更新。优选地，获得GNSS/SINS松耦合输出的具体方法为：根据GNSS测量协方差信息，建立GNSS状态方程与量测方程；根据GNSS与SINS分别测算得到的位置、速度误差量测方程解算松耦合量测方程，得到松耦合输出。优选地，根据GNSS与SINS分别测算得到的位置、速度误差量测方程解算松耦合量测方程的具体过程为：载体真实位置为(x,y,z)、速度为(vx,vy,vz)，SINS解算得到的位置、速度分别为(x1,y1,z1)，GNSS得到的位置、速度分别为(xG,yG,zG)、(vxG,vyG,vzG)，其中：SINS解算得到的位置具体为：式中，x1、y1、z1分别是捷联惯导系统输出位置，δx、δy、δz沿地理坐标系三个轴上的位置误差噪声；GNSS得到的位置具体为：式中，xG、yG、zG分别是GNSS系统输出位置，nx、ny、nz分别是接收机沿地理坐标系三个轴上的位置误差噪声；得到位置误差方程如下：将位置误差转换到大地坐标系中，得到位置误差的量测方程为：其中，Vp＝[-nx,-ny,-nz]T，，R为地球赤道半径，L为地理纬度；得到速度量测方程：式中nvx、nvy、nvz分别是接收机沿地理坐标系轴向上的速度误差噪声，δvx、δvy、δvz分别是接收机沿地理坐标系轴向上的速度。Hv＝[O3×3I3O3×9],Vv＝[-nvx-nvy-nvz]。将位置量测方程和速度量测方程合并，得到松耦合量测方程为：优选地，融合定位结果具体为：P＝wPA+(1-w)PB式中，PA为GNSS/SINS松耦合输出；PB为GNSS的输出，w为GNSS/SINS松耦合输出的权值。本专利技术与现有技术相比，其显著优点在于：本专利技术在切换GNSS输出前，进行SINS/GNSS松耦合模式预切换，并且采用软切换算法，避免系统切换时产生的跳变，保证了定位输出的平滑过渡。附图说明图1是本专利技术的原理示意图。图2是本专利技术中SINS/里程计融合定位原理示意图。图3是本专利技术中SINS/GNSS松耦合原理示意图。图4是本专利技术的流程示意图。具体实施方式下面结合附图和实施例对本专利技术作进一步描述。一种多源传感器融合定位切换方法，包括以下步骤：通过GNSS接收机判断GNSS卫星信号是否满足定位条件，具体如下：根据NMEA0183协议，GNSS接收机状态帧GPGSV反映卫星状态信息，通过该帧信息判断可接收卫星数量小于4颗时，GNSS卫星数据为不可用。当可接收卫星数量≥4颗时，GNSS卫星数据获取初始位姿信息；当无法使用GNSS卫星信号时，利用SINS/里程计融合导航，建立SINS/里程计误差模型并对导航结果进行修正，具体如下：建立SINS动态误差模型，模型为Φ角误差方程，包括位置误差、速度误差、失准角和惯性器件漂移误差；建立里程计动态误差模型，模型包含安装夹角误差(航向/俯仰安装夹角误差)和刻度因子误差；对SINS/里程计误差进行估计和校正，计算组合定位系统状态向量的估计值、预测方差、估计方差和滤波增益，利用误差估计结果对SINS系统位置输出、速度输出和姿态矩阵输出进行比较，并对陀螺仪零偏，安装夹角和刻度因子进行更新。当GNSS信号恢复后，进行GNSS/SINS松耦合模式初始化、滤波收敛、趋势预测等工作，并利用软切换算法，将SINS/里程计融合导航输出预切换至GNSS/SINS松耦合输出，再通过软切换将GNSS的输出与松耦合输出进行加权平均，保证定位输出平滑过渡，直至完全切换至GNSS卫星信号输出，具体如下：根据GNSS测量协方差信息，建立GNSS状态方程与量测方程，选择误差状态，建立SINS平台本文档来自技高网...

【技术保护点】
1.一种多源传感器融合定位切换方法，其特征在于，包括以下步骤：/n利用GNSS接收机获取初始位姿信息；/n当无法使用GNSS卫星信号时，利用SINS/里程计融合导航，并利用组合导航系统动态误差模型对SINS/里程计融合导航结果进行修正；/n当GNSS卫星信号恢复后，将融合导航输出预切换至GNSS/SINS松耦合输出，并将GNSS卫星信号输出与松耦合输出进行加权平均，获得融合定位结果，按设定值减小松耦合输出权值，直至完全切换至GNSS卫星信号输出。/n

【技术特征摘要】
1.一种多源传感器融合定位切换方法，其特征在于，包括以下步骤：
利用GNSS接收机获取初始位姿信息；
当无法使用GNSS卫星信号时，利用SINS/里程计融合导航，并利用组合导航系统动态误差模型对SINS/里程计融合导航结果进行修正；
当GNSS卫星信号恢复后，将融合导航输出预切换至GNSS/SINS松耦合输出，并将GNSS卫星信号输出与松耦合输出进行加权平均，获得融合定位结果，按设定值减小松耦合输出权值，直至完全切换至GNSS卫星信号输出。


2.根据权利要求1所述的多源传感器融合定位切换方法，其特征在于，利用组合导航系统动态误差模型对SINS/里程计融合导航结果进行修正的具体过程为：
建立SINS动态误差模型，具体为：



其中，XINS表示惯导系统动态误差模型的状态向量，δP表示位置误差，δVn表示速度误差，表示失准角，δεg表示陀螺零偏误差，表示加速度计零偏误差，FINS表示捷联惯导动态误差矩阵，WINS表示捷联惯导动态误差模型的系统噪声；
建立里程计动态误差模型，具体为：



其中，XOD＝[δαδβδτ]T，XOD表示里程计动态误差模型的状态向量，δα表示航向安装夹角误差，δβ表示俯仰安装夹角误差，δτ表示里程计刻度因子误差，WOD表示里程计动态误差模型的系统噪声；FOD表示里程计动态误差矩阵；
综合惯性导航系统动态误差模型和里程计动态误差模型，构成如下组合导航系统动态误差模型：



其中，03×15、015×3表示零矩阵；
采用组合导航系统动态误差模型对SINS/里程计输出位置进行校正，利用误差估计结果对K时刻SINS的位置输出速度输出和姿态矩阵输出Ck进行校正，同时对陀螺仪零偏εg、加速度计零偏航向安装夹角α、俯仰安装夹角β和里程计刻度因子τ进行更新。


3.根据权利要求1所述的多源传感器融合定位切换方法，其特征在于，获得GNSS/SINS松耦合输出的具体方法为：
根据GNSS测量协方差信息，建立GNSS状态方程与量...

【专利技术属性】
技术研发人员：李胜，潘海瑞，张磊，陈庆伟，向峥嵘，郭健，吴益飞，
申请(专利权)人：南京理工大学，
类型：发明
国别省市：江苏;32