Matlab仿真——基于NMPC实现飞行机器人运动规划

本文主要是介绍Matlab仿真——基于NMPC实现飞行机器人运动规划，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

本文参考Matlab官方例程实现。

【问题描述】

假设有一台飞行机器人，如下，其位置用 $(x,y,\theta)$ 表示，速度为 $(v_x,v_y,w)$ ，一共有四个推力矢量输入，分别用 $(u_1,u_2,u_3,u_4)$ 表示

【数学模型表示】

建立上述机器人的状态空间模型[1]，状态表示为 $X$

$X=\left[ x,y,\theta ,v_x,v_y,w \right]$

由模型可得：

$\left\{ \begin{array}{l} \dot{x}_1=x_4\\ \dot{x}_2=x_5\\ \dot{x}_3=x_6\\ \dot{x}_4=\left( u_1-u_2 \right) +\left( u_3-u_4 \right) *\cos \left( x_3 \right)\\ \dot{x}_5=\left( u_1-u_2 \right) +\left( u_3-u_4 \right) *\sin \left( x_3 \right)\\ \dot{x}_6=\alpha \left( u_1-u_2 \right) +\beta \left( u_3-u_4 \right)\\ \end{array} \right.$

【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:nunlobj.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:iterxx = x(i,1);yy = x(i,2);theta = x(i,3);drawRectangle(xx, yy, theta);hold on;endxlabel('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.

这篇关于Matlab仿真——基于NMPC实现飞行机器人运动规划的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！