一种无人驾驶轨迹跟踪中实时判别当前参考轨迹点的算法的制作方法

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

背景技术：

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

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

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

技术实现要素：

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

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

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

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

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

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

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

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

进一步的，所述全局坐标系的构建方法为：在参考轨迹附近任取一个原点并建立x-y坐标系，将车辆实时位置点及所有参考轨迹点的经纬度坐标转换到该x-y坐标系下。

进一步的，当前判断目标为参考轨迹的出发点时，取出发点和出发点后一个参考轨迹点所连成直线在出发点处的垂线，并得到垂线在全局坐标下的直线表达式；

然后将车辆在全局坐标系下的实时x-y坐标代入垂线的直线表达式得到实时位置的结果值，并将出发点后一个参考轨迹点的x-y坐标代入垂线的直线表达式得到出发点后一个参考轨迹点的结果值；

若实时位置的结果值和出发点后一个参考轨迹点的结果值异号，即判定为车辆尚未通过出发点；若实时位置的结果值和出发点后一个参考轨迹点的结果值同号，则判定为车辆已经通过出发点。

进一步的，当前判断目标为参考轨迹的终点时，取终点和终点前一个参考轨迹点所连成直线在终点处的垂线，并得到垂线在全局坐标下的直线表达式；

然后将车辆在全局坐标系下的实时x-y坐标代入垂线的直线表达式得到实时位置的结果值，并将终点前一个参考轨迹点的x-y坐标代入垂线的直线表达式得到终点前一个参考轨迹点的结果值；

若实时位置的结果值和终点前一个参考轨迹点的结果值同号，即判定为车辆尚未抵达终点；若实时位置的结果值和终点前一个参考轨迹点的结果值异号，则判定为车辆已经抵达终点。

进一步的，从参考轨迹的出发点开始，按照参考轨迹的行进路线，对每一个当前参考轨迹点只做车辆是否经过当前参考轨迹点的判断，并只在判断得到车辆经过当前参考轨迹点时，才对后一个参考轨迹点进行判断。

进一步的，在车辆轨迹跟踪时，按照参考轨迹经过的顺序将所有参考轨迹点进行排序，每经过一个参考轨迹点就将该参考轨迹点的序号记录下来；在轨迹跟踪调试时，若出现问题需要重新运行程序，程序运行伊始便会从记录的最后一个参考轨迹点的序号开始进行判断，实现从中断点继续测试。

进一步的，参考轨迹的起始参考点序号可通过手动输入，重新运行程序时便会从手动输入的起始参考点开始进行判断。

与现有技术相比，本发明的有益效果在于：

1.本发明可实时地鲁棒地判断当前经过的参考轨迹点，可按照参考轨迹依次地对参考轨迹点进行判断，不受参考轨迹重叠、交叉或接近所带来的影响。

2.本发明可在车辆刚刚经过当前参考轨迹点时立即对后一个参考轨迹点进行跟踪，解决了依靠距离对参考轨迹点进行判断所带来的经过当前参考轨迹点后无法立即切换到下一个轨迹参考点的问题。

3.本发明可使无人驾驶车辆从程序中段位置或任意指定参考轨迹点位置重新开始调试，解决了按照顺序依次判断轨迹点所带来的每次运行从起始点开始判断的问题。

附图说明

图1为本发明所述的参考轨迹点的算法的示意图。

图2为本发明所述的参考轨迹点的算法的逻辑示意图。

具体实施方式

为使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体实施方式，进一步阐述本发明。

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

实现本发明第一个目的的技术方案是：

由出发点开始，在每三个连续参考轨迹点中的第二个轨迹点处取第一点和第二点所连成的直线与第二点和第三点所连成直线的角平分线并得到该角平分线在全局坐标下(在试验场地附近任取一个原点并建立x-y坐标系，将车辆实时位置点及所有参考轨迹点的经纬度坐标转换到该坐标系下)的直线表达式，随后将车辆在全局坐标下的实时x-y坐标代入角平分线的直线方程得到结果值，并将第三个参考轨迹点的x-y坐标代入角平分线的直线方程得到结果值，一旦这两个结果值异号，则说明当前车辆坐标与第三个参考轨迹点分别位于角平分线两侧，即认定为尚未通过第二个参考轨迹点；一旦这两个结果值同号，则说明当前车辆坐标与第三个参考轨迹点位于此角平分线同侧，即认定为已通过第二个参考轨迹点，之后立即进行对后三个连续参考轨迹点的同样判断。从而可实时地得到当前经过的参考轨迹点。

本发明的第二个目的是使无人驾驶车辆在判别当前参考轨迹点时不会因为参考轨迹的重叠、交叉、接近(如掉头行驶)所造成的判断失误问题，从而鲁棒地对参考轨迹进行跟踪。

实现本发明第二个目的的技术方案是：

从参考轨迹出发点开始，按照参考轨迹的行进路线，依次判断每一个参考轨迹点，只做是否经过当前参考轨迹点的判断，并只在判断得到经过当前参考轨迹点时才对后一个参考轨迹点进行判断。因此，当轨迹发生重叠、交叉或较接近的情况时，不会错误地判断为经过其他轨迹点，从而导致车辆行驶方向发生错误的改变。

本发明的第三个目的是使无人驾驶车辆在轨迹跟踪调试过程中时，时常会出现需要中断运行中的程序并重新运行该程序的情况。如果出现问题，程序随时可从当前参考轨迹点或从指定参考轨迹点重新进行路径跟踪。

实现本发明第三个目的的技术方案是：

在车辆轨迹跟踪时，按照参考轨迹经过的顺序将所有参考轨迹点进行排序，每经过一个参考轨迹点就将该点的序号记录下来，存到文档当中。在轨迹跟踪调试时，一旦出现问题需要重新运行程序，程序运行伊始便会从此文档中的最后一个参考轨迹点的序号开始进行判断，因此便可从中断点继续测试。同时，也可在文档中的最后一行人为手动输入期望的参考轨迹起始点序号，重新运行程序时便会从此序号开始进行判断。

实施例

参见图1和图2，当无人驾驶车辆开始进行轨迹跟踪时，首先获取所有参考轨迹点的全局坐标系下的坐标，并获取指定起始参考轨迹点的序号。

由指定参考轨迹点开始，如图1中的b点，计算点a与点b的距离s1及点b与下一个点c的距离s2，取s1,s2中的最小值s1并在线段bc上取离b点距离与s1相同的点e，取e点与a点中点f，并计算由b、f连成直线在全局坐标系下的方程。

从无人驾驶车辆的组合导航实时订阅车辆的位置经纬度坐标，并转换为车辆在全局坐标下的坐标。将车辆实时位置坐标代入方程计算得到结果v1，并将参考轨迹点c的全局坐标代入方程计算得到结果v2，判断v1和v2的乘积是否大于0。如果结果小于0，说明此时车辆的位置和参考轨迹点c处于直线bf的两侧，因此还未经过点b，重新订阅车辆位置信息并重复计算，知道结果大于0为止。如果结果大于0，说明此时车辆的位置和参考轨迹点c处于直线bf的同一侧，则说明此时无人驾驶车辆已经经过了点b，此时需要判断点b是否为参考轨迹的最后一个参考轨迹点，如果是则程序结束，无人驾驶车辆刹停，如果不是则将点b及后两个参考轨迹点c,d依次再代入上述计算，判断是否经过点c，直到到达最后一个参考轨迹点为止。

当前判断目标为参考轨迹的出发点时，取出发点和出发点后一个参考轨迹点所连成直线在出发点处的垂线，并得到垂线在全局坐标下的直线表达式；

然后将车辆在全局坐标系下的实时x-y坐标代入垂线的直线表达式得到实时位置的结果值，并将出发点后一个参考轨迹点的x-y坐标代入垂线的直线表达式得到出发点后一个参考轨迹点的结果值；

若实时位置的结果值和出发点后一个参考轨迹点的结果值异号，即判定为车辆尚未通过出发点；若实时位置的结果值和出发点后一个参考轨迹点的结果值同号，则判定为车辆已经通过出发点。

当前判断目标为参考轨迹的终点时，取终点和终点前一个参考轨迹点所连成直线在终点处的垂线，并得到垂线在全局坐标下的直线表达式；

然后将车辆在全局坐标系下的实时x-y坐标代入垂线的直线表达式得到实时位置的结果值，并将终点前一个参考轨迹点的x-y坐标代入垂线的直线表达式得到终点前一个参考轨迹点的结果值；

若实时位置的结果值和终点前一个参考轨迹点的结果值同号，即判定为车辆尚未抵达终点；若实时位置的结果值和终点前一个参考轨迹点的结果值异号，则判定为车辆已经抵达终点。

以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。