CL-RRT Implementation for single articulated vehicle in Matlab - Simulink - Stateflow

Table of contents

• Closed-Loop system Design
• Heuristic map generating Algorithm
• CL-RRT PLANNER

Closed-Loop system Design

Pure Pursuit controller and virtual steering wheel concept are used to forward simulate a Single Articulated Vehicle(SAV). To see performance of the above combination Run purepursuitcontroller_waypoints/purepursuit_waypoints.m

Heuristic map generating Algorithm

CL-RRT planner makes use of strategies that aid in obtaining feasible and optimal solution paths. The following strategies aid in obtaining probabilistically increasing optimal paths. The strategies that are implemented in this project are

• Exploring strategy Used to explore the region of interest within the provided obstacle map
• Towards goal Biasing strategy Used to drive the expanding planning tree in the direction of the goal For detailed explaination refere thesis doc. The Heuristic map generating Algorithm implementation in Stateflow/Simulink is shown below.

To achieve both these strategies a Heuristic map has to be calculated. This Heuristic map is a collection of distance metric from a (0,0) position to each point in the predefined grid region. For this thesis a grid region -50<=x<+50, -50<=y<=+50 is obtained. Initially calculated with resolution of 5m and lateron interpolated with the resolution of 0.5m. To generate a Heuristic map go to lib>hmapsf_Initializ.m and follow the instructions.

CL-RRT PLANNER

In this project the CL-RRT algorithm implemented incrementally builds a planning tree by forward simulating the "Closed-Loop system Design" over a predefined region of map with defined static obstacles. While building the planning tree the previously mentioned, "Heuristic map generating Algorithm" generated "Heuristic map forward direction and reverse directions (lib>heuristicmap.mat>Hmap_frwd and Hmap_rev)" are used to increase the optimal nature of the tree extensions.

To obtain results from the motion planner, three consecutive files have to be run one after(one has to wait until the individual file is run completely to start the next ).

1. clrrtsf_Initializ.m, this file initializes the planner with required heuristic maps, variables like initial pose and final pose etc.
2. clrrtsf.slx, this is a Simulink Stateflow model that has the CL-RRT motion planner.
3. clrrtsolutionplot.m, this file makes use of the data generated by the planning tree, and finds the obtained number of solution, single best optimal path(shortest path) and conseqvently plots the expanded planning tree, obtained solution path and its respective piece wise linear reference path.

A flowchart for the current implementation is as follows.

A Simulink-Stateflow implementation is as follows.

Obtained result showing complex reverse parking scenario of Single Articulated Vehicle is shown below.