狭窄环境下牵引车-挂车系统基于环境和行为的搜索轨迹规划方法

摘要：牵引车-拖车系统(TTSs)由于其巨大的载货能力已被应用于物流和运输领域。轨迹规划在自动驾驶中起着重要的作用，现有的具有优势的规划通常是基于图搜索和采样的。然而，由于TTS独特的欠驱动特性和复杂的运动学约束，这种主流规划器存在实时性差的困难。基于TTS运动特性及其应用环境，提出了一种混合A* -跳点搜索(HA-JPS)规划器，其中JPS提高了规划器的实时性，而混合A*提供了运动约束的保证。为验证HA-JPS的规划效率、轨迹可行性和完整性，构建了若干TTS配置和典型环境。对比仿真实验结果表明，HA-JPS在不损失完整性的情况下，在不同场景下可将实时性提高50%以上。

A.背景和动机

随着物流运输需求的不断增长，智能牵引挂车系统(TTS)的研究日益受到学者们的关注。TTS是由一辆动力牵引车辆(牵引车)和与其相连的多个无动力轮式平台(挂车)组成的连接结构[1]。TTS由于其诸多优点，被广泛应用于农业生产[2]、干线运输[3]、物流和货运[4]等各个领域。这些优点包括能够根据任务需要调整拖车的数量，具有高运输效率的大货物容量[5]，以及与相同尺寸的刚体车辆(RBV)相比，具有更小的转弯半径，可以在狭窄的空间内灵活机动[6]。

然而，由于其欠驱动特性和铰链约束，TTS在运动规划方面提出了独特的挑战。目前流行的TTS运动规划是参考RBV规划，不太适合TTS规划。具体而言，TTS规划方法面临的挑战包括基于图搜索方法的维数诅咒，基于采样的方法对TTS模型约束考虑不足，以及在基于最优化的方法中需要平衡计算速度和求解质量。此外，铰链的存在会导致牵引车和拖车之间的速度不一致，从而导致折叠状态甚至侧翻现象。 

总的来说，RBV中使用的运动规划方法对TTS的可移植性较低，因为每种规划方法都有其独特的优缺点。考虑到TTS的高状态维特性，TTS规划任务的优先级倾向于快速性，满足了基于搜索的规划器的核心思想。本文提出了一种专门针对TTS的轨迹规划方法，旨在提高TTS规划的实时性。混合A算法与跳点搜索(jump point search, JPS)算法相结合[7]，实现TTS规划任务。通过有目的地修剪不必要的节点，优先探索与当前TTS状态取向角相同的方向，在开放和狭窄的环境中采用可变采样间隔，构建A -跳点搜索(HA-JPS)混合规划器，进一步增强搜索的方向性，从而提高搜索速度。传统混合A*与HA-JPS的主要区别如图1所示。

图1. Hybrid A*(左)和HA-JPS(右)的节点扩展模式比较:在开放或狭窄的环境中，修剪一些不需要的节点，可变采样间隔(将在第三节详细介绍)。

B.相关工作

RBV中主要的运动规划算法可以通过适当的修改在TTS中引用。主流的TTS规划技术根据其实现可分为四类:基于图搜索的、基于采样的、插值曲线的和基于优化的。此外，基于学习的方法作为一种新兴的技术，由于计算能力领域的进步而吸引了更多的关注。 

基于图搜索的方法是一种确定性的方法，它将构型空间描绘成一个有节点和边的图，具有快速找到最优解的能力。典型的基于搜索的算法有Dijkstra[8]、A算法[9]和State lattice[10]。基于采样的方法通过对构型空间或状态空间进行随机采样，寻找其内部连通性，解决了确定性方法在高维空间中不能满足时间约束的问题[11]。其中，应用于TTS运动规划的具有代表性的基于采样的方法是概率路线图方法(PRM)[12]、快速拓展随机树 (RRT)及其变体(如闭环RRT[13]、Theta-RRT[14])。插值曲线方法通过拟合一组预定义的路径点来产生连续的光滑路径。 

最被广泛认可的曲线插值方法是多项式曲线[15]。基于优化的方法将TTS规划任务作为最优控制问题(OCP)，并对成本函数进行数值优化。在寻找最优数值解时，OCP可以表示为一个非线性规划(NLP)问题。通常使用内点法(IPM)[16]、随机分形搜索(SFS)[17]、顺序二次规划(SQP)[18]来求解优化问题。尽管基于优化的方法保证了规划轨迹的运动可行性和最优性，但其计算复杂度严格限制了在线规划的使用。为了缩小最优性与实时性之间的差距，最近的文献集中在多方法集成上，以提高实用性，将基于搜索或抽样的方法与基于优化的方法相结合[19]-[21]，前者用于提供初始猜测，后者随着计算速度的逐步提高，将其收敛到局部最优。然而，现有的优化求解方法严格要求高质量的初始猜测，在一定程度上继承了这两种方法的缺点。 

基于学习的方法揭示和建模数据中隐藏的和复杂的规律。[22]提出了一种终身学习框架，该框架将基于生成对抗网络(GAN)的轻量级模型与RRT相结合，以处理非线性动力学。提出了基于半监督学习的半卡车路径规划任务端到端学习[23]，生成扫描面积最小的路径，避免与静态障碍物发生碰撞。然而，基于学习的方法通常依赖于现有的数据集，这些数据集必须涵盖TTS在特殊或困难情况下需要学习的所有内容。此外，主流的学习仿真软件(如Carla)不包含TTS模型，限制了基于学习的规划方法的发展。

在工厂或货运物流环境中，TTS装载大量有效载荷且低速机动，因此车轮打滑效应可以忽略不计。TTS模型如图2所示，与经典的自行车模型类似，可以建立牵引车的运动学模型：

图2. TTS模型。 

对于第个车身()，其轮轴的中点可计算为:

请注意，表示挂载偏移量，表示牵引车和拖车的轴距。根据TTS的挂载方式，可将其分为单轴模式()和离轴模式()两类，如果TTS为离轴模式，则可直接应用式(2);如果TTS为轴上或轴上/离轴集成模式，也可以通过将轴上模式指定为0来应用式(2)，从而省去式(2)中包含的求和项。 

第个物体的方向角和速度由[24]规定:

当状态变量, ， 确定时，任意数量的拖车的状态可以用式(2)-式(4)来描述。

A.机械约束

牵引车-挂车系统具有内在的力学性能，反映在任意时刻的状态变量和控制变量的可行域:

其中，、、分别表示牵引车的速度、加速度、转向角和转向速率的上限。另外，牵引车与第一挂车及相邻挂车的方位角差限制在90度以内，避免出现折叠现象：

B.状态限制

确定TTS的初始状态和终端状态是运动规划的重要组成部分。通常，可以显式指定TTS第 个车体的初始状态:

当TTS到达指定目的地时，其速度、加速度、转向速率均应为0，保证TTS稳定停车:

由于要求每辆拖车的朝向角严格符合期望朝向角是极其困难的，因此当拖车的实际朝向角与的差值小于某一阈值时，我们确定规划任务成功。但需要注意的是，牵引机的方向角是严格要求的，以满足期望值。此外，在、和已经返回0的条件下，转向角不限制为0。

C. 碰撞约束

在不失一般性的前提下，我们首先研究了TTS车身与第个障碍物之间的无碰撞条件。为简单起见，将TTS的第个物体视为矩形()，将第个障碍物视为多边形避碰条件可表示为:

其中,,式(9)明确了确定TTS无碰撞条件的标准:

• TTS体的每个顶点都在障碍物之外。 

• 障碍物的每个顶点都在TTS体之外。 

这个结论可以推广到任意的TTS体和任意的障碍物。注意，当扩展到多体时，应避免系统的内部碰撞。因此，TTS的避碰约束可进一步表示为:

其中和分别表示第个物体和第个障碍物的顶点。和分别表示第个物体和第个障碍物被占据的面积。TTS相关参数的符号和定义见表1。

表1. TTS的参数定义。

仿真在MATLAB 2022b中运行，并在具有Inter i7-12700H CPU和运行频率为2.30GHz的16GB RAM的计算机上执行。参数定义及取值如表1所示。障碍物在二维地图中可以抽象为如图3(a)所示的凸多边形和非凸多边形。某些类型的非凸障碍物对TTS规划任务提出了额外的挑战，因为它们的凹区域可能导致更多的搜索消耗，如图3(b)所示的死胡同。因此，我们对几种典型情况进行了测试，以验证HA-JPS的规划效率、轨迹可行性和完整性。

图3. 障碍物的分类和测试图的组成。

如图3(a)所示。某些类型的非凸障碍物对TTS规划任务提出了额外的挑战，因为它们的凹区域可能导致更多的搜索消耗，如图3(b)所示的死胡同。因此，我们对几种典型情况进行了测试，以验证HA-JPS的规划效率、轨迹可行性和完整性。

A.规划的效率表现

包含一辆拖车()的TTS在狭窄的走廊中进行了两次90度转弯和一次u型转弯的测试，如图4所示。当经过一个相对开放的环境(图4(a)和图4(b))时，两种算法搜索的路径相同，而我们的方法耗时更短(1.659405秒比2.200872秒)。当环境变得更窄时(图4(c)和图4(d))，我们的方法在更短的时间消耗方面的优势进一步被证明(1.839772秒比4.313189秒)。 

此外，在如图2所示的环境中，对包含不同数量拖车()的TTS进行了测试。在图4(c)和图4(d)中，MHA和HA-JPS的时间消耗比较见表2。在三种情况下，时间消耗分别减少了57%、65%和66%。

表2. 对比仿真结果。

B. 轨迹的可行性表现

主要从防止不必要的倒车和非凸避障两个方面考察了HA-JPS的轨迹可行性。倒车确实增加了找到最优路线的概率，但它可能不受用户的喜爱，甚至在携带大载荷时也不可行。因此，为TTS规划一个可行的轨迹，避免不必要的反转是合理的。另一个值得考虑的问题是环境中的非凸障碍物，这可能会将搜索误导到死胡同，并加剧时间消耗。本小节将通过对比实验演示HA-JPS轨迹的可行性，并展示以上两方面的改进。

图4. MHA(左)和HAJPS(右)在相对开放(上)和相对狭窄(下)环境下的性能比较。

在倒车预防谱上，如图5所示，TTS ()在应用MHA时进行了3次倒车(图5(a))，这对于RBV规划是可以接受的，但对于TTS来说是极不可能的。相比之下，HA-JPS能够以更少的时间(23.623835秒比71.112108秒，图5(b))规划无倒车轨迹。在非凸避障谱上，如图6所示，MHA和HA-JPS都成功规划了一条轨迹()， HA-JPS通过扩展较少的节点减少了进入死角的搜索程度，从而减少了时间消耗(6.644171秒比15.090989秒)。

图5. MHA(左)与HA-JPS(右)预防倒车动作的比较

图6. MHA(左)与HA-JPS(右)非凸避障比较

C. 完备性表现

有人可能会担心，我们是否通过进一步牺牲算法的完整性来加快求解速度。然而，在模拟过程中发现了一个有趣的情况：当拖车长度从5 m扩大到5.4 m ()时，MHA倾向于反向搜索，无法规划轨迹，而我们的方法成功推导出可行的轨迹，如图7所示。

图7. MHA(左)和HA-JPS(右)算法完备性比较。 

从理论上讲，我们的修改并没有涉及到算法完备性的提高。一种可能的解释是，MHA更注重寻找一个“最优”的解决方案，而不是一个“可行”的解决方案，导致在特定情况下更多的反向搜索，最终失败。假设混合A*是一个概率完备规划器，这种情况表明HA-JPS并没有进一步降低其完备性。