本发明涉及车辆定位技术领域，尤其是涉及一种基于序贯卡尔曼滤波的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为：
其中，
在序贯卡尔曼滤波中，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(k)为gnss观测更新的状态向量，x2(k)为视觉航向观测更新的状态向量，p1(k)为gnss观测更新的协方差矩阵的观测值，p2(k)为视觉航向更新的协方差矩阵的观测值，k和k-1分别表示当前时刻和上一时刻。
一种基于序贯卡尔曼滤波的gnssins视觉融合定位系统，该系统包括：
惯性测量单元信号处理模块：通过三轴加速度计和三轴陀螺仪获取载体在三个方向的加速度和速度，通过积分更新载体的姿态、速度和位置信息，根据速度和位置信息得到伪距和伪距率的预测值，将预测值与经过gnss信号处理模块修正后的伪距和伪距率的观测值做差，得到伪距误差和伪距率误差，并发送给融合定位模块；
gnss信号处理模块：接收gnss信号并进行信号处理，得到gnss的伪距信息和伪距率的观测信息，并且对伪距和伪距率的观测信息进行电离层、对流层的延迟修正；
视觉惯性里程计信号处理模块：根据视觉提供的航向增量和惯性测量单元信号处理模块的航向增量得到航向误差，并将航向误差发送给融合定位模块；
融合定位模块：接收gnss信号处理模块、惯性测量单元信号处理模块和视觉惯性里程计信号处理模块发生的信息并进行序贯卡尔曼滤波处理，包括时间更新模块和量测更新模块，分别进行时间更新和量测更新得到误差修正信息并反馈至惯性测量单元信号处理模块和gnss信号处理模块进行信息修正。
与现有技术相比，本发明具有以下优点：
一、本发明仅利用惯性测量单元和gnss传感器即可实现车辆载体的精确快速定位，成本低廉易实现。
二、本发明采用序贯卡尔曼滤波，将gnss、ins和视觉里程计更稳定地融合，得到精度较高的车辆姿态、位置和速度信息。
三、本发明采用了gnss和ins的紧耦合算法，较现有普遍使用的gnss/ins松耦合算法精度更高。
四、本发明在gnss/ins紧耦合的基础上融合了视觉里程计的特征级信息，针对紧耦合航向角弱可观的特性，有效提高了车辆的航向精度。
五、本发明在序贯卡尔曼滤波中的输入值为17维状态向量，与普遍使用的15维状态向量增加了接收机钟差和接收机钟差漂移两个信息，建模准确，估计结果精度高，鲁棒性强，应用范围广。
附图说明
图1为本发明一种基于序贯卡尔曼滤波的gnss/ins/视觉融合定位系统的结构功能框图。
图2为本发明的序贯卡尔曼滤波的计算流程图。
图中标记说明：
1、惯性测量单元信号处理模块，2、gnss信号处理模块，3、视觉惯性里程计信号处理模块，4、融合定位模块，5、时间更新模块，6、量测更新模块。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。
实施例
如图1所示，本发明提供一种基于序贯卡尔曼滤波的gnss/ins/视觉融合定位系统，该系统包括：
惯性测量单元信号处理模块1，对惯性单元测量得到的三轴角速度和加速度进行积分处理，得到姿态、位置和速度信息；
gnss信号处理模块2，对gnss传感器的信号和进行处理，得到伪距误差和伪距率误差的观测值，再对其进行电离层延迟和对流层延迟的修正，将修正后的数据与预测的数值做差，获得伪距误差和伪距率误差；
视觉惯性里程计信号处理模块3，采用视觉提供的航向增量与惯性导航系统积分计算的航向增量做差，获得航向误差；处理之后得到的信号发送至融合定位模块；
融合定位模块4，基于序贯卡尔曼滤波处理传感器信号处理模块发送的信号，对车辆的姿态、位置和速度进行估计。
如图2所示，融合定位模块4采用序贯卡尔曼滤波技术，具体包括时间更新模块5和量测更新模块6。
时间更新模块5的输入为初始状态向量x0、初始协方差矩阵p0和系统噪声q1，输出为状态向量x和协方差矩阵p。
量测更新模块6具体包括gnss观测更新和视觉航向观测更新，gnss观测更新的输入为gnss观测向量z1，视觉航向观测更新的输入为视觉航向观测值z2，输出为状态向量x和协方差矩阵p，具体计算过程如下所示：
系统状态向量：
系统状态转移矩阵：
gnss测量矩阵：
视觉航向测量矩阵：
h2＝[01×2；1；014×1]
gnss观测向量：
视觉航向观测值：
z2＝ψ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)
其中，
其中，φ为姿态误差角，δvn为速度误差，δp为位置误差，ε为载体坐标系下的陀螺零偏，▽为载体坐标系下的加速度计零偏，τ为接收机时钟误差，dτ为接收机时钟误差漂移，l为载体的地理纬度，λ是载体的地理经度，rm为子午圈主曲率半径，rn为卯酉圈主曲率半径，h是载体重心距地面的高度，e是地球的偏心率，ve是东-北-天坐标系下的东向速度，vn是北向速度，vu是天向速度，
通过融合定位模块4更新误差状态矩阵，反馈至惯性测量单元信号处理模块1修正车辆信息并输出车辆位姿、速度及位置。
基于序贯卡尔曼滤波的gnss/ins/视觉融合定位系统，本发明采用上述融合定位系统的定位方法实现对车辆姿态、速度和位置的准确快速估计。
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为：
其中，
7.根据权利要求6所述的一种基于序贯卡尔曼滤波的gnssins视觉融合定位方法，其特征在于，在序贯卡尔曼滤波中，gnss测量矩阵h1为：
视觉航向测量矩阵h2为：
h2＝[01×2；1；014×1]
其中，lt为伪距相关的观测矩阵，lc为伪距率相关的观测矩阵，im×n表示m×n维单位阵，0m×n表示m×n维0矩阵，λ为载体的地理经度，los表示地心地固坐标系下载体到不同卫星方向的单位向量，e为地球的偏心率。
8.根据权利要求7所述的一种基于序贯卡尔曼滤波的gnssins视觉融合定位方法，其特征在于，在序贯卡尔曼滤波中，gnss观测向量z1为：
视觉航向观测值z2为：
z2＝ψmeasure-ψins
其中，ρgnss为gnss的伪距观测量，δρgnss为gnss的伪距率观测量，ρins为ins和星历计算的伪距预测量，δρins表示ins与星历计算的伪距率预测量，ψmeasure为视觉惯性里程计测量得到的航向，ψins为惯性测量单元处理模块得到的航向。
9.根据权利要求8所述的一种基于序贯卡尔曼滤波的gnssins视觉融合定位方法，其特征在于，在序贯卡尔曼滤波中，计算预测及更新包括：
计算状态预测：
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(k)为gnss观测更新的状态向量，x2(k)为视觉航向观测更新的状态向量，p1(k)为gnss观测更新的协方差矩阵的观测值，p2(k)为视觉航向更新的协方差矩阵的观测值，k和k-1分别表示当前时刻和上一时刻。
10.一种基于序贯卡尔曼滤波的gnssins视觉融合定位系统，其特征在于，该系统包括：
惯性测量单元信号处理模块(1)：通过三轴加速度计和三轴陀螺仪获取载体在三个方向的加速度和速度，通过积分更新载体的姿态、速度和位置信息，根据速度和位置信息得到伪距和伪距率的预测值，将预测值与经过gnss信号处理模块修正后的伪距和伪距率的观测值做差，得到伪距误差和伪距率误差，并发送给融合定位模块；
gnss信号处理模块(2)：接收gnss信号并进行信号处理，得到gnss的伪距信息和伪距率的观测信息，并且对伪距和伪距率的观测信息进行电离层、对流层的延迟修正；
视觉惯性里程计信号处理模块(3)：根据视觉提供的航向增量和惯性测量单元信号处理模块的航向增量得到航向误差，并将航向误差发送给融合定位模块；
融合定位模块(4)：接收gnss信号处理模块、惯性测量单元信号处理模块和视觉惯性里程计信号处理模块发生的信息并进行序贯卡尔曼滤波处理，包括时间更新模块(5)和量测更新模块(6)，分别进行时间更新和量测更新得到误差修正信息并反馈至惯性测量单元信号处理模块和gnss信号处理模块进行信息修正。
技术总结