Autonomous Navigation of Underactuated Bipedal Robots
in Height-Constrained Environments ^††thanks: All authors are with Hybrid Robotics Group at the Dept. of Mechanical Engineering, UC Berkeley, USA. {zhongyu_li, zengjunsjtu, shuxiao.chen, koushils}@berkeley.edu

Motion planning for legged robots has been an attractive topic and usually involves planning in the configuration space. In some prior work, paths of the root of robots are firstly found, for example, by sampling-based methods such as RRTs presented in [4, 3, 5, 6]. Later, robot footholds are selected over uneven terrain and the configuration of the whole body is planned by imposing foot contacts and stability constraints, such as [7, 8, 5]. There are other approaches that choose to plan reachable and optimal footholds over terrains first, and then generate the whole body motion using constrained optimization, see [9, 10, 11, 12]. Besides stepping over uneven terrain, some approaches also consider avoiding obstacles in confined spaces as demonstrated in [1, 3, 13], but only simulated results are presented. Recently, a hierarchical motion planning framework and experiments on quadrupedal robots to travel in height-constrained environments is presented in [6]. In this work, a path of robot pose is firstly randomly sampled by RRT-Connect in [14], followed by path smoothing while avoiding collision through CHOMP in [15]. Footholds are selected via the method shown in [7] and posture of the robot is optimized again to adapt to the planned foot placement. However, this last step in the pipeline ignores potential collisions, which prevents the entire framework from making collision-free guarantees.

Moreover, the above-mentioned motion planning work for legged robots only considers planning in configuration space without the dynamics of the legged robot, which results in slow and statically stable gaits. This could also lead to failures in experiments as reported in [6].

More recently, a kino-dynamics model is leveraged in a Model Predictive Control (MPC) framework to enable a quadrupedal robot to autonomously avoid obstacles in confined spaces including height constraints by [16] using quasi-static gaits. However, collision-free navigation while considering dynamic feasibility for bipedal robots is more challenging. This can be solved by formulating online optimization problems that enforce the bipedal robot’s dynamics constraints. Using full-order dynamic models of high dimensional bipedal robots in online optimization is computationally expensive. A widely-used alternative approach is through simplified models which approximate the walking dynamics, such as the Linear Inverted Pendulum (LIP) models, see [17, 18], and Spring-Loaded Inverted Pendulum (SLIP) models, see [19, 20, 21, 22], based on stability criterion such as Zero Moment Point (ZMP) in [23] or Capturability in [24].

In some approaches, reduced-order models such as centroidal model in [2] or Center-of-Mass (CoM) model in [25] are utilized to optimize for CoM trajectories while enforcing contacts and ZMP constraints, see [26, 27, 28]. Among these, [2] enables person-sized humanoid robots to travel in confined spaces, such as an obstructed door, without collision in simulation. However, in implementations on physical robots, these methods usually limit the robot’s vertical movement, i.e. robot’s walking height is fixed to make the ZMP dynamics linear for online control, such as [29, 18, 30]. Dynamics of LIP without fixed height constraints is studied in [31] where a Variable-Height Inverted Pendulum (VHIP) is proposed to enable walking while changing heights for humanoids based on capturability in simulation. However, the introduction of variable height to the LIP model is more to improve the control performance on the robot rather than to change the robot walking height significantly and enable travelling underneath obstacles.

Cassie, which is more agile with linear feet, cannot use the above-mentioned ZMP-based approaches, see [18]. Trajectory optimization for Cassie is demonstrated in [32] and is formulated using MPC based on SLIP model in simulation. A LIP model based MPC is also developed in [33] for discrete-time safety-critical trajectory generation proposed in [34] for Cassie. But both of them lack experiments on hardware. MPC is also utilized in [35] on a Hybrid LIP (H-LIP) model of Cassie to track a simple global trajectory in experiments and to avoid nearby obstacles in simulation. There is a concurrent work in [36] on building navigation autonomy of Cassie using a RRT as a global planner and a Control-Lyapunov Function (CLF) as a local trajectory planner. But this work doesn’t consider obstacle avoidance during the local planning. Moreover, these works don’t consider the coupled walking dynamics between planar and vertical directions and don’t study autonomous navigation in confined or height-constrained spaces.

Furthermore, almost all prior work on using reduced-order models to generate trajectories for walking robots forces the robot to mimic walking patterns computed by reduced models, like [32, 31, 35]. Such an approach is effective but loses information and properties of the full-order model dynamics of the robots, thus leading to difficulties during experimental implementation on real robots, such as on Cassie as described in [32, 35]. As we will see, we leverage a reduced-order model to capture the control design and its limitation on the full-order model, enabling us to achieve more agile motions during navigation on Cassie.

Developing a reliable locomotion controller for a bipedal robot to follow the planned footholds and configurations is critical to complete the navigation pipeline. There is a large body of literature to tackle this problem, see [37, 29, 38]. Hybrid Zero Dynamics (HZD) presented in [39] offers a mathematically rigorous procedure to generate stable, fast and energy-efficient periodic gaits using the full robot model based on input-output linearization. Moreover, the stability of gait transition using HZD is studied in [40] and applied to the problem of navigation by transitioning between limit-cycles and validated on bipedal walkers in simulations, such as [41, 42, 43]. In experiments, HZD is validated on a range of bipedal robots with point or linear feet in [44, 45, 46], and more recently, on Cassie in [47, 48, 49]. Based on [47] and HZD, a walking controller that is able to control a bipedal robot to crouch is firstly developed in [48]. Variable walking height control problems are later explored by a reinforcement learning method in [50] and by an H-LIP-based approach in [51]. In this work, we use the HZD-based controller on Cassie constructed by [48].

The trajectory optimization is applied on vSLIP and is formulated using a collocation method. States $\mathbf{x}=[q_{x},q_{y},q_{z},q_{\phi},\dot{q}_{x},\dot{q}_{y},\dot{q}_{z},\dot{q}_{\phi}]^{T}$ and inputs $\mathbf{u}=[u_{x},u_{y},u_{z},u_{\phi}]^{T}$ are defined in the vSLIP model in Sec. IV. The nonlinear optimization problem is on $N+1$ discrete collocation nodes with time step $\Delta t=T/N$ and fixed time span $T$.

The time span for this optimization could cover multiple walking steps which are described in Sec. IV-B. At the end of each step, the foothold $\mathbf{f}$ in the next step will be updated by the foot placement heuristics (4).

Moreover, $\bm{\delta}$ is defined as $\bm{\delta}=[\bm{\delta}^{T}_{cfeas},\bm{\delta}^{T}_{obs},\bm{\delta}^{T}_{final}]^{T}$. This represent a vector of slack variables used in the constraints, and diagonal matrix $D$ has all positive entries and is sufficiently large to minimize the slack.

The constraints that the optimization problem (6) enforces during the navigation in height-constrained environments mainly contains the state and input bounds, initial and final state conditions, dynamic feasibility, avoidance for both ground obstacles and overhanging obstacles, and gait stability of the robot. These can be summarized as follows.

The role of the local planner is to generate a smooth local trajectory which complies to constraints to lead the robot to the given local goal that is 1 meter ahead. As shown in Table III, this planner uses a preview with $6$ walking steps elapsing 3 seconds, and replans around every 1 second. Such long-term preview brings benefits to the robot with longer prediction and therefore enables the robot to proceed to avoid obstacles and change walking height early with a larger safety margin. For example, as illustrated in Fig. 7, the height constraint (6f) is enforced on the last half of the planned local trajectory. This prevents the robot from being too late to crouch down and to avoid the height constraint. The target state $\mathbf{x}^{l}_{final}$ is set to $[q_{x}^{l},q_{y}^{l},q_{z}^{l},q_{\phi}^{l},\mathbf{0}]^{T}$, as marked as dark yellow in Fig. 7(a), where the desired position terms is the given local goal location, and the velocity term is zero. The given local goal is selected as one-meter ahead of the robot’s current position, and is found on the global path. Moreover $q_{z}^{l}$ is set to 1 m to encourage the robot to recover to normal walking height (1 m) if possible. As this 3-second local trajectory will be updated every 1 second, robot will only follow less than half of the trajectory. The second half of the local trajectory with zero terminal velocities serves as a backup plan to decelerate the robot and step in place in case of a planning failure.

However, this planner suffers from a drawback. The robot may deviate from the planned trajectory during the 1-second update due to tracking errors of the controller or due to environmental perturbations. Especially, it is risky to allow person-sized bipedal robots to follow a path without any feedback and correction for 1 second. Therefore, a reactive planner running at a higher frequency is needed.

The reactive planner is designed to run at 10 Hz in real time to enable the robot to track the planned local trajectory. The fast computational speed is achieved by only planning for a single step with a horizon length of 6. Besides the horizon difference, the reactive planner has several variances from the local planner. Firstly, as the planned reactive trajectory is short, satisfying the height constraints for half of the horizon may not be feasible. Therefore, as mentioned in Table III, (6f) is not enforced in this planner. Secondly, the target state for the reactive planner $\mathbf{x}^{r}_{final}$ is chosen from the local trajectory with a range of $0.3$ m, and not only includes target position but planned velocity, as shown in Fig. 7(a). Apart from these two changes, the reactive planner shares the same constraints and costs as the local planner, ensuring the reactive plan to meet all the constraints. The output from the local planner is a trajectory with 6 nodes, but it only passes the next state to the controller as command $\mathbf{c}=[\dot{q}^{d}_{x},\dot{q}^{d}_{y},q^{d}_{z},\dot{q}^{d}_{\phi}]^{T}=[\mathbf{x}_{1}(5),\mathbf{x}_{1}(6),\mathbf{x}_{1}(3),\mathbf{x}_{1}(8)]^{T}$ where the subscript 1 represents the first node after the initial node 0.

If the initial condition $\mathbf{x}_{init}$ is set to the observed current robot state $[\hat{q}_{x,y,z,\phi},\hat{\dot{q}}_{x,y,z,\phi}]^{T}$, the reactive planner is a nonlinear model predictive controller (NMPC). However, due to several factors such as the vSLIP only approximates the robot dynamics and the measured velocities are very noisy on the walking robot, NMPC doesn’t work well in the experiments on Cassie. Therefore, the initial condition is set to a combination of feedback and feedforward terms: the position part except $q_{z}$ in the $\mathbf{x}_{init}$ is measured while the rest is the last timestep output from the reactive planner, and the local and reactive planners share a same initial condition.

The optimization problems for the long-term planner and reactive planner are formulated using CasADi in [56] and solved by IPOPT in [57] with the initial guess linearly interpolated between the given initial condition $\mathbf{x}_{init}$ and target state $\mathbf{x}_{final}$.

In this manner, we obtain the major part of the proposed autonomy which is the pipeline to go from a given local goal to real-time control commands for walking robots. This is realized by using the cascading trajectory planners to optimize for a local plan with long preview and a reactive plan with fast replanning. The trajectory optimization schematic used in these two planners leverages vSLIP to ensure dynamic feasibility, collision-free safety, and gait stability for bipedal robots, and can be utilized online.

In order to utilize the spatial information for spontaneous planning, the 3D map is later translated to a 2.5D map where the minimum height of the voxels on the Octomap is recorded on the corresponding 2D cell on the grid, as exemplified in Fig. 8. The minimum height of the voxels over the ground at the same 2D position is termed as admissible height to the robot, because it describes the maximal walking height on that 2D cell.

Compared to Octomap, the 2.5D map has a size of 20 $m^{2}$ and has a lower resolution of 0.5 m. The admissible height recorded in this map is obtained by finding the lowest height of the 0.1 m resolution voxels inside the low-resolution cell, as shown in Fig. 8. Such low resolution grid is able to take the robot shape into account because Cassie’s footprint radius is around 0.2 m. In this way, each cell on this 2.5D map can be classified into four types of space for the robot with respect to its admissible height:

For optimization-based trajectory planners that only plan for short trajectories, the information over the entire map is not necessary. Therefore, a local map that only contains the environment information of the robot’s surroundings is built. In this work, the local map is designed to be a bounding box whose width is 1.2 m and length is 2.75 m, and is shown in Fig. 8 as the pink bounding box. The location and orientation of this local region can be well defined by the robot’s current position and the local goal for the local planner and can be updated in real time. The locations of all the obstacles, i.e., 0.5 m untraversable cells, inside this local map will be considered in the collision-free constraints via (6e) in Sec. V, which are termed as local obstacles in Fig. 8. The minimum admissible height $\underline{h}$ in this region will be added to the height constraint by (6f).

In the scenario where the robot is exploring an unknown environment, a global planner is needed to quickly find a collision-free path (a list of waypoints) to lead the robot to reach the goal location on the global map and should be able to replan when the map is updated. Moreover, in the height-constrained environment, we prefer the robot to avoid traveling underneath obstacles if the robot can walk around the obstacles. Therefore, an A* search presented in [61] that encodes the costs of different types of regions on the map is introduced to serve as the global planner. The cost $\chi(n)$ of each node $n$, i.e., traversal cell, during the search is defined as $\chi(n)=\gamma(n)+\eta(n)$ where $\gamma(n)$ is the cost of coming from the start node to the current node while $\eta(n)$ is the heuristic cost that is defined as

where $\eta_{d}(n)$ is the Euclidean distance to the goal location and $\eta_{h}(n)$ is the cost of admissible height of the node, i.e., a node that has lower admissible height will have a larger $\eta_{h}(n)$. In this way, the A* will prefer to include the node which has a larger admissible height into the path. Moreover, a node that has not been explored will be avoided if there is a well-known node around, and this is achieved by having a larger weight $w_{d}$ on the node that is unexplored. In this way, the A* search is able to find an optimal path that is shortest to the goal location while avoiding height-constrained and unexplored regions if possible.

This global planner returns a sequence of waypoints which are the 2D coordinates of the selected nodes in the planned path and these are updated at 1 Hz. The local goal that is ahead of the robot’s current position in a given range will be selected from this global path by a breadth-first search, as marked in Fig. 8. In this work, the local goal is chosen as 1-meter ahead of the current estimated robot position. Specifically, the nearest two waypoints around one-meter ahead are found first and then a linear interpolation between these two waypoints allows us to select the local goal. This local goal selected by the proposed approach is continuous with respect to the robot’s movement and is passed to the local planner as part of the target state via (6h) in Sec. V.

By now, the infrastructure for the autonomy is developed, and the trajectory planners and walking controller can be embedded in this framework to drive the bipedal robot to safely explore height-constrained environments as illustrated in Fig. 2. The entire pipeline is validated next in simulations and experiments in the following sections.

In this scenario, three obstacles are distributed in a cluttered space with 4 m width and 10 m length without any height constraints, as shown in Fig. 10(a). Each of the obstacles is in the shape of 0.5 m$\times$1.0 m and they form a maze for the robot. In order to test the ability of the autonomy to explore an unknown and complex environment, Cassie is initialized in front of the first obstacle which blocks most of the view of the robot and therefore the robot has a limited prior information about the entire maze. During the test, as illustrated in Fig. 10(a),10(b), the global planner is able to quickly reroute and create an updated path to avoid the new detected obstacle, while the local planners update in real-time to regulate the robot movement to avoid collision. Moreover, several decelerating maneuvers take place when the robot walks close to the obstacles to ensure the robot’s safety. This is realized by outputting a negative walking speed from the reactive local planner as presented in Fig. 10(c). Between 58 s to 69 s, Cassie enters a very narrow space whose size is only 0.5 m$\times$0.5 m, a similar size as the robot footprint, as marked in Fig. 10(b). At this time, the planners slowly change the robot’s movement using almost zero velocity commands, as shown in Fig. 10(c), in order to keep the robot safe. After the robot moves to a more open space, the planners quickly find a trajectory to lead the robot to get out of the current narrow space, which happens after 69 s in Fig. 10(c). As this environment doesn’t have any height constraints, the robot maintains a normal height of around 1 m all the time, as exhibited in Fig. 10(d). This experiment demonstrates the capability of the proposed autonomy of a large-scale humanoid robot to safely navigate in a tight space that has almost the same width as the robot’s footprint.

In this test, there is an arch-shaped obstacle with a height of 1 m, causing 0.75 m admissible height, along with two obstacles pinned on the ground. This is shown in Fig. 11(a). The robot is initialized right behind the first obstacle in order to have a limited view of the entire map, and the goal is set to be 8 m ahead of the robot’s start location, as illustrated in Fig. 11(b). After Cassie avoided the first obstacle and saw the overhanging arch obstacle and detected that the admissible height in this area is about 0.75 m, it starts to crouch down to use a walking height of 0.7 m to avoid collision with the ceiling, as illustrated in Fig. 11(d). Later, Cassie starts to enlarge to its normal walking height to 1 m after it passed through this arch. In the meantime, the second obstacle appears in the view of the robot and accordingly, Cassie starts to walk to its right to avoid it and reach its goal location, as shown in Fig. 11(c). Humanoid robots walking underneath similar scenarios are mostly presented in simulation in prior efforts such as [1, 2, 3], which use offline optimization with perfect knowledge of the environment and robot dynamics. This is one of the first experimental demonstrations of a person-sized bipedal robot safely navigating in a cluttered environment that has an overhanging obstacle through real-time online planning.

In order to show the autonomy framework is capable to operate in more constrained environments where the robot needs to keep a low walking height, two arches are placed in the two ends of the 4 m$\times$10 m space, respectively, representing the entrance and exit of this constrained area, as demonstrated in Fig. 12(a). Moreover, a 0.5 m$\times$0.5 m obstacle is placed inside of this region. In the experiments, Cassie changes its walking height to 0.7 m in order to walk through the first arch, starting after 11 s as shown in Fig. 12(d). During the robot’s travel inside this tight space using a 0.7 m walking height illustrated in Fig. 12(b), there are several deceleration happening in order to finely regulate Cassie’s position to avoid collision with the walls and obstacles nearby, such as 26 s, 38 s, and 42 s in Fig. 12(c). Since there are no more overhanging obstacles detected, the planners start trying to recover Cassie’s height to a normal height, as shown between 21 s to 27 s in Fig. 12(d). However, at that time, the robot has traveled forward and detected the second arch, therefore, the planners quickly replan to keep the robot at a low walking height. After the robot exited this area after 50 s, the robot recovers to a normal walking height. However, as shown in Fig. 12(b) and attached video, Cassie passed the goal location while it is enlarging the walking height. The planner is then able to correct the robot back to the goal location by sending backwards velocity commands as recorded after 65 s in Fig. 12(c). This experiment first exhibits a capability of the proposed autonomy for bipedal robots to keep low walking height under multiple height-constrained spaces while reactively avoiding obstacles.

As demonstrated in Fig. 13(a), this environment consists of two parts: a lab space that has a size of 4 m$\times$10 m and a corridor that is outside of the lab and has a width of 4 m. There is a door left open connecting these two space and an arch-like obstacle is placed in front of the door. Such an overhanging obstacle adds height constraint to the doorway. The robot is initialized inside the lab and the goal location is set to be outside the lab, as shown in Fig. 13(b). After the robot detected the obstacles and the doorway, the planners start to drive Cassie to its left in order to move towards the target, as shown in Fig. 13(c). In the meantime, Cassie starts to crouch down to avoid the arch obstacle over the doorway. After Cassie moved outside of the lab, obstacles in the corridor, such as the stanchion and the opened door shown in Fig. 13(a) (View 2), can be detected and robot therefore changes its previous plan and keeps moving to its left to avoid these new-detected obstacles. Moreover, when the robot is moving across the doorway, the detected height-constrained grids surrounding the robot are included in the local map, even though the robot is not walking exactly underneath them. Therefore, as illustrated in Fig. 13(d), Cassie keeps a low walking height between 15 s and 30 s in this cluttered environment until 30 s when it travels to an open space and recovers to a normal height. This shows a successful demonstration of a large-scale humanoid robot walking underneath an obstructed door and traveling between different rooms.