[发明专利]一种倒车轨迹跟踪方法

说明书

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

技术领域

本发明涉及一种倒车轨迹跟踪方法，特别是一种利用改进的视线法的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)＝θ-θ₂