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

权利要求书

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时，切换跟踪轨迹，跟踪下一条直线轨迹。