For the proposed controller, a sequence of reference temporal waypoints is the control input. The information of each waypoint mainly consists of the three-dimensional (3D) position and yaw angle
.
Figure 2 provides the framework of the controller. The LMPC algorithm is employed in the position loop. The relevant model constraints are implemented on the state and input variables, thus planning the reference trajectory within a future time domain. Based on the differential flatness property of the quadrotor-UAV, the planned trajectory in the position loop can be mapped to the desired input for the attitude controller. In addition, an attitude control law is designed based on the
space. The nonlinear robust control law achieves accurate tracking of the desired attitude angles. Regarding the control allocation of the quadrotor-UAV, an inverse kinematics solution is obtained based on the optimization approach. Each module will be described in detail in the following sections.
3.1. Linear Model Predictive Control
The ability to predict over a wide time domain and consider the constraints of an actual model are the reasons why MPC is widely employed. In this paper, the LMPC is employed in the position controller, and the model constraints primarily incorporate considerations of motor speed limitations and motor response constraints. At each time step, the optimal control sequence for the entire prediction horizon is computed, but only the first control input is applied to the system. This process is iterated in a receding horizon manner, where optimization is performed repeatedly at each time step.
As shown in
Figure 2, the selected state variables are the position, velocities, accelerations, and jerks along three axes. These variables are denoted by their respective initials.
The output of the system is the 3D position, and the control input is the snap, which is the second derivative of acceleration. The reference input corresponds to the desired position. Among these variables, , . The reasons for optimizing for the snap will be thoroughly analyzed in the subsequent sections.
The corresponding difference equation can be formulated based on the aforementioned input and state variables.
where
,
, and
are defined as
in which
represents the discrete time interval, and
denotes the identity matrix. Given the prediction horizon
N, we define the state vector
, the reference input vector
, the output vector
, and the control input vector
over the predicted time horizon.
The relationship between them can be expressed as follows:
where
represents the state values at time step
k, and
represents the control input at the previous time step.
Considering the limitations of the control inputs and the tracking accuracy, a problem aiming to minimize tracking errors and control input variations can be formulated as
where
is the weight matrix related to tracking errors, and
is the weight matrix related to the control input. Both
and
are positive definite diagonal matrices.
The model constraints considered in the controller are the motor speed constraints and motor response constraints. Combining Equations (
1) and (
5), we obtain
The motor’s maximum speed constraint is imposed on the acceleration term of the state variables.
and
represent the maximum and minimum values of each component in the vector, respectively. The attitude of quadrotor-UAV is assumed to be unchanged within a single time step. This assumption may lead to motor speed saturation in some extreme cases. However, the optimization-based control allocation approach significantly reduces the impact of motor saturation.
By taking the second derivative of both sides of Equation (
5) and applying the motor response constraints to the snap term of the input variables, we have
where
and
represent the minimum and maximum motor responsiveness, respectively. Additional constraints, such as velocity limitations, can also be incorporated into the current algorithm. By substituting the variables in Equation (
15), an optimization problem is formulated as
It is worth mentioning that the aforementioned optimization problem can be simplified into a quadratic programming problem due to the absence of nonlinearity in the attitude loop. As a result, it can be solved quickly in real-time. Through solving this optimization equation, the desired position, velocity, acceleration, jerk, and snap can be effectively planned within the predicted time horizon. The first control sequence of the optimization results can then be applied to the controller in the next time step.
3.2. Nonlinear Attitude Controller Based on
As errors in attitude significantly undermine the position tracking accuracy, an attitude controller that can achieve high-precision tracking is very important to the overall controller. In this paper, a nonlinear and robust attitude control law is designed based on the space, which ensures the attitude control is continuous on the manifold. Furthermore, the proposed attitude controller is proved to be global asymptotically stable in the sense of Lyapunov.
The Euler equation for the attitude control is
where
represents external disturbances and
is the input moment. Equation (
22) can be rewritten as
where
represents the external perturbation term, satisfying
.
According to the Euler rotation theorem, any rotation matrix of
can be equivalently achieved by rotating around a specific axis by a certain angle. Here,
denotes the rotation matrix of the current body coordinate system with respect to the world coordinate system, while
signifies the desired rotation matrix of the body coordinate system with respect to the world coordinate system. Without causing any ambiguity, they are denoted as
and
for simplicity, respectively. The error of the rotation matrix can be obtained as
Following the definition of the error of the rotation matrix, the angle error can be calculated as
The angular velocity error is defined as the difference between the desired angular velocity
and the actual angular velocity
:
Hence, the attitude control law can be designed based on the angle error and the angular velocity error:
Referring to the exponential mapping and algebraic mapping between Lie groups and Lie algebras [
27], the calculation of the desired rotation axis based on the error of the rotation matrix can be carried out as follows:
where the operator ∧ denotes a conversion of a three-dimensional vector into a 3 × 3 skew-symmetric matrix, while the operator ∨ represents the inverse operation of the operator ∧.
The term in the control law corresponds to the sliding mode control term for angular velocity control, and it effectively mitigates the influence of external disturbances. Moreover, this term exhibits negligible numerical values, thus ensuring the stability of the controller without inducing any instability concerns.
To demonstrate the robustness of the control law, a Lyapunov function is conducted as follows:
By taking the derivative of the Lyapunov function, we have
The derivative of the angle term is expanded and the calculation is derived as
Substituting the above equation and the control law into Equation (
31) and scaling it appropriately, we have
where
represents the sum of the absolute values of each element in
. It is evident that the constructed Lyapunov function is positive definite, and its derivative is negative definite. Consequently, it can be concluded that the control law guarantees the global asymptotic stability of the system in the sense of Lyapunov. By adjusting the coefficients
and
, the convergence rate of the system can be tailored.
The attitude control law is designed based on the
space to avoid singularities. Concurrently, the control outputs are more efficient on the
manifold. Simulations presented in
Section 4 have demonstrated the exceptional performance of the proposed attitude control law, even in the presence of disturbances and observation uncertainties. The precise tracking of the attitude loop ensures its capability to fulfill the demands of the position loop. Although the attitude loop is excluded from the MPC optimization, its nonlinearity can still be compensated by the attitude controller, thus the overall control performance remains superior.
3.3. Differential Flatness
Given the above control scheme, the position and attitude controller can be designed independently. By leveraging the differential flatness properties of the quadrotor-UAV, a mapping from the position loop to the attitude loop is achieved. Differential flatness is referred to as the selection of appropriate variables from the state space and describing the entire state space using these variables and their derivatives. Similar to most articles in the literature, the following flat outputs are selected, namely the 3D position and the yaw angle:
The position is planned using the LMPC method within the position loop, and the yaw angle information is pre-defined. A direct mapping from the flat outputs to the desired attitude, angular rate, and angular acceleration is obtained based on these variables and their derivatives.
According to Mellinger and Kumar [
26], the desired attitude is calculated through acceleration. As mentioned in
Section 2, the coordinate system
is an intermediate coordinate system obtained by rotating the world coordinate system around the
z-axis with the yaw angle, and
indicates the unit vectors of the x-axis for this coordinate system.
By differentiating both sides of Equation (
5), the jerk planned in the position loop can be mapped into the desired angular velocity.
Also in paper [
26], however, the z-axis angular velocity is taken as zero for a constant yaw setpoint, which is actually nonzero. In this paper, the
z-axis angular velocity is calculated through Equation (
7). By taking the inverse of Equation (
7), we obtain
By solving Equation (
38), the
z-axis angular velocity can be determined based on the derivative value of the yaw angle.
In the attitude controller, both angular velocity and angular acceleration are required as feed-forward terms to enhance tracking accuracy in attitude control. Thus, the differential flatness property is extended to the angular acceleration to calculate its desired value. Similarly, by taking the second derivative of Equation (
5), we have
The equation for calculating the second derivative of thrust
T is
Expanding Equation (
39), the cross product of the angular acceleration with the
z-axis of the world coordinate system can be calculated. By projecting this quantity onto the
x-axis and
y-axis of the body coordinate system, the values of
and
can be derived as
Furthermore, we take the derivative of Equation (
38):
By solving Equation (
42), the angular acceleration of the
z-axis in the body coordinate system can be calculated. Based on differential flatness, the information in the position controller,
, can be mapped to the inputs of the attitude controller,
. Therefore, the second derivative of acceleration, i.e., the snap variable, must be considered in the position controller. Apart from better handling of the constraint of motor responsiveness, another crucial reason for this is to calculate the desired angular acceleration for the attitude controller.