【无人机控制】基于PID实现无人机巡航控制附Matlab代码

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

更多Matlab仿真内容点击👇

智能优化算法       神经网络预测       雷达通信       无线传感器        电力系统

信号处理              图像处理               路径规划       元胞自动机        无人机 

⛄ 内容介绍

我们将重点放在了无人机外环控制上。这一部分我们采用了传统的PID控制模块对无人机飞行进行控制。PID的原型可以表示为：

它是一个根据反馈误差来修正输出函数的过程。K0，K1，K2这三个参数可以为0，那么就形成了PI、PD、ID、PID这几种形式。一般PID、PD这两种用的比较多。而我们使用的为PD。

在确定好航线之后，我们可以得到无人机巡航仿真路线图，位移图与速度图如下：

⛄ 部分代码

function [t_out, s_out, QP] = simulation_3d_new(trajhandle, controlhandle)

%[t_out, s_out] = simulation_3d(trajhandle, controlhandle)

% NOTE: This srcipt will not run as expected unless you fill in proper

% code in trajhandle and controlhandle

% You should not modify any part of this script except for the

% visualization part

%

% ***************** QUADROTOR SIMULATION *****************

% *********** YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW **********

addpath('utils');

% real-time

real_time = true;

% max time

max_time = 100;

% parameters for simulation

params = sys_params;

%% **************************** FIGURES *****************************

disp('Initializing figures...');

h_fig = figure;

h_3d = gca;

axis equal

grid on

%view(3);

view(48.8, 25.8);

xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]')

quadcolors = lines(1);

set(gcf,'Renderer','OpenGL')

%% *********************** INITIAL CONDITIONS ***********************

disp('Setting initial conditions...');

tstep    = 0.01; % this determines the time step at which the solution is given

cstep    = 0.05; % image capture time interval

max_iter = max_time/cstep; % max iteration

nstep    = cstep/tstep;

time     = 0; % current time

err = []; % runtime errors

% Get start and stop position

des_start = trajhandle(0, []);

des_stop  = trajhandle(inf, []);

stop_pos  = des_stop.pos;

x0    = init_state(des_start.pos, 0);

xtraj = zeros(max_iter*nstep, length(x0));

ttraj = zeros(max_iter*nstep, 1);

x       = x0;        % state

pos_tol = 0.01;

vel_tol = 0.01;

%% ************************* RUN SIMULATION *************************

disp('Simulation Running....');

% Main loop

for iter = 1:max_iter

    timeint = time:tstep:time+cstep;

    tic;

    % Initialize quad plot

    if iter == 1

        QP = QuadPlot(1, x0, 0.1, 0.04, quadcolors(1,:), max_iter, h_3d);

        current_state = stateToQd(x);

        desired_state = trajhandle(time, current_state);

        QP.UpdateQuadPlot(x, [desired_state.pos; desired_state.vel], time);

        h_title = title(sprintf('iteration: %d, time: %4.2f', iter, time));

    end

    % Run simulation

    [tsave, xsave] = ode45(@(t,s) quadEOM(t, s, controlhandle, trajhandle, params), timeint, x);

    x    = xsave(end, :)';

    % Save to traj

    xtraj((iter-1)*nstep+1:iter*nstep,:) = xsave(1:end-1,:);

    ttraj((iter-1)*nstep+1:iter*nstep) = tsave(1:end-1);

    % Update quad plot

    current_state = stateToQd(x);

    desired_state = trajhandle(time + cstep, current_state);

    QP.UpdateQuadPlot(x, [desired_state.pos; desired_state.vel], time + cstep);

    set(h_title, 'String', sprintf('iteration: %d, time: %4.2f', iter, time + cstep))

    time = time + cstep; % Update simulation time

    t = toc;

    % Check to make sure ode45 is not timing out

    if(t> cstep*50)

        err = 'Ode45 Unstable';

        break;

    end

    % Pause to make real-time

    if real_time && (t < cstep)

        pause(cstep - t);

    end

    % Check termination criteria

    if terminate_check(x, time, stop_pos, pos_tol, vel_tol, max_time)

        break

    end

end

%% ************************* POST PROCESSING *************************

% Truncate xtraj and ttraj

xtraj = xtraj(1:iter*nstep,:);

ttraj = ttraj(1:iter*nstep);

% Truncate saved variables

QP.TruncateHist();

% Plot position

h_pos = figure('Name', ['Quad position']);

plot_state(h_pos, QP.state_hist(1:3,:), QP.time_hist, 'pos', 'vic');

plot_state(h_pos, QP.state_des_hist(1:3,:), QP.time_hist, 'pos', 'des');

% Plot velocity

h_vel = figure('Name', ['Quad velocity']);

plot_state(h_vel, QP.state_hist(4:6,:), QP.time_hist, 'vel', 'vic');

plot_state(h_vel, QP.state_des_hist(4:6,:), QP.time_hist, 'vel', 'des');

if(~isempty(err))

%    error(err);

end

disp('finished.')

t_out = ttraj;

s_out = xtraj;

end

⛄ 运行结果

图中蓝线为理想状况下无人机飞行路线情况以及参数情况，红线为在考虑重力与呈正态分布的干扰噪声环境下无人机的路线情况以及参数情况。

在此基础上，为了方便初学者对于无人机飞行情况真实把握以及感受PID参数调节对无人机的影响，我们建立了用户交互界面。用户可以自主调节PID参数并自主控制无人机飞行，通过这种方式来模拟无人机飞行的真实情况，并了解PID参数是如何影响无人机飞行状态的。

⛄ 参考文献

[1] 文锟.基于计算动词PID控制器的四旋翼无人机控制及地面控制系统设计[J].  2013.

[2] 穆鲁肯.无人机控制器的建模与比较研究[D].大连理工大学,2016.

[3] 邵霖文,廖芳,丁黎明,等.基于MATLAB的四旋翼无人机飞控PID仿真设计[J].山西电子技术, 2021(005):000.

⛳️ 代码获取关注我

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料

​