Intell In Syst (15) 1:179 186 DOI 1.17/s493-15-1- ORIGINAL PAPER A New Nonlinear H-infinity Feeback Control Approach to the Problem of Autonomous Robot Navigation G. Rigatos 1 P. Siano Receive: 8 December 14 / Revise: 7 April 15 / Accepte: 19 July 15 / Publishe online: 16 September 15 Springer Science+Business Meia Singapore 15 Abstract This research work introuces a new control metho for feeback control of nonlinear ynamical systems with application eample the problem of trajectory tracking by an autonomous robotic vehicle. The control metho consists of a repetitive solution of an H-infinity control problem for the mobile robot, that makes use of a locally linearize moel of the robot an takes place at each iteration of the control algorithm. The vehicle s moel is locally linearize roun its current position through the computation of the associate Jacobian matrices. Using the linearize moel of the vehicle an H-infinity feeback control law is compute. The known robustness features of H-infinity control enable to compensate for the errors of the approimative linearization, as well as to eliminate the effects of eternal perturbations. The efficiency of the propose control scheme is shown analytically an is confirme through simulation eperiments. The metho can be applie to a wie class of nonlinear ynamical systems. Keywors Nonlinear H-infinity feeback control Robust control Autonomous robot Navigation B G. Rigatos grigat@ieee.org P. Siano psiano@unisa.it 1 Unit of Inustrial Automation, Inustrial Systems Institute, 654 Rion Patras, Greece Department of Inustrial Engineering, University of Salerno, 8484 Fisciano, Italy Introuction Nonlinear an embee control an autonomous navigation of robotic vehicles is of primary importance for the automotive inustry. By succeeing motion control of the vehicle, safety in riving can be improve while other several practical problems, such as lane keeping an maneuvering or parallel parking can be solve [1 8]. Up to now several research results have been evelope to enable the steering control an autonomous navigation of vehicles. The evelope approaches are base on nonlinear control, such as ifferential geometry an ifferential flatness theory approaches as well as on Lyapunov stability methos [9 15]. In this paper a new solution to the problem of autonomous vehicle navigation is given, using a linearization scheme together with H robust control theory [16]. The kinematic moel of a unicycle robotic vehicle is consiere as a case stuy, however the propose approach can be also applie to other types of vehicles (such as four wheel vehicles, heavy uty vehicles with implements, etc.). Actually the paper proposes the application of an approimate linearization scheme for the kinematic moel of the unicycle robotic vehicle that is base on Taylor series epansion roun the vehicle s current position. To perform this linearization the computation of Jacobian matrices is neee while the inuce linearization error terms are treate as isturbances. For the linearize equivalent of the robot s moel an H feeback control scheme is evelope. The formulation of the H control problem is base on the minimization of a quaratic cost function that comprises both the isturbance an the control input effects. The isturbance tries to maimize the cost function while the control signal tries to minimize it, within a mini-ma ifferential game. Comparing to nonlinear feeback control approaches which are base on eact feeback linearization (as the ones
18 Intell In Syst (15) 1:179 186 base on ifferential flatness theory an analyze in [16 18]) the propose H control scheme is assesse as follows: (i) it uses an approimative linearization approach of the system s ynamic or kinematic moel which oes not follow the elaborate transformations (iffeomorphisms) of the eact linearization methos, (ii) it introuces aitional isturbance error which is ue to the approimative linearization of the system ynamics coming from the application of Taylor series epansion, (iii) it requires the computation of Jacobian matrices, which in the case of systems of high imensionality can be also a cumbersome proceure (see inustrial robotic manipulators), (iv) unlike eact feeback linearization, the H control term has to compensate not only for moelling uncertainties an eternal isturbances but nees also to annihilate the effects of the cumulative linearization error, an (v) the H control approach follows optimal control methos for the computation of the control signal, however unlike eact feeback linearization control it requires the solution of Riccati equations which for systems of high imensionality can be again a cumbersome proceure. The structure of the paper is as follows: in Linearization of the Robot s Kinematic Moel section the approach for approimate linearization of the vehicle s kinematic moel is eplaine. In The Nonlinear H-infinity Control section the formulation of the H nonlinear control problem for the linearize equivalent of the system is provie. In Lyapunov Stability Analysis section Lyapunov stability analysis for the evelope control loop is given. In Simulation Tests section simulation tests are carrie out to assess the performance of the feeback control loop. Finally, in Conclusions section concluing remarks are state. Linearization of the Robot s Kinematic Moel Major Approaches in Control of Nonlinear Dynamical Systems Motion control of unmanne vehicles is a nonlinear control problem. One can istinguish three main approaches in the esign of nonlinear control systems: (i) control an filtering base on global linearization methos, (ii) control an filtering base on asymptotic linearization methos, an (iii) Lyapunov methos. As far as approach (i) is concerne, that is methos of global linearization, one can classify there methos for the transformation of nonlinear vehicles ynamics into equivalent linear state space form. For the linear equivalent forms of the vehicles ynamics one can esign feeback controllers an can solve the problem of state estimation (filtering). In this area one can consier methos base on ifferential flatness theory an methos base on Lie algebra. These approaches avoi approimation errors in moelling an arrive at controllers of increase precision an robustness. In this area one can also note research on a new nonlinear filtering metho (erivative-free nonlinear Kalman filter) which is more precise an computationally faster than other nonlinear estimation approaches. As far as approach (ii) is concerne, that is methos of asymptotic linearization, it is anticipate to continue research on robust an aaptive control with the use of a ecomposition of the vehicles ynamics into a set of linear local moels. One can pursue solutions to the problem of nonlinear control, base on local linear moels (roun linearization points). For such systems one can select the parameters of the local controllers by following linear feeback controller esign methos. These controllers succee asymptotically, that is in the course of, the compensation of the nonlinear system ynamics an the stabilization of the feeback control loops. In this thematic area one can note research on a new nonlinear H-infinity control metho, which is base on the local an approimative linearization of the vehicles ynamics an which makes use of the computation of Jacobian matrices. As far as approach (iii) is concerne, that is Lyapunov theory-base nonlinear control methos one comes against problems of minimization of the Lyapunov functions so as to compute control signals for nonlinear vehicle ynamics. For the evelopment of Lyapunov-type controllers one can either eploit a moel of the vehicles ynamics, or can avoi completely the use of such a moel as in the case of aaptive control. In the later case, the vehicles ynamics is completely unknown an can be approimate by aaptive algorithms which are suitably esigne so as to assure the stability an robustness of the control loop. The article s results serve the evelopment of the aforementione research irection (ii), that is nonlinear control base on approimate linearization methos. Linearization of the Mobile Robot Through Taylor Series Epansion A unicycle autonomous robotic vehicle is consiere. Its kinematic moel is given by ẋ cos(θ) ( ) v ẏ = sin(θ), (1) ω θ 1 where, y are the cartesian coorinates of the robot s center of gravity an θ is its orientation angle. Input v is the vehicle s linear velocity an ω is the vehicle s angular velocity for rotations roun its transversal ais. Consiering linearization of the moel roun the current position of the robot an roun the velocity value v(t T s ), where T s is the sampling perio,
Intell In Syst (15) 1:179 186 181 the Jacobian matrices of the robotic moel are: v(t T s ) sin(θ) A = v(t T s ) cos(θ), () cos(θ) B = sin(θ). (3) 1 The state vector of the robotic vehicle is enote as = [, y, θ] T while the input vector is enote as u =[v, w] T. After linearization roun its current position, the robot s kinematic moel is written as ẋ = A + Bu + 1. (4) Parameter 1 stans for the linearization error in the robot s kinematic moel appearing in Eq. (4). The esirable trajectory of the robot is enote by = [, y,θ ]. Tracking of this trajectory is succeee after applying the control input u. At every instant the control input u is assume to iffer from the control input u appearing in Eq. (4) by an amount equal to u, that is u = u + u ẋ = A + Bu +. (5) The ynamics of the controlle system escribe in Eq. (4) can be also written as ẋ = A + Bu + Bu Bu + 1, (6) an by enoting 3 = Bu + 1 as an aggregate isturbance term one obtains ẋ = A + Bu + Bu + 3. (7) By subtracting Eq. (5) from Eq. (7) one has ẋ ẋ = A ( ) + Bu + 3. (8) By enoting the tracking error as e = an the aggregate isturbance term as = 3, the tracking error ynamics becomes ė = Ae + Bu +. (9) The above linearize form of the robotic moel can be efficiently controlle after applying an H-infinity feeback control scheme. The Nonlinear H-infinity Control Mini-ma Control an Disturbance Rejection The initial nonlinear system is assume to be in the form ẋ = f (, u) R n, u R m. (1) Linearization of the system (autonomous vehicle) is performe at each iteration of the control algorithm roun its present operating point (, u ) = ((t), u(t T s )). The linearize equivalent of the system is escribe by ẋ = A + Bu + L R n, u R m, R q, (11) where matrices A an B are obtaine from the computation of the Jacobians f 1 f 1 1 f 1 n f f A = 1 f n (, u ), (1) f n f n 1 f n n f 1 f 1 f u 1 u 1 u m f f f B = u 1 u u m (, u ), (13) f n u m f n u 1 f n u an vector enotes isturbance terms ue to linearization errors. The problem of isturbance rejection for the linearize moel that is escribe by ẋ = A + Bu + L, y = C, (14) where R n, u R m, R q an y R p, cannot be hanle efficiently if the classical LQR control scheme is applie. This is because of the eistence of the perturbation term. The isturbance term apart from moeling (parametric) uncertainty an eternal perturbation terms can also represent noise terms of any istribution. In the H control approach, a feeback control scheme is esigne for trajectory tracking by the system s state vector an simultaneous isturbance rejection, consiering that the isturbance affects the system in the worst possible manner. The isturbances effects are incorporate in the following quaratic cost function: J(t) = 1 [ y T (t)y(t) ] + ru T (t)u(t) ρ T (t) (t) t, r, ρ>. (15)
18 Intell In Syst (15) 1:179 186 The significance of the negative sign in the cost function s term that is associate with the perturbation variable (t) is that the isturbance tries to maimize the cost function J(t) while the control signal u(t) tries to minimize it. The physical meaning of the relation given above is that the control signal an the isturbances compete to each other within a mini-ma ifferential game. This problem of mini-ma optimization can be written as min u ma J(u, ). (16) The objective of the optimization proceure is to compute a control signal u(t) which can compensate for the worst possible isturbance, that is eternally impose to the system. However, the solution to the mini-ma optimization problem is irectly relate to the value of the parameter ρ.this means that there is an upper boun in the isturbances magnitue that can be annihilate by the control signal. H-infinity Feeback Control For the linearize system given by Eq. (14) the cost function of Eq. (15) is efine, where the coefficient r etermines the penalization of the control input an the weight coefficient ρ etermines the rewar of the isturbances effects. It is assume that (i) the energy that is transferre from the isturbances signal (t) is boune, that is T (t) (t)t <. (17) (ii) The matrices [AB] an [AL] are stabilizable, (iii) the matri [AC] is etectable. Then, the optimal feeback control law is given by u(t) = K(t), (18) with K = 1 r BT P, (19) where P is a positive semi-efinite symmetric matri which is obtaine from the solution of the Riccati equation ( 1 A T P + PA+ Q P r BBT 1 ) ρ LLT P =, () where Q is also a positive efinite symmetric matri. The worst case isturbance is given by (t) = 1 ρ L T P(t). (1) The Role of Riccati Equation Coefficients in H Control Robustness The parameter ρ in Eq. (15), is an inication of the closeloop system robustness. If the values of ρ> are ecessively ecrease with respect to r, then the solution of the Riccati equation is no longer a positive efinite matri. Consequently there is a lower boun ρ min of ρ for which the H control problem has a solution. The acceptable values of ρ lie in the interval [ρ min, ). If ρ min is foun an use in the esign of the H controller, then the close-loop system will have increase robustness. Unlike this, if a value ρ>ρ min is use, then an amissible stabilizing H controller will be erive but it will be a suboptimal one. The Hamiltonian matri ( A H = Q ( 1r BB T 1 ) ) LL T ρ, () A T provies a criterion for the eistence of a solution of the Riccati equation Eq. (). A necessary conition for the solution of the algebraic Riccati equation to be a positive semi-efinite symmetric matri is that H has no imaginary eigenvalues [16]. Lyapunov Stability Analysis Through Lyapunov stability analysis it will be shown that the propose nonlinear control scheme assures H tracking performance, an that in case of boune isturbance terms asymptotic convergence to the reference setpoints is succeee. The tracking error ynamics for the robotic vehicle is writtenintheform ė = Ae + Bu + L, (3) where in the unicycle robot s application eample L = I R with I being the ientity matri. The following Lyapunov equation is consiere V = 1 et Pe, (4) where e = is the tracking error. By ifferentiating with respect to one obtains V = 1 ėt Pe + 1 epė V = 1 [Ae + Bu + L ] T P + 1 et P[Ae + Bu + L ], (5)
Intell In Syst (15) 1:179 186 183 V = 1 [ e T A T + u T B T + ] T L T Pe + 1 et P[Ae + Bu + L ], (6) V = 1 et A T Pe + 1 ut B T Pe + 1 T L T Pe + 1 et PAe+ 1 et PBu+ 1 et PL. (7) The previous equation is rewritten as V = 1 ( ) ( 1 et A T P + PA e + ( 1 + T L T Pe + 1 et PL ut B T Pe + 1 ) et PBu ). (8) Assumption For given positive efinite matri Q an coefficients r an ρ there eists a positive efinite matri P, which is the solution of the following matri equation ( 1 A T P + PA= Q + P r BBT 1 ) ρ LLT P. (9) Moreover, the following feeback control law is applie to the system u = 1 r BT Pe. (3) By substituting Eqs. (9) an (3) one obtains V = 1 ( 1 [ Q et + P r BBT 1 ) ] ρ LLT P e ( + e T PB 1 ) r BT Pe + e T PL, (31) V = 1 ( 1 et Qe + r PBBT Pe 1 ) ρ et PLL T Pe 1 r et PBB T Pe + e T PL, (3) which after intermeiate operations gives V = 1 et Qe 1 ρ et PLL T Pe + e T PL, (33) or, equivalently V = 1 et Qe 1 ρ et PLL T Pe + 1 et PL + 1 T L T Pe. (34) Lemma The following inequality hols 1 et L + 1 L T Pe 1 ρ et PLL T Pe 1 ρ T. (35) ( ) Proof The binomial ρα ρ 1 b is consiere. Epaning the left part of the above inequality one gets ρ a + 1 ρ b ab 1 ρ a + 1 ρ b ab ab 1 ρ b 1 ρ a 1 ab + 1 ab 1 ρ b 1 ρ a. (36) The following substitutions are carrie out: a = an b = e T PL an the previous relation becomes 1 T L T Pe + 1 et PL 1 ρ et PLL T Pe 1 ρ T. (37) Equation (37) is substitute in Eq. (34) an the inequality is enforce, thus giving V 1 et Qe + 1 ρ T. (38) Equation (38) shows that the H tracking performance criterion is satisfie. The integration of V from to T gives V (t)t 1 V (T ) + e Q t + 1 ρ t e Q t V () + ρ t. (39) Moreover, if there eists a positive constant M > such that t M, (4) then one gets e Q t V () + ρ M. (41) Thus, the integral e Qt is boune. Moreover, V (T ) is boune an from the efinition of the Lyapunov function V in Eq. (4) it becomes clear that e(t) will be also boune since e(t) e ={e e T Pe V () + ρ M }. Accoring to the above an with the use of Barbalat s Lemma one obtains lim t e(t) =. Simulation Tests The performance of the propose nonlinear H control scheme is teste is two eamples: (i) when the mobile robot tracks a reference trajectory, an (ii) when the mobile robot
184 Intell In Syst (15) 1:179 186 Fig. 1 a Plot of the circular y trajectory followe by the mobile robot, an b convergence of the robot s state variables 1 =, = y an 3 = θ to the associate reference setpoints y 6 4 1 1 5 5 5 1 15 5 4 3 3 5 5 1 15 1 6 6 4 4 6 (a) 1 5 1 15 (b) Fig. a Plot of the eight-shape y trajectory followe by the mobile robot, an b convergence of the robot s state variables 1 =, = y an 3 = θ to the associate reference setpoints y 8 6 4 1 1 1 1 1 3 4 5 1 4 6 8 8 6 4 4 6 8 (a) 3 3 1 1 3 4 5 5 5 1 3 4 5 (b) performs the automate parallel parking task. In both cases the performance of the propose controller was satisfactory, with minimum tracking error an fast convergence to the reference setpoints. In the computation of the reference path, the coorinates of the reference trajectory in the D-plane (, y ) have been use, while the esirable steering angle has been compute by θ = tan 1 (ẏ /ẋ ). The obtaine results are epicte in Figs. 1, an 3. The tracking performance of the control metho is shown in Tables 1 an. It can be observe that the tracking error for all state variables of the mobile robot was etremely small. Besies, in the simulation iagrams one can note the ecellent transient performance of the control algorithm, which means that convergence to the reference path was succeee in a smooth manner, while also avoiing overshoot an oscillations. Remark 1 The errors an isturbances that affect the propose control metho are as follows: (i) linearization errors ue to the truncation of higher orer terms in the Taylor series epansion of the vehicle s nonlinear moel, an (ii) eternal perturbations that may affect the vehicle s motion. H-infinity control aims at proviing maimum robustness to this kin of moeling errors an isturbances, an this is succeee through the appropriate selection of the attenuation coefficient ρ which appears in the associate Riccati equation. Actually, the minimum value of ρ for which there eists a solution for the algebraic Riccati equation (in the form of a positive efinite symmetric matri P) is the one that provies the control loop with maimum robustness. The above have been eplaine in The Role of Riccati Equation Coefficients in H Control Robustness section of manuscript. Remark The control metho that is presente in the article an which is base on nonlinear H-infinity control theory can be compare against global linearization methos, e.g., those base on ifferential flatness theory an Lyapunovbase methos (use by aaptive control schemes) [16 18]. As a general remark it can be state that the nonlinear H- infinity control, yet conceptually more simple than the other
Intell In Syst (15) 1:179 186 185 Fig. 3 a Plot of the y trajectory followe by the mobile robot in case of the parallel parking maneuver, an b convergence of the robot s state variables 1 =, = y an 3 = θ to the associate reference setpoints Y 16 14 1 1 8 6 4 1 1 3 3 1 4 6 8 1 5 4 6 8 1 4 3 15 1 5 5 X (a) 4 6 8 1 (b) Table 1 Tracking RMSE in the isturbance-free case RMSE RMSE y RMSE θ Path 1 36. 1 4 39. 1 4 1. 1 4 Path 7.9 1 4 1. 1 4.4 1 4 Table Tracking RMSE in motion uner isturbances RMSE RMSE y RMSE θ Path 1 37. 1 4 54. 1 4 13. 1 4 Path 14. 1 4 16. 1 4.41 1 4 two control approaches, is a reliable an efficient solution for the problem of autonomous vehicles control. About the computational buren of the propose control metho, this is somehow elevate with respect to the control base on linearizing iffeomorphisms. This is because the propose control metho requires the solution of a Riccati equation at each step of the control algorithm followe by the computation of the gains of the feeback controller. Unlike this, methos base on iffeomorphisms procee irectly to the computation of the feeback control gains since the preceing linearization transformation is performe offline. However, the iffeomorphisms-base linearization can be cumbersome an not an easy to conceive proceure. Therefore, the benefit from avoiing a certain amount of computations in the iffeomorphisms-base control may be lost if the iffeomorphisms-stage is mathematically emaning. Conclusions A new nonlinear feeback control metho has been evelope base on approimate linearization an the use of H control an stability theory. As a case stuy the problem of motion control of an autonomous robotic vehicle has been consiere. It has been shown that the propose control scheme enables the robot to track any type of reference path an to eecute elaborate tasks such as parallel parking. The first stage of the propose control metho is the linearization of the robot s kinematic moel using first orer Taylor series epansion an the computation of the associate Jacobian matrices. The errors ue to the approimative linearization have been consiere as isturbances that affect, together with eternal perturbations, the robot s moel. At a secon stage the implementation of H feeback control has been propose. Using the linearize moel of the vehicle an H-infinity feeback control law is compute at each iteration of the control algorithm, after previously solving an algebraic Riccati equation. The known robustness features of H-infinity control enable to compensate for the errors of the approimative linearization, as well as to eliminate the effects of eternal perturbations. The efficiency of the propose control scheme is shown analytically an is confirme through simulation eperiments. Comparing to other nonlinear control methos which are base on the eact linearization of the robot s moel it can be state that the propose H control uses the approimately linearize moel of the robotic vehicle without implementing elaborate state transformations (iffeomorphisms) that finally bring the system to a linear form. Of course the computation of Jacobian matrices an the nee to solve at each iteration of the algorithm a Riccati equation is also a computationally cumbersome proceure, especially for state-space moels of large imensionality. Moreover, this approimative linearization introuces aitional perturbation terms which the H controller has to eliminate. The continuous nee for compensation of such cumulative linearization errors brings the H controller closer to its robustness limits.
186 Intell In Syst (15) 1:179 186 References 1. Li, T.H.S., Yeh, Y.C., Wu, J.D., Hsiao, M.Y., Chen, C.Y.: Multifunctional intelligent autonomous parking controllers for carlike mobile robots. IEEE Trans. In. Electron. 57(5), 1687 1699 (1). Vepa, R.: Nonlinear unscente H suspension an tracking control of mobile vehicles. IEEE Trans. Veh. Technol. 61(4), 1543 1553 (1) 3. Rigatos, G., Siano, P.: An H-infinity feeback control approach to autonomous robot navigation. In: IEEE IECON 14, Dallas, Teas, October 14 4. Kanghyun, N., Fujimoto, M., Hori, Y.: Avance motion control of electric vehicles base on robust lateral tire force control via active front steering. IEEE/ASME Trans. Mechatron. 19(1), 89 99 (14) 5. Li, T.H.S., Chang, S.J., Chen, Y.X.: Implementation of human-like riving skills by autonomous fuzzy behavior control on an FPGAbase car-like mobile robot. IEEE Trans. In. Electron. 5(5), 867 88 (3) 6. Hwang, C.L., Chang, L.J., Yu, Y.S.: Network-base fuzzy ecentralize sliing-moe control for car-like mobile robots. IEEE Trans. In. Electron. 54(1), 574 585 (7) 7. Waa, M., Yoon, K., Hashimoto, H.: Development of avance parking assistance system. IEEE Trans. In. Electron. 5(1), 4 17 (3) 8. Hwang, C.L., Shih, C.-Y.: A istribute active-vision networkspace approach for the navigation of a car-like wheele robot. IEEE Trans. In. Electron. 56(3), 846 855 (9) 9. Yamaguchi, Y., Murakami, T.: Aaptive control for virtual steering characteristics on electric vehicle using steer-by-wire system. IEEE Trans. In. Electron. 56(5), 1585 1594 (9) 1. Chen, H., Ma, M.M., Wang, H., Liu, Z.Y., Cai, Z.X.: Moving horizon tracking H control of wheele mobile robots with actuator saturation. IEEE Trans. Control Syst. Technol. 17(), 449 457 (9) 11. Coelho, P., Nunes, U.: Path-following control of mobile robots in presence of uncertainties. IEEE Trans. Robot. 1(), 5 61 (5) 1. Sanchez-Solano, S., Cabrera, A.J., Baturone, I., Moreno-Velo, F.J., Bro, M.: FPGA implementation of embee fuzzy controllers for robotic applications. IEEE Trans. In. Electron. 54(4), 1937 1945 (7) 13. Baturone, I., Moreno-Velo, F.J., Blanco, V., Ferruz, J.: Design of embee DSP-base fuzzy controllers for autonomous mobile robots. IEEE Trans. In. Electron. 55(), 98 936 (8) 14. Möller, B., Deutscher, J., Groe, S.: Continuous curvature trajectory esign an feeforwar control for parking a car. IEEE Trans. Control Syst. Technol. 15(3), 541 553 (7) 15. Lee, T.C., Tsai, C.Y., Song, K.T.: Fast parking control of mobile robots: a motion planning approach with eperimental valiation. IEEE Trans. Control Syst. Technol. 1(5), 661 676 (4) 16. Rigatos, G.: Moelling an Control for Intelligent Inustrial Systems: Aaptive Algorithms in Robotics an Inustrial Engineering. Springer, Berlin (11) 17. Rigatos, G.: Avance Moels of Neural Networks: Nonlinear Dynamics an Stochasticity in Biological Neurons. Springer, Dorrecht (13) 18. Rigatos, G.: Nonlinear Control an Filtering Using Differential Flatness Theory Approaches: Applications to Electromechanical Systems. Springer, Switzerlan (15)