Direct Approach via Backstepping Barrier Functions

APPLICATION ⊃ ACTUATED SLIP

6.3 Direct Approach via Backstepping Barrier Functions

(e-x) (e-y) (c)

0 2 4 6 8

0.7 0.8 0.9

(a) (b)

DSP

(d)

P1 P2

Figure 6.8: An example of the generated 3D periodic walking: (a-b) horizontal trajectories of the mass vs time (the red indicates the velocities in the DSP), (c) the vertical trajectory of the mas, (d) the leg length tracking (the red and blue are the desired and actual𝐿, respectively), and (e) the phase portraits of the mass (blue) in the 𝑥− 𝑧 plane (e-x) and 𝑦−𝑧 plane (e-y) comparing to the desired orbits of the H-LIP (black).

from the stepping controller perturbs the stepping-in-place walking into desired 3D walking. Fig. 6.8 shows the simulation result. The aSLIP starts to execute the forward walking at𝑡 =3s. As indicated in the phase plot in Fig. 6.8 (d1-d2), the translational states of the aSLIP converge closely to the desired P1 and P2 orbits of the H-LIP in each plane. 3D walking is thus generated on the aSLIP.

COM state of the aSLIP, and then present the results to realize the aSLIP walking via the H-LIP based synthesis. The periodic vertical trajectory of the swing foot 𝑧_sw = 𝑧− 𝐿_swcos(𝛽_sw) is controlled via a feedback linearizing controller. Similar to that on AMBER in Chapter 5, its desired trajectory is simply constructed to be 𝑧^des

sw =𝑧^max

sw cos(_𝑇^𝑡

SSP

𝜋− ¹

2𝜋), where𝑧^max

sw > 0 is chosen to avoid scuffing.

Backstepping-Control Lyapunov Function for Vertical Stabilization

In this part, we present the dynamics structure of the vertical mass state in each domain. Then, we show the canonical Lyapunov backstepping that guarantees to stabilize this class of control problem. Lastly, we formulate a control Lyapunov backstepping, which yields an inequality condition for the control and opens oppor- tunities to include extra constraints on the input in an optimization.

Strict-feedback Form of the Vertical State: The objective is to drive the vertical position of the mass to follow a desired trajectory. We define the output as

𝜂 :=

"

𝑧−𝑧^des(𝑡)

¤

𝑧− ¤𝑧^des(𝑡)

#

, (6.21)

where [𝑧^des(𝑡),𝑧¤^des(𝑡)] is the desired vertical trajectory for the point mass of the aSLIP to follow. For walking on flat ground,𝑧^desis a constant. Differentiating the output yields the output dynamics:

¤

𝜂 = 𝑓_𝜂+𝑔_𝜂𝐹^SSP/DSP

𝑧 , (6.22)

𝑓_𝜂 :=

"

¤ 𝑧− ¤𝑧^des

−g− ¥𝑧^des

#

, 𝑔_𝜂 :=

"

0

1 𝑚

#

. (6.23)

𝐹^SSP/DSP

𝑧 is the net vertical force in each domain:

SSP: 𝐹^SSP

𝑧 =𝐹_𝑠cos(𝛽_𝑠), (6.24)

DSP: 𝐹^DSP

𝑧 =𝐹_𝑠

1cos(𝛽_𝑠

1) +𝐹_𝑠

2cos(𝛽_𝑠

2), (6.25)

where the stance leg is denoted by the subscript 𝑠. Recall 𝛽 is the leg angle. We view 𝐹^SSP/DSP

𝑧 as the fictitious input to this system in Eq. (6.22). Differentiating 𝐹^SSP/DSP

𝑧 yields the affine dynamics with the actual actuation of the leg length as the input. The dynamics are different in the SSP and DSP:

𝐹¤^SSP

𝑧 = 𝑓^SSP

𝑧 +𝑔_𝑧𝜏^SSP

𝑧 , (6.26)

𝐹¤^DSP

𝑧 = 𝑓^DSP

𝑧 +𝑔_𝑧𝜏^DSP

𝑧 , (6.27)

where𝑔_𝑧 =D≠0, and, 𝜏^SSP

𝑧 :=cos(𝛽_𝑠)𝜏_𝑠, (6.28)

𝜏^DSP

𝑧 :=cos(𝛽_𝑠

1)𝜏_𝑠

1+cos(𝛽_𝑠

2)𝜏_𝑠

2. (6.29)

The expressions of 𝑓^SSP

𝑧 and 𝑓^DSP

𝑧 are omitted. 𝜏^SSP/DSP

𝑧 represents the vertical component of the leg length actuation. The leg angles 𝛽 ∈ (−^𝜋

2, ^𝜋

2), thus given a desired𝜏^SSP/DSP

𝑧 , there always exists leg length actuation for realization. Note that the desired𝜏^DSP

𝑧 is not uniquely realized by the leg length actuation𝜏_𝑠

1and𝜏_𝑠

2. Note that here we use𝜏_𝑠 represent the leg length actuation on the stance leg in the SSP, and thus𝜏_𝑠

1 and𝜏_𝑠

2 represent the leg length actuation on the two stance legs in the DSP.

As a result, for walking in both domains, we can synthesize𝜏_𝑧 ∈ Rto stabilize the vertical trajectory of the mass based on the dynamics in the strict-feedback form [66]:

¤

𝜂= 𝑓_𝜂+𝑔_𝜂𝐹_𝑧, 𝐹¤_𝑧 = 𝑓_𝑧+𝑔_𝑧𝜏_𝑧.

(6.30) (6.31) Lyapunov Backstepping: Lyapunov Backstepping [67] can be applied to stabilize the dynamics in Eq. (6.30) and (6.31). For the dynamics in Eq. (6.30) with 𝐹_𝑧 being the input, a feedback linearization controller can be synthesized:

¯ 𝐹_𝑧 = 1

𝑔_𝜂

2

(−𝑓_𝜂

2+𝐾_IO𝜂), (6.32)

where𝐾_IO = [𝐾_𝑝, 𝐾_𝑑] is the linear feedback gain and the subscript₂indicates the second element of the vector. This yields the linear closed-loop dynamics:

¤

𝜂 = 𝑓_𝜂+𝑔_𝜂𝐹¯_𝑧 =

"

0 1

𝐾_𝑝 𝐾_𝑑

#

𝜂 := 𝐴_𝑐𝑙𝜂. (6.33)

Thus 𝐾_IO is chosen with 𝐾_𝑝 < 0, 𝐾_𝑑 < 0 so that 𝐴_𝑐𝑙 is stable (with negative eigenvalues). On the closed-loop dynamics, a Lyapunov function can be found:

𝑉_𝜂 = 𝜂^𝑇𝑃𝜂, with 𝑃 > 0 (being positive definite). 𝑃 satisfies the continuous-time Lyapunov function𝑃 𝐴_𝑐𝑙+𝐴^𝑇

𝑐𝑙𝑃 =−𝑄, where𝑄 >0 is selected. It is easy to verify that:

𝑉¤_𝜂 =−𝜂^𝑇𝑄 𝜂 ≤ −𝜆_min(𝑄) ||𝜂||², (6.34) where𝜆_min(𝑄)is the smallest eigenvalue of the matrix𝑄.

To synthesize the actual control from𝜏_𝑧 to stabilize𝜂→0, we define

𝐹_𝛿 :=𝐹_𝑧−𝐹¯_𝑧, (6.35)

and the Lyapunov function to be

𝑉(𝜂, 𝐹_𝑧) =𝑉_𝜂+ 1 2𝐹²

𝛿. (6.36)

Differentiating this yields 𝑉¤(𝜂, 𝐹_𝑧) =

𝜕𝑉_𝜂

𝜕 𝜂

(𝑓_𝜂+𝑔_𝜂𝐹_𝑧) +𝐹_𝛿𝐹¤_𝛿

=

𝜕𝑉_𝜂

𝜕 𝜂

(𝑓_𝜂+𝑔_𝜂𝐹¯_𝑧) + 𝜕𝑉_𝜂

𝜕 𝜂

𝑔_𝜂𝐹_𝛿 +𝐹_𝛿𝐹¤_𝛿

=𝑉¤_𝜂+ 𝜕𝑉_𝜂

𝜕 𝜂

𝑔_𝜂𝐹_𝛿+𝐹_𝛿𝐹¤_𝛿 (6.37)

≤ −𝜆_min(𝑄) ||𝜂||²+

𝜕𝑉_𝜂

𝜕 𝜂

𝑔_𝜂𝐹_𝛿+𝐹_𝛿𝐹¤_𝛿. (6.38) If we choose

𝐹¤_𝛿 =−

𝜕𝑉_𝜂

𝜕 𝜂

𝑔_𝜂−𝑘 𝐹_𝛿, (6.39)

with𝑘 > 0, then

𝑉¤(𝜂, 𝐹_𝑧) ≤ −𝜆_min(𝑄) ||𝜂||²−𝑘 𝐹²

𝛿

≤ −min(𝜆_min(𝑄), 𝑘) || [𝜂^𝑇, 𝐹_𝛿]^𝑇||². (6.40) By Lyapunov’s method, the system with (𝜂, 𝐹_𝛿) as states is exponentially stable to the origin(𝜂, 𝐹_𝛿)= (0,0). This provides a closed-formBackstepping controlleron 𝜏_𝑧 from Eq. (6.39). Since𝐹¤_𝛿 = 𝑓_𝑧+𝑔_𝑧𝜏_𝑧− ¤𝐹¯_𝑧,the controller is

𝜏_𝑧 = 1 𝑔_𝑧

(−

𝜕𝑉_𝜂

𝜕 𝜂

𝑔_𝜂−𝑘 𝐹_𝛿+ ¤𝐹¯_𝑧− 𝑓_𝑧). (6.41) Backstepping-CLF: The closed-form controller in Eq. (6.41) appears to be able to stabilize 𝜂 → 0. However, the resultant leg force 𝐹_𝑧 can be negative, which is not valid for walking. Here we develop the control Lyapunov function (CLF) of the backstepping to provide aninequality conditionon the input𝜏_𝑧for stabilizing𝜂. The inequality motivates an optimization-based controller, in which the condition of non-negative leg forcing can be enforced via additional constraints, e.g., the control barrier function (CBF) in the next part.

Note that𝑉¤ is an affine function w.r.t. the input𝜏_𝑧: 𝑉¤(𝜂, 𝐹_𝑧)=𝑉¤_𝜂+

𝜕𝑉_𝜂

𝜕 𝜂

𝑔_𝜂𝐹_𝛿+𝐹_𝛿(𝑓_𝑧+𝑔_𝑧𝜏_𝑧 − ¤𝐹¯_𝑧)

=𝑉¤_𝜂+

𝜕𝑉_𝜂

𝜕 𝜂

𝑔_𝜂𝐹_𝛿+𝐹_𝛿(𝑓_𝑧− ¤𝐹¯_𝑧) +𝐹_𝛿𝑔_𝑧𝜏_𝑧. The exponential stability can be established via enforcing

𝑉¤(𝜂, 𝐹_𝑧) ≤ −𝛾𝑉(𝜂, 𝐹_𝑧), (6.42) with some chosen𝛾 >0. This yields aBackstepping-CLF inequality:

𝐴Backstepping

CLF 𝜏_𝑧 ≤ 𝑏Backstepping

CLF , (6.43)

where𝐴Backstepping

CLF :=𝐹_𝛿𝑔_𝑧, 𝑏Backstepping

CLF :=− ¤𝑉_𝜂−^𝜕𝑉𝜂

𝜕 𝜂 𝑔_𝜂𝐹_𝛿−𝐹_𝛿(𝑓_𝑧− ¤𝐹¯_𝑧) −𝛾𝑉 .When 𝐹_𝛿 ≠ 0, Eq. (6.43) is a constraint on 𝜏_𝑧. When 𝐹_𝛿 = 0, the inequality becomes 𝑉¤_𝜂 ≤ −𝛾𝑉 = −𝛾𝑉_𝜂, which is automatically satisfied as long as 0 ≤ 𝛾 ≤ ^{𝜆min(𝑄)}

𝜆_max(𝑃). As a result, as long as𝜏_𝑧satisfies the backstepping-CLF inequality,𝜂exponentially converges to 0. Note that this inequality is an affine condition on𝜏_𝑠 in SSP or 𝜏_𝑠1 and𝜏_𝑠2in DSP as indicated by Eq. (6.28). Thus, in the next part, we will formulate quadratic program (QP) based controllers that include the inequality in Eq. (6.43) with the incorporation of the control barrier functions.

Control Barrier Functions for Walking

In the application of walking, the leg forces should be positive during contact.

Moreover, in the DSP, one leg force should gracefully cross 0 to initiate lift-off.

These conditions can be described via sets and thus be enforced via control barrier function (CBF) with an inequality condition that guarantees set invariance on the dynamics. We start by introducing the CBF, show the application for the walking of the aSLIP, and finally integrate it with the Backstepping-CLF to formulate the final backstepping-barrier function based quadratic program (BBF-QP) controllers for walking.

Control Barrier Functions:The control barrier function [11] describes a condition for control input that guarantees for set invariance. We consider a super level set C of a continuously differentiable scalar function ℎ : R^𝑛 → R. By definition:

C = {𝑥 ∈ R^𝑛|ℎ(𝑥) ≥ 0}. Here we use𝑥 for a general state representation, instead of the horizontal position of the aSLIP. ℎis acontrol barrier functionfor the affine control system𝑥¤ = 𝑓(𝑥) +𝑔(𝑥)𝑢if

sup

𝑢∈𝑈

[L𝑓ℎ(𝑥) + L𝑔ℎ(𝑥)𝑢+𝛼(ℎ(𝑥))] ≥0, (6.44)

whereL𝑓ℎ(𝑥) = ^{𝜕 ℎ}_{𝜕 𝑥} 𝑓(𝑥) and L𝑔ℎ(𝑥) = ^{𝜕 ℎ}_{𝜕 𝑥}𝑔(𝑥) are the Lie derivatives; L𝑓ℎ(𝑥) + L𝑔ℎ(𝑥) = ℎ¤(𝑥). 𝑈 is the set where the input𝑢is in, and𝛼(·) is an extended class K_∞ function1. This condition indicates that there exists an input to stabilize the set C, i.e., making sureℎ(𝑥) ≥0. If the state is inC, it will stay in the set forever if the CBF inequalityis satisfied:

L𝑓ℎ(𝑥) + L𝑔ℎ(𝑥)𝑢 ≥ −𝛼(ℎ(𝑥)). (6.45) This makes sure that the lower bound of the derivative ℎ¤ is increasing with the decrease of ℎ. It can be proven that the set C is exponentially stable under this condition [11].

CBF for aSLIP Walking: Eq. (6.45) represents an inequality constraint on the input to make sure thatℎ ≥ 0, for which ℎis defined differently for the walking in the SSP and the DSP.

SSP:The stance leg force 𝐹_𝑠 should be non-negative, so is its vertical component 𝐹^SSP

𝑧 . Thus, we let

ℎ_𝑠 =𝐹^SSP

𝑧 , ℎ¤_𝑠 ≥ −𝛼(ℎ_𝑠), (6.46) which provides an inequality on the input𝜏_𝑠:

𝐴^𝑠

CBF𝜏_𝑠 ≤ 𝑏^𝑠

CBF, (6.47)

𝐴^𝑠

CBF :=−𝑔_𝑧cos(𝛽_𝑠), (6.48) 𝑏^𝑠

CBF := 𝑓^SSP

𝑧 +𝛼(ℎ_𝑠), (6.49)

where we simply select𝛼(·)to be a linear function, i.e.,𝛼(ℎ_𝑠) =𝛼 ℎ_𝑠This inequality naturally fits with the Backstepping-CLF inequality in Eq. (6.43) to formulate a backstepping-barrier function based quadratic program (BBF-QP) controller:

(𝜏_𝑠, 𝛿)= argmin

(𝜏_𝑠,𝛿)∈R²

𝜏²

𝑠 +𝛿² s.t. 𝐴Backstepping

CLF 𝜏^SSP

𝑧 ≤ 𝑏Backstepping

CLF +𝛿,

𝐴^𝑠

CBF𝜏_𝑠 ≤ 𝑏^𝑠

CBF,

(6.50)

where𝛿is a relaxation variable to avoid infeasibility, similar to that in the CLF-QP in Chapter 2. In the case when the CBF constraint violates the Backstepping-CLF constraint, the Backstepping-CLF constraint is relaxed and the CBF constraint is still enforced.

1𝛼:R→R,𝛼(0)=0 and𝛼is strictly monotonically increasing.

DSP:Both leg forces should remain non-negative. The leg force on𝑠₂should remain non-negative through out the DSP. Thusℎ_𝑠

2 =𝐹_𝑠

2 and the CBF inequality is 𝐴^𝑠2

CBF𝜏_𝑠

2 ≤ 𝑏^𝑠2

CBF. (6.51)

The leg force on 𝑠₁ should gradually decrease and reach to zero to trigger the transition into the next SSP. Let𝐹⁰

𝑠₁ be the leg force on 𝑠₁ in the beginning of the DSP. A desired leg force trajectory can be designed: 𝐹^𝑑

𝑠₁(𝑡) = 𝐹⁰

𝑠₁(1− ^𝑡

𝑇_DSP). One may consider to design a feedback controller to drive 𝐹_𝑠

1 → 𝐹^𝑑

𝑠₁. However, this creates high restriction on𝜏_𝑠

1and can lead to conflict between the Backstepping-CLF inequality in Eq. (6.43) and the CBF inequality in Eq. (6.51).

To decrease𝐹_𝑠

1in a relaxed fashion, we create the inequality condition: (1−𝑐)𝐹^𝑑

𝑠₁ ≤ 𝐹_𝑠

1 ≤ (1+𝑐)𝐹^𝑑

𝑠₁, where 𝑐 ∈ (0,1) is a relaxation coefficient. As shown in Fig.

6.9, this generates an admissible force region (indicated by the blue region), which decreases as the desired force 𝐹^𝑑

𝑠₁ decreases with time. This two inequalities can be converted into a single inequality: ℎ_𝑠

1 = (𝑐 𝐹^𝑑

𝑠₁)²− (𝐹_𝑠

1 − 𝐹^𝑑

𝑠₁)² ≥ 0. Note that this barrier function is ill-defined as𝐹^𝑑

𝑠₁ approaches to 0 (the setCbecomes trivial).

Thus we increase the relaxation by adding a positive valueΔ𝐹 in the inequality:

(1−𝑐)𝐹^𝑑

𝑠₁ −Δ𝐹 ≤ 𝐹_𝑠

1 ≤ (1+𝑐)𝐹^𝑑

𝑠₁ +Δ𝐹 , (6.52) which generates the red admissible region. By defining

ℎ_𝑠

1 =(𝑐 𝐹^𝑑

𝑠₁ +Δ𝐹)²− (𝐹_𝑠

1 −𝐹^𝑑

𝑠₁)² ≥ 0, (6.53) the setCis always non-trivial before lift-off. Thus we have another CBF inequality:

𝐴^𝑠1

CBF𝜏_𝑠

1 ≤ 𝑏^𝑠1

CBF.

Similarly, the two CBF inequalities are incorporated with the Backstepping-CLF inequality to formulate the final BBF-QP controller for the walking in DSP:

(𝜏_𝑠

1, 𝜏_𝑠

2, 𝛿) = argmin

(𝜏_𝑠

1,𝜏_𝑠

2,𝛿)∈R³

𝜏²

𝑠₁+𝜏²

𝑠₂+𝛿² s.t. 𝐴Backstepping

CLF 𝜏^DSP

𝑧 ≤ 𝑏Backstepping

CLF +𝛿

𝐴^𝑠1

CBF𝜏_𝑠

1 ≤ 𝑏^𝑠1

CBF, 𝐴^𝑠2

CBF𝜏_𝑠

2 ≤ 𝑏^𝑠2

CBF.

(6.54)

Despite the complexity in the derivation, the BBF-QPs are designed simply to stabilize the vertical position of the mass to the desired trajectory and simultaneously satisfy the conditions on the leg forces during walking.

Figure 6.9: Contact force condition for lift-off in the DSP (the red region represents the admissible region for the leg force).

Results

The control procedure with the chosen parameters is presented in Algorithm 2. The stepping gain 𝐾 in Eq. (3.17) is chosen to be the deadbeat gain. The BBF-QP controller is solved at 1kHz. The aSLIP starts from an initial static configuration and walks to a desired pre-impact velocity 𝑣^∗, which is chosen based on the orbits of the H-LIP. The aSLIP parameters are chosen to match with the robot Cassie1. A video of the results can be seen inhttps://youtu.be/fUZu6y-Gu4g.

Algorithm 2BBF-QP with H-LIP based Walking Synthesis

Initialization: Behavior: 𝑧₀ = 1m,𝑇_SSP = 0.4s, 𝑇_DSP = 0.1s. Control: 𝛼 = 500, 𝛾 =10,𝑘 =10, 𝑐=0.5,Δ𝐹 =20.

whileSimulation/Control loopdo if SSPthen

Desired step size←H-LIP stepping in Eq. (3.17).

Desired swing foot position←Eq. (6.20).

𝜏←BBF-QP in Eq. (6.50) else

𝜏←BBF-QP in Eq. (6.54) end if

end while

We evaluate the approach for walking on flat ground to different desired velocities.

Fig. 6.10 shows the results. The aSLIP converges closely to the desired walking of the H-LIP. The leg forces behave as expected. Fig. 6.10 (c) shows the trajectories of the horizontal velocity, which are not constant in the DSP. This contributes to the error𝑤. We numerically calculate all 𝑤 for different walking simulations and

1𝑚=33kg, and K=8000N/m, D=100Ns/m, which are the nonlinear leg spring parameters of Cassie at𝐿≈1m; see Appendix B. The spring is chosen to be linear for generality.

DSP ^0.8_0.5 0.2

(a)

(c) (d)

𝐸 𝐞

H‐LIP

Admissible Region

(b)

𝑝

𝑣

𝑒

𝑒𝑣

Figure 6.10: Walking using direct synthesis via BBF-QP: (a) the phase portrait of the horizontal state trajectory (blue) in the SSP for walking with𝑣^∗ =0.5m/s, and the black is the corresponding orbit of the H-LIP (green lines are the orbital lines, (b) the leg forces during walking with the gray region being the admissible region in the DSP; (c) the walking velocity and (d) the trajectories of the discrete error state e^𝑘 = [𝑒_𝑝, 𝑒_𝑣]^𝑇 of walking with different𝑣^∗ =0.2,0.5,0.8m/s (indicated by dashed lines in (c)).

inner approximate𝑊 by a square. Since(𝐴+𝐵𝐾)²=0,𝐸 =𝑊 ⊕ (𝐴+𝐵𝐾)𝑊; we get an inner approximation of𝐸 (shown in Fig. 6.10 (d)), and all the error statese are inside 𝐸, which validates the proposed approach of the H-LIP based stepping in Chapter 3. It is also important to note that, similar to the indirect synthesis on 3D-aSLIP, the direct synthesis via BBF-QP can be readily applied to 3D-aSLIP with two orthogonal planar stepping stabilizations via the H-LIP.