Optimal Aggressive Constrained Trajectory Synthesis and Control for Multi-Copters

Abstract

In this paper, we propose a novel time and control effort optimal aggressive trajectory synthesis and control design methodology. The trajectory synthesis is a modified minimum snap design but with specific position and orientation constraints on a multi-copter, such as flying through tight spaces (windows) at specific orientations. The paper also introduces a means to stitch together multiple flight segments, enforce smoothness, and minimize segment times as well as the overall time, thereby resulting in very aggressive and feasible trajectories. A novel analysis for a specific scenario when no yaw angle specifications are provided is conducted, wherein a trade-off results in additional aggressiveness. The control algorithms to follow these trajectories are based on an inverse dynamics approach. Several candidate high-fidelity simulations are performed to verify the effectiveness of the proposed approach.

1. Introduction

Multi-copters, such as quadcopters, hexacopters, octocopters, and others have been very popular in recent years mainly because of their availability, mobility, agility, and flexibility. They are a great platform for control experiments and various applications. Their characteristics also make them an attractive choice for high-speed aerial navigation through complex environments. Various researches related to aggressive trajectory generation and aggressive maneuver tracking have been conducted. Some recent examples include [1], in which the authors proposed a novel control law for accurate tracking of aggressive quadcopter trajectories. In [2] the authors presented a framework to do optimal time allocation for quadcopter trajectory generation. In [3] the authors addressed the problem of performing aggressive quadcopter maneuvers that are attitude-constrained.

Our main contribution in this paper is the development of a synthesis framework that fully utilizes the available dynamic capability of the multi-copter when performing the most aggressive maneuver. We pursue aggressiveness because time is a critical issue in the given scenario. We adopt aggressive maneuvers because the environment is complicated and sometimes very specific maneuvers are needed in order to satisfy path and vehicle state constraints, given the environment model. However, the level of aggressiveness one can achieve depends on the optimality of the trajectory plan and the dynamic capability of the vehicle. It will be valuable if one can generate a trajectory that fully utilizes the dynamic capability of the vehicle while accommodating the maneuvers required at specific locations. Therefore, in this research, we assume that the dynamic capability of the multi-copter is limited by its maximum rotor thrust and define the aggressiveness accordingly.

For multi-copter trajectory generation and motion control, ideas such as multi-segment polynomial, minimum derivative optimization, and differential flatness inverse dynamics analysis are widely adopted [4,5,6]. To achieve an aggressive trajectory, methods such as segment time allocation [7] and spatial-temporal trajectory [2] have been proposed. However, we do not yet have a synthetic architecture to find the most aggressive trajectory based on the waypoints, the requirement for vehicle heading and maneuvering at waypoints, and the constraints of rotor thrust for a multi-coper. We would further like to emphasize that the contribution of this research lies in the development of a complete synthesis to fulfill this gap. The constraint on vehicle maneuvering at waypoints is posed as a window passing through problem and solved through the trajectory optimization as trajectory derivative constraints. The maximum rotor thrust for the trajectory is obtained through the inverse dynamics analysis. As the optimized aggressive trajectory is obtained, a geometric controller is used to perform the polynomial trajectory tracking and verify the feasibility of the trajectory and the accuracy of the inverse dynamics analysis. Further, a novel method for yaw trajectory optimization is also proposed to further improve the aggressive performance in the case when there are no specific requirements on the vehicle heading.

The rest of the paper is organized as follows. Section 2 describes the problem and the setup. Section 3 summarizes the mathematical modeling providing the basic governing equations. Section 4 discusses the solution methodology in detail, followed by the numerical results in Section 5. Section 6 summarizes the performance of the geometric controller with Section 7 providing details on the modification to the trajectory should there be no heading constraints. Finally, Section 8 provides concluding remarks and summarizes future work.

2. Problem Description and Setup

While aggressive flight using multi-copters is widely discussed/mentioned, there is not much effort put toward defining aggressiveness and finding the most aggressive trajectory with complex maneuver requirements for a given vehicle model. Therefore, in this paper, the problem is addressed as: Given waypoint coordinates, heading angles, and some vehicle velocity and attitude constraints at these waypoints, how can we find a feasible trajectory that fully utilizes the dynamic capability of a multi-copter to achieve the optimal aggressiveness? A formal definition of aggressiveness will be provided in the latter sections.

3. Mathematical Model Description

A generic model for quadcopters is used in this paper. Note that the proposed approach can be applied to different configurations of multi-copters by modifying the vehicle force and moment model according to the desired rotor configuration and control allocation method. The coordinate systems and forces and moments generated by the rotors are shown in Figure 1.

The body frame, B, is attached to the center of mass of the quadcopter and rotor 1 is on the positive $X_B$-axis. The gravitational acceleration g is in the $-Z_I$ direction of the inertial frame, I. Euler angles roll $\varphi$, pitch $\theta$, and yaw $\psi$ are used to define orientation from inertial frame to body frame. Note, $Z-X-Y$ rotation order is used here, and therefore the rotation matrix for transforming coordinates from B to I is given by:

$${\mathit{R}}_{IB}=\left[\begin{array}{ccc}cos\psi cos\theta -sin\varphi sin\psi sin\theta & -cos\varphi sin\psi & cos\psi sin\theta +cos\theta sin\varphi sin\psi \\ cos\theta sin\psi +cos\psi sin\varphi sin\theta & cos\varphi cos\psi & sin\psi sin\theta -cos\psi cos\theta sin\varphi \\ -cos\varphi sin\theta & sin\varphi & cos\varphi cos\theta \end{array}\right]$$

(1)

The position vector of the quadcopter in the inertial frame is denoted by $\mathrm{r}$. With the gravity force acting in the $-Z_I$ direction and the forces of the rotors acting in the $Z_B$ direction, the equation governing the acceleration of the quadcopter with respect to inertial frame is given by:

$$m\ddot{\mathbf{r}}=\left[\begin{array}{c}0\\ 0\\ -mg\end{array}\right]+{\mathit{R}}_{IB}\left[\begin{array}{c}0\\ 0\\ F_1+F_2+F_3+F_4\end{array}\right]$$

(2)

With p, q, and r denoting the components of angular velocity of the quadcopter in the body frame, the rotational kinematics equation is given by:

$$\left[\begin{array}{c}p\\ q\\ r\end{array}\right]=\left[\begin{array}{ccc}cos\theta & 0& -cos\varphi sin\theta \\ 0& 1& sin\varphi \\ sin\theta & 0& cos\varphi cos\theta \end{array}\right]\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]$$

(3)

Assuming rotors 1 and 3 rotate in the $-Z_B$ direction while 2 and 4 rotate in the $Z_B$ direction, $M_1$ and $M_3$ act in the $Z_B$ direction while $M_2$ and $M_4$ act in the $-Z_B$ direction since the moment produced by the rotor is opposite to the direction of rotation of the blade. With $\mathit{I}$ denoting the moment of inertia of the quadcopter referenced to the center of mass and L denoting the distance from the axis of rotation of the rotors to the center of the quadcopter, the rotational dynamics equation is given by:

$$\mathit{I}\left[\begin{array}{c}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right]=\left[\begin{array}{c}L(F_2-F_4)\\ L(F_3-F_1)\\ M_1-M_2+M_3-M_4\end{array}\right]-\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\times \mathit{I}\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$$

(4)

Define the input $u={\left[u_1{u}_2{u}_3{u}_4\right]}^T$ wherein $u_1$ is the total force from the rotors and $u_2$, $u_3$ and $u_4$ are the moments about $X_B$, $Y_B$ and $Z_B$ axes. Following [6], we assume that the force and moment produced by the ith rotor are proportional to the square of its rotational speed as:

$$F_i=k_F{\omega }_i^2,M_i=k_M{\omega }_i^2$$

(5)

The relationship between the input and the angular speed of the rotors can be represented as:

$$\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\\ u_4\end{array}\right]=\left[\begin{array}{c}{F}_1+F_2+F_3+F_4\\ L(F_2-F_4)\\ L(F_3-F_1)\\ M_1-M_2+M_3-M_4\end{array}\right]=\left[\begin{array}{cccc}{k}_F& k_F& k_F& k_F\\ 0& k_{F}L& 0& -k_{F}L\\ -k_{F}L& 0& k_{F}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)

4. Solution Methodology

We use multi-segment polynomials to generate the trajectory and assure its smoothness by having trajectory derivative continuities. To optimize the polynomial coefficients, we form the quadratic problem with the cost on the trajectory derivative which is related to the actuator input of the vehicle. We accommodate waypoint velocity and attitude requirements in the optimization problem as equality constraints. To optimize the segment time between waypoints, we introduce an augmented cost on both trajectory derivative and total time. We connect the polynomial trajectory and actual quadcopter dynamics and rotor inputs by performing inverse dynamics analysis. We finally define aggressiveness and find the optimal trajectory with desired aggressiveness by tracking the desired maximum rotor force needed for the optimized trajectory. The whole methodology and the solution procedure are illustrated below in Figure 2.

4.1. Multi-Segment Polynomial Trajectory Optimization

Multi-segment polynomials are used to generate the trajectory. For each segment connecting one and the subsequent waypoint, independent polynomials are used to represent the quadcopter states x, y, z, and $\psi$ (yaw angle). Each polynomial segment is represented as:

$$P\left(t\right)=p_N{t}^N+p_{N-1}{t}^{N-1}+\dots +p_0=\sum _{i=0}^N{p}_i{t}^i$$

(7)

The cost function on the integral of the quadratic of the trajectory derivatives is:

$$J={\int }_0^T[c_{0}P{\left(t\right)}^2+c_1{P}^{\prime }{\left(t\right)}^2+c_2{P}^{″}{\left(t\right)}^2+\dots +c_N{P}^{\left(N\right)}{\left(t\right)}^2]dt$$

(8)

where T is the flight time for the trajectory segment and $c_r$ is user-specified penalty on the $r^{th}$ derivative of the trajectory. This function can be written in matrix form as:

$$J={\overline{p}}^T\mathit{Q}\overline{p}$$

(9)

where $\overline{p}={[p_0{p}_1\dots p_N]}^T$ and $\mathit{Q}$ is the cost matrix. Following the formulation in [4], $\mathit{Q}$ can be constructed as:

$$\begin{array}{cc}\hfill & \mathrm{for}r=0,1,\dots N,i=0,1,\dots N,l=0,1,\dots N\hfill \\ \hfill & {\mathit{Q}}_r(i+1,l+1)=\left\{\begin{array}{cc}[\prod _{m=0}^{r-1}(i-m)(l-m)]\frac{T^{i+l-2r+1}}{i+l-2r+1}\hfill & \mathrm{if}i\ge r\wedge l\ge r\hfill \\ 0\hfill & \mathrm{if}i<r\vee l<r\hfill \end{array}\right.\hfill \end{array}$$

(10)

$$\mathit{Q}=\sum _{r=0}^N{c}_r{\mathit{Q}}_r$$

(11)

In this paper, to minimize the input needed to achieve optimal aggressiveness, we adopted the choice in [5] which is to minimize the snap (4th derivative) in trajectory x, y, and z while minimizing the acceleration (2nd derivative) in trajectory $\psi$. Therefore, we use $c_4=1$ while all other coefficients are set to zero for x, y, and z polynomial, and only $c_2=1$ for yaw polynomial. The constraints on the derivatives on the endpoints of a polynomial segment can be imposed as a linear function of the coefficients:

$$\begin{array}{c}\mathit{A}\overline{p}=\mathit{b}\\ \mathit{A}=\left[\begin{array}{c}{\mathit{A}}_0\\ {\mathit{A}}_T\end{array}\right],\mathit{b}=\left[\begin{array}{c}{\mathit{b}}_0\\ {\mathit{b}}_T\end{array}\right]\end{array}$$

(12)

where $\mathit{A}$ is constructed by evaluating the components in the derivative formulations of the polynomial at $t=0$ and $t=T$ corresponding to the appropriate coefficients as:

$${\mathit{A}}_{0_{rn}}=\left\{\begin{array}{cc}\prod _{m=0}^{r-1}(n-m)\hfill & \mathrm{if}r=n\hfill \\ 0\hfill & \mathrm{if}r\ne n\hfill \end{array}\right.$$

(13)

$${\mathit{A}}_{T_{rn}}=\left\{\begin{array}{cc}[\prod _{m=0}^{r-1}(n-m)]T^{n-r}\hfill & \mathrm{if}r\le n\hfill \\ 0\hfill & \mathrm{if}r>n\hfill \end{array}\right.$$

(14)

We use the 0th order derivative constraint to specify the waypoint position. Higher order derivatives can be used to specify desired waypoint velocity, acceleration, etc., e.g., to enforce that the quadcopter starts from rest at the beginning of a trajectory. If not specified, these derivatives are subject to minimization of the cost function. Having assembled $\mathit{Q}$, $\mathit{A}$, and $\mathit{b}$, the quadratic problem can be written below. There are methods to solve such a standard equality constrained QP [8]. In this research, the Matlab solver “quadprog” is used.

$$\begin{array}{cc}\hfill \underset{\overline{p}}{min}& {\overline{p}}^T\mathit{Q}\overline{p}\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \mathit{A}\overline{p}-\mathit{b}=0\hfill \end{array}$$

(15)

For M polynomial segments, the joint optimization can be composed by concatenating their cost matrices in a block-diagonal fashion as:

$$J_{\mathrm{joint}}={\left[\begin{array}{c}{\overline{p}}_1\\ ⋮\\ {\overline{p}}_M\end{array}\right]}^T\left[\begin{array}{ccc}{\mathit{Q}}_1\left(T_1\right)& 0& 0\\ 0& \ddots & 0\\ 0& 0& {\mathit{Q}}_M\left(T_M\right)\end{array}\right]\left[\begin{array}{c}{\overline{p}}_1\\ ⋮\\ {\overline{p}}_M\end{array}\right]$$

(16)

The derivative constraints can also be concatenated in a block-diagonal fashion as:

$$\left[\begin{array}{ccc}{\mathit{A}}_1\left(T_1\right)& 0& 0\\ 0& \ddots & 0\\ 0& 0& {\mathit{A}}_M\left(T_M\right)\end{array}\right]\left[\begin{array}{c}{\overline{p}}_1\\ ⋮\\ {\overline{p}}_M\end{array}\right]={\mathit{A}}_{\mathrm{der}}\left[\begin{array}{c}{\overline{p}}_1\\ ⋮\\ {\overline{p}}_M\end{array}\right]=\left[\begin{array}{c}{\mathit{b}}_1\\ ⋮\\ {\mathit{b}}_M\end{array}\right]$$

(17)

To have a feasible smooth trajectory, the derivatives should be continuous between segments. The continuity constraints must be imposed to ensure that the derivatives at the end of the ith segment match the derivatives at the beginning of the $(i+1)$th segment:

$${\mathit{A}}_{T,i}{\overline{p}}_i={\mathit{A}}_{0,i+1}{\overline{p}}_{i+1}$$

(18)

$$\left[\begin{array}{ccccccc}{\mathit{A}}_{T,1}& -{\mathit{A}}_{0,2}& 0& 0& \dots & 0& 0\\ 0& {\mathit{A}}_{T,2}& -{\mathit{A}}_{0,3}& 0& \dots & 0& 0\\ ⋮& ⋮& ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& 0& 0& 0& \dots & {\mathit{A}}_{T,M-1}& -{\mathit{A}}_{0,M}\end{array}\right]\left[\begin{array}{c}{\overline{p}}_1\\ {\overline{p}}_2\\ {\overline{p}}_3\\ ⋮\\ {\overline{p}}_{M-1}\\ {\overline{p}}_M\end{array}\right]=A_{\mathrm{con}}\left[\begin{array}{c}{\overline{p}}_1\\ {\overline{p}}_2\\ {\overline{p}}_3\\ ⋮\\ {\overline{p}}_{M-1}\\ {\overline{p}}_M\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\\ ⋮\\ 0\\ 0\end{array}\right]$$

(19)

These constraints can be compiled into a single set of linear equality constraints for the joint optimization problem:

$$\left[\begin{array}{c}{\mathit{A}}_{\mathrm{der}}\\ {\mathit{A}}_{\mathrm{con}}\end{array}\right]\left[\begin{array}{c}{\overline{p}}_1\\ ⋮\\ {\overline{p}}_M\end{array}\right]=\left[\begin{array}{c}{\mathit{b}}_1\\ ⋮\\ {\mathit{b}}_M\\ 0\\ ⋮\\ 0\end{array}\right]$$

(20)

4.2. Constraints on Vehicle Velocity and Attitude at Waypoints

In scenarios such that the quadcopter needs to pass through some narrow gap (e.g., a window) as shown in Figure 3, we can utilize the derivative constraints to achieve the objective.

Assume a waypoint at the center of the window and the window orientation is defined with $Z-X-Y$ Euler angles roll $\varphi$, pitch $\theta$, and yaw $\psi$. The window forward vector in inertial frame can be obtained as:

$${\mathit{w}}_{\mathrm{F}}={\mathit{R}}_{IB}\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}cos\psi cos\theta -sin\varphi sin\psi sin\theta \\ cos\theta sin\psi +cos\psi sin\varphi sin\theta \\ -cos\varphi sin\theta \end{array}\right]=\left[\begin{array}{c}{W}_{{\mathrm{F}}_x}\\ W_{{\mathrm{F}}_y}\\ W_{{\mathrm{F}}_z}\end{array}\right]$$

(21)

The window upward vector in inertial frame can be obtained as:

$${\mathit{w}}_{\mathrm{U}}={\mathit{R}}_{IB}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=\left[\begin{array}{c}cos\psi sin\theta +cos\theta sin\varphi sin\psi \\ sin\psi sin\theta -cos\psi cos\theta sin\varphi \\ cos\varphi cos\theta \end{array}\right]=\left[\begin{array}{c}{W}_{{\mathrm{U}}_x}\\ W_{{\mathrm{U}}_y}\\ W_{{\mathrm{U}}_z}\end{array}\right]$$

(22)

To fly through the window safely, the velocity vector at the waypoint should be aligned with the window forward vector. Therefore, the cross product of the vectors should be zero, and the constraints for the waypoint 1st order derivatives can be composed as:

$$\mathit{v}\times {\mathit{w}}_{\mathrm{F}}=\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{z}\end{array}\right]\times \left[\begin{array}{c}{W}_{{\mathrm{F}}_x}\\ W_{{\mathrm{F}}_y}\\ W_{{\mathrm{F}}_z}\end{array}\right]=\left[\begin{array}{c}\dot{y}{W}_{{\mathrm{F}}_z}-\dot{z}{W}_{{\mathrm{F}}_y}\\ \dot{z}{W}_{{\mathrm{F}}_x}-\dot{x}{W}_{{\mathrm{F}}_z}\\ \dot{x}{W}_{{\mathrm{F}}_y}-\dot{y}{W}_{{\mathrm{F}}_x}\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]$$

(23)

From the equation of acceleration, we can deduce that ${\mathit{z}}_B$ in inertial frame is in the direction of the vector ${[\ddot{x}\ddot{y}\ddot{z}+g]}^T$:

$$m\left[\begin{array}{c}\ddot{x}\\ \ddot{y}\\ \ddot{z}\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ -mg\end{array}\right]+{\mathit{R}}_{IB}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\sum F\Rightarrow m\left[\begin{array}{c}\ddot{x}\\ \ddot{y}\\ \ddot{z}+g\end{array}\right]={\mathit{z}}_B·\sum F$$

(24)

For safely flying through the window, the ${\mathit{z}}_B$ vector of the quadcopter should be aligned with the window upward vector. Therefore, the constraints for the waypoint 2nd order derivatives can be composed as:

$$\left[\begin{array}{c}\ddot{x}\\ \ddot{y}\\ \ddot{z}+g\end{array}\right]\times \left[\begin{array}{c}{W}_{{\mathrm{U}}_x}\\ W_{{\mathrm{U}}_y}\\ W_{{\mathrm{U}}_z}\end{array}\right]=\left[\begin{array}{c}\ddot{y}{W}_{{\mathrm{U}}_z}-(\ddot{z}+g)W_{{\mathrm{U}}_y}\\ (\ddot{z}+g)W_{{\mathrm{U}}_x}-\ddot{x}{W}_{{\mathrm{U}}_z}\\ \ddot{x}{W}_{{\mathrm{U}}_y}-\ddot{y}{W}_{{\mathrm{U}}_x}\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]$$

(25)

$$\Rightarrow \left[\begin{array}{c}\ddot{y}{W}_{{\mathrm{U}}_z}-\ddot{z}{W}_{{\mathrm{U}}_y}\\ \ddot{z}{W}_{{\mathrm{U}}_x}-\ddot{x}{W}_{{\mathrm{U}}_z}\\ \ddot{x}{W}_{{\mathrm{U}}_y}-\ddot{y}{W}_{{\mathrm{U}}_x}\end{array}\right]=\left[\begin{array}{c}g{W}_{{\mathrm{U}}_y}\\ -g{W}_{{\mathrm{U}}_x}\\ 0\end{array}\right]$$

(26)

For these constraints, the relationship between x, y, and z derivatives needs to be specified. Therefore, the joint optimization of x, y, and z should be composed by concatenating their cost and constraint matrices in a block-diagonal fashion as:

$$J_{xyz}={\left[\begin{array}{c}{\overline{\overline{p}}}_x\\ {\overline{\overline{p}}}_y\\ {\overline{\overline{p}}}_z\end{array}\right]}^T\left[\begin{array}{ccc}{\mathit{Q}}_x& 0& 0\\ 0& {\mathit{Q}}_y& 0\\ 0& 0& {\mathit{Q}}_z\end{array}\right]\left[\begin{array}{c}{\overline{\overline{p}}}_x\\ {\overline{\overline{p}}}_y\\ {\overline{\overline{p}}}_z\end{array}\right]$$

(27)

$$\left[\begin{array}{ccc}{\mathit{A}}_x& 0& 0\\ 0& {\mathit{A}}_y& 0\\ 0& 0& {\mathit{A}}_z\end{array}\right]\left[\begin{array}{c}{\overline{\overline{p}}}_x\\ {\overline{\overline{p}}}_y\\ {\overline{\overline{p}}}_z\end{array}\right]=\left[\begin{array}{c}{\mathit{b}}_x\\ {\mathit{b}}_y\\ {\mathit{b}}_z\end{array}\right]$$

(28)

where ${\overline{\overline{p}}}_x={[{\overline{p}}_{x_1}\dots {\overline{p}}_{x_M}]}^T$, ${\overline{\overline{p}}}_y={[{\overline{p}}_{y_1}\dots {\overline{p}}_{y_M}]}^T$ and ${\overline{\overline{p}}}_z={[{\overline{p}}_{z_1}\dots {\overline{p}}_{z_M}]}^T$. With this joint optimization setup, the constraints for a window passage on waypoint s in an M-segment trajectory can be implemented as:

$$\left[\begin{array}{ccc}0& W_{{\mathrm{F}}_z}{\mathit{v}}_{0,s}& -W_{{\mathrm{F}}_y}{\mathit{v}}_{0,s}\\ -W_{{\mathrm{F}}_z}{\mathit{v}}_{0,s}& 0& W_{{\mathrm{F}}_x}{\mathit{v}}_{0,s}\\ W_{{\mathrm{F}}_y}{\mathit{v}}_{0,s}& -W_{{\mathrm{F}}_x}{\mathit{v}}_{0,s}& 0\\ 0& W_{{\mathrm{U}}_z}{\mathit{a}}_{0,s}& -W_{{\mathrm{U}}_y}{\mathit{a}}_{0,s}\\ -W_{{\mathrm{U}}_z}{\mathit{a}}_{0,s}& 0& W_{{\mathrm{U}}_x}{\mathit{a}}_{0,s}\\ W_{{\mathrm{U}}_y}{\mathit{a}}_{0,s}& -W_{{\mathrm{U}}_x}{\mathit{a}}_{0,s}& 0\end{array}\right]\left[\begin{array}{c}{\overline{\overline{p}}}_x\\ {\overline{\overline{p}}}_y\\ {\overline{\overline{p}}}_z\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\\ g{W}_{{\mathrm{U}}_y}\\ -g{W}_{{\mathrm{U}}_x}\\ 0\end{array}\right]$$

(29)

$$\left\{\begin{array}{c}{\mathit{v}}_{0,s}=\left[\begin{array}{ccc}{\left[0_{\times (n+1)}\right]}_{\times (s-1)}& {\mathit{v}}_0& {\left[0_{\times (n+1)}\right]}_{\times (M-s)}\end{array}\right]\hfill \\ {\mathit{a}}_{0,s}=\left[\begin{array}{ccc}{\left[0_{\times (n+1)}\right]}_{\times (s-1)}& {\mathit{a}}_0& {\left[0_{\times (n+1)}\right]}_{\times (M-s)}\end{array}\right]\hfill \end{array}\right.$$

(30)

For a segment polynomial $P\left(t\right)=p_0+p_1{t}^1+p_2{t}^2+p_3{t}^3+\dots +p_N{t}^N$, the 1st order derivative (velocity) at $t=0$ is ${\mathit{v}}_0\overline{p}$ and ${\mathit{v}}_0=[0100\dots 0]$. The 2nd order derivative (acceleration) at $t=0$ is ${\mathit{a}}_0\overline{p}$ and ${\mathit{a}}_0=[0020\dots 0]$.

4.3. Segment Time Optimization

The above optimization finds the optimal trajectory polynomials for the given segment times. However, in general cases, we do not specify segment times, and instead, we would like to also optimize them to achieve better aggressiveness. To optimize segment times, we modify the cost function in the form:

$$J_T=J_{org}+c_T\sum _{i=1}^M{T}_i$$

(31)

where $J_{org}$ combines the original cost on x, y, z, and $\psi$ while $c_T$ is a penalty on total time. The cost on x, y, z is the integral of square of derivatives on distance while the cost on $\psi$ is on angle. To combine the costs, two coefficients ${\mu }_r$ and ${\mu }_{\psi }$ are introduced to non-dimensionalize the costs.

$$J_{org}={\mu }_r{J}_{xyz}+{\mu }_{\psi }{J}_{\psi }$$

(32)

In this research, we minimize the 4th derivative in trajectory x, y, and z while minimizing the 2nd derivative in trajectory yaw angle. Therefore, the above equation can be represented as:

$$\begin{array}{cc}\hfill J_{org}=& {\mu }_r{\int }_0^{T_f}\left[{\left(\frac{d^{4}x}{d{t}^4}\right)}^2+{\left(\frac{d^{4}y}{d{t}^4}\right)}^2+{\left(\frac{d^{4}z}{d{t}^4}\right)}^2\right]dt\hfill \\ \hfill & +{\mu }_{\psi }{\int }_0^{T_f}{\left(\frac{d^2\psi }{d{t}^2}\right)}^{2}dt\hfill \end{array}$$

(33)

where ${\mu }_r$ and ${\mu }_{\psi }$ are defined as:

$${\mu }_r={\left[\frac{1}{max\left(\left|\frac{d^{4}x}{d{t}^4}\right|,\left|\frac{d^{4}y}{d{t}^4}\right|,\left|\frac{d^{4}z}{d{t}^4}\right|\right)}\right]}^2$$

(34)

$${\mu }_{\psi }={\left[\frac{1}{max\left(\left|\frac{d^2\psi }{d{t}^2}\right|\right)}\right]}^2$$

(35)

We find ${\mu }_r$ and ${\mu }_{\psi }$ in the first iteration with initial segment times and then use them as constants throughout the optimization process. With the cost function defined, we perturb each segment time by some $\delta t$ to obtain the gradient of the cost function with respect to each segment time. This is then used in a gradient descent method to find the time allocation for the minimum cost iteratively.

$$\overline{T}={\left[\begin{array}{ccc}{T}_1& \dots & T_M\end{array}\right]}^T$$

(36)

$${\nabla }_i{J}_T=\frac{J_T{\left[\begin{array}{ccccc}{T}_1& \dots & T_i+\delta t& \dots & T_M\end{array}\right]}^T-J_T\left(\overline{T}\right)}{\delta t}$$

(37)

$$T_i^{new}=T_i+\alpha \Delta T_i,\Delta T_i=-\frac{{\nabla }_i{J}_T}{\parallel {\nabla }_i{J}_T\parallel }$$

(38)

Since this is a high-dimensional problem with a complex cost function, the step size $\alpha$ can easily become too small or too large during the iterations and lead to slow convergence or even divergence. For numerical efficiency, stability, and convergence, we use the backtracking line search method [9] to find a suitable step size $\alpha$ in every iteration.

$$\mathrm{while}J(\overline{T}+\alpha \Delta \overline{T})>J\left(\overline{T}\right)+ϵ\alpha \nabla J_T^T\Delta \overline{T},\alpha :=\beta \alpha$$

(39)

With $\beta =0.5$ and $ϵ=0.0001$, we have fast and stable convergence in our test cases. Figure 4 shows the result of segment time optimization for a simple 2D 4-waypoint scenario. The initial segment time is $\overline{T}={\left[131\right]}^T$ s, and we optimize it with two different $c_T$ values, 50 and 5000. The result trajectories look identical because they have similar segment time distribution while the total time turned out to be 6.28 and 3.53 seconds, respectively. It is observed that, with this segment time optimization process, the optimal segment time distribution will be found to minimize the cost on integral of the square of trajectory derivatives, while the optimal total time is found based on the value of $c_T$. The larger $c_T$ used, the smaller the total time will be, which makes the trajectory more aggressive.

4.4. Inverse Dynamics

To connect the polynomial trajectory optimization framework and actual quadcopter dynamics, we perform the inverse dynamics analysis. The differential flatness method is widely adopted in inverse dynamics analysis for multi-copters [6]. In this process, we will find all the states and inputs of the quadcopter according to the trajectory x, y, z, $\psi$, and their derivatives. For orientation $\varphi$ and $\theta$, first from the equation of acceleration we have:

$${\mathit{z}}_B=\frac{{\left[\begin{array}{ccc}\ddot{x}& \ddot{y}& \ddot{z}+g\end{array}\right]}^T}{\parallel \left[\begin{array}{ccc}\ddot{x}& \ddot{y}& \ddot{z}+g\end{array}\right]\parallel },u_1=m\parallel \left[\begin{array}{ccc}\ddot{x}& \ddot{y}& \ddot{z}+g\end{array}\right]\parallel$$

(40)

Assume ${\mathit{x}}_C$ is the vector obtained by rotating ${\mathit{x}}_I$ around ${\mathit{z}}_I$ by yaw angle $\psi$:

$${\mathit{x}}_C={\left[\begin{array}{ccc}cos\psi & sin\psi & 0\end{array}\right]}^T$$

(41)

We can determine ${\mathit{x}}_B$ and ${\mathit{y}}_B$ by:

$${\mathit{y}}_B=\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$$

(42)

With vehicle body frame defined, we can determine the rotation matrix and roll and pitch angles by:

$${\mathit{R}}_{IB}=\left[\begin{array}{ccc}{\mathit{x}}_B& {\mathit{y}}_B& {\mathit{z}}_B\end{array}\right]$$

(43)

$$\varphi =arcsin\left[{\mathit{R}}_{IB}(3,2)\right],\theta =arctan\left[\frac{-{\mathit{R}}_{IB}(3,1)}{{\mathit{R}}_{IB}(3,3)}\right]$$

(44)

For angular velocity p, q, and r, first we take the 1st derivative of the equation of acceleration:

$$m\dot{\mathit{a}}={\dot{u}}_1{\mathit{z}}_B+{\mathbf{\omega }}_B\times u_1{\mathit{z}}_B$$

(45)

Substituting ${\dot{u}}_1={\mathit{z}}_B·m\dot{\mathit{a}}$ we have:

$${\mathbf{\omega }}_B\times {\mathit{z}}_B=\frac{m}{u_1}[\dot{\mathit{a}}-({\mathit{z}}_B·\dot{\mathit{a}}){\mathit{z}}_B]$$

(46)

With $\dot{\mathit{a}}=\left[\stackrel{⃛}{x}\stackrel{⃛}{y}\stackrel{⃛}{z}\right]$, the RHS is known. With ${\mathit{x}}_B$, ${\mathit{y}}_B$, and ${\mathit{z}}_B$ being unit vectors, ${\mathbf{\omega }}_B\times {\mathit{z}}_B$ can be considered as the projection of ${\mathbf{\omega }}_B$ onto the ${\mathit{x}}_B-{\mathit{y}}_B$ plane with 90${}^{\circ }$ shift. Therefore, the body angular velocities p and q can be determined as:

$$p=-({\mathbf{\omega }}_B\times {\mathit{z}}_B)·{\mathit{y}}_B,q=({\mathbf{\omega }}_B\times {\mathit{z}}_B)·{\mathit{x}}_B$$

(47)

From the rotational kinematics equation, we have:

$$\begin{array}{cc}\hfill & \dot{\psi }=-\frac{sin\theta }{cos\varphi }p+\frac{cos\theta }{cos\varphi }r\hfill \\ \hfill & \Rightarrow r=\left(\dot{\psi }+\frac{sin\theta }{cos\varphi }p\right)\frac{cos\varphi }{cos\theta }\hfill \end{array}$$

(48)

With p, q, and r solved, we have ${\mathbf{\omega }}_B={\mathit{R}}_{IB}{\left[pqr\right]}^T$, and $\dot{\theta }$ and $\dot{\varphi }$ can also be obtained from the inversion of the rotational kinematics equation. For angular acceleration $\dot{p}$, $\dot{q}$ and $\dot{r}$, first we take the 2nd derivative of the equation of acceleration:

$$\begin{array}{c}\hfill m\ddot{\mathit{a}}={\ddot{u}}_1{\mathit{z}}_B+2{\mathbf{\omega }}_B\times {\dot{u}}_1{\mathit{z}}_B+{\mathbf{\omega }}_B\times {\mathbf{\omega }}_B\times u_1{\mathit{z}}_B+{\dot{\mathbf{\omega }}}_B\times u_1{\mathit{z}}_B\\ \hfill \Rightarrow {\dot{\mathbf{\omega }}}_B\times {\mathit{z}}_B=(m\ddot{\mathit{a}}-{\ddot{u}}_1{\mathit{z}}_B-2{\mathbf{\omega }}_B\times {\dot{u}}_1{\mathit{z}}_B-{\mathbf{\omega }}_B\times {\mathbf{\omega }}_B\times u_1{\mathit{z}}_B)/u_1\end{array}$$

(49)

With $\ddot{\mathit{a}}=\left[\stackrel{⃜}{x}\stackrel{⃜}{y}\stackrel{⃜}{z}\right]$, ${\ddot{u}}_1={\mathit{z}}_B·(m\ddot{\mathit{a}}-{\mathbf{\omega }}_B\times {\mathbf{\omega }}_B\times u_1{\mathit{z}}_B)$ and ${\dot{u}}_1={\mathit{z}}_B·m\dot{\mathit{a}}$, the RHS is known and the body angular accelerations $\dot{p}$ and $\dot{q}$ can be determined as:

$$\dot{p}=-({\dot{\mathbf{\omega }}}_B\times {\mathit{z}}_B)·{\mathit{y}}_B,\dot{q}=({\dot{\mathbf{\omega }}}_B\times {\mathit{z}}_B)·{\mathit{x}}_B$$

(50)

Taking derivative of previous $\dot{\psi }$ equation, we have:

$$\begin{array}{c}\hfill \ddot{\psi }=-\frac{sin\theta }{cos\varphi }\dot{p}+\frac{cos\theta }{cos\varphi }\dot{r}-p\frac{d}{dt}\left(\frac{sin\theta }{cos\varphi }\right)+r\frac{d}{dt}\left(\frac{cos\theta }{cos\varphi }\right)\\ \hfill \Rightarrow \dot{r}=\frac{cos\varphi }{cos\theta }\left[\ddot{\psi }+\frac{sin\theta }{cos\varphi }\dot{p}+p\left(\frac{cos\theta cos\varphi \dot{\theta }+sin\theta sin\varphi \dot{\varphi }}{{cos}^2\varphi }\right)\right.\\ \hfill \left.-r\left(\frac{cos\theta sin\varphi \dot{\varphi }-sin\theta cos\varphi \dot{\theta }}{{cos}^2\varphi }\right)\right]\end{array}$$

(51)

Next we determine the moment inputs from obtained angular velocity and acceleration by the rotational dynamics equation:

$$\left[\begin{array}{c}{u}_2\\ u_3\\ u_4\end{array}\right]=\mathit{I}\left[\begin{array}{c}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right]+\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\times \mathit{I}\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$$

(52)

Now we have found all the states and inputs of the quadcopter derived from the trajectory x, y, z, $\psi$, and their derivatives. We can further determine the angular speed and force produced of each rotor by:

$$\left[\begin{array}{c}{\omega }_1^2\\ {\omega }_2^2\\ {\omega }_3^2\\ {\omega }_4^2\end{array}\right]={\left[\begin{array}{cccc}{k}_F& k_F& k_F& k_F\\ 0& k_{F}L& 0& -k_{F}L\\ -k_{F}L& 0& k_{F}L& 0\\ k_M& -k_M& k_M& -k_M\end{array}\right]}^{-1}\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\\ u_4\end{array}\right],F_i=k_F{\omega }_i^2$$

(53)

4.5. Max Force Tracking and Aggressiveness Defined

We find the maximum rotor force needed for the trajectory through the inverse dynamics analysis and observe how the maximum force and the trajectory total time vary with different $c_T$ values. Figure 5 shows the analysis result for a 3D 4-waypoint scenario. It is observed that, with the increment in $c_T$, the total time decreases smoothly while the maximum force rises smoothly with increasing slope. With this relationship, we can track a particular total time or maximum force for a given scenario by the gradient descent method through $c_T$.

With optimal aggressiveness, the quadcopter should finish the tasks in the shortest time possible by means of its dynamic capability. Generally speaking, the dynamic capability of a quadcopter is restricted by the maximum thrust of its rotors. Therefore, we define aggressiveness as the percentage of excess thrust required for the optimized trajectory.

$$\mathrm{Aggressiveness}=\frac{F_{max\_req}-F_{hover}}{F_{max}-F_{hover}}·100\%$$

(54)

where $F_{max\_req}$ is the maximum rotor force required for the trajectory, $F_{hover}$ is the rotor force for steady hover, and $F_{max}$ is the maximum thrust of the rotor. One might conserve the aggressiveness for safety reasons. Given waypoints x, y, z, $\psi$, and the desired aggressiveness, the whole methodology to find the optimal trajectory and segment time allocation for a given quadcopter model is summarized in Figure 2.

5. Numerical Results

The quadcopter parameters we use in this research are from [10] and tabulated below in

Table 1. Quadcopter parameters.

m	1.023 kg	g	$9.81\mathbf{m}/{\mathbf{s}}^2$
L	0.2223 m	$I_{xx}$	$0.0095\mathrm{kg}·{\mathrm{m}}^2$
$k_F$	$1.4865·{10}^{-7}\mathrm{N}/{\mathrm{RPM}}^2$	$I_{yy}$	$0.0095\mathrm{kg}·{\mathrm{m}}^2$
$k_M$	$2.9250·{10}^{-9}\mathrm{N}·\mathrm{m}/{\mathrm{RPM}}^2$	$I_{zz}$	$0.0186\mathrm{kg}·{\mathrm{m}}^2$

.

We test the optimization framework with a four-waypoint scenario. The waypoint settings are tabulated below (yaw angles at waypoints 2 and 3 are not specified) in

Table 2. Waypoint settings in the scenario.

WPT Setting	$\mathit{x}\left(\mathit{m}\right)$	$\mathit{y}\left(\mathit{m}\right)$	$\mathit{z}\left(\mathit{m}\right)$	$\mathit{\psi }$
Waypoint 1	0	2	0	0
Waypoint 2	1	2	0	-
Waypoint 3	1	0	0.5	-
Waypoint 4	0	0	0.5	$-{180}^{\circ }$

.

We specify the mission to be from rest to rest, therefore the velocity and acceleration at the first and the last waypoint are constrained to be zero. There are two narrow windows to pass through in this scenario. The window settings are tabulated below in

Table 3. Window (WDW) settings in the scenario.

WDW Setting	$\mathit{\varphi }$	$\mathit{\theta }$	$\mathit{\psi }$	Location
Window 1	$0^{\circ }$	${15}^{\circ }$	$0^{\circ }$	Waypoint 2
Window 2	$-{30}^{\circ }$	$0^{\circ }$	$-{20}^{\circ }$	Waypoint 3

.

We specify the aggressiveness to be 80%. For this quadcopter model the rotor force for steady hover is $F_{hover}=2.5$ N. Assuming $F_{max}=3.75$ N, we have the maximum rotor force we can use for the trajectory as $F_{max\_req}=3.5$ N. With initial guess of $\overline{T}={\left[555\right]}^T$ s and $c_T=100$, we use 10th order segment polynomials and have derivative continuity constraints on up to 6th order derivative (Pop). Figure 6 shows the scenario and the optimal trajectory.

Figure 7, Figure 8 and Figure 9 show the result of the inverse dynamics analysis from the trajectory with respect to the particular quadcopter model. The magenta dashed lines mark the times of waypoint passages. It can be noticed that the quadcopter had a spike in angular acceleration and angular rate around waypoint 3 because it was making the maneuver to pass through the highly tilted window 2.

Figure 10 and Figure 11 show the control input and the rotor thrust along the trajectory. From the plot, we confirmed that the maximum rotor thrust used is $3.5$ N as expected.

Figure 12 shows the quadcopter on the trajectory with a fixed time interval of 0.33 s. It can be observed that the quadcopter had the highest speed in segment 2 so it can maneuver through the highly tilted window 2. The successful passage through the narrow windows can be visually confirmed. If we further check the data at waypoint 3, we have the window forward and upward vectors as ${\mathit{w}}_F={[0.939-0.3420]}^T$ and ${\mathit{w}}_F={\left[0.1710.4690.866\right]}^T$. Additionally, the quadcopter velocity and thrust vectors (aligned with ${\mathit{z}}_B$) are $\mathit{v}={[-1.7770.64700]}^T$ and $\mathit{f}={\left[1.4543.9957.364\right]}^T$. The velocity and thrust vectors are confirmed to be aligned with the window forward and upward vectors. Finally, the optimal time allocation was found as ${\overline{T}}_{opt}={\left[1.292.292.62\right]}^T$ s.

6. Geometric Control and Simulation Result

To perform the polynomial trajectory tracking and verify the result of the aggressive trajectory optimization, we adopted the geometric controller proposed by Lee et al. in [11] and implemented it in MATLAB/Simulink. The controller is constructed in two parts, trajectory tracking and attitude tracking. The control input of total force f is obtained by the trajectory tracking part with:

$$f=-(-k_x{\mathit{e}}_x-k_v{\mathit{e}}_v-mg{\mathit{e}}_3+m{\ddot{\mathit{x}}}_d)·\mathit{R}{\mathit{e}}_3$$

(55)

The control input of the moments is obtained by the attitude tracking part with:

$$M=-k_R{\mathit{e}}_R-k_{\omega }{\mathit{e}}_{\omega }+\mathbf{\omega }\times \mathit{I}\mathbf{\omega }-I(\widehat{\mathbf{\omega }}{\mathit{R}}^T{\mathit{R}}_d{\mathbf{\omega }}_d-{\mathit{R}}^T{\mathit{R}}_d{\dot{\mathbf{\omega }}}_d)$$

(56)

where vector ${\mathit{e}}_3$ defines the direction of gravity and $\mathit{R}$ is the rotation matrix from body to inertial frame. The tracking errors for position ${\mathit{e}}_x$, velocity ${\mathit{e}}_v$, attitude ${\mathit{e}}_R$, and angular velocity ${\mathit{e}}_{\omega }$ are defined as:

$$\begin{array}{cc}\hfill & {\mathit{e}}_x=\mathit{x}-{\mathit{x}}_d\hfill \\ \hfill & {\mathit{e}}_v=\mathit{v}-{\mathit{v}}_d\hfill \end{array}$$

(57)

$$\begin{array}{cc}\hfill & {\mathit{e}}_R=\frac{1}{2}{({\mathit{R}}_d^T\mathit{R}-{\mathit{R}}^T{\mathit{R}}_d)}^{\vee }\hfill \\ \hfill & {\mathit{e}}_{\omega }=\mathbf{\omega }-{\mathit{R}}^T{\mathit{R}}_d{\mathbf{\omega }}_d\hfill \end{array}$$

(58)

where the $hatmap{}^{^}:{\mathbb{R}}^3\to \mathrm{SO}\left(3\right)$ is defined by the condition that $\widehat{\mathit{x}}\mathit{y}=\mathit{x}\times \mathit{y}$ for all $\mathit{x},\mathit{y}\in {\mathbb{R}}^3$. Additionally, the $veemap{}^{\vee }:\mathrm{SO}\left(3\right)\to {\mathbb{R}}^3$ is the inverse of the hat map. At a given moment, $\mathit{x}$, $\mathit{v}$, $\mathbf{\omega }$, and $\mathit{R}$ represent the position, linear velocity, body angular velocity, and rotation matrix of the vehicle. The corresponding desired position ${\mathit{x}}_d$ and velocity ${\mathit{v}}_d$ are captured from the polynomial trajectory. The desired body z axis can be obtained as:

$${\mathit{z}}_d=-\frac{-k_x{\mathit{e}}_x-k_v{\mathit{e}}_v-mg{\mathit{e}}_3+m{\ddot{\mathit{x}}}_d}{\parallel -k_x{\mathit{e}}_x-k_v{\mathit{e}}_v-mg{\mathit{e}}_3+m{\ddot{\mathit{x}}}_d\parallel }$$

(59)

With the desired body z axis, yaw angle, and derivatives of the polynomial trajectory available, we can find the desired rotation matrix ${\mathit{R}}_d$, angular velocity ${\mathbf{\omega }}_d$, and angular acceleration ${\dot{\mathbf{\omega }}}_d$ by following the process discussed previously in inverse dynamics. The simulation is implemented with the following equations of motion:

$$\begin{array}{cc}\hfill & \dot{\mathit{x}}=\mathit{v}\hfill \\ \hfill & m\dot{\mathit{v}}=mg{\mathit{e}}_3-f\mathit{R}{\mathit{e}}_3\hfill \end{array}$$

(60)

$$\begin{array}{cc}\hfill & \dot{\mathit{R}}=\mathit{R}\widehat{\mathbf{\omega }}\hfill \\ \hfill & \mathit{I}\dot{\mathbf{\omega }}+\mathbf{\omega }\times \mathit{I}\mathbf{\omega }=M\hfill \end{array}$$

(61)

With $10\%$ error in estimated inertia, $\pm 1$ N noise added to f and $\pm 0.003$ Nm noise added to M (around $10\%$ of the maximum input used) as control input disturbance, we have the simulation result shown in figures below.

From the simulation result we can see that, despite the existence of inertia estimation error and control input disturbance, the geometric control had a good performance in tracking the polynomial trajectory. Position and yaw angle are well tracked and roll and pitch angles turned out to be very close to the inverse dynamics estimation as shown in Figure 13 and Figure 14. Therefore, the successful passage through the narrow windows is confirmed. Figure 15 and Figure 16 show that the linear acceleration is well tracked and the angular acceleration is very close to the estimation. From Figure 17, the control input used to track this polynomial trajectory is verified to be very close to the inverse dynamics estimation. The maximum rotor thrust used in the simulation is also confirmed to be around the maximum rotor force (3.5 N) requirement we specified in the trajectory generation as shown in Figure 18.

7. Yaw Trajectory Optimization

If we do not have a specific requirement on yaw angle, can we optimize the yaw trajectory to achieve better aggressive performance? In other words, by finding the yaw trajectory such that no $M_z$ control moment input is required to track the aggressive polynomial trajectory, we can further reduce the maximum rotor force needed and thereby track the aggressive trajectory with less motor force or achieve a faster trajectory with the same aggressiveness specified. From the equations of motion:

$$\left[\begin{array}{c}{M}_x\\ M_y\\ M_z\end{array}\right]=\mathit{I}\left[\begin{array}{c}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right]+\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\times \mathit{I}\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$$

(62)

Because of the symmetry, the moment of inertia of the quadcopter is assumed as:

$$\mathit{I}=\left[\begin{array}{ccc}{I}_{xx}& 0& 0\\ 0& I_{yy}& 0\\ 0& 0& I_{zz}\end{array}\right],\mathrm{and}{I}_{xx}=I_{yy}$$

(63)

$$M_z=I_{zz}\dot{r}+pq(I_{yy}-I_{xx})=I_{zz}\dot{r}$$

(64)

Therefore, to make $M_z=0$, we need $\dot{r}=0$. Assuming the initial state $r_0=0$, this means $\dot{r}=r=0$ for the whole trajectory. Based on this, we can modify the inverse dynamics analysis to solve for $\dot{\psi }$ and $\ddot{\psi }$ as:

$$\begin{array}{cc}\hfill & r=\left(\dot{\psi }+\frac{sin\theta }{cos\varphi }p\right)\frac{cos\varphi }{cos\theta }=0\hfill \\ \hfill & \Rightarrow \dot{\psi }=-\frac{sin\theta }{cos\varphi }p\hfill \end{array}$$

(65)

$$\begin{array}{c}\hfill \dot{r}=\frac{cos\varphi }{cos\theta }\left[\ddot{\psi }+\frac{sin\theta }{cos\varphi }\dot{p}+p\left(\frac{cos\theta cos\varphi \dot{\theta }+sin\theta sin\varphi \dot{\varphi }}{{cos}^2\varphi }\right)\right.\\ \hfill \left.-r\left(\frac{cos\theta sin\varphi \dot{\varphi }-sin\theta cos\varphi \dot{\theta }}{{cos}^2\varphi }\right)\right]=0\\ \hfill \Rightarrow \ddot{\psi }=-\frac{sin\theta }{cos\varphi }\dot{p}-p\left(\frac{cos\theta cos\varphi \dot{\theta }+sin\theta sin\varphi \dot{\varphi }}{{cos}^2\varphi }\right)\end{array}$$

(66)

Having $\dot{\psi }$ and $\ddot{\psi }$ at each moment in the trajectory, we can integrate $\psi$ iteratively throughout the inverse dynamics analysis by ${\psi }_{n+1}={\psi }_n+\dot{\psi }\Delta t+\frac{1}{2}\ddot{\psi }\Delta t^2$. Figure 19 and Figure 20 show a simple four-waypoint aggressive trajectory and the corresponding optimal yaw trajectory obtained by this method.

We converted this optimal yaw trajectory into a polynomial form and used the geometric controller to track this aggressive polynomial trajectory. Figure 21 shows that, by tracking this optimal yaw trajectory, the $M_z$ control input will be indeed close to zero.

Figure 22 shows the comparison of rotor thrust used to track the aggressive trajectory between constant zero yaw and the optimal yaw trajectory. The additional yaw control effort $M_z$ in the constant zero yaw case pulls the $F_1$ and $F_3$ rotor forces away from the $F_2$ and $F_4$ rotor forces and thereby increases the maximum rotor force needed. From the result, we can see that, by tracking the optimal yaw trajectory, the maximum rotor force is reduced significantly by 17.5% (from 4 N to 3.3 N).

Remark 1.

With the optimization and analysis framework developed, we review the choice of minimum snap trajectory. For the same four-waypoint scenario and segment times, we compare the optimal trajectory and the maximum rotor force required with the cost of different orders of trajectory derivatives to minimize. Figure 23 shows the geometry of the trajectories. As shown in Figure 24, the trajectory that minimizes the cost on the fourth derivative has the minimum force required. Therefore, we conclude that minimum snap is indeed the optimal choice for this approach of multi-copter aggressive trajectory optimization.

8. Conclusions

With the framework developed in this research, we can find the optimal polynomial trajectory and the corresponding segment time allocation for a given quadcopter model, a specified scenario, and a desired aggressiveness. We can also tell if the given scenario is beyond the capability of the given quadcopter by examining whether the solution exists. Furthermore, we can use the algorithm to evaluate the minimum required rotor thrust to achieve the scenario. Instead of tracking the maximum force, we can also use the algorithm to track the total time. That is, given a desired flight time, we can find the optimal trajectory that has the minimum required rotor thrust. Though the quadcopter model is used in this paper, the inverse dynamics analysis for control input (collective force and three-axis moment) applies to generic multi-copters. Therefore, the method developed in this research can be applied to multi-copters with minor modifications to the vehicle control allocation model. The geometric controller used in this research showed the capability of tracking the optimized aggressive trajectory. The tracking result verified the feasibility of the optimized trajectory and the credibility of the inverse dynamics analysis. In addition, the yaw trajectory optimization method is capable of improving the aggressive performance in the case of no requirement on heading angle.