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

权利要求书

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表示单位矩阵，/为常值。