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

权利要求书

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

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

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

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

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

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

2.根据权利要求1所述的无人驾驶轨迹跟踪中实时判别当前参考轨迹点的算法，其特征在于，所述全局坐标系的构建方法为：在参考轨迹附近任取一个原点并建立x-y坐标系，将车辆实时位置点及所有参考轨迹点的经纬度坐标转换到该x-y坐标系下。

3.根据权利要求1所述的无人驾驶轨迹跟踪中实时判别当前参考轨迹点的算法，其特征在于，当前判断目标为参考轨迹的出发点时，取出发点和出发点后一个参考轨迹点所连成直线在出发点处的垂线，并得到垂线在全局坐标系下的直线表达式；

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

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

4.根据权利要求1所述的无人驾驶轨迹跟踪中实时判别当前参考轨迹点的算法，其特征在于，当前判断目标为参考轨迹的终点时，取终点和终点前一个参考轨迹点所连成直线在终点处的垂线，并得到垂线在全局坐标系下的直线表达式；

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

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

5.根据权利要求1所述的无人驾驶轨迹跟踪中实时判别当前参考轨迹点的算法，其特征在于，从参考轨迹的出发点开始，按照参考轨迹的行进路线，对每一个当前参考轨迹点只做车辆是否经过当前参考轨迹点的判断，并只在判断得到车辆经过当前参考轨迹点时，才对后一个参考轨迹点进行判断。

6.根据权利要求1所述的无人驾驶轨迹跟踪中实时判别当前参考轨迹点的算法，其特征在于，在车辆轨迹跟踪时，按照参考轨迹经过的顺序将所有参考轨迹点进行排序，每经过一个参考轨迹点就将该参考轨迹点的序号记录下来；在轨迹跟踪调试时，若出现问题需要重新运行程序，程序运行伊始便会从记录的最后一个参考轨迹点的序号开始进行判断，实现从中断点继续测试。

7.根据权利要求6所述的无人驾驶轨迹跟踪中实时判别当前参考轨迹点的算法，其特征在于，参考轨迹的起始参考点序号可通过手动输入，重新运行程序时便会从手动输入的起始参考点开始进行判断。