多旋翼无人机横向飞行的时间最优轨迹规划研究（Matlab代码实现）
💥💥💞💞欢迎来到本博客❤️❤️💥💥
🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。
⛳️座右铭：行百里者，半于九十。
📋📋📋本文内容如下：🎁🎁🎁
⛳️赠与读者
👨💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能解答你胸中升起的一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。
或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎
💥第一部分——内容介绍
多旋翼无人机横向飞行的时间最优轨迹规划
摘要：本文聚焦于多旋翼无人机在横向飞行过程中的时间最优轨迹规划问题。首先介绍了不考虑旋转动力学时多旋翼无人机的横向飞行动力学模型，接着阐述了基于庞特里亚金最优性条件推导时间最优输入和状态轨迹的方法，并展示了相关数值结果。随后，通过欧拉离散化将时间最优轨迹规划问题构建为非线性规划（NLP）问题，给出了该NLP问题的数值解，并与之前结果进行比较。进一步，将旋转动力学引入横向飞行动力学模型，再次应用NLP方法求解并分析结果。本文回顾的两种方法为多旋翼无人机轨迹规划提供了有效途径，所得轨迹可作为模型预测控制应用的参考轨迹。
关键词：多旋翼无人机；横向飞行；时间最优轨迹规划；庞特里亚金最优性条件；非线性规划
一、引言
随着自动驾驶技术的飞速发展，多旋翼无人机作为重要的自动驾驶载体，在摄影、精准农业、三维重建、监控以及医疗物资运输等众多领域得到了广泛应用。为了准确评估多旋翼无人机的飞行机动性能以及新型多旋翼设计的飞行特性，建立多旋翼无人机轨迹的基准至关重要。其中，时间最优轨迹规划是评估无人机性能的关键指标之一，它能够帮助我们找到无人机在特定任务下完成飞行所需的最短时间轨迹，从而优化无人机的飞行效率和性能。
本文重点关注多旋翼无人机在垂直和水平位移过程中的翻转机动动作下的横向飞行轨迹优化方法。在简化情形下，将多旋翼无人机的动力学特性近似为双旋翼无人机的动力学特性，以便于进行理论分析和计算。同时，本文将文献[1]中提出的优化算法与更为通用的轨迹优化算法进行对比，旨在探索更高效、准确的轨迹规划方法。
二、不考虑旋转动力学的多旋翼无人机横向飞行动力学
在不考虑旋转动力学的情况下，多旋翼无人机的横向飞行动力学可以简化为一个相对简单的模型。假设无人机在水平面内进行横向飞行，忽略其绕自身轴的旋转运动。此时，无人机的运动主要受到推力和空气阻力的影响。
此动力学模型描述了无人机在横向平面内的基本运动规律，为后续的轨迹规划提供了基础。
三、基于庞特里亚金最优性条件的时间最优轨迹规划
（一）优化问题的构建
时间最优轨迹规划的目标是在满足无人机动力学约束和边界条件的情况下，找到使飞行时间 T 最小的输入 Fx(t) 和状态轨迹 s(t)。数学上，该优化问题可以表示为：
（二）庞特里亚金最优性条件
（三）边值问题（BVP）的构建
通过上述最优性条件，可以得到一个边值问题。边界条件为初始状态 s(0)=s0 和终端状态 s(T)=sf，以及协态变量的终端条件（通常需要根据具体问题确定）。该边值问题描述了最优状态轨迹和协态变量的变化规律。
（四）射击法（STO）算法
文献[1]中提出了使用射击法求解上述边值问题。射击法的基本思想是通过不断调整初始协态变量 λ(0)，使得状态轨迹在终端时刻满足终端条件。具体步骤如下：
- 猜测初始协态变量 λ(0)。
- 数值求解状态方程和协态方程，得到状态轨迹 s(t) 和协态轨迹 λ(t)。
- 检查终端状态 s(T) 是否满足终端条件。如果不满足，调整初始协态变量 λ(0)，重复步骤 2。
- 当终端状态满足终端条件时，此时的输入轨迹和状态轨迹即为最优解。
四、基于庞特里亚金最优性条件的数值结果
将上述优化问题应用于不考虑旋转动力学的多旋翼无人机横向飞行动力学模型，通过射击法进行数值求解。假设初始状态 s0=[00]，终端状态 sf=[xf0]，其中 xf 为预设的横向位移。
经过数值计算，得到时间最优的输入轨迹 Fx∗(t) 和状态轨迹 s∗(t)。结果表明，无人机在飞行过程中，推力 Fx 会根据无人机的速度和位置进行动态调整，以实现最短时间的横向飞行。同时，状态轨迹显示无人机能够准确地从初始位置到达终端位置。
五、基于非线性规划（NLP）的时间最优轨迹规划
（一）欧拉离散化
为了将时间最优轨迹规划问题构建为非线性规划问题，首先对多旋翼无人机的动力学进行欧拉离散化。将时间区间 [0,T] 划分为 N 个子区间，每个子区间的长度为 Δt=NT。则状态方程可以离散化为：
（二）NLP问题的构建
将时间最优轨迹规划问题转化为NLP问题，目标函数为最小化总时间 T，可以表示为：
此外，还可以根据实际情况添加输入约束和状态约束，例如推力的上下限约束等。
（三）步间时间作为优化变量
六、NLP问题的数值结果及与PMP方法的比较
利用CasADi软件工具箱求解上述NLP问题。假设与基于庞特里亚金最优性条件的方法相同的初始状态和终端状态，进行数值计算。
结果表明，通过NLP方法得到的时间最优轨迹与基于庞特里亚金最优性条件的方法得到的结果极为相似。这表明NLP方法能够有效地解决多旋翼无人机横向飞行的时间最优轨迹规划问题，而且NLP的构建方式更加通用，无需推导PMP的最优性条件，适用于更为一般的问题情形。
七、引入旋转动力学的多旋翼无人机横向飞行动力学
在实际飞行中，多旋翼无人机的旋转动力学对其飞行轨迹有重要影响。引入旋转动力学后，无人机的状态变量不仅包括横向位移 x 和横向速度 v，还包括绕各轴的角速度 ωx,ωy,ωz 和姿态角 ϕ,θ,ψ。
此时，无人机的动力学方程更加复杂，涉及到推力、重力、空气动力以及陀螺效应等多个因素。具体动力学方程可根据无人机的具体结构和受力分析得到。
八、引入旋转动力学后的NLP问题数值结果
将第5节中介绍的NLP问题应用于引入旋转动力学的多旋翼无人机横向飞行动力学模型。同样利用CasADi软件工具箱进行求解。
数值结果显示，考虑旋转动力学后，无人机的最优轨迹发生了变化。推力和姿态角需要根据旋转动力学进行动态调整，以实现时间最优的横向飞行。与不考虑旋转动力学的情况相比，飞行时间可能会有所增加，这是因为旋转运动需要消耗额外的能量和时间来调整无人机的姿态。
九、结论
本文回顾了两种计算多旋翼无人机横向飞行时间最优状态与输入轨迹的方法。第一种方法基于庞特里亚金极大值原理，通过推导最优性必要条件构建边值问题，并使用射击法求解。第二种方法采用欧拉法对无人机动力学进行离散化，构建非线性规划问题，利用CasADi软件工具求解。
数值结果表明，两种方法得到的结果极为相似，但NLP方法具有更强的通用性，适用于更为复杂的问题情形。同时，考虑旋转动力学后，无人机的最优轨迹会发生变化，飞行时间可能会增加。
这些轨迹规划方法可为多旋翼无人机的模型预测控制应用提供参考轨迹，有助于提高无人机的飞行性能和效率。未来的研究可以进一步考虑更复杂的环境因素和不确定性，以提高轨迹规划的鲁棒性和实用性。
📚第二部分——运行结果
部分代码：
%% Scenario choice
scenario = 1; % 1: Scenario 1, 2: Scenario 2
%% Save log of results option
save_log = 0; % 0: Save results in .mat file, 1: Do not save results in .mat file.
%% Setup
% Set discretization variables
T = MX.sym('T'); % Final time variable
N = 400; % Number of discrete-time iteration steps considered, increase if solution is infeasible
dt = T/N; % time step
% Declare model variables
nx = 6;
x1 = SX.sym('x1'); % Horizontal position x
x2 = SX.sym('x2'); % Horizontal velocity \dot{x}
x3 = SX.sym('x3'); % Vertical position z
x4 = SX.sym('x4'); % Vertical velocity \dot{z}
x5 = SX.sym('x5'); % Pitch angle \theta
x6 = SX.sym('x6'); % Pitch rate angular velocity \dot{\theta}
x = [x1; x2; x3; x4; x5; x6];
nu = 2;
u1 = SX.sym('u1'); % Normalized thrust control input u_T
u2 = SX.sym('u2'); % Normalized rotational control input u_R
u = [u1; u2];
g = 9.81; % Gravitational acceleration constant
% Model equations
xdot = multicopter_model(x, u, g);
% Continuous time dynamics
f = Function('f', {x, u}, {xdot});
% Initial conditions
x0 = 0;
vx0 = 0;
z0 = 0;
vz0 = 0;
theta0 = 0;
thetadot0 = 0;
Xinit = [x0; vx0; z0; vz0; theta0; thetadot0];
% Final conditions
if scenario == 1
% Scenario 1: Multicopter vertical motion and flip
xf = 0;
vxf = 0;
zf = 3;
vzf = 0;
thetaf = 2*pi;
thetadotf = 0;
% State constraints to discourage the optimization algorithm from
% seeking trajectories too far away from the line trajectory between the initial
% and final state conditions.
min_x = -1;
max_x = 1;
min_z = 0;
max_z = zf;
elseif scenario == 2
% Scenario 2: Multicopter horizontal motion and flip
xf = 12;
vxf = 0;
zf = 0;
vzf = 0;
thetaf = 2*pi;
thetadotf = 0;
% State constraints to discourage the optimization algorithm from
% seeking trajectories too far away from the line trajectory between the initial
% and final state conditions.
min_x = 0;
max_x = xf;
min_z = -2;
max_z = 5;
end
Xfinal = [xf; vxf; zf; vzf; thetaf; thetadotf];
% Final input values for the multicopter to be in hover conditions at the
% end of the trajectory.
uTf = g;
uRf = 0;
Ufinal = [uTf; uRf];
% Constraints
uTmax = 20;
uTmin = 1;
uTmid = (uTmax+uTmin)/2; %Used for optimization problem initializati
uRmax = 15;
% Start with an empty NLP
w={}; % Optimization vector w
w0 = []; % Initial guess of optimization vector solution w_0
lbw = []; % Lower bounds of optimization vector
ubw = []; % Upper bounds of optimization vector
J = 0; % Cost function J
% Note: The default nonlinear programming problem formulation in CasADi
% allows the user to set lower and upper constraints to a nonlinear
% function of its optimization vector. Since we want to use equality
% constraints, we set the lower and upper bounds to 0.
g={}; % Equality constraints
lbg = []; % Lower equality constraint bound, set to 0.
ubg = []; % Upper equality constraint bound, set to 0.
% Final time condition
w = {w{:}, T}; % First variable of the optimization vector is the final time.
lbw = [lbw; 0]; % Lower bound of final time is 0
ubw = [ubw; inf]; % Upper bound of final time not specified (infinity).
w0 = [w0; 5]; % Initial guess of the final time.
% Set initial conditions
Xk = MX.sym('X0', nx); % Second set of variables are componsed of the initial state conditions.
w = {w{:}, Xk};
lbw = [lbw; Xinit]; % Lower and upper constraints are the same to fix the initial conditions.
ubw = [ubw; Xinit];
w0 = [w0; Xinit]; % Initial condition guess is set to be the known values.
% Setting up the equality and inequality constraints for the states and
% inputs in the NLP for each discrete-time iteration step k
for k=0:N-1
% Setting up U_k variable and its lower and upper bounds
Uk = MX.sym(['U_' num2str(k)], nu);
w = {w{:}, Uk};
lbw = [lbw; uTmin; -uRmax];
ubw = [ubw; uTmax; uRmax];
w0 = [w0; uTmid; 0];
% Setting up X_{k+1} variable and its lower and upper bounds
Xkp1 = MX.sym(['X_' num2str(k+1)], nx);
w = [w, {Xkp1}];
% Upper and lower bounds are used to prevent instability of optimization algorithm
lbw = [lbw; min_x; -inf; min_z; -inf; -inf; -inf];
ubw = [ubw; max_x; inf; max_z; inf; inf; inf];
w0 = [w0; 0; 0; 0; 0; 0; 0];
% Setting up equality constraints, given by the discretized system
% dynamics.
g = [g, {dt*f(Xk, Uk)+Xk-Xkp1}]; % X_{k+1} = X_k + f(X_k, U_k)
🎉第三部分——参考文献
文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。(文章内容仅供参考，具体效果以运行结果为准)
🌈第四部分——Matlab代码、数据、文章下载
资料获取，更多粉丝福利，MATLAB|Simulink|Python资源获取