CN110849362A - 一种基于车载惯性的激光雷达与视觉组合导航算法 - Google Patents

Abstract

本发明公开了一种基于车载惯性的激光雷达与视觉组合导航算法，首先，利用张正友标定算法进行深度相机的标定，根据标定后的相机对载体车行驶过程的视频进行关键帧提取，利用提取的帧信息与初始对准后的惯导信息进行卡尔曼滤波算法的优化，实现初步的导航定位；同时，激光雷达通过测距信息，在基站的辅助下得到初步定位信息，然后把惯导指示航迹所确定的序列在数字地图匹配搜索范围内进行遍历搜索，与激光雷达匹配序列进行最小距离度量，然后通过卡尔曼滤波算法的优化确定激光雷达组合导航的定位信息，最后，将两者的定位信息传送到滤波器，通过两种算法的互补滤波对车载定位进行优化，然后将优化参数反馈校正惯导，同时实时对点云地图进行更新。

Description

一种基于车载惯性的激光雷达与视觉组合导航算法

技术领域

本发明涉及视觉车载导航技术领域，更具体的说是涉及一种基于车载惯性的激光雷达与视觉组合导航算法。

背景技术

近年来随着经济的快速发展，计算机视觉技术的日趋成熟，以及互联网、人工智能、云计算等新兴技术的快速发展，自动驾驶行业的发展要求越来越强，而自动驾驶技术的发展必须依靠高精度的导航定位，因此，基于车载导航的研究也成为一大研究热点，确实是自动驾驶的迫切需求。

目前，车载导航主要依靠车载GPS(全球定位系统)配合电子地图来进行的，在进行导航过程中主要涉及四个环节卫星信号、信号接收、信号处理和地图数据库，而信号接收最为关键，但GPS信号又易受到信号的干扰，容易遭受黑客攻击，导致信号丢失，此外，在一些偏远山区，由于信号质量差，将无法实现载体车驾驶过程中的同步更新，因此新型的车载导航系统得到迫切需求。随着近年来硬件设计和制造的进步，低成本的轻量化微电子机械(MEMS)IMU已经普遍存在，这使得高精度定位成为可能，也为视觉导航领域的发展提供了一定的技术支持。因此，利用可提供丰富的环境信息，具有体积小、重量轻、能效高特性的相机作为INS的辅助来源，产生视觉惯性导航系统(VINS)成为新型研究趋势。

虽然视觉传感器在大多数纹理丰富的场景中效果很好，但是如果遇到玻璃，白墙等特征较少的场景，基本上无法工作，此外，视觉相机的工作频率具有一定的限制，在高速行驶过程中，视觉更新存在更新延迟现象。

因此，如何实现复杂环境下车载视觉惯性导航的精确定位是本领域技术人员亟需解决的问题。

发明内容

有鉴于此，本发明提供了一种基于车载惯性的激光雷达与视觉组合导航算法，针对复杂环境下车载导航定位，主要基于车载导航现状，综合考虑惯导、深度相机、激光雷达的优势，进行优势互补，实现在复杂环境下高精度的导航定位与点云图的构建。

为了实现上述目的，本发明采用如下技术方案：

一种基于车载惯性的激光雷达与视觉组合导航算法，包括如下具体步骤：

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

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

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

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

步骤14：根据所述特征点采用解析解估算方法计算出内部参数和外部参数，所述内部参数为与所述深度相机自身特性相关的参数，包括焦距、像素等，所述外部参数为世界坐标系中的参数，包括所述深度相机的位置、旋转方向等；其中包括5个所述内部参数和6个所述外部参数；

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

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

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

步骤22：根据所述载体车的陀螺仪输出

与加速度计输出的比力值f^b，计算出所述载体车的平台误差角并构造矩阵

步骤23：根据所述矩阵

对所述捷联惯导矩阵

进行修正，完成初对准获得对准矩阵

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

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

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

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

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

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

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

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

步骤43：根据所述平移估计矩阵T求出所述载体车的视觉测量位置信息，根据所述旋转估计矩阵R求出所述载体车的视觉测量姿态信息；所述视觉测量姿态信息包括航向角fai、俯仰角theta和横滚角gama；

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

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

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

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

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

经由上述的技术方案可知，与现有技术相比，本发明公开提供了一种基于车载惯性的激光雷达与视觉组合导航算法，在传统视觉导航基础上，利用激光雷达进行辅助车载导航定位。首先，利用张正友标定算法进行深度相机的标定，根据标定后的相机对载体车行驶过程的视频进行关键帧提取，利用提取的帧信息与初始对准后的惯导信息进行卡尔曼滤波算法的优化，实现初步的导航定位；同时，激光雷达通过测距信息，在基站的辅助下得到激光雷达的初步定位信息，然后把惯导系统指示航迹所确定的序列在数字地图匹配搜索范围内进行遍历搜索，与激光雷达匹配序列进行最小距离度量，然后通过卡尔曼滤波算法的优化确定激光雷达组合导航的定位信息，最后，将两者的定位信息传送到滤波器，通过两种算法的互补滤波对车载定位进行进一步优化，然后将优化参数反馈校正惯导，同时实时对点云地图进行更新。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1附图为本发明提供的基于车载惯性的激光雷达与视觉组合导航算法的流程示意图。

具体实施方式

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

本发明实施例公开了一种基于车载惯性的激光雷达与视觉组合导航算法，具体步骤如下：

S1：对载体车上安装的深度相机进行标定；

打印一张棋盘格A4纸张(黑白间距已知)，并贴在一个平板上；针对棋盘格纸张拍摄若干张图片(一般10-20张)；在图片中检测特征点(Harris特征)；利用解析解估算方法计算出5个内部参数，以及6个外部参数；根据极大似然估计策略，设计优化目标并实现参数的改进；

相机模型建立：

p＝K[r|t]P

其中p是像点坐标，P是标定的棋盘坐标，r为深度相机的旋转参数，t为深度相机的平移参数；

令H＝K[R|t]，来表示像平面与用来标定棋盘的单位矩阵，然后利用小孔成像原理，根据棋盘与成像平面点的对应原则可求出单位矩阵H。

H＝λK[r₁ r₂ t]

然后利用旋转矩阵为正交矩阵的特性，可以获得约束参数等式如下：

通过标定板的图像获得内部参数的关系式：

其中，u₀为图片中心，v₀为主点，α，β，γ为坐标系转换参数；

从而得到相机内部各参数：

一般在进行标定过程中，为了取得好的成像效果，通常要在相机的镜头前添加透镜。在相机成像的过程中，透镜会对光线的传播产生影响，从而影响相机的成像效果，产生畸变，然后通过消除径向畸变，实现标定优化。

其中，k1，k2为畸变参数，n表示径向畸变个数，m表示切向畸变个数，M表示有相机参数的函数；

S2：进行惯导系统的初对准：

在进行捷联惯导解算过程中，地球坐标系是固连在地球上的坐标系，表示为O_ex_ey_ez_e；载体坐标系Ox_by_bz_b固连在载体车上，坐标原点位于载体车的重心处，x_b沿载体车前进方向指向右，y_b沿载体车前进方向纵轴指向前，z_b垂直于Ox_by_b，指向天向。

设当地的纬度为L，重力加速度为g，地球自转角速度为w_ie，载体运动而引起的相对地球旋转角度为w_eg，然后计算出捷联惯导矩阵

然后根据陀螺仪输出

与加速度计输出的比力值f^b粗略计算出平台误差角

构造出矩阵然后上述

进行修正，完成初对准。

其中，U表示为东向，N表示为北向，E表示为天向；

S3：车载组合导航算法的解算：

S3.1：激光雷达进行角点扫描，利用激光雷达粗估计位置确定匹配序列x₁,x₂,…，x_n，根据惯导解算轨迹确定匹配搜索区域，然后利用惯导匹配序列y₁,y₂,…,y_n在数字地图中的匹配搜索区域内进行遍历搜索，利用MAD算法进行最小距离度量：

其中，N为匹配序列点的个数；

然后将匹配定位结果与惯导解算结果做差，作为测量信息输入卡尔曼滤波器，通过卡尔曼滤波进行参数优化，最后实现激光雷达与惯性的组合导航定位。

S3.2：通过标定过得深度相机获取载体车行驶过程中的视频，然后经过关键帧图片的提取获得图片的Kinect信息，求出平移估计矩阵T和旋转估计矩阵R：

然后进而求出视觉测量的载体位置信息和姿态信息(航向角、俯仰角、横滚角)，分别用fai，theta，gama变量表示，最后与惯导信息进行深度融合，通过卡尔曼滤波实现对惯导定位信息的校正与定位。因为惯导解算具有发散特性，在实现组合定位后，对惯导解算位置信息进行校正，是为了避免发散过大导致匹配搜索区域大，进一步导致匹配精度下降，耗时增加。

S3.3：将深度视觉与惯导滤波参数和激光雷达与惯导的滤波参数再次通过卡尔曼滤波算法实现互补滤波，进一步对匹配定位算法进行优化。

S3.4：由互补滤波参数得到的惯性、激光雷达与视觉组合导航定位的位置信息对惯导进行反馈较真，抑制惯导的发散，同时根据精准定位结果绘制出载体车运行过程的轨迹点云图并保存。

S3.5：将优化后的参数对惯导当前位置进行较真后，继续进行惯导解算，为下一时刻的定位做准备。

其中，本发明采用的卡尔曼滤波算法的具体公式为：

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (5)

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：对所述融合信息进行卡尔曼滤波，获得深度视觉与惯导滤波参数，对所述惯导定位信息进行校正与定位，获得所述视觉组合导航定位信息。