[发明专利]一体式平行泊车轨迹规划和跟踪控制方法及系统

权利要求书

1.一种一体式平行泊车轨迹规划和跟踪控制方法，其特征在于，包括：

搭建车辆运动学模型；

基于所述车辆运动学模型，设计预测模型和泊车系统的约束；

设计代价函数，将车辆的速度规划、路径规划和跟踪控制整合为一个优化问题，得到服从于所述预测模型和所述约束的控制问题描述；

采用内点法求解所述控制问题，得到最优开环控制序列，选取所述最优开环控制序列中的第一组元素应用于待控制的车辆，实现自动平行泊车。

2.如权利要求1所述的一体式平行泊车轨迹规划和跟踪控制方法，其特征在于，所述车辆运动学模型的表达式为：

其中，v为车辆的运动速度，和分别为车辆在X和Y方向上的速度分量，θ为车辆的航向角，δ为车辆的转向轮等效转角，l为车辆的轴距。

3.如权利要求2所述的一体式平行泊车轨迹规划和跟踪控制方法，其特征在于，基于所述车辆运动学模型，设计预测模型和泊车系统的约束，包括：

将式(1)向量化，得到时域预测模型：

其中，ξ＝[x y θ]^T为状态变量，u＝[v δ]^T为控制输入，(t)表示时域；

采用式(4)，将所述时域预测模型转化空间域预测模型：

其中，(s)表示空间域，s和分别为车辆行驶过的路程及其变化率；

得到空间域预测模型如下：

将所述空间域预测模型离散化，得到离散化的预测模型：

其中，(k_s+1)表示相对当前采样点位置(k_s)下一个步长对应的位置；

将离散化的预测模型转化为增量预测模型：

其中，Δu(k_s)＝u(k_s)-u(k_s-1)，(k_s)表示当前采样位置，(k_s-1)表示前一采样点的位置；

假设预测时域为N_p，根据模型预测控制理论，将式(7)所示的增量预测模型在预测时域N_p内的预测方程表示为：

设计泊车系统的约束，以对终止时刻车辆的速度和车身姿态、泊车过程中车辆的横向位置和纵向位置，以及执行器进行约束。

4.如权利要求3所述的一体式平行泊车轨迹规划和跟踪控制方法，其特征在于，将所述空间域预测模型离散化，包括：

采用四阶龙格库塔方法，以离散步长h＝0.1，对式(5)进行离散化。

5.如权利要求3或4所述的一体式平行泊车轨迹规划和跟踪控制方法，其特征在于，设计泊车系统的约束，以对终止时刻车辆的速度和车身姿态、泊车过程中车辆的横向位置和纵向位置，以及执行器进行约束，包括：

设计泊车目标位置和姿态约束，对终止时刻车辆速度和车身姿态进行约束：

其中，(x_ref,y_ref)为泊车目标点的位置；(x_r,y_r)为车辆后轴的中点；

设计安全约束，对泊车过程中车辆的横向位置和纵向位置进行约束：

其中，X_left和X_right分别为库位的左右边界位置，a为前悬长度与轴距以及车头安全裕度之和，b为后悬长度与车尾安全裕度之和，Y_bound为车库的侧边界，w为车辆的宽度；

设计执行器约束，对执行器进行约束：

其中，Δv_min和Δv_max是设定的速度增量输入的上、下限，Δδ_min和Δδ_max是设定的前轮转向角变化量输入的上、下限，δ_min和δ_max是前轮转角的上、下限。