Application of Log-Linear Dynamic Inversion Control to a Multi-rotor

Li-Yu Lin, James Goppert, and Inseok Hwang This work is supported in part by NASA University Leadership Initiative Project on Secure and Safe Assured Autonomy (S2A2) under Grant 80NSSC20M0161.L. Lin, J. Goppert, and I. Hwang are with the School of Aeronautics and Astronautics, Purdue University, West Lafayette, IN 47906, USA (e-mail: lin1191@purdue.edu; jgoppert@purdue.edu; ihwang@purdue.edu).

Abstract

This paper presents an approach that employs log-linearization in Lie group theory and the Newton-Euler equations to derive exact linear error dynamics for a multi-rotor model, and applies this model with a novel log-linear dynamic inversion controller to simplify the nonlinear distortion and enhance the robustness of the log-linearized system. In addition, we utilize Linear Matrix Inequalities (LMIs) to bound the tracking error for the log-linearization in the presence of bounded disturbance input and use the exponential map to compute the invariant set of the nonlinear system in the Lie group. We demonstrate the effectiveness of our method via an illustrative example of a multi-rotor system with a reference trajectory, and the result validates the safety guarantees of the tracking error in the presence of bounded disturbance.

Index Terms:

Log-Linearization, Dynamic Inversion, Invariant Set

I Introduction

Multi-rotors have played an important role in the Urban Air Mobility (UAM) system and have been deployed in various applications, including package delivery and aerial inspections. However, the safety of these vehicles is critical, and methods for safety verification must be developed to ensure public trust and acceptance of UAM. One promising approach is to utilize the invariant set for the dynamical system [1, 2, 3], which represents a set that the system will never leave if the initial conditions are within the set. This technique can be highly effective in ensuring the safety of multi-rotor systems, making them more reliable and trustworthy for a range of applications.

Lyapunov functions can be used to construct invariant sets for dynamical systems [2]. While nonlinear systems can be analyzed using hand-crafted Lyapunov functions or Lyapunov-based control design methods like backstepping control [4], these approaches are challenging to generalize and can be tedious to implement. For linear systems with bounded input, however, invariant sets can be constructed using backstepping control or Linear Matrix Inequality (LMI) techniques [5]. Although linear time-varying backstepping exists, it requires the disturbance to be a known function of time; however, the function of time for the disturbance is unknown in the multi-rotor system, the linear time-varying backstepping cannot be used in our case. Since the use of LMI is a more efficient method for finding the Lyapunov function and constructing the invariant set, we propose utilizing the linearization of the multi-rotor system to apply LMI techniques and construct the invariant set.

The general linearization technique cannot give an exact solution, and thus the invariant set obtained must be over-approximated to account for linearization error. To address this issue, we employ the log-linearization technique from Lie group theory [6, 7]. While previous studies [6, 8] have utilized the log-linear property for linearization, this technique only provides an approximation, not an exact solution. In our previous work [9], we proposed a generalized method for exact linearization of the nonlinear kinematics model in the Lie group to a corresponding linear system in the Lie algebra, using the derivative of the exponential map [10]. Since the result is an exact linearization, it allows us to analyze the nonlinear dynamics in a more efficient way using the linear dynamics in the Lie algebra, which is a vector space. A similar work in [11], derives the exact log-linearization for a specific Lie group, $SE_{2}(3)$ , our approach in [9] applies to all matrix Lie groups. Since the kinematic model alone is insufficient for describing the behavior of a multi-rotor system, we employ the $SE_{2}(3)$ Lie group [6, 12], which is a group of double homogeneous matrices, as part of our model.

This paper presents an efficient method to compute the invariant set for a nonlinear multi-rotor system with feedback control and subject to disturbance. Our method is based on log-linearity in the $SE_{2}(3)$ Lie group and LMI theory. To achieve this, we first embed the multi-rotor model in the $SE_{2}(3)$ Lie group and employ log-linearization to the left-invariant error dynamics. However, since the differential equation governing the evolution of the angular velocity derived from the Newton-Euler equations cannot be embedded in the $SE_{2}(3)$ Lie group, we treat this as a separate sub-system. Moreover, to further enhance the robustness and simplify the system, we follow our previous work [9] to design a log-linear dynamic inversion based control law for a multi-rotor with provable safety guarantees in the presence of bounded disturbance. Previous work in [13] shows a succinct synopsis of Lie group theory as it applies to state estimation and in addition, also the control methods we use in this paper.

The rest of this paper is organized as follows. In Section II, we embed the three-dimensional multi-rotor model in the $SE_{2}(3)$ Lie group. In Section III, we derive the error dynamics of log-linearization and the angular velocity, and design the dynamic inversion control for both systems. In Section IV, we illustrate the simulation results of the invariant set for our model with two different scenarios. Finally, in Section V, we present our concluding remarks and immediate future works.

II Vehicle Dynamics Embedded in $SE_{2}(3)$ Lie group

Consider the multi-rotor model evolving in a 3D space given as:

\frac{d}{dt}p=v,\;\frac{d}{dt}v=g+Ra,\;\frac{d}{dt}R=R\omega_{\times},\;\frac{d}{dt}\omega=\alpha

(1)

where $R\in\mathbb{R}^{3\times 3}$ and $p\in\mathbb{R}^{3\times 1}$ denote the vehicle attitude and position, $\omega\in\mathbb{R}^{3\times 1}$ and $v\in\mathbb{R}^{3\times 1}$ denote the angular and translational velocity, $g\in\mathbb{R}^{3\times 1}$ denotes the gravity, $a\in\mathbb{R}^{3\times 1}$ and $\alpha\in\mathbb{R}^{3\times 1}$ denote the angular and translational acceleration respectively. $\omega_{\times}$ represents the skew symmetric matrix of $\omega$ .

From Barrau’s paper[6], we can embed the attitude, position, and translational velocity to a group of double homogeneous matrices $SE_{2}(3)$ . The dynamics can be written in the mixed-invariant system [14, 9], as follow:

\dot{X}=X(C+[\nu_{v}]^{\wedge})+([\nu_{g}]^{\wedge}-C)X

(2)

where $X$ represents the state of the 3D vehicle in the $SE_{2}(3)$ Lie group. Note that $\nu_{v}=\begin{bmatrix}0&a&\omega\end{bmatrix}^{T}$ and $\nu_{g}=\begin{bmatrix}0&g&0\end{bmatrix}^{T}$ may be functions of time and represent the vehicle input, including feedback control and disturbance, and $g$ denotes gravity. $C=\begin{bmatrix}0_{3\times 3}&0_{3\times 1}&0_{3\times 1}\\ 0_{1\times 3}&0&1\\ 0_{1\times 3}&0&0\end{bmatrix}$ is a constant matrix, which embeds the kinematic equation $\frac{d}{dt}p=v$ .

The $SE_{2}(3)$ Lie group can be represented by a matrix of the form:

X=\begin{bmatrix}R&v&p\\ 0&1&0\\ 0&0&1\end{bmatrix}

(3)

The corresponding $\mathfrak{se}_{2}(3)$ Lie algebra can be represented by a matrix of the form:

[x]^{\wedge}=\begin{bmatrix}\omega_{\times}&a&v\\ 0&0&0\\ 0&0&0\end{bmatrix}

(4)

where $x=\begin{bmatrix}v&a&\omega\end{bmatrix}^{T}$ is the element in the Lie algebra, and $[\cdot]^{\wedge}$ indicates the wedge operator that maps the element from $\mathbb{R}^{9}$ to the $\mathfrak{se}_{2}(3)$ Lie algebra. Although the angular velocity, $\omega$ , cannot be embedded in the $SE_{2}(3)$ Lie group, we can consider the dynamics of the angular velocity as a separate sub-system in our application.

III Control Design for Error Dynamics

In this section, we first derive the log-linear property for the error dynamics in the $SE_{2}(3)$ Lie group. Based on the log-linear property, we then log-linearize the nonlinear system with feedback control, disturbance, and noise to a linear bounded input system. We then derive the error dynamics for the angular velocity, $\omega$ , with different frame considerations for the vehicle and reference systems.

III-A Log-Linearization of Error Dynamics in the $SE_{2}(3)$ Lie Group

Consider the dynamics of two systems, where $X_{b}\in SE_{2}(3)$ is the state of the vehicle system in the vehicle body frame, which is denoted by _b, and $\bar{X}_{r}\in SE_{2}(3)$ is the state of the reference trajectory in the reference frame, which is denoted by _r:

	$\displaystyle\dot{X_{b}}$	$\displaystyle=X_{b}(C+[\nu_{v_{b}}]^{\wedge})+([\nu_{g_{e}}]^{\wedge}-C)X_{b}$		(5)
	$\displaystyle\dot{\bar{X}}_{r}$	$\displaystyle=\bar{X}_{r}(C+[\bar{\nu}_{v_{r}}]^{\wedge})+([\nu_{g_{e}}]^{\wedge}-C)\bar{X}_{r}$		(6)

where $\nu_{v_{b}},\;\bar{\nu}_{v_{r}}\in\mathfrak{se}_{2}(3)$ are system inputs and may be functions of time. The relationship between the vehicle and reference input, $\nu_{v_{b}}$ and $\bar{\nu}_{v_{r}}$ , is $\nu_{v_{b}}=\bar{\nu}_{v_{r}}+\tilde{\nu}$ , where $\tilde{\nu}$ represents the difference between two inputs, which includes feedback control inputs, and disturbances. $\nu_{g_{e}}\in\mathfrak{se}_{2}(3)$ represents the gravity in the earth frame. Applying the chain rule in the left-invariant error, $\eta=X_{b}^{-1}\bar{X}_{r}$ , the dynamics of the left-invariant error can be written as:

\dot{\eta}=\eta(C+[\bar{\nu}_{v_{r}}]^{\wedge})-(C+[\nu_{v_{b}}]^{\wedge})\eta

(7)

Based on our previous work [9], we denote the left-invariant error $\eta$ by $\eta=\exp{[\zeta]^{\wedge}}$ . Applying the derivative of the exponential map [10], we can observe the error dynamics in the $\mathfrak{se}_{2}(3)$ Lie algebra, as follows:

	^∧	$\displaystyle=ad_{[\zeta]^{\wedge}}(C+[\bar{\nu}_{v_{r}}]^{\wedge})+[U_{\zeta}\tilde{\nu}]^{\wedge}$		(8)
	$\displaystyle U_{\zeta}$	$\displaystyle\equiv-\frac{ad_{[\zeta]^{\wedge}}\exp{(-ad_{[\zeta]^{\wedge}})}}{I-\exp{(-ad_{[\zeta]^{\wedge}})}}$		(8)

where $\zeta=\begin{bmatrix}\zeta_{p}&\zeta_{v}&\zeta_{R}\end{bmatrix}^{T}$ is the state of error dynamics in the $\mathfrak{se}_{2}(3)$ Lie algebra, and $U_{\zeta}$ is the matrix of nonlinear distortion of inputs in the $\mathfrak{se}_{2}(3)$ Lie algebra for the left-invariant error dynamics.

The adjoint function of $\zeta$ operating on $C$ can be written as a linear operator of $C^{\triangle}$ operating on $\zeta$ :

	$\displaystyle ad_{[\zeta]^{\wedge}}C$	$\displaystyle=-[C^{\triangle}\zeta]^{\wedge}$		(9)
	$\displaystyle C^{\triangle}$	$\displaystyle=\begin{bmatrix}0_{3\times 3}&I_{3}&0_{3\times 3}\\ 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}\\ 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}\end{bmatrix}$		(10)

and the adjoint linear operator of an $\mathfrak{se}_{2}(3)$ Lie algebra element, $\bar{\nu}_{v_{r}}$ , can be written as:

ad_{[\bar{\nu}_{v_{r}}]^{\wedge}}=\begin{bmatrix}\bar{\omega}_{\times}&0_{3\times 3}&\bar{v}_{\times}\\ 0_{3\times 3}&\bar{\omega}_{\times}&\bar{a}_{\times}\\ 0_{3\times 3}&0_{3\times 3}&\bar{\omega}_{\times}\end{bmatrix}

(11)

We apply the Lie vee operator, $[\cdot]^{\vee}$ , an inverse of the Lie wedge operator, to map the error dynamics from a Lie algebra matrix representation to elements of the vector space $\mathbb{R}^{9}$ , as follows:

\displaystyle\dot{\zeta}

\displaystyle=-(ad_{[\bar{\nu}_{v_{r}}]^{\wedge}}+C^{\triangle})\zeta+U_{\zeta}\tilde{\nu}

(12)

Here, we want to control the angular velocity and acceleration in the thrust, which correspond to the moment and force for the multi-rotor; therefore, the system can be written as:

	$\displaystyle\dot{\zeta}$	$\displaystyle=-(ad_{[\bar{\nu}_{v_{r}}]^{\wedge}}+C^{\triangle})\zeta+B_{u}U_{\zeta}u_{\zeta}+B_{d}U_{\zeta}d_{\nu}$		(13)
	$\displaystyle B_{u}$	$\displaystyle\equiv\begin{bmatrix}0_{5\times 4}\\ I_{4}\end{bmatrix}B_{d}\equiv\begin{bmatrix}0_{3\times 3}&0_{3\times 3}\\ I_{3}&0_{3\times 3}\\ 0_{3\times 3}&I_{3}\end{bmatrix}$		(13)

where $u_{\zeta}$ and $d_{\nu}$ represent the feedback control input and disturbance, respectively. The feedback control input $u_{\zeta}$ is designed by dynamic inversion, and thus the error dynamics in the Lie algebra can be written as:

	$\displaystyle\dot{\zeta}$	$\displaystyle=(-ad_{[\bar{\nu}_{v_{r}}]^{\wedge}}-C^{\triangle}+B_{u}K_{\zeta})\zeta+B_{d}U_{\zeta}d_{\nu}$		(14)
	$\displaystyle u_{\zeta}$	$\displaystyle=U_{\zeta}^{-1}K_{\zeta}\zeta$		(14)

where $K_{\zeta}$ is the control gain matrix for the state feedback controller, which can be designed using a Linear Quadratic Regulator (LQR) in the Lie algebra.

III-B Error Dynamics of Angular Velocity

Consider the dynamics of the angular velocity of two systems, where $\omega^{eb}_{b}\in\mathbb{R}^{3}$ represents the angular velocity of the vehicle in the vehicle body frame. The angular velocity is relative to the earth frame, which is denoted by ^eb. $\bar{\omega}^{er}_{r}$ represents the angular velocity of the reference trajectory in the reference frame.

We consider the error in the reference frame is $\omega^{br}_{r}={\bar{\omega}}^{er}_{r}-R^{rb}{\omega}^{eb}_{b}$ . The error dynamics can be written as:

\dot{\omega}^{br}_{r}=\dot{\bar{\omega}}^{er}_{r}-R^{rb}\dot{\omega}^{eb}_{b}+\omega^{br}_{r}\times\bar{\omega}^{er}_{r}

(15)

where $R^{rb}$ is the rotational matrix for rotating the vector from the vehicle body frame to the reference frame.

In our control design, we employ the dynamic inversion approach similar to [15]; therefore, our control law is:

	$\displaystyle\dot{\omega}^{eb}_{b}$	$\displaystyle={R^{rb}}^{-1}(\dot{\bar{\omega}}^{er}_{r}-K_{\omega}{\omega}^{br}_{r}-d_{\alpha})$		(16)
		$\displaystyle=R^{br}(\dot{\bar{\omega}}^{er}_{r}-K_{\omega}{\omega}^{br}_{r}-d_{\alpha})$		(16)

where $K_{\omega}$ is the control gain matrix and $d_{\alpha}$ is the disturbance. The actual control in the multi-rotor, which is the moment of the motor in the vehicle body frame, is given by:

M_{b}=\prescript{e}{}{}\frac{d}{dt}J_{b}\omega^{eb}_{b}=J\dot{\omega}^{eb}_{b}+\omega^{eb}_{b}\times J_{b}\omega^{eb}_{b}

(17)

where $J_{b}$ is the inertia matrix of the vehicle in the vehicle body frame. Substituting the designed control law to the actual control, the moment in the body frame can be written as:

M_{b}=J_{b}R^{br}(\dot{\bar{\omega}}^{er}_{r}+K_{\omega}\omega^{br}_{r}-d_{\alpha})+\omega^{eb}_{b}\times J_{b}\omega^{eb}_{b}

(18)

The error dynamics can be rewritten as:

\dot{\omega}^{br}_{r}=-{\bar{\omega}}^{er}_{r}\times{\omega}^{br}_{r}+K_{\omega}{\omega}^{br}_{r}+d_{\alpha}

(19)

which is a linear system with bounded input.

IV Application of Invariant Set

Since the log-linear error dynamical system in (14) is a linear system with bounded input, the invariant set can be found efficiently employing LMIs. To find the invariant set for the error bound of position, velocity, and rotation, we first apply LMIs to the error dynamics of angular velocity in (19) to find the error bound of the angular velocity, since the error dynamics in (14) depends on the error of $\omega$ . We then use the bound of the angular velocity as the bounded input for the error dynamics in (14) and apply LMIs to find the invariant set for the log-linear system. Once we have the invariant set for the log-linear system in the $\mathfrak{se}_{2}(3)$ Lie algebra, we can compute the invariant set for the actual nonlinear system, which is the system in the $SE_{2}(3)$ Lie group, by applying the exponential map. Our LMI guarantees boundedness of the invariant set of the given system subject to bounded disturbance, but neglects sensor noise and estimator dynamics.

We consider a multi-rotor flying along a reference trajectory, which is generated by polynomial trajectory planning [15]; however, our algorithm is not limited to this case. The multi-rotor model given in (1) can be embedded in the $SE_{2}(3)$ Lie group, (2). As we mentioned above, we first compute the feedback control with LQR and a Lyapunov function of the error dynamics of the angular velocity using an LMI in (19) with a reference angular velocity $[\omega_{1},\omega_{2},\omega_{3}]=[5,5,1]\;rad/s$ . We then use the Lyapunov function to compute the error bound of the angular velocity with a bounded external disturbance in $\alpha$ , $||d_{\alpha}||_{\infty}=0.1\;rad/s^{2}$ . The result of the maximum tracking error in the angular velocity is $||\omega||_{\infty}=0.03162\;rad/s$ .

Refer to caption — Figure 1: Projected 2D Invariant Sets Comparison with Dynamic Inversion, Left: Invariant Set in the Lie Algebra, Right: Invariant Sets in the Lie Group, Top: Small Disturbance, Bottom: Large Disturbance

The invariant set of the position error is computed with $[a_{x},a_{y},a_{z}]=[7.5,7.5,0]\;m/s^{2}$ . For external disturbance in translational acceleration, we consider two different magnitudes. We assume the magnitude of bounded disturbance inputs, $||a||_{\infty}$ , as $0.1\;m/s^{2}$ and $1\;m/s^{2}$ . We use the LQR method to find the feedback controller for the log-linearized system in the $\mathfrak{se}_{2}(3)$ Lie algebra, and find the Lyapunov function using the LMIs. The final result of the invariant sets in the Lie algebra and the Lie group is shown in Fig. 1 and Fig. 2, respectively. The figures show the invariant set with zero initial states in the $SE_{2}(3)$ Lie group, and the invariant set in the Lie group is constructed by applying the exponential map from the set in the Lie algebra. The Lyapunov function obtained in the LMI approach is an ellipsoid, which is the shape of the invariant sets for the log-linearized system in the $\mathfrak{se}_{2}(3)$ Lie algebra as shown on the left of Fig. 1.

Figure 3 shows a time history plot of the invariant set bound computed by our approach, along with simulated trajectories using our dynamic inversion controller and both sinusoidal and square wave disturbances randomly sample. The left figure in Fig. 3 shows the scenario under small disturbance, while the right figure shows the scenario under large wind disturbance. In both cases, the invariant sets calculated from the LMIs successfully bound all simulated trajectories. The precision of the invariant set shown in Fig. 3 can be improved by doing an iterative process between our log-linearized system in 14 and sub-system in 19. This is a useful result, since it can be applied to safety verification of multi-rotors with a more complicated reference trajectory.

V Conclusion

In this paper, we presented an efficient method to compute the invariant sets for the error dynamics of a multi-rotor with the proposed log-linear dynamics inversion controller with bounded disturbances. Our method was based on log-linearization in the Lie group, which allowed us to exactly log-linearize the nonlinear system. Additionally, we utilized the Linear Matrix Inequalities (LMIs) for a linear system with bounded inputs to bound the tracking error. Our simulation demonstrated the application of our algorithm to a multi-rotor with a reference trajectory and showed the usefulness of our algorithm.

As future work, we intend to consider the sensor noise and estimator dynamics in our multi-rotor model to have a more precise computation, and apply our approach with flow pipe creation with a more complicated reference trajectory in order to illustrate a more realistic Urban Air Mobility (UAM) scenario.

References

[1] F. Blanchini, “Set invariance in control,” Automatica, vol. 35, no. 11, pp. 1747–1767, 1999.
[2] H. K. Khalil and J. W. Grizzle, Nonlinear systems. Prentice hall Upper Saddle River, NJ, 2002, vol. 3.
[3] S. Coogan and M. Arcak, “A dissipativity approach to safety verification for interconnected systems,” IEEE Transactions on Automatic Control, vol. 60, no. 6, pp. 1722–1727, 2014.
[4] Z. Wang, X. Liu, K. Liu, S. Li, and H. Wang, “Backstepping-based lyapunov function construction using approximate dynamic programming and sum of square techniques,” IEEE transactions on cybernetics, vol. 47, no. 10, pp. 3393–3403, 2016.
[5] S. Boyd, L. El Ghaoui, E. Feron, and V. Balakrishnan, Linear matrix inequalities in system and control theory. SIAM, 1994.
[6] A. Barrau and S. Bonnabel, “The invariant extended kalman filter as a stable observer,” IEEE Transactions on Automatic Control, vol. 62, no. 4, pp. 1797–1812, 2016.
[7] T. D. Barfoot and P. T. Furgale, “Associating uncertainty with three-dimensional poses for use in estimation problems,” IEEE Transactions on Robotics, vol. 30, no. 3, pp. 679–693, 2014.
[8] S. Teng, W. Clark, A. Bloch, R. Vasudevan, and M. Ghaffari, “Lie algebraic cost function design for control on lie groups,” in 2022 IEEE 61st Conference on Decision and Control (CDC). IEEE, 2022, pp. 1867–1874.
[9] L.-Y. Lin, J. Goppert, and I. Hwang, “Correct-by-construction control design for mixed-invariant systems in lie groups,” arXiv preprint arXiv:2211.03310, 2022.
[10] W. Rossmann, Lie groups: an introduction through linear groups. Oxford University Press on Demand, 2006, vol. 5.
[11] X. Li, H. Jiang, X. Chen, H. Kong, and J. Wu, “Closed-form error propagation on $se\_$ { $n$ } $(3)$ group for invariant ekf with applications to vins,” IEEE Robotics and Automation Letters, vol. 7, no. 4, pp. 10 705–10 712, 2022.
[12] Y. Luo, M. Wang, and C. Guo, “The geometry and kinematics of the matrix lie group $se\_k(3)$ ,” arXiv preprint arXiv:2012.00950, 2020.
[13] J. Sola, J. Deray, and D. Atchuthan, “A micro lie theory for state estimation in robotics,” arXiv preprint arXiv:1812.01537, 2018.
[14] A. Khosravian, J. Trumpf, R. Mahony, and T. Hamel, “State estimation for invariant systems on lie groups with delayed output measurements,” Automatica, vol. 68, pp. 254–265, 2016.
[15] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in 2011 IEEE international conference on robotics and automation. IEEE, 2011, pp. 2520–2525.