Route Planning for Autonomous Vehicles

Route Planning for Autonomous Vehicles

A Thesis Proposal

ABSTRACT

This thesis addresses the path planning problem which is one of the most crucial parts of autonomous driving by utilizing the Rapidly-Exploring Random Tree star (RRT*) search algorithm. Two focus points, i.e., the start and the goal, are used to create an ellipse which is used to focus sampling and search tree towards the goal instead of the whole graph to reduce the pathfinding time. To find the nearest node towards the goal 2D-Tree binary search technique is used. This algorithm should respond to various speed levels and also avoid the moving object in the path without reducing its speed. Matlab is used to acquire the numerical results of the modified RRT* that will also be considered regarding Field Programmable Gate Array (FPGA) implementation with parallel programming for the faster processing.

The proposed research will investigate different aspects of the RRT* algorithm such as different methods of sampling, binary search algorithms, and parallel programming to reduce the time and find the shortest obstacle-free path for the autonomous vehicles. 

TABLE OF CONTENTS

Page

ABSTRACT………………………………………………………………………………………iii

TABLE OF CONTENTS ..………………………………………………………………………iv

LIST OF FIGURES ………………..……………………………………………………………..v

LIST OF ACRONYMS ..…………………………………………………………………………vi

CHAPTER 1. INTRODUCTION ..……………………………………………………………….1

1.1  Background ……………………………………………………………………………………1

1.2  Technical Approaches …………………………………………………………………………2

CHAPTER 2. METHODOLOGY ..………………………………………………………………..5

2.1 RRT* Algorithm ………………………………………………………………………………5

2.2 Implementation ………………………………………………………………………………..6

REFERENCES ……………………………………………………………………………………10

APPENDIX I …………………………………………………………………………………….13

LIST OF FIGURES

Figure 1.1 Tree Expansion .………………………………………………………………………..3

Figure 1.2 RRT* vs. Informed RRT* ……………………………………………………………..4

Figure 2.1 Algorithm for RRT* .…………………………………………………………………..5

Figure 2.2 Plane subdivision and the corresponding tree schematics .……………………………6

Figure 2.3 The graph with 200 iterations …………………………………………………………7

Figure 2.4 The graph with 1500 iterations ..………………………………………………………7

Figure 2.5 The graph and path after RRT* Implementation ………………………………………8

Figure 2.6 The graph and path after simple modification in RRT* ……………………………….9

LIST OF ACRONYMS

RRT – Rapidly Exploring Random Tree

PRM – Probabilistic RoadMap

FPGA – Field Programmable Gate Array

INTRODUCTION

1.1 Background

The autonomous vehicle would be the future of transportation. Millions of people lost their lives due to road accidents, and a significant cause of road accidents are mistakes made by drivers. According to the World Health Organization (WHO), approximately 1.24 million people die due to road accidents [1]. The Autonomous car developed by many companies and universities to reduce road accidents and make roads safer. For example, Google has tested its driverless car for 3 million miles [2]. Even universities such as Stanford and Carnegie Mellon develop their Autonomous car and test it in Grand and Urban DARPA challenges [3].

Path planning is the critical issue in the autonomous vehicle. There are different objectives of the path planning, as listed below [4].

• Global planning
• Behavioral planning
• Local planning

Global planning takes care of mission planning or route planning using different techniques such as D*, A*, Probabilistic Road Map (PRM), RRT* to find shortest and obstacle-free path [4]. Behavioral planning focused on event and maneuver management such as when to overtake. Local planning executes the decision taken by the behavioral planning in the manner that dynamic and kinematic model of the car taken consideration to make the ride more comfortable [4].

1.2 Technical Approaches

 A* is a graph based search algorithm, which was implemented widely in different autonomous vehicles. i.e., hybrid A* was implemented by a junior in DARPA challenge [5]. A* Algorithm is used in a static environment where the algorithm is continuously searching for the nearest node that approaches the destination [6].  A heuristic method is used to find the optimal path. Algorithm introduces three different variables which are H (Heuristic) value of the node, G value a movement cost of node and F value is an addition of H and G value. Open list and closed list of the nodes have been maintaining all the time. A* Algorithm has guarantee solution if its possible in the graph. The disadvantage of the algorithm is the cost of the solution is very high in long and practical situations [7][8].

 Ant Colony Optimization (ACO) is also one of the widely used technique for small robots. ACO is a probabilistic search technique which can work in a static environment where algorithm mocks the real ants [9]. Ants produce a pheromone that can be used for pheromone trail to indicate the optimal path. The system requires the previously known environment to perform [10].

 Clothoid curve [11], Polynomial curves [12], Bézier curves [13] and Functional optimization [14] are the different techniques, but this technique more focused on vehicle dynamics and kinematic model which is more useful in motion planning instead of path planning [4].

 Various algorithms have been developed as mentioned above but to find the most appropriate route in a dynamic environment; sampling-based algorithms are widely used. RRT is a sampling based algorithm. RRT is introduced by, i.e., S. M. LaValle and J. J. Kuffner in [15]. There is a graph of the randomly generated sampling points with the start and the goal point. The tree is developed with the use of samples to connect the start and the goal points. RRT algorithms do not need entirely modeled obstacles to avoid them and create an obstacle-free path.

Fig. 1.1 Tree expansion [14].

RRT* is an improved version of the RRT which creates nodes alongside with the random sampling points [16][17]. New node works as a parent node and starts to find nearest node or sample point for tree expansion. Using this kind of heuristic method RRT* can be used with the moving objects. The disadvantage of this sort of system is tree expansion. Tree expansion is not focused towards the goal which caused more time in execution [16]. Fig. 1.1 shows the update in Xinitial to Xparent node.

Cloud RRT* is a novel approach for biased sampling method [19]. During this technique, the objective is to get more sampling density in promising regions for faster and more appropriate route planning. Cloud RRT* is one of the ways to focus the tree expansion towards a goal instead of expanding in the whole graph [19]. To achieve that generalized Voronoi diagram is used to define objects in C-space. In the set of obstacle-free space spherical cloud of the samples has been created.

Fig. 1.2 RRT* vs. Informed RRT* [20]

Anytime RRTs introduce by, i.e., D. Ferguson, A. Stentz, they added cost function in traditional RRT* [20]. First, the path was found with the use of RRT* after that cost function Cs is introduced now they start to see the path again, but they only consider the nodes which have less Cs value and create a new path which is more cost efficient, but the computational price has to pay.

  Informed RRT* uses the ellipsoidal space to focus the search and the tree expansion [21]. This is a simple but effective modification in RRT* algorithm which helps to reduce the computational time.  As it can be seen in Fig. 1.2, tree expansion is more focused in informed RRT* compared to RRT*, which result to reduce CPU time.

 Algorithms such as RRT computational time is a significant issue. The maximum time has been consumed in the process of tree expansion. So far the work has been done in that manner to focus that and find the best optimal path.

METHODOLOGY

2.1 RRT* ALGORITHM

This section introduces the basic RRT algorithm structure, which is introduced by, i.e., S. M. LaValle and J. J. Kuffner in [11]. Configuration space defined as Ⅽ and the obstacle region is

 RRT* algorithm was implemented on the Matlab. The first algorithm executes with the 200 nodes later with an increase in the number of nodes path shorter but computational time is increased. To reduce the computational time and the time taken by the tree expansion  in the different region on the graph sampling in Fig. 2.6 is concentrated near to the start and the goal regions and the tree expansion is also adjusted accordingly. Here step size 50 and number of nodes are 1500. The execution time for the RRT* algorithm is 132 seconds. Fig. 2.5 shows the
implementation of RRT* algorithm.

Fig. 2.3 The graph with 200 iterations

Fig. 2.4 The graph with 1500 iterations

Fig. 2.5 The graph and path after the RRT* implementation

 Now with the simple modification in the RRT* algorithm, the computational time can be reduced to find the similar results. Fig. 2.6 shows the implementation of the simple modification in the RRT* algorithm where step size is 50 but a number of nodes are only 500 so the execution time is reduced to only 25 seconds.

 Future work will concentrate on different objectives of the RRT* algorithm such as different possible sampling techniques, uncertain or dynamical obstacle characterizations, binary or convex search algorithm for the nearest neighbor and parallel programming for implementation on the FPGA.

Fig. 2.6 The graph and path after simple modification in RRT* algorithm

REFERENCES

[1]   http://www.who.int/mediacentre/factsheets/fs358/en/ (LAST ACCESS ON 11/01/2017)

[2]   https://waymo.com/ontheroad/ (LAST ACCESS ON 11/01/2017)

[3]   C. Urmson, “Autonomous driving in urban environments: Boss and the urban challenge,” J.field Robot., vol. 25, no. 8, pp. 425-466, Aug. 2008.

[4]   D. González, J. Pérez, V. Milanés and F. Nashashibi, “A Review of Motion Planning Techniques for Automated Vehicles,” in IEEE Transactions on Intelligent Transportation Systems, vol. 17, no. 4, pp. 1135-1145, April 2016.

[5]   M. Montemerlo, “Junior: The Stanford entry in the urban challenge,” J. Field Robot., vol. 25, no. 9, pp. 569-597, sep., 2008.

[6]   H. Pan, C. Guo, and Z. Wang, “Research for path planning based on improved astart algorithm,” 2017 4th International Conference on Information, Cybernetics and Computational Social Systems (ICCSS), Dalian, pp. 225-230, July 2017.

[7]   A. Stentz, “Optimal and efficient path planning for partially-known environments.” Proc. IEEE int. Conf. Robot. Autom., pp. 3310-3317, 1994.

[8]   D. Ferguson and A. Stentz, “Using interpolation to improve path planning: The field d^* algorithm,” J. Field Robot., vol. 23, no. 2, pp. 79-101, Feb. 2006.

[9]   G. N. Varela and M. C. Sinclair, “Ant colony optimization for virtual-wavelength-path routing and wavelength allocation,” Proceedings of the 1999 Congress on Evolutionary Computation-CEC99 (Cat. No. 99TH8406), Washington, DC, pp. 1816 Vol. 3, July 1999.

[10]  C. C. Hsu, W. Y. Wang, Y. H. Chien, R. Y. Hou and C. W. Tao, “FPGA implementation of improved ant colony optimization algorithm for path planning,” 2016 IEEE Congress on Evolutionary Computation (CEC), Vancouver, BC, 2016, pp. 4516-4521.

[11]  M.F. Hsieh and U. Ozguner, “A parking algorithm for an autonomous vehicle,” Proc. IEEE Intell. Veh. Symp., pp. 1155*1160, Jun 2008.

[12]  P. Petrov and F. Nashashibi, “Modeling and nonlinear adaptive control for autonomous vehicle overtaking” IEEE Trans. Intell. Transp. Syst., vol. 15, no. 4, pp. 1643-1656, Aug 2014.

[13]  J. Pérez, J. Godoy, J. Villagra and E. Onieva, “Trajectory generator for autonomous vehicles in urban environments,” Proc. IEEE ICRA. pp. 409-414, May 2013

[14]  D. Kogan and R. Murray, “Optimization-based navigation for the DARPA grand challenge,” Proc. CDC, pp. 1-6, 2006

[15]  S. M. LaValle, “Rapidly Exploring Random Tree – A new tool for path planning,” in Tr 98-11, computer science dept., Iowa State University, October 1998. (http://msl.cs.uiuc.edu/~lavalle/rrtpubs.html)

[16]  K. Sertac, E. Frazzoli, “Incremental sampling-based algorithms for optimal motion planning,” Robotics Science and Systems VI 104, 2010.

[17]  C. Urmson and R. Simmons, “Approaches for heuristically biasing RRT growth,” Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453), 2003, pp. 1178-1183 vol.2.

[18]  S. Xiao, N. Bergmann, A. Postula, “Parallel RRT* architecture design for motion planning,” 2017 27^th International Conference on Field Programmable Logic and Applications (FPL) sept. 2017

[19]  D. Kim, J. Lee and S. e. Yoon, “Cloud RRT^∗: Sampling Cloud based RRT^∗,” 2014 IEEE International Conference on Robotics and Automation (ICRA), Hong Kong, 2014, pp. 2519-2526.

[20]  D. Ferguson and A. Stentz, “Anytime RRTs,” 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems, Beijing, 2006, pp. 5369-5375.

[21]  J. D. Gammell, S. S. Srinivasa and T. D. Barfoot, “Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic,” 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, Chicago, IL, 2014, pp. 2997-3004.

[22]  F. Islam, J. Nasir, U. Malik, Y. Ayaz and O. Hasan, “RRT∗-Smart: Rapid convergence implementation of RRT∗ towards an optimal solution,” 2012 IEEE International Conference on Mechatronics and Automation, Chengdu, 2012, pp. 1651-1656.

[23]  S.Arya, D.M. Mount, “Approximate nearest neighbor queries in fixed dimension,” In a 47^th annual ACM-SIAM symposium on Discrete Algorithms (SODA), pages 271-280, 1993.

[24]  A. O. Dempster, A.N. Laird, and D.B. Rubin. “Maximum likehood from incomplete data via the EM algorithm,” Journal of the Royal Statistical Society, Series B, 39(1):1-38,1977.

[25]  C. Howie M. Principles of Robot Motion : Theory, Algorithms, and Implementations. Cambridge, Mass: A Bradford Book, 2005. eBook Academic Collection (EBSCOhost), EBSCOhost (accessed Nov 27, 2017).

APPENDIX I

clearvars

close all

% define area and boundry

width = 1000;

height = 1000;

obstacle = [200,500,300,300];   %obstacle coordinates

EPS = 50;       %step size

numNodes = 500;     % number of nodes

q_start.coord = [150 50];   %start point coordinates

q_start.cost = 0;

q_start.parent = 0;

q_goal.coord = [550 350];       %end point coordinates

q_goal.cost = 0;

nodes(1) = q_start;

figure(1)

axis([0 width 0 height])

rectangle(‘position’,obstacle,’Facecolor’,[0 0.9 0.91])

hold on

plot(q_start.coord(1),q_start.coord(2),’r+’)

plot(q_goal.coord(1),q_goal.coord(2),’go’)

for i = 1:1:numNodes

    q_rand = [floor(rand(1)*700) floor(rand(1)*500)];

    plot(q_rand(1),q_rand(2), ‘x’, ‘color’, [0 0.45 0.75])

    q_rand1 = [floor(rand(1)*width) floor(rand(1)*height)];

    plot(q_rand1(1),q_rand1(2), ‘x’, ‘color’, [0 0.45 0.75])

    %break if goal node is reached

    for j = 1:1:length(nodes)

        if nodes(j).coord == q_goal.coord

            break

        end

    end

    %pick closest node from existing list of branch

    ndist = [];    %dist from new node to q random node

    for j = 1:1:length(nodes)

        n = nodes(j);    % new node

         %temporary store distance from new find node to random point

        tmp = dist(n.coord, q_rand); 

        ndist = [ndist tmp];

    end

    % find the minimum dist node from branch and store its coordinates

    [val, idx] = min(ndist);   

    q_near = nodes(idx);        %new nearest node

    q_new.coord = steer(q_rand, q_near.coord, val, EPS);    %branch from q rand to q new with respect to step size

    if noCollision(q_rand, q_near.coord, obstacle)

        line([q_near.coord(1), q_new.coord(1)], [q_near.coord(2), q_new.coord(2)], ‘Color’, ‘k’, ‘LineWidth’, 0.1);

        drawnow

        hold on

        q_new.cost = dist(q_new.coord, q_near.coord) + q_near.cost;

        % Within a radius of r, find all existing nodes

        q_nearest = [];

        r = 60;

        neighbor_count = 1;

        for j = 1:1:length(nodes)

            if noCollision(nodes(j).coord, q_new.coord, obstacle) && dist(nodes(j).coord, q_new.coord) <= r

                q_nearest(neighbor_count).coord = nodes(j).coord;

                q_nearest(neighbor_count).cost = nodes(j).cost;

                neighbor_count = neighbor_count+1;

            end

        end

        % Initialize cost to currently known value

        q_min = q_near;

        C_min = q_new.cost;

        % Iterate through all nearest neighbors to find alternate lower

        % cost paths

        for k = 1:1:length(q_nearest)

            if noCollision(q_nearest(k).coord, q_new.coord, obstacle) && q_nearest(k).cost + dist(q_nearest(k).coord, q_new.coord) < C_min

                q_min = q_nearest(k);

                C_min = q_nearest(k).cost + dist(q_nearest(k).coord, q_new.coord);

                line([q_min.coord(1), q_new.coord(1)], [q_min.coord(2), q_new.coord(2)], ‘Color’, ‘y’);               

                hold on

            end

        end

        % Update parent to least cost-from node

        for j = 1:1:length(nodes)

            if nodes(j).coord == q_min.coord

                q_new.parent = j;

            end

        end

        % Append to nodes

        nodes = [nodes q_new];

    end

end

D = [];

for j = 1:1:length(nodes)

    tmpdist = dist(nodes(j).coord, q_goal.coord);

    D = [D tmpdist];

end

% Search backwards from goal to start to find the optimal least cost path

[val, idx] = min(D);

q_final = nodes(idx);

q_goal.parent = idx;

q_end = q_goal;

nodes = [nodes q_goal];

while q_end.parent ~= 0

    start = q_end.parent;

    line([q_end.coord(1), nodes(start).coord(1)], [q_end.coord(2), nodes(start).coord(2)], ‘Color’, ‘r’, ‘LineWidth’, 2);

    hold on

    q_end = nodes(start);

end

% Euclidean distance function

function d = dist(q1,q2)

d = sqrt((q1(1)-q2(1))^2 + (q1(2)-q2(2))^2);

end

function A = steer(qr, qn, val, eps)

   qnew = [0 0];

   % Steer towards qn with maximum step size of eps

   if val >= eps

       qnew(1) = qn(1) + ((qr(1)-qn(1))*eps)/dist(qr,qn);

       qnew(2) = qn(2) + ((qr(2)-qn(2))*eps)/dist(qr,qn);

   else

       qnew(1) = qr(1);

       qnew(2) = qr(2);

   end  

   A = [qnew(1), qnew(2)];

end

% collision check function

function nc = noCollision(n2, n1, o)

    A = [n1(1) n1(2)];

    B = [n2(1) n2(2)];

    obs = [o(1) o(2) o(1)+o(3) o(2)+o(4)];

    C1 = [obs(1),obs(2)];

    D1 = [obs(1),obs(4)];

    C2 = [obs(1),obs(2)];

    D2 = [obs(3),obs(2)];

    C3 = [obs(3),obs(4)];

    D3 = [obs(3),obs(2)];

    C4 = [obs(3),obs(4)];

    D4 = [obs(1),obs(4)];

    % Check if path from n1 to n2 intersects any of the four edges of the

    % obstacle

    ints1 = ccw(A,C1,D1) ~= ccw(B,C1,D1) && ccw(A,B,C1) ~= ccw(A,B,D1);

    ints2 = ccw(A,C2,D2) ~= ccw(B,C2,D2) && ccw(A,B,C2) ~= ccw(A,B,D2);

    ints3 = ccw(A,C3,D3) ~= ccw(B,C3,D3) && ccw(A,B,C3) ~= ccw(A,B,D3);

    ints4 = ccw(A,C4,D4) ~= ccw(B,C4,D4) && ccw(A,B,C4) ~= ccw(A,B,D4);

    if ints1==0 && ints2==0 && ints3==0 && ints4==0

        nc = 1;

    else

        nc = 0;

    end

end