CN110068325A - 一种车载ins/视觉组合导航系统的杆臂误差补偿方法 - Google Patents

Abstract

本发明涉及一种车载INS/视觉组合导航系统的杆臂误差补偿方法，本发明技术解决的问题是：通过考虑INS和视觉作为导航子系统的数据信号优劣差异分别处理，和两者由于安装位置不重合引起杆臂误差，并在测量方程中补偿组合导航系统的INS与摄像头安装位置不同导致的杆臂误差，滤波器输出的陀螺和加速度零偏反馈至器件补偿处进行校正，将姿态反馈至姿态更新补偿，将速度、位置误差反馈至INS解算的输出值上进行校正，能够有效提高INS/视觉组合导航系统的精度。

Description

一种车载INS/视觉组合导航系统的杆臂误差补偿方法

技术领域

本发明涉及车载导航与定位技术领域，尤其是涉及一种车载INS/视觉组合导航系统的杆臂误差补偿方法。

背景技术

惯性导航系统(Inertial Navigation System，INS)具有高自主性、抗干扰性、高的短期精度、高数据输出率、完备的导航信息、适应范围广等特点，但是其系统误差具有周期振荡的特性，某些导航参数误差具有随时间积累的特性且初始对准所需时间较长；而视觉里程计由于采用递推的方式求解位姿，视觉里程计不可避免地存在累积误差，且这种累积误差和基于光电编码器的里程计类似，是空间相关而非时间相关的；并且当运动体机动过大时，会由于图像场景变化过大而造成匹配失效进而影响定位精度。视觉里程计通常利用视觉传感器获取的图像数据提取特征点进行匹配或通过直接法在灰度不变假设下进行求解位姿，而进行特征点的提取与匹配需要耗费大量的时间，对处理器具有极高的要求，难以实现实时运算。考虑到两者的互补性，基于车载INS和视觉里程计的组合导航受到广泛的重视.

然而，可选用的组合信息很多，包括速度、位置、姿态，如果将这些信息全部组合，虽然精度可以达到甚至超过要求，但是计算量巨大，完全无法在实际中使用，如果仅选取部分信息组合，那么选取以哪一个传感器为主，建立系统方程，采用信息的类型、组合的先后顺序、具体的组合方式都对结果的精度产生巨大影响。现有技术中尚没有一种比较完美的组合方式，能够既兼顾到计算量，又可以最大程度的减少误差。而当前的导航参数误差反馈校正方案根据校正方法以及校正的状态参数分为：混合校正(初始采用输出校正，后期采用反馈校正)、不完全反馈(只对位置、速度、姿态误差进行反馈校正)和完全反馈校正(对位置、速度、姿态误差以及惯性器件随机常值误差进行反馈校正)。

现有INS和视觉里程计的组合导航大多直接使用视觉里程计输出的姿态速度位置与INS对应输出量做差作为测量量进行反馈，但是该方法未考虑由于INS与摄像头安装位置不同导致的杆臂误差影响，降低了组合导航系统的精度。

如专利申请号201810477871的一种基于量测模型优化的惯性/视觉里程计组合导航定位方法公开了如下内容：“首先建立组合导航系统的状态方程，将惯性传感器误差扩展为系统状态变量，包括陀螺的随机常数漂移、陀螺仪的一阶马尔科夫过程漂移和加速度计的一阶马尔科夫过程漂移；随后，将视觉里程计作为角速度、线速度以及位置传感器以获取量测数据进而构建量测方程；最后在载体运动过程中对导航误差进行实时反馈校正，获得误差校正后的惯性导航系统导航结果。本发明方法能够在载体运动过程中有效利用视觉里程计的角速度、线速度和位置信息,实现与惯性导航的有效融合，提高组合导航系统的精度和可靠性，适用于工程应用。”

如专利申请号201711026822的一种捷联惯性导航系统/视觉里程计组合导航方法公开了如下内容：“一种捷联惯性导航系统/视觉里程计组合导航方法，利用非线性滤波器容积卡尔曼滤波完成捷联惯性导航系统/视觉里程计组合导航系统的时间更新和量测更新，对系统状态进行估计，实现捷联惯性导航系统/视觉里程计组合导航系统的导航定位。本发明方法优化了特征匹配算法，并利用非线性容积卡尔曼滤波算法，提高了组合导航系统的定位精度和鲁棒性。”

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种车载INS/视觉组合导航系统的杆臂误差补偿方法。

本发明的目的可以通过以下技术方案来实现：

一种车载INS/视觉组合导航系统的杆臂误差补偿方法，包括以下步骤：

步骤1：车辆静止时，利用测量仪器测量摄像头集合中心与惯组中心在车辆坐标系下的杆臂值；

步骤2：车辆静止状态下进行INS的初始化和摄像头的标定；

步骤3：采集车辆行驶过程中的INS原始导航数据和摄像头数据；

步骤4：将INS原始导航数据进行器件补偿、姿态解算和导航计算后将得到速度增量姿态、速度和位置均输入到组合滤波器中；

步骤5：将摄像头数据进行畸变矫正、特征提取与匹配、特征点跟踪，将获取的相机位姿估计的角速度、线速度以及位置均输入到组合滤波器中；

步骤6：当组合滤波器中的数据输入完毕后，建立组合导航系统的状态方程，对状态方程进行估计，采用误差状态向量具体包括位置、速度、姿态、陀螺随机常值漂移ε、加速度计随机常值零偏的共15维误差状态量，对15维误差状态向量进行估计，每次滤波后，利用滤波估计出的位置误差速度误差失准角误差陀螺随机常值漂移加速度计随机常值零偏结果对INS解算结果进行反馈校正。

优选地，所述的步骤4中速度增量姿态、速度和位置采用二子样圆锥误差补偿算法计算，其对应的计算方程组为：

式中，Δθ_m1和Δθ_m2为陀螺两次等间隔采样对应的角增量，T为采样时间，为以惯性坐标系作为参考基准，载体系从t_m-1时刻到t_m时刻的旋转变化，为以惯性坐标系作为参考基准，地理系从t_m时刻到t_m-1时刻的旋转变化，下标i表示惯导系统解算值，上下标b表示载体系，上下标n表示地理系，(m)表示t_m时刻，(m-1)表示t_m-1时刻，φ带上下标表示对应姿态，I表示单位矩阵，为常值。

优选地，所述步骤6包括以下分步骤：

步骤61：建立系统方程；

步骤62：建立量测方程；

步骤63：建立kalman滤波系统方程并使量测方程离散化；

步骤64：利用kalman滤波系统方程进行反馈校正。

优选地，所述步骤61中的系统方程，其描述公式为：

式中，X为状态向量，φ_E、φ_N和φ_U分别为东-北-天地理坐标系下的姿态误差，δv_E、δv_N和δv_U分别为东-北-天地理坐标系下的速度误差，δL、δλ和δh为经度、纬度和高度的位置误差，ε_x、ε_y和ε_z分别为陀螺仪三个坐标轴的零偏，和分别为加速度计的三个坐标轴的零偏；

式中，为地理系相对于惯性系的角速度，为地球系相对于惯性系的角速度误差，为地理系相对于地球系的角速度误差，为载体系到地理系的坐标变换矩阵，为载体系相对于惯性系的角速度误差，为载体系相对于地理系下的惯导系统加速度计的输出比力，vⁿ为载体在地理系下的速度，为地球系相对于惯性系的角速度，为地理系相对于地球系的角速度，δvⁿ为载体在地理系下的速度误差，为载体系相对于地理系下的惯导系统加速度计的输出比力误差，δgⁿ为重力加速度误差，R_M为卯酉圈半径，h为当地海拔高度，L为当地纬度，R_N为子午圈半径，单独φ表示捷联惯导系统中的数学平台误差角。

优选地，所述步骤62中的量测方程，其描述公式为：

式中，Z为量测方程，上标n表示地理系，下标INS表示惯性系统，下标camera表示摄像头，上标～表示实际值，v表示速度，p表示位置，表示载体系相对于地球系的角速度，δl^b表示杆臂值，R_Mh＝R_M+h，R_Nh＝R_N+h。

优选地，所述步骤6还包括：将kalman滤波后的陀螺和加速度零偏反馈至器件补偿处进行校正，将姿态反馈至姿态更新补偿处，将速度、位置误差反馈至INS解算的输出值上进行校正，即：由修正后的可解得航向角ψ、俯仰角θ和滚动角γ，一次滤波反馈后，误差状态归0。

优选地，所述由修正后的可解得航向角ψ、俯仰角θ和滚动角γ，其对应的描述公式为：

式中，(数字1，数字2)表示矩阵内具体对应的矩阵元素。

与现有技术相比，本发明具有以下优点：

(1)本发明中，考虑了INS和视觉作为导航子系统的数据信号优劣差异分别处理，并且考虑两者由于安装位置不重合引起杆臂误差变动，对杆臂误差进行补偿，提高了INS/视觉组合导航系统的精度。

附图说明

图1为本发明INS惯组中心与摄像头几何中心相对位置的杆臂简图；

图2为本发明中组合导航系统结构框图。

具体实施方式

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

实施例

一种车载INS/视觉组合导航系统的杆臂误差补偿方法主要分为四个阶段：杆臂测量和INS的初始化、INS和视觉里程计的数据处理、15维系统误差状态向量的传感器数据滤波融合，得到的状态估计对INS的导航输出结果以及自身器件误差进行反馈校正。

本发明的具体实施步骤如下，如图2所示：

(1)车辆静止时，利用测量仪器测量摄像头几何中心与惯组(IMU)中心在车辆坐标系下的杆臂值δl^b，如图1所示；

(2)车辆静止状态下进行INS的初始化和摄像头的标定；

(3)车辆行驶过程采集数据，惯性测量数据：三轴陀螺仪数据和三轴加速度计数据三轴比力和摄像头输出数据；

(4)将步骤(3)中的INS原始导航数据，进行器件补偿、姿态解算和导航计算后将得到速度增量姿态、速度和位置等信息，均输入到组合滤波器中；

一.步骤(4)中的姿态计算

选“东-北-天(E-N-U)”地理坐标系(g系)作为捷联惯导系统的导航参考坐标系，重新记为n系，则以n系作为参考系的姿态微分方程为：

其中，矩阵表示以i系(惯性坐标系)作为参考基准，b系从t_m-1时刻到t_m时刻的旋转变化，可由陀螺角速度确定；表示以i系作为参考基准，n系从t_m时刻到t_m-1时刻的旋转变化，可由计算角速度确定，和分别表示t_m-1和t_m时刻的捷联姿态矩阵。若陀螺在时间段[t_m-1,t_m]内(T＝t_m-t_m-1)进行了两次等间隔采样，角增量分别为Δθ_m1和Δθ_m2，采用二子样圆锥误差补偿算法，有：

取四阶截断，并取近似：

导航更新周期[t_m-1,t_m]内，可以认为由速度和位置引起的变化很小，即可视为常值，记为则有：

二.发明方案中步骤6中的滤波解算

2.1建立系统方程

其中：X：误差状态向量；

F：系统矩阵；

G：噪声分配矩阵；

w：零均值高斯白噪声向量；

Z：量测向量；

H：量测矩阵；

V：量测噪声状态向量；

相关上下标位置处的b表示载体系，n表示地理系，e表示地球系，i表示惯性系。

式中，X为状态向量，φ_E、φ_N和φ_U分别为东-北-天地理坐标系下的姿态误差，δv_E、δv_N和δv_U分别为东-北-天地理坐标系下的速度误差，δL、δλ和δh为经度、纬度和高度的位置误差，ε_x、ε_y和ε_z分别为陀螺仪三个坐标轴的零偏，和分别为加速度计的三个坐标轴的零偏；

式中，为地理系相对于惯性系的角速度，为地球系相对于惯性系的角速度误差，为地理系相对于地球系的角速度误差，为载体系到地理系的坐标变换矩阵，为载体系相对于惯性系的角速度误差，为载体系相对于地理系下的惯导系统加速度计的输出比力，vⁿ为载体在地理系下的速度，为地球系相对于惯性系的角速度，为地理系相对于地球系的角速度，δvⁿ为载体在地理系下的速度误差，为载体系相对于地理系下的惯导系统加速度计的输出比力误差，δgⁿ为重力加速度误差，R_M为卯酉圈半径，h为当地海拔高度，L为当地纬度，R_N为子午圈半径，单独φ表示捷联惯导系统中的数学平台误差角。

载体系下的陀螺零偏：

载体系下的加速计零偏：

以下将方程(姿态-速度-位置)依次展开：

其中

其中：

为陀螺测量误差，m带不同a、x、y、z下标表示为陀螺测量中的两轴间的交叉耦合系数，s带a、x、z下标表示为陀螺测量中的比例因子误差。

其中：

为加速度计测量误差，m带不同g、x、y、z下标表示为加速度计测量中的交叉耦合系数，s带g、x、z下标表示为加速度计测量中比例因子误差。

根据WGS-84(World Geodetic System 1984)地球系给出的地球参数为：

半长轴：R_e＝6378137m，扁率：f＝1/298.257223563，

地心引力常数(含大气层)：μ＝3.986004418×10¹⁴m³/s²，

地球自转角速率：ω_ie＝7.2921151467×10^-5rad/s

g_e和g_p分别为赤道重力和极点重力，地球重力扁率为：

β₁表示与赤道重力的比值：

β₂表示重力随高度下降梯度：

整理有式1-5有：

F₁₅＝0_3×3

F₂₄＝0_3×3，F₃₁＝0_3×3

F₃₄＝0_3×3，F₃₅＝0_3×3，F₄₁＝F₄₂＝F₄₃＝F₄₄＝F₄₅＝F₅₁＝F₅₂＝F₅₃＝F₅₄＝F₅₅＝0_3×3

2.2建立量测方程

INS至摄像头几何中心的杆臂误差、姿态误差、位置误差和速度误差表示如下：

式中，Z为量测方程，上标n表示地理系，下标INS表示惯性系统，下标camera表示摄像头，上标～表示实际值，v表示速度，p表示位置，表示载体系相对于地球系的角速度，δl^b表示杆臂值，R_Mh＝R_M+h，R_Nh＝R_N+h。

整理有：

2.3Kalman滤波系统方程和量测方程离散化

做近似离散化等效：

X_k＝Φ_k/k-1X_k-1+Γ_k-1W_k-1

其中设定离散化时间间隔T_s＝t_k-t_k-1，则状态转移矩阵取一阶截断，有：

W_k-1为系统噪声向量，V_k为量测噪声向量，两者都是零均值的高斯白噪声向量序列(服从正态分布)，且它们之间互不相关，即满足：

{E[W_k]＝0,E[V_k]＝0,

在Kalman滤波状态空间模型中对于噪声要求的基本假设，一般要求Q_k是半正定的且R_k是正定的，即Q_k≥0且R_k＞0。Kalman滤波全套算法，可划分为五个基本公式，如下：

(1)状态一步预测

(2)状态一步预测均方误差

(3)滤波增益

(4)状态估计

(5)状态估计均方误差

P_k＝(I-K_kH_k)P_k/k-1

2.4反馈校正

将kalman滤波后的陀螺和加速度零偏反馈至器件补偿处进行校正，将姿态反馈至姿态更新补偿处，将速度、位置误差反馈至INS解算的输出值上进行校正，反馈后，误差状态归0。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (7)

1.一种车载INS/视觉组合导航系统的杆臂误差补偿方法，其特征在于，包括以下步骤：

步骤1：车辆静止时，利用测量仪器测量摄像头集合中心与惯组中心在车辆坐标系下的杆臂值；

步骤2：车辆静止状态下进行INS的初始化和摄像头的标定；

步骤3：采集车辆行驶过程中的INS原始导航数据和摄像头数据；

步骤4：将INS原始导航数据进行器件补偿、姿态解算和导航计算后将得到速度增量姿态、速度和位置均输入到组合滤波器中；

步骤5：将摄像头数据进行畸变矫正、特征提取与匹配、特征点跟踪，将获取的相机位姿估计的角速度、线速度以及位置均输入到组合滤波器中；

步骤6：当组合滤波器中的数据输入完毕后，建立组合导航系统的状态方程，对状态方程进行估计，每次滤波后，利用滤波估计出的结果对INS解算结果进行反馈校正。

2.根据权利要求1所述的一种车载INS/视觉组合导航系统的杆臂误差补偿方法，其特征在于，所述的步骤4中速度增量姿态、速度和位置采用二子样圆锥误差补偿算法计算，其对应的计算方程组为：

式中，Δθ_m1和Δθ_m2为陀螺两次等间隔采样对应的角增量，T为采样时间，为以惯性坐标系作为参考基准，载体系从t_m-1时刻到t_m时刻的旋转变化，为以惯性坐标系作为参考基准，地理系从t_m时刻到t_m-1时刻的旋转变化，下标i表示惯导系统解算值，上下标b表示载体系，上下标n表示地理系，(m)表示t_m时刻，(m-1)表示t_m-1时刻，φ带上下标表示对应姿态，I表示单位矩阵，为常值。

3.根据权利要求1所述的一种车载INS/视觉组合导航系统的杆臂误差补偿方法，其特征在于，所述步骤6包括以下分步骤：

步骤61：建立系统方程；

步骤62：建立量测方程；

步骤63：建立kalman滤波系统方程并使量测方程离散化；

步骤64：利用kalman滤波系统方程进行反馈校正。

4.根据权利要求3所述的一种车载INS/视觉组合导航系统的杆臂误差补偿方法，其特征在于，所述步骤61中的系统方程，其描述公式为：

X＝[φ_E φ_N φ_U δv_E δv_N δv_U δL δλ δh ε_x ε_y ε_z ▽_x ▽_y ▽_z]^T

式中，X为状态向量，φ_E、φ_N和φ_U分别为东-北-天地理坐标系下的姿态误差，δv_E、δv_N和δv_U分别为东-北-天地理坐标系下的速度误差，δL、δλ和δh为经度、纬度和高度的位置误差，ε_x、ε_y和ε_z分别为陀螺仪三个坐标轴的零偏，▽_x、▽_y和▽_z分别为加速度计的三个坐标轴的零偏；

式中，为地理系相对于惯性系的角速度，为地球系相对于惯性系的角速度误差，为地理系相对于地球系的角速度误差，为载体系到地理系的坐标变换矩阵，为载体系相对于惯性系的角速度误差，为载体系相对于地理系下的惯导系统加速度计的输出比力，vⁿ为载体在地理系下的速度，为地球系相对于惯性系的角速度，为地理系相对于地球系的角速度，δvⁿ为载体在地理系下的速度误差，为载体系相对于地理系下的惯导系统加速度计的输出比力误差，δgⁿ为重力加速度误差，R_M为卯酉圈半径，h为当地海拔高度，L为当地纬度，R_N为子午圈半径，单独φ表示捷联惯导系统中的数学平台误差角。

5.根据权利要求4所述的一种车载INS/视觉组合导航系统的杆臂误差补偿方法，其特征在于，所述步骤62中的量测方程，其描述公式为：

式中，Z为量测方程，上标n表示地理系，下标INS表示惯性系统，下标camera表示摄像头，上标～表示实际值，v表示速度，p表示位置，表示载体系相对于地球系的角速度，δl^b表示杆臂值，R_Mh＝R_M+h，R_Nh＝R_N+h。

6.根据权利要求1所述的一种车载INS/视觉组合导航系统的杆臂误差补偿方法，其特征在于，所述步骤6还包括：将kalman滤波后的陀螺和加速度零偏反馈至器件补偿处进行校正，将姿态反馈至姿态更新补偿处，将速度、位置误差反馈至INS解算的输出值上进行校正，即：由修正后的可解得航向角ψ、俯仰角θ和滚动角γ，一次滤波反馈后，误差状态归0。

7.根据权利要求6所述的一种车载INS/视觉组合导航系统的杆臂误差补偿方法，其特征在于，所述由修正后的可解得航向角ψ、俯仰角θ和滚动角γ，其对应的描述公式为：

式中，(数字1，数字2)表示矩阵内具体对应的矩阵元素。