【无人机】四旋翼飞行器控制、路径规划和轨迹优化（Matlab代码实现）

 欢迎来到本博客❤️❤️❤️

博主优势：博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

​

⛳️座右铭：行百里者，半于九十

目录

1 概述

2 运行结果

2.1 情景1

2.2 情景2

‍3 参考文献

4 Matlab代码实现

---

1 概述

       在过去七年中，材料、电子、传感器和电池的进步推动了微型无人飞行器（MAV）的发展，MAV的长度在0.1-0.5米到0.1-0.5千克之间。一些小组已经建立并分析了10cm范围内的MAV。其中最小的是Picoflyer，螺旋桨直径为60毫米，质量为3.3克。50 cm范围内的平台更为普遍，一些团体已经建造并飞行了这种尺寸的系统。事实上，在这种尺寸范围内有几种商用RC直升机和研究级直升机。介绍了自主微型无人机开发面临的挑战。在本项目中，我们介绍了GRASP多用途微型飞行器试验台中使用的四旋翼建模，以支持微型飞行器协调动态飞行的研究。，其中描述了现成四转子的建模、控制和集成方法。详细文章讲解见第四部分。

2 运行结果

2.1 情景1

2.2 情景2

部分代码：

function sdot = quadEOM_readonly(t, s, F, M, params)
% QUADEOM_READONLY Solve quadrotor equation of motion
%   quadEOM_readonly calculate the derivative of the state vector
%
% 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]
% F      - 1 x 1, thrust output from controller (only used in simulation)
% M      - 3 x 1, moments output from controller (only used in simulation)
% 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

%************ EQUATIONS OF MOTION ************************
% Limit the force and moments due to actuator limits
A = [0.25,                      0, -0.5/params.arm_length;
     0.25,  0.5/params.arm_length,                      0;
     0.25,                      0,  0.5/params.arm_length;
     0.25, -0.5/params.arm_length,                      0];

prop_thrusts = A*[F;M(1:2)]; % Not using moment about Z-axis for limits
prop_thrusts_clamped = max(min(prop_thrusts, params.maxF/4), params.minF/4);

B = [                 1,                 1,                 1,                  1;
                      0, params.arm_length,                 0, -params.arm_length;
     -params.arm_length,                 0, params.arm_length,                 0];
F = B(1,:)*prop_thrusts_clamped;
M = [B(2:3,:)*prop_thrusts_clamped; M(3)];

% Assign states
x = s(1);
y = s(2);
z = s(3);
xdot = s(4);
ydot = s(5);
zdot = s(6);
qW = s(7);
qX = s(8);
qY = s(9);
qZ = s(10);
p = s(11);
q = s(12);
r = s(13);

quat = [qW; qX; qY; qZ];
bRw = QuatToRot(quat);
wRb = bRw';

% Acceleration
accel = 1 / params.mass * (wRb * [0; 0; F] - [0; 0; params.mass * params.grav]);

% Angular velocity
K_quat = 2; %this enforces the magnitude 1 constraint for the quaternion
quaterror = 1 - (qW^2 + qX^2 + qY^2 + qZ^2);
qdot = -1/2*[0, -p, -q, -r;...
             p,  0, -r,  q;...
             q,  r,  0, -p;...
             r, -q,  p,  0] * quat + K_quat*quaterror * quat;

% Angular acceleration
omega = [p;q;r];
pqrdot   = params.invI * (M - cross(omega, params.I*omega));

% Assemble sdot
sdot = zeros(13,1);
sdot(1)  = xdot;
sdot(2)  = ydot;
sdot(3)  = zdot;
sdot(4)  = accel(1);
sdot(5)  = accel(2);
sdot(6)  = accel(3);
sdot(7)  = qdot(1);
sdot(8)  = qdot(2);
sdot(9)  = qdot(3);
sdot(10) = qdot(4);
sdot(11) = pqrdot(1);
sdot(12) = pqrdot(2);
sdot(13) = pqrdot(3);

end
 

‍3 参考文献

[1]谭凯元,朱嘉林,邓君,王荔蔷,杨家源.基于双目视觉的SLAM四旋翼无人机[J].机电工程技术,2022,51(09):83-87+141.

[2]李帅,张杰,杜立杰,李盼.工程测量中无人机航测技术的应用[J].黑龙江科学,2022,13(16):56-58.

4 Matlab代码实现