基于分段高斯伪谱法的平行自主泊车路径规划*

钱立军，吴 冰，仇多洋，胡伟龙

（合肥工业大学汽车与交通工程学院，合肥 230009）

前言

随着辅助驾驶和无人驾驶技术的发展，汽车自动化、智能化程度在逐渐提高，其中自主泊车技术可以减小驾驶员在面对狭小空间泊车时的难度，同时也提高了驾驶员舒适性和泊车安全性。自主泊车技术研究包括3个方面：环境感知、路径规划和路径跟踪控制。路径规划作为其中一项重要技术，对此国内外学者已做大量研究，文献［1］和文献［2］中基于几何方法构造泊车路径，通过汽车从泊车位逆向驶出和起始泊车位置结合，以最小转弯半径的圆弧与其他半径的圆弧相切进行连接得到泊车路径，该方法较为简单实用，但得到的路径曲率不连续。文献［3］～文献［5］中分别采用回旋曲线、B样条曲线和反正切多项式曲线进行泊车路径规划，解决了几何路径曲率不连续的问题，但需要较大的车位空间，在面对狭小空间时无法完成路径规划。文献［6］和文献［7］中基于智能算法进行泊车路径规划和优化，将平行自主泊车过程分为3段，采用模糊控制方法设计泊车控制算法；以最短路径为目标，采用遗传算法对垂直泊车路径进行优化，可扩大泊车初始区域。文献［8］和文献［9］中基于最优化和动态优化理论，分别采用传统高斯伪谱法和联立动态优化法对自主泊车路径规划问题进行求解。

针对以上问题，本文中提出一种在不同泊车区域内采用分段高斯伪谱法求解的方法，在快速求解前提下以最短泊车完成时间为最优目标函数，建立车辆运动学模型、动力学约束、端点约束和路径约束，用最优控制问题描述自主泊车路径规划问题。通过分段高斯伪谱法先将最优控制问题进行离散化处理，根据泊车区域不同建立相应的约束模型，再采用内点法对非线性问题进行求解。对5种不同平行泊车工况路径规划进行仿真分析，并与传统伪谱法进行对比，最后通过实车试验验证了该方法得到的自主泊车路径的有效性。

1 车辆泊车运动过程分析

1.1 车辆运动学模型建立

车辆在自主泊车过程中车速较低，可忽略轮胎发生侧滑，根据阿克曼前轮转向原理，建立车辆自主泊车运动学模型，如图1所示。

图中，R（x，y）为车辆后轴中心点，v为后轴中心点速度且方向沿车辆轴线，O为车辆瞬时转弯圆心，L为车辆轴距，f和r分别为车辆的前轴到车头距离和后轴到车尾距离，d为车辆宽度，φ为前轮摆角，θ为车身姿态角，A，B，C，D分别为车身的4个顶点。

根据车辆运动学模型可得运动学微分方程如下：

图1 车辆运动学模型

式中：a为车辆加速度；ω为前轮摆角角速度。为了提高驾乘人员的舒适性，对车辆加速度变化率j进行了约束。

根据几何关系，由后轴中心点R（x，y）可得A，B，C，D的坐标值如下：

1.2 泊车避障约束分析

自主泊车过程可看作是车辆在道路和障碍车辆组成空间内的避障运动，泊车路径必须满足在泊车过程中无碰撞发生和自身约束条件。平行自主泊车区域如图2所示。图中Ls1＋Ls2和Lp分别为道路和泊车位的长度，Ws和Wp分别为道路和泊车位的宽度。

图2 平行自主泊车区域示意图

根据文献［10］对上述自主泊车空间划分为两个区域，分别为自由运动区域Zf和临近泊车位区域Zp，通过分别对两个空间建立不同边界约束，减少约束条件规模，提高自主泊车最优化控制问题的求解速度，两区域范围表示为

区域Zf和Zp构成了泊车区域，因此车辆4个顶点A，B，C，D在泊车过程中需要落在区域Zf和Zp中。

虽然上述约束可避免车辆顶点不与障碍物发生碰撞，但不能保证车身与障碍物之间不发生碰撞，车辆从区域Zf进入Zp时，如图2所示，车身BC边可能会与P点发生碰撞，若无碰撞发生，在车辆进入区域Zp之前，BC边需保持处于P点的左侧。由几何关系可得

2 泊车最优控制问题建立

2.1 最优控制问题

不失一般性，非线性最优控制Bolza问题可以描述为

式中的系统状态变量和控制变量分别为x和μ，且x（t）∈Rn，μ（t）∈Rm，t0和tf分别为系统初始时刻和终止时刻。min J为最优控制问题的目标函数，式（9）中第1式为系统状态微分方程，第2式为路径约束不等式方程，第3式为端点约束等式方程。

2.2 车辆动力学条件约束

车辆在运动过程中受到车辆自身物理条件约束，其对系统的部分状态变量和控制变量的约束为

式中：vmax为车辆运动的最大车速；amax为最大加速度；jmax为最大加速度变化率；φmax为最大前轮摆角；ωmax为最大前轮摆角角速度。

2.3 端点约束

基于上述的运动学模型和约束条件，车辆可以完成从起始时刻t0到终止时刻tf的无碰撞运动，本文中仅考虑在t0和tf时刻车辆为静止情况，起始时刻t0车辆后轴中心点处于指定位置（x0，y0），终止时刻tf车辆4个顶点均在泊车位空间内，在t0和tf时刻都要求车身水平，转向盘处于中间位置，即车身姿态角和转向盘转角均为0°，端点约束条件为

2.4 最优目标函数

自主泊车技术解决交通问题其中之一就是避免造成交通阻塞，帮助驾驶员快速完成车辆泊车入位，减少对车道的占用时间，因此本文中选用最短泊车完成时间min tf为最优控制问题的目标函数。

3 分段高斯伪谱法算法设计

高斯伪谱法（GPM）是一种直接求解最优化方法，该方法首先对连续的最优化问题进行离散化处理，再进行非线性规划问题求解［11-12］，但该方法为全局优化方法，随着问题约束数量增多和求解精度要求提高，需要增加大量配置点而导致计算时间急剧增加，甚至出现求解不收敛。本文中提出一种在不同泊车区域内采用分段高斯伪谱法求解的方法，并对区域Zf和区域Zp连接处的时刻条件进行松弛处理，提高最优控制问题求解收敛速率。

3.1 时域变换

式（8）中最优控制问题的时间取值范围为t∈［t0，tf］，将时间分成p段，即0＝t0＜t1…tq-1＜tq＜…tp＝tf，由此每段时间域的步长为

由于GPM中选取离散点的区间为τ∈［-1，1］，因此通过时域变换将第q段上时间变量转换为变量τ，变换如下：

由式（13）可得每段时间区域内的最优控制问题，其描述如下：

3.2 变量离散化

GPM是将控制变量和状态变量均需要在Legendre-Gauss（LG）配置点上进行离散化，再通过构建拉格朗日插值函数逼近控制变量和状态变量，在区间［τ0，τf］上，N阶的Legendre多项式为

以式（15）的零点和τ0组成N＋1个配置点，在每个t（q）时间域中，状态变量、控制变量由多项式近似表示为

式（16）中，Lagrange插值基函数为

由式（16）第1式求导，可将插值函数微分逼近状态变量的微分形式，得到在τk点的状态变量导数，可近似表示为

结合式（14）和式（18）可得微分代数等式约束：

离散后的路径约束和端点约束分别为

在自主泊车过程中需要保证路径曲率连续性，因此状态变量也必须为连续变化，即每段终端时刻状态变量作为下一段初始时刻状态变量，控制变量可以不连续，其连接处约束等式为

由于LG配置点中不包含终端时刻状态值，由式（22），根据Legendre-Gauss型求积公式计算终端时刻状态变量：

式中Ak和f（τk）分别为高斯积分权重系数和求积函数。

经过上述离散化过程，可将自主泊车路径规划的最优控制问题转化为非线性规划问题进行求解，可采用SQP、内点法等方法，本文中使用IPOPT［13］内点法工具箱进行求解。

3.3 改进的分段高斯伪谱法

在最优控制问题的约束数量较多时，传统伪谱法为了满足求解精度会增加大量配置点，这导致求解速度和收敛速度都会降低，因此，一方面可通过选取合适的配置点数和段数提高求解效率［14-15］，另一方面针对研究对象将最优控制问题分解为若干求解难度较低的子问题后再进行求解［16-17］。根据上文中的自主泊车避障约束分析，将泊车空间划分为区域Zf和区域Zp，由于两个区域连接点位置不确定，因此将此连接点处的状态参数和控制参数作为待优化的参数。

本文中提出一种改进的分段高斯伪谱法，首先对控制问题进行无最优目标全局求解，将求得最优解作为后续优化的初值，再分别在区域Zf和区域Zp内采用高斯伪谱法对自主泊车路径最优控制问题进行求解，减小求解过程中约束规模，提高算法收敛性，其算法描述见表1。

表1 算法描述

4 仿真分析

根据车辆自主泊车运动学模型，通过Matlab／Simulink搭建自主泊车仿真平台，以某SUV为仿真对象，其车辆参数和物理约束如表2所示。

本文中以平行自主泊车工况为例进行路径规划算法验证和仿真分析。为了验证算法统一有效性，针对5种不同长度车位选取同一泊车起始位置，选取后轴中心点位置为x0＝9 m；y0＝1.5 m，车位长度Lp分别为4.8～7.8 m，即分别为车身长度的1.1～1.8倍，车位宽度Wp为2.5 m，道路宽度Ws为3.5 m，车辆泊入车位的起始条件和终止条件如式（11）所示，仿真结果如图3～图12所示。

表2 车辆参数和物理约束

图3 L p＝7.8 m自主泊车示意图

通过上文的自主泊车示意图可知，在泊车的过程中，车辆与道路边界和前后车辆未发生碰撞，泊车结束时，车辆处于泊车位空间内且车身水平满足自主泊车路径的避障约束和端点约束条件，结果表明分段高斯伪谱法可对自主泊车路径规划最优控制问题进行求解并得到满足条件的泊车路径。

由图3和图5可知，在泊车空间较大工况下，车辆经过一次倒退和一次车位内向前微调完成泊车过程，由图4和图6可知，泊车路径较平滑且完成泊车用时较短，分别为11.363和11.816 s，状态变量、控制变量都满足约束条件。由图7、图9和图11可知，随着泊车空间逐渐减小，车辆在区域Zp内前后运动次数和泊车完成时间也逐渐增加，泊车用时分别为16.337，24.078和55.057 s，这也与驾驶员日常泊车过程类似，但从区域Zf到区域Zp的过程中路径依然较为平滑。由图8、图10和图12可知，车辆通过左右转动转向盘分别在泊车位内经过2，5和19次前后移动微调直至泊车完成，对比发现在泊车位长度小于车身长度1.5倍时，车辆在泊车位内的微调次数急剧增加。在图12中，轮胎摆角角速度最大绝对值为2.904 2 rad／s，其数值超过上文设定约束范围，表明车辆在狭小泊车位内进行微调时出现原地转向现象。

图4 L p＝7.8 m自主泊车仿真结果

图5 L p＝6.8 m自主泊车示意图

对上述5种不同长度泊车位平行泊车工况采用传统GPM方法进行求解，并与本文方法求解结果进行对比分析，两种不同高斯伪谱法的泊车完成用时最优目标tf和问题求解CPU计算耗时结果如图13所示，在泊车位长度为4.8和5.3 m时，采用传统GPM求解最优控制问题时未收敛，在泊车位较大时，两种方法都能够收敛，泊车完成用时tf较为接近，但分段GPM在计算耗时方面占优，分别为传统GPM的15.85%，17.69%和27.44%，这是由于本文算法选用合理初值，在两个泊车区域内分别建立约束条件，减小了约束条件数量规模和运算复杂度，提高了算法收敛性。

图6 L p＝6.8 m自主泊车仿真结果

图7 L p＝5.8 m自主泊车示意图

5 实车试验

试验车采用超声波传感器作为环境感知传感器，为自主泊车控制系统提供车位探测信息和车辆与周围障碍物距离信息。电动助力转向系统的角度传感器提供转向盘角度信号，制动系统的轮速传感器和加速度传感器分别提供轮速脉冲信号、纵向加速度信号和横摆角速度信号，自主泊车控制器对上述信号处理得到泊车目标路径信息，包括目标转向盘转角、目标车速和目标挡位等，通过转向系统、制动系统、动力系统和挡位系统分别实现对试验车的转向控制、车速控制和挡位控制，各系统之间信号采用CAN总线进行通信，如图14所示。

图8 L p＝5.8 m自主泊车仿真结果

图9 L p＝5.3 m自主泊车示意图

考虑到超声波传感器探测存在盲区，为了避免车辆发生碰撞，车辆在泊车位内有效运动范围比实际泊车位小，因此泊车路径规划时采用有效运动范围作为算法输入参数。本文中试验车上装配的短距超声波传感器盲区范围为0～0.15 m，所以规定有效车位长度比实测车位长度小0.3 m，试验中实际泊车位长度为在前述仿真中5种不同车位长度上分别增加0.3 m，泊车起始横向位置为9.15 m，其他参数设置与仿真工况相同。车辆在驶过车位时，通过侧向长距超声波传感器进行车位探测，行驶至起始泊车位置并保持车辆静止，自主泊车系统接管车辆，采用仿真得到的路径数据指导车辆进行泊车，实车试验如图15所示。

图10 L p＝5.3 m自主泊车仿真结果

图11 L p＝4.8 m自主泊车示意图

通过记录实测车位长度、泊车过程中车辆是否与障碍物发生碰撞，以及测量泊车完成后车辆外侧顶点A与D纵向位置和车身姿态角来判断泊车完成情况，试验结果如表3所示。

由试验结果可知，由于超声波传感器波束角的物理特性导致对车位的测量长度比实际长度小，将测量长度作为路径规划算法的输入参数也保证了泊车过程中的安全性。在泊车过程中车辆与障碍物未发生碰撞现象，泊车完成后，从顶点位置数据对比可知，随着车位长度逐渐减小，车辆在纵向上也逐渐靠近车位外侧，由于车位长度较小时车辆在泊车位内前后移动微调次数增多，在微调过程中车辆逐渐偏向车位外侧，但试验中完成泊车后，车辆外侧的顶点均落在车位内，车身姿态角在±1.5°范围以内，因此采用本文方法得到的路径信息可有效指导自主泊车完成。

图12 L p＝4.8 m自主泊车仿真结果

图13 两种方法求解结果对比图

图14 试验车自主泊车系统架构图

图15 自主泊车平行工况实车试验

表3 实车试验结果

6 结论

（1）本文中提出一种在不同泊车区域采用分段高斯伪谱法的自主泊车路径规划方法，通过建立车辆运动学模型、动力学约束、端点约束和路径避障约束，将泊车路径规划问题转换为最优控制问题，并以车辆泊车完成时间最短为最优目标函数，通过无最优目标求解得到问题合理初值，再根据不同泊车区域设置约束条件并进行求解。

（2）选取5种不同长度泊车位的平行自主泊车工况进行路径规划仿真，结果表明，所得路径满足自主泊车过程中约束条件，并与传统伪谱法进行对比分析表明，本文方法具有更快收敛速度。

（3）在实车试验中采用本文算法求解得到的路径信息指导车辆进行自主泊车，在泊车过程中无碰撞发生，车辆最终停在车位内。实践表明，本文的研究结果可为自主泊车提供有效参考路径。