Driving is a big part of your life in the modern world since an average American spends approximately 3 hours per day in the car. Unfortunately car accident is a major cause of dealths. There were about 3.4 million injuries and 41,611 people killed in auto accidents in 1999. Most of these accidents are due by human factor such as sleep deprivation, or driving under the influence. Taking the autonomous approach to driving has been a necessary step as well as an interesting challenge. Big automaker companies, namely Lexus and Acura, has been invested millions of dollars in developing the self-parking and self-driving system in their luxury models. The right image is the Lexus LS460h, a car which can drive and parallel parking by itself.
The entire autonomous driving system involves many engineering components. In this project, only the motion planning part is implemented. The goal is to autonomousally control a driving vehicle to a goal destination without colliding into any obstacle. In addition, this project is aimed to help students understand the car-like motion in various scenarios through a interactive simulation.
Software
Key features
The project is implemented in MATLAB. The motion planner software, called Simulator AVD, is developed as the result of the method. Simulator ADV offers an array of amazing features in which users can experience. Below is a few key features that is available in Simulator ADV.
Fig 6. The interface of Simulator ADV.
Fig 7. The tunning panel of Simulator ADV.
Interactive inputs
You will get to decide where the obstacles (buildings and parked car) as well as the intial and starting position should be. After each simulation, you can decide to keep the obstacle for the next one by simple choosing reload option.
Two motion modes
There's two modes of motion you can choose from: driving or drifting. When driving is selected, you robot will move like a car. You will experience the real motion of car driving. When drifting is selected, you robot will move like a walking robot.
Step-by-step simulation
The machanism of RRTs can be clearly explained in step-by-step simulation. The animation shows how the RRT trees in constructed and grow. User can gain a true understanding on how RRT works by looking at the simulation.
Robot's attribute tunning
You get to tune how fast should the car-robot go, how sharp should it turn. You can also determine the shape of the car, should it a long van, or big SUV, or a small compact. It's all up to you!
Downloads
The program is free for non-commericial usage. This download include a single executible file to run in Windows/Linux OS. After download, double click on ADV.exe inside the folder to run. If you do not have MATLAB installed on your computer, you also need to download and install the MATLAB Compiler Runtime (MCR) in order to run this program. MCR Installer can be downloaded below.
Motion Planning for a car-like robot (such as the left image) has many challenges:
Fig 2. A car-like robot
Non-holonomic System: the car-like robot is consider a non-holonomic system, which means any path in C_free does not necessary correspond to a feasible path satisfying the non-holonomic constraints. The state of the robot has been added with the orientation component besides the x and y coordinates.
Complex Transition Equation: the state transition equation for car-like robot is more complicated to implement then a regular point robot. It involves solving differential equations for each time step of the robot state.
Steering Angle Constraints: since a car-like robot wheels has a limit on steering angle, the reachable configurations have to be within the turning radius. The robot cannot "shift" itself on the sideway direction.
Narrow Passages: the problem of narrow passage has been well-posed in probabilistic motion planning literature. When the robot is large, going throught a narrow path is even more challenges especially it's a car-like robot.
Conceptual Generization: understanding a motion planning algorithm is difficult. Most of the concept are complex and rather abstract. So far, there's only primitive examples or psedocode to explain to concepts. To make the field going forward, there should be more effort putting explaining and educating the graduate students.
Rapidly-exploring Random Tree (RRT)
"RRT is a data structure and algorithm that is designed for efficiently searching nonconvex high-dimensional spaces. RRTs are constructed incrementally in a way that quickly reduces the expected distance of a randomly-chosen point to the tree. RRTs are particularly suited for path planning problems that involve obstacles and differential constraints (nonholonomic or kinodynamic). RRTs can be considered as a technique for generating open-loop trajectories for nonlinear systems with state constraints. An RRT can be intuitively considered as a Monte-Carlo way of biasing search into largest Voronoi regions. Some variations can be considered as stochastic fractals. Usually, an RRT alone is insufficient to solve a planning problem. Thus, it can be considered as a component that can be incorporated into the development of a variety of different planning algorithms."
Steve LaValle.
The environment
Fig 3. A sample environment for motion planning.
The environment is a 2-dimensional space (plane) which is similar into real-world application where vehicle only move in a road, or flat area. The location obstacles (such as buildings or parked cars) are defined by the user. In such way, the user is in full interaction with the environment, thus many experiment about the algorithm can be carried out easily. The user can also set the Initial (Starting) Configuration and the Final (Goal) configuration. The sample environment can be showed in the figure 3.
Implementation for both holomonic and non-holonomic (car-like) robots. For holonomic robot, which can move in any direction, an balanced bidirectional search gives much better performance. The search is bi-directional because there're two trees growing, one from the starting configuration and the other from the goal configuration. The solution is found when the trees branches arrive at the same configuration. The search is balanced because it ensures that both trees are the same size. The figure 4 on the left shows the example of two trees (one in blue and one in cyan) that are able to connect and form the solution path (in magenta).
For non-holonomic robot, the design decision is to keep only one RRT. As two-tree case is considered, a complicated decision problem occurs. A new dimensionality, vehicle orientation, is added to the state vector. In order for two RRT trees connect, all three dimensions have to be equal. The search space is, therefore, more difficult to find. The computation time has increased, and it is not clear which connection should be made. Therefore, only one tree is used. In addition, the non-holonomic constraints have been added to the system to ensure the car-like movement. The result is show on figure 5.
Two types of robot
Fig 4. A balanced bidirectional RRT-based planner.
Fig 5. A RRT-based planner with non-holonomic constraints.
Non-holonomic Constraints
Consider a car-like robot in the plane configuration q = (x,y,theta), since the sideways velocity of the rear wheels is zero, The equation defines a non-inegrable non-holonomic constaint for the system is x' cos(theta) - y' sin(theta) = 0
Another constraints is the minimum turning radius. Let the steering angle is phi, its minimum turning radius is delta_min = L / tan(phi_max) where L is the length of the car. The constraint can be written as x'^2 + y'^2 - delta_min^2 theta'^2 >= 0
The transition equation for car-like robot is
x' = s cos(theta)
y' = s sin(theta)
theta' = s / L * tan(phi)
Numeric integration of this differential equation system will update the state of the vehicle.
Discussions
Understanding RRT through simulation
RRT is a complex concept and difficult to understand. Fortunately, simulation seems to help students get a better interpretation on how it works using step-by-step approach. Moreover, this project gives students an interactive experience with RRT. For example, student can place the buildings and car around the scence, set the starting and goal configuration, and watch how RRT is constructed and performs the search. I look forward to integrate this frame work to perhaps teach students about the RRT concept.
1. Holonomic motion vs. Non-holonomic motion
Holonomic Motion
Non-holonomic Motion
Holonomic motion planning is easier to implement than non-holonomic motion planning. In holonomic motion, the last position of the object to update its current position. On the other hand, in non-holonomic motion, the planner must incorporate the object's orientation in a differential state equation. Moreover, RRT with non-holonomic have to explore to larger search space an additional dimension, the object orientation. In the example above, while RRT for holonomic motion only requires 32 iterations, RRT for non-holonomic takes 1221 iteration to complete.
2. High-speed vs. Low-speed motion
High Speed Motion
Low Speed Motion
High-speed and low-speed motion has been planned very different on the same set of obstacles as well as same starting/goal configuration. In high speed, fewer turns have been made. In addition, the high-speed robot tends to move with more abrupt turns (changing steering from hard-left to hard-right). High speed solution is also take more iterations to complete.
3. Sharp-turn vs. wide-turn motion
Sharp Turn Motion
Wide Turn Motion
Wide-turn seems to simulate the motion of real-life car whether sharp turn motion is more racing-cart-like. The wide-turn motion is a lots smoother and without any abrupt change in direction. Therefore, wide-turn motion planning is recommended if you want the autonomous driving car smooth and easily on the inside persons.
4. Effect of car size in narrow passage
Big SUV cannot find way over a narrow passage
Small compact car is able find the way the goal
Vehicle dimension has impacted the solution for motion planning, especially in narrow passge case. While Big SUV is unable to find a path through the narrow passage, small compact car is able to find the path to the goal state.
