1 简介

This project is intended to control the motion of the Unmanned Aerial Vehicle in the real worldto follow a specifific trajectory smoothly and quickly. In this report, we will discuss how we designthe controller and trajectory generator to reach this purpose.

2 部分代码

function sdot = quadEOM(t, s, qn, controlhandle, trajhandle, params)
% QUADEOM Wrapper function for solving quadrotor equation of motion
%   quadEOM takes in time, state vector, controller, trajectory generator
%   and parameters and output the derivative of the state vector, the
%   actual calcution is done in quadEOM_readonly.
%
% INPUTS:
% t             - 1 x 1, time
% s             - 13 x 1, state vector = [x, y, z, xd, yd, zd, qw, qx, qy, qz, p, q, r]
% qn            - quad number (used for multi-robot simulations)
% controlhandle - function handle of your controller
% trajhandle    - function handle of your trajectory generator
% params        - struct, output from crazyflie() and whatever parameters you want to pass in
%
% OUTPUTS:
% sdot          - 13 x 1, derivative of state vector s
%
% NOTE: You should not modify this function
% See Also: quadEOM_readonly, crazyflie
% convert state to quad stuct for control
qd{qn} = stateToQd(s);
% Get desired_state
desired_state = trajhandle(t, qn);
% The desired_state is set in the trajectory generator
qd{qn}.pos_des      = desired_state.pos;
qd{qn}.vel_des      = desired_state.vel;
qd{qn}.acc_des      = desired_state.acc;
qd{qn}.yaw_des      = desired_state.yaw;
qd{qn}.yawdot_des   = desired_state.yawdot;
% get control outputs
[F, M, trpy, drpy] = controlhandle(qd, t, qn, params);
% compute derivative
sdot = quadEOM_readonly(t, s, F, M, params);
end

3 仿真结果

4 参考文献

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

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