Unified Path and Gait Planning for Safe Bipedal Robot Navigation

Abstract

Safe path and gait planning are essential for bipedal robots to navigate complex real-world environments. The prevailing approaches often plan the path and gait separately in a hierarchical fashion, potentially resulting in unsafe movements due to neglecting the physical constraints of walking robots. A safety-critical path must not only avoid obstacles but also ensure that the robot’s gaits are subject to its dynamic and kinematic constraints. This work presents a novel approach that unifies path planning and gait planning via a Model Predictive Control (MPC) using the Linear Inverted Pendulum (LIP) model representing bipedal locomotion. This approach considers environmental constraints, such as obstacles, and the robot’s kinematics and dynamics constraints. By using discrete-time Control Barrier Functions for obstacle avoidance, our approach generates the next foot landing position, ensuring robust walking gaits and a safe navigation path within clustered environments. We validated our proposed approach in simulation using a Digit robot in 20 randomly created environments. The results demonstrate improved performance in terms of safety and robustness when compared to hierarchical path and gait planning frameworks.

I Introduction

Bipedal robotics has always been a fascinating and challenging topic in robotics research. However, associated with its promising applications, bipedal robots exhibit complex and high-dimensional models that are particularly hard to use for planning purposes. A way to deal with complex models is to use a template-based model that can be used for gait planning, such as [1, 2]. Moreover, given the growing prevalence of robots operating in safety-critical environments, such as around people or crowded spaces, ensuring the safety of robot motion is becoming increasingly crucial for deploying these intelligent machines.

Traditional path planning algorithms, such as Dijkstra, A*, and RRT, primarily generate collision-free paths from the start to the goal point geometrically [3, 4]. When implementing the resulting path on an actual robot, it is often necessary to utilize the path-following process, typically employing a closed-loop controller driven by the error between the reference and the actual robot paths [4, 5, 6]. In addition, these approaches generate paths without considering the robot’s dynamics and kinematics constraints leading to infeasible or unsafe paths for robots, particularly bipedal robots, to follow. Some planning algorithms integrate steering controllers and path planning, such as Kinodynamic RRT*, LQR-RRT*, and their variants [7, 8, 9, 10], to accommodate the dynamical constraints of mobile robots. Simplified dynamical models, such as differential-drive vehicles, are often considered for synthesizing dynamically consistent paths [11, 12]. While these algorithms can generate relatively smooth trajectories for mobile robots compared to geometric path-planning methods, they do not provide guaranteed control solutions applicable to bipedal robots due to the non-smooth and unstable nature of bipedal locomotion. Guiding a bipedal robot to follow these resultant paths still requires a separate path-following controller, either via way-point tracking or velocity tracking [13].

Figure 1: Safe navigation planning is tested in MuJoCo with the robot Digit, which generates stable walking gaits that avoid obstacles.

Utilizing the full-order robot model for long-range path planning in bipedal locomotion poses significant challenges, primarily due to its demanding computational requirements. These challenges arise from the robot’s high degrees of freedom, combined with its intricate and inherently unstable dynamics. Moreover, the system’s sensitivity to uncertainties further complicates the complexity of real-time motion planning processes. To mitigate these challenges, researchers often turn to simplified template models such as the Linear Inverted Pendulum Model (LIP) [14, 15, 16]. By employing such reduced-dimensional representations, LIP-based approaches utilize preview-based optimization algorithms to determine the next step** position of the swing foot. The primary objective is to stabilize the robot’s center of mass (CoM) while maintaining alignment with the desired velocity trajectory [16, 15, 17, 18]. However, it’s important to note that conventional LIP-based approaches typically do not account for the robot’s turning. Consequently, the process of turning remains entirely independent of LIP-based gait planning approaches.

In this work, we present a unified path and gait planning framework for bipedal robots that generates collision-free, stable walking gaits. We formulate a multi-step preview plan based on the discrete-time step-to-step LIP model using a Model Predictive Control (MPC) formulation. This work is significant for two primary reasons: First, the MPC framework enables the incorporation of essential kinematic and dynamic constraints relevant to bipedal locomotion. These constraints include limitations on maximum forward and lateral speeds, rates of turning, the extremities of leg separation, and the interaction between walking speed and turning dynamics. Second, we integrate discrete-time Control Barrier Functions (D-CBFs) within our framework. This inclusion is pivotal for ensuring the generation of safe (i.e., collision-free) pathways in clustered environments. Control Barrier Functions (CBF) have been successfully applied in controlling legged robots [17, 19, 20] and are now commonly used for ensuring safety in path planning [21, 22, 23, 13]. Specifically for bipedal robots, Liu et al. created an obstacle avoidance planner that combines Control Lyapunov Functions (CLF) with CBF constraints in a quadratic programming (QP) setup [18]. Agrawal et al. extended this idea to a discrete-time step planner with the Hybrid Zero Dynamics (HZD) based locomotion controller [24]. To accommodate long-range path planning, Teng et al. proposed a safety-critical multi-step planner based on the Linear Inverted Pendulum (LIP) model subject to Discrete-time CBF safety constraints [15]. However, this work did not explicitly consider the heading angle and robot steering in the path planning process.

The rest of the paper is organized as follows: Section II reviews the background of the LIP model and introduces the new expression of the LIP model with heading angle state. In Section III, we present the design process of the LIP-MPC by integrating the LIP model, MPC, and its relevant constraints. Section IV showcases the utilization of LIP-MPC in simulation to implement the navigation of a bipedal robot, Digit. Through these simulations, we highlight the effectiveness and enhanced performance achieved by our LIP-MPC approach. Finally, Section V briefly discusses the proposed work, its limitations, and some future directions.

II 3D Linear Inverted Pendulum Model with Heading Angle

The full-order dynamics of a bipedal robot are characterized by high dimensionality and non-linearity, rendering them impractical for path planning purposes. By employing a template model, we can simplify the robot’s dynamics into a lower-dimensional representation, avoiding the computational complexity of the full dynamics in planning. In our approach, we utilize the Linear Inverted Pendulum (LIP) model [14], anchored in a globally fixed frame to facilitate long-range path planning.

Figure 2: The state transition of the Linear Inverted Pendulum model in the global frame. The state $\mathbf{x}_{k}$ in step $k$ (shown in cyan) evolves to the state in the next step $\mathbf{x}_{k+1}$ (shown in orange) when the stance foot locates at point $(f_{x_{k}},f_{y_{k}})$ and with a turning rate of $\omega_{k}$.

By maintaining a constant height of the Center of Mass (CoM), denoted as $H$, the motion of the robot along the $x$-axis can be effectively approximated by the linear dynamics of the LIP model, given by:

$$\begin{bmatrix}\dot{p}_{x}\\ \dot{v}_{x}\end{bmatrix}=\begin{bmatrix}v_{x}\\ \frac{g}{H}(p_{x}-f_{x})\end{bmatrix},$$ (1)

where $p_{x}$ and $v_{x}$ are the CoM position and velocity in $x$-axis, $f_{x}$ is the stance foot position, and $g$ is the gravitational acceleration. We further assume that each walking step takes a fixed time duration $T$. Note that all positions are defined in the fixed global coordinate frame so that there is no need to reset the coordinate before and after a foot impact occurs. Assuming that the position of the stance foot in a walking step $k$ is $f_{x_{k}}$, then the step-to-step discrete dynamics can be determined by:

$$\begin{bmatrix}p_{x_{k+1}}\\ v_{x_{k+1}}\end{bmatrix}=\mathbf{A_{d}}\begin{bmatrix}p_{x_{k}}\\ v_{x_{k}}\end{bmatrix}+\mathbf{B_{d}}f_{x_{k}},$$ (2)

with:

$$\mathbf{A_{d}} \coloneqq\left[{\begin{array}[]{cc}\cosh(\beta T)&\frac{\sinh(\beta T)}{\beta}\\ \beta\sinh(\beta T)&\cosh(\beta T)\end{array}}\right],$$ (3)

$$\mathbf{B_{d}} \coloneqq\begin{bmatrix}1-\cosh(\beta T)\\ -\beta\sinh(\beta T)\end{bmatrix},$$

where $\beta=\sqrt{g/H}$, and $p_{x_{k}}$ and $v_{x_{k}}$ represent the CoM position and velocity at the beginning of $k$-th step.

For 3D bipedal locomotion, the motion in the y-axis direction is similar to the x-axis, and therefore, the LIP model can describe it in the same fashion. In addition, we introduced a heading angle state $\theta$ and turning control $\omega$ to consider robot turning motion. Let $\mathbf{x}\coloneqq[p_{x},v_{x},p_{y},v_{y},\theta]^{T}\in\mathcal{X}\subset\mathbb{R}^{5}$ and the control variable $\mathbf{u}\coloneqq[f_{x},f_{y},\omega]^{T}\in\mathcal{U}\subset\mathbb{R}^{3}$, in which the subscript $x$ and $y$ represent variables in $x$-axis and $y$-axis in the global coordinate frame. Thus, the step-to-step dynamics of the modified 3D-LIP model can be written as:

$$\mathbf{x}_{k+1}=\mathbf{A}\mathbf{x}_{k}+\mathbf{B}\mathbf{u}_{k},$$ (4)

with:

$$\mathbf{A} \coloneqq\begin{bmatrix}\mathbf{A_{d}}&\mathbf{0}&\mathbf{0}\\ \mathbf{0}&\mathbf{A_{d}}&\mathbf{0}\\ \mathbf{0}&\mathbf{0}&1\end{bmatrix},\quad\mathbf{B} \coloneqq\begin{bmatrix}\mathbf{B_{d}}&\mathbf{0}&\mathbf{0}\\ \mathbf{0}&\mathbf{B_{d}}&\mathbf{0}\\ 0&0&1\end{bmatrix}.$$ (5)

An illustration of the LIP states and controls, projected in the $x-y$ plane, is shown in Fig. 2. Defining the LIP dynamics in the global coordinate allows us to conveniently calculate obstacle avoidance in the form of CBF constraints in the following section.

III Unified Path and Gait Planning via LIP-MPC

This section introduces an MPC formulation that unifies path and gait planning subject to robot dynamics approximated by the LIP model.

III-A LIP-based Model Predictive Control

Model Predictive Control (MPC) utilizes the prediction of system states over a finite time horizon to optimize control actions to realize desired performances by minimizing a cost function while satisfying specific constraints.

In our work, we use the step-to-step LIP dynamics in (4) to compute the optimal step** positions of the next $N$ steps. Let $\mathbf{x}_{cur}$ be the LIP states at the current time, and $\mathbf{u}_{cur}$ is the current known stance foot position and turning rate. With the time remaining in the current step, we initially integrate the continuous LIP dynamics to ascertain the end-of-step LIP states, $\mathbf{x}_{0}$, which also serve as the starting point for the subsequent step. This process sets the stage for formulating the LIP-MPC as follows:

$$J^{*}= \min_{\mathbf{u}_{0:N-1}}\hskip 10.00002pt\sum_{k=1}^{N}q(\mathbf{x}_{k})$$ (6)

$$\quad\mathrm{s.t}\quad \mathbf{x}_{k}\in\mathcal{X},\text{ }k\in[1,N]$$

$$\mathbf{u}_{k}\in\mathcal{U},\text{ }k\in[0,N-1]$$

$$\mathbf{x}_{k+1}=\mathbf{A}\mathbf{x}_{k}+\mathbf{B}\mathbf{u}_{k},\text{ }k\in[0,N-1]$$

$$\mathbf{c}_{l}\leq\mathbf{c}(\mathbf{x}_{k},\mathbf{u}_{k})\leq\mathbf{c}_{u},\text{ }k\in[0,N-1]$$

The decision variables in our approach are the next $N$ control actions with their corresponding states predicted using the 3D-LIP model, as shown in Fig. 3 for $N=3$. Given the incorporation of nonlinear constraints within our framework, we use the IPOPT solver [25] in this work. The cost function $q(\mathbf{x}_{k})$ is defined to evaluate a sequence of predicted states against the goal positions, aiming to minimize this cost to steer the robot towards its goal. In our design, we also consider the heading angle cost, defined as:

$$\theta_{goal_{k}}=\arctan\left(\frac{p_{y_{k}}-y_{goal}}{p_{x_{k}}-x_{goal}}\right),$$ (7)

for each step $k$. We add the heading angle cost function to mitigate unnecessary adjustments and promote smoother navigation. Without this cost function, the planner tends to adjust heading angles frequently to realize maximum walking speeds. Thus, the cost function in each step is defined as:

$$q(\mathbf{x}_{k})= q_{k}\left(\left(p_{x_{k}}-x_{goal}\right)^{2}+\left(p_{y_{k}}-y_{goal}\right)^{2}\right)$$

$$+r_{k}\left(\left(\theta_{k}-\theta_{goal_{k}}\right)^{2}\right),$$ (8)

where $q_{k}$ and $r_{k}$ are positive weights associated with position and orientation, respectively.

Figure 3: An illustration of the LIP-MPC formulation when $N=3$. The planner first estimates the states at the end of the current step, $\mathbf{x}_{0}$, given the current CoM states, $\mathbf{x}_{cur}$, and the stance foot position, $\mathbf{u}_{cur}$. The LIP-MPC will determine the next three control actions (i.e., step** positions and turning rates), as well as subsequent states for further evaluation.

The function $\mathbf{c}(\mathbf{x}_{k},\mathbf{u}_{k})$ represents the safety constraints for the bipedal robot. There are two types of safety constraints. The first is environmental safety constraints, primarily implemented by constructing Discrete Control Barrier Functions (DCBFs) to avoid a collision with the obstacles. Since planning a safe path does not guarantee safety when a bipedal robot walks along that path, we have added a second type of safety constraint. These constraints are constructed based on the kinematics of bipedal robots.

III-B Environmental Safety Constraints

Consider the discrete states of the robot $\mathbf{x}_{k}\in\mathcal{X}\subseteq\mathbb{R}^{n}$, and $\mathbf{u}_{k}\in\mathcal{U}\subseteq\mathbb{R}^{m}$ are the discrete control inputs. If there exists a continuous and differentiable function $h:\mathbb{R}^{n}\rightarrow\mathbb{R}$, the safety set $\mathcal{C}$ of the system may be defined as:

$$\mathcal{C} =\{\mathbf{x}_{k}\in\mathcal{X}|h(\mathbf{x}_{k})\geq 0\},$$ (9)

$$\partial\mathcal{C} =\{\mathbf{x}_{k}\in\mathcal{X}|h(\mathbf{x}_{k})=0\}.$$

Then, $h(\cdot)$ is a discrete control barrier function (DCBF) if it follows the condition in the discrete-time domain for all $\mathbf{x}_{k}\in\mathcal{C}$[24, 26]:

$$\exists\text{ }\mathbf{u}_{k} \text{ s.t. }\bigtriangleup h(\mathbf{x}_{k},\mathbf{u}_{k})\geq-\gamma(h(\mathbf{x}_{k})),$$ (10)

where $\bigtriangleup h(\mathbf{x}_{k},\mathbf{u}_{k})\coloneqq h(\mathbf{x}_{k+1})-h(\mathbf{x}_{k})$, and $\gamma$ is a class $\kappa$ function that satisfies $0<\gamma(h(x))\leq h(x)$. In the discrete domain, $\gamma$ can be also a scalar that $0<\gamma\leq 1$. So, a DCBF constraint can be written as:

$$\exists\text{ }\mathbf{u}_{k} \text{ s.t. }h(\mathbf{x}_{k+1})+(\gamma-1)h(\mathbf{x}_{k})\geq 0.$$ (11)

In our case, we consider the obstacles in the environment to be circular or elliptical. Therefore the expression of the barrier functions $h(.)$ are:

$$\text{Circular: }h(\mathbf{x}_{k})= (p_{x_{k}}-x_{c})^{2}+(p_{y_{k}}-y_{c})^{2}-r^{2}$$

$$\text{Elliptical: }h(\mathbf{x}_{k})= A(p_{x_{k}}-x_{e})^{2}+B(p_{x_{k}}-x_{e})(p_{y_{k}}-y_{e})$$

$$+ C(p_{y_{k}}-y_{e})^{2}-D^{2}$$

Where $[x_{c},y_{c},r]$ represents the center and radius of the circular obstacles. For elliptical obstacles, $[x_{e},y_{e}]$ represents the geometric center, and parameters $A,B,C,D$ determine the shape and size of the obstacles, which can be determined by ellipse width, length, and rotation angle. The LIP-MPC generates a collision-free path for the Center of Mass (CoM) references in global coordinates. However, to maintain the robot’s end-effectors, like feet and hands, at a safe distance from obstacles, we consider a safety margin by enlarging the original obstacle size by $0.4$m. This adjustment simplifies the enforcement of environmental safety and improve collision avoidance.

III-C Kinematics Safety Constraints

Since our LIP-MPC implements path and gait planning simultaneously, we need to consider the safety of the planned path (avoiding obstacles) as well as the safety and feasibility of the robot’s actions. A planned optimal path may require the robot to cross its legs, stretch them beyond limits, or be compelled to maintain maximum forward speed while making a large turn. While these situations may be acceptable at the mathematical level for the LIP model, they pose safety risks for the actual robot during walking. Such scenarios can easily lead to the robot falling or even being damaged. Hence, incorporating these kinematics constraints is essential for comprehensive safety and feasibility planning.

Linear Velocity. One key constraint we address involves the robot’s linear velocities in its local coordinate, whose $x$-axis points along its heading angle as shown in Fig. 2. Bipedal robots have the capability to move in all directions; however, their design allows for quicker movement longitudinally compared to laterally. Because the LIP model describes states in global coordinates, applying these velocity constraints requires converting them into the robot’s local coordinate frame. Additionally, the sign of lateral velocity at the end of a step is determined by the stance foot—it’s positive when the right foot is the stance foot and negative for the left foot as the stance. The inclusion of the heading angle in the LIP states allows for stating the velocity constraints as follows. When the right foot is the stance foot, we have

$$\begin{bmatrix}v_{x_{\mathrm{min}}}\\ v_{y_{\mathrm{min}}}\end{bmatrix}\leq\begin{bmatrix}\cos\theta_{k}&\sin\theta_{k}\\ -\sin\theta_{k}&\cos\theta_{k}\end{bmatrix}\begin{bmatrix}v_{x_{k}}\\ v_{y_{k}}\end{bmatrix}\leq\begin{bmatrix}v_{x_{\mathrm{max}}}\\ v_{y_{\mathrm{max}}}\end{bmatrix},$$ (12)

where $v_{x_{\mathrm{min}}},v_{x_{\mathrm{max}}},v_{y_{\mathrm{min}}},v_{y_{\mathrm{max}}}$ are the lower bounds and upper bounds of the robot longitudinal velocity and lateral velocity, respectively. When the stance foot is the left foot, the constraints can be expressed as follows:

$$\begin{bmatrix}v_{x_{\mathrm{min}}}\\ -v_{y_{\mathrm{max}}}\end{bmatrix}\leq\begin{bmatrix}\cos\theta_{k}&\sin\theta_{k}\\ -\sin\theta_{k}&\cos\theta_{k}\end{bmatrix}\begin{bmatrix}v_{x_{k}}\\ v_{y_{k}}\end{bmatrix}\leq\begin{bmatrix}v_{x_{\mathrm{max}}}\\ -v_{y_{\mathrm{min}}}\end{bmatrix}.$$ (13)

Imposing strict constraints on the sign of lateral velocity at the end of each step can implicitly prevent leg crossing during locomotion. This measure is crucial since crossing legs can lead to collisions between the two legs, significantly increasing the risk of the robot falling.

Leg Reachability. We impose a swing foot reachability constraint to prevent over-extensioning the leg and violating the physical limits of leg joints. In particular, we limit the horizontal distance from the robot’s CoM position $(p_{x_{k}},p_{y_{k}})$ to the subsequent step** position $(f_{x_{k}},f_{y_{k}})$, ensuring it remains within a defined threshold. This constraint is formulated as:

$$0<(p_{x_{k}}-f_{x_{k}})^{2}+(p_{y_{k}}-f_{y_{k}})^{2}\leq\mathcal{L}_{\mathrm{max}}^{2},$$ (14)

where $\mathcal{L}_{\mathrm{max}}$ is the maximum reachable distance of the swing foot on the ground.

Turning Rate. Considering a turning rate $\omega$, we directly impose a turning constraint with maximal turning rate $\Omega_{max}$ to prevent the LIP model from generating an excessive turning motion:

$$-\Omega_{\mathrm{max}}\leq\omega_{k}\leq\Omega_{\mathrm{max}}.$$ (15)

Maneuverability Constraint. Decelerating while turning is an important strategy that can improve safety during walking. This approach mirrors human walking behavior, where there’s a documented linear relationship between longitudinal velocity and changes in the heading angle, as observed in previous studies [27, 28, 29]. The maneuverability constraint couples the turning rate with the longitudinal velocity, and can be expressed as follows:

$$v_{x_{\mathrm{min}}}\leq\frac{\alpha}{\pi}|\omega_{k}|+(v_{x_{k}}\cos\theta_{k}+v_{y_{k}}\sin\theta_{k})\leq v_{x_{\mathrm{max}}},$$ (16)

where $\alpha$ is a positive coefficient that balances the turning rate and longitudinal velocity. A higher value means a greater need for deceleration to achieve the same angular displacement during a turn. In our work, we empirically set $\alpha=3.6$ for the Digit robot based on our observation. We design these constraints in our LIP-MPC to achieve a specific behavior: when the robot walks straight ahead (with a turning rate of 0), it can attain its maximum longitudinal velocity, $v_{x_{max}}$. However, when the robot turns, either left or right, it needs to decrease the longitudinal velocity in response to turning motion. This adjustment will minimize the oscillation of the turning rate, facilitating smoother turns.

Figure 4: The hierarchical framework for planning and control for safe bipedal navigation. The high-level planner unifies both path and gait planning, whereas the low-level task space controller tracks the target swing foot-step** positions to follow the planned path.

III-D Hierarchical Locomotion Control Framework

To implement safe navigation of the robot using the unified LIP-MPC planner, we implemented a hierarchical locomotion controller, as shown in Fig. 4.

The high-level LIP-MPC planner takes environmental information and robot states as inputs, and determines the next foot-step** position. In this work, we assume that the goal position remains constant and the geometric characteristics of static obstacles are predefined and known. The LIP-MPC is recalculated several times during a single walking step, leveraging real-time feedback on the robot’s Center of Mass (CoM) states, resulting in an update rate of 20 Hz for the high-level planner. A state pre-processing component predicts the end-of-step states from the current robot states, factoring in the remaining time for the current step. This prediction acts as the initial state for the next cycle of the discrete-time LIP-MPC in (6). This iterative process enables the planner to dynamically adjust the positioning of the next swing foot, ensuring the robot maintains stability and avoids obstacles effectively over extended distances.

The low-level task space controller (TSC) runs at 1 kHz and determines the motor torques required for each joint to track the target swing foot positions, while maintaining a constant CoM height and ensuring the robot’s torso remains upright. Further details on the low-level task space controller are available in our prior work [1].

IV Simulation Results

This section presents simulation results that demonstrate the effectiveness and improved performance of our proposed LIP-MPC in navigating clustered environments.

IV-A Simulation Setup

In particular, we use the Agility Robotics’ Digit humanoid as our testing platform in simulation. To evaluate the efficacy and performance of our proposed method, we designed several test environments within the MuJoCo simulator, each featuring six to eight obstacles of varying sizes and shapes. For all test scenarios, the starting position was set at $[0,0]$ m, and the goal position was designated at $[10,10]$ m. Specifically, we maintained the robot’s Center of Mass (CoM) height at a constant $H=1$ m, set the step duration to $T=0.4$ s, and defined the MPC prediction horizon as $N=3$. Table I shows the chosen values of weights and limits we used throughout all tests in this paper.

Parameters	Value
$q_{k}$	1
$r_{k}$	50
$[v_{x_{\mathrm{min}}},v_{x_{\mathrm{max}}}]$	$[0.4,0.8]$ m/s
$[v_{y_{\mathrm{min}}},v_{y_{\mathrm{max}}}]$	$[0.15,0.35]$ m/s
$\mathcal{L}_{\mathrm{max}}$	$0.3$m
$\Omega_{max}$	$\frac{\pi}{16}$ rad/T
$\alpha$	3.6
$\gamma$	$0.3$

TABLE I: The value of each control parameter used in the simulation throughout this work.

IV-B Improved Performance with Maneuverability Constraint and Heading Angle Cost

One important contribution of this work is the incorporation of a heading angle cost and maneuverability constraints into the planning framework. These modifications facilitate smoother navigation and safer locomotion in clustered environments. To evaluate their impact, we conducted tests comparing the performance of the LIP-MPC with these modifications (Modified LIP-MPC) against the version without them (Original LIP-MPC). This comparison aims to clearly demonstrate the benefits these adjustments bring to the overall planning and execution of bipedal robot locomotion.

Figure 5: Actual trajectories using the original LIP-MPC planner without considering heading angle cost and maneuverability constraints. Left shows the resultant CoM global position (m), and Right shows the robot colliding with obstacles and falling in simulation.

(a)

(b)

Figure 6: Actual trajectories using the modified LIP-MPC planner adding angle cost and maneuverability constraints. Left shows the resultant CoM global position (m), and Right shows the robot successfully reaching the goal in simulation.

Figure 7: States comparison of the original LIP-MPC and modified LIP-MPC. The left shows the longitudinal velocity (m/s) of two planners. The right shows the heading angle of two planners.

The comparative simulation results of the Original LIP-MPC and the Modified LIP-MPC are shown in Fig. 5 and Fig. 6, respectively. The Original LIP-MPC encountered an obstacle collision leading to a fall, in contrast to the Modified LIP-MPC, which adeptly completed the navigation task. Further analysis of longitudinal velocity and heading angle changes throughout the trials in Fig. 7 shows that the Original LIP-MPC tends to walk at the maximum longitudinal velocity regardless of the robot turning by frequently altering the robot’s heading angle. This resulted in significant oscillation of the heading angle and a lack of deceleration during turns, which compromised the robot’s stability, leading to a notable disparity between the actual CoM trajectory and the planned path. Consequently, the risk of the robot colliding with obstacles was noticeably increased. Moreover, such aggressive maneuvers could pose risks to the physical robot on hardware implementation. On the other hand, the Modified LIP-MPC planner adopted a more cautious approach by reducing speed during turns and allowing for smoother heading angle transitions. The planner only reaches the maximum velocity when walking in a straight line. These strategic modifications remarkably enhanced the safety and reliability of the LIP-MPC in navigating through obstacles.

IV-C Improved Safety with LIP-MPC

To show the enhancement in path and gait safety through the incorporation of robot dynamics approximated by the LIP model, we compare our method with a conventional hierarchical planning framework. This framework consists of two components: a high-level path planner that considers the robot as a differential drive and a mid-level gait planner that tracks desired walking velocities. For comparison purposes, we utilized a LIP-based gait planner, described in our previous work [30, 2]. The gait planner will track the desired velocity commands from the path planner to follow the planned path. The high-level differential-drive (DD) based path planner utilizes the same MPC formulation (referred to here as DD-MPC) with discrete-time Control Barrier Functions for obstacle avoidance described in the previous section. However, while DD-MPC can accommodate linear velocity and turning rate constraints, it lacks the capability to enforce additional kinematic constraints, such as leg reachability. Furthermore, due to the differential drive model’s nonholonomic dynamics, DD-MPC only generates longitudinal velocity and turning rate, underscoring a limitation in its adaptability to complex locomotion tasks.

To evaluate the effectiveness of incorporating robot dynamics through the LIP model for safer path and gait planning, we conducted simulations in environments with six obstacles (three circular and three elliptical) to compare the performance of our LIP-MPC planner with the DD-MPC planner. The simulation setup aimed to navigate the robot from an identical start to an end point across both planners. The simulation results are shown in Fig. 8 and Fig. 9, respectively. Fig. 8 shows that the robot successfully navigated around all obstacles to reach the goal without colliding and falling. On the other hand, the DD-MPC planner resulted in a collision with an obstacle, eventually causing the robot to fall.

Fig. 10 highlights the planned paths and actual paths of two planners at every step around the area when the failure occurs. Fig. 10(a) shows the actual CoM trajectory of the robot in simulation closely following the planned paths from the LIP-MPC planner, thereby significantly improving the safety of the robot in navigation. Although some instances of infeasibility due to CBF constraint violations occurred with the LIP-MPC, the robot’s CoM trajectories remained at a safe distance from obstacles, enabling collision-free navigation through real-time reactive planning. In contrast, the significant model mismatch between the differential-drive model and the dynamics of bipedal robots led to discrepancies between DD-MPC’s planned trajectories and the robot’s actual CoM trajectories, as shown in Fig. 10(b). This mismatch resulted in repeated infeasible solutions from the DD-MPC, culminating in the robot breaching the safety margin, colliding with an obstacle, and subsequently falling.

Figure 8: Actual trajectories using the LIP-MPC planner. Left shows the resultant CoM global position (m), and Right shows the robot successfully reaching the goal in simulation.

Figure 9: Actual trajectories using the DD-MPC planner. Left shows the resultant CoM global position (m), and Right shows the robot collides with an obstacle and falls in simulation.

(a) LIP-MPC

(b) DD-MPC

Figure 10: Planned and actual paths of both LIP-MPC and DD-MPC planners. The dashed line around obstacles is the extended safety region, which is used to form the CBF constraints. The infeasibility often occurs when the MPC planners violate the CBF constraints.

Figure 11: Twenty distinctive environments populated with eight randomly sized and shaped obstacles.

Planner	Finish	Violate	Enter	Collide
LIP-MPC	20	15	10	0
DD-MPC	4	20	17	16

TABLE II: The results of applying the two planners to the 20 randomly generated scenarios.

In an effort to rigorously compare the two planning approaches, we conducted simulations in 20 distinct environments, each populated with eight randomly sized and shaped obstacles, as illustrated in Fig. 11. Across all scenarios, the robot was tasked to navigate from a starting point at $[0,0]$ m to a goal point at $[10,10]$ m. The outcomes of these simulations are summarized in Table II. The results reveal that the LIP-MPC consistently led the robot to successfully navigate to its destination without colliding with any obstacles in all trials. Despite this, there were 15 instances of constraint violations, and in 10 of these cases, the robot briefly entered into the safety margins surrounding obstacles. Nonetheless, these violations did not result in the robot falling, likely because the violations were minor. Fine-tuning of control parameters, such as adjusting $\gamma$ to smaller values, could yield more cautious planning, reducing the robot’s proximity to obstacles. However, this might also increase the chances of infeasibility in scenarios where navigable space is limited. Adjustments to kinematic constraints may also enhance optimization convergence, though infeasibility might still arise under particularly challenging conditions, such as in densely obstacle-laden environments. Moreover, the model mismatch between the LIP model and the actual robot would also make the optimization problem infeasible.

On the other hand, the DD-MPC managed to complete the navigation task in only 4 out of the 20 trials. The remaining 16 attempts ended in collisions with obstacles, leading to the robot’s fall. Constraint violations were noted in every test, with the robot entering the safety margins in 17 instances. Although similar adjustments could theoretically improve DD-MPC’s performance, the fundamental mismatch between the differential-drive model and the actual dynamics of bipedal robots greatly compromises the safety and effectiveness of this planner.

V Conclusion

In this paper, we presented a linear inverted pendulum (LIP) model-based Model Predictive Control (MPC) framework for unified path and gait planning of bipedal locomotion. The results demonstrated that our framework significantly enhances the safety and stability of the Digit robot in various clustered environments. A key to our approach is the facilitation of smoother turning and dynamic speed adjustments during turns, which collectively bolster the system’s reliability. Compared to traditional hierarchical planning methods, our model consistently delivered superior performance in all evaluated scenarios. Despite its effectiveness, there is room for further refinement to achieve seamless real-time navigation. Future directions for this work include optimizing the structure of the optimization problem to enhance computational efficiency. Additionally, to tackle the challenge of infeasible planning scenarios, we intend to develop more sophisticated safety constraints and consider the incorporation of regularization techniques within the optimization problem. This adjustment aims to reduce the likelihood of constraint violations. Furthermore, exploring new models to fully harness the locomotion capabilities of bipedal robots remains a priority. Through these enhancements, we aspire to push the boundaries of bipedal robot navigation and operational agility.