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

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

【技术实现步骤摘要】
一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法


[0001]专利技术涉及导航定位
，具体涉及一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法。

技术介绍

[0002]自动驾驶应用越来越广泛，为了实现更好的无人化和智能化的管理，实时获得自动驾驶车辆的位置、速度、姿态等状态是必不可少的。目前主要的定位方法有卫星导航、视觉同步定位与制图(SLAM)和惯性导航等。
[0003]卫星导航通过测量卫星到接收机之间伪距，载波相位，多普勒三类观测值，进行定位。卫星导航除了估计接收机在大地坐标系下的坐标和速度外，还需要估计接收机时钟误差和接收机时钟漂移。视觉SLAM技术通过使用图像匹配的技术计算两帧图像之间的旋转和平移参数。惯性导航使用惯性测量单元(IMU)测量自身的角速度和加速度，然后通过惯导算法(INS)实现定位。
[0004]然而GNSS受噪声干扰严重，频率低；视觉SLAM局部坐标精度高，但是无法得到全球坐标，频率也不高且随着距离发散；IMU频率高，但是发散的很快。
[0005]现有技术中通常将视觉同步定位和惯性导航组合导航，常规组合导航采用的是扩展卡尔曼滤波器(EKF)，普通的EKF通过对动态方程的线性化，来估计状态与状态的协方差。但是存在一个问题，误差传递方程中，系统矩阵依赖当前状态的估计量。在有噪声引入时，状态估计量是无法预测的，这就导致很难对系统方程做收敛性分析，实际上，任何EKF都没有收敛性保证。当状态估计值与真值差距较大时，直接导致依赖状态估计值的系统方程也有较大偏差，使用这样的系统方程继续传递误差后，又进一步放大误差，整个误差传递系统形成正反馈，最终导致滤波器发散。
[0006]实际上滤波器进行状态更新时也有类似问题，EKF还会导致不一致性。每当新的观测到来时，EKF会更新当前状态的估计。但是用于计算新状态的量，都是通过旧状态的线性化得来的。因此，EKF的更新方法会出现不一致性，导致本来不该观测到信息的方向上观测到信息，在SLAM问题中这种现象比较明显，表现为较大的漂移，以及过于乐观的协方差估计。

技术实现思路

[0007]有鉴于此，专利技术要解决的问题是提供一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法，能够将不变扩展卡尔曼滤波理论应用于多状态约束卡尔曼滤波框架中，使卫星导航、视觉同步定位与制图(SLAM)、惯性导航三类定位方式紧组合，通过不变扩展卡尔曼滤波改变状态误差的定义方法，实现误差传递矩阵与状态估计值的独立，可对三类定位进行优势互补，实现不易发散的高频高精度定位。
[0008]为解决上述技术问题，专利技术采用的技术方案是：
[0009]一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法，包括有，步骤一，使用
时序差分获取GNSS观测值构建不变滤波下的卫星观测方程；
[0010]步骤二，获取INS惯导数据并构建不变滤波状态方程，使用卫星观测方程对不变滤波状态方程进行观测更新。
[0011]进一步的，构建所述不变滤波状态方程包括有：定义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获取的加速度计零偏；
[0012]按照IEKF对误差的定义，任意时刻IMU的状态X和误差e都可以定义成：
[0013][0014]其中是IMU状态真值；X是IMU状态；e是IMU状态的误差；e
θ
是IMU姿态的误差，是IMU姿态的真值，是IMU速度的真值，e
v
是IMU速度的误差，J
r
()是右雅克比函数，是IMU位置的真值，e
p
是IMU位置的误差，是陀螺仪零偏的真值，e
bg
是陀螺仪零偏的误差，是加速度计零偏的真值，e
ba
是加速度计零偏的误差。
[0015]进一步的，获取视觉传感器数据并设定视角特征点，对视觉特征点进行特征提取并跟踪，并以多状态约束卡尔曼框架下的观测对步骤二所构建的不变滤波状态方程进行观测更新。
[0016]进一步的，所述步骤一之前包括有GNSS坐标系初始化，以将GNSS的坐标系转换至INS坐标系下。
[0017]进一步的，所述不变滤波状态方程状态转移计算时，包括有获取GNSS传感器的钟差和钟速数据，以使视觉惯性系统的时间与GNSS传感器时间同步。
[0018]进一步的，所述卫星观测方程包括有伪距观测方程、载波相位观测方程和多普勒观测方程；
[0019]所述伪距观测方程用于获取两相邻时刻之间的伪距变化量z
Dp
，所述载波相位观测方程用于获取两相邻时刻之间载波相位的变化量z
Dc
，所述多普勒观测方程用于获取任一时刻的多普勒观测值z
d
。
[0020]专利技术具有的优点和积极效果是：
[0021]通过将不变扩展卡尔曼滤波理论应用于多状态约束卡尔曼滤波框架中，依据IMU传感器数据构建不变卡尔曼滤波方程，实现误差传递矩阵与状态估计值的独立，并使用GNSS传感器数据构建不变卡尔曼录波下的卫星观测方程，卫星观测方程的计算数值输入至卡尔曼滤波器内，以对不变卡尔曼滤波方程进行观测更新(误差传递矩阵进行更新)，由于GNSS传感器数据不受时间传递影响，以实现长时间定位准确定位的效果。
[0022]同时视觉SLAM局部坐标精度高，可将视觉导航与惯性导航紧组合构建视觉惯性系统，进一步提高定位精度。
附图说明
[0023]附图用来提供对专利技术的进一步理解，并且构成说明书的一部分，与专利技术的实施例一起用于解释专利技术，并不构成对专利技术的限制。在附图中：
[0024]图1是专利技术的一种基于不变滤波的GNSS、INS和视觉紧组合导航定位方法的整体原理图。
具体实施方式
[0025]下面将结合专利技术实施例中的附图，对专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是专利技术一部分实施例，而不是全部的实施例。基于专利技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于专利技术保护的范围。
[0026]需要说明的是，当组件被称为“固定于”另一个组件，它可以直接在另一个组件上或者也可以存在居中的组件。当一个组件被认为是“连接”另一个组件，它可以是直接连接到另一个组件或者可能同时存在居中组件。当一个组件被认为是“设置于”另一个组件，它可以是直接设置在另一个组件上或者可能同时存在居中组件。本文所使用的术语“垂直的”、“水平的”、“左”、“右”以及类似的表述只本文档来自技高网...

【技术保护点】

【技术特征摘要】
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

【专利技术属性】
技术研发人员：高同跃，李利明，
申请(专利权)人：苏州华米导航科技有限公司，
类型：发明
国别省市：