本发明属于无人驾驶汽车技术中的定位与导航领域，具体涉及一种基于点到面距离和互相关熵配准的无人车位姿估计方法。
背景技术
近年来随着汽车产业的高速发展，交通事故已经成为全球性的问题，全世界每年交通事故的死伤人数估计超过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和
3.3)经过步骤3.2)后，利用3.2)求得的对应关系，最大化下述的使用点到面距离和互相关熵定义的目标函数，求取刚体变换：
其中，σ是一个自由变量；
3.4)判断当前的配准误差是否已经符合要求，若已符合要求，则输出刚体变换
步骤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所示的激光雷达安装位置在局部空间直角坐标系中的位置，可以通过三角函数完成极坐标到直角坐标的转换，转换等式如下所示：
xi＝di·sin(αi)·sin(βi)
yi＝di·sin(αi)·cos(βi)
zi＝h-di·cos(αi)
步骤2)三维激光雷达数据降采样：由于激光点过于密集，会给系统带来极大的计算量；而把数据点进行降采样后，依旧能够对当前环境有明确的描述。在本步骤中使用随机降采样的标准，将三维激光雷达数据降采样到原来的10％-20％；
步骤2.1)三维激光雷达每扫描一圈周围场景都将产生大量的点云数据，并且其特点为近处的点云稠密而远处的点云稀疏。过于稠密的点集会给计算带来极大的冗余性，因此在本步骤中需要对点云进行降采样操作。
步骤2.2)假设每一帧三维激光点云中都有n个点，其对应的数据索引为1到n。降采样的方式为根据确定的降采样比例p(0<p<1)，随机在1到n中选出n×p个值(向下取整)，然后取出这些索引值对应的数据点，这些点即为降采样后的数据。
步骤3)将步骤2)获得的当前场景描述点云数据与已有的占据栅格点云地图数据进行精确刚体配准，其中地图点云作为目标点集，在配准过程中其物理位置保持不变，当前点云作为模型点集，通过后文描述的迭代算法，不断的调整该点云的物理位置，最终得到两帧数据间的刚体旋转平移变换r和
步骤3.1)无人车当前扫描的场景与地图场景存在部分对应关系，即当前场景描述是地图描述的一部分。因此如何准确寻找确切的对应关系就成为了无人驾驶汽车定位的关键。本发明中使用基于互相关熵和点到面距离的迭代最近点(iterativeclosestpoint，icp)算法将当前场景描述点云数据与已有的地图点云数据进行精确刚体配准，其中地图点云作为目标点集，在配准过程中其物理位置保持不变，当前点云作为模型点集，从而得到无人驾驶汽车的当前位置。不失一般性的，假设三维激光当前扫描得到的点云为
步骤3.2)对于地图中的每一个点
步骤3.3)给定刚体变换初值。由于icp算法及其改进算法大多为局部收敛算法，在某一给定的初值条件下，算法能够自动地收敛于局部极值，如果初值条件给的不好，则算法很有可能配准失败，特别是在部分配准和噪声、异常点较多的情况下。因此，一个良好的配准初值能够有效地防止算法陷入局部极值。
在算法的使用过程中，第一次配准的初值可由gps位置作为初值，也可以使用特征匹配的方法获得初值。如果在实际问题中，有gps作为辅助传感器输入，那么可以将gps位置变换为配准所需要的变换初值。比如在城市道路中，虽有建筑物、树木等遮挡，单纯凭借gps的定位不足以满足无人驾驶汽车的需求，但是gps依旧可以为本算法提供配准初值；再比如在地下车库等场景的定位问题中，可以使用进入车库前的gps位置作为初值，等等。如果gps位置信息获取受限，则可以抽取当前帧点云和地图点云的三维特征(比如rops，3dsurf等)，然后使用特征匹配的方式得出配准初值。但这种方法一般而言计算复杂度较高，耗时较长。
配准算法的后续迭代过程中，则可以使用上一帧的配准结果作为初值。由于三维激光采集数据频率较高，依据型号和设置的不同，大约在10hz到20hz不等，两帧之间的旋转和平移变换不大，因此，可以用上一帧的配准结果作为本帧配准的初值。
步骤3.4)设定迭代求解的最大次数m，并设定好配准收敛条件，循环进行步骤3.5)和步骤3.6)，直到循环次数达到最大值m或者满足了收敛条件：
步骤3.5)在第k次循环中，寻找两个点集间的对应关系。对于点
步骤3.6)通过使
对于上述公式，最终的求取结果被表示为一个向量的形式：
其中，
r11＝cosγcosβ
r12＝sinγcosα+cosγsinβsinα
r13＝sinγsinα+cosγsinβcosα
r21＝sinγcosβ
r22＝cosγcosα+sinγsinβsinα
r23＝-cosγsinα+sinγsinβcosα
r31＝-sinβ
r32＝cosβsinα
r33＝cosβcosα
步骤4)根据无人驾驶汽车的初始位置和初始姿态，得到无人车当前的位置和姿态。假设无人驾驶汽车的初始位置为(x0,y0,z0)t，初始姿态为(α0,β0,γ0)t，那么根据步骤3)得到的旋转平移变换，无人车当前的位置(x,y,z)t和姿态(α,β,γ)t为：
(α,β,γ)t＝r(α0,β0,γ0)t
经过上面的处理，从三维激光点云和已有的地图点云中即可得到车体的位置和姿态信息。一个无人车在地下车库定位的例子参见图4，图中给出了无人车在某地下车库中使用本发明方法进行定位的实验结果，其中用直线连起来的圆圈代表无人车的行驶轨迹。
以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。