一种用于自动驾驶卡车‐拖车泊车的新型级联路径规划算法

摘要

对于卡车驾驶员而言，最艰巨的任务之一是在不同泊车场景中对卡车‐拖车系统进行机动操作。本文提出了一种用于卡车‐拖车泊车的新型路径规划方法，其中提出了一种真实且确定性的泊车行为模型——迭代解析法（IAM），并将其与闭环快速探索随机树（CL‐RRT）方法结合，构成级联路径规划。通过将CL‐RRT与模拟实际泊车操作的迭代解析法（IAM）相结合的级联路径规划方法，能够生成具备运动学可行性、确定性并实现障碍物规避的泊车操作。为进行评估，通过开发的案例生成工具生成并选取了多种泊车场景。所提出的路径规划方法的性能通过MATLAB仿真进行了验证。结果表明，该方法在生成可行泊车操作方面取得了显著成功，成功率较高。

索引术语

高级驾驶辅助系统，自动泊车，级联路径规划，迭代解析解，闭环RRT。

I. 引言

ADVANCED 驾驶员辅助系统（ADAS）技术已日益普遍应用于乘用车和重型车辆行业，以满足更高的安全需求。目前市场上已有一定数量的功能，包括一级系统如自适应巡航控制（ACC）、车道保持辅助（LKA）和自动紧急制动（AEB），以及二级系统如自动泊车辅助、变道控制和高速公路辅助。然而，泊车辅助系统通常面向乘用车，而非卡车‐拖车系统。卡车驾驶员经常需要高精度地倒车牵引拖车，以便在码头站和建筑工地进行装卸。但由于任务复杂，驾驶员必须具备丰富经验，才能将拖车准确倒车至狭窄的停车位而不发生碰撞。近年来，随着对卡车‐拖车系统泊车辅助兴趣的增加，已有多项关于拖车倒车的研究被提出。

卡车‐拖车系统具有非完整约束，即包含速度和状态向量的不可积微分表达式。一种已知的非完整路径规划方法是首先为全向车辆生成一条路径，然后修改路径以满足非完整约束[1]。然而，由于提前将非完整运动约束注入运动规划，并基于这些约束设计运动基元能够提高算法的速度和鲁棒性，因此这种方法正变得越来越流行。Divelbiss等人[2]提出了一种非完整迭代运动规划方法，其中无障路径规划和障碍物规避分别被建模为使用等式约束和不等式约束的非线性最小二乘问题并加以求解。为了同时满足等式和不等式约束，该算法在每次迭代中对整个路径进行调整。Zöbel等人[3]提出了一种基于规则的轨迹分割方法用于安全的牵引车‐挂车路径规划。Gomez‐Bravo等人[4]开发了一种基于改变拖车方向的卡车‐拖车平行泊车规划算法，其中引入一组可解析推导的操作来改变泊车第一步中的拖车方向。一旦通过引入车辆模型仿真和适当的碰撞检测算法设计出闭环系统，各种基于图的[5],、基于插值曲线的或基于采样的[6]单体车辆系统的路径规划算法便可扩展应用于卡车‐拖车系统。Stahn等人[7]提出了一种基于网格的多维路径规划器，最多支持五个自由度，并采用A*搜索算法实现。Rimmer等人[8]基于曲率特性的经验关系构建了一个预计算可行路径段库，并提出一种基于Dijkstra网格搜索的路径规划算法，通过预计算路径段生成无碰撞路径。Elhassan等人[9]提出将RRT算法与Dubins路径计算相结合，以获得运动学上可行的泊车操作，但仅考虑单向操纵。Evest‐Edt等人提出了用于倒车双挂车系统的运动规划算法，采用闭环RRT[10]和栅格规划器[11],，二者均利用基于采样的搜索算法（分别为结合闭环稳定的牵引车‐挂车模型的RRT和状态格点中的A*搜索），提供包含前进和后退操作的运动学可行双向路径。为进一步提升搜索速度，引入了用于快速成本估算的预计算查找表[10],、运动基元生成以及用于状态格点构建的运动基元缩减技术[11]。此外，Palmieri等人[12]开发了Theta*‐RRT算法，并在卡车‐拖车系统上进行了测试，首先从车辆到目标位置推导出一条任意角度路径，然后进行树扩展。最后，李等人[13]提出了一种用于狭窄环境中卡车‐拖车路径规划的逐步约束最优控制算法（PCOC），该算法从由混合A*结合Reed‐Shepp曲线生成的粗略路径开始，并根据边界条件、避障和运动学约束逐步优化路径。

由于实现简单且能保持一定的完备性保证，基于概率采样的方法在运动规划算法中正变得越来越受欢迎，但它们无法保证最优性。此外，由于其非确定性特性，对于相同或非常接近的初始构型可能会产生发散路径解，这通常不会被驾驶员所接受。

本文提出了一种新颖的路径规划方法，其中提出了一种真实且确定性的泊车规划器——迭代解析法（IAM），并将其与闭环快速探索随机树（CL‐RRT）方法相结合，实现级联路径规划。这两种算法在解路径中均提供前进和后退方向的操作能力，这是在狭窄环境（如码头站点和建筑工地）中拖车泊车用例中最常见的特征。为了实现更快的收敛速度，CL‐RRT方法通过引入扩展概率随机采样生成方法得到增强，使得树扩展进一步偏向于某些兴趣点（如中间点和目标点）附近，从而更可能在IAM所提出的操作集对应的操作空间周围进行扩展。级联路径规划方法为用例选择合适的算法，能够生成运动学上可行且具备障碍物规避能力的泊车操作。

II. 卡车‐拖车系统的运动学模型

对于运动模型，引入了一种通用的单挂车汽车‐挂车系统，如图1所示。该系统的广义坐标表示为P=[β, δ, x, y],，其中β和 δ分别为卡车航向角和拖车航向角，x 和y为挂车后轴位置。或者，P=[β, δ, x, y] 可被使用，其中x 和y表示卡车后轴的位置。此外，在机器人操作系统（ROS）、ADAS和计算机视觉中常用的“位姿”一词，指的是相应车体的位置和姿态的组合配置。

在无滑移条件下，推导出了一般n‐挂车系统的运动学模型[15]，该条件在泊车场景中的低速操纵中是一个可持续的假设。特殊单拖车情况下的系统方程以如下矩阵形式表示；
$$
\begin{bmatrix}
\dot{\beta} \
\dot{x} \
\dot{y} \
\dot{\delta}
\end{bmatrix}
=
\begin{bmatrix}
0 & 1 \
\frac{\sin(\beta - \delta)}{L_1} & \frac{H \cos(\beta - \delta)}{L_1} \
\cos(\beta - \delta) & -H\sin(\beta - \delta) \
\cos\delta & \cos(\beta - \delta) \
-H\sin(\beta - \delta) & \sin \delta
\end{bmatrix}
[ v \quad vK]
$$
and
$$
K(t)= \frac{\tan \alpha}{L_2}
$$
(1)

其中，L1和L2分别表示拖车和卡车前后轴之间的距离。H为非同轴挂钩长度， α为转向角，K为卡车转向的瞬时曲率，v为卡车的纵向速度。该用于P的运动学模型在CL‐RRT算法的闭环部分中用于正向和反向仿真。P的模型另一种形式在(2)中提出，并用于IAM的部分解。
$$
\begin{bmatrix}
\dot{\beta} \
\dot{x} \
\dot{y} \
\dot{\delta}
\end{bmatrix}
=
\begin{bmatrix}
0 & 1 \
\frac{\sin(\beta - \delta)}{L_1} & \frac{H \cos(\beta - \delta)}{L_1} \
\cos(\beta) & 0 \
\sin(\beta) & 0
\end{bmatrix}
[ v \quad vK]
$$
(2)

III. 路径规划方法

A. 级联路径规划算法

本文提出了将IAM和CL‐RRT作为级联路径规划算法的组成部分，这两种方法彼此具有不同的优势。IAM在较简单的工作空间中执行速度更快，能够提供确定性的、更符合实际的路径，并且在起点处能够以更低的初始状态容差计算出解。另一方面，CL‐RRT在障碍物密集环境中具有更高的求解率，对卡车‐拖车系统的初始状态和构型以及停车环境的依赖性较低，在复杂环境中仍具备足够快的求解速度，并且由于在树扩展过程中可以从多个成功解中灵活选择成本最低的替代方案，因此能够生成成本更低的路径。IAM中的每个操作都反映了卡车‐拖车倒车泊车的不同特性，这些特性是通过分析驾驶员在码头站点的实际停车操作得出的。由于IAM模拟的是实际停车方法，其性能取决于初始状态和构型与所提出的实际停车操作集的匹配可能性；而CL‐RRT则确保了更为通用的路径规划方案。IAM的一个主要优势在于其确定性特征，因为驾驶员通常期望可预测的操作行为。

期望在相同的初始条件下得到相同的运动规划建议，以及在略微不同的初始条件下得到非常接近的运动规划建议。在这方面，IAM提供了一种确定性解，即对于相同的输入集，输出始终相同；而CL‐RRT由于其概率特性，可能对相同的输入集产生不同的解。提出级联路径规划框架的动机是结合这两种算法的优势，并引入算法选择器来确定哪种算法更适合特定的停车场景。

如图2所示的级联路径规划算法首先检查实际场景中卡车‐拖车的初始和期望的最终广义坐标Pi和Pf ，以及障碍物的广义坐标[Pobs]；然后推导出初始构型的可能性参数 μL，以及所提出的停车操作集M（如图3所示）的约束条件，同时推导感兴趣工作空间内障碍物密度ρobs。在推导 μL之前，首先基于卡车驾驶员倒车停车的实际停车操作，在局部化的初始卡车‐拖车广义坐标P1 s到Pn s的数据集上，通过推导均值和协方差矩阵 μˆn和 ˆn的最大似然估计量，对多元高斯分布进行离线最大似然估计。为了得到卡车‐拖车相对于期望位姿的初始相对构型的估计，将卡车‐拖车的广义坐标相对于停车位中与最终构型对齐的坐标系进行局部化处理。

停车构型
$$
L(P_s | \mu,\Sigma) =(2\pi)^{-2} |\Sigma|^{-n/2} \exp\left(-\frac{1}{n}\sum_{k=1}^n(P_{sk} -\mu)^T\Sigma^{-1}(P_{sk} -\mu)\right)
$$
$$
\hat{\mu} n = \frac{1}{n}\sum {k=1}^n P_{sk}; \quad \hat{\Sigma} n= \frac{1}{n}\sum {k=1}^n(P_{sk} - \hat{\mu} n)(P {sk} - \hat{\mu}_n)^T
$$
(3)

随后，使用Pi和Pf生成一个新的相对构型候选Ps∗，并按如下方式计算P∗ s的可能性：
$$
L(P^ _s | \hat{\mu}_n, \hat{\Sigma}_n)=(2\pi)^{-2} |\hat{\Sigma}_n|^{-1/2} \exp\left(-\frac{1}{2}(P^ _s - \hat{\mu}_n)^T \hat{\Sigma}^{-1}_n(P^*_s - \hat{\mu}_n)\right)
$$
(4)

此外，为了确定工作空间边界可行性的概率pf，需要在实际场景的工作空间边界框区间Wi{xmin,ymin}和Wf{xmax, ymax}之间对感兴趣工作空间M 的概率密度函数进行积分。因此，基于实际停车操作，对工作空间边界框BBw{x1−4, y1−4}进行类似的离线最大似然估计，得到多元高斯分布L(BB ∗ w | \hat{\mu}_m, \hat{\Sigma}_m)。最后， μL Lp: 被设计为P∗ r 的可能性与工作空间边界可行性概率pf 的加权和：
$$
p_f= p(W_i< BB^ _w< W_f | \hat{\mu}_m, \hat{\Sigma}_m)
$$
$$
\mu_L= \tau_sL(P^ | \hat{\mu}_n, \hat{\Sigma}_n)+ \tau_f p_f
$$
(5)

感兴趣工作空间内的障碍物密度ρobs:通过计算工作空间边界框内障碍物平均值的最大似然估计μˆm所表示的感兴趣工作空间中障碍物总占用率的加权值得出。障碍物的占用率通过将 μˆm中障碍物占据总面积除以 μˆm的工作空间总面积得到，并使用增益kobs Lp:进行加权。
$$
\rho_{obs}= k_{obs}\frac{\sum_{k=1}^{#obs} A_k^{occ}}{A_{\hat{\mu}_m}}
$$
(6)

通过检查参数 μL Lp:和 ρobs是否超过特定阈值，级联算法决定在迭代解析法和CL‐RRT之间为泊车场景选择可行的路径规划算法。算法选择器中的阈值以及参数中的权重增益是通过覆盖整个工作空间的各种测试用例仿真估算得出的。随后，D节将说明在IAM和CL‐RRT算法中使用的碰撞检测方法，以及作为生成路径后处理的路径平滑方法。

B. 迭代解析法 (IAM)

IAM通过一种通用泊车行为引入，该行为包含如图3所示的四个主要操作。每个操作反映了卡车‐拖车倒车泊车的不同特性。

这些是通过考察卡车驾驶员在停靠站的实际停车操作来确定的。前两个前进方向的操作有助于车辆定位到理想位置，为拖车在后退过程中驶向停车位提供足够的可用空间。第三个后退方向的操作使车辆接近停车位，最后一个操作则在避免与相邻停车位发生碰撞的同时成功进入停车位。在此方法中，采用纯追踪转向方法来规划操作。即，每个操作都被规划为一个纯追踪操作，其中定义了一个需要跟踪的恒定曲率路径，然后通过纯追踪控制器在前进或后退仿真中跟踪该路径，以获得卡车和拖车的仿真轨迹，并用于碰撞检查。因此，操作M1−4由恒定曲率和操作持续时间[K1−4, t1−4] t1−4:定义，车辆以恒定速度V行驶，从而可以根据系统的初始和终止条件求解广义坐标P的运动学模型微分方程。所提出的方法通过优化方法在最终状态求解误差方程组，以将误差最小化至某一允许阈值以下，因此在最终状态时位姿误差不为零。为此，问题从停车位反向求解至车辆初始位置，以确保在最终构型时实现零构型误差，算法从第四个操作（M4）开始，依次逆序完成至第一个操作（M1 M1−4:）。如图5所示的算法流程图中，算法首先通过迭代求解纯追踪脱离操作（M4），利用曲率递减的K4从K4,max（即M4允许的最大曲率）开始进行前向纯追踪仿真并进行碰撞检查，从而确定从停车位出发的无碰撞脱离路径。利用基本三角学，基于图4所示卡车的无碰撞脱离情况，可解析计算K4,max ，如公式(7)所示，其中w truck为卡车宽度，wpark和hpark分别为停车位的宽度和高度。由于公式(7)中的解析解仅能保证卡车与相邻停车位之间的避障，还需对仿真操作M4,进行碰撞检查，即在包含障碍物的工作空间占用区域内检查卡车和拖车的仿真路径。
$$
K_{4,max}=\left|\frac{w_{park} - w_{truck}}{0.25(w^2_{park}- w^2_{truck})+ h^2_{park}}\right|
$$
(7)

仿真实验持续特定时长tsim，近似如(8)所示，以确保卡车‐拖车系统完全驶出停车位。一旦获得无碰撞的M4，则将该操作缩短至拖车保险杠中心点侧面越过停车位上边界线时的首次脱离点。相应地，t4根据缩短后的操作M4的持续时间确定。
$$
t_{sim}= \frac{1}{KV} \sin^{-1} (K(L_1+ L_2 - H))
$$
(8)

随后，规划并模拟下一个操作(M3)，其曲率高于K4 ，以为后续操作（M1 和M2）提供横向空间，并以更狭窄空间内的解路径为目标。曲率初始选择为最大曲率K3,max，该曲率通过最大允许转向角αmax得出；操作时间t3按照(9)求解，以实现卡车最终位姿垂直于停车位（ β2= 0位于图 3所示的局部坐标系中）。
$$
K_{3,max}= \frac{\tan(\alpha_{max})}{L_2}, \quad t_3= \frac{\beta_2 - \beta_3}{K_3 V}
$$
(9)

仿真生成的卡车‐拖车路径会检查其与工作空间占用区域是否存在碰撞；如果发生碰撞，则在每一步迭代中逐步降低曲率K3 。一旦获得无碰撞的M3，便对M1 和M2 的解析解进行最小化问题求解。如果解存在，则检查该解是否满足卡车‐拖车系统的允许的位置和角度边界。如果解失败，算法将回退并以更低的曲率重新规划M3 ，以便为M1 和M2 留出更多空间，其中M3 的最大迭代次数根据工作空间的允许边界确定。当解位于允许边界内时，算法完成路径规划，并将最终得到的四个操作作为成功泊车路径，如图3所示。

对于操作M1和M2,，给定关于M1和M2的初始和终止条件，其操作参数和广义坐标P的解析解如图6所示。在初始位姿M1且给定初始条件P2的情况下， β、x、 y和 δ的解由以下公式推导得出，其中初始位姿与最终位姿之间的总航向差对于M1和M2分别定义为 θ1= K1t1V 和 θ2= K2t2V。
$$
\beta_i= \beta_2+(\theta_1+ \theta_2)
$$
(10)
$$
x_i= X_2+ \frac{1}{K_2}[\sin( \beta_2+ \theta_2) - \sin \beta_2] + \frac{1}{K_1}[ \sin( \beta_2+(\theta_1+ \theta_2)) - \sin( \beta_2+ \theta_2)]
$$
(11)
$$
y_i= Y_2+ \frac{1}{K_2}[(\cos( \beta_2+ \theta_2)-\cos \beta_2)] - \frac{1}{K_1}[\cos( \beta_2+(\theta_1+ \theta_2)) - \cos( \beta_2+ \theta_2)]
$$
(12)
$$
\delta_i= \beta_2+(\theta_1+ \theta_2) -2 \tan^{-1} (DM)
$$
(13)

其中,
$$
DM= \frac{1}{K_1(L_1 - H)} (1+ Da)
$$
$$
Da= \tan\left( \frac{D_b t_1 V}{2L_1} - \tan^{-1}\left( \frac{D_c}{K_2 D_b}\right)\right)
$$
$$
D_b= \sqrt{-1+ K_2^2 (L_1^2 - H^2)}
$$
$$
D_c= K_2 - K_1 - K_1 D_d \tan\left(\frac{D_d t_2 V}{2L_1} -\tan^{-1}\left(\frac{D_e}{D_d}\right)\right)
$$
$$
D_d= \sqrt{-1+ K_1^2(L_1^2 - H^2)}
$$
$$
D_e= 1 - K_2(L_1+ H \tan( \frac{\beta_2 - \delta_2}{2}))
$$

以矩阵形式定义的误差方程组e=[ βe, δe, xe, ye]=[ βi− β0, δi − δ0, xi−x0,yi−y0]= 0 需要求解 K1, t1, K2 和t2 。由于方程的复杂性和非线性，无法获得解析解，因此将其转化为优化问题，通过最小化代价函数C=[ βe^2+ δe^2+ xe^2+ ye^2] 来求解K1, t1, K2 和t2 ，并采用Nelder‐Mead单纯形算法[16]进行求解，直到坐标误差在允许公差范围内。为进一步提升性能，将 βi 和 yi 方程求解出的K2 和t2 代入xi 和 δi 方程中，由此推导出代价函数C=[δe^2+xe^2]，并用于最小化问题。该代价函数可加速收敛至K1 和t1的期望解。

C. 闭环快速探索随机树（CL-RRT）

由于RRT框架在状态空间中进行随机采样，而卡车‐拖车模型涉及非完整约束，因此除非将系统通过稳定系统模型的控制器转换为闭环，否则无法保证扩展树中分支的跟踪可行性。Kuwata等[14]提出了闭环RRT（ CL‐RRT），其中通过执行非线性闭环仿真来计算动态可行轨迹，并在控制器输入空间中进行采样。由于卡车‐拖车系统的前向路径跟踪是一个类似于运动学汽车模型的稳定问题，因此可以使用纯追踪控制器进行前向闭环仿真。然而，由于后退运动中的路径跟踪对卡车‐拖车系统而言是一个不稳定问题，因此需要一个稳定控制器作为内环与后向纯追踪控制器配合使用[10],[17],[13]。

本文提出了一种用于卡车‐拖车系统在反向情况下的 CL‐RRT算法，该算法从停车位开始进行树扩展，并搜索到达车辆初始状态的解。由于在小误差容限下通过随机采样和前向仿真进入狭窄停车区域难以实现，这类似于 RRTs所面临的已知陷阱问题。从停车位逆向开始路径规划可确保停车位处的误差将为零，并在目标位姿与车辆初始位姿之间检查允许的误差容限，该误差容限可在机动过程开始时由控制器轻松处理，或通过从车辆位姿到最近可行路径位姿的Dubins路径或闭环转向[18]附加连接曲线来处理。随机样本生成采用概率多层方法设计，考虑了车辆的初始状态、选定停车位的位置以及工作空间的边界。避障通过与IAM方法中使用的相同碰撞检测算法实现。为进一步优化路径，采用了Elhassan等人[9]实现中的路径优化功能，以消除不必要的分支并减少解路径中的总曲率变化，从而获得更平滑的解。所实现的CL‐RRT算法的流程图如图7所示。CL‐RRT算法在每一步通过在控制器输入空间中随机生成的样本Srnd=[Sx, Sy] 进行树扩展，利用闭环模型仿真定义可行路径，其中到达样本点的尝试以预定义概率确定在前向或后向方向进行，以实现包含后退和前进操作的路径规划。

代价启发式被定义用于确定在每次迭代中到达采样节点的最佳节点，其中树节点根据代价启发式进行排序，并选择代价最低的节点Nmin进行到达尝试。Evestedt等人[10]定义了用于确定最可行的到达树节点Nmin的代价启发式，以指向随机生成的点Srnd，其代价函数为路径长度与路径过程中卡车‐拖车朝向的角度变化之和。由于角度变化包含在代价函数中，因此每次节点尝试都需要执行仿真；为此，他们引入了一张离线生成的代价启发式地图，通过插值覆盖所有可能的仿真输出。本文中定义的代价启发式仅考虑分支候选的纯追踪弧线长度Lb ，因此在选择最可行的到达树节点之前无需执行任何仿真即可轻松计算代价。

从成本最低的树节点到采样节点的到达尝试被设计为纯追踪弧形路径，并且首先仅检查该弧形路径是否发生碰撞，而忽略卡车‐拖车系统的轮廓。如果弧形路径存在碰撞，则选择下一个成本最低的节点作为Nmin进行到达尝试。如果弧形路径无碰撞，则向前或向后仿真过程中，仿真时间在低于预期仿真时间的允许区间内随机选择，以实现更短的分支，从而提高树扩展的灵活性。从仿真中获得的卡车‐拖车路径会根据卡车‐拖车系统的轮廓再次检查碰撞情况。若无碰撞，则将仿真的最终状态作为新节点添加到树中，同时将卡车和拖车的路径作为树的新分支进行存储。在成功连接到新节点后，以概率性选择后退或前进操纵方向进行到达目标尝试。在此步骤中，执行一次新的仿真，并根据卡车‐拖车轮廓再次检查卡车‐拖车路径的碰撞情况。若无碰撞且成功到达目标，则进行目标评估，检查卡车‐拖车的最终状态是否满足预定义的目标要求。若满足目标要求，则将该目标连接存储为一个解；否则，将目标连接轨迹的较短前段作为新节点和新分支再次加入树中，以增强树的灵活性。当达到足够的解数量 Solmax或最大允许路径规划时间Tmax时，算法停止。根据解成本函数的定义，选择最小成本解作为路径规划的最佳解。该函数定义为总轨迹长度、相对于最终构型的角度误差以及操作方向变化次数nc的函数，具体如下：
$$
C= \sum_{i=2}^{N} \sqrt{(x_i -x_{i-1})^2+(y_i - y_{i-1})^2} + k_{ang}( |\beta_N - \beta_f| + |\delta_N - \delta_f|) + k_c n_c
$$
(14)

表I 随机采样策略

其中，N 是轨迹中的节点总数，kang 和kc 分别是角度误差和操作方向变化的权重系数。对操作方向变化施加惩罚，可促使规划器选择在轨迹过程中方向变化次数较少的解。算法实现的一个示例如图8所示，其中蓝线和绿色线条表示树中的前进和后退分支，黄色和红色曲线分别表示前进和后退泊车操作。关于随机样本生成以及仿真和算法参数等更多实现细节将在下一节及路径规划仿真中进行说明。

1) 随机样本生成：

采用均匀采样来探索有界搜索区域，提出一种对操作方向进行概率选择的偏差方法，Pb=[xb、yb]及标准差[σx、 σ y] 。为了快速收敛到可行解，采用表 I所示的偏差选择策略，其中树扩展部分指向工作空间中点Pmid、目标点Pf以及一个中间点Pm所在的区域，该中间点位于从目标点Pf到起点连线方向上距离为LmPm Pf Ps的位置。使用Pmid作为偏置可在有界搜索区域内实现均匀扩展；另一方面，添加Pm可增强树扩展，为泊车操作提供足够的空间，从而提高树扩展模拟先前提出的操作集中的操作M3的可能性。进一步添加Pf作为偏置有助于将树扩展导向目标区域。表I中选取的值是通过对图8所示的测试场案例进行仿真获得的。

D. 避障与路径平滑

1) 碰撞检测方法：

通过两级碰撞检测方法实现避障。第一级是对可操作空间进行环境碰撞检测，第二级是与障碍物的碰撞检查，如图10所示。在进行环境碰撞检测时，采用了一种基于圆‐矩形分解的方法[19]，其中车辆的形状首先被分解为多个圆，然后将每个圆进一步分解为矩形。由于圆形具有旋转不变性，因此只需根据更新后的车辆位姿对圆心点进行变换，并基于更新后的圆心位置确定所有分解出的矩形，这些矩形的方向固定并与环境的自由空间地图对齐。为了更快速地检查矩形区域的占用情况，每当占用地图更新时，都会在每一帧时间中计算占用地图的积分图像，如下所示。
$$
I_o(X, Y)= \sum_{x\leq X,y\leq Y} o(x, y)
$$
(15)

有了积分图像Io，可以通过以下算术方程轻松检查占用图中矩形区域的占用情况：
$$
\sum_{x,y\in A} o(x, y) = I_o(x_0, y_0)+ I_o(x_1, y_1) - I_o(x_0, y_1) - I_o(x_1, y_0)
$$
(16)
其中，若矩形满足∑x,y∈A o(x, y) = 0，则该矩形为无碰撞状态。因此，在轨迹上车辆的每一位置位姿处，均需对分解后的每个矩形执行此检查。在与障碍物进行碰撞检查时，优先采用级联算法，结合轴对齐包围盒（AABB）相交和线段相交方法。由于轴对齐包围盒（AABB）相交计算速度非常快[20], ，因此首先将其作为必要条件，在卡车‐拖车与障碍物之间进行检查。若通过AABB算法未发现相交，则碰撞检查完成，并判定为无碰撞状态；否则，进一步检查卡车‐拖车的定向矩形包围盒的每条边与障碍物之间的线段相交情况，一旦发现相交，算法即停止[21]。

该路径规划框架通过两级碰撞检测方法实现在线碰撞预测，并根据用例触发平稳或紧急制动，从而在存在移动障碍物的情况下确保适应性，而非依赖在线重新规划。具体采用的方法取决于系统环境感知是否能够提供障碍物轨迹预测。当可获取动态障碍物的预测轨迹时，可在同一时间帧内，针对卡车和拖车在各自轨迹上每一时刻的位姿，检查其边界框与障碍物在其轨迹上相应位姿的边界框是否存在可能的碰撞。因此，在机动过程中每个时间步长均可结合障碍物预测进行在线碰撞预测。相反，若缺乏障碍物轨迹预测，则在每个时间步长中，通过环境感知实时更新障碍物的边界框，并为其增加额外的安全裕度，而不依赖轨迹预测。本研究仅考虑了静态障碍物场景。因此，在未来工作中，可通过所提出的碰撞检测方法轻松引入具有或不具有预测轨迹的动态障碍物，并进一步将路径规划方法扩展以支持在线重新规划。

2) 路径平滑：

在基于IAM和CL‐RRT的路径生成情况下，G1连续性仅满足路径的斜率（偏航角）连续性，因为路径由恒曲率圆弧段组成，而在连接不同恒曲率圆弧段的路径节点处会出现曲率不连续[22]。这将导致在不连续点处转向需求发生突变，从而恶化横向控制器的跟踪性能。因此，为了确保生成的最终路径的可行性与可跟踪性，本文实现了路径平滑后处理，β−样条曲线生成[23], ，以满足路径上的曲率连续性，G2连续性，并基于卡车‐拖车系统的动力学约束（最大转向速率、最大车辆速度）计算允许的最大曲率边界。该算法以一组具有曲率不连续性的恒定半径圆弧段作为输入（G1连续性），输出满足卡车‐拖车系统动力学约束和边界条件的连续曲率路径（G2连续性）。β−样条曲线通过一组称为控制顶点的点定义，每四个相邻的控制顶点构成一个控制多边形，且样条曲线在多边形内的有界性得到保证[23]。

IV. 路径规划仿真

A. CL-RRT的仿真与算法参数

对于CL‐RRT算法外环，定义了五个不同的仿真参数以实现解收敛，如表II所示的性能。CL‐RRT算法循环运行最长时间为T max，若未找到解，则重新启动树扩展，最大迭代次数为 Itermax Itermax:，此时整个算法在无解情况下的总运行时间最长可达Tmaxx Itermax。如果在一次CL‐RRT循环中至少找到一个解，则将最大运行时间Tmax降低至Tsmax，以减少运行时间；此时，若已获得足够数量的解Solmax，或计算时间达到存在解的循环的最大持续时间Tsmax，算法即停止。此外，为了进一步增加从树的起点Ps n出发的分支变化，引入了一种基于完整路径长度Lp Ps Srnd从起点Ps到S rnd的成本启发式的概率选择方法。注意，在每次迭代中都会进行从Nmin到Srnd的仿真，并根据表III中列出的卡车‐拖车方向变化所允许的角度容差，检查该分支在碰撞和角度容差方面的可行性。如果目标仿真涉及一个可行解，则参考表IV中引入的目标评估参数。这些参数包括欧几里得距离和卡车‐拖车偏航角的误差容限，并区分向前和向后到达目标位姿的情况。表II、表III和表IV中选择的值是通过对图9所示测试场案例进行仿真获得的。

表II CL‐RRT仿真参数

表III CL‐RRT算法参数

表IV CL‐RRT目标参数

B. 级联路径规划仿真

通过在配备Intel Core i7‐6600U@2.6GHz CPU的标准笔记本电脑上运行一系列停车测试用例仿真，对路径规划算法的性能进行评估。开发了一种停车测试用例生成工具，该工具随机确定并生成卡车拖车系统的初始构型以及不同类型的静态障碍物（行人、自行车、乘用车和卡车）的随机数量，作为可能的测试用例。用于案例生成和成功泊车操作仿真的用户界面如图11所示。经过进一步手动剔除不切实际的测试场景后，共得到47种不同的测试场景，包含3种不同的停车位选择（总计141个测试用例）。由5种不同的卡车‐拖车初始位姿和8到12种不同的环境配置组合生成，每种卡车‐拖车位姿均设置一组障碍物。级联路径规划算法对每个测试用例运行10次，将10次运行的平均值作为对应测试的解存储。

在狭窄停车位泊车。图12展示了在4个测试用例中选择IAM方法以及在2个测试用例中选择CL‐RRT方法所得到的成功泊车解。在1410次求解尝试中，该算法实现了99.7%的求解成功率，其中67.38%的解通过选择IAM方法获得，32.62%的解通过CL‐RRT方法获得。算法选择器在选择IAM时的成功估计率达到85.1%，即IAM为测试用例找到解路径的情况。因此，仅有14.9%的IAM选择需要进一步运行CL‐RRT以找到成功的解路径。使用级联算法获得的解中，有89.2%在2秒内找到，总平均求解时间为0.82秒，其中IAM和CL‐RRT的平均求解时间分别为0.31秒和1.87秒。从图13a的求解时间直方图可以看出，IAM求解速度快得多，并且集中在非常短的时间间隔内，而另一方面，由于CL‐RRT算法具有概率特性，其求解时间分布较广。CL‐RRT的解路径成本较低，这是因为它在树扩展过程中能够通过多种解来成功完成测试案例，并选择成本最小的解。大多数CL‐RRT解扩展的节点数少于500个，而在达到最大规划时间时，平均扩展了1293个节点。

为了评估级联路径规划算法的性能，通过仅引入CL‐RRT作为路径规划器，对相同的测试用例进行了进一步的仿真，其中1410次仿真的结果如图14所示。由于在级联算法仿真中为IAM所选测试用例的平均求解时间为1.33秒，总仿真时间的平均值增加至1.49秒。需要注意的是，其余测试用例的平均求解时间为1.89秒，这与级联算法仿真中CL‐RRT选择的结果相比，该值非常接近预期。求解成功率也十分接近级联路径规划算法，达到99.4%。此外，路径代价和树扩展中的节点数分布也与级联算法仿真结果相近。值得注意的是，树扩展中节点数少于500的测试用例占比略有增加。由于为IAM所选测试用例的树扩展相比级联算法仿真中为CL‐RRT所选测试用例更为简单，因此这些测试用例的解收敛到解路径所需的扩展更少。可以看出，在级联算法中引入IAM可将计算量减少约45%。此外，它还为大量测试用例提供了确定性的且符合实际的路径。

接下来，对相同的测试用例进行了仿真，以评估随机样本生成方法的性能，其中算法中排除了所提出的随机采样方法，仅包含围绕工作空间中点的随机正态采样。1410次仿真的结果如图15所示，总平均求解时间进一步增加至1.76秒，求解成功率下降至94.9%。显然，引入随机采样方法可将计算量减少约15.3%，并将求解成功率提高约4.5%。

最后，使用栅格规划器和A*搜索对预定义的141个测试用例进行测试，如[11],所述，其中栅格规划的位置网格在r = 0.5 m的离散化精度下进行，x d和y d方向均采用该精度，挂车平衡角度离散化 δ d为16个角度，转向平衡角度离散化为3个角度，α d e = [−0.2117, 0, 0.2117]。格网规划器与级联规划器的对比结果如下所示在图16中，与级联规划器相比，格网规划器仿真的平均路径成本增加了5.09%，达到69.22，最大路径成本增加了33.5%，达到115.6。此外，在141个测试用例中，格网规划器仿真的求解成功率相比级联规划器降低了22.47%，降至77.3。

V. 相关工作与讨论

Palmieri等人[12]提出了一种用于非完整运动规划的 Theta*‐RRT算法，并在卡车‐拖车系统上进行了测试。该算法首先生成一条从车辆当前位置到目标位置的任意角度路径，然后利用转向函数在生成的路径周围进行树扩展，以满足非完整约束。随后，李等人[13]提出了一种渐进约束最优控制算法（PCOC），用于狭窄环境下的卡车‐拖车路径规划。该方法首先通过结合Reed‐Shepp曲线的混合A*算法生成一条粗略路径，此阶段完全忽略边界条件和避障约束；接着，将生成的粗略路径作为初始猜测，逐步优化以满足边界条件、避障和运动学约束。该算法输出的路径在狭窄环境中表现出优异的机动性。这两种算法的主要缺点在于Theta*和混合A*中搜索空间的离散化、使用Theta*和混合A*初始化单一操作解，以及最终仅生成单一操作解路径。因此，与基于CL‐RRT的方法[10],[18],相比，这两种算法缺乏双向机动性，不适用于码头站点和建筑工地等狭窄环境中的拖车停车用例。

其中常见的解路径主要由前进和后退方向的多次操作组成。特别是在图12所示的高障碍物密度环境下，由于初始阶段需要多操作解的用例中，使用混合A* + Reed‐Shepp在[13]中未考虑碰撞约束的初始猜测可能会生成单次操作解，从而无法为包含避障约束的进一步优化提供合适的粗略路径。因此，该算法在这种用例中无法保证得到最优解。此外，解路径中的初始操作需要为车辆提供足够的空间，以成功执行最终倒车进入停车位的机动过程。因此，在前述用例中，围绕任意角度路径进行树扩展对于卡车‐拖车停车来说并非一种现实可行的方法，因为它可能无法提供必要的树扩展来找到解路径。此外，混合A*被用于具有双向机动性的乘用车，包括掉头和停车功能[24], ，其中运动基元为在考虑碰撞约束的情况下，可在前进和后退方向生成路径。因此，该方法可扩展应用于具有高效转向方式选择的卡车‐拖车系统。然而，该算法仍受限于搜索空间的离散化，在狭窄环境中可能无法实现可行的解操作。因此，混合A*虽能保证生成可实现的路径，但不具备完备性，可能会找不到路径，且随着离散化程度变粗，成功的可能性会降低[25]。因此，使用Reed‐Shepp算法的计算优势在很大程度上取决于环境中的障碍物密度。因此，在障碍物密集的环境中（如图12所示），Reed‐Shepp扩展大多会导致碰撞，因此在这种用例中混合A*+ Reed‐Shepp将无法工作[24]。

Ljungqvist等人[11],、Stahn等人[7],和Rimmer等人[8]为双挂车系统开发了基于格网的运动规划框架，其中生成了一组运动基元库，并用于构建离线状态格网，然后在该状态格点中使用A*搜索算法对[11, 7]进行无碰撞路径搜索，以及使用Dijkstra网格搜索算法对[8]进行搜索。这类基于格网或网格并生成离线运动基元的算法相比基于RRT的解决方案能够提供更快的解，因为在RRT中运动基元是在树扩展过程中实时生成的。此外，基于格网的方法不是随机化的，并且具有分辨率最优性。然而，基于格网的算法存在局限性：无法充分利用卡车‐拖车系统的全部机动能力，并可能由于格网点的离散化导致初始和最终构型误差，即卡车‐拖车的位置和姿态可能无法精确匹配到某个网格点上。提高网格分辨率可以减小构型误差，但同时也会增加计算量。因此，CL‐RRT提供了更灵活的采样方式，往往能以更少的迭代次数和更低成本的解完成搜索。前文已展示了格网规划器与级联规划器的对比结果，可以看出，相较于级联规划器，格网规划器的求解率较低，且路径解成本更高。还值得注意的是，可以通过引入简单的过渡曲线方法（如Dubins路径或闭环转向[18]），将自车起始位姿连接到生成路径的第一个操作，从而克服初始构型误差，因为在初始操作周围可能存在足够的自由空间允许这种连接。然而，为了消除最终构型误差，则需要采用更复杂且计算量更大的后处理方法，例如求解一个优化问题[26]，因为最终构型位于停车位内，必须将避障约束注入路径优化过程中。否则，由于CL‐RRT方法无法精确达到目标构型[10]，必须进行大量采样才能获得可接受的目标位姿。另一方面，IAM和CL‐RRT若从停车位内的目标位姿开始反向规划，则可保证最终构型误差为零，而初始构型误差也可通过上述简单的过渡曲线方法之一轻松消除。

在温和的技术条件下，基于采样的算法的解代价几乎必然收敛到一个非最优值。因此，CL‐RRT算法对解不提供任何最优性保证，并倾向于产生明显次优的解，即快速找到一条可行路径，然后利用剩余的计算时间来改进该解[14]。另一方面，扩展算法RRT*保证渐近最优性，并已在[27]中通过局部最优转向方法将其进一步实现于非完整系统，例如杜宾车（不适用于卡车‐拖车系统），以满足局部可控性条件。然而，Dubins路径的性能也与障碍物密度密切相关，如同里德‐谢普情况一样。因此，首先需要一种针对卡车‐拖车系统、在高障碍物密度环境中表现优异的可行局部最优转向方法，以便将CL‐RRT方法扩展至RRT*，应用于狭窄停车位并保证渐近最优性。相应地，为完整性及最优性考虑，相关算法在表V中进行了总结。

本文提出了IAM作为一种更快且更具确定性的方法，可覆盖狭窄环境中卡车‐拖车停车的大多数场景；同时提出CL‐RRT作为一种更通用的方法，凭借其概率完备性[28]来应对其余主要表现为障碍物密集环境的场景。两种算法在解路径中均提供双向机动性，包含前进和后退方向的操作，这是码头站点和建筑工地等狭窄环境中拖车停车用例中最常见的特征。本文提出一种级联路径规划框架，通过引入算法选择器，结合两种算法的优势，以确定哪种算法更适合特定的停车场景。由于级联框架在IAM求解尝试失败后会将问题转交给CL‐RRT，因此算法选择器的估计性能对级联框架的计算性能具有重要影响。该算法选择器在各种测试用例中实现了对IAM选择的成功估计率，减少了不必要的IAM求解尝试次数。

VI. 结论

本文提出了一种针对卡车‐拖车系统的路径规划框架。该框架中提出了一种新的级联路径规划算法，该算法结合了迭代解析法（IAM）和闭环快速探索随机树算法（CL‐RRT），并根据卡车驾驶员在倒车停车时的实际泊车习惯，利用生成的可能性和障碍物密度参数进行选择。如果迭代解析法（IAM）存在可行解，则因其具有确定性且求解速度更快，将优先于CL‐RRT被选用。通过多种泊车场景的仿真对级联路径规划算法的性能进行了测试，结果表明，该级联算法在选择IAM时的成功估计率达到85.1%。在同一组仿真中，该级联算法的总体成功率达到99.7%，且获得可行解的平均求解时间为0.82秒，无论选择哪种方法。将IAM与级联规划器结合使用后，相比CL‐RRT算法，计算量减少了约45%。随后，CL‐RRT算法通过引入一种随机样本生成方法，在树扩展过程中实现了比在工作空间中点附近进行随机正态采样快约15.3%的收敛速度。由于CL‐RRT算法能够在多次成功解中灵活选择成本最低的选项，因此其路径成本低于IAM。为了便于估算，在每次从随机样本到树节点的尝试中，仅通过分支候选的纯追踪弧线长度定义了成本启发式，以避免在选择最可行的到达树节点之前执行任何仿真。在未来工作中，可通过增加角度变化项，并在网格上通过离线仿真生成查找表对成本进行插值，从而进一步改进成本定义。此外，可以引入一种针对卡车‐拖车系统的可行的局部最优转向方法，将CL‐RRT方法扩展为RRT*，以在狭窄停车位中保证渐近最优性。