无人驾驶车辆控制程序编写

在第4章中，我们已经采用S函数编写了基于车辆运动学模型的轨迹跟踪控制器。在这一章中，需要继续用S函数建立控制器，完成的程序为“chapter 5_2_2.m”。在主函数中，仍然和第4章保持一致，通过标志位调用不同的函数。在初始化函数中，主要改变了输入与输出参数的个数，相应地需要对系统状态量进行初始化。

程序的主体为函数mdlOutputs()，需要完成程序基本参数初始化、车辆参数输入、参考轨迹生成、二次规划问题求解等几个模块。

首先介绍第一部分，也即程序参数初始化，主要功能是设置模型预测控制器的基本参数，包括输入与输出个数、预测和控制时域等。由于CarSim中采用的速度单位为“km/h”，角度单位为“°”，而我们在S函数中采用的单位分别为“m/s”和“rad”，因此需要加入对输入参数的转换，而这一点往往也是我们最容易忽视的。

预测模型是模型预测控制的三项基本原理之一。在控制器中需要根据我们采用的动力学模型设置参数，包括车辆轴距、轮胎刚度以及车辆质量等，具体代码如下：

完成车辆动力学参数设置后，需要给出参考轨迹。在仿真过程中，既可以将参考轨迹生成的部分放在控制器中，也可以单独提供一个模块并建立与控制器的接口。本书将参考轨迹生成的部分放在控制器中，而有兴趣的读者也可以自己尝试外加模块，以实现同样功能。参考轨迹的含义将在仿真结果部分介绍，以下仅给出实现代码：(www.xing528.com)

上述步骤都可以被看成建立模型预测控制器的准备工作，而线性时变模型预测控制的主要工作是根据线性时变模型预测系统输出，结合系统约束，将车辆轨迹跟踪问题转换为标准二次规划问题进行求解。线性时变模型预测控制的算式推导部分可以参考本书3.3节。读者在完成了3.3.3中的实例程序后，再结合本章中对动力学模型的线性化求解，就可以比较顺利地完成以下程序：

在上面的程序中，我们已经获得了二次规划问题中至关重要的两个矩阵H和f，而接下来就是要根据需要的约束设定好约束条件，包括控制量约束、控制增量约束和输出量约束。

最后，结合二次规划的标准矩阵和约束条件进行最优求解。需要完成的工作包括选择优化方法(一般在有效集与内点法之间选择)、设定初始点等。求解结束后，根据模型预测控制的基本原理，将控制增量序列的第一个元素与上一个时刻的控制量相加，输出给CarSim动力学模块。

至此，我们就完成了基于动力学模型的主动转向控制器的设计。读者可以分段将程序读懂，然后将各段程序合在一起，在联合仿真平台上测试程序的功能。以下将介绍本书针对无人驾驶车辆行驶工况所设计的仿真环境及验证结果。