CN115840451A - 狭窄通道无人驾驶拖挂避障转弯轨迹规划方法 - Google Patents

Abstract

本发明公开了一种狭窄通道无人驾驶拖挂避障转弯轨迹规划方法，包括：S1，在t∈[t₀,t₀+Δt]时段，利用车载传感器得到无人驾驶拖挂的状态量；S2，构建车辆运动学约束；S3，构建避障约束；S4，构建控制量约束；S5，构建两点边界约束；S6，构建牵引车与拖箱夹角约束；S7，构建最优控制问题；S8，求解得到初始轨迹；S9，构建车辆加加速度约束；S10，用动态规划法求解LQR；S11：对所述优化后的轨迹再次进行迭代LQR求解，直到代价函数收敛到10^‑4，停止迭代，得到最优轨迹。该方法约束处理巧妙，迭代优化求解能得到最优轨迹且求解速度快，提高了无人驾驶拖挂在狭窄通道内的通行效率与行驶安全性。

Description

狭窄通道无人驾驶拖挂避障转弯轨迹规划方法

技术领域

本发明涉及无人驾驶领域，尤其涉及一种在港口等狭窄通道内行驶的无人驾驶拖挂避障转弯的轨迹规划方法。

背景技术

随着经济全球化的不断深入，电商平台和进出口贸易蓬勃发展，带动了交通运输业的发展。拖挂车辆因其具有运输成本低廉、运输量大、装卸货物方便等优点，成为交通运输业公路长途运输的首选，广泛活跃于各大工厂、港口。但港口集装箱排布较密，通道较为狭窄，这给动辄二十几米的拖挂车辆行驶带来了困难。特别是面临直角转弯时，要求车辆在狭小的空间内完成无碰撞过弯，这给无人驾驶拖挂的运动规划提出了更高的要求。

现有技术中，常用基于采样和搜索的路径规划方法来规划无人驾驶汽车的运动路径。拖挂车辆牵引车与拖箱中间用铰接点进行连接，车辆运动的灵活度大，轨迹规划中的各类约束条件复杂。基于采样与搜索的路径规划方法难以保证在满足复杂约束条件下规划出可行路径。同时，这两种方法导出的路径并不光滑，给后续跟踪模块进行路径跟踪增加了困难，即使使用数学方法对路径进行平滑处理，也难以保证得到的路径为最优路径。

发明内容

针对无人驾驶拖挂车辆在港口等狭窄通道内转弯困难的问题，本发明提供一种基于最优控制理论的狭窄通道无人驾驶拖挂避障转弯轨迹规划方法。该方法运用最优控制理论直接生成车辆行驶轨迹，能在给定狭小空间内规划出满足所有约束条件的最优轨迹。提高了拖挂车辆在狭窄空间内的通行效率与行驶安全性。

为此，本发明采用以下技术方案：

一种狭窄通道无人驾驶拖挂避障转弯轨迹规划方法，包括以下步骤：

S1，在t∈[t₀,t₀+Δt]时段，利用车载传感器得到无人驾驶拖挂的状态量：

[x_r,y_r,θ₁,θ₂,v₁,δ_f]

其中，(x_r，y_r)为在笛卡尔坐标系下牵引车后轴中点的位置坐标、θ₁为牵引车横摆角、θ₂为拖箱横摆角，v₁为牵引车后轴中点的速度，δ_f为牵引车前轮转角；

获取车辆周围的点云数据及无人驾驶拖挂周围静态障碍物外轮廓的一系列位置坐标(Xi，Yi)，以及动态障碍物的当前和预测位置外轮廓的一系列坐标(Xi′，Yi′，T)；

由所述无人驾驶拖挂的状态量及其周围静态、动态障碍物的位置点集，形成无人驾驶拖挂与周围障碍物的瞬时位姿映射关系；

S2，构建车辆运动学约束：

将牵引车前轴、后轴和拖箱车轴在大地坐标系下简化为三个点，分别记为O_f(x_f,y_f)，O_r(x_r,y_r)，O_l(x_l,y_l)，设牵引车与拖箱的铰接点位于牵引车后轴中点的正上方，设牵引车的轴距为l₁，铰接点到拖箱后轴中点的距离为l₂，不考虑侧向滑动，牵引车与拖箱所有车轴中点的轴向速度为零，满足下式：

根据几何位置关系，建立牵引车后轴中点O_r与拖箱轴中点O_f之间的关系、牵引车后轴中点O_r与牵引车前轴中点O_l之间的关系，计算牵引车后轴中点在大地坐标系下的横纵向车速、牵引车前轴中点的速度、拖箱车轴中点的速度，进而求得铰接点位于牵引车后轴中点的车辆运动学方程；

S3，构建避障约束：

设牵引车车头到拖箱前端的距离为S，牵引车宽为W，拖箱的长为L、宽亦为W，用圆形和椭圆分别覆盖所述牵引车和拖箱，根据牵引车的几何尺寸与圆最大内接矩形的定义，得到所述圆形的圆心位于笛卡尔坐标系原点的圆方程和圆心位于笛卡尔坐标系原点的椭圆方程；

由步骤S1中的车辆映射外轮廓得到覆盖车体的圆与椭圆的映射外轮廓，基于圆与椭圆的映射外轮廓与各静态、动态障碍物的映射外轮廓，用改进的Minkowski sum方法生成无人驾驶拖挂的避障区域；用改进的Minkowski sum方法将无人驾驶拖挂映射的圆心沿步骤S1中生成的障碍物映射的外轮廓绕行一周时无人驾驶拖挂映射所包围的全部区域定义为规划时圆与椭圆的圆心分别要避开的碰撞区域；

将圆心/椭圆心与避障区域圆弧与直线的切点相连，判断所有连线的夹角和是否等于360°，若等于360°，则证明圆心在避障区域内部，不满足碰撞约束条件；若夹角和不等于360°，则满足碰撞约束条件；碰撞约束条件简写为：

L_barrier(X(t))≤0，

其中，X(t)为车辆状态量；

S4，构建控制量约束，对控制量和控制量的增量进行约束，以保证车辆行驶的稳定性；

S5，构建两点边界约束：识别初始时刻的状态量和控制量以及给定目标时刻的状态量和控制量，一起构成两点边界约束；

S6，构建牵引车与拖箱夹角约束；

S7，构建最优控制问题：由代价函数

与所述车辆运动学约束、避障约束、控制量约束、两点边界约束、牵引车与拖箱夹角约束一起构成求解最优轨迹的最优控制问题；

S8，由基于搜索的路径规划算法A^*生成连接车辆起点与目标点的初始路径，用样条曲线对初始路径进行平滑处理，在笛卡尔坐标系下求初始路径的斜率k，由公式

求得路径点上的参考前轮转角；设该参考线上的参考速度方向沿参考线切线方向，为定值ν_f；初始路径有了转角和速度参考控制量δ_r与ν_r，加入了控制量路径就有了时间信息，从而得到初始轨迹；

S9，构建车辆加加速度约束：

jerk_min≤jerk≤jerk_max

转化为代价函数的一部分，构造内点惩罚函数；通过CILQR算法求解复杂约束下的非线性规划问题；

S10，用动态规划法求解LQR：将车辆状态转移方程沿规划的轨迹在参考点泰勒一阶展开，完成状态转移方程的线性化，将求解ILQR问题转化为沿规划轨迹，求解一系列的LQR问题，得到一条优化后的轨迹；

S11：对所述优化后的轨迹再次进行迭代LQR求解，直到代价函数收敛到10^-4，停止迭代，得到最优轨迹。

其中，步骤S2牵引车后轴中点O_r与拖箱轴中点O_f之间的关系为：

牵引车后轴中点O_r与牵引车前轴中点O_l之间的关系式为：

牵引车后轴中点在大地坐标系下的横纵向车速为：

牵引车前轴中点的速度为：

拖箱车轴中点的速度为：

铰接点位于牵引车后轴中点的车辆运动学方程为：

步骤S3中所述的圆方程为：

所述椭圆方程为：

步骤S4中控制量约束满足下式：

其中，a为牵引车的加速度，jerk为牵引车加速度的导数，v₁为牵引车后轴中点速度，δ_f为牵引车前轮转角，ω为牵引车前轮转角的角速度。

步骤S5中，识别初始时刻t＝0的状态量与控制量为：X(0)＝X_initial，U(0)＝U_initial；给定目标时刻t＝T的状态量与控制量为：X(T)＝X_aim，U(T)＝U_aim。

步骤S6中，牵引车与拖箱夹角约束条件为：0°≤|θ₁-θ₂|＜90°。

步骤S9中，加加速度约束代价为：

jerk_min≤jerk≤jerk_max，

内点惩罚函数如下式所示：

式中，q₁、q₂为系数；

通过调节式中q₁、q₂的值，获得一个性质更加符合代价函数要求的曲线；若当前加速度在约束范围内，惩罚函数会得到一个很小的值；若在范围外，则得到一个指数级别的惩罚值；利用惩罚函数表示约束函数，转化为代价函数的一部分，将CILQR问题转化为ILQR问题；

步骤S10的求解过程如下：

延初始轨迹划分一系列连续时段，在每个时段内将状态转移方程进行泰勒展开，泰勒展开后的状态转移方程与代价函数为：

引入Q函数，描述在X_T状态下采取动作U_T的代价：

引入V函数，描述状态本身的代价，这里定义状态代价等于可以使得后续状态代价最优的Q代价：

将代价矩阵重写成如下形式：

(1)backward过程：

对Q函数求导可得令其等于零：

可解得：

令：

可得控制量为：

u_T＝K_Tx_T+k_T

根据上述推导结果，有：

得：

式中：

将T时刻向前回溯到T-1时刻，由于X_T-1不是末端节点状态，因此扩展Q函数定义成：

由公式：

重写V函数可得：

可得T-1时刻的Q函数为：

式中：

再次对Q函数求导，令其导数等于零：

可得T-1时刻的控制量：

u_T-1＝K_T-1x_T-1+k_T-1

通过多次向前回溯，即可得到初状态：

u₁＝K₁x₁+k₁

(2)forward过程：

在backward过程中，通过从后往前的计算依次得到了一系列控制量，然后将这些控制量从前往后依次带入到车辆状态转移方程，即得到一条优化后的轨迹：

与现有技术相比，本发明具有以下有益效果：

(1)本方案针对在港口等狭窄通道下行驶的无人驾驶拖挂运动规划困难的问题，提出了一种基于最优控制理论的轨迹规划方法。该方法将轨迹规划问题描述为求解满足约束条件下的最优控制问题，通过求解最优控制问题得到车辆行驶的最优轨迹，提高了无人驾驶拖挂在狭窄通道内的通行效率与行驶安全性。对推动无人驾驶技术的发展，提高港口货物运输效率具有重要意义。

(2)采用CILQR算法求解复杂约束下的非线性规划问题，利用惩罚函数表示约束函数，转化为代价函数的一部分。使用动态规划法求解LQR问题，通过不断迭代得到最优轨迹。与其他求解最优控制问题的方法相比，该方法约束处理巧妙，迭代优化求解能得到最优轨迹且求解速度快。

(3)在描述避障约束时，分别用圆和椭圆描述拖挂牵引车与货箱的外廓体积。引入了碰撞多边形的概念，碰撞多边形定义为某一时刻覆盖车体的圆与障碍物的改进Minkowski sum。碰撞多边形在生成时考虑了车辆的位姿，与其他生成障碍区域的方法相比，碰撞多边形面积更小，相应的可通行区域也就越大，也就越容易得到最优轨迹。

附图说明

图1为狭窄通道无人驾驶拖挂避障转弯示意图；

图2为无人驾驶拖挂简化车辆运动学模型示意图；

图3为无人驾驶拖挂车身圆与椭圆覆盖示意图；

图4为用改进的Minkowski sum方法生成的拖箱避障区域示意图；

图5为ILQR求解流程图。

具体实施方式

本发明采用CILQR(constraint iteration LQR)算法求解最优控制问题。CILQR是为了解决LQR算法无法处理复杂约束的优化问题而提出的，是一种扩展的ILQR算法。它利用惩罚函数来表示约束函数的方法来处理含约束的LQR问题，最终采用与ILQR相同的框架来获得问题的最优解。将问题中的不等式约束转化为代价函数的一部分，构造内点惩罚函数：

通过调节式中q₁和q₂的值，我们可以获得一个性质更加符合我们代价函数要求的曲线。LQR要求系统状态转移方程是线性的，ILQR拓展到了非线性的模型。ILQR求解的整体思路是：先确定一条初始轨迹，然后用LQR优化这条轨迹。初始轨迹采用基于搜索的路径规划算法A^*获得，用样条曲线对初始路径进行平滑处理，在笛卡尔坐标系下由初始路径的斜率k求得路径点上的参考前轮转角，设该参考线上的参考速度为定值，方向沿参考线切线方向。初始路径有了参考控制量就形成了初始参考轨迹。

在用LQR优化轨迹时，运用了动态规划法。先是从后往前进行backward过程，该过程得到了一系列的δu。然后通过forward过程，从前往后依次将backward过程当中得到的一系列的控制量的变化量带入状态转移方程中，更新状态量，得到一条优化后的轨迹。迭代进行backward与forward过程，直到代价函数收敛，得到满足约束条件的最优轨迹。

以下结合附图对本发明的方法进行详细说明。

本发明中所述无人驾驶拖挂包括牵引车和拖箱。

参见图1，本发明的狭窄通道无人驾驶拖挂避障转弯轨迹规划方法包括以下步骤：

S1，在t∈[t₀,t₀+Δt]时段，利用GPS、IMU等车载传感器得到无人驾驶拖挂的状态量：

[x_r,y_r,θ₁,θ₂,v₁,δ_f]

其中，(x_r，y_r)为在笛卡尔坐标系下牵引车后轴中点的位置坐标、θ₁为牵引车横摆角、θ₂为拖箱横摆角，v₁为牵引车后轴中点的速度，δ_f为牵引车前轮转角。

利用激光雷达扫描得到车辆周围点云数据，经由感知预测模块得到无人驾驶拖挂周围静态障碍物外轮廓的一系列位置坐标(Xi，Yi)，以及动态障碍物的当前和预测位置外轮廓的一系列坐标(Xi′，Yi′，T)。由所述无人驾驶拖挂的状态量及其周围静态、动态障碍物的位置点集，形成无人驾驶拖挂与周围障碍物的瞬时位姿映射关系。

S2，构建车辆运动学约束：

因无人驾驶拖挂转弯时车速低、轮胎侧偏角小，车辆运动学模型足以描述其在转弯时的运动特性，根据牵引车后轴中心点的速度v₁、车辆前轮转角δ_f建立拖挂简化运动学示意图，如图2所示。

牵引车前轴、后轴和拖箱车轴在大地坐标系下简化为三个点，分别记为O_f(x_f,y_f)，O_r(x_r,y_r)，O_l(x_l,y_l)。通过定位牵引车后轴中心的位置坐标，假设牵引车与拖箱的铰接点位于牵引车后轴中点的正上方。设牵引车的轴距为l₁，铰接点到拖箱后轴中点的距离为l₂。不考虑侧向滑动，牵引车与拖箱所有车轴中点的轴向速度为零，即满足下式：

根据几何位置关系，建立牵引车后轴中点O_r与拖箱轴中点O_f之间的关系，如下式：

根据几何位置关系，建立牵引车后轴中点O_r与牵引车前轴中点O_l之间的关系，如下式：

牵引车后轴中点在大地坐标系下的横纵向车速表示为：

牵引车前轴中点的速度表示为：

拖箱车轴中点的速度表示为：

由以上公式求得铰接点位于牵引车后轴中点的车辆运动学方程为：

S3，构建避障约束：

设牵引车车头到拖箱前端的距离为S，牵引车宽为W，拖箱的长为L、宽亦为W。用圆形和椭圆分别覆盖拖挂的牵引车与拖箱，如图3所示，根据牵引车的几何尺寸与圆最大内接矩形的定义，可得图中圆心位于笛卡尔坐标系原点的圆方程为：

根据拖箱的几何尺寸与椭圆最大内接矩形的定义，可得圆心位于笛卡尔坐标系原点的椭圆方程为：

由步骤S1中的车辆映射外轮廓可得覆盖车体的圆与椭圆的映射外轮廓，基于圆与椭圆的映射外轮廓与各静态、动态障碍物的映射外轮廓，用改进的Minkowski sum方法生成无人驾驶拖挂的避障区域。用改进的Minkowski sum方法将无人驾驶拖挂映射的圆心沿步骤S1中生成的障碍物映射的外轮廓绕行一周时无人驾驶拖挂映射所包围的全部区域定义为规划时圆与椭圆的圆心分别要避开的碰撞区域。图4中右图最外侧虚线包围的区域(避障区)为用改进的Minkowski sum方法生成的拖箱避障区域，该区域即为在规划轨迹时图中左侧椭圆圆心(拖箱的中心)要避开的区域。

将圆心/椭圆心与避障区域圆弧与直线的切点相连，判断所有连线的夹角和是否等于360°，若等于360°则证明圆心在避障区域内部，不满足碰撞约束条件；若夹角和不等于360°则满足碰撞约束条件。碰撞约束条件简写为：

L_barrier(X(t))≤0，

其中，X(t)为车辆状态量

S4，构建控制量约束：

控制量约束描述车辆控制量的最大值与最小值。在实际控制过程中，不仅要对控制量进行约束，还要对控制量的增量进行约束，使控制量的增量变化不能过快，保证车辆行驶的稳定性。控制量约束满足下式：

在上述不等式中，a为牵引车的加速度，jerk为牵引车加速度的导数(加加速度)，v₁为牵引车后轴中点速度，δ_f为牵引车前轮转角，ω为牵引车前轮转角的角速度。

S5，构建两点边界约束：

识别初始时刻t＝0的状态量与控制量：X(0)＝X_initial，U(0)＝U_initial，反应初始时刻的真实值；

给定目标时刻t＝T的状态量与控制量：X(T)＝X_aim，U(T)＝U_aim，与初始时刻的状态量与控制量一起构成两点边界约束。

S6，构建牵引车与拖箱夹角约束：

为了防止牵引车发生折叠，影响行车安全性，必须对拖挂牵引车与拖箱之间的夹角进行限制，牵引车与拖箱夹角约束条件为：

0°≤|θ₁-θ₂|＜90°

S7，构建最优控制问题：

由代价函数：

与车辆运动学约束、避障约束、控制量约束、两点边界约束、夹角约束一起构成求解最优轨迹的最优控制问题。

S8，由基于搜索的路径规划算法A^*生成连接车辆起点与目标点的初始路径，用样条曲线对初始路径进行平滑处理。在笛卡尔坐标系下求初始路径的斜率k，由公式：

δ_fr＝arctan(k)

求得路径点上的参考前轮转角，设该参考线上的参考速度方向沿参考线切线方向，为定值v_f。初始路径有了转角和速度参考控制量δ_r与ν_r，加入了控制量路径就有了时间信息(带有时间信息的路径为轨迹)，也就得到初始轨迹；

S9，构建车辆加加速度约束：

通过将问题中的不等式约束转化为代价函数的一部分，构造内点惩罚函数。通过CILQR算法求解复杂约束下的非线性规划问题。

加加速度约束代价为：

jerk_min≤jerk≤jerk_max

内点惩罚函数如下式所示：

式中q₁、q₂为系数，通过调节式中q₁、q₂的值，可以获得一个性质更加符合我们代价函数要求的曲线。若当前加速度在约束范围内惩罚函数会得到一个很小的值；若在范围外则会得到一个指数级别的惩罚值。利用惩罚函数表示约束函数，转化为代价函数的一部分，CILQR问题就转化为了ILQR问题。

S10，用动态规划法求解LQR：

将车辆状态转移方程沿规划的轨迹在参考点泰勒一阶展开，完成了状态转移方程的线性化。求解ILQR问题就转化为了沿规划轨迹，求解一系列的LQR问题。

延初始轨迹划分一系列连续时段，在每个时段内将状态转移方程进行泰勒展开，泰勒展开后的状态转移方程与代价函数为：

引入Q函数，描述在X_T状态下采取动作U_T的代价：

引入V函数，描述状态本身的代价，这里定义状态代价等于可以使得后续状态代价最优的Q代价：

将代价矩阵重写成如下形式：

(1)backward过程：

对Q函数求导可得令其等于零：

可解得：

令：

可得控制量为：

u_T＝K_Tx_T+k_T

根据上述推导结果，有：

得：

式中：

将T时刻向前回溯到T-1时刻，由于X_T-1不是末端节点状态，因此扩展Q函数定义成：

由公式：

重写V函数可得：

可得T-1时刻的Q函数为：

式中：

再次对Q函数求导，令其导数等于零：

可得T-1时刻的控制量：

u_T-1＝K_T-1x_T-1+k_T-1

通过多次向前回溯，即可得到初状态：

u₁＝K₁x₁+k₁

(2)forward过程：

在backward过程中，通过从后往前的计算依次得到了一系列控制量，然后将这些控制量从前往后依次带入到车辆状态转移方程，即得到一条优化后的轨迹：

S11：对优化后的轨迹再次进行迭代LQR求解，迭代LQR求解流程如图5所示，直到代价函数收敛到10^-4，则停止迭代，得到最优轨迹。

Claims (8)

1.一种狭窄通道无人驾驶拖挂避障转弯轨迹规划方法，包括以下步骤：

S1，在t∈[t₀，t₀+Δt]时段，利用车载传感器得到无人驾驶拖挂的状态量：

[x_r，y_r，θ₁，θ₂，v₁，δ_f]

其中，(x_r，y_r)为在笛卡尔坐标系下牵引车后轴中点的位置坐标、θ₁为牵引车横摆角、θ₂为拖箱横摆角，v₁为牵引车后轴中点的速度，δ_f为牵引车前轮转角；

获取车辆周围的点云数据及无人驾驶拖挂周围静态障碍物外轮廓的一系列位置坐标(Xi，Yi)，以及动态障碍物的当前和预测位置外轮廓的一系列坐标(X_i′，Y_i′，T)；

由所述无人驾驶拖挂的状态量及其周围静态、动态障碍物的位置点集，形成无人驾驶拖挂与周围障碍物的瞬时位姿映射关系；

S2，构建车辆运动学约束：

将牵引车前轴、后轴和拖箱车轴在大地坐标系下简化为三个点，分别记为O_f(x_f，y_f)，O_r(x_r，y_r)，O_l(x_l，y_l)，设牵引车与拖箱的铰接点位于牵引车后轴中点的正上方，设牵引车的轴距为l₁，铰接点到拖箱后轴中点的距离为l₂，不考虑侧向滑动，牵引车与拖箱所有车轴中点的轴向速度为零，满足下式：

根据几何位置关系，建立牵引车后轴中点O_r与拖箱轴中点O_f之间的关系、牵引车后轴中点O_r与牵引车前轴中点O_l之间的关系，计算牵引车后轴中点在大地坐标系下的横纵向车速、牵引车前轴中点的速度、拖箱车轴中点的速度，进而求得铰接点位于牵引车后轴中点的车辆运动学方程；

S3，构建避障约束：

设牵引车车头到拖箱前端的距离为S，牵引车宽为W，拖箱的长为L、宽亦为W，用圆形和椭圆分别覆盖所述牵引车和拖箱，根据牵引车的几何尺寸与圆最大内接矩形的定义，得到所述圆形的圆心位于笛卡尔坐标系原点的圆方程和圆心位于笛卡尔坐标系原点的椭圆方程；

由步骤S1中的车辆映射外轮廓得到覆盖车体的圆与椭圆的映射外轮廓，基于圆与椭圆的映射外轮廓与各静态、动态障碍物的映射外轮廓，用改进的Minkowski sum方法生成无人驾驶拖挂的避障区域；用改进的Minkowski sum方法将无人驾驶拖挂映射的圆心沿步骤S1中生成的障碍物映射的外轮廓绕行一周时无人驾驶拖挂映射所包围的全部区域定义为规划时圆与椭圆的圆心分别要避开的碰撞区域；

将圆心/椭圆心与避障区域圆弧与直线的切点相连，判断所有连线的夹角和是否等于360°，若等于360°，则证明圆心在避障区域内部，不满足碰撞约束条件；若夹角和不等于360°，则满足碰撞约束条件；碰撞约束条件简写为：

L_barrier(X(t))≤0，

其中，X(t)为车辆状态量；

S4，构建控制量约束，对控制量和控制量的增量进行约束，以保证车辆行驶的稳定性；

S5，构建两点边界约束：识别初始时刻的状态量和控制量以及给定目标时刻的状态量和控制量，一起构成两点边界约束；

S6，构建牵引车与拖箱夹角约束；

S7，构建最优控制问题：由代价函数

与所述车辆运动学约束、避障约束、控制量约束、两点边界约束、牵引车与拖箱夹角约束一起构成求解最优轨迹的最优控制问题；

S8，由基于搜索的路径规划算法A^*生成连接车辆起点与目标点的初始路径，用样条曲线对初始路径进行平滑处理，在笛卡尔坐标系下求初始路径的斜率k，由公式

求得路径点上的参考前轮转角；设该参考线上的参考速度方向沿参考线切线方向，为定值v_f；初始路径有了转角和速度参考控制量δ_r与v_r，加入了控制量路径就有了时间信息，从而得到初始轨迹；

S9，构建车辆加加速度约束：

jerk_min≤jerk≤jerk_max

转化为代价函数的一部分，构造内点惩罚函数；通过CILQR算法求解复杂约束下的非线性规划问题；

S10，用动态规划法求解LQR：将车辆状态转移方程沿规划的轨迹在参考点泰勒一阶展开，完成状态转移方程的线性化，将求解ILQR问题转化为沿规划轨迹，求解一系列的LQR问题，得到一条优化后的轨迹；

S11：对所述优化后的轨迹再次进行迭代LQR求解，直到代价函数收敛到10^-4，停止迭代，得到最优轨迹。

2.根据权利要求1所述的狭窄通道无人驾驶拖挂避障转弯轨迹规划方法，其特征在于，步骤S2中，牵引车后轴中点O_r与拖箱轴中点O_f之间的关系为：

牵引车后轴中点O_r与牵引车前轴中点O_l之间的关系式为：

牵引车后轴中点在大地坐标系下的横纵向车速为：

牵引车前轴中点的速度为：

拖箱车轴中点的速度为：

铰接点位于牵引车后轴中点的车辆运动学方程为：

3.根据权利要求1所述的狭窄通道无人驾驶拖挂避障转弯轨迹规划方法，其特征在于，步骤S3中所述的圆方程为：

所述椭圆方程为：

4.根据权利要求1所述的狭窄通道无人驾驶拖挂避障转弯轨迹规划方法，其特征在于，步骤S4中控制量约束满足下式：

其中，a为牵引车的加速度，jerk为牵引车加速度的导数，v₁为牵引车后轴中点速度，δ_f为牵引车前轮转角，ω为牵引车前轮转角的角速度。

5.根据权利要求1所述的狭窄通道无人驾驶拖挂避障转弯轨迹规划方法，其特征在于，步骤S5中，识别初始时刻t＝0的状态量与控制量为：X(0)＝X_initial，U(0)＝U_initial；给定目标时刻t＝T的状态量与控制量为：X(T)＝X_aim，U(T)＝U_aim。

6.根据权利要求1所述的狭窄通道无人驾驶拖挂避障转弯轨迹规划方法，其特征在于，步骤S6中，牵引车与拖箱夹角约束条件为：0°≤|θ₁-θ₂|＜90°。

7.根据权利要求1所述的狭窄通道无人驾驶拖挂避障转弯轨迹规划方法，其特征在于，步骤S9中，加加速度约束代价为：

jerk_min≤jerk≤jerk_max，

内点惩罚函数如下式所示：

式中，q₁、q₂为系数；

通过调节式中q₁、q₂的值，获得一个性质更加符合代价函数要求的曲线；若当前加速度在约束范围内，惩罚函数会得到一个很小的值；若在范围外，则得到一个指数级别的惩罚值；利用惩罚函数表示约束函数，转化为代价函数的一部分，将CILQR问题转化为ILQR问题。

8.根据权利要求1所述的狭窄通道无人驾驶拖挂避障转弯轨迹规划方法，其特征在于，步骤S10的求解过程如下：

延初始轨迹划分一系列连续时段，在每个时段内将状态转移方程进行泰勒展开，泰勒展开后的状态转移方程与代价函数为：

引入Q函数，描述在X_T状态下采取动作U_T的代价：

引入V函数，描述状态本身的代价，这里定义状态代价等于可以使得后续状态代价最优的Q代价：

将代价矩阵重写成如下形式：

(1)backward过程：

对Q函数求导可得令其等于零：

可解得：

令：

可得控制量为：

u_T＝K_Tx_T+k_T

根据上述推导结果，有：

式中：

将T时刻向前回溯到T-1时刻，由于X_T-1不是末端节点状态，因此扩展Q函数定义成：

由公式：

与

重写V函数可得：

可得T-1时刻的Q函数为：

式中：

再次对Q函数求导，令其导数等于零：

可得T-1时刻的控制量：

u_T-1＝K_T-1x_T-1+k_T-1

通过多次向前回溯，即可得到初状态：

u₁＝K₁x₁+k₁

(2)forward过程：

在backward过程中，通过从后往前的计算依次得到了一系列控制量，然后将这些控制量从前往后依次带入到车辆状态转移方程，即得到一条优化后的轨迹：

Images