CN111176298B - 一种无人车轨迹录制与轨迹跟踪方法 - Google Patents

Abstract

一种无人车轨迹录制与轨迹跟踪方法，包括下列步骤：使用差分GPS或者多线激光雷达提供的定位信息，手动控制无人车行走，录制路径轨迹，记录各轨迹点的坐标信息和速度信息；寻找距离最近的第一目标点和下一目标点，三点拟合圆，求出圆上第一目标点的切线的反正切函数，作为第一目标点的弧度值，再计算第一目标点的弧度与无人车当前的弧度差值来计算车辆打角，并载入速度信息控制车辆行走，并实时更新打角行走，让车辆沿轨迹点一步一步进行跟踪轨迹，实现巡逻功能，具有跟踪准确的特点，启动点可在轨迹线上或线外的任意一点，适用性强。本发明技术方案适用于室外巡逻，能够长时间来回兜圈，并且鲁棒性强。

Description

一种无人车轨迹录制与轨迹跟踪方法

技术领域

本发明涉及无人车辆路径导航领域，具体涉及一种无人车轨迹录制与轨迹跟踪方法。

背景技术

随着自动驾驶车辆技术的发展，无人驾驶的导航方式多种多样，有激光导航、视觉导航、GPS导航、IMU等等。路径规划算法也有多种，例如全局路径规划、局部路径规划等等。现有传感器中，差分GPS、多线激光雷达都可以利用这种方式来进行轨迹录制与轨迹跟踪。差分GPS通过4个或4个以上的卫星来对车辆所在的经纬度和角度信息进行对准，但是GPS要求在比较空旷的地方运行，在有乌云遮挡、树林底下、隧道内等都无法接收到信号。而多线激光雷达要求在特征明显的地方运行，多线激光雷达上下各一定倾角同时发射出一系列激光束，通过计算光线返回的时间来推算所扫描物体的距离信息。多线激光雷达能够通过建图算法构建所处环境的地图信息，也就是生成点云图，后面使用点云图来进行定位与路径规划。

发明内容

本发明的目的是克服上述现有技术的缺点，本发明针对室外巡逻任务的场景，提出一种适用于室外巡逻、能够长时间来回兜圈、且鲁棒性强的轨迹录制与轨迹跟踪方法。

本发明是通过以下技术方案来实现的：

一种无人车轨迹录制与轨迹跟踪方法，包括下列步骤：

S1、手动控制无人车行走，录制路径轨迹，记录路径轨迹上各轨迹点的坐标信息和速度信息，形成具有坐标的路径轨迹图；

S2、根据所述路径轨迹图上无人车当前的位置点、预行走的方向以及录制的所述路径轨迹，从所述路径轨迹上的轨迹点中选取两点分别作为无人车的第一目标点和下一目标点，以无人车当前的位置点、所述第一目标点和下一目标点拟合出一个过该三点的圆，得出该圆的圆心坐标；

S3、将无人车当前的航向角的弧度值记为第一弧度值；计算所述第一个目标点在圆上的切线的倾斜角的弧度值记为第二弧度值；实时计算车辆的打角(即车辆方向盘的转角，下同)，所述车辆的打角＝第二弧度值-第二弧度值；根据车辆实时的打角、所述第一目标点上录制的速度信息来控制无人车行走，在行走的过程中不断计算更新车辆的打角，并根据实时更新的打角行走；

S4、实时计算无人车当前的位置点与第一目标点之间的距离，当该距离在容错范围内表示达到第一目标点；

S5、重复上述步骤S2、S3、S4进行轨迹跟踪。

进一步地，所述第二弧度值的倾斜角为所述第一个目标点在圆上的切线向量与x轴正方向的夹角，第二弧度值的取值范围为(-π，π)。取值为正表示从X轴逆时针旋转的角度，取值为负表示从X轴顺时针旋转的角度。所述第二弧度值可由C语言中的反正切函数atan2得到，以反正切函数值求出弧度值，根据目标弧度与当前航向角的弧度值之差即可直接得出车辆行走的打角，省去根据直线斜率的正负来判断弧度值的步骤。本发明以三个点拟合求出圆的圆弧曲线，以此控制无人车的行走，相比于常规两个点求出的弧线更加切合车辆行走轨迹，可以更准确控制车辆行走。

进一步地，所述无人车当前的位置点信息由差分GPS、IMU或多线激光雷达传感器得到，所述位置点信息包括位置点坐标和航向角。由上述传感器得到的位置信息通过计算换算成路径轨迹图上的位置坐标信息。进一步地，还可包括速度信息。

进一步地，所述位置点信息中的航向角为航向与x轴正方向的夹角，其取值范围为(-π，π)。取值为正表示从X轴逆时针旋转的角度，取值为负表示从X轴顺时针旋转的角度。

进一步地，所述步骤S2中无人车当前的位置点为所述路径轨迹上或路径轨迹外的任意点。

进一步地，所述步骤S2中选取第一目标点的方法为：如果车辆当前位点置为录制的轨迹点中的一点，则根据车辆行走方向，将路径轨迹图上该轨迹点的下一轨迹点作为第一目标点；如果车辆当前位置点不在录制的轨迹点中，则启动遍历算法，遍历所有录制的轨迹点，然后跟车辆当前位置点做差值比较，寻找距离车辆当前位置点最近的轨迹点作为第一目标点。可保证无人车可在任意位置启动，当偏离轨迹时，也可保证无人车重新回到轨迹线上。

所述步骤S4中无人车当前的位置点与第一目标点之间的距离、以及步骤S2中距离采用通用的坐标轴上两点间距离的算法计算得到。

进一步地，所述无人车上设置有防撞雷达，所述防撞雷达在车辆行驶过程中探测到有障碍物时，通过程序控制车辆进行避障，以防止意外发生，保证车辆行驶过程中的安全性。

进一步地，所述步骤S1中录制路径轨迹的方法，包括下述步骤：手动控制车辆在需要巡逻的地方行走，每隔一定距离记录车辆的当前位置点和当前速度。

进一步地，所述距离为1m，或根据路况决定。可根据需要巡逻的地方的路径情况，灵活设置各录制轨迹点间的间距，如当路线平缓、曲率半径大时，轨迹点间的间距可适当大，而路线较弯曲，曲率半径较小时，轨迹点间的间距可根据情况缩小，以能拟合出符合实际路线的轨迹为宜。

进一步地，所述下一目标点为所述路径轨迹上第一目标点的下一顺序点。

本发明使用差分GPS或者多线激光雷达提供的定位信息，先让车辆在需要行走的场景手动控制行驶来录取轨迹点，每一个录制的轨迹点都记录有车辆的位置信息和速度信息，然后通过寻找距离最近的第一目标点和下一目标点，三点拟合圆，通过圆求出圆上第一目标点的切线的倾斜角的反正切函数，作为第一目标点的弧度值，再计算第一目标点的弧度与无人车当前航向角的弧度差值来计算车辆打角，并载入速度信息控制车辆行走，并在行走的过程中根据车辆当前的角度实时更新车辆的打角，让车辆沿轨迹点跟踪一个个第一目标点以进行一步步轨迹跟踪，实现巡逻功能，具有跟踪准确的特点；以三个点拟合求出圆的圆弧曲线，以此控制无人车的行走，相比于常规两个点求出的弧线更加切合车辆行走轨迹，可以更准确控制车辆行走；启动点可在轨迹线上或线外的任意一点，适用性强。本发明技术方案适用于室外巡逻，能够长时间来回兜圈，并且鲁棒性强。

附图说明

图1为本发明实施例中的流程图；

图2为本发明实施例中录制的路径轨迹示意图；

图3为本发明实施例中的三点示意图；

图4为本发明实施例中的三点拟合圆的示意图；

图5为本发明实施例中第一弧度值和第二弧度值示意图；

图6为本发明实施例中无人车行走过程中实时更新车辆角度的示意图；

图7为本发明实施例中第二弧度值的表示示意图。

具体实施方式

一种无人车轨迹录制与轨迹跟踪方法，如图1所示，包括下列步骤：

S1、手动控制无人车行走，录制路径轨迹，记录路径轨迹上各轨迹点的坐标信息和速度信息，形成具有坐标的路径轨迹图；

S2、根据所述路径轨迹图上无人车当前的位置点、预行走的方向以及录制的所述路径轨迹，从所述路径轨迹上的轨迹点中选取两点分别作为无人车的第一目标点和下一目标点，以无人车当前的位置点、所述第一目标点和下一目标点拟合出一个过该三点的圆，得出该圆的圆心坐标；

S3、将无人车当前的航向角的弧度值记为第一弧度值；计算所述第一个目标点在圆上的切线的倾斜角的弧度值，记为第二弧度值；实时计算车辆的打角，所述车辆的打角＝第二弧度值-第二弧度值；根据车辆实时的打角、所述第一目标点上录制的速度信息来控制无人车行走，在行走的过程中不断计算更新车辆的打角，并根据实时更新的打角行走；

S4、当车辆接近所述第一目标点时，计算无人车当前的位置点与第一目标点之间的距离，当该距离在容错范围内表示达到第一目标点；

S5、重复上述步骤S2、S3、S4进行轨迹跟踪。

以图2至图6为例，进一步阐述本发明的技术方案，包括如下步骤：

(1)手动控制车辆在需要巡逻的地方行走，每隔一定距离(比如1米)记录车辆的当前位置及当前速度，形成一个轨迹点，并在坐标轴上形成如图2的路径轨迹图，轨迹图上包含有各轨迹点的坐标及速度信息。车辆的当前位置可由传感器探测得到，也可人工测量得到；当前速度可由测量上的仪表读数得到，也可由传感器探测得到，或者由相关速度测量装置测量得到。

(2)如图3中，L线为录制的路径轨迹中的一小段，图中A点为无人车当前的位置点，预行走的方向为沿L线逆时针的方向，计算路径轨迹L线上距离A点最近的点B作为无人车的第一目标点，C点为下一目标点，B点和C点均为L线上预先录制的轨迹点；如图4所示，以A点、B点和C点三点拟合出一个圆，该圆过A、B、C三点，圆心为O点；O点的坐标(x0,y0)由计算得出，A点坐标(x1,y1)由传感器探测计算得到，B点坐标(x2,y2)和C点坐标(x3,y3)由预先录制的数据信息得到，设B点预先录制的速度为v2。

(3)以图5为例，圆心O与B点的连线记为R1，B点的切线记为L1，A点处车辆当前的航向角的弧度值记为α0；L1线的倾斜角的弧度值记为α1；车辆的打角α＝第二弧度值-第一弧度值＝α1-α0；启动车辆，以车辆的打角α＝α1-α0、B点处录制的速度v2来控制无人车行走，如图6，在无人车行走过程中实时更新(如间隔20ms更新计算一次)车辆当前的航向角α01、α02、α03……，并计算车辆当前的打角，根据更新的打角实时调整车辆的行走方向，使之精确跟踪。

(4)实时计算(如间隔20ms计算一次)无人车当前位置点D(x4,y4)(由传感器探测计算得到)与第一目标点B之间的距离，检测线段BD的长度是否在预设的容错范围内，如是则判断车辆到达第一目标点B；如否则按照实时计算的打角和速度v2继续行走直至到达第一目标点B；

(5)重复上述步骤，重新寻找新的第一目标点、下一目标点进行轨迹跟踪。

如图5，所述第二弧度值的倾斜角为所述第一个目标点B在圆上的切线向量

与x轴正方向的夹角α1，第二弧度值的取值范围为(-π，π)。所述第二弧度值由C语言中的反正切函数atan2(double y,double x)得到。切线向量由切线及方向决定，故即使切线相同，如图7所示，切线L11、L13在同一条直线上，但由于运动的方向不同，其切线向量的倾斜角不同，切线向量

的倾斜角为α2，其范围在(π/2，π)之间，而切线向量

的倾斜角为α3，其范围在(0，-π/2)之间。切线向量

的倾斜角α4的范围在(-π/2，-π)之间。

C语言编程中，用反正切函数atan2(double y,double x)来求取切线向量的弧度值，由此得出第二弧度值。结果为正表示从X轴逆时针旋转的角度，结果为负表示从X轴顺时针旋转的角度。以反正切函数值求出弧度值，根据目标弧度与当前弧度之差即可直接得出车辆行走的打角，省去根据直线斜率的正负来判断弧度值的步骤。第一弧度值为A点处车辆的当前航向角的弧度值，弧度值与角度间的转换公式参照：弧度值＝角度×π÷180°。

所述步骤S1中录制路径轨迹的方法，包括下述步骤：手动控制车辆在需要巡逻的地方行走，每隔一定距离记录车辆的当前位置点和当前速度。所述距离为1m，或者根据需要巡逻的地方的路径情况，灵活设置各录制轨迹点间的间距，如当路线平缓、曲率半径大时，轨迹点间的间距可适当大，而路线较弯曲，曲率半径较小时，轨迹点间的间距可根据情况缩小，以能拟合出符合实际路线的轨迹为宜。轨迹间的间距可根据勾股定理计算。

所述无人车当前的位置点A点、D点的信息由差分GPS、IMU、多线激光雷达或GPS-INS得到。无人车当前的位置点信息主要包括坐标点信息和航向角信息，还可包括速度信息。所述位置点信息中的航向角为航向与x轴正方向的夹角，其取值范围为(-π，π)。如正则表示从X轴逆时针旋转的角度，如为负则表示从X轴顺时针旋转的角度。如差分GPS可输出相对于坐标信息的经纬度和航向角度，多线激光雷达可运用SLAM算法(同步定位与地图构建)在坐标轴上得出点坐标和航向角，也可采用采陀螺仪获取车辆的绝对角度，上述获取车辆当前位置点信息的方法可借鉴现有技术，在此不一一赘述。

所述步骤S2中无人车当前的位置点为所述路径轨迹上或路径轨迹外的任意点。所述步骤S2中选取第一目标点的方法为：如果车辆当前位置点为录制的轨迹点中的一点，则根据车辆行走方向，将路径轨迹图上该轨迹点的下一轨迹点作为第一目标点；如果车辆当前位置点不在录制的轨迹点中，则启动遍历算法，遍历所有录制的轨迹点，然后跟车辆当前位置点做差值比较，寻找距离车辆当前位置点最近的轨迹点作为第一目标点。即无人车可在录制的轨迹路径上或路径外的任意一点启动，当偏离轨迹时，也可保证无人车重新回到轨迹线上。

按照上述方法，无人车能够一个一个到达第一目标点，无人车与第一目标点的之间距离的容错范围值可根据实际情况调整，可考虑的因素有路况、周边环境以及第一目标点的速度值，如当速度较快时，容错范围可适当放大。如无法到达容错范围内，即无法到达第一目标点时，如无人车与第一目标点间的距离渐变大，即偏离跟踪轨迹时，根据当前位置点重新寻找第一目标点和下一目标点，按照上述S2至S4步骤重新控制行走。

所述步骤S4中无人车当前的位置点与第一目标点之间的距离、以及步骤S2中距离采用通用的坐标轴上两点间距离的算法计算得到。以上述为例，D点与B点的距离

步骤S2中无人车当前的位置点A点与路径轨迹上轨迹点间的距离计算也可采用此种方法，通过遍历比较距离各轨迹点的距离得到第一目标点，而所述下一目标点为所述路径轨迹上第一目标点的下一顺序点。如路径轨迹上C点为B点的下一轨迹点，当选定B点为第一目标点时，C点就成为下一目标点。

所述无人车上设置有防撞雷达，所述防撞雷达在车辆行驶过程中探测到有障碍物时，通过程序控制车辆进行避障，以防止意外发生，保证车辆行驶过程中的安全性。而车辆的避障技术可参照现有相关技术方案，如检测到行驶路线上有障碍物，则根据障碍物的距离采取刹车行动，如果障碍物长时间不动的情况下，则根据设计的绕行程序进行避障行走。

上列详细说明是针对本发明可行实施例的具体说明，该实施例并非用以限制本发明的专利范围，凡未脱离本发明所为的等效实施或变更，均应包含于本案的专利范围中。

Claims (10)

1.一种无人车轨迹录制与轨迹跟踪方法，其特征在于，包括下列步骤：

S1、手动控制无人车行走，录制路径轨迹，记录路径轨迹上各轨迹点的坐标信息和速度信息，形成具有坐标的路径轨迹图；

S2、根据所述路径轨迹图上无人车当前的位置点、预行走的方向以及录制的所述路径轨迹，从所述路径轨迹上的轨迹点中选取两点分别作为无人车的第一目标点和下一目标点，以无人车当前的位置点、所述第一目标点和下一目标点拟合出一个过该三点的圆，得出该圆的圆心坐标；

S3、将无人车当前的航向角的弧度值记为第一弧度值；计算所述第一个目标点在圆上的切线的倾斜角的弧度值，记为第二弧度值；实时计算车辆的打角，所述车辆的打角＝第二弧度值-第二弧度值；根据车辆实时的打角、所述第一目标点上录制的速度信息来控制无人车行走，在行走的过程中不断计算更新车辆的打角，并根据实时更新的打角行走；

S4、实时计算无人车当前的位置点与第一目标点之间的距离，当该距离在容错范围内表示达到第一目标点；

S5、重复上述步骤S2、S3、S4进行轨迹跟踪。

2.根据权利要求1所述的一种无人车轨迹录制与轨迹跟踪方法，其特征在于，所述步骤S2中选取第一目标点的方法为：如果车辆当前位置点为录制的轨迹点中的一点，则根据车辆行走方向，将路径轨迹图上该轨迹点的下一轨迹点作为第一目标点；如果车辆当前位置点不在录制的轨迹点中，则启动遍历算法，遍历所有录制的轨迹点，然后跟车辆当前位置点做差值比较，寻找距离车辆当前位置点最近的轨迹点作为第一目标点。

3.根据权利要求2所述的一种无人车轨迹录制与轨迹跟踪方法，其特征在于，所述第二弧度值的倾斜角为所述第一个目标点在圆上的切线向量与x轴正方向的夹角，第二弧度值的取值范围为-π到π。

4.根据权利要求3所述的一种无人车轨迹录制与轨迹跟踪方法，其特征在于，所述无人车当前的位置点信息由差分GPS、IMU或多线激光雷达得到，所述位置点信息包括位置点坐标和航向角。

5.根据权利要求4所述的一种无人车轨迹录制与轨迹跟踪方法，其特征在于，所述位置点信息中的航向角为航向与x轴正方向的夹角，其取值范围为-π到π。

6.根据权利要求2所述的一种无人车轨迹录制与轨迹跟踪方法，其特征在于，所述步骤S2中无人车当前的位置点为所述路径轨迹上或路径轨迹外的任意点。

7.根据权利要求1至6任一项所述的一种无人车轨迹录制与轨迹跟踪方法，其特征在于，所述无人车上设置有防撞雷达，所述防撞雷达在车辆行驶过程中探测到有障碍物时，控制车辆进行避障。

8.根据权利要求1至6任一项所述的一种无人车轨迹录制与轨迹跟踪方法，其特征在于，所述步骤S1中录制路径轨迹的方法，包括下述步骤：手动控制车辆在需要巡逻的地方行走，每隔一定距离记录车辆的当前位置点和当前速度。

9.根据权利要求8所述的一种无人车轨迹录制与轨迹跟踪方法，其特征在于，所述距离为1m或根据路况决定。

10.根据权利要求1至6任一项所述的一种无人车轨迹录制与轨迹跟踪方法，其特征在于，所述下一目标点为所述路径轨迹上第一目标点的下一顺序点。

Images