CN111487976A - 一种倒车轨迹跟踪方法 - Google Patents

Abstract

本发明公开了一种倒车轨迹跟踪方法，首先确定倒车半径，然后计算得到牵引车和拖车的预期的连接角，根据预期连接角得到所需的舵机角度并反馈给舵机，每隔给定的时间间隔重复上述方法，直至轨迹跟踪结束，同时每次执行上述方法均判断是否需要切换跟踪轨迹，以及时进行轨迹切换，实现多轨迹跟踪。本发明实现了二节分段式车辆的倒车路径跟踪功能，适用于复杂环境下的机器人移动作业，可广泛应用于工厂牵引机器人、机场拖拽牵引车等领域，算法的快速性和鲁棒性可保证轮式机器人在运动过程中具有较精确的跟踪能力；同时所提出的改进的LOS控制方法，可提高分段式驱动车辆倒车转向灵活性及改善路径跟踪精度。

Description

一种倒车轨迹跟踪方法

技术领域

本发明涉及一种倒车轨迹跟踪方法，特别是一种利用改进的视线法的PID控制法的一种倒车轨迹跟踪方法，属于路径跟踪控制领域。

背景技术

对已规划的路线进行路径跟踪，是指控制车辆在保持稳定性的前提下迅速、精确地跟踪参考轨迹，是轮式移动机器人和无人驾驶车辆研究领域的基础问题和热点问题。其衍生的二节拖车模型被广泛地应用于工厂、机场、航母等需要前驱车进行拖拽作业的场合。所以，对其倒车过程的研究，一直被广泛的应用在机器人定位的研究中。

在传统的路径跟踪方法中，如预瞄控制、滑模控制、模型预测控制和线性二次型调节器等方法中，算法运算量较大、不具备实时性，且易受到环境变化所影响。本发明利用视线法(line of sight,LOS)将牵引车的位置跟踪问题转化为牵引车和后部拖车的航向控制问题，通过PID控制器，使牵引车的航向角收敛于期望航向角，从而使牵引车不断倒车驶向期望航向点。

发明内容

针对上述现有技术，本发明要解决的技术问题是提供一种针对轮式移动机器人与二节拖车的具有快速性和鲁棒性的倒车轨迹跟踪方法，利用简化的向量计算，对传统的视线法(line of sight,LOS)算法进行改进，实现轮式移动机器人及拖挂二节拖车以倒车行进方式跟踪预期直线及多条直线的路径跟踪功能。

为解决上述技术问题，本发明的一种倒车轨迹跟踪方法，车辆为二节分段式车辆，车辆前部为牵引车，后部为无动力拖车，跟踪轨迹为直线轨迹，包括以下步骤：

S1：从拖车的尾部点，按照拖车的方位角向拖车后方延长出一条直线，得到一条反方向向量Yaw_Array，直线长度为裕度余量Margin，直线末端为倒车裕度点Margin_Point，余量Margin与拖车长度L2之间满足：

Margin＝L2×A

A为给定比例系数；

S2：将Margin_Point点视作后部拖车的虚拟尾部，并确定向量LOS_Array，具体为：

根据跟踪轨迹的起点和终点获取轨迹方向，先由Margin_Point点向跟踪轨迹作垂线，垂足为P，由P点沿着跟踪轨迹的方向移动LOS_Length长度得到LOS_Point点，LOS_Length与拖车长度L2满足：

LOS_Length＝L2×B

B为给定的比例系数；

将LOS_Point点与Margin_Point点连线，获得向量LOS_Array；

S3：计算向量LOS_Array和Yaw_Array向量所夹的圆弧半径，作为倒车的半径R；

S4：计算得到预期的牵引车和拖车的连接角θ₂，θ₂满足：

S5：计算出当前时刻所需要的舵机角度φ并反馈给舵机，φ满足：

θ(k)＝θ-θ₂

式中θ为当前时刻牵引车和拖车连接角，θ(k)为当前时刻连接角θ与预期连接角θ₂的差值，θ(k-1)为前一次计算时得到的差值，∑θ(k)为当前时刻之前全部时刻计算得到的差值的累计和；Kp、Ki、Kd为设定的PID控制系数；

当未完成轨迹跟踪时，执行S6，否则结束轨迹跟踪；

S6：t间隔时长后，返回S1，t为设定的时间间隔。

本发明还包括一种倒车轨迹跟踪方法，车辆为二节分段式车辆，车辆前部为牵引车，后部为无动力拖车，跟踪轨迹为折线轨迹，包含至少两条直线轨迹，包括以下步骤：

S1：从拖车的尾部点，按照拖车的方位角向拖车后方延长出一条直线，得到一条反方向向量Yaw_Array，直线长度为裕度余量Margin，直线末端为倒车裕度点Margin_Point，余量Margin与拖车长度L2之间满足：

Margin＝L2×A

A为给定比例系数；

S2：将Margin_Point点视作后部拖车的虚拟尾部，并确定向量LOS_Array，具体为：

根据跟踪轨迹的起点和终点获取轨迹方向，先由Margin_Point点向跟踪轨迹作垂线，垂足为P，由P点沿着跟踪轨迹的方向移动LOS_Length长度得到LOS_Point点，LOS_Length与拖车长度L2满足：

LOS_Length＝L2×B

B为给定的比例系数；

将LOS_Point点与Margin_Point点连线，获得向量LOS_Array；

S3：计算向量LOS_Array和Yaw_Array向量所夹的圆弧半径，作为倒车的半径R；

S4：计算得到预期的牵引车和拖车的连接角θ₂，θ₂满足：

S5：计算出当前时刻所需要的舵机角度φ并反馈给舵机，φ满足：

θ(k)＝θ-θ₂

式中θ为当前时刻牵引车和拖车连接角，θ(k)为当前时刻连接角θ与预期连接角θ₂的差值，θ(k-1)为前一次计算时得到的差值，∑θ(k)为当前时刻之前全部时刻计算得到的差值的累计和；Kp、Ki、Kd为设定的PID控制系数；

当未完成轨迹跟踪时，执行S6，否则结束轨迹跟踪；

S6：t间隔时长后，判断是否需要切换跟踪的直线轨迹：当需要切换时，返回S1并将切换后的直线轨迹的起点和终点作为S2中的起点和终点，当不需要时，返回S1。

本发明还包括：

1.S3中倒车的半径R满足：

其中，

(x4，y4)和(x5，y5)分别为LOS_Array和Yaw_Array的向量坐标。

2.S6中判断是否需要切换跟踪的倒车轨迹具体为：

设定半径r，当拖车尾P2点距离两条直线轨迹的转折点Q点小于r时，切换跟踪轨迹，跟踪下一条直线轨迹。

本发明的有益效果：本发明设计的方法解决了轮式移动机器人与二节拖车的倒车路径跟踪的快速性和鲁棒性问题，本发明针对二节拖车倒车不稳定现象，严格控制其倒车方式，将牵引车的位置跟踪问题转化为牵引车和后部拖车的倒车航向控制问题，通过PID控制器，使牵引车的航向角收敛于期望航向角，行驶的轨迹收敛于期望的倒车圆弧。从而使牵引车不断驶向期望航向点，同时让后部拖车的姿态能调整为预期的路径方向一致。本发明实现了二节分段式车辆的倒车路径跟踪功能，适用于复杂环境下的机器人移动作业，可广泛应用于工厂牵引机器人、机场拖拽牵引车等领域，算法的快速性和鲁棒性可保证轮式机器人在运动过程中具有较精确的跟踪能力；同时所提出的改进的LOS控制方法，可提高分段式驱动车辆倒车转向灵活性及改善路径跟踪精度。

附图说明

图1是方法流程图；

图2是牵引车和后部拖车的简易示意图；

图6是二节拖车圆弧运动半径示意图；

图3是LOS规划示意图；

图4是找寻LOS向量示意图；

图5是计算向量所夹圆弧的半径示意图；

图7是切换跟踪目标示意图；

图8是多路径跟踪轨迹记录图；

具体实施方式

本发明以前部牵引车和后部拖车能够稳定倒车为前提，为其规划期望的倒车圆弧，并严格控制两车连接角θ，使其按照预期规定的半径进行倒车，并一步步接近预期直线轨迹，同时让后部拖车的姿态能调整为与预期的路径方向一致。利用改进的LOS算法，将牵引车的位置跟踪问题转化为牵引车的航向控制问题，通过PID控制器，使牵引车的航向角收敛于期望航向角，从而使牵引车不断驶向期望航向点。通过设置不同的期望航向点，实现牵引车航迹跟踪控制；当跟踪的直线为两条或多条时，改进的算法能自动判断是否抵达直线转折处，并让牵引车自动切换跟踪目标，跟踪下一条目标直线，以此实现多直线目标跟踪；此方法不仅可以适用于纯仿真系统，还可以在现实中搭建实物车辆模型，进行实物仿真，予以实现。利用传感器检测路径位置、路径方向、前部牵引车位置、姿态、后部拖车位置、姿态、两车连接角等车辆实时数据的情况下，通过相同的计算方式，能够对每个时刻进行实时计算，及时矫正倒车半径，从而让现实中的牵引车实现跟踪路径。

路径跟踪方法具体为：

(1)由轮式机器人在极坐标系中的运动学方程出发，研究其倒车稳定性问题，并使用PID控制，使其能沿着算法规定的圆弧进行稳定的圆弧倒车；

(2)利用在目标路径上设置期望航向点的方式，将牵引车的跟踪目标由直线转化为具体的期望航向点；

(3)从后部拖车的尾部，按照拖车的倒车方向，画一条反方向向量，向后方延长出一条直线，选取一个固定长度的余量，找到一个倒车裕度点；

(4)将拖车的尾部与倒车裕度点连接，获得倒车方向向量。将倒车裕度点与期望航向点连接，获得期望航向向量。

(5)对两个向量，求其所夹圆弧的半径，并让牵引车按照这个期望的半径进行圆弧倒车；

(6)每隔0.02秒进行一次计算，将直线路径跟踪划分为每个时刻的圆弧跟踪。

切换跟踪路径的方法具体为：

(1)当跟踪的直线为两条时，两直线存在一个转折点。以转折点为圆心，以半径r画一个虚拟圆；

(2)当拖车尾部进入该虚拟圆内，即尾部坐标距转折点的距离小于r时，牵引车自动切换跟踪目标，跟踪下一条目标直线；

(3)每隔0.02秒进行一次权利要求2所述的计算后，都需要判断一下以上条件是否成立，若成立，切换跟踪下一条直线；反之，继续跟踪原直线，不切换。

(4)跟踪2条以上多直线路径的方法同理，据此，可以控制牵引车依次跟踪多条目标直线。

搭建由传感器组成的检测系统，实时检测所需的车辆数据。

(1)测量的传感器可以选用UWB定位模块、电子罗盘、角度编码器等，不作限制，可以根据实际情况自行选用；

(2)需要测量的数据包括目标直线所有拐点的位置、轨迹方向，和前部牵引车位置、姿态、后部拖车位置、姿态、两车连接角等车辆实时数据；

获取车辆的状态数据后，上述计算方式，每隔0.02秒采集一次传感器传输来的位姿数据，对每个时刻进行实时计算，及时矫正倒车需跟踪圆弧的半径，并据此计算牵引车需要倒车行驶的方向，即牵引车的舵机角度，进行实时控制，便可在现实中实现牵引车和二节拖车的倒车路径跟踪。

本发明可以分为跟踪单一直线和跟踪多条直线的方法。

针对二节分段式车辆进行倒车控制，前部为牵引车，后部为无动力拖车。前部牵引车为主要控制单位，采用常见的4轮机构，前轮为转向机构，后轮为驱动机构，本发明简化为2轮自行车模型，长度为L1。

后部拖车为3轮机构，其中前轮抱夹在牵引车的后轮中心，采用无杆牵引的方式，由前部牵引车提供动力，拖车自身无动力系统，同样简化为2轮自行车模型，长度为L2。

牵引车和后部拖车的连接点为牵引车的后轮中心，标记为P1。拖车尾部为拖车2个后轮的中心点，标记为P2。牵引车和拖车之间的角度可以自由旋转，但是有角度限位。如图2所示。

结合图1，本发明具体实施方式包括以下步骤：

步骤1：本发明主要目标是控制牵引车与后部拖车的实物进行倒车行驶。所以需要传感器测量车辆的行驶数据，需要的重要数据有：

1.牵引车的坐标，可通过UWB定位模块或蓝牙定位模块进行定位。

2.牵引车和后部拖车之间的连接角θ，可用角度编码器测得。

3.牵引车的行驶方向，可通过电子罗盘或者陀螺仪测得。

4.后部拖车的行驶方向，可用电子罗盘测得，也可通过角度编码器叠加牵引车的行驶方向获得。

5.欲跟踪路径的2个端点坐标，与路径的方向向量。

上述传感器不唯一，可根据实际情况自行选用。

步骤2：该步骤需要进行坐标与向量计算，计算牵引车倒车过程中走过的圆弧半径，并严格控制连接角θ，使其按照预期规定的半径进行倒车。最终目标是，一步步接近欲跟踪路径，同时让后部拖车的姿态能调整为欲跟踪路径方向一致。

基本思路是，先在欲跟踪轨迹上找一个期望航向点，由后部拖车的尾部后端的Margin_Point与期望航向点连线，获得一个新的向量LOS_Array，让舵机跟踪这个LOS_Array。

寻找Margin_Point的方法是从后部拖车的尾部，按照拖车的方位角，画一条反方向向量Yaw_Array，向后方延长出一条直线，选取一个长度的裕度Margin(余量)，找到倒车裕度点Margin_Point。这个点相当于是超前调节，看看牵引车行进的前方会不会遇障，如图3所示。长度余量Margin的大小与拖车长度L2有关，一般取：

Margin＝L2×0.5 (1)

比例系数可以取0.5-0.7。

步骤3：把这个Margin_Point点视作后部拖车的虚拟尾部，并在欲跟踪轨迹上寻找一个期望航向点LOS_Point，画出向量LOS_Array。这一步骤需要获知欲跟踪直线的两个端点坐标，并获取轨迹方向。

先将Margin_Point点对着目标直线作垂线，垂足为P，由P点沿着欲跟踪直线的方向，向上以长度LOS_Length寻找LOS_Point，如图4所示。LOS_Length的大小与拖车长度L2有关，一般取：

LOS_Length＝L2×2 (2)

比例系数可以取1.8-2.7。

找到LOS_Point后，将其Margin_Point点连线，便获得向量LOS_Array。

步骤4：将向量LOS_Array和Yaw_Array向量组合，寻找两向量所夹的圆弧，即图4中两个向量中间的圆弧，计算其半径，作为倒车的半径。

在已知LOS_Array和Yaw_Array的向量坐标(x4，y4)和(x5，y5)后，可以用公式(3)计算两向量夹角：

获取向量夹角α后，可以继续下一步计算两个向量所夹扇形的半径，该半径即为所求半径，即需要二节拖车跟踪的圆弧半径。

如图5所示，单独拿出这两个向量，求其夹角圆弧。由于倒车的圆弧一定经过后部拖车尾部P2点，所以图中线段长度OP2＝Margin，则圆弧半径为：

根据公式(4)，可以计算出倒车半径R。

步骤5：在倒车过程中，由于其倒车轨迹所经过的圆弧大小，完全是由连接角θ来决定的。如图6所示，若已知连接角θ，则运动的半径也能唯一确定。

由公式(5)得到旋转半径。半径和当前的连接角θ有关，所以若想得到一个圆弧半径为R的倒车轨迹，则需要控制两车的预期连接角为θ₂，才能控制后部拖车以给定半径的圆弧进行倒车。

于是，得到预期的连接角θ₂为：

同时，根据工程需求，进行限位操作：

工程中，有部分二节拖车对牵引车和拖车之间连接角度有限制，限位角度37°可以根据工程需求自行更改。

舵机角度φ、连接角θ与倒车半径的关系如图6所示。当前的角度和预期的角度的偏差θ(k)＝θ-θ₂时，采用PID控制，则经过修正后的舵机角度φ为：

式中θ为当前时刻两车连接角度数，θ(k)为当前角度与预期角度的差值，θ(k-1)为前一次测量时即上一计算时刻时的二者差值，∑θ(k)为历史时刻所有偏差的累计和，k＝1,2,3……。

Kp、Ki、Kd为PID系数，根据多次试验，合适的PID系数，与牵引车和拖车的长度比有关系。

式中Kp、Kd、Ki为工程中最合适的数值，根据实际项目需要，取值范围可以上下浮动0.2。

由于很多工程中限制舵机的转角最大为45°，所以，对求出的舵机角度φ需要有一个限位操作：

限位角度可以根据工程需求自行更改。每次计算，均需要限位操作。

公式(8)需要实时获取牵引车和后部拖车之间的连接角θ的度数，并代入公式(8)进行计算，每隔0.02秒进行一次计算。据此计算出倒车所需的舵机角，传输给牵引车的控制核心。并实时修改牵引车的舵机角度，将倒车直线路径跟踪分解为计算每个时刻的圆弧半径，即可控制牵引车推动后部拖车，倒车跟踪预期直线。

可根据实际情况，适当修改计算的时间密度。

步骤6：当跟踪的直线为两条时，两直线的转折点位于Q点。可规定一个半径r，当拖车尾部P2点距离Q点小于r时，即拖车尾部已进入图中圆内时，自动切换跟踪目标，跟踪下一条目标直线，如图7所示。

若后部拖车尾部的坐标为P2(x0，y0)，转折点Q(x2，y2)，则根据公式(11)，进行判断，一般取r＝LOS_Length。

每次进行步骤5的计算后，都需要判断一下公式(11)是否成立，若成立，切换跟踪下一条直线；反之，继续跟踪原直线，不切换。

切换所跟踪直线的方法，只需要把直线的起点和终点进行更换。如图7所示，假设当前所跟踪直线的起点为N(x1，y1)，终点为Q(x2，y2)；需要切换的直线的起点为Q(x2，y2)，终点M为(x3，y3)。

只需根据公式(12)，改变欲跟踪的直线的起点和终点。修改后，需要变更的为步骤3中寻找LOS_Point方法，需要根据新的直线的起点和终点重新寻找LOS_Point和向量LOS_Array，其余的步骤不变。

根据该公式(11)，便可判断是否需要切换目标直线。同理，可以控制牵引车依次跟踪多条目标直线。直到所有直线全部走完为止，算法终止。

(1)注意事项：在现实中，如需对二节分段式车辆进行实物仿真。需要提前进行计算机算法仿真，如图8仿真轨迹记录图所示。实验需要详细记录牵引车和后部拖车的倒车运动轨迹，并由轨迹记录判断其倒车灵活性，是否满足实物仿真的要求，有没有出现不稳定的情况。

确认二节分段式车辆的倒车转向具有足够的灵活性，路径跟踪具有足够的精度后，方可进行实物仿真。

最后的实物仿真采用等比缩放的模型进行实验，比例为1:56。该实验使用“3轮飞机模型”作为后部拖车进行实验，后部拖车的前轮悬空挂于牵引车的尾部(无杆牵引方式)，通过步骤1所示的传感器检测系统来测量车辆信息，代入步骤2-6的方法中，由牵引车控制转向舵机角度，并提供动力推动后部拖车进行倒车跟踪实验。

Claims (4)

1.一种倒车轨迹跟踪方法，车辆为二节分段式车辆，车辆前部为牵引车，后部为无动力拖车，跟踪轨迹为直线轨迹，其特征在于，包括以下步骤：

S1：从拖车的尾部点，按照拖车的方位角向拖车后方延长出一条直线，得到一条反方向向量Yaw_Array，直线长度为裕度余量Margin，直线末端为倒车裕度点Margin_Point，余量Margin与拖车长度L2之间满足：

Margin＝L2×A

A为给定比例系数；

S2：将Margin_Point点视作后部拖车的虚拟尾部，并确定向量LOS_Array，具体为：

根据跟踪轨迹的起点和终点获取轨迹方向，先由Margin_Point点向跟踪轨迹作垂线，垂足为P，由P点沿着跟踪轨迹的方向移动LOS_Length长度得到LOS_Point点，LOS_Length与拖车长度L2满足：

LOS_Length＝L2×B

B为给定的比例系数；

将LOS_Point点与Margin_Point点连线，获得向量LOS_Array；

S3：计算向量LOS_Array和Yaw_Array向量所夹的圆弧半径，作为倒车的半径R；

S4：计算得到预期的牵引车和拖车的连接角θ₂，θ₂满足：

S5：计算出当前时刻所需要的舵机角度φ并反馈给舵机，φ满足：

θ(k)＝θ-θ₂

式中θ为当前时刻牵引车和拖车连接角，θ(k)为当前时刻连接角θ与预期连接角θ₂的差值，θ(k-1)为前一次计算时得到的差值，∑θ(k)为当前时刻之前全部时刻计算得到的差值的累计和；Kp、Ki、Kd为设定的PID控制系数；

当未完成轨迹跟踪时，执行S6，否则结束轨迹跟踪；

S6：t间隔时长后，返回S1，t为设定的时间间隔。

2.根据权利要求1所述的一种倒车轨迹跟踪方法，车辆为二节分段式车辆，车辆前部为牵引车，后部为无动力拖车，跟踪轨迹为折线轨迹，包含至少两条直线轨迹，其特征在于，包括以下步骤：

S1：从拖车的尾部点，按照拖车的方位角向拖车后方延长出一条直线，得到一条反方向向量Yaw_Array，直线长度为裕度余量Margin，直线末端为倒车裕度点Margin_Point，余量Margin与拖车长度L2之间满足：

Margin＝L2×A

A为给定比例系数；

S2：将Margin_Point点视作后部拖车的虚拟尾部，并确定向量LOS_Array，具体为：

根据跟踪轨迹的起点和终点获取轨迹方向，先由Margin_Point点向跟踪轨迹作垂线，垂足为P，由P点沿着跟踪轨迹的方向移动LOS_Length长度得到LOS_Point点，LOS_Length与拖车长度L2满足：

LOS_Length＝L2×B

B为给定的比例系数；

将LOS_Point点与Margin_Point点连线，获得向量LOS_Array；

S3：计算向量LOS_Array和Yaw_Array向量所夹的圆弧半径，作为倒车的半径R；

S4：计算得到预期的牵引车和拖车的连接角θ₂，θ₂满足：

S5：计算出当前时刻所需要的舵机角度φ并反馈给舵机，φ满足：

θ(k)＝θ-θ₂

式中θ为当前时刻牵引车和拖车连接角，θ(k)为当前时刻连接角θ与预期连接角θ₂的差值，θ(k-1)为前一次计算时得到的差值，∑θ(k)为当前时刻之前全部时刻计算得到的差值的累计和；Kp、Ki、Kd为设定的PID控制系数；

当未完成轨迹跟踪时，执行S6，否则结束轨迹跟踪；

S6：t间隔时长后，判断是否需要切换跟踪的直线轨迹：当需要切换时，返回S1并将切换后的直线轨迹的起点和终点作为S2中的起点和终点，当不需要时，返回S1。

3.根据权利要求1或2所述的一种倒车轨迹跟踪方法，其特征在于：S3所述倒车的半径R满足：

其中，

(x4，y4)和(x5，y5)分别为LOS_Array和Yaw_Array的向量坐标。

4.根据权利要求2所述的一种倒车轨迹跟踪方法，其特征在于：S6所述判断是否需要切换跟踪的倒车轨迹具体为：

设定半径r，当拖车尾P2点距离两条直线轨迹的转折点Q点小于r时，切换跟踪轨迹，跟踪下一条直线轨迹。

Images