Planning and Decision Making for Autonomous Robots
Driving Games
The course Planning and Decision Making for Autonomous Robots (PDM4AR) at ETH Zurich, taught by Prof. Frazzoli had a really fun exam, which was to plan and control the paths for multiple agents in a driving simulator, dg-commons.
Simulation Setup
From the CommonRoad API one scenario with lanes and multiple agents each with unique goals is initialized. The agent can theoretically see the whole map from a bird's eye view, but cannot see other agents until there is a line of sight between them.
To make the problem of path planning a bit more interesting, a bunch of obstacles were also sprinkled over the map.
My Solution
I broke the problem of reaching the goal point into three different subsections that I could address individually,
Path Planning which would provide the reference trajectory for the agent to follow
Control which would compare the agent's trajectory with the reference trajectory and translate that into to steering and throttle commands through feedback control
Collision Avoidance which would avoid the dynamic obstacles (other agents)
The Simulation Setup
Path Planning
I implemented the RRT* sampling based path planning algorithm to generate the first path. RRT stands for Rapidly-exploring Random Trees which explores the search space while also being biased towards the goal . The asterisk or more apt in this case, the star in RRT* (RRT star) comes from the modification that everytime a new node is added, the shortest path to it is retained and all other connections to it are discarded. This is termed "rewiring" usually.
I made a few modifications to this to increase the efficiency, distance from obstacles and also the smoothness of the path. My main goal was to have a reliable and repeatable planner.
Although it is necessary to avoid the obstacles and the walls, I did not modify them and allowed the path to go as close to them as they were sampled. This ensured there were no bottlenecks in the search space, so the algorithm didn't have to "get lucky" to find a path. This also greatly reduced the number of samples I needed to draw to get a path.
I restricted the maximum angle between the edges of between nodes so that "sharp" turns could be avoided. This really helped out when it came to smoothing the path and getting the car to track the reference trajectory.
Once a path was found I continued to draw samples but constrained them to a small radius around the nodes which made the path from the start to the goal. The intention of this was to shorten the path.
If a path could not be found after a certain number of samples, I just threw away the whole graph and started again.
Once the planner outputs the tree, I used my implementation of the A* algorithm to find the shortest path.
RRT* Path
Control
The reference trajectory from the planner was usually not very smooth and would be often too close to obstacles for the agent to navigate through. So at this stage I inflate and "snap" the path to the boundaries of the inflated obstacles. After that I resample and smooth the path with a b-spline.
To track this reference trajectory I decoupled longitudinal and lateral control - used a pure pursuit controller for lateral control and a PID controller for longitudinal control. For the reference velocity I just scaled the required lateral acceleration based on how confident I was with my planner.
Collision Avoidance
For collision avoidance I would simply get the agents that see each other to stop in their place. And the one with the smaller name re-plans and moves towards the goal. In the meantime the one with the larger name waits for this one to go out of the field of view after which it carries on with it's planned path.