[发明专利]一种无人驾驶轨迹跟踪中实时判别当前参考轨迹点的算法

说明书

本发明公开了一种无人驾驶轨迹跟踪中实时判别当前参考轨迹点的算法，首先从参考轨迹的出发点开始，在每三个连续的参考轨迹点中的第二个参考轨迹点处，取前两个参考轨迹点所连成的直线与后两个参考轨迹点所连成直线的角平分线，并得到角平分线在全局坐标系下的直线表达式；然后通过实时位置的结果值和第三个参考轨迹点的结果值乘积来判断车辆是否通过第二个参考轨迹点，实时地得到车辆当前经过的参考轨迹点，直到到达最后一个参考轨迹点为止。本发明能实时地判别车辆当前经过的参考轨迹点，并对后一个参考轨迹点进行跟踪的功能，从而使得车辆可以得到当前的预瞄点及预瞄点的语意信息，进行局部轨迹规划、轨迹跟踪和避开障碍物的控制。

技术领域

本发明涉及无人驾驶领域，具体地说，特别涉及到一种无人驾驶轨迹跟踪中实时判别当前参考轨迹点的算法。

背景技术

轨迹跟踪，是无人驾驶的最基本环节。轨迹跟踪，指的是无人车在得到一条参考轨迹(由一系列参考轨迹点组成)之后，对车辆进行实时有效地控制，使无人车安全地跟随参考轨迹指定的路线进行自主行驶。

轨迹跟踪需要车辆进行实时地精准地定位，判断当前处于参考轨迹的哪个参考轨迹点，并选择合适的预瞄模型，对车辆进行精准的纵横向控制，从而达到轨迹跟踪的效果。同时，如果车辆使用的是具有语义信息的语义拓扑地图时，也需要通过判断当前经过了哪个参考轨迹点来获取当前轨迹点的语意，从而指导车辆执行相关动作。因此，如何实时地鲁棒地判断当前车辆经过了参考轨迹的哪个参考轨迹点是非常重要的。

目前的判断方法都存在各自的缺陷。比如以距离最近为原则找寻当前车辆位置距离最近的参考轨迹点，这种方法无法应对参考轨迹中出现轨迹重叠或交叉的情况，极有可能将当前经过轨迹点判断为已经过或未经过的轨迹点，从而导致无人车发生跟随错误轨迹行驶的情况发生。同时，若以车辆实时位置到参考轨迹点的距离大小方式进行判断，则在刚刚经过当前轨迹点后，存在车辆位置到刚经过的参考轨迹点的距离小于到后一个参考轨迹点的距离时，则会判断当前瞄准的参考轨迹点依然为上一个刚经过的点。因此，轨迹跟踪中亟需一种可以在参考轨迹出现重叠或交叉的情况下，仍然能鲁棒地依次判断当前经过的跟踪参考轨迹点的算法。

发明内容

本发明的目的在于针对现有技术中的不足，提供一种无人驾驶轨迹跟踪中实时判别当前参考轨迹点的算法，在轨迹跟踪时可以实时地判别当前经过了哪些参考轨迹点，并立即对后一个参考轨迹点进行跟踪的功能，从而使得车辆可以得到当前的预瞄点及预瞄点的语意信息，从而进行局部轨迹规划，进行轨迹跟踪和避开障碍物的控制。

本发明所解决的技术问题可以采用以下技术方案来实现：

一种无人驾驶轨迹跟踪中实时判别当前参考轨迹点的算法，包括如下步骤：

1)从参考轨迹的出发点开始，在每三个连续的参考轨迹点中的第二个参考轨迹点处，取第一个参考轨迹点和第二个参考轨迹点所连成的直线与第二个参考轨迹点和第三个参考轨迹点所连成直线的角平分线，并得到角平分线在全局坐标系下的直线表达式；

2)将车辆在全局坐标系下的实时x-y坐标代入角平分线的直线表达式得到实时位置的结果值，并将第三个参考轨迹点的x-y坐标代入角平分线的直线表达式得到第三个参考轨迹点的结果值；

3)若实时位置的结果值和第三个参考轨迹点的结果值异号，则说明车辆的当前坐标与第三个参考轨迹点位于角平分线的两侧，即认定为车辆尚未通过第二个参考轨迹点，返回步骤2)；

若实时位置的结果值和第三个参考轨迹点的结果值同号，则说明车辆的当前坐标与第三个参考轨迹点位于角平分线的同侧，即认定为车辆已经通过第二个参考轨迹点，进入步骤4)；

4)对后三个连续的参考轨迹点的进行步骤1)至步骤3)的判断，实时地得到车辆当前经过的参考轨迹点，直到到达最后一个参考轨迹点为止。