Introduction

Objective

• The objective of this project is create motion planning simulation for a car using Webots simulator.
• A sample hybrid planner, which consists a global and local planner, is implemented to drive the vehicle autonomously in the pre-existing environment.
• This project does not make attempts on mapping the environment and the focus is purely on motion planning
• More details about the architecture can be found here

Simulation

• Webots is an open source simulation service
• This project created a new controller for webots city world simulation.

Demo

A demo of the vehicle planning and executing a path that avoids the obstacles

Architecture

The architecture consists of two planners, a global and local planner.

• The global planner consists of logic to help the vehicle navigate a road segment, in a manner such that no obstacles are present.

• When an obstacle is pressent, the planner switches to the local planner, which executed a motion planning algorithm to figure out a path avoiding the obstacle.

Hybrid A* Algorithm

• This is a variant of the $A^$ algorithm applied to the 3D kinematics state space of the vehicle, with modified state-update rule that captures continuous-state data in the discrete searcg nodes of $A^$.

• Hybrid-state $A^*$ is not guaranteed to find the minimal-cost solution, due to its merging of continuous-coordinate states that occupy the same cell in the discretized space, however we will get a path through which the vehicle can commute.

Figure: Graphical comparison of search algorithms.

Left: A* associates costs with centers of cells and only visits states that correspond to grid-cell centers.
Center: Field D* and Theta* associate costs with cell corners and allow arbitrary linear paths from cell to cell.
Right: Hybrid A* associates a continuous state with each cell whose score is the cost of its associated continuous state.

• The algorithm provides provisions to penalise motions that switch directions

• The heuristics used ignores obstacles and takes into account the non-holonomic nature of the car, by considering the distance between $(x_t, y_t, \theta_t)$ and $(x_g, y_g, \theta_g)$

• Euclidean distance between the current state and the goal state is used to find the heuristics. A small penalty was also introduced to the angular distance and $\beta=0.5$:

$$ \sqrt{(x - x_{\text{goal}})^2 + (y - y_{\text{goal}})^2 + \beta(\theta - \theta_{\text{goal}})^2} $$

• Here the kinematic bicycle model (ref) to update the state space of the vehicle, for each iteration of the algorithm, where $\delta$ is the change in steering angle

$$ \begin{aligned} \dot{x} &= x + v \cdot \cos(\theta) \cdot dt \\ \dot{y} &= y + v \cdot \sin(\theta) \cdot dt \\ \dot{\theta} &= \theta + \left( \frac{v \cdot \tan(\delta)}{L} \right) \cdot dt \end{aligned} $$

Figure: A visualisation of the planned path

A downsampled path representing road segment is constructed for motion planning.
Necessary interpolations are done when generating the desired output steering angle provided to the simulation vehicle.