一种基于互相关熵配准的无人车定位融合方法与流程

本发明属于无人驾驶汽车技术中的定位与导航领域，具体涉及一种基于互相关熵配准的无人车定位融合方法。

背景技术

近年来随着汽车产业的高速发展，交通事故已经成为全球性的问题，全世界每年交通事故的死伤人数估计超过50多万人，因此集自动控制、人工智能、模式识别等技术于一体的无人驾驶应用而生。无人车定位技术是无人驾驶技术中关键组成部分之一。为了实时得到无人驾驶汽车的当前位置，无人驾驶汽车上装备了各种主动传感器与被动传感器，包括相机、轮速编码器、gps和imu等。在早期的研究中，使用gps进行定位的方法得到了大量的应用。但是，gps的使用受周围环境因素的影响巨大。当周围环境出现gps信号遮挡甚至屏蔽时，其定位结果就会十分不可靠，特别是有可能出现的gps定位跳变问题，其将会对无人车的运动规划和控制带来不可预估的影响。以imu、陀螺仪等为代表的惯性器件则不会出现此问题，然而，利用惯性器件进行定位和导航时，定位结果会极大的受到累计误差的影响，当系统较长时间工作时，惯性定位系统的定位误差极大，失去利用价值。而使用相机或激光等传感器进行定位的方法大部分需要提前建立相应的场景地图，并且此类方法将消耗较大的计算量。

技术实现要素：

本发明的目的在于克服上述现有技术的缺点，提供一种高效、实时、鲁棒的基于互相关熵配准的无人车定位融合方法，无人车能够通过轮速传感器和陀螺仪的数据将gps定位数据平滑化，得到更为可靠的融合定位结果

为达到上述目的，本发明采用以下技术方案予以实现：

一种基于互相关熵配准的无人车定位融合方法，包括以下步骤：

首先使用轮速传感器和陀螺仪的数据进行无人车航迹推算；然后将gps定位数据与航迹推算数据使用基于互相关熵的icp算法进行配准，得到经过长时修正的航迹推算定位结果；进而根据融合长时修正的航迹推算结果，给定新的带有较大误差的gps定位结果，输出融合后的定位结果。

本发明进一步的改进在于：

具体包括以下步骤：

1)使用无人驾驶汽车的轮速编码器数据和陀螺仪的车辆航向数据进行航迹推算；以此得到车辆积分推演的定位结果；

2)将通过gps得到的定位序列点和航迹推算的定位序列点使用基于互相关熵的icp算法进行配准，得到经过长时修正的航迹推算定位结果；

3)当有新的gps数据输入时，选取在航迹推算位置序列中的对应点作为融合后的定位结果进行输出。

步骤1)的具体方法如下：

1.1)确定航迹推算更新间隔；根据输出频率较低的传感器确定航迹推算的更新间隔t；对于另一个输出频率较高的传感器，在更新时取最新的输出结果作为当前帧数据；

1.2)设置航迹推算起点为无人车定位程序启动时的gps位置p0＝(x0,y0)；并设置陀螺仪的起始航向值为0或gps给出的航向；

1.3)对于从轮速传感器和陀螺仪获得的每一帧车体速度数据vk和航向数据θk，在上一帧的位置pk-1＝(xk-1,yk-1)的基础上，得到第k帧的位置pk＝(xk,yk)，计算公式为：

其中，t表示两帧之间的时间间隔，表示航向角相加。

步骤2)的具体方法如下：

2.1)将航迹推算序列点集设为设gps定位序列点集设为每当得到新的定位数据时，将新的定位结果分别增加到两点集中；

2.2)依据最近邻原则，寻找步骤2.1)中点集x中所有点到点集y中的对应点计算公式为：

其中，k指算法的迭代次数，rk-1和指k-1次迭代得到的刚体变换的结果，nx指点集x中点的个数，ny指点集y中点的个数；

2.3)经过步骤2.2)后，利用2.2)求得的对应关系，最大化下述的使用互相关熵定义的目标函数，求取刚体变换：

其中，σ是一个自由变量；

2.4)判断当前的配准误差是否已经符合要求，若已符合要求，则输出刚体变换r和若尚未符合要求，则继续判断当前迭代次数k是否已经达到预设的迭代最大值，若已经达到，则输出刚体变换r和若尚未达到，则返回到步骤2.2)，继续迭代计算刚体变换。

步骤3)的具体方法如下：

3.1)当无人车的新的gps坐标点得到时，将其顺延加入到点集y的最后；新的航迹推算坐标点顺延加入到点集x的最后；

3.2)使用点集配准算法将两个点集进行配准，取配准后x点集中最新加入的数据点的位置作为融合后的定位数据进行输出。

与现有技术相比，本发明具有以下有益效果：

本发明利用轮速传感器和陀螺仪得到无人车的航迹推算定位结果，将gps位置点序列与航迹推算位置点序列进行配准以消除航迹推算的累积误差的影响，最后输出配准后的航迹推算位置点作为融合定位结果。

本发明利用到的传感设备要求简单，可靠性高，没有使用到无人车的环境感知信息。轮速传感器的使用给系统提供了稳定的车速信息，相对于基于imu等惯性器件测量车体速度的方案更为鲁棒和简练，减弱了因速度的观测不准确而导致的积分漂移现象。另外，步骤2)中使用基于互相关熵的配准方法将两种定位序列点进行配准，极大的抑制了gps中可能出现的跳变点对定位融合带来的影响。同时，本发明计算复杂度低，适应性强，能够在绝大多数无人驾驶汽车中应用，实现实时稳定的定位融合。

附图说明

图1为基于互相关熵配准的无人车定位融合方法总体框图；

图2为定位融合示例，针对gps跳变情况；

图3为定位融合示例，针对gps定位误差较大的情况。

具体实施方式

下面结合附图对本发明做进一步详细描述：

参见图1-3，本发明基于互相关熵配准的无人车定位融合方法，首先使用轮速传感器和陀螺仪的数据进行无人车航迹推算；然后将gps定位数据与航迹推算数据使用基于互相关熵的icp算法进行配准，得到经过长时修正的航迹推算定位结果；进而根据融合长时修正的航迹推算结果，给定新的可能带有较大误差的gps定位结果，输出融合后的定位结果。如图1所示。具体按以下步骤进行：

步骤1)使用无人驾驶汽车的轮速编码器数据和陀螺仪的车辆航向数据进行航迹推算。以此得到车辆积分推演的定位结果。与通过惯性器件得到的定位数据相似，本步骤中的航迹推算结果也将受到积分的累积误差的影响。

步骤1.1)确定航迹推算更新间隔。由于是使用轮速传感器和陀螺仪两种传感器，不同的传感器的更新频率并不相同，因此需要根据输出频率较低的传感器确定航迹推算的更新间隔t。对于另一个输出频率较高的传感器，在更新时取最新的输出结果作为当前帧数据。

步骤1.2)系统初始化。将航迹推算的起点设置为程序启动时gps的位置p0＝(x0,y0)，并设置陀螺仪的起始航向值为0或gps给出的航向。由于本专利提出的配准算法基于迭代最近点(iterativeclosestpoint,icp)算法改进而来，而icp算法是一个局部收敛算法，将系统按照此步骤进行初始化将能够极大的简化算法收敛难度，特别是将航向值的初始值设置为由多帧gps位置估计出的车体航向时，算法的迭代次数将进一步减少，提高本发明运行的速度。

步骤1.3)对于从轮速传感器和陀螺仪获得的每一帧车体速度数据vk(m/s)和航向数据θk(θk∈(-180°,180°])，在上一帧的位置pk-1＝(xk-1,yk-1)的基础上，得到第k帧的位置pk＝(xk,yk)，计算公式为：

其中，t表示两帧之间的时间间隔，表示航向角相加，其保证两个航向角求平均后的值域仍为θavg∈(-180°,180°]，其具体计算方式为：

步骤2)将通过gps得到的定位序列点和航迹推算的定位序列点使用基于互相关熵的icp算法进行配准，得到经过长时修正的航迹推算定位结果。通过本步骤，可以减弱航迹推算的累积误差。

步骤2.1)将航迹推算序列点集设为设gps定位序列点集设为每当得到新的定位数据时，将新的定位结果分别增加到两点集中。

步骤2.2)依据最近邻原则，寻找步骤2.1)中点集x中所有点到点集y中的对应点计算公式为：

其中，k指算法的迭代次数，rk-1和指k-1次迭代得到的刚体变换的结果，nx指点集x中点的个数，ny指点集y中点的个数。

步骤2.3)经过步骤2.2)后，利用2.2)求得的对应关系，最大化下述的使用互相关熵定义的目标函数，求取刚体变换：

其中，σ是一个自由变量。

步骤2.4)判断当前的配准误差是否已经符合要求，若已符合要求，则输出刚体变换r和若尚未符合要求，则继续判断当前迭代次数k是否已经达到预设的迭代最大值，若已经达到，则输出刚体变换r和若尚未达到，则返回到步骤2.2)，继续迭代计算刚体变换。

步骤3)当有新的gps数据输入时，选取在航迹推算位置序列中的对应点作为融合后的定位结果进行输出。

步骤3.1)当有新的航迹推算结果时，把该坐标点加入到点集x中；当有新的gps位置数据输入时，把新的位置坐标点加入到点集y中。根据步骤2求得的刚体变换，得到航迹推算点在经过该刚体变换后的位置。将此位置作为融合结果输出。

步骤3.2)本系统是一个迭代求解的系统，因此执行完输出结果后需要返回步骤1.3循环进行。

经过上述步骤，根据离散的甚至可能存在跳变的gps定位数据，配合以从轮速传感器和陀螺仪的数据，即可融合得到光滑性较好的定位数据，满足无人车的规划和控制对定位数据平滑性的需要。

图2和图3给出了两个本算法应用的例子。图2显示了当gps位置突然发生跳变时，本算法的融合性能，其中蓝色的点位航迹推算的点，红色的点位gps位置，黑色的点为融合后的定位结果。从图中可以看出虽然gps的定位结果发生了跳变，但是融合的结果并没有发生严重跳变，而是依着gps变化的趋势慢慢地向gps位置靠拢，保证了输出结果的平滑性。

图3显示了当gps定位方差比较大时的融合结果。可以从图中看出gps定位结果存在较大的高斯噪声。经过本专利的算法融合航迹推算结果之后，定位结果也变得更加平滑。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。