本文参考Matlab官方例程实现。
【问题描述】
假设有一台飞行机器人,如下,其位置用表示,速度为
,一共有四个推力矢量输入,分别用
表示
【数学模型表示】
建立上述机器人的状态空间模型[1],状态表示为
由模型可得:
【MPC控制】
建立完状态空间表达式,接下来是MPC控制器的实现,相关原理可参考其他博客,在这里使用Matlab工具箱实现。
代价函数:最小化输入量
终端约束:到达目标位置
【代码实现】
分为4个m文件,将它们放在同一文件夹下,运行demo.m文件即可
1.demo:MPC控制器与相关函数调用
%% 自由飞行机器人
clear;clc;
% 定义控制器
nx = 6;
ny = 6;
nu = 4;
nlobj = nlmpc(nx,ny,nu);
% 状态空间方程
nlobj.Model.StateFcn = "FlyingRobotStateFcn";
% 时间参数
Ts = 0.4;
p = 30;
nlobj.Ts = Ts;
% 设置预测时域与控制时域相等
nlobj.PredictionHorizon = p;
nlobj.ControlHorizon = p;
% 代价函数
nlobj.Optimization.CustomCostFcn = @(X,U,e,data) Ts*sum(sum(U(1:p,:)));
% 替代标准代价函数
nlobj.Optimization.ReplaceStandardCost = true;
% 终端等式约束
nlobj.Optimization.CustomEqConFcn = @(X,U,data) X(end,:)';
% 输入约束
for ct = 1:nu
nlobj.MV(ct).Min = 0;
nlobj.MV(ct).Max = 1;
end
% 声明初始条件
x0 = [-10;-10;pi/2;0;0;0]; % robot parks at [-10, -10], facing north
u0 = zeros(nu,1); % thrust is zero
% 验证用户提供的模型
validateFcns(nlobj,x0,u0);
% 控制器计算
[~,~,info] = nlmpcmove(nlobj,x0,u0);
%% 绘图
disp(info)
% 执行控制输入
FlyingRobotPlotPlanning(info);
2.drawRectangle:输入x,y,theta绘制方块
function drawRectangle(x, y, theta)
% 定义矩形的宽度和高度
width = 0.8;
height = 0.4;
% 计算矩形的四个顶点坐标
vertices = [
-width/2, -height/2;
-width/2, height/2;
width/2, height/2;
width/2, -height/2
];
% 定义旋转矩阵
R = [cos(theta), -sin(theta); sin(theta), cos(theta)];
% 对矩形的顶点进行旋转和平移
rotatedVertices = (R * vertices')' + [x, y];
% 绘制矩形
hold on;
plot(rotatedVertices([1:end, 1], 1), rotatedVertices([1:end, 1], 2), 'r');
axis equal;
grid on;
hold off;
end
3.FlyingRobotStateFcn:模型状态空间表达式
function out = FlyingRobotStateFcn(x,u)
% 自由飞行机器人状态空间方程
x_dot = x(4,:);
y_dot = x(5,:);
theta_dot = x(6,:);
T1 = u(1) - u(2);
T2 = u(3) - u(4);
vx_dot = (T1 + T2)*cos(x(3,:));
vy_dot = (T1 + T2)*sin(x(3,:));
alpha = 0.2;
beta = 0.2;
w_dot = alpha*T1 - beta*T2;
out = [x_dot;y_dot;theta_dot;vx_dot;vy_dot;w_dot];
end
4.FlyingRobotPlotPlanning:绘制曲线与机器人动作
function FlyingRobotPlotPlanning(info)
x = info.Xopt;
t = info.Topt;
%% 绘制曲线
figure(1)
states = {'x','y','theta','vx','vy','omega'};
color = ['r', 'g', 'b', 'm', 'y', 'k'];
for i = 1:size(x,2)
subplot(3,2,i)
plot(t,x(:,i), 'color', color(i), 'Marker', 'o')
title(states{i})
end
%% 绘制动画
figure(2)
iter = size(x,1);
for i = 1:iter
xx = x(i,1);
yy = x(i,2);
theta = x(i,3);
drawRectangle(xx, yy, theta);
hold on;
end
xlabel('x')
ylabel('y')
title('Optimal Trajectory')
end
【仿真结果】
贴一下仿真结果的曲线和效果
参考文献:
[1]Y. Sakawa. "Trajectory planning of a free-flying robot by using the optimal control." Optimal Control Applications and Methods, Vol. 20, 1999, pp. 235-248.