Universal Trajectory Optimization Framework for Differential Drive Robot Class

Mengke Zhang ^{1, 2}
Nanhe Chen ^{1, 2}
Hu Wang ³
Jianxiong Qiu ³
Zhichao Han ^{1, 2}
Qiuyu Ren ^{1, 2}
Chao Xu ^{1, 2}
Fei Gao ^{1, 2}
Yanjun Cao ^{1, 2}

¹ ZJU-CSE
² HIZJU
³ Zhejiang Zhongyan

Various differential-drive (DD) robots, kinematics models and planning results. (a) Two-wheeled differential-drive (SDD) robot. (b) Skid-steering (SKDD) robot. (c) Tracked (TDD) robot.

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 exist lateral slip.

Abstract

Differential drive robots are widely used in various scenarios thanks to their straightforward principle, from household service robots to disaster response field robots. The nonholonomic dynamics and possible lateral slip of these robots lead to difficulty in getting feasible and high-quality trajectories. Although there are several types of driving mechanisms for real-world applications, they all share a similar driving principle, which involves controlling the relative motion of independently actuated tracks or wheels to achieve both linear and angular movement. Therefore, a comprehensive trajectory optimization to compute trajectories efficiently for various kinds of differential drive robots is highly desirable. In this paper, we propose a universal trajectory optimization framework, enabling the generation of high-quality trajectories within a restricted computational timeframe for these robots. 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 computation 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.

System

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.

Simulation results

Planning results of TEB (a), and the proposed method without (b) and with (c) lateral slip of $x_{Iv}$, when lateral slip cannot be ignored. We calculate the average tracking error and maximum tracking error of the controller. By modeling lateral slip into the MS trajectory, the controller achieves better performance in tracking the proposed trajectory.

Visual comparison of different methods. Proposed method and TEB are set to allow moving backward. To better illustrate the details, we zoom in three areas(\textcircled{1} to \textcircled{3}) with distinct differences. Specifically, in region \textcircled{2}, we show the via-points of TEB.

Patrolling with DF and Proposed methods, the next target is provided to the planner upon nearing the current target. We zoom in four position(\textcircled{1} -- \textcircled{4}) to show the smoothness and efficiency difference. \textcircled{1} is the starting position, with the initial state directed towards the positive right-hand side. We use the gradient color to show the angular velocity in \textcircled{2}-\textcircled{4}. The robot moves around the obstacle in \textcircled{2}. In \textcircled{3} and \textcircled{4}, a new target is provided to the planner, prompting the robot to turn towards the new target.

Simulation results for replanning. We use 360° radar simulator to create maps during runtime. The upper figure shows the simulation results of the U-shaped environment. The lower figure shows a specially designed environment containing many obstacles that may be sheltered.

Experiments

Two-wheel differential drive robot

TRACER MINI is employed as our two-wheel differential drive robot platform, which is controlled through linear and angular velocity. Map is pre-built and the motion capture system is utilized for localization.

Skid-steering robot

SCOUT MINI is employed as our skid-steering robot platform, which is controlled via linear and angular velocity. We randomly place obstacles and use fastlio for localization, simulating the real-world application.