Walking with Confidence: Safety Regulation for Full Order Biped Models

TL;DR

This paper presents a novel method combining sums-of-squares optimization and hybrid zero dynamics to generate safety guarantees for complex 10-dimensional walking robot models, enabling safer control strategies.

Contribution

It introduces a hybrid approach that applies sums-of-squares verification on a hybrid zero dynamics manifold for high-dimensional walking robots.

Findings

Generated a guaranteed safe set for a 10D walking robot model

Developed a safety-maintaining controller by adjusting manifold parameters

Demonstrated the approach on a bipedal Rabbit model

Abstract

Safety guarantees are valuable in the control of walking robots, as falling can be both dangerous and costly. Unfortunately, set-based tools for generating safety guarantees (such as sums-of-squares optimization) are typically restricted to simplified, low-dimensional models of walking robots. For more complex models, methods based on hybrid zero dynamics can ensure the local stability of a pre-specified limit cycle, but provide limited guarantees. This paper combines the benefits of both approaches by using sums-of-squares optimization on a hybrid zero dynamics manifold to generate a guaranteed safe set for a 10-dimensional walking robot model. Along with this set, this paper describes how to generate a controller that maintains safety by modifying the manifold parameters when on the edge of the safe set. The proposed approach, which is applied to a bipedal Rabbit model, provides a roadmap for applying sums-of-squares verification techniques to high dimensional systems. This opens the door for a broad set of tools that can generate safety guarantees and regulating controllers for complex walking robot models.

Equations

$$\dot{x}(t)=f(x(t))+g(x(t))u(t).$$

$$x(t^{+})$$

$$=\left[\begin{array}{c}{\Delta }_{q}q(t^{-})\\ {\Delta }_{\dot{q}}(q(t^{-}))\dot{q}(t^{-})\end{array}\right].$$

$${\dot{x}}_{\alpha }(t)=f_{\alpha }(x_{\alpha }(t))+g_{\alpha }(x_{\alpha }(t))u_{\alpha }(t),$$

$$\frac{d}{dt}\left[\begin{array}{c}\alpha (t)\\ \dot{\alpha }(t)\end{array}\right]=\left[\begin{array}{c}\dot{\alpha }(t)\\ 0\end{array}\right]+\left[\begin{array}{c}0\\ 1\end{array}\right]u_{\alpha }(t),$$

$$\begin{gathered} Z:=\{(q,\dot{q},\alpha ,\dot{\alpha })\in TQ\times TA|h(q,\alpha )=0, \\ ({\mathcal{L}}_f^{x}h)(q,\alpha ,\dot{q})+({\mathcal{L}}_{f_{\alpha }}^{x_{\alpha }}h)(q,\alpha ,\dot{\alpha })=0\} \end{gathered}$$

$$\left[\begin{array}{c}\Delta (x(t^{-}))\\ x_{\alpha }(t^{-})\end{array}\right]\in Z\forall \left[\begin{array}{c}x(t^{-})\\ x_{\alpha }(t^{-})\end{array}\right]\in Z\cap \left(S\times TA\right).$$

$$u^{*}(x,x_{\alpha},u_{\alpha})=-(\mathcal{L}_{g}^{x}(\mathcal{L}_{f}^{x}h+\mathcal{L}_{f_{\alpha}}^{x_{\alpha}}h))^{-1}\Big{(}\mathcal{L}_{f}^{x}(\mathcal{L}_{f}^{x}h+\mathcal{L}_{f_{\alpha}}^{x_{\alpha}}h)+\\ +\mathcal{L}_{g_{\alpha}}^{x_{\alpha}}(\mathcal{L}_{f}^{x}h+\mathcal{L}_{f_{\alpha}}^{x_{\alpha}}h)u_{\alpha}+\mathcal{L}_{f_{\alpha}}^{x_{\alpha}}(\mathcal{L}_{f}^{x}h+\mathcal{L}_{f_{\alpha}}^{x_{\alpha}}h)\Big{)}$$

$$\dot{\hat{x}}=\left[\begin{array}{c}\dot{\theta }\\ {\mathcal{L}}_f^x{\mathcal{L}}_f^x\theta +{\mathcal{L}}_g^x{\mathcal{L}}_f^x\theta u^{\ast }\\ f_{\alpha }+g_{\alpha }{u}_{\alpha }\end{array}\right]=\hat{f}(\hat{x})+\hat{g}(\hat{x})u_{\alpha },$$

$$\hat{x}(t^{+})$$

$$\hat{S}$$

$$\hat{\Delta }(\hat{x}(t^{-}))$$

$$h(q,\alpha )=\left[\begin{array}{c}{q}_1-q_1^{Fr}(t_{\theta }(\theta (q)))-\alpha \\ q_3-q_3^{Fr}(t_{\theta }(\theta (q)))\\ q_4-q_4^{Fr}(t_{\theta }(\theta (q)))+\alpha \\ q_5-q_5^{Fr}(t_{\theta }(\theta (q)))\end{array}\right]+h_m(\theta (q),\alpha ).$$

$$Z^F=(((TQ\setminus T\tilde{Q})\times TA)\cap Z)\cup Z_{LMB}\cup (Z\setminus Z_{Lim}).$$

$${\hat{e}}_p(\hat{x})\ge \left|\hat{f}(\hat{x})-{\hat{f}}_p(\hat{x})+\left(\hat{g}(\hat{x})-{\hat{g}}_p(\hat{x})\right)\hat{u}\right|,$$

$$(\hat{x},\hat{\Delta }(\hat{x}))\in R_p,\forall \hat{x}\in \hat{S}.$$

$${\hat{u}}_{min}(\hat{x})\le \hat{u}(\hat{x})\le {\hat{u}}_{max}(\hat{x}),\forall \hat{x}\in Z\setminus Z_p^F$$

$$-\hat{v}-{\sigma }_1{h}_F\in \text{SoS}$$

$${\hat{v}}^{+}-{\hat{v}}^{-}-{\sigma }_2{h}_R-{\sigma }_3{h}_S^{-}\in \text{SoS}$$

$${\mathcal{L}}_{{\hat{f}}_p}^{\hat{x}}\hat{v}+{\mathcal{L}}_{{\hat{g}}_p}^{\hat{x}}\hat{v}{\hat{u}}_s+\sum _{j=1}^{n_z}{q}_j+\hat{v}\lambda +{\sigma }_4{h}_F$$

$$q-{\mathcal{L}}_{{\hat{e}}_p}^{\hat{x}}\hat{v}+{\sigma }_5{h}_F$$

$$q+{\mathcal{L}}_{{\hat{e}}_p}^{\hat{x}}\hat{v}+{\sigma }_6{h}_F$$

$${\hat{u}}_s-{\hat{u}}_{min}+h_F{\sigma }_7\in \text{SoS}$$

$$-{\hat{u}}_s+{\hat{u}}_{max}+h_F{\sigma }_8\in \text{SoS}$$

$${\int }_Z\hat{v}(\hat{x})d\hat{x}.$$

$$1-\hat{v}\in \text{SoS}.$$

$$\underset{\begin{array}{c}\hat{v},{\hat{u}}_s,q,\lambda \\ {\sigma }_1,\dots ,{\sigma }_6\end{array}}{\sup }{\int }_Z\hat{v}(\hat{x})d\hat{x}$$

$$\begin{array}{rl}s.t.& \mathrm{SoS}\mathrm{Constraints}1-5,\\ & {\sigma }_1,\dots ,{\sigma }_8\in SoS\end{array}$$

$${\hat{u}}_r={\hat{u}}_0+({\hat{u}}_s(\hat{x})-{\hat{u}}_0)w_s(\hat{v}(\hat{x}),ϵ),$$

$$u_{\alpha }^d(x_{\alpha },t)=k_p({\alpha }_d(t)-\alpha )+k_d(\dot{{\alpha }_d}(t)-\dot{\alpha })+\ddot{{\alpha }_d}(t).$$

$${\hat{u}}_0(x,x_{\alpha },t)=$$

$$s.t.u^{\ast }(x,x_{\alpha },u_{\alpha })\in [-30\mathrm{Nm},30\mathrm{Nm}{]}^4$$

Full text

I Introduction

Avoiding falls is a safety critical and challenging task for legged robotic systems. This challenge is compounded by strong limits on the available actuation torques; particularly at the ankle or ground contact point. These limits in actuation mean that the motion of a legged robot is often dominated by its mechanical dynamics, which are hybrid, nonlinear, and unstable. A consequence of these limitations is that a controller might be required to take a safety preserving action well before the moment a failure occurs.

Consider, for example, a bipedal robot that just entered single stance during a fast walking gait. The robot is pivoting dynamically over the stance foot and can only apply limited ankle torques to control its motion. To catch the robot again, the swing foot needs to be brought forward rapidly and be placed well in front of the robot. If the forward velocity of the robot and hence the pivoting motion is too fast, there will not be enough time to complete this foot placement far enough in front of the stance leg to slow the robot down [1]. As a result, the robot’s speed increases further, leaving even less time for leg swing in the subsequent steps. The robot might manage to complete another couple of strides, but at this point a fall is inevitable and no control action can prevent it.

Knowing the limits of safe operation is akin to knowing the set of states from which falls, even in the distant future, can be avoided. Such knowledge is valuable for many reasons. Knowing that a fall is inevitable is useful in itself, as it allows a robot to brace for the imminent impact. Knowing the distance from the border of the safe set could allow a robot to estimate the set of impulses that can be withstood without failing. This would allow it to judge whether or not it can safely interact with the environment in a given situation; for example, to push a cart while walking.

Most importantly, this knowledge is valuable due to the flexibility it can create. Rather than stabilizing the robot motion along a specified trajectory, one could imagine controllers that are adaptive to adjust to the environment, to maximize performance, or to fulfill a secondary task such as pointing a sensor onto a target. Any of these secondary tasks can be pursued as long as the state of the robot is within the safe set.

In this context, a representation of the set of safe states enables the construction of a regulator that monitors the system state and takes safety preserving actions only when the robot is at risk of failure [4]. Such a regulator could guarantee safe operation, while allowing a secondary control system to behave flexibly as long as safety is not threatened.

Identifying such safety limits, however, is a challenging problem for nonlinear and hybrid systems. A promising tool for identifying the safety limits of a legged robotic system is sums-of-squares (SoS) optimization [3]. This approach uses semi-definite programming to identify the limits of safety in the state space of a system as well as associated controllers for a broad class of nonlinear [5, 6, 7] and hybrid systems [8, 9]. These safe sets can take the form of reachable sets (sets that can reach a known safe state) [10, 9, 5] or invariant sets (sets whose members can be controlled to remain in the set indefinitely) in state space [11, 8, 12]. However, the representation of each of these sets in state space severely restricts the size of the problem that can be tackled by these approaches.

To accommodate this limitation, sums-of-squares analysis has been primarily applied to reduced models of walking robots: ranging from spring mass models [13], to inverted pendulum models [10, 14] and to inverted pendulum models with an offset torso mass [12]. The substantial differences between these simple models and real robots causes difficulty when applying these results to hardware.

A contrasting approach to designing stable controllers for high dimensional, underactuated robot models uses hybrid zero dynamics (HZD) [15]. In this approach, feedback linearization is used to drive the actuated degrees of freedom of the robot towards a lower dimensional hybrid zero dynamics manifold. This manifold is specified as the zero levelset of a configuration-dependent output vector and represents the motion of the robot in its underactuated degrees of freedom.

Significant progress has been made in the generation of safety certificates for HZD controllers. Much of this work [16, 17, 18, 19, 20] relies on the Poincaré stability of a periodic limit cycle in order to generate safety guarantees. This reliance is restrictive, as it precludes behaviors that would leave the neighborhood of the limit cycle. Recent work has been done to extend the range of safe HZD behaviours beyond a single limit cycle neighborhood [21, 22, 23]. In [21] and [22], the controller is allowed to discretely switch between a family of periodic gaits. Safety is then ensured using a dwell time constraint that limits how frequently switching can occur. In [23], a combination of HZD and finite state abstraction is used to safely regulate forward speed of a fully-actuated bipedal robot in continuous time. Our approach shares similarities with these recent papers, but allows for continuous-time variation of behaviour (instead of discrete switching), and applies to underactuated robotic systems.

In this paper, we build on both of these broad approaches to safety and control synthesis for legged robotic systems. To combine the full-model accuracy of hybrid zero dynamics and the set-based safety guarantees of sums-of-squares programming, we propose the following approach (Fig. 1). First, we use hybrid zero dynamics to map the full order dynamics to a low dimensional hybrid manifold. We control the dynamics on the manifold using a set of shaping parameters, which are modified in continuous time to modify robot behaviour. We then use sums-of-squares programming to find a subset of this manifold which can be rendered forward control invariant. Once this subset is found on the low dimensional manifold, a regulator can be constructed that allows for free control of the manifold dynamics when safety is not at risk, but switches to a safety preserving controller when safety is threatened.

The approach is presented in a general form that extends to a large class of underactuated bipedal robots. Throughout the paper, an example implementation is given for a 10-dimensional model of the robot Rabbit [2] and a tracking task is used to illustrate semi-autonomous safe control. To the best of our knowledge, this is the highest dimensional walking robot model for which set-based safety guarantees have been generated thus far.

The rest of this paper is organized as follows: Section II formally defines the assumptions and objective of this paper. The next two sections describe our method. Section III constructs a low dimensional zero dynamics manifold with control input. In Section IV we present a sums-of-squares optimization which finds a control invariant subset of the manifold that avoids a designated set of unsafe states. Section V describes the results of our implementation on the robot Rabbit [2], and conclusions are presented in Section VI.

II Problem Setup

II-A Robot Model

For simplicity, we apply similar modeling assumptions to those made in [15]. That is, the robot is modeled as a planar chain of rigid links with mass. Each joint is directly torque actuated except for the point of contact with the ground, leading to one degree of underactuation for a planar model.

The full configuration of the robot is given by the set of joint angles $q=\{q_{1},\ldots,q_{n_{q}}\}\in Q\subset{}^{n_{q}}$. We next define the set of feasible configurations $\tilde{Q}\subset Q$ (similarly to [11]):

Definition 1**.**

A configuration is feasible if the joint angles satisfy actuator limits, and only foot points are touching the ground (i.e. the robot has not fallen over).

Using the method of Lagrange, we can obtain a continuous dynamic model of the robot during swing phase:

[TABLE]

where $x(t)=[q^{\top}(t),\dot{q}^{\top}(t)]^{\top}\in TQ\subset R^{2n_{q}}$ denotes the tangent space of $Q$, $u(t)\in U$, $U$ describes the permitted inputs to the system, and $t$ denotes time.

We assume that an instantaneous and impulsive impact occurs each time the swing foot hits the ground, with the stance leg leaving the ground immediately after impact. As in [15], we can construct a reset map for the state after impact:

[TABLE]

Here the superscript plus indicates the time just after the event and the superscript minus indicates the time just before the event. $\Delta:TQ\rightarrow TQ$ is the reset map of the robot state. $\Delta_{q}\in{}^{n_{q}\times n_{q}}$ is a coordinate transformation matrix that swaps the swing leg and the stance leg after impact. $\Delta_{\dot{q}}:Q\rightarrow{}^{n_{q}\times n_{q}}$, is the configuration-dependent reset map of the configuration velocities.

This equation holds true for all states in $S\subset TQ$, which is called the guard of the hybrid system, and represents the states of the robot with zero swing foot height and downwards swing foot velocity. Any time the state of the robot enters $S$, the reset event must occur.

Example 1**.**

The configuration $q$ for Rabbit is shown in Figure 1 (top left). $\tilde{Q}$ is the set of robot configurations in which only foot points intersect the ground and all joints are within the limits: $q_{1},q_{2},q_{4}\in[-\pi/2,\pi/2]$, $q_{3},q_{5}\in[-\pi/2,0].$ When the swing foot intersects with the ground, we enter the guard $S$. This causes an impulse to be transmitted to the colliding foot, and the swing and stance feet swap. The impulse and coordinate swap are given by $\Delta$. The joint torques torques are saturated to take values in the interval $U=[-30\,\mathrm{Nm},30\,\mathrm{Nm}]^{4}$. All kinematic and inertial properties of the model are given in [2].

II-B Safety

In this paper, safety is defined as keeping the configuration feasible for all time (i.e. $q(t)\in\tilde{Q},\forall t$). To guarantee safety, this paper finds a viability domain [11]:

Definition 2**.**

A viability domain $V\subset{}^{2n_{q}}$ is any set satisfying $V\subset T\tilde{Q}$ which is also forward control invariant. That is, there exists a Lipschitz state feedback controller $u_{s}:T\tilde{Q}\to U$, such that for every initial condition $x_{0}\in V$, the execution of the system from the initial condition remains in $V$ for all time $t\in[0,\infty)$. We refer to any feedback controller that is able to ensure that the system is forward control invariant as an Autonomous Viable Controller.

The forward control invariance property ensures that any state that begins within a viability domain $V$ can be controlled to remain within the domain. Since $V$ contains only feasible configurations ($V\subset T\tilde{Q}$), we know that safety can be maintained by at least one controller from all states in $V$.

Once a viability domain is found, we use it to construct a semi-autonomous, safety preserving controller. Given an initial state within $V$, a user defined control input is applied without modification to the system. The state of the system is then continuously monitored. If the state approaches the boundary of the viability domain, the control input is overridden by an autonomous viable controller. This gives the user full control over the system until safety is threatened, at which point, safety is automatically enforced. Once safety is no longer at risk, control is returned to the user.

II-C Goal

Using these definitions, we state our objective as:

Find a viability domain and a corresponding autonomous viable controller. 2. 2.

Use this domain and autonomous viable controller to construct a semi-autonomous viable controller.

III Controlled Hybrid Zero Dynamics Manifold

We intend to use sums-of-squares optimization to achieve these objectives. However, the state-space dimension of realistic robot models far exceeds the limits of this tool. For instance, the state-space of the benchmark model Rabbit [2] has dimension 10, while many sums-of-squares problems become computationally challenging above dimension 6 [12]. In this section, we show how the the state-space dimension can be reduced to a feasible size using the idea of hybrid zero dynamics [15].

III-A Shaping Parameters

The hybrid zero dynamics approach uses feedback linearization to drive the actuated degrees of freedom onto a low-dimensional manifold specified by a set of user-chosen outputs, which depend on the robot configuration $q\in Q$. We modify this approach by making these outputs also depend on a set of time varying shaping parameters $\alpha(t)\in A\subset{}^{n_{\alpha}}$. The shaping parameters $\alpha$ are used in this paper to provide an input within the manifold dynamics. By varying $\alpha$ continuously over time, the user can change the hybrid zero dynamics manifold to modify the robot behaviour in real-time. The idea of modifying the HZD manifold in real-time is similar to [23] in which the desired hip velocity $v$ acts as an input to the manifold.

We define the dynamics of $\alpha$ as:

[TABLE]

where $x_{\alpha}(t)=[\alpha^{\top}(t),\dot{\alpha}^{\top}(t)]^{\top}\in TA$, $u_{\alpha}(t)\in U_{\alpha}\subset{}^{n_{\alpha}}$ are the shaping parameter inputs (with permitted values $U_{\alpha}$), and $t$ denotes time. We require that $\alpha$ has vector relative degree two under these dynamics. We assume a trivial discrete update for the shaping parameters when the robot state hits a guard: $x_{\alpha}(t^{+})=x_{\alpha}(t^{-})$.

Example 2**.**

*As shown in the bottom left of Figure 1, we use a single shaping parameter $\alpha(t)\in[-\pi/2,\pi/2]$ to modify the desired pitch angle of Rabbit.

Note that this choice is somewhat arbitrary; $\alpha$ could instead modify properties such as step length or center of mass height. We define the dynamics of $\alpha$ as follows:*

[TABLE]

where $u_{\alpha}$ represents the user-controlled pitch acceleration.

III-B Constructing the Manifold

In this subsection, we incorporate these shaping parameters in the construction of the hybrid zero dynamics manifold described in [15]. Throughout the section, we use $\mathcal{L}_{f}^{x}$ and $\mathcal{L}_{g}^{x}$ to represent the Lie derivatives in $TQ$ with respect to $f$ and $g$, and $\mathcal{L}_{f_{\alpha}}^{x_{\alpha}}$ and $\mathcal{L}_{g_{\alpha}}^{x_{\alpha}}$ to represent the Lie derivatives in $TA$ with respect to $f_{\alpha}$ and $g_{\alpha}$ (where we drop the arguments).

We begin by using a set of outputs: $h:Q\times A\to{}^{n_{u}}$ to implicitly define the hybrid zero dynamics manifold as:

[TABLE]

These outputs must satisfy hypotheses similar to HH 1-4 in [15], and the resulting manifold $Z$ must satisfy the hybrid invariance condition:

[TABLE]

Provided these conditions are met, we can use the results in [24, Chapter 9.3.2] to show that $Z$ is a smooth submanifold in $TQ\times TA$ of dimension $n_{z}=2(n_{q}-n_{u}+n_{\alpha})$. In addition, the control input $u^{*}:TQ\times TA\times U_{\alpha}\rightarrow U$ given by:

[TABLE]

renders $Z$ invariant under the hybrid dynamics of the robot (note the right hand side arguments are suppressed to simplify presentation).

As in hypothesis HH 3 in [15], we define a set of phasing coordinates $\theta:Q\rightarrow{}^{n_{q}-n_{u}}$ which represent the underactuated degrees of freedom of the robot. Using these coordinates, we can parameterize the on-manifold state of the robot $\hat{x}(t)\in Z$ as: $\hat{x}(t)=[\theta(q)^{\top},\dot{\theta}(q,\dot{q})^{\top},\alpha^{\top},\dot{\alpha}^{\top}]^{\top}$ (where we have suppressed the time dependence on the right hand side). The continuous dynamics under this parameterization are then:

[TABLE]

where we have suppressed the time dependence. The discrete manifold dynamics are given by:

[TABLE]

where $t^{-}$ is the state before impact, and the manifold guard and reset ($\hat{S}$ and $\hat{\Delta}$) are defined as:

[TABLE]

Example 3**.**

We begin by using the trajectory optimization toolbox FROST [25] to find a time-varying, periodic walking trajectory: $q^{Fr}:[0,t_{max}]\to Q$. For this trajectory, the stance leg angle of the robot: $\theta(q)=-q_{1}-q_{2}-\frac{q_{3}}{2}$ is monotonic in time and varies from $\theta_{min}$ to $\theta_{max}$. This allows us to define a phasing function $t_{\theta}:[\theta_{min},\theta_{max}]\to[0,t_{max}]$ which satisfies $q^{Fr}(t_{\theta}(\theta(q^{Fr}(t))))=q^{Fr}(t)$ (i.e. $t_{\theta}$ maps from points in the state space to points along the trajectory).

We modify the pitch angle of the FROST trajectory using the shaping parameter $\alpha$, giving us the output function:

[TABLE]

Here we also added the function $h_{m}:Q\times A\to{}^{4}$ which is chosen to ensure satisfaction of the hybrid invariance condition (7). This technique for ensuring hybrid invariance is similar to the procedure given in [26].

The guard of our HZD manifold $Z$ is given as $\hat{S}=\{\hat{x}\mid\theta=\theta_{max},\dot{\theta}>0\}$ and the reset is defined as in (12).

III-C Safety on the Manifold

We now revisit the safety criteria from Section II-B under the assumption that our state is controlled to lie on $Z$. For the biped to be safe, we require that the manifold state remains in the feasible set $\tilde{Q}$, and that the state does not leave the manifold (either by leaving the manifold boundary, or by encountering actuator limits when trying to stay on $Z$). We define the unsafe states $Z^{F}\subset Z$ as the union of:

• •

The infeasible states: $((TQ\setminus T\tilde{Q})\times TA)\cap Z$

• •

The states that leave the manifold boundary, i.e. all members of the boundary set ($\partial Z=\{\hat{x}\in Z\mid q_{0}(\hat{x})\in\partial Q\text{ or }\alpha\in\partial A\}$) which do not lie on a guard, and that have an outward velocity.

• •

The states requiring unattainable actuation to remain on $Z$, i.e. all states $(x,x_{\alpha})\in Z$ for which $u^{*}(x,x_{\alpha},u_{\alpha})\notin U,\;\forall u_{\alpha}\in U_{\alpha}$.

Additionally we define the state-dependent set of realizable shaping parameter inputs $\hat{U}:TQ\times TA\to 2^{U_{\alpha}}$, as $\hat{U}(x,x_{\alpha})=\{u_{\alpha}\in U_{\alpha}\,\rvert\,u^{*}(x,x_{\alpha},u_{\alpha})\in U\}$ (where $2^{U_{\alpha}}$ denotes the set of all subsets of $U_{\alpha}$).

Provided we constrain the manifold state to avoid $Z^{F}$, and constrain the shaping parameter input to lie within $\hat{U}$, our safety criteria is maintained.

Our goal from Section II-C can now be re-stated as:

Find a viability domain on $Z$ that does not intersect $Z^{F}$, and an autonomous viable controller $\hat{u}_{s}:Z\to\hat{U}$. 2. 2.

Use this domain and autonomous viable controller to construct a semi-autonomous controller.

Example 4**.**

For the Rabbit example, the set of states that leave the manifold boundary are given by $Z_{LMB}=\{\hat{x}\mid\alpha=\pi/2,\;\dot{\alpha}>0\}\cup\{\hat{x}\mid\alpha=-\pi/2,\;\dot{\alpha}<0\}$. All other states on the manifold boundary either lie on a guard ($\theta=\theta_{max},\dot{\theta}>0$), or flow inwards. We use sampling and fitting to find a region $Z_{Lim}\subset Z$ where the actuator torque limits can be satisfied for some $u_{\alpha}$. We then define our unsafe set (see Fig. 2):

[TABLE]

The set of attainable inputs $\hat{U}$ is given by the minimum and maximum values of $u_{\alpha}$ at each sample point $(x,x_{\alpha})\in Z$ that satisfy $u^{*}(x,x_{\alpha},u_{\alpha})\in U$.

IV Hybrid Control Invariant Set

This section outlines how the low dimensional safety problem from Section III-C can be solved using sums-of-squares optimization [3]. Broadly, the sums -of-squares approach enforces constraints of the form $p\geq 0$ (where p is a function) by constraining $p$ to be a sum-of-squares polynomial, i.e. $p=\sum_{i}p_{i}^{2}$ (where $p_{i}$ are polynomials). We refer to this constraint as $p\in SoS$.

We begin by showing how the sets and dynamics from the preceding section can be represented using polynomials. We next define a bilinear semi-definite program for finding a viability domain, and describe the alternation used to solve it. Finally, we construct a guaranteed safe semi-autonomous controller for the full robot, based on this viability domain.

IV-A Polynomial Representation

For the dynamics of the system to be used inside our sums-of-squares program, they must be represented in a polynomial form. In particular, we require polynomial representations of the functions $\hat{f},\hat{g},\hat{\Delta}$ and the sets $\hat{S},Z^{F},\hat{U}$. Since these sets and functions can contain trigonometric as well as rational terms in their definition, we rely on approximate representations. It is important to take care to ensure that the safety guarantee is preserved under approximation.

To generate polynomial approximations and verify bounding relations, we use sampling to obtain the exact function values over a dense grid in the state space. This sampling approach is made tractable by the reduction in dimension of the previous section. In our example, this reduces the dimension that must be sampled from 10 to 4. We use a $30\times 30\times 30\times 30$ sample grid to fit and bound the polynomials. The bounds are then verified using a dense set of randomly generated test points.

We begin by sampling $\hat{f}:Z\to{}^{n_{z}}$ and $\hat{g}:Z\to{}^{n_{z}\times n_{\alpha}}$ over our grid of points in $Z$. Least-squares fitting can then be used to obtain the corresponding polynomial representations: $\hat{f}_{p}$ and $\hat{g}_{p}$. To account for the approximation error in the continuous dynamics functions, we introduce a set of error-bounding polynomials $\hat{e}_{p}:Z\to{}^{n_{z}}$ which satisfy:

[TABLE]

for all $\hat{x}\in Z$ and $\hat{u}\in\hat{U}$ where the inequality and absolute value are taken element-wise. These polynomials can be found using a linear program that minimizes the integral of $\hat{e}_{p}$ subject to (15) enforced at our set of sample points.

To represent sets in polynomial form, we require them to take the form of semi-algebraic sets (i.e. a set $X\subset Y$ is defined as $X=\{y\in Y\mid h_{i}(y)\geq 0,\;\forall i=1,\ldots n\}$, where $h:Y\to{}^{n}$ is a collection of polynomials). We use a bounding set to approximate the reset map $\hat{\Delta}:\hat{S}\to Z$ in a conservative manner. That is, we find a set $R_{p}\subset Z\times Z$ that bounds all possible reset behaviours:

[TABLE]

The sets $\hat{S}$ and $Z^{F}$ are represented with semi-algebraic outer approximations as follows: $Z^{F}_{p}\supset Z^{F}$, $\hat{S}_{p}\supset\hat{S}$. We define the sets $R_{p},Z^{F}_{p},\hat{S}_{p}$ using the respective polynomials: $h_{R}:Z\times Z\to{}^{n_{hr}}$, $h_{F}:Z\to{}^{n_{hf}}$, $h_{S}:Z\to{}^{n_{hs}}$. The space of feasible inputs $\hat{U}$ can be approximated using a state-dependent box constraint:

[TABLE]

where $\hat{u}_{min},\hat{u}_{max}:Z\setminus Z^{F}_{p}\to U_{\alpha}$ are polynomial input bounds, and the inequality is taken element-wise. The set of inputs that satisfy this box constraint is denoted by $\hat{U}_{p}$.

IV-B Optimization Formulation

We use an optimization similar to [12] to find the largest possible viability domain $\hat{V}\subset Z\setminus Z^{F}$ for our hybrid zero dynamics system. We represent $\hat{V}$ as the zero super-levelset of a polynomial function $\hat{v}:Z\to\real$ (i.e. $\hat{V}=\{\hat{x}\in Z\mid\hat{v}(\hat{x})\geq 0\}$), and represent the autonomous viable controller using a polynomial function $\hat{u}_{s}:Z\to{}^{n_{\alpha}}$. To enforce the viability of $\hat{V}$ according to Definition 2, we require $\hat{v}$ and $\hat{u}_{s}$ to satisfy four conditions:

Viability Conditions**.**

---

$\hat{V}$* does not intersect $Z^{F}$ (i.e. $\hat{v}(\hat{x})<0,\;\forall\hat{x}\in Z^{F}$)* 2. 2.

All states that are contained in both the guard and $\hat{V}$ must be mapped to a state in $\hat{V}$ (i.e. $\hat{v}(\hat{\Delta}(\hat{x}))\geq 0,\;\forall\hat{x}\in\{\hat{x}\in\hat{S}\mid\hat{v}(\hat{x})\geq 0\}$) 3. 3.

At the boundary of $\hat{V}$ (i.e. where $\hat{v}(\hat{x})=0$), the state flows inward under the controller $\hat{u}_{s}$ (i.e. $\frac{d\hat{v}}{dt}>0$) 4. 4.

The autonomous safe controller must satisfy the input bounds within the safe set (i.e. $\hat{u}_{s}(\hat{x})\in\hat{U},\;\forall\hat{x}\in\hat{V}$)

Condition 1 ensures that states can not leave the viability domain by simply leaving the space $Z$. Condition 2 ensures that states can not leave the viability domain when traversing a guard. Condition 3 ensures that states cannot leave the viability domain under the continuous dynamics of the system. Finally, Condition 4 ensures that our controller respects the robot torque constraints. Each of these conditions are ensured with a corresponding sums-of-squares constraint, giving us:

SoS Constraint 1**.**

(Viability Condition 1)

[TABLE]

Here $\sigma_{1}:Z\to{}^{1\times n_{hf}}\in\textit{SoS}$ are sums-of-squares polynomials that relax the positivity constraint outside $Z^{F}_{p}$. We refer to such polynomials as s-functions.

SoS Constraint 2**.**

(Viability Condition 2)

[TABLE]

Here $\sigma_{2}:Z\times Z\to{}^{1\times n_{hr}},\sigma_{3}:Z\to{}^{1\times n_{hs}}\in\textit{SoS}$ are s-functions. The superscripts $-$ and $+$ indicate whether a function is evaluated using the first ($-$) or second ($+$) argument of $h_{R}:Z\times Z\to{}^{n_{hr}}$. That is, this constraint enforces: $\hat{v}(\hat{x}^{+})-\hat{v}(\hat{x}^{-})-h_{R}(\hat{x}^{-},\hat{x}^{+})\sigma_{2}(\hat{x}^{-},\hat{x}^{+})-h_{S}(\hat{x}^{-})\sigma_{3}(\hat{x}^{-})>0,\;\forall(\hat{x}^{-},\hat{x}^{+})\in Z\times Z$. Note that the addition of the $\sigma_{3}$ term is not strictly necessary, since points in $\hat{S}_{p}$ must lie in $R_{p}$. However, this term can help relax the constraint when points in $R_{p}$ lie outside $\hat{S}_{p}$.

SoS Constraint 3**.**

(Viability Condition 3)

[TABLE]

Here $\sigma_{4}:Z\to{}^{1\times n_{hf}}\in\textit{SoS}$ and $\sigma_{5},\sigma_{6}:Z\to{}^{n_{d}\times n_{hf}}$ are s-functions that relax the constraint inside $Z^{F}_{p}$, and $\lambda:Z\to\real$ is a slack polynomial that can relax the constraint whenever $\hat{v}\neq 0$. The polynomials $q:Z\to{}^{n_{z}}$ are used to bound the effects of the dynamics error $\hat{e}_{p}$ on the time derivative of $\hat{v}$.

SoS Constraint 4**.**

(Viability Condition 4)

[TABLE]

Here $\sigma_{7},\sigma_{8}:Z\to{}^{n_{\alpha}}\in\textit{SoS}$ are s-functions that relax the constraint inside $Z^{F}_{p}$.

The desired objective of our optimization is to maximize the volume of $\hat{V}$. This volume is difficult to compute exactly for an arbitrary $\hat{v}$, since the domain of integration is given by a semi-algebraic set. We propose an analytically tractable approximation to this objective:

[TABLE]

This objective is combined with the following constraint in order to approximate the volume of $\hat{V}$:

SoS Constraint 5**.**

(Objective Constraint)

[TABLE]

To understand how this objective and constraint approximate the volume of $\hat{V}$, take a continuous function $\hat{v}$ that satisfies the constraints of the previous section. For every point $\hat{x}$ not in the set $Z^{F}$, the value $\hat{v}(\hat{x})$ is constrained only by Constraint (5). This means that $\hat{v}(\hat{x})$ can increase to a value of 1 for points inside $\hat{V}$, and $\hat{v}(\hat{x})$ increases to a value of 0 for points outside this set. As a result, $\hat{v}$ approaches the indicator function over $\hat{V}$, and the integral in the objective function approaches the volume of $\hat{V}$.

Combining the constraints and objective, we arrive at the following sums-of-squares problem:

[TABLE]

To express this problem as a semi-definite program or SDP (which can be solved with commercial solvers), all SoS constraints must be linear functions of the decision variable polynomials. However, Constraint 3 in the above problem includes the terms $\mathcal{L}_{\hat{g}_{p}}^{\hat{x}}\hat{v}\hat{u}_{s}$ and $\lambda\hat{v}$ which are bilinear in $\hat{u}_{s},\,\hat{v}$ and in $\lambda,\,\hat{v}$ respectively. Problems of this form are referred to as bilinear sums-of-squares problems. The bilinear nature of the constraints means that these problems are non-convex, and we can no longer guarantee a globally optimal solution to this problem.

To solve this nonconvex bilinear sums-of-squares program we turn to a strategy called alternation. This strategy breaks (19) into a pair of linear sums-of-squares programs which can each be solved using a commercial solver. In each program one of the bilinear variables is kept fixed while the other is optimized over. The variables that were optimized are then fixed while the other pair of variables are optimized. If the final solution satisfies the constraints of the original program, the solution is guaranteed to be a viability domain. Computationally, each SDP is formulated in spotless111https://github.com/spot-toolbox/spotless and solved using Mosek.

IV-C Guaranteed Safe Semi-autonomous Controller

We use a feasible solution to the above optimization problem to generate a guaranteed safe semi-autonomous controller. This controller modifies user input to ensure that the Viability Conditions 3 and 4 are always satisfied. Condition 4 can be enforced by saturating the user inputs to always lie within the input bounds. To enforce condition 3, we note that it is only active on the boundary of $\hat{V}$. This means that we can ensure safety so long as we use the autonomous safe controller $\hat{u}_{s}$ when the state lies on the boundary of $\hat{V}$, i.e. $\{\hat{x}\in Z\rvert\hat{v}(\hat{x})=0\}$.

Since a controller that is discontinuous on the boundary of the safe set would pose difficulties for systems with finite bandwidth, we additionally must ensure that the new controller is continuous near the boundary. To achieve this, we smoothly interpolate between the user input $\hat{u}_{0}$ and the guaranteed safe controller $\hat{u}_{s}$ (which satisfies the safety condition when $\hat{v}(\hat{x})=0$) to get the regulated input $\hat{u}_{r}$:

[TABLE]

where $\hat{u}_{s}$ and $\hat{v}$ are computed using (19), $w_{s}:\real\to[0,1]$ is a smooth step-like function that satisfies $w_{s}(v,\epsilon)=0,\,\forall v\geq\epsilon$, and $w_{s}(v,\epsilon)=0,\,\forall v\leq\epsilon/2$, and $\epsilon\in(0,1)$ controls the smoothness of the interpolation.

When $\hat{x}$ satisfies $\hat{v}(\hat{x})>\epsilon$, the user input is unmodified, as we are sufficiently removed from the boundary of the safe set. When $0\leq\hat{v}(\hat{x})\leq\epsilon/2$, the safe controller is fully active, keeping the state in the safe set.

V Results

We used the proposed approach to compute a viability domain for the robot Rabbit [2]. The viability domain is represented using a set of 8 degree-4 polynomials, each covering an interval within the full range of $\theta$. A two-dimensional slice of the viability domain $\hat{V}$ is shown in Fig. 2.

To demonstrate the semi-autonomous safe controller, we used it to ensure safety while performing a reference following task. The task is to track a time-varying pitch angle $\alpha_{d}:[0,\infty)\to A$. To follow the target, we set a desired pitch acceleration $u_{\alpha}$ using a ”naïve” PD controller:

[TABLE]

We used the feedback controller (8) to map the desired acceleration to the four motor torques of the Rabbit model.

For the feedback controller to respect Rabbit’s actuator torque limits, we first saturated $u_{\alpha}$ with a real-time Quadratic Program (QP) to get the input to our safety regulator:

[TABLE]

Using a QP to satisfy the actuator constraints of the system is similar to many state of the art approaches for high-dimensional robot control [17, 18, 19, 20] . A major limitation of these approaches is the inability to guarantee the feasibility of the QP. That is, for some states, there may not be an input that satisfies the actuator constraints (the set of such states is shown in red in Fig. 2).

Our approach guarantees the feasibility of (22) by constraining the state of the robot to be within the QP-feasible region (i.e. outside of $Z^{F}$ in Fig. 2). To maintain this state constraint, we modified the input $\hat{u}_{0}$ using the guaranteed safe semi-autonomous controller defined in (20). In Fig. 3, we compare the results of the naïve controller (21) and the safe controller (20) using a simulation of the full dynamics of the robot Rabbit.

When tracking the backwards pitch target, the naïve controller slows to the point of falling backwards, while the safe controller deviates slightly to maintaint forward walking. For the forward pitch target, the naïve controller speeds up as it leans forward. At a certain speed, it cannot longer stay on the low dimensional manifold under the torque limits and falls. The safe controller recognizes this risk early and deviates from the desired forward pitch before reaching this speed. The bottom-left figure shows how the set of torque-limit satisfying control inputs disappears for the naïve controller.

This task demonstrates that robot safety can be maintained even for states that are far away from any periodic limit cycle. Indeed, the only periodic limit cycle used in our approach keeps the body pitch relatively upright ($\alpha=0$). As such, our approach broadens the set of real-time safe behaviours that can be executed by Rabbit, since previous methods [16, 18, 20, 21] would all require a pre-computed limit cycle for each new reference trajectory.

VI Conclusion

This paper presents a method to construct a guaranteed safe semi-autonomous controller for high-dimensional walking robots. The resulting controller guarantees viability and allows for flexible input when viability is not at risk. The method is evaluated on a model of the robot Rabbit, and a tracking task is used to illustrate its capabilities. With a 10-dimensional state space, this model is larger than any known model for which continuous-time safety guarantees have been generated.

Despite this increase in model dimension, our example is still somewhat simplified: the dynamics are two dimensional, the terrain is flat, and the range of behaviour is limited to modifying the torso pitch angle. In contrast, bipedal robots in the world must traverse three dimensional, varied terrain while performing a wide range of tasks.

When extending our method to these cases, a trade-off arises between the degree of underactuation of the model, the genericity of the behaviour (i.e. the number of shaping parameters), and the computational complexity of the optimization problem. From Section III-B, the dimension of the reduced order manifold (our state space) is twice the sum of the degree of underaction and the number of shaping parameters. In [12], the authors show that a 6 dimensional state space is tractable for similar sums-of-squares programs. Our approach can thus currently handle a maximum of three degrees of underactuation and/or shaping parameters.

Under this constraint, we can directly extend our method to 3D. For instance, take the 3D biped with controlled steering given in [21]. This application has two degrees of underactuation (pitch and yaw) and would have one shaping parameter controlling yaw rate (i.e. turning left or right). Using our method, we could construct a safe steering controller for the robot that avoids the risk of turning too quickly and falling. An extension to rough terrain, however, will likely require improvements in scaling of the sums-of-squares problem. Such scaling improvements are an active research target [27, 28].

The core insight behind our approach is that sums-of-squares and hybrid zero dynamics are remarkably complementary tools. Sums-of-squares analysis generates the set based guarantees needed to render hybrid zero dynamics safe, and hybrid zero dynamics provides the dimensionality reduction needed for sums-of-squares analysis to be tractable. The key innovation for combining these two tools was the introduction of a set of shaping parameters which control the dynamics on the manifold. The ability to combine sums-of-squares and hybrid zero dynamics presents a promising path forward for building guaranteed safe walking controllers for complex legged robots.