阶段约束下Gauss配点离散化平行车位自动泊车轨迹规划*

刘 平，陈 卓，刘明杰，，朴昌浩，Jang Soohyun，万凯林

（1.重庆邮电大学自动化学院，重庆 400065；2.韩国电子技术研究所，Korea Seongnam 13230；3.重庆长安汽车股份有限公司，重庆 400023）

前言

汽车作为现代社会重要的交通工具，在日常出行和生活中扮演着越来越重要的角色。然而，日益复杂的城市车位环境对泊车技术提出了更高要求。自动泊车作为解放驾驶员双手、提高泊车效率和安全性的重要技术在近些年得到了广泛关注和研究［1-2］。

自主泊车技术中，泊车轨迹规划是实现自动泊车的重要基础。当前，泊车轨迹规划算法主要包括：几何方法、图搜索算法、随机采样方法、智能学习算法和数值优化方法等。其中，几何方法主要通过曲线组合和插值拟合获取轨迹，比如文献［3］中采用两段圆弧添加一段直线的方式来规划泊车路径，车辆可从水平初始位姿行驶一段S 形曲线路径后进入泊车位并摆正车身姿态；文献［4］中则基于回旋曲线的平行泊车路径规划，避免了原地转向现象，实现规划路径基本可控。而图搜索算法则首先将二维环境地图或者三维环境地图离散化成栅格或者晶格，然后从起点出发向目标点搜索出一条满足最优准则的路径，比如文献［5］中构建栅格地图搜索空间，并采用混合A*算法进行了自主代客泊车路径规划；文献［6］中以停车场信息为基础，设计了多分辨率路线图，可以在较小的在线计算成本下获得自动泊车轨迹规划。随机采样方法具有概率完备性，在构型空间中通过生成样本点，并寻找满足任务的规划结果，如文献［7］中结合RRT*（rapidly-exploring random tree）算法与非线性优化算法，实现曲率连续的泊车轨迹并适应多泊车场景。智能学习算法主要通过设计策略神经网络，利用行为克隆或强化学习对策略网络进行训练从而获得轨迹［8］，如文献［9］中利用非线性规划获得演示数据，进而搭建策略神经网络模型获取泊车轨迹。数值优化方法则是将自动泊车轨迹规划整理为车辆在环境约束和车辆自身特性约束下从起始位置行驶至目标位置满足某些评价指标的约束动态优化问题（dynamic optimization problem，DOP），采用数值计算方法求解并得到车辆最优运动轨迹［10］。

相较于其他几种方法，数值优化方法采用车辆运动学方程、约束条件和评价车辆行驶性能的指标来描述轨迹规划问题，得到的轨迹具有直观准确的特点［11］，因而备受关注。其中，伪谱法作为泊车轨迹数值优化方法中的一种，具有求解精度高、计算量少、结构简单、求解效率高等优点，得到众多学者的青睐。比如，文献［10］和文献［12］中基于最优化和动态优化理论，分别采用传统高斯伪谱法和hp 自适应伪谱法对自主泊车路径规划问题进行了求解。伪谱方法的思想是通过正交配点将轨迹规划DOP 问题离散化，并通过全局插值多项式逼近状态变量和控制变量，从而转化为非线性规划（nonlinear programming，NLP）问题再进行数值优化方法求解［13-14］。当前，Gauss 伪谱法在状态变量、控制变量和协调变量的近似精度更高、收敛速度更快，在处理初始和终端约束问题上具有优势，得到了广泛的应用［15-16］。然而，文献［17］中指出，全局Gauss 配点具有两端密集中间稀疏的特点，此种离散化配点方式对于泊车轨迹求解是不利的，特别是在稀疏配点的地方约束变化剧烈而导致无法求解。为了解决此问题，文献［18］中提出了一种在不同泊车空间采用分段高斯伪谱法的自主泊车路径规划方法，在不同阶段进行单独求解；文献［14］中则提出了两阶段配点重构来提升高斯伪谱法的求解能力；文献［19］中研究了伪谱拼接法对多阶段泊车进行求解并获得泊车路径规划。以上方法在分段多次求解的方式下可以获取不同车位大小的泊车轨迹。

为了进一步提升Gauss 伪谱法在自动泊车轨迹规划上的求解能力和轨迹规划精度，从本质上实现Gauss伪谱法阶段配点与整体求解，本文提出基于阶段约束下Gauss 配点伪谱法，用于平行车位自动泊车轨迹规划的整体求解。首先，在全面分析平行车位泊车过程的基础上，将泊车过程分为了泊车起动、接近车位、进入车位、姿态调整和泊车落位5 个阶段，并结合范围边界和圆弧理论建立相应的约束条件；以此为基础，结合车辆运动学方程建立了泊车轨迹DOP 数学模型；然后，将泊车轨迹在时间区间上划分为5段，并采用非均匀多Gauss配点对每个阶段进行离散，在易发生碰撞的区域增加配点数，在安全区域减少配点数，实现多阶段分区域配点，并进行整体求解，以此降低优化时间，进而提升轨迹优化精度；最后，在算法实现的基础上，进行仿真实验，在不同车位长度下泊车进行仿真测试，并与分区域高斯伪谱法全局优化方法对比以验证所提方法的有效性。

1 车辆自动泊车数学模型

1.1 车辆运动学模型

基于阿克曼转向原理［20］，结合图1 所示的车辆运动学示意图，建立车辆泊车动力学模型。首先，作如下假设：

图1 车辆运动学示意图

（1）车辆在行驶过程中车轮没有产生侧向力；

（2）车辆的侧向速度为0，车轮的侧偏角为0°。

图1 中：R(x，y)为车辆后轴中心点，m；L为车辆轴距，m；f和r分别为车辆前轴到车头距离和后轴到车位的距离，m；d为车辆宽度，m；φ(t)为前轮摆角，（°）；θ(t)为车身姿态角，（°）；A、B、C、D为车身4 个顶点，m。根据上述模型，可得车辆微分方程模型如下：

式中：v(t)为后轴中心点速度，m/s；a(t)为车辆加速度，m/s2；ω(t)为前轮转角角速度。同时，得到的A、B、C、D4点x坐标关系为

A、B、C、D4点y坐标关系为

1.2 自动泊车阶段约束

泊车运动本质上是一个避障运动，泊车路径要满足无碰撞发生且满足自身约束条件，结合车辆泊车过程，在文献［18］中泊车区域的基础上，本文将自动泊车过程划分为5个阶段。

（1）泊车起动阶段（N1段）

根据日常生活中的泊车经验，车辆一般在前车车辆附近开始泊车［21］。本文设定泊车起动阶段为车辆从起动泊车开始，车辆到达泊车车位右前端设定位置的过程如图2 所示。本文划定泊车起动阶段区域为Z1，即车辆从起始位置开始，短暂行驶一段距离后，车辆A、B、C、D4 点均处于Z1区域内。在此阶段中，包含车辆从较远起始位置到达规划的合适起始位置以及车辆在泊车起始位置开始起动两种情况。

图2 泊车起动阶段

图2 中：L1+L2和L3分别为泊车道路长度和泊车车位的长度，m；W1和W2分别为道路和泊车位的宽度，m。以车位左顶点为坐标原点，建立坐标系，并分析泊车起动阶段约束条件。在此阶段，主要是让车辆从起始的位置到达距离车位较近的位置，则A、B、C、D4 点均需要在车位上方区域Z1内运动，根据道路长度与车位长度，区域Z1表示范围为

式中：Z1(x)表示Z1区域内x坐标的取值；Z1(y)表示Z1区域内y坐标的取值。因此，对于泊车起动阶段的约束条件为

式 中Ax、Bx、Cx、Dx和Ay、By、Cy、Dy分别表示车辆A、B、C、D顶点的x坐标和y坐标。

（2）接近车位阶段（N2段）

接近车位阶段是指车辆完成泊车起动阶段后，车身进入泊车车位平行区域的过程，如图3 所示。此阶段中，车辆4 顶点A、B、C、D从Z1区域开始逐渐进入区域Z3。此外，在接近车位过程中需要保证车辆与车库右顶点P不发生碰撞。基于倒推的最小半径规划方法［22］，本文提出了结合点到直线距离以及圆弧理论的约束构建方法。

图3 接近车位阶段

在车辆泊车过程中，车辆以圆心O做逆时针圆弧运动，在圆弧运动中，车辆右侧车位C点不能与P点发生碰撞。接近车位阶段，车辆A、B、C、D4 点在自由大区域（定义为Z2）内运动，区域Z2由Z1和Z3两个区域组成，其中Z3表示范围为

然而，为了使泊车时车辆运动路径最短，减少所需要的泊车空间，在此区域内，本文根据泊车设定点P0(x0，y0)，取圆心坐标点为O(x0，-1)构建圆弧约束条件，如图4 所示。首先，车辆航向角即为BC边的斜角，根据截距式直线方程可得BC边表达式：

图4 接近车位约束条件

图5 进入车位阶段

式中Cx、Cy为C点的x、y坐标。

取O到BC边的垂直交点为E，则根据几何关系构建得到泊车路径约束：

①泊车轨迹满足|OE| ＞|OP|，即

式中：|OE|由点到直线距离公式构建；(Px，Py)为车库左顶点P点的x，y坐标。

②泊车轨迹满足车辆左侧顶点A的y坐标始终小于W1/2，即

此阶段结束后C、D点在Z3区域内。

（3）进入车位阶段（N3段）

进入车位阶段为车辆车身从泊车车位平行区域进入泊车车位的过程，如图 5 所示。进入车位阶段主要实现车辆从Z1到Z3的完全过渡，最终A、B、C、D4 点完全进入了Z3区域，在此阶段车辆仍在Z2大区域内运动。此阶段，同样需要防止车辆与车位右顶点碰撞，由于车辆做圆弧运动，因此进入车位阶段与接近车位阶段联合采用式（8）和式（9）约束条件。

（4）姿态调整阶段（N4段）

姿态调整阶段为车辆车身从泊车车位平行区域逐步调整姿态全部已进入泊车车位的过程，如图6所示。车辆从泊车区域Z3逐渐进入车位最终到达区域Z4内，因此只须确保车辆4 个顶点落在区域Z3内才不会发生碰撞。

图6 姿态调整阶段

因此，在此区域内，构建车辆A、B、C、D4顶点的约束条件为

（5）泊车落位阶段（N5段）

泊车落位阶段为车辆在泊车车位内通过姿态调整到达泊车目标点的过程，如图7 所示。在此阶段中，车辆A、B、C、D4 点需要在泊车目标区域Z4内，因而需要满足的约束条件为

图7 泊车落位阶段

此外，在上述5 阶段划分的基础上，车辆通常需要从预先设定的初始时刻状态到达指定的终端时刻状态，因而自动泊车轨迹规划过程还须满足初始和终端约束条件，同时还须满足自身物理约束，分别表示为

式中：vmax表示泊车时最大速度，m/s；amax表示泊车运动最大加速度，m/s2；φmax表示最大前轮摆角，（°）；ωmax表示最大前轮摆角角速度，（°）/s；t0表示泊车起始时刻，s；tf表示泊车终止时刻，s。

1.3 自动泊车轨迹规划DOP模型

结合模型分析后，定义车辆泊车过程状态向量x(t)=[x(t)，y(t)，θ(t)，v(t)，φ(t)]T，控制向量u(t)=[a(t)，ω(t)]T，则式（1）车辆自动泊车运动微分方程式可以简化为

式中f：R×Rn×Rm是关 于时 间t∈R、状 态向 量x(t) ∈Rn和控制向量u(t) ∈Rm的非线性函数，n和m分别表示状态向量和控制向量的维度。结合式（13），系统的初始状态值和终端状态值可以表示为

同时，根据5 阶段划分，得到阶段约束条件，将式（5）、式（8）～式（11）路径约束写成通用路径约束形式：

式（12）控制边界条件则表示为

此外，在车辆泊车过程中，通常存在各种各样的优化目标，如能耗最低，泊车时间最短等。本文主要对泊车时间最短进行研究，用终值项表示为

最终，自动泊车轨迹优化问题可以描述为如下动态优化问题。

DOP 问题（P1）：给定式（14）连续动态方程和式（15）状态初始条件，通过优化控制量u(t)使其在整个控制时域t内满足式（17）路径约束条件和式（18）边界约束，在到达式（16）指定终端约束的同时，式（19）目标函数为最优。

2 多阶段Gauss 配点参数化轨迹优化算法

通常，DOP问题的参数化优化求解思路为：将状态/控制变量进行离散，然后采用插值多项式逼近，进而将最优控制问题转化为离散点处状态、控制变量为待优化参量的NLP问题。

2.1 Legendre-Gauss配点插值逼近

本文采用Lagrange插值对泊车DOP的状态变量和控制变量进行离散逼近。首先，结合泊车的5 阶段划分，将整体泊车时间区间划分成5 段，初始时刻为t0、终端时刻为t5=tf，各阶段内部时间节点为t1，t2，t3，t4，整个区间划分为5 段，每一段的长度分别记 为n1，n2，…，n4+1，第m段对应的时间区间为[tm-1，tm] (m=1，2，...，5)，每段长度分别记为θ1，θ2，...，θ5，引入新的时间变量τ将控制时域进行时间尺度变换：

经过式（20）转换后，每个子区间的时间域转换为[-1，1]区间，而t∈[tm-1，tm]对应的时刻则可以利用以下公式进行反变换：

此时，问题P1 变换为基于时间域[-1，1]的多段最优控制问题（optimal control problem，OCP），其中式（14）微分方程转换为如下形式：

进一步，在时间尺度转换的基础上，本文采用逼近效果优良的Legendre 多项式在各段[-1，1]区间进行离散配点选择进而完成状态和控制变量的离散化。根据Legendre 正交多项式的性质可知，该多项式的零点是位于开区间(-1，1)内的单重实根，零点的计算可以由定理1 保证，证明过程请参阅文献［14］。

定理1.已知N次Legendre多项式如下：

该多项式的根可由以下Q矩阵的特征值求取：

此时，对于任意分段区间[tm-1，tm]可以得到Nm个位于开区间(-1，1) 内的离散点，通常称为Legendre Gauss（LG）配点。在获取离散配点的基础上，状态变量和控制变量采用Lagrange 插值方法进行逼近，记第m时间段[tm-1，tm]的LG 配点的数目为Km，则该段的状态向量xm(τ)和控制向量um(τ)的Lagrange插值逼近可以表示为

式中：Xm，i为状态向量在第m段初始时刻处或在LG配置点处的值；Um，i为控制向量在LG 配点处的值；Lm，i(τ)和为Lagrange插值多项式的基函数。

式中τm，i(i=1，…，Km)为第m段的第i个配置点，τm，0=-1。

2.2 DOP离散化

经Lagrange 插值离散逼近后，则可以对DOP问题进行离散化。对式（23）中状态变量对τ求导可得：

进而，联立式（25）和式（22）可得：

进一步定义：

并令Zm=[Hm，1...Hm，m]，代入式（26）后可以得到经LG配置点离散化后的非线性等式方程组：

式中：Xm=[Xm，1，…，Xm，Km]T∈R 是状态向量第m阶段Km个配置点处的值；Um=[Um，1，…，Um，Km]T∈R 是对应的离散控制参数；F(Xm，Um，τm，1，…，τm，Km)由式（30）求取。

最终，经过多阶段状态变量和控制变量LG配点离散多项式插值后，自动泊车轨迹规划DOP 问题被转化为如下NLP问题（P2）：

3 算法实现步骤

基于上述多阶段Gauss 配点离散参数化轨迹优化理论过程，给出本文提出算法的实现步骤，其优化策略流程如图8所示，具体实现步骤如下。

图8 基于阶段划分的多Gauss配点参数化优化算法流程图

（1）输入泊车车辆模型及车位环境信息；

（2）得到自动泊车轨迹规划DOP问题P1；

（3）设置分段阶段数m，选择每阶段LG 配点数Km；

（4）采用定理1 计算各阶段LG 配点，计算微分矩阵Hm；

（5）将各阶段状态量和控制量进行Lagrange 插值离散，得到式（29）离散化动力学方程非线性等式方程组；

（6）使用阶段划分多Gauss 配点参数化将问题（P1）离散化为NLP问题（P2）；

（7）设定优化参数初始值和求解精度，优化NLP求解器求解问题（P2）；

（8）得到状态变量和控制变量优化求解结果，采用时间尺度转换优化结果；

（9）输出优化结果。

4 仿真测试

以文献［18］中采用的某SUV 车辆为测试对象，选择泊车时间最短作为优化目标函数进行测试。车辆参数和车辆控制约束取值如表1 所示。取车辆后轴中心点位置为x0=9 m，y0=1.5 m；设定车位宽度W2为2.5 m，泊车道路长度L1长为6 m，L2长为14 m，泊车道路总长为 20 m，道路宽度中线宽度为3.5 m；道路边界宽度即W1为7 m；泊车车位长度L3设置为7.8、6.8、5.8 m 3 种不同长度进行测试。结合泊车过程5 阶段采用等间隔划分，自动泊车DOP问题现为多段动态优化问题，各阶段分别表示为N1，N2，…，N5。同时，测试初始值和终端约束值如表2 所示。本文仿真测试在配置为2.1 GHz AMD Ryzen 5 4600U with Radeon Graphics处理器和16 GB、3 200 MHz 内存的个人电脑MATLAB 2016a 平台上进行。

表1 车辆参数及控制约束

表2 车辆自动泊车轨迹优化边界值约束

由于国家道路标准平衡式车位长度为长6 m，宽2.4 m［23］，故而本文将7.8 和6.8 m 车位归类为长车位，而5.8 m 归类为窄车位。在长车位测试条件下，本文采用相同阶段配点进行测试，而在窄车位条件下，因为初始位置（9.0 m，1.5 m）对于窄车位来说较远，故采用将泊车起动阶段起始泊车点较远时规划至合适的设定位置（7.0 m，1.5 m）后再进行满足约束条件方式进行泊车，长、窄车位测试条件下的多阶段配点选择，如表3所示。

表3 长窄车位下各阶段配点数配置

此外，为了验证本文提出方法求解的有效性，将本文提出的阶段划分多Gauss 配点参数化优化算法与文献［18］中等提出的全局分区域高斯伪谱在同种车辆模型、同等车位约束下进行对比测试。表4 列出了本文所提出的方法与文献方法的优化结果对比。分析可知：在7.8 和6.8 m 长车位条件下，本文方法得到的泊车时间分别为8.879 和10.16 s，相较于分区域伪谱法平均减少2.07 s，泊车时间减少比例达17.9%；在5.8 m 窄车位条件下，本文方法得到的泊车时间为11.547 s，相较于分区域伪谱法减少4.79 s，泊车时间减少比例达29.3%。总体来讲，在3 种车位测试条件下，本文方法泊车时间平均减少2.976 s，泊车时间性能平均提升21.7%，展示出本文研究方法对于平行车位自动泊车轨迹规划的有效性。

表4 不同方法泊车轨迹优化结果对比

为了直观展示自动泊车轨迹规划效果，图9 给出了长车位下本文方法得到的泊车轨迹规划图。由图可知，在7.8 和6.8 m 长车位条件下，本文方法均得到了满足车位约束和道路约束的平滑泊车轨迹，并在较短时间内到达指定的泊车位置。同时，分析图9 可知，泊车车辆的车身均在车道中线以内，显示出本文方法泊车轨迹规划的效能。相应地，2 种长车位下得到的各阶段自动泊车状态和控制变量曲线如图10 所示。图中结果显示在不同车位下各状态和控制曲线趋势一致，无明显振动，各个子段均满足约束条件，在不同车位条件下能做出对应的优化调整，确保规划轨迹满足要求。此外，在7.8 m 车位条件下，本文方法规划得到前轮转向角在约束范围内且速度曲线均未超过零，表明此时不需要进行前后移动车辆即可满足泊车要求，同时速度曲线显示泊车过程速度调节较少；而在6.8 m 车位条件下，车辆仅在第4 阶段，即姿态调整阶段以微小的正速度对车辆进行调整。图9 和图10 结果显示出本文提出算法在长车位泊车条件下泊车规划的正确性。

图9 长车位下本文方法泊车轨迹规划图

图10 长车位下自动泊车状态和控制变量曲线

在窄车位（5.8 m 车位）条件下，考虑到车辆起始位置与车辆位置较远（与车位距离达3.2 m），在测试中将泊车起动阶段设置为（7.0 m，1.5 m），利用本文提出方法进行规划得到的轨迹、状态、控制曲线如图11所示。

图11 窄车位下本文方法泊车轨迹规划图

由图可知，在窄车位下本文方法依旧规划得到了平滑的泊车轨迹，车辆先进行平行移动，然后进行泊车入位，在泊车过程中均未与泊车边界发生碰撞并准确到达设定的终端位置，显示出本文方法对于窄车位泊车轨迹规划的适应性。

图12 给出了窄车位条件下得到的各阶自动泊车状态和控制变量曲线。图中航向角曲线与长车位条件下曲线轨迹趋势一致；在速度曲线方面，由于车位变窄，车辆可调整的空间变小，因而泊车过程中速度调整增加，在接近车位阶段，为了防止与边界发生碰撞，规划速度有明显的减速过程，这也是泊车时间相较于长车位增加的原因所在，但在远离边界后速度回升，实现快速泊车。图11和图12测试结果显示在窄车位条件下本文方法能够很好地规划得到泊车轨迹，并优化得到规划速度和转角曲线，进一步验证了本文提出方法的正确性和适应性。

图12 窄车位泊车轨迹以及状态变量曲线

5 结论

本文提出一种阶段约束下多Gauss 配点参数化自动泊车轨迹规划算法。在分析泊车动作过程的基础上将自动泊车划分成了5 个阶段并建立了对应的约束条件。根据泊车阶段划分，在高斯配点离散化逼近基础上，提出了多阶段局部高斯配点策略，以此提高泊车轨迹规划的精准度；在谱方法算法框架下，提出了多阶段Gauss 配点参数化轨迹优化算法，可以有效改善车辆泊车过程的轨迹规划精度，提升泊车效率。通过对通用小型汽车在长窄车位轨迹优化仿真测试发现，与等分区域全局优化伪谱法算法相比，改进方法得到的轨迹更平滑，泊车时间更短，泊车时间平均减少2.976 s，泊车时间性能平均提升达到21.7%，显示了所提出方法对车辆自动泊车的有效性和实用价值。下一步，将针对泊车过程时间自适应分配，开展实车验证工作，并在时间分配和配点配置中寻求最佳组合，不断提升泊车算法的效率。