- Email: [email protected]
Parameter Sensitivity and Boundedness of Robotic Hybrid Periodic Orbits*
Preprints, 5th IFAC Conference on Analysis and Design of Hybrid Preprints, 5th IFAC Conference on Analysis and Design of Hybrid Preprints, 5th 5th IFAC IFAC Conference Conference on on Analysis Analysis and and Design Design of of Hybrid Hybrid Systems Preprints, Systems Available Systems October 2015. Georgia Tech, Atlanta, USAonline at www.sciencedirect.com Systems14-16, October 14-16, 2015. Georgia Tech, Atlanta, USA October October 14-16, 14-16, 2015. 2015. Georgia Georgia Tech, Tech, Atlanta, Atlanta, USA USA
ScienceDirect
IFAC-PapersOnLine 48-27 (2015) 377–382
Parameter Sensitivity and Boundedness of Parameter Sensitivity and Boundedness of Parameter Sensitivity and Boundedness of Robotic Hybrid Periodic Orbits Robotic Hybrid Periodic Orbits Robotic Hybrid Periodic Orbits ∗ ∗ Shishir Kolathaya ∗ Aaron D. Ames ∗ Shishir Kolathaya ∗ Aaron D. Ames ∗ ∗ ∗ Shishir Kolathaya Aaron D. Ames Shishir Kolathaya Aaron D. Ames ∗ ∗ Georgia Institute of Technology, Atlanta, GA 30332 Institute of Technology, Atlanta, GA 30332 ∗ ∗ Georgia Georgia of Atlanta, (e-mail: shishirny, [email protected]). Georgia Institute Institute of Technology, Technology, Atlanta, GA GA 30332 30332 (e-mail: shishirny, [email protected]). (e-mail: shishirny, shishirny, [email protected]). [email protected]). (e-mail:
USA USA USA USA
Abstract: Model-based Model-based nonlinear nonlinear controllers controllers like like feedback linearization linearization and and control control Lyapunov Lyapunov Abstract: Abstract: Model-based nonlinear controllers like feedback feedback linearization and control Lyapunov functions are are highly sensitive sensitive to the the model parameters parameters of the the robot. This This paper addresses the Abstract: Model-based nonlinear controllers like feedback linearization and control Lyapunov functions highly to model of robot. paper addresses the functions are highlythese sensitive to the the in model parameters of the robot. This paper paper addresses addresses the problem of realizing controllers a particular class of hybrid models–systems with impulse functions are highly sensitive to model parameters of the robot. This the problem of realizing these controllers in a particular class of hybrid models–systems with impulse problem of these in particular class of models–systems with effects–through a parameter parameter sensitivity measure. This measure quantifies the sensitivity sensitivity of aa problem of realizing realizing these controllers controllers in aa measure. particularThis classmeasure of hybrid hybridquantifies models–systems with impulse impulse effects–through a sensitivity the of effects–through a parameter sensitivity measure. This measure quantifies the sensitivity of aa given model-based controller to parameter uncertainty along a particular trajectory. By using effects–through a parameter sensitivity measure. This measure quantifies the sensitivity of given model-based controller to uncertainty along trajectory. By given model-based controller to parameter parameter uncertainty along aaa particular particular trajectory. By using using this measure, output boundedness of the controller (computed torque+PD) will be analyzed. given model-based controller to parameter uncertainty along particular trajectory. By using this measure, output boundedness of the (computed torque+PD) will be this measure, output boundedness ofcontrol the controller controller (computed torque+PD) willthese be analyzed. analyzed. Given outputsoutput that characterize characterize theof objectives, i.e., the the torque+PD) goal is is to to drive drive outputs this measure, boundedness the controller (computed will be analyzed. Given outputs that the control objectives, i.e., goal these outputs Given outputs that characterize the control objectives, i.e., the goal is to drive these outputs to zero, we consider Lyapunov functions obtained from these outputs. The main result of this this Given outputs that characterize the control objectives, i.e., the goal is to drive these outputs to zero, we consider Lyapunov functions obtained from these outputs. The main result of to zero, we consider Lyapunov functions obtained from these outputs. The main result of this paper establishes the ultimate boundedness of the output dynamics in terms of this measure to zero, we consider Lyapunov functions obtained from these outputs. The main result of this paper establishes the ultimate boundedness of dynamics in terms of paper establishes thefunctions ultimate under boundedness of the the output output dynamics inzero terms of this this measure measure via these these Lyapunov the assumption assumption of stable stable hybridin dynamics. This is is paper establishes the ultimate boundedness of the output dynamics terms of this measure via Lyapunov functions under the of hybrid zero dynamics. This via these Lyapunov functions under the assumption of stable hybrid zero dynamics. This is demonstrated in simulation on a 5-DOF underactuated bipedal robot. via these Lyapunov functions under the assumption of stable hybrid zero dynamics. This is demonstrated in simulation on a 5-DOF underactuated bipedal robot. demonstrated demonstrated in in simulation simulation on on a a 5-DOF 5-DOF underactuated underactuated bipedal bipedal robot. robot. © 2015, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Hybrid Zero Dynamics, Parameter Sensitivity Measure, System Identification. Identification. Keywords: Keywords: Hybrid Hybrid Zero Zero Dynamics, Dynamics, Parameter Parameter Sensitivity Sensitivity Measure, Measure, System System Identification. Identification. Keywords: Hybrid Zero Dynamics, Parameter Sensitivity Measure, System 1. INTRODUCTION 1. zz 1. INTRODUCTION INTRODUCTION 1. INTRODUCTION èèsh è zz èshshsh èènsk Model based based controllers controllers like like stochastic stochastic controllers controllers Byl Byl and and nsk èènsh Model nsk nsk Model based controllers likelinearization stochastic controllers controllers Byletand and Tedrakebased (2009), feedback Westervelt al. Model controllers like stochastic Byl ènsh nsh Tedrake (2009), feedback linearization Westervelt et al. nsh Tedrake (2009), feedback linearization Westervelt et al. (2007), the the controlfeedback Lyapunov functions (CLFs) (CLFs) Ameset et al. Tedrake (2009), linearization Westervelt (2007), control Lyapunov functions et (2007), therequire controlthe Lyapunov functions (CLFs) Ames Ames et al. al. (2014) all knowledge of an accurate dynamical (2007), the control Lyapunov functions (CLFs) Ames et al. (2014) all require the knowledge of dynamical èèsf èèsk (2014) allthe require the The knowledge of an anofaccurate accurate dynamical model of system. advantage these methods are (2014) all require the knowledge of an accurate dynamical sf è model of the system. The advantage of these methods are èsksksk sf sf model of the the system. The convergence advantage of offor these methods are that they they yield sufficient highly dynamic model of system. The advantage these methods are that yield sufficient convergence for highly dynamic x that they yield sufficient sufficient convergence for highly highly dynamic x robotic applications, e.g., convergence quadrotors and and bipedaldynamic robots, that they yield for robotic applications, e.g., quadrotors bipedal robots, x x robotic applications, e.g., quadrotors and bipedal robots, where exponential convergence of control objectives is used robotic applications, e.g., quadrotors and bipedal robots, where exponential convergence of control objectives is used where exponential convergence of control control objectives is used used to achieve achieve guaranteed stability of the objectives system. This This is where exponential convergence of is to guaranteed stability of system. is 1. The biped AMBER (left) and the stick figure of to achievetrue guaranteed stability of the thewhere system. This is Fig. Fig. 1. The biped AMBER (left) and the stick figure of especially of bipedal walking robots rapid expoto achieve guaranteed stability of the system. This is especially true of bipedal walking robots where rapid expoFig. 1. The biped AMBER (left) and the stick figure of AMBER showing the configuration angles (right). Fig. 1. The biped AMBER (left) and the stick figure of especially true of bipedal walking robots where rapid expoAMBER showing the configuration angles (right). nential convergence is used Ames et al. (2014). While these especially true of bipedal walking robots where rapid exponential convergence is used Ames et al. (2014). While these AMBER showing the configuration angles (right). AMBER showing the configuration angles (right). eter variations in the discrete event. The resulting overall nential convergence is used Ames et al. (2014). While these controllers have yielded good results when an accurate nential convergence is usedgood Amesresults et al. (2014). While these eter variations in the discrete event. The resulting overall controllers have when an accurate variations in discrete event. The resulting overall sensitivity measure represents given eter variations in the thethus discrete event.how The sensitive resulting a overall controllers have yielded yielded good results when an accurate eter dynamical model model is known, known, there is aa need need foran quantifying controllers have yielded good results when accurate sensitivity measure thus represents how sensitive a given dynamical is there is for quantifying sensitivity measure thus represents how sensitive a given controller is to parameter variations for hybrid systems. sensitivity measure thus represents how sensitive a given dynamical model is known, there is a need for quantifying how accurate the model has to be to realize the desired dynamical model is known, there is a need for quantifying controller is to parameter variations for hybrid systems. how accurate the model has to be to realize the desired controller is to parameter variations for hybrid systems. When described in terms of Lyapunov functions, which controller is to parameter variations for hybrid systems. how accurate the model has to be to realize the desired tracking error bounds. bounds. These domains point to When described in terms of Lyapunov functions, which how accurate the model has application to be to realize the point desired tracking error These domains to described in of functions, which are constructed the zeroing outputs of the robot, the When describedfrom in terms terms of Lyapunov Lyapunov functions, which tracking error bounds. These application application domains pointand to When the need need error for aa bounds. way to to measure measure parameterdomains uncertainty tracking These application point to are constructed from the zeroing outputs of the robot, the the for way parameter uncertainty and are constructed from the zeroing outputs of the robot, the parameter sensitivity measure naturally yields the ultimate constructed from the zeroing outputs of the robot, the the need for for aa way way to measure measure parameter uncertainty and are a methodology to design controllers for nonlinear hybrid the need to parameter uncertainty and parameter sensitivity measure naturally yields the ultimate aa methodology to design controllers for nonlinear hybrid parameter sensitivity measure naturally yields the ultimate bound on the outputs. Considering a 5-DOF bipedal robot, parameter sensitivity measure naturally yields the ultimate methodology to design controllers for nonlinear hybrid systems, like bipedal robots, that can converge to the a methodology to design controllers for nonlinear hybrid bound on the outputs. Considering a 5-DOF bipedal robot, systems, like robots, that can bound on on the the outputs. Considering 5-DOF bipedal robot, AMBER, shown in Fig. 1, where aa stable periodic orbit on outputs. Considering aa 5-DOF bipedal robot, systems, like bipedal bipedal robots, thatuncertainty. can converge converge to to the the bound control objective objective underrobots, parameter systems, like bipedal that can converge to the AMBER, shown in Fig. 1, where periodic orbit on control under parameter uncertainty. AMBER, shown in Fig. 1, where aa stable stable periodic orbit on the hybrid zero dynamics translates to a stable walking AMBER, shown in Fig. 1, where stable periodic orbit on control objective objective under under parameter parameter uncertainty. uncertainty. control the hybrid zero dynamics translates to a stable walking The goal of this paper is to establish a relationship between the hybrid zero dynamics translates to a stable walking gait on the bipedal robot, the ultimate bound on this the hybrid zero dynamics translates to a bound stable walking The goal of this paper is to establish a relationship between gait on the bipedal robot, the ultimate on this The goal of of uncertainty this paper paper is is and to establish establish relationship between parameter the output error bounds bounds on gait The goal this to aa relationship between gait on on the the bipedal robot, the ultimate ultimate bound on of thisa periodic orbit will be determined through the use bipedal robot, the bound on this parameter uncertainty and the error on orbit will be determined through the use of a parameter uncertainty and the output output error bounds on periodic systems with alternating continuous and discrete events, parameter uncertainty and the output error bounds on periodic orbit will be determined through the use of a particular controller: computed torque+PD. periodic orbit will be determined through the use of a systems with alternating continuous and discrete events, particular controller: computed torque+PD. systems withsystems, alternating continuous and discrete events, i.e., hybrid while considering a specific examsystems with alternating continuous and discrete events, particular controller: computed torque+PD. particular controller: computed torque+PD. i.e., hybrid systems, while considering a specific exampaper is structured in the following fashion: Section 2 i.e., hybrid systems, systems, while considering specific exam- The ple: bipedal bipedal walking while robots.considering Inspired by byaa the the sensitivity i.e., hybrid specific examThe paper is structured in the following fashion: Section 2 ple: walking robots. Inspired sensitivity The paper is structured in the following fashion: Section 2 introduces the robot model and the control methodology The paper is structured in the following fashion: Section 2 ple: bipedal walking robots. Inspired by the sensitivity functions utilized for linear systems Zhou et al. (1996), ple: bipedal walking robots. Inspired by the sensitivity introduces the robot model and the control methodology functions utilized for linear systems Zhou et al. (1996), introduces the robot model and the control methodology used–CLFs through the method of computed torque. Secthe robot model and the control methodology functions utilized for linear linear systems Zhou for et continuous al. (1996), (1996), introduces a parameter sensitivity measure is defined functions utilized for systems Zhou et al. used–CLFs through the of computed torque. Secaa parameter sensitivity measure is for continuous used–CLFs through the method method offor computed torque. Section 33 assesses the controller used the uncertain model through the method of computed torque. Secparameter sensitivity measurebetween is defined defined forboundedness continuous used–CLFs systems and sensitivity the relationship relationship thefor a parameter measure is defined continuous tion assesses the controller used for the uncertain model systems and the between the boundedness tion 3 assesses the controller used for the uncertain model of the robot and establishes the resulting uncertain behavtion 3 assesses the controller used for the uncertain model systems and the relationship between the boundedness and the the and measure is establishedbetween throughthe theboundedness use of LyaLya- of the robot and establishes the resulting uncertain behavsystems the is relationship and measure established through the use the robot and the uncertain behavior through functions. In Section 4, the resulting of the robotLyapunov and establishes establishes the resulting resulting uncertain behavand thefunctions. measure In is the established through the use of of along Lya- of punov context of hybrid systems, and the measure is established through the use of Lyaior through Lyapunov functions. In Section 4, the resulting punov functions. In the context of hybrid systems, along ior through Lyapunov functions. In Section 4, the resulting uncertain dynamics exhibited by the robot is measured ior through Lyapunov functions. In Section 4, the resulting punov functions. Inmeasure the context context of hybrid hybrid systems, along with defining the for the continuous event, an punov functions. In the of systems, along uncertain dynamics exhibited by the robot measured with defining the measure measure for for the event, an uncertainthrough dynamics exhibited by of theparameter robot is is sensitivity measured formally the construction dynamics exhibited by the robot is measured with defining the continuous continuous event, an uncertain impactdefining measurethe is defined defined tofor include the effect effect of of paramwith the measureto the continuous event, an formally through the construction of parameter sensitivity impact measure is include the paramformally through the construction of parameter sensitivity measure, which is the main formulation of this on formally through the construction of parameter sensitivity impact measure is defined to include the effect of paramimpact measure is defined to include the effect of param- measure, which is the main formulation of this paper paper on measure, which is the main formulation of this paper on This work is supported by the National Science Foundation which the formal results will build. It will be shown that measure, which is the main formulation of this paper on This work is supported by the National Science Foundation which the formal results will build. It will be shown that which the formal results will build. It will be shown that This work is supported by the National Science Foundation there is a direct relationship between the ultimate bound through grantsisCNS-0953823 andthe CNS-1136104. which the formal results will build. It will be shown that This work supported by National Science Foundation there through grants CNS-0953823 and CNS-1136104. there is is aaa direct direct relationship relationship between between the the ultimate ultimate bound bound through grants grants CNS-0953823 CNS-0953823 and and CNS-1136104. CNS-1136104. there is direct relationship between the ultimate bound through Copyright IFAC 2015 377 Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright © 2015, IFAC 2015 377 Copyright IFAC 2015 377 Peer review© of International Federation of Automatic Copyright ©under IFAC responsibility 2015 377Control. 10.1016/j.ifacol.2015.11.203
2015 IFAC ADHS 378 October 14-16, 2015. Atlanta, USA
Shishir Kolathaya et al. / IFAC-PapersOnLine 48-27 (2015) 377–382
on the Lyapunov function and the parameter sensitivity measure, which motivates the introduction of an auxiliary controller–computed torque+PD. This will be utilized for establishing bounds for the entire dynamics, under the assumption of a stable limit cycle in the zero dynamics. This method is extended to hybrid systems through the introduction of an impact measure in Section 5. Under the assumption that the hybrid zero dynamics is stable, the computed torque controller appended with the auxiliary input is applied on the model, which results in bounded dynamics of the underactuated hybrid system. The paper concludes with simulation results on a 5-DOF bipedal robot, AMBER, in Section 6. 2. ROBOT DYNAMICS AND CONTROL
−1 H1 0 D1 − ˙ , (4) µ J J q˙ where µ is a linear control input. The resulting torque controller that realizes this desired acceleration in the robot can be defined as: ˙ q˙ + G(q). (5) BT = D(q)¨ qd + C(q, q) Substituting (5) and (4) in (1) results in linear dynamics: y¨ = µ, with µ chosen through a CLF based controller. q¨d =
Zero Dynamics and CLF. If we define the vector: η = [y T , y˙ T ]T , the dynamics can be reformulated as: 0 0k×k 1k×k η + k×k µ, (6) η˙ = 0k×k 0k×k 1k×k F
A robotic model can be modeled as n-link manipulator. Given the configuration space Q ⊂ Rn , with the coordinates q ∈ Q, and the velocities q˙ ∈ Tq Q, the equation of motion of the n-DOF robot can be defined as: D(q)¨ q + C(q, q) ˙ q˙ + G(q) = BT, (1) n×n where D(q) ∈ R is the mass inertia matrix of the robot that includes the motor inertia terms, C(q, q) ˙ ∈ Rn×n is the matrix of coriolis and centrifugal forces, G(q) ∈ Rn is the gravity vector, T ∈ Rk is the torque input and B ∈ Rn×k is the mapping from torque to joints. AMBER. Considering the 5-DOF underactuated bipedal robot shown in Fig. 1, the configuration can be defined as: q = (qsa , qsk , qsh , qnsh , qnsk ) corresponding to stance ankle (sa), stance and non-stance knee (sk,nsk), stance and nonstance hip angles (sh,nsh) of the robot. Since the ankle is not actuated, the number of actuators is k = 4. Outputs. We will utilize the method of computed torque since it is widely used in robotic systems. It is also convenient in the context of uncertain models which will be considered in the next section. To realize the controller, outputs are picked which are functions of joint angles referred to as actual outputs ya : Q → Rk , which are made to track functions termed the desired outputs yd : Q → Rk . The objective is to drive the error y(q) = ya (q) − yd (q) → 0. These outputs are also termed virtual constraints in Westervelt et al. (2007). The outputs are picked such that they are relative degree two outputs (see Sastry (1999)). Given the output y: ∂y ∂2y y¨ = q¨ + q˙T 2 q. ˙ (2) ∂q ∂q J
J˙
Since, k < n, we include n−k rows to J and J˙ to make the co-efficient matrix of q¨ full rank. These rows correspond to the configuration which are underactuated resulting in: H1 0 D1 q¨ + ˙ , = (3) J y¨ J q˙ ˙ = C(q, q) ˙ q+G(q), ˙ and where H1 is the n−k rows of H(q, q) D1 is the n − k rows of the expression, D(q). It should be observed that since the underactuated degrees of freedom have zero torque being applied, the resulting EOM of the robot leads to 0 on the left hand side of (3), and hence the choice of rows. Accordingly, we can define the desired acceleration for the robot to be: 378
G
which represent the controllable dynamics of the system. Since, k < n there are states that are not directly controllable which represent the zero dynamics of the system and can be expressed as: z˙ = Ψ(η, z), (7)
where z ∈ Z ⊆ R2(n−k) is the zero dynamic coordinates of the system (see Westervelt et al. (2007)). Consider the Lyapunov Function: V (η) = η T P η, where P is the solution to the continuous-time algebraic Riccati equation (CARE). Taking the derivative yields: V˙ (η) = η T (F T P + P F )η + 2η T P Gµ. (8) To find a specific value of µ, we can utilize a minimum norm controller (see Freeman and Kokotovic (2008)) which minimizes µT µ subject to the inequality constraint: (9) V˙ = η T (F T P + P F )η + 2η T P Gµ ≤ −γV, where γ > 0 is a constant obtained from CARE. Satisfying (9) implies exponential convergence. We can impose stronger bounds on convergence by constructing a rapidly exponentially stable control Lyapunov function (RES-CLF) that can be used to stabilize the output dynamics in a rapidly exponentially fashion (see Ames et al. (2014) for more details). Choosing ε > 0: 1 1 I 0 I 0 T Vε (η) := η P ε η =: η T Pε η. ε 0 I 0 I It can be verified that this is a RES-CLF in Ames et al. (2014). Besides, the bounds on RES-CLF can be given as: α2 α1 ||η||2 ≤ Vε (η) ≤ 2 ||η||2 , (10) ε where α1 , α2 > 0 are the minimum and maximum eigenvalues of P , respectively. Differentiating (10) yields: V˙ ε (η) = LF Vε (η) + LG Vε (η)µ, (11) T T T where LF Vε (η) = η (F Pε + Pε F )η, LG Vε (η) = 2η Pε G. We can define a minimum norm controller which minimizes µT µ subject to the inequality constraint: γ (12) LF Vε (η) + LG Vε (η)µ ≤ − Vε (η), ε which when satisfied implies exponential convergence. Therefore, we can define a class of controllers Kε : γ Kε (η) = {u ∈ Rk : LF Vε (η) + LG Vε (η)u + Vε (η) ≤ 0}, ε which yields the set of control values that satisfies the desired convergence rate.
2015 IFAC ADHS October 14-16, 2015. Atlanta, USA
Shishir Kolathaya et al. / IFAC-PapersOnLine 48-27 (2015) 377–382
3. UNMODELED DYNAMICS Since the parameters are not perfectly known, the equation of motion, (1), computed with the given set of parameters will henceforth haveˆover the symbols. Therefore, Da , Ca , ˆ C, ˆ G ˆ Ga represent the actual model of the robot, and D, represent the assumed model of the robot. It is a well known fact that the inertial parameters of a robot are affine in the EOM (see Spong et al. (2006)). Therefore (1) can be restated as: Y(q, q, ˙ q¨)Θ = BT, (13) where Y(q, q, ˙ q¨) is the regressor Spong et al. (2006), and Θ is the set of base inertial parameters. Accordingly, Θa ˆ are the actual and the assumed set of base inertial and Θ parameters respectively. Computed Torque Redefined. The method of computed torque becomes very convenient to apply if the regressor and the inertial parameters are being computed simultaneously. If q¨d is the desired acceleration for the robot, the method of computed torque can be defined as: ˆ ˙ q¨d )Θ. (14) BTct = Y(q, q, For convenience, the mapping matrix B on the left hand side of (14) will be omitted i.e., BTct = Tct . Due to the difference in parameters, the dynamics will deviate from the nominal model, which is shown below: Lemma 1. Define: ˆ −1 Y(q, q, ˙ q¨), Φ=D
(15) ˆ which is a function of the estimated parameters, Θ. If the control law used is (4) combined with the computed torque (14), then the resulting dynamics of the robot evolve as: ˆ − Θa ). y¨ = µ + JΦ(Θ (16) Described in terms of η, we have the following: ˜ z˙ = Ψ(η, z), η˙ = F η + Gµ + GJΦΘ, (17) ˜ = 0, we could apply µ(η) ∈ Kε (η) ˜ =Θ ˆ − Θa . If Θ where Θ to drive η → 0. But since the parameters are uncertain, ˜ = 0, the resulting dynamics will be observed in the i.e., Θ derivative of the Lyapunov function, Vε , via (18) V˙ ε (η, µ) = η T (F T Pε + Pε F )η + 2η T Pε Gµ ˜ + 2η T Pε GJΦΘ, where η˙ is obtained via (17). The next section will establish the relationship between parameter uncertainty and the uncertain dynamics appearing in the CLF. 4. PARAMETER SENSITIVITY MEASURE Due to the unmodeled dynamics, applying the controller, µ(η) ∈ Kε (η), does not result in exponential convergence of the controller. The controller will still yield Global Uniform Ultimate Boundedness (GUUB) based on how the unmodeled dynamics affect V˙ ε . The parameter sensitivity measure, ν, that quantifies the ultimate bound on the Lyapunov function Vε is defined as: ˜ ν := Y(q, q, ˙ q¨)Θ.
(19)
˜ = Y(q, q, ˆ − It can be observed that: Y(q, q, ˙ q¨)Θ ˙ q¨)Θ Y(q, q, ˙ q¨)Θa , which is the difference between the actual 379
379
and the expected torque being applied on the robot. Therefore, the parameter sensitivity measure is effectively the difference in torques applied on the robot. Bounds on the Measure through RES-CLF. By (15) ˆ −1 ν = ΦΘ. ˜ Therefore, (18) can be and (19), we have: D expressed as: V˙ ε (η, µ) =η T (F T Pε + Pε F )η + 2η T Pε Gµ ˆ −1 ν, + 2η T Pε GJ D (20) which is now a function of ν. This provides an important connection with Lyapunov theory, and the notion of parameter sensitivity is motivated by this observation. In other words, if the path of least parameter sensitivity is followed, then the convergence of the Lyapunov function to a value very close to zero can be realized. Therefore by (20), the control input µ must be chosen such that ν is well within the bounds specified. If a suitable controller is applied: µ(η) ∈ Kε (η) , the stability of the Lyapunov function can be achieved as long as the following equation is satisfied: γ ˆ −1 ν ≤ 0, (21) V˙ ε ≤ − Vε + 2η T Pε GJ D ε Since the measure ν is a function of the control input µ, (21) has an algebraic loop. But, given the control input, it is possible to restrict the outputs η within a certain region. Therefore, we first assume the bounds on the inertia matrix as: ˆ ≤α α3 ≤ ||D|| ≤ α4 , α ˆ 3 ≤ ||D|| ˆ4, (22) where α3 , α4 , α ˆ3, α ˆ 4 are constants (see Mulero-Martinez (2007); From et al. (2010)). Since the outputs are degree one functions of q, the jacobian is bounded by the constant: ||J|| ≤ κ.
By varying γ, it can be shown that the controller exponentially drives the outputs to a ball of radius β. In particular, by considering γ1 > 0, γ2 > 0, which satisfy γ = γ1 +γ2 , we can rewrite γε Vε = γε1 Vε + γε2 Vε in (21). The first term can thus be used to cancel the uncertain dynamics and yield exponential convergence until ||η|| becomes sufficiently small. In other words, the outputs exponentially converge to an ultimate bound β as given by the following lemma. Lemma 2. Given the controllers µ(η) ∈ Kε (η), µ ¯(η) ∈ ¯ ε¯(η), and δ > 0, ∃ β > 0 such that whenever ||ν|| ≤ δ, K V˙ ε (η) < − γε2 Vε (η) ∀ Vε (η) > β. 4α2 κ2 δ 2
The ultimate bound is given by β = α1 αˆ22 γ 2 ε2 . A discussion 3 1 in this regard can be found in Dixon et al. (2004); Abdallah et al. (1991), where β is considered the uniform ultimate bound on the given controller. It must be noted that Lemma 2 yields a low convergence rate γ2 which is less than the original rate γ. The uncertain dynamics can be nullified separately by considering an auxiliary input µ ¯ satisfying: ˆ qd + C(q, ˆ q) ˆ BTctn = D(q)¨ ˙ q˙ + G(q) + Bµ ¯. (23) Note that this is not unique and other types of controllers can also be used. Computed torque with linear inputs appended have also been used in Slotine and Li (1987) in order to realize asymptotic convergence. The resulting dynamics of the outputs then reduces to:
2015 IFAC ADHS 380 October 14-16, 2015. Atlanta, USA
Shishir Kolathaya et al. / IFAC-PapersOnLine 48-27 (2015) 377–382
˜ + JD ˆ −1 B µ y¨ = µ + JΦΘ ¯.
(24)
Therefore, V˙ ε for the new input can be reformulated as: ¯) = η T (F T Pε + Pε F )η + 2η T Pε Gµ (25) V˙ ε (η, µ, µ T −1 ˆ + 2η Pε GJ D (ν + B µ ¯). ˆ −1 B. Consider the input µ ¯ = − 1ε¯ ΓT GT Pε η, where Γ = J D µ and µ ¯ together form the computed torque+PD control on the robot. The end result is a positive semidefinite expression: 1ε¯ η T Pε GΓΓT GT Pε η ≥ 0, which motives the construction of a positive semidefinite function: (26) V¯ε (η) = η T Pε GΓΓT GT Pε η =: η T P¯ε η. Using the property of positive semidefiniteness, we can establish new bounds on the outputs. Let N (P¯ε ) be the null space of the matrix P¯ε . If η ∈ N (P¯ε ), then V¯ε (η) = 0. Otherwise, for some α7 , α8 > 0: α8 α7 ||η||2 ≤ V¯ε (η) ≤ 4 ||η||2 . (27) ε Note that (27) can be used to restrict the uncertain dynamics in (25). Utilizing these constructions, we can define the following class of controllers: ¯ ε¯(η) = {u ∈ Rk : 2η T Pε GJ D ˆ −1 Bu + 1 V¯ε (η) ≤ 0}, K ε¯ Lemma 2 can now be redefined to obtain the new ultimate bound βη for the new control input (23). Lemma 3. Given the controllers µ(η) ∈ Kε (η), µ ¯(η) ∈ ¯ ε¯(η), and δ > 0, ∃ βη > 0 such that whenever ||ν|| ≤ δ, K V˙ ε (η) < − γε Vε (η) ∀ Vε (η) > βη . The resulting ultimate bound is: βη = α9 =
V¯ε Vε .
4α22 κ2 δ 2 ε¯2 , α7 α29 α ˆ 23 ε4
where
It can be inferred that: γ
Vε (η(t)) ≤ e− ε t Vε (η(0)) for Vε (η(0)) > βη , (28) 1 α2 − γ t or ||η(t)|| ≤ e 2ε ||η(0)|| for Vε (η(0)) > βη . ε α1 It must be noted that both ε, ε¯ affect ||ν||. We can now consider utlizing the boundedness properties in underactuated systems given that the zero dynamics of the robot has a locally exponentially stable periodic orbit. Zero Dynamics. Assume that there is an exponentially stable periodic orbit in the zero dynamics, denoted by Oz ⊂ Z. This means that there is a Lyapunov function Vz : Z → R≥0 such that in a neighborhood Br (Oz ) of Oz (see Hauser and Chung (1994)) it is exponentially stable. If O = ι(Oz ) ⊂ X ×Z is the periodic orbit of the full order dynamics through the canonical embedding ι : Z → X ×Z, then by defining the composite Lyapunov function: (29) Vc (η, z) = σVε (η) + Vz (z), we can introduce a theorem that shows ultimate boundedness of the entire dynamics of the robot to the periodic orbit O under parameter uncertainty. ¯(η) ∈ Theorem 1. Given the controllers µ(η) ∈ Kε (η), µ ¯ ε¯(η), and δ > 0, ∃ βη , βz > 0 such that whenever ||ν|| ≤ K δ, Vc (η, z) is exponentially convergent ∀ Vc (η, z) > βη +βz .
Proof of Theorem 1 is omitted due to space constraints but is an extension of Theorem 1 of Ames et al. (2014) which shows that by choosing a suitable σ, exponential convergence of Vc until the bound βη + βz can be achieved. 380
5. HYBRID DYNAMICS We now extend Theorem 1 to hybrid robotic systems which involve alternating phases of continuous and discrete dynamics. A hybrid system with a single continuous and a discrete event is defined as follows: ˜ η˙ = F η + Gµ + GJΦΘ, z˙ = Ψ(Θ; η, z), if (η, z) ∈ D\S (30) H = + − − η = ∆ (Θ, η , z ), η + z = ∆z (Θ, η − , z − ), if (η − , z − ) ∈ S It must be noted that the parameter vector Θ is included in zero dynamics, z, in (30), which is a reformulation of (7). D, S are the domain and switching surfaces and are given by: D = {(η, z) ∈ X × Z : h(η, z) ≥ 0}, (31) ˙ S = {(η, z) ∈ X × Z : h(η, z) = 0 and h(η, z) < 0},
for some continuously differentiable function h : X × Z → R. ∆(Θ, η − , z − ) = (∆η (Θ, η − , z − ), ∆z (Θ, η − , z − )) is the reset map representing the discrete dynamics of the system. For the bipedal robot, AMBER, h represents the non-stance foot height and ∆ represents the impact dynamics of the system. Plastic impacts are assumed. For (q − , q˙− ) ∈ S, being the pre-impact angles and velocities of the robot, the post impact velocity for the assumed model qˆ˙+ , and for the actual model q˙+ will be: ˆ −1 J T (J D ˆ −1 J T )−1 J )q˙− , qˆ˙+ = (I − D
ˆ − J T RD) ˜ qˆ˙+ − (I − J T R)D ˜ q˙− , q˙+ = Da−1 (D (32) where J is the jacobian of the end effector where the impulse forces from the ground are acting on the robot, and R = (J Da−1 J T )−1 J Da−1 . Given the hybrid system (30), denote the flow as ϕt (Θ; ∆(Θ, η − , z − )) with the initial condition (η − , z − ) ∈ S ∩ Z. Impact Measure. Using the impact model, measuring uncertainty of post-impact dynamics can be achieved by introducing an impact measure, νs , defined as follows: ˜ q˙− , νs := D(q) (33) It should be noted that the impact equations are Lipschitz continuous w.r.t. the impact measure νs . Accordingly, we have the following bounds on the impact map: ˆ 0, z − )|| ||∆η (Θa , η − , z − ) − ∆η (Θ, ˆ η− , z− ) ≤ ||∆η (Θa , η − , z − ) − ∆η (Θ, ˆ η − , z − ) − ∆η (Θ, ˆ 0, z − )|| + ∆η (Θ,
≤ L1 ||νs || + L2 ||η − ||, (34) where L1 , L2 are Lipschitz constants for ∆η . Similarly: ˆ 0, z − )|| ≤ L3 ||νs || + L4 ||η − ||, ||∆z (Θa , η − , z − ) − ∆z (Θ, where L3 , L4 are Lipschitz constants for ∆z . In order to obtain bounds on the output dynamics for hybrid periodic orbits, it is assumed that H has a hybrid zero dynamics ˆ of the robot. The hybrid zero for the assumed model, Θ, dynamics can be described as: ˆ 0, z) if z ∈ Z\(S ∩ Z) z˙ = Ψ(Θ; (35) H |Z = + ˆ 0, z − ) if z − ∈ (S ∩ Z) z = ∆Z (Θ,
ˆ 0, z − ) = 0, so that More specifically we assume that ∆η (Θ; the surface Z is invariant under the discrete dynamics. ˆ ∆(Θ, ˆ 0, z − )) Given (35), we can define the flow as ϕzt (Θ;
2015 IFAC ADHS October 14-16, 2015. Atlanta, USA
Shishir Kolathaya et al. / IFAC-PapersOnLine 48-27 (2015) 377–382
with the initial state (0, z − ) ∈ S ∩ Z. If a periodic orbit Oz exists in (35), then there exists a periodic ˆ ∆(Θ, ˆ 0, z ∗ )) of period T ∗ for the fixed point flow ϕzt (Θ; ∗ (0, z ). Through the canonical embedding, the corresponding periodic flow of the periodic orbit O in (30) will be ˆ ∆(Θ, ˆ 0, z ∗ )). Note that existence of periodic orbit ϕt (Θ; ˆ does not guarantee existence of for the assumed model Θ the periodic orbit for the actual model Θa . Therefore, we define the Poincar´e map P : S → S given by: P(Θ; η, z) = ϕT (Θ,η,z) (Θ; ∆(Θ, η, z)), (36) ˆ and T is the time to impact where Θ can be either Θa or Θ, function defined by: T (Θ; η, z) = inf{t ≥ 0 : ϕt (Θ; ∆(Θ, η, z)) ∈ S}. (37) It is shown in Ames et al. (2014) that T is Lipschitz continuous. Therefore, for constants α15 < 1 < α16 , α15 T ∗ ≤ T (Θ, η, z) ≤ α16 T ∗ . These constants depend on ˆ and how far (η, z) the deviation from the nominal model Θ is from the fixed point. The corresponding Poincar´e map ρ : S ∩ Z → S ∩ Z for the hybrid zero dynamics (35) is termed the restricted Poincar´e map: ˆ ∆(Θ, ˆ 0, z)), ρ(z) = ϕz (Θ; (38) Tρ (z)
where Tρ is the restricted time to impact function which ˆ 0, z). Without loss of generality, is given by Tρ (z) = T (Θ; ˆ = 0, z ∗ = 0. The following Lemma we can assume that Θ will introduce the relationship between time to impact, Poincar´e functions with η, νs . Lemma 4. Let Oz be the periodic orbit of the hybrid zero dynamics H |Z transverse to S ∩ Z for the nominal model ˆ Given the controllers µ(η) ∈ Kε (η), µ ¯ ε¯(η) for Θ. ¯(η) ∈ K the actual model Θa which render ultimate boundedness on Vε (η), (η, z) ∈ S∩Z such that Vε (∆η (Θ, η, z)) > βη for the given robot model Θ, and r > 0 such that (η, z) ∈ Br (0, 0), there exist finite constants A1 , A2 , A3 , A4 > 0 such that: ||T (Θa ; η, z) − Tρ (z)|| ≤ A1 ||η|| + A2 ||νs || ||P(Θa ; η, z) − ρ(z)|| ≤ A3 ||η|| + A4 ||νs ||
(39) (40)
Proof (Sketch). Let µ1 ∈ R2(n−k) , µ2 ∈ R2k be constant vectors. Define an auxiliary time to impact function: TB (µ1 , µ2 , z) = inf{t ≥ 0 : h(µ1 , ϕzt (0, ∆(0, 0, z)) + µ2 ) = 0}, which is Lipschitz continuous with the Lipschitz constant LB : ||TB (µ1 , µ2 , z) − Tρ (z)|| ≤ LB (||µ1 || + ||µ2 ||).
Let (η1 (t), z1 (t)) satisfy z˙1 = Ψ(Θa ; η1 (t), z1 (t)) with η1 (0) = ∆η (Θa , η, z) and z1 (0) = ∆z (Θa , η, z). Similarly let z2 (t) satisfy z˙2 (t) = Ψ(0; 0, z2 (t)) such that z2 (0) = ∆z (0, 0, z). The bounds on the ||η|| can now be given as ||η1 (0)|| = ||∆η (Θ, η, z) − ∆η (0, 0, z)|| ≤ L1 ||νs || + L2 ||η||, which is obtained through (34). Since Vε (η1 ) > βη , we use (59) of Lemma 1 of Ames et al. (2014) and (28) to obtain ||µ1 ||. Similartly, to obtain ||µ2 ||, we use the GronwallBellman argument from the proof of Lemma 1 of Ames et al. (2014) to get the following inequality: ||z1 (t) − z2 (t)|| ≤ (C2 ||η|| + C3 ||νs ||) eLq t , (41) where C2 , C3 , Lq are constants. Proof of (39) can now be obtained by substituting for ||µ1 ||, ||µ2 ||.
Similarly, the derivation of (54) of Lemma 1 of Ames et al. (2014) is used to obtain (40).
381
381
Main Theorem. We can now introduce the main theorem of the paper. Similar to the continuous dynamics, it is assumed that the periodic orbit OZ is exponentially stable in the hybrid zero dynamics. Theorem 2. Let Oz be an exponentially stable periodic orbit of the hybrid zero dynamics H |Z transverse to S ∩ Z ˆ Given the controllers µ(η) ∈ for the nominal model Θ. ¯ ε¯(η) for the hybrid system H . Given r > Kε (η), µ ¯(η) ∈ K 0 such that (η, z) ∈ Br (0, 0), there exist δ > 0, δs > 0 such that whevener ||ν|| < δ,||νs || < δs , the orbit O = ι(OZ ) is uniformly ultimately bounded by βη + βz . Proof (Sketch). Results of Lemma 4 and the exponential stability of OZ imply that there exists r > 0 such that ρ : Br (0) ∩ (S ∩ Z) → Br (0) ∩ (S ∩ Z) is well defined for all z ∈ Br (0) ∩ (S ∩ Z) and zk+1 = ρ(zk ) is locally exponentially stable, i.e., ||zk || ≤ N ξ k ||z0 || for some N > 0, 0 < ξ < 1 and all k ≥ 0. Therefore, by the converse Lyapunov theorem for discrete systems, there exists an exponentially convergent Lyapunov function Vρ , defined on Br (0) ∩ (S ∩ Z) for some r > 0 (possibly smaller than the previously defined r). It must be first ensured that the region βη must be within the bounds defined by r. For ||η|| = r, βη < Vε (η) is ensured through the following condition: βη < Vε (η) < αε22 ||η||2 < αε22 r2 . From Lemma 2 we have:
4α32 κ2 δ 2 α21 α23 γ12 ε4
<
α2 2 ε2 r
=⇒ δ <
α 1 α 3 γ1 ε 2 2α2 κ r.
For the RES-CLF Vε , denote its restriction to the switching surface by Vε,η = Vε |S , which is exponentially convergent. With these two Lyapunov functions we define the following candidate Lyapunov function on Br (0, 0) ∩ S: VP (η, z) = Vρ (z) + σVε,η (η) (42) The idea is to show that there exists a bounded region β into which the dynamics of the robot exponentially converge. Since the origin is an exponentially stable equilibrium for zk+1 = ρ(zk ), we have the following inequalities: ||Pz (Θa ; η, z)|| = ||Pz (Θa ; η, z) − ρ(z) + ρ(z) − ρ(0)|| ≤ A3 ||η|| + A4 ||νs || + Lρ ||z|| ||ρ(z)|| ≤ N ξ||z||, (43) where Lρ is the Lipschitz constant for ρ. Therefore: Vρ (Pz (Θa ; η, z)) − Vρ (ρ(z)) (44) ≤ α20 (A3 ||η|| + A4 ||νs ||) (A3 ||η|| + A4 ||νs || + (Lρ + N ξ)||z||), where α20 is equivalent to the constant r4 given in (61) of proof of Theorem 2 in Ames et al. (2014). It follows that: Vρ (Pz (Θa , η, z)) − Vρ (z) = Vρ (Pz (Θa ; η, z)) − Vρ (ρ(z)) +Vρ (ρ(z)) − Vρ (z). Combining the entire Lyapunov function we have: T ||η|| ||η|| ΛH ||z|| VP (P(Θa ; η, z)) − VP (η, z) ≤ − ||z|| ||νs || ||νs || where the symmetric matrix ΛH ∈ R3×3 , is obtained by collecting the constant terms. It can found in the derivation of Theorem 2 of Ames et al. (2014) that by choosing a suitable σ positive definiteness of ΛH can be established to render the discrete Lyapunov function VP exponentially convergent. The upper bound on the impact measure is obtained by applying the contraint on the above equation to ensure exponential convergence.
2015 IFAC ADHS 382 October 14-16, 2015. Atlanta, USA
Shishir Kolathaya et al. / IFAC-PapersOnLine 48-27 (2015) 377–382
6. SIMULATION RESULTS AND CONCLUSIONS In this section, we will invesigate how the uncertainty in parameters affects the stability of the controller applied to the 5-DOF bipedal robot AMBER shown in Fig. 1. The model, Θa , which has 61 parameters is picked such that ˆ the error is 30% compared to the assumed model Θ.
1.2
Non-stance knee
Stance knee
To realize walking on the robot, the actual and desired outputs are chosen as in Yadukumar et al. (2012) (specifically, see (6) for determining the actual and the desired outputs). The end result is outputs of the form y(q) = ya (q) − yd (q) which must be driven to zero. Therefore, the objective of the computed torque controller (5) combined with Kε for ¯ ε¯ for µ ˆ µ and K ¯ is to drive y → 0. For the nominal model Θ a stable walking gait is observed. In other words, a stable hybrid periodic orbit is observed for the assumed given model. Since, the actual model of the robot has an error of 30%, applying the controller yields the dynamics that evolves as shown in (24). The value of ε chosen was 1, and ε¯ was 2. Fig. 2 shows the comparison between actual and desired outputs, and Fig. 3 shows the Lyapunov function Vε . It can be observed that after every impact, Vε is thrown outside the ball defined by βη ≈ 0.04 and the controllers act to get back into βη before the next impact. 0.4
0.35
0.3
0.25
0.2 0
0.5
1
1.5
2
2.5
1.1 1 0.9 0.8 0.7 0.6 0.5 0.4 0.3
3
0
0.5
1
1.5
2
2.5
3
Time(s) 0.4
0.2
0.3
Torso angle
Non-stance slope
Time(s)
0.1
0
-0.1
-0.2
0.2 0.1 0 -0.1 -0.2 -0.3 -0.4
0
0.5
1
1.5
2
2.5
3
0
0.5
1
Time(s)
1.5
2
2.5
3
Time(s)
Fig. 2. Actual (blue) and desired (red) outputs as a function of time are shown here. Lyapunov Function 0.3
2.5
Measure (||ν||)
CLF (Vǫ )
0.25 0.2 0.15 0.1 0.05
2
1.5
1
0.5
0 −0.05 0
0.5
1
1.5
2
Time (s)
2.5
3
3.5
0 0
0.5
1
1.5
2
2.5
3
Time (s)
Fig. 3. The RES-CLF (left) and the measure (right) as a function of time are shown here. V˙ ε crosses 0 in every step, but the CLF is still seen to be ultimately bounded by βη . Conclusions. The concept of a measure for evaluating the robustness to parameter uncertainty of hybrid system models of robots was introduced. This relationship is created by introducing the formula for the parameter 382
sensitivity measure. The notion of measure is extended to hybrid systems (which includes discrete events) by considering the impact measure. This impact measure along with the sensitivity measure determine the output boundedness of the composite Lyapunov function for hybrid periodic orbits of the robot. This was then verified by realizing a stable walking gait on AMBER having a parameter error of 30%. It is important to observe that while the parameter sensitivity measure yields the difference between the actual and the predicted torque applied on the robot, the impact measure yields the impulsive ground reaction forces acting upon the robot during impacts. Future work will be devoted to evaluating the formal results of this paper experimentally. REFERENCES Abdallah, C., Dawson, D., Dorato, P., and Jamshidi, M. (1991). Survey of robust control for rigid robots. Control Systems, IEEE, 11(2), 24–30. doi:10.1109/37.67672. Ames, A.D., Galloway, K., Sreenath, K., and Grizzle, J.W. (2014). Rapidly exponentially stabilizing control lyapunov functions and hybrid zero dynamics. Automatic Control, IEEE Transactions on, 59(4), 876–891. Byl, K. and Tedrake, R. (2009). Metastable walking machines. The International Journal of Robotics Research, 28(8), 1040–1064. Dixon, W.E., Zergeroglu, E., and Dawson, D.M. (2004). Global robust output feedback tracking control of robot manipulators. Robotica, 22(04), 351–357. Freeman, R.A. and Kokotovic, P.V. (2008). Robust nonlinear control design: state-space and Lyapunov techniques. Springer. From, P.J., Schjølberg, I., Gravdahl, J.T., Pettersen, K.Y., and Fossen, T.I. (2010). On the boundedness and skew-symmetric properties of the inertia and coriolis matrices for vehicle-manipulator systems. In Intelligent Autonomous Vehicles, volume 7, 193–198. Hauser, J. and Chung, C.C. (1994). Converse lyapunov functions for exponentially stable periodic orbits. Systems & Control Letters, 23(1), 27–34. Mulero-Martinez, J. (2007). Uniform bounds of the coriolis/centripetal matrix of serial robot manipulators. Robotics, IEEE Transactions on, 23(5), 1083–1089. doi: 10.1109/TRO.2007.903820. Sastry, S.S. (1999). Nonlinear Systems: Analysis, Stability and Control. Springer, New York. Slotine, J.J.E. and Li, W. (1987). On the adaptive control of robot manipulators. The International Journal of Robotics Research, 6(3), 49–59. Spong, M.W., Hutchinson, S., and Vidyasagar, M. (2006). Robot modeling and control, volume 3. Wiley New York. Westervelt, E.R., Grizzle, J.W., Chevallereau, C., Choi, J.H., and Morris, B. (2007). Feedback Control of Dynamic Bipedal Robot Locomotion. CRC Press, Boca Raton. Yadukumar, S.N., Pasupuleti, M., and Ames, A.D. (2012). From formal methods to algorithmic implementation of human inspired control on bipedal robots. In Tenth International Workshop on the Algorithmic Foundations of Robotics (WAFR). Cambridge, MA. Zhou, K., Doyle, J.C., Glover, K., et al. (1996). Robust and optimal control, volume 40. Prentice Hall Upper Saddle River, NJ.
ScienceDirect
IFAC-PapersOnLine 48-27 (2015) 377–382
Parameter Sensitivity and Boundedness of Parameter Sensitivity and Boundedness of Parameter Sensitivity and Boundedness of Robotic Hybrid Periodic Orbits Robotic Hybrid Periodic Orbits Robotic Hybrid Periodic Orbits ∗ ∗ Shishir Kolathaya ∗ Aaron D. Ames ∗ Shishir Kolathaya ∗ Aaron D. Ames ∗ ∗ ∗ Shishir Kolathaya Aaron D. Ames Shishir Kolathaya Aaron D. Ames ∗ ∗ Georgia Institute of Technology, Atlanta, GA 30332 Institute of Technology, Atlanta, GA 30332 ∗ ∗ Georgia Georgia of Atlanta, (e-mail: shishirny, [email protected]). Georgia Institute Institute of Technology, Technology, Atlanta, GA GA 30332 30332 (e-mail: shishirny, [email protected]). (e-mail: shishirny, shishirny, [email protected]). [email protected]). (e-mail:
USA USA USA USA
Abstract: Model-based Model-based nonlinear nonlinear controllers controllers like like feedback linearization linearization and and control control Lyapunov Lyapunov Abstract: Abstract: Model-based nonlinear controllers like feedback feedback linearization and control Lyapunov functions are are highly sensitive sensitive to the the model parameters parameters of the the robot. This This paper addresses the Abstract: Model-based nonlinear controllers like feedback linearization and control Lyapunov functions highly to model of robot. paper addresses the functions are highlythese sensitive to the the in model parameters of the robot. This paper paper addresses addresses the problem of realizing controllers a particular class of hybrid models–systems with impulse functions are highly sensitive to model parameters of the robot. This the problem of realizing these controllers in a particular class of hybrid models–systems with impulse problem of these in particular class of models–systems with effects–through a parameter parameter sensitivity measure. This measure quantifies the sensitivity sensitivity of aa problem of realizing realizing these controllers controllers in aa measure. particularThis classmeasure of hybrid hybridquantifies models–systems with impulse impulse effects–through a sensitivity the of effects–through a parameter sensitivity measure. This measure quantifies the sensitivity of aa given model-based controller to parameter uncertainty along a particular trajectory. By using effects–through a parameter sensitivity measure. This measure quantifies the sensitivity of given model-based controller to uncertainty along trajectory. By given model-based controller to parameter parameter uncertainty along aaa particular particular trajectory. By using using this measure, output boundedness of the controller (computed torque+PD) will be analyzed. given model-based controller to parameter uncertainty along particular trajectory. By using this measure, output boundedness of the (computed torque+PD) will be this measure, output boundedness ofcontrol the controller controller (computed torque+PD) willthese be analyzed. analyzed. Given outputsoutput that characterize characterize theof objectives, i.e., the the torque+PD) goal is is to to drive drive outputs this measure, boundedness the controller (computed will be analyzed. Given outputs that the control objectives, i.e., goal these outputs Given outputs that characterize the control objectives, i.e., the goal is to drive these outputs to zero, we consider Lyapunov functions obtained from these outputs. The main result of this this Given outputs that characterize the control objectives, i.e., the goal is to drive these outputs to zero, we consider Lyapunov functions obtained from these outputs. The main result of to zero, we consider Lyapunov functions obtained from these outputs. The main result of this paper establishes the ultimate boundedness of the output dynamics in terms of this measure to zero, we consider Lyapunov functions obtained from these outputs. The main result of this paper establishes the ultimate boundedness of dynamics in terms of paper establishes thefunctions ultimate under boundedness of the the output output dynamics inzero terms of this this measure measure via these these Lyapunov the assumption assumption of stable stable hybridin dynamics. This is is paper establishes the ultimate boundedness of the output dynamics terms of this measure via Lyapunov functions under the of hybrid zero dynamics. This via these Lyapunov functions under the assumption of stable hybrid zero dynamics. This is demonstrated in simulation on a 5-DOF underactuated bipedal robot. via these Lyapunov functions under the assumption of stable hybrid zero dynamics. This is demonstrated in simulation on a 5-DOF underactuated bipedal robot. demonstrated demonstrated in in simulation simulation on on a a 5-DOF 5-DOF underactuated underactuated bipedal bipedal robot. robot. © 2015, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Hybrid Zero Dynamics, Parameter Sensitivity Measure, System Identification. Identification. Keywords: Keywords: Hybrid Hybrid Zero Zero Dynamics, Dynamics, Parameter Parameter Sensitivity Sensitivity Measure, Measure, System System Identification. Identification. Keywords: Hybrid Zero Dynamics, Parameter Sensitivity Measure, System 1. INTRODUCTION 1. zz 1. INTRODUCTION INTRODUCTION 1. INTRODUCTION èèsh è zz èshshsh èènsk Model based based controllers controllers like like stochastic stochastic controllers controllers Byl Byl and and nsk èènsh Model nsk nsk Model based controllers likelinearization stochastic controllers controllers Byletand and Tedrakebased (2009), feedback Westervelt al. Model controllers like stochastic Byl ènsh nsh Tedrake (2009), feedback linearization Westervelt et al. nsh Tedrake (2009), feedback linearization Westervelt et al. (2007), the the controlfeedback Lyapunov functions (CLFs) (CLFs) Ameset et al. Tedrake (2009), linearization Westervelt (2007), control Lyapunov functions et (2007), therequire controlthe Lyapunov functions (CLFs) Ames Ames et al. al. (2014) all knowledge of an accurate dynamical (2007), the control Lyapunov functions (CLFs) Ames et al. (2014) all require the knowledge of dynamical èèsf èèsk (2014) allthe require the The knowledge of an anofaccurate accurate dynamical model of system. advantage these methods are (2014) all require the knowledge of an accurate dynamical sf è model of the system. The advantage of these methods are èsksksk sf sf model of the the system. The convergence advantage of offor these methods are that they they yield sufficient highly dynamic model of system. The advantage these methods are that yield sufficient convergence for highly dynamic x that they yield sufficient sufficient convergence for highly highly dynamic x robotic applications, e.g., convergence quadrotors and and bipedaldynamic robots, that they yield for robotic applications, e.g., quadrotors bipedal robots, x x robotic applications, e.g., quadrotors and bipedal robots, where exponential convergence of control objectives is used robotic applications, e.g., quadrotors and bipedal robots, where exponential convergence of control objectives is used where exponential convergence of control control objectives is used used to achieve achieve guaranteed stability of the objectives system. This This is where exponential convergence of is to guaranteed stability of system. is 1. The biped AMBER (left) and the stick figure of to achievetrue guaranteed stability of the thewhere system. This is Fig. Fig. 1. The biped AMBER (left) and the stick figure of especially of bipedal walking robots rapid expoto achieve guaranteed stability of the system. This is especially true of bipedal walking robots where rapid expoFig. 1. The biped AMBER (left) and the stick figure of AMBER showing the configuration angles (right). Fig. 1. The biped AMBER (left) and the stick figure of especially true of bipedal walking robots where rapid expoAMBER showing the configuration angles (right). nential convergence is used Ames et al. (2014). While these especially true of bipedal walking robots where rapid exponential convergence is used Ames et al. (2014). While these AMBER showing the configuration angles (right). AMBER showing the configuration angles (right). eter variations in the discrete event. The resulting overall nential convergence is used Ames et al. (2014). While these controllers have yielded good results when an accurate nential convergence is usedgood Amesresults et al. (2014). While these eter variations in the discrete event. The resulting overall controllers have when an accurate variations in discrete event. The resulting overall sensitivity measure represents given eter variations in the thethus discrete event.how The sensitive resulting a overall controllers have yielded yielded good results when an accurate eter dynamical model model is known, known, there is aa need need foran quantifying controllers have yielded good results when accurate sensitivity measure thus represents how sensitive a given dynamical is there is for quantifying sensitivity measure thus represents how sensitive a given controller is to parameter variations for hybrid systems. sensitivity measure thus represents how sensitive a given dynamical model is known, there is a need for quantifying how accurate the model has to be to realize the desired dynamical model is known, there is a need for quantifying controller is to parameter variations for hybrid systems. how accurate the model has to be to realize the desired controller is to parameter variations for hybrid systems. When described in terms of Lyapunov functions, which controller is to parameter variations for hybrid systems. how accurate the model has to be to realize the desired tracking error bounds. bounds. These domains point to When described in terms of Lyapunov functions, which how accurate the model has application to be to realize the point desired tracking error These domains to described in of functions, which are constructed the zeroing outputs of the robot, the When describedfrom in terms terms of Lyapunov Lyapunov functions, which tracking error bounds. These application application domains pointand to When the need need error for aa bounds. way to to measure measure parameterdomains uncertainty tracking These application point to are constructed from the zeroing outputs of the robot, the the for way parameter uncertainty and are constructed from the zeroing outputs of the robot, the parameter sensitivity measure naturally yields the ultimate constructed from the zeroing outputs of the robot, the the need for for aa way way to measure measure parameter uncertainty and are a methodology to design controllers for nonlinear hybrid the need to parameter uncertainty and parameter sensitivity measure naturally yields the ultimate aa methodology to design controllers for nonlinear hybrid parameter sensitivity measure naturally yields the ultimate bound on the outputs. Considering a 5-DOF bipedal robot, parameter sensitivity measure naturally yields the ultimate methodology to design controllers for nonlinear hybrid systems, like bipedal robots, that can converge to the a methodology to design controllers for nonlinear hybrid bound on the outputs. Considering a 5-DOF bipedal robot, systems, like robots, that can bound on on the the outputs. Considering 5-DOF bipedal robot, AMBER, shown in Fig. 1, where aa stable periodic orbit on outputs. Considering aa 5-DOF bipedal robot, systems, like bipedal bipedal robots, thatuncertainty. can converge converge to to the the bound control objective objective underrobots, parameter systems, like bipedal that can converge to the AMBER, shown in Fig. 1, where periodic orbit on control under parameter uncertainty. AMBER, shown in Fig. 1, where aa stable stable periodic orbit on the hybrid zero dynamics translates to a stable walking AMBER, shown in Fig. 1, where stable periodic orbit on control objective objective under under parameter parameter uncertainty. uncertainty. control the hybrid zero dynamics translates to a stable walking The goal of this paper is to establish a relationship between the hybrid zero dynamics translates to a stable walking gait on the bipedal robot, the ultimate bound on this the hybrid zero dynamics translates to a bound stable walking The goal of this paper is to establish a relationship between gait on the bipedal robot, the ultimate on this The goal of of uncertainty this paper paper is is and to establish establish relationship between parameter the output error bounds bounds on gait The goal this to aa relationship between gait on on the the bipedal robot, the ultimate ultimate bound on of thisa periodic orbit will be determined through the use bipedal robot, the bound on this parameter uncertainty and the error on orbit will be determined through the use of a parameter uncertainty and the output output error bounds on periodic systems with alternating continuous and discrete events, parameter uncertainty and the output error bounds on periodic orbit will be determined through the use of a particular controller: computed torque+PD. periodic orbit will be determined through the use of a systems with alternating continuous and discrete events, particular controller: computed torque+PD. systems withsystems, alternating continuous and discrete events, i.e., hybrid while considering a specific examsystems with alternating continuous and discrete events, particular controller: computed torque+PD. particular controller: computed torque+PD. i.e., hybrid systems, while considering a specific exampaper is structured in the following fashion: Section 2 i.e., hybrid systems, systems, while considering specific exam- The ple: bipedal bipedal walking while robots.considering Inspired by byaa the the sensitivity i.e., hybrid specific examThe paper is structured in the following fashion: Section 2 ple: walking robots. Inspired sensitivity The paper is structured in the following fashion: Section 2 introduces the robot model and the control methodology The paper is structured in the following fashion: Section 2 ple: bipedal walking robots. Inspired by the sensitivity functions utilized for linear systems Zhou et al. (1996), ple: bipedal walking robots. Inspired by the sensitivity introduces the robot model and the control methodology functions utilized for linear systems Zhou et al. (1996), introduces the robot model and the control methodology used–CLFs through the method of computed torque. Secthe robot model and the control methodology functions utilized for linear linear systems Zhou for et continuous al. (1996), (1996), introduces a parameter sensitivity measure is defined functions utilized for systems Zhou et al. used–CLFs through the of computed torque. Secaa parameter sensitivity measure is for continuous used–CLFs through the method method offor computed torque. Section 33 assesses the controller used the uncertain model through the method of computed torque. Secparameter sensitivity measurebetween is defined defined forboundedness continuous used–CLFs systems and sensitivity the relationship relationship thefor a parameter measure is defined continuous tion assesses the controller used for the uncertain model systems and the between the boundedness tion 3 assesses the controller used for the uncertain model of the robot and establishes the resulting uncertain behavtion 3 assesses the controller used for the uncertain model systems and the relationship between the boundedness and the the and measure is establishedbetween throughthe theboundedness use of LyaLya- of the robot and establishes the resulting uncertain behavsystems the is relationship and measure established through the use the robot and the uncertain behavior through functions. In Section 4, the resulting of the robotLyapunov and establishes establishes the resulting resulting uncertain behavand thefunctions. measure In is the established through the use of of along Lya- of punov context of hybrid systems, and the measure is established through the use of Lyaior through Lyapunov functions. In Section 4, the resulting punov functions. In the context of hybrid systems, along ior through Lyapunov functions. In Section 4, the resulting uncertain dynamics exhibited by the robot is measured ior through Lyapunov functions. In Section 4, the resulting punov functions. Inmeasure the context context of hybrid hybrid systems, along with defining the for the continuous event, an punov functions. In the of systems, along uncertain dynamics exhibited by the robot measured with defining the measure measure for for the event, an uncertainthrough dynamics exhibited by of theparameter robot is is sensitivity measured formally the construction dynamics exhibited by the robot is measured with defining the continuous continuous event, an uncertain impactdefining measurethe is defined defined tofor include the effect effect of of paramwith the measureto the continuous event, an formally through the construction of parameter sensitivity impact measure is include the paramformally through the construction of parameter sensitivity measure, which is the main formulation of this on formally through the construction of parameter sensitivity impact measure is defined to include the effect of paramimpact measure is defined to include the effect of param- measure, which is the main formulation of this paper paper on measure, which is the main formulation of this paper on This work is supported by the National Science Foundation which the formal results will build. It will be shown that measure, which is the main formulation of this paper on This work is supported by the National Science Foundation which the formal results will build. It will be shown that which the formal results will build. It will be shown that This work is supported by the National Science Foundation there is a direct relationship between the ultimate bound through grantsisCNS-0953823 andthe CNS-1136104. which the formal results will build. It will be shown that This work supported by National Science Foundation there through grants CNS-0953823 and CNS-1136104. there is is aaa direct direct relationship relationship between between the the ultimate ultimate bound bound through grants grants CNS-0953823 CNS-0953823 and and CNS-1136104. CNS-1136104. there is direct relationship between the ultimate bound through Copyright IFAC 2015 377 Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright © 2015, IFAC 2015 377 Copyright IFAC 2015 377 Peer review© of International Federation of Automatic Copyright ©under IFAC responsibility 2015 377Control. 10.1016/j.ifacol.2015.11.203
2015 IFAC ADHS 378 October 14-16, 2015. Atlanta, USA
Shishir Kolathaya et al. / IFAC-PapersOnLine 48-27 (2015) 377–382
on the Lyapunov function and the parameter sensitivity measure, which motivates the introduction of an auxiliary controller–computed torque+PD. This will be utilized for establishing bounds for the entire dynamics, under the assumption of a stable limit cycle in the zero dynamics. This method is extended to hybrid systems through the introduction of an impact measure in Section 5. Under the assumption that the hybrid zero dynamics is stable, the computed torque controller appended with the auxiliary input is applied on the model, which results in bounded dynamics of the underactuated hybrid system. The paper concludes with simulation results on a 5-DOF bipedal robot, AMBER, in Section 6. 2. ROBOT DYNAMICS AND CONTROL
−1 H1 0 D1 − ˙ , (4) µ J J q˙ where µ is a linear control input. The resulting torque controller that realizes this desired acceleration in the robot can be defined as: ˙ q˙ + G(q). (5) BT = D(q)¨ qd + C(q, q) Substituting (5) and (4) in (1) results in linear dynamics: y¨ = µ, with µ chosen through a CLF based controller. q¨d =
Zero Dynamics and CLF. If we define the vector: η = [y T , y˙ T ]T , the dynamics can be reformulated as: 0 0k×k 1k×k η + k×k µ, (6) η˙ = 0k×k 0k×k 1k×k F
A robotic model can be modeled as n-link manipulator. Given the configuration space Q ⊂ Rn , with the coordinates q ∈ Q, and the velocities q˙ ∈ Tq Q, the equation of motion of the n-DOF robot can be defined as: D(q)¨ q + C(q, q) ˙ q˙ + G(q) = BT, (1) n×n where D(q) ∈ R is the mass inertia matrix of the robot that includes the motor inertia terms, C(q, q) ˙ ∈ Rn×n is the matrix of coriolis and centrifugal forces, G(q) ∈ Rn is the gravity vector, T ∈ Rk is the torque input and B ∈ Rn×k is the mapping from torque to joints. AMBER. Considering the 5-DOF underactuated bipedal robot shown in Fig. 1, the configuration can be defined as: q = (qsa , qsk , qsh , qnsh , qnsk ) corresponding to stance ankle (sa), stance and non-stance knee (sk,nsk), stance and nonstance hip angles (sh,nsh) of the robot. Since the ankle is not actuated, the number of actuators is k = 4. Outputs. We will utilize the method of computed torque since it is widely used in robotic systems. It is also convenient in the context of uncertain models which will be considered in the next section. To realize the controller, outputs are picked which are functions of joint angles referred to as actual outputs ya : Q → Rk , which are made to track functions termed the desired outputs yd : Q → Rk . The objective is to drive the error y(q) = ya (q) − yd (q) → 0. These outputs are also termed virtual constraints in Westervelt et al. (2007). The outputs are picked such that they are relative degree two outputs (see Sastry (1999)). Given the output y: ∂y ∂2y y¨ = q¨ + q˙T 2 q. ˙ (2) ∂q ∂q J
J˙
Since, k < n, we include n−k rows to J and J˙ to make the co-efficient matrix of q¨ full rank. These rows correspond to the configuration which are underactuated resulting in: H1 0 D1 q¨ + ˙ , = (3) J y¨ J q˙ ˙ = C(q, q) ˙ q+G(q), ˙ and where H1 is the n−k rows of H(q, q) D1 is the n − k rows of the expression, D(q). It should be observed that since the underactuated degrees of freedom have zero torque being applied, the resulting EOM of the robot leads to 0 on the left hand side of (3), and hence the choice of rows. Accordingly, we can define the desired acceleration for the robot to be: 378
G
which represent the controllable dynamics of the system. Since, k < n there are states that are not directly controllable which represent the zero dynamics of the system and can be expressed as: z˙ = Ψ(η, z), (7)
where z ∈ Z ⊆ R2(n−k) is the zero dynamic coordinates of the system (see Westervelt et al. (2007)). Consider the Lyapunov Function: V (η) = η T P η, where P is the solution to the continuous-time algebraic Riccati equation (CARE). Taking the derivative yields: V˙ (η) = η T (F T P + P F )η + 2η T P Gµ. (8) To find a specific value of µ, we can utilize a minimum norm controller (see Freeman and Kokotovic (2008)) which minimizes µT µ subject to the inequality constraint: (9) V˙ = η T (F T P + P F )η + 2η T P Gµ ≤ −γV, where γ > 0 is a constant obtained from CARE. Satisfying (9) implies exponential convergence. We can impose stronger bounds on convergence by constructing a rapidly exponentially stable control Lyapunov function (RES-CLF) that can be used to stabilize the output dynamics in a rapidly exponentially fashion (see Ames et al. (2014) for more details). Choosing ε > 0: 1 1 I 0 I 0 T Vε (η) := η P ε η =: η T Pε η. ε 0 I 0 I It can be verified that this is a RES-CLF in Ames et al. (2014). Besides, the bounds on RES-CLF can be given as: α2 α1 ||η||2 ≤ Vε (η) ≤ 2 ||η||2 , (10) ε where α1 , α2 > 0 are the minimum and maximum eigenvalues of P , respectively. Differentiating (10) yields: V˙ ε (η) = LF Vε (η) + LG Vε (η)µ, (11) T T T where LF Vε (η) = η (F Pε + Pε F )η, LG Vε (η) = 2η Pε G. We can define a minimum norm controller which minimizes µT µ subject to the inequality constraint: γ (12) LF Vε (η) + LG Vε (η)µ ≤ − Vε (η), ε which when satisfied implies exponential convergence. Therefore, we can define a class of controllers Kε : γ Kε (η) = {u ∈ Rk : LF Vε (η) + LG Vε (η)u + Vε (η) ≤ 0}, ε which yields the set of control values that satisfies the desired convergence rate.
2015 IFAC ADHS October 14-16, 2015. Atlanta, USA
Shishir Kolathaya et al. / IFAC-PapersOnLine 48-27 (2015) 377–382
3. UNMODELED DYNAMICS Since the parameters are not perfectly known, the equation of motion, (1), computed with the given set of parameters will henceforth haveˆover the symbols. Therefore, Da , Ca , ˆ C, ˆ G ˆ Ga represent the actual model of the robot, and D, represent the assumed model of the robot. It is a well known fact that the inertial parameters of a robot are affine in the EOM (see Spong et al. (2006)). Therefore (1) can be restated as: Y(q, q, ˙ q¨)Θ = BT, (13) where Y(q, q, ˙ q¨) is the regressor Spong et al. (2006), and Θ is the set of base inertial parameters. Accordingly, Θa ˆ are the actual and the assumed set of base inertial and Θ parameters respectively. Computed Torque Redefined. The method of computed torque becomes very convenient to apply if the regressor and the inertial parameters are being computed simultaneously. If q¨d is the desired acceleration for the robot, the method of computed torque can be defined as: ˆ ˙ q¨d )Θ. (14) BTct = Y(q, q, For convenience, the mapping matrix B on the left hand side of (14) will be omitted i.e., BTct = Tct . Due to the difference in parameters, the dynamics will deviate from the nominal model, which is shown below: Lemma 1. Define: ˆ −1 Y(q, q, ˙ q¨), Φ=D
(15) ˆ which is a function of the estimated parameters, Θ. If the control law used is (4) combined with the computed torque (14), then the resulting dynamics of the robot evolve as: ˆ − Θa ). y¨ = µ + JΦ(Θ (16) Described in terms of η, we have the following: ˜ z˙ = Ψ(η, z), η˙ = F η + Gµ + GJΦΘ, (17) ˜ = 0, we could apply µ(η) ∈ Kε (η) ˜ =Θ ˆ − Θa . If Θ where Θ to drive η → 0. But since the parameters are uncertain, ˜ = 0, the resulting dynamics will be observed in the i.e., Θ derivative of the Lyapunov function, Vε , via (18) V˙ ε (η, µ) = η T (F T Pε + Pε F )η + 2η T Pε Gµ ˜ + 2η T Pε GJΦΘ, where η˙ is obtained via (17). The next section will establish the relationship between parameter uncertainty and the uncertain dynamics appearing in the CLF. 4. PARAMETER SENSITIVITY MEASURE Due to the unmodeled dynamics, applying the controller, µ(η) ∈ Kε (η), does not result in exponential convergence of the controller. The controller will still yield Global Uniform Ultimate Boundedness (GUUB) based on how the unmodeled dynamics affect V˙ ε . The parameter sensitivity measure, ν, that quantifies the ultimate bound on the Lyapunov function Vε is defined as: ˜ ν := Y(q, q, ˙ q¨)Θ.
(19)
˜ = Y(q, q, ˆ − It can be observed that: Y(q, q, ˙ q¨)Θ ˙ q¨)Θ Y(q, q, ˙ q¨)Θa , which is the difference between the actual 379
379
and the expected torque being applied on the robot. Therefore, the parameter sensitivity measure is effectively the difference in torques applied on the robot. Bounds on the Measure through RES-CLF. By (15) ˆ −1 ν = ΦΘ. ˜ Therefore, (18) can be and (19), we have: D expressed as: V˙ ε (η, µ) =η T (F T Pε + Pε F )η + 2η T Pε Gµ ˆ −1 ν, + 2η T Pε GJ D (20) which is now a function of ν. This provides an important connection with Lyapunov theory, and the notion of parameter sensitivity is motivated by this observation. In other words, if the path of least parameter sensitivity is followed, then the convergence of the Lyapunov function to a value very close to zero can be realized. Therefore by (20), the control input µ must be chosen such that ν is well within the bounds specified. If a suitable controller is applied: µ(η) ∈ Kε (η) , the stability of the Lyapunov function can be achieved as long as the following equation is satisfied: γ ˆ −1 ν ≤ 0, (21) V˙ ε ≤ − Vε + 2η T Pε GJ D ε Since the measure ν is a function of the control input µ, (21) has an algebraic loop. But, given the control input, it is possible to restrict the outputs η within a certain region. Therefore, we first assume the bounds on the inertia matrix as: ˆ ≤α α3 ≤ ||D|| ≤ α4 , α ˆ 3 ≤ ||D|| ˆ4, (22) where α3 , α4 , α ˆ3, α ˆ 4 are constants (see Mulero-Martinez (2007); From et al. (2010)). Since the outputs are degree one functions of q, the jacobian is bounded by the constant: ||J|| ≤ κ.
By varying γ, it can be shown that the controller exponentially drives the outputs to a ball of radius β. In particular, by considering γ1 > 0, γ2 > 0, which satisfy γ = γ1 +γ2 , we can rewrite γε Vε = γε1 Vε + γε2 Vε in (21). The first term can thus be used to cancel the uncertain dynamics and yield exponential convergence until ||η|| becomes sufficiently small. In other words, the outputs exponentially converge to an ultimate bound β as given by the following lemma. Lemma 2. Given the controllers µ(η) ∈ Kε (η), µ ¯(η) ∈ ¯ ε¯(η), and δ > 0, ∃ β > 0 such that whenever ||ν|| ≤ δ, K V˙ ε (η) < − γε2 Vε (η) ∀ Vε (η) > β. 4α2 κ2 δ 2
The ultimate bound is given by β = α1 αˆ22 γ 2 ε2 . A discussion 3 1 in this regard can be found in Dixon et al. (2004); Abdallah et al. (1991), where β is considered the uniform ultimate bound on the given controller. It must be noted that Lemma 2 yields a low convergence rate γ2 which is less than the original rate γ. The uncertain dynamics can be nullified separately by considering an auxiliary input µ ¯ satisfying: ˆ qd + C(q, ˆ q) ˆ BTctn = D(q)¨ ˙ q˙ + G(q) + Bµ ¯. (23) Note that this is not unique and other types of controllers can also be used. Computed torque with linear inputs appended have also been used in Slotine and Li (1987) in order to realize asymptotic convergence. The resulting dynamics of the outputs then reduces to:
2015 IFAC ADHS 380 October 14-16, 2015. Atlanta, USA
Shishir Kolathaya et al. / IFAC-PapersOnLine 48-27 (2015) 377–382
˜ + JD ˆ −1 B µ y¨ = µ + JΦΘ ¯.
(24)
Therefore, V˙ ε for the new input can be reformulated as: ¯) = η T (F T Pε + Pε F )η + 2η T Pε Gµ (25) V˙ ε (η, µ, µ T −1 ˆ + 2η Pε GJ D (ν + B µ ¯). ˆ −1 B. Consider the input µ ¯ = − 1ε¯ ΓT GT Pε η, where Γ = J D µ and µ ¯ together form the computed torque+PD control on the robot. The end result is a positive semidefinite expression: 1ε¯ η T Pε GΓΓT GT Pε η ≥ 0, which motives the construction of a positive semidefinite function: (26) V¯ε (η) = η T Pε GΓΓT GT Pε η =: η T P¯ε η. Using the property of positive semidefiniteness, we can establish new bounds on the outputs. Let N (P¯ε ) be the null space of the matrix P¯ε . If η ∈ N (P¯ε ), then V¯ε (η) = 0. Otherwise, for some α7 , α8 > 0: α8 α7 ||η||2 ≤ V¯ε (η) ≤ 4 ||η||2 . (27) ε Note that (27) can be used to restrict the uncertain dynamics in (25). Utilizing these constructions, we can define the following class of controllers: ¯ ε¯(η) = {u ∈ Rk : 2η T Pε GJ D ˆ −1 Bu + 1 V¯ε (η) ≤ 0}, K ε¯ Lemma 2 can now be redefined to obtain the new ultimate bound βη for the new control input (23). Lemma 3. Given the controllers µ(η) ∈ Kε (η), µ ¯(η) ∈ ¯ ε¯(η), and δ > 0, ∃ βη > 0 such that whenever ||ν|| ≤ δ, K V˙ ε (η) < − γε Vε (η) ∀ Vε (η) > βη . The resulting ultimate bound is: βη = α9 =
V¯ε Vε .
4α22 κ2 δ 2 ε¯2 , α7 α29 α ˆ 23 ε4
where
It can be inferred that: γ
Vε (η(t)) ≤ e− ε t Vε (η(0)) for Vε (η(0)) > βη , (28) 1 α2 − γ t or ||η(t)|| ≤ e 2ε ||η(0)|| for Vε (η(0)) > βη . ε α1 It must be noted that both ε, ε¯ affect ||ν||. We can now consider utlizing the boundedness properties in underactuated systems given that the zero dynamics of the robot has a locally exponentially stable periodic orbit. Zero Dynamics. Assume that there is an exponentially stable periodic orbit in the zero dynamics, denoted by Oz ⊂ Z. This means that there is a Lyapunov function Vz : Z → R≥0 such that in a neighborhood Br (Oz ) of Oz (see Hauser and Chung (1994)) it is exponentially stable. If O = ι(Oz ) ⊂ X ×Z is the periodic orbit of the full order dynamics through the canonical embedding ι : Z → X ×Z, then by defining the composite Lyapunov function: (29) Vc (η, z) = σVε (η) + Vz (z), we can introduce a theorem that shows ultimate boundedness of the entire dynamics of the robot to the periodic orbit O under parameter uncertainty. ¯(η) ∈ Theorem 1. Given the controllers µ(η) ∈ Kε (η), µ ¯ ε¯(η), and δ > 0, ∃ βη , βz > 0 such that whenever ||ν|| ≤ K δ, Vc (η, z) is exponentially convergent ∀ Vc (η, z) > βη +βz .
Proof of Theorem 1 is omitted due to space constraints but is an extension of Theorem 1 of Ames et al. (2014) which shows that by choosing a suitable σ, exponential convergence of Vc until the bound βη + βz can be achieved. 380
5. HYBRID DYNAMICS We now extend Theorem 1 to hybrid robotic systems which involve alternating phases of continuous and discrete dynamics. A hybrid system with a single continuous and a discrete event is defined as follows: ˜ η˙ = F η + Gµ + GJΦΘ, z˙ = Ψ(Θ; η, z), if (η, z) ∈ D\S (30) H = + − − η = ∆ (Θ, η , z ), η + z = ∆z (Θ, η − , z − ), if (η − , z − ) ∈ S It must be noted that the parameter vector Θ is included in zero dynamics, z, in (30), which is a reformulation of (7). D, S are the domain and switching surfaces and are given by: D = {(η, z) ∈ X × Z : h(η, z) ≥ 0}, (31) ˙ S = {(η, z) ∈ X × Z : h(η, z) = 0 and h(η, z) < 0},
for some continuously differentiable function h : X × Z → R. ∆(Θ, η − , z − ) = (∆η (Θ, η − , z − ), ∆z (Θ, η − , z − )) is the reset map representing the discrete dynamics of the system. For the bipedal robot, AMBER, h represents the non-stance foot height and ∆ represents the impact dynamics of the system. Plastic impacts are assumed. For (q − , q˙− ) ∈ S, being the pre-impact angles and velocities of the robot, the post impact velocity for the assumed model qˆ˙+ , and for the actual model q˙+ will be: ˆ −1 J T (J D ˆ −1 J T )−1 J )q˙− , qˆ˙+ = (I − D
ˆ − J T RD) ˜ qˆ˙+ − (I − J T R)D ˜ q˙− , q˙+ = Da−1 (D (32) where J is the jacobian of the end effector where the impulse forces from the ground are acting on the robot, and R = (J Da−1 J T )−1 J Da−1 . Given the hybrid system (30), denote the flow as ϕt (Θ; ∆(Θ, η − , z − )) with the initial condition (η − , z − ) ∈ S ∩ Z. Impact Measure. Using the impact model, measuring uncertainty of post-impact dynamics can be achieved by introducing an impact measure, νs , defined as follows: ˜ q˙− , νs := D(q) (33) It should be noted that the impact equations are Lipschitz continuous w.r.t. the impact measure νs . Accordingly, we have the following bounds on the impact map: ˆ 0, z − )|| ||∆η (Θa , η − , z − ) − ∆η (Θ, ˆ η− , z− ) ≤ ||∆η (Θa , η − , z − ) − ∆η (Θ, ˆ η − , z − ) − ∆η (Θ, ˆ 0, z − )|| + ∆η (Θ,
≤ L1 ||νs || + L2 ||η − ||, (34) where L1 , L2 are Lipschitz constants for ∆η . Similarly: ˆ 0, z − )|| ≤ L3 ||νs || + L4 ||η − ||, ||∆z (Θa , η − , z − ) − ∆z (Θ, where L3 , L4 are Lipschitz constants for ∆z . In order to obtain bounds on the output dynamics for hybrid periodic orbits, it is assumed that H has a hybrid zero dynamics ˆ of the robot. The hybrid zero for the assumed model, Θ, dynamics can be described as: ˆ 0, z) if z ∈ Z\(S ∩ Z) z˙ = Ψ(Θ; (35) H |Z = + ˆ 0, z − ) if z − ∈ (S ∩ Z) z = ∆Z (Θ,
ˆ 0, z − ) = 0, so that More specifically we assume that ∆η (Θ; the surface Z is invariant under the discrete dynamics. ˆ ∆(Θ, ˆ 0, z − )) Given (35), we can define the flow as ϕzt (Θ;
2015 IFAC ADHS October 14-16, 2015. Atlanta, USA
Shishir Kolathaya et al. / IFAC-PapersOnLine 48-27 (2015) 377–382
with the initial state (0, z − ) ∈ S ∩ Z. If a periodic orbit Oz exists in (35), then there exists a periodic ˆ ∆(Θ, ˆ 0, z ∗ )) of period T ∗ for the fixed point flow ϕzt (Θ; ∗ (0, z ). Through the canonical embedding, the corresponding periodic flow of the periodic orbit O in (30) will be ˆ ∆(Θ, ˆ 0, z ∗ )). Note that existence of periodic orbit ϕt (Θ; ˆ does not guarantee existence of for the assumed model Θ the periodic orbit for the actual model Θa . Therefore, we define the Poincar´e map P : S → S given by: P(Θ; η, z) = ϕT (Θ,η,z) (Θ; ∆(Θ, η, z)), (36) ˆ and T is the time to impact where Θ can be either Θa or Θ, function defined by: T (Θ; η, z) = inf{t ≥ 0 : ϕt (Θ; ∆(Θ, η, z)) ∈ S}. (37) It is shown in Ames et al. (2014) that T is Lipschitz continuous. Therefore, for constants α15 < 1 < α16 , α15 T ∗ ≤ T (Θ, η, z) ≤ α16 T ∗ . These constants depend on ˆ and how far (η, z) the deviation from the nominal model Θ is from the fixed point. The corresponding Poincar´e map ρ : S ∩ Z → S ∩ Z for the hybrid zero dynamics (35) is termed the restricted Poincar´e map: ˆ ∆(Θ, ˆ 0, z)), ρ(z) = ϕz (Θ; (38) Tρ (z)
where Tρ is the restricted time to impact function which ˆ 0, z). Without loss of generality, is given by Tρ (z) = T (Θ; ˆ = 0, z ∗ = 0. The following Lemma we can assume that Θ will introduce the relationship between time to impact, Poincar´e functions with η, νs . Lemma 4. Let Oz be the periodic orbit of the hybrid zero dynamics H |Z transverse to S ∩ Z for the nominal model ˆ Given the controllers µ(η) ∈ Kε (η), µ ¯ ε¯(η) for Θ. ¯(η) ∈ K the actual model Θa which render ultimate boundedness on Vε (η), (η, z) ∈ S∩Z such that Vε (∆η (Θ, η, z)) > βη for the given robot model Θ, and r > 0 such that (η, z) ∈ Br (0, 0), there exist finite constants A1 , A2 , A3 , A4 > 0 such that: ||T (Θa ; η, z) − Tρ (z)|| ≤ A1 ||η|| + A2 ||νs || ||P(Θa ; η, z) − ρ(z)|| ≤ A3 ||η|| + A4 ||νs ||
(39) (40)
Proof (Sketch). Let µ1 ∈ R2(n−k) , µ2 ∈ R2k be constant vectors. Define an auxiliary time to impact function: TB (µ1 , µ2 , z) = inf{t ≥ 0 : h(µ1 , ϕzt (0, ∆(0, 0, z)) + µ2 ) = 0}, which is Lipschitz continuous with the Lipschitz constant LB : ||TB (µ1 , µ2 , z) − Tρ (z)|| ≤ LB (||µ1 || + ||µ2 ||).
Let (η1 (t), z1 (t)) satisfy z˙1 = Ψ(Θa ; η1 (t), z1 (t)) with η1 (0) = ∆η (Θa , η, z) and z1 (0) = ∆z (Θa , η, z). Similarly let z2 (t) satisfy z˙2 (t) = Ψ(0; 0, z2 (t)) such that z2 (0) = ∆z (0, 0, z). The bounds on the ||η|| can now be given as ||η1 (0)|| = ||∆η (Θ, η, z) − ∆η (0, 0, z)|| ≤ L1 ||νs || + L2 ||η||, which is obtained through (34). Since Vε (η1 ) > βη , we use (59) of Lemma 1 of Ames et al. (2014) and (28) to obtain ||µ1 ||. Similartly, to obtain ||µ2 ||, we use the GronwallBellman argument from the proof of Lemma 1 of Ames et al. (2014) to get the following inequality: ||z1 (t) − z2 (t)|| ≤ (C2 ||η|| + C3 ||νs ||) eLq t , (41) where C2 , C3 , Lq are constants. Proof of (39) can now be obtained by substituting for ||µ1 ||, ||µ2 ||.
Similarly, the derivation of (54) of Lemma 1 of Ames et al. (2014) is used to obtain (40).
381
381
Main Theorem. We can now introduce the main theorem of the paper. Similar to the continuous dynamics, it is assumed that the periodic orbit OZ is exponentially stable in the hybrid zero dynamics. Theorem 2. Let Oz be an exponentially stable periodic orbit of the hybrid zero dynamics H |Z transverse to S ∩ Z ˆ Given the controllers µ(η) ∈ for the nominal model Θ. ¯ ε¯(η) for the hybrid system H . Given r > Kε (η), µ ¯(η) ∈ K 0 such that (η, z) ∈ Br (0, 0), there exist δ > 0, δs > 0 such that whevener ||ν|| < δ,||νs || < δs , the orbit O = ι(OZ ) is uniformly ultimately bounded by βη + βz . Proof (Sketch). Results of Lemma 4 and the exponential stability of OZ imply that there exists r > 0 such that ρ : Br (0) ∩ (S ∩ Z) → Br (0) ∩ (S ∩ Z) is well defined for all z ∈ Br (0) ∩ (S ∩ Z) and zk+1 = ρ(zk ) is locally exponentially stable, i.e., ||zk || ≤ N ξ k ||z0 || for some N > 0, 0 < ξ < 1 and all k ≥ 0. Therefore, by the converse Lyapunov theorem for discrete systems, there exists an exponentially convergent Lyapunov function Vρ , defined on Br (0) ∩ (S ∩ Z) for some r > 0 (possibly smaller than the previously defined r). It must be first ensured that the region βη must be within the bounds defined by r. For ||η|| = r, βη < Vε (η) is ensured through the following condition: βη < Vε (η) < αε22 ||η||2 < αε22 r2 . From Lemma 2 we have:
4α32 κ2 δ 2 α21 α23 γ12 ε4
<
α2 2 ε2 r
=⇒ δ <
α 1 α 3 γ1 ε 2 2α2 κ r.
For the RES-CLF Vε , denote its restriction to the switching surface by Vε,η = Vε |S , which is exponentially convergent. With these two Lyapunov functions we define the following candidate Lyapunov function on Br (0, 0) ∩ S: VP (η, z) = Vρ (z) + σVε,η (η) (42) The idea is to show that there exists a bounded region β into which the dynamics of the robot exponentially converge. Since the origin is an exponentially stable equilibrium for zk+1 = ρ(zk ), we have the following inequalities: ||Pz (Θa ; η, z)|| = ||Pz (Θa ; η, z) − ρ(z) + ρ(z) − ρ(0)|| ≤ A3 ||η|| + A4 ||νs || + Lρ ||z|| ||ρ(z)|| ≤ N ξ||z||, (43) where Lρ is the Lipschitz constant for ρ. Therefore: Vρ (Pz (Θa ; η, z)) − Vρ (ρ(z)) (44) ≤ α20 (A3 ||η|| + A4 ||νs ||) (A3 ||η|| + A4 ||νs || + (Lρ + N ξ)||z||), where α20 is equivalent to the constant r4 given in (61) of proof of Theorem 2 in Ames et al. (2014). It follows that: Vρ (Pz (Θa , η, z)) − Vρ (z) = Vρ (Pz (Θa ; η, z)) − Vρ (ρ(z)) +Vρ (ρ(z)) − Vρ (z). Combining the entire Lyapunov function we have: T ||η|| ||η|| ΛH ||z|| VP (P(Θa ; η, z)) − VP (η, z) ≤ − ||z|| ||νs || ||νs || where the symmetric matrix ΛH ∈ R3×3 , is obtained by collecting the constant terms. It can found in the derivation of Theorem 2 of Ames et al. (2014) that by choosing a suitable σ positive definiteness of ΛH can be established to render the discrete Lyapunov function VP exponentially convergent. The upper bound on the impact measure is obtained by applying the contraint on the above equation to ensure exponential convergence.
2015 IFAC ADHS 382 October 14-16, 2015. Atlanta, USA
Shishir Kolathaya et al. / IFAC-PapersOnLine 48-27 (2015) 377–382
6. SIMULATION RESULTS AND CONCLUSIONS In this section, we will invesigate how the uncertainty in parameters affects the stability of the controller applied to the 5-DOF bipedal robot AMBER shown in Fig. 1. The model, Θa , which has 61 parameters is picked such that ˆ the error is 30% compared to the assumed model Θ.
1.2
Non-stance knee
Stance knee
To realize walking on the robot, the actual and desired outputs are chosen as in Yadukumar et al. (2012) (specifically, see (6) for determining the actual and the desired outputs). The end result is outputs of the form y(q) = ya (q) − yd (q) which must be driven to zero. Therefore, the objective of the computed torque controller (5) combined with Kε for ¯ ε¯ for µ ˆ µ and K ¯ is to drive y → 0. For the nominal model Θ a stable walking gait is observed. In other words, a stable hybrid periodic orbit is observed for the assumed given model. Since, the actual model of the robot has an error of 30%, applying the controller yields the dynamics that evolves as shown in (24). The value of ε chosen was 1, and ε¯ was 2. Fig. 2 shows the comparison between actual and desired outputs, and Fig. 3 shows the Lyapunov function Vε . It can be observed that after every impact, Vε is thrown outside the ball defined by βη ≈ 0.04 and the controllers act to get back into βη before the next impact. 0.4
0.35
0.3
0.25
0.2 0
0.5
1
1.5
2
2.5
1.1 1 0.9 0.8 0.7 0.6 0.5 0.4 0.3
3
0
0.5
1
1.5
2
2.5
3
Time(s) 0.4
0.2
0.3
Torso angle
Non-stance slope
Time(s)
0.1
0
-0.1
-0.2
0.2 0.1 0 -0.1 -0.2 -0.3 -0.4
0
0.5
1
1.5
2
2.5
3
0
0.5
1
Time(s)
1.5
2
2.5
3
Time(s)
Fig. 2. Actual (blue) and desired (red) outputs as a function of time are shown here. Lyapunov Function 0.3
2.5
Measure (||ν||)
CLF (Vǫ )
0.25 0.2 0.15 0.1 0.05
2
1.5
1
0.5
0 −0.05 0
0.5
1
1.5
2
Time (s)
2.5
3
3.5
0 0
0.5
1
1.5
2
2.5
3
Time (s)
Fig. 3. The RES-CLF (left) and the measure (right) as a function of time are shown here. V˙ ε crosses 0 in every step, but the CLF is still seen to be ultimately bounded by βη . Conclusions. The concept of a measure for evaluating the robustness to parameter uncertainty of hybrid system models of robots was introduced. This relationship is created by introducing the formula for the parameter 382
sensitivity measure. The notion of measure is extended to hybrid systems (which includes discrete events) by considering the impact measure. This impact measure along with the sensitivity measure determine the output boundedness of the composite Lyapunov function for hybrid periodic orbits of the robot. This was then verified by realizing a stable walking gait on AMBER having a parameter error of 30%. It is important to observe that while the parameter sensitivity measure yields the difference between the actual and the predicted torque applied on the robot, the impact measure yields the impulsive ground reaction forces acting upon the robot during impacts. Future work will be devoted to evaluating the formal results of this paper experimentally. REFERENCES Abdallah, C., Dawson, D., Dorato, P., and Jamshidi, M. (1991). Survey of robust control for rigid robots. Control Systems, IEEE, 11(2), 24–30. doi:10.1109/37.67672. Ames, A.D., Galloway, K., Sreenath, K., and Grizzle, J.W. (2014). Rapidly exponentially stabilizing control lyapunov functions and hybrid zero dynamics. Automatic Control, IEEE Transactions on, 59(4), 876–891. Byl, K. and Tedrake, R. (2009). Metastable walking machines. The International Journal of Robotics Research, 28(8), 1040–1064. Dixon, W.E., Zergeroglu, E., and Dawson, D.M. (2004). Global robust output feedback tracking control of robot manipulators. Robotica, 22(04), 351–357. Freeman, R.A. and Kokotovic, P.V. (2008). Robust nonlinear control design: state-space and Lyapunov techniques. Springer. From, P.J., Schjølberg, I., Gravdahl, J.T., Pettersen, K.Y., and Fossen, T.I. (2010). On the boundedness and skew-symmetric properties of the inertia and coriolis matrices for vehicle-manipulator systems. In Intelligent Autonomous Vehicles, volume 7, 193–198. Hauser, J. and Chung, C.C. (1994). Converse lyapunov functions for exponentially stable periodic orbits. Systems & Control Letters, 23(1), 27–34. Mulero-Martinez, J. (2007). Uniform bounds of the coriolis/centripetal matrix of serial robot manipulators. Robotics, IEEE Transactions on, 23(5), 1083–1089. doi: 10.1109/TRO.2007.903820. Sastry, S.S. (1999). Nonlinear Systems: Analysis, Stability and Control. Springer, New York. Slotine, J.J.E. and Li, W. (1987). On the adaptive control of robot manipulators. The International Journal of Robotics Research, 6(3), 49–59. Spong, M.W., Hutchinson, S., and Vidyasagar, M. (2006). Robot modeling and control, volume 3. Wiley New York. Westervelt, E.R., Grizzle, J.W., Chevallereau, C., Choi, J.H., and Morris, B. (2007). Feedback Control of Dynamic Bipedal Robot Locomotion. CRC Press, Boca Raton. Yadukumar, S.N., Pasupuleti, M., and Ames, A.D. (2012). From formal methods to algorithmic implementation of human inspired control on bipedal robots. In Tenth International Workshop on the Algorithmic Foundations of Robotics (WAFR). Cambridge, MA. Zhou, K., Doyle, J.C., Glover, K., et al. (1996). Robust and optimal control, volume 40. Prentice Hall Upper Saddle River, NJ.