CN105335797A - 一种基于全联立动态优化框架的自主泊车轨迹优化方法 - Google Patents

Abstract

本发明公开了一种基于全联立动态优化框架的自主泊车轨迹优化方法。该方法针对城市快节奏狭窄车位下最短时间安全泊车问题，提出了一种基于全联立求解策略的车辆-环境一体化建模的动态优化框架，有效消除了不同车位形状对轨迹规划策略造成的影响，可以优化出最短时间下的满足车辆低速泊车非完整约束的安全无碰轨迹。本发明的关键在于应用MPCC数学优化技术实现了车辆-车位避障一体化建模，同时可以对车辆运动学、动力学的相关指标进行优化。本方法能够直接获得车辆跟踪优化轨迹的速度、前轮转角、加速度、前轮角速度等操纵信息，便于实际辅助泊车。

Description

一种基于全联立动态优化框架的自主泊车轨迹优化方法

技术领域

本发明涉及无人驾驶系统中关键的自主泊车轨迹优化技术。

背景技术

今年来自主泊车(auto-parkingsystem，APS)技术发展迅猛，作为无人驾驶系统的重要组成部分，其目的是辅助或替代驾驶员完成车辆的安全泊位。随着车辆的增多，泊车位空间的紧缩，驾驶员泊车更加困难，因此自主泊车技术快速发展起来。

国内外学者主要用以下两种方法研究自主泊车：(1)基于模糊控制的方法：将技术成熟的驾驶员的泊车经验整理为模糊规则，通过控制汽车转向角和相对停车位的位置实现泊车。然而控制过程缺乏连贯的规划性，需要反复调整车速和转向角，前后挪动需要较大停车空间，难以量化。(2)基于路径规划的方法运用A*、D*等多种搜索算法寻找优化路径。例如Dubins提出的最小转向半径圆、单方向从任意起始位置运动到任意目标位置的最短路径规划等。由此发展而来的几何法成为目前实现障碍环境下泊车的主要方法，即通过分析泊车环境约束，结合最小转向半径圆和直线段得到分段连续的无碰路径。然而几何法在处理自主泊车问题时有其固有的局限性：(1)几何法下得到的无碰路径不一定满足小车的运动学约束和物理约束，所以不一定可行。(2)几何路径不包含与时间相关的信息，所以需要通过控制等方法进行再次规划，将无碰路径转换为带有时间信息的、车辆可跟踪的轨迹。(3)几何法只能得到最短泊车路径，而无法实施如泊车时间最短、油耗最少等目标函数下的优化。(4)几何法受限于车位形状和泊车轨形，例如在垂直泊车时设计的是1/4圆弧和直线段相接的轨形，在平行泊车中运用最小转向半径圆设计两段圆弧相接的S形路径，而不同的轨形又需要研究不同的转向策略。

发明内容

本发明的目的是针对现有技术的不足，在联立框架下研究自主泊车的最短时间轨迹优化问题。通过建立包括车辆与泊车环境在内的行车系统模型，构造最短时间动态优化命题，运用高性能非线性规划求解算法得到同时满足无碰、以及车辆物理约束的泊车轨迹。

本发明的目的是通过以下技术方案来实现的：一种基于全联立动态优化框架的自主泊车轨迹优化方法，包括以下步骤：

(1)基于前轮驱动小车建立自主泊车过程车辆低速运动学模型，如式(1)所示；

$\left\{\begin{array}{c}\frac{\mathrm{dx}}{\mathrm{dt}}=v\left(t\right)\cos \mathrm{\φ}\left(t\right)\cos \mathrm{\θ}\left(t\right)\\ \frac{\mathrm{dy}}{\mathrm{dt}}=v\left(t\right)\cos \mathrm{\φ}\left(t\right)\sin \mathrm{\θ}\left(t\right)\\ \frac{\mathrm{d\θ}}{\mathrm{dt}}=\frac{v\left(t\right)}{L}\sin \mathrm{\φ}\left(t\right)\\ \frac{\mathrm{dv}}{\mathrm{dt}}=a\left(t\right)\\ \frac{\mathrm{d\φ}}{\mathrm{dt}}=w\left(t\right)\end{array}\right.---\left(1\right)$

其中，(x，y)表示小车后轮轴中心点的坐标，v表示前轴中心点纵向速度，a表示前轴中心点纵向加速度，φ表示小车前轴中心点转向角；w表示小车前轴中心点转向角速度；θ表示车辆中心轴与水平方向的夹角；

(2)根据具体泊车车型输入车体参数及车体运动中的物理极限约束，所述车体参数包括：车辆前后轮轴距L；前悬长度Lp；后悬长度Lr；所述车体运动中的物理极限约束包括：最高车速v_max、最大前轮转角φ_max、最大加速度a_max、最大角速度w_max等；其中，

$\left\{\begin{array}{c}\left|v\left(t\right)\right|\≤v_{\max }\\ \left|a\left(t\right)\right|\≤a_{\max }\\ \left|\mathrm{\φ}\left(t\right)\right|\≤{\mathrm{\φ}}_{\max }\\ \left|w\left(t\right)\right|\≤w_{\max }\end{array}\right.---\left(2\right)$

(3)确定待泊车位在泊车系统中的相对尺寸，以泊车位底部中点为原点，确定底部与泊车入口车位线间的距离y1、底部宽度pw、以及车位倾斜度la。平行泊车、垂直泊车的车位倾斜度为0，斜式车位的车位倾斜度从0°变化到90°，角度越大越倾斜。

(4)建立基于MPCC(mathematicalprogramswithcomplementarityconstraints)的车位避障模型，步骤如下：

自主泊车避障的条件约束如式(3)所示，含义是小车四角如果不在车位线上方则必然被夹在车位线和车位底部之间的平行线区域内：

$\begin{array}{c}{y}_i\≥y_1\mathrm{or}{x}_{\mathrm{il}}\≤x_i\≤x_{\mathrm{iu}}\\ (x_{\mathrm{il}}=L1,x_{\mathrm{iu}}=L2)\\ \mathrm{i\; in}\left\{\mathrm{1,2,3,4}\right\}\end{array}---\left(3\right)$

(4.1)明确参数计算规则：

其中x_il，x_iu表示车位平行线区域的两边界，根据步骤(3)中的车位底部宽度和倾斜角度来确定。由步骤(3)中设定的地面坐标系可知：

(a)平行泊车模式下：L1＝-pw/2；L2＝pw/2

(b)垂直泊车模式下：L1＝-pw/2；L2＝pw/2

(c)斜式泊车模式下：L1＝cot(la)(x+pw/2)；L2＝cot(la)(x-pw/2)

i表示小车车身朝向X轴正方向时从右后轮逆时针环绕到左后轮的四角序号，根据步骤(2)中的车体参数L，Lp，Lr可以计算出小车任意位姿下的车身四角坐标，如公式(4)：

$\left\{\begin{array}{c}{x}_1\left(t\right)=x\left(t\right)-L_r\cos \mathrm{\θ}\left(t\right)+\frac{d}{2}\sin \mathrm{\θ}\left(t\right)\\ y_1\left(t\right)=y\left(t\right)-L_r\sin \mathrm{\θ}\left(t\right)-\frac{d}{2}\cos \mathrm{\θ}\left(t\right)\\ x_2\left(t\right)=x\left(t\right)+(L+L_P)\cos \mathrm{\θ}\left(t\right)+\frac{d}{2}\sin \mathrm{\θ}\left(t\right)\\ y_2\left(t\right)=y\left(t\right)+(L+L_P)\sin \mathrm{\θ}\left(t\right)-\frac{d}{2}\cos \mathrm{\θ}\left(t\right)\\ x_3\left(t\right)=x\left(t\right)+(L+L_P)\cos \mathrm{\θ}\left(t\right)-\frac{d}{2}\sin \mathrm{\θ}\left(t\right)\\ y_3\left(t\right)=y\left(t\right)+(L+L_P)\sin \mathrm{\θ}\left(t\right)+\frac{d}{2}\cos \mathrm{\θ}\left(t\right)\\ x_4\left(t\right)=x\left(t\right)-L_r\cos \mathrm{\θ}\left(t\right)-\frac{d}{2}\sin \mathrm{\θ}\left(t\right)\\ y_4\left(t\right)=y\left(t\right)-L_r\sin \mathrm{\θ}\left(t\right)+\frac{d}{2}\cos \mathrm{\θ}\left(t\right)\end{array}\right.---\left(4\right)$

(4.2)将公式(3)中的条件约束转化为带有互补约束的MPCC可以处理的模型：

通过引入非负辅助变量s^p，sⁿ，μ得到公式(5)：

$\begin{array}{c}{y}_i-y_1=s^{\mathrm{pi}}-s^{\mathrm{ni}}\\ s^{\mathrm{pi}}\≥0\⊥{\mathrm{\μ}}_i\≥0\\ s^{\mathrm{ni}}\≥0\⊥(1-{\mathrm{\μ}}_i)\≥0\\ {\mathrm{\μ}}_i(x_{\mathrm{il}}+{\mathrm{\ϵ}}_0)\≤{\mathrm{\μ}}_i{x}_i\≤{\mathrm{\μ}}_i(x_{\mathrm{iu}}-{\mathrm{\ϵ}}_0)\end{array}---\left(5\right)$

(4.2.1)将公式(5)中的模型转化为MPCC下的Reg(ε)模型：

$\mathrm{Reg}\left(\mathrm{\ϵ}\right):\⇒\left\{\begin{array}{c}{y}_i-y_1=s^{\mathrm{pi}}-s^{\mathrm{ni}}\\ s^{\mathrm{pi}}{\mathrm{\μ}}_i\≥0\\ s^{\mathrm{pi}}{\mathrm{\μ}}_i\≤\mathrm{\ϵ}\\ s^{\mathrm{ni}}(1-{\mathrm{\μ}}_i)\≥0\\ s^{\mathrm{ni}}(1-{\mathrm{\μ}}_i)\≤\mathrm{\ϵ}\\ {\mathrm{\μ}}_i(x_{\mathrm{il}}+{\mathrm{\ϵ}}_0)\≤{\mathrm{\μ}}_i{x}_i\≤{\mathrm{\μ}}_i(x_{\mathrm{iu}}-{\mathrm{\ϵ}}_0)\end{array}\right.---\left(6\right)$

ε为任意小的正数，表征模型转化等价的精度，越小则越逼近原来的车位条件避障模型。ε₀是小车距离车位平行线边界的安全裕量。这种转化下优化目标不变仍为时间最短：minTf。

(4.2.2)也可以将公式(5)中模型转化为PF(ρ)模型：

$\mathrm{PF}\left(\mathrm{\ρ}\right):\⇒\left\{\begin{array}{c}\min f(x,y,z)+{\mathrm{\ρ}}_{1i}{s}^{\mathrm{pi}}{\mathrm{\μ}}_i+{\mathrm{\ρ}}_{2i}{s}^{\mathrm{ni}}(1-{\mathrm{\μ}}_i)\\ y_i-y_1=s^{\mathrm{pi}}-s^{\mathrm{ni}}\\ s^{\mathrm{pi}}{\mathrm{\μ}}_i\≥0,s^{\mathrm{ni}}(1-{\mathrm{\μ}}_i)\≥0\\ {\mathrm{\μ}}_i(x_{\mathrm{il}}+{\mathrm{\ϵ}}_0)\≤{\mathrm{\μ}}_i{x}_i\≤{\mathrm{\μ}}_i(x_{\mathrm{iu}}-{\mathrm{\ϵ}}_0)\end{array}\right.---\left(7\right)$

ρ为惩罚因子，这种转化通过改变目标函数简化了约束条件，惩罚因子越大优化模型的解越符合原来的车位避障约束。

(4.3)MPCC技术转换的车位避障模型可以限制小车的四角在车位线之外，为了防止车位两拐角撞入车身，还需要增加一些约束。这里采用面积法进行判断：若车位拐点位于矩形小车之外，那么该点与矩形四角连成的三角形面积之和大于矩形的面积，否则二者面积相等。

(5)由步骤(1)、(2)、(4)构造的以最短时间为优化目标的自主泊车轨迹优化命题如式(8)所示：

当泊车避障约束选择MPCC-Reg模型，即公式(6)时，γ＝0；当选择MPCC-PF模型，即公式(7)时， $\mathrm{\γ}={\mathrm{\ρ}}_{1i}{s}^{\mathrm{pi}}{\mathrm{\μ}}_i+{\mathrm{\ρ}}_{2i}{s}^{\mathrm{ni}}(1-{\mathrm{\μ}}_i).$

确定待泊车的初始位姿参数(x₀，y₀，θ₀，v₀，φ₀)和终止位姿参数(x_tf，y_tf，θ_tf，v_tf，φ_tf)。

(6)对于步骤(5)中建立的轨迹优化命题的求解分为两步骤：

(6.1)离散化：采用全联立有限元正交配置的离散化方法：将步骤(1)中涉及的车辆运动学模型变量(x，y，θ，v，φ)通过选择基于Radau正交配置点的Lagrange插值函数进行离散化。其中(x，y，θ，v，φ)为模型的状态变量，插值函数构造如(9)：

$\begin{array}{c}{l}_j\left(\mathrm{\τ}\right)=\underset{k=0,\≠j}{\overset{K}{\mathrm{\Π}}}\frac{\left({\mathrm{\τ}-\mathrm{\τ}}_k\right)}{({\mathrm{\τ}}_j-{\mathrm{\τ}}_k)}\\ {\mathrm{\τ}}_0=0;{\mathrm{\τ}}_1=0.155;{\mathrm{\τ}}_2=0.645;{\mathrm{\τ}}_3=1;\end{array}---\left(9\right)$

K为插值阶次，本发明选择K＝3，使离散化求解具有5阶精度。(x，y，θ)离散化如(10)：

$\begin{array}{c}x\left(t\right)=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}{l}_j\left(\mathrm{\τ}\right)\\ y\left(t\right)=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}{l}_j\left(\mathrm{\τ}\right)\\ \mathrm{\θ}\left(t\right)=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{\mathrm{\θ}}_{\mathrm{ij}}{l}_j\left(\mathrm{\τ}\right)\mathrm{i\; in}1,..\mathrm{NE};\mathrm{j\; in}1..3\\ v\left(t\right)=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{v}_{\mathrm{ij}}{l}_j\left(\mathrm{\τ}\right)\\ \mathrm{\φ}\left(t\right)=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{\mathrm{\φ}}_{\mathrm{ij}}{l}_j\left(\mathrm{\τ}\right)\end{array}---\left(10\right)$

NE表示将优化时间分成的有限元段数，x_ij、y_ij、θ_ij分别表示第i个有限元第j个配置点上状态变量的值。状态变量的初值和终值条件为：

$\begin{array}{c}{x}_{\mathrm{1,0}}=x_0,x_{\mathrm{NE},K}=x_{\mathrm{tf}}\\ y_{\mathrm{1,0}}=y_0,y_{\mathrm{NE},K}=y_{\mathrm{tf}}\\ {\mathrm{\θ}}_{\mathrm{1,0}}={\mathrm{\θ}}_0,{\mathrm{\θ}}_{\mathrm{NE},K}={\mathrm{\θ}}_{\mathrm{tf}}\\ v_{\mathrm{1,0}}=v_0,v_{\mathrm{NE},K}=v_{\mathrm{tf}}\\ {\mathrm{\φ}}_{\mathrm{1,0}}={\mathrm{\φ}}_0,{\mathrm{\φ}}_{\mathrm{NE},K}={\mathrm{\φ}}_{\mathrm{tf}}\end{array}---\left(11\right)$

由于状态变量可导，所以相邻有限元连接处的节点上状态变量值也应该连续，故有下面的连续性条件：

$\begin{array}{c}{x}_{i+\mathrm{1,0}}=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{l}_j\left(1\right)x_{\mathrm{ij}},\mathrm{i\; in}1,..\mathrm{NE}-1\\ y_{i+\mathrm{1,0}}=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{l}_j\left(1\right)y_{\mathrm{ij}},\mathrm{i\; in}1,..\mathrm{NE}-1\\ {\mathrm{\θ}}_{i+\mathrm{1,0}}=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{l}_j\left(1\right){\mathrm{\θ}}_{\mathrm{ij}},\mathrm{i\; in}1,..\mathrm{NE}-1\\ v_{i+\mathrm{1,0}}=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{l}_j\left(1\right)v_{\mathrm{ij}},\mathrm{i\; in}1,..\mathrm{NE}-1\\ {\mathrm{\φ}}_{i+\mathrm{1,0}}=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{l}_j\left(1\right){\mathrm{\φ}}_{\mathrm{ij}},\mathrm{i\; in}1,..\mathrm{NE}-1\end{array}---\left(12\right)$

控制变量为小车前轮轴中心纵向加速度和前轮转角角速度(a，w)，其Lagrange插值多项式如下：

$\widetilde{l_j}\left(\mathrm{\τ}\right)=\underset{k=1,\≠j}{\overset{K}{\mathrm{\Π}}}\frac{\left({\mathrm{\τ}-\mathrm{\τ}}_k\right)}{({\mathrm{\τ}}_j-{\mathrm{\τ}}_k)}---\left(13\right)$

离散化后如(14)：

$\begin{array}{c}a\left(t\right)=\underset{j=1}{\overset{K}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}{\tilde{l}}_j\left(\mathrm{\τ}\right)\\ w\left(t\right)=\underset{j=1}{\overset{K}{\mathrm{\Σ}}}{w}_{\mathrm{ij}}{\tilde{l}}_j\left(\mathrm{\τ}\right)\end{array}---\left(14\right)$

对于控制变量不要求在有限元节点处的连续性。

相比于其他插值方法，Lagrange插值多项式的优势在于变量在各个配置点上的值恰好等于其系数，即

$\begin{array}{c}{t}_{\mathrm{ij}}=t_{i-1}+(t_i-t_{i-1}){\mathrm{\τ}}_j\\ x\left(t_{\mathrm{ij}}\right)=x_{\mathrm{ij}}\\ y\left(t_{\mathrm{ij}}\right)=y_{\mathrm{ij}}\\ \mathrm{\θ}\left(t_{\mathrm{ij}}\right)={\mathrm{\θ}}_{\mathrm{ij}}\\ v\left(t_{\mathrm{ij}}\right)=v_{\mathrm{ij}}\\ \mathrm{\φ}\left(t_{\mathrm{ij}}\right)={\mathrm{\φ}}_{\mathrm{ij}}\\ a\left(t_{\mathrm{ij}}\right)=a_{\mathrm{ij}}\\ w\left(t_{\mathrm{ij}}\right)=w_{\mathrm{ij}}\end{array}---\left(15\right)$

这样轨迹动态优化命题(8)离散化后的NLP命题形式如下：

(6.2)对于离散化后产生的大规模NLP问题(16)，调用基于内点法的求解器IPOPT来求解。一次性得到(x(t_i，j)，y(t_i，j)，θ(t_i，j)，v(t_i，j)，φ(t_i，j)，a(t_i，j)，w(t_i，j))泊车离散时间点上的小车后轴中心轨迹值、车身方向角值、前轮转角值、前轴中心纵向速度值及加速度值和前轮角速度值信息。

(7)步骤(6)求解完成后，用MATLAB整理模型输出数据，绘制泊车轨迹曲线、车辆的相关变量曲线，包括后轴中心纵向速度-时间、车身方向角-时间、前轮转角-时间、前轮角速度-时间和后轴中心纵向加速度-时间曲线。

本发明的有益效果是：

(1)对于不同车位下的自主泊车问题建立了统一描述框架，设计了最短时间的优化目标，便于多样化深入研究泊车问题。而主流的几何法受限于车位形状，不同的泊车模式下路径规划模型差别很大。

(2)高效的全联立求解算法一次性规划出小车轨迹及操作变量，为后续的控制跟踪做好了准备。而几何规划方法没有直接考虑小车的动力学参数、无法得到满足小车动力学性能指标的实时轨迹信息。

(3)在车位避障建模的优化技术中，R函数模型解决泊车问题有高效、鲁棒的优点。

附图说明

图1为本发明中建立的小车模型重要参数示意图；

图2为不同泊车模式下的示意图，(a)车位无倾斜，(b)为斜式车位；

图3为避障建模中需要增加面积法的意外情况；

图4为本发明的步骤流程图。

具体实施方式

下面结合附图和实施案例对本发明作进一步说明。

参照图4，本发明所描述的一种基于全联立动态优化框架的自主泊车轨迹优化方法，包括以下步骤：

1.建立自主泊车过程车辆低速运动学模型，也称为非完整约束模型。如图1，本发明基于前轮驱动小车建立其运动学微分方程模型如公式(1)所示。(x，y)为小车后轮轴中心点的坐标；v、a为前轴中心点纵向速度及加速度；φ、w为小车前轴中心点转向角(即前轮方向与车身方向的夹角)及角速度，这里假设两前轮转向角近似相等，所以上述模型又称作“自行车”模型；θ为车辆中心轴与水平方向的夹角；L为前后轮轴距，即车辆前后轮轴中心点连线的距离.

$\left\{\begin{array}{c}\frac{\mathrm{dx}}{\mathrm{dt}}=v\left(t\right)\cos \mathrm{\φ}\left(t\right)\cos \mathrm{\θ}\left(t\right)\\ \frac{\mathrm{dy}}{\mathrm{dt}}=v\left(t\right)\cos \mathrm{\φ}\left(t\right)\sin \mathrm{\θ}\left(t\right)\\ \frac{\mathrm{d\θ}}{\mathrm{dt}}=\frac{v\left(t\right)}{L}\sin \mathrm{\φ}\left(t\right)\\ \frac{\mathrm{dv}}{\mathrm{dt}}=a\left(t\right)\\ \frac{\mathrm{d\φ}}{\mathrm{dt}}=w\left(t\right)\end{array}\right.---\left(1\right)$

2.自主泊车系统除了考虑步骤1中小车的非完整约束之外，还要考虑车体运动中的物理极限约束，如最高车速v_max、最大前轮转角φ_max、最大加速度a_max、最大角速度w_max等。其中最大加速度表征实际中油门刹车的快慢，其值的大小选择要考虑驾驶员舒适性的需求。由瞬时曲率函数的一阶导数可知，角速度w(t)有界用以保障曲率的瞬时变化有界。本文借鉴实际行车情况对操作变量采用如下约束：

$\left\{\begin{array}{c}\left|v\left(t\right)\right|\≤v_{\max }\\ \left|a\left(t\right)\right|\≤a_{\max }\\ \left|\mathrm{\φ}\left(t\right)\right|\≤{\mathrm{\φ}}_{\max }\\ \left|w\left(t\right)\right|\≤w_{\max }\end{array}\right.---\left(2\right)$

3.根据图2泊车模式示意图确定待泊车位在泊车系统中的相对尺寸，以泊车位底部中点为原点，确定底部与泊车入口车位线间的距离y1、底部宽度pw、以及车位倾斜度la。平行泊车、垂直泊车的车位倾斜度为0，斜式车位的车位倾斜度从0°变化到90°，角度越大越倾斜。

4.车位避障建模，为了达到泊车过程无碰撞的目的，建立障碍环境模型，以及相应的避免碰撞的约束。

泊车位示意图见附图2。其中O为坐标原点，矩形框表示小车，框内小圆圈表示小车后轴中心点。不管是水平泊车位、垂直泊车位还是斜车位，都可以将无碰撞约束表达为如下条件语句：

其中(x_i，y_i)为小车四角坐标，i代表小车四角的序号，在图2中表示ACDB；ε₀为车位避障安全裕量，L1、L2对于平行/垂直车位或是斜式车位可分别代表常数或者线性函数。上面if-else条件需要转化为优化算法可处理的模型形式，参与泊车优化求解，具体如下：

4.1.一种转换方法被称作带有互补约束的数学规划技术，即MPCC(mathematicalprogramswithcomplementarityconstraints)，该技术在化工领域中用于结合非线性规划(NLP)方法处理条件模型、离散决策等问题。

MPCC的一般形式如下：

$\begin{array}{c}\min f(x,y,z)\\ s.t.h(x,y,z)=0\\ g(x,y,z)\≤0\\ 0\≤x\⊥y\≥0\end{array}---\left(4\right)$

其中互补约束⊥的含义为x＝0ory＝0orx＝0，y＝0。互补约束需要进一步转化才可以交给NLP算法处理。本发明选用了下面两种转化方法：

$\mathrm{Reg}\left(\mathrm{\ϵ}\right):0\≤x\⊥y\≥0\⇒\left\{\begin{array}{c}x,y\≥0\\ x\·y\≤\mathrm{\ϵ}\end{array}\right.---\left(5a\right)$

$\mathrm{PF}\left(\mathrm{\ρ}\right):0\≤x\⊥y\≥0\⇒\left\{\begin{array}{c}\min f(x,y,z)+{\mathrm{\ρx}}^{T}y\\ x,y\≥0\end{array}\right.---\left(5b\right)$

其中，Reg模型用一个较小的正数ε来控制条件建模转化的精度；而PF(penaltyfunction)模型在目标函数中加入惩罚项使得条件变量x、y尽量靠近边界约束，选择不同的罚因子ρ对求解效率的影响不同。

运用MPCC方法对自主泊车避障的条件约束(3)进行转化。首先条件约束(3)可以写作下面的逻辑表达式：

$\begin{array}{c}{y}_i\≥y_1\mathrm{or}{x}_{\mathrm{il}}\≤x_i\≤x_{\mathrm{iu}}\\ (x_{\mathrm{il}}=L1,x_{\mathrm{iu}}=L2)\\ \mathrm{i\; in}\{A,C,D,B\}\end{array}---\left(6\right)$

然后引入非负辅助变量s^p，sⁿ，μ将上式转换为MPCC可以处理的一般模型：

$\begin{array}{c}{y}_i-y_1=s^{\mathrm{pi}}-s^{\mathrm{ni}}\\ s^{\mathrm{pi}}\≥0\⊥{\mathrm{\μ}}_i\≥0\\ s^{\mathrm{ni}}\≥0\⊥(1-{\mathrm{\μ}}_i)\≥0\\ {\mathrm{\μ}}_i(x_{\mathrm{il}}+{\mathrm{\ϵ}}_0)\≤{\mathrm{\μ}}_i{x}_i\≤{\mathrm{\μ}}_i(x_{\mathrm{iu}}-{\mathrm{\ϵ}}_0)\end{array}---\left(7\right)$

可以证明这样的一种转化是合理的，例如：

$s^{\mathrm{pi}}>0\⇒{\mathrm{\μ}}_i=0\⇒(1-{\mathrm{\μ}}_i)>0\⇒s^{\mathrm{ni}}=0$

$\⇒y_i-y_1=s^{\mathrm{pi}}-s^{\mathrm{ni}}>0$

即小车四角都在车位线y₁上方，并且横坐标不受车位宽度的约束，此时小车可以在车位上方空间自由移动。

将(7)中的互补约束转化为Reg(ε)和PF(ρ)两种形式：

$\mathrm{Reg}\left(\mathrm{\ϵ}\right):\left(7\right)\⇒\left\{\begin{array}{c}{s}^{\mathrm{pi}}{\mathrm{\μ}}_i\≥0\\ s^{\mathrm{pi}}{\mathrm{\μ}}_i\≤\mathrm{\ϵ}\\ s^{\mathrm{ni}}(1-{\mathrm{\μ}}_i)\≥0\\ s^{\mathrm{ni}}(1-{\mathrm{\μ}}_i)\≤\mathrm{\ϵ}\end{array}\right.---\left(8\right)$

$\mathrm{PF}\left(\mathrm{\ρ}\right):\left(7\right)\⇒\left\{\begin{array}{c}\min f(x,y,z)+{\mathrm{\ρ}}_{1i}{s}^{\mathrm{pi}}{\mathrm{\μ}}_i+{\mathrm{\ρ}}_{2i}{s}^{\mathrm{ni}}(1-{\mathrm{\μ}}_i)\\ s^{\mathrm{pi}}{\mathrm{\μ}}_i\≥0,s^{\mathrm{ni}}(1-{\mathrm{\μ}}_i)\≥0\end{array}\right.---\left(9\right)$

4.2.步骤4.1中提到的MPCC技术转换的车位避障模型可以限制小车的四角在车位线之外，为了防止车位两拐角撞入车身如图3，还需要增加一些约束。这里采用面积法进行判断：车位拐点位于车体外面，那么该点与矩形四角连成的三角形面积之和大于矩形的面积，否则二者面积相等。

5.基于前面步骤建立的车辆模型、避障模型，构造泊车动态优化命题。

首先确定动态问题的初值条件，即初始时刻小车相对于车位坐标系的位姿和速度信息。参考车辆的运动学模型可以表示如下：

$\left\{\begin{array}{c}x\left(0\right)=x_0\\ y\left(0\right)=y_0\\ \mathrm{\θ}\left(0\right)={\mathrm{\θ}}_0\\ v\left(0\right)=v_0\\ \mathrm{\φ}\left(0\right)={\mathrm{\φ}}_0\end{array}\right.---\left(10\right)$

其次动态问题的终值条件，即泊车完成时的状态信息(如位移、车身方向、速度、前轮转角)，以及操作变量信息(如加速度和前轮转角速度)描述如下：

$\left\{\begin{array}{c}x\left(\mathrm{Tf}\right)=x_{\mathrm{tf}}\\ y\left(\mathrm{Tf}\right)=y_{\mathrm{tf}}\\ \mathrm{\θ}\left(\mathrm{Tf}\right)={\mathrm{\θ}}_{\mathrm{tf}}\\ v\left(\mathrm{Tf}\right)=v_{\mathrm{tf}}=0\\ \mathrm{\φ}\left(\mathrm{Tf}\right)={\mathrm{\φ}}_{\mathrm{tf}}=0\end{array}\right.---\left(11\right)$

上面最后两式规定小车停靠时前轮转角回正。系统根据小车四角和车位关系来确定小车终端位姿(x_tf，y_tf，θ_tf)。

由上述分析可以基于MPCC和R函数方法构造自主泊车的最短时间轨迹优化命题如下：

其中，当泊车避障约束选择MPCC-Reg模型或R函数模型时，γ＝0；当选择MPCC-PF模型时， $\mathrm{\γ}={\mathrm{\ρ}}_{1i}{s}^{\mathrm{pi}}{\mathrm{\μ}}_i+{\mathrm{\ρ}}_{2i}{s}^{\mathrm{ni}}(1-{\mathrm{\μ}}_i).$

6.步骤5中得到的轨迹优化问题的求解类似于如下一般形式的微分-代数方程(DAE)动态优化问题的求解：

其中z为优化问题的状态变量，y为代数变量，u为控制变量。它们都是时间的函数。上述动态问题比较复杂，通常得不到解析解，需要运用数值方法求解。此时必须将无限维的动态问题离散化，转化成有限维的问题来处理。离散化方法对动态问题的求解精度具有重要影响。

6.1.采用配置法离散化动态优化命题(13)：

配置法的思想是用配置点上的插值函数来逼近原问题，在配置点上无离散化误差。Lagrange插值函数常用于数值逼近，但高阶插值会导致龙格现象，因此分段低阶插值是常用方式。通过选择正交配置点可使Lagrange插值具有高阶代数精度。正交配置点的选择并不唯一，本发明选择基于Radau正交配置点的Lagrange插值函数逼近状态变量和控制变量的原函数。状态变量的Lagrange插值函数如下：

$l_j\left(\mathrm{\τ}\right)=\underset{k=0,\≠j}{\overset{K}{\mathrm{\Π}}}\frac{\left({\mathrm{\τ}-\mathrm{\τ}}_k\right)}{({\mathrm{\τ}}_j-{\mathrm{\τ}}_k)},z\left(t\right)=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{{z_{\mathrm{ij}}}^l}_j\left(\mathrm{\τ}\right)---\left(14\right)$

K为插值阶次，本文选择K＝3，使离散化求解具有5阶精度。z_ij为第i个有限元第j个配置点上状态变量的值。状态变量的初值和终值条件为：

z_1，0＝z⁰，z_f＝z_NE，K(15)

由于状态变量可导，所以相邻有限元连接处的节点上状态变量值也应该连续，故有下面的连续性条件：

$z_{i+\mathrm{1,0}}=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{l}_j\left(1\right)z_{\mathrm{ij}},\mathrm{i\; in}1,..\mathrm{NE}-1---\left(16\right)$

控制变量的Lagrange插值多项式如下：

$\widetilde{l_j}\left(\mathrm{\τ}\right)=\underset{k=1,\≠j}{\overset{K}{\mathrm{\Π}}}\frac{\left({\mathrm{\τ}-\mathrm{\τ}}_k\right)}{({\mathrm{\τ}}_j-{\mathrm{\τ}}_k)},u\left(t\right)=\underset{j=1}{\overset{K}{\mathrm{\Σ}}}{u}_{\mathrm{ij}}\widetilde{l_j}\left(\mathrm{\τ}\right)---\left(17\right)$

对于控制变量不要求在有限元节点处的连续性。

相比于其他插值方法，Lagrange插值多项式的优势在于变量在各个配置点上的值恰好等于其系数，即

$\begin{array}{c}{t}_{\mathrm{ij}}=t_{i-1}+(t_i-t_{i-1}){\mathrm{\τ}}_j\\ z\left(t_{\mathrm{ij}}\right)=z_{\mathrm{ij}},u\left(t_{\mathrm{ij}}\right)=u_{\mathrm{ij}}\end{array}---\left(18\right)$

这样动态优化问题(14)离散化后的NLP命题形式如下：

6.2.调用非线性求解器求解离散化后的命题(19)：

离散化后产生的大规模NLP问题求解方法主要有序列二次规划法(SQP)和内点法。目前应用广泛的SQP算法多是基于有效集方法实现，并在中小规模的优化求解中具有较好性能。但是随着问题规模的扩大，不等式约束的增多，对最优有效集的确定成为SQP算法的瓶颈。而内点法在求解包含不等式约束的NLP问题时，将边界约束作为障碍项加入到目标函数当中构造障碍问题。通过一系列的障碍问题求解来逼近原优化问题的解。从而在求解过程中避免了确定最优有效约束集的困难。

因此，内点法在求解含有大量不等式约束的优化问题时有明显的优势。目前，在学术界和工业界应用较多的内点法求解器包括KNITRO、LOQO、IPOPT等。本发明采用IPOPT求解离散化后的自主泊车优化命题(19)。

7.求解完成后，用MATLAB整理模型输出数据，绘制泊车轨迹曲线、车辆的相关变量曲线，包括后轴中心纵向速度-时间、车身方向角-时间、前轮转角-时间、前轮角速度-时间和后轴中心纵向加速度-时间曲线。

综上，本发明所述的一种基于全联立动态优化框架的自主泊车轨迹优化方法，能够在平行、垂直、斜式车位下优化出满足车辆运动学、动力学约束的最短时间泊车轨迹。采用不同的车位避障建模技术获得联立框架下的动态优化命题，运用高性能NLP求解算法得到无碰优化轨迹。

Claims (1)

1.一种基于全联立动态优化框架的自主泊车轨迹优化方法，其特征在于，包括以下步骤：

(1)基于前轮驱动小车建立自主泊车过程车辆低速运动学模型，如式(1)所示；

$\left\{\begin{array}{c}\frac{\mathrm{dx}}{\mathrm{dt}}=v\left(t\right)\cos \mathrm{\φ}\left(t\right)\cos \mathrm{\θ}\left(t\right)\\ \frac{\mathrm{dy}}{\mathrm{dt}}=v\left(t\right)\cos \mathrm{\φ}\left(t\right)\sin \mathrm{\θ}\left(t\right)\\ \frac{\mathrm{d\θ}}{\mathrm{dt}}=\frac{v\left(t\right)}{L}\sin \mathrm{\φ}\left(t\right)\\ \frac{\mathrm{dv}}{\mathrm{dt}}=a\left(t\right)\\ \frac{\mathrm{d\φ}}{\mathrm{dt}}=w\left(t\right)\end{array}\right.---\left(1\right)$

其中，(x，y)表示小车后轮轴中心点的坐标，v表示前轴中心点纵向速度，a表示前轴中心点纵向加速度，φ表示小车前轴中心点转向角；w表示小车前轴中心点转向角速度；θ表示车辆中心轴与水平方向的夹角；

(2)根据具体泊车车型输入车体参数及车体运动中的物理极限约束，所述车体参数包括：车辆前后轮轴距L；前悬长度Lp；后悬长度Lr；所述车体运动中的物理极限约束包括：最高车速v_max、最大前轮转角φ_max、最大加速度a_max、最大角速度w_max等；其中，

$\left\{\begin{array}{c}\left|v\left(t\right)\right|\≤v_{\max }\\ \left|a\left(t\right)\right|\≤a_{\max }\\ \left|\mathrm{\φ}\left(t\right)\right|\≤{\mathrm{\φ}}_{\max }\\ \left|w\left(t\right)\right|\≤w_{\max }\end{array}---\left(2\right)\right.$

(3)确定待泊车位在泊车系统中的相对尺寸，以泊车位底部中点为原点，确定底部与泊车入口车位线间的距离y1、底部宽度pw、以及车位倾斜度1a。平行泊车、垂直泊车的车位倾斜度为0，斜式车位的车位倾斜度从0°变化到90°，角度越大越倾斜。

(4)建立基于MPCC的车位避障模型，步骤如下：

自主泊车避障的条件约束如式(3)所示，含义是小车四角如果不在车位线上方则必然被夹在车位线和车位底部之间的平行线区域内：

$\begin{array}{c}\begin{array}{ccc}{y}_i\≥y_1& \mathrm{or}& x_{\mathrm{il}}\≤x_i\≤x_{\mathrm{iu}}\end{array}\\ (x_{\mathrm{il}}=L1,x_{\mathrm{iu}=L2})\\ \begin{array}{ccc}i& \mathrm{in}& \left\{\mathrm{1,2,3,4}\right\}\end{array}\end{array}---\left(3\right)$

(4.1)明确参数计算规则：

其中x_il，x_iu表示车位平行线区域的两边界，根据步骤(3)中的车位底部宽度和倾斜角度来确定。由步骤(3)中设定的地面坐标系可知：

(a)平行泊车模式下：L1＝-pw/2；L2＝pw/2

(b)垂直泊车模式下：L1＝-pw/2；L2＝pw/2

(c)斜式泊车模式下：L1＝cot(1a)(x+pw/2)；L2＝cot(1a)(x-pw/2)

i表示小车车身朝向X轴正方向时从右后轮逆时针环绕到左后轮的四角序号，根据步骤(2)中的车体参数L，Lp，Lr可以计算出小车任意位姿下的车身四角坐标，如公式(4)：

$\left\{\begin{array}{c}{x}_1\left(t\right)=x\left(t\right)-L_r\cos \mathrm{\θ}\left(t\right)+\frac{d}{2}\sin \left(t\right)\\ y_1\left(t\right)=y\left(t\right)-L_r\sin \mathrm{\θ}\left(t\right)-\frac{d}{2}\cos \mathrm{\θ}\left(t\right)\\ x_2\left(t\right)=x\left(t\right)+(L+L_P)\cos \mathrm{\θ}\left(t\right)+\frac{d}{2}\sin \mathrm{\θ}\left(t\right)\\ y_2\left(t\right)=y\left(t\right)+(L+L_P)\sin \mathrm{\θ}\left(t\right)-\frac{d}{2}\cos \mathrm{\θ}\left(t\right)\\ x_3\left(t\right)=x\left(t\right)+(L+L_P)\cos \mathrm{\θ}\left(t\right)-\frac{d}{2}\sin \mathrm{\θ}\left(t\right)\\ y_3\left(t\right)=y\left(t\right)+(L+L_P)\sin \mathrm{\θ}\left(t\right)-\frac{d}{2}\mathrm{cis\θ}\left(t\right)\\ x_4\left(t\right)=x\left(t\right)-L_r\cos \mathrm{\θ}\left(t\right)-\frac{d}{2}\sin \mathrm{\θ}\left(t\right)\\ y_4\left(t\right)=y\left(t\right)-L_r\sin \mathrm{\θ}\left(t\right)+\frac{d}{2}\cos \mathrm{\θ}\left(t\right)\end{array}\right.---\left(4\right)$

(4.2)将公式(3)中的条件约束转化为带有互补约束的MPCC可以处理的模型：

通过引入非负辅助变量s^p，sⁿ，μ得到公式(5)：

$\begin{array}{c}{y}_i-y_1=s^{\mathrm{pi}}-s^{\mathrm{ni}}\\ s^{\mathrm{pi}}\≥0\⊥{\mathrm{\μ}}_i\≥0\\ s^{\mathrm{ni}}\≥0\⊥(1-{\mathrm{\μ}}_i)\≥0\\ {\mathrm{\μ}}_i(x_{\mathrm{il}}+{\mathrm{\ϵ}}_0)\≤{\mathrm{\μ}}_i{x}_i\≤{\mathrm{\μ}}_i(x_{\mathrm{iu}}-{\mathrm{\ϵ}}_0)\end{array}---\left(5\right)$

(4.2.1)将公式(5)中的模型转化为MPCC下的Reg(ε)模型：

$\mathrm{Reg}\left(\mathrm{\ϵ}\right):\⇒\left\{\begin{array}{c}{y}_i-y_1=s^{\mathrm{pi}}-s^{\mathrm{ni}}\\ s^{\mathrm{pi}}{\mathrm{\μ}}_i\≥0\\ s^{\mathrm{pi}}{\mathrm{\μ}}_i\≤\mathrm{\ϵ}\\ s^{\mathrm{ni}}(1-{\mathrm{\μ}}_i)\≥0\\ s^{\mathrm{ni}}(1-{\mathrm{\μ}}_i)\≤\mathrm{\ϵ}\\ {\mathrm{\μ}}_i(x_{\mathrm{il}}+{\mathrm{\ϵ}}_0)\≤{\mathrm{\μ}}_i{x}_i\≤{\mathrm{\μ}}_i(x_{\mathrm{iu}}-{\mathrm{\ϵ}}_0)\end{array}\right.---\left(6\right)$

ε为任意小的正数，表征模型转化等价的精度，越小则越逼近原来的车位条件避障模型。ε₀是小车距离车位平行线边界的安全裕量。这种转化下优化目标不变仍为时间最短：minTf。

(4.2.2)也可以将公式(5)中模型转化为PF(ρ)模型：

$\mathrm{PF}\left(\mathrm{\ρ}\right):\⇒\left\{\begin{array}{c}\min f(x,y,z)+{\mathrm{\ρ}}_{1i}{s}^{\mathrm{pi}}{\mathrm{\μ}}_i+{\mathrm{\ρ}}_{2i}{s}^{\mathrm{ni}}(1-{\mathrm{\μ}}_i)\\ y_i-y_1=s^{\mathrm{pi}}-s^{\mathrm{ni}}\\ s^{\mathrm{pi}}{\mathrm{\μ}}_i\≥0,s^{\mathrm{ni}}(1-{\mathrm{\μ}}_i)\≥0\\ {\mathrm{\μ}}_i(x_{\mathrm{il}}+{\mathrm{\ϵ}}_0)\≤{\mathrm{\μ}}_i{x}_i\≤{\mathrm{\μ}}_i(x_{\mathrm{iu}}-{\mathrm{\ϵ}}_0)\end{array}\right.---\left(7\right)$

ρ为惩罚因子，这种转化通过改变目标函数简化了约束条件，惩罚因子越大优化模型的解越符合原来的车位避障约束。

(4.3)MPCC技术转换的车位避障模型可以限制小车的四角在车位线之外，为了防止车位两拐角撞入车身，还需要增加一些约束。这里采用面积法进行判断：若车位拐点位于矩形小车之外，那么该点与矩形四角连成的三角形面积之和大于矩形的面积，否则二者面积相等。

(5)由步骤(1)、(2)、(4)构造的以最短时间为优化目标的自主泊车轨迹优化命题如式(8)所示：

当泊车避障约束选择MPCC-Reg模型，即公式(6)时，γ＝0；当选择MPCC-PF模型，即公式(7)时，γ＝ρ_lis^piμ_i+ρ_2isⁿⁱ(1-μ_i)。

确定待泊车的初始位姿参数(x₀，y₀，θ₀，v₀，φ₀)和终止位姿参数(x_tf，y_tf，θ_tf，v_tf，φ_tf)。

(6)对于步骤(5)中建立的轨迹优化命题的求解分为两步骤：

(6.1)离散化：采用全联立有限元正交配置的离散化方法：将步骤(1)中涉及的车辆运动学模型变量(x，y，θ，v，φ)通过选择基于Radau正交配置点的Lagrange插值函数进行离散化。其中(x，y，θ，v，φ)为模型的状态变量，插值函数构造如(9)：

$\begin{array}{c}{l}_j\left(\mathrm{\τ}\right)=\underset{k=0,\≠j}{\overset{K}{\mathrm{\Π}}}\frac{(\mathrm{\τ}-{\mathrm{\τ}}_k)}{({\mathrm{\τ}}_j-{\mathrm{\τ}}_k)}\\ {\mathrm{\τ}}_0=0;{\mathrm{\τ}}_1=0.155;{\mathrm{\τ}}_2=0.645;{\mathrm{\τ}}_3=1;\end{array}---\left(9\right)$

K为插值阶次，本发明选择K＝3，使离散化求解具有5阶精度。(x，y，θ)离散化如(10)：

$\begin{array}{c}x\left(t\right)=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}{l}_j\left(\mathrm{\τ}\right)\\ y\left(t\right)=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}{l}_j\left(\mathrm{\τ}\right)\\ \mathrm{\θ}\left(t\right)=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{\mathrm{\θ}}_{\mathrm{ij}}{l}_j\left(\mathrm{\τ}\right)\\ v\left(t\right)=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{v}_{\mathrm{ij}}{l}_j\left(\mathrm{\τ}\right)\\ \mathrm{\φ}\left(t\right)=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{\mathrm{\φ}}_{\mathrm{ij}}{l}_j\left(\mathrm{\τ}\right)\end{array}\mathrm{i\; in}1,..\mathrm{NE};\mathrm{j\; in}1..3---\left(10\right)$

NE表示将优化时间分成的有限元段数，x_ij、y_ij、θ_ij分别表示第i个有限元第j个配置点上状态变量的值。状态变量的初值和终值条件为：

$\begin{array}{c}{x}_{\mathrm{1,0}}=x_0,x_{\mathrm{NE},K}=x_{\mathrm{tf}}\\ y_{\mathrm{1,0}}=y_0,y_{\mathrm{NE},K}=y_{\mathrm{tf}}\\ {\mathrm{\θ}}_{\mathrm{1,0}}={\mathrm{\θ}}_0,{\mathrm{\θ}}_{\mathrm{NE},K}=v_{\mathrm{tf}}\\ v_{\mathrm{1,0}}=v_0,v_{\mathrm{NE},K}=v_{\mathrm{tf}}\\ {\mathrm{\φ}}_{\mathrm{1,0}}={\mathrm{\φ}}_0,{\mathrm{\φ}}_{\mathrm{NE},K}={\mathrm{\φ}}_{\mathrm{tf}}\end{array}---\left(11\right)$

由于状态变量可导，所以相邻有限元连接处的节点上状态变量值也应该连续，故有下面的连续性条件：

$\begin{array}{c}{x}_{i+\mathrm{1,0}}=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{l}_j\left(1\right)x_{\mathrm{ij}},\mathrm{i\; in}1,..\mathrm{NE}-1\\ y_{i+\mathrm{1,0}}=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{l}_j\left(1\right)y_{\mathrm{ij}},\mathrm{i\; in}1,..\mathrm{NE}-1\\ {\mathrm{\θ}}_{i+\mathrm{1,0}}=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{l}_j\left(1\right){\mathrm{\θ}}_{\mathrm{ij}},\mathrm{i\; in}1,..\mathrm{NE}-1\\ v_{i+\mathrm{1,0}}=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{l}_j\left(1\right)v_{\mathrm{ij}},\mathrm{i\; in}1,..\mathrm{NE}-1\\ {\mathrm{\φ}}_{i+\mathrm{1,0}}=\underset{j=0}{\overset{K}{\mathrm{\Σ}}}{l}_j\left(1\right){\mathrm{\φ}}_{\mathrm{ij}},\mathrm{i\; in}1,..\mathrm{NE}-1\end{array}---\left(12\right)$

控制变量为小车前轮轴中心纵向加速度和前轮转角角速度(a，w)，其Lagrange插值多项式如下：

${\tilde{l}}_j\left(\mathrm{\τ}\right)=\underset{k=1,\≠j}{\overset{K}{\mathrm{\Π}}}\frac{(\mathrm{\τ}-{\mathrm{\τ}}_k)}{({\mathrm{\τ}}_j-{\mathrm{\τ}}_k)}---\left(13\right)$

离散化后如(14)：

$\begin{array}{c}a\left(t\right)=\underset{j=1}{\overset{K}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}{\tilde{l}}_j\left(\mathrm{\τ}\right)\\ w\left(t\right)=\underset{j=1}{\overset{K}{\mathrm{\Σ}}}{w}_{\mathrm{ij}}{\tilde{l}}_j\left(\mathrm{\τ}\right)\end{array}---\left(14\right)$

对于控制变量不要求在有限元节点处的连续性。

相比于其他插值方法，Lagrange插值多项式的优势在于变量在各个配置点上的值恰好等于其系数，即

$\begin{array}{c}{t}_{\mathrm{ij}}=t_{i-1}+(t_i-t_{i-1}){\mathrm{\τ}}_j\\ x\left(t_{\mathrm{ij}}\right)=x_{\mathrm{ij}}\\ y\left(t_{\mathrm{ij}}\right)=y_{\mathrm{ij}}\\ \mathrm{\θ}\left(t_{\mathrm{ij}}\right)={\mathrm{\θ}}_{\mathrm{ij}}\\ v\left(t_{\mathrm{ij}}\right)=v_{\mathrm{ij}}\\ \mathrm{\φ}\left(t_{\mathrm{ij}}\right)={\mathrm{\φ}}_{\mathrm{ij}}\\ a\left(t_{\mathrm{ij}}\right)=a_{\mathrm{ij}}\\ w\left(t_{\mathrm{ij}}\right)=w_{\mathrm{ij}}\end{array}---\left(15\right)$

这样轨迹动态优化命题(8)离散化后的NLP命题形式如下：

$\begin{array}{c}\begin{array}{cc}\min & \mathrm{Tf}\end{array}\\ s.t.\left\{\begin{array}{c}\underset{k=0}{\overset{K}{\mathrm{\Σ}}}{l}_k\left({\mathrm{\τ}}_j\right)x_{\mathrm{ik}}-h_i{v}_{\mathrm{ik}}\stackrel{.}{\cos }\left({\mathrm{\φ}}_{\mathrm{ik}}\right)\cos \left({\mathrm{\θ}}_{\mathrm{ik}}\right)=0\\ \underset{k=0}{\overset{K}{\mathrm{\Σ}}}{l}_k\left({\mathrm{\τ}}_j\right)y_{\mathrm{ik}}-h_i{v}_{\mathrm{ik}}\stackrel{.}{\cos }\left({\mathrm{\φ}}_{\mathrm{ik}}\right)\sin \left({\mathrm{\θ}}_{\mathrm{ik}}\right)=0\\ \underset{k=0}{\overset{K}{\mathrm{\Σ}}}{l}_k\left({\mathrm{\τ}}_j\right){\mathrm{\θ}}_{\mathrm{ik}}-h_i\stackrel{.}{v_{\mathrm{ik}}}\sin \left({\mathrm{\φ}}_{\mathrm{ik}}\right)/L=0\\ \underset{k=0}{\overset{K}{\mathrm{\Σ}}}{l}_k\left({\mathrm{\τ}}_j\right){v_{\mathrm{ik}}}^{.}-h_i{a}_{\mathrm{ik}}=0\\ \underset{k=0}{\overset{K}{\mathrm{\Σ}}}{l}_k\left({\mathrm{\τ}}_j\right){{\mathrm{\φ}}_{\mathrm{ik}}}^{.}-h_i{w}_{\mathrm{ik}}=0\\ C(x_{\mathrm{ij}},y_{\mathrm{ij}},{\mathrm{\θ}}_{\mathrm{ij}},v_{\mathrm{ij}},{\mathrm{\φ}}_{\mathrm{ij}})=0\\ G(x_{\mathrm{ij}},y_{\mathrm{ij}},{\mathrm{\θ}}_{\mathrm{ij}},v_{\mathrm{ij}},{\mathrm{\φ}}_{\mathrm{ij}})\≤0\\ z\left(0\right)=z^0\\ z\left(\mathrm{Tf}\right)=z_{\mathrm{tf}}\\ t\∈[0,\mathrm{Tf}],\mathrm{i\; in}1..\mathrm{NE};\mathrm{j\; in}1..K\end{array}\right.\end{array}$

其中，C表示避障等式约束的离散化表示，G表示小车物理约束及避障不等式约束的离散化。

(6.2)对于离散化后产生的大规模NLP问题(16)，调用基于内点法的求解器IPOPT来求解。一次性得到(x(t_i，j)，y(t_i，j)，θ(t_i，j)，v(t_i，j)，φ(t_i，j)，a(t_i，j)，w(t_i，j))泊车离散时间点上的小车后轴中心轨迹值、车身方向角值、前轮转角值、前轴中心纵向速度值及加速度值和前轮角速度值信息。

(7)步骤(6)求解完成后，用MATLAB整理模型输出数据，绘制泊车轨迹曲线、车辆的相关变量曲线，包括后轴中心纵向速度-时间、车身方向角-时间、前轮转角-时间、前轮角速度-时间和后轴中心纵向加速度-时间曲线。