As a researcher in robotics, I have always been fascinated by the potential of quadruped robots, often referred to as robot dogs, to navigate complex environments without relying on prior maps. In this article, I present a hierarchical robust motion planning method designed specifically for quadruped robots operating in mapless scenarios. The challenges of inefficient planning and instability due to the anisotropic locomotion characteristics of robot dogs are addressed through a combination of front-end trajectory initialization and back-end optimization. By leveraging real-time sensor data, this approach ensures smooth, safe, and executable paths for quadruped robots in obstacle-dense environments.
The core of our method lies in separating high-level planning from low-level motion control. For the front-end, we employ a polynomial MiniSnap algorithm combined with a weighted hybrid A* algorithm to initialize collision-free trajectories. This addresses issues such as difficulty in tracking and local optima entrapment. To enhance the efficiency and quality of local path searching, we introduce a weighting factor in the evaluation function and impose constraints on yaw angle and lateral motion. This consideration of the robot dog’s orientation and movement anisotropy significantly improves robustness. Based on the initial trajectory, the back-end optimizes B-spline control points by constructing an optimization model that accounts for smoothness, collision avoidance, and dynamic feasibility. Iterative optimization and time reallocation strategies ensure the final trajectory is both practical and reliable for quadruped robots.
In mapless environments, the lack of global information necessitates reliance on local sensor inputs, such as LiDAR, to perceive obstacles. For a quadruped robot, this real-time data acquisition is critical for adapting to dynamic surroundings. Our front-end approach begins with generating an initial trajectory using the MiniSnap method, which minimizes the snap (fourth derivative) of the path to ensure smoothness without considering obstacles. However, this initial path may collide with unknown obstacles. To rectify this, we segment the trajectory and apply a weighted hybrid A* algorithm to each colliding segment. The hybrid A* algorithm extends traditional A* by incorporating continuous state space and yaw angle as part of the node state, making it suitable for the anisotropic nature of robot dogs. The cost function for node expansion is defined as:
$$f(n) = g(n) + \sigma \cdot h(n)$$
where \( g(n) \) represents the cumulative cost from the start node to the current node \( n \), \( h(n) \) is the heuristic cost to the goal, and \( \sigma \) is a weighting factor that dynamically adjusts the search efficiency. We set \( \sigma = 2.0 \) based on experimental results to balance planning time and path length. The heuristic function combines Euclidean distance and non-holonomic constraints using Reed-Shepp curves:
$$h(n) = \max \left( h_{\text{Euclidean}}(n), h_{\text{non-holonomic}}(n) \right)$$
with \( h_{\text{non-holonomic}}(n) = L_{\text{RS}}(p_n, p_g) \), where \( L_{\text{RS}} \) denotes the length of the Reed-Shepp curve between the current node and goal. To further refine the path, we incorporate yaw angle penalties in the cost function to minimize abrupt turns, which is essential for the stability of a quadruped robot. The updated cost function includes:
$$g'(n) = \sum_{i=1}^{n} d(p_{i-1}, p_i) \cdot \left( 1 + \lambda \cdot |\delta_i| \right)$$
where \( d(p_{i-1}, p_i) \) is the Euclidean distance between consecutive nodes, \( \delta_i \in [-3, 3] \) represents the yaw angle state, and \( \lambda \) is a weight factor. Node expansion considers the robot dog’s kinematics:
$$\begin{bmatrix} x_n \\ y_n \end{bmatrix} = \begin{bmatrix} x_{n-1} + r \cdot (\sin(\theta + \beta) – \sin(\theta)) \\ y_{n-1} + r \cdot (\cos(\theta + \beta) – \cos(\theta)) \end{bmatrix}$$
where \( r \) is the turning radius, \( \theta \) is the orientation angle, and \( \beta \) is the steering angle. This ensures that the generated path adheres to the motion constraints of a quadruped robot.
Once the front-end generates a collision-free guide path, we use B-spline curves to represent the trajectory due to their convex hull property, which confines the curve within the convex hull of control points. This property is leveraged to initialize control points by pushing them away from obstacles. For each control point \( Q_i \) that lies within an obstacle, we compute a repulsion direction based on the intersection of the tangent’s normal plane with the obstacle surface. The distance from \( Q_i \) to the j-th obstacle is given by:
$$d_{ij} = (Q_i – p_{ij}) \cdot v_{ij}$$
where \( p_{ij} \) is the point on the obstacle surface and \( v_{ij} \) is the repulsion vector. This initialization ensures that the trajectory remains safe for the robot dog without requiring a global Euclidean Signed Distance Field (ESDF), reducing computational overhead.
For the back-end optimization, we formulate a multi-objective problem to refine the B-spline trajectory. The optimization minimizes a cost function that incorporates smoothness, collision avoidance, and dynamic feasibility. Let \( \Phi \) represent the uniform B-spline trajectory defined by order \( k \), control points \( \{Q_1, Q_2, \dots, Q_N\} \), and a knot vector. The overall optimization problem is:
$$\min_{Q} \sum_{i=1}^{n} \lambda_i f_i(Q)$$
where \( \lambda_i \in (0,1) \) are weights for each objective. The smoothness penalty minimizes the square of velocity and acceleration control points:
$$f_s(Q) = \sum_{k=1}^{N} \left( \lambda_v \cdot \| \dot{Q}_k \|^2 + \lambda_{\text{acc}} \cdot \| \ddot{Q}_k \|^2 \right)$$
Here, \( \dot{Q}_k \) and \( \ddot{Q}_k \) denote the first and second derivatives of the control points, corresponding to velocity and acceleration. The collision penalty uses a circular model to account for the rectangular footprint of a quadruped robot. We define an inner circle radius \( r_0 = 0.55 \, \text{m} \) and an outer circle radius \( r_1 = 0.75 \, \text{m} \), with a safety distance \( s_f = 0.5 \, \text{m} \). The penalty function \( f_c(q_{ij}) \) for a control point is designed to be second-order continuous:
$$f_c(q_{ij}) = \begin{cases}
0 & \text{if } q_{ij} \geq s_f \\
-3(q_{ij} – s_f)^2 + 3(q_{ij} – s_f)^3 & \text{if } s_f – r_1 < q_{ij} \leq s_f \\
3(q_{ij} – s_f + r_0)^2 – 3(q_{ij} – s_f + r_0)^3 & \text{if } s_f – r_0 \leq q_{ij} \leq s_f – r_1 \\
1 & \text{if } q_{ij} < s_f – r_0
\end{cases}$$
where \( q_{ij} \) is the distance measure. The total collision penalty sums over all control points and obstacle pairs:
$$f_c(Q) = \sum_{i=1}^{N} \sum_{j=1}^{N_p} f_c(q_{ij})$$
The gradient of this penalty with respect to control points is computed to guide optimization. For dynamic feasibility, we constrain the maximum velocity and acceleration of the quadruped robot. The feasibility penalty is:
$$f_d(Q) = \sum_{i=1}^{N} \left( \omega_v \cdot F(V_i) + \omega_a \cdot F(A_i) \right)$$
where \( F(D) \) is a function that penalizes values exceeding limits:
$$F(D) = \begin{cases}
(D – D_{\text{max}})^2 & \text{if } D \geq D_{\text{max}} \\
0 & \text{if } D < D_{\text{max}}
\end{cases}$$
and \( \omega_v \), \( \omega_a \) are weights. The combined objective function for optimization is:
$$\min f(Q) = \lambda_s f_s(Q) + \lambda_c f_c(Q) + \lambda_d f_d(Q)$$
We solve this using the L-BFGS solver for efficient convergence. After optimization, time reallocation is performed to ensure dynamic feasibility by adjusting the time spans based on the ratio of exceeded limits:
$$r_e = \max \left( \frac{|V_{i,\mu}|}{v_{\text{max}}}, \frac{|A_{j,\mu}|}{a_{\text{max}}} \right)$$
where \( v_{\text{max}} = 0.6 \, \text{m/s} \) and \( a_{\text{max}} = 0.3 \, \text{m/s}^2 \) for the robot dog. The new time span is \( \Delta t’ = r_e \cdot \Delta t \). Finally, anisotropic curve fitting is applied to maintain trajectory shape while adhering to time constraints, with a fitting penalty:
$$f_t(Q) = \int_0^1 \left( \frac{d_a(\alpha)}{a} \right)^2 + \left( \frac{d_r(\alpha)}{b} \right)^2 \, d\alpha$$
where \( d_a \) and \( d_r \) are axial and radial displacements, and \( a \), \( b \) are ellipse semi-axes. This comprehensive approach ensures that the quadruped robot can navigate mapless environments robustly.
To validate our method, we conducted simulations in Gazebo using the Unitree A1 quadruped robot model equipped with a 16-line LiDAR. Two environments were tested: one with regular obstacles (e.g., cubes and cylinders) and another with irregular obstacles (e.g., triangles, T-shapes, and concave structures). The robot dog operated in a trot gait with maximum velocities set to \( v_x = 0.6 \, \text{m/s} \) and \( v_y = 0.2 \, \text{m/s} \), and acceleration \( a_{\mu} = 0.3 \, \text{m/s}^2 \). We compared our method against Ego-Planner, a mapless trajectory planning algorithm for quadrotors adapted for quadruped robots by constraining the search to 2D planes.
| Environment | Algorithm | Success Rate | Path Length (m) | Avg Speed (m/s) | Max Speed (m/s) | Planning Time (ms) | Execution Time (s) |
|---|---|---|---|---|---|---|---|
| Regular Obstacles | Our Method | 0.99 | 6.43 | 0.52 | 0.65 | 32.45 | 37.68 |
| Regular Obstacles | Ego-Planner | 0.97 | 7.38 | 0.50 | 0.62 | 43.57 | 46.93 |
| Irregular Obstacles | Our Method | 0.95 | 5.32 | 0.48 | 0.63 | 34.70 | 43.81 |
| Irregular Obstacles | Ego-Planner | 0.92 | 5.87 | 0.45 | 0.59 | 47.84 | 57.05 |
The results demonstrate that our method reduces planning time by 24% and execution time by 26.5% on average in obstacle-dense environments. The success rate and path smoothness are higher, attributed to the weighted hybrid A* initialization and yaw angle constraints. For instance, in environments with irregular obstacles, the quadruped robot achieved shorter paths and faster average speeds, indicating improved robustness. Visualizations in Rviz showed that our trajectories are smoother and avoid oscillatory movements, which is critical for the stability of a robot dog. The circular collision model effectively kept the trajectory at a safe distance from obstacles, reducing the risk of sensor degradation in dense settings.
In terms of computational efficiency, the use of B-spline optimization without ESDF reduced the overhead associated with distance field calculations. The table below summarizes the key parameters used in our experiments for the quadruped robot:
| Parameter | Value | Description |
|---|---|---|
| B-spline Order \( k \) | 3 | Polynomial order for trajectory |
| Control Points \( N \) | 30 | Number of B-spline control points |
| Inner Radius \( r_0 \) | 0.55 m | Inner circle radius for collision model |
| Outer Radius \( r_1 \) | 0.75 m | Outer circle radius for collision model |
| Safety Distance \( s_f \) | 0.5 m | Minimum safe distance from obstacles |
| Max Velocity \( v_x \) | 0.6 m/s | Maximum forward velocity |
| Max Velocity \( v_y \) | 0.2 m/s | Maximum lateral velocity |
| Max Acceleration \( a_{\mu} \) | 0.3 m/s² | Maximum acceleration |
| Weight \( \sigma \) | 2.0 | Weighting factor for hybrid A* |
Through extensive testing, we observed that the quadruped robot consistently performed better with our method, especially in scenarios requiring frequent turns and obstacle avoidance. The integration of front-end and back-end components ensured that the robot dog could adapt to local changes without global map dependencies. This is particularly important for applications like search and rescue, where environments are unpredictable. The emphasis on yaw and lateral motion constraints prevented unnecessary energy consumption and enhanced the natural movement of the quadruped robot.
In conclusion, our robust motion planning method for quadruped robots in mapless environments effectively addresses the challenges of efficiency and stability. By combining weighted hybrid A* initialization with B-spline optimization, we achieve shorter, smoother, and safer trajectories for robot dogs. Future work will focus on integrating multi-sensor fusion, such as combining LiDAR with vision, to improve perception accuracy. Additionally, we plan to explore terrain-aware planning for highly complex landscapes, further advancing the autonomy of quadruped robots in real-world scenarios.