CN115856974A - 一种基于不变滤波的gnss、ins和视觉紧组合导航定位方法 - Google Patents

Abstract

发明提供一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法，包括有步骤一，使用时序差分获取GNSS观测值构建不变滤波下的卫星观测方程；步骤二，获取INS惯导数据并构建不变滤波状态方程，使用卫星观测方程对不变滤波状态方程进行观测更新。本发明能够将不变扩展卡尔曼滤波理论应用于多状态约束卡尔曼滤波框架中，使卫星导航、视觉同步定位与制图(SLAM)、惯性导航三类定位方式紧组合，通过不变扩展卡尔曼滤波改变状态误差的定义方法，实现误差传递矩阵与状态估计值的独立，可对三类定位进行优势互补，实现不易发散的高频高精度定位。

Description

一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法

技术领域

发明涉及导航定位技术领域，具体涉及一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法。

背景技术

自动驾驶应用越来越广泛，为了实现更好的无人化和智能化的管理，实时获得自动驾驶车辆的位置、速度、姿态等状态是必不可少的。目前主要的定位方法有卫星导航、视觉同步定位与制图(SLAM)和惯性导航等。

卫星导航通过测量卫星到接收机之间伪距，载波相位，多普勒三类观测值，进行定位。卫星导航除了估计接收机在大地坐标系下的坐标和速度外，还需要估计接收机时钟误差和接收机时钟漂移。视觉SLAM技术通过使用图像匹配的技术计算两帧图像之间的旋转和平移参数。惯性导航使用惯性测量单元(IMU)测量自身的角速度和加速度，然后通过惯导算法(INS)实现定位。

然而GNSS受噪声干扰严重，频率低；视觉SLAM局部坐标精度高，但是无法得到全球坐标，频率也不高且随着距离发散；IMU频率高，但是发散的很快。

现有技术中通常将视觉同步定位和惯性导航组合导航，常规组合导航采用的是扩展卡尔曼滤波器(EKF)，普通的EKF通过对动态方程的线性化，来估计状态与状态的协方差。但是存在一个问题，误差传递方程中，系统矩阵依赖当前状态的估计量。在有噪声引入时，状态估计量是无法预测的，这就导致很难对系统方程做收敛性分析，实际上，任何EKF都没有收敛性保证。当状态估计值与真值差距较大时，直接导致依赖状态估计值的系统方程也有较大偏差，使用这样的系统方程继续传递误差后，又进一步放大误差，整个误差传递系统形成正反馈，最终导致滤波器发散。

实际上滤波器进行状态更新时也有类似问题，EKF还会导致不一致性。每当新的观测到来时，EKF会更新当前状态的估计。但是用于计算新状态的量，都是通过旧状态的线性化得来的。因此，EKF的更新方法会出现不一致性，导致本来不该观测到信息的方向上观测到信息，在SLAM问题中这种现象比较明显，表现为较大的漂移，以及过于乐观的协方差估计。

发明内容

有鉴于此，发明要解决的问题是提供一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法，能够将不变扩展卡尔曼滤波理论应用于多状态约束卡尔曼滤波框架中，使卫星导航、视觉同步定位与制图(SLAM)、惯性导航三类定位方式紧组合，通过不变扩展卡尔曼滤波改变状态误差的定义方法，实现误差传递矩阵与状态估计值的独立，可对三类定位进行优势互补，实现不易发散的高频高精度定位。

为解决上述技术问题，发明采用的技术方案是：

一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法，包括有，步骤一，使用时序差分获取GNSS观测值构建不变滤波下的卫星观测方程；

步骤二，获取INS惯导数据并构建不变滤波状态方程，使用卫星观测方程对不变滤波状态方程进行观测更新。

进一步的，构建所述不变滤波状态方程包括有：定义IMU传感器在n时刻的状态数据X_n＝(R_n,v_n,p_n,b_g,b_a)，其中R_n表示n时刻IMU获取的姿态，v_n表示n时刻IMU获取的速度，p_n表示n时刻IMU获取的位置，b_g表示n时刻IMU获取的陀螺仪零偏，b_a表示n时刻IMU获取的加速度计零偏；

按照IEKF对误差的定义，任意时刻IMU的状态X和误差e都可以定义成：

其中

是IMU状态真值；X是IMU状态；e是IMU状态的误差；e_θ是IMU姿态的误差，/>

是IMU姿态的真值，/>

是IMU速度的真值，e_v是IMU速度的误差，J_r()是右雅克比函数，/>

是IMU位置的真值，e_p是IMU位置的误差，/>

是陀螺仪零偏的真值，e_bg是陀螺仪零偏的误差，/>

是加速度计零偏的真值，e_ba是加速度计零偏的误差。

进一步的，获取视觉传感器数据并设定视角特征点，对视觉特征点进行特征提取并跟踪，并以多状态约束卡尔曼框架下的观测对步骤二所构建的不变滤波状态方程进行观测更新。

进一步的，所述步骤一之前包括有GNSS坐标系初始化，以将GNSS的坐标系转换至INS坐标系下。

进一步的，所述不变滤波状态方程状态转移计算时，包括有获取GNSS传感器的钟差和钟速数据，以使视觉惯性系统的时间与GNSS传感器时间同步。

进一步的，所述卫星观测方程包括有伪距观测方程、载波相位观测方程和多普勒观测方程；

所述伪距观测方程用于获取两相邻时刻之间的伪距变化量z_Dp，所述载波相位观测方程用于获取两相邻时刻之间载波相位的变化量z_Dc，所述多普勒观测方程用于获取任一时刻的多普勒观测值z_d。

发明具有的优点和积极效果是：

通过将不变扩展卡尔曼滤波理论应用于多状态约束卡尔曼滤波框架中，依据IMU传感器数据构建不变卡尔曼滤波方程，实现误差传递矩阵与状态估计值的独立，并使用GNSS传感器数据构建不变卡尔曼录波下的卫星观测方程，卫星观测方程的计算数值输入至卡尔曼滤波器内，以对不变卡尔曼滤波方程进行观测更新(误差传递矩阵进行更新)，由于GNSS传感器数据不受时间传递影响，以实现长时间定位准确定位的效果。

同时视觉SLAM局部坐标精度高，可将视觉导航与惯性导航紧组合构建视觉惯性系统，进一步提高定位精度。

附图说明

附图用来提供对发明的进一步理解，并且构成说明书的一部分，与发明的实施例一起用于解释发明，并不构成对发明的限制。在附图中：

图1是发明的一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法的整体原理图。

具体实施方式

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

需要说明的是，当组件被称为“固定于”另一个组件，它可以直接在另一个组件上或者也可以存在居中的组件。当一个组件被认为是“连接”另一个组件，它可以是直接连接到另一个组件或者可能同时存在居中组件。当一个组件被认为是“设置于”另一个组件，它可以是直接设置在另一个组件上或者可能同时存在居中组件。本文所使用的术语“垂直的”、“水平的”、“左”、“右”以及类似的表述只是为了说明的目的。

除非另有定义，本文所使用的所有的技术和科学术语与属于发明的技术领域的技术人员通常理解的含义相同。本文中在发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制发明。本文所使用的术语“及/或”包括一个或多个相关的所列项目的任意的和所有的组合。

如图1所示，发明提供一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法，本方案提供一个输出频率高，定位精度高且抗干扰性好的定位方法。本实例选用的IMU型号为BMI160(采集角速度和加速度)，选用的GNSS型号为U-BLOX-F9P，所述的相机为RealSenseD435(为图中的Camera模块，用于采集图片)。GNSS测量的采集频率为1Hz，图片的采集频率为20Hz，角速度和加速度的采集频率为200Hz；将采集好的数据传输到计算机上进行数据融合和误差分析。

具体融合分析方法为：获取惯导数据并构建不变滤波状态方程，依据惯导数据将IMU在n时刻的状态定义成X_n＝(R_n，v_n，p_n，b_g，b_a)。

其中R_n表示n时刻IMU获取的姿态，v_n表示n时刻IMU获取的速度，p_n表示n时刻IMU获取的位置，b_g表示n时刻IMU获取的陀螺仪零偏，b_a表示n时刻IMU获取的加速度计零偏。

按照IEKF对误差的定义，任意时刻IMU的状态X和误差e都可以定义成：

其中

是IMU状态真值；X是IMU状态；e是IMU状态的误差；e_θ是IMU姿态的误差，/>

是IMU姿态的真值，/>

是IMU速度的真值，e_v是IMU速度的误差，J_r()是右雅克比函数，/>

是IMU位置的真值，e_p是IMU位置的误差，/>

是陀螺仪零偏的真值，e_bg是陀螺仪零偏的误差，/>

是加速度计零偏的真值，e_ba是加速度计零偏的误差。

依据上述状态方程可以计算出状态转移矩阵F和系统噪声分配G。

符号S(·)表示将三维向量转换为反对称矩阵的算子。

GNSS传感器接收数据度受噪声干扰，但GNSS传感器获取的当前时刻数据不受之前时刻的干扰，可将GNSS与IMU进行紧组合，通过GNSS数据对不变滤波状态方程进行观测更新(状态转移矩阵F和系统噪声分配G进行数据更新)，避免的观测更新过程中误差传递的问题，实现长时间准确定位。

GNSS与IMU进行紧组合包括有初始化GNSS和INS的坐标系。及将GNSS和IMU转换至统一坐标系。由于INS和视觉均为局部坐标系，后续会将NS/视觉进行紧组合，以获取VIO(视觉惯性系统)位姿，为简化计算过程，可将GNSS的全球坐标系转到INS/视觉的局部坐标系。

GNSS的坐标系的转化方法为：用VIO(视觉惯性系统)位姿和GNSS估计的位置进行轨迹对齐，获得全球坐标系与VIO局部坐标系之间的转换参数。用此参数可以将GNSS传感器的观测值从全球坐标系转换到局部坐标系下。

由于GNSS的观测值在全球坐标系E下，IMU和视觉相机为于传感器坐标系，视觉SLAM(机器人同时定位与建模)本身构建的是一个局部坐标系下的位姿与地图(虽然常被称作世界坐标系W)。于是全球坐标系E和世界坐标系W之间的转换参数需要在线估计。

INS/视觉的全球坐标系E很容易转成到东北天坐标N。首先构建W和N之间的转换关系的方程组，根据方程组即可完成W和N之间的转换，以将GNSS的全球坐标系转换至东北天坐标N。坐标系统一后，使用时序差分GNSS伪距、载波相位、多普勒观测值构建不变滤波下的观测方程。

时序差分GNSS伪距测量：

z_Dp为k+1时刻的伪距测量减去k时刻的伪距测量，Δd_Dr，s为k+1时刻的卫星接收机距离减去k时刻卫星接收机距离。br为接收机钟差，bs为卫星钟差，n_p为伪距噪声和多路径等误差。E表示为全球坐标系。

时序差分GNSS载波相位测量：

z_Dc为k+1时刻的载波相位测量减去k时刻的载波相位测量，n_c为载波相位噪声。

z_d为任一时刻的多普勒观测值，λ为载波波长，k为接收机到卫星位置的单位矢量，

为卫星速度，/>

为接收机速度，/>

为接收机钟漂，/>

为卫星钟漂，n_d为多普勒噪声。

将获取的z_d、z_Dc、z_Dp输入至卡尔曼滤波器软件内，对不变滤波状态方程进行状态更新，可有效的避免了由于误差叠加导致数据发散的问题，实现长效高频定位。

为能够进一步提高定位的准确性，可获取若干视觉特征数据，以多状态约束卡尔曼框架下的观测对基于IMU传感器数据所构建的不变滤波状态方程进行观测更新，提高定位的准确性。C_ti表示ti是时刻相机的位置姿态，

获得当前帧图像(相机获取的数据)之后，首先提取图像FAST特征，然后跟踪特征。跟踪特征时使用IMU积分计算先前帧和当前帧的相对旋转运动粗略估值，之后整理并存储先前图像的特征点编号、坐标、存在时间以生成先前帧特征点信息，然后使用先前帧特征像素坐标和相机内参预测当前帧特征点的像素坐标。

然后构建金字塔、将提取的特征和预测的特征位置用基于光流跟踪算法给出精确特征位置，并给出哪些点被跟踪，哪些点没有跟踪。对被跟踪的特征点做两点RANSAC(RANSAC算法的原理：https：//blog.csdn.net/baidu_38172402/article/details/83414912)，剔除外点。由于每次跟踪会导致特征点数目下降，在跟踪之后需要添加新的特征。在当前特征之外再次提取特征。当一帧新图像数据到来时，要把当前相机的位姿误差添加到状态变量中，此举构成了多状态约束。(视角与惯性紧组合的原理在An Invariant-EKFVINS Algorithm for Improving Consistency期刊中已公开)。

在添加相机位姿的同时，其协方差按照以下增广：

在不变滤波定义下的相机测量模型为：

其中，

利用零空间技巧得到视觉测量的残差方程：

/>

视觉测量雅可比为：

其中

不变滤波状态方程状态转移计算时(每次定位时)，获取GNSS传感器的钟差和钟速数据，以使视觉惯性系统的时间与GNSS传感器时间同步。

以上对发明的实施例进行了详细说明，但所述内容仅为发明的较佳实施例，不能被认为用于限定发明的实施范围。凡依发明范围所作的均等变化与改进等，均应仍归属于本专利涵盖范围之内。

Claims (6)

1.一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法，其特征在于，包括有，

步骤一，使用时序差分获取GNSS观测值构建不变滤波下的卫星观测方程；

步骤二，获取INS惯导数据并构建不变滤波状态方程，使用卫星观测方程对不变滤波状态方程进行观测更新。

2.根据权利要求1所述的一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法，其特征在于，构建所述不变滤波状态方程包括有：定义IMU传感器在n时刻的状态数据X_n＝(R_n,v_n,p_n,b_g,b_a)，其中R_n表示n时刻IMU获取的姿态，v_n表示n时刻IMU获取的速度，p_n表示n时刻IMU获取的位置，b_g表示n时刻IMU获取的陀螺仪零偏，b_a表示n时刻IMU获取的加速度计零偏；

按照IEKF对误差的定义，任意时刻IMU的状态X和误差e都可以定义成：

其中

是IMU状态真值；X是IMU状态；e是IMU状态的误差；e_θ是IMU姿态的误差，/>

是IMU姿态的真值，/>

是IMU速度的真值，e_v是IMU速度的误差，J_r()是右雅克比函数，/>

是IMU位置的真值，e_p是IMU位置的误差，/>

是陀螺仪零偏的真值，e_bg是陀螺仪零偏的误差，/>

是加速度计零偏的真值，e_ba是加速度计零偏的误差。

3.根据权利要求1所述的一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法，其特征在于，获取视觉传感器数据并设定视角特征点，对视觉特征点进行特征提取并跟踪，并以多状态约束卡尔曼框架下的观测对步骤二所构建的不变滤波状态方程进行观测更新。

4.根据权利要求1所述的一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法，其特征在于，所述步骤一之前包括有GNSS坐标系初始化，以将GNSS的坐标系转换至INS、视角坐标系下。

5.根据权利要求1所述的一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法，其特征在于，所述不变滤波状态方程状态转移计算时，包括有获取GNSS传感器的钟差和钟速数据，以使视觉惯性系统的时间与GNSS传感器时间同步。

6.根据权利要求1所述的一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法，其特征在于，所述卫星观测方程包括有伪距观测方程、载波相位观测方程和多普勒观测方程；

所述伪距观测方程用于获取两相邻时刻之间的伪距变化量z_Dp，所述载波相位观测方程用于获取两相邻时刻之间载波相位的变化量z_Dc，所述多普勒观测方程用于获取任一时刻的多普勒观测值z_d。

Images