Safe and Robust Motion Planning for Autonomous Navigation of Quadruped Robots in Cluttered Environments

Quadruped robots, with their superior terrain adaptability and flexible movement capabilities, demonstrate greater application potential in complex environments compared to traditional ground robots. However, their non-negligible body shape and anisotropic motion characteristics complicate the achievement of high-precision motion planning and autonomous navigation. In this paper, we propose a safe and robust motion planning system tailored for autonomous navigation of quadruped robots in cluttered environments. We adopt a hierarchical architecture and decompose the planning process into front-end searching and back-end optimization. In the front-end searching stage, the robot finds a smooth, feasible, and energy-efficient initial trajectory with safety consideration. In the back-end optimization stage, we leverage B-splines to enhance the trajectory smoothness, safety, and motion stability. Finally, the time allocation is fine-tuned through iterative refinement, ensuring the feasibility of the optimized trajectory. Our method is extensively validated in challenging simulations as well as in real-world testing environments, benchmark comparisons also demonstrate the improved performance of our method.

SECTION I. Introduction

In recent years, owing to their superior flexibility, terrain adaptability, and payload capacity in unstructured environments, quadruped robots have gradually become a research focus in the field of robotics. They have achieved commendable outcomes in various applications such as terrain exploration, post-disaster search and rescue, material transportation, and industrial inspection, significantly reducing human labor costs [1].

Compared to traditional wheeled and tracked ground robots, quadruped robots possess significant advantages. On the one hand, they can adjust their gaits in response to different ground structures, enabling stable locomotion similar to quadruped animals in unstructured environments such as rugged terrains. On the other hand, they have approximate omnidirectional mobility to navigate through narrow spaces such as tunnels and caves by adjusting their body postures, thus exhibiting higher flexibility. Therefore, it is meaningful and promising to integrate autonomous navigation technology with quadruped robots to enhance their autonomy and further expand their practical application range.

Existing work on quadruped robots primarily focuses on novel mechanical structures [2], [3] and the study of their gaits motion, including gait generation, foot trajectory planning, as well as body balance and control robustness [4], [5], [6]. These studies ensure stable and efficient low-level motion for quadruped robots across different environments and tasks but they lack the research on their high-level center-of-mass motion behaviors, limiting their autonomous capabilities and range of application in task-oriented missions within complex environments. Moreover, the non-negligible body shape of quadruped robots, along with their movement capability difference in various orientations, introduces additional complexity and challenges to the motion planning and autonomous navigation.

In this paper, we propose a novel motion planning framework for the autonomous navigation of quadruped robots in cluttered environments. Figure 1 shows a composite image of an Aliengo robot navigating through a cluttered test field with our proposed method. Our framework employs a hierarchical planning structure dividing the planning process into front-end and back-end stages. In the front-end stage, we propose a kinodynamic path searching method, named Improved Kinodynamic A$^{\ast }$ , that integrates the robot’s kinematics into the search process. This method balances the control energy consumption and the path smoothness to quickly search for a guiding global trajectory connecting the initial and terminal states. In the back-end stage, the guiding trajectory is parameterized into a B-spline trajectory, and we optimize the control points of the trajectory by leveraging the local controllability and convex hull properties of B-splines, thus enhancing its smoothness and improving clearance with obstacles. Finally, to ensure the feasibility of the generated trajectory, iterative refinement is employed, with the motion characteristics of quadruped robots taking into account, thereby eliminating infeasible velocities and accelerations within the trajectory.

FIGURE 1.

Composite image of an Aliengo robot navigating through the test field cluttered with boxes and boards.

We compare our method with an existing state-of-the-art work employing a similar planning method in both simulated and real-world environments. Benchmark comparisons demonstrate that our method has lower energy consumption and higher stability, with higher success rates in the autonomous navigation tasks. The main contributions of this paper are summarized as follows:

1. We propose a novel path searching method Improved Kinodynamic A$^{\ast }$ , which generates a guiding initial trajectory conforming to the motion characteristics of quadruped robots.

2. Incorporating the body shape and motion characteristics of quadruped robots, we propose a trajectory optimization method based on B-splines to generate smooth, safe, and feasible trajectories.

3. Based on the optimized trajectory and the motion characteristics of quadruped robots, we propose an iterative refinement method that ensures the feasibility of the final trajectory.

The remainder of this article is organized as follows. Section II summarizes the related works. Section III introduces the quadruped robot hardware platform and the system architecture. Section IV details the Improved Kinodynamic A$^{\ast }$ path searching algorithm, while Section V presents the gradient-based trajectory optimization algorithm. Section VI validates and analyzes the proposed method through both simulation and real-world experiments. Finally, Section VII concludes the article.

SECTION II. Related Works

As stated before, only a limited number of works have explored autonomous navigation of quadruped robots, including navigation in unstructured terrains or unknown environments [7], [8], [9], [10], [11], [12], [13], [14], the navigation with multi-gait combinations (e.g. walking and jumping) [15], [16], [17], and multi-robot cooperative exploration [18], [19]. For the motion planning and autonomous navigation of robotic systems, the prevalent approach is to utilize a two-stage hierarchical architecture, which can be decomposed into front-end searching and back-end optimization.

A. Front-End Searching

Front-end searching, also known as global planning in some works, aims to search for a guiding global path in a known or partially known environment. The majority of existing works utilize search-based methods such as A$^{\ast }$ [7], [8], [9], [10], [15], D$^{\ast }$ Lite [13], or sampling-based methods such as RRT$^{\ast }$ [14] and RRT-Connect [16], [17] for global searching. However, while these approaches can find a guiding path geometrically, they lack the robot’s kinematics, which may lead to infeasible trajectories for the actual robot, or increased energy consumption in the final trajectory [20]. To address this shortcoming, an intuitive solution is employing the kinodynamic searching. For instance, Hybrid-state A$^{\ast }$ [21] generates dynamically feasible trajectories for autonomous vehicles in the searching stage. Inspired by this, Zhou et al. [22] designed a kinodynamic path searching method for quadrotors. Considering the approximate omnidirectional mobility of quadruped robots within their motion plane, we propose the Improved Kinodynamic A$^{\ast }$ approach, which generates a smooth and feasible initial trajectory in the front-end searching stage.

B. Back-End Optimization

Back-end optimization aims at optimizing the global path into an executable trajectory. Some works [8], [15], [17] refine the gait of quadruped robots, directly controlling leg movements to track the global path. Others focus on iteratively optimizing the local trajectory based on the robot’s current position [11], [12], [14], [15]. For instance, Wang et al. [12] utilizes the Timed Elastic Band (TEB) for local trajectory optimization. Feng et al. [11] generates multiple local trajectories expressed by Bézier curves, and selects the optimal local trajectory using a scoring strategy. The method proposed in [10] formulates the generation of the executable trajectory into a numerical optimization problem which aims at minimizing the integral of the squared acceleration over time. Wooden et al. [7] fits the global path into a time-parametrized polynomial curve to obtain a smooth and continuous trajectory directly. Such methods are more beneficial to optimizing the trajectory holistically and have demonstrated excellent performance in the autonomous navigation of other types of mobile robots [22], [23], [24]. We parameterize the initial global trajectory into a B-spline and leverage the convex hull as well as the local control properties of B-splines to enhance the smoothness, safety, and motion stability of the trajectory. Our method not only ensures the generated trajectory is smooth and feasible, but also offers the advantage of low computation cost.

SECTION III. System Overview

A. Experiment Platform

We use the Unitree Aliengo1 quadruped robot as our experiment platform (Figure 2). It is equipped with a Slamtec Mapper LiDAR2 for environmental perception, which integrates a high-performance SLAM algorithm outputting 2D grid maps up to 0.05m precision whilst performing high-accuracy robot localization. The Aliengo’s on-board computing resources are provide by an NVIDIA Jetson TX2 and a miniPC equipped with an Intel Core i5-6300U processor (2.4 GHz, 2 cores, 4 threads) and 4GB RAM.

FIGURE 2.

The Aliengo robot (weight: $21.5kg$ , standing dimensions: $0.8m \times 0.35m \times 0.6m$ ). It is equipped with a Slamtec mapper LiDAR for environmental perception, an NVIDIA Jetson TX2 for motion planning, and an onboard miniPC for motion control.

B. System Architecture

The overall architecture of our proposed system is illustrated in Figure 3. The perception module updates the map and robot localization at a frequency of 10Hz. The planning module begins by searching for a global initial trajectory using the Improved Kinodynamic A$^{\ast }$ (see Section IV), then parameterizes this trajectory into a B-spline and optimizes it based on the gradient of control points (see Section V-A). Finally, it iteratively refines the trajectory to eliminate the infeasibilities (see Section V-B). The control module evaluates and publishes the position and velocity commands in real-time based on the robot’s current state and the ideal trajectories, facilitating autonomous navigation for the quadruped robot.

FIGURE 3.

System architecture. The orange part represents the perception module, which computes within the LiDAR’s internal processor. The blue part denotes the planning module, deployed on the TX2. The green part signifies the control module, running on the onboard miniPC.

SECTION IV. Improved Kinodynamic A$^{\ast }$ Path Searching

We extend the Fast-Planner [22] algorithm by introducing the smoothness cost to design our path searching method. We expand nodes by generating a set of motion primitives through discretized control inputs, design the cost function by combining the energy consumption and the path smoothness, and enhance search efficiency through analytic expansions. Figure 4 illustrates the search process of our method.

FIGURE 4.

Illustration of the $Improved~Kinodynamic~A ^{\ast }$ search process. The algorithm expands nodes using motion primitives (red curves) and employs periodic analytic expansions (green curves) to enhance the search efficiency. The dashed trajectories are abandoned due to collision with obstacles.

A. Motion Primitives Generation

For the generation of motion primitives, we employ a second-order integrator model adapted for the quadruped robot. We decompose the robot’s trajectory on the plane into two independent one-dimensional polynomial curves with respect to time under the world coordinate system, and consider the robot’s position and velocity on the plane as the system states, with acceleration as the system input. The state-space model of the system can be defined as:

$$\begin{equation*} \dot {\boldsymbol {s}}=\boldsymbol {As}+\boldsymbol {Bu}, \tag {1}\end{equation*}$$ where $$\begin{align*} \boldsymbol {A}=\left [{{\begin{array}{cccccccccccccccccccc} \boldsymbol {0} & \quad \boldsymbol {I}_{2} \\ \boldsymbol {0} & \quad \boldsymbol {0} \\ \end{array}}}\right ]\boldsymbol {,~ B}=\left [{{\begin{array}{cccccccccccccccccccc} \boldsymbol {0} \\ \boldsymbol {I}_{2} \\ \end{array}}}\right ].\end{align*}$$

Subsequently, with discretized accelerations$\left \{{{-u_{max}\mathrm {, -} \frac {1}{2}u_{max}\mathrm {,0,}\frac {1}{2}u_{max},u_{max}}}\right \}$ in each axis as the system input, a set of motion primitives can be generated through the following state transition equation:

$$\begin{equation*} \boldsymbol {s}\left ({{ t }}\right)=e^{\boldsymbol {A}t}\boldsymbol {s}\left ({{ 0 }}\right)+\int _{0}^{t} {e^{\boldsymbol {A}\left ({{ t-\tau }}\right)}\boldsymbol {Bu}\left ({{ \tau }}\right)d\tau }. \tag {2}\end{equation*}$$

It is noteworthy that quadruped robots exhibit distinct mobility capabilities longitudinally and latitudinally. However, we do not differentiate such differences in the path searching. We will further optimize the trajectory with the quadruped robot kinematics in the back-end optimization, and the directional mobility differences will be inherently embedded in it.

B. Edge Cost

The edge cost consists of two parts. The first part represents the control energy consumption, calculated through the control input with a time trade-off. Specifically, we define the control energy consumption cost of one motion primitive with a duration of $\tau$ and control input $\boldsymbol {u}\!\left ({{ t }}\right)$ as:

$$\begin{equation*} e_{egy}\left ({{ \tau }}\right)=\int _{0}^{\tau }{\left \|{{ \boldsymbol {u}\!\left ({{ t }}\right) }}\right \|^{2}dt+\rho \tau }, \tag {3}\end{equation*}$$ where $\rho \tau$ represents the time weight of the motion primitive, with $\rho$ being the time trade-off coefficient.

The second part represents the smoothness of the trajectory. We denote $\boldsymbol {q}_{cur}$ as the current node, $\boldsymbol {q}_{par}$ as its parent node, and $\boldsymbol {q}_{pro}$ as the node being expanded. Take the x-axis as an example, we introduce $\mathrm {\Delta }x=\boldsymbol {q}_{cur}\left ({{ x }}\right)-\boldsymbol {q}_{pro}\left ({{ x }}\right)$ and $\mathrm {\Delta }x\mathrm {'} = \boldsymbol {q}_{par}\left ({{ x }}\right)-\boldsymbol {q}_{cur}\left ({{ x }}\right)$ for clarity. $\mathrm {\Delta }y$ and $\mathrm {\Delta }y\mathrm {'}$ are defined in a similar manner for the y-axis.

The smoothness cost of one motion primitive is defined as:

$$\begin{equation*} e_{smt}=\left |{{ {tan}^{-1}\left ({{ \frac {\Delta y}{\Delta x} }}\right)- {tan}^{-1}\left ({{ \frac {\Delta y^{\prime }}{\mathrm {\Delta }x^{\prime }} }}\right) }}\right |. \tag {4}\end{equation*}$$

The total cost of a motion primitive is then expressed as the weighted sum of the energy consumption and the smoothness cost:

$$\begin{equation*} e_{total}=\lambda _{e}e_{egy}\left ({{ \tau }}\right)+\lambda _{s}e_{smt}, \tag {5}\end{equation*}$$ where $\lambda _{e}$ , $\lambda _{s}$ are the respective weights. For a trajectory with N motion primitives, the cumulative edge cost is: $$\begin{equation*} g_{c}=\sum \nolimits _{n=1}^{N} e_{total_{n}}. \tag {6}\end{equation*}$$

C. Heuristic Cost and Analytic Expansion

For the heuristic cost, we follow the design in Fast-Planner, utilizing Pontryagin’s Minimum Principle [25] to compute the energy-optimized closed-form trajectory from the current state to the goal state:

$$\begin{align*} \left [{{\begin{array}{cccccccccccccccccccc} \alpha _{n} \\ \beta _{n} \\ \end{array}}}\right ]& =\frac {1}{T^{3}}\left [{{\begin{array}{cccccccccccccccccccc} -12 & \mathrm {6T} \\ 6T & -2T^{2} \\ \end{array}}}\right ]\left [{{\begin{array}{cccccccccccccccccccc} p_{ng}-p_{nc}-v_{nc}T \\ v_{ng}-v_{nc} \\ \end{array}}}\right ], \\ \mathcal {H}^{\ast }\left ({{ T }}\right)& =\mathop {\min }\limits _{T}\left ({{\int _{0}^{T} {\left \|{{ \boldsymbol {u}\!\left ({{ t }}\right) }}\right \|^{2}dt+\rho T} }}\right) \\ & =\mathop {\min }\limits _{T}\left ({{\sum \limits _{n\in \left \{{{x,y }}\right \}} \left ({{ \frac {1}{3}\alpha _{n}^{2}T^{3}+\alpha _{n}\beta _{n}T^{2}+\beta _{n}^{2}T }}\right) +\rho T}}\right), \tag {7}\end{align*}$$ where $p_{nc},v_{nc}$ are the position and velocity of the current node, $p_{ng},v_{ng}$ are the position and velocity of the goal.

Similar to Fast-Planner, we utilize periodic analytic expansions to enhance the search efficiency (illustrated by the green curves in Figure 4). If the trajectory is collision-free (solid green curve), we consider this trajectory as the remaining path segment, thus allowing the search to terminate early.

SECTION V. Gradient-Based Trajectory Optimization

A. Problem Formulation

We first sample the initial trajectory at a given sampling frequency and fit the sampled points into a uniform B-spline. A p-degree B-spline is defined by $N+1$ control points $\left \{{{ \boldsymbol {Q}_{0},\boldsymbol {Q}_{1},\boldsymbol {Q}_{2}\mathrm {,\ldots,}\boldsymbol {Q}_{N} }}\right \}$ and a monotonically non-decreasing knot vector $[t_{0},t_{1},t_{2}\mathrm {,\ldots,}t_{M}]$ , where $\boldsymbol {Q}_{i}~\mathrm {\in }~\mathrm {\mathbb {R}}^{2},t_{m}~\mathrm {\in ~\mathbb {R}}$ and $M=N+p+1$ . A uniform B-spline has identical knot span $\mathrm {\Delta }t_{i}=t_{i+1}-t_{i}$ , and the control points of velocity and acceleration in the world frame is calculated as:

$$\begin{equation*} \boldsymbol {V}_{w,i}=\frac {1}{\Delta t}\left ({{ \boldsymbol {Q}_{i+1}-\boldsymbol {Q}_{i} }}\right), \boldsymbol {A}_{w,i}=\frac {1}{\Delta t}\left ({{ \boldsymbol {V}_{w,i+1}-\boldsymbol {V}_{w,i} }}\right). \tag {8}\end{equation*}$$

We use B-splines in the trajectory optimization because they have some unique properties. Firstly, modifying a few control points only affects the local region of the curve, which can be helpful for localized optimization. Secondly, the B-spline curve lies within the convex hull formed by its control points, ensuring the safety of the trajectory. Finally, the derivatives of a B-spline are also B-splines and adhere to the convex hull property, which ensures the feasibility of controller as long as the control points of the velocity and acceleration trajectories are within the actuator range. Figure 5 summarizes these properties.

FIGURE 5.

Illustration of the B-spline properties, exemplified by third-order B-splines. (a) demonstrates the local control property, where different colors indicate the relationship between control points and their corresponding local curves. (b) illustrates the convex hull property, with different colors showing the relationship between the convex hull and the corresponding segmented curves. By ensuring the safety of the convex hull, the safety of the entire trajectory is ensured. (c) shows the velocity curve derived from the B-spline trajectory in (b). The feasibility is ensured by the convex hull formed by the velocity control points.

We construct the optimization problem in a soft constrained form as:

$$\begin{equation*} \mathop {\min }\limits _{Q}J=\lambda _{s}J_{s}+\lambda _{c}J_{c}+\lambda _{f}J_{f}+\lambda _{t}J_{t}, \tag {9}\end{equation*}$$ where $J_{s},J_{c},J_{f}$ , and $J_{t}$ represent the costs of smoothness, collision, feasibility, and stability, respectively. $\lambda _{s},\lambda _{c},\lambda _{f}$ , and $\lambda _{t}$ are the corresponding weights. For a p-degree B-spline, we purposefully do not optimize the initial and final p control points, as they determine the trajectory’s initial and terminal states and thus should remain unchanged.

1) Smoothness Cost

We minimize the square of trajectory acceleration and jerk to optimize the smoothness of the trajectory. The smoothness cost is formulated as:

$$\begin{equation*} J_{s}=\sum \limits _{i=0}^{N-2} \left \|{{ \boldsymbol {A}_{i} }}\right \|^{2} +\sum \limits _{i=0}^{N-3} \left \|{{ \boldsymbol {J}_{i} }}\right \|^{2}, \tag {10}\end{equation*}$$ where $$\begin{align*} \boldsymbol {A}_{i}& =\left ({{ \boldsymbol {Q}_{i+2}-\boldsymbol {Q}_{i+1} }}\right)-\left ({{ \boldsymbol {Q}_{i+1}-\boldsymbol {Q}_{i} }}\right) \\ & =\boldsymbol {Q}_{i+2}-2\boldsymbol {Q}_{i+1}+\boldsymbol {Q}_{i} \\ {\boldsymbol {J}}_{i}& =\left ({{ \boldsymbol {Q}_{i+3}-2\boldsymbol {Q}_{i+2}+\boldsymbol {Q}_{i+1} }}\right)-\left ({{ \boldsymbol {Q}_{i+2}-2\boldsymbol {Q}_{i+1}+\boldsymbol {Q}_{i} }}\right) \\ & =\boldsymbol {Q}_{i+3}-3\boldsymbol {Q}_{i+2}+3\boldsymbol {Q}_{i+1}-\boldsymbol {Q}_{i}\end{align*}$$

2) Collision Cost

[22] adopts a traditional ESDF-based method for safety cost. However, this approach demands continuous updates of a local ESDF, consuming considerable computational resources and memory. Moreover, it treats the robot as a point mass, potentially leading to conservative or unsafe planning results for quadruped robots.

Therefore, we design our collision cost based on Robo-centric ESDF (RC-ESDF) [26], which requires only a one-time, offline construction of ESDF for the robot’s body shape. This approach not only eliminates the resource consumption of maintaining a local environmental ESDF map but also takes the robot’s shape into account. The RC-ESDF of the quadruped robot is shown in Figure 6, and Figure 7 illustrates its obstacle avoidance process. For a trajectory control point, we first calculate the gradient for each obstacle grid in the body frame within RC-ESDF through bilinear interpolation of the ESDF values. These gradients are then summed and converted into the world frame to obtain the gradient of the control point, enhancing trajectory safety. For a detailed introduction to RC-ESDF, readers may refer to [26], and the RC-ESDF values can be efficiently computed by [27].

FIGURE 6.

(a) visualizes the constructed RC-ESDF, with different colors corresponding to different ESDF values. (b) explains the relationship between RC-ESDF and the robot’s body size, with the dimensions slightly expanded to $1.2m \times 0.75m$ beyond the robot’s body.

FIGURE 7.

The purple squares in (a) denote the obstacle grids, and the orange arrows indicate the gradients of each obstacle grid in the body frame. (b) illustrates the gradient of the trajectory control point in the world frame, derived from the obstacle grid gradients shown in (a).

Using the convex hull property of B-splines, we formulate the safety cost function as:

$$\begin{equation*} J_{c}=\sum \limits _{i=0}^{N-1} {F_{c}\left ({{ \boldsymbol {Q}_{i} }}\right),} \tag {11}\end{equation*}$$ where $F_{c}\left ({{ \boldsymbol {Q}_{i} }}\right)$ is the cumulative cost calculated from M obstacle grids in the RC- ESDF at control point $\boldsymbol {Q}_{i}$ . $$\begin{align*} F_{c}\left ({{ \boldsymbol {Q}_{i} }}\right)& =\sum \limits _{k=1}^{M} F_{k}, \tag {12}\\ F_{k}& =\begin{cases} \displaystyle \left ({{ d_{k}-d_{th} }}\right)^{2},& d_{k}\lt d_{th} \\ \displaystyle 0, & d_{k}\ge d_{th} \end{cases}, \tag {13}\end{align*}$$ where $d_{k}$ is the ESDF value for the k-th obstacle grid, and $d_{th}$ is the predetermined distance threshold.

To calculate $d_{k}$ in (13), we transform the obstacle grids around control point $\boldsymbol {Q}_{i}$ into the body frame at $\boldsymbol {Q}_{i}$ . For the k-th obstacle grid whose world coordinates are $\boldsymbol {q}_{w}^{k}$ , its coordinates $\boldsymbol {q}_{b}^{k}$ in the body frame at control point $\boldsymbol {Q}_{i}$ can be calculated by:

$$\begin{equation*} {\boldsymbol { q}}_{b}^{k}=\boldsymbol {R}_{wb,i}^{-1}\left ({{ \boldsymbol {q}_{w}^{k}-\boldsymbol {Q}_{i} }}\right), \tag {14}\end{equation*}$$ where $\boldsymbol {R}_{wb,i}$ is the rotation matrix from body frame to world frame at control point $\boldsymbol {Q}_{i}$ : $$\begin{align*} \boldsymbol {R}_{wb,i}=\left [{{\begin{array}{cccccccccccccccccccc} cos\theta _{i} & \quad -sin\theta _{i} \\ sin\theta _{i} & \quad cos\theta _{i} \\ \end{array}}}\right ],\end{align*}$$ where $\theta _{i}$ is the orientation of the robot at control point $\boldsymbol {Q}_{i}$ . Considering the optimal longitudinal motion capability of the quadruped robot, we set the robot’s orientation to be aligned with the trajectory’s tangent direction. The orientation of the robot at control point $\boldsymbol {Q}_{i}$ can be calculated by: $$\begin{equation*} \theta _{i}=\tan ^{-1}\left ({{ \frac {\boldsymbol {Q}_{i+1}\left ({{ y }}\right)-\boldsymbol {Q}_{i}\left ({{ y }}\right)}{\boldsymbol {Q}_{i+1}\left ({{ x }}\right)-\boldsymbol {Q}_{i}\left ({{ x }}\right)} }}\right). \tag {15}\end{equation*}$$

The gradient of the cost function $F_{c}\left ({{ \boldsymbol {Q}_{i} }}\right)$ with respect to the control point $\boldsymbol {Q}_{i}$ is obtained by:

$$\begin{equation*} \frac {\partial F_{c}\left ({{ \boldsymbol {Q}_{i} }}\right)}{\partial \boldsymbol {Q}_{i}} =-2\boldsymbol {R}_{wb,i}\sum \limits _{k=1}^{M} {\left ({{ d_{k}-d_{th} }}\right)\frac {\partial d_{k}}{\partial \boldsymbol {q}_{b}^{k}}}, \tag {16}\end{equation*}$$ where ${\partial d_{k}} \mathord {\left /{{\vphantom {{\partial d_{k}} {\partial \boldsymbol {q}_{b}^{k}}}}}\right. \hspace {-1.2pt} } {\partial \boldsymbol {q}_{b}^{k}}$ can be efficiently computed through bilinear interpolation of the RC-ESDF values.

3) Feasibility Cost

The feasibility cost primarily aims to constrain the trajectory’s velocity and acceleration to remain within the actuator range. The feasibility cost is defined as the penalization of velocities and accelerations that exceed the actuator limits.

Due to the distinct longitudinal and latitudinal movement capabilities of quadruped robots, we design our feasibility cost as follows:

$$\begin{equation*} J_{f}=\sum \limits _{i=0}^{N-1} {F_{d}\left ({{ {\boldsymbol {V}}_{b,i} }}\right)+} \sum \limits _{i=0}^{N-2} {F_{d}\left ({{ {\boldsymbol {A}}_{b,i} }}\right)}, \tag {17}\end{equation*}$$ where $$\begin{align*} & F_{d}\left ({{ \boldsymbol {V}_{b,i} }}\right) \\ & =f_{x}\left ({{ \boldsymbol {V}_{b,i}^{\left ({{ x }}\right)} }}\right)+f_{y}\left ({{ \boldsymbol {V}_{b,i}^{\left ({{ y }}\right)} }}\right), \\ & f_{x}\left ({{ \boldsymbol {V}_{b,i}^{\left ({{ x }}\right)} }}\right) \\ & =\begin{cases} \displaystyle e^{\boldsymbol {V}_{b,i}^{\left ({{ x }}\right)}\boldsymbol {-}V_{max,f}}-1,& \boldsymbol {V}_{b,i}^{\left ({{ x }}\right)}\boldsymbol {\ge }V_{max,f} \\ \displaystyle 0, & {V_{max,b}\boldsymbol {\le V}}_{b,i}^{\left ({{ x }}\right)}\boldsymbol {\lt }V_{max,f} \\ \displaystyle e^{V_{max,b}\boldsymbol {-}\boldsymbol {V}_{b,i}^{\left ({{ x }}\right)}}\boldsymbol {-}1,& \boldsymbol { V}_{b,i}^{\left ({{ x }}\right)}\boldsymbol {\lt }V_{max,b} \end{cases}, \\ & f_{y}\left ({{ \boldsymbol {V}_{b,i}^{\left ({{ y }}\right)} }}\right) \\ & =\begin{cases} \displaystyle e^{\boldsymbol {V}_{b,i}^{\left ({{ y }}\right)}\boldsymbol {-}V_{max,l}}-1,& \boldsymbol {V}_{b,i}^{\left ({{ y }}\right)}\boldsymbol {\ge }V_{max,l} \\ \displaystyle 0, & -V_{max,l}\boldsymbol {\le } \boldsymbol {V}_{b,i}^{\left ({{ y }}\right)}\boldsymbol {\lt }V_{max,l} \\ \displaystyle e^{\boldsymbol {-}V_{max,l}\boldsymbol {-}\boldsymbol {V}_{b,i}^{\left ({{ y }}\right)}}-1,& \boldsymbol {V}_{b,i}^{\left ({{ y }}\right)}\boldsymbol {\lt }-V_{max,l} \end{cases}. \tag {18}\end{align*}$$ The velocity cost $F_{d}\left ({{ \boldsymbol {V}_{b,i} }}\right)$ is designed as (18), where $\boldsymbol {V}_{b,i}$ is the trajectory’s velocity control point in the body frame. $V_{max,f}\boldsymbol {,}V_{max,b}\boldsymbol {,}V_{max,l}$ are the maximum allowable velocities of the robot in the forward, backward, and lateral directions, respectively. The acceleration cost $F_{d}\left ({{ \boldsymbol {A}_{b,i} }}\right)$ has the identical form. The control points $\boldsymbol {V}_{b,i}$ and $\boldsymbol {A}_{b,i}$ for the velocity and acceleration in the body frame are calculated with $\boldsymbol {R}_{wb,i}$ : $$\begin{equation*} \boldsymbol {V}_{b,i}=\boldsymbol {R}_{wb,i}^{-1}\boldsymbol {V}_{w,i},~ \boldsymbol {A}_{b,i}=\boldsymbol {R}_{wb,i}^{-1}\boldsymbol {A}_{w,i}. \tag {19}\end{equation*}$$

4) Stability Cost

Since quadruped robots often work on uneven terrain, excessive velocity changes may introduce significant instability. To mitigate this risk, we design the stability cost by imposing penalties on the excessive changes in the robot’s velocity. This cost aims to ensure smoother transitions in the robot’s speed:

$$\begin{equation*} J_{t}=\sum \limits _{\mu \in \left \{{{x,y }}\right \}} \sum \limits _{i=0}^{N-2} {F_{t}\left ({{ {\Delta V}_{\mu,i} }}\right)}, \tag {20}\end{equation*}$$ where $$\begin{align*} & F_{t}\left ({{ {\Delta V}_{\mu,i} }}\right) \\ & =\begin{cases} \displaystyle \left ({{ {\Delta V}_{\mu,i}^{2}-{\Delta V}_{\mu,max}^{2} }}\right)^{2}, & {\Delta V}_{\mu,i}^{2}\ge {\Delta V}_{\mu,max}^{2} \\ \displaystyle 0,& {\Delta V}_{\mu,i}^{2}\lt {\Delta V}_{\mu,max}^{2} \end{cases},\end{align*}$$ ${\mathrm {\Delta }\boldsymbol {V}}_{i}=\boldsymbol {V}_{b,i+1}-\boldsymbol {V}_{b,i}$ is the difference between adjacent velocity control points in the body frame, ${\mathrm {\Delta }\boldsymbol {V}}_{max}$ is the maximum allowable velocity change of the robot’s body.

B. Trajectory Iterative Refinement

To ensure the feasibility of the planned trajectory, we enlarge the infeasible knot spans and represent the trajectory as a non-uniform B-spline. For a p-degree non-uniform B-spline trajectory, the velocity and acceleration control points in the world frame are:

$$\begin{align*} \boldsymbol {V}_{w,i}& =\frac {p\left ({{ \boldsymbol {Q}_{i+1}-\boldsymbol {Q}_{i} }}\right)}{t_{i+p+1}-t_{i+1}}, \\ \boldsymbol {A}_{w,i}& =\frac {(p-1)\left ({{ \boldsymbol {V}_{w,i+1}-\boldsymbol {V}_{w,i} }}\right)}{t_{i+p+1}-t_{i+2}}. \tag {21}\end{align*}$$

The corresponding control points in the body frame are obtained by (19). Denote $\boldsymbol {V}_{b,i}$ the infeasible velocity and $\boldsymbol {A}_{b,i}$ the acceleration in the body frame, the enlarged knot spans are calculated as follows:

$$\begin{align*} {\Delta t}_{i}^{\prime }& =min\left \{{{\gamma,max\left \{{{F_{re,v}\left ({{ \boldsymbol {V}_{b,i} }}\right),F_{re,a}\left ({{ \boldsymbol {A}_{b,i} }}\right) }}\right \} }}\right \}\cdot \Delta t_{i}, \\ & F_{re,v}\left ({{ \boldsymbol {V}_{b,i} }}\right) \\ & =\begin{cases} \displaystyle max\left \{{{\frac {\boldsymbol {V}_{b,i}^{\left ({{ x }}\right)}}{V_{max,f}},\frac {\boldsymbol {V}_{b,i}^{\left ({{ y }}\right)}}{V_{max,l}} }}\right \},& \boldsymbol {V}_{b,i}^{\left ({{ x }}\right)}\ge 0 \\ \displaystyle max\left \{{{\frac {\boldsymbol {V}_{b,i}^{\left ({{ x }}\right)}}{V_{max,b}},\frac {\boldsymbol {V}_{b,i}^{\left ({{ y }}\right)}}{V_{max,l}} }}\right \},& \boldsymbol {V}_{b,i}^{\left ({{ x }}\right)}\lt 0 \end{cases}, \\ & F_{re,a}\left ({{ \boldsymbol {A}_{b,i} }}\right) \\ & =\begin{cases} \displaystyle max\left \{{{\left ({{ \frac {\boldsymbol {A}_{b,i}^{\left ({{ x }}\right)}}{A_{max,f}} }}\right)^{\frac {1}{2}},\left ({{ \frac {\boldsymbol {A}_{b,i}^{\left ({{ y }}\right)}}{A_{max,l}} }}\right)^{\frac {1}{2}} }}\right \}, & \boldsymbol {A}_{b,i}^{\left ({{ x }}\right)}\ge 0 \\[2pt] \displaystyle max\left \{{{\left ({{ \frac {\boldsymbol {A}_{b,i}^{\left ({{ x }}\right)}}{A_{max,b}} }}\right)^{\frac {1}{2}},\left ({{ \frac {\boldsymbol {A}_{b,i}^{\left ({{ y }}\right)}}{A_{max,l}} }}\right)^{\frac {1}{2}} }}\right \}, & \boldsymbol {A}_{b,i}^{\left ({{ x }}\right)}\lt 0 \end{cases}, \tag {22}\end{align*}$$ where $\gamma$ is a constant slightly larger than 1 to prevent over-enlarging the knot spans. $V_{max,f},V_{max,b},V_{max,l},A_{max,f}, A_{max,b}$ and $A_{max,l}$ are the robot’s maximum allowable forward, backward, and lateral velocities and accelerations, respectively. As quadruped robots have different maximum allowable speeds and accelerations in the forward and backward directions, the enlarged knot spans need to be calculated based on the robot’s direction of movement.

SECTION VI. Experiment Results

A. Implementation Details

In this section, utilizing the system architecture described in Section III-B (see Figure 3), a finite state machine is adopted to coordinate the various modules of the system. In each planning cycle, it executes the path searching described in Section IV and the trajectory optimization described in Section V. The optimized trajectory is then evaluated by the control module for execution by the quadruped robot.

We use the nonlinear optimization solver NLopt3 to solve the constructed unconstrained optimization problem in section V-A. The Aliengo robot runs in trot gait and its kinematic constraints are shown in Table 1. For the perception and state estimation, we utilize the map and localization provided by the LiDAR’s internal high-precision SLAM algorithm. To mitigate the impact of environmental perceptual errors on autonomous navigation, we pre-constructed the map with the LiDAR mounted on the robot, which is then used to correct only minor errors during navigation.

TABLE 1 Kinematic Constraints of Quadruped Robot

In the path searching stage, considering the quadruped robot model is not precisely integrated, we select moderate values within the kinematic limits. For the trajectory optimization, we set the B-spline order to $p=3$ and set the weights $\lambda _{s},\lambda _{c}$ to be much larger than $\lambda _{f}$ , $\lambda _{t}$ . This is because we prefer to first generate a trajectory that is geometrically safe and smooth, and then eliminate the infeasibilities through iterative refinement later.

We compare our method with Fast-Planner [22] in both simulated and real-world environments. Because Fast-Planner is unable to handle the distinct motion capabilities of quadruped robots in longitudinal and latitudinal directions, we adopt the constraints of path searching in Table 1 for it and use its default parameters for the rest, and we modify Fast-Planner’s planning space to better align with the movement plane of the quadruped robot.

In the computation of the collision cost, the extensive interpolation calculations lead to the long plan time. To increase the optimization efficiency, we pre-calculate ESDF values and corresponding gradients for RC-ESDF at $0.01m$ resolution, which enables the quick retrieval of the ESDF values and gradients using a look-up table method. In addition, we adopt a receding-horizon re-planning strategy to enhance system robustness.

B. Simulation Benchmark Comparisons

In the simulations, we focus on the planning performance without the execution of the robot. The simulated environment is a $30m \times 30m$ field with 120 randomly deployed obstacles. Fixed start and goal positions are set approximately 40 meters apart. We perform simulations on three different map resolutions ($0.05m$ , $0.75m$ , $0.1m$ ). For each resolution, both algorithms are run 50 times, with each test featuring a random obstacle distribution. The simulation results are listed in Table 2, and Figure 8 shows one simulation result.

TABLE 2 Simulation Trajectory Planning Comparison

FIGURE 8.

Visualization of one simulation result. The black areas represent environmental obstacles. The red and blue areas illustrate the obstacle expansion regions for our method and Fast-Planner, respectively.

Simulation results indicate that our method plans trajectories with shorter distances and lower energy consumption (Integral of squared acceleration) across different resolutions. This can be attributed to our non-point mass model in the computation of the collision cost, which requires only slight expansion of the obstacles to compensate for the LiDAR scanning gaps, yielding larger free spaces and shorter trajectories through narrower spaces, as shown in Figure 8. Furthermore, our method integrates the movement characteristics of the quadruped robot into the optimization and mitigates aggressive velocity changes in the trajectory through stability optimization, which helps improve the energy efficiency. The planning time of our method is longer, primarily due to the separate cost calculations required for each obstacle grid within the RC-ESDF at each control point for collision cost. However, our approach efficiently avoids the resource consumption to maintain a local environmental ESDF map (see Section VI-C) and incorporates the robot’s shape into the optimization. Moreover, due to our hierarchical deployment of planning and control modules, the real-time performance of the system is ensured to some extent, which mitigates the impact of the longer planning time on the system.

C. Real-World Benchmark Comparison

Our test field is a $7.8m \times 5.4m$ area, containing 15 obstacles (each measuring $0.6m \times 0.6m \times 0.6m$ ), and multiple wooden boards randomly placed on the ground to emulate the uneven terrain, introducing additional instability for the quadruped robot (Figure 1).

1) Autonomous Navigation Experiment

We first conduct the autonomous navigation experiment with fixed start and goal positions about 6 meters apart, and repeat the experiment 10 times for each method. Figure 1 shows a composite image of one such trajectory using our method. The results of the 10 experiments are averaged, and the comparison results are shown in Table 3.

TABLE 3 Real-World Autonomous Navigation Comparison

The experiment results are consistent with those from the simulation experiments. In particular, in the autonomous navigation experiments, the trajectories planned by our algorithm demonstrated advantages in terms of lower energy consumption and shorter trajectory length. Notably, our method benefits from a one-time RC-ESDF construction at system initialization. In contrast, Fast-Planner requires continuous updates of the local environmental ESDF during navigation (set to update within a $5m$ radius of the robot), resulting in a significantly longer ESDF construction time.

Additionally, our algorithm fully integrates the anisotropic motion characteristics of the quadruped robot in trajectory optimization, effectively avoiding overly aggressive movements. Although this results in a slightly longer completion time compared to Fast-Planner, it achieves a higher success rate. In contrast, the success rate of Fast-Planner is only 60%, with some aggressive movements during navigation, causing collisions with the obstacles.

To demonstrate the stability during navigation, we analyze the velocity and acceleration, with the comparative results shown in Table 4. The results indicate that although our method exhibits a lower average speed and acceleration, leading to a marginally longer completion time compared to Fast-Planner, it demonstrates smaller standard deviations in both velocity and acceleration, which indicates higher stability of our proposed method.

TABLE 4 Autonomous Navigation Stability Comparison

2) Autonomous Cruising Experiment

To comprehensively verify the performance of our method, we also conduct two sets of autonomous cruising experiments involving multiple continuous navigation tasks. Within the test field, we preset multiple fixed waypoints for the quadruped robot to sequentially navigate through and finally return to the start. Table 5 shows the comparative results for these experiments, and Figure 9 visualizes the cruise trajectories.

TABLE 5 Real-World Autonomous Cruising Comparison

FIGURE 9.

Composite images and visualization of two sets of autonomous cruising experiments with our method. The green dots and arrows represent the waypoints and the trajectories directions, respectively. The blue curves show the trajectories of the quadruped robot.

As shown in Table 5, the results of the autonomous cruising experiments are consistent with those of the autonomous navigation experiment. In the autonomous cruising experiment, our proposed method takes less time. This is primarily due to our shorter obstacle expansion distance in narrow passages, which decreases the collision cost and leads to higher speeds, thereby reducing the completion time.

SECTION VII. Conclusion

In this paper, a safe and robust motion planning system is proposed for the autonomous navigation of quadruped robots in cluttered environments.

We decompose the planning process into front-end and back-end stages. In the front-end stage, we quickly search for a smooth and energy-efficient initial trajectory. In the back-end stage, we enhance the trajectory’s smoothness, safety, and motion stability by utilizing the convex hull and local control properties of B-splines. Finally, we eliminate the infeasibilities through iterative refinement to ensure the feasibility of the trajectory. The system is validated in both simulated and real-world environments. Benchmark comparisons demonstrate that the trajectories generated by our method have low energy consumption and high stability, while achieving high success rates in autonomous navigation. The proposed method achieves 100% success rates in real-world experiments, compared to 60% and 75% for Fast-Planner, and reduces energy consumption by approximately 35%–50% on average. Additionally, the proposed method demonstrates more stable velocity and acceleration profiles, with lower mean and standard deviation values compared to Fast-Planner.