CN110501024B - 一种车载ins/激光雷达组合导航系统的量测误差补偿方法 - Google Patents

Abstract

本发明涉及一种车载INS/激光雷达组合导航系统的量测误差补偿方法，本发明的技术解决的问题是：以激光雷达辅助惯性导航系统进行车辆的组合导航系统，系统考虑INS与激光雷达两个系统的安装偏置角和杆臂误差，并据此作为组合导航系统量测方程中的测量量进行修正，构建以惯导位置、速度、姿态误差以及惯性器件随机常值误差为状态量的滤波系统，并进行反馈校正，实现基于量测误差补偿的组合导航定位精度提高。

Description

一种车载INS/激光雷达组合导航系统的量测误差补偿方法

技术领域

本发明涉及车载导航与定位技术领域，尤其是涉及一种车载INS/激光雷达组合导航系统的量测误差补偿方法。

背景技术

惯性导航系统(Inertial Navigation System，INS)具有高自主性、抗干扰性、高的短期精度、高数据输出率、完备的导航信息、适应范围广等特点，但是其系统误差具有周期振荡的特性，某些导航参数误差具有随时间积累的特性且初始对准所需时间较长；而激光雷达(Light Detection and Ranging,LiDAR)以其高采样频率、高精度和低运算量、不受环境光照影响、不需要对环境进行改造等优势被中外学者广泛用来辅助惯性导航。但在实际应用中，由于设备体积、安装误差等诸多限制，往往无法在物理上实现设备的坐标系重合，即激光雷达系统与惯性导航系统之间存在安装偏置角和杆臂误差，两个坐标系之间存在坐标系的旋转/平移变换，即量测一致性问题。导航中要准确获取外部环境数据，就必须知道旋转/平移变换参数，这些参数可以通过事后修正和预先标定两种方式解决。在激光雷达/惯性组合导航系统中，预先标定方法更符合实际需求，预先标定方法一种是通过精确测量，另一种是通过跑动实验后对参数进行估计，估计方法包括平均控制法(ACS)、普通最小二乘法(NLS)和广义最小二乘法(GLS)进行参数估计。

激光雷达/惯性组合导航主要有三种模式：1)空载系统的地形匹配导航模式；2)地面系统的激光雷达扫描匹配辅助导航；3)基于几何特征(地标)的激光雷达辅助导航即基于特征地标的滤波估计模式。在车辆导航相对定位中多以INS为主，其他导航定位方式做辅助，即第二种模式激光雷达扫描匹配辅助导航。同时组合导航系统的导航参数误差反馈校正方案根据校正方法以及校正的状态参数分为：混合校正(初始采用输出校正，后期采用反馈校正)、不完全反馈(只对位置、速度、姿态误差进行反馈校正)和完全反馈校正(对位置、速度、姿态误差以及惯性器件随机常值误差进行反馈校正)。由于惯性器件随机常值误差的校正对于系统输出影响显著，此时必须考虑惯性器件随机常值误差的反馈校正方案，由于现有车载的INS/激光雷达组合导航系统未考虑量测误差，大多使用激光雷达输出位姿估计与INS对应输出量做差作为测量量进行反馈，但是车辆应用中未考虑由于INS与激光雷达安装位置不同导致的量测误差影响和进行补偿，降低了组合导航系统的精度。

如专利申请号201510727853的一种惯性/视觉里程计/激光雷达的组合导航方法公开了如下内容：“本发明使用机器视觉的自主导航技术，单目相机通过前后帧图像的差异，在已知距离的情况下可以测量出载体的速度；激光雷达可以精确测量出到观测点的距离，然后测出载体的速度，利用测量得到的速度与惯导速度组合导航，最终能在无外界参考信息输入情况下实现高精度导航。”专利中未考虑不同导航子系统的量测误差以及在测量方程中进行补偿。

INS/激光雷达组合导航系统方案存在以下不足：

缺乏对安装于车辆上的惯性导航系统与激光雷达测试系统进行量测一致性参数标定与估计；

缺乏对由于量测坐标系间存在旋转与平移造成的误差进行误差补偿。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种车载INS/激光雷达组合导航系统的量测误差补偿方法，该方法以激光雷达辅助惯性导航系统进行车辆的组合导航系统，系统考虑INS与激光雷达两个系统的安装偏置角和杆臂误差，并据此作为组合导航系统量测方程中的测量量进行修正，构建以惯导位置、速度、姿态误差以及惯性器件随机常值误差为状态量的滤波系统，并进行反馈校正，实现基于量测误差补偿的组合导航定位精度提高。

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

一种车载INS/激光雷达组合导航系统的量测误差补偿方法，包括以下步骤：

步骤1：车辆静止时进行INS的初始化，并利用测量仪器和相关图纸对杆臂误差进行测量标定得到初值；

步骤2：设定参考点并获取激光雷达对参考点的观测和惯性导航系统的位置，构建不同坐标系下的空间矢量量测，利用已知的激光雷达量测几何模型构造方程，借助GPS推导确定旋转和平移参数的最优估计；

步骤3：采集车辆行驶过程中的INS原始导航数据和激光雷达数据；

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

步骤5：将激光雷达数据进行运动畸变去除、特征提取与匹配、特征点跟踪，将获取的位姿估计的速度、位移和姿态变量均输入到组合滤波器中；

步骤6：当组合滤波器的数据输入完毕后，建立组合导航系统的状态方程，采用误差状态向量具体包括位置、速度、姿态、陀螺随机常值漂移ε、加速度计随机常值零偏▽的共15维误差状态量，对状态方程进行估计，用两个系统结算的速度，位置与姿态差值并补偿量测误差后结果作为测量量，每次滤波后，利用滤波估计出的位置误差

速度误差/>

失准角误差/>

陀螺随机常值漂移/>

加速度计随机常值零偏/>

结果对INS解算结果进行反馈校正。

进一步地，所述步骤2中的激光雷达量测几何模型，其描述公式为：

式中，L为激光雷达量测几何模型，ρ为量测距离，α,β为激光量测角。

进一步地，所述步骤2中的空间矢量量测，其描述公式为：

式中，P_i为空间矢量量测，P_i ⁿ为地理系下的空间矢量量测，

为载体系到地理系的坐标变换矩阵，/>

和δl^b分别代表旋转参数和杆臂值，L_i为第i个激光雷达量测几何模型。

进一步地，所述步骤2中的最优估计对应的目标函数为：

式中，k为参考点数目。

进一步地，所述步骤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 ▽_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为子午圈半径，单独φ表示捷联惯导系统中的数学平台误差角。

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

式中，上标n表示为地理系，Z为量测方程，下标INS表示惯性系统，下标L表示激光雷达，上标～表示实际值，v表示速度，p表示位置，

表示载体系相对于地球系的角速度，R_Mh＝R_M+h，R_Nh＝R_N+h。

进一步地，所述步骤6还包括：将kalman滤波后的陀螺和加速度零偏反馈至器件补偿处进行校正，将姿态反馈至姿态更新补偿处，将速度、位置误差反馈至INS解算的输出值上进行校正，即：由修正后的

可解得航向角ψ、俯仰角θ和滚动角γ，一次滤波反馈后，误差状态归0。

进一步地，所述由修正后的

可解得航向角ψ、俯仰角θ和滚动角γ，其对应的描述公式为：

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

本发明的原理是：

在车辆行驶前，先进行惯性导航系统的初始化，再利用测量仪器初步测量INS与激光雷达两个系统的安装偏置角和杆臂误差，并据此作为组合导航系统量测方程中的测量量进行修正，构建以惯导位置、速度、姿态误差以及惯性器件随机常值误差为状态量的滤波系统，并进行反馈校正。本发明分为四个阶段，第一阶段是INS初始化阶段：此阶段采用利用外部航向信息辅助初始化导航姿态角和位置。第二阶段利用安装角误差和杆臂误差初值，设定参考点及激光雷达对参考点的观测和惯性导航系统的位置，构成不同坐标系下的空间矢量量测，利用已知的激光雷达量测几何模型构造方程，以及借助GPS，对旋转/平移参数进行非线性最优估计。第三阶段是数据采集与处理阶段，包括激光雷达的运动畸变矫正，特征识别与匹配，位姿估计和INS的器件误差补偿、姿态解算和导航计算。第四阶段为组合滤波以及反馈校正阶段，即将估计的位置误差、速度误差、姿态误差、惯性器件随机常值误差反馈至INS处进行反馈补偿。

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

(1)本发明中，考虑安装于车辆上的INS与激光雷达两个系统的安装偏置角和杆臂误差的量测一致性问题，对量测误差参数进行标定与估计，并在据此作为组合导航系统量测方程中的测量进行修正，实现基于量测误差补偿的组合导航定位精度提高。

附图说明

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

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

具体实施方式

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

实施例

本发明分为四个阶段，第一阶段是INS初始化阶段和量测误差初值测量，第二阶段利用安装角误差和杆臂误差初值，对旋转/平移参数进行非线性最优估计。第三阶段是传感器的数据采集与处理阶段，第四阶段为组合滤波以及反馈校正阶段。

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

1)车辆静止状态下进行INS的初始化和量测误差初值测量(R_ini,t_ini)，其中R_ini表示激光雷达坐标系(记作L系)相对于INS坐标系(载体系)(记作b系)的相对旋转

t_ini表示在载体坐标系下的杆臂值δl^b误差，如图1所示；

2)激光雷达系统与INS量测误差的标定，利用安装角误差和杆臂误差初值，设定一系列参考点(控制点)P₁,P₂,…P_k(参考点数目k可根据实际情况选择)，同时获取激光雷达对参考点相对于激光雷达的坐标系观测值L₁,L₂,…L_k和惯性导航系统的位置

构成不同坐标系下的空间矢量量测，利用已知的激光雷达量测几何模型构造方程，以及借助GPS，通过推导或计算确定旋转和平移的参数，即对旋转/平移参数的非线性最优估计。

激光雷达量测几何模型：

其中ρ为量测距离，α,β分别为激光量测角。

空间矢量量测：

通过联立k方程，满足如下指标，得到非线性最优估计

3)车辆行驶过程采集数据，惯性测量数据：三轴陀螺仪数据

三轴加速度计数据/>

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]内，可以认为由速度和位置引起的

变化很小，即可视/>

为常值，记为/>

则有：

5)将步骤(3)中的激光雷达数据进行包括激光雷达的运动畸变矫正，特征识别与匹配，位姿估计输入到组合滤波器中；

6)当(4)(5)中数据输入滤波器后，建立组合导航系统的状态方程，采用误差状态向量具体包括位置、速度、姿态、陀螺随机常值漂移ε、加速度计随机常值零偏▽的共15维误差状态量，对15维误差状态向量进行估计。每次滤波后，利用滤波估计出的位置误差

速度误差/>

失准角误差/>

陀螺随机常值漂移/>

加速度计随机常值零偏/>

对INS解算结果进行反馈校正。

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 ▽_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为子午圈半径，单独φ表示捷联惯导系统中的数学平台误差角。

载体系下的陀螺零偏：

载体系下的加速计零偏：

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

其中

/>

其中：

为陀螺测量误差，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分别为赤道重力和极点重力，地球重力扁率为：

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

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

设定局部坐标系下的地理信息保持不变，h≈0，

整理有式1-5有：

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.建立量测方程：

式中，上标n表示为地理系，Z为量测方程，下标INS表示惯性系统，下标L表示激光雷达，上标～表示实际值，v表示速度，p表示位置，

表示载体系相对于地球系的角速度，R_Mh＝R_M+h，R_Nh＝R_N+h。

整理有：

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

做近似离散化等效：

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

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

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

在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

4.反馈校正

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

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

Claims (6)

1.一种车载INS/激光雷达组合导航系统的量测误差补偿方法，其特征在于，包括以下步骤：

步骤1：车辆静止时进行INS的初始化，并利用测量仪器和相关图纸对杆臂误差进行测量标定得到初值；

步骤2：设定参考点并获取激光雷达对参考点的观测和惯性导航系统的位置，构建不同坐标系下的空间矢量量测，利用已知的激光雷达量测几何模型构造方程，借助GPS推导确定旋转和平移参数的最优估计；

步骤3：采集车辆行驶过程中的INS原始导航数据和激光雷达数据；

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

步骤5：将激光雷达数据进行运动畸变去除、特征提取与匹配、特征点跟踪，将获取的位姿估计的速度、位移和姿态变量均输入到组合滤波器中；

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

所述步骤2中的激光雷达量测几何模型，其描述公式为：

式中，L为激光雷达量测几何模型，ρ为量测距离，α,β为激光量测角；

所述步骤2中的空间矢量量测，其描述公式为：

式中，P_i为空间矢量量测，P_i ⁿ为地理系下的空间矢量量测，

为载体系到地理系的坐标变换矩阵，/>

和δl^b分别代表旋转参数和杆臂值，L_i为第i个激光雷达量测几何模型；

所述步骤2中的最优估计对应的目标函数为：

式中，k为参考点数目；

所述步骤6包括以下分步骤：

步骤61：建立系统方程；

步骤62：建立量测方程；

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

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

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/激光雷达组合导航系统的量测误差补偿方法，其特征在于，所述步骤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为子午圈半径，单独φ表示捷联惯导系统中的数学平台误差角。

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

式中，上标n表示为地理系，Z为量测方程，下标INS表示惯性系统，下标L表示激光雷达，上标～表示实际值，v表示速度，p表示位置，

表示载体系相对于地球系的角速度，R_Mh＝R_M+h，R_Nh＝R_N+h。

5.根据权利要求1所述的一种车载INS/激光雷达组合导航系统的量测误差补偿方法，其特征在于，所述步骤6还包括：将kalman滤波后的陀螺和加速度零偏反馈至器件补偿处进行校正，将姿态反馈至姿态更新补偿处，将速度、位置误差反馈至INS解算的输出值上进行校正，即：由修正后的

可解得航向角ψ、俯仰角θ和滚动角γ，一次滤波反馈后，误差状态归0。

6.根据权利要求5所述的一种车载INS/激光雷达组合导航系统的量测误差补偿方法，其特征在于，所述由修正后的

可解得航向角ψ、俯仰角θ和滚动角γ，其对应的描述公式为：/>

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

Images