Universal Trajectory Optimization Framework for Differential Drive Robot Class

TL;DR

The paper tackles universal trajectory optimization for differential drive robots by introducing a Motion State (MS) trajectory representation that linearizes high-order kinematics and naturally incorporates nonholonomic constraints and lateral slip. It couples a MINCO-based, high-order polynomial optimization with final-position augmentation (PHR-ALM), trajectory preprocessing, EKF-based parameter estimation, and NMPC tracking to form a robust planning-control stack. The approach is validated through extensive simulations and real-world experiments across SDD, SKDD, and TDD platforms, demonstrating superior trajectory quality, safety, and computational efficiency in crowded and unknown environments. The framework's universality and open-source release promise broad applicability and rapid adoption in autonomous mobile robotics planning and control.

Abstract

Differential drive robots are widely used in various scenarios thanks to their straightforward principle, from household service robots to disaster response field robots. There are several types of driving mechanisms for real-world applications, including two-wheeled, four-wheeled skid-steering, tracked robots, and so on. The differences in the driving mechanisms usually require specific kinematic modeling when precise control is desired. Furthermore, the nonholonomic dynamics and possible lateral slip lead to different degrees of difficulty in getting feasible and high-quality trajectories. Therefore, a comprehensive trajectory optimization framework to compute trajectories efficiently for various kinds of differential drive robots is highly desirable. In this paper, we propose a universal trajectory optimization framework that can be applied to differential drive robots, enabling the generation of high-quality trajectories within a restricted computational timeframe. We introduce a novel trajectory representation based on polynomial parameterization of motion states or their integrals, such as angular and linear velocities, which inherently matches the robots' motion to the control principle. The trajectory optimization problem is formulated to minimize complexity while prioritizing safety and operational efficiency. We then build a full-stack autonomous planning and control system to demonstrate its feasibility and robustness. We conduct extensive simulations and real-world testing in crowded environments with three kinds of differential drive robots to validate the effectiveness of our approach.

Figures (16)

• Figure 1: Various driving mechanisms of differential drive (DD) robots, along with their corresponding kinematic models and planning results. (a) Two-wheeled standard differential drive (SDD) robot. (b) Skid-steering (SKDD) robot. (c) Tracked (TDD) robot.
• Figure 2: The optimized trajectory and the simulated execution results for two types of robots in narrow environments. The robots should map online to perceive the environment and replan to avoid obstacles. To verify the performance of the planner, a specifically designed map requires the robot to execute rotations or reversals at both the start and end points. In the upper right corner, from left to right, snapshots showcase the motion and mapping of the TDD robot, which moves with lateral slip. In the lower left corner, from right to left, is the SDD robot, which does not experience lateral slip.
• Figure 3: Kinematic models of SDD (left) and TDD (right) robots. The green point represents the ICR of the robot's body, and the blue points represent the ICRs at the contact points between the driving wheels and the ground. For tracked robots, slipping causes the ICRs of the tracks to misalign with their ground projections. $(x_{Iv}, y_{Il}), (x_{Iv}, y_{Ir}), (x_{Iv}, y_{Iv})$ represent the positions of the ICRs of the left track, right track, and the body, respectively, in the robot's body coordinate frame.
• Figure 4: Comparison of the trajectory in $x-y$ space (left) and $\theta-s$ space (right). When planning on $x-y$ space with differential flatness, singularities occur when the moving direction changes (red points). However, these can be represented as a smooth trajectory in $\theta-s$ space.
• Figure 5: Overview of the a example navigation system with our planner. In perception, lidar is used for Lidar-Inertial Odometry (LIO) and mapping. With the result of odometry and ESDF map, our planner uses Jump Point Search (JPS) to get the global path (brown) and to generate the initial MS trajectory (purple). Trajectory pre-processing (blue) ensures close alignment of the trajectory with the global path. The Augmented Lagrangian Method (ALM) iteratively optimizes the trajectory to satisfy constraints (yellow) and reach the desired final positions (red). The system uses pre-integration to get the reference trajectory and selects appropriate NMPC controller depending on the driving method.