Introduction

Truck-trailer systems are widely used in various industries for transporting goods. These systems have to function in dynamic warehouse environments and require accurate and efficient planning to ensure a safe operation. But, due to the nonlinear dynamics of the system, traditional motion planning methods may not be suitable for generating feasible paths in real- time. This project proposes the use of a sampling-based motion planning algorithm for a truck trailer system.

Closed Loop RRT (CL-RRT) is one such popular alogirthm that was first introduced for team MIT’s entry vehicle for the 2007 DARPA Urban Challenge, Talos. For the competition, their team needed to develop an algorithm that was efficient, real-time and could dynamically re-plan with changing obstacles in the environment.

Project Report Demo: Truck-Trailer (inaccurate dynamics)

Goals

This project uses Closed Loop Rapidly Exploring Random Tree (CL-RRT) to generate collision-free paths for the truck-trailer system while performing U-turn maneuver in compact spaces. This project explores the challenges faced in the implementation of such an algorithm on a non-holonomic system. The effectiveness of the proposed method has been shown through experiments in simulation. The results show that several performance benefits can be obtained and the proposed approach can prove to be a promising solution for real-time path planning while ensuring the kino-dynamic constraints are satisfied.

The algorithm requires a hierarchical structure for planning. A low level controller to execute actions. These action samples are fed back into the high level planner to determine a path plan. The implementation in this project uses the following:

1. Pure Pursuit controller for lateral control
2. PID controller for longitudinal control
3. LQR controller for stabilizing dynamics

Work Done

Initially, the vehicle dynamics was modeled in Javascript for quick and easy visualization, but due to limited vectorization, Matlab was used with C++ modules. The truck trailer model was made as per the following dynamics equations shown below. Also, for the algorithm to work, the low level controller needs to be self stabilizing. While pure pursuit controller is stable in forward kinematics, it diverges in reverse motion. So a LQR controller was used to stabilize the angle Beta between the truck and trailer.

RRT based algorithms require an estimate of the distance. Normally the L2 norm is used for the distance function. But this is not feasible as a vehicle would take a longer path. A dubin's path could be used but that fails for a complex truck-trailer model. Thus, a lookup table for the cost function is used in this version. This helped speed up the forward passes for cost heuristics estimated by the planner.

CL-RRT runs simultaneously as the previously calculated trajectories are being executed. It also retains the generated trees and reuses them to save on computation. In this project however, simulating the dynamics on a laptop processor was computationally intensive. So the process of planning could not be run in parallel with the simulation.

Results

Two different environments were used with different start and end goals in each case. This was done to test the algorithm in easy as well as challenging condition. The first involved a straight forward maneuver which only should involve forward kinematics. The second environment required the truck to go in reverse, thus execute non stabilizing dynamics.

In the first experiment, I ran an iteration of control based RRT to see how much time does the algorithm take to plan and then traverse to its end location. This was then compared to the CL-RRT timing for the same loop for two different time steps of 8 seconds and 15 seconds respectively. In the second experiment, points were sampled only from a specific region which might make a left u-turn easier. This was done to evaluate the effectiveness of smart sampling. These were sampled from a two regions using variable parameters for radius and theta.

The performance of CL-RRT is inconsistent. However, if lots of samples can be generated to cover most of the sample space, the results become more and more consistent as seen with the smaller test environment. CL-RRT did show some improvements in the time required for planning and execution however the method is highly dependent on getting the time- step value right. The optimizations mentioned in sampling did not seem to yield successful results at least in the test environment.

Introduction

Goals

Work Done

Cost function

Serial vs dynamic replanning

Results

Results

Total time for both environments

Variation across time step

Variation with sampling strategy

Reverse dynamics

Test run 1

Test run 2

Contact me!

Linkedin

Github