CN113821026B - 一种基于rhc的有杆拖挂式无人系统在线轨迹跟踪控制方法 - Google Patents

Abstract

一种基于RHC的有杆拖挂式无人系统在线轨迹跟踪控制方法。首先，基于有杆拖挂式无人系统的运动学模型建立有杆拖挂式无人系统的运动学模型。其次，建立有杆拖挂式无人系统跟踪控制问题中的不等式约束。最后，建立有杆拖挂式无人系统的跟踪控制模型，根据RHC思想将全区间内的跟踪控制问题转化为单个时间窗口内的最优控制问题，对单个时间窗口内的最优控制问题进行高效求解，基于每个时间窗口内的结果依次滚动时间窗口以实现全区间内的在线轨迹跟踪控制。本发明能以更高的精度、效率和稳定性，在线处理环境中的实时、动态扰动，能够有效解决有杆拖挂式无人系统的倒车或正向行驶过程中的跟踪控制问题，还可应用于带状态‑控制约束的跟踪控制问题，具有很强的可操作性和可行性。

Description

一种基于RHC的有杆拖挂式无人系统在线轨迹跟踪控制方法

技术领域

本发明属于有杆拖挂式无人系统的自动化与控制技术领域，涉及一种基于滚动时域控制(RHC：Receding Horizon Control)的有杆拖挂式无人系统在线轨迹跟踪控制方法。

背景技术

有杆拖挂式无人系统的轨迹跟踪控制问题是通过设计合理的跟踪方法，并结合其运动学/动力学及相关约束关系，对标准轨迹或者理想轨迹进行跟踪，从而得出实际轨迹和控制律的过程。

有杆拖挂式无人系统为一种牵引-拖挂车系统(Tractor-Trailer System)，该类系统被广泛应用于航空、农业、运输等各个领域。目前，基于线性化模型的线性控制技术已广泛应用于牵引-拖挂车系统控制，但如果转向角大于10度，线性化模型的精度就会降低。为了提高精度，目前提出基于非线性控制理论来设计跟踪方法，如横向函数法、虚拟转向法、非线性模型预测控制和李雅普诺夫方法等。

然而，有杆拖挂式无人系统的轨迹跟踪控制问题中仍存在以下难点和重点：(1)系统的非线性和耦合特性；(2)输入和输出的物理约束；(3)环境不确定性的影响。此外，现有的大部分跟踪控制算法难以处理环境中的实时、动态扰动，且均只能解决系统的倒车或者正向行驶问题，而对于既涉及倒车又涉及正向行驶的参考轨迹，其跟踪问题没有得到很好的解决。这主要是因为有杆拖挂式无人系统的运动学或者动力学模型较为复杂，对于正向运动与倒车之间切换的过程很容易发散，而现有方法难以有效解决这一过程的轨迹跟踪控制问题。

综上所述，目前，工程应用上迫切需要一种能兼顾计算精度和效率的、具有良好适用性的有杆拖挂式无人系统轨迹跟踪控制算法。

发明内容

为了解决上述技术问题，本发明提出了一种用于有杆拖挂式无人系统轨迹跟踪控制问题，能在线处理实时动态扰动，且可兼顾效率、精度及工程许用性的方法。

为了达到上述目的，本发明采用的技术方案为：

一种基于RHC的有杆拖挂式无人系统在线轨迹跟踪控制方法，包括以下步骤：

步骤1：建立有杆拖挂式无人系统的运动学模型

将挂车和拖车的速度分别定义为v₁和v₃；变量θ₁、θ₂和θ₃分别是挂车、牵引杆和拖车的方位角，β₁＝θ₂-θ₁、β₂＝θ₃-θ₂，其中，β₁表示牵引杆与挂车纵向方向之间的夹角，β₂表示牵引杆与拖车纵向方向之间的夹角；α表示拖车前轮转向角；L₁、L₂、M、L₃分别表示挂车后轮中心距挂车与牵引杆铰接点处的长度、牵引杆的长度、牵引杆与拖车的铰接点距拖车后轮中心处的长度、拖车前后轮距；x₁、y₁分别表示挂车后轮中心位置的横坐标和纵坐标。则将有杆拖挂式无人系统的状态空间表达为X＝(x₁,y₁,θ₁,β₁,β₂,α,v₁)^T，控制向量定义为U＝(u₁,u₂)^T，其中，和/>

为提高基于最优控制的轨迹规划方法的计算效率，且选择的状态变量应便于施加边界条件和其他机械约束，建立如下所示的有杆拖挂式无人系统的运动学模型：

步骤2：建立有杆拖挂式无人系统跟踪控制问题中的不等式约束

步骤2-1：建立避碰不等式约束

通常标准轨迹为无碰撞的可行轨迹，但由于外界扰动等因素的原因，实际轨迹与标准轨迹不可能完全重合，若实际轨迹与标准轨迹偏差过大，可能会导致实际轨迹与障碍物发生碰撞的现象。为避免这个问题的发生，将实际轨迹与标准轨迹的偏差约束在一个可行的范围之内，具体为：

(x₁+D cosθ₁-x_cr)²+(y₁+D sinθ₁-y_cr)²-S²≤0 (2)

其中，x_cr和y_cr为标准轨迹中描述有杆拖挂式无人系统外形轮廓的特征圆圆心横、纵坐标；D为挂车后轮中心位置距描述有杆拖挂式无人系统外形轮廓的特征圆圆心位置长度，其取值为非负数；S为安全缓冲距离，其取值为正数。

步骤2-2：建立状态不等式约束

系统速度v₁、β₁、β₂和α应该在某一安全范围之内，为此下述约束需要同时满足：

步骤2-3：建立控制不等式约束

通过限制控制变量，使其符合系统的机械限制，控制约束可以表示为：

步骤3：建立有杆拖挂式无人系统的跟踪控制模型

为实现离轴有杆牵引系统的自主运动控制，系统应尽量沿理想轨迹运动。在满足运动约束、避碰约束、控制约束和状态约束的同时，必须使跟踪误差尽可能小，且要保证控制的稳定性。因此，可建立如下所示的有杆拖挂式无人系统跟踪控制模型：

其中，为公式(1)；h≤0为公式(2)、公式(3)、公式(4)组成；(·)_r表示参考轨迹；P、R为目标系数矩阵，P为半正定矩阵或正定矩阵，R为正定矩阵；X₀表示t₀时刻的状态变量。

步骤4：基于RHC的有杆拖挂式无人系统跟踪控制模型在线求解

步骤4-1：初始化时间窗口长度T、窗口滚动频率δT(δT≤T)，滚动计数参数k＝1，初始化第一个时间窗口边界条件其中，(·)^k表示第k个时间窗口内的变量，/>表示当前时间窗口左侧对应时刻的状态变量，/>表示前时间窗口右侧对应时刻的状态变量。

步骤4-2：将第k个时间窗口内(t∈[t₀+(k-1)T,t₀+kT])的跟踪控制问题描述如下形式的最优控制模型。

其中，为第k个时间窗口左侧边界条件。

步骤4-3：利用最优控制方法求解公式(6)，得到第k个时间窗口内的最优控制变量状态变量/>和对应的时间/>并记录输出结果/>其中，/>表示第k个时间窗口内左侧时刻对应的控制变量，/>表示第k个时间窗口内左侧时刻对应的状态变量，/>

步骤4-4：滚动时间窗口，确定时间窗口初始时间及窗口左侧边界条件。若则向前滚动时间窗口，确定该时间窗口左侧边界条件，该时间窗口左侧边界条件由两部分组成。第一部分可结合步骤4-3结果，采用龙格库塔法对下式进行计算得到时刻处的状态变量。

其中，dw_U为随机控制扰动。通过上式，可得到第一部分状态变量为第二部分是干扰或观测误差/>则该时间窗口左侧边界条件可确定为：

同时，令k＝k+1，返回步骤4-2。

若则停止滑动时间窗口并退出。

相对于现有技术，本发明有益效果为：

本发明能以更高的精度、效率和稳定性，在线处理环境中的实时、动态扰动，能够有效解决有杆拖挂式无人系统的倒车或正向行驶过程中的跟踪控制问题，还可应用于带状态-控制约束的跟踪控制问题。所得结论可直接应用于工程实际，具有更好的工程许用性，且具有很强的可操作性和可行性，便于实际应用。

附图说明

图1为本发明的整体思路图。

图2为本发明的有杆拖挂式无人系统的实际轨迹与标准轨迹图。

图3为本发明的有杆拖挂式无人系统的跟踪误差图；图3(a)为挂车后轮中心位置横坐标跟踪误差随时间的变化图，图3(b)为挂车后轮中心位置纵坐标跟踪误差随时间的变化图，图3(c)为挂车方位角跟踪误差随时间的变化图。

图4为本发明的挂车速度、拖车速度以及拖车转向角图；图4(a)为挂车速度随时间的变化图，图4(b)为拖车速度随时间的变化图，图4(c)为拖车转向角随时间的变化图。

图5为本发明的挂车以及牵引杆转向角变化图；图5(a)为挂车转向角随时间的变化图，图5(b)为牵引杆转向角随时间的变化图。

图6为本发明的系统控制变量变化图；图6(a)为系统控制变量u₁随时间的变化图，图6(b)为系统控制变量u₂随时间的变化图。

具体实施方式

以下结合具体实施例对本发明做进一步说明。

一种基于RHC的有杆拖挂式无人系统在线轨迹跟踪控制方法，包括以下步骤：

根据步骤1，对于某一有杆拖挂式无人系统，根据其几何参数，建立有杆拖挂式无人系统的运动学模型如下：

根据步骤2-1，取D＝3、S＝1，从而建立避碰不等式约束如下：

(x₁+3cosθ₁-x_cr)²+(y₁+3sinθ₁-y_cr)²-1≤0

根据步骤2-2，取v_1min＝-2.0m/s、v_1max＝2.0m/s、β_1min＝-0.925、β_1max＝0.925、β_2min＝-0.925、β_2max＝0.925、α_min＝-0.925、α_max＝0.925，建立状态不等式约束为：

根据步骤2-3，取u_1min＝-2.00、u_1max＝2.00、u_2min＝-1.20、u_2max＝1.20，建立控制不等式约束为：

根据步骤3，建立有杆拖挂式无人系统跟踪控制模型。在本实施例中，X₀＝(80,62.5,-90°,0°,0°,0°,0)^T，P＝diag([6.30,6.30,9.20,2.06,0.04,0.88,3.72])×10⁵，R是单位矩阵，t₀＝0s，t_f＝112.15s，则有杆拖挂式无人系统的跟踪控制模型为：

s.t.

根据步骤4-1，初始化时间窗口长度T＝8s，窗口滚动频率δT＝0.1s，滚动计数参数k＝1，初始化第一个时间窗口边界条件

根据步骤4-2，将第k个时间窗口内(t∈[t₀+8(k-1),t₀+8k])的跟踪控制问题描述为如下形式的最优控制模型：

根据步骤4-3，采用最优控制方法中的保辛伪谱算法求解式(6)，得到第k个时间窗口内的最优控制变量状态变量/>和对应的时间并记录输出结果/>

根据步骤4-4，滚动时间窗口，确定时间窗口初始时间及窗口左侧边界条件。若向前滚动时间窗口，确定该时间窗口左侧边界条件，该时间窗口左侧边界条件由两部分组成，第一部分结合步骤4-3结果，采用龙格库塔法对下式进行计算得到时刻处的状态变量：

其中，dw_U为随机控制扰动，本实施例中取为0；第二部分是干扰或观测误差本案例中取/>rand(1，1)表示(0，1)之间均匀分布的随机数，则该时间窗口左侧边界条件可确定为：

同时，令k＝k+1，返回步骤4-2。

若停止滑动时间窗口并退出。

根据步骤4，得到有杆拖挂式无人系统的实际轨迹如图2所示，从而得到有杆牵引飞机系统与虚拟飞机系统的运动轨迹误差如图3所示，挂车速度、拖车速度以及拖车转向角如图4所示，挂车以及牵引杆转向角如图5所示，系统控制变量如图6所示。由结果可知，所有约束均可得到满足，且平均计算耗时约14.6ms，可实现对该有杆拖挂式无人系统的在线轨迹规划与控制。

本发明首先基于有杆拖挂式无人系统的运动学模型，结合其状态约束、控制约束以及避碰约束，建立有杆拖挂式无人系统的跟踪控制模型；然后，结合滚动时域控制思想，将全区间内的跟踪控制问题转化为单个时间窗口内的最优控制问题，并采用最优控制方法进行高效求解。本发明不仅可解决环境中的实时、动态扰动给跟踪控制带来的影响，还能克服大多数算法难以适用于带状态-控制约束的跟踪控制问题，且有效地解决了有杆拖挂式无人系统正向运动和倒车过程中的精确轨迹跟踪与控制难题，具有较高的计算效率和鲁棒性。

以上所述实施例仅表达本发明的实施方式，但并不能因此而理解为对本发明专利的范围的限制，应当指出，对于本领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些均属于本发明的保护范围。

Claims (2)

1.一种基于RHC的有杆拖挂式无人系统在线轨迹跟踪控制方法，其特征在于，包括以下步骤：

步骤1：建立有杆拖挂式无人系统的运动学模型

将挂车和拖车的速度分别定义为v₁和v₃；将挂车、牵引杆和拖车的方位角分别定义为θ₁、θ₂和θ₃，β₁＝θ₂-θ₁、β₂＝θ₃-θ₂，其中，β₁表示牵引杆与挂车纵向方向之间的夹角，β₂表示牵引杆与拖车纵向方向之间的夹角；将拖车前轮转向角定义为α；将挂车后轮中心距挂车与牵引杆铰接点处的长度、牵引杆的长度、牵引杆与拖车的铰接点距拖车后轮中心处的长度、拖车前后轮距分别定义为L₁、L₂、M、L₃；将挂车后轮中心位置的横坐标和纵坐标分别定义为x₁、y₁；则有杆拖挂式无人系统的状态空间表达为X＝(x₁,y₁,θ₁,β₁,β₂,α,v₁)^T，控制向量定义为U＝(u₁,u₂)^T，其中，和/>

为提高基于最优控制的轨迹规划方法的计算效率，建立如下所示的有杆拖挂式无人系统的运动学模型：

步骤2：建立有杆拖挂式无人系统跟踪控制问题中的不等式约束

步骤2-1：建立避碰不等式约束

将实际轨迹与标准轨迹的偏差约束在可行的范围之内，具体为：

(x₁+Dcosθ₁-x_cr)²+(y₁+Dsinθ₁-y_cr)²-S²≤0 (2)

其中，x_cr和y_cr为标准轨迹中描述有杆拖挂式无人系统外形轮廓的特征圆圆心横、纵坐标；D为挂车后轮中心位置距描述有杆拖挂式无人系统外形轮廓的特征圆圆心位置长度，其取值为非负数；S为安全缓冲距离，其取值为正数；

步骤2-2：建立状态不等式约束

下述约束需要同时满足，使v₁、β₁、β₂和α在安全范围之内；

步骤2-3：建立控制不等式约束

通过限制控制变量，以符合系统的机械限制，控制约束表示为：

步骤3：建立有杆拖挂式无人系统的跟踪控制模型

为实现离轴有杆牵引系统的自主运动控制，系统沿理想轨迹运动；在满足运动约束、避碰约束、控制约束和状态约束的同时，使跟踪误差尽可能小，且要保证控制的稳定性；因此，建立如下所示的有杆拖挂式无人系统跟踪控制模型：

其中，为公式(1)；h≤0为公式(2)、公式(3)、公式(4)组成；(·)_r表示参考轨迹；P、R为目标系数矩阵，P为半正定矩阵或正定矩阵，R为正定矩阵；X₀表示t₀时刻的状态变量；

步骤4：基于RHC的有杆拖挂式无人系统跟踪控制模型在线求解

步骤4-1：初始化时间窗口长度T、窗口滚动频率δT，其中δT≤T，滚动计数参数k＝1，初始化第一个时间窗口边界条件其中，(·)^k表示第k个时间窗口内的变量，/>表示当前时间窗口左侧对应时刻的状态变量，/>表示前时间窗口右侧对应时刻的状态变量；

步骤4-2：将第k个时间窗口内(t∈[t₀+(k-1)T,t₀+kT])的跟踪控制问题描述如下形式的最优控制模型：

其中，为第k个时间窗口左侧边界条件；

步骤4-3：利用最优控制方法求解式(6)，得到第k个时间窗口内的最优控制变量状态变量/>和对应的时间/>并记录输出结果/>其中，/>表示第k个时间窗口内左侧时刻对应的控制变量，/>表示第k个时间窗口内左侧时刻对应的状态变量，/>

步骤4-4：滚动时间窗口，确定时间窗口初始时间及窗口左侧边界条件；若则向前滚动时间窗口，确定该时间窗口左侧边界条件，该时间窗口左侧边界条件由两部分组成：第一部分可结合步骤4-3结果，对下式进行计算得到/>时刻处的状态变量；

其中，dw_U为随机控制扰动；通过公式(7)得到第一部分状态变量为第二部分是干扰或观测误差/>

则该时间窗口左侧边界条件确定为：

同时，令k＝k+1，返回步骤4-2；

若则停止滑动时间窗口并退出。

2.根据权利要求1所述的一种基于RHC的有杆拖挂式无人系统在线轨迹跟踪控制方法，其特征在于，所述步骤4-4，采用龙格库塔法对公式(4)进行计算。