自动驾驶（六）---------模型预测控制

模型预测控制(Model Predictive Cont

          顾名思义，基于模型，预测未来，进行控制。所谓模型，就是车辆运动学模型。

                               

         模型预测控制是一种基于模型的闭环优化控制策略。预测控制算法的三要素：内部（预测）模型、参考轨迹、控制算法。

• 预测模型：预测模型能够在短时间内很好地预测系统状态的变化
• 在线滚动优化：通过某种最优化算法来优化未来一段短时间的控制输入，使得在这种控制输入下预测模型的输出与参考值的差距最小
• 反馈校正：到下一个时间点根据新的状态重新进行预测和优化  

轨迹模型：

          我们的轨迹模型用一个3阶多项式函数。对x求导即为车身角度ψ的正切：

                                   

动态模型：

          基于以上的轨迹，用车辆运动学模型，计算预测车辆轨迹，利用t时刻的状态去预测在t+1拍时刻的车辆状态。

          车辆状态包括：车辆位置，速度，角度：

滚动优化：

          假设我们给出一组控制参数，按照控制模型，会得到一个控制预测轨迹，

          我们期望这组轨迹和我们的控制目标要要一至，但是这中间要经历运动模型（非线性）变换，并且我们知道的是一条3阶多项式，也要转化成油门转角，所以中间的模型参数，都需要建模处理。

           现在我们知道控制目标（一串轨迹，或者3阶多项式），按照控制模型反求合适的控制参数，定义损失函数

           首先想到的一个损失函数就是我们的模型预测的轨迹和参考线之间的“差距”， 我们优化的目标就是找出一组控制量（即 油门刹车系数和方向盘转角），使得我们的Loss function最小化。

                                                            

          上文中我们只考量未来10个时间间隔内的模型预测，所以i的取值范围是(1,10) 。

          可以给损失函数添加更多项，比如想控制车辆在每个点上的速度，则添加一项速度的平方差：

                                                             

           为了控制平滑，我们希望油门系数不要突变

                                                  

           还可以再加，不过我已经够了，这个最优化问题中的变量是 带约束的 ，比如说车辆的前轮转角有一定的取值范围，车辆油门系数有取值范围的。

           求解这个最优解问题，解出当前对应的油门转角，作为控制输入。

反馈矫正：

          当我们通过上面最优化方法得到一组控制输出以后，车辆执行控制指令，并且反馈当前车辆的状态，依据新的车辆状态，结合感知模块的信息以及地图信息重新做出规划（新的3阶多项式）。需要注意的是，车辆真实状态的反馈并不是一个预测时间段的控制执行完以后才反馈的，反馈的时间间隔往往小于一个预测时间段：

                                                    

总结：

         模型预测控制往往可以分解成如下几步： 
          1. 从 tt 时刻开始，预测未来 aa 步系统的输出信号 
          2. 基于模型的控制信号以及相应的输出信号，构造损失函数，并且通过调整控制信号最优化损失函数 
          3. 将控制信号输入到系统 
          4. 等到下一个时间点，在新的状态重复步骤1

喜闻乐结 C++代码：https://blog.csdn.net/tingfenghanlei/article/details/85046120