基于四足动物动态运动的无表征模型预测控制——用于无人机（Matlab代码实现）

本文链接：https://blog.csdn.net/m0_73907476/article/details/131274518

💥1 概述

📚2 运行结果

🎉3 参考文献

👨‍💻4 Matlab代码

💥1 概述

射频（Radio frequency，缩写为RF），又称无线电频率、无线射频、高周波，为在3kHz至300 GHz这个范围内的震荡频率，这个频率相当于无线电波的频率，以及携带着无线电信号的交流电的频率。RF通常被用来指电子震荡，而不被用在机械震荡上，然而机械射频系统仍然是存在的（如机械滤波器与射频微机电系统（英语：RF MEMS））。

无表示模型预测控制（RF-MPC）是用于动态腿机器人的 MATLAB 仿真框架。RF-MPC使用旋转矩阵表示方向，因此不存在与欧拉角相关的奇点问题。旋转矩阵上的线性动力学是使用基于变化的线性化（VBL）得出的。

RF-MPC算法最初是为四足机器人设计的，经过一些修改后适用于无人机。

📚2 运行结果

主函数部分代码：

%% initialization
clear all;close all;clc
addpath(genpath("Utils"));
addpath fcns fcns_MPC

%% --- parameters ---
% ---- gait ----
% 0-control; 1-traj; 
gait = 1;
p = get_params_UAV(gait);

p.playSpeed = 1;
p.flag_movie = 1;       % 1 - make movie
use_qpSWIFT = 0;        % 0 - quadprog, 1 - qpSWIFT (external)


dt_sim = p.simTimeStep;
SimTimeDuration =p.SimTimeDuration;  % [sec]
MAX_ITER = floor(SimTimeDuration/p.simTimeStep);

% desired trajectory
p.acc_d = 0; %acceleration
p.vel_d = [0;0];% x,y
p.yaw_d = 2;%yaw

%% Model Predictive Control
% --- initial condition ---
% Xt = [pc dpc vR wb]': [18,1] represents the robot's position, velocity, attitude, and angular velocity information.
% Ut=[t1 t2 t3 t4]' : [4,1]  represents thrusts of four propeller

 
[Xt,Ut] = fcn_gen_XdUd_UAV(0,[],p);


% --- logging ---
tstart = 0;
tend = dt_sim;

[tout,Xout,Uout,Xdout,Udout,Uext,FSMout] = deal([]);

% --- simulation ----
h_waitbar = waitbar(0,'Calculating...');
tic
% 离线计算
for ii = 1:MAX_ITER
    % --- time vector ---
    t_ = dt_sim * (ii-1) + p.Tmpc * (0:p.predHorizon-1);%MPC horizon

    % --- Traj ---
 
    if gait == 1
        % Ud: the simple force allocation of gravity based on the number of propeller.
       [p,Xd,Ud] = fcn_UAV_ref_traj(t_, Xt, p);
    else
        [Xd,Ud] = fcn_gen_XdUd_UAV(t_,Xt,p);
    end

    % --- MPC ----
    % form QP 
    [H,g,Aineq,bineq,Aeq,beq] = fcn_get_QP_form_eta_UAV(Xt,Ut,Xd,Ud,p);

    if ~use_qpSWIFT
        % solve QP using quadprog
        % options1 = optimset('Display', 'iter');
        % [zval] = quadprog(H,g,Aineq,bineq,Aeq,beq,[],[],[],options1);
        [zval] = quadprog(H,g,Aineq,bineq,Aeq,beq,[],[],[]);
    else
        % interface with the QP solver qpSWIFT
        [zval,basic_info] = qpSWIFT(sparse(H),g,sparse(Aeq),beq,sparse(Aineq),bineq);
    end
    Ut = Ut + zval(1:4);

    % --- external disturbance ---
    [u_ext,p_ext] = fcn_get_disturbance(tstart,p);
    p_ext=0*p_ext;
    u_ext = 0*u_ext;
    p.p_ext = p_ext;        % position of external force


    % --- simulate ---
    [t,X] = ode45(    @(t,X)dynamics_SRB_UAV(t,X,Ut,Xd,0*u_ext,p),  [tstart,tend] ,Xt);


    % --- update ---
    %  更新
    Xt = X(end,:)';
    tstart = tend;
    tend = tstart + dt_sim;

    % --- log ---
    %  拼接和记录
    lent = length(t(2:end));
    tout = [tout;t(2:end)];
    Xout = [Xout;X(2:end,:)];
    Uout = [Uout;repmat(Ut',[lent,1])];
    Xdout = [Xdout;repmat(Xd(:,1)',[lent,1])];
    Udout = [Udout;repmat(Ud(:,1)',[lent,1])];
    Uext = [Uext;repmat(u_ext',[lent,1])];
    %     FSMout = [FSMout;repmat(FSM',[lent,1])];