Nonlinear Control of Robotic Systems

Chapter III: Preliminaries

3.2 Nonlinear Control of Robotic Systems

While we could construct a control input𝑢with a CLF to guarantee the system with zero disturbance, 𝑑 = 0, is stable, with an unknown disturbance, stability can not be guaranteed. However, we could guarantee stability to a set. For disturbances in nonlinear systems, ISS [251], [252] can guarantee convergence to a set around the origin, with the bound in terms of ∥𝑑∥_∞, where ∥𝑑∥_∞ = sup𝑡≥0{|𝑑(𝑡) |}. Here we consider the conditions for exponential ISS. For the classical definition see [251]. The system (3.20) isexponential input-to-state stable (e-ISS) if there exists 𝛽 ∈ K L_∞,𝜄 ∈ K_∞, and constant𝑐 > 0 such that

∥𝑥(𝑡 , 𝑥₀, 𝑑) ∥ ≤ 𝛽( ∥𝑥₀∥, 𝑡)𝑒^−𝑐𝑡+𝜄( ∥𝑑∥_∞), ∀𝑥₀, 𝑑 , ∀𝑡 ≥ 0.

We can quantity e-ISS via Lyapunov functions. A continuously differentiable func- tion𝑉 :R^𝑛 → R≥0is ane-ISS Lyapunov functionwith constants𝑐₁, 𝑐₂, 𝑐₃ > 0 and 𝜄 ∈ K_∞such that∀𝑥 ∈R^𝑛, 𝑑 ∈R^𝑛𝑑,

𝑐₁∥𝑥∥² ≤𝑉(𝑥) ≤ 𝑐₂∥𝑥∥²

∥𝑥∥ ≥ 𝜄( ∥𝑑∥_∞) ⇒ ¤𝑉(𝑥 , 𝑑) ≤ −𝑐₃𝑉(𝑥).

(3.21)

The background theory presented in this section will be used as a basis for the theoretical contributions of this thesis, developed in Chapters 4, 5, and 6. All of the novel theoretical constructions in this thesis are first developed in the context of general nonlinear control systems, and they are followed with applications to general robotic systems. We will now present the general constructions used for

constraints. The Jacobian matrix of the holonomic constraints𝐽(𝑞) = ^{𝜕 ℎ}_{𝜕 𝑞} ∈ R^{𝑛ℎ×𝑛𝑞} enforces the holonomic constraints by:

𝐽¤(𝑞,𝑞¤) ¤𝑞+𝐽(𝑞) ¥𝑞=0. (3.23) Solving (3.22) and (3.23) simultaneously yields theconstrained dynamics.

To relate this to the continuous dynamics of the hybrid control system of (3.2), we write the following ODE,

¤ 𝑥 = 𝑑

𝑑 𝑡

"

𝑞

¤ 𝑞

#

=

"

¤ 𝑞

𝐷⁻¹(𝑞) (−𝐻(𝑞,𝑞¤) +𝐽^𝑇(𝑞)𝜆)

#

| {z }

𝑓(𝑥)

+

"

0 𝐷⁻¹(𝑞)𝐵

#

| {z }

𝑔(𝑥)

𝑢 .

The discrete dynamicsΔ𝑋 of (3.2) are determined by assuming a perfectly plastic impact. See [253], [254] for details.

Bipedal Robot Gait Generation

Multi-Domain Hybrid System. To have bipedal walking emulate human heel- toe roll, multiple contact points need to be accounted for. As the contact points change, the different holonomic constraints change the continuous dynamics, re- quiring multiple domains in a hybrid system. To model this multi-domain hybrid system, we use a directed graph Γ = (𝑉 , 𝐸) with vertices 𝑣 ∈ 𝑉 that connect edges {𝑒 = {𝑣 → 𝑣⁺}}|𝑣∈𝑉 = 𝐸, where 𝑣⁺ is the subsequent vertex of 𝑣 in the directed graph. For each vertex 𝑣, there is a domain D𝑣 (3.3) and control input 𝑢_𝑣. OnD𝑣, there is a control system (𝑓_𝑣, 𝑔_𝑣), that define the continuous dynamics

¤

𝑥 = 𝑓_𝑣(𝑥) +𝑔_𝑣(𝑥)𝑢_𝑣for each𝑥 ∈ D𝑣and𝑢_𝑣 ∈ U. The transition point between one domainD𝑣 and the next D𝑣⁺ in the directed cycle is defined by the guard 𝑆_𝑒. The guard triggers the reset map,Δ𝑒 : 𝑆_𝑒 ⊂ D𝑣 → D𝑣⁺, giving the postimpact states of the system: 𝑥⁺ = Δ𝑒(𝑥), where𝑥 ∈ D𝑣 and𝑥⁺ ∈ D𝑣⁺.

With the sets of each of these objects,D ={D𝑣}|𝑣∈𝑉,U ={𝑢_𝑣}|𝑣∈𝑉,𝑆 ={𝑆_𝑒}|𝑒∈𝐸, Δ = {Δ𝑒}|𝑒∈𝐸, and𝐹 𝐺 ={(𝑓_𝑣, 𝑔_𝑣)}𝑣∈𝑉, we define thismulti-domain hybrid control systemas a tuple [255], [256]:

ℋ𝒞_md= (Γ, D, U, 𝑆, Δ, 𝐹 𝐺). (3.24) Gait Generation. To prescribe outputs to this multi-domain hybrid control system, typically relative degree 1 and 2 outputs are used for walking robots and respectively

defined as,

𝑦_1,𝑣(𝑞,𝑞¤)= 𝑦^𝑎

1,𝑣(𝑞,𝑞¤) −𝑦^𝑑

1,𝑣, (3.25)

𝑦_2,𝑣(𝑞)= 𝑦^𝑎

2,𝑣(𝑞) −𝑦^𝑑

2,𝑣(𝜏_𝑣, 𝛼_𝑣). (3.26) While the phase variable 𝜏_𝑣 can be time- or state-based, for more robust control, a state-based phase variable𝜏(𝑞) is used [154] and is typically defined as follows,

𝜏(𝑞)𝑣 = 𝑝_𝑣(𝑞) −𝑝_0,𝑣 𝑝_{𝑓 ,𝑣} − 𝑝_0,𝑣

. (3.27)

Here 𝑝_𝑣(𝑞)is a state-dependent function that is monotonic in D𝑣 and𝑝_0,𝑣 and𝑝_{𝑓 ,𝑣} are the initial and final values of this function in this domain. For walking, previous work found the forward progression of the stance hip to be monotonic during a human gait cycle [150]. This qualifies it to be used as 𝑝_𝑣(𝑞)for the phase variable.

As described previously in Subsection 3.1, a control input 𝑢 is designed to drive these outputs to 0. We previously discussed how this results in the system evolving on the zero dynamics surface 3.17. However, when a relative degree 1 output 𝑦_1,𝑣 is used, the zero dynamics cannot necessarily remain invariant through impacts. In fact, enforcing impact invariance on the velocity-modulating output is too restrictive due to the jump of velocities by the impact map. Hence, we only enforce an impact invariance condition on the relative degree 2 outputs, resulting in partial zero dynamics:

𝑃 𝑍_𝛼

𝑣 ={(𝑞,𝑞¤) ∈ D𝑣 : 𝑦_2,𝑣(𝑞, 𝛼_𝑣) =0,𝑦¤_2,𝑣(𝑞, 𝛼_𝑣) =0}.

For a domain where a relative degree 1 output is used, the optimization problem (3.19) uses this partial zero dynamics surface 𝑃 𝑍_𝛼 instead of 𝑍_𝛼, as shown in the following domain-specific optimization problem:

{𝛼^∗

𝑣,C_𝑣^∗} =argmin

𝛼𝑣,C𝑣

J𝑣

s.t. Δ𝑒(𝑆_𝑒∩𝑃 𝑍_𝛼

𝑣) ⊆ 𝑃 𝑍_𝛼

𝑣+

(𝜂₀, 𝑧₀) =𝜑^ℋ

𝑇 (𝜂₀, 𝑧₀), C_min ≼ C𝑣 ≼ C_max c_min ≼ c𝑣(C𝑣) ≼ c_max,

(3.28)

where 𝜑^ℋ

𝑇 is the hybrid periodic flow for the whole multi-domain hybrid system (3.24), and c𝑣(C𝑣) enforce real-world constraints of the robot such as torque and

joint limits and contact conditions. To solve this optimization problem, we use a direct collocation based optimization algorithm, FROST [198].

The solution to the optimization provides the𝛼parameters that define Bézier poly- nomials,

B(𝜏_𝑣) =

m

∑︁

𝑖=0

𝛼_{𝑣 ,𝑖} m!

(m−𝑖)!𝑖!𝜏^𝑖

𝑣(1−𝜏_𝑣)^m−𝑖, (3.29) where m is the degree of the Bézier polynomial with coefficients𝛼_𝑣 ={𝛼_{𝑣 ,𝑖}}|𝑖=1,...,m. ID-CLF-QP

A RES-CLF, as described in Subsection 3.1, could be used to construct a control input𝑢to track these generated trajectories.

We can use the linearized output dynamics (3.9) to construct a CLF for the robotic system. First, using𝐹 and𝐺 from (3.9), we solve the CARE equation,

𝐹^𝑇𝑃+𝑃 𝐹+𝑃𝐺 𝐺^𝑇𝑃+𝑄=0,

for 𝑃 = 𝑃^𝑇 > 0, with the user selected weighting matrix𝑄 = 𝑄^𝑇 > 0. From the method of [199], we construct a CLF by the following,

𝑉(𝜂) =𝜂^𝑇𝑃𝜂.

For this robotic system with relative degree 1 and 2 outputs, we define 𝜂 = (𝑦^𝑇

1, 𝑦^𝑇

2,𝑦¤^𝑇

2)^𝑇 and transform the CLF into a RES-CLF using the method in [199]

with 0 < 𝜀 <1:

𝑉_𝜀(𝜂) =𝜂^𝑇















𝐼 0 0

0 ¹_𝜀𝐼 0

0 0 𝐼













 𝑃















𝐼 0 0

0 ¹_𝜀𝐼 0

0 0 𝐼















𝜂=:𝜂^𝑇𝑃^𝜀𝜂.

To obtain the convergence constraint, we take the derivative, 𝑉¤_𝜀(𝜂) = 𝐿_𝐹𝑉_𝜀(𝜂) +𝐿_𝐺𝑉_𝜀(𝜂)𝜇 ≤ − 𝜆_min(𝑄)

𝜆_max(𝑃^𝜀)𝑉_𝜀(𝜂), with Lie derivatives along the linearized output dynamics as,

𝐿_𝐹𝑉_𝜀(𝜂) =𝜂^𝑇(𝐹^𝑇𝑃^𝜀+𝑃^𝜀𝐹)𝜂, 𝐿_𝐺𝑉_𝜀(𝜂) =2𝜂^𝑇𝑃^𝜀𝐺 ,

and𝜇, as given by (3.6), is

𝜇= 𝐿^∗

𝑓𝑦(𝑥) + 𝐴(𝑥)𝑢 .

However, to implement a CLF on hardware, the feedback linearizing terms 𝐿^∗

𝑓𝑦(𝑥) and𝐴(𝑥)in (3.2) pose a challenge in that they require inversion of the inertia matrix 𝐷(𝑞) which is computationally expensive and can have numerical instability. To avoid this, [37] developed an inverse dynamics CLF quadratic program (ID-CLF- QP) that includes the CLF stability condition (3.2), the dynamics (3.22), and the holonomic constraints (3.23) as constraints in the QP. This way the QP can determine the control input𝑢, accelerations𝑞¥, and constraint wrenches𝜆simultaneously in a way that satisfies the stability constraint with respect to the dynamics and holonomic constraints.

To form this controller, we recall 𝜇= ( ¤𝑦^𝑇,𝑦¥^𝑇)^𝑇 and rewrite the outputs in terms of the robotic system’s configuration coordinates𝑞and velocities𝑞¤,

"

¤ 𝑦₁

¥ 𝑦₂

#

=













𝜕 𝑦₁

𝜕 𝑞

𝜕

𝜕 𝑞

𝜕 𝑦₂

𝜕 𝑞 𝑞¤











| {z }

𝐽¤_𝑦(𝑞,𝑞¤)

¤ 𝑞+

"𝜕 𝑦₁

𝜕𝑞¤

𝜕 𝑦

𝜕 𝑞

#

|{z}

𝐽𝑦(𝑞)

¥ 𝑞 .

We will include these terms in the QP cost with the holonomic constraints, enforcing these as soft constraints, using,

𝐽_qp(𝑞) =

"

𝐽_𝑦(𝑞) 𝐽(𝑞)

#

, 𝐽¤_qp(𝑞,𝑞¤)=

"

𝐽¤_𝑦(𝑞,𝑞¤) 𝐽¤(𝑞,𝑞¤)

# . With these terms we formulate our ID-CLF-QP:

Υ^★=argmin

Υ∈R^𝑛qp

𝐽¤_qp(𝑞,𝑞¤) ¤𝑞+𝐽_qp(𝑞) ¥𝑞−𝜈^pd

2

+𝜎𝑊(Υ) +𝜌 𝜁 s.t. 𝐷(𝑞) ¥𝑞+𝐻(𝑞,𝑞¤) =𝐵𝑢+𝐽^𝑇(𝑞)𝜆

𝐿_𝐹𝑉(𝑥) +𝐿_𝐺𝑉(𝑥) ( ¤𝐽_𝑦(𝑞,𝑞¤) ¤𝑞+𝐽_𝑦(𝑞) ¥𝑞) ≤ −𝑐 𝜀

𝑉(𝑥) +𝜁

−𝑢_max ≤ 𝑢 ≤ 𝑢_max,

(3.30)

with decision variables Υ = ( ¥𝑞^𝑇, 𝑢^𝑇, 𝜆^𝑇)^𝑇. Here 𝜈^pd = (𝜇^𝑇

pd,0^𝑇)^𝑇, 𝑊(Υ) is a regularization term designed to make the system well-posed, 𝜎 and 𝜌 are user- selected weights,𝜁is a relaxation term to allow the torque bounds(−𝑢_max, 𝑢_max)to be met.

Figure 3.1: Human-Prosthesis Model and Platform. (Left) Human-prosthesis model with generalized coordinates. (Right) AMPRO3 powered prosthesis platform with components and coordinates labeled.

3.3 Human-Prosthesis Model and Powered Prosthesis Platform