[发明专利]一种基于互相关熵配准的无人车定位融合方法

权利要求书

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

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

具体包括以下步骤：

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

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

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

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

其中，k指算法的迭代次数，R_k-1和指k-1次迭代得到的刚体变换的结果，N_x指点集X中点的个数，N_y指点集Y中点的个数；

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

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

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

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

2.根据权利要求1所述的基于互相关熵配准的无人车定位融合方法，其特征在于，步骤1)的具体方法如下：

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

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

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

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

3.根据权利要求1所述的基于互相关熵配准的无人车定位融合方法，其特征在于，步骤3)的具体方法如下：

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

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