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

片酷

已于 2023-08-19 11:24:18 修改

阅读量976

点赞数 5

文章标签： matlab 机器人开发语言

于 2023-08-17 19:51:42 首次发布

本文链接：https://blog.csdn.net/weixin_55188463/article/details/132347765

版权

本文参考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: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.