Gravity compensation and optimal control of actuated multibody system dynamics

Abstract

This work investigates the gravity compensation topic, from a control perspective. The gravity could be levelled by a compensating mechanical system or in the control law, such as proportional derivative (PD) plus gravity, sliding mode control, or computed torque method. The gravity compensation term is missing in linear and nonlinear optimal control, in both continuous- and discrete-time domains. The equilibrium point of the control system is usually zero and this makes it impossible to perform regulation when the desired condition is not set at origin or in other cases, where the gravity vector is not zero at the equilibrium point. The system needs a steady-state input signal to compensate for the gravity in those conditions. The stability proof of the gravity compensated control law based on nonlinear optimal control and the corresponding deviation from optimality, with proof, are introduced in this work. The same concept exists in discrete-time control since it uses analog to digital conversion of the system and that includes the gravity vector of the system. The simulation results highlight two important cases, a robotic manipulator and a tilted-rotor hexacopter, as an application to the claimed theoretical statements.

1 INTRODUCTION

This paper declares that gravity compensation is necessary for continuous- and discrete-time optimal control. The common form of optimal control design considers the total system dynamics information for constructing the control law, Figure 1a. This generates a problem for regulation to desired conditions rather than equilibrium points or systems with non-zero gravity at the equilibrium point. We propose the gravity compensation in control law, Figure 1b, and prove that the stability of the system will be guaranteed for the total system using a common form of Lyapunov function, $V(\mathbf{x})={\mathbf{x}}^T\mathbf{K}(\mathbf{x})\mathbf{x}$ where $\mathbf{K}(\mathbf{x})$ is the optimal gain of the Riccati equation. Then we show the cost of this compensation and its deviation from optimality.

FIGURE 1

Block diagram of pure optimal control and the one with gravity compensation

Gravity compensation is an important subject in robotics and multibody system dynamics, addressed by case-dependent novel mechanical designs; or controller-based mechanisms in the literature [1, 2]. It is usually difficult to compensate for the exact force [3]. The compensating mechanical design increases the total weight of the system though the actuation efficiency balances the cost-effectiveness of this point of view. The compensation mechanism was applied to counterbalance the constant weight of the system; however, for practical use, a mechanism for weight variation is necessary since the moving mechanism changes the centre-of-mass (CoM) of different parts [4]. Nakamoto and Matsuhira presented a collaborative manipulator with a gravity compensation mechanism compatible with the variation of load [5]. The idea was to use joint driving motors as counterweights in a compact design. The use of the gravity compensating mechanisms is quite diverse such as machining with manipulators [6], cooperative dual-arm robot [7], deployable mesh antenna [8] etc. Yun et al. researched the compensation mechanism for a dual-arm cooperative manipulator which was subjected to variable weight due to the change of the waist of the robot, or in other words, the CoM of the system [7]. Ugartemendia et al. investigated gravity compensation in rehabilitation robotics [9]. It was concluded that it is a mandatory feature in mechatronics systems for rehabilitations to present a neutral motion feeling for the people subjected to treatment.

On the one hand, it is not always possible to compensate the gravity with a mechanical mechanism due to space limitation, increase in the weight of the total system, cost-effectiveness of the design etc. On the other hand, some systems need gravity compensation, that is, unmanned aerial vehicles (UAVs), helicopters, autonomous underwater vehicles (AUVs) etc. For the later systems, gravity cannot be compensated mechanically; it needs additional thrust, force, or input signal. So, the gravity compensation is studied here, as a control point of view in the optimal input law. The proportional derivative (PD) control is a well-established method for independent control of the actuators, as a non-model-based approach. The structure is also simple, and the constant control gains are responsible for the tuning of the system in question. Appling a PD for systems working under gravity effect, that is, a spatial manipulator, requires high gains for achieving the regulation or trajectory tracking [10]. The classical nonlinear method, PD plus gravity is a good solution to address this problem, avoiding high gain selection [11]. Sliding mode control (SMC) [12], and feedback linearization (FL) [13, 14], are nonlinear controllers with gravity compensation mechanisms, naturally embedded inside the control law; though particular methods were introduced to study this subject as well. De Luca and Panzieri presented an iterative scheme for compensation of the gravity for flexible link manipulators [15]. The learning part found a feedforward term (a signal for gravity compensation) for cases that the modelling was not precise and the load at the end-effector was unknown. The same approach was extended under the input saturation constraint [16]. Bembli et al. applied terminal sliding mode control on a two-DoF exoskeleton limb to gain robust characteristics in gravity compensation [17].

Optimal control usually considers zero equilibrium point and constructs Hamiltonian function and applies a cost function to reach optimal control law. The control law is unified and includes all the system dynamics. On the contrary to PD plus gravity, FL and SMC, in optimal control policies, the gravity is inside the gain and re-scaled (enhanced or weakened). Razmi and Macnab presented a neural network near-optimal control with compensated gravity [18]. The performance was shown by comparison with a linear quadratic regulator (LQR), in which better results were gained after 400 learning iterations.

Adding feedforward gravity compensator to state-dependent Riccati equation (SDRE) controller was reported [19-21]; however, the stability proof only concerned the pure optimal control part (without feedforward gravity). This made the stability discussion vulnerable to severe criticism and also deviated from the results from optimality. So, the works [19-21], were not considered gravity in the stability proof. Here in this work, the stability proof and optimality analysis are addressed with the gravity compensation mechanism. The same subject is open in discrete-time control laws which include the gravity in the system in analog to digital (A/D) conversion of the system matrices. More clarification and the main contribution of this work will be presented in Section 2.

Section 2 expresses the problem statements. Section 3 reports the SDRE controller with stability and optimality analysis. Section 4 presents the same topic for the discrete-time domain. Simulation results for continuous- and discrete-time domains were shown in Section 5. Concluding results are summarized in Section 6.

2 THE DEFINITION OF THE SYSTEM AND MOTIVATION

An actuated-dynamical-multibody-mechanical system (robotic manipulator, multirotor aerial system, underwater vehicle, aircraft etc.) possesses a general second-order differential equation, usually in the form of:

$$\mathbf{M}\left(\mathbf{q}\right)\ddot{\mathbf{q}}+\mathbf{c}\left(\mathbf{q},\dot{\mathbf{q}}\right)+\mathbf{d}\left(\mathbf{q},\dot{\mathbf{q}}\right)+\mathbf{g}\left(\mathbf{q}\right)=\mathbf{u},$$(1)

where $\mathbf{q}(t)\in {\mathbb{R}}^N$ is the generalized coordinate vector, $N$ represents the degree-of-freedom (DoF) of the system, $\mathbf{M}(\mathbf{q}(t)):{\mathbb{R}}^N\to {\mathbb{R}}^{N\times N}$ represents inertia, $\mathbf{c}(\mathbf{q}(t),\dot{\mathbf{q}}(t)):{\mathbb{R}}^N\times {\mathbb{R}}^N\to {\mathbb{R}}^N$ is the Coriolis and centrifugal vector, $\mathbf{d}(\mathbf{q}(t),\dot{\mathbf{q}}(t)):{\mathbb{R}}^N\times {\mathbb{R}}^N\to {\mathbb{R}}^N$ collects the terms related to friction, aerodynamics, drag etc., $\mathbf{g}(\mathbf{q}(t)):{\mathbb{R}}^N\to {\mathbb{R}}^N$ is gravity vector and $\mathbf{u}(t)\in {\mathbb{R}}^M$ includes the input force, moments, thrust, etc. in which $M=N$ for fully actuated cases and $M<N$ for under-actuated ones.

Selecting the state-vector as $\mathbf{x}(t)={[{\mathbf{q}}^T(t),{\dot{\mathbf{q}}}^T(t)]}^T$, the state-space reduced-order differential equation is obtained:

$$\begin{array}{ccc}\hfill \dot{\mathbf{x}}& =& \mathbf{f}\left(\mathbf{x}\right)+\mathbf{B}\left(\mathbf{x}\right)\mathbf{u}\hfill \\ & =& \left[\begin{array}{c}\dot{\mathbf{q}}\\ -{\mathbf{M}}^{-1}\left(\mathbf{q}\right)\left(\mathbf{c}\left(\mathbf{q},\dot{\mathbf{q}}\right)+\mathbf{d}\left(\mathbf{q},\dot{\mathbf{q}}\right)+\mathbf{g}\left(\mathbf{q}\right)\right)\end{array}\right]\hfill \\ & & +\left[\begin{array}{c}\mathbf{0}\\ {\mathbf{M}}^{-1}\left(\mathbf{q}\right)\end{array}\right]\mathbf{u}.\hfill \end{array}$$(2)

Remark 1.It is common to assume that the equilibrium point of the system (2) is considered zero $\mathbf{f}(\mathbf{0})=\mathbf{0}$. This condition shows that i.e. an elevator mechanism (without physical weight compensation) is at an equilibrium point when all the coordinates are zero that results $\mathbf{g}(\mathbf{0})=\mathbf{0}$, which is on the ground. For other cases such as a multirotor drone, there is always a constant gravity force applying on the system $\mathbf{g}$ which violates the equilibrium condition $\mathbf{f}(\mathbf{0})\ne \mathbf{0}$, see Figure 2. For a robotic manipulator, a configuration represents the equilibrium point in which the same condition holds, zero gravity, although the home position is usually a different configuration, see Figure 3. Note that gravity vector in robotic manipulators is not usually zero even at the equilibrium point, $\mathbf{g}(\mathbf{0})\ne \mathbf{0}$, due to existence of trigonometric functions such as cosine function, $\cos 0=1$. So, there exist systems that correspond to $\mathbf{f}(\mathbf{0})\ne \mathbf{0}$.

Remark 2.The regulation between two arbitrary points in the workspace, not necessarily regulation to the equilibrium point, results in $\mathbf{g}({\mathbf{q}}_{\mathrm{des}})\ne 0$, and consequently $\mathbf{f}({\mathbf{x}}_{\mathrm{des}})\ne 0$ where index “des” represents the desired condition.

The error of the system is defined by $\mathbf{e}(t)=\mathbf{x}(t)-{\mathbf{x}}_{\mathrm{des}}(t)$ and an arbitrary closed-loop negative-feedback control is considered:

$$\mathbf{u}=-\mathbf{X}\left(·\right)\mathbf{e}.$$(3)

where $\mathbf{X}(·)$ is linear/nonlinear stable control gain, for example LQR [22], PD, robust finite-time feedback linearization [23], H-infinity control [24] etc. To keep the system at rest, after the regulation to the desired condition, additional input is necessary since for a controllable system ${\lim }_{\mathbf{e}\to 0}\mathbf{u}(\mathbf{e})=\mathbf{0}$, where ${\lim }_{\mathbf{x}\to {\mathbf{x}}_{\mathrm{des}}}\mathbf{f}(\mathbf{x})\ne \mathbf{0}$.

FIGURE 2

The schematic view of a multirotor UAV in regulation

FIGURE 3

The schematic view of a robotic manipulator in regulation

So, a control law:

$$\mathbf{u}=-\mathbf{X}\left(·\right)\mathbf{e}+\mathbf{g}\left(\mathbf{q}\right),$$(4)

is required to compensate the gravity of the system after the regulation, to keep the system in a steady-state condition. PD plus gravity, computed torque method, feedback linearization, and sliding mode control have the compensating part in the design.

Case 1: In continuous-time optimal control, that is, LQR or the state-dependent Riccati equation, the design, and optimal control is based on the representation of the System (1) as a linearized system:

$$\dot{\mathbf{x}}=\mathbf{Ax}+\mathbf{Bu},$$(5)

for LQR or a state-dependent coefficient (SDC) representation:

$$\dot{\mathbf{x}}=\mathbf{A}\left(\mathbf{x}\right)\mathbf{x}+\mathbf{B}\left(\mathbf{x}\right)\mathbf{u},$$(6)

for SDRE. The corresponding optimal control law also covers the gravity inside the overall gain, similar to Equation (3).

Case 2: In discrete-time control, the $Z$-transform is done based on the linearized system (5) for the entire time of simulation or nonlinear SDC representation of the system (6) at each time step of simulation during the regulation. In the discrete case, the gravity vector is also inside the system matrices $\mathbf{A}$ or $\mathbf{A}(\mathbf{x}(t))$ and the control law possesses the digital version of input law (3).

In both cases 1 and 2, it is clear that the regulation to an arbitrary point, except to equilibrium point, results in zero error and therefore, zero control signal. So, how the system should compensate for the gravity to keep the system there. For the sake of brevity, the general nonlinear form $\mathbf{A}(\mathbf{x}(t))$ will be presented that includes the linear case $\mathbf{A}$ as well.

The main contribution of this work is to answer:

• - Could we exclude the gravity from the system and then compensate it in control law, similar to Equation (4), in continuous optimal control? How does it violate stability or optimality?
• - In discrete-time control, could we do the same gravity compensation? How should we manage it since the $Z$-transformed matrices are on the digital scale and the gravity vector is in the continuous-time domain?

In Section 3, the stability proof and optimality discussion for a control law in the type of (4) are provided and Section 4 presents the discrete version and its solution method.

3 THE CONTINUOUS-TIME SDRE CONTROLLER

The system in question is mechanical, Equation (2), for implementing nonlinear optimal control with gravity compensation and proof of stability and optimality. Excluding the gravity from the system, one could consider a nonlinear autonomous affine-in-control system as

$$\dot{\mathbf{x}}=\mathbf{A}\left(\mathbf{x}\right)\mathbf{x}+\mathbf{B}\left(\mathbf{x}\right)\mathbf{u}+{\mathbf{g}}_{\mathrm{r}}\left(\mathbf{x}\right),$$(7)

where $\mathbf{u}(t)\in {\mathbb{R}}^m$ is an input vector and $\mathbf{x}(t)\in {\mathbb{R}}^n$ is a state vector. ${\mathbf{g}}_{\mathrm{r}}(\mathbf{x}(t)):{\mathbb{R}}^n\to {\mathbb{R}}^n$ includes all terms related to gravity, $\mathbf{A}(\mathbf{x}(t)):{\mathbb{R}}^n\to {\mathbb{R}}^{n\times n}$ and $\mathbf{B}(\mathbf{x}(t)):{\mathbb{R}}^n\to {\mathbb{R}}^{n\times m}$ are state-dependent coefficient parameterization of a nonlinear system $\dot{\mathbf{x}}(t)=\mathbf{f}(\mathbf{x}(t))+\mathbf{g}(\mathbf{x}(t),\mathbf{u}(t))+{\mathbf{g}}_{\mathrm{r}}(\mathbf{x}(t))$ where $\mathbf{f}(\mathbf{x}(t)):{\mathbb{R}}^n\to {\mathbb{R}}^n$, $\mathbf{g}(\mathbf{x}(t),\mathbf{u}(t)):{\mathbb{R}}^n\times {\mathbb{R}}^m\to {\mathbb{R}}^n$, and ${\mathbf{g}}_{\mathrm{r}}(\mathbf{x}(t))$ are piecewise-continuous vector-valued functions that satisfy the Lipschitz condition:

$$\begin{array}{ccc}\hfill \mathbf{A}(\mathbf{x})& =& \left[\begin{array}{cc}\mathbf{0}& \mathbf{I}\\ \mathbf{0}& -{\mathbf{M}}^{-1}\left(\mathbf{q}\right)\left(\mathbf{C}\left(\mathbf{q},\dot{\mathbf{q}}\right)+\mathbf{D}\left(\mathbf{q},\dot{\mathbf{q}}\right)\right)\end{array}\right],\hfill \\ \hfill \mathbf{B}(\mathbf{x})& =& \left[\begin{array}{c}\mathbf{0}\\ {\mathbf{M}}^{-1}\left(\mathbf{q}\right)\end{array}\right],{\mathbf{g}}_{\mathrm{r}}\left(\mathbf{x}\right)=\left[\begin{array}{c}\mathbf{0}\\ -{\mathbf{M}}^{-1}\left(\mathbf{q}\right)\mathbf{g}\left(\mathbf{q}\right)\end{array}\right],\hfill \end{array}$$

in which $\mathbf{c}(\mathbf{q},\dot{\mathbf{q}})=\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}$ and $\mathbf{d}(\mathbf{q},\dot{\mathbf{q}})=\mathbf{D}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}$ are held. The Lipschitz condition expresses that the components inside SDC matrices have bounded first derivatives [25], which implies the dynamic components of the system are limited to linear growth.

The dimension of the system is $n=2N$ and $m=M$. The cost functional integral of continuous-time optimal control is1:

$$\begin{array}{ccc}\hfill J_{\mathrm{c}}\left(·\right)& =& \frac{1}{2}\left\{{\displaystyle {\mathbf{x}}^T\left(t_{\mathrm{f}}\right)\mathbf{F}\left(\mathbf{x}\left(t_{\mathrm{f}}\right)\right)\mathbf{x}\left(t_{\mathrm{f}}\right)+\underset{0}{\overset{t_{\mathrm{f}}}{\int }}\left[{\mathbf{x}}^T\mathbf{Q}\left(\mathbf{x}\right)\mathbf{x}\right.}\right.\hfill \\ & & \left.\left.+{\left(\mathbf{u}-\mathbf{g}\left(\mathbf{q}\right)\right)}^T\mathbf{R}\left(\mathbf{x}\right)\left(\mathbf{u}-\mathbf{g}\left(\mathbf{q}\right)\right)\right]\mathrm{d}t\phantom{{\displaystyle \underset{0}{\overset{t_{\mathrm{f}}}{\int }}}}\right\},\hfill \end{array}$$(8)

where $\mathbf{Q}(\mathbf{x}(t)):{\mathbb{R}}^n\to {\mathbb{R}}^{n\times n}$ is symmetric positive semi-definite weighting matrix for states in $t\in [0,t_{\mathrm{f}})$, $\mathbf{F}(\mathbf{x}(t_{\mathrm{f}}))\in {\mathbb{R}}^{n\times n}$ is symmetric positive semi-definite weighting matrix for states at $t=t_{\mathrm{f}}$, and $\mathbf{R}(\mathbf{x}(t)):{\mathbb{R}}^n\to {\mathbb{R}}^{m\times m}$ is symmetric positive definite weighting matrix for inputs in $t\in [0,t_{\mathrm{f}}]$.

Assumption 1.(Controllability condition, continuous-time domain). The pair of $\{\mathbf{A}(\mathbf{x}(t)),\mathbf{B}(\mathbf{x}(t))\}$ is a completely controllable parameterization of a nonlinear system (7) [26].

Assumption 2.(Observability condition, continuous-time domain). The pair of $\{\mathbf{A}(\mathbf{x}(t)),{\mathbf{Q}}^{1/2}(\mathbf{x}(t))\}$ is a completely observable parameterization of a nonlinear system (7) [26].

The Hamiltonian is shaped:

$$\begin{array}{ccc}\hfill \mathcal{H}\left(\mathbf{x},\mathit{\lambda },\mathbf{u}\right)& =& J_{\mathrm{c}}+{\mathit{\lambda }}^T\dot{\mathbf{x}}=\frac{1}{2}\left\{{\mathbf{u}}^T\mathbf{R}\left(\mathbf{x}\right)\mathbf{u}-2{\mathbf{g}}^T\left(\mathbf{q}\right)\mathbf{R}\left(\mathbf{x}\right)\mathbf{u}\right.\hfill \\ & & \left.+{\mathbf{g}}^T\left(\mathbf{q}\right)\mathbf{R}\left(\mathbf{x}\right)\mathbf{g}\left(\mathbf{q}\right)+{\mathbf{x}}^T\mathbf{Q}\left(\mathbf{x}\right)\mathbf{x}\right\}\hfill \\ & & +{\mathit{\lambda }}^T\left[\mathbf{A}\left(\mathbf{x}\right)\mathbf{x}+\mathbf{B}\left(\mathbf{x}\right)\mathbf{u}+{\mathbf{g}}_{\mathrm{r}}\left(\mathbf{x}\right)\right],\hfill \end{array}$$(9)

where $\mathit{\lambda }$ is the co-state vector. Considering $\mathit{\lambda }=\mathbf{K}(\mathbf{x})\mathbf{x}$ and applying the stationary condition $\frac{\partial \mathcal{H}(\mathbf{x},\mathit{\lambda },\mathbf{u})}{\partial \mathbf{u}}=\mathbf{0}$ on Hamiltonian (9) result in the control law

$$\begin{array}{ccc}& & \mathbf{R}\left(\mathbf{x}\right)\mathbf{u}-\mathbf{R}\left(\mathbf{x}\right)\mathbf{g}\left(\mathbf{q}\right)+{\mathbf{B}}^T\left(\mathbf{x}\right)\mathit{\lambda }=\mathbf{0}\Rightarrow \hfill \\ & & \mathbf{u}=-{\mathbf{R}}^{-1}\left(\mathbf{x}\right){\mathbf{B}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}+\mathbf{g}\left(\mathbf{q}\right),\hfill \end{array}$$(10)

in which $\mathbf{q}\in \mathbf{x}$ and $\mathbf{K}(\mathbf{x}):{\mathbb{R}}^n\to {\mathbb{R}}^{n\times n}$ is the symmetric positive definite control gain. The necessary condition for optimality $\frac{\partial \mathcal{H}(\mathbf{x},\mathit{\lambda },\mathbf{u})}{\partial \mathbf{x}}=-\dot{\mathit{\lambda }}=-\dot{\mathbf{K}}(\mathbf{x})\mathbf{x}-\mathbf{K}(\mathbf{x})\dot{\mathbf{x}}$ and substitution of the system (7) in that, also delivers:

$$\begin{array}{c}\hfill \begin{array}{c}{\displaystyle \frac{1}{2}}{\mathbf{x}}^T{\left[{\displaystyle \frac{\partial \mathbf{Q}}{\partial \mathbf{x}}}\right]}^T\mathbf{x}+{\displaystyle \frac{1}{2}}{\mathbf{u}}^T{\left[{\displaystyle \frac{\partial \mathbf{R}}{\partial \mathbf{x}}}\right]}^T\mathbf{u}+{\displaystyle \frac{1}{2}}{\left({\displaystyle \frac{\partial \mathbf{g}}{\partial \mathbf{x}}}\right)}^T\mathbf{Rg}+{\displaystyle \frac{1}{2}}{\mathbf{g}}^T{\displaystyle \frac{\partial \mathbf{R}}{\partial \mathbf{x}}}\mathbf{g}\hfill \\ +{\displaystyle \frac{1}{2}}{\left({\displaystyle \frac{\partial \mathbf{g}}{\partial \mathbf{x}}}\right)}^T\mathbf{Rg}-{\left({\displaystyle \frac{\partial \mathbf{g}}{\partial \mathbf{x}}}\right)}^T\mathbf{Ru}-{\mathbf{u}}^T{\displaystyle \frac{\partial \mathbf{R}}{\partial \mathbf{x}}}\mathbf{g}\hfill \\ +\left\{{\mathbf{x}}^T{\left[{\displaystyle \frac{\partial \mathbf{A}}{\partial \mathbf{x}}}\right]}^T+{\mathbf{u}}^T{\left[{\displaystyle \frac{\partial \mathbf{B}}{\partial \mathbf{x}}}\right]}^T+{\left[{\displaystyle \frac{\partial {\mathbf{g}}_{\mathrm{r}}}{\partial \mathbf{x}}}\right]}^T\right\}\hfill \\ \mathbf{Kx}+\mathbf{KBg}+\mathbf{K}{\mathbf{g}}_{\mathrm{r}}+\left(\dot{\mathbf{K}}+\mathbf{KA}+{\mathbf{A}}^T\mathbf{K}-\mathbf{KB}{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{K}+\mathbf{Q}\right)\hfill \\ \mathbf{x}=\mathbf{0},\hfill \end{array}\end{array}$$

which could be simplified considering ${\mathbf{g}}_{\mathrm{r}}(\mathbf{x})=-\mathbf{B}(\mathbf{x})\mathbf{g}(\mathbf{q})$ as:

$$\begin{array}{ccc}& & \frac{1}{2}{\mathbf{x}}^T{\left[\frac{\partial \mathbf{Q}}{\partial \mathbf{x}}\right]}^T\mathbf{x}+\frac{1}{2}{\mathbf{u}}^T{\left[\frac{\partial \mathbf{R}}{\partial \mathbf{x}}\right]}^T\mathbf{u}+\frac{1}{2}{\mathbf{g}}^T\frac{\partial \mathbf{R}}{\partial \mathbf{x}}\mathbf{g}+{\left(\frac{\partial \mathbf{g}}{\partial \mathbf{x}}\right)}^T\hfill \\ & & \mathbf{Rg}-{\left(\frac{\partial \mathbf{g}}{\partial \mathbf{x}}\right)}^T\mathbf{Ru}-{\mathbf{u}}^T\frac{\partial \mathbf{R}}{\partial \mathbf{x}}\mathbf{g}\hfill \\ & & +\left\{{\mathbf{x}}^T{\left[\frac{\partial \mathbf{A}}{\partial \mathbf{x}}\right]}^T+{\mathbf{u}}^T{\left[\frac{\partial \mathbf{B}}{\partial \mathbf{x}}\right]}^T+{\left[\frac{\partial {\mathbf{g}}_{\mathrm{r}}}{\partial \mathbf{x}}\right]}^T\right\}\hfill \\ & & \mathbf{Kx}+\left(\dot{\mathbf{K}}+\mathbf{KA}+{\mathbf{A}}^T\mathbf{K}-\mathbf{KB}{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{K}+\mathbf{Q}\right)\mathbf{x}=\mathbf{0}.\hfill \end{array}$$(11)

Substituting control law (10), and ${[\mathbf{g}(\mathbf{q})]}_{m\times 1}={[\mathbf{G}(\mathbf{x})]}_{m\times n}{[\mathbf{x}]}_{n\times 1}$, into Equation (11), we will find:

$$\begin{array}{ccc}& & {\displaystyle \frac{1}{2}}{\mathbf{x}}^T{\left[{\displaystyle \frac{\partial \mathbf{Q}}{\partial \mathbf{x}}}\right]}^T\mathbf{x}+{\displaystyle \frac{1}{2}}{\mathbf{x}}^T\mathbf{KB}{\mathbf{R}}^{-1}{\left[{\displaystyle \frac{\partial \mathbf{R}}{\partial \mathbf{x}}}\right]}^T{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{Kx}\hfill \\ & & -{\displaystyle \frac{1}{2}}{\mathbf{x}}^T\mathbf{KB}{\mathbf{R}}^{-1}{\left[{\displaystyle \frac{\partial \mathbf{R}}{\partial \mathbf{x}}}\right]}^T\hfill \\ & & \mathbf{Gx}-{\displaystyle \frac{1}{2}}{\mathbf{x}}^T{\mathbf{G}}^T{\left[{\displaystyle \frac{\partial \mathbf{R}}{\partial \mathbf{x}}}\right]}^T{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{Kx}+{\displaystyle \frac{1}{2}}{\mathbf{x}}^T{\mathbf{G}}^T{\left[{\displaystyle \frac{\partial \mathbf{R}}{\partial \mathbf{x}}}\right]}^T\hfill \\ & & \mathbf{Gx}+{\displaystyle \frac{1}{2}}{\mathbf{x}}^T{\mathbf{G}}^T{\displaystyle \frac{\partial \mathbf{R}}{\partial \mathbf{x}}}\mathbf{Gx}+{\left({\displaystyle \frac{\partial \mathbf{g}}{\partial \mathbf{x}}}\right)}^T\mathbf{RGx}+{\left({\displaystyle \frac{\partial \mathbf{g}}{\partial \mathbf{x}}}\right)}^T\hfill \\ & & \mathbf{R}{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{Kx}-{\left({\displaystyle \frac{\partial \mathbf{g}}{\partial \mathbf{x}}}\right)}^T\mathbf{RGx}+{\mathbf{x}}^T\mathbf{KB}{\mathbf{R}}^{-1}{\displaystyle \frac{\partial \mathbf{R}}{\partial \mathbf{x}}}\hfill \\ & & \mathbf{Gx}-{\mathbf{x}}^T{\mathbf{G}}^T{\displaystyle \frac{\partial \mathbf{R}}{\partial \mathbf{x}}}\mathbf{Gx}\hfill \\ & & +\left\{{\mathbf{x}}^T{\left[{\displaystyle \frac{\partial \mathbf{A}}{\partial \mathbf{x}}}\right]}^T-{\mathbf{x}}^T\mathbf{KB}{\mathbf{R}}^{-1}{\left[{\displaystyle \frac{\partial \mathbf{B}}{\partial \mathbf{x}}}\right]}^T\right.\hfill \\ & & \left.+{\mathbf{x}}^T{\mathbf{G}}^T{\left[{\displaystyle \frac{\partial \mathbf{B}}{\partial \mathbf{x}}}\right]}^T+{\left[{\displaystyle \frac{\partial {\mathbf{g}}_{\mathrm{r}}}{\partial \mathbf{x}}}\right]}^T\right\}\hfill \\ & & \mathbf{Kx}+\left(\dot{\mathbf{K}}+\mathbf{KA}+{\mathbf{A}}^T\mathbf{K}-\mathbf{KB}{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{K}+\mathbf{Q}\right)\mathbf{x}=\mathbf{0},\hfill \end{array}$$

which can be rewritten considering ${[{\displaystyle \frac{\partial \mathbf{R}}{\partial \mathbf{x}}}]}^T={\displaystyle \frac{\partial \mathbf{R}}{\partial \mathbf{x}}}$ and $\mathbf{R}{\mathbf{R}}^{-1}=\mathbf{I}$:

$$\begin{array}{ccc}& & \frac{1}{2}{\mathbf{x}}^T{\left[\frac{\partial \mathbf{Q}}{\partial \mathbf{x}}\right]}^T\mathbf{x}+\frac{1}{2}{\mathbf{x}}^T\mathbf{KB}{\mathbf{R}}^{-1}{\left[\frac{\partial \mathbf{R}}{\partial \mathbf{x}}\right]}^T{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{Kx}+{\left(\frac{\partial \mathbf{g}}{\partial \mathbf{x}}\right)}^T{\mathbf{B}}^T\mathbf{Kx}\hfill \\ & & +\left\{{\mathbf{x}}^T{\left[\frac{\partial \mathbf{A}}{\partial \mathbf{x}}\right]}^T-{\mathbf{x}}^T\mathbf{KB}{\mathbf{R}}^{-1}{\left[\frac{\partial \mathbf{B}}{\partial \mathbf{x}}\right]}^T+{\mathbf{x}}^T{\mathbf{G}}^T{\left[\frac{\partial \mathbf{B}}{\partial \mathbf{x}}\right]}^T+{\left[\frac{\partial {\mathbf{g}}_{\mathrm{r}}}{\partial \mathbf{x}}\right]}^T\right\}\hfill \\ & & \mathbf{Kx}+\left(\dot{\mathbf{K}}+\mathbf{KA}+{\mathbf{A}}^T\mathbf{K}-\mathbf{KB}{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{K}+\mathbf{Q}\right)\mathbf{x}=0.\hfill \end{array}$$(12)

Since there are matrix derivatives in Equation (12), it should be rewritten (using ${\mathbf{x}}^T{[\frac{\partial \mathbf{Q}}{\partial \mathbf{x}}]}^T={[\frac{\partial \mathbf{Q}}{\partial \mathbf{x}}\mathbf{x}]}^T$ [27]) to deliver a compatible version to the SDRE and avoid having tensor:

$$\begin{array}{ccc}& & \left(\frac{1}{2}{\left[\frac{\partial \mathbf{Q}}{\partial \mathbf{x}}\mathbf{x}\right]}^T+\frac{1}{2}{\left[\frac{\partial \mathbf{R}}{\partial \mathbf{x}}{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{Kx}\right]}^T{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{K}\right.\hfill \\ & & +{\left(\frac{\partial \mathbf{g}}{\partial \mathbf{x}}\right)}^T{\mathbf{B}}^T\mathbf{K}+\left\{{\left[\frac{\partial \mathbf{A}}{\partial \mathbf{x}}\mathbf{x}\right]}^T-{\left[\frac{\partial \mathbf{B}}{\partial \mathbf{x}}{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{Kx}\right]}^T\right.\hfill \\ & & \left.\left.+{\left[\frac{\partial \mathbf{B}}{\partial \mathbf{x}}\mathbf{Gx}\right]}^T+{\left[\frac{\partial {\mathbf{g}}_{\mathrm{r}}}{\partial \mathbf{x}}\right]}^T\right\}\mathbf{K}\right)\hfill \\ & & \mathbf{x}+\left(\dot{\mathbf{K}}+\mathbf{KA}+{\mathbf{A}}^T\mathbf{K}-\mathbf{KB}{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{K}+\mathbf{Q}\right)\mathbf{x}=\mathbf{0}.\hfill \end{array}$$(13)

Equation (13) is divided into two parts, first one, the “SDRE necessary condition for optimality” [28]:

$$\begin{array}{ccc}& & \left(\frac{1}{2}{\left[\frac{\partial \mathbf{Q}}{\partial \mathbf{x}}\mathbf{x}\right]}^T+\frac{1}{2}{\left[\frac{\partial \mathbf{R}}{\partial \mathbf{x}}{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{Kx}\right]}^T{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{K}+{\left(\frac{\partial \mathbf{g}}{\partial \mathbf{x}}\right)}^T{\mathbf{B}}^T\mathbf{K}\right.\hfill \\ & & +\left\{{\left[\frac{\partial \mathbf{A}}{\partial \mathbf{x}}\mathbf{x}\right]}^T-{\left[\frac{\partial \mathbf{B}}{\partial \mathbf{x}}{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{Kx}\right]}^T+{\left[\frac{\partial \mathbf{B}}{\partial \mathbf{x}}\mathbf{Gx}\right]}^T\right.\hfill \\ & & \left.\left.+{\left[\frac{\partial {\mathbf{g}}_{\mathrm{r}}}{\partial \mathbf{x}}\right]}^T\right\}\mathbf{K}\right)\mathbf{x}\le \mathbf{\Lambda }\left(\mathbf{x}\right)\mathbf{x},\hfill \end{array}$$(14)

where $\mathbf{\Lambda }(\mathbf{x})$ is an upper-bound matrix. The reason for representing (14) as a matrix is compatibility with the dimension of the SDDRE as:

$${\mathbf{\Lambda }}_{n\times n}\left(\mathbf{x}\right){\mathbf{x}}_{n\times 1}+{\left[\mathrm{SDDRE}\right]}_{n\times n}{\mathbf{x}}_{n\times 1}=\mathbf{0}.$$

The matrix derivatives could be found in Ref. [19]; it was stated that Equation (14) possesses a negligible value [29]. The second part of (13) shapes the state-dependent differential Riccati equation [19, 30]:

$$\begin{array}{ccc}& & -\dot{\mathbf{K}}\left(\mathbf{x}\right)=\mathbf{K}\left(\mathbf{x}\right)\mathbf{A}\left(\mathbf{x}\right)+{\mathbf{A}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\hfill \\ & & -\mathbf{K}\left(\mathbf{x}\right)\mathbf{B}\left(\mathbf{x}\right){\mathbf{R}}^{-1}\left(\mathbf{x}\right){\mathbf{B}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)+\mathbf{Q}\left(\mathbf{x}\right),\hfill \end{array}$$(15)

with $\dot{\mathbf{K}}(\mathbf{x}(t_{\mathrm{f}}))=\mathbf{F}(\mathbf{x}(t_{\mathrm{f}}))$. For a $N$-DoF system, $N(2N+1)$ equations must be solved to find $\mathbf{K}(\mathbf{x})$.

Remark 3.The general form of gravity compensated control (10) is:

$$\mathbf{u}=-{\mathbf{R}}^{-1}\left(\mathbf{x}\right){\mathbf{B}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\left[\mathbf{x}-{\mathbf{x}}_{\mathrm{des}}\right]+\mathbf{g}\left(\mathbf{q}\right),$$

where ${\lim }_{\mathbf{x}\to {\mathbf{x}}_{\mathrm{des}}}\mathbf{e}=\mathbf{0}$; similar to Equation (10) when $\mathbf{x}\to \mathbf{0}$; so, for the sake of simplicity in the derivation process, we consider control law (10) in the derivation. Considering Remark 1, for manipulators with $\mathbf{g}(\mathbf{0})\ne \mathbf{0}$, and cases in which the desired condition is shifted from zero to another position, the application of the proposed control law (10) preserves the generic view of the approach.

Remark 4.The gravity compensation was used in the literature, that is, Refs. [19, 21], however, the stability/optimality proof for the new form (10) was not presented. Theorem 1 and 2 are addressing the proofs.

Theorem 1.(Stability) The nonlinear system (7), considering Assumptions 1 and 2 and quadratic cost function (8), can be controlled by the gravity-compensated input law (10) in which $\mathbf{K}(\mathbf{x})$ is the symmetric positive definite solution to SDDRE (15) in finite time $t\in [0,t_{\mathrm{f}}]$, subjected to boundary condition $\dot{\mathbf{K}}(\mathbf{x}(t_{\mathrm{f}}))=\mathbf{F}(\mathbf{x}(t_{\mathrm{f}}))$.

Proof.A Lyapunov candidate is used, $V(\mathbf{x})={\mathbf{x}}^T\mathbf{K}(\mathbf{x})\mathbf{x}>0$, concerning $V(\mathbf{0})=0$, to prove the stability by computing the first derivative and following condition $\dot{V}(\mathbf{x})\le 0$:

$$\dot{V}\left(\mathbf{x}\right)={\dot{\mathbf{x}}}^T\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}+{\mathbf{x}}^T\dot{\mathbf{K}}\left(\mathbf{x}\right)\mathbf{x}+{\mathbf{x}}^T\mathbf{K}\left(\mathbf{x}\right)\dot{\mathbf{x}}.$$(16)

Substituting, $\dot{\mathbf{x}}$ from Equation (7) and $\dot{\mathbf{K}}(\mathbf{x})$ from Equation (15) into Equation (16), results in:

$$\begin{array}{c}\dot{V}\left(\mathbf{x}\right)={\mathbf{x}}^T{\mathbf{A}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}+{\mathbf{u}}^T{\mathbf{B}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}\hfill \\ +{\mathbf{g}}_{\mathrm{r}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}-{\mathbf{x}}^T\mathbf{K}\left(\mathbf{x}\right)\mathbf{A}\left(\mathbf{x}\right)\mathbf{x}-{\mathbf{x}}^T{\mathbf{A}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}\hfill \\ +{\mathbf{x}}^T\mathbf{K}\left(\mathbf{x}\right)\mathbf{B}\left(\mathbf{x}\right){\mathbf{R}}^{-1}\left(\mathbf{x}\right){\mathbf{B}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}-{\mathbf{x}}^T\mathbf{Q}\left(\mathbf{x}\right)\mathbf{x}\hfill \\ +{\mathbf{x}}^T\mathbf{K}\left(\mathbf{x}\right)\mathbf{A}\left(\mathbf{x}\right)\mathbf{x}+{\mathbf{x}}^T\mathbf{K}\left(\mathbf{x}\right)\mathbf{B}\left(\mathbf{x}\right)\mathbf{u}+{\mathbf{x}}^T\mathbf{K}\left(\mathbf{x}\right){\mathbf{g}}_{\mathrm{r}}\left(\mathbf{x}\right),\hfill \end{array}$$

which can be simplified as:

$$\begin{array}{ccc}\hfill \dot{V}\left(\mathbf{x}\right)& =& {\mathbf{u}}^T{\mathbf{B}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}+{\mathbf{g}}_{\mathrm{r}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}\hfill \\ & & +{\mathbf{x}}^T\mathbf{K}\left(\mathbf{x}\right)\mathbf{B}\left(\mathbf{x}\right){\mathbf{R}}^{-1}\left(\mathbf{x}\right){\mathbf{B}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}-{\mathbf{x}}^T\mathbf{Q}\left(\mathbf{x}\right)\mathbf{x}\hfill \\ & & +{\mathbf{x}}^T\mathbf{K}\left(\mathbf{x}\right)\mathbf{B}\left(\mathbf{x}\right)\mathbf{u}+{\mathbf{x}}^T\mathbf{K}\left(\mathbf{x}\right){\mathbf{g}}_{\mathrm{r}}\left(\mathbf{x}\right).\hfill \end{array}$$(17)

Replacing the control law (10) into Equation (17) provides:

$$\begin{array}{ccc}\hfill \dot{V}\left(\mathbf{x}\right)& =& {\mathbf{g}}^T\left(\mathbf{q}\right){\mathbf{B}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}+{\mathbf{g}}_{\mathrm{r}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}-{\mathbf{x}}^T\mathbf{Q}\left(\mathbf{x}\right)\mathbf{x}\hfill \\ & & -{\mathbf{x}}^T\mathbf{K}\left(\mathbf{x}\right)\mathbf{B}\left(\mathbf{x}\right){\mathbf{R}}^{-1}\left(\mathbf{x}\right){\mathbf{B}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}\hfill \\ & & +{\mathbf{x}}^T\mathbf{K}\left(\mathbf{x}\right)\mathbf{B}\left(\mathbf{x}\right)\mathbf{g}\left(\mathbf{q}\right)+{\mathbf{x}}^T\mathbf{K}\left(\mathbf{x}\right){\mathbf{g}}_{\mathrm{r}}\left(\mathbf{x}\right).\hfill \end{array}$$(18)

Considering that ${\mathbf{g}}_{\mathrm{r}}(\mathbf{x})=-\mathbf{B}(\mathbf{x})\mathbf{g}(\mathbf{q})$, Equation (18) is rewritten as:

$$\begin{array}{ccc}\hfill \dot{V}\left(\mathbf{x}\right)& =& {\mathbf{g}}^T\left(\mathbf{q}\right){\mathbf{B}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}-{\mathbf{g}}^T\left(\mathbf{q}\right){\mathbf{B}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}\hfill \\ & & -{\mathbf{x}}^T\mathbf{Q}\left(\mathbf{x}\right)\mathbf{x}-{\mathbf{x}}^T\mathbf{K}\left(\mathbf{x}\right)\mathbf{B}\left(\mathbf{x}\right){\mathbf{R}}^{-1}\left(\mathbf{x}\right){\mathbf{B}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}\hfill \\ & & +{\mathbf{x}}^T\mathbf{K}\left(\mathbf{x}\right)\mathbf{B}\left(\mathbf{x}\right)\mathbf{g}\left(\mathbf{q}\right)-{\mathbf{x}}^T\mathbf{K}\left(\mathbf{x}\right)\mathbf{B}\left(\mathbf{x}\right)\mathbf{g}\left(\mathbf{q}\right)\hfill \\ & =& -{\mathbf{x}}^T\left(\mathbf{K}\left(\mathbf{x}\right)\mathbf{B}\left(\mathbf{x}\right){\mathbf{R}}^{-1}\left(\mathbf{x}\right){\mathbf{B}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)+\mathbf{Q}\left(\mathbf{x}\right)\right)\mathbf{x}\le 0,\hfill \end{array}$$

which is negative definite and that concludes the proof.

Remark 5.The effect of gravity compensation on stability was checked in Theorem 1 and proved that the gravity compensated control law (10) guarantees stability similar to the case when the gravity is inside $\mathbf{A}(\mathbf{x})$ matrix. The new form answers the main intention of this work, to provide the signal after reaching the desired condition ${\mathbf{x}}_{\mathrm{des}}(t)$, which was not possible in previous versions. The use of gravity compensation was reported though the stability was not discussed with input law (10) substituted into the Lyapunov function.

The next subject is that how much we deviate from the optimality. The linearized system $\mathbf{A}=\frac{\partial \mathbf{f}(\mathbf{x})}{\partial \mathbf{x}}{|}_{\mathbf{x}=\mathbf{0}}$, $\mathbf{B}=\frac{\partial \mathbf{g}(\mathbf{x},\mathbf{u})}{\partial \mathbf{u}}{|}_{\mathbf{x}=\mathbf{0}}$, and constant weighting matrices $\mathbf{R}$ and $\mathbf{Q}$ provide the optimal control for the algebraic matrix Riccati equation at equilibrium point $\mathbf{x}=\mathbf{0}$ with the constant gain $\mathbf{K}$. The solution $\mathbf{K}(\mathbf{x})$ to the SDDRE tends to the optimal value near the equilibrium point $\mathbf{f}(\mathbf{0})$ wherein a sufficiently small neighborhood we are close to optimality [31]. Assumption 3 is an extended form which was previously stated in Ref. [31], without the excluded gravity vector ${\mathbf{g}}_{\mathrm{r}}(\mathbf{x})$. Theorem 2 is also an extended representation of asymptotically optimal results, presented for the systems without gravity compensation [28].

Assumption 3.The matrices $\mathbf{A}(\mathbf{x})$, $\mathbf{B}(\mathbf{x})$, $\mathbf{g}(\mathbf{q})$, $\mathbf{Q}(\mathbf{x})$, $\mathbf{R}(\mathbf{x})$ and $\mathbf{K}(\mathbf{x})$ and their gradients $\frac{\partial \mathbf{Q}(\mathbf{x})}{\partial \mathbf{x}}$, $\frac{\partial \mathbf{R}(\mathbf{x})}{\partial \mathbf{x}}$, $\frac{\partial \mathbf{A}(\mathbf{x})}{\partial \mathbf{x}}$, $\frac{\partial \mathbf{g}(\mathbf{q})}{\partial \mathbf{x}}$ are bounded in a neighborhood around the equilibrium point.

Theorem 2.(Optimality). Considering Assumption 3 and guaranteed gravity-compensated control law (10), for the system (7), limited to cases with $\mathbf{g}(\mathbf{0})=\mathbf{0}$ and constant $\mathbf{g}.$, the necessary condition for optimality $\frac{\partial \mathcal{H}(\mathbf{x},\mathit{\lambda },\mathbf{u})}{\partial \mathbf{x}}=-\dot{\mathit{\lambda }}$ is asymptotically satisfied near the equilibrium point when $\mathbf{x}\to 0$, with a quadratic rate with respect to $\mathbf{x}$.

Proof.To prove the optimality, the three conditions, obtained from Pontryagin's minimum principle, must be held. Considering Hamiltonian function (9), the first necessary condition for optimality results in:

$$\frac{\partial \mathcal{H}\left(\mathbf{x},\mathit{\lambda },\mathbf{u}\right)}{\partial \mathit{\lambda }}=\mathbf{A}\left(\mathbf{x}\right)\mathbf{x}+\mathbf{B}\left(\mathbf{x}\right)\mathbf{u}+{\mathbf{g}}_{\mathrm{r}}\left(\mathbf{x}\right)=\dot{\mathbf{x}}.$$

The stationary condition also provides:

$$\begin{array}{c}\hfill \frac{\partial \mathcal{H}\left(\mathbf{x},\mathit{\lambda },\mathbf{u}\right)}{\partial \mathbf{u}}=\mathbf{0}\Rightarrow \mathbf{R}\left(\mathbf{x}\right)\mathbf{u}-\mathbf{R}\left(\mathbf{x}\right)\mathbf{g}\left(\mathbf{q}\right)+{\mathbf{B}}^T\left(\mathbf{x}\right)\mathit{\lambda }=\mathbf{0}.\end{array}$$(19)

Substituting control law (10) and $\mathit{\lambda }=\mathbf{K}(\mathbf{x})\mathbf{x}$ into stationary condition, Equation (19), obtains:

$$\begin{array}{ccc}& & \mathbf{R}\left(\mathbf{x}\right)\left[-{\mathbf{R}}^{-1}\left(\mathbf{x}\right){\mathbf{B}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}+\mathbf{g}\left(\mathbf{q}\right)\right]\hfill \\ & & -\mathbf{R}\left(\mathbf{x}\right)\mathbf{g}\left(\mathbf{q}\right)+{\mathbf{B}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}=\mathbf{0},\hfill \end{array}$$

which verifies $\frac{\partial \mathcal{H}(\mathbf{x},\mathit{\lambda },\mathbf{u})}{\partial \mathbf{u}}=\mathbf{0}$. Based on Theorem 2, the second necessary condition for optimality must be asymptotically satisfied in a quadratic rate for $\mathbf{x}$. Substituting $\dot{\mathit{\lambda }}=\mathbf{K}(\mathbf{x})\dot{\mathbf{x}}+\dot{\mathbf{K}}(\mathbf{x})\mathbf{x}$ into $-\dot{\mathit{\lambda }}=\frac{\partial \mathcal{H}(\mathbf{x},\mathit{\lambda },\mathbf{u})}{\partial \mathbf{x}}$ results in Equation (11).

Since we solve the SDDRE (15) to obtain the symmetric positive definite gain $\mathbf{K}(\mathbf{x})$, $\frac{\partial \mathcal{H}(\mathbf{x},\mathit{\lambda },\mathbf{u})}{\partial \mathbf{x}}=-\dot{\mathit{\lambda }}$ turns into Equation (14).

Regarding $\mathbf{q}\in \mathbf{x}$, and using $\mathbf{g}(\mathbf{x})=\mathbf{G}(\mathbf{x})\mathbf{x}$, also substituting control law (10) into Equation (14), result in:

$$\begin{array}{ccc}& & \frac{1}{2}{\mathbf{x}}^T{\left[\frac{\partial \mathbf{Q}}{\partial \mathbf{x}}\right]}^T\mathbf{x}+\frac{1}{2}{\mathbf{x}}^T\mathbf{KB}{\mathbf{R}}^{-1}{\left[\frac{\partial \mathbf{R}}{\partial \mathbf{x}}\right]}^T{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{Kx}\hfill \\ & & +{\left(\frac{\partial \mathbf{g}}{\partial \mathbf{x}}\right)}^T{\mathbf{B}}^T\mathbf{Kx}+\left\{{\mathbf{x}}^T{\left[\frac{\partial \mathbf{A}}{\partial \mathbf{x}}\right]}^T-{\mathbf{x}}^T\mathbf{KB}{\mathbf{R}}^{-1}{\left[\frac{\partial \mathbf{B}}{\partial \mathbf{x}}\right]}^T\right.\hfill \\ & & \left.+{\mathbf{x}}^T{\mathbf{G}}^T{\left[\frac{\partial \mathbf{B}}{\partial \mathbf{x}}\right]}^T+{\left[\frac{\partial {\mathbf{g}}_{\mathrm{r}}}{\partial \mathbf{x}}\right]}^T\right\}\mathbf{Kx}\le \mathbf{\Lambda }\left(\mathbf{x}\right)\mathbf{x},\hfill \end{array}$$

which can be rewritten as a quadratic form:

$$\begin{array}{ccc}\hfill {\mathbf{x}}^T\mathbf{\Pi }\left(\mathbf{x}\right)\mathbf{x}& :=& {\mathbf{x}}^T\left(\frac{1}{2}{\left[\frac{\partial \mathbf{Q}}{\partial \mathbf{x}}\right]}^T+\frac{1}{2}\mathbf{KB}{\mathbf{R}}^{-1}{\left[\frac{\partial \mathbf{R}}{\partial \mathbf{x}}\right]}^T{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{K}\right.\hfill \\ & & +{\left({\mathbf{x}}^{†}\right)}^T{\left(\frac{\partial \mathbf{g}}{\partial \mathbf{x}}\right)}^T{\mathbf{B}}^T\mathbf{K}+\left\{{\left[\frac{\partial \mathbf{A}}{\partial \mathbf{x}}\right]}^T\right.\hfill \\ & & -\mathbf{KB}{\mathbf{R}}^{-1}{\left[\frac{\partial \mathbf{B}}{\partial \mathbf{x}}\right]}^T+{\mathbf{G}}^T{\left[\frac{\partial \mathbf{B}}{\partial \mathbf{x}}\right]}^T\hfill \\ & & \left.\left.+{\left({\mathbf{x}}^{†}\right)}^T{\left[\frac{\partial {\mathbf{g}}_{\mathrm{r}}}{\partial \mathbf{x}}\right]}^T\right\}\mathbf{K}\right)\mathbf{x},\hfill \end{array}$$(20)

where ${\mathbf{x}}^{†}$ is generalized-inverse of $\mathbf{x}$ and that plays the role of factorization from the matrices ${(\frac{\partial \mathbf{g}}{\partial \mathbf{x}})}^T$ and ${[\frac{\partial {\mathbf{g}}_{\mathrm{r}}}{\partial \mathbf{x}}]}^T$. Under the boundedness of matrices and their derivatives, in Assumption 3, and guaranteed stability in Theorem 1, there is a constant positive definite matrix such that [30]:

$$\max \left|{\mathbf{x}}^T\mathbf{\Pi }\left(\mathbf{x}\right)\mathbf{x}\right|\le {\mathbf{x}}^T\mathit{{\rm Y}}\mathbf{x},$$(21)

where $\mathit{{\rm Y}}$ is a positive constant matrix and that shows Equation (7) is asymptotically satisfied (bounded above) with a quadratic rate, and consequently $\frac{\partial \mathcal{H}(\mathbf{x},\mathit{\lambda },\mathbf{u})}{\partial \mathbf{x}}=-\dot{\mathit{\lambda }}$ holds near $\mathbf{x}\to \mathbf{0}$.

There are three possible cases for gravity:

1. $\mathbf{g}(\mathbf{0})=\mathbf{0}$, when $\mathbf{g}(\mathbf{q})$ includes direct position coordinate or $\sin (·)$ function which results in $\mathbf{G}(\mathbf{0})=\mathbf{0}$, since $\underset{q=0}{\lim }\frac{\sin (q)}{q}=0$. In this case, the optimality discussion is straightforward and is based on the presented proof.

2. $\mathbf{g}(\mathbf{0})\ne \mathbf{0}$, when $\mathbf{g}(\mathbf{q})$ includes i.e. $\cos (·)$ function (robotic manipulators). Since ${\lim }_{q=0}\frac{\cos (q)}{q}=\infty$, the gravity parameterized matrix tends to infinity $\mathbf{G}(\mathbf{0})=\infty$, for the system with $\mathbf{f}(\mathbf{0})\ne \mathbf{0}$, the necessary condition for optimality, Equation (20), is not satisfied and $\mathbf{\Pi }(\mathbf{0})\to \infty$, so we cannot find a upper-bounded matrix $\mathit{{\rm Y}}$ for the case of $\mathbf{f}(\mathbf{0})\ne \mathbf{0}$.

3. Optimality for a special case, flying objects. Theorem 2 stated that if $\mathbf{g}(\mathbf{0})\ne \mathbf{0}$, the necessary condition for optimality is not satisfied; however, there is a special case for systems with constant $\mathbf{g}.$ where we have the optimality. For that case, we consider constant gravity and put the derivatives equal to zero, then Equation (20) turns into:

   $$\begin{array}{ccc}\hfill {\mathbf{x}}^T\mathbf{\Pi }\left(\mathbf{x}\right)\mathbf{x}& :=& {\mathbf{x}}^T\left(\frac{1}{2}{\left[\frac{\partial \mathbf{Q}}{\partial \mathbf{x}}\right]}^T+\frac{1}{2}\mathbf{KB}{\mathbf{R}}^{-1}{\left[\frac{\partial \mathbf{R}}{\partial \mathbf{x}}\right]}^T{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{K}\right.\hfill \\ & & +\left\{{\left[\frac{\partial \mathbf{A}}{\partial \mathbf{x}}\right]}^T-\mathbf{KB}{\mathbf{R}}^{-1}{\left[\frac{\partial \mathbf{B}}{\partial \mathbf{x}}\right]}^T+{\mathbf{G}}^T{\left[\frac{\partial \mathbf{B}}{\partial \mathbf{x}}\right]}^T\right.\hfill \\ & & \left.\left.+{\left({\mathbf{x}}^{†}\right)}^T{\left[\frac{\partial {\mathbf{g}}_{\mathrm{r}}}{\partial \mathbf{x}}\right]}^T\right\}\mathbf{K}\right)\mathbf{x}.\hfill \end{array}$$(22)

Substituting ${\mathbf{g}}_{\mathrm{r}}$ into Equation (22) results in

$$\begin{array}{ccc}\hfill {\mathbf{x}}^T\mathbf{\Pi }\left(\mathbf{x}\right)\mathbf{x}& :=& {\mathbf{x}}^T\left(\frac{1}{2}{\left[\frac{\partial \mathbf{Q}}{\partial \mathbf{x}}\right]}^T+\frac{1}{2}\mathbf{KB}{\mathbf{R}}^{-1}{\left[\frac{\partial \mathbf{R}}{\partial \mathbf{x}}\right]}^T{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{K}\right.\hfill \\ & & \left.+\left\{{\left[\frac{\partial \mathbf{A}}{\partial \mathbf{x}}\right]}^T-\mathbf{KB}{\mathbf{R}}^{-1}{\left[\frac{\partial \mathbf{B}}{\partial \mathbf{x}}\right]}^T\right\}\mathbf{K}\right)\mathbf{x}.\hfill \end{array}$$(23)

in which ${({\mathbf{x}}^{†})}^T{[\frac{\partial {\mathbf{g}}_{\mathrm{r}}}{\partial \mathbf{x}}]}^T=-{({\mathbf{x}}^{†})}^T{\mathbf{x}}^T{\mathbf{G}}^T{[\frac{\partial \mathbf{B}}{\partial \mathbf{x}}]}^T$ cancels the gravity term and provides the upper-bounded condition (21) similar to case (a) $\mathbf{g}(\mathbf{0})=\mathbf{0}$. So, for the drones, i.e. one example presented in Section 5, the optimality condition is satisfied.

The conclusion is that optimality, based on Theorem 2, is satisfied for systems with $\mathbf{f}(\mathbf{0})=\mathbf{0}$ and systems with the constant gravity vector. However, stability is guaranteed for all systems $\mathbf{f}(\mathbf{0})=\mathbf{0}$ and $\mathbf{f}(\mathbf{0})\ne \mathbf{0}$ since the control law and Lyapunov function use $\mathbf{g}(\mathbf{q})$ without factorization. That concludes the optimality proof. The deviation from optimality for case (a) is defined by Equation (20) and for case (c) is computed by Equation (23). It is clear that $\underset{\mathbf{x}\to 0}{\lim }{\mathbf{x}}^T\mathbf{\Pi }(\mathbf{x})\mathbf{x}=0$.

For linear systems, the LQR problem, the optimality is completely satisfied since the gradient terms in Assumption 3 are zero. For the SDRE problem, considering the differential form of the Riccati equation, the deviation from optimality is found in the optimality proof, a negligible value; however, this value avoids naming the SDRE as an optimal controller. For that, the SDRE/SDDRE controller has been referred to as a suboptimal controller in the literature [21, 26, 31–33].

4 THE DISCRETE-TIME SDRE CONTROLLER

To present the discrete-time-control design, system (7) must be transformed into discrete-time domain by the $Z$-transform algorithm and a sampling time $T_{\mathrm{s}}(\mathrm{s})$. Simple systems could be transformed analytically nevertheless complex ones could be transformed at each sample time after updating with measured feedback. Similar to Section 3, putting gravity inside the system matrices $\mathbf{A}(\mathbf{x})$ and $\mathbf{B}(\mathbf{x})$ imposes a glitch in implementation on systems with gravity $\mathbf{g}(\mathbf{0})\ne \mathbf{0}$ or at arbitrary desired conditions, ${\mathbf{x}}_{\mathrm{des}}\ne \mathbf{0}$. So, the discretization is done on system (7).

Suppose there exists the corresponding discrete-time system:

$$\mathbf{x}\left[k+1\right]=\mathbf{A}\left(\mathbf{x}\left[k\right]\right)\mathbf{x}\left[k\right]+\mathbf{B}\left(\mathbf{x}\left[k\right]\right)\mathbf{u}\left[k\right],$$(24)

with appropriate dimensions such as (7). The initial condition, at $t=0$ or first step, is $\mathbf{x}(0)=\mathbf{x}[1]={\mathbf{x}}_0$.

Optimal control intends to minimize the cost function [34]:

$$\begin{array}{ccc}\hfill J_{\mathrm{d}}\left(·\right)& =& \frac{1}{2}\left\{{\mathbf{x}}^T\left[N_{\mathrm{d}}\right]\mathbf{F}\left(\mathbf{x}\left[N_{\mathrm{d}}\right]\right)\mathbf{x}\left[N_{\mathrm{d}}\right]+\min \sum _{k=N_{\mathrm{d}}-1}^1\right.\hfill \\ & & \left.\times \left({\left[\begin{array}{c}\mathbf{x}\left[k\right]\\ \mathbf{u}\left[k\right]\end{array}\right]}^T\left[\begin{array}{cc}\mathbf{Q}\left(\mathbf{x}\left[k\right]\right)& \mathbf{S}\left(\mathbf{x}\left[k\right]\right)\\ {\mathbf{S}}^T\left(\mathbf{x}\left[k\right]\right)& \mathbf{R}\left(\mathbf{x}\left[k\right]\right)\end{array}\right]\left[\begin{array}{c}\mathbf{x}\left[k\right]\\ \mathbf{u}\left[k\right]\end{array}\right]\right)\right\},\hfill \end{array}$$(25)

and finish a control task in a finite predefined time $t\in [0,t_{\mathrm{f}}]$, and $\left[\begin{array}{cc}\mathbf{Q}(\mathbf{x}[k])& \mathbf{S}(\mathbf{x}[k])\\ {\mathbf{S}}^T(\mathbf{x}[k])& \mathbf{R}(\mathbf{x}[k])\end{array}\right]>0$. Here in Equation (25), $N_{\mathrm{d}}$ is the last step of control loop, $\mathbf{Q}(\mathbf{x}[k]):{\mathbb{R}}^n\to {\mathbb{R}}^{n\times n}$ and $\mathbf{F}(\mathbf{x}[N_{\mathrm{d}}])\in {\mathbb{R}}^{n\times n}$ penalize the states in $k\in [1,N_{\mathrm{d}}-1]$ and at $N_{\mathrm{d}}$ with respect (both symmetric positive semi-definite); $\mathbf{R}(\mathbf{x}[k]):{\mathbb{R}}^n\to {\mathbb{R}}^{m\times m}$ weights the inputs and is positive definite, also symmetric.

Assumption 4.(Controllability Condition, discrete-time domain). The pair of $\{\mathbf{A}(\mathbf{x}[k]),\mathbf{B}(\mathbf{x}[k])\}$ is a completely controllable parameterization of the system (24).

Assumption 5.(Observability Condition, discrete-time domain). The pair of $\{\mathbf{A}(\mathbf{x}[k]),{\mathbf{Q}}^{1/2}(\mathbf{x}[k])\}$ is a completely observable parameterization of the system (24).

The minimization of Equation (25) starts from $N_{\mathrm{d}}$, the last step of control loop backward in time. Introducing a solution $\mathbf{K}(\mathbf{x}[k])$ in a Lyapunov function

$$V\left[k\right]={\mathbf{x}}^T\left[k\right]\mathbf{K}\left(\mathbf{x}\left[k\right]\right)\mathbf{x}\left[k\right],$$

and computing the next step, result in:

$$V\left[k+1\right]={\mathbf{x}}^T\left[k+1\right]\mathbf{K}\left(\mathbf{x}\left[k+1\right]\right)\mathbf{x}\left[k+1\right].$$(26)

System (24) is substituted in Equation (26)

$$\begin{array}{ccc}\hfill V\left[k+1\right]& =& {\left(\mathbf{B}\left(\mathbf{x}\left[k\right]\right)\mathbf{u}\left[k\right]+\mathbf{A}\left(\mathbf{x}\left[k\right]\right)\mathbf{x}\left[k\right]\right)}^T\hfill \\ & & \times \mathbf{K}\left(\mathbf{x}\left[k+1\right]\right)\left(\mathbf{A}\left(\mathbf{x}\left[k\right]\right)\mathbf{x}\left[k\right]+\mathbf{B}\left(\mathbf{x}\left[k\right]\right)\mathbf{u}\left[k\right]\right),\hfill \end{array}$$

and is rewritten as:

$$V[k+1]={\left[\begin{array}{c}\mathbf{x}\left[k\right]\hfill \\ \mathbf{u}\left[k\right]\hfill \end{array}\right]}^T\left[\begin{array}{cc}{\mathbf{A}}^T\left(\mathbf{x}\left[k\right]\right)\mathbf{K}\left(\mathbf{x}\left[k+1\right]\right)\mathbf{A}\left(\mathbf{x}\left[k\right]\right)\hfill & {\mathbf{A}}^T\left(\mathbf{x}\left[k\right]\right)\mathbf{K}\left(\mathbf{x}\left[k+1\right]\right)\mathbf{B}\left(\mathbf{x}\left[k\right]\right)\hfill \\ {\mathbf{B}}^T\left(\mathbf{x}\left[k\right]\right)\mathbf{K}\left(\mathbf{x}\left[k+1\right]\right)\mathbf{A}\left(\mathbf{x}\left[k\right]\right)\hfill & {\mathbf{B}}^T\left(\mathbf{x}\left[k\right]\right)\mathbf{K}\left(\mathbf{x}\left[k+1\right]\right)\mathbf{B}\left(\mathbf{x}\left[k\right]\right)\hfill \end{array}\right]\left[\begin{array}{c}\mathbf{x}\left[k\right]\\ \mathbf{u}\left[k\right]\end{array}\right].$$(27)

Expressing Hamilton-Jacobi-Bellman equation:

$$\begin{array}{ccc}\hfill V^{*}\left[k\right]& =& \underset{\mathbf{u}\left[k\right]}{\min }\left\{{\left[\begin{array}{c}\mathbf{x}\left[k\right]\\ \mathbf{u}\left[k\right]\end{array}\right]}^T\left[\begin{array}{cc}\mathbf{Q}\left(\mathbf{x}\left[k\right]\right)& \mathbf{S}\left(\mathbf{x}\left[k\right]\right)\\ {\mathbf{S}}^T\left(\mathbf{x}\left[k\right]\right)& \mathbf{R}\left(\mathbf{x}\left[k\right]\right)\end{array}\right]\left[\begin{array}{c}\mathbf{x}\left[k\right]\\ \mathbf{u}\left[k\right]\end{array}\right]\right.\hfill \\ & & +V^{*}\left[k+1\right]\},\hfill \end{array}$$

and using Equation (27), one could present:

$$\begin{array}{c}\hfill V^{*}\left[k\right]=\underset{\mathbf{u}\left[k\right]}{\min }\left\{\left[\begin{array}{cc}{\mathbf{x}}^T\left[k\right]& {\mathbf{u}}^T\left[k\right]\end{array}\right]\mathit{\chi }\left(\mathbf{x}\left[k\right]\right)\left[\begin{array}{c}\mathbf{x}\left[k\right]\\ \mathbf{u}\left[k\right]\end{array}\right]\right\},\end{array}$$(28)

where

$$\begin{array}{ccc}\hfill \mathit{\chi }\left(\mathbf{x}\left[k\right]\right)& =& \left[\begin{array}{cc}{\mathit{\chi }}_{11}\left(\mathbf{x}\left[k\right]\right)& {\mathit{\chi }}_{12}\left(\mathbf{x}\left[k\right]\right)\\ {\mathit{\chi }}_{12}^T\left(\mathbf{x}\left[k\right]\right)& {\mathit{\chi }}_{22}\left(\mathbf{x}\left[k\right]\right)\end{array}\right]\hfill \\ & =& \left[\begin{array}{cc}{\mathbf{A}}_k^T{\mathbf{K}}_{k+1}{\mathbf{A}}_k+{\mathbf{Q}}_k& {\mathbf{A}}_k^T{\mathbf{K}}_{k+1}{\mathbf{B}}_k+{\mathbf{S}}_k\\ {\mathbf{B}}_k^T{\mathbf{K}}_{k+1}{\mathbf{A}}_k+{\mathbf{S}}_k^T& {\mathbf{B}}_k^T{\mathbf{K}}_{k+1}{\mathbf{B}}_k+{\mathbf{R}}_k\end{array}\right].\hfill \end{array}$$

To satisfy $\mathit{\chi }(\mathbf{x}[k])>\mathbf{0}$, both ${\mathit{\chi }}_{11}(\mathbf{x}[k])$ and ${\mathit{\chi }}_{22}(\mathbf{x}[k])$ must be positive. Using Schur complement to force ${\mathit{\chi }}_{22}(\mathbf{x}[k])>\mathbf{0}$, one could find [35]:

$${\mathbf{x}}_k^T\left\{{\mathit{\chi }}_{11}-{\mathit{\chi }}_{12}{\mathit{\chi }}_{22}^{-1}{\mathit{\chi }}_{12}^T\right\}{\mathbf{x}}_k=V_k^{\ast }={\mathbf{x}}_k^T{\mathbf{K}}_k{\mathbf{x}}_k.$$(29)

Expanding Equation (28) provides:

$$\underset{\mathbf{u}\left[k\right]}{\min }\left({\mathbf{x}}_k^T{\mathit{\chi }}_{11}{\mathbf{x}}_k+{\mathbf{x}}_k^T{\mathit{\chi }}_{12}{\mathbf{u}}_k+{\mathbf{u}}_k^T{\mathit{\chi }}_{12}^T{\mathbf{x}}_k+{\mathbf{u}}_k^T{\mathit{\chi }}_{22}{\mathbf{u}}_k\right)=V_k^{\ast }.$$(30)

Adding and subtracting ${\mathbf{x}}_k^T\{{\mathit{\chi }}_{12}{\mathit{\chi }}_{22}^{-1}{\mathit{\chi }}_{12}^T\}{\mathbf{x}}_k$ to Equation (30) and mathematical manipulation result in a complete square form:

$$\begin{array}{ccc}& & \underset{\mathbf{u}\left[k\right]}{\min }\left({\mathbf{x}}_k^T{\mathit{\chi }}_{11}{\mathbf{x}}_k+{\left({\mathbf{u}}_k+{\mathit{\chi }}_{22}^{-1}{\mathit{\chi }}_{12}^T{\mathbf{x}}_k\right)}^T{\mathit{\chi }}_{22}\left({\mathbf{u}}_k+{\mathit{\chi }}_{22}^{-1}{\mathit{\chi }}_{12}^T{\mathbf{x}}_k\right)\right.\hfill \\ & & \left.-{\mathbf{x}}_k^T\left\{{\mathit{\chi }}_{12}{\mathit{\chi }}_{22}^{-1}{\mathit{\chi }}_{12}^T\right\}{\mathbf{x}}_k\phantom{\frac{1}{1}}\right)=V_k^{\ast }.\hfill \end{array}$$(31)

It is clear that holding Equation (31), similar to Equation (29), requires ${\mathbf{u}}_k+{\mathit{\chi }}_{22}^{-1}{\mathit{\chi }}_{12}^T{\mathbf{x}}_k=0$, that generates the control law:

$$\begin{array}{c}\hfill \mathbf{u}\left[k\right]=-{\left({\mathbf{B}}_k^T{\mathbf{K}}_{k+1}{\mathbf{B}}_k+{\mathbf{R}}_k\right)}^{-1}\left({\mathbf{B}}_{\mathrm{k}}^{\mathrm{T}}{\mathbf{K}}_{k+1}{\mathbf{A}}_k+{\mathbf{S}}_k^T\right){\mathbf{x}}_k,\end{array}$$(32)

where ${\mathbf{K}}_{k+1}$ is the positive definite symmetric solution to the discrete-time SDRE, resulting from Equation (29):

$$\begin{array}{ccc}\hfill \mathbf{K}\left(\mathbf{x}\left[k\right]\right)& =& {\mathbf{A}}_k^T{\mathbf{K}}_{k+1}{\mathbf{A}}_k+{\mathbf{Q}}_k-\left({\mathbf{A}}_k^T{\mathbf{K}}_{k+1}{\mathbf{B}}_k+{\mathbf{S}}_k\right)\hfill \\ & & \times {\left({\mathbf{B}}_k^T{\mathbf{K}}_{k+1}{\mathbf{B}}_k+{\mathbf{R}}_k\right)}^{-1}\left({\mathbf{B}}_k^T{\mathbf{K}}_{k+1}{\mathbf{A}}_k+{\mathbf{S}}_k^T\right).\hfill \end{array}$$(33)

Considering backward counting of $k$ in Equations (32) and (33), the current loop is $k$ and gain is updated by the ${\mathbf{K}}_{k+1}$ solution, in the loop $k+1$ where $k$ counts from $N_{\mathrm{d}}-1$ to $1$.

Remark 6.Control law (32) must be used in the backward solution to find the gain $\mathbf{K}(\mathbf{x}[k])$ over discrete steps, $k$, that covers the time span $t\in [0,t_{\mathrm{f}}]$. After completing the backward solution, the control law for the forward (main) control loop is Equation (32); however, $k$ counts forward from $1$ to $N_{\mathrm{d}}-1$.

Remark 7.The system Equation (24), for the backward solution, is changed to:

$$\mathbf{x}\left[k\right]=\mathbf{A}\left(\mathbf{x}\left[k+1\right]\right)\mathbf{x}\left[k+1\right]+\mathbf{B}\left(\mathbf{x}\left[k+1\right]\right)\mathbf{u}\left[k+1\right],$$

where $k$ counts backward from $N_{\mathrm{d}}-1$ to 1.

Implementation.It is necessary to use a digital to analog (D/A) converter to bring back the input Equation (32) to the scale of the continuous-time domain (a continuous step-like signal using, i.e. zero-order hold) and send the signal to the actuators [36]. Revisiting the main objective of this work, Equation (32) does not provide the necessary input for the system at the steady-state position or arbitrary desired condition, rather than $\mathbf{x}[N_{\mathrm{d}}]=0$.

For practical implementation, we add the gravity after D/A conversion to the control signal:

$${\mathbf{u}}_{\mathrm{gc}}\left(t\right)=\mathbf{u}\left(t\right)+\mathbf{g}\left(\mathbf{q}\left(t\right)\right),$$

where ${\mathbf{u}}_{\mathrm{gc}}(t)$ denotes gravity compensated control law and $\mathbf{u}(t)$ is the converted input of $\mathbf{u}[k]$ in Equation (32) using zero-order hold, etc. In that case, the real input signals to the actuators compensate for the gravity and control the system.

For computer simulations, the input Equation (32) is augmented by the gravity $\mathbf{g}(\mathbf{q}(k))$, and the system:

$$\begin{array}{ccc}\hfill \mathbf{x}\left[k+1\right]& =& \mathbf{A}\left(\mathbf{x}\left[k\right]\right)\mathbf{x}\left[k\right]+\mathbf{B}\left(\mathbf{x}\left[k\right]\right)\mathbf{u}\left[k\right]\hfill \\ & & -\mathbf{B}\left(\mathbf{x}\left[k\right]\right)\mathbf{g}\left(\mathbf{q}\left(k\right)\right),\hfill \end{array}$$

will cancel the gravity to complete the simulation.

5 SIMULATIONS

5.1 Continuous-time control, robotic manipulator

Consider a 3-DoF articulated-joint manipulator [37], see Figure 3 for a schematic view of the system. The second and third links of the manipulator are subjected to gravity. The home position of the robot requires a steady-state input for maintaining the robot in that position. The length of the links are $d_1=0.6$, $a_2=0.5$ and $a_3=0.4\mathrm{m}$ with respect. The mass of the links and load are $m_1=m_2=m_3=2$ and $m_p=0.25\text{kg}$; gravity constant is $g=9.81\mathrm{m}/{\mathrm{s}}^2$; the friction terms in the joints are presented by $\mathbf{D}=\text{diag}(0.05,0.05,0.05)\text{kgm}/\mathrm{s}$; the moment of inertia of the links are ${\mathbf{I}}_{\mathrm{c},1}=\text{diag}(6.25\times {10}^{-4},6.25\times {10}^{-4},0.06)\mathrm{kg}{\mathrm{m}}^2$, ${\mathbf{I}}_{\mathrm{c},2}=\text{diag}(0.0417,6.25\times {10}^{-4},6.25\times {10}^{-4})\mathrm{kg}{\mathrm{m}}^2$ and ${\mathbf{I}}_{\mathrm{c},3}=\text{diag}(0.0267,6.25\times {10}^{-4},6.25\times {10}^{-4})\mathrm{kg}{\mathrm{m}}^2$ with respect. The complete dynamic equation could be derived by general codes, presented in Appendix of Ref. [20].

The time of simulation was set $t_{\mathrm{f}}=10\mathrm{s}$, the start point and endpoint were also defined $A(0,-0.2,0.2)$ and $B(0.5,0.4,1.2)\mathrm{m}$ which results in initial and final conditions in joint space:

$$\mathbf{x}\left(0\right)={\left[-1.5708,0.2419,2.1235,0,0,0\right]}^T,$$

$$\mathbf{x}\left(t_{\mathrm{f}}\right)={\left[0.6747,-0.9529,0.4510,0,0,0\right]}^T.$$

The control parameters were chosen as $\mathbf{R}=0.1\times {\mathbf{I}}_{3\times 3}$, $\mathbf{Q}=\text{diag}(1,1,1,0.5,0.5,0.5)$ and $\mathbf{F}=2\mathbf{Q}$. An increase in $\mathbf{Q}$ increases the precision and reduction in $\mathbf{R}$ also enhances the amplitude of the input signal [21, 38]; an increase in $\mathbf{F}$ provides more accuracy in regulation near the final time. The configuration of the system and the trajectory are presented in Figure 4, successfully regulated to the desired condition, ${\mathbf{x}}_{\mathrm{des}}\ne \mathbf{0}$ and $\mathbf{g}({\mathbf{x}}_{\mathrm{des}})\ne \mathbf{0}$, with $0.0015\mathrm{mm}$ end-effector error. The input torque signals to the system are presented in Figure 5. The steady-state value of the first input was gained zero since it was not subjected to gravity based on the configuration though the other two links need gravity compensation. The norm of the input and gravity vector are illustrated in Figure 6. It is clear that without gravity compensation, the system would fail to stay at desired condition since it needs to hold the weight of the second and third links in addition to the weight of load at the end-effector.

FIGURE 4

Trajectory and configuration of the robot in regulation, continuous-time control

FIGURE 5

The input of the manipulator in regulation, continuous-time control

FIGURE 6

The norm of the input and gravity vector

Noticing Figure 6 brings up this question that optimizing the gain for gravity is necessary or not. Considering a system with precise modelling and lack of disturbance, one knows the value of gravity in a trajectory. The necessary input for compensation of the gravity should be the closest value. Enhanced gain provides excessive use of energy and smaller gains reduce the performance. The proposed gravity compensated control law balances the known gravity value and presents an optimal control policy.

5.2 Discrete-time control, hexacopter with tilted rotors

Multirotor unmanned aerial vehicles with fixed vertical rotors can generate a total thrust parallel to the axis of the rotor. The total thrust on the body frame can be projected to the global frame providing three components, though the system is under-actuated. Cascade design is a common way to solve the under-actuation problem for multirotor UAVs [39]. The method benefits from a virtual constraint to define the desired orientation of the system for translation control. Rotating the rotor axis around the connecting rod between the motor and the centre of mass (CoM) of the UAV generates thrust in all three axes $x_{\mathrm{c}},y_{\mathrm{c}},z_{\mathrm{c}}$, rather than only $z_{\mathrm{c}}$, for parallel rotors. This new configuration allowed full-actuation for multirotor UAVs [40]. The schematic view of a multirotor copter with fixed titled rotors is presented in Figure 7.

FIGURE 7

Schematic view of the multirotor UAV with fixed tilted rotors

The global fixed frame is named $[X,Y,Z]$ and the local moving body frame is named $[x_{\mathrm{c}},y_{\mathrm{c}},z_{\mathrm{c}}]$. The orientation variables are $[\varphi ,\theta ,\psi ]$ rotating around $[X,Y,Z]$ with respect, considering the right-hand rule. So, the generalized coordinates of the system are selected $\mathbf{q}(t)={[{\mathit{\xi }}_1^T(t),{\mathit{\xi }}_2^T(t)]}^T={[x_{\mathrm{c}}(t),y_{\mathrm{c}}(t),z_{\mathrm{c}}(t),\varphi (t),\theta (t),\psi (t)]}^T,(\mathrm{m},\mathrm{rad})$. The derivatives of them (on the local body frame) are ${\mathit{\upsilon }}_1(t)={[u(t),v(t),w(t)]}^T(\mathrm{m}/\mathrm{s})$ and ${\mathit{\upsilon }}_2(t)={[p(t),q(t),r(t)]}^T(\mathrm{rad}/\mathrm{s})$; the angular velocities are set on $[x_{\mathrm{c}}(t),y_{\mathrm{c}}(t),z_{\mathrm{c}}(t)]$ with respect. This definition provides the kinematics relations:

$${\dot{\mathit{\xi }}}_1={\mathbf{R}}_{ZYX}\left({\mathit{\xi }}_2\right){\mathit{\upsilon }}_1,$$(34)

$${\dot{\mathit{\xi }}}_2=\mathbf{T}\left({\mathit{\xi }}_2\right){\mathit{\upsilon }}_2.$$(35)

There is an input torque vector ${\mathit{\tau }}_{\mathrm{B}}(t)={[\begin{array}{ccc}{\tau }_{\varphi }(t)& {\tau }_{\theta }(t)& {\tau }_{\psi }(t)\end{array}]}^T(\mathrm{N}.\mathrm{m})$, acting against three Euler angles $[\varphi (t),\theta (t),\psi (t)]$ for the rotational dynamics. Fully-actuation design also provides three independent forces ${\mathbf{F}}_{\mathrm{B}}(t)={[\begin{array}{ccc}{f}_{\mathrm{x}}(t)& f_{\mathrm{y}}(t)& f_{\mathrm{z}}(t)\end{array}]}^T(\mathrm{N})$ acting against body frame $[x_{\mathrm{c}},y_{\mathrm{c}},z_{\mathrm{c}}]$. To project the local force into the global frame, the rotation matrix is used $\mathbf{F}({\mathit{\xi }}_2)={\mathbf{R}}_{ZYX}({\mathit{\xi }}_2){\mathbf{F}}_{\mathrm{B}}$. Dynamics equations of motion for both translation and orientation of the UAV are found using the Lagrange method:

$$m{\ddot{\mathit{\xi }}}_1+mg{\mathbf{e}}_3=\mathbf{F}({\mathit{\xi }}_2)={\mathbf{R}}_{ZYX}\left({\mathit{\xi }}_2\right){\mathbf{F}}_{\mathrm{B}},$$(36)

$$\mathbf{J}\left({\mathit{\xi }}_2\right){\ddot{\mathit{\xi }}}_2+\mathbf{C}\left({\mathit{\xi }}_2,{\dot{\mathit{\xi }}}_2\right){\dot{\mathit{\xi }}}_2={\mathit{\tau }}_{\mathrm{B}},$$(37)

where $g(\mathrm{m}/{\mathrm{s}}^2)$ is the gravity constant, $m(\mathrm{kg})$ represents total mass and ${\mathbf{e}}_3={[0,0,1]}^T$. $\mathbf{J}({\mathit{\xi }}_2)={\mathbf{W}}^T({\mathit{\xi }}_2)\mathbf{IW}({\mathit{\xi }}_2)$ is an inertial matrix (in global frame) in which $\mathbf{I}=\text{diag}(I_{xx},I_{yy},I_{zz})(\mathrm{kg}{\mathrm{m}}^2)$ is local inertia matrix; $\mathbf{T}({\mathit{\xi }}_2)={\mathbf{W}}^{-1}({\mathit{\xi }}_2)$ and $[\mathbf{C}({\mathit{\xi }}_2,{\dot{\mathit{\xi }}}_2){\dot{\mathit{\xi }}}_2]$ is a vector consists of Coriolis and centrifugal terms.

Assuming that the regulations and tracking cases will be performed with small changes in orientation of the UAV, hovering condition, the following assumptions are valid ${\dot{\mathit{\xi }}}_2(t)\simeq {\mathit{\upsilon }}_2(t)$ and ${\dot{\mathit{\xi }}}_1(t)\simeq {\mathit{\upsilon }}_1(t)$. So, the state-vector is selected as $\mathbf{x}(t)={[{\mathit{\xi }}_1^T(t),{\mathit{\xi }}_2^T(t),{\mathit{\upsilon }}_1^T(t),{\mathit{\upsilon }}_2^T(t)]}^T={[x_{\mathrm{c}},y_{\mathrm{c}},z_{\mathrm{c}},\varphi ,\theta ,\psi ,u,v,w,p,q,r]}^T.$ Considering the kinematic Equations (34)–(35) and the dynamics of the system Equations (36) and (37), the state-space equation is found:

$$\begin{array}{c}\hfill \dot{\mathbf{x}}\left(t\right)=\left[\begin{array}{c}{\dot{\mathit{\xi }}}_1\left(t\right)\\ {\dot{\mathit{\xi }}}_2\left(t\right)\\ {\dot{\mathit{\upsilon }}}_1\left(t\right)\\ {\dot{\mathit{\upsilon }}}_2\left(t\right)\end{array}\right]=\left[\begin{array}{c}{\mathbf{R}}_{\mathrm{ZYX}}\left({\mathit{\xi }}_2\right){\mathit{\upsilon }}_1\\ \mathbf{T}\left({\mathit{\xi }}_2\right){\mathit{\upsilon }}_2\\ 1/m{\mathbf{I}}_{3\times 3}\left[{\mathbf{R}}_{\mathrm{ZYX}}\left({\mathit{\xi }}_2\right){\mathbf{F}}_{\mathrm{B}}-\mathrm{mg}{\mathbf{e}}_3-{\mathbf{D}}_{\mathrm{f}}{\dot{\mathit{\xi }}}_1\right]\\ {\mathbf{J}}^{-1}\left({\mathit{\xi }}_2\right)\left[{\mathit{\tau }}_{\mathrm{B}}-\mathbf{C}\left({\mathit{\xi }}_2,{\dot{\mathit{\xi }}}_2\right){\dot{\mathit{\xi }}}_2\right]\end{array}\right],\end{array}$$(38)

where ${\dot{\mathit{\upsilon }}}_1(t)\simeq {\ddot{\mathit{\xi }}}_1(t)={\underbrace{{\dot{\mathbf{R}}}_{ZYX}({\mathit{\xi }}_2)}}_{\simeq 0}{\mathit{\upsilon }}_1+{\underbrace{{\mathbf{R}}_{ZYX}({\mathit{\xi }}_2)}}_{\simeq \mathbf{I}}{\dot{\mathit{\upsilon }}}_1$ and ${\dot{\mathit{\upsilon }}}_2(t)\simeq {\ddot{\mathit{\xi }}}_2(t)$ hold based on hovering assumption; and ${\mathbf{D}}_{\mathrm{f}}=\text{diag}(D_x,D_y,D_z)(\mathrm{kg}/\mathrm{s})$ denotes the aerodynamics effect [41].

Unified SDC: The state-dependent coefficient parameterization is structured based on Equation (24) on the system (38). The SDC design for under-actuated quadrotors was designed in two separate translation and orientation parts [39]; however, for the fully-actuated system in this research, a unified SDC design is proposed:

$$\mathbf{A}\left(\mathbf{x}\right)=\left[\begin{array}{cc}{\mathbf{0}}_{6\times 6}& {\mathbf{W}}_1\left(\mathbf{x}\right)\\ {\mathbf{0}}_{6\times 6}& {\mathbf{W}}_2\left(\mathbf{x}\right)\end{array}\right],\mathbf{B}\left(\mathbf{x}\right)=\left[\begin{array}{c}{\mathbf{0}}_{6\times 6}\\ {\mathbf{W}}_3\left(\mathbf{x}\right)\end{array}\right],$$

where

$${\mathbf{W}}_1\left(\mathbf{x}\right)=\left[\begin{array}{cc}{\mathbf{R}}_{ZYX}\left({\mathit{\xi }}_2\right)& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& \mathbf{T}\left({\mathit{\xi }}_2\right)\end{array}\right],$$(39)

$$\begin{array}{ccc}\hfill {\mathbf{W}}_2\left(\mathbf{x}\right)& =& \left[\begin{array}{cc}-{\displaystyle \frac{1}{m}}\times {\mathbf{D}}_{\mathrm{f}}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& -{\mathbf{J}}^{-1}\left({\mathit{\xi }}_2\right)\mathbf{C}\left({\mathit{\xi }}_2,{\dot{\mathit{\xi }}}_2\right)\end{array}\right],\hfill \\ \hfill {\mathbf{W}}_3\left(\mathbf{x}\right)& =& \left[\begin{array}{cc}{\displaystyle \frac{1}{m}}\times {\mathbf{R}}_{ZYX}\left({\mathit{\xi }}_2\right)& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{J}}^{-1}\left({\mathit{\xi }}_2\right)\end{array}\right].\hfill \end{array}$$

In hovering condition, Equation (39), is almost an identity matrix ${\mathbf{W}}_1(\mathbf{x})={\mathbf{I}}_{6\times 6}$ satisfying the proposed SDC structure.

The control law is designed based on generalized force and moments in global coordinates $\mathbf{u}(t)={[\begin{array}{cc}{\mathbf{F}}^T({\mathit{\xi }}_2)& {\mathit{\tau }}_{\mathrm{B}}^T\end{array}]}^T$. To define the local force vector for applying force limitation, the following relation is used ${\mathbf{F}}_{\mathrm{B}}={\mathbf{R}}_{ZYX}^T({\mathit{\xi }}_2)\mathbf{F}({\mathit{\xi }}_2)$.

Mixer matrix and actuator limit: The ultimate control inputs of the multirotor UAV are the voltage signals to the motors, attached to the propellers. The input voltage signals define the angular velocity of the rotors. The relation between the input force/moment vector and the angular velocities of the motor is indicated by a linear map, a mixer matrix. The mixer matrix for common quadrotors is straightforward and could be calculated easily. The mixer matrix for fixed tilted rotors, specifically with two rotations, is more complex.

To define the mixer matrix, the centre of mass of the UAV is named $O$ and the position of the $i$-th motor is named $O_i$. The distance between CoM and each motor is ${\overline{OO}}_i=L(\mathrm{m})$, illustrated in Figure 8. The first rotation is around $z_{\mathrm{c}}$ axis is defined by ${\lambda }_i=(i-1)\pi /3$ that results in the first rotation matrix. The second rotation is constant ${\alpha }_i={(-1)}^{i-1}\alpha$, around ${\overline{OO}}_i$ axis for all six motors; however, the sign is negative for 2, 4, and 6. The third rotation is constant $\beta$, around $y_2$ axis, for all motors toward the outside of the UAV:

$$\begin{array}{ccc}\hfill {\mathbf{R}}_1& =& \left[\begin{array}{ccc}\cos {\lambda }_i& -\sin {\lambda }_i& 0\\ \sin {\lambda }_i& \cos {\lambda }_i& 0\\ 0& 0& 1\end{array}\right],{\mathbf{R}}_2=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\alpha }_i& -\sin {\alpha }_i\\ 0& -\sin {\alpha }_i& \cos {\alpha }_i\end{array}\right],\hfill \\ \hfill {\mathbf{R}}_3& =& \left[\begin{array}{ccc}\cos \beta & 0& \sin \beta \\ 0& 1& 0\\ -\sin \beta & 0& \cos \beta \end{array}\right].\hfill \end{array}$$

FIGURE 8

Illustration of coordinates for tilted rotors; (a) first rotation around $z_{\mathrm{c}}$, (b) second rotation around ${\overline{OO}}_i$, (c) third rotation around $y_{2,i}$

The combination of the rotation matrices is:

$$\begin{array}{ccc}\hfill {\mathbf{R}}_{\mathrm{P},i}& =& {\mathbf{R}}_1{\mathbf{R}}_2{\mathbf{R}}_3\hfill \\ & =& \left[\begin{array}{ccc}{c}_{\beta }{c}_{{\lambda }_i}-s_{{\alpha }_i}{s}_{\beta }{s}_{{\lambda }_i}& -c_{{\alpha }_i}{s}_{{\lambda }_i}& c_{{\lambda }_i}{s}_{\beta }+s_{{\alpha }_i}{c}_{\beta }{s}_{{\lambda }_i}\\ c_{\beta }{s}_{{\lambda }_i}+s_{{\alpha }_i}{c}_{{\lambda }_i}{s}_{\beta }& c_{{\alpha }_i}{c}_{{\lambda }_i}& s_{\beta }{s}_{{\lambda }_i}-s_{{\alpha }_i}{c}_{\beta }{c}_{{\lambda }_i}\\ -c_{{\alpha }_i}{s}_{\beta }& s_{{\alpha }_i}& c_{{\alpha }_i}{c}_{\beta }\end{array}\right].\hfill \end{array}$$

The third column of ${\mathbf{R}}_{\mathrm{P},i}$ is named ${\mathbf{R}}_{\mathrm{P}3,i}$.

There are three main contributing components in the mixer matrix. The first is the thrust (force) parallel to $z_3$ axis, presented by $T_i(t)=k{\omega }_i^2(t)$ where $k(\mathrm{N}{\mathrm{s}}^2/\mathrm{ra}{\mathrm{d}}^2)$ is lift constant or thrust factor and ${\omega }_i(t)$ is the rotor's angular velocity of the $i$-th motor. Since the thrust only projects on $z_3$ axis, it collects the third column of the rotation matrix:

$$\begin{array}{ccc}& & \left[\begin{array}{ccc}{c}_{\beta }{c}_{{\lambda }_i}-s_{{\alpha }_i}{s}_{\beta }{s}_{{\lambda }_i}& -c_{{\alpha }_i}{s}_{{\lambda }_i}& c_{{\lambda }_i}{s}_{\beta }+s_{{\alpha }_i}{c}_{\beta }{s}_{{\lambda }_i}\\ c_{\beta }{s}_{{\lambda }_i}+s_{{\alpha }_i}{c}_{{\lambda }_i}{s}_{\beta }& c_{{\alpha }_i}{c}_{{\lambda }_i}& s_{\beta }{s}_{{\lambda }_i}-s_{{\alpha }_i}{c}_{\beta }{c}_{{\lambda }_i}\\ -c_{{\alpha }_i}{s}_{\beta }& s_{{\alpha }_i}& c_{{\alpha }_i}{c}_{\beta }\end{array}\right]\left[\begin{array}{c}0\\ 0\\ k{\omega }_i^2\left(t\right)\end{array}\right]\hfill \\ & & ={\mathbf{R}}_{\mathrm{P}3,i}k{\omega }_i^2\left(t\right).\hfill \end{array}$$

The second contributing term is the applied torque by each rotor, the cross product of thrust, and $L$:

$$L\left[\begin{array}{c}cos{\lambda }_i\\ sin{\lambda }_i\\ 0\end{array}\right]\times {\mathbf{R}}_{\mathrm{P}3,i}k{\omega }_i^2\left(t\right).$$

The third contributing term of the mixer is caused by the drag force of the propellers $k_{\psi }{\omega }_i^2(t)$ where $k_{\psi }$ is drag constant $(\mathrm{Nm}{\mathrm{s}}^2/\mathrm{ra}{\mathrm{d}}^2)$. The direction of the propellers is different for even and odd numbers that make the sign of drag force different. To assemble these three contributing components in the mixer matrix, one could shape [42]:

$${\left[\begin{array}{c}{\mathit{F}}_{\mathrm{B}}\\ {\mathit{\tau }}_{\mathrm{B}}\end{array}\right]}_{6\times 1}=\underset{{\mathbf{M}}_{\mathrm{X}}}{\underbrace{\underset{}{\left[\begin{array}{c}k\mathbf{A}\\ -------\\ k\mathbf{LG}+k_{\psi }\mathbf{H}\end{array}\right]}}}{\left[{\mathit{\omega }}^2\left(t\right)\right]}_{6\times 1},$$

where

$$\mathbf{A}={\left[{\mathbf{R}}_{\mathrm{P}3,1},{\mathbf{R}}_{\mathrm{P}3,2},{\mathbf{R}}_{\mathrm{P}3,3},{\mathbf{R}}_{\mathrm{P}3,4},{\mathbf{R}}_{\mathrm{P}3,5},{\mathbf{R}}_{\mathrm{P}3,6}\right]}_{3\times 6},$$(40)

$$\mathbf{G}={\left[{\mathbf{G}}_{\mathrm{M},1},{\mathbf{G}}_{\mathrm{M},2},{\mathbf{G}}_{\mathrm{M},3},{\mathbf{G}}_{\mathrm{M},4},{\mathbf{G}}_{\mathrm{M},5},{\mathbf{G}}_{\mathrm{M},6}\right]}_{3\times 6},$$(41)

$$\mathbf{H}=\left[{\mathbf{R}}_{\mathrm{P}3,1},-{\mathbf{R}}_{\mathrm{P}3,2},{\mathbf{R}}_{\mathrm{P}3,3},-{\mathbf{R}}_{\mathrm{P}3,4},{\mathbf{R}}_{\mathrm{P}3,5},-{\mathbf{R}}_{\mathrm{P}3,6}\right],$$(42)

in which:

$${\mathbf{G}}_{\mathrm{M},i}=\left[\begin{array}{c}\cos {\lambda }_i\\ \sin {\lambda }_i\\ 0\end{array}\right]\times {\mathbf{R}}_{\mathrm{P}3,i}.$$

Replacing the values into Equations (40)–(42), the components of the mixer are defined:

$$\begin{array}{ccc}& & \mathbf{A}=\left[\begin{array}{cccccc}{s}_{\beta }& 1/2s_{\beta }-\sqrt{3}/2c_{\beta }{s}_{\alpha }& \sqrt{3}/2c_{\beta }{s}_{\alpha }-1/2s_{\beta }& -s_{\beta }& -1/2s_{\beta }-\sqrt{3}/2c_{\beta }{s}_{\alpha }& 1/2s_{\beta }+\sqrt{3}/2c_{\beta }{s}_{\alpha }\\ -c_{\beta }{s}_{\alpha }& \sqrt{3}/2s_{\beta }+1/2c_{\beta }{s}_{\alpha }& \sqrt{3}/2s_{\beta }+1/2c_{\beta }{s}_{\alpha }& -c_{\beta }{s}_{\alpha }& -\sqrt{3}/2s_{\beta }+1/2c_{\beta }{s}_{\alpha }& -\sqrt{3}/2s_{\beta }+1/2c_{\beta }{s}_{\alpha }\\ c_{\alpha }{c}_{\beta }& c_{\alpha }{c}_{\beta }& c_{\alpha }{c}_{\beta }& c_{\alpha }{c}_{\beta }& c_{\alpha }{c}_{\beta }& c_{\alpha }{c}_{\beta }\end{array}\right],\hfill \\ & & \mathbf{G}=\left[\begin{array}{cccccc}0& \sqrt{3}/2c_{\alpha }{c}_{\beta }& \sqrt{3}/2c_{\alpha }{c}_{\beta }& 0& -\sqrt{3}/2c_{\alpha }{c}_{\beta }& -\sqrt{3}/2c_{\alpha }{c}_{\beta }\\ -c_{\alpha }{c}_{\beta }& -1/2c_{\alpha }{c}_{\beta }& 1/2c_{\alpha }{c}_{\beta }& c_{\alpha }{c}_{\beta }& 1/2c_{\alpha }{c}_{\beta }& -1/2c_{\alpha }{c}_{\beta }\\ -c_{\beta }{s}_{\alpha }& c_{\beta }{s}_{\alpha }& -c_{\beta }{s}_{\alpha }& c_{\beta }{s}_{\alpha }& -c_{\beta }{s}_{\alpha }& c_{\beta }{s}_{\alpha }\end{array}\right],\hfill \\ & & \mathbf{H}=\left[\begin{array}{cccccc}{s}_{\beta }& -1/2s_{\beta }+\sqrt{3}/2c_{\beta }{s}_{\alpha }& \sqrt{3}/2c_{\beta }{s}_{\alpha }-1/2s_{\beta }& s_{\beta }& -1/2s_{\beta }-\sqrt{3}/2c_{\beta }{s}_{\alpha }& -1/2s_{\beta }-\sqrt{3}/2c_{\beta }{s}_{\alpha }\\ -c_{\beta }{s}_{\alpha }& -\sqrt{3}/2s_{\beta }-1/2c_{\beta }{s}_{\alpha }& \sqrt{3}/2s_{\beta }+1/2c_{\beta }{s}_{\alpha }& c_{\beta }{s}_{\alpha }& -\sqrt{3}/2s_{\beta }+1/2c_{\beta }{s}_{\alpha }& \sqrt{3}/2s_{\beta }-1/2c_{\beta }{s}_{\alpha }\\ c_{\alpha }{c}_{\beta }& -c_{\alpha }{c}_{\beta }& c_{\alpha }{c}_{\beta }& -c_{\alpha }{c}_{\beta }& c_{\alpha }{c}_{\beta }& -c_{\alpha }{c}_{\beta }\end{array}\right].\hfill \end{array}$$

The angular velocities of the rotors are limited to ${\omega }_{i,\max }$, so the following constraint is limiting the performance of the system $\mathrm{if}{\omega }_i^2(t)>{\omega }_{i,\max }^2,{\omega }_i^2(t)={\omega }_{i,\max }^2,$ where

$$\mathit{\omega }\left(t\right)=\sqrt{{\mathbf{M}}_{\mathrm{X}}^{-1}\left[\begin{array}{c}{\mathbf{F}}_{\mathrm{B}}\\ {\mathit{\tau }}_{\mathrm{B}}\end{array}\right]}.$$(43)

The parameters of the hexacopter are presented in Table 1. The initial condition of the UAV was set on zero condition, the equilibrium point, and the desired position was defined as ${\mathbf{x}}_{\mathrm{des}}(t_{\mathrm{f}})={[-0.5,-0.75,0.8,0.1,0.05,-0.05,0_{1\times 6}]}^T$. The simulation time was 10 seconds and the weighting matrices were chosen as $\mathbf{R}={\mathbf{I}}_{6\times 6}$, $\mathbf{Q}=\text{diag}(0.1\times 1_{1\times 3}$, $0.01\times 1_{1\times 3}$, $0.2\times 1_{1\times 3}$, $0.001\times 1_{1\times 3})$, in which $1_{1\times 3}=[1,1,1]$ and $\mathbf{F}=10\times \mathbf{Q}$. Considering the number of discretisation $N_{\mathrm{d}}=49$, the error of the regulation was found $24\mathrm{mm}$. The configuration and trajectory of the UAV are depicted in Figure 9. The position states of the hexacopter are demonstrated in Figure 10. The translation forces of the system are presented in Figure 11. Since the desired orientation of the UAV is not zero, the translation forces are not zero in $X,Y$ directions. The mixer matrix defines the angular velocities of the system, Figure 12, and reduces to the steady-state values. The steady-state values of the rotors’ angular velocities are not similar since we have the desired pose.

TABLE 1. Parameters of the hexacopter

Par.	Values	Units	Definition
L	0.2	m	Dist. motor & CoM
R	0.125	m	radius of the propeller
k	$2.98\times {10}^{-5}$	$\mathrm{N}{\mathrm{s}}^2/\mathrm{ra}{\mathrm{d}}^2$	lift constant - thrust factor
$k_{\psi }$	$1.14\times {10}^{-6}$	$\mathrm{Nm}{\mathrm{s}}^2/\mathrm{ra}{\mathrm{d}}^2$	drag constant
$I_{xx}$	$7\times {10}^{-3}$	$\mathrm{kg}{\mathrm{m}}^2$	moment of inertia X axis
$I_{yy}$	$7.3\times {10}^{-3}$	$\mathrm{kg}{\mathrm{m}}^2$	moment of inertia Y axis
$I_{zz}$	$3.3\times {10}^{-3}$	$\mathrm{kg}{\mathrm{m}}^2$	moment of inertia Z axis
m	1.2	kg	mass of the quadrotor
${\mathbf{D}}_{\mathrm{f}}$	$[\begin{array}{ccc}0.25& 0& 0\\ 0& 0.25& 0\\ 0& 0& 0.25\end{array}]$	kg/s	drag matrix
${\omega }_{\max }$	282.83	rad/s	Max. Ang. Vel. of rotors
α	π/6	rad	rotation around ${\overline{OO}}_i$ axis
β	π/6	rad	rotation around $y_2$ axis

FIGURE 9

Configuration and trajectory of the hexacopter in regulation, discrete-time control

FIGURE 10

Translation states of the hexacopter, discrete-time control

FIGURE 11

Input force of the hexacopter, discrete-time control

FIGURE 12

Angular velocity of the rotors, discrete-time control

The angular velocities of the rotors, presented in Figure 12, are limited between two positive bounds, $[2700,2970]\mathrm{rpm}$. If the gravity is not compensated by the control law and one gets zero error and the end of the simulation, therefore, zero inputs, Equation (43) fails to deliver a solution. The reason is that all the control signals tend to zero (without gravity compensation) and the angular velocities of the rotors will be found around zero after a couple of oscillations. The second reason for failure without gravity compensation is that Equation (43) does not release negative values because of the square root. So, the steady-state shift in thrust input is necessary for the successful simulation. Optimal control without gravity compensation is not able to deliver a solution in a unified manner. In other words, the drone needs around $2830\mathrm{rpm}$ for hovering in the desired condition steadily.

6 CONCLUSION

Gravity compensation in the framework of the optimal control was investigated to check the effect of the modified control law plus gravity on the stability and the deviation from the optimality. The stability proof was guaranteed using the Lyapunov method and the optimality was analysed by the implementation of Pontryagin's Minimum Principle. The simulation results confirmed the proposed structure and showed successful results for the continuous- and discrete-time control domain. In brief, we presented a new optimal control that optimized the control signal around the steady-state value of the gravity vector (see the introduced cost function (8)) whether it is zero or not. The stability proof for this special presentation of the systems was presented for the first time and the deviation from optimality was shown.

For the systems with the state and input constraint, the stability and optimality proof, presented in this work, are held. The state constraint is applied by modification of the weighting matrix $\mathbf{Q}$, preserving the symmetric positive definite form and observability condition [43]. The input constraint is applied externally on the input signal [19]; hence, the derivation and stability/optimality analysis are valid. An example for the input constraint has been considered in simulation, Section 5, see Equation (43).

ACKNOWLEDGEMENT

This work is supported by the ERC as part of GRIFFIN advanced Grant 2017, Action 788247, and by the EU H2020 under AERIAL-CORE project contract 871479, and HYFLIERS project 779411.

CONFLICT OF INTEREST

The authors declare no conflict of interests.

Open Research

DATA AVAILABILITY STATEMENT

Data sharing not applicable – no new data generated.