【四旋翼控制】基于双环纯P控制器级联 外环调节姿态，内环控制电机推力实现快速干扰抑制和精确设定点跟踪附Matlab代码
✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。
🍎 往期回顾关注个人主页：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。
🔥 内容介绍
一、背景
（一）四旋翼飞行器的特点与控制挑战
四旋翼飞行器作为一种典型的欠驱动系统，具有结构简单、机动性强、垂直起降等优点，在航拍、物流配送、农业植保、搜索救援等领域得到了广泛应用。然而，其动力学模型具有高度非线性、强耦合性以及易受外界干扰等特性，这给精确控制带来了巨大挑战。例如，在执行航拍任务时，四旋翼飞行器需要在复杂多变的环境中保持稳定姿态，以获取高质量的图像；在物流配送中，要准确地跟踪设定的飞行路径，将货物安全送达目的地。因此，设计一种高效的控制策略对于四旋翼飞行器的稳定运行和精确控制至关重要。
（二）传统控制方法的局限性
传统的单环控制方法，如单纯的比例 - 积分 - 微分（PID）控制，在处理四旋翼飞行器这种复杂系统时存在一定局限性。由于四旋翼飞行器的姿态和位置控制之间存在强耦合关系，单环控制难以同时兼顾姿态调整和位置跟踪的性能，尤其在面对快速变化的外界干扰时，控制效果往往不尽如人意。例如，当遇到突然的阵风干扰时，单环控制可能无法迅速有效地调整飞行器的姿态，导致飞行轨迹偏离设定路径。
（三）双环纯 P 控制器级联的优势
基于双环纯 P 控制器级联的控制策略，通过外环调节姿态、内环控制电机推力的方式，能够有效地解耦姿态和位置控制之间的复杂关系，增强系统对干扰的抑制能力，提高设定点跟踪的精度。这种控制策略结构相对简单，易于实现，且在实际应用中表现出良好的动态性能和鲁棒性。它能够快速响应外界干扰，使四旋翼飞行器在各种复杂环境下都能稳定地跟踪设定点，满足不同应用场景的需求。
二、原理
（一）四旋翼飞行器动力学模型基础
（三）快速干扰抑制和精确设定点跟踪原理
- 快速干扰抑制
：当四旋翼飞行器受到外界干扰（如阵风、气流等）时，飞行器的姿态会发生变化，导致实际姿态角与设定姿态角之间产生误差。外环姿态控制器检测到这个误差后，迅速计算出期望的力矩变化，通过内环电机推力控制器快速调整电机推力，使飞行器产生相应的力矩来抵消干扰的影响，恢复到设定的姿态。由于双环控制结构能够快速响应姿态误差，并且内环直接作用于电机推力，所以能够在短时间内抑制干扰，保持飞行器的稳定姿态。例如，当遇到阵风使飞行器产生滚转偏差时，外环控制器立即计算出纠正滚转的期望力矩，内环快速调整电机推力，使飞行器恢复到正确的滚转姿态。
- 精确设定点跟踪
：在跟踪设定点时，外环姿态控制器根据设定的姿态值与实际姿态值的误差，不断调整期望的力矩，使飞行器的姿态逐渐向设定姿态靠近。内环电机推力控制器则根据外环的期望力矩，精确地控制电机推力，确保飞行器按照期望的姿态变化，从而实现精确的位置跟踪。由于外环和内环的协同工作，能够不断地修正姿态和位置误差，使飞行器准确地跟踪设定点。例如，在飞行器按照设定路径飞行时，外环根据路径所需的姿态调整期望力矩，内环通过精确控制电机推力使飞行器保持正确姿态，从而精确地沿着设定路径飞行。
基于双环纯 P 控制器级联的控制策略，通过合理设计外环和内环的控制参数，能够有效地实现四旋翼飞行器的快速干扰抑制和精确设定点跟踪，为四旋翼飞行器在各种复杂环境下的稳定运行和精确控制提供了有力保障。
⛳️ 运行结果
📣 部分代码
positionData = out.position.Data;
attitudeData = -out.attitude.Data;
x = squeeze(positionData(1, 1, :));
y = squeeze(positionData(2, 1, :));
h = -squeeze(positionData(3, 1, :));
% Set up the figure
fig1 = figure; hold on;
view(3);
grid on;
set(fig1, 'Position', [100, 000, 1200, 700]); % Wider and taller window
% Partition settings and UAV arm length
partition = size(x, 1) / 3;
lineLength = l * 100 / 2;
dataLength = size(x, 1);
bounds = max([max(abs(x)), max(abs(y)), max(abs(h))]) + 5;
% Set axis limits explicitly so they remain constant throughout
xlim([-bounds, bounds]);
ylim([-bounds, bounds]);
zlim([-bounds, bounds]);
xlabel('X-axis');
ylabel('Y-axis');
zlabel('H-axis');
title('Flight Data of UAV');
% Plot the entire flight trajectory in the background
plot3(x, y, h, 'b-', 'LineWidth', 2);
% Plot the UAV in its initial position (one marker)
uavHandle = plot3(x(1), y(1), h(1), 'ko', 'MarkerFaceColor', 'k', 'MarkerSize', 3);
% Initialize
sampleIndex = round(0 * dataLength / partition);
sampleIndex = min(max(sampleIndex, 1), dataLength);
roll = squeeze(attitudeData(1, 1, sampleIndex));
pitch = squeeze(attitudeData(2, 1, sampleIndex));
yaw = squeeze(attitudeData(3, 1, sampleIndex));
R = eul2rotm([yaw, pitch, roll], 'ZYX');
midpoint = [x(sampleIndex); y(sampleIndex); h(sampleIndex)];
% UAV arms (rotated according to its orientation)
uavArm1 = [[x(sampleIndex), x(sampleIndex) + lineLength/2];
[y(sampleIndex), y(sampleIndex)];
[h(sampleIndex), h(sampleIndex)]];
uavArm1 = rotatePoints(uavArm1, R, midpoint);
uavArm3 = [[x(sampleIndex) - lineLength/2, x(sampleIndex)];
[y(sampleIndex), y(sampleIndex)];
[h(sampleIndex), h(sampleIndex)]];
uavArm3 = rotatePoints(uavArm3, R, midpoint);
uavArm2 = [[x(sampleIndex), x(sampleIndex)];
[y(sampleIndex) - lineLength/2, y(sampleIndex) + lineLength/2];
[h(sampleIndex), h(sampleIndex)]];
uavArm2 = rotatePoints(uavArm2, R, midpoint);
% Propellers
circle1 = propellerPoints(midpoint, [lineLength / 2, 0], R);
circle2 = propellerPoints(midpoint, [-lineLength / 2, 0], R);
circle3 = propellerPoints(midpoint, [0, lineLength / 2], R);
circle4 = propellerPoints(midpoint, [0, -lineLength / 2], R);
% Pre-allocate handles for arms and propellers
arm1Handle = plot3(uavArm1(1, :), uavArm1(2, :), uavArm1(3, :), 'black', 'LineWidth', 2); % Red line for arm1
arm2Handle = plot3(uavArm2(1, :), uavArm2(2, :), uavArm2(3, :), 'r-', 'LineWidth', 2); % Red line for arm2
arm3Handle = plot3(uavArm3(1, :), uavArm3(2, :), uavArm3(3, :), 'r-', 'LineWidth', 2); % Red line for arm2
propHandle1 = plot3(circle1(1, :), circle1(2, :), circle1(3, :), 'blue', 'MarkerSize', 5); % Green dot for propeller1
propHandle2 = plot3(circle2(1, :), circle2(2, :), circle2(3, :), 'blue', 'MarkerSize', 5); % Green dot for propeller2
propHandle3 = plot3(circle3(1, :), circle3(2, :), circle3(3, :), 'g.', 'MarkerSize', 5); % Green dot for propeller3
propHandle4 = plot3(circle4(1, :), circle4(2, :), circle4(3, :), 'g.', 'MarkerSize', 5); % Green dot for propeller4
disp(true)
% Loop to animate UAV movement
while true
for part = 1: partition
sampleIndex = round(part * dataLength / partition);
sampleIndex = min(max(sampleIndex, 1), dataLength);
roll = squeeze(attitudeData(1, 1, sampleIndex));
pitch = squeeze(attitudeData(2, 1, sampleIndex));
yaw = squeeze(attitudeData(3, 1, sampleIndex));
R = [roll, pitch, yaw];
midpoint = [x(sampleIndex); y(sampleIndex); h(sampleIndex)];
% UAV arms (rotated according to its orientation)
uavArm1 = [[x(sampleIndex), x(sampleIndex) + lineLength/2];
[y(sampleIndex), y(sampleIndex)];
[h(sampleIndex), h(sampleIndex)]];
uavArm1 = rotatePoints(uavArm1, R, midpoint);
uavArm3 = [[x(sampleIndex) - lineLength/2, x(sampleIndex)];
[y(sampleIndex), y(sampleIndex)];
[h(sampleIndex), h(sampleIndex)]];
uavArm3 = rotatePoints(uavArm3, R, midpoint);
uavArm2 = [[x(sampleIndex), x(sampleIndex)];
[y(sampleIndex) - lineLength/2, y(sampleIndex) + lineLength/2];
[h(sampleIndex), h(sampleIndex)]];
uavArm2 = rotatePoints(uavArm2, R, midpoint);
% Propellers
circle1 = propellerPoints(midpoint, [lineLength / 2, 0], R);
circle2 = propellerPoints(midpoint, [-lineLength / 2, 0], R);
circle3 = propellerPoints(midpoint, [0, lineLength / 2], R);
circle4 = propellerPoints(midpoint, [0, -lineLength / 2], R);
% Update UAV position in the plot
figure(fig1);
set(uavHandle, 'XData', x(sampleIndex), 'YData', y(sampleIndex), 'ZData', h(sampleIndex));
% Update the arms (as lines)
set(arm1Handle, 'XData', uavArm1(1, :), 'YData', uavArm1(2, :), 'ZData', uavArm1(3, :));
set(arm2Handle, 'XData', uavArm2(1, :), 'YData', uavArm2(2, :), 'ZData', uavArm2(3, :));
set(arm3Handle, 'XData', uavArm3(1, :), 'YData', uavArm3(2, :), 'ZData', uavArm3(3, :));
% Update the propellers (as circles or points)
set(propHandle1, 'XData', circle1(1, :), 'YData', circle1(2, :), 'ZData', circle1(3, :));
set(propHandle2, 'XData', circle2(1, :), 'YData', circle2(2, :), 'ZData', circle2(3, :));
set(propHandle3, 'XData', circle3(1, :), 'YData', circle3(2, :), 'ZData', circle3(3, :));
set(propHandle4, 'XData', circle4(1, :), 'YData', circle4(2, :), 'ZData', circle4(3, :));
% Pause for animation effect
pause(0.009);
end
end
hold off;