Minimum Snap Trajectory Planning and Augmented MPC for Morphing Quadrotor Navigation in Confined Spaces

Abstract

Existing studies rarely investigate the dynamic morphology factor on motion planning and control, which is crucial for morphing quadrotors to achieve autonomous flight. Therefore, this paper studies the collaborative optimization of trajectory generation and flight control for the morphing quadrotor with real-time adjustable arms. In the motion planning layer, an objective function that combines position and morphology is constructed by embedding variable arm length as a decision variable into the conventional minimum snap trajectory generation framework. The generated trajectory not only satisfies the speed and acceleration constraints, but also smoothly passes through the narrow spaces that are difficult for traditional quadrotors to traverse. In the control layer, a constrained augmented model predictive control based on the dynamics of the morphing quadrotors is proposed to follow the generated trajectory with an embedded integrator, which is added by exploiting the differential flat variables to improve the tracking performance. In the numerical studies, a scenario with a corridor was considered to demonstrate the effectiveness of the proposed control strategy to achieve optimal trajectory under multiple constraints.

1. Introduction

Quadrotors have garnered widespread attention from researchers due to their low cost, simple structure, and high maneuverability [1], making them suitable for various applications, including aerial photography, reconnaissance, and logistics [2]. However, despite their success in open environments, their flight in complex and confined spaces is still limited due to their inability to change shape. This has sparked interest in shape-morphing quadrotors [3,4,5,6,7,8], which can dynamically adjust their airframe during flight to adapt to the environment or can promote their functionality. Currently, research on morphing quadrotors primarily focuses on mechanical design and optimization [9], as well as controller design [10]. Few studies, however, have considered morphological modification.

Compared to conventional quadrotors, morphing quadrotors face the following challenges in terms of motion planning and control [11]. The first one is the lack of consideration for the variable arm. Existing trajectory planning methods typically focus solely on the position of the center of gravity, overlooking changes in the quadrotor’s boundaries, which may lead to collisions when the quadrotor’s size exceeds the width of a confined space and fails to fully utilize the advantages of the morphing system. The second one is the lack of consideration for changes in the inertia matrix and control allocation matrix. During the in-flight morphing process, the system inertia and control allocation matrices change in response to structural changes, resulting in variations in system dynamics and even instability [12]. These problems make the study of path planning and control more challenging than the study of structural design.

Given that there is currently little work that considers morphing factors in motion planning, we primarily introduce methods for conventional quadrotors, which can be categorized into three main approaches: search-based, sampling-based, and numerical optimization-based planning methods [13]. The search-based planning method is represented by the A* algorithm [14], which typically rasterizes the environment, or represents it as a graph, and then utilizes a graph search algorithm to find an optimal path. The sampling-based planning method is represented by rapidly exploring random trees (RRTs) [15], which constructs a path tree or graph by randomly sampling points in the environment and then uses a specific strategy to find a feasible path. This method does not aim to find the optimal path, making it more computationally efficient. In general, the trajectories generated by the above two methods are a combination of a series of broken lines, and the transition between adjacent segments is not smooth, which would cause dynamic fluctuations. The numerical optimization-based trajectory planning method is typically modeled as an optimization problem with constraints on a continuous state space, where the constraints primarily arise from two aspects: the quadrotor and the environment. The optimization objective is, typically, to minimize the expected cost and time of trajectory execution. The trajectory optimization problem is essentially a nonlinear, non-convex optimization problem. There are two commonly used methods: one is the direct solution method; and the other is to transform non-convex optimization problems into convex optimization problems, such as quadratic programming problems and quadratic-constrained quadratic programming problems. The direct solution method typically addresses the original non-convex optimization problem directly and finds the local optimal solution through numerical optimization techniques, such as gradient descent and Newton methods. The advantage of this type of method is that it can handle complex nonlinear constraints and objective functions. However, due to the nature of non-convex problems, the solving process may fall into local optima, and computational efficiency may be low. Another approach is to transform the original problem into a convex problem by introducing slack variables, convex approximations, or reparameterization. The advantage of convex optimization problems lies in their unique global optimal solution and efficient solving ability. As a representative, the minimum snap trajectory generation algorithm proposed in [16] can generate smooth and energy-efficient trajectories by minimizing the trajectory jerk. It represents trajectories as piecewise polynomials with constraints, including trajectory continuity, starting and ending states, and constraints, such as obstacles or dynamic constraints. In this form, the minimum snap problem is transformed into a convex optimization problem, allowing for the global optimal solution to be found using convex optimization solvers.

Path planning technology addresses the problem of trajectory generation for a quadrotor, while the actual flight performance highly depends on the control system’s performance. Currently, researchers have proposed quite a few controllers for morphing quadrotors. A review work [17] provided a detailed summary; here, we introduce several typical controllers. In our previous work [3], we proposed a novel deformable quadrotor based on scissor-like structures, which allows the quadrotor’s volume to be dynamically adjusted to accommodate obstacles of various sizes. To further enhance the adaptability and versatility of the quadrotor, we proposed a novel deployable quadrotor aerial gripper that does not require the integration of extra manipulators [9]. To control these morphing quadrotor systems and to test the benefits of morphing, PID was used for automation. Desbiex et al. [18] proposed a foldable quadrotor that can change to an X-shape during flight, and they designed an adaptive model reference adaptive control (MRAC) strategy to adjust the quadrotor’s attitude and shape in real-time to cope with dynamic changes and uncertainties. Falanga et al. [5] proposed a quadrotor whose shape can transform from the conventional configuration to other shapes (e.g., H, O, and T morphology) in order to adapt to different conditions and operational requirements. Experiments, such as narrow gap crossing, grasping, and transporting, were performed using a linear quadratic regulator (LQR) with adaptive morphology online gain updates. However, performing these tasks in the field can be challenging due to the lack of full six-degree of freedom (DOF) control [17]. Yang et al. [19] developed an origami-based foldable quadrotor, whose arm length can change through a wire-driven mechanism. A feedback controller was designed to ensure stability when the arm changes. In general, most controllers have the following issues: (i) they solve the problems within a fixed window instead of dynamically solving according to current states, such as LQR [5]; (ii) they have not considered the dynamic changes caused by morphing behavior; and (iii) they cannot independently handle multiple constraints, such as PID [9], adaptive controllers [18], and feedback controllers [19], which require combining with other controllers to address constraints. To tackle these issues, we proposed a MPC for the morphing quadrotor [20], which can handle multiple state variables and control inputs under different constraints and adopt orthogonal functions to approximate the control input sequences to reduce the computational burden. The imperfection of this work is that it only studied the attitude control of the morphing quadrotor in different size states, which is a quasi-static study and lacks full six-DOF control.

In this paper, a custom-built morphing quadrotor was used as our platform, which features an additional DOF, allowing it to transform in addition to the functions of a traditional quadrotor. We considered this DOF in the system dynamics and incorporated it into the path planning and controller. The system architecture of this paper, which employs a layered optimization strategy to achieve trajectory tracking, is illustrated in Figure 1. Firstly, a multi-constraint optimization model was constructed based on the minimum snap algorithm, with the minimization of the position state change rate and arm length change rate as the optimization goal, thus adding continuity constraints to ensure the smoothness of the motion, imposing differential constraints to achieve dynamic feasibility, and combining corridor constraints to ensure the safety of obstacle avoidance. Finally, the optimal trajectory and the corresponding arm length sequence were generated as desired values and fed back to the control layer. Then, the desired Euler angles were obtained through the position controller, combined with the expected servo angle obtained from the arm dynamics model, and then input into the attitude loop to acquire the torques and subsequent control allocation. The contributions of this work are as follows: (i) We introduced the morphological factor into the minimum snap trajectory optimization framework and added the quadrotor’s variable arm length as an optimization variable to find an optimal trajectory and arm length. To the best of our knowledge, this was the first attempt to consider the morphing variables in the trajectory optimization framework for the path planning of the morphing quadrotor. (ii) We expanded the work to include full-DOF control based on the augmented model with an embedded integrator in the control to eliminate steady-state error and to ensure accurate trajectory tracking. More importantly, after considering the variable arm length in the path planning algorithm, the obtained quadrotor parameters were dynamically adjusted during flight, which poses an additional challenge compared to the quasi-static work [20].

This paper is organized as follows. Section 2 presents the dynamic model and the augmented MPC controller design based on the custom-built morphing quadrotor. Section 3 details the optimal trajectory generation algorithm, with corridor constraints added and morphology modifications considered. Section 4 demonstrates the trajectory planning and control results based on the numerical studies. Finally, the work is then summarized.

2. Dynamic Modeling and Controller Design

In this section, the dynamic model of the quadrotor, incorporating variable arm-length dynamics, is first established. Then, to simplify the trajectory planning and tracking control design of the system, we give the proof of differential flatness for the morphing system. Finally, a linear parameter-varying augmented state space model is formed to prepare for the MPC-based controller design.

2.1. Dynamic Model of the Morphing Quadrotor

To describe the posture of the quadrotor, we established the world coordinate system ${\mathit{o}}_w{\mathit{x}}_w{\mathit{y}}_w{\mathit{z}}_w$, the transition coordinate system ${\mathit{o}}_c{\mathit{x}}_c{\mathit{y}}_c{\mathit{z}}_c$, and the body coordinate system ${\mathit{o}}_b{\mathit{x}}_b{\mathit{y}}_b{\mathit{z}}_b$, as shown in Figure 2a. The transition coordinate system was obtained by rotating the world coordinate system to the quadrotor’s center of mass by a yaw angle, and then the body coordinate system was obtained through a roll and pitch rotation. Assume that the system center of mass coincides with the origin of the body coordinate system, ${\mathit{o}}_b$. The dynamic models of the position and attitude loops are briefly given below.

2.1.1. Position Dynamics

Define $\mathit{p}={[x,y,z]}^{\mathrm{T}}$ as the position vector of the quadrotor; thus, we have the corresponding translational dynamics as

$$m\ddot{\mathit{p}}={[00-mg]}^{\mathrm{T}}+\mathit{R}{\left[00T\right]}^{\mathrm{T}},$$

(1)

where m is the mass of the quadrotor, g denotes the gravitational acceleration, T represents the total thrust generated by the four propellers, and $\mathit{R}$ is the rotation matrix of any vector from the body coordinate system to the world coordinate system.

$$\mathit{R}=\left[\begin{array}{ccc}c\theta c\psi & c\psi s\theta s\varphi -s\psi c\varphi & c\psi s\theta c\varphi +s\psi s\varphi \\ c\theta s\psi & s\psi s\theta s\varphi +c\psi c\varphi & s\psi s\theta c\varphi -c\psi s\varphi \\ -s\theta & s\varphi c\theta & c\varphi c\theta \end{array}\right]$$

(2)

where $cx$ and $sx$ denote $cosx$ and $sinx$, respectively.

Assuming that the attitude angles are small, then (1) can be approximated around the hovering condition as

$$m\ddot{\mathit{p}}={[00-mg]}^{\mathrm{T}}+\overline{\mathit{R}}{\left[\varphi \theta T\right]}^{\mathrm{T}},$$

(3)

where $\overline{\mathit{R}}={[0mg0;-mg00;001]}^{\mathrm{T}}$.

2.1.2. Attitude Dynamics

Define $\mathit{\gamma }={\left[\varphi \theta \psi \right]}^{\mathrm{T}}$ as the attitude vector of the quadrotor, and, according to Euler’s equation, the corresponding attitude dynamics can be expressed as

$$\mathit{I}·\dot{\mathit{\omega }}=-\mathit{\omega }\times (\mathit{I}·\mathit{\omega })+\mathit{\tau },$$

(4)

where $\mathit{I}=diag(I_{xx},I_{yy},I_{zz})$ is the inertia matrix, and $\mathit{\omega }={\left[pqr\right]}^{\mathrm{T}}$ denotes the angular velocity in the body coordinate system, which satisfies the below relationship with the angular velocity in the world coordinate system.

$${\left[\dot{\varphi }\dot{\theta }\dot{\psi }\right]}^{\mathrm{T}}=\tilde{\mathit{R}}{\left[pqr\right]}^{\mathrm{T}},$$

(5)

where $\tilde{\mathit{R}}=[1s\varphi t\theta c\varphi t\theta ;0c\varphi -s\varphi ;0s\varphi /c\theta c\varphi /c\theta ]$, and $tx=tanx$. The updated rotation matrix $\tilde{\mathit{R}}$ can be further simplified to a unit matrix; thus, we have ${\left[\dot{\varphi }\dot{\theta }\dot{\psi }\right]}^{\mathrm{T}}={\left[pqr\right]}^{\mathrm{T}}$. As such, $\mathit{\tau }={\left[{\tau }_1{\tau }_2{\tau }_3\right]}^{\mathrm{T}}$ is the torque vector, indicating the torque generated by the propeller around the airframe axis, which is

$$\left[\begin{array}{c}{\tau }_1\\ {\tau }_2\\ {\tau }_3\end{array}\right]=\left[\begin{array}{cccc}0& k_F\widehat{l}& 0& -k_F\widehat{l}\\ -k_F\widehat{l}& 0& k_F\widehat{l}& 0\\ k_M& -k_M& k_M& -k_M\end{array}\right]\left[\begin{array}{c}{\omega }_1^2\\ {\omega }_2^2\\ {\omega }_3^2\\ {\omega }_4^2\end{array}\right],$$

(6)

where $k_M$ is the rotor moment constant; $k_F$ is the thrust constant of the rotor; ${\omega }_j$, $j=1,2,3,4$, is the motor speed; and $\widehat{l}$ is the arm length of the morphing quadrotor, which is a time-varying parameter and will be identified later.

Then, (4) can be linearized around the operating point as follows:

$${\left[\ddot{\varphi }\ddot{\theta }\ddot{\psi }\right]}^{\mathrm{T}}={\left[{\displaystyle \frac{{\tau }_1}{{\widehat{I}}_{xx}}}{\displaystyle \frac{{\tau }_2}{{\widehat{I}}_{yy}}}{\displaystyle \frac{{\tau }_3}{{\widehat{I}}_{zz}}}\right]}^{\mathrm{T}},$$

(7)

where the symbol with hat indicates that the variable is an estimated value.

2.1.3. Identification of Arm Length Dynamics

According to the custom-built morphing quadrotor, it is known that the arm length is controlled by a servo motor. Therefore, the motor rotation angle and arm length correspond to the input and output of the identification model, respectively. According to the geometric relationship illustrated in Figure 2b, we have $l_1=\sqrt{{(l_{h}sin\alpha )}^2+{(r_1-l_{h}cos\alpha )}^2}$, where $\alpha$ is the rotation angle of the servo motor, $l_h$ is the radius of the servo horn, and $r_1$ is the length from the center of servo horn to the central body. Then, we can obtain $\widehat{l}=l_e+l_m+r_1-\sqrt{{(l_{h}sin\alpha )}^2+{(r_1-l_{h}cos\alpha )}^2}$ and $l_m=l_1+l_2$, where $l_e$ is the length of the rod that connects the variable arm and the motor base, $l_1$ is the length from the center of servo horn to the scissor structure, and $l_2$ is the length of the variable arm. As $l_e$, $l_h$ and $r_1$ are constants and $l_m$ can be measured, the relationship between $\widehat{l}$ and $\alpha$ can be expressed as

$$\ddot{\widehat{l}}\left(\alpha \right)=a\dot{\widehat{l}}\left(\alpha \right)+b\widehat{l}\left(\alpha \right)+c\alpha ,$$

(8)

where a, b, and c are dimensionless constants that can be obtained through system identification, as indicated in [19]. It is noted that this method can provide us with a rough range, and ultimately, stable control can be achieved by adjusting the control gain.

2.2. Differential Flatness of the Morphing Quadrotor

The theory of differential flatness, by introducing the parametric form of flat outputs, enables the mapping of system dynamics to a low-dimensional subspace, thereby significantly simplifying trajectory planning and control design. Here, we give the proof of the differential flatness for the morphing system, which means that the states and inputs can be expressed as algebraic functions of the carefully chosen flat outputs and their derivatives.

The state vector of the morphing system is given by the position, attitude angle, velocity, angular velocity, and arm length, that is, ${[x,y,z,\varphi ,\theta ,\psi ,\dot{x},\dot{y},\dot{z},p,q,r,l]}^{\mathrm{T}}$. We chose the flat output as $\mathit{\xi }={[x,y,z,\psi ,l]}^{\mathrm{T}}$. The input vectors include total thrust T, the torque vector $\mathit{\tau }$, and rotation angle of the servo motor $\alpha$. Then, a trajectory $\mathit{\xi }\left(t\right)$ in space can be represented by the flat output, as shown in (9). Clearly, the position ${[x,y,z]}^{\mathrm{T}}$, velocity ${[\dot{x},\dot{y},\dot{z}]}^{\mathrm{T}}$, yaw angle $\psi$, and arm length l can be represented by $\mathit{\xi }$ and $\dot{\mathit{\xi }}$. Next, we will show that other state vectors and inputs can also be represented by $\mathit{\xi }$ and its derivatives.

$$\mathit{\xi }\left(t\right):[t_0,t_N]\to {\mathbb{R}}^3\times SO\left(2\right)\times {\mathbb{R}}^1.$$

(9)

Firstly, to prove that $\varphi$ and $\theta$ can be represented by $\mathit{\xi }$ and its derivatives, we introduce the corresponding coordinate axes in (1) as follows:

$$m\ddot{\mathit{p}}=-mg{\mathit{z}}_w+T{\mathit{z}}_b.$$

(10)

Thus, we have

$${\mathit{z}}_b={\displaystyle \frac{\mathit{\epsilon }}{\parallel \mathit{\epsilon }\parallel }},\mathit{\epsilon }={[\ddot{x},\ddot{y},\ddot{z}+g]}^{\mathrm{T}}.$$

(11)

The rotation transformation from the world coordinate system to the transition coordinate system can be obtained as ${\mathit{x}}_c={[cos\psi ,sin\psi ,0]}^{\mathrm{T}}$. From the coordinate system relation illustrated in Figure 2a, ${\mathit{y}}_b$ is perpendicular to the plane formed by ${\mathit{x}}_b$ and ${\mathit{z}}_b$, which is coplanar with ${\mathit{x}}_c$ and ${\mathit{z}}_c$, so ${\mathit{y}}_b$ is also perpendicular to ${\mathit{x}}_c$ and ${\mathit{z}}_b$. Hence, we have

$${\mathit{y}}_b={\displaystyle \frac{{\mathit{z}}_b\times {\mathit{x}}_c}{\parallel {\mathit{z}}_b\times {\mathit{x}}_c\parallel }},{\mathit{x}}_b={\mathit{y}}_b\times {\mathit{z}}_b,$$

(12)

where ${\mathit{z}}_b\times {\mathit{x}}_c\ne 0$.

We then have the coordinates of the basis of the body coordinate system in the world coordinate system, which means that (2) can be uniquely determined by (13). Then, $\varphi$ and $\theta$ can be uniquely determined.

$$\mathit{R}=\left[{\mathit{x}}_b{\mathit{y}}_b{\mathit{z}}_b\right].$$

(13)

Afterward, we need to prove that the angular velocity can be expressed in terms of $\mathit{\xi }$ and its derivatives. Define the linear acceleration of the quadrotor as $\mathit{a}$, and take the first-order derivative of (10). Then, we have

$$m\dot{\mathit{a}}=\dot{T}{\mathit{z}}_b+\mathit{\omega }\times T{\mathit{z}}_b.$$

(14)

Substitute $\dot{T}=m\dot{\mathit{a}}{\mathit{z}}_b$ into (14), and then we can acquire

$$\mathbf{\Gamma }=\mathit{\omega }\times {\mathit{z}}_b={\displaystyle \frac{m}{T}}(\dot{\mathit{a}}-\left(\dot{\mathit{a}}{\mathit{z}}_b\right){\mathit{z}}_b).$$

(15)

By substituting the angular velocity $\mathit{\omega }=p{\mathit{x}}_b+q{\mathit{y}}_b+r{\mathit{z}}_b$ into (15), we then have

$$\mathbf{\Gamma }=\mathit{\omega }\times {\mathit{z}}_b=-p{\mathit{y}}_b+q{\mathit{x}}_b.$$

(16)

Therefore, the angular velocity components p, q can be expressed as

$$p=-\mathbf{\Gamma }{\mathit{y}}_b,q=\mathbf{\Gamma }{\mathit{x}}_b.$$

(17)

Note that the angular velocity has the following coordinate transformation relationship: $\mathit{\omega }={\mathit{\omega }}_{bc}+{\mathit{\omega }}_{cw}$, where ${\mathit{\omega }}_{bc}$ represents the angular velocity of the body coordinate system relative to the transition coordinate system, and ${\mathit{\omega }}_{cw}$ represents the angular velocity of the transition coordinate system relative to the world coordinate system. At the same time, ${\mathit{\omega }}_{bc}$ has no component in the ${\mathit{z}}_b$ direction, so

$$r=({\mathit{\omega }}_{bc}+{\mathit{\omega }}_{cw}){\mathit{z}}_b=\dot{\psi }{\mathit{z}}_w{\mathit{z}}_b.$$

(18)

Finally, we show that the input can be represented by $\mathit{\xi }$ and its derivatives. Combining (10) and (11), we can obtain $T=m\parallel \mathit{\epsilon }\parallel$. As the angular velocity and inertia matrix are functions of the flat output and its derivatives, the control input $\mathit{\tau }$ can be calculated from (4). Meanwhile, the rotation angle of the servo motor $\alpha$ can be calculated by (8).

In summary, this section proves the global differential flatness of the system in state-input space by constructing the flat output. This property demonstrates that the entire dynamic behavior of the morphing system can be uniquely parameterized by the flat output and its finite derivatives, thereby reducing the original high-dimensional, nonlinear dynamics constraint to a low-dimensional trajectory planning problem in a flat space. This proof provides theoretical support for the trajectory optimization algorithm in Section 3, which enables the programming problem under complex dynamic constraints to be transformed into a convex optimization problem in flat space.

2.3. Augmented Model Predictive Control

2.3.1. Augmented Model

The state space model of the linear continuous time system is given as follows:

$$\begin{array}{ccc}\hfill {\left[\dot{\mathit{p}}\ddot{\mathit{p}}\right]}^{\mathrm{T}}& =& {\mathit{A}}_{\mathit{1}}{\left[\mathit{p}\dot{\mathit{p}}\right]}^{\mathrm{T}}+{\mathit{B}}_{\mathit{1}}{\mathit{u}}_{\mathit{1}}\hfill \\ \hfill \mathit{y}& =& {\mathit{C}}_{\mathit{1}}{\left[\mathit{p}\dot{\mathit{p}}\right]}^{\mathrm{T}}\hfill \end{array},$$

(19)

$$\begin{array}{ccc}\hfill {\left[\dot{\mathit{\gamma }}\ddot{\mathit{\gamma }}\right]}^{\mathrm{T}}& =& {\mathit{A}}_{\mathit{2}}{[\mathit{\gamma }\dot{\mathit{\gamma }}]}^{\mathrm{T}}+{\mathit{B}}_{\mathit{2}}{\mathit{u}}_{\mathit{2}}\hfill \\ \hfill \mathit{y}& =& {\mathit{C}}_{\mathit{2}}{[\mathit{\gamma }\dot{\mathit{\gamma }}]}^{\mathrm{T}}\hfill \end{array},$$

(20)

where ${\mathit{u}}_{\mathit{1}}={\left[\varphi \theta T\right]}^{\mathrm{T}}$, ${\mathit{u}}_{\mathit{2}}={\left[{\tau }_1{\tau }_2{\tau }_3\right]}^{\mathrm{T}}$, and the combo $\{{\mathit{A}}_{\mathit{1}}$,${\mathit{B}}_{\mathit{1}}$,${\mathit{C}}_{\mathit{1}}$,${\mathit{A}}_{\mathit{2}}$,${\mathit{B}}_{\mathit{2}}$,${\mathit{C}}_{\mathit{2}}\}$ are used to define the system in the form of the state-space equation, and the matrices are as follows:

$$\begin{array}{cc}{\mathit{A}}_{\mathit{1}}={\mathit{A}}_{\mathit{2}}=\left[\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\end{array}\right]& {\mathit{B}}_{\mathit{1}}=\left[\begin{array}{ccc}0& 0& 0\\ 0& g& 0\\ 0& 0& 0\\ -g& 0& 0\\ 0& 0& 0\\ 0& 0& {\displaystyle \frac{1}{m}}\end{array}\right]\\ \\ {\mathit{C}}_{\mathit{1}}={\mathit{C}}_{\mathit{2}}=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]& {\mathit{B}}_{\mathit{2}}=\left[\begin{array}{ccc}0& 0& 0\\ {\displaystyle \frac{1}{I_{xx}}}& 0& 0\\ 0& 0& 0\\ 0& {\displaystyle \frac{1}{I_{yy}}}& 0\\ 0& 0& 0\\ 0& 0& {\displaystyle \frac{1}{I_{zz}}}\end{array}\right]\end{array}.$$

It is noted that the position control loop, described in (19), is a linear time-invariant (LTI) system whose dynamics are independent of the morphing, while the attitude control loop, defined in (20), is a linear parameter-varying (LPV) system whose dynamic characteristics vary with the morphing parameters. The generalized form of (19) and (20) after discretization can be described as

$$\mathit{x}(k+1)={\mathit{A}}_{\mathit{d}}\mathit{x}\left(k\right)+{\mathit{B}}_{\mathit{d}}\mathit{u}\left(k\right),$$

(21)

$$\mathit{y}\left(k\right)={\mathit{C}}_{\mathit{d}}\mathit{x}\left(k\right),$$

(22)

where the subscript d indicates discretization.

To reduce the steady-state error of the system, we rewrote it in the augmented form by incorporating the integral. Taking a difference operation on (21) and (22), we have

$$\mathit{x}(k+1)-\mathit{x}\left(k\right)={\mathit{A}}_{\mathit{d}}(\mathit{x}\left(k\right)-\mathit{x}(k-1))+{\mathit{B}}_{\mathit{d}}(\mathit{u}\left(k\right)-\mathit{u}(k-1)),$$

(23)

$$\mathit{y}(k+1)-\mathit{y}\left(k\right)={\mathit{C}}_{\mathit{d}}(\mathit{x}(k+1)-\mathit{x}\left(k\right)),$$

$$\mathit{y}(k+1)={\mathit{C}}_{\mathit{d}}(\mathit{x}(k+1)-\mathit{x}\left(k\right))+\mathit{y}\left(k\right).$$

(24)

Here, let $\mathbf{\Delta }\mathit{x}(k+1)=\mathit{x}(k+1)-\mathit{x}\left(k\right)$, $\mathbf{\Delta }\mathit{u}\left(k\right)=\mathit{u}\left(k\right)-\mathit{u}(k-1)$, and $\mathbf{\Delta }\mathit{x}\left(k\right)=\mathit{x}\left(k\right)-\mathit{x}(k-1)$, and then substitute these into (23) and (24) as follows:

$$\Delta \mathit{x}(k+1)={\mathit{A}}_{\mathit{d}}\Delta \mathit{x}\left(k\right)+{\mathit{B}}_{\mathit{d}}\Delta \mathit{u}\left(k\right),$$

(25)

$$\mathit{y}(k+1)={\mathit{C}}_{\mathit{d}}{\mathit{A}}_{\mathit{d}}\Delta \mathit{x}\left(k\right)+{\mathit{C}}_{\mathit{d}}{\mathit{B}}_{\mathit{d}}\Delta \mathit{u}\left(\mathit{k}\right)+\mathit{y}\left(\mathit{k}\right).$$

(26)

Thus, we have an updated state space equation with a new input variable $\mathbf{\Delta }\mathit{u}\left(k\right)$ and a new state variable vector $\mathit{x}\left(k\right)={[\mathbf{\Delta }\mathit{x}{\left(k\right)}^{\mathrm{T}},\mathit{y}\left(k\right)]}^{\mathrm{T}}$ as follows:

$$\begin{array}{cc}\hfill \underset{\mathit{x}(k+1)}{\underbrace{\left[\begin{array}{c}\mathbf{\Delta }\mathit{x}(k+1)\\ \mathit{y}(k+1)\end{array}\right]}}& =\underset{A}{\underbrace{\left[\begin{array}{cc}{\mathit{A}}_{\mathit{d}}& {\mathit{O}}_{q_y\times n_x}^{\mathrm{T}}\\ {\mathit{C}}_{\mathit{d}}{\mathit{A}}_{\mathit{d}}& {\mathit{E}}_{q_y\times q_y}\end{array}\right]}}\underset{\mathit{x}\left(k\right)}{\underbrace{\left[\begin{array}{c}\mathbf{\Delta }\mathit{x}\left(k\right)\\ \mathit{y}\left(k\right)\end{array}\right]}}+\underset{B}{\underbrace{\left[\begin{array}{c}{\mathit{B}}_{\mathit{d}}\\ {\mathit{C}}_{\mathit{d}}{\mathit{B}}_{\mathit{d}}\end{array}\right]}}\mathbf{\Delta }\mathit{u}\left(k\right)\hfill \\ \hfill \mathit{y}\left(k\right)& =\underset{C}{\underbrace{\left[\begin{array}{cc}{\mathit{O}}_{q_y\times n_x}& {\mathit{E}}_{q_y\times q_y}\end{array}\right]}}\underset{\mathit{x}\left(k\right)}{\underbrace{\left[\begin{array}{c}\mathbf{\Delta }\mathit{x}\left(k\right)\\ \mathit{y}\left(k\right)\end{array}\right]}}\hfill \end{array},$$

(27)

where ${\mathit{O}}_{q_y\times n_x}$ is a zero matrix with dimension $q_y\times n_x$; ${\mathit{E}}_{q_y\times q_y}$ is a unit matrix with dimension $q_y\times q_y$; $n_x$ is the number of states; and $q_y$ is the number of outputs.

2.3.2. Output Prediction

Define the sampling time as k, the future state vector as $\mathit{x}(k+1),\mathit{x}(k+2),\cdots ,\mathit{x}(k+n_y)$, and the future control trajectory as $\mathbf{\Delta }\mathit{U}={[\mathbf{\Delta }\mathit{u}\left(k\right),\mathbf{\Delta }\mathit{u}(k+1),\cdots ,\mathbf{\Delta }\mathit{u}(k+n_u-1)]}^{\mathrm{T}}$, where $n_u$ is the control time domain and $n_y$ is the predicted time domain, satisfying $n_u\le n_y$. The detailed process of calculating the future state vector and output prediction is described in [21], and it is omitted here. Finally, the output prediction after $n_y$ step can be expressed as

$$\mathit{y}(k+n_y)=\mathit{C}[{\mathit{A}}^{n_y}\mathit{x}\left(k\right)+{\mathit{A}}^{n_y-1}\mathit{B}\mathbf{\Delta }\mathit{u}\left(k\right)+\cdots +\mathit{B}\mathbf{\Delta }\mathit{u}(k+n_y-1)],$$

(28)

and the predicted output of each step is $\mathit{Y}={[\mathit{y}(k+1),\mathit{y}(k+2),\cdots ,\mathit{y}(k+n_y)]}^{\mathrm{T}}$. Then, we have

$$\mathit{Y}=\mathit{Px}\left(k\right)+\mathit{H}\mathbf{\Delta }\mathit{U},$$

(29)

where

$$\mathit{P}={[\mathit{CA},{\mathit{CA}}^2,{\mathit{CA}}^3,\cdots ,{\mathit{CA}}^{n_y}]}^{\mathrm{T}}\mathrm{and}\mathit{H}=\left[\begin{array}{cccc}\mathit{CB}& 0& \cdots & 0\\ \mathit{CAB}& \mathit{CB}& \cdots & 0\\ {\mathit{CA}}^2\mathit{B}& \mathit{CAB}& \cdots & 0\\ ⋮& ⋮& ⋮& ⋮\\ {\mathit{CA}}^{n_y-1}\mathit{B}& {\mathit{CA}}^{n_y-2}\mathit{B}& \cdots & {\mathit{CA}}^{n_y-n_u}\mathit{B}\end{array}\right].$$

2.3.3. Constraints

As the integral term is introduced to reduce the steady-state error, the design of MPC controller needs to consider the constraints on the changing rate $\mathbf{\Delta }\mathit{u}\left(k\right)$ in addition to the control variable $\mathit{u}\left(k\right)$. For the position loop, as shown in (19), the constraints on the control input ${\mathit{u}}_{\mathit{1}}\left(k\right)={\left[\varphi \left(k\right)\theta \left(k\right)T\left(k\right)\right]}^{\mathrm{T}}$ and the corresponding changing rate $\mathbf{\Delta }{\mathit{u}}_{\mathit{1}}\left(k\right)={[\Delta \varphi \left(k\right)\Delta \theta \left(k\right)\Delta T\left(k\right)]}^{\mathrm{T}}$ need to be considered. Similarly, for the attitude loop, as shown in (20), constraints on the control input ${\mathit{u}}_{\mathit{2}}\left(k\right)={\left[{\tau }_1\left(k\right){\tau }_2\left(k\right){\tau }_3\left(k\right)\right]}^{\mathrm{T}}$ and the corresponding changing rate $\mathbf{\Delta }{\mathit{u}}_{\mathit{2}}\left(k\right)={[\Delta {\tau }_1\left(k\right)\Delta {\tau }_2\left(k\right)\Delta {\tau }_3\left(k\right)]}^{\mathrm{T}}$ need to be considered. Therefore, at the sampling time k, we have the following generalized constraints formula: ${\mathit{u}}_{\eta }^{min}\le {\mathit{u}}_{\eta }\left(k\right)\le {\mathit{u}}_{\eta }^{max}$ and $\mathbf{\Delta }{\mathit{u}}_{\eta }^{min}\le \Delta {\mathit{u}}_{\eta }\left(k\right)\le \mathbf{\Delta }{\mathit{u}}_{\eta }^{max}$ with $\eta =1,2$.

Considering the sampling times, the maximum and minimum changing rate can be expressed as ${[\mathbf{\Delta }\mathit{U}\left(k\right)\le \mathbf{\Delta }{\mathit{U}}^{max},\mathbf{\Delta }\mathit{U}(k+1)\le \mathbf{\Delta }{\mathit{U}}^{max},\cdots ,\mathbf{\Delta }\mathit{U}(k+n_u-1)\le \mathbf{\Delta }{\mathit{U}}^{max}]}^{\mathrm{T}}$ and ${[-\mathbf{\Delta }\mathit{U}\left(k\right)\le -\mathbf{\Delta }{\mathit{U}}^{min},-\mathbf{\Delta }\mathit{U}(k+1)\le -\mathbf{\Delta }{\mathit{U}}^{min},\cdots ,-\mathbf{\Delta }\mathit{U}(k+n_u-1)\le -\mathbf{\Delta }{\mathit{U}}^{min}]}^{\mathrm{T}}$, respectively, which can be grouped into the following form:

$${\left[-\mathit{E}\mathit{E}\right]}^{\mathrm{T}}\mathbf{\Delta }\mathit{U}\le {\left[-\mathbf{\Delta }{\mathit{U}}^{min}\mathbf{\Delta }{\mathit{U}}^{max}\right]}^{\mathrm{T}},$$

(30)

where $\mathbf{\Delta }{\mathit{U}}^{min}$ and $\mathbf{\Delta }{\mathit{U}}^{max}$ are the column vectors with the same number as the control time domain.

The control input ${\left[\mathit{u}\left(k\right),\mathit{u}(k+1),\cdots ,\mathit{u}(k+n_u-1)\right]}^{\mathrm{T}}$ can be written in the form that incorporates the input changing rate as ${\mathit{C}}_{\mathit{1}}\mathit{u}(k-1)+{\mathit{C}}_{\mathit{2}}{[\mathbf{\Delta }\mathit{u}\left(k\right),\mathbf{\Delta }\mathit{u}(k+1),\cdots ,\mathbf{\Delta }\mathit{u}(k+n_u-1)]}^{\mathrm{T}}$, which can be further expressed as

$$\begin{array}{cc}\hfill -({\mathit{C}}_{\mathit{1}}\mathit{u}(k-1)+{\mathit{C}}_{\mathit{2}}\mathbf{\Delta }\mathit{U})& \le -{\mathit{U}}^{min}\hfill \\ \hfill ({\mathit{C}}_{\mathit{1}}\mathit{u}(k-1)+{\mathit{C}}_{\mathit{2}}\mathbf{\Delta }\mathit{U}& \le {\mathit{U}}^{max},\hfill \end{array}$$

(31)

where ${\mathit{C}}_{\mathit{1}}={[\mathit{E},\mathit{E},\cdots ,\mathit{E}]}^{\mathrm{T}}$ and ${\mathit{C}}_{\mathit{2}}=[\mathit{E},0,0,\cdots ,0;\mathit{E},\mathit{E},$ $0,\cdots ,0;\cdots ;\mathit{E},\mathit{E},\mathit{E},\cdots ,\mathit{E}]$.

Combining (30) and (31), we can obtain a linear constraint inequality as

$$\left[\begin{array}{c}-{\mathit{C}}_{\mathit{2}}\\ {\mathit{C}}_{\mathit{2}}\\ \mathit{E}\\ \mathit{E}\end{array}\right]\mathbf{\Delta }\mathit{U}\le \left[\begin{array}{c}-{\mathit{U}}^{min}+{\mathit{C}}_{\mathit{1}}\mathit{u}(k-1)\\ {\mathit{U}}^{max}-{\mathit{C}}_{\mathit{1}}\mathit{u}(k-1)\\ -\mathbf{\Delta }{\mathit{U}}^{min}\\ \mathbf{\Delta }{\mathit{U}}^{max}\end{array}\right].$$

(32)

2.3.4. Optimization

The purpose of MPC is to minimize the error between the prediction and the reference so that the predicted output at each sampling time reaches the expected reference value. The final result is a series of control inputs $\mathbf{\Delta }\mathit{U}$, and then the first obtained control input is returned to the system to make the system reach the reference value. The optimization objective function can be written as $\mathit{J}={({\mathit{R}}_{\mathit{s}}-\mathit{Y})}^{\mathrm{T}}({\mathit{R}}_{\mathit{s}}-\mathit{Y})+\mathbf{\Delta }{\mathit{U}}^{\mathrm{T}}\mathit{W}\mathbf{\Delta }\mathit{U}$, where ${\mathit{R}}_{\mathit{s}}={[\mathit{E},\mathit{E},\cdots ,\mathit{E}]}^{\mathrm{T}}\mathit{r}\left(k\right)$, $\mathit{r}\left(k\right)$ is the reference value at time k, and $\mathit{W}$ is the diagonal weight matrix of the control input. The final cost function is obtained by solving the first derivative

$$\mathit{J}={\displaystyle \frac{1}{2}}\mathbf{\Delta }{\mathit{U}}^{\mathrm{T}}\mathit{M}\mathbf{\Delta }\mathit{U}+\mathbf{\Delta }{\mathit{U}}^{\mathrm{T}}\mathit{F},$$

(33)

subject to (32), where $\mathit{M}=2({\mathit{H}}^{\mathrm{T}}\mathit{H}+\mathit{W})$ and $\mathit{F}=-2{\mathit{H}}^{\mathrm{T}}({\mathit{R}}_{\mathit{s}}-\mathit{Px}\left(k\right))$, which are compatible matrices in the quadratic programming problem.

3. Trajectory Generation with Corridor Constraints

The key to achieving autonomous flight for the quadrotor is to utilize existing information to realize reasonable trajectory planning. As shown in this section, we randomly generated a series of waypoints in 3D space using existing information, and we then calculated the optimal trajectory based on the minimum snap method. This allows the quadrotor to fly from the starting point, pass through the waypoints along the trajectory in sequence, and, finally, return to the starting point. Unlike the trajectory generation of traditional quadrotors, this method leverages the scalability advantage of the morphing quadrotor to achieve better trajectories when navigating narrow corridors.

3.1. Determining Objective Function

Suppose that we have obtained N waypoints through the front-end path planning, then we need to plan them and turn them into a smooth curve. In this work, the polynomial was adopted to express the trajectory, which is defined as $\mathit{\sigma }\left(t\right)={[x,y,z,l]}^{\mathrm{T}}$, where $\psi$ is set to 0 in this study. Thus, we have

$$\mathit{\sigma }\left(t\right)=\left\{\begin{array}{c}{\mathit{\sigma }}_1\left(t\right)=\sum _{s=0}^n{\mathit{\sigma }}_{s1}{t}^s,t_0\le t<t_1\hfill \\ {\mathit{\sigma }}_2\left(t\right)=\sum _{s=0}^n{\mathit{\sigma }}_{s2}{t}^s,t_1\le t<t_2\hfill \\ ⋮\hfill \\ {\mathit{\sigma }}_N\left(t\right)=\sum _{s=0}^n{\mathit{\sigma }}_{sN}{t}^s,t_{N-1}\le t\le t_N\hfill \end{array},\right.$$

(34)

where $t_0$ and $t_N$ represent the initial time and the end time, respectively; n is the order of the polynomial; and ${\mathit{\sigma }}_i\left(t\right)$ denotes the i-th segment with $i=1,2,\cdots ,N$. The time of each segment t can be set freely according to the requirements.

Next, we calculated the parameters ${\mathit{\sigma }}_{si}={[x_{si},y_{si},z_{si},l_{si}]}^{\mathrm{T}}$$(s=0,1,\cdots ,n)$ while satisfying a series of constraints, including equality constraints and inequality constraints. The equality constraints include the fixed waypoints that the quadrotor is expected to pass through, as well as the initial velocity, acceleration, and constraints set to ensure smoothness at the intersection of adjacent trajectories. The inequality constraints include setting the maximum and minimum values of velocity and acceleration, as well as the constraints on the trajectory position and arm length within the corridor area. In addition, the energy consumption during flight is expected to be as minimal as possible; accordingly, the objective function can be modified by minimizing the rate of change in the quadrotor’s position and arm length. Thus, we have the following cost function:

$$\min {\int }_{t_0}^{t_N}{\mu }_r{\parallel {\displaystyle \frac{d^{k_r}\mathit{r}}{d{t}^{k_r}}}\parallel }^2+{\mu }_l{\left({\displaystyle \frac{d^{k_l}l}{d{t}^{k_l}}}\right)}^2,$$

(35)

where position vector $\mathit{r}={[x,y,z]}^{\mathrm{T}}$, ${\mu }_r,{\mu }_l$ are dimensionless constants. As ${\tau }_1$ and ${\tau }_2$ are functions of the fourth-order derivative of the position, the generated trajectory minimizes the integral of the square of the snap norm, and $k_r$ is set to be 4. Similarly, the control input $\alpha$ related to the arm length is presented in the second-order derivative; thus, we have $k_l=2$.

The objective function can be transformed into a quadratic programming problem:

$$\begin{array}{cc}\hfill & \min {\mathit{c}}^{\mathrm{T}}\mathit{H}\mathit{c}+{\mathit{f}}^{\mathrm{T}}\mathit{c}\hfill \\ \hfill & \mathrm{s}.\mathrm{t}.{\mathit{A}}_{eq}\mathit{c}={\mathit{b}}_{eq}\hfill \\ \hfill & {\mathit{A}}_{ineq}c\le {\mathit{b}}_{ineq}\hfill \end{array},$$

(36)

where $\mathit{c}=\{x_{si},y_{si},z_{si},l_{si}\}$. The equality and inequality constraints can be found in Section 3.2 and Section 3.3.

3.2. Adding Equality Constraints

The equality constraints, including the waypoint constraint, continuity constraint, and differential constraint, are described separately below, with the schematic diagram shown in Figure 3.

3.2.1. Waypoint Constraint

To ensure that the trajectory passes through N waypoints, the following constraints are required:

$$\mathit{\sigma }\left(t_i\right)={\mathit{\sigma }}_i,$$

(37)

where ${\mathit{\sigma }}_i={[x_i,y_i,z_i,l_i]}^{\mathrm{T}}.$

3.2.2. Continuity Constraint

We consider two types of continuity. One is to ensure the continuity of the trajectory, that is, the values at the intersection of the two adjacent trajectories are equal. The other is to minimize snap, which requires the continuity of the fourth-order derivative at the waypoint. Thus, we have

$${\mathit{\sigma }}_i^{\left({\kappa }_1\right)}\left(t_i\right)={\mathit{\sigma }}_{i+1}^{\left({\kappa }_1\right)}\left(t_i\right),$$

(38)

where ${\kappa }_1=0,1,2,3$ indicates that the ${\kappa }_1$-th order derivatives are smooth.

3.2.3. Differential Constraint

Continuity constraints ensure that the position, velocity, acceleration, jerk, and snap values are equal at the intersection of adjacent trajectories. Additionally, differential constraints require these values to match predefined setpoints. The general expression can be written as

$${\mathit{\sigma }}_i^{\left({\kappa }_2\right)}\left(t_i\right)={\mathit{\sigma }}_i^{\left({\kappa }_2\right)},$$

(39)

where ${\kappa }_2=1,2,3,4$. Generally, the velocity, acceleration, jerk, and snap values for the starting and ending points of the trajectory are set to 0.

3.3. Adding Inequality Constraint

We incorporated the corridor constraints, which account for the variable arm length of the morphing quadrotor, into the inequality constraints of the quadratic programming problem to ensure that the quadrotor can safely traverse the corridor. To this end, we first defined the unit vector from the starting points ${\mathit{r}}_i$ to the end points ${\mathit{r}}_{i+1}$ of the i-th segment as $\mathit{e}=({\mathit{r}}_{i+1}-{\mathit{r}}_i)/\parallel {\mathit{r}}_{i+1}-{\mathit{r}}_i\parallel$. The vertical distance vector ${\mathit{d}}_i\left(t\right)$ of the i-th segment can be obtained by subtracting the projection component of the vector ${\mathit{r}}_{i+1}-{\mathit{r}}_i$ in the direction of the unit vector.

$${\mathit{d}}_i\left(t\right)=(\mathit{r}\left(t\right)-{\mathit{r}}_i)-((\mathit{r}\left(t\right)-{\mathit{r}}_i)·\mathit{e})\mathit{e}.$$

(40)

The width of the i-th corridor is defined as ${\delta }_i$. To ensure that the path stays within the corridor, it is necessary to satisfy ${\parallel {\mathit{d}}_i\left(t\right)\parallel }_{\infty }\le {\delta }_i$. Considering that the arm length of the quadrotor changes in flight, there should be ${\parallel {\mathit{d}}_i\left(t\right)\parallel }_{\infty }+l_i\le {\delta }_i$ when $t_i\le t\le t_{i+1}$. By introducing h intermediate points, the inequality constraints are established as follows:

$$|{\mathit{x}}_{\mathit{w}}·{\mathit{d}}_i(t_i+{\displaystyle \frac{\zeta }{1+n}}(t_{i+1}-t_i))|+l_i\le {\delta }_i,\zeta =1,\cdots ,h,$$

(41)

and, equivalently, for ${\mathit{y}}_{\mathit{w}}$ and ${\mathit{z}}_{\mathit{w}}$.

4. Numerical Studies and Results

This section presents the trajectory optimization results of a morphing quadrotor with corridor constraints added and morphology modification considered. A three-dimensional position coordinate and arm length sequence was generated, which were input into the MPC controller and the servo motor that controls the morphing, respectively. The computational unit (HP Z2 Mini G9 Workstation Desktop PC, manufactured by HP Inc., Beijing, China) was equipped with an Intel Core i7-13700 processor and 32 GB RAM. The numerical studies were conducted based on MATLAB R2024b.

4.1. Trajectory Planning Results

4.1.1. Experimental Setup

We designed a scenario in which a morphing quadrotor with variable arms needs to pass through $N=5$ waypoints and a narrow channel. The positions of these waypoints were randomly set to $P_0=(0,0,0)$, $P_1=(5,-7.5,0.8)$, $P_2=(14,-4.5,2.3)$, $P_3=(10,5,4)$, and $P_4=(4.5,6,4.5)$, respectively, as shown as the black dots in Figure 4a. The narrow channel with a width of $D=0.20$ m appeared between $P_2$ and $P_3$, as shown by the magenta solid line in Figure 4b and with the partially enlarged view shown in Figure 4c. Given that there was still a distance from the rotor motor to the quadrotor physical boundary, to ensure the morphing quadrotor could pass through intact, the corridor width was further constrained to $\delta =0.15$ m ($\delta <D$), as shown by the cyan dashed line in Figure 4c. This setting can ensure that, when the quadrotor encounters a narrow channel, it can reduce its size from a larger one to a smaller one to cross. The trajectory, represented by a sixth-order polynomial, starts from $P_0$ at $t_0=0$ s, sequentially passes through five waypoints with each flight segment lasting 2 s, and then returns to $P_0$ at $t_N=10$ s.

4.1.2. Experimental Results

During the simulation process, it was found that the selection of the number of intermediate points h within the corridor in (41) had a significant influence on the result. We employed an exhaustive method to test a series of possible h values and observed the results. When the h value was inappropriate, two types of overshoot would occur. One was constraint violation overshoot, where the arm length exceeded the predefined safety boundary. The other was redundant overshoot, which resulted in an unnecessary increase in arm length due to the suboptimal solution caused by the weight imbalance in the cost function. The above situation will not occur when the h value is appropriate, such as $h=5$ and $h=30$. However, as h increases, the time overhead increases, and computational efficiency decreases. Therefore, considering the above factors, a lower value of $h=5$ is determined as the optimal number of intermediate points.

The regular corridor-free trajectory generated by five waypoints is plotted as a black dashed line in Figure 4a–c. When a narrow corridor was added between $P_2$ and $P_3$, the trajectory was replanned to the red solid curve. Figure 5a,b demonstrate the generated position coordinates and the arm length, where the subscript d indicates the replanned information after adding the corridor, which will be used as the desired coordinates for the control. It can be observed that, before entering the corridor, the quadrotor’s arm length was initially 0.18 m, and it then decreased to approximately 0.14 m within the first 2 s, thus optimizing the flight state by shortening the arm length in preparation for the subsequent crossing of the corridor. After a short period of stabilization, the arm length continues to decrease, reaching 0.11 m at $t=3.85$ s, which created favorable conditions for crossing the corridor. When $t\in [4,6]$ s, the quadrotor successfully crossed the corridor with a length of 0.11 m. After that, the arm length remained 0.11 m until the quadrotor returned to $P_0$.

The results show that the optimization-based minimum snap trajectory generation method can generate smooth flight trajectories. When considering narrow corridors, by incorporating arm length changes into the optimization objective, the quadrotor is expected to adjust the arm length to safely and stably traverse the confined space without deviating from the trajectory while minimizing position changes and arm length changes, that is, minimizing energy. At this point, we obtained the coordinates of the optimal trajectory and optimal arm length for the morphing quadrotor, $[x_d,y_d,z_d,l_d]$, which were then input into the subsequent controller.

4.2. Augmented MPC-Based Controller Results

4.2.1. Experimental Setup

An inner–outer double-loop-augmented MPC controller was designed to realize the trajectory tracking. The trajectory generated above was used as a reference for the outer position loop, the generated control sequence served as a reference for the inner attitude loop, and the generated control input sequence determined the propeller speed. In designing the controller, the parameters and constraints were set as follows: $m=1.6$ kg; $g=9.81$ m/s²; $\varphi ,\theta \in [-{\displaystyle \frac{\pi }{5}},{\displaystyle \frac{\pi }{5}}]$ rad; $\psi =0$ rad; $T\in [0,2mg]$; $[\Delta {\varphi }_{min},\Delta {\varphi }_{max}]=[-0.6,0.6]{\varphi }_{max}$, $[\Delta {\theta }_{min},\Delta {\theta }_{max}]=[-0.6,0.6]{\theta }_{max}$; $[\Delta T_{min},\Delta T_{max}]=[-0.6,0.6]mg$, ${\omega }_j\in [3093,4720]$ rpm; and the prediction horizon and control horizon in the position control and attitude control were both $ny=20$ and $nu=3$, respectively. Additionally, a random noise of $0.01\times rand\left(1\right)$ was added to the morphing quadrotor to make it more realistic.

4.2.2. Experimental Results

The control results of the position and attitude are shown in Figure 6 and Figure 7. The trajectory generated by the trajectory optimization algorithm in the above step was used as the desired trajectory for the position control in this step, which is represented by the red curve in Figure 6a, with the black dashed line indicating the actual trajectory. It can be seen that, despite the addition of random noise to the system, the position controller was still able to track the desired trajectory coordinates well, with a maximum error of 0.02 m in the $xy$ plane. In addition, Figure 6b,c show the control input required for the outer loop $[\varphi ,\theta ,T]$, and their changing rate $[\Delta \varphi ,\Delta \theta ,\Delta T]$ was constrained to their respective fixed ranges (see the dashed lines). It is worth noting that, in order to eliminate the high-frequency jitter caused by noise and system nonlinearity, the control inputs are processed by median filtering. As shown by the solid red line in Figure 6b, the filtered control inputs were relatively smooth, without obvious oscillations or mutations. Then, $\varphi$, $\theta$, together with the preset $\psi$, was used as the reference for the inner loop. Similarly, it can be seen from Figure 7a that the attitude controller achieved a high attitude tracking accuracy, with the maximum tracking error being less than 0.006 rad. The control input of the attitude loop was the three-axis torque. It is noted here that, different from the position control, the constraints of the torques and their corresponding changing rate were dynamically adjusted over time due to the variable arm length characteristic, as shown the high bound and low bound curves in Figure 7b,c. Nonetheless, the controller can ensure that the control inputs and their changing rate are always determined within the constraints without exceeding the boundaries. These results demonstrate that the constructed augmented MPC-based controller exhibits tracking accuracy in the presence of random noise, effectively coping with the uncertainty caused by changes in system parameters and thereby proving its adaptive ability.

In summary, the position loop achieved high-precision trajectory tracking despite random noise disturbances, and the attitude loop maintained excellent tracking performance, even in the presence of dynamic variations. These results validate the effectiveness of the proposed dual-loop augmented MPC-based control strategy for the morphing quadrotor system.

4.3. Robustness Test Results

To comprehensively evaluate the robustness of the proposed controller, a series of disturbance increment tests were conducted by adding bidirectional burst disturbances to the position loop at a specific time to observe the system’s dynamic compensation ability in response to external disturbances. The results show that the maximum disturbance amplitude that the system can withstand is ±0.1 m. Therefore, we will representatively demonstrate the system’s response at a certain moment (such as, for example, t = 3 s) when the disturbance amplitude is 0.1 m, as shown in Figure 8 and Figure 9. Overall, in the position loop, the horizontal axis ($x/y$) tracking errors remained stable throughout, while the vertical axis (z) exhibited instantaneous overshoot after disturbance but converged to a steady state within 0.1 s, as illustrated in Figure 8a. For control inputs, we have $\varphi$, $\theta$, and T, along with their changing rate transient fluctuations due to disturbance compensation demands, but these stabilized within 0.2 s, as shown in Figure 8b,c. In the attitude loop, the attitude angle tracking errors remained within 0.002°, as shown in Figure 9a, validating the high-precision tracking capability of the inner loop. Although torque ${\tau }_1$ and ${\tau }_2$, as well as their changing rate, exhibited overshoot at the disturbance moment, they converged rapidly within 0.2 s without triggering attitude oscillations, as shown in Figure 9b,c. The experimental results demonstrate that the proposed controller effectively blocks disturbance chain propagation while ensuring position tracking accuracy, providing a reliable solution for full-state stability control in dynamic environments.

5. Conclusions

In response to the current research situation, where few morphological modifications have been considered in the motion planning of existing morphing quadrotors, a synergistic framework for trajectory planning and control is proposed to enable the autonomous flight of quadrotors. In motion planning, the arm length variable is integrated into the minimum snap optimization framework to achieve joint optimization of position and morphology. The generated trajectory can circumvent obstacles that cannot be passed by traditional fixed quadrotors by dynamically adjusting the arm length. In terms of control, we designed full-system control based on the augmented model and a dual-loop MPC architecture, with the outer loop tracking the position trajectory and the inner loop synchronizing the attitude and arm length. An integrator was also embedded in the control to eliminate the steady-state error, and precise trajectory tracking of the quadrotor was achieved under dynamic changes in the quadrotor parameters. Additionally, a step disturbance was introduced to verify the system’s robustness.