[发明专利]一种基于全联立动态优化框架的自主泊车轨迹优化方法

摘要

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

主权项

1.一种基于全联立动态优化框架的自主泊车轨迹优化方法，其特征在于，包括以下步骤：(1)基于前轮驱动小车建立自主泊车过程车辆低速运动学模型，如式(1)所示；其中，(x,y)表示小车后轮轴中心点的坐标，v表示前轴中心点纵向速度，a表示前轴中心点纵向加速度，φ表示小车前轴中心点转向角；w表示小车前轴中心点转向角速度；θ表示车辆中心轴与水平方向的夹角；(2)根据具体泊车车型输入车体参数及车体运动中的物理极限约束，所述车体参数包括：车辆前后轮轴距L；前悬长度Lp；后悬长度Lr；所述车体运动中的物理极限约束包括：最高车速v_max、最大前轮转角φ_max、最大加速度a_max、最大角速度w_max等；其中，(3)确定待泊车位在泊车系统中的相对尺寸，以泊车位底部中点为原点，确定底部与泊车入口车位线间的距离y1、底部宽度pw、以及车位倾斜度la；平行泊车、垂直泊车的车位倾斜度为0，斜式车位的车位倾斜度从0°变化到90°，角度越大越倾斜；(4)建立基于MPCC的车位避障模型，步骤如下：自主泊车避障的条件约束如式(3)所示，含义是小车四角如果不在车位线上方则必然被夹在车位线和车位底部之间的平行线区域内：(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)：(4.2)将公式(3)中的条件约束转化为带有互补约束的MPCC可以处理的模型：通过引入非负辅助变量s^p,sⁿ,μ得到公式(5)：(4.2.1)将公式(5)中的模型转化为MPCC下的Reg(ε)模型：ε为任意小的正数，表征模型转化等价的精度，越小则越逼近原来的车位条件避障模型；ε₀是小车距离车位平行线边界的安全裕量；这种转化下优化目标不变仍为时间最短：min Tf；(4.2.2)也可以将公式(5)中模型转化为PF(ρ)模型：ρ为惩罚因子，这种转化通过改变目标函数简化了约束条件，惩罚因子越大优化模型的解越符合原来的车位避障约束；(4.3)MPCC技术转换的车位避障模型可以限制小车的四角在车位线之外，为了防止车位两拐角撞入车身，还需要增加一些约束；这里采用面积法进行判断：若车位拐点位于矩形小车之外，那么该点与矩形四角连成的三角形面积之和大于矩形的面积，否则二者面积相等；(5)由步骤(1)、(2)、(4)构造的以最短时间为优化目标的自主泊车轨迹优化命题如式(8)所示：当泊车避障约束选择MPCC‑Reg模型，即公式(6)时，γ＝0；当选择MPCC‑PF模型，即公式(7)时，γ＝ρ_1is^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)：τ₀＝0；τ₁＝0.155；τ₂＝0.645；τ₃＝1；  (9)K为插值阶次，选择K＝3，使离散化求解具有5阶精度；(x,y,θ)离散化如(10)：NE表示将优化时间分成的有限元段数，x_ij、y_ij、θ_ij分别表示第i个有限元第j个配置点上状态变量的值；状态变量的初值和终值条件为：由于状态变量可导，所以相邻有限元连接处的节点上状态变量值也应该连续，故有下面的连续性条件：控制变量为小车前轮轴中心纵向加速度和前轮转角角速度(a,w)，其Lagrange插值多项式如下：离散化后如(14)：对于控制变量不要求在有限元节点处的连续性；相比于其他插值方法，Lagrange插值多项式的优势在于变量在各个配置点上的值恰好等于其系数，即这样轨迹动态优化命题(8)离散化后的NLP命题形式如下：min Tf其中，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整理模型输出数据，绘制泊车轨迹曲线、车辆的相关变量曲线，包括后轴中心纵向速度‑时间、车身方向角‑时间、前轮转角‑时间、前轮角速度‑时间和后轴中心纵向加速度‑时间曲线。