代码详解——MATLAB/Simulink实时性仿真

实时性一直是限制NMPC应用的一个瓶颈，我们在下述论文中讨论了这个问题：

[1] 白国星, 刘丽, 孟宇, 等. 基于非线性模型预测控制的移动机器人实时路径跟踪[J/OL]. 农业机械学报: 1-13 [2020-09-27]. http://kns.cnki.net/kcms/detail/11.1964.S.20200703.1012.004.html.

那么如何在仿真中读出NMPC的运算时间？

首先介绍一个Simulink模块，Real-Time Synchronization。

这个模块的作用是同步Simulink仿真和Windows系统时间，同步后读出的时间即实际运算时间。

读取运算时间采用的函数是clock和etime，mpc001代码修改为如下：

function [sys,x0,str,ts] = mpc001(t,x,u,flag)

switch flag

case 0

[sys,x0,str,ts]=mdlInitializeSizes;

% case 2

% sys = mdlUpdate(t,x,u);

case 3

sys = mdlOutputs(t,x,u);

case {1,2,4,9}

sys=[];

otherwise

DAStudio.error('Unhandled flag=', num2st(flag));

end

function[sys,x0,str,ts]=mdlInitializeSizes

sizes=simsizes;

sizes.NumContStates=0;

sizes.NumDiscStates=3;

sizes.NumOutputs=5;

sizes.NumInputs=3;

sizes.DirFeedthrough=1;

sizes.NumSampleTimes=1;

sys=simsizes(sizes);

x0 =[0;0;0];

str = [];

ts = [0.05 0];

global rr;

global T;

global vd1;

global w;

global tt;

N=30000;

T=0.05;

vd1=3;

w=0;

tt=0;

rr=zeros(N+10,4);

%生成产考轨迹

for ir=1:1:N

if ir<=5100

rr(ir,1)=19.5+0.1*(ir-1)*T;

rr(ir,2)=80;

rr(ir,3)=0;

rr(ir,4)=0;

elseif ir<=9787

rr(ir,1)=45+15*sin(0.0067*(ir-5100)*T);

rr(ir,2)=65+15*cos(0.0067*(ir-5100)*T);

rr(ir,3)=0-0.0067*(ir-5100)*T;

rr(ir,4)=-0.06666667;

elseif ir<=15787

rr(ir,1)=60;

rr(ir,2)=65-0.1*(ir-9787)*T;

rr(ir,3)=-1.57;

rr(ir,4)=0;

elseif ir<=20473

rr(ir,1)=75-15*cos(0.0067*(ir-15787)*T);

rr(ir,2)=35-15*sin(0.0067*(ir-15787)*T);

rr(ir,3)=-1.57+0.0067*(ir-15787)*T;

rr(ir,4)=0.06666667;

elseif ir<=30473

rr(ir,1)=75+0.1*(ir-20473)*T;

rr(ir,2)=20;

rr(ir,3)=0;

rr(ir,4)=0;

else

rr(ir,1)=150;

rr(ir,2)=20;

rr(ir,3)=0;

rr(ir,4)=0;

end

end

function sys = mdlOutputs(t,x,u)

t1=clock;

global rr;

global T;

global vd1;

global w;

global tt;

i = round(t*20)+1;

%控制器参数

Nx =3;%状态量的个数

Nu =2;%控制量的个数

Np =15;%预测/控制步长

Nc=1;

l=2.7;

vkey=vd1*10;%由于参考路径分辨率的问题，设置vd1的最低分辨率为0.1m/s

%初始状态

vx=vd1;

td=u(3);

State_Initial(1,1)=u(1);%x

State_Initial(2,1)=u(2);%y

State_Initial(3,1)=u(3);%phi

for Nref=1:1:Np

Xref(Nref,1)=rr((i+Nref-1)*vkey,1);

Yref(Nref,1)=rr((i+Nref-1)*vkey,2);

PHIref(Nref,1)=rr((i+Nref-1)*vkey,3);

end

%%约束条件

lv=0.05;

lw=0.0082;

dvx1=-lv;dvx2=lv;dw1=-lw;dw2=+lw;

if w>=0.44

dw2=0.0;

elseif w<=-0.44

dw1=-0.0;

end

for ir=1:2:2*Nc

lb(ir)=dvx1;

lb(ir+1)=dw1;

ub(ir)=dvx2;

ub(ir+1)=dw2;

end

%%选取求解算法

options = optimset('Algorithm','active-set'); %选择active-set为求解算法

%%求解算法预留

A=[];

b=[];

Aeq=[];

beq=[];

%%求解

[A,fval,exitflag]=fmincon(@(x)NMPC(x,State_Initial,Np,Nc,T,Xref,Yref,PHIref,vx,w),[0;0;0;0;0;0],A,b,Aeq,beq,lb,ub,[],options);%有约束求解，需要有2*Nc个0

%%获得控制输入

vx=vx+A(1);

w=w+A(2);

%控制器输出

mpcout(1)=vx;

mpcout(2)=w;

mpcout(3)=rr((i-1)*vkey+1,1);

mpcout(4)=rr((i-1)*vkey+1,2);

mpcout(5)=tt;

t2=clock;

tt=etime(t2,t1);

sys=mpcout;

Simulink系统中，控制器模块内部改为：

示波器t所示即控制器在每个控制周期内的实时运行时间，该变量也可以用To Workspace读到Workspace空间，方便后续数据处理。

2020年9月28日源码