2 路径跟踪控制
为了使车辆顺利跟踪期望路径，当车速已知时，在横向方向上，需要对车辆的转向进行精确控制。路径跟踪控制的原理是根据车辆实时反馈的状态信息进行计算，得到最优的前轮转角或方向盘转角控制量，将控制量反馈到车辆，以此来控制车辆跟踪期望路径。
路径跟踪的过程实际是对期望路径上的一系列点的逐个跟踪过程，当路径跟踪过程中车速发生变化时，将会影响到跟踪效果。许多研究学者研究路径跟踪控制方法时，将车速视为匀速条件进行研究，忽略了车速变化对跟踪效果的影响。车辆在泊车过程中可能会遇到行人、动物等动态障碍物，此时就需要停车避让，传统的路径跟踪控制策略中，对车速要求较高，在泊车过程中车速需要尽可能保持匀速状态，转角变化与时间相关，刹车后就失去了跟踪能力。
为了减小车速变化对跟踪效果的影响，以及考虑泊车过程中遇到动态障碍物的避障控制，本文基于模型预测控制算法设计了路径跟踪控制器，模型预测控制算法可以根据车辆位姿信息和车速信息实时调整预测模型，修正转角控制量，降低车速对路径跟踪效果的影响。
2.1 模型预测控制理论
模型预测控制（Model Predictive Control, MPC）是一种先进的过程控制方法，近年来被广泛讨论的一种反馈控制策略，该算法不仅在工业上得到了广泛的应用，在车辆横纵向控制方面也有所应用。
MPC的机理可以描述为：在每一个采样时刻，根据当前采样时刻获取的测量信息，求解一个有限时域开环的优化问题，并将求解出的控制序列的第一个元素作用于被控对象，在下一采样时刻重复上述过程，用新的测量值刷新优化问题并重新求解，从而实现整个时域的优化求解。MPC实际上是一种时间相关的，利用系统当前状态和当前的控制量，来实现对系统未来状态的控制。而系统未来的状态是不定的，因此在控制过程中要不断根据系统状态对未来的控制量作出调整。而且相较于经典的PID控制，它具有优化和预测能力，也就是说，MPC是一种致力于将更长时间跨度、甚至于无穷时间的最优化控制问题，分解为若干个更短时间跨度，或者有限时间跨度的最优化控制问题，并且在一定程度上仍然追求最优解。
在实现MPC的过程中需要三个关键步骤：
（1）预测模型：预测模型是MPC的基础，它的主要功能是根据系统的历史信息和未来输入预测系统未来的输出。
（2）滚动优化：由于采用有限时域预测存在外部干扰和模型不确定性的影响，因此不能直接将求解出的控制序列作用于系统，而是要将每个采样时刻的第一个分量作用于系统，所以预测控制不是采用一个不变的全局优化目标，而是采用时间向前滚动的有限时段优化策略，是反复在线进行的。
（3）反馈校正：为了防止模型失配或环境干扰引起控制对理想状态的偏离，在新的采样时刻首先需要检测对象的实际输出，并利用这一实时信息进行修正，然后再进行新的优化，从而构成闭环优化。
2.2 线性时变车辆模型
由于线性时变MPC相比于非线性MPC计算更为简单，实时效果更好，因此为了保证泊车系统的实时性，以及车辆跟踪期望路径的稳定性，本文以线性时变模型作为MPC算法的预测模型。由于泊车过程车辆是低速运动的过程，可以用车辆运动学模型作为预测模型，但无论车辆模型是运动学建模，或是动力学建模，最后得到的都是非线性系统，因此需要将车辆运动学模型进行线性化处理。非线性系统线性化的方法可近似分为精确线性化和近似线性化两种，由于近似线性化方法简单、适用性强，因此一般都采用近似线性化的方法。
由车辆运动学模型可知：
式(3.6)即为线性化车辆误差模型，为了使误差模型能够用于模型预测控制器的设计，需要将式(3.6)进行离散化处理。离散化方法分为精确离散化方法和近似离散化方法，由于本文使用的是车辆运动学模型，模型较为简单，因此采用近似离散化方法。
假设有如下所示的连续系统状态方程：
2.3 车辆预测模型
2.4 二次规划问题转化
2.5 约束问题转化
2.6 路径跟踪控制器搭建与仿真
（本节代码后续会补上链接）本节的matlab代码和simulink模型
在自动泊车路径跟踪问题上，本文使用的基于MPC的路径跟踪控制器如下图所示。
为了验证控制模型的跟踪效果，结合目标车辆参数，在Simulink中搭建车辆运动学模型作为控制器目标车辆模型，对控制器进行初步检验。搭建的simulink模型结构如下图所示，模型由模型预测控制器和车辆运动学模型组成。首先，设置车速为-1m/s ，将车速信号输入进车辆运动学模型中；然后，通过理论公式，根据车速信号和初始前轮转角信号计算出车辆运动学模型的横线位移、纵向位移、横摆角和横摆角速度，将车辆的各个状态量输入进模型预测控制器中，控制器根据设定的参考路径和车辆状态量计算出最优前轮转角控制量，将其反馈回车辆运动学模型和模型预测控制器中，由此进行循环操作，完成路径的跟踪过程。
运行上述模型，仿真结果如下图所示。从图(a)中可以看出，车辆沿着期望路径行驶时，车身各顶点的运动轨迹没有与车位线和道路边界线发生接触，车辆轮廓没有与车位边界和道路边界发生碰撞。由图(b)可知，车辆后轴中心Y轴坐标随时间变化连续，Y轴方向的误差范围保持在 以内。由图?可知，车辆横摆角随时间变化连续，车辆实际横摆角与期望路径横摆角误差范围保持在 以内，车辆行驶至路径终止点时，横摆角保持在零附近，车身与车位保持平行。由图(d)可知，车辆前轮转角随时间的变化连续，无突变情况，且前轮转角的变化范围能够满足车辆转向机构要求，车辆行驶至路径终止点时，前轮转角为回正状态，避免了原地转向的情况。由图(e)可知，车辆前轮转角转速随时间的变化连续，无突变情况，且前轮转角转速的变化范围能够满足车辆转向机构的要求。综上所述，模型预测控制器能较好的控制车辆运动学模型跟踪期望路径行驶，初步验证了控制器的有效性，为后续使用动力学模型作为目标车辆验证控制器提供了参考。
