本发明涉及自动泊车轨迹规划，尤其涉及一种基于模型预测控制的自动泊车轨迹规划方法。
背景技术：
1、理想的自动泊车，使能使车辆从起始点到达预设点，不仅位置满足预设要求，而且车辆的姿态也要满足要求，于是需要一种关于预设轨迹规划跟踪的自动泊车，在泊车之前，能够规划出一条起始点与目标点之间的合理路径，而后选择一种合适的控制规律，控制车辆从起始点，按照生成轨迹到达预设点。
2、现有技术中基本是基于车辆的运动学模型进行自动泊车轨迹追踪的控制，满足运动学模型的小车，它的轨迹是由一系列圆弧组成的，在众多泊车轨迹中，最简单的是两圆弧轨迹，顾名思义，它是由两段相切的圆弧组成的，它的优点在于，轨迹数学模型简单，容易表达和编写，缺点是他的曲率不连续，在切点变化太大，换句话说，实际情况中，当车辆处于在切点位置的时刻，司机需要把方向盘在瞬间从一边转到另一边，这会带来极大的操作难度，并且在控制时会产生很大的误差，导致仿真结果十分不理想。现在很多境况下采用的轨迹都是圆弧—直线—圆弧轨迹，这种轨迹在两段圆弧之间会有一段直线来进行过度，较好的改进了两圆弧轨迹带来的弊端。
3、模型预测控制(modelpredictive control)，是一种广泛被应用于自动驾驶的轨迹跟踪算法，它与pid控制、lqr控制、滑模控制等几乎是目前最热门的几种关于轨迹跟踪的控制方法，模型预测控制是一种依照我们建立的车辆动力学模型来进行数学优化的一种控制方法。
4、从本质上来说，模型预测控制也可以说是反馈控制，模型预测控制是基于目前车辆的状态，以及期望路径，对于车辆的行动做出预测，也就是对于方向盘的转角做出预测，并且将控制信号传输给执行机构，使车辆转动。它在每个时刻都会做出一系列的预测动作，来追踪目标轨迹，但为了追求高精度，减小误差，避免其他因素对控制过程的影响，每次只取每个预测的第一步进行控制。不断重复，这样就能以极小的误差不断逼近需要追踪的轨迹。因此，加强基于模型预测控制在自动泊车轨迹规划中的应用研究具有重要的技术价值。
技术实现思路
1、本发明提供一种基于模型预测控制的自动泊车轨迹规划方法，以五次多项式路径为主的路径规划设计和以模型预测控制为主的轨迹跟踪控制设计，大大提高了自动泊车轨迹规划的准确性。
2、本发明提供一种基于模型预测控制的自动泊车轨迹规划方法，所述方法包括：
3、s1，将给定速度输入车辆运动学模型，车辆运动学模型输出车辆的预设初始位置和姿态，传输给泊车控制器与预设的泊车轨迹相比较，得出车辆位置与姿态的误差信号；其中，所述预设的泊车轨迹基于五次多项式曲线进行设计；
4、s2，基于车辆位置与姿态的误差信号，由泊车控制器基于模型预测控制进行计算控制，输出当前时刻控制车辆的转向角的数值；
5、s3，将转向角的数值输入到车辆运动学模型，和给定速度信号共同控制车辆的运行；
6、s4，重复步骤s1-s3，车辆不断逼近预设的泊车轨迹，实现自动泊车轨迹规划。
7、进一步地，所述预设的泊车轨迹基于五次多项式曲线进行设计，具体包括：
8、确定车辆和车库尺寸后，确定车辆后轴中心点在惯性坐标系中的位置，设起始点坐标为(x0，y0)和终止点坐标为(x1，y1)，起始点和终止点两点之间的轨迹规划的起始时刻是t0，结束的时刻是t1；
9、后轴中心点的轨迹用函数由y(x)来表示，其表达式采用五次多项式，为：
10、y(x)＝a0+a1x+a2x2+a3x3+a4x4+a5x5 (1)
11、其中，a0，a1，a2，a3，a4，a5为五次多项式的系数，设定五次多项式曲线的约束条件，包括车辆的起始点坐标和终止点坐标、车身方位、曲线的曲率，即初始条件要满足以下要求：
12、y(x0)＝y0，
13、y(x1)＝y1，
14、根据五次多项式以及其一阶的导数和二阶的导数，经过计算可求得系数：a0，a1，a2；
15、将求得的a0，a1，a2再带回原式以及其一阶的导数和二阶的导数，经过计算可求出系数a3，a4，a5；
16、将计算出的五次多项式轨迹改写成车辆后轴中心点横坐标x和时间t的关系，以及航向角θ和时间t的关系。
17、进一步地，航向角θ和时间t的关系，表示如下：
18、车辆的运动学模型公式：
19、
20、式中，为车辆的转向角，θ为车辆的航向角，为航向角速度；和为车辆在x轴和y轴位移的一阶导数，l为车辆的轴距，v为车辆后轴中心的速度；
21、根据式子(2)，选择横向位移x和航向角θ的关系推导出θ与时间t的关系：
22、
23、进一步地，所述步骤s2，基于车辆位置与姿态的误差信号，由泊车控制器基于模型预测控制进行计算控制，具体包括：
24、取状态向量为x＝[x，y，θ]t，控制向量为车辆的运动学模型公式(2)改写成向量形式：
25、
26、式中，f＝[f1，f2，f3]t，f1＝vcosθ，f2＝vsinθ，
27、由五次多项式(1)规划的理想自动泊车路径在每一时刻的状态向量为xr＝[xr，yr，θr]t、控制向量为
28、
29、式中，(xr，yr)为规划的车辆后轴中心点坐标，θr为规划的航向角，为规划的车辆转向角，vr为规划的车辆后轴中心速度；
30、通过将式(4)在f(xr，ur)处进行泰勒展开，保留一阶项，忽略高阶项，可以得到线性化的误差状态方程：
31、
32、其中，
33、
34、将式(6)进行前向欧拉离散化，设t0＝0，当前时刻为t＝kt，t为采样时间，可以得到离散状态方程：
35、
36、式中
37、为了进行模型预测，设置中间变量推导出离散状态空间表达式：
38、
39、式中i为单位阵；
40、未来np个时刻的输出量η(k)的预测值，可以由以下方程得出：
41、y(k)＝ψk(k)ξ(k)+θkδu(k) (9)
42、式中
43、np为预测时域步数，nc为控制时域步数；
44、目标函数表示为：
45、
46、式中，q为输出权重矩阵，r为控制增量权重矩阵，ρ为松弛因子权重，ε为松弛因子。
47、进一步地，关于约束部分，控制量增量误差约束为：
48、
49、其中，δumin和δumax分别为控制增量的最小值和最大值，m为松弛因子的最大值。
50、本发明提供的基于模型预测控制的自动泊车轨迹规划方法，基于车辆的运动学模型，确定了以五次多项式路径为主的路径规划设计和以模型预测控制为主的轨迹跟踪控制设计，在多项式曲线中能够实现平行泊车效果最好的最小的阶数是五阶多项式曲线，五次多项式曲线作为自动泊车系统的轨迹规划曲线，提高路径规划准确性，模型预测控制在每个时刻都会做出一系列的预测动作，来追踪目标轨迹，但为了追求高精度，减小误差，避免其他因素对控制过程的影响，每次只取每个预测的第一步进行控制，不断重复，这样就能以极小的误差不断逼近需要追踪的轨迹；将泊车控制器输出的转向角数值输入到目标车辆的运动学模型，和给定速度信号共同控制车辆的运行，车辆又可以不断的输出车辆的姿态和位置继续执行控制方法的第一步流程，并且不断地循环，车辆就可以不断的逼近规划好的泊车轨迹，达到路径追踪的效果。
1.一种基于模型预测控制的自动泊车轨迹规划方法，其特征在于，包括：
2.根据权利要求1所述的基于模型预测控制的自动泊车轨迹规划方法，其特征在于，所述预设的泊车轨迹基于五次多项式曲线进行设计，具体包括：
3.根据权利要求2所述的基于模型预测控制的自动泊车轨迹规划方法，其特征在于，航向角θ和时间t的关系，表示如下：
4.根据权利要求3所述的基于模型预测控制的自动泊车轨迹规划方法，其特征在于，所述步骤s2，基于车辆位置与姿态的误差信号，由泊车控制器基于模型预测控制进行计算控制，具体包括：
5.根据权利要求4所述的基于模型预测控制的自动泊车轨迹规划方法，其特征在于，关于约束部分，控制量增量误差约束为：
