Lab 6: Vehicle Path Planning with RRTs Solution

In this lab you will implement a Rapidly-exploring Random Tree (RRT) algorithm to perform path planning for a vehicle.



















Rapidly-exploring Random Tree (RRT)







RRT is a random search algorithm designed to explore nonconvex and high dimensional spaces. For this lab the goal of RRT is to nd a path from an initial state x0 to a terminal state or set of states Xgoal. Furthermore, this path must always stays in a subset of the space Xsaf e. We will rst implement a standard RRT and then we will adapt this to take into account the kinematic vehicle model.




Figure 1 displays an RRT with




0

x0 = 0

Xsaf e = x 2 R2 j 0 x(1) 100; 0 x(2) 100

Xgoal = x 2 R2 j 70 x(1) 75; 45 x(2) 50 :





















































































Figure 1: RRT




Each dot in the gure is a node of the RRT. As can be seen all of the nodes of the tree are in Xsaf e




1 of 6
Due: May 4 ME C231B/EECS C220C










and the red nodes display a path found from x0 to Xgoal. For this RRT the distance d between nodes was chosen to be 1.




The following psuedocode describes the standard RRT algorithm. It returns the nodes x = (x0; x1; x2; :::) and the index of each nodes parent p = (p1; p2; :::) (note that there is no p0 since the rst node x0 does not have a parent node). The algorithm uses a while loop so that it will run until it nds a node

in Xgoal.







Algorithm 1 RRT

Inputs: x0, Xgoal, Xsaf e, d







Initialize: k = 0

while xk 2= Xgoal do

Choose a random xrand 2 Xsaf e

Find node xj that is closest to xrand

xnewd
(xrand
xj )


(from xj move distance d towards xrand)
kxrand
xj k
if xnew 2 Xsaf e then

k + 1(Increment k)
xk xnew (Add new node)

pk j (Save index of parent node)

end if




end while




return x; p







Note, there are many variations of this algorithm, so it most likely will be presented di erently elsewhere. For example often the algorithm is used just to randomly sample a space, not determine a path. In this case there is no Xgoal set and the algorithm terminates after a speci ed number of iterations.




1. Implement a standard RRT with




0

x0 = 0

Xsaf e = fx 2 R2 j 0 x(1) 100; 0 x(2) 100g

Xgoal = fx 2 R2 j 70 x(1) 75; 45 x(2) 50g

d = 1:




Plot the resulting nodes of the RRT. As in Figure 1 indicate the sequence of nodes that reach Xgoal. Also report the total number of nodes in the tree and the number of nodes in the sequence that reaches Xgoal.




2. Implement a standard RRT with Xsaf e as shown in Figure 2 and




0

x0 = 2




Xgoal = fx 2 R2 j 900 x(1) 950; 1 x(2) 1:5g d = 1:










2 of 6
Due: May 4 ME C231B/EECS C220C

































































































Figure 2: Xsaf e




Plot the resulting nodes of the RRT and indicate the sequence of nodes that reach Xgoal. Also report the total number of nodes in the tree and the number of nodes in the sequence that reaches

Xgoal.




RRT using a Vehicle Kinematic Model







Generating a path with a standard RRT algorithm is problematic because there are no guarantees that the vehicle will be able to follow the path. However, we can modify the RRT algorithm so that only paths that the vehicle can follow will be generated.




In this lab we will use a simple kinematic vehicle model:




X_ = vx cos( )

Y_ = vx sin( )

_ = vx tan( )




L




where X and Y are the lateral and longitudinal position and is the heading angle. The steering angle

is the input.We assume that vx = 30 m/s is constant and the wheelbase L = 3 m. We denote the

3
X




state vector x := 4Y 5.







In order to incorporate the constraints imposed by the vehicle model we only need to modify the step of




3 of 6
Due: May 4 ME C231B/EECS C220C










the algorithm were we assign a value to xnew. In the standard RRT we just choose xnew in the direction of xrand, but this may not be possible for a vehicle. Therefore, we want to select a control input such that the vehicle moves towards xrand.




As discussed in class this can be done many di erent ways (PI control, MPC, sliding mode, etc) however for this example we will use a simpler, brute force approach. To get xnew we will try multiple di erent steering angles and then check which one is the best. Speci cally, for each value of 2

20; 18; :::; 18; 20) simulate the vehicle kinematic model (using ode45) for T = 0:1 seconds starting at xj and check that the entire state trajectory is in Xsaf e. Of the safe trajectories, the nal state



that ends closest to xrand is assigned to xnew If none of the trajectories are safe then we return to the beginning of the while loop and select a new xrand. Otherwise, once xnew is selected the algorithm continues as described.




3. Implement a RRT with the vehicle kinematic model and brute force control strategy described above with

3
0



0










x0
= 405










Xsaf e
= fx 2 R3
j 0 X 1000;
20
Y 20;


g
Xgoal
= fx 2 R3
j 900 X 950;
1
Y 1;
=6
=6gg
Plot the X and Y components of the resulting nodes of the RRT and indicate the sequence of nodes that reach Xgoal. Also report the total number of nodes in the tree and the number of nodes in the sequence that reaches Xgoal.




4. Now modify the safe set so that X and Y are constrained to Xsaf e in Figure 2 and . Implement the RRT with the vehicle kinematic model with this safe set and




3
0

x0 = 425

0




Xgoal = fx 2 R3 j 900 X 950; 1 Y 1:5; =6 =6gg




Plot the X and Y components of the resulting nodes of the RRT and indicate the sequence of nodes that reach Xgoal. Also report the total number of nodes in the tree and the number of nodes in the sequence that reaches Xgoal.




How does this RRT compare to the one found in Problem 2? Roughly, how much more compu-tation time does it take to run?




The RRT algorithm presented here is very basic. There are many modi cations and variations in RRT algorithms for di erent applications. What are some potential drawbacks or issues with the RRT algoriothm? What are some possible modi cations that could be made to the algorithm to improve it’s performance for vehicle path planning?



Suppose we want to implement a more advanced vehicle model (like the dynamic model used in Lab 5) with the RRT algorithm. Brie y describe what modi cations to the algorithm would be required to do this.



4 of 6
Due: May 4 ME C231B/EECS C220C










Optimization-Based Navigation







On bCourses there are 2 script les that implement the various optimization-based navigation strategies discussed in class:




navigation NLP.m - navigation problem formulated as a nonlinear program




navigation MIQP bigM.m - navigation problem formulated as a mixed-integer quadratic program







For the following problems you will modify these les.




For the optimization-based navigation you will need to install the following Matlab packages:




Multi-Parameteric Toolbox (MPT) - This toolbox includes Yalmip and other tool for optimization based control and path planning. It is available from http://people.ee.ethz.ch/~mpt/3/.




IPOPT - This is an interior point solver for nonlinear problems. The easiest way to install it is via the OPTI Toolbox which can be downloaded from https://www.inverseproblem.co.nz/ OPTI/index.php/DL/DownloadOPTI




Gurobi - This is a solver for integer programming problems. It can be downloaded from http: //www.gurobi.com. To use Gurobi you will need to request an academic license. This license allows full use of Gurobi, but must be activated while connected to internet on campus(i.e. AirBears2). After activation it can be used anywhere. Instructions for installing Gurobi in Mat-lab can be found here: http://www.gurobi.com/documentation/7.0/quickstart_windows/ matlab_setting_up_gurobi_f.html







Modify the navigation NLP.m script to nd a feasible path contained in the safe space described in Figure 3 with






0

x0 = 1




 

850

xT =




where x0 is the initial condition and xT is the terminal constraint. Since the state space for this problem is much larger than the example in the le you may need to change the variables N, sampling, dzmin, and dzmax to get a good solution in a reasonable amount of time. I got reasonable results changing N = 50 and dzmax = -dzmin = [20; 2].




Include the plot of the obstacles and resulting trajectory. Comment on the computational time compared to the RRT method.







Repeat Problem 7 using the MIQP formulation in navigation MIQP bigM.m.



Include the plot of the obstacles and resulting trajectory. Comment on the computational time compared to the RRT and NLP methods.
















5 of 6
Due: May 4 ME C231B/EECS C220C




















































































































































Figure 3: Xsaf e


























































6 of 6