Fast Online Optimization for Terrain-Blind Bipedal Robot Walking With a Decoupled Actuated SLIP Model

We present an online optimization algorithm which enables bipedal robots to blindly walk over various kinds of uneven terrains while resisting pushes. The proposed optimization algorithm performs high-level motion planning of footstep locations and center-of-mass height variations using the decoupled actuated spring-loaded inverted pendulum (aSLIP) model. The decoupled aSLIP model simplifies the original aSLIP with linear inverted pendulum (LIP) dynamics in horizontal states and spring dynamics in the vertical state. The motion planning can be formulated as a discrete-time model predictive control (MPC) problem and solved at a frequency of 1 kHz. The output of the motion planner is fed into an inverse-dynamics–based whole body controller for execution on the robot. A key result of this controller is that the feet of the robot are compliant, which further extends the robot’s ability to be robust to unobserved terrain variations. We evaluate our method in simulation with the bipedal robot SLIDER. The results show that the robot can blindly walk over various uneven terrains including slopes, wave fields, and stairs. It can also resist pushes of up to 40 N for a duration of 0.1 s while walking on uneven terrains.


I. INTRODUCTION
To make bipedal robots really suitable for many applications, it is important that they can go out of the lab and walk in the complex real world environment.Real world environments contain various kinds of uneven terrains: slopes, wave fields, stairs, etc.Most existing controllers that allow a bipedal robot to walk over uneven terrains require predefined footstep locations or exact information about the terrain height changes [1], [2], [3].However, even with most advanced sensors there are some uncertainties on the perception of the terrain.In contrast, humans can easily walk on uneven terrains, such as outdoor environments.without extra thought or careful planning.Therefore to have a reactive controller that is robust to unobserved uneven terrain changes is important.
The Spring Loaded Inverted Pendulum (SLIP) model has become a popular model for walking and running in the legged robotic research [4].Despite its simplicity, it has been proven to capture essential dynamics properties of walking and running [5].The standard setting of SLIP model is energy-conservative: it assumes there is no energy loss at impact.Though this assumption simplifies the control analysis, it doesn't resemble the reality.There is energy loss on physical systems, and robots [6] [7] designed to approximate SLIP dynamics have added actuation to compensate for the energy dissipation.As a result, the actuated Spring Loaded Inverted Pendulum (aSLIP) model [8] is proposed for a better approximation of the real robot dynamics.The aSLIP model has been successfully used to design controllers not only for SLIP-like robots [9] [10] [11] but also as a template model for humanoid robots on uneven terrain walking [12].
An important step in making a controller reactive is to achieve real-time constrained optimization.However, due to the nonlinear dynamics that arise from the 3D aSLIP model, fast optimization is difficult.[12] used a gait synthesized from a library of gaits acquired from off-line optimization, but this requires a large computation load offline and cannot cover all possible situations.[9] decoupled 3D aSLIP model to facilitate fast computation, but the continuous dynamics of decoupled aSLIP model is still nonlinear and there is no theoretical guarantee of fast convergence.[13] used the simpler Linear Inverted Pendulum (LIP) model to design a reactive controller for flat ground walking, but this model cannot be applied to walking on uneven terrains.
Our paper proposes a reactive controller that enables robots to blindly walk over uneven terrains by optimizing horizontal footstep locations and center of mass (CoM) height online.Under the assumption that the angle the modelled inverted pendulum makes with the vertical is relatively small, we can decouple the 3D aSLIP model into a 1D actuated spring model responsible for z direction and 2D LIP model responsible for x and y directions.The dynamics of all three dimensions can be written as linear equations in a discrete-time state space manner.We formulate the online step planner as a discrete-time model predictive control (MPC) problem and solve it by Quardratic Programming (QP).To facilitate fast computation, the spring length is constrained to change linearly.As a result, we get a step planner which runs at a frequency of 1000 HZ.The step planner using a simple model is embedded into the inverse-dynamics based whole body controller [14] [15] which tackles the inconsistency between the simple model used in high level planning, and full robot dynamics.With the whole body controller, the feet of the robot show great compliance and this helps the robot to transition between different terrains without any information about the terrain.Due to the fast execution frequency and compliance of the foot, our proposed controller enables the robot to blindly walk over various kinds of moderately uneven terrains including slopes, wave fields and stairs.Our controller can also handle disturbances from all direction while walking on uneven terrain.We validated our controller on the straight-legged bipedal robot SLIDER [16] in Gazebo simulation.
The main contribution of the paper is the reactive controller which enables bipedal robots to blindly walk over various uneven terrains while resisting disturbances.With reasonable assumptions, we can decouple the nonlinear 3D aSLIP dynamics and optimize the footstep location and CoM height separately.By proper reformulation, the control of the vertical dynamics can be formulated as QP and therefore a solution is guarenteed to be found.Our controller is simple to implement and computationally efficient; the optimization of both vertical and horizontal motions can be solved by QP and executed at 1k HZ 1 .

A. The SLIDER robot
SLIDER is a knee-less bipedal robot designed by the Robot Intelligence Lab at Imperial College London, as shown in Fig. 2. SLIDER is 1.2 m tall and has 10 Degrees of Freedom (DoF), namely hip pitch, hip roll, hip slide, ankle roll and ankle pitch on each leg.The robot is very lightweight (14.5 kg in total) and most of its weight is concentrated at the pelvis.The legs are made of carbon fiber reinforced polymer and each leg weights only 0.4 kg.The prismatic knee joint design is an unique feature of this robot that differentiate it from many other robots with anthropomorphic design.Due to its sliding mechanism and lightweight leg design, SLIDER can be well approximated with an aSLIP model which can greatly simplify the planning and control problem.Moreover the lightweight leg with a large range of motion makes the robot suitable for agile locomotion.

B. The Control Hierarchy
The controller presented has a hierarchical structure as shown in Fig 3 .Due to the computational complexity of using full body motion planning, a high level planner optimizes online only the foot placement in x, y directions and the CoM height using the decoupled aSLIP model.The low Fig. 2: The dimension and joint configuration of the SLIDER robot.level whole body controller [14] [17] tracks the trajectory generated by high level planner.The whole body controller considers the full dynamics of the robot and generates the consistent torque command for each joint.

III. ONLINE FOOTSTEP PLANNING
Motion planning with the full dynamics of the robot is too computationally expensive to be executed at a fast frequency.Instead we use reduced order models for online footstep planning: the LIP model is used to plan footsteps in the horizontal plane and the 1D actuated spring model is used generate in the vertical direction.

A. The Decoupled aSLIP Model
The aSLIP model is different from the classical SLIP model in that the aSLIP model is a combination of the SLIP model and a virtual linear actuator, as shown in Figure 4. Introducing such a virtual actuator gives the aSLIP model the capability to actively modify the reference spring length during each walking phase and makes aSLIP model better at handling vertical changes than the SLIP model.However, similar to the SLIP model, the dynamics of aSLIP model presented above is nonlinear because of the coupling in vertical and horizontal dynamics, and the exact dynamic equation needs to be numerically integrated.An approximate model which decouples the CoM states into horizontal and vertical directions will be used in this paper.This decoupled approximate model simplifies the nonlinear dynamics of the original aSLIP model and makes the fast online optimization of footstep locations and CoM height possible.To make the approximation valid, two assumptions are made.The first assumption is that the vertical deviation of CoM over each walking period is small relative to the height of the CoM, such that individual steps can be modelled using the LIP model.The second assumption is that the angle the inverted pendulum makes with the vertical is small, such that the spring contributes to the CoM's vertical behavior only.This is reasonable, considering a typical small step length is small

B. Vertical Dynamics of decoupled aSLIP Model
The dynamics of the decoupled aSLIP model in vertical direction can be calculated as 1, where z denotes position of CoM in vertical direction, r denotes the reference spring length, and k is the stiffness of the spring.
Compared with the classical SLIP model, the reference spring length r(t) is treated as a time-varying optimization variable, which is contributed by the virtual linear actuator.We also constrain r(t) to change linearly between initial and final values over the phase to facilitate fast computation.Therefore, the reference spring length should satisfy the condition below, where, r 0 and r T represents the reference spring length at the start and at the end of one step respectively, T is the step duration.We can formulate the dynamic equation ( 1) into a state space equation as, where the states of CoM in vertical direction are defined as Z = [z, ż] T and we define ω z = k m .Further, we can denote r − g ω 2 z as u z , and equation (3b) can be written into a linear state space equation as, We discretize equation (3b) with sampling time T s and can obtain, where, We can also write this equation as: where Z k and Z k+1 are CoM states in the vertical direction at time k and k+1 respectively, A z (T s ) and B z (T s ) are time-varying matrices, u z,k is the control input for the spring at time k.Therefore, N future steps can be computed given fixed sampling time T s : where N is the number of steps to be optimized which should be more than 1.In this formulation, the control input vector for the spring Ū = [u z,0 , u z,1 , ...u z,N −1 ] is the Fig. 4: The aSLIP model.This model combines the SLIP model with a virtual linear actuator.θ is the angle the inverted pendulum makes with the vertical.optimization variable.Equation (5a) can also be written in a matrix form for MPC: where Z and Ūz are the vectors of states and control inputs in z direction.

C. Horizontal Dynamics of Decoupled aSLIP Model
Under the assumption that the leg angle θ is relatively small and therefore the horizontal and the vertical dynamics can be decoupled, the horizontal dynamics then becomes a classical Linear Inverted Pendulum (LIP) model.Similar to [13], given the continuous dynamics of LIP model in the x direction, the discrete state space equation can be reformulated into a linear state space as below, with state defined as X = [x, ẋ] T , and input u x defined as the footstep positions in x direction. Where Similarly, states in the predicted time horizon can also be obtained by, where X and Ūx are the vectors of states and control inputs in x direction.The dynamics in the y direction has identical formulation to the dynamics in the x direction.

D. Foot Placement and CoM Trajectory Optimization
The footstep planning can be formulated as a QP problem due to the fact that both horizontal and vertical dynamics are linear and the constraints are linear.The formulation is: where Γ = Γ 1 + Γ 2 + Γ 3 is the cost function term, X and u are general representations of states and control inputs respectively and p j is the left or right foot position.
The cost formulation of the vertical and horizontal control problems are both composed of the three parts: Γ 1 , Γ 2 and Γ 3 .The first part is minimizing the difference between the predicted state and the referenced state, which drives the CoM state to reach the desired one from the given current state.The formulation is: where P and Q are the weight matrix respectively.For the horizontal trajectory planning, we only care about the velocity of CoM in both directions, because we want a reactive footstep planner which is not constrained by a absolute reference trajectory.From another point of view, tracking the reference velocity is equivalent to tracking the relative CoM reference position.X ref is easy to define with the desired sagittal and frontal velocity.However, the reference velocity in vertical direction is not constant during one step.To get the reference velocity, we can integrate equation ( 1) and get the continuous dynamics equation: where In this equation, z 0 and ż0 are the CoM vertical position and velocity at the start of the current step respectively.Then the time-varying reference velocity in z direction can be derived from the continuous dynamics.
The second part is minimizing the difference between the reference control input u ref k and the optimized control input u k and R is the weight matrix.
For the reference control input in z direction, recall that the spring length is constrained to change linearly, as indicated by equation ( 2).If we denote ∆r i as the change of spring length during one sampling time T s at step i, then it is clear that ∆r i is constant throughout step i, and the reference control input in z direction Ū ref z is therefore obtained as below, where N r is the remaining number of sampling points for the current step and N s is the number of sampling points for one step.N steps is the number of predicted steps.
For the horizontal trajectory planning, the input of the system is the footstep position.The reference control input in x direction Ū ref x is then obtained according to the current support foot position P 0 and the difference between two consecutive steps ∆P x,i .
The third part is tracking the desired change of the control input between two consecutive steps, with d i denoting the desired difference and W denoting weights.For the vertical trajectory planning we set the desired difference to equal zero, this means the footstep planner tries to keep a constant CoM height during one step to make the decoupled aSLIP assumption valid.
For horizontal trajectory planning, In x direction, d x i is the step length which is calculated by the desired speed multiplied by the step time.In y direction, d y i is defined as d y i = d step * 2 * (−1) j , where d step is the desired inter-feet clearance distance, and j is the flag for the supporting foot, 0 stands for left support and 1 stands for right support.This relative distance regularization term is introduced to keep the feet away from each other to avoid self-collision.Morever, as this part of the cost function only includes relative distances, it helps to produce a reactive footstep planner which keeps the robot walking even when unexpected disturbance is applied.
The reachability constraint is responsible for making sure the footstep location is physically possible, where L is the nominal offset of the foot position from the CoM of the robot, r x,y is the reachability constraint in the x and y directions and c is the CoM position.

IV. TRAJECTORY TRACKING
The low level trajectory tracking generates corresponding torques for each joint to minimize the difference between the actual body trajectory and the desired trajectory given by the high level trajectory planner.Since the trajectory planning does not include the full body dynamics, the dynamic inconsistency of the torque command generated by the high level planner is significant.The whole body controller solves the inverse dynamics based on the full robot dynamics.

A. Rigid Body Dynamics
The walking robot is modeled as a floating based rigid body system with coordinates q = [q b , q r ].Here, q b ∈ R 7 represents the position and orientation of the floating base using quaternions, and q r ∈ R 10 represents the joint configuration.Inspired by [14], the full dynamics can be decomposed into an underactuated part and an actuated part: where M , H, S a , τ , J and f are the mass matrix, the Coriolis force vector and the gravitation force vector, the actuator selection matrix, the joint torques vector, the stacked contact Jacobian and the reaction force vector respectively.The subscript f and a, indicates the floating part and the actuated part respectively.

B. Contact Force Constraint
Friction is a important contributing factor to the stability of walking to prevent the foot from slipping.The criterion is that the contact force should satisfy the inequality below, where µ is the friction coefficient.However, this formulation is a quadratic inequality constraint and rises the problem of constraining fast optimization.Therefore, we use the pyramidal friction model which is a linear inequality constraint for fast optimization:

C. Swing Foot Trajectory Generation
The swing foot trajectory is generated using a fifth order polynomial.The start and final positions, velocities and accelerations are specified and a parametric quintic curve is generated in x, y and z directions.The polynomial in z direction has two halves, with the predefined foot height and zero velocity at the midpoint.The start and final positions in z direction are calculated using the estimated CoM z position at the start of current step subtracted by a constant offset.Because the robot is walking blindly we don't define a trajectory for the foot orientation, and this allows the foot to be compliant to a range of surfaces.

D. Whole Body Controller
The whole-body controller takes the responsibility of computing the joint torques to achieve the desired motions defined in operational space while respecting a set of constraints.The tasks of interest in this paper are the CoM position, the pelvis orientation, the angular momentum of the robot, the feet positions and orientations.The command of each task is comprised of a desired acceleration as a feedforward term and a state feedback term to stabilize the trajectory.
The task for the linear motion can be expressed as: J T q = ẍcmd − JT q, ẍcmd = ẍdes + K pos P (x des − x) + K pos D ( ẋdes − ẋ), where J T is the translational Jacobian for the task and x is the actual position of the link.For the task of angular momentum the centroidal momentum matrix [18] is used as the task jacobian.In uneven terrain walking the angular momentum task is defined as a damping task that damps out excess angular momentum to make the walking robot more stable.
For the task of the angular motion, the command can be formulated as: J R q = ωcmd − JR q, ωcmd = ωdes +K ang P (AngleAxis(R des R T ))+K ang D (ω des −ω), where J R is the rotational Jacobian for the task, R and R des denote the actual and desired orientation of the pelvis link respectively, AngleAxis() maps a rotation matrix to the corresponding axis-angle representation, ω ∈ R 3 is the angular velocity of the link.The angular motion is needed because when walking on the uneven terrain, the pelvis orientation needs to be regulated around a nominal orientation to maintain a good posture.We set small values for K ang P and K ang D in the foot orientation task to make the ankle compliant so that the foot can adapt to different terrains.
The whole body controller can be formulated as a QP problem below: where A is a stack of the Jacobian matrices for the tasks of interest, X cmd is a stack of the commanded accelerations and W is the weighting matrix, P denotes the linearized friction cone.We treat the unilateral contact constraint as a soft constraint by simply assigning a large weight on the desired zero acceleration of the foot [9] [19].This can speed up the optimization and it is reported in [17] that this gives better stability.The output joint torque commands τ at each control iteration can be computed by V. RESULTS This section discusses implementation details and simulation results of SLIDER robot walking on different kinds of uneven terrains including slopes, wave fields and stairs.All the experiments are shown in detail in the accompanying video.Fig. 7: With the whole body controller the foot is compliant and can do transitions to different terrains even the robot doesn't know how the terrain looks like.This makes the robot robust to unobserved terrain changes.

A. Implementation
Both the high level footstep planner and low level trajectory tracking controller are implemented in C++ for realtime performance.In the whole body controller we use Pinocchio [20] to compute the full rigid body dynamics and qpOASES [21] to solve the related QP problem in both levels.All experiments were carried out in robot simulation environment Gazebo [22] with the physics engine ODE [23], using the full dynamics of the real SLIDER robot.The communication through different levels of the control hierarchy is achieved through ROS.
The decoupled aSLIP parameters are set to match with the physical SLIDER robot, here r 0 = 0.715 m, m = 14.5 kg, k = 1470 N/m.The step duration is chosen to be 0.7 s and the footheight in the swing foot trajectory generation is 5 cm.The sampling time in the discrete-time MPC is 0.1 s in x and y directions and 0.05 s in z direction.The motion planner predicts 4 steps in horizontal states and 1 step in the vertical state.We use the same parameters among all walking experiments except that the forward velocity is different.In the stair walking experiment the robot has to walk faster otherwise the foot might hit the edge of the stair.

B. Flat Ground Walking
We first validate our approach on flat ground.Because there are no changes in z direction on flat ground, the z position of CoM oscillates around r 0 , as shown in Fig. (6a)(6e).

C. Walking on Smooth Uneven Terrain
We then validate our approach on slopes and wave fields where the change of terrain height is smooth.With a forward velocity of 0.3 m/s, the SLIDER robot can walk on a slope with 15 degree and a wave field approximated by slopes with 3 different angles: 15, 10 and 5 degrees, as shown in Fig. (6b) (6c)(6f)(6g).In the experiment the whole body controller plays an important role in making the robot remain robust to unobserved terrain changes.By properly tuning the PD gain of the foot orientation task in the whole body controller, the foot is compliant to adapt to large terrain changes, as shown in Fig. 7.
Further experiments like pushing the robot while it was walking on uneven terrains were performed.As shown in Fig. 8, the robot was pushed in x or y direction twice while it walking on a wave field.When the robot was pushed in x direction, the robot quickly took a large step in x direction to regulate the CoM velocity back to the desired velocity.There is also a big change in z direction because the wave field is ascending in x direction when the robot got pushed, but the motion quickly got stabilized.In the case of y direction push, the robot also took a large step in y direction and stabilized in one step.As the terrain height is only changing along x direction, there is no big change in z direction in this case.

D. Walking on Discrete Uneven Terrain
Discrete uneven terrain provides bigger instantaneous changes to terrain height compared with smooth uneven terrain.As shown in Fig. (6d), the robot walked blindly over a set of stairs with a biggest height change of 3 cm.The robot walked with a forward velocity of 0.6 m/s and a foot height of 5 cm in swing foot phase.As shown in Fig. (6h) the CoM Z position exhibits large variations when walking over stairs but was stabilized quickly, which demonstrates the robustness of our approach.In our experiment the highest steps the robot can walk over is 3 cm, for higher steps the foot would hit the edge of the stair and get stuck.

VI. DISCUSSION
The proposed methods is successfully applied to SLIDER robot and demonstrates its performance of 3D blind walking on various uneven terrain and robustness to disturbances.Because of SLIDER's lightweight legs, the robot can perform fast leg movements which helps to stable the robot.However the proposed method is general and can be applied to various legged robots.
There are other techniques dealing with variable height CoM trajectories [24], [25].But these techniques either requires the controller to be terrain-aware to plan future CoM motion or having nonlinear dynamics so a solution is not guaranteed.Our proposed controller keeps the average CoM height constant in one step by following a spring dynamics.This has two advantages: firstly the controller doesn't require terrain information, secondly the spring dynamics enables the vertical compliance of the robot so that the robot is robust to unexpected height variations.
The author observed that the tracking error is larger when the robot walks down the wavefield or stairs than walking up, as shown in Fig. 6g, Fig. 6h.This happens because the feet are still in the air at the end of one step when the robot walks down.The unexpected sudden drop of the feet at the start of next step gives the robot a large impact.For robot walking upwards, the effect of early touchdown can be alleviated by the compliant feet and vertical compliance of the CoM.To improve, a controller with contact detection or terrain information can achieve a smaller tracking error.

VII. CONCLUSIONS AND FUTURE WORK
We present a highly reactive controller which enables robots to blindly walk over various kinds of uneven terrains while resisting pushes.The high level motion planner performs fast online optimization on footstep locations and CoM height, and the low level inverse-dynamics based whole body controller tracks the trajectory.We show in simulation that using this controller, the robot, SLIDER, can walk over wave fields and stairs without any terrain information and can also recover from pushes while walking.Future work will involve implementing our approach on the real SLIDER robot, and also incorporating perception information into the high level step planner.

Fig. 1 :
Fig. 1: SLIDER robot walks on uneven terrain.The three traces are trajectories of CoM, left foot and right foot respectively.

Fig. 3 :
Fig.3: The hierarchical controller with the execution frequency of each level.The high level trajectory planner takes the desired velocity as input and generates the optimal CoM trajectory along with the foot placement.The low level whole body controller[14] [17] considers full dynamics of the robot and tracks the trajectory at a frequency of 1k HZ. compared to the height of the CoM.These assumptions are summarised in Fig.4.

Fig. 6 :
Fig.6: Snapshots and plots of SLIDER blindly walking on different kinds of terrains, with a constant forward velocity of 0.3 m/s (a ∼ c) and 0.6 m/s (d).On top row, from left to right: (a) ∼ (d): Walk on flat terrain.Walk on a slope with an angle of 15 degrees.Walk on a wave field approximated by slopes with 3 different angles: 15, 10 and 5 degree.Walk over stairs, the successive elevation changes are +2 cm, +2 cm, +3 cm, +3 cm, -2 cm, -3 cm, -2 cm, -3 cm.On bottom row, from left to right, (e) ∼ (f): The plots of desired and measured CoM z position in corresponding scenarios.

Fig. 8 :
Fig. 8: Plots showing the states of CoM when the robot got pushed in x or y direction while walking on a wave field at a speed of 0.3 m/s.The pushes are indicated with circles.All impulses were applied with a value of 40 N for a duration of 0.1 s.Top row: states of CoM when got pushed in x direction.Bottom row: states of CoM when got pushed in y direction.