[发明专利]一种基于车载惯性的激光雷达与视觉组合导航算法

权利要求书

1.一种基于车载惯性的激光雷达与视觉组合导航方法，其特征在于，包括如下具体步骤：

步骤1：利用张正友标定法对车载的深度相机进行标定，获得标定相机；

步骤2：对车载的惯导系统进行初对准，获得对准矩阵；

步骤3：采用车载的激光雷达进行角点扫描，获得雷达信息，根据所述对准矩阵获得所述惯导系统的惯导信息，对所述惯导信息进行惯导解算获得惯导定位信息，对所述雷达信息和所述惯导定位信息进行匹配定位和卡尔曼滤波，获得激光雷达与惯导滤波参数，实现所述激光雷达与所述惯导系统的组合导航定位，获得激光雷达组合导航定位信息；

步骤4：利用所述标定相机采集载体车行驶过程中的视频，对所述视频进行处理后与所述惯导系统的所述惯导定位信息进行深度融合，获得深度视觉与惯导滤波参数，实现对所述惯导定位信息的校正与定位，获得视觉组合导航定位信息；

步骤5：将深度视觉与惯导滤波参数、激光雷达与惯导滤波参数通过卡尔曼滤波算法进行互补滤波，获得互补滤波参数，对所述激光雷达组合导航定位信息进行优化；

步骤6：根据所述互补滤波参数得到的惯性、所述激光雷达组合导航定位信息与所述视觉组合导航定位信息对所述惯导系统进行反馈校正，抑制所述惯导系统的发散，获得精准定位结果，同时根据所述精准定位结果绘制所述载体车行驶过程的轨迹点云图并保存；

步骤7：根据优化后的参数对所述惯导系统获得的所述惯导定位信息进行校正后，进入所述步骤3，继续进行所述惯导解算，为下一时刻定位做准备。

2.根据权利要求1所述的一种基于车载惯性的激光雷达与视觉组合导航方法，其特征在于，所述步骤1中对所述深度相机进行标定的过程为：

步骤11：打印一张间距已知的棋盘格纸张，并固定贴覆于平板上；

步骤12：利用所述深度相机采集所述棋盘格纸张若干图片；

步骤13：对所述图片检测特征点；

步骤14：根据所述特征点采用解析解估算方法计算出内部参数和外部参数；

步骤15：根据极大似然估计策略对所述内部参数和所述外部参数进行优化，获得优化参数。

3.根据权利要求2所述的一种基于车载惯性的激光雷达与视觉组合导航方法，其特征在于，所述步骤2中对所述惯导系统进行初对准的具体步骤为：

步骤21：根据所述载体车所在位置的纬度L、重力加速度g、地球自转角速度w_ie和载体车运动引起的相对地球旋转角度w_eg，计算出捷联惯导矩阵

步骤22：根据所述载体车的陀螺仪输出与加速度计输出的比力值f^b，计算出所述载体车的平台误差角并构造矩阵

步骤23：根据所述矩阵对所述捷联惯导矩阵进行修正，完成初对准获得对准矩阵

4.根据权利要求3所述的一种基于车载惯性的激光雷达与视觉组合导航方法，其特征在于，所述步骤3中实现所述激光雷达与所述惯导系统组合导航定位的过程为：

步骤31：采用所述激光雷达进行角点扫描，并计算所述载体车的粗估计位置，确定匹配序列x₁,x₂,…，x_n；

步骤32：根据所述惯导系统的所述对准矩阵通过惯导解算获得惯导解算轨迹，从而确定匹配搜索区域和惯导匹配序列；

步骤33：令所述匹配序列x₁,x₂,…，x_n根据所述惯导匹配序列y₁,y₂,…,y_n在数字地图中的所述匹配搜索区域内进行遍历搜索，采用MAD算法获得最小距离度量；

步骤34：根据所述惯导解算轨迹获得惯导位置信息，将所述惯导位置信息和所述最小距离度量的差采用卡尔曼滤波进行优化，获得激光雷达与惯导滤波参数，以及所述激光雷达与所述惯导系统的所述激光雷达组合导航定位信息。

5.根据权利要求4所述的一种基于车载惯性的激光雷达与视觉组合导航方法，其特征在于，所述步骤4中通过所述深度相机实现对所述惯导系统的惯导定位信息进行校正与定位的过程为：

步骤41：通过标定后的所述深度相机获得所述载体车行驶过程中的视频；

步骤42：根据所述深度相机的所述优化参数对所述视频进行关键帧图片提取，获得所述关键帧图片的Kinect信息，并求出平移估计矩阵T和旋转估计矩阵R；

步骤43：根据所述平移估计矩阵T求出所述载体车的视觉测量位置信息，根据所述旋转估计矩阵R求出所述载体车的视觉测量姿态信息；

步骤44：所述加速度计通过所述初对准，经过所述惯导解算获得惯导速度信息，所述陀螺仪通过所述初对准，经过所述惯导解算获得所述惯导位置信息；对所述视觉测量位置信息、所述视觉测量姿态信息、所述惯导速度信息和所述惯导位置信息进行深度融合，获得融合信息；

步骤45：对所述融合信息进行卡尔曼滤波，获得深度视觉与惯导滤波参数，对所述惯导定位信息进行校正与定位，获得所述视觉组合导航定位信息。