基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法及系统技术方案

本发明专利技术涉及一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法及系统，该方法包括以下步骤：1)对惯性单元测量得到的三轴角速度和加速度进行积分处理，得到姿态、位置和速度信息，并根据速度和位置信息得到伪距和伪距率的预测值；2)获取GNSS信号中的伪距和伪距率的观测信息，与伪距和伪距率的预测值做差得到伪距误差和伪距率误差；3)采用视觉提供的航向增量，与惯性测量单元得到的航向增量做差得到航向误差；4)根据速度、位置和姿态信息、伪距误差和伪距率误差以及航向误差采用序贯卡尔曼滤波进行时间更新和量测更新，并得到误差修正信息并反馈进行信息修正。与现有技术相比，本发明专利技术具有计算量小、成本低、精度高、可广泛应用等优点。

【技术实现步骤摘要】
基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法及系统
本专利技术涉及车辆定位
，尤其是涉及一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法及系统。
技术介绍
近年来，随着智能汽车技术的快速发展，智能车对环境感知、定位、决策、规划与控制等系统的要求越来越高，其中，定位技术为其它系统提供姿态、速度、位置等信息，是智能汽车技术中不可或缺的一环。如何获得连续、准确、可靠的车辆位姿信息成为智能车定位技术要解决的主要问题。目前常用的车辆定位方式包括全球导航卫星系统(GNSS)定位、惯性导航系统(INS)、激光雷达(Lidar)定位、视觉定位和基于V2X技术的定位。其中，融合GNSS与INS的组合定位算法是目前最常见的定位技术。GNSS定位通过对接收到的卫星信号进行实时解算伪距、伪距率和卫星位置等信息，根据三角定位法计算接收机天线中心位置，能提供低频的、无累积偏差的绝对位置信息。INS通过处理IMU输出的加速度、角速度数据，依次进行姿态更新、比力坐标转换、有害加速度/地球自转角速度补偿、速度更新、位置更新等步骤，获得高频的、存在累积误差的位置、速度、姿态信息。为解决GNSS低频、定位精度较低、位置噪声较大和INS存在累积误差的问题，GNSS/INS组合定位通过INS进行短时间的积分，使用GNSS作为观测修正载体状态，可以提供比GNSS或INS单独定位更准确的定位结果。GNSS/INS组合定位作为主流的定位方案，目前仍然存在两个比较显著的问题。其一，GNSS/INS系统并不是在任何运动状态下都能提供精确的航向估计。在某些运动状态下，一些状态在理论上就是难以准确估计的，这与系统的可观性有关。其二，GNSS会由于卫星信号遮挡或者GNSS异常观测导致其解算出来的位置信息存在较大误差，甚至不能解算位置。GNSS信号不佳会显著降低GNSS/INS组合定位的状态估计精度，以上两个问题会对GNSS/INS组合定位的精度和系统的鲁棒性造成不小的影响。
技术实现思路
本专利技术的目的就是为了克服上述现有技术存在的缺陷而提供一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法及系统。本专利技术的目的可以通过以下技术方案来实现：一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法，包括以下步骤：1)对惯性单元测量得到的三轴角速度和加速度进行积分处理，得到姿态、位置和速度信息，并根据速度和位置信息得到伪距和伪距率的预测值；2)获取GNSS信号中的伪距和伪距率的观测信息，经过电离层、对流层的延迟修正处理后与伪距和伪距率的预测值做差得到伪距误差和伪距率误差；3)采用视觉提供的航向增量，与惯性测量单元得到的航向增量做差得到航向误差；4)根据速度、位置和姿态信息、伪距误差和伪距率误差以及航向误差采用序贯卡尔曼滤波进行时间更新和量测更新，并得到误差修正信息并反馈进行信息修正。所述的步骤1)具体为：根据惯性测量单元测量得到的三轴加速度积分处理并进行误差补偿得到速度信息，对速度积分处理并进行误差补偿得到位置信息，根据惯性测量单元测量得到的三轴角速度积分处理并进行误差补偿得到姿态信息，并根据速度和位置信息得到伪距和伪距率的预测值。所述的步骤3)具体为：采用单目摄像机特征点匹配得到航向增量与惯性测量单元得到的航向增量做差得到航向误差。所述的步骤4)中，时间更新的输入为初始状态估计向量、状态估计误差的初始均方误差阵和系统噪声，更新得到状态估计向量和状态估计误差的均方误差阵；量测更新的输入为时间更新输出的状态估计向量、状态估计误差的均方误差阵、GNSS观测向量以及视觉航向的观测向量，更新得到状态估计向量和状态估计误差的均方误差阵。在序贯卡尔曼滤波中，系统状态向量X为：其中，φ为姿态误差角，δvn为速度误差，δp为位置误差，ε为载体坐标系下的陀螺仪零偏，▽为载体坐标系下的加速度计零偏，τ为接收机时钟误差，dτ为接收机时钟误差漂移。在序贯卡尔曼滤波中，系统状态转移矩阵F为：其中，为导航坐标系相对于惯性坐标系的角速度在导航坐标系下的投影，RM为子午圈主曲率半径，RN为卯酉圈主曲率半径，L为载体的地理纬度，ωie为地球自转角速率，vE为东-北-天坐标系下的东向速度，vN为北向速度，vU为天向速度，为载体坐标系转化至导航坐标系的姿态转移矩阵，为加速度计测量的比力，h为载体重心距地面的高度，×表示反对称矩阵，在序贯卡尔曼滤波中，GNSS测量矩阵H1为：视觉航向测量矩阵H2为：H2＝[01×2；1；014×1]其中，LT为伪距相关的观测矩阵，LC为伪距率相关的观测矩阵，Im×n表示m×n维单位阵，0m×n表示m×n维0矩阵，λ为载体的地理经度，LOS表示地心地固坐标系下载体到不同卫星方向的单位向量，e为地球的偏心率。在序贯卡尔曼滤波中，GNSS观测向量Z1为：视觉航向观测值Z2为：Z2＝ψmeasure-ψINS其中，ρGNSS为GNSS的伪距观测量，δρGNSS为GNSS的伪距率观测量，ρINS为INS和星历计算的伪距预测量，δρINS表示INS与星历计算的伪距率预测量，ψmeasure为视觉惯性里程计测量得到的航向，ψINS为惯性测量单元处理模块得到的航向。在序贯卡尔曼滤波中，计算预测及更新包括：计算状态预测：X(k|k-1)＝F(k-1)X(k-1)计算协方差矩阵预测：P1(k|k-1)＝F(k-1)P1(k-1)FT(k-1)+Q1计算GNSS观测卡尔曼滤波增益：K1(k)＝P1(k|k-1)H1T(k)[H1(k)P1(k|k-1)H1T(k)+R1]-1计算GNSS观测状态更新：X1(k)＝X(k|k-1)+K1(k)[Z1(k)-H1T(k)X(k|k-1)]计算GNSS观测协方差矩阵更新：P1(k)＝(I-K1(k)H1(k))P1(k|k-1)计算视觉航向观测卡尔曼滤波增益：K2(k)＝P1(k)H2T(k)[H2(k)P1(k)H2T(k)+R2]-1计算视觉航向观测状态更新：X2(k)＝X1(k)+K1(k)[Z2(k)-H2T(k)X1(k)]计算视觉航向观测协方差矩阵更新：P2(k)＝(I-K2(k)H2(k))P1(k)其中，X(k|k-1)为系统状态预测值，F(k-1)为上一时刻系统状态转移矩阵，X(k-1)为上一时刻系统状态，P1(k|k-1)为协方差矩阵的预测值，Q1为状态方程过程噪声方差，R1为GNSS量测噪声方差，R2为视觉航向量测噪声方差，K1(k)为GNSS观测卡尔曼滤波增益，K2(k)为GNSS观测卡尔曼滤波增益，X1本文档来自技高网...

【技术保护点】
1.一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法，其特征在于，包括以下步骤：/n1)对惯性单元测量得到的三轴角速度和加速度进行积分处理，得到姿态、位置和速度信息，并根据速度和位置信息得到伪距和伪距率的预测值；/n2)获取GNSS信号中的伪距和伪距率的观测信息，经过电离层、对流层的延迟修正处理后与伪距和伪距率的预测值做差得到伪距误差和伪距率误差；/n3)采用视觉提供的航向增量，与惯性测量单元得到的航向增量做差得到航向误差；/n4)根据速度、位置和姿态信息、伪距误差和伪距率误差以及航向误差采用序贯卡尔曼滤波进行时间更新和量测更新，并得到误差修正信息并反馈进行信息修正。/n

【技术特征摘要】
1.一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法，其特征在于，包括以下步骤：
1)对惯性单元测量得到的三轴角速度和加速度进行积分处理，得到姿态、位置和速度信息，并根据速度和位置信息得到伪距和伪距率的预测值；
2)获取GNSS信号中的伪距和伪距率的观测信息，经过电离层、对流层的延迟修正处理后与伪距和伪距率的预测值做差得到伪距误差和伪距率误差；
3)采用视觉提供的航向增量，与惯性测量单元得到的航向增量做差得到航向误差；
4)根据速度、位置和姿态信息、伪距误差和伪距率误差以及航向误差采用序贯卡尔曼滤波进行时间更新和量测更新，并得到误差修正信息并反馈进行信息修正。


2.根据权利要求1所述的一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法，其特征在于，所述的步骤1)具体为：
根据惯性测量单元测量得到的三轴加速度积分处理并进行误差补偿得到速度信息，对速度积分处理并进行误差补偿得到位置信息，根据惯性测量单元测量得到的三轴角速度积分处理并进行误差补偿得到姿态信息，并根据速度和位置信息得到伪距和伪距率的预测值。


3.根据权利要求1所述的一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法，其特征在于，所述的步骤3)具体为：
采用单目摄像机特征点匹配得到航向增量与惯性测量单元得到的航向增量做差得到航向误差。


4.根据权利要求1所述的一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法，其特征在于，所述的步骤4)中，时间更新的输入为初始状态估计向量、状态估计误差的初始均方误差阵和系统噪声，更新得到状态估计向量和状态估计误差的均方误差阵；
量测更新的输入为时间更新输出的状态估计向量、状态估计误差的均方误差阵、GNSS观测向量以及视觉航向的观测向量，更新得到状态估计向量和状态估计误差的均方误差阵。


5.根据权利要求4所述的一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法，其特征在于，在序贯卡尔曼滤波中，系统状态向量X为：



其中，φ为姿态误差角，δvn为速度误差，δp为位置误差，ε为载体坐标系下的陀螺仪零偏，▽为载体坐标系下的加速度计零偏，τ为接收机时钟误差，dτ为接收机时钟误差漂移。


6.根据权利要求5所述的一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法，其特征在于，在序贯卡尔曼滤波中，系统状态转移矩阵F为：

































其中，为导航坐标系相对于惯性坐标系的角速度在导航坐标系下的投影，RM为子午圈主曲率半径，RN为卯酉圈主曲率半径，L为载体的地理纬度，ωie为地球自转角速率，vE为东-北-天坐标系下的东向速度，vN为北向速度，vU为天向速度，为载体坐标系转化至导航坐标系的姿态转移矩阵，为加速度计测量的比力，h为载体重心距地面的高度，×表示反对称矩阵。


7.根据权利要求6所述的一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法，其特征在于，在序贯卡尔曼滤波中，GNSS测量矩阵H1为：



视觉航向测量矩阵H2为：
H2＝[01×2；1；014×1]






其中，LT为伪距相关的观测矩阵，LC为伪距率相关的观测矩阵，Im×n表示m×n维单位阵，0m×n表示m×n维0矩阵，λ为载体的地理经度，LO...

【专利技术属性】
技术研发人员：熊璐，王添，陆逸适，沈翔翔，陈梦源，朱周麟，谢智龙，高乐天，余卓平，
申请(专利权)人：同济大学，
类型：发明
国别省市：上海;31