预测控制（一）：MPC轨迹跟踪
预测控制（一）：MPC轨迹跟踪
本文先讲解MPC如何应用于差速机器人，然后使用MATLAB进行仿真测试。
MPC原理
MPC轨迹跟踪的思路不难理解，在目前位姿，预测后面N个时刻机器人所处的位置，与目标轨迹进行比较，计算位姿误差最小和控制量最小的解。使用下一时刻的控制量控制机器人，不等机器人走完预测的轨迹，马上再次进行循环，有点类似DWA算法。
模型线性化
MPC全称模型预测控制，那首先就先得有模型，因为是在计算机上编程，所以用离散模型。
[ x k y k θ k ] = [ x k − 1 + v k t cos θ k − 1 y k − 1 + v k t sin θ k − 1 θ k − 1 + w k t ] \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} ⎣ ⎡xkykθk⎦ ⎤=⎣ ⎡xk−1+vktcosθk−1yk−1+vktsinθk−1θk−1+wkt⎦ ⎤
很明显，这是一个非线性系统，应用MPC的第一步就是要把模型线性化，对这个系统进行泰勒展开。
X k = X r + ∂ f ( X k − 1 , U k ) ∂ X k − 1 ( X k − 1 − X r ) + ∂ f ( X k − 1 , U k ) ∂ U k ( U k − U r ) [ x k − x r y k − y r θ k − θ r ] = [ 1 0 − v r t sin θ r 0 1 v r t cos θ r 0 0 1 ] [ x k − 1 − x r y k − 1 − y r θ k − 1 − θ r ] + [ t cos θ r 0 t sin θ r 0 0 t ] [ u k − u r u k − u r ] 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}\\ Xk=Xr+∂Xk−1∂f(Xk−1,Uk)(Xk−1−Xr)+∂Uk∂f(Xk−1,Uk)(Uk−Ur)⎣ ⎡xk−xryk−yrθk−θr⎦ ⎤=⎣ ⎡100010−vrtsinθrvrtcosθr1⎦ ⎤⎣ ⎡xk−1−xryk−1−yrθk−1−θr⎦ ⎤+⎣ ⎡tcosθrtsinθr000t⎦ ⎤[uk−uruk−ur]
这样就线性化了，注意这里是在点 ( X r , U r ) (X_{r},U_{r}) (Xr,Ur)处展开。然后我们就得到了线性化后的系统，最后结果为：
X ˉ k = A X ˉ k − 1 + B U ˉ k \bar{X} _{k}=A\bar{X} _{k-1}+B\bar{U} _{k} Xˉk=AXˉk−1+BUˉk这里的 X ˉ k \bar{X} _{k} Xˉk是表示误差，至于这个误差怎么用，后文会进行提及。
预测
比如我要向后预测N个点，那么就有：
X ˉ k + 1 = A k X ˉ k + B k U ˉ k + 1 X ˉ k + 2 = A k + 1 X ˉ k + 1 + B k + 1 U ˉ k + 2 . . . X ˉ k + N = A k + N X ˉ k − 1 + B k + N 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}\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} Xˉk+1=AkXˉk+BkUˉk+1Xˉk+2=Ak+1Xˉk+1+Bk+1Uˉk+2...Xˉk+N=Ak+NXˉk−1+Bk+NUˉk+N
把前一个式子代入后一个进行化简：
X ˉ k + 1 = A k X ˉ k + B k U ˉ k + 1 X ˉ k + 2 = A k + 1 A k X ˉ k + A k + 1 B k U ˉ k + 1 + B k + 1 U ˉ k + 2 X ˉ k + 3 = A k + 2 A k + 1 A k X ˉ k + A k + 1 A k B k U ˉ k + 1 + A k + 1 B k + 1 U ˉ k + 2 + B k + 2 U ˉ k + 2 . . . \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}\\ ... Xˉk+1=AkXˉk+BkUˉk+1Xˉk+2=Ak+1AkXˉk+Ak+1BkUˉk+1+Bk+1Uˉk+2Xˉk+3=Ak+2Ak+1AkXˉk+Ak+1AkBkUˉk+1+Ak+1Bk+1Uˉk+2+Bk+2Uˉk+2...
化简得到，这里就写到N=3。
[ X ˉ k + 1 X ˉ k + 2 X ˉ k + 3 ] = [ A k A k + 1 A k A k + 2 A k + 1 A k ] X ˉ k + [ 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 ] [ U ˉ k U ˉ k + 1 U ˉ k + 2 ] \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} ⎣ ⎡Xˉk+1Xˉk+2Xˉk+3⎦ ⎤=⎣ ⎡AkAk+1AkAk+2Ak+1Ak⎦ ⎤Xˉk+⎣ ⎡BkAk+1Bk Ak+2Ak+1Ak 0BkAk+1Bk 00Bk⎦ ⎤⎣ ⎡UˉkUˉk+1Uˉk+2⎦ ⎤
这里需要注意的问题是：
1、以上A和B矩阵都加上了下标，因为这两个矩阵是时变的。A、B矩阵是通过泰勒展开得到的，泰勒展开是在小范围内进行线性近似，而我们预测步骤会使系统偏离初始点，这样如果还是使用初始点的近似，误差会很大，因此这里的 A k + r A_{k+r} Ak+r使用的是机器人此时位置对应的路径点往后的第r个点 ( X r , U r ) (X_{r},U_{r}) (Xr,Ur)，如果路径是实时规划的，那就是路径上的第r个点。而 X k X_{k} Xk就是机器人此时位姿减去对应路径上对应时刻的点。
2、从上述式子也可以看出，要使用MPC进行轨迹跟踪，就需要知道路径的所有运动学参数。如果只有路径点 ( x , y ) (x,y) (x,y)，那可以通过差分的方式计算速度，角度即为速度方向，通过角度差分计算角速度。如果有路径的方程，那通过微分的方法计算速度，角度，角速度更准确。
滚动优化
我们的目标是求解以下的二次规划问题：
min J U = X ˉ T Q X ˉ + U ˉ T R U ˉ \underset{ U}{\min J} =\bar{X}^{T} Q \bar{X}+\bar{U}^{T} R\bar{U} UminJ=XˉTQXˉ+UˉTRUˉQ和R一般为对角矩阵，那么展开这个式子就是求误差和控制量最小值，Q、R矩阵就是权重。
q 1 x 1 2 + q 2 x 2 2 + . . . + r 1 u 1 2 + r 2 u 2 2 + . . . q_1x_1^2+q_2x_2^2+...+r_1u_1^2+r_2u_2^2+... q1x12+q2x22+...+r1u12+r2u22+...
把X代入这个二次规划得到：
( A X ˉ + B U ˉ ) T Q ( A X ˉ + B U ˉ ) + U ˉ T R U ˉ X ˉ T A T Q A X ˉ + X ˉ T A T Q B U ˉ + U ˉ T B T Q A X ˉ + U ˉ T ( B T Q B + R ) U ˉ (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} (AXˉ+BUˉ)TQ(AXˉ+BUˉ)+UˉTRUˉXˉTATQAXˉ+XˉTATQBUˉ+UˉTBTQAXˉ+UˉT(BTQB+R)UˉJ是一个1×1的矩阵，或者说是一个数，那得到的计算公式这四项也都是数，很明显，第一项是常数，与U无关可以忽略，第二和第三项互为转置，可以合并。所以最后的计算公式为：
2 ( A X ˉ ) T Q B U ˉ + U ˉ T ( B T Q B + R ) U ˉ 2(A\bar {X})^TQB\bar{U}+ \bar{U}^T(B^TQB+R)\bar{U} 2(AXˉ)TQBUˉ+UˉT(BTQB+R)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