无人机NMPC控制器Matlab代码案例解读

进一步学习仿真ing，欢迎纠错指点

直接看核心初始化及step函数部分

function setupImpl(obj,~,~,~)

% Perform one-time calculations, such as computing constants

% 初始化函数，只执行一次，重点在于solver求解器的初始化。

import casadi.*

%model params

dt=;  %根据实际参数及需求填入数据

m=;

g=;

J=diag([  ]);

l=;

belta=pi/4;

cm=;

ct=;

%control params

Q_p=diag([,,]);

Q_v=diag([,,]);

Q_q=diag([,,]);

Q_Omega=diag([,,]);

Q_u=diag([,,,]);

Omega_max_tilt=；

Omega_max_tor=;

%continous model

x=SX.sym('x',3);

v=SX.sym('v',3);

q=SX.sym('q',4);

Omega=SX.sym('Omega',3);

u=SX.sym('u',4);

X=[x;v;q;Omega];

T=u(1)+u(2)+u(3)+u(4);

G1_bar=[-l*sin(belta) l*sin(belta) l*sin(belta) -l*sin(belta);

l*cos(belta) -l*cos(belta) l*cos(belta) -l*cos(belta);

cm/ct cm/ct -cm/ct -cm/ct];

tao=G1_bar*u;

Omega_dot=J\(-cross(Omega,J*Omega)+tao);

A=[-q(2) -q(3) -q(4);

q(1) -q(4) q(3)

q(4) q(1) -q(2)

-q(3) q(2) q(1)];

q_dot=0.5*A*Omega;

R=[q(1)^2+q(2)^2-q(3)^2-q(4)^2 2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3));

2*(q(2)*q(3)+q(1)*q(4)) q(1)^2-q(2)^2+q(3)^2-q(4)^2 2*(q(3)*q(4)-q(1)*q(2));

2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) q(1)^2-q(2)^2-q(3)^2+q(4)^2];

v_dot=[0;0;g]-T/m*R*[0;0;1];

x_dot=v;

X_dot=[x_dot;v_dot;q_dot;Omega_dot];

f=Function('f',{X,u},{X_dot}); % 以上案例意在构建四旋翼连续系统动力学模型，{X,u}输入，{X_dot}输出。

%discrete model

X_now=SX.sym('X_now',13); % x v q Omega：3+3+4+3共13维。当前状态。

u_now=SX.sym('u_now',4); % actuator input % 当前控制输入

k1=f(X_now,u_now);

k2=f(X_now+dt/2*k1,u_now);

k3=f(X_now+dt/2*k2,u_now);

k4=f(X_now+dt*k3,u_now); 

X_next=X_now+dt/6*(k1+2*k2+2*k3+k4); % 由四步微分预测下一步状态

F=Function('F',{X_now,u_now},{X_next},{'X_now','u_now'},{'X_next'}); % 以上内容在连续系统的基础上构建离散的动力学模型，{X_now,u_now}输入，{X_next}输出。

% 离散模型形如公式：

%cost fcn & constraints

cost=0;

N=20;

U_N={};%decision variables %初始化为空 % 元胞数组可包含不同格式数据

U0=[];% initial guess

lbU=[];% lower bound   % 对状态及控制输入的限制

ubU=[];% upper bound

ineq={};% constraints of NLP % 不等式约束，后边作为casadi求解的参数需求。

lbg = []; % 对四元数及omega的限制

ubg = [];

%add mesured system state X0 in decision variables

X0=SX.sym('X0',13);

U_N={U_N{:},X0};  % 大U_N先填入13维x0，最终U_N为[13维状态，4维当前控制输入，4维下一步控制输入...，4维第N步控制输入]的形式

U0=[U0;zeros(13,1)];

lbU=[lbU;zeros(13,1)];

ubU=[ubU;zeros(13,1)];

Ref=SX.sym('Ref',17,N+1); %期望值为17*(N+1)矩阵 % 此处只是声名,step函数中赋值

Xr=Ref(1:13,:);  % 13*(N+1)维

Ur=Ref(14:17,:);

Xk=X0;  %  xk和Uk是纯的一步状态（13维）和输入控制（4维）

for i=0:N-1

Uk=SX.sym(['U_' num2str(i)],4); ;% 4维第i步控制输入u，待求解

% cost=cost+(Xr_N(:,i+1)-Xk)'*Q_x*(Xr_N(:,i+1)-Xk)+(Ur_N(:,i+1)-Uk)'*Q_u*(Ur_N(:,i+1)-Uk);

q_e=quat_subtract(Xr(7:10,i+1),Xk(7:10));    %(7:10)的数据为4*1的四元数，q_e求四元数差值

 % cost函数参考：

cost=cost+(Xr(1:3,i+1)-Xk(1:3))'*Q_p*(Xr(1:3,i+1)-Xk(1:3))+(Xr(4:6,i+1)-Xk(4:6))'*Q_v*(Xr(4:6,i+1)-Xk(4:6))+q_e(2:4)'*Q_q*q_e(2:4)+(Xr(11:13,i+1)-Xk(11:13))'*Q_Omega*(Xr(11:13,i+1)-Xk(11:13))+(Ur(:,i+1)-Uk)'*Q_u*(Ur(:,i+1)-Uk); 

% 此部分cost对应公式前半部分

U_N={U_N{:},Uk};% 动态扩展，将Uk添加至尾部。U_N扩展为[13维状态，4维当前控制输入...，4维第i步控制输入]。

U0=[U0;1.84;1.84;1.84;1.84];

lbU=[lbU;0;0;0;0]; % max_thrust - u_max，此时lbU增至13+4*i维向量。

ubU=[ubU;8.5;8.5;8.5;8.5]; % 同理

ineq={ineq{:},Xk(7)^2+Xk(8)^2+Xk(9)^2+Xk(10)^2,Xk(11),Xk(12),Xk(13)};

lbg=[lbg;1;-Omega_max_tilt;-Omega_max_tilt;-Omega_max_tor];

ubg=[ubg;1;Omega_max_tilt;Omega_max_tilt;Omega_max_tor]; 

% 以上三句：将姿态表示限制为单位四元数，对角速度上下限进行限制

% 数组扩展变化同理

Xk1=F('X_now',Xk,'u_now',Uk); % 由F求解下一个状态

Xk=Xk1.X_next; % 更新状态

end

% 循环后U_N成为[13维状态，4维当前控制输入，4维下一步控制输入...，4维第N步控制输入]形式，后续lbg、ubg同理。

q_e=quat_subtract(Xr(7:10,end),Xk(7:10));

cost=cost+(Xr(1:3,end)-Xk(1:3))'*Q_p*(Xr(1:3,end)-Xk(1:3))+(Xr(4:6,end)-Xk(4:6))'*Q_v*(Xr(4:6,end)-Xk(4:6))+q_e(2:4)'*Q_q*q_e(2:4)+(Xr(11:13,end)-Xk(11:13))'*Q_Omega*(Xr(11:13,end)-Xk(11:13));

% end为最后一次，对应公式后半部分

ineq={ineq{:},Xk(7)^2+Xk(8)^2+Xk(9)^2+Xk(10)^2,Xk(11),Xk(12),Xk(13)};

lbg=[lbg;1;-Omega_max_tilt;-Omega_max_tilt;-Omega_max_tor];

ubg=[ubg;1;Omega_max_tilt;Omega_max_tilt;Omega_max_tor];

%create solver %下文仅创建求解器，未求解

prob=struct('f',cost,'x',vertcat(U_N{:}),'g',vertcat(ineq{:}),'p',Ref);

solver = nlpsol('solver', 'ipopt', prob);

% vertcat(U_N{:})使得求解结果x为13+4*N？维垂直向量形式

obj.mpc_solver=solver;

obj.x0=U0;   % 初始值

obj.lbx=lbU; 

obj.ubx=ubU;

obj.lbg=lbg;

obj.ubg=ubg;

参考自 用casadi求解求解具有线性约束的二次规划 - 知乎 (zhihu.com)

end

function u = stepImpl(obj,Xr_N,Ur_N,X_in)

% Implement algorithm. Calculate y as a function of input u and

% discrete states.

w0=obj.x0; 

lbw=obj.lbx;

ubw=obj.ubx;

solver=obj.mpc_solver;

lbw(1:13)=X_in; %substitute mesured state into the solver

ubw(1:13)=X_in; 

% 以上两句话以边界约束的形式将此刻状态X_in传入求解函数，也即求解结果x的前13维其实是输入的当前状态

Ref=zeros(17,21);

Ref(1:13,:)=Xr_N;  % 期望状态为矩阵，也即连续轨迹

Ref(14:17,:)=Ur_N;

sol = solver('x0', w0, 'lbx', lbw, 'ubx', ubw,...

'lbg', obj.lbg, 'ubg', obj.ubg, 'p', Ref);

u = full(sol.x(14:17)); % 结果的14-17项即为所需控制输入u

end

function resetImpl(~)

% Initialize / reset discrete-state properties

end

end

end