Autonomous robots that can safely travel in unknown and unstructured environments with obstacles distributed in three dimensional space can be useful for various practical applications such as navigating in narrow complex spaces with varying ceiling heights in tunnels or caves or in collapsed buildings after disasters. This motivates us to have robots that are able to crouch down and travel underneath obstacles in confined spaces. Moreover, there are few wheeled mobile robots that are capable of changing their intrinsic dimensions while moving around obstacles due to their low Degree-of-Freedoms (DoFs) and design limitations. This restrains their usage in complex environments. Bipedal robots, on the other hand, are able to dynamically change their configuration by stretching or crouching to navigate environments with height constraints. However, bipedal robots are high-dimensional, nonlinear and underactuated systems with hybrid dynamics. Trajectory generation for such robots with guarantees of dynamic feasibility while safely avoiding obstacles is challenging. There is existing work trying to tackle these All authors are with Hybrid Robotics Group at the Dept. of Mechanical Engineering, UC Berkeley, USA. {zhongyu li, zengjunsjtu, shuxiao.chen, koushils}@berkeley.edu 1 https://youtu.be/Da0tebC3WuE Fig. 1: A bipedal robot Cassie autonomously travels in congested height-constrained environments while maintaining walking gait stability and collision-free safety. The vision-aided autonomous navigation framework developed in this paper enables a large-scale underactuated bipedal robot to not only navigate around obstacles but also to crouch to go under height-constrained obstacles.
problems for bipedal robots in confined environments [1]- [3], but these are only validated in simulated environments. According to the best of our knowledge, navigation with height constraints for large-scaled bipedal walking robots in the real world has not been experimentally demonstrated.
In this paper, we seek to ascertain the feasibility of utilizing a dynamic bipedal walker Cassie to explore and navigate in congested and unknown environments, as shown in Fig. 1. To enable bipedal robots, like Cassie, to navigate autonomously in height-constrained spaces without prior knowledge of the environment, three major problems should be addressed. Firstly, a collision-free path should be found and updated quickly from the current position to the goal location while exploring in the unknown space with varying admissible heights. Next, a local trajectory for the bipedal robot is required with consideration of obstacle avoidance and robot's physical limitations. Moreover, the planners also need to ensure the gait stability of the underactuated bipedal robot, i.e., the planners should respect the underactuated dynamic coupling and prevent the robot falling and hurting nearby humans and the robot itself. Finally, to enable the robot to reliably follow the planned motions, we need a walking controller that can change the robot's configuration, e.g., walking height, while maintaining stable walking gaits with different velocities. While these problems are challenging, a solution that addresses them will enable end-to-end navigation for bipedal robots for autonomous and safe travel in unknown height-constrained environments.
A. Related Work 1) Motion Planning for Legged Robots: 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 [3]- [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 [5], [7], [8]. 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 [9]- [12]. Besides stepping over uneven terrain, some approaches also consider avoiding obstacles in confined spaces [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 [14], followed by path smoothing while avoiding collision through CHOMP [15]. Footholds are selected via [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].
2) Trajectory Optimization for Bipedal Robots: 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 in [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) [17], [18] and Spring-Loaded Inverted Pendulum (SLIP) [19]- [22] models, based on stability criterion such as Zero Moment Point (ZMP) [23] or Capturability [24].
In some approaches, reduced-order models such as centroidal model [2] or Center-of-Mass (CoM) model [25] are utilized to optimize for CoM trajectories while enforcing contacts and ZMP constraints [26]- [28]. Among these, researchers [2] enable 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 [18], [29], [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 [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 safety-critical trajectory generation for Cassie. But both of them lack experiments on hardware. MPC is also utilized in [34] 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 [35] 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 [31], [32], [34]. 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], [34]. 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.
3) Bipedal Locomotion Control: 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 [29], [36], [37]. Hybrid Zero Dynamics (HZD) [38] 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 [39] and applied to the problem of navigation by transitioning between limit-cycles and validated on bipedal walkers in simulations [40]- [42]. In experiments, HZD is validated on a range of bipedal robots with point or linear feet [43]- [45], and more recently, on Cassie [46]- [48]. Based on [46] and HZD, a walking controller that is able to enable a bipedal robot to crouch is firstly developed in [47]. Variable walking height control problems are later explored by a reinforcement learning method [49] and by a H-LIP based approach [50]. In this work, we use the HZD-based controller on Cassie constructed in [47].
The primary contributions of this work are: (1) the design and development of one of the first autonomous navigation framework for underactuated bipedal robots to navigate unknown height-constrained environments. In this framework, search-based A* and collocation on a reduced-order dynamical biped model work as global and local planners, respectively. (2) A vertically-actuated SLIP model (vSLIP) is introduced to describe the coupled walking dynamics for variable walking heights and walking velocity and to reduce the online computation burden. Unlike previous attempts to force physical robots to mimic reduced-order models, we introduce a modelling method in the vSLIP to capture the properties and limitations of HZD-based locomotion control that leverages the full-order dynamics model. (3) Such a reduced-order model is utilized in hierarchical collocationbased trajectory planners to find trajectory profile and realtime control commands that also respect gait stability and safety constraints for bipedal robots. (4) Using the proposed framework, we present experiments on the bipedal robot Cassie illustrating safe navigation in height-constrained environments. This framework is one of the first to demonstrate considerable level of autonomy to enable a bipedal robot to travel without collision nor losing balance while exploring various unknown confined environments. This proposed autonomy serves as a milestone towards real-world deployment of person-sized bipedal robots like Cassie.
The paper is structured as follow. The overview of the proposed navigation autonomy is introduced in Sec. II. In Sec. III, the Cassie experimental platform is introduced and its HZD-based variable walking height controller is briefly discussed. The gait stability constraint of this walking controller is also introduced in this section. Sec. IV develops vSLIP model to capture variable height dynamics and limitations of HZD-based controller, which is later utilized in optimization-based local trajectory planners developed in Sec. V. Robot localization, mapping, and global planning are presented in Sec. VI. Simulation validation and experimental results are presented in Sec. VII and Sec. VIII, respectively, and conclusion and future work are discussed in Sec. IX.
The proposed autonomous navigation framework for the Cassie bipedal robot is illustrated in Fig. 2. The core part of this autonomy is the hierarchical design of the planners. This includes a (1) global planner to search for a collisionfree path on the global map, (2) a local planner to generate dynamically-feasible trajectory to lead the robot to track a local goal location from the global planner while avoiding nearby obstacles, and (3) a reactive planner outputting realtime commands to the walking controller while providing dynamic feasibility and safety guarantees.
In this framework, the environment is perceived by a RGB-Depth camera, and the detected obstacles are classified as non-traversal areas and height-constrained areas (Sec. VI-B). The detected obstacles are registered into a 2.5D grid map [51] which encodes the information of admissible walking height in each grid, and the map is updated at 1 Hz. Moreover, the robot's odometry is estimated simultaneously by a tracking camera based on Visual Inertial Odometry (VIO) [52] at 30 Hz.
After being given a goal location, a global planner using A* is designed to find a path of collision-free waypoints on the map from the robot's current position to the goal (Sec. VI-C). A local goal is found on the global path within a range of the robot's current position, e.g., a local goal which is 1-meter ahead of the current robot position, to send to the local planner. The global planner and the local goal are updated at 1 Hz in order to synchronize with the local planner.
After being given the position of the local goal from the global planner, an optimization-based local planner (Sec. V-B.1) uses vSLIP dynamics to compute a dynamically feasible state profile to reach the local goal. This local planner not only respects the robot's gait stability constraints but also ensures the robot avoids obstacles. The resulting trajectory from this local planner lasts 3 seconds and is replanned every 1 second in this work. This local planner provides a smooth trajectory to lead the robot to reach the local goal that is 1 meter ahead. However, the 1 Hz frequency of this planner is still not fast enough for an agile dynamic walker when it is exploring an unknown and confined environment. Therefore, a reactive planner (Sec V-B.2) that replans at higher frequency, 10 Hz, is developed. This reactive planner uses the same optimization schematic as the local planner but has a shorter-term preview, replans at 10 Hz and outputs a short-term discrete state profile. Its target state is chosen as 0.3 m ahead of robot's position in the planned state profile from the local planner. It only sends the walking velocity and walking height from the closest planned state as the real-time command to the walking controller. As we will see, this reactive planning strategy helps ensure that each control command has guarantee of dynamics feasibility, gait stability, and collision-free safety. Moreover, in this way, we can combine the advantages from the local planner, that is the ability to have longer preview of the robot's trajectory, and the one from a fast reactive planner that is to handle realtime disturbances and modeling error of the reduced model.
After being given the planned walking velocity and walking height, the variable walking height controller sends torque commands to the robot at 2 kHz. Moreover, the robot states for the controller is estimated quickly based on robot kinematics model using onboard sensors such as motor encoders and IMU. As there is no pre-built map used, this vision-to-torque framework can enable bipedal robots to safely navigate with agility in new unknown cluttered and height-constrained environments without losing balance.
Vision Data Global Planner, Sec. VI Local Planner, Sec. V Reactive Planner, Sec. V Gait Regulator Joint-Level PD Controllers Kinematics-based State Estimator Visual-Inertial Odometry, Sec. VI Gait Library Vision-based Mapping, Sec. VI Variable Walking Height Controller, Sec. III [1 Hz] [1 Hz] [10 Hz] [30 Hz] [2000 Hz] Goal Location [1 Hz] Obstacles Robot Positions Observed Robot States Reference Gait vSLIP States Profile Local Goal Desired Joint States Motor Torques Sensor Data vSLIP, Sec. IV Real-Time Commands 𝑞 2 𝑅 𝑞 1 𝑅 𝑞 3 𝑅 𝑞 4 𝑅 𝑞 5 𝑅 𝑞 6 𝑅 𝑞 7 𝑅 𝑞 𝐿𝐿 𝐿 𝑞 𝐿𝐴 𝐿 𝑞 𝑥 𝑞 𝑦 𝑞 𝑧 𝑞 𝜙 𝑞 𝜓 𝑞 𝜃 Tracking Camera RGB-Depth Camera L/R LL and the leg angle q L/R LA (right). The robot is equipped with a tracking camera and a RGB-Depth camera on its top.
Having introduced the overall navigation framework, we next present Cassie and its variable walking height controller to better understand properties of the system to plan for.
As presented in Fig. 3, Cassie is a dynamic, underactuated legged robot and has 20 DoFs. It has ten actuated motors q L/R 1,2,3,4,7 and four passive joints q L/R 5,6 on its Left/Right legs. Its pelvis, i.e., floating base, has 6 DoFs q x,y,z,ψ,θ,φ that represents sagittal, lateral, vertical transitions and roll, pitch, yaw rotations, respectively. The floating-based coordinate q ∈ R 20 captures the joint angles and the pose of the robot. The underactuation in the robot is primarily due to the fact that the feet only have a pitch DOF and can't roll. Additional underactuated DOFs exist in the form of the two passive springs in each leg. The virtual leg length and leg pitch angle are denoted by q LL (q 3 , q 4 ) and q LA (q 3 , q 4 ), respectively. Unlike its predecessor robots [43], [44], Cassie can not only
TABLE I: Notations and Symbols Full-Order Model q ∈ R 20
Floating based coordinates qx,y,z Base (pelvis) transnational coordinates q ψ,θ,φ
Base rotational coordinates q
Motor positions on Left/Right Leg q LL , q LA Virtual leg length and leg angle walk in the saggital direction but can also walk in the lateral direction as well as turn. Such an ability gives it advantages to serve as a robotic platform in navigation tasks. Moreover, we summarize the notations and symbols we frequently use in this paper in Tab. I.
To obtain dynamically-feasible walking behaviors for the bipedal robot Cassie, we generated a set of periodic walking gaits by formulating a direct-collocation based optimization using the full dynamics model of Cassie with HZD as described in [47, Sec. III] based on [46], [53]. A gait g is
TABLE II: Gait Library G qx qy [ -1, -0.8, . . . , 0.8, 1.0] [ -0.3, -0.24, . . . , 0.24, 0.3] qz number of gaits [0.65, 0.685, . . . , 0.965, 1.0] 11 × 11 × 11 = 1331
defined as a periodic state trajectory for all actuated joints. For each gait, p = [ qx , qy , q z ] T represents its gait parameter. Then, a gait library G = {g p } is formulated by indexing each optimized gait with its gait parameter p. We generated a gait library offline with 1331 gaits with the gait parameters as described in Tab. II.
In the real-time controller as shown in Fig. 2, a reference gait is obtained by linearly interpolating the gait library with respect to the observed current robot's gait parameter p. A PD-based gait regulator is deployed to correct the gait in order to track the desired commands c. The control command c in this work includes desired sagittal walking speed qd
x , lateral walking speed qd y , walking height q d z , and turning yaw velocity qd φ , i.e., c = [ qd x , qd y , q d z , qd φ ] T . This is achieved by adding regulating terms to the reference gait and the resulted regulated gait is sent to joint level PD controllers to generate motor torques to control the robot. More details about this controller can be found in [47, Sec. IV] which is extended from [46]. In particular, this controller is able to regulate the robot to the command c.
There exists varying velocity upper and lower bounds with respect to the variable walking height controller described, e.g. when robot walks with a low walking height, it can't walk fast. In order to quantitatively analyze such constraints, a feasible command set is constructed as shown in Fig. 4. This set is obtained in a high-fidelity simulation, MATLAB SimMechanics, with full-order dynamics of the Cassie walking robot. During each iteration in the simulation, a test command is sent to the tracking controller. If the controller succeeds to maintain a stable walking gait on the robot for 30 seconds, i.e. the robot doesn't fall, that input command is recorded as a feasible command. Otherwise, that command will be considered as infeasible and not recorded. The testing range for [ qx , qy , q z ] T is between [-1.5, -0.6, 0.6] T and [1.5, 0.6, 1.0] T with a resolution of [0.1, 0.1, 0.5] T .We collect all the feasible commands as the yellow circles shown in Fig. 4 and these form a Feasible Command Set.
In this set, the maximum positive sagittal walking velocity qx grows as the walking height q z increases, after reaching its maximum around +1.2 m/s, at around 0.95 m, it drops dramatically. The maximum negative qx and lateral walking velocity qy have similar trends but they reach their maximums, -0.6 m/s and ±0.5 m/s, respectively, at q z = 0.85 m. Note that the backward walking behavior in the feasible set is not symmetric to the forward walking one while side walking behavior is symmetric about the qy = 0 plane. Moreover, the feasible walking height command range is Fig. 4: The feasible command set which consists of feasible command gait parameters (yellow circles) and its subset which is a convex feasible command set X cf eas drawn as red convex set. Yellow circles represents different feasible combination of walking velocity qx, qy and walking height qz. These are the commanded gait parameters that the variable walking height controller is able to stably maintain a walking gait in a high-fidelity simulator of Cassie. To describe those feasible commands, we obtain a convex subset denoted as X cf eas whose vertices p cf eas are manually picked and marked as solid red points.
q z ∈ [0.65, 1.0] m. The range of feasible turning yaw velocity command qφ is not dependent on other dimensions, therefore, for simplicity, it is not visualized in Fig. 4, and the range is qφ ∈ [-20, 20] deg/s.
Furthermore, several feasible points are selected as vertices, which are represented by solid red points in Fig. 4 to form a conservative convex subset of the original feasible command set. This is defined as Convex Feasible Command Set X cf eas , and its vertices are p cf eas . As long as a given command is within this convex feasible command set, the resulting closed-loop gait obtained by the controller enforces gait stability (in the sense of not falling) on the full-order model. As we will see, the convex feasible command set will enable us to plan using the reduced-order model while still ensuring stability on the full-order model. The concept of a feasible command set was initially introduced in [49] to quantitatively compare the ability of different controllers on bipedal robots. However, using the convex subset to guarantee the gait stability of the command input is first introduced here.
In this section, we develop a reduced-order model we call vertically-actuated Spring Loaded Inverted Pendulum (vS-LIP) to approximate the walking dynamics in planar and vertical direction for bipedal walking robots. Compared to previous reduced-order models, such as SLIP, vSLIP introduces one more actuation in the vertical direction. Moreover, the modeling of vSLIP considers the design and limitation of the robot controlled by a HZD-based controller in order to bridge the full-order model and the reduced-order model.
Prior approaches first design reduced-order models and later develop controllers to force the robot to mimic the motions obtained from the reduced-order model [31], [32], [34]. Our approach differs from this and first develops a library of dynamically feasible periodic gaits on the fullorder model along with a variable walking height controller that stabilizes to a chosen periodic gait on the robot. As illustrated in Fig. 5, we utilize restrictions of the full-order dynamics, the controller and the robot into the reduced-order model to enable online generation of motion plans on the reduced-order model that will be feasible on the full-order model. Specifically this is achieved in a threefold manner: (i) We extract the foot placements from the periodic orbits in the gait library that are optimized with the full-order model;
(ii) We use the convex feasible command set describing the feasible commands to the controller, and this represents restrictions on walking velocity and walking height that result in stable gaits; (iii) We measure physical parameters of the robot. These are then used into a reduced-order model, vSLIP, so that this model captures the information and limitations of the full-order dynamics, the controller and the physical robot.
The mathematical representation of vSLIP model is described in detail in this section. In vSLIP, the bipedal walking robot is simplified as a point mass supported by a massless leg with a spring and actuators during one walking step. When one step ends and another step begins, i.e., transition Γ happens, the stance leg spontaneously switches. The state x = [q x , q y , q z , q φ , qx , qy , qz , qφ ] T represents the Cartesian 3D position and velocity of the Center-of-Mass (CoM), and turning yaw and turning velocity of vSLIP. The dynamics could be defined as ẋ(t) = f (x(t), u(t)), presented as follows:
where u = [u x , u y , u z , u φ ] T is the virtual input acting on the CoM and γ(t
Here, m is the total mass, l and l 0 are the leg length and uncompressed leg length of the reduced-order model, respectively, and K(l) is the stiffness of the leg. Moreover, the values of m, l 0 and K(l) are measured from the robot. Note that the leg stiffness parameterized as a function of the leg length is defined as below (see [54] for more details),
(2)
In the vSLIP model, the walking dynamics of planar motion (xy-plane) and vertical motion are coupled by virtual leg length. Moreover, vSLIP has one more actuation in the vertical direction u z compared to previous SLIP models in order to change the virtual leg length significantly. Furthermore, we decouple turning dynamics from other dimensions and simply model it as a double-integrator system. Remark 1:
The way that we model the turning yaw dynamics is inspired by previous HZD-based locomotion controls in simulation and experiments where yaw doesn't affect motions in other dimensions significantly. But it does have constraint on turning velocity, i.e., the robot cannot change its heading orientation too fast.
In (1), the foothold position is defined f = [x f , y f , 0] T which represents the step length x f and step width y f of vSLIP on flat ground (z f = 0) between two adjacent walking phases. The foothold for the next step f is obtained at the end of each step. For brevity, the superscript prime indicates the next step. To plan for trajectories for walking robots, the foothold position f is usually required to be jointly planned with model state as in prior work [6], [27]. Moreover, there exists constraints on step length and step width to make the planned foothold reachable, e.g., the leg cannot extend too long. This actually brings challenges to quickly solve the planning problem. In the following part, we introduce the foot placement heuristics coming from the HZD-based gait library that can be utilized in vSLIP for online computation.
In this part, we take advantage of the fact that the foothold planning and constraints are already considered during fullorder model based gait optimization, and use the resulting foot placement into the reduced-order model vSLIP. In Sec. III-B, the precomputed gait library optimized by the model of the robot already contains the foothold position for different gait parameters. The optimized foothold position represents the limitations on the robot's strides since the robot dynamics and physical constraints are considered during optimization. Therefore, taking advantage of the HZDbased gait library, in this work, foothold position of vSLIP doesn't need to be planned online. More specifically, the information of step length and step width is decoded from the optimized library and is utilized into the reduced-order model dynamics to fasten computational speed while ensuring the planned foothold to be reachable. Interestingly, through our study, the foot placement for next step is a function of the gait parameters p at the end of step, i.e., during transition between adjacent gaits. We denote this function as foot placement heuristics, and it can be calculated via:
where [q f inal x , q f inal y ] T is the state of the end of one step, and q LL , q LA , q 1 are the leg length, leg pitch, and abduction, at the beginning of next gait.
They can be formulated as explicit functions of the state of the end of one step, as shown in Fig. 6,and the data points are obtained from the gait library with different gait parameters. To alleviate the complexity, we choose a firstorder polynomial model to fit those data. This fitting could be described as:
where a ij and b are the coefficients that could be solved with linear regression. In this way, the foothold values can be updated by the gait parameters at the end of each step, presented in (4) and (5).
In conclusion, we describe the reduced vSLIP model in a manner of using the information and limitations from optimized gait library, walking controller, and physical robot based on full dynamics model. In return, this reduced model can be used online to generate dynamically-feasible reduceddimensional state profile for the actual closed-loop system which is the robot controlled by a walking controller.
The goal of this section is to use the vSLIP model to formulate a nonlinear program to optimize for dynamically-feasible trajectories to drive the robot to track the given local goal while respecting various constraints. We first introduce the formulation of collocation-based trajectory optimization on vSLIP to reach a target state. As mentioned in Sec. II, such a formulation is used in both a local planner and a reactive planner. After presenting the trajectory optimization problem, we also discuss and compare the local and reactive planners.
A. Trajectory Optimization 1) Formulation: The trajectory optimization is applied on vSLIP and is formulated using a collocation method. States x = [q x , q y , q z , q φ , qx , qy , qz , qφ ] T and inputs u = [u x , u y , u z , u φ ] T are defined in the vSLIP model in Sec. IV. The nonlinear optimization problem is on N + 1 discrete collocation nodes with time step ∆t = T /N and fixed time span T .
Given initial and target state as x init and x f inal , the collocation optimization could be formulated as follows,
where the cost function is designed as below,
where
, and they are diagonal matrices with non-negative entries. Specifically, Q is the weight for state cost and has positive values on the velocity terms and zero for the position ones. dQ is introduced to smooth the trajectory, and R penalizes the virtual input. Furthermore, admissible state set X adm and input set U adm , safe set SS, admissible height h and its safety margin s h , initial condition x init and target state x f inal will be explained in the following section.
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 f in the next step will be updated by the foot placement heuristics (4).
Moreover, δ is defined as δ = [δ T cf eas , δ T obs , δ T f inal ] 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. f inal is set as dark yellow and target state for the reactive planner x r f inal is marked as dark blue. Yellow dots represents the planned state profile from the local planner while the blue dots stands for the reactive planner's output. (a) The robot should avoid obstacles represented by circles with radius r obs with s obs being added to the obstacle to act as safety margin, and the robot footprint having a radius of r robot . (b) The robot needs to crouch down and travel underneath the minimal admissible height h in the surroundings. Safety margin in height direction is included as s h . Moreover, the last half of the local trajectory, i.e., the nodes indexing from N/2 to N , should be lower than the minimal height to lead the robot to change its configuration early.
2) Constraints: The constraints that the optimization problem (6) enforces during the navigation in heightconstrained 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.
a) Dynamic feasibility: The vSLIP dynamics in (1) are imposed in the planned trajectory by trapezoidal collocation constraints between adjacent nodes via (6b).
b) Leg length: The leg length of vSLIP defined by (3) should be always less than or equal to the uncompressed leg length l 0 . This is realized by (6c).
c) States bounds and gait stability: The admissible state set X adm in (6d) is the intersection of two sets: states within upper and lower bounds and states within the convex feasible command set X cf eas introduced in Sec. III-D. The admissible state set is
Since X cf eas is a convex set, it can be described by its S vertices p cf eas (Fig. 4) such that
The above constraint at the i th node can then be rewritten as follows,
where λ ij are Lagrangian multipliers. If the optimizing states meet (10), the walking controller will be able to maintain a stable walking gait on the robot since the command is feasible for the controller as illustrated in Sec. III-D. A slack variable vector δ cf eas is added to allow small violation to ensure the feasibility of the optimization problem.
d) Collision-free safety: The robot should be able to avoid obstacles along the trajectory and this is achieved by limiting all states in the safe set SS i of i th node in (6e), as illustrated in Fig. 7a. This safe set is formulated as follows, SS i = (q x,i , q y,i ) : ∀j, r = r obs,j + r robot + s obs,j , q x,i -x obs,j r , q y,i -y obs,j r 4 4 ≥ 1 -δ obs,j , δ obs,j ∈ R + } (11) where (x obs,j , y obs,j ) represents planar position of the detected j th obstacle and r obs,j encodes that obstacle's size. r robot stands for the robot shape. In this case, it is set to 0.5 m which is Cassie's footprint size. l 4 norm is used to better describe the shape of rectangle obstacles. The constant s obs is set to be a safety margin for obstacle avoidance. Another vector of slack variable δ obs is included to attain the feasibility of the optimization problem. However, this slack variable should be always larger or equal to zero to avoid collision.
e) Admissible height: In the environment with varying height constraints, there may exist different admissible heights in the robot surrounding, as shown in Fig. 7b. Therefore, a conservative way to apply the height constraint is to set the height of the robot in the last half of the trajectory to be less than the minimal admissible height h in the local region as given by (6f). Moreover, a safety margin of the robot height s h is included. Enforcing this constraint in the last half of the trajectory could guide the robot to lower its height early to have a smooth height transition and to prevent the robot crouching down too slow to avoid colliding with the ceiling obstacles.
f) Initial and final states: The starting node x 0 should be equal to the current observed robot state x init , enforced by (6g). Similarly, the terminal node x N should also stay close to the given target state x f inal , enforced by (6h). But
TABLE III: Comparison between Trajectory Planners Local Planner Reactive Planner Num. of steps 6 1 Time span T 3 s 0.5 s Horizon N 6 × 6 = 36 6 × 1 = 6 Apply (6f) Yes No Target State
x l f inal = [q l x , q l y , q l z , q l φ , 0] T x r f inal the target state may not satisfy other constraints, e.g., the target state is very close to the obstacles or is unreachable for the robot dynamics in the given time span. Therefore, a vector of slack variable δ f inal is added to adjust the distance between x N and x f inal to ensure the feasibility of the optimization. g) Input bounds: Equation (6g) restricts the virtual input to stay in the admissible input set U adm = {u | u l ≤ u ≤ u u }. This could help to bound the acceleration along the trajectory, which could also smooth the optimized trajectory as u contains acceleration of the vSLIP.
As explained in Sec. II, the local planner and reactive planner share the same optimization schematic explained above but serve different proposes: the local planner has longer preview to generate a smooth trajectory to lead the robot to reach the local goal location, while the reactive planner has a shorter preview but replans in a high frequency to obtain real-time commands for the controller. The output from the local planner is marked as the yellow trajectory in Fig. 7 and the plan from the reactive planner is shown as the blue trajectory in Fig. 7. The comparison between these two planners is illustrated in Tab. III and is explained in detail as follows.
1) Local Planner: 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 Tab. 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 prevent the robot from being too late to crouch down and to avoid the height constraint. The target state x l f inal is set to [q l x , q l y , q l z , q l φ , 0] T , as marked as dark yellow in Fig. 7a, where the desired position terms is the given local goal location, and the velocity term is zero. The given local goal is selected as onemeter ahead of the robot's current position, and is found on the global path. Moreover q l z 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.
2) Reactive Planner: 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 Tab. III, (6f) is not enforced in this planner. Secondly, the target state for the reactive planner x r f inal 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. 7a. 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 c = [ qd x , qd y , q d z , qd φ ] T = [x 1 (5), x 1 (6), x 1 (3), x 1 (8)] T where the subscript 1 represents the first node after the initial node 0.
3) Initial Condition: If the initial condition x init is set to the observed current robot state [q x,y,z,φ , qx,y,z,φ ] 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 x init is measured while the rest is the last output from the reactive planner, and the local and reactive planners share a same initial condition.
4) Solving the Optimization: The optimization problems for the long-term planner and reactive planner are formulated using CasADi [55] and solved by IPOPT [56] with the initial guess linearly interpolated between the given initial condition x init and target state x f inal .
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 guarantee the dynamic feasibility, collision free safety, and gait stability for bipedal robots, and can be utilized online.
After the development of trajectory planners for Cassie, the infrastructure of the navigation autonomy, such as per-ception and localization of the robot, world representation via a 2.5D map, and the global planner to provide a path to reach the given goal, is introduced in this section.
Cassie's perception is based on robot vision, and it is equipped with one tracking camera (Intel RealSense T265) and one RGB-Depth camera (Intel RealSense D435i). Both of them are fixed on the top of Cassie's pelvis, as shown in Fig. 3. The tracking camera is able to estimate the odometry of the robot's floating base which is [q x,y,z,ψ,θ,φ , qx,y,z,ψ,θ,φ ] T by Visual Inertial Odometry (VIO) [52] at 30 Hz. The RGB-Depth camera is utilized to perceive the robot's surroundings by filtered pointclouds and runs at 30 Hz.
A RTAB-Map [57] is used in real time to localize the perceived historical pointclouds in the map by optimizing over the 3D map's graph with proximity detection [58] and estimated robot odometry. The RTAB-Map updates at 1 Hz and the resolution of the voxels is 0.1 m in 3D space. In this way, the perceived 3D environment can be described by an Octomap [59] as shown in Fig. 8. With an assumption that the robot walks on a flat ground, the obstacles on the ground can be extracted by removing the voxels that are lower than a given threshold.
1) Global Map: 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.
Remark 2: The sensor stack on top of Cassie adds a 0.25 m vertical dimension to the pelvis height q z , as shown in Fig. 3. In this work, admissible height refers to the maximum walking height q z and ceiling height includes this 0.25 m vertical dimension.
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 the 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: a) Obstacle: The admissible height is lower than the robot's lowest walking height (0.7 m) and is therefore traversable for the robot.
b) Height-constrained: The admissible height is within the robot's lowest and largest walking height (1 m), because such a region is traversable but requires the robot to lower its walking height to be below this admissible height. c) Free: The robot can travel through it using its largest walking height.
d) Unexplored: The low resolution cell that has not been well perceived and does not have obstacles, i.e., only part of this cell is detected as free space.
In this way, planners can utilize the information on this 2.5D map to find optimal path to navigate the robot while considering the varying admissible height and obstacles in the environment. The global planner is deployed on this 2.5D global map while trajectory planners developed in Sec. V only use the information of a small region on the global map which are denoted as a local map.
2) Local Map: For the 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 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 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 h in this region will be added to the height constraint by (6f).
1) A* Search: 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 travelling underneath obstacles if the robot can walk around the obstacles. Therefore, an A* search [60] that encodes the costs of different types of regions on the map is introduced to serve as the global planner. The cost f (n) of each node n, i.e., traversal cell, during the search is defined as f (n) = g(n) + h(n) where g(n) is the cost of coming from the start node to the current node while h(n) is the heuristic cost that is defined as
where h d (n) is the euclidean distance to the goal location and h h (n) is the cost of admissible height of the node, i.e., a node that has lower admissible height will have a larger h 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 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 region if possible.
2) Local goal: 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 robot's current position in a given range will be selected from this global path by a breadthfirst search, as marked in Fig. 8. In this work, the local goal is chosen as 1-meter ahead of current estimated robot position. Specifically, the nearest two waypoints around onemeter ahead are found firstly 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 environment as illustrated in Fig. 2. The entire pipeline is validated next in simulations and experiments in the following sections.
In order to validate the proposed navigation framework and exemplify the necessity of the feasible command set introduced in Sec. III-D and used in Sec. V, the proposed navigation autonomy is deployed on the bipedal robot Cassie, and is tested extensively in a joint simulation.
We utilize Gazebo to simulate the robot's depth camera reading and MATLAB Simulink to calculate the robot dynamics in order to have a higher fidelity simulation of the robot's walking dynamics. These two simulators are synchronized and the planning data is visualized via ROS RViz, as shown in Fig. 9a,9c.
For the simulation test, a congested space with an arch whose admissible height is 0.75m and two obstacles are constructed in Gazebo, as demonstrated in Fig. 9. The initial position of the robot is behind the first obstacle and the goal location is set to be close to the second obstacle.
The proposed navigation pipeline shown in Fig. 2 is firstly tested in the simulation. It demonstrates the capacity to drive the robot to avoid the first obstacle, crouch down to pass through the arch, recover to a normal walking height afterwards, and then stop in front of the second obstacle. All this is done without a single fall or collision, as exhibited in Fig. 9a. The profiles of robot's planned and actual walking speeds and walking height are recorded in Fig. 9b.
In order to demonstrate the necessity of the feasible command set during the navigation, we consider the pipeline as introduced in Sec. V but remove the feasible command set which introduces a gait stability constraint via (10). This is compared with the original pipeline without changes. The parameters used in the walking controller, cost functions (7), and rest of the constraints in Sec. V-A.2 are kept identical to obtain a fair comparision.
As shown in Fig. 9c, the navigation autonomy without the feasible set causes the robot to fall. This occurs when the robot is approaching the goal location while increasing the walking height after the robot walks through the arch and avoids collision with the second obstacle.
According to the recorded data in Fig. 9d, during the period of 28 s to 30 s, when the robot is changing its walking height from 0.75 m to 1 m while walking to the left, the profile of planned walking height is much steeper than the one using (10) (28.5 s to 32 s in Fig. 9b). Because of the coupled dynamics between planar walking velocity and walking height, such fast change of the walking height triggers a larger robot lateral walking speed under similar planar walking velocity commands. As a result, the robot's actual lateral speed accelerates to over 0.4 m/s during 33 s to 35 s, and the planner needs to use a large lateral speed command later during 36 s to 40 s to drive the robot back. At this period, the lateral walking speed command is over -0.2 m/s at q z of 1 m which lies outside of the feasible command set in Fig. 4, and directly results in gait instability. This showcases the importance of the feasible command set. Without it, the planned trajectory is not dynamically feasible in the sense that the plan does not take into account dynamic coupling that exists between the planar velocity and the walking height, leading to unbounded robot states and eventually a walking failure like Fig. 9c,9d. We repeated such test at least 5 times with different robot initial states in this simulation and results are consistent across all trials.
More extensive tests on the proposed autonomy are conducted in the experiments on the real robot Cassie in the following section.
Cassie has a real-time computer running the variable walking height controller and kinematics-based state estimation at 2 kHz. There are two Intel NUC mini computers with i7 processors used for the entire navigation pipeline. One computer processes the perceived spatial information and registers it into the 2.5D map. It also communicates with the Cassie's computer in order to send real-time commands to the controller while processing robot's state feedback. Another mini computer receives the maps and deals with all planningrelated work, data visualization, and provides the planned control commands. The testing cases are indoor scenarios where obstacles are represented by cardboard boxes. Note that there are some QR codes used in the environments, like the ones in Fig. 10a. These are only used to add more features to a relative-plain background to enhance the performance of VIO and are not exploited to store prior information of ground truth locations in the environment.
In this work, the autonomy is tested in four types of indoor environments which includes (1) a maze without height constraint, (2) a cluttered space with multiple obstacles with one height-constrained space in the form of an arch, (3) an area that has multiple arches and a ground obstacle, and (4) an obstructed door. The proposed autonomy is tested successfully in at least 4 trials where the robot is able to safely walk to the given goal location without a single fall in each of these scenarios. Representative experiments (videofoot_0 ) are exemplified and analyzed below.
1) 2D Maze: 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. 10a. Each of the obstacles is in the shape of 0.5 m×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. 10a,10b, 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 robot's safety. This is realized by outputting a negative walking speed from the reactive local planner as presented in Fig. 10c. Between 58 s to 69 s, Cassie enters a very narrow space whose size is only 0.5 m×0.5 m, a similar size as the robot footprint, as marked in Fig. 10b. At this time, the planners slowly change the robot's movement using almost zero velocity commands, as shown in Fig. 10c, 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. 10c. As this environment doesn't have any height constraints, the robot maintains a normal height around 1 m all the time, as exhibited in Fig. 10d. 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.
2) Height-Constrained Space: In this test, there is an arch-shaped obstacle with a height of 1 m, causing 0.75 m
(a) A snapshot of experiment in the 2D maze Top View Side View Obs. 1 Obs. 3 Obs. 2 Start Goal Start Goal N.S. (b) Planning snapshot during the experiment in the 2D maze ሶ 𝑞 𝑥 (𝑚/𝑠) ሶ 𝑞 𝑦 (𝑚/𝑠) time (𝑠) (c) A comparison between planned and actual walking speeds time (𝑠) 𝑞 𝑧 (𝑚) (d) A comparison between planned and actual walking height (a) A snapshot of experiment in the height-constrained space Top View Side View Obs. 1 Obs. 2 A. 1 Start Goal Start Goal (b) Planning snapshot during the experiment ሶ 𝑞 𝑥 (𝑚/𝑠) ሶ 𝑞 𝑦 (𝑚/𝑠) time (𝑠) (c) A comparison between planned and actual walking speeds 𝑞 𝑧 (𝑚) time (𝑠) (d) Planned and actual walking height and non-admissible walking height Fig. 11: Height-Constrained Space Experiment. There are two Obstacles (Obs. 1-2) and one Arch (A.) area as circled in Fig. 11b in this scenario. During the exploration, Cassie firstly avoids the Obs. 1, then crouches down to walking underneath the overhanging obstacle in the arch A. 1. In this area, the lifted obstacle has only 1 m clearance to the ground, 1 m in width and 1.5 m in length, and the robot needs to use a walking height below 0.75 m to avoid collision, as recorded in Fig. 11d. After passed this region, the Obs. 2 appears in robot's view and Cassie quickly reroute to avoid it until it reach the goal location.
admissible height, along with two obstacles pinned on the ground. This is shown in Fig. 11a. 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 robot's start location, as illustrated in Fig. 11b. 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. 11d. 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. 11c. Humanoid robots walking underneath such similar scenario are presented in simulation with by offline computation in [1]- [3]. According to the best of our knowledge, this is one of the first experimental demonstration of a person-sized bipedal robot safely navigating in a cluttered environment that has an overhanging obstacle by real-time online planning.
3) Multiple Arches: 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×10 m space, respectively, representing the entrance and exit of this constrained area, as demonstrated in Fig. 12a. Moreover, a 0.5 m×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. 12d. During the robot's travel inside this tight space using a 0.7 m walking height illustrated in Fig. 12b, 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. 12c. 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. 12d. 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. 12b 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 Fig. 13: Obstructed Door Experiment. An obstacle is hanged in front a door that is left opened, and the robot travels from a lab space (View 1 in Fig. 13a) to the corridor outside (View 2 in Fig. 13a) through the obstructed door. In the lab space, the robot uses lateral movement and turning to avoid the obstacles (Obs. 1) and to crouch to walk outside of the lab space. Once the robot perceived the new environment and obstacles in the corridor, such as the stanchion and the opened door circled as an Obstacle (Obs. 2), it quickly replans to find a new safe path to the goal location.
the goal location by sending backwards velocity commands as recorded after 65 s in Fig. 12c. This experiment first exhibits a capability of the proposed autonomy for bipedal robots to keep low walking height under multiple heightconstrained spaces while reactively avoiding obstacles.
As demonstrated in Fig. 13a, this environment consists of two parts: a lab space which is in a size of 4 m×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. 13b. 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. 13c. 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. 13a (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, during the robot 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. 13d, Cassie keeps a low walking height between 15 s and 30 s in this cluttered environment until 30 s when it travels to a open space and recovers to a normal height. This shows successful demonstration of a large-scale humanoid robot walking underneath an obstructed door and traveling between different rooms.
We have presented one of the first autonomous navigation framework for person-size bipedal robots to travel and explore unknown height-constrained and cluttered environments while maintaining gait stability and safety. The autonomy framework is deployed and validated on a bipedal robot Cassie to travel in various kinds of congested environments. This autonomy leverages a light-weight perception system which only includes a RGB-Depth camera and a tracking camera to perceive the world, along with hierarchical realtime planners.
A global planner using A* search is able to find and reroute the path from the robot's current position to the goal location while considering the cost of different types of areas while exploring an unknown environment. In order to find a dynamically feasible trajectory for the bipedal robot to track the local goal from the global planner, two optimizationbased trajectory planners that consider the robot's physical limitation, gait stability, and obstacle avoidance are developed for real-time replanning. In this trajectory planning schematic, a local planner schedules a trajectory to reach the given local goal and a reactive planner considers the realtime commands for the variable walking height controller to follow the planned trajectory.
Moreover, in order to capture the coupled walking dynamics of the bipedal robot in the varying walking heights scenarios during trajectory optimization, a vertical-actuated Spring-Loaded Inverted Pendulum (vSLIP) is developed. We introduce a new and practical method to develop and utilize reduced model that is to firstly investigate the walking patterns and restrictions on the robot controlled by a HZDbased walking controller, and later to utilize such information in the vSLIP reduced-order model for online optimization.
Empowered by the proposed autonomous navigation framework, we demonstrates several success experiments for Cassie to autonomously explore a diverse repertoire of height-constrained narrow environments, such as a cluttered space with overhanging obstacle, a region that has multiple arches, and an obstructed door. During the experiments, Cassie exemplifies its capacity to reactively avoid obstacles on the ground and to crouch down and travel underneath arches, which is only realized in simulation with the motions pre-computed offline in most previous work for humanoid robots. Besides allowing the robot to crouch under a minimal ceiling height of 1.0 m overhanging obstacle, the proposed walking autonomy also exhibits the ability to safely walk through a narrow space that is as wide as the robot's footprint, such as the experiment in the 2D maze. Using the proposed vSLIP model and cascading planning strategy, Cassie never looses its gait stability, i.e., never falls over, throughout the tests of this work.
However, although using a single RGB-Depth camera for obstacle detection allows the robot to have a small size of the sensor stack and therefore to walk underneath a low-hanging obstacle, the angle of the view of a single depth camera is limited and therefore robot cannot take wider range of environments into consideration, such as the obstacles behind the robot. This can be solved by upgrading to a 3D LiDAR and this is one of our future work. Moreover, the variable walking height controller used in this work shows some tracking errors and delay not only in the walking velocity but in the walking height. This forces us to include safety margins during planning and further limits the agility of this autonomy. Replacing the walking controller in this pipeline to a more powerful one with better control performance is another future work. Furthermore, Cassie is able to maintain a lower walking height at 0.6 m by the current walking controller, which can further extend its capacity in more tighter spaces. However, Cassie cannot reach that limit in the presented experiments because the state estimation by the tracking camera is no longer stable when the robot keeps that low walking height with large and relative-high frequency stepping. Investigating a reliable state estimator for bipedal robot with consistent impacts for varying walking height is a very interesting research work in the robotics community.
https://youtu.be/Da0tebC3WuE