1. Introduction
Nonlinear model predictive control (NMPC) of mechanical systems has been widely studied in the literature, including cases which evolve on Riemannian manifolds such as SO(3) and SE(3) [Reference Kalabic, Gupta, Di Cairano, Bloch and Kolmanovsky1–Reference Pereira, Leite and Raffo6]. Although hardware performance in terms of operational speed has reached a relative bounce nowadays, nonetheless the successive process of optimization required by this particular method is still very time-consuming. Therefore, alleviating the computation burden while preserving the accuracy of the results is a preliminary solution to the problem of real-time implementation. Linear matrix inequality (LMI) method is used to solve the optimizations related to the constrained model predictive control (MPC) control of a 3D pendulum on SO(3) in ref. [Reference Mansourinasab, Sojoodi and Moghadasi7]. It is shown that using LMI for solving MPC problem on manifold needs less control efforts than standard MPC. However, the difference is not noticeable in terms of computation burden and time. In ref. [Reference Pereira, Leite and Raffo6], direct shooting method has been used to solve the optimization problem of implementing NMPC to flight control of a quadrotor on SE(3). Direct methods are less accurate than indirect shooting methods and they may terminate with non-optimal solutions [Reference Sabeh, Shamsi and Novon8]. Generally, using fast solvers based on indirect shooting and Newton-like methods can increase the accuracy and speed of computations for optimal control problems. Another aspect to consider toward enhancing computational efficiency consists of deriving an appropriate discretized set of equations. One conventional approach is to apply the variational principle and then to discretize the resulting equations. In contrast, another method known as the discretized Hamilton’s principle consists of applying the variational principle to the discretized Lagrangian of the system. This leads to another set of equations for which the numerical integrators are termed variational integrators [Reference Hairer, Lubich and Wanner9]. Lie group variational integrators (LGVI) which exploit the main aspects of Lie group methods are much more accurate than classical methods of integration such as Runge–Kutta for the same step size [Reference Iserles, Munthe-Kaas, Nørsett and Zanna10], mainly because LGVI are symplectic and momentum-preserving systematically and show good energy behavior [Reference Leok and Shingel11, Reference Marsden and West12]. Furthermore, the discretization of equations and extraction of necessary conditions of optimality based on LGVI can preserve the geometric structure of the system which is of primary importance in tracking the evolution of the system configuration [Reference Leok13]. In terms of the optimal control problem on Lie group manifolds, the first-order discrete conditions for optimality are derived based on discrete equations of motion [Reference Lee, Leok and McClamroch14]. An exact and fast solver is then used in order to solve the corresponding two-point boundary value problem (TPBVP) based on sensitivity derivatives. This latter determines the sensitivity of the specified terminal boundary conditions to the change in the (unspecified) initial conditions of the Lagrange multipliers [Reference Lee, Leok and McClamroch14]. Forasmuch as the sensitivity derivatives are represented in the context of Lie algebra, singularities and other ambiguities that may manifest themselves in other contexts such as Euler angles and quaternions are avoided during the group actions; thus, sensitivity derivatives are able to provide a geometrically exact and efficient method for solving the TPBVPs [Reference Lee, McClamroch and Leok15]. The method expressed above is used in refs. [Reference Lee, Leok and McClamroch14, Reference Lee, McClamroch and Leok16] for the optimal control of rigid bodies evolving on Lie groups SO(3) and SE(3), respectively. The method developed in ref. [Reference Lee, Leok and McClamroch14] is adapted in ref. [Reference Gupta, Kalabic, Di Cairano, Bloch and Kolmanovsky17] for discrete-time MPC of mechanical systems evolving on SO(3), where using efficient fast solver is revealed to reduce the amount of optimization time in comparison with baseline solvers. However, since direct and indirect shooting methods are still considered as time-consuming procedures, it is necessary to find methods to reduce the computation time, especially when it comes to online implementation [Reference Sabeh, Shamsi and Novon8]. It is shown in ref. [Reference Bagherzadeh, Karimpour and Keshmiri18] that one can gain a considerable reduction in computational time in comparison with the procedure presented in refs. [Reference Lee, Leok and McClamroch14, Reference Gupta, Kalabic, Di Cairano, Bloch and Kolmanovsky17], only by applying some subtle simplifications on the TPBVP equations and employing some techniques for calculating the sensitivity derivatives. The procedure of substituting non-essential nonlinear equations by simplified linear equations in the trend of solving the optimal control problem is being considered in the present work for mechanical systems evolving on SO(3). Indeed, the terms which are considered non-essential for the solver process are nonetheless added to the system of equations during the last iteration, permitting to omit them in other iterations. These are some of the aspects that will be discussed in this work in the first place.
In addition, optimal control problems may face changes in their initial conditions or parameters, due to some reasons. In such cases, it is not necessary to perform the optimal control process from the scratch if an initial answer to the problem (corresponding to the first set of initial conditions) is already available. Using neighboring extremal (NE) methods can help to obtain the solution for the new set of initial conditions based on estimating the corresponding variations to the optimal control inputs [Reference Bryson and Ho19, Reference Pesch20]. This method has been considered on Riemannian manifolds such as SO(3) in a limited number of ref. [Reference Bloch, Gupta and Kolmanovsky21]. Juxtaposing the NE method with NMPC could enhance the efficiency of the control system significantly. However, this issue has not been widely addressed in related sources, unless only on Euclidean spaces [Reference Ghaemi, Sun and Kolmanovsky22, Reference Würth, Hannemann and Marquardt23]. In some cases, NE methods lead to a two-layer formulation [Reference Esche and Repke24, Reference Würth, Hannemann and Marquardt25]. In the present work, for a system evolving on a Lie group SO(3), the combination of NE method and NMPC is considered to estimate the optimal solution in the presence of some alteration in the initial conditions.
Therefore, two main goals are pursued in this article. First, it is shown that the potential of the present method to reduce the computation time is much more noticeable compared to what is offered in ref. [Reference Gupta, Kalabic, Di Cairano, Bloch and Kolmanovsky17], which is one of the great achievements of this work that provides a practical way toward real-time implementations of NMPC on SO(3). In this manner, discrete-time NMPC is applied to the equations of a rigid body evolving on the Lie group SO(3). Afterward, it is demonstrated that by reducing the nonlinearity of TPBVP equations and sensitivity derivatives, the NMPC-related calculations are lessened considerably compared with previous works. Secondly, NE method is applied to the governing equations in order to alter their optimal solution in case of change in the initial conditions, without having to perform the whole control planning procedure from the scratch. This method is highly valued whenever some solution is already available, or if some inner delays occur in NMPC systems, or in case the initial conditions for a time step changed unpredictably. In such cases, the computations required for obtaining the updated solution based on the basic initial conditions can be performed using NE method in a much shorter time. The NE method can apply to such systems lonely or in combination with the NMPC process. It is shown that the NE method is not able to preserve the accuracy of the responses for long periods. In contrast, updating the NE method with NMPC computed at some predetermined steps through the main TPBVP equations can increase the efficiency of the method while reducing the computation time. The methodologies employed for aforementioned objectives are implemented on an example and the simulation results confirm their validity.
Accordingly, this article is organized as follows: The equations of motion of a rigid body on SO(3) are presented in Section 2. NMPC formulation and the proposed simplifications of TPBVP and sensitivity derivative equations are expressed in Section 3. The applications of NMPC with and without applying the proposed simplifications of Section 3 are compared in terms of accuracy and time via an example in Section 4. Section 5 is dedicated to introduce the NE method and its reformulation for the NMPC control of mechanical systems on Lie group SO(3). The results of Section 5 are implemented to an example in Section 6. Finally, Section 7 is the conclusion part.
2. Equations of a rigid body on SO(3)
The Lagrangian form of continuous equations of motion of a rigid body in the absence of potential, on Lie group SO(3), is obtained using Hamilton’s principle as follows [Reference Lee, McClamroch and Leok26]:
\begin{align}J\dot{{\Omega} }+{\Omega} \times J{\Omega} =Bu\\[-25pt]\nonumber\end{align}
\begin{equation}\dot{R}=R\text{S}({\Omega} )\end{equation}
By applying the Legendre transformation to the above equations, one can express the equations of motion in the Hamiltonian form as [Reference Lee, Leok and McClamroch27]:
\begin{align}\dot{{\Pi} }+{\Omega} \times {\Pi} =Bu\\[-25pt]\nonumber\end{align}
\begin{equation}\dot{R}=R\text{S}({\Omega} )\end{equation}
where
${\Pi} =J{\Omega}$
which belongs to
$\mathbb{R}^{3}$
is the angular momentum of the body. The vector
${\Omega} \in \mathbb{R}^{3}$
is the angular velocity of the body expressed in the body-fixed frame. The matrix
$J\in \mathbb{R}^{3\times 3}$
is the standard moment of inertia matrix which is related to the nonstandard moment of inertia matrix,
$J_{d}$
, by
$J_{d}=\dfrac{1}{2}\text{tr}\!\left(J\right)\text{I}_{3}-J$
. The vector of control inputs is denoted by
$u\in \mathbb{R}^{m}$
, where
$B\in \mathbb{R}^{3\times m}$
is its coefficient matrix. The Lie group
$\text{SO}(3)=\left\{R\in \mathbb{R}^{3\times 3}|RR^{T}=I_{3},\det\! (R)=1\right\}$
is the special orthogonal group where its Lie algebra
$\mathfrak{so}(3)$
is the set of all skew-symmetric matrices on
$\mathbb{R}^{3\times 3}$
.
$\text{S}(.)\,:\, \mathbb{R}^{3}\rightarrow \mathfrak{so}(3)$
is an isomorphism, that is, a one-to-one invertible map between the vectors of
$\mathbb{R}^{3}$
and the matrices of
$\mathfrak{so}(3)$
and it is defined as follows: for all
$x,y\in \mathbb{R}^{3},\text{S}(x)y=x\times y$
[Reference Holm28, Reference Bullo and Lewis29]. In order to extract the discrete format of the rigid body equations of motion on SO(3), referred to as LGVI, the discrete version of the Hamilton’s principle is used instead of discretizing the continuous equations of motion [Reference Lee, Leok and McClamroch14]. These two methods are not equivalent generally [Reference Hussein, Leok, Sanyal and Bloch30]. The discrete equations of motion extracted using variational approach based on discrete Hamilton’s principle are systematically symplectic and momentum preserving and show good energy behavior in long-time integration [Reference Lee, McClamroch and Leok15]. The discrete equations of motion in the Lagrangian form extracted using the aforementioned method are presented below [Reference Lee, McClamroch and Leok26]:
\begin{align}\frac{1}{h}\!\left(F_{k+1}J_{d}-J_{d}F_{k}-J_{d}{F_{k+1}}^{T}+F^{T}_{k}J_{d}\right)=h\text{S}(Bu_{k})\\[-25pt]\nonumber\end{align}
\begin{equation}R_{k+1}=R_{k}F_{k}\end{equation}
where the matrix
$F_{k}$
is an auxiliary orthogonal matrix [Reference Lee, Leok and McClamroch14]. Using the discrete format of Legendre transformation, the discrete-time format of equations of motion, referred to as LGVI, is given by:
\begin{align}h\text{S}\!\left({\Pi}_{k}\right)=F_{k}J_{d}-J_{d}F^{T}_{k}\\[-28pt]\nonumber\end{align}
\begin{align}R_{k+1}=R_{k}F_{k}\\[-28pt]\nonumber\end{align}
\begin{equation}{\Pi}_{k+1}=F^{T}_{k}{\Pi}_{k}+hBu_{k}\end{equation}
where parameter
$h$
is the time step of the problem. Also, since
$F\in \text{SO}(3)$
and the group operation on SO(3) is matrix multiplication, Eq. (8) ensures that the computed rotation matrix will always remain on SO(3), that is, the geometric structure is preserved without the need to use any constraints or reprojections [Reference Leok13].
3. NMPC formulation
NMPC is a finite-horizon, iterative optimal control method. Therefore, the NMPC formulation for a system evolving on a Lie group is accomplished based on the principles of optimal control problem on Lie groups. The first step toward reaching that goal is to extract the LGVI [Reference Leok13]. In order to formulate the NMPC on SO(3), the same method as what is employed in ref. [Reference Bagherzadeh, Karimpour and Keshmiri18] is pursued. The following general discrete-time cost functional for a finite control horizon based on minimizing error and energy is considered:
\begin{equation}min\ \mathfrak{J}_{d}\!\left(u\right)=\Phi_{d}\!\left(R_{N},\text{S}\!\left({\Pi}_{N}\right)\right)+\sum_{k=0}^{N-1}\mathfrak{L}_{d}(R_{k},\text{S}({\Pi}_{k}),\text{S}(u_{k}))\end{equation}
which is subjected to Eqs. (7), (8), and (9), given
$R_{0}$
and
${\Pi}_{0}$
, and the inequality constraints of the form:
\begin{equation}C_{d}(R_{k}, \text{S}({\Pi}_{k}),\text{S}(\text{u}_{k}))\leq 0\end{equation}
where
$\Phi_{d}\,:\, \text{SO}(3)\times \mathfrak{so}(3)\rightarrow \mathbb{R}_{\geq 0}$
,
$\mathfrak{L}_{d}\,:\, \text{SO}(3)\times \mathfrak{so}(3)\times \mathfrak{so}(3)\rightarrow \mathbb{R}_{+}$
and
$C_{d}\,:\, \text{SO}(3)\times \mathfrak{so}(3)\times$
$\mathfrak{so}(3)\rightarrow \mathbb{R}$
are twice differentiable functions with respect to their arguments. The augmented cost functional to implicate the constraints is written as follows:
\begin{align}\overline{\mathfrak{J}}_{d}\!\left(u\right) =\, & \Phi_{d}\!\left(R_{N},\text{S}\!\left({\Pi}_{N}\right)\right)+\sum_{k=0}^{N-1}\!\left[\mathfrak{L}_{d}\!\left(R_{k},\text{S}\!\left({\Pi}_{k}\right),\text{S}\!\left(u_{k}\right)\right)+\ll \!\left(\lambda_{k}^{1}\right)^{\diamond },log\!\left(F_{k}-{R_{k}}^{T}R_{k+1}\right)\gg\right.\nonumber\\& \left. +\ll \text{S}\!\left(\lambda_{k}^{2}\right),\left(F^{T}_{k}{\Pi}_{k}+hBu_{k}-{\Pi}_{k+1}\right)^{\diamond }\gg \right]+\mu ^{T}\psi_{d}(C_{d}(R_{k},\text{S}({\Pi}_{k}),\text{S}(\text{u}_{k}))\end{align}
where
$\lambda_{k}^{1},\lambda_{k}^{2}\in \mathbb{R}^{3}$
and
$\mu \in \mathbb{R}^{l}$
are the Lagrange multipliers and
$\psi_{d}\,:\, \mathbb{R}\rightarrow \mathbb{R}^{l}$
is the penalty function that incorporates inequality constraints into the cost functional as soft constraints [Reference Poole and Mackworth31]. Thus, Eq. (12) is the augmented form of Eq. (10) obtained by imposing the LGVI as dynamical constraints and the system state constraints to the cost functional using Lagrange multipliers. The
$({\diamond})\,:\, \mathbb{R}^{3}\rightarrow \mathfrak{so}(3)^{*}$
represents an isomorphism defined as:
$x^{\diamond }=\dfrac{1}{2}\text{S}(x)$
for any
$x\in \mathbb{R}^{3}$
, and
$\mathfrak{so}(3)^{*}$
indicates the dual space of
$\mathfrak{so}(3)$
[Reference Holm, Schmah and Stoica32]. As well,
$\ll .,.\gg \,:\, \mathfrak{so}(3)\times \mathfrak{so}(3)^{*}\rightarrow \mathbb{R}$
denotes the natural pairing between the elements of
$\mathfrak{so}(3)$
and
$\mathfrak{so}(3)^{*}$
[Reference Holm28].
3.1. Extracting the necessary conditions for optimality
According to the above relations and the scheme presented in ref. [Reference Bagherzadeh, Karimpour and Keshmiri18], the first variation of the augmented cost functional is computed. The infinitesimal variations of
$R_{k}$
and
$F_{k}$
are defined through
$\delta R_{k}=R_{k}\text{S}(\eta_{k})$
and
$\delta F_{k}=F_{k}\text{S}(\zeta_{k})$
where
$\eta_{k},\zeta_{k}\in \mathbb{R}^{3}$
. Consequently, the following relations can be obtained [Reference Lee, Leok and McClamroch14]:
\begin{align}\eta_{k+1}=F^{T}_{k}\eta_{k}+\zeta_{k}\\[-25pt]\nonumber\end{align}
\begin{equation}\zeta_{k}=M_{k}\delta {\Pi}_{k}\end{equation}
where
$M_{k}=hF^{T}_{k}[tr(F_{k}J_{d})I_{3}-F_{k}J_{d}]^{-1}\in \mathbb{R}^{3\times 3}$
. In addition, without loss of generality, the functions
$\mathfrak{L}_{d}$
,
$\Phi_{d}$
,
$C_{d}$
, and
$\psi_{d}(C_{d})$
are considered to be in the following formats:
\begin{align}\mathfrak{L}_{d}=\frac{h}{2}\!\left\| P_{1}^{\frac{1}{2}}(R_{k}-I_{3})\right\|_{F}^{2}+\frac{h}{2}\!\left\| P_{2}^{\frac{1}{2}}\text{S}({\Pi}_{k})\right\|_{F}^{2}+\frac{h}{2}\!\left\| P_{3}^{\frac{1}{2}}\text{S}(u_{k})\right\|_{F}^{2}\\[-25pt]\nonumber\end{align}
\begin{align}\Phi_{d}=\frac{1}{2}\!\left\| Q_{1}^{\frac{1}{2}}(R_{N}-I_{3})\right\|_{F}^{2}+\frac{1}{2}\!\left\| Q_{2}^{\frac{1}{2}}\text{S}({\Pi}_{N})\right\|_{F}^{2}\\[-25pt]\nonumber\end{align}
\begin{align}C_{d}=\frac{1}{2}\!\left\| \text{S}(u_{k})\right\|_{F}^{2}-\alpha\\[-25pt]\nonumber\end{align}
\begin{equation}\psi_{d}\!\left(C_{d}\right)=h \max \{0,{C_{d}}\}^{2}\end{equation}
where
$P_{1}$
,
$P_{2}$
,
$P_{3}$
,
$Q_{1}$
, and
$Q_{2}$
are positive definite coefficient matrices. According to [Reference Kalabic, Gupta, Cairano, Bloch and Kolmanovsky33], the quadratic format of the above functionals can suffice to undertake the stability of the control system. However, since the matter of stability analysis for NMPC on Euclidean space and Riemannian manifolds has already been studied in the literature, it is out of the scope of this paper. The authors propose [Reference Kalabic, Gupta, Cairano, Bloch and Kolmanovsky33–Reference Findeisen, Imsland, Allgower and Foss35] for more information about the stability conditions. It should be mentioned that the imposed constraints,
$C_{d}$
, prevent saturation of the actuators. According to the above equations, the necessary conditions for optimality are extracted as below:
\begin{align}h\text{S}\!\left({\Pi}_{k}\right)=F_{k}J_{d}-J_{d}F^{T}_{k}\\[-25pt]\nonumber\end{align}
\begin{align}R_{k+1}=R_{k}F_{k}\\[-25pt]\nonumber\end{align}
\begin{align}{\Pi}_{k+1}=F^{T}_{k}{\Pi}_{k}+hBu_{k}\\[-25pt]\nonumber\end{align}
\begin{align}\lambda_{N-1}^{1}=-\text{S}^{-1}\!\left(\left({R_{N}}^{T}Q_{1}\right)_{A}\right)\\[-25pt]\nonumber\end{align}
\begin{align}\lambda_{k+1}^{1}={F_{k+1}}^{T}\!\left[\lambda_{k}^{1}+h\text{S}^{-1}\!\left(\left({R_{k+1}}^{T}P_{1}\right)_{A}\right)\right]\\[-25pt]\nonumber\end{align}
\begin{align}\lambda_{N-1}^{2}=\text{S}^{-1}\!\left(\left(Q_{2}\text{S}({\Pi}_{N}\right)_{A}\right)\\[-25pt]\nonumber\end{align}
\begin{align}\lambda_{k+1}^{2}=\left(F_{k+1}-{M_{k+1}}^{T}\text{S}\!\left({F_{k+1}}^{T}{\Pi}_{k+1}\right)\right)^{-1}\!\left[-{M_{k+1}}^{T}\lambda_{k+1}^{1}+\lambda_{k}^{2}-h\text{S}^{-1}\!\left(\left(P_{2}\text{S}({\Pi}_{k+1})\right)_{A}\right)\right]\\[-25pt]\nonumber\end{align}
\begin{equation}hB^{T}\lambda_{k}^{2}=-h\text{S}^{-1}\!\left(\left(P_{3}\text{S}(u_{k})\right)_{A}\right)-\mu \text{S}^{-1}\!\left(\left(\text{D}_{{\text{S}(u_{k}})}\psi_{d}\!\left(C_{d}\right)\right)_{A}\right)\end{equation}
where
$(\mathcal{C})_{A}$
represents the antisymmetric part and
$\| \mathcal{C}\|_{F}$
represents the Frobenius norm for every matrix
$\mathcal{C}\in \mathbb{R}^{3\times 3}$
.
The above equations express a TPBVP that can be solved using the initial conditions of
$R_{k}$
,
${\Pi}_{k}$
,
$\lambda_{k}^{1}$
, and
$\lambda_{k}^{2}$
. Given
${\Pi}_{0}$
,
$F_{0}$
is computed using Eq. (19).
$R_{0}$
and
$F_{0}$
permit to calculate
$R_{1}$
through Eq. (20). By means of the initial values
$\lambda_{0}^{1}$
and
$\lambda_{0}^{2}$
,
$u_{0}$
will be computed using Eq. (26). Then
$\lambda_{0}^{1}$
and
$\lambda_{0}^{2}$
are computed via Eqs. (23) and (25). This process is repeated until
$k=N-2$
. Since the initial values for the Lagrange multipliers are not available, an indirect shooting method should be used to estimate them. Accordingly, similar to ref. [Reference Bagherzadeh, Karimpour and Keshmiri18], the first variation of Eqs. (19) to (26) is derived to extract the sensitivity derivatives:
\begin{align}\eta_{k+1}=F^{T}_{k}\eta_{k}+M_{k}\delta {\Pi}_{k}\\[-25pt]\nonumber\end{align}
\begin{align}\delta {\Pi}_{k+1}=\left[F^{T}_{k}+\text{S}\!\left(F^{T}_{k}{\Pi}_{k}\right)M_{k}\right]\delta {\Pi}_{k}+hB\delta u_{k}\\[-25pt]\nonumber\end{align}
\begin{equation}\delta \lambda_{N-1}^{1}=\text{S}^{-1}\!\left(\left(\text{S}(\eta_{N}){R_{N}}^{T}Q_{1}\right)_{A}\right)\end{equation}
\begin{align}\delta \lambda_{k+1}^{1}=\left(F_{k+1}\text{S}(M_{k+1}\delta {\Pi}_{k+1})\right)^{T}\!\left[\lambda_{k}^{1}+h\text{S}^{-1}\!\left(\left({R_{k+1}}^{T}P_{1}\right)_{A}\right)\right]+{F_{k+1}}^{T}\!\left[\delta \lambda_{k}^{1}+h\text{S}^{-1}\!\left(\left(\left(R_{k+1}\text{S}\!\left(\eta_{k+1}\right)\right)^{T}P_{1}\right)_{A}\right)\right]\\[-25pt]\nonumber\end{align}
\begin{align}\delta \lambda_{N-1}^{2}=S^{-1}\!\left(\left(Q_{2}\text{S}(\delta {\Pi}_{N}\right)_{A}\right)\\[-25pt]\nonumber\end{align}
\begin{align}\delta \lambda_{k+1}^{2} & =\left(F_{k+1}-{M_{k+1}}^{T}\text{S}\!\left({F_{k+1}}^{T}{\Pi}_{k+1}\right)\right)^{-1}\!\left[\left(-\delta {M_{k+1}}^{T}\lambda_{k+1}^{1}-{M_{k+1}}^{T}\delta \lambda_{k+1}^{1}+\delta \lambda_{k}^{2}-h\text{S}^{-1}\!\left(\left(P_{2}\text{S}(\delta {\Pi}_{k+1})\right)_{A}\right)\right)\right.\nonumber\\[4pt]& \quad \left. -\left(\delta F_{k+1}-\delta {M_{k+1}}^{T}\text{S}\!\left({F_{k+1}}^{T}{\Pi}_{k+1}\right)-{M_{k+1}}^{T}\delta \text{S}\!\left({F_{k+1}}^{T}{\Pi}_{k+1}\right)\right)\lambda_{k+1}^{2}\right]\\[-25pt]\nonumber\end{align}
\begin{equation}B^{T}\delta \lambda_{k}^{2}=-\text{S}^{-1}\!\left(({P_{3}}\text{S}\!\left(\delta u_{k}\right)+2\mu {C_{d}}\text{S}\!\left(\delta u_{k}\right)+2\mu \text{S}({u_{k}})\text{S}({\delta u_{k}})\text{S}({u_{k}}))_{A}\right)\end{equation}
where
\begin{equation}\delta M_{k}=\left[h\!\left(F_{k}\text{S}\!\left(M_{k}\delta {\Pi}_{k}\right)\right)^{T}-M_{k}(tr\!\left(F_{k}\text{S}\!\left(M_{k}\delta {\Pi}_{k}\right)J_{d}\right)I_{3}-F_{k}\text{S}\!\left(M_{k}\delta {\Pi}_{k}\right)J_{d})\right]\left(tr\!\left(F_{k}J_{d}\right)I_{3}-F_{k}J_{d}\right)^{-1}\end{equation}
These equations can be rewritten in the following form considering Eqs. (26) and (33):
\begin{equation}\left[\begin{array}{c} \eta_{k+1}\\[4pt] \delta {\Pi}_{k+1}\\[4pt] \delta \lambda_{k+1}^{1}\\[4pt] \delta \lambda_{k+1}^{2} \end{array}\right]=\mathcal{K}_{k}\left[\begin{array}{c} \eta_{k}\\[4pt] \delta {\Pi}_{k}\\[4pt] \delta \lambda_{k}^{1}\\[4pt] \delta \lambda_{k}^{2} \end{array}\right]\end{equation}
where
$\mathcal{K}_{k}$
is the transition matrix to the next step. Thus,
\begin{equation}\left[\begin{array}{c} \eta_{N}\\[4pt] \delta {\Pi}_{N}\\[4pt] \delta \lambda_{N}^{1}\\[4pt] \delta \lambda_{N}^{2} \end{array}\right]=\left(\Pi_{k=0}^{N-1}\mathcal{K}_{k}\right)\left[\begin{array}{c} \eta_{0}\\[4pt] \delta {\Pi}_{0}\\[4pt] \delta \lambda_{0}^{1}\\[4pt] \delta \lambda_{0}^{2} \end{array}\right]=\left[\begin{array}{c@{\quad}c} \text{K}_{11} & \text{K}_{12}\\[4pt] \text{K}_{21} & \text{K}_{22} \end{array}\right]\left[\begin{array}{c} \eta_{0}\\[4pt] \delta {\Pi}_{0}\\[4pt] \delta \lambda_{0}^{1}\\[4pt] \delta \lambda_{0}^{2} \end{array}\right]\end{equation}
Since for each step of the NMPC, the original values of the spacecraft attitude and the angular momentum are specified, and the amounts of
$\eta_{0}$
and
$\delta {\Pi}_{0}$
are set to zero. Consequently,
\begin{equation}\left[\begin{array}{c} \eta_{N}\\[4pt] \delta {\Pi}_{N} \end{array}\right]=\text{K}_{12}\left[\begin{array}{c} \delta \lambda_{0}^{1}\\[4pt] \delta \lambda_{0}^{2} \end{array}\right]\end{equation}
The submatrix
$\text{K}_{12}$
, which is called the sensitivity derivative, determines the sensitivity of the specified terminal boundary conditions to the changes in the (unspecified) initial conditions of the Lagrange multipliers [Reference Lee, McClamroch and Leok15]. At this stage and in the context of NMPC formulation, based on given initial conditions
$R_{0}$
and
${\Pi}_{0}$
and an initial guess for
$\lambda_{0}^{1}$
and
$\lambda_{0}^{2}$
, the final values for the rigid body attitude matrix and angular momentum,
$R_{N}$
and
${\Pi}_{N}$
, are forward computed and associated with the final values of the Lagrange multipliers,
$\lambda_{N-1}^{1}$
and
$\lambda_{N-1}^{2}$
, through Eqs. (22) and (24). On the other hand, these latter are also predicted through Eqs. (23) and (25) for
$k=N-2$
. Based on these two approaches, the following error function can be defined:
\begin{equation}Er=\left[\begin{array}{c} \lambda_{N-1}^{1}+\text{S}^{-1}\!\left(\left({R_{N}}^{T}Q_{1}\right)_{A}\right)\\[7pt] \lambda_{N-1}^{2}-\text{S}^{-1}\!\left(\left(Q_{2}\text{S}({\Pi}_{N}\right)_{A}\right) \end{array}\right]\end{equation}
By applying an indirect shooting method (using a similar procedure used in refs. [Reference Lee, Leok and McClamroch14, Reference Gupta, Kalabic, Di Cairano, Bloch and Kolmanovsky17]), the guessed initial values of the Lagrange multipliers are re-estimated as:
\begin{equation}\left[\begin{array}{c} \lambda_{0}^{1}\\[4pt] \lambda_{0}^{2} \end{array}\right]^{(l+1)}=\left[\begin{array}{c} \lambda_{0}^{1}\\[4pt] \lambda_{0}^{2} \end{array}\right]^{\left(l\right)}-\gamma \!\left[\frac{\delta Er^{\left(l\right)}}{\left[\begin{array}{c} \delta \lambda_{0}^{1}\\[4pt] \delta \lambda_{0}^{2} \end{array}\right]^{\left(l\right)}}\right]^{-1}Er^{\left(l\right)}\end{equation}
where
$\gamma$
is a real number that belongs to the interval
$(0,1]$
,
$l$
is an index that specifies the number of iterations executed until
$\| Er\| \leq \epsilon$
, and
$\epsilon$
is an allowed preset value. Therefore, the optimal control problem is solved with the inputs
$(R_{0})^{i}$
,
$({\Pi}_{0})^{i}$
, and the final guess for
$(\lambda_{0}^{1})^{i}$
and
$(\lambda_{0}^{2})^{i}$
at each NMPC time step. The index
$(i)$
represents the step number with the maximum value
$i_{max}=\frac{\mathcal{T}}{h}$
where
$\mathcal{T}$
is the total time of executing the defined maneuver. The process will be repeated from the scratch for the next steps until the entire time allotted for the problem ends.
3.2. Simplification of the sensitivity derivatives and TPBVP equations
In this subsection, a procedure for improving numerical efficiency of the NMPC calculations is presented as the first main contribution of this paper, which consists of simplifying the expression for the sensitivity derivatives and TPBVP equations without affecting the accuracy of the whole process. By removing some nonlinear terms that are non-essential in the iterative shooting step of the algorithm, a reduction in the computation burden of the optimal control sub-problem related to the NMPC method is obtained [Reference Bagherzadeh, Karimpour and Keshmiri18]. Eq. (33) in Section 3.1 is an implicit nonlinear equation for the purpose of computing
$\delta u_{k}$
that has to be dealt with in the process of estimating
$(\lambda_{0}^{1})^{l}$
and
$(\lambda_{0}^{2})^{l}$
. However, Eq. (33) has no direct effect on the overall trend of solving the necessary conditions for optimality and may only increase the number of Newton iterations in the indirect shooting method. In order to be more explicit, at each step of solving the NMPC problem, Eqs. (19)–(26) are solved based on initial guesses for
$(\lambda_{0}^{1})^{l}$
and
$(\lambda_{0}^{2})^{l}$
. Then if
$\| Er\| \gt \epsilon$
, Eqs. (27)–(33) are solved to rectify those guessed values based on a Newton-like iterative method, and this process continues until
$\| Er\| \leq \epsilon$
. Therefore, Eqs. (27)–(33) are solved based on constraint-imposed control inputs. Hence, the constraint-related part of Eq. (33) can be removed from the solution procedure since the constraints are already applied to the system through Eqs. (19)–(26). Consequently, Eq. (33) is substituted with the following linear equation:
\begin{equation}B^{T}\delta \lambda_{k}^{2}=-\text{S}^{-1}\!\left(({P_{3}}\text{S}\!\left(\delta u_{k}\right)_{A}\right)\end{equation}
In addition, the TPBVP equations may be prone to constraints that are activated only when their associated variables exceed some defined limits. In the problem under consideration, constraints on
$u_{k}$
only apply when the limits are exceeded and saturation occurs. Accordingly, assuming that the constraints are still inactive, Eq. (26) can be considered as the linear form below:
\begin{equation}hB^{T}\lambda_{k}^{2}=-h\text{S}^{-1}\!\left(\left(P_{3}\text{S}(u_{k})\right)_{A}\right)\end{equation}
Then
$u_{k}$
computed through Eq. (41) is checked for being within the allowed range of the imposed constraints. If the restrictions are not met,
$u_{k}$
is re-computed using Eq. (26).
These simplifications can reduce the time of computations to a considerable extent while maintaining the accuracy within acceptable limits compared to complete usage of nonlinear equations. In the next section, the results of applying simplifications proposed above are compared with the results of the methodology employed in refs. [Reference Lee, Leok and McClamroch14] and [Reference Gupta, Kalabic, Di Cairano, Bloch and Kolmanovsky17], where
$u_{k}$
and
$\delta u_{k}$
are computed without omitting the nonlinear terms, for the sake of comparison in terms of time saving, efficiency, and accuracy.
4. An example on SO(3)
As an example of this section, a fully actuated spacecraft evolving on SO(3) with Eqs. (7), (8), and (9) as the LGVI and numerical values tabulated in Table I is considered.
Table I. Numerical values employed in the simulation of the spacecraft.
In the following figures, the results obtained by fully accounting for the nonlinear terms in both the constrained TPBVP and the sensitivity equations [Reference Gupta, Kalabic, Di Cairano, Bloch and Kolmanovsky17] are compared with those extracted by solely solving the TPBVP using the simplified cast of the sensitivity derivatives, and those obtained by simplifying both the TPBVP and the sensitivity derivatives, as discussed in Subsection 3.2. Diagrams of the control inputs
$u_{1}$
,
$u_{2}$
,
$u_{3}$
, and
$\| u\|$
are plotted in Fig. 1. The results show a relatively good match independent of the level of simplification employed. In fact, the noteworthy point about these simplifications is that they reduce the computation time of the problem by a considerable amount without affecting the accuracy of the results. In order to show the effect of removing the nonlinear parts from Eqs. (26) and (33) on the optimal control action, diagrams of integral square error are plotted in Fig. 2. The black line shows the difference between the elements of exact input which is computed through Eqs. (26) and (33) and the input computed using the simplified sensitivity derivative (Eq. (40)). The red line represents the difference between the exact input and the input computed using both the simplified TPBVP (Eq. (41)) and the simplified sensitivity derivative (Eq. (40)). As described in this figure, using simplified sensitivity derivatives lead to less error in comparison with using both simplified TPBVP and sensitivity derivative. However, the amounts of error are in the order of about 10–7 which are definitely negligible. The body-fixed components of angular momentum are illustrated in Fig. 3. As predictable, there is a good agreement between the results achieved regardless of the approach that is used.
Figure 1.
Diagrams of
$u_{1}$
,
$u_{2}$
,
$u_{3}$
, and
$\| u\|$
, obtained using exact solution, estimated
$\delta u$
, and estimated
$u$
and
$\delta u$
.
Figure 2.
Diagrams of ISE
$(u_{1})$
, ISE
$(u_{2})$
, ISE
$(u_{3})$
, and ISE
$(\| u\|$
), obtained using exact solution minus solution extracted using estimated
$\delta u$
, and exact solution minus solution extracted using estimated
$u$
and
$\delta u$
.
It is worthy to notice that the similarity between the second and third components in both Figs 1 and 3 is justified as the second and the third moment of inertia of the spacecraft are equal,
$J_{2}=J_{3}$
, so are the initial conditions
$R_{0}$
and
${\Pi}_{0}$
according to Table I, leading to a symmetry in the optimal maneuver. Figure 4 represents the rotation matrices of the spacecraft,
$R_{k}$
, on S2. The columns of
$R_{0}$
and
$R_{N}$
are indicated by dashed and solid colored lines, respectively. By comparing diagrams of Fig. 4, it is completely clear that regardless of which methods are chosen, the attitude maneuver of the spacecraft is the same.
As demonstrated, the results corresponding to the control efforts, angular momenta, and spacecraft attitude obtained by the foregoing methods matched precisely. However, what prioritizes these methods over each other is the time consumption for performing the computations. Table II provides the amounts of time needed to perform one step and the total computation time. In addition, the time of performing each step is plotted against time, for each of the three methods in Fig. 5.
As it is clear by comparing the numerical values of Table II and diagrams of Fig. 5, solving the exact TPBVP equations is very time-consuming. Using Eq. (40) to estimate
$\delta u$
via a linear equation in sensitivity derivatives can reduce the average time of performing each step and the total computation time about 73.4%. By considering both Eqs. (40) and (41) to estimate
$\delta u$
and
$u$
, the average time of performing each step and the total computation time would be reduced by about 84.8%.
Table II. Numerical values of time to perform each step and total computational time.
Figure 3.
Diagrams of
${\Pi}_{1}$
,
${\Pi}_{2}$
, and
${\Pi}_{3}$
, obtained using exact solution, estimated
$\delta u$
, and estimated
$u$
and
$\delta u$
.
Figure 4.
Diagrams of the columns of rotation matrices
$R_{k}$
, obtained using exact solution, estimated
$\delta u$
, and estimated
$u$
and
$\delta u$
.
Figure 5.
Diagram of time to perform each step, obtained using exact solution, estimated
$\delta u$
, and estimated
$u$
and
$\delta u$
.
By comparing the results, one can come to the conclusion that both procedures presented in Subsection 3.2 have the ability of reducing the optimization process time while keeping the accuracy at a reasonable level. Thus, both methods could be used as a suitable alternative for what is presented in [Reference Gupta, Kalabic, Di Cairano, Bloch and Kolmanovsky17] where iterative computations to extract
$u$
and
$\delta u$
are performed based on solving nonlinear equations which is unnecessary according to the explanations given in Subsection 3.2. Nevertheless, approximating
$\delta u$
in the calculation of sensitivity derivatives has no direct effect on the general trend of solving the equations and the compliance with the constraints is also guaranteed. In addition, unlike referring to Eq. (41) for estimating
$u$
, the constraints do not need to be checked at each iteration. Thus, estimating
$\delta u$
is preferred over estimating both
$u$
and
$\delta u$
in the process of solving the NMPC optimization problem and that will be employed in the following sections.
5. Application of NE method in NMPC control of mechanical systems on SO(3)
In this section, it is supposed that the initial conditions of the system and consequently, the initial conditions for each interval may change due to some re-planning. The goal is to find a way to estimate the responses of the system to the new initial conditions without performing the optimization process from the scratch. Thus, the trend ahead is to use the NE method in the formulation of NMPC in order to find a relation between the new initial conditions and the variations induced in the control inputs and states of the system. The responses of the system are thus extracted based on the NE method, reducing the computation time to a considerable extent in comparison with repeating the NMPC process. Indeed, since the NMPC method is a repetitive time-consuming procedure, it makes perfect sense to reformulate the NMPC construction based on NE method to be used for situations where the initial conditions of a once-solved system are changed. In the following, first, a brief introduction to the NE method on Euclidean configuration is given and then, the equations of the reformulated NMPC based on NE method on SO(3) are extracted.
5.1. A brief introduction to NE method on Euclidean Space
NE method is a rapid method for finding the change in the optimal solution according to the changes in the initial conditions and parameters of the system [Reference Bryson and Ho19, Reference Bloch, Gupta and Kolmanovsky21]. Suppose that the discrete equations of motion of a system are expressed in the following form on Euclidean space:
\begin{equation}x_{k+1}=f\!\left(x_{k},u_{k}\right),\quad x_{k}\in \mathbb{R}^{n},\quad u_{k}\in \mathbb{R}^{m}\end{equation}
with the initial conditions and constraints as:
\begin{align}x_{k=0}=x_{0}\\[-29pt]\nonumber\end{align}
\begin{equation}C(x_{k},u_{k})\leq 0\end{equation}
The optimal control is obtained through minimizing the following cost functional:
\begin{equation}min\ J_{u}=\Phi \!\left(x_{N}\right)+\sum_{k=0}^{N-1}L\!\left(x_{k},u_{k}\right)\end{equation}
subjected to Eqs. (42), (43), and (44). The functions
$f$
,
$C$
,
$\Phi$
, and
$L$
are twice differentiable. By defining the function
$H$
as:
\begin{equation}H\!\left(x_{k},u_{k}\right)=L\!\left(x_{k},u_{k}\right)+{\lambda_{k}}^{T}f\!\left(x_{k},u_{k}\right)+{\mu_{k}}^{T}C(x_{k},u_{k})\end{equation}
the augmented cost functional is obtained by adding the constraints to Eq. (45):
\begin{equation}min\ \overline{J}_{u}=\Phi \!\left(x_{N}\right)+\sum_{k=0}^{N-1}\!\left(H\!\left(x_{k},u_{k}\right)-{\lambda_{k}}^{T}x_{k+1}\right)\end{equation}
where
$\lambda_{k}$
and
$\mu_{k}$
are the Lagrange multipliers. As well,
$x_{k+1}$
is the predicted state vector based on the initial condition
$x_{0}$
and the input vector
$u_{k}$
at instant
$k$
for
$k=0,\ldots,N$
. Now suppose that there is a small change in the initial conditions of the system. The variation of the initial conditions is defined by
$\delta x_{0}=\overline{x}_{0}-x_{0}$
, where
$\overline{x}_{0}$
represents the new set of initial conditions. The NE method is established to find
$\delta u_{k}$
based on minimizing the second-order variation of the system cost functional, since the first-order necessary conditions of optimality vanish at nominal solutions. Therefore, the new optimal control problem is formulated as follows [Reference Bryson and Ho19, Reference Ghaemi, Sun and Kolmanovsky36]:
\begin{equation}min\, \delta ^{2}\overline{J}_{u}=\frac{1}{2}\delta {x_{N}}^{T}\Phi_{{x_{k}}{x_{k}}}\!\left(x_{N}\right)\delta x_{N}+\frac{1}{2}\sum_{k=0}^{N-1}\!\left[\begin{array}{c} \delta x_{k}\\[3pt] \delta u_{k} \end{array}\right]^{T}\!\left[\begin{array}{c@{\quad}c} H_{{x_{k}}{x_{k}}}(x_{k},u_{k}) & H_{{x_{k}}{u_{k}}}(x_{k},u_{k})\\[3pt] H_{{u_{k}}{x_{k}}}(x_{k},u_{k}) & H_{{u_{k}}{u_{k}}}(x_{k},u_{k}) \end{array}\right]\left[\begin{array}{c} \delta x_{k}\\[3pt] \delta u_{k} \end{array}\right]\end{equation}
subjected to the variation of the dynamics of the system and the variation of the constraints as given below:
\begin{align}\delta x_{k+1}=f_{{x_{k}}}\!\left(x_{k},u_{k}\right)\delta x_{k}+f_{{u_{k}}}(x_{k},u_{k})\delta u_{k}\\[-25pt]\nonumber\end{align}
\begin{align}\delta x_{k=0}=\delta x_{0}\\[-25pt]\nonumber\end{align}
\begin{equation}C_{{x_{k}}}\!\left(x_{k},u_{k}\right)\delta x_{k}+C_{{u_{k}}}\!\left(x_{k},u_{k}\right)\delta u_{k}=0\end{equation}
The NE solution based on variations of the initial conditions is obtained through solving the above new optimal control problem [Reference Bryson and Ho19, Reference Ghaemi, Sun and Kolmanovsky36, Reference McReynolds37]:
\begin{equation}\delta u_{k}=-[I_{m}\quad 0_{\aleph}]\left[\begin{array}{c@{\quad}c} Z_{{u_{k}}{u_{k}}}(x_{k},u_{k}) & {C_{{u_{k}}}}^{T}\!\left(x_{k},u_{k}\right)\\[4pt] C_{{u_{k}}}\!\left(x_{k},u_{k}\right) & 0_{\daleth } \end{array}\right]^{-1}\left[\begin{array}{c} Z_{{u_{k}}{x_{k}}}\!\left(x_{k},u_{k}\right)\\[4pt] C_{{x_{k}}}\!\left(x_{k},u_{k}\right) \end{array}\right]x_{k}\end{equation}
where
$\aleph$
and
$\daleth$
depend on whether the constraints are active or not. Moreover,
\begin{align}Z_{{u_{k}}{u_{k}}}\!\left(x_{k},u_{k}\right)=H_{{u_{k}}{u_{k}}}\!\left(x_{k},u_{k}\right)+{f_{{u_{k}}}}({x_{k}},{u_{k}})^{T}G(x_{k+1},u_{k+1})f_{{u_{k}}}(x_{k},u_{k})\\[-25pt]\nonumber\end{align}
\begin{align}Z_{{u_{k}}{x_{k}}}\!\left(x_{k},u_{k}\right)={Z_{{x_{k}}{u_{k}}}}\!\left(x_{k},u_{k}\right)^{T}=H_{{u_{k}}{x_{k}}}\!\left(x_{k},u_{k}\right)+{f_{{u_{k}}}}({x_{k}},{u_{k}})^{T}G(x_{k+1},u_{k+1})f_{{x_{k}}}(x_{k},u_{k})\\[-25pt]\nonumber\end{align}
\begin{equation}Z_{{x_{k}}{x_{k}}}\!\left(x_{k},u_{k}\right)=H_{{x_{k}}{x_{k}}}\!\left(x_{k},u_{k}\right)+{f_{{x_{k}}}}({x_{k}},{u_{k}})^{T}G(x_{k+1},u_{k+1})f_{{x_{k}}}(x_{k},u_{k})\end{equation}
where
\begin{equation}G\!\left(x_{k},u_{k}\right)=Z_{{x_{k}}{x_{k}}}\!\left(x_{k},u_{k}\right)-\left[\begin{array}{c@{\quad}c} Z_{{x_{k}}{u_{k}}}\!\left(x_{k},u_{k}\right) & {C_{x\!\left(k\right)}}^{T}\!\left(x_{k},u_{k}\right) \end{array}\right]\left[\begin{array}{c@{\quad}c} Z_{{u_{k}}{u_{k}}}\!\left(x_{k},u_{k}\right) & {C_{{u_{k}}}}^{T}\!\left(x_{k},u_{k}\right)\\[5pt] C_{{u_{k}}}\!\left(x_{k},u_{k}\right) & 0_{\daleth } \end{array}\right]^{-1}\left[\begin{array}{c} Z_{{u_{k}}{x_{k}}}\!\left(x_{k},u_{k}\right)\\[5pt] C_{x\!\left(k\right)}\left(x_{k},u_{k}\right) \end{array}\right]\nonumber\\[3pt]\end{equation}
\begin{align}k=0,\ldots,N-1\\[-25pt]\nonumber\end{align}
\begin{equation}G\!\left(x_{N},u_{N}\right)=\Phi_{{x_{k}}{x_{k}}}\!\left(x_{N},u_{N}\right)\end{equation}
Consequently, the NE method which is established based on the following equations [Reference Schättler and Ledzewicz38]:
\begin{align}\overline{x}_{k}=x_{k}+\delta x_{k}\\[-25pt]\nonumber\end{align}
\begin{equation}\overline{u}_{k}=u_{k}+\delta u_{k}\end{equation}
provides a method to find the updated optimal control for the new set of initial conditions using the above-mentioned recursive method.
5.2. Equations extraction for application of NE method into NMPC on SO(3)
The equations presented in Subsection 3.1should be converted into a suitable form for implementing the NE method in the NMPC on SO(3). Hence, Eq. (47) is rewritten in the following form:
\begin{align}\overline{\mathfrak{J}}_{d} & =\Phi_{d}\!\left(R_{N},\text{S}\!\left({\Pi}_{N}\right)\right)+\sum_{k=0}^{N-1}H_{d}\!\left(R_{k},\text{S}({\Pi}_{k}),\text{S}(\text{u}_{k})\right)-\ll \!\left(\lambda_{k}^{1}\right)^{\diamond },log\!\left(R_{k}^{T}R_{k+1}\right)\gg -\ll \text{S}\!\left(\lambda_{k}^{2}\right),\left({\Pi}_{k+1}\right)^{\diamond }\gg\nonumber\\[3pt]& \quad +\mu ^{T}\psi_{d}(C_{d}(R_{k},\text{S}({\Pi}_{k}),\text{S}(\text{u}_{k}))\end{align}
where
\begin{align}H_{d}(R_{k},\text{S}({\Pi}_{k}),\text{S}(\text{u}_{k}))=\mathfrak{L}_{d}(R_{k},\text{S}({\Pi}_{k}),\text{S}(\text{u}_{k}))+\ll \!\left(\lambda_{k}^{1}\right)^{\diamond },log\!\left(F_{k}\right)\gg +\ll \text{S}\!\left(\lambda_{k}^{2}\right),(F_{k}^{T}{{\Pi}_{k}}+hB{u_{k}})^{\diamond }\gg\nonumber\\\end{align}
By denoting
$\upsilon (k)=[{\eta_{k}}, \delta {{\Pi}_{k}}]^{T}$
, the second variation of
$\overline{\mathfrak{J}}_{d}$
would be
\begin{equation}{\delta ^{2}}\overline{\mathfrak{J}}_{d}=\frac{1}{2}\upsilon \!\left(N\right)^{T}{\Phi_{d}}_{\upsilon \upsilon }\!\left(N\right)\upsilon \!\left(N\right)+\frac{1}{2}\sum_{k=0}^{N-1}\!\left[\begin{array}{c} \upsilon (k)\\[4pt] \delta u_{k} \end{array}\right]^{T}\left[\begin{array}{c@{\quad}c} {H_{d}}_{\upsilon \upsilon }(k) & {H_{d}}_{\upsilon u}(k)\\[4pt] {H_{d}}_{u\upsilon }(k) & {H_{d}}_{uu}(k) \end{array}\right]\left[\begin{array}{c} \upsilon (k)\\[4pt] \delta u_{k} \end{array}\right]\end{equation}
constrained to the first variation of the necessary conditions for optimality as follows:
\begin{align}\delta R_{k+1}=\delta (R_{k}F_{k})=\left[\begin{array}{c@{\quad}c} F_{k}^{T} & M_{k} \end{array}\right]\upsilon (k)\\[-25pt]\nonumber\end{align}
\begin{align}\delta {\Pi}_{k+1}=\delta \!\left(F_{k}^{T}{\Pi}_{k}+hBu_{k}\right)=\left[\begin{array}{c@{\quad}c} 0_{3\times 3} & \left(F_{k}^{T}+\text{S}\!\left(F_{k}^{T}{\Pi}_{k}\right)M_{k}\right) \end{array}\right]\upsilon (k)+hB\delta u_{k}\\[-25pt]\nonumber\end{align}
\begin{equation}{{C_{d}}_{\upsilon (k)}}^{T}\!\left(k\right)\upsilon (k)+{{C_{d}}_{{u_{k}}}}^{T}\!\left(k\right)\delta u_{k}=0\end{equation}
Similar to what has been explained in Subsection 3.1 and in order to find the relationship between the changes in the initial conditions and the associated variations in the control inputs, the following equations for mechanical systems evolving on the manifold SO(3) are extracted:
\begin{equation}\delta u_{k}=-[I_{3}\quad 0_{\aleph}]\left[\begin{array}{c@{\quad}c} Z_{uu}(k) & {{C_{d}}_{{u_{k}}}}^{T}\!\left(k\right)\\[4pt] {C_{d}}_{{u_{k}}}\!\left(k\right) & 0_{\daleth } \end{array}\right]^{-1}\left[\begin{array}{c} Z_{u\upsilon }(k)\\[4pt] {C_{d}}_{\upsilon (k)}\!\left(k\right) \end{array}\right]\upsilon (k)\end{equation}
Also,
\begin{equation}Z_{uu}\!\left(k\right)={H_{d}}_{uu}\!\left(k\right)+\left[\begin{array}{c@{\quad}c} \dfrac{\delta R_{k+1}}{\delta u_{k}} & \dfrac{\delta {\Pi}_{k+1}}{\delta u_{k}} \end{array}\right]G(k+1)\!\left[\begin{array}{c@{\quad}c} \dfrac{\delta R_{k+1}}{\delta u_{k}} & \dfrac{\delta {\Pi}_{k+1}}{\delta u_{k}} \end{array}\right]^{T}\end{equation}
\begin{equation}Z_{u\upsilon }\!\left(k\right)={Z_{\upsilon u}}\!\left(k\right)^{T}={H_{d}}_{u\upsilon }\!\left(k\right)+\left[\begin{array}{c@{\quad}c} \dfrac{\delta R_{k+1}}{\delta u_{k}} & \dfrac{\delta {\Pi}_{k+1}}{\delta u_{k}} \end{array}\right]G(k+1)\left[\begin{array}{c@{\quad}c} \begin{array}{c@{\quad}c} \dfrac{\delta R_{k+1}}{\delta \eta_{k}} & \dfrac{\delta R_{k+1}}{\delta {\Pi}_{k}} \end{array} & \begin{array}{c@{\quad}c} \dfrac{\delta {\Pi}_{k+1}}{\delta \eta_{k}} & \dfrac{\delta {\Pi}_{k+1}}{\delta {\Pi}_{k}} \end{array} \end{array}\right]^{T}\end{equation}
\begin{align}Z_{\upsilon \upsilon }\!\left(k\right) & = {H_{d}}_{\upsilon \upsilon }\!\left(k\right)\nonumber\\[4pt]& \quad +\left[\begin{array}{c@{\quad}c} \begin{array}{c@{\quad}c} \dfrac{\delta R_{k+1}}{\delta \eta_{k}} & \dfrac{\delta R_{k+1}}{\delta {\Pi}_{k}} \end{array} & \begin{array}{c@{\quad}c} \dfrac{\delta {\Pi}_{k+1}}{\delta \eta_{k}} & \dfrac{\delta {\Pi}_{k+1}}{\delta {\Pi}_{k}} \end{array} \end{array}\right] G\!\left(k+1\right)\left[\begin{array}{c@{\quad}c} \begin{array}{c@{\quad}c} \dfrac{\delta R_{k+1}}{\delta \eta_{k}} & \dfrac{\delta R_{k+1}}{\delta {\Pi}_{k}} \end{array} & \begin{array}{c@{\quad}c} \dfrac{\delta {\Pi}_{k+1}}{\delta \eta_{k}} & \dfrac{\delta {\Pi}_{k+1}}{\delta {\Pi}_{k}} \end{array} \end{array}\right]^{T}\end{align}
where
$G(k)$
,
$k=0,\ldots,N$
, are obtained by substituting
$\upsilon$
instead of
$x(k)$
in Eqs. (56) and (57).
The elements in Eqs. (60)–(69) are extracted considering Eqs. (15)–(18). Should any changes occur in the initial conditions of the system, above equations can be used instead of employing the normal trend, resulting to a considerable reduction in the amount of computations needed for the successive optimizations related to the NMPC process on SO(3). The remarkable superiority of this method over solving the optimization problem from the scratch in reducing the computation burden and hence increasing the speed of calculations is shown in the next section through an example.
6. NMPC and NE control of a spacecraft on SO(3) with changes in the initial conditions
In this section, the extracted relationships of Subsection 5.2 are implemented on the example of Section 4. It is supposed that the initial conditions of the system are altered. The example is solved using three methods. First, it is solved by the method provided in Subsection 3.2. Second, the NE method is used to estimate the necessary adaptation of the control inputs and its effect on the response of the system, and finally, the combination of these two methods is employed to get the solution. The combination of the methods is established based on solving the problem with altered initial conditions using NE method. However, at some predetermined steps the optimization problem is solved from the scratch in order to compensate for error accumulation. In fact, whenever the control inputs and system variables deviate inexorably from their actual value, a round of running exact NMPC-related optimization at intermediate steps takes them back to the expected actual path. Figure 6 shows the logic behind this proposed method in a simplified schematic form. The diagrams of the control inputs and the evolution of the system states as well as the diagrams of the time needed to perform each step are plotted and the results are compared in order to determine the more appropriate method.
Table III. First and second set of initial conditions.
Figure 6. The basic schematic diagram which shows how the three methods work for solving the NMPC problem.
Figure 7.
Diagrams of
$u_{1}$
,
$u_{2}$
,
$u_{3}$
, and
$\| u\|$
, obtained using exact solution for the NMPC-related TPBVP and the NE solution.
Figure 8.
Diagrams of
${\Pi}_{1}$
,
${\Pi}_{2}$
, and
${\Pi}_{3}$
, obtained using exact solution for the NMPC-related TPBVP and the NE solution.
Figure 9.
Diagrams of the columns of the rotation matrices
$R_{k}$
, obtained using exact solution for the NMPC-related TPBVP, and the NE solution.
6.1. The spacecraft problem: Exact solution of the NMPC versus NE solution
Consider the first and second set of initial conditions of the system given in Table III. The (
$-$
) symbol above the variables indicates the new initial conditions.
The example has been solved for the second set of initial conditions in two ways: the first approach is according to the method offered in Subsection 3.2, part A, and the second one consists of using the NE method presented in Subsection 5.2. Figure 7 shows the control inputs obtained by using these two methods. Based on these diagrams, the control inputs do not match for the initial steps but the differences vanish as time goes ahead. However, based on what can be seen in Fig. 8, the resulted angular momenta do not match and the difference even accentuates with time. The same trend can also be seen in the attitude of the spacecraft, plotted in Fig. 9. In this figure, black, green, and pink lines correspond to the exact solution of TPBVP, whereas blue, red, and cyan lines indicate the NE solution. As it is clear, the two methods represent considerable differences, and this happens due to the error accumulated at each step. Therefore, one can conclude that the NE method accompanied with exact TPBVP solutions calculated at some intermediate steps may provide better answers.
6.2. The spacecraft problem: Exact solution of the NMPC versus the combination of NE and exact TPBVP solution
In this subsection, the combination of NE and the exact solution of NMPC-related TPBVP is used in order to prevent the accumulation of errors during the whole run time of the simulation. The procedure to follow is explained next: As a first step, the “exact” solution to the new initial conditions is obtained through the TPBVP equations using the method explained in Subsection 3.2, part A. Then, for a number of steps which depends on the size of the problem, the NE method is used to find the neighborhood solution. Next, the exact solution is computed for another step via NMPC-related TPBVP and the whole process continues until the end. The strategy of our proposed procedure relies on employing the direct solution of NMPC-related TPBVP to return to the actual path whenever the neighborhood solution is deemed to deviate too much. By this way, a compromise between accuracy and the amount of computational operations can be reached. The diagrams for this example seem to confirm this claim. It turns out from Figs. 10, 11, and 12 that this method is able to regulate the error and to extract the relevant solution. In this example, the number of steps to reiterate the exact solution is considered equal to 10. Needless to say, the lower the number, the more accurate the result, but the heavier the calculations.
6.3. Time comparison of the introduced methods
The methods provided in Subsections 6.2 and 6.3 should also be compared in terms of computational time. The maximum, minimum, and the average values of time consumed to perform one step and the total computation duration are given in Table IV for the aforementioned approaches, namely i. solving the exact NMPC-related TPBVP directly according to Subsection 3.2, ii. using the NE method to estimate the control inputs and system states similar to what is offered in Subsection 6.1, and iii. using the combination of these two methods according to what is presented in Subsection 6.2. The time consumed for performing each step is plotted against time for each of the three methods in Fig. 13 as well.
Table IV. Numerical values of time to perform each step and total computational time.
Figure 10.
Diagrams of
$u_{1}$
,
$u_{2}$
,
$u_{3}$
, and
$\| u\|$
, obtained using exact solution for the NMPC-related TPBVP, and the combination of the NE and the exact TPBVP solution.
Considering the numerical values of Table IV, it is completely clear that using the NE method alone is absolutely superior to the other two methods in terms of computation time reduction. By using this method, the average time of performing each step and the total computation time would reduce by about 97.6%. However, the results of Subsection 6.1 indicate that using the NE method alone is not sufficient to estimate the expected responses of the system properly. Consequently, the combination of using the NE method and solving the NMPC optimal control problem using the exact TPBVP equations at some predefined intermediate steps, as presented in Subsection 6.2, is the suggested method of this article to meet the dual objectives. In addition, through achieving an accurately enough estimate of the control inputs and system states, this method can reduce the average time of performing each step by 85.5% and the total computation time by 86.1%. So, if some changes occur in the initial conditions of the system, the method as presented in Subsection 6.2 can prove efficient in updating the optimal control inputs and responses of the system without solving the whole optimization process from the scratch.
Figure 11.
Diagrams of
${\Pi}_{1}$
,
${\Pi}_{2}$
, and
${\Pi}_{3}$
, obtained using exact solution for the NMPC-related TPBVP, and the combination of the NE and the exact TPBVP solution.
Figure 12.
Diagrams of the columns of rotation matrices
$R_{k}$
, obtained using exact solution for the NMPC-related TPBVP, and the combination of the NE and the exact TPBVP solution.
Figure 13. Diagram of time to perform each step, obtained using i. exact solution for the NMPC-related TPBVP, ii. the NE solution, and iii. the combination of the NE and the exact TPBVP solution.
7. Conclusion
In this article, the NMPC of mechanical systems on SO(3) is considered. LGVI are used to extract the necessary conditions for optimality. The extracted TPBVP is solved using an indirect shooting method based on iterative Newton-like schemes. The proposed method in this article offers two computation tricks based on reducing the nonlinearity of the equations in order to alleviate the computation burden of solving TPBVP and sensitivity derivatives. These simplifications are explicitly applied to the equations and their effects on reducing the computation time while keeping the accuracy of the results are investigated through an example. Since the simplification applied on the sensitivity derivatives reduces the optimization time significantly while it has no effect on the overall process of the optimization (in contrast to both TPBVP and sensitivity derivatives simplification), owing to the fact that the constraints of the equations are ultimately applied in the last round of calculations, it is designated as the preferred procedure. The obtained numerical values for the integral square error and the percentage of reducing the computation time express the efficiency of the proposed method in a better way.
In the following, it is supposed that the initial conditions of the system may be prone to some changes. Instead of solving the issue from the scratch, the NE method is used to estimate the corresponding variations in the control inputs and system states based on the variations in the initial conditions. The NE method is employed alone or in intermittent combination with the exact solution of the NMPC-related optimal control problem at some predefined intermediate steps, in order to remediate from error accumulation. The results show that although using the NE method alone can reduce the computation time of the problem extensively, nonetheless it does not provide the required accuracy. On the other hand, using the combination of NE method together with intermittent exact solutions of TPBVP at some limited steps can effectively estimate the control inputs and system states related to the NMPC problem with altered initial conditions while still reducing the computation time to a considerable extent.
The methods proposed in this article could also be implemented on underactuated systems on SO(3), and systems evolving on SE(3). Moreover, experiments can be considered for taking implementation issues into account. All these issues will hopefully be addressed in our future works.
Authors contributions
SB, HK, and MK conceived and designed the model; SB and HK proposed the computational framework; SB performed simulations; SB, HK, and MK analyzed and interpreted the results; and SB and HK wrote the paper.
Financial support
There was no financial support.
Conflict of interest
The authors declare that they have no known competing financial interests or personal relationships that could have appeared to influence the work reported in this paper.
