EXTENSION ⊃ GLOBAL POSITION CONTROL
9.1 S2S Dynamics with Global Position Control
C h a p t e r 9
be put into the S2S formulation, which yields
𝑐𝑥
𝑘+1
𝑝𝑥
𝑘+1
𝑣𝑥
𝑘+1
| {z }
˜ x𝑘+1
=
1 𝐴1,1−1 𝐴1,2 0 𝐴1,1 𝐴1,2 0 𝐴2,1 𝐴2,2
| {z }
˜ 𝐴
𝑐𝑥
𝑘
𝑝𝑥
𝑘
𝑣𝑥
𝑘
|{z}
˜ x𝑘
+
𝐵1+1
𝐵1 𝐵2
| {z }
˜ 𝐵
𝑢𝑥
𝑘, (9.2)
where the subscripts indicate the elements of the matrices𝐴and𝐵in Eq. (3.12) and Eq. (3.12). We use the overhead tilde to denote the elements in the S2S dynamics that includes the global position. In the latter, we call ˜x theextended pre-impact state in the 𝑥 −𝑧 plane. For 3D walking, ˜y = [𝑐𝑦, 𝑝𝑦, 𝑣𝑦]𝑇 is used to denote the extended pre-impact state in the𝑦−𝑧plane.
Global Position Control
We consider the bipedal robot walks on flat ground in a 3D environment. The robot is given a walking path with a terminal location that it should reach. We assume the path is planned via a high-level planner from all the sensors on the robot. The path is supposed to be relatively smooth and obstacle-free. Additionally, the path is also generated with a speed profile, which is assumed to be feasible for the robot to realize. We parameterize the desired path byr(𝑡)as
r(𝑡) = [𝑟𝑑
𝑥(𝑡), 𝑟𝑑
𝑦(𝑡), 𝑟𝑑
𝜃(𝑡)]𝑇, (9.3)
where𝑟𝑑
𝑥(𝑡), 𝑟𝑑
𝑦(𝑡)are the positions in the global frame and𝑟𝑑
𝜃(𝑡)be the angle of the tangent line to the path. The task for the walking is to drive the robot (depicted by its COM) to follow the path with a given time.
For underactuated robotic walking, it is not possible to exactly track the given path.
To address the GPC problem on bipedal robots, we consider applying the H-LIP based synthesis: the GPC problem is first solved on the H-LIP, the motion of which is then realized on the bipedal robot approximately via the H-LIP based stepping controller. Note that since the extend state in the S2S dynamics has three elements, the deadbeat gain ˜𝐾 is solved from (𝐴˜ +𝐵˜𝐾˜)3 = 0, and the H-LIP based stepping for generating the desired step size on the robot becomes
𝑢des=𝑢H-LIP+𝐾˜(𝑥˜−𝑥˜H-LIP). (9.4) Similarly, the application of the H-LIP based stepping makes sure that the robot stays closely to the H-LIP and thus follows the path.
MPC on H-LIP
We solve the GPC on the H-LIP by formulating a Model Predictive Control (MPC) problem to online generate optimal step location for best-tracking the desired path.
The superscriptH-LIPis omitted in this part.
Optimization Variables: At each step, the next 𝑁 steps are planned. Thus, all the states ˜x𝑘, ˜y𝑘 and all the inputs𝑢
𝑥 , 𝑦
𝑘 are selected as the optimization variables, where 𝑘 =1, . . . , 𝑁. The current step is indexed as 1.
Cost Function:The cost function of the MPC includes two parts: one is encoding the tracking performance as the distance between the mass states and the desired global trajectory, and the other past is penalizing the step sizes. Again let𝑇 =𝑇SSP+𝑇DSP be the period of the walking. Then the look-ahead time horizon is 𝑁 𝑇. Recall that, inside each step, the trajectory of the mass at a specific time instant𝑡 can be expressed by a linear function of the states:
h
𝑐𝑥(𝑡), 𝑣𝑥(𝑡) i𝑇
= 𝐴𝑡(𝑡)x˜𝑘 +𝐵𝑡(𝑡)𝑢𝑥
𝑘, (9.5)
h
𝑐𝑦(𝑡), 𝑣𝑦(𝑡) i𝑇
= 𝐴𝑡(𝑡)y˜𝑘 +𝐵𝑡(𝑡)𝑢
𝑦 𝑘
, (9.6)
where𝑡 ∈ [𝑘 𝑇 ,(𝑘+1)𝑇], and 𝐴𝑡(𝑡), 𝐵𝑡(𝑡)are derived from the piecewise linear dy- namics of the H-LIP. Supposing𝑛points are sampled in the time horizon[𝑡0, 𝑡0+𝑁 𝑇] to represent the trajectory of the states, the cost function on tracking performance is
𝐽𝑡 =Í𝑛
𝑘=1(𝑐𝑥(𝑡𝑘)−𝑟𝑑
𝑥(𝑡𝑘))2+(𝑣𝑥(𝑡𝑘)−𝑣𝑑
𝑥(𝑡𝑘))2+(𝑐𝑦(𝑡𝑘)−𝑟𝑑
𝑦(𝑡𝑘))2+(𝑣𝑦(𝑡𝑘)−𝑣𝑑
𝑦(𝑡𝑘))2, where𝑡𝑘 = 𝑡0+𝑘𝑁 𝑇
𝑛 . Additionally, we add cost on the input to penalize large step sizes:
𝐽𝑢 =
𝑁
Õ
𝑘=1
𝑢𝑥
2
𝑘 +𝑢
𝑦2 𝑘
. (9.7)
The final cost function is a combination of the two:
𝐽MPC=𝐽𝑡+𝛼 𝐽𝑢, (9.8)
which is a quadratic function of all the variables. 𝛼 ∈Ris an coefficient to leverage the tracking and planned step sizes.
Constraint Encoding:The MPC on the H-LIP should be cognitive about the potential constraints that the robot has such as limited step sizes. Theoretically, this can be set by 𝑢H-LIP ∈ 𝑈 𝐾 𝐸. The easiest way is to let the robot face the direction of
walking since the step size in the sagittal plane is much less limited than that in the lateral plane. As the point mass model of the H-LIP has no definition of orientation, we add a trivial torso with no inertia on the H-LIP (Fig. 9.1 (a)) to indicate the orientation of the model. The transversal dynamics of the torso is trivial and does not affect the horizontal dynamics of the COM. Suppose the horizontal dynamics is always expressed in the inertial frame for the consistency of global path tracking, the turning motion affects available step size in the 𝑥 − 𝑧 plane and 𝑦 − 𝑧 plane.
Assuming the torso is controlled to be𝑟𝑑
𝜃(𝑡), the sagittal plane is aligned with the tangent line. The step length in the sagittal plane and the step width in the lateral plane can be expressed as
𝑠L =𝑢𝑥cos(𝑟𝑑
𝜃) +𝑢𝑦sin(𝑟𝑑
𝜃(𝑡)), (9.9)
𝑠W =−𝑢𝑥sin(𝑟𝑑
𝜃) +𝑢𝑦cos(𝑟𝑑
𝜃), (9.10)
where𝑢𝑥, 𝑢𝑦are the step sizes projected to the𝑥−𝑧and𝑦−𝑧planes, and𝑠L, 𝑠Ware the step length and step width in its sagittal and lateral planes, respectively. Then the step size constraints in the MPC are modified by the following two inequality constraints:
𝑠min
L ≤ 𝑢𝑥
𝑘cos(𝑟𝑑
𝜃) +𝑢
𝑦 𝑘sin(𝑟𝑑
𝜃) ≤ 𝑠max
L , (9.11)
𝑠min
W ≤ −𝑢𝑥
𝑘sin(𝑟𝑑
𝜃) +𝑢
𝑦 𝑘cos(𝑟𝑑
𝜃) ≤ 𝑠max
W , (9.12)
where 𝑠min/max
L , 𝑠min/max
W are the available step sizes in each plane, which are linear functions of the states. Additionally, the robot should avoid kinematic conflicts for foot stepping. It can be easily specified through enforcing a finite minimum step width in the lateral plane, which can be added into the above constraint by changing 𝑠min/max
W .
The dynamics in each step are encoded via linear equality constraints. Additional constraints include the initial state constraint, step size limits (input limits), and the step size difference constraint, which are constraints considered for the application of the robots. The step size limits come from the physical kinematic feasibility of the robot. The step size difference is that,|𝑢𝑘+1−𝑢𝑘| ≤ Δ𝑈whereΔ𝑈is a constant.
This constraint avoids the system to dramatically change step sizes consecutively, which may lead to undesirable behaviors on the robot.
MPC: We compactly present the MPC formulation for the H-LIP. At each step, a
Figure 9.1: (a) The H-LIP with a trivial torso walking in 3D. (b) An example of path tracking of the H-LIP in 3D (blue dashed line is the desired path)
constrained quadratic program (QP) is formulated and solved. The QP is as follows:
𝑢
𝑥 , 𝑦 1,..., 𝑁
,x˜1,..., 𝑁,y˜1,..., 𝑁 = argmin
{𝑢
𝑥 , 𝑦 1, ..., 𝑁
,𝑥˜1, ..., 𝑁,𝑦˜1, ..., 𝑁∈R8×𝑁
𝐽MPC (9.13)
s.t. x˜𝑘+1 =𝐴˜x˜𝑘 +𝐵𝑢˜ 𝑥
𝑘, 𝑘 ∈K y˜𝑘+1 =𝐴˜y˜𝑘 +𝐵𝑢˜
𝑦
𝑘, 𝑘 ∈K
|𝑢𝑥
𝑘+1−𝑢𝑥
𝑘| ≤ Δ𝑈 , 𝑘 ∈K Eq. (9.11), (9.12), 𝑘 ∈K x˜1 =x˜now,y˜1=y˜now,
where K = {1, . . . , 𝑁 −1}, ˜xnow,y˜now are the states of current step of the H-LIP.
The first solution of the step sizes[𝑢𝑥
1, 𝑢
𝑦
1] is applied at current optimization, which are used as the nominal step sizes in the H-LIP based stepping in Eq. (3.17).
Fig. 9.1 (b) shows an example of the optimized walking of the H-LIP with a torso in 3D for GPC. Here we assume that the torso orientation is aligned with the path direction but it can be freely decided. The feasibility of the MPC is not guaranteed in general, which should be cognitive by the high-level planer on the global trajectory. Additionally, we should note that the H-LIP based approximation reduces the available step sizes in the MPC. Robust MPC [83] can also be directly applied to the robot to directly generate the step size on the robot. We do not present these modifications in this chapter. Instead, we show that the proposed MPC with the H-LIP based stepping can be realized on the bipedal walking systems to track global trajectories in the following two sections.
(a) (b)
𝑥 𝑦
desired path
H‐LIP aSLIP
(a-v)
(a-u)
(b-v1)
(b-u1)
(b-v2)
(b-u2)
Path tracking
𝑥
aSLIP H‐LIP To 1m location
Figure 9.2: (a) Results of the fixed location tracking in terms of the forward positions 𝑥, forward velocities 𝑣 and step sizes𝑢𝑥. (b) Results of the path tracking with the velocities and step sizes in each plane. The black and blue indicate the results of the H-LIP and the aSLIP, respectively.