arXiv:1705.01332v1 [cs.SY] 3 May 2017  1  LiDAR-based Control of Autonomous Rotorcraft for the Inspection of Pier-like Structures: Proofs  Bruno J. Guerreiro, Carlos Silvestre, Rita Cunha, David Cabecinhas  Abstract —This is a complementary document to the paper presented in [1], where more detailed proofs are provided for some results. The main paper addresses the problem of trajectory tracking control of autonomous rotorcraft in operation scenarios where only relative position measurements obtained from LiDAR sensors are possible. The proposed approach defines an alterna- tive kinematic model, directly based on LiDAR measurements, and   uses   a   trajectory-dependent   error   space   to   express   the dynamic   model   of   the   vehicle.   An   LPV   representation   with piecewise affine dependence on the parameters is adopted to describe the error dynamics over a set of predefined operating regions, and a continuous-time   H 2   control problem is solved using LMIs and implemented within the scope of gain-scheduling control theory. In this document, Section A presents the stability analysis of the attitude inner-loop presented in [1, Section II.B], whereas Section B presents a more detailed version of the stability and performance guarantees for the LPV system, extending the results presented in [1, Section IV.A].  A PPENDIX   A I NNER - LOOP   D YNAMICS  This appendix presents a possible inner-loop stabilization method, within the framework of feedback linearization and Lyapunov stability methods, that results in a second-order linear model for the pitch and roll angular motion, as well as a first-order linear model for the yaw angular velocity.  Theorem 1   (Inner-loop Stability) .   Consider the control law given by  n ext   = S ( ω ω ω )   J B   ω ω ω   +   J B   Q − 1   ( λ λ λ )[  ̇ Q ( λ λ λ )   ω ω ω   −   K 2   (  ̇ λ λ λ   −   e 3   e T  3   u IL   )  −   Π Π Π T  e 3   K 1   Π Π Π e 3   ( λ λ λ   −   u IL   )]   (1)  where   K 1   ∈   R 2   and   K 2   ∈   R 3   are positive definite diagonal matrices and the inner-loop input vector is denoted as   u IL   =  [ u φ   u θ   u    ̇ ψ  ] T   , accounting for the desired roll angle, pitch  This work was partially funded by the Macau Science and Technology De- velopment Fund (FDCT), grant FDCT/048/2014/A1, by project MYRG2015- 00127-FST   of   the   Univ.   of   Macau,   by   the   European   Union’s   Horizon 2020 research and innovation programme under grant agreement No 731667 (MULTIDRONE), and by the Portuguese Foundation for Science and Tech- nology (FCT), under projects LARSyS UID/EEA/50009/2013 and LOTUS PTDC/EEI-AUT/5048/2014. The work of B. Guerreiro and R. Cunha were respectively supported by the FCT Post-doc Grant SFRH/BPD/110416/2015 and FCT Investigator Programme IF/00921/2013. This publication reflects the authors’ views only. The European Commission and other funding institutions are not responsible for any use that may be made of the information it contains. B.   Guerreiro   and   R.   Cunha   are   with   the   Institute   for   Systems and   Robotics   (ISR-Lisbon),   LARSyS,   Instituto   Superior   T ́ ecnico (IST),   Universidade   de   Lisboa,   Av.   Rovisco   Pais   1,   1049   Lisboa, Portugal.   ( bguerreiro@isr.tecnico.ulisboa.pt   and  rita@isr.tecnico.ulisboa.pt ) C. Silvestre and D. Cabecinhas are with the Department of Electrical and Computer Engineering, Faculty of Science and Technology, University of Macau, Taipa, Macau, China, and C. Silvestre is also on leave from IST, Portugal. ( csilvestre@umac.mo   and   dcabecinhas@umac.mo )  angle, and yaw angular rate, respectively. Then, the resulting attitude dynamics is given by   ̈ λ λ λ   =   − K 2   (  ̇ λ λ λ   −   e 3 e T  3   u IL   )   −   Π Π Π T  e 3   K 1   Π Π Π e 3   ( λ λ λ   −   u IL   )   .   (2)  which, for constant inputs, guarantees that the equilibrium point   (Π Π Π e 3   λ λ λ,    ̇ λ λ λ ) = (Π Π Π e 3   u IL   ,   e 3 e T  3   u IL   )   is exponentially stable. Proof.   Recalling the rigid body dynamics, the angular motion equations can be written as  {    ̇ ω ω ω   =   J − 1  B   [ − S ( ω ω ω )   J B ω ω ω   +   n ext   ]  ̇ λ λ λ   =   Q ( λ λ λ )   ω ω ω  (3a) (3b) Noting that the angular velocity can be defined as   ω ω ω   =  Q − 1 ( λ λ λ )  ̇ λ λ λ   and taking the time derivative of angular kinematics, the angular motion dynamics can be expressed as   ̈ λ λ λ   =  ̇ Q ( λ λ λ )   ω ω ω   +   Q ( λ λ λ )   J − 1  B   [ − S ( ω ω ω )   J B   ω ω ω   +   n ext   ]   (4) Consider the desired roll angle, pitch angle, and yaw rate to be denoted as   u φ   ∈   R ,   u θ   ∈   ( − π/ 2 , π/ 2) , and   u    ̇ ψ   ∈   R , respectively, indicating that they will be considered as inputs in the overall system. Let also the input vector of this inner- loop system be defined as   u IL   =   [ u φ   u θ   u    ̇ ψ  ] T   . Thus, a feedback linearizing control law can be designed as  n ext   = S ( ω ω ω )   J B   ω ω ω   +   J B   Q − 1 ( λ λ λ )[  ̇ Q ( λ λ λ )   ω ω ω   −   K 2   (  ̇ λ λ λ   −   e 3   e T  3   u IL   )  −   Π Π Π T  e 3   K 1   Π Π Π e 3   ( λ λ λ   −   u IL   )]   (5) where   K 1   ∈   R 2 × 2 ,   K 2   ∈   R 3 × 3 ,   e 3   =   [ 0   0   1 ] T   , and as such   e 3 e T  3   u IL   =   [ 0   0   u    ̇ ψ  ] T   , while   Π Π Π e 3   =   [ I 2   0 2 × 1  ]  such that   Π Π Π e 3   u IL   =   [ u φ   u θ  ] T   . Replacing the control law (5) in the angular dynamics (4), it can be seen that the closed- loop dynamics results in   ̈ λ λ λ   =   − K 2   (  ̇ λ λ λ   −   e 3   e T  3   u IL   )   −   Π Π Π T  e 3   K 1   Π Π Π e 3   ( λ λ λ   −   u IL   )   .   (6) Considering that   u IL   ∈ U IL   ⊂   R 3   and define a state vector as   x IL   =   [ Π Π Π e 3   λ λ λ    ̇ λ λ λ ] T   ∈ X IL   , the resulting system is a linear time-invariant (LTI) system of the form   ̇ x IL   =   A IL   x IL   +   B IL   u IL   ,  where the system matrices are given by  A IL   =  [   0 2 × 2   Π Π Π e 3  − Π Π Π T  e 3   K 1   − K 2  ]  ,  B IL   =  [   0 2 × 3  Π Π Π T  e 3   K 1   Π Π Π e 3   +   K 2   e 3   e T  3  ]  .  It can be seen that matrix   A IL   is Hurwitz for any positive definite matrices   K 1   and   K 2 , and, therefore, the system is locally input-to-state stable, noting that the local part of the
2  result is a direct consequence of the domain of the Euler angles not being   R 3 . Considering the matrices   K 1   = diag( k φ , k θ   )   and   K 2   = diag( k    ̇ φ , k    ̇ θ   , k    ̇ ψ   )   the   system described in   (6)   can also   be written as            ̈ φ   =   − k    ̇ φ    ̇ φ   −   k φ   ( φ   −   u φ )  ̈ θ   =   − k    ̇ θ    ̇ θ   −   k θ   ( θ   −   u θ )   .   ̈ ψ   =   − k    ̇ ψ   (  ̇ ψ   −   u    ̇ ψ   )  (7a) (7b) (7c) It can be seen that, with constant inputs and the change of variables    ̃ φ   =   φ   −   u φ ,    ̃ θ   =   θ   −   u θ , and    ̇  ̃ ψ   =    ̇ ψ   −   u    ̇ ψ   , the resulting autonomous system is exponentially stable. Thus, it can be concluded that the state variables   φ ,   θ , and    ̇ ψ  converge exponentially to the constant inputs   u φ ,   u θ   , and   u    ̇ ψ , respectively.  A PPENDIX   B C ONTROLLER   S YNTHESIS  In this section an LMI approach is used to tackle the continuous-time state   feedback   H 2   synthesis   problem   for polytopic LPV systems. Consider a general LPV system of the form   {    ̇ x   =   A ( ξ ξ ξ ) x   +   B w   ( ξ ξ ξ ) w   +   B ( ξ ξ ξ ) u z   =   C ( ξ ξ ξ ) x   +   D ( ξ ξ ξ ) w   +   E ( ξ ξ ξ ) u  (8a) (8b) where   x   is the state,   u   is the control input,   z   denotes the error signal to be controlled, and   w   denotes the exogenous input signal. The system is parameterized by   ξ ξ ξ , which is a possibly time-varying parameter vector and belongs to the convex set  E j   =   co ( E j  0   ) . Here, the operator co ( . )   denotes the convex hull of the elements of the argument set,   E j  0   =   { ξ ξ ξ 1 , . . . , ξ ξ ξ n j   } , where   ξ ξ ξ 1   to   ξ ξ ξ n j   are the vertices of a polytope. It is also noted that the controller synthesis presented in the following subsection will only be valid for a specific operating region, here represented by   E j   ⊂ E . Applying the static state feedback law given by   u   =   K x  to (8) results in the closed-loop system given by  T zw   ( ξ ξ ξ ) :=  {    ̇ x   =   A c ( ξ ξ ξ )   x   +   B c ( ξ ξ ξ )   w z   =   C c ( ξ ξ ξ )   x   +   D c ( ξ ξ ξ )   w  (9a) (9b) where   T zw   ( ξ ξ ξ )   denotes the resulting closed-loop operator from the disturbance input   w   to the performance output   z , and the system matrices are defined as   A c ( ξ ξ ξ ) =   A ( ξ ξ ξ ) +   B ( ξ ξ ξ )   K ,  B c ( ξ ξ ξ ) =   B w   ( ξ ξ ξ ) ,   C c ( ξ ξ ξ ) =   C ( ξ ξ ξ )+ E ( ξ ξ ξ )   K   and   D c ( ξ ξ ξ ) =   D ( ξ ξ ξ ) . The closed-loop system can be characterized in terms of quadratic stability using the following definition, where   X   ≻   0  denotes that matrix   X   is positive definite.  Definition 1   (Quadratic stability, [2]) .   The system is said to be quadratically stable if there exists a matrix   X   ≻   0   such that   A T c   ( ξ ξ ξ )   X   +   X A c ( ξ ξ ξ )   ≺   0   is satisfied for all   ξ ξ ξ   ∈ E j   .  It can be seen that testing for stability or solving the synthesis problem without any further result, involves an infinite number of LMIs. Thus, several different structures for LPV systems have been proposed which reduce the problem to that of solving a finite number of LMIs. In this section, an affine polytopic description is adopted, which can also be used to model a wide spectrum of systems and, as shown in the results presented in [ ? ], is an adequate choice for the system at hand.  Definition 2   (Affine polytopic LPV system) .   The system   (8)  is said to be a polytopic LPV system if the system matrix  P ( ξ ξ ξ ) =  [ A ( ξ ξ ξ )   B w   ( ξ ξ ξ )   B ( ξ ξ ξ )  C ( ξ ξ ξ )   D ( ξ ξ ξ )   E ( ξ ξ ξ )  ]  (10)  verifies   P ( ξ ξ ξ )   ∈   co   ( P 1 , . . . ,   P n r   )   for all   ξ ξ ξ   ∈ E j   , where  P i   =  [ A i   B w i   B i  C i   D i   E i  ]  for all   i   = 1 , . . . , n j   . Moreover, if   E j   is a polytopic set, such as  E j   = co( E j  0   ) ,   E j  0   =   { ξ ξ ξ 1 , . . . , ξ ξ ξ n j   } , and   P ( ξ ξ ξ )   depends affinely on   ξ ξ ξ , then   P i   =   P ( ξ ξ ξ i )   for all   i   = 1 , . . . , n j   , i.e., the vertices of the parameter set can be uniquely identified with the vertices of the system.  This   polytopic   structure   used   with   the   following   lemma, enables the use of a powerful set of results.  Proposition 2   ( [3, Proposition 1.19]) .   Let   f   :   E j   →   R   be a convex function defined on the convex set   E j   = co( E j  0   ) . Then, for some   γ   ∈   R ,   f   ( ξ ξ ξ )   ≤   γ   for all   ξ ξ ξ   ∈ E j   if and only if  f   ( ξ ξ ξ )   ≤   γ   for all   ξ ξ ξ   ∈ E j  0   .  Thus, the quadratic stability of an affine polytopic LPV system can be easily established if there exists a matrix   X   ≻   0   such that   A T c   ( ξ ξ ξ )   X   +   X A c ( ξ ξ ξ )   ≺   0   is satisfied for all   ξ ξ ξ   ∈ E j  0   . The   H 2   synthesis problem can be described as that of finding a control matrix   K   that stabilizes the closed-loop system and minimizes the   H 2 -norm of   T zw   ( ξ ξ ξ ) , denoted by  ‖ T zw   ( ξ ξ ξ ) ‖ H 2   . It is assumed that matrix   D ( ξ ξ ξ ) = 0   in order to guarantee that   ‖ T zw   ( ξ ξ ξ ) ‖ H 2   is finite for every internally sta- bilizing and strictly proper controller. The following theorem is used for controller design and relies on results available in [4] and [3], after being rewritten for the case of polytopic LPV systems. In the following,   tr ( . )   denotes the trace of the argument matrix.  Theorem 3   (Polytopic stability) .   If there are real matrices  X   =   X T   ≻   0 ,   Y   ≻   0 , and   W   such that  [ A ( ξ ξ ξ ) X + XA T ( ξ ξ ξ )+ B ( ξ ξ ξ ) W + W T   B T ( ξ ξ ξ )   B w   ( ξ ξ ξ )  B T w ( ξ ξ ξ )   − I  ]  ≺   0   (11a)  [   Y   C ( ξ ξ ξ )   X   +   E ( ξ ξ ξ )   W X C T   ( ξ ξ ξ ) +   W T   E T   ( ξ ξ ξ )   X  ]  ≻   0   (11b)  tr ( Y ) < γ 2   (11c)  for all   ξ ξ ξ   ∈ E j  0   , where the static feedback controller is defined as   K   =   W X − 1 , then, the closed-loop system is quadratically stable and there exists an upper-bound   γ   for the continuous- time   H 2 -norm of the closed-loop operator   T zw   ( ξ ξ ξ )   for all   ξ ξ ξ   ∈ E j   , i.e.,  ‖ T zw   ( ξ ξ ξ ) ‖ H 2   < γ ,   ∀ ξ ξ ξ   ∈ E j   .   (12)  Proof.   Using Proposition 2 and assuming an affine polytopic LPV system, it can be seen that satisfying the LMI system for all   ξ ξ ξ   ∈ E j  0   is equivalent to satisfying the same system for all
3  ξ ξ ξ   ∈ E j   . The proof that the LMI system (11) implies (12) can be obtained from the definition of   H 2 -norm of   T zw   ( ξ ξ ξ ) . Let the transfer function matrix of the closed-loop operator   T zw   ( ξ ξ ξ )   be denoted as   T i zw   ( s )   for some parameter vector   ξ ξ ξ i   ∈ E j  0   , and defined as  T i zw   ( s ) = =   C c ( ξ ξ ξ i ) [ s   I   −   A c ( ξ ξ ξ i )] − 1   B c ( ξ ξ ξ i ) +   D c ( ξ ξ ξ i ) = [ C ( ξ ξ ξ i ) +   E ( ξ ξ ξ i )   K ] [ s   I   −   A ( ξ ξ ξ i )   −   B ( ξ ξ ξ i )   K ] − 1   B w ( ξ ξ ξ i ) +   D ( ξ ξ ξ i )   .  Then, the   H 2 -norm of   T i zw   ( s )   is defined as  ‖ T i zw   ‖ 2  H 2   =   1  2   π   tr  (∫   + ∞ −∞  T H i zw   ( jω )   T i zw   ( jω )   dω  )  which using Parseval’s theorem can be rewritten as  ‖ T i zw   ‖ 2  H 2   = tr  (∫   + ∞  0  H T i   ( t )   H i ( t )   dt  )  where   H i ( t )   is the   impulse response matrix of   T i zw   ( s ) . Noting that the impulse response can be defined as  H i ( t ) =   C c ( ξ ξ ξ i )   e A c ( ξ ξ ξ i )   t   B c ( ξ ξ ξ i )   ,  it is possible to see that, after some algebraic manipulation,  ‖ T i zw   ‖ 2  H 2   = tr   ( C c ( ξ ξ ξ i )   W ctr ( ξ ξ ξ i )   C T c   ( ξ ξ ξ i ) )   ,  where  W ctr   ( ξ ξ ξ i ) =  ∫   + ∞  0  e A c ( ξ ξ ξ i )   t   B c ( ξ ξ ξ i )   B T c   ( ξ ξ ξ i )   e A T c   ( ξ ξ ξ i )   t   dt  stands for the controllability Grammian, given by the symmet- ric positive definite solution of the Lyapunov equation  A c ( ξ ξ ξ i )   W ctr   ( ξ ξ ξ i ) +   W ctr   ( ξ ξ ξ i )   A T c   ( ξ ξ ξ i ) +   B c ( ξ ξ ξ i )   B T c   ( ξ ξ ξ i ) =   0   .  It can be seen that, for each parameter vector   ξ ξ ξ i   ∈ E j  0   ,   γ   is an upper bound for the   H 2 -norm of the closed-loop operator  T i zw   if and only if there exists   X   ≻   0 ,   0   ≺   W ctr ( ξ ξ ξ i )   ≺   X  such that  A c ( ξ ξ ξ i )   X   +   X A c ( ξ ξ ξ i ) T   +   B c ( ξ ξ ξ i )   B T c   ( ξ ξ ξ i )   ≺   0   (13) and  tr   ( C c ( ξ ξ ξ i )   X C T c   ( ξ ξ ξ i ) )   < γ 2 .   (14) Equation (13) can be rewritten as  [ A c ( ξ ξ ξ i )   X   +   X A T c   ( ξ ξ ξ i ) +   B c ( ξ ξ ξ i )   B T c   ( ξ ξ ξ i )   0 0   − I  ]  ≺   0   ,  that, using Schur complements, becomes  [ A c ( ξ ξ ξ i )   X   +   X A T c   ( ξ ξ ξ i )   X B c ( ξ ξ ξ i )  B T c   ( ξ ξ ξ i )   X   − I  ]  ≺   0   ,  or equivalently, introducing the matrix   W   =   K X   yields    A ( ξ ξ ξ i ) X + XA T ( ξ ξ ξ i )+ B ( ξ ξ ξ i ) W + W T   B T ( ξ ξ ξ i )   B ( ξ ξ ξ i )  B T   ( ξ ξ ξ i )   − I    ≺ 0 .   (15) Under the conditions of the theorem, it can be seen that (15) is satisfied for all   ξ ξ ξ i   ∈ E j  0   and that (11b) can be written as  [   Y   C c ( ξ ξ ξ i )  C T c   ( ξ ξ ξ i )   X − 1  ]  ≻   0   ,  which, applying once again Schur complements, implies that  [ Y   −   C c ( ξ ξ ξ i )   X C T c   ( ξ ξ ξ i )   0 0   X − 1  ]  ≻   0  and consequently  tr   ( C c ( ξ ξ ξ i )   X C c ( ξ ξ ξ i ) T   )   <   tr ( Y )   < γ 2   ,  also, for all   ξ ξ ξ i   ∈ E j  0   . Thus, (14) is implied and the bound on the  H 2 -norm is established. As (13) is implied by the conditions of the theorem, it can be noted that  A c ( ξ ξ ξ i )   X   +   X A c ( ξ ξ ξ i ) T   ≺ − B c ( ξ ξ ξ i )   B T c   ( ξ ξ ξ i )      0   ,  for all   ξ ξ ξ i   ∈ E j  0   , implying the quadratic stability of the closed- loop and, thus, concluding the proof.  With this result, the optimal solution for the continuous-time  H 2   control problem is approximated through the minimization of   γ   subject to the LMIs of Theorem 3. R EFERENCES [1]   B. J. Guerreiro, C. Silvestre, R. Cunha, and D. Cabecinhas, “Lidar-based control of autonomous rotorcraft for the inspection of pier-like structures,”  IEEE Transactions in Control Systems Technology , 2017. [2]   S. Boyd, L. Ghaoui, E. Feron, and V. Balakrishnan,   Linear Matrix Inequality in System and Control Theory , ser. SIAM studies in applied mathematics.   Philadelphia,   PA: Society for Industrial and Applied Mathematics, SIAM, 1994, vol. 15. [3]   C. Scherer and S. Weiland, “Lecture notes on linear matrix inequality methods in control,” Dutch Institute of Systems and Control, 2000. [4]   L. Ghaoui and S.-I. Niculescu,   Advances in Linear Matrix Inequality Methods in Control .   Philadelphia, PA: Society for Industrial and Applied Mathematics, SIAM, 1999.