预测控制（一）：MPC轨迹跟踪

本文先讲解MPC如何应用于差速机器人，然后使用MATLAB进行仿真测试。

MPC原理

MPC轨迹跟踪的思路不难理解，在目前位姿，预测后面N个时刻机器人所处的位置，与目标轨迹进行比较，计算位姿误差最小和控制量最小的解。使用下一时刻的控制量控制机器人，不等机器人走完预测的轨迹，马上再次进行循环，有点类似DWA算法。

模型线性化

MPC全称模型预测控制，那首先就先得有模型，因为是在计算机上编程，所以用离散模型。
$\begin{bmatrix} x_{k}\\ y_{k}\\ \theta_{k} \end{bmatrix}=\begin{bmatrix} x_{k-1}+v_{k}t\cos\theta_{k-1}\\ y_{k-1}+v_{k}t\sin\theta_{k-1}\\ \theta_{k-1}+w_{k}t \end{bmatrix}$
很明显，这是一个非线性系统，应用MPC的第一步就是要把模型线性化，对这个系统进行泰勒展开。
$X_{k} = X_{r}+\frac{\partial f(X_{k-1},U_{k})}{\partial X_{k-1}}({X_{k-1}-X_{r}})+\frac{\partial f(X_{k-1},U_{k})}{\partial U_{k}}({U_{k}-U_{r}})\\ \begin{bmatrix} x_{k}-x_{r}\\ y_{k}-y_{r}\\ \theta_{k}-\theta _{r} \end{bmatrix}=\begin{bmatrix} 1 & 0&-v_{r}t\sin\theta_{r} \\ 0 & 1&v_{r}t\cos\theta_{r} \\ 0 & 0& 1 \end{bmatrix} \begin{bmatrix} x_{k-1}-x_{r}\\ y_{k-1}-y_{r}\\ \theta_{k-1}-\theta _{r} \end{bmatrix}+\begin{bmatrix} t\cos \theta _{r}& 0\\ t\sin \theta _{r}& 0\\ 0& t \end{bmatrix}\begin{bmatrix} u_{k}-u_{r}\\ u_{k}-u_{r}\\ \end{bmatrix}\\$
这样就线性化了，注意这里是在点 $X_{r},U_{r})$ 处展开。然后我们就得到了线性化后的系统，最后结果为：
$\bar{X} _{k}=A\bar{X} _{k-1}+B\bar{U} _{k}$ 这里的 $\bar{X} _{k}$ 是表示误差，至于这个误差怎么用，后文会进行提及。

预测

比如我要向后预测N个点，那么就有：
$\bar{X} _{k+1}=A_{k}\bar{X} _{k}+B_{k}\bar{U} _{k+1}\\ \bar{X} _{k+2}=A_{k+1}\bar{X} _{k+1}+B_{k+1}\bar{U} _{k+2}\\ ...\\ \bar{X} _{k+N}=A_{k+N}\bar{X} _{k-1}+B_{k+N}\bar{U} _{k+N}$
把前一个式子代入后一个进行化简：
$\bar{X} _{k+1}=A_{k}\bar{X} _{k}+B_{k}\bar{U} _{k+1}\\ \bar{X} _{k+2}=A_{k+1}A_{k}\bar{X} _{k}+A_{k+1}B_{k}\bar{U} _{k+1}+B_{k+1}\bar{U} _{k+2}\\ \bar{X} _{k+3}=A_{k+2}A_{k+1}A_{k}\bar{X} _{k}+A_{k+1}A_{k}B_{k}\bar{U} _{k+1}+A_{k+1}B_{k+1}\bar{U} _{k+2}+B_{k+2}\bar{U} _{k+2}\\ ...$
化简得到，这里就写到N=3。
$\begin{bmatrix} \bar{X} _{k+1}\\ \bar{X} _{k+2}\\ \bar{X} _{k+3} \end{bmatrix}=\begin{bmatrix} A_{k}\\ A_{k+1}A_{k}\\ A_{k+2}A_{k+1}A_{k} \end{bmatrix}\bar{X} _{k}+\begin{bmatrix} B_{k} & 0 & 0\\ A_{k+1}B_{k}\ & B_{k} & 0\\ A_{k+2}A_{k+1}A_{k}\ & A_{k+1}B_{k}\ & B_{k} \end{bmatrix}\begin{bmatrix} \bar{U} _{k} \\ \bar{U} _{k+1}\\ \bar{U} _{k+2} \end{bmatrix}$
这里需要注意的问题是：
1、以上A和B矩阵都加上了下标，因为这两个矩阵是时变的。A、B矩阵是通过泰勒展开得到的，泰勒展开是在小范围内进行线性近似，而我们预测步骤会使系统偏离初始点，这样如果还是使用初始点的近似，误差会很大，因此这里的 $A_{k+r}$ 使用的是机器人此时位置对应的路径点往后的第r个点 $X_{r},U_{r})$ ，如果路径是实时规划的，那就是路径上的第r个点。而 $X_{k}$ 就是机器人此时位姿减去对应路径上对应时刻的点。
2、从上述式子也可以看出，要使用MPC进行轨迹跟踪，就需要知道路径的所有运动学参数。如果只有路径点 $(x, y)$ ，那可以通过差分的方式计算速度，角度即为速度方向，通过角度差分计算角速度。如果有路径的方程，那通过微分的方法计算速度，角度，角速度更准确。

滚动优化

我们的目标是求解以下的二次规划问题：
$\underset{ U}{\min J} =\bar{X}^{T} Q \bar{X}+\bar{U}^{T} R\bar{U}$ Q和R一般为对角矩阵，那么展开这个式子就是求误差和控制量最小值，Q、R矩阵就是权重。
$q_1x_1^2+q_2x_2^2+...+r_1u_1^2+r_2u_2^2+...$
把X代入这个二次规划得到：
$(A\bar X+B \bar{U})^TQ(A\bar X+B \bar{U})+\bar{U}^TR\bar{U}\\ \bar {X}^TA^TQA\bar X+\bar {X}^TA^TQB\bar{U}+\bar{U}^TB^TQA\bar{X}+ \bar{U}^T(B^TQB+R)\bar{U}$ J是一个1×1的矩阵，或者说是一个数，那得到的计算公式这四项也都是数，很明显，第一项是常数，与U无关可以忽略，第二和第三项互为转置，可以合并。所以最后的计算公式为：
$2(A\bar {X})^TQB\bar{U}+ \bar{U}^T(B^TQB+R)\bar{U}$
这里计算出来的U是Np×1的矩阵，N为预测点的个数，p为控制量个数，这里就是v和w两个，MPC中只使用下一个时刻的控制量，也就是U(1)和U(2)。将这个控制量输入给机器人后就进行下一轮的循环。

MATLAB仿真

直接看代码吧，弄清楚以上原理就不难。
主函数：

clc;
clear all;
close all;

% 定义轨迹
steps = 200;
dt = 0.1;
fin = steps*dt;
t = linspace(1,fin,steps);

x = ones(1,steps); % reference x 
y = ones(1,steps); % reference y
for i = 1:steps
    % 要修改轨迹，改这个地方就行
    x(i) = 2*cos(0.5*i*dt);
    y(i) = sin(0.5*i*dt);
end

v = ones(1,steps); % reference v
theta = zeros(1,steps); % reference theta
w = zeros(1,steps); % reference omiga
for i =1:steps-1
    vx =  (x(i+1)-x(i))/dt;
    vy =  (y(i+1)-y(i))/dt;
    v(i) =  sqrt(vx^2+vy^2);
    theta(i)= atan2(vy,vx);
end
v(steps) = v(steps-1);
theta(steps) = theta(steps-1);
for i =1:steps-1
    w(i)= angle_bound(theta(i+1)-theta(i))/dt;
end
w(steps) = 0;


% 初始状态
pose = [x(1),y(1),theta(1)]; % x y theta
control = [0,0]; % v w
n = size(pose,2); % 状态变量个数
p = size(control,2); % 控制变量个数

% 机器人模型
I=eye(n);

A = zeros(n); % 3*3
B = zeros(n,p); % 3*2

% 控制
N = 20; % 预测步
Q=[1 0 0;0 1 0;0 0 5]; % 定义Q矩阵，n x n 矩阵，x_err y_err theta_err权重
R=[1 0;0 1;]; % 定义R矩阵，p x p 矩阵 delta_v delta_w 权重

X = [pose,control]; %机器人最新的状态
X_real = []; %记录机器人每一时刻的状态，进行画图
X_real = [X_real;X];

for k = 1 : steps
    N = min(N,steps-k);
    if N == 0
        break
    end

    x0=[X(1)-x(k),X(2)-y(k),angle_bound(X(3)-theta(k))];

    % A B矩阵就是Ak Bk矩阵
    A = zeros(n,n,N);
    B = zeros(n,p,N);
    for i = 1:N
        A(1,3,i)=-1*v(k+i-1)*sin(theta(k+i-1));
        A(2,3,i)= v(k+i-1)*cos(theta(k+i-1));
        A(:,:,i) = A(:,:,i)*dt+I;

        B(1,1,i) = cos(theta(k+i-1))*dt;
        B(2,1,i) = sin(theta(k+i-1))*dt;
        B(3,2,i) = 1*dt;  
    end

    % Ahat Bhat 就是那个A B矩阵
    Ahat = zeros(n*N,n);
    Bhat = zeros(n*N,p*N);
    Atemp=1;
    for i=1:N
        Atemp=Atemp*A(:,:,i);
        Ahat(n*(i-1)+1:i*n,:)=Atemp;
        Btemp=1;
        for kk=1:i
            j=i-kk+1;
            Bhat((i-1)*n+1:i*n,(j-1)*p+1:j*p)=Btemp*B(:,:,j);
            Btemp=Btemp*A(:,:,j);
        end
    end

    %二次规划标准式 (X'*(1/2 *H) *X +f*X 最小值
    Q_bar = kron(eye(N),Q);
    R_bar = kron(eye(N),R);
    
    H = Bhat'*Q_bar*Bhat + R_bar;
    H = (1/2)*H;
    f = 2*(Ahat*x0')'*Q_bar*Bhat;
    
    lb=-10*ones(p*N,1);
    ub=10*ones(p*N,1);
    [dertau, fval, exitflag]=quadprog(H,f',[],[],[],[],lb,ub);
    u1=dertau(1)+v(k);
    u2=dertau(2)+w(k);

    %将控制量输入对象
    X(4) = u1+0.05*randn;
    X(5) = u2+0.05*randn;
    X(1) = X(1)+ X(4)*cos(X(3))*dt;
    X(2) = X(2)+ X(4)*sin(X(3))*dt;
    X(3) = X(3)+ X(5)*dt;
    X(3) = angle_bound(X(3));
    X_real = [X_real;X];
end

% 画图
% X_real = [X_real;X];
figure(1);
plot(X_real(:,1),X_real(:,2),'ro');
hold on;
plot(x,y,'b-');
title("path");

figure(2);
plot(t,X_real(:,1),"ro");
hold on;
plot(t,x);
title("X");

figure(3);
plot(t,X_real(:,2),"ro");
hold on;
plot(t,y);
title("Y");

其它函数angle_bound.m

function res = angle_bound(x)
    res=x;
    for i=1:size(res,2)
        while res(i)<-pi 
            res(i)=res(i)+2*pi;
        end
        while res(i)>pi
           res(i)=res(i)-2*pi;
        end
    end
    for i=1:size(res,1)
        while res(i)<-pi
            res(i)=res(i)+2*pi;
        end
        while res(i)>pi
           res(i)=res(i)-2*pi;
        end
    end
end