我所理解的mpc控制（模型预测控制），举例实操。

什么是 mpc(模型预测控制)

模型预测控制是指在当前时刻下，控制器基于当前控制模型预测未来一段时间内的控制结果，通过不断的修正模型参数得到控制结果，再通过一个系统评价得到最优结果，使得当前时刻下做出的控制动作，对于整个预测时间段而言是最优的。

运用mpc控制的注意点
1。mpc占用的内存、算力会比较高，需要选择性能较强的控制器。
2。评价函数非常关键，直接决定了mpc控制的性能。

mpc控制过程：

1.确定控制对象。
2.确定控制模型。
3.确定系统采样时间以及预测的总时间。
4.确定评价函数。
5.确定仿真模型。
6.确认参数合理范围并对每一种可能的参数进行预测。
7.得到当前时刻下最优参数。
8.输出。

我举个例子（matlab写了，比较方便）

1。确定控制对象

我控制的是一辆小车的方向盘，我希望小车沿y = 0直线行驶。

初始化，车辆在（0.5，0，pi/2）的位置，车辆以100km/h匀速运动。

carLength = 2;%车长
carWidth = 1;%车宽
kTurn = 15;%转向比
carX = 0;%初始位置
carY = 0;
carYaw = pi/2;%初始方向
speed = 100/3.6;%车速

2。确定控制模型。

我图个方便，选用基于与y=0距离的pd（比例、微分）控制，那么在模型中我们可调的参数就是kp 和 kd。

if model==0
     targetX = 0;
         targetY = carY + 30;
else
         targetY = carY + 30;
         targetX = 2000 - sqrt(2000*2000 -targetY *targetY) + randXSensor;
end

difYaw = (atan2(targetY - carY,targetX - carX) -carYaw)/pi*180;
controlSteerAngle = kp *  (difYaw)  + kd * (difYaw -difLast );

if controlSteerAngle>500
    controlSteerAngle=500;
elseif controlSteerAngle<-500
    controlSteerAngle=-500;
end

if controlSteerAngle> controlSteerAngleLast + 5
    controlSteerAngle=controlSteerAngle+5;
elseif controlSteerAngle

3.确定系统采样时间以及预测的总时间。

采样时间20ms。
总时间1min。

sampleTime = 0.02;
mpcTime = 60;

4.确定评价函数。

先简单点，与标准位置的距离平均值吧。

 if model ==0
	difSum=difSum+abs(carX);
 else
	difSum = difSum + abs(2000 - sqrt((carX - 2000)^2+carY^2));
 end

5.确定仿真模型。

基于方向盘角度与速度对下一时刻位置进行积分，得到。

randXSensor = 0.2 * randn(); %默认传感器输入不是理想状态，误差正态分布
randXCar = 0.01 * randn();%默认汽车不是理想的响应状况,误差正态分布
            
R = carLength/sin(controlSteerAngle/kTurn/180*pi);
deltaYaw = speed * sampleTime / R;
carX = carX + speed * sampleTime * cos((carYaw + carYaw + deltaYaw)/2) + randXCar;
carY = carY + speed * sampleTime * sin((carYaw + carYaw + deltaYaw)/2);
carYaw = carYaw + deltaYaw;

if carYaw>pi
	carYaw=carYaw-2*pi;
elseif carYaw<-pi
	carYaw =carYaw + 2*pi;
end    

6.确认参数合理范围并对每一种可能的参数进行预测。

这里我取kd的范围在1-100;
kd的范围在1-100;

7.得到当前时刻下最优参数。
对所有可能情况跑for循环，选出评价函数最小的那一组数据，作为最终输出。
8.输出。

分别运行一下model = 0与model=1的效果，得到minkd与minkp的值，效果看着还行，单仔细看看其实是远远达不到要求的。
关键是评价函数的选择是很复杂的，不是简单的用一两个条件就可以确定的，而且对传感器输入以及车辆模型的模拟也是与实际相差比较大的。

下面把代码发出来。

clear ;
close all;
hold on;
grid on;

speed = 100/3.6;
sampleTime = 0.02;
mpcTime = 60;

carLength = 2;
carWidth = 1;
kTurn = 15;

minKp = 0;
minKd = 0;
minAvg = 1000;

data = [];
dataNum = 0;

model = 1;
for kp = 1:1:100
    for kd = 1:1:100
        difSum = 0;
        difAvg = 0;
        numAvg = 0;
        carX = 0;
        carY = 0;
        carYaw = pi/2;
        dataTemp = [];
        dataTempNum = 0;
        difLast = 0;
        controlSteerAngleLast = 0;
        for i = 0:sampleTime:mpcTime
            randXSensor = 0.2 * randn();%传感器误差，正态分布
            randXCar = 0.01 * randn();%汽车响应误差，正态分布
            
            if model==0
                targetX = 0;
                targetY = carY + 30;
            else
                targetY = carY + 30;
                targetX = 2000 - sqrt(2000*2000 -targetY *targetY) + randXSensor;
            end
            difYaw = (atan2(targetY - carY,targetX - carX) -carYaw)/pi*180;
            controlSteerAngle = kp *  (difYaw)  + kd * (difYaw -difLast );
            %得到方向盘角度
            if controlSteerAngle>500
                controlSteerAngle=500;
            elseif controlSteerAngle<-500
                controlSteerAngle=-500;
            end
            
            if controlSteerAngle> controlSteerAngleLast + 5
                controlSteerAngle=controlSteerAngle+5;
            elseif controlSteerAnglepi
                carYaw=carYaw-2*pi;
            elseif carYaw<-pi
                carYaw =carYaw + 2*pi;
            end
            
            dataTempNum =dataTempNum+1;%记录数据，绘图使用
            dataTemp(dataTempNum,1) = carX;
            dataTemp(dataTempNum,2) = carY;
            if model ==0
                difSum=difSum+abs(carX);
            else
                difSum = difSum + abs(2000 - sqrt((carX - 2000)^2+carY^2));
            end
            numAvg = numAvg +1;
            
        end
        difAvg = difSum/numAvg;
        if difAvg