控制四旋翼飞行器以进行多目标航点导航的MPC算法研究（Matlab代码实现）

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

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

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

📋📋📋本文目录如下：🎁🎁🎁

目录

 ⛳️赠与读者

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码、数据

---

 ⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学，什么是电的时候，不要觉得这些问题搞笑。哲学是科学之母，哲学就是追究终极问题，寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能让人胸中升起一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它居然给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

     或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

多目标航点导航对于四旋翼飞行器而言是一项至关重要的任务，它要求飞行器能够按照预定的路径依次飞过多个航点。为了高效精确地完成这一任务，模型预测控制（Model Predictive Control, MPC）算法被广泛应用于四旋翼飞行器的航点规划与控制中。MPC算法基于对四旋翼飞行器动力学模型的预测，通过优化未来一段时间内的控制输入来确保四旋翼飞行器以最优的方式达到每一个航点，同时满足各种约束条件（如速度、加速度限制，避障限制等）。

#### MPC算法的关键步骤:

1. **模型建立**：首先需要建立四旋翼飞行器的动力学模型或者运动学模型，这是进行预测的基础。动力学模型描述了飞行器的加速度、速度、位置与控制输入（如电机的转速）之间的关系，是实现精确控制的关键。

2. **目标函数与约束条件设置**：在MPC中，目标函数通常定义为最小化飞行器与航点之间的距离、路径平滑度或者控制力矢量的大小等，以达到精确、平稳的航点导航。同时，需要考虑如飞行高度、速度、加速度、倾角等物理约束条件，以及可能的避障约束。

3. **优化求解**：利用优化算法（如线性规划、二次规划等），在给定的约束条件下，求解目标函数在未来一段时间内的最优控制策略。优化过程需要预测每一时间步飞行器的状态，以确保控制指令的可行性和有效性。

4. **滚动优化**：MPC属于滚动时域优化的范畴，意味着在每一控制周期，都会基于当前状态重新进行未来路径的预测和优化，然后仅实施当前时刻计算得到的最优控制输入，之后循环此过程。

#### MPC在四旋翼飞行器中的应用优势:

- **适应性和灵活性**：MPC可以很好地适应四旋翼飞行器动态变化的飞行环境，如风速变化、突发避障等，因为它在每一个控制周期都会进行重新的预测和优化。
  
- **多目标优化**：MPC可以同时考虑到达预定航点的精确性、路径的平滑性以及能耗最优等多个目标，通过调整目标函数和约束条件为不同的飞行任务提供定制化的解决方案。
  
- **约束处理能力**：MPC算法内建的约束处理机制使其能够有效地处理各种物理约束和飞行安全约束，从而保证飞行器的安全和稳定运行。

#### 总结：
MPC算法在四旋翼飞行器的多目标航点导航中的应用，提供了一种高效、可靠且安全的解决方案。通过对未来路径的预测与优化，使得飞行器能够按照设定的航线平滑、精确地通过每个航点，同时满足各种飞行约束条件，非常适合于复杂环境下的飞行任务规划与控制。

📚2 运行结果

部分代码：

%% Initialisation
Ts = 0.02;                      % sample time [s]

% Weighting matrices Q, Qt, R, W and control horizon N
mpc_sim.Q   =   [1e4   0   0   0   0   0;
                 0     1e2 0   0   0   0;
                 0     0   1e4 0   0   0;
                 0     0   0   1e2 0   0;
                 0     0   0   0   1e4 0;
                 0     0   0   0   0   1e2];                    
mpc_sim.Qt  = mpc_sim.Q*1e2;    % Terminal state matrix
mpc_sim.R   = eye(3)*1e2;       % Input matrix
mpc_sim.W   = mpc_sim.Q;        % Waypoint matrix
mpc_sim.N   = ceil(2/Ts);       % Control horizon [s]

%% Ring initialisation
% Ring 1
randx1 = -1+2*rand(1);  % Generate x-coordinate for ring 1
randy1 = 1+3*rand(1);   % Generate y-coordinate for ring 1
randz1 = -1+2*rand(1);  % Generate z-coordinate for ring 1

% Ring 2
randx2 = -1+2*rand(1);  % Generate x-coordinate for ring 2
randy2 = 5+4*rand(1);   % Generate y-coordinate for ring 2
randz2 = -1+2*rand(1);  % Generate z-coordinate for ring 2

%% Simulation settings
x0 = [0 0 0 0 0 0]';            % Define start position and velocity [m and m/s]
ring1 = [randx1 0  randy1  5  randz1  0];   % [x, desired x-velocity, y, desired y-velocity, z, desired z-velocity]
ring2 = [randx2 0  randy2  5 randz2  0];    % [x, desired x-velocity, y, desired y-velocity, z, desired z-velocity]
goal = [0 0 10 0 0 0];          % Define goal position and velocity [m and m/s]
simT = 7;                       % Simulation time [s]
ulim = [1 1 1]*7;               % Maximum absolute value input [N]

%% Run simulations
% Run simulation and MPC controller. Inputs are simulation settings, start
% en goal positions, simulation time and sampling time, ring locations.
[results] = MPC_Controller(mpc_sim, x0, simT, Ts, ulim, ring1, ring2, goal);

%% Construct rings
t = linspace(0,2*pi);
x = cos(t);
y = sin(t);
z = 0*t;
pnts = [x;y;z];

n0 = [0;0;0.1]; 
n0 = n0/norm(n0);
n1 = [0;1;0]; 
n1 = n1/norm(n1); 

c = dot(n0,n1) / ( norm(n0)*norm(n1) ); % cos(theta)
s = sqrt(0.1-c*c);                        % sin(theta)
u = cross(n0,n1) / ( norm(n0)*norm(n1) ); % rotation axi

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

[1]王尧.进行多地面目标操控的四旋翼飞行器导航方法研究[D].中国民航大学[2024-04-27].DOI:CNKI:CDMD:2.1016.776641.

[2]张鑫,吴伊凡,崔永琪.基于改进MPC算法的四旋翼无人机轨迹跟踪控制[J].电子技术与软件工程, 2023(3):148-153.

[3]赵帅.四旋翼飞行器几种姿态控制算法的研究[D].广西师范大学,2017.

[4]姚婷婷.四旋翼飞行器控制算法的设计与研究[D].大连海事大学,2015.

🌈4 Matlab代码、数据