基于点到面距离和互相关熵配准的无人车位姿估计方法技术

本发明专利技术公开了一种基于点到面距离和互相关熵配准的无人车位姿估计方法，首先标定三维激光雷达，然后将采集到的三维激光雷达数据进行坐标转换；然后将降采样后的数据与已有的地图数据进行点云配准得到刚体的旋转平移变换；进而根据该旋转平移变换得到自主运动体的位置和姿态。本发明专利技术利用三维激光雷达作为数据来源，通过坐标系转换、数据降采样、点集配准等步骤，完成无人车位姿估计的功能。本发明专利技术能够很好的克服天气、光照等环境因素的影响。另外，使用基于点到面距离和互相关熵的误差评价函数针对场景与地图描述部分不匹配、动态障碍物等噪声和异常点等具有良好的抵抗能力，因此可以实现无人驾驶汽车鲁棒而精准的位姿估计的功能。

Unmanned Vehicle Position and Attitude Estimation Method Based on Point-to-surface Distance and Cross-correlation Entropy Registration

The invention discloses an unmanned vehicle position and attitude estimation method based on point-to-surface distance and cross-correlation entropy registration. Firstly, the three-dimensional lidar is calibrated, and then the collected three-dimensional lidar data is coordinate transformed; then, the downsampled data is registered with the existing map data to obtain the rotation translation of the rigid body. Then the position and attitude of the autonomous moving body can be obtained according to the rotation and translation transformation. The method uses three-dimensional lidar as data source, completes the function of position and attitude estimation of unmanned vehicle through coordinate system conversion, data downsampling, point set registration and other steps. The invention can overcome the influence of weather, light and other environmental factors. In addition, the error evaluation function based on point-to-surface distance and cross-correlation entropy has good resistance to noise and abnormal points such as mismatch between scene and map description, dynamic obstacles and so on, so it can realize robust and accurate pose estimation of unmanned vehicle.

【技术实现步骤摘要】
基于点到面距离和互相关熵配准的无人车位姿估计方法
本专利技术属于无人驾驶汽车技术中的定位与导航领域，具体涉及一种基于点到面距离和互相关熵配准的无人车位姿估计方法。
技术介绍
近年来随着汽车产业的高速发展，交通事故已经成为全球性的问题，全世界每年交通事故的死伤人数估计超过50多万人，因此集自动控制、人工智能、模式识别等技术于一体的无人驾驶应用而生。无人车定位技术是无人驾驶技术中关键组成部分之一。为了实时得到无人驾驶汽车的当前位置，无人驾驶汽车上装备了各种主动传感器与被动传感器，包括相机、激光雷达、GPS和IMU等。在早期的研究中，使用GPS进行定位的方法得到了大量的应用。但是，GPS的使用受周围环境因素的影响巨大。当周围环境出现GPS信号遮挡甚至屏蔽时，其定位结果就会十分不可靠，甚至消失。以IMU为代表的惯性器件则不会出现此问题，然而，利用惯性器件进行定位和导航时，定位结果会极大的受到累计误差的影响，当系统较长时间工作时，惯性定位系统的定位误差极大，失去利用价值。随着技术的发展，很多的研究人员开始研究视觉技术在定位中的应用，其有信息含量大、成本低、运行功率低、扫描时间短等特点。但是，视觉技术一个最主要的不足就是对于外界环境的要求比较严格。对于环境中出现阴影、驾驶环境较为复杂、路边标记(或标志)丢失、光照不佳、能见度低或天气恶劣等情况，视觉技术(摄像机)获取的图像信息往往信噪比很低，给定位算法精度都带来极大的挑战，不能保证系统的定位精度。
技术实现思路
本专利技术的目的在于克服上述现有技术的缺点，提供一种高效、实时、鲁棒的基于点到面距离和互相关熵配准的无人车位姿估计方法。为达到上述目的，本专利技术采用以下技术方案予以实现：基于点到面距离和互相关熵配准的无人车位姿估计方法，首先标定三维激光雷达，然后将采集到的三维激光雷达数据进行降采样；然后将降采样后的数据与已有的三维地图数据进行点云配准得到刚体的旋转平移变换；进而根据该旋转平移变换得到自主运动体的位置和姿态。本专利技术进一步的改进在于：具体包括以下步骤：1)首先确定三维激光雷达安装的高度，然后将采集到的三维激光雷达数据从三维极坐标系转化到局部三维直角坐标系；2)使用随机降采样的标准，将三维激光雷达数据降采样到原来的10％-20％，获得的当前场景描述点云数据；3)将当前场景描述点云数据与已有的占据栅格点云地图数据进行精确刚体配准，其中地图点云作为目标点集，当前点云作为模型点集，得到两帧数据间的刚体旋转平移变换R和4)根据无人驾驶汽车的初始位置和初始姿态，得到无人车当前的位置和姿态。步骤1)的具体方法如下：1.1)以三维激光雷达安装位置为参考点定义局部三维直角坐标系，然后测量三维激光雷达距离水平面的安装高度H；1.2)建立三维激光雷达扫描极坐标到局部三维直角坐标系中坐标的转换关系，根据转换关系获得三维激光雷达数据点Pi对应的三维直角坐标值，每一个三维激光雷达扫描极坐标数据点包括发射所在激光线束的垂直角度αn，发射时的三维激光旋转角度βi与扫描距离di三个参数，即Pi＝(αi,βi,di)，则Pi＝(αi,βi,di)对应局部三维直角坐标系的坐标值为Pi＝(xi,yi,zi)，转化方程如下：xi＝di·sin(αi)·sin(βi)yi＝di·sin(αi)·cos(βi)zi＝H-di·cos(αi)。步骤3)的具体方法如下：3.1)对已有的地图点云数据中的每个点使用主成分分析法估计点云中每个点的法向量3.2)依据最近距离的原则，寻找步骤2)到的数据点集中每个点在地图点云中的对应点其计算公式如下：其中，k指算法的迭代次数，Rk-1和指k-1次迭代得到的刚体变换的结果，Ns指数据点集中点的个数；3.3)经过步骤3.2)后，利用3.2)求得的对应关系，最大化下述的使用点到面距离和互相关熵定义的目标函数，求取刚体变换：其中，σ是一个自由变量；3.4)判断当前的配准误差是否已经符合要求，若已符合要求，则输出刚体变换和若尚未符合要求，则继续判断当前迭代次数k是否已经达到预设的迭代最大值，若已经达到，则输出刚体变换R和若尚未达到，则返回到步骤3.2)，继续迭代计算刚体变换。步骤4)得到无人车当前的位置和姿态的具体方法如下：根据无人驾驶汽车的初始位置(x0,y0,z0)T和初始姿态(α0,β0,γ0)T，得到无人车当前的位置和姿态，其计算方法如下：与现有技术相比，本专利技术具有以下有益效果：本专利技术利用三维激光雷达扫描无人车周围场景，获取当前场景的点云描述；然后将数据降采样到合适水平；随后使用迭代的配准算法把扫描的场景数据与地图数据进行精确配准，得到当前帧点云数据的刚体变换；最后利用该刚体变换得到无人车在三维点云地图内的位置和当前车体姿态。本专利技术利用三维激光雷达作为数据来源，通过坐标系转换、数据降采样、点集配准等步骤，完成无人车位姿估计的功能。由于三维激光雷达的工作原理与相机等不同，能够很好的克服天气、光照等环境因素的影响。另外，使用步骤3.3)中基于点到面距离和互相关熵的误差评价函数针对场景与地图描述部分不匹配、动态障碍物等噪声和异常点等具有良好的抵抗能力，因此可以实现无人驾驶汽车鲁棒而精准的位姿估计的功能。附图说明图1为基于三维激光雷达的无人车定位总体框图；图2为三维激光扫描模型；图3为基于互相关熵和点到面距离的点云配准框图；图4为无人车在地下车库定位的实例图。具体实施方式下面结合附图对本专利技术做进一步详细描述：参见图1-3，本专利技术基于点到面距离和互相关熵配准的无人车位姿估计方法，首先标定三维激光雷达，然后将采集到的三维激光雷达数据进行坐标转换；然后将降采样后的数据与已有的地图数据进行点云配准得到刚体的旋转平移变换；进而根据该旋转平移变换得到自主运动体的位置和姿态。本专利技术具体按以下步骤进行：步骤1)为了完成对于无人车位置和姿态的计算和描述，首先确定三维激光雷达在车辆上安装的高度，同时计算出三维激光雷达向下倾斜的角度，然后将采集到的三维激光雷达数据从极坐标系转化到局部三维直角坐标系；步骤1.1)为了实现使用三维激光雷达进行定位的功能，需要将三维激光雷达水平安装在车顶上方，保证其能够扫描到车身360°范围内的场景。步骤1.2)由于激光雷达是扫描式的主动传感器，其数据是以激光雷达的激光发射点为极点，正前方或是其他特定方向为极轴的极坐标系的距离和角度，而后面的计算全部是基于直角坐标系来实现的，所以需要建立激光雷达扫描数据极坐标系到局部空间直角坐标系坐标的转换关系，获得激光点对应的直角坐标值。三维激光雷达由多组激光发射器构成，以64线三维激光为例，其一共有64个激光发射器，每个发射器呈不同的角度向斜下方进行扫描。此时定义如图2所示局部空间直角坐标系，测量激光雷达距离局部空间直角坐标系水平面(X-Y，其中Y向为车头正前方所指的方向)的安装高度H，确定激光雷达安装位置在局部空间直角坐标系的位置。每一个激光数据点包括发射所在激光线束的垂直角度αn，发射时的三维激光旋转角度βi与扫描距离di三个参数，即Pi＝(αi,βi,di)，而局部空间直角坐标系则需要xi,yi,zi三个坐标值，即Pi＝(xi,yi,zi)。根据图2所示的激光雷达安装位置在局部空间直角坐标系中的位置，可以通过三角函数完成极坐标到直本文档来自技高网...

【技术保护点】
1.基于点到面距离和互相关熵配准的无人车位姿估计方法，其特征在于，首先标定三维激光雷达，然后将采集到的三维激光雷达数据进行降采样；然后将降采样后的数据与已有的三维地图数据进行点云配准得到刚体的旋转平移变换；进而根据该旋转平移变换得到自主运动体的位置和姿态。

【技术特征摘要】
1.基于点到面距离和互相关熵配准的无人车位姿估计方法，其特征在于，首先标定三维激光雷达，然后将采集到的三维激光雷达数据进行降采样；然后将降采样后的数据与已有的三维地图数据进行点云配准得到刚体的旋转平移变换；进而根据该旋转平移变换得到自主运动体的位置和姿态。2.根据权利要求1所述的基于点到面距离和互相关熵配准的无人车位姿估计方法，其特征在于，具体包括以下步骤：1)首先确定三维激光雷达安装的高度，然后将采集到的三维激光雷达数据从三维极坐标系转化到局部三维直角坐标系；2)使用随机降采样的标准，将三维激光雷达数据降采样到原来的10％-20％，获得的当前场景描述点云数据；3)将当前场景描述点云数据与已有的占据栅格点云地图数据进行精确刚体配准，其中地图点云作为目标点集，当前点云作为模型点集，得到两帧数据间的刚体旋转平移变换R和4)根据无人驾驶汽车的初始位置和初始姿态，得到无人车当前的位置和姿态。3.根据权利要求2所述基于点到面距离和互相关熵配准的无人车位姿估计方法，其特征在于，步骤1)的具体方法如下：1.1)以三维激光雷达安装位置为参考点定义局部三维直角坐标系，然后测量三维激光雷达距离水平面的安装高度H；1.2)建立三维激光雷达扫描极坐标到局部三维直角坐标系中坐标的转换关系，根据转换关系获得三维激光雷达数据点Pi对应的三维直角坐标值，每一个三维激光雷达扫描极坐标数据点包括发射所在激光线束的垂直角度αn，发射时的三维激光旋转角度βi与扫描距离di三个参数，即Pi＝(αi,βi,d...

【专利技术属性】
技术研发人员：杜少毅，许光林，高跃，崔迪潇，陈霸东，万腾，
申请(专利权)人：西安交通大学，
类型：发明
国别省市：陕西,61