Home

On the Development of a Search-Based Trajectory

1. Figure 2 11 Example of ARA execution with decreasing inflation factor This algorithm is particularly suitable for trajectory planning because if an autonomous vehicle must do a complex plan it can execute this algorithm find rapidly the solution with epsilon equals to a big value and if remain some times it can improve the solution otherwise it can use the solution found to execute a plan The only problem is the incapacity of the algorithm to manage changes of the maps for example if robot discover a new obstacle during path execution 2 4 4 ANA We have seen in the previous algorithm based on Weighted A that the subop timality of the solution is bounded by e So the solution is guaranteed to be cheaper than e times the cost of the optimal path This observation is useful in ARA algorithm which initially runs Weighted A with a large value of to quickly find an initial solution and continues the search with progressively smaller values of to improve the solution and reduce its suboptimality bound A known problem of many anytime algorithm is the request of setting some parameters for instance in ARA we have two parameters the initial value of e and the amount by which e is decreased in each iteration Setting this parameters requires trial and error and domain expertise This problem has brought to develop an anytime A algorithm that does not require paramet
2. xye xyev _XYeV_LLT Figure 5 14 Solution found for a problem of the first test set with a zoom on two part of the solution to see better the differences between them 124 Chapter 5 Experimental tests Table 5 8 Number of solutions found also suboptimal in the 70 problems of the second test set xvo XYOV XYOV_LLT XY V XYOV _LLT AD 55 55 55 38 56 Second set of tests environment evaluation We report and comment here some data extracted from the second set of tests executed Considering the results of the first set of tests we have used only AD search algorithm to execute these tests We have put the data into tables reporting only one row for the search algorithm and on the column the different pairs of environment and lattice primitives In particular the column contains in order the following elements XY0 Environment with 3 state variable x y 9 and primitives created with the simple kinematic model XYOV Environment with 4 state variable x y 0 v and primitives created with the model that take into account actuators dynamics XYOV_LLT Environment with 4 state variable x y 0 v and primitives created with the extended model that take into account LLT XYOV Environment with 5 state variable x y v 6 and primitives created with the model that take into account actuators dynamics XYOV6_LLT Environment with 5 state variable x y 9 v 6 and primitiv
3. 9 0 3 0 1 5 0 0 1 5 3 0 9 0 m s 0 1m s in stead of 0 0 m s in case of motion primitives created with the extended model and the set of steering angles A 9 3 45 rad For the environment with three state variables we have used an average velocity of 3 0m s We have created the footprint of the robot using its real sizes The planning is done forward the behavior of the planner with forward or backward searches is really similar sometimes with backward searches it is faster We have set initial e for AD and ARA planners to 3 0 with a decreasing factor of 0 2 and we have set the maximum time to find the optimal solution 116 Chapter 5 Experimental tests to 1500 seconds If the planner finds the optimal solution in time it continues with the next goal until all planning problems are executed If it does not find the solution in time it stops to the suboptimal solution found or notify a failure if it does not find any solution The cost map used in this tests is represented in Figure 5 11 b It is obtained starting from a DTM vegetations and buildings are not taken into account corresponding approximately to the place in Figure 5 11 a Its area is 650 meters width and 500 meters high Therefore using a resolution of 1 meter we have a map of 650 x 500 cells that is a smaller area than the previous one but it is still a relatively wide area This map considers the slope to define the costs and in Figure 5
4. g s h s end if end for end while end function and the g value of its successor s and possibly other successors of s Whenever s is expanded on the other hand this inconsistency is corrected by setting v s to g s and re evaluating the g values of the successors of s This in turn may potentially make the successors of s inconsistent In this way the inconsistency is propagated to the children of s via a series of expansions Eventually the children no longer rely on s none of their g values are lowered and none of them are inserted into the OPEN list In this version of ComputePath function the g values only decrease and since the v values are initially infinite all inconsistent states have v s gt g 5 We call such states overconsistent To continue this algorithm study priority functions can be generalized A expands the states in order of increasing f values For any admissible heuris tic this ordering guarantees optimality However we can generalize A search to handle more general expansion priorities as long as they satisfy certain re strictions These restrictions will allow the search to guarantee suboptimality bounds even when the heuristics are inadmissible We first introduce a function key s that returns the priority of a state s used for ordering expansions So in the previous algorithm the line of insertion update is replaced with insert update s in OPEN with key s Now key restri
5. it is possible to create some simple plans by using them Al though there is a way to use and extend the library 1 with some state variables to adapt the library to our ATV vehicle For our intentions some features are missing for instance at the current stage of development there is no possibility to generate plans for an ackermann vehicle there is no possibility to take into account the not instantaneous changes of speed and steer during a plan and the safeness of the plan is not taken into account So our work starts from the feasibility study done in 1 where the possibility to extend SBPL library with some states variables to adapt it to the ATV vehicle was analyzed to reach previous objectives The thesis is subdivided in 7 chapters following a logical subdivision Chapter 1 this chapter is the introduction to the work where the general prob lem is explained and a simple description of how we faced the problem is given Chapter 2 the second chapter discusses the state of art therefore it reports the existing kind of planning algorithm the main libraries that use them the main search algorithms and an explanation of the main concepts needed to understand the approach to the work Chapter 3 this chapter describes the main choices done for the discretization and for the costs and heuristics used by the planner moreover it reports how the cost map can be created practically this chapter contains a description of the necessary info
6. 4 3 2 1 0 1 2 3 4 amp amp 7 8 x b arctan 6 discretization Figure 3 1 Sample of discretization of 0 in 16 values in the two ways explained The intersections of the grid are the center of the lattice cells 3 1 Introduction 59 3 get the sum of the costs of the cells crossed by the vehicle during its action 4 get the average cost of the crossed cells during a vehicle movement The second technique is the best to maximize the possibility of making a path on a flat terrain So defined a pair of states s s the cost function of the action that leads from state s to state s is reported in Equation 3 1 t H 1 A c s s eoo te 3 1 TCC Traversed Cells Cost t time to traverse path Using this cost function a strong weight is given to the terrain morphology leading to a plain path when is possible rather than hilly and irregular one Naturally to every action that passes over an obstacle an infinite cost is assigned Once the cost function is defined it is important to define also the heuristic function It must estimate the cost to reach the goal from a state respecting the unit of measurement of the cost function Moreover the heuristic function must be consistent and admissible to find the optimal solution So given the previous considerations the heuristic function must be a time and for each pair of states it must be less or equal than the time needed to reach one state from the other Furthermore t
7. Terrain generated and plotted with realistic colors Example of map created with Automatic Terrain Generation Example of map resulting from a DEM of the Sardinia Real map and cost map of the first test set Real map and cost map of the second test set Cost trend for a solution of the first test set Expansions done in a problem of the first test set Solution of a problem of the first test set Cost trend for a solution of the second test set Expansions done in a problem of the second test set Solution of a problem of the second test set Solution of a problem of the second test set on aerial map Example of communication between ROS nodes Example of solution refinement List of Tables 5 1 Number of solutions found first test set 5 2 Number of optimal solutions found first test set 5 3 Average cost first test set 5 4 Average suboptimality first test set 5 5 Average time first test set 5 6 Planner classification without equality first test set 5 7 Planner classification first test set 5 8 Number of solutions found second test set 5 9 Number of optimal solutions found second test set 5 10 Average costs second test Set 5 11 Average suboptimality second test set 5 12 Average time second test set A 1 Number of solu
8. at the best solution found when is found in the 100 problems of the first test set XYO XYOV XYOV_LLT XYOVO XYOV6_LLT ARA 320 529 115 089 108 132 121 631 108 093 ANA 320 529 117 722 111 153 132 868 108 356 AD 320 529 115 089 108 278 121 631 108 093 Table 5 4 Average of the suboptimality bound reached at the best solution found when is found in the 100 problems of the first test set XYO XYOV XYOV_LLT XYOV6 XYOV6_LLT ARA 1 00 1 01 1 01 1 17 1 05 ANA 1 00 1 04 1 07 1 25 1 13 AD 1 00 1 01 1 02 1 17 1 05 certain velocity In this case the average velocity was set at 3 0m s It can be set at every value and the path found does not change This is not good if we want consider the real feasibility of the path found The other results are similar but seeing the numbers in Table 5 3 we can see that using the lattice primitives that take into account the LLT the solution have a lower cost than the others Moreover we can see that ANA search algorithm have often an average cost higher than others planner This happen because it finds rarely the optimal solution in time with respect to the other search algorithms The Table 5 4 reports the average suboptimality value reached at the best solution found by the planner This table confirms the considerations done for the previous tables In fact for the environment with three state variables the optimal solution is always found when a solution exists For the others envi
9. e ANA e AD and for each of them we have used the following pairs of environment and primitives e Environment with 3 state variable x y 0 and primitives created with the simple kinematic model e Environment with 4 state variable x y 0 v and primitives created with the model that take into account actuators dynamics e Environment with 4 state variable x y v and primitives created with the extended model that take into account LLT e Environment with 5 state variable x y 9 v and primitives created with the model that take into account actuators dynamics e Environment with 5 state variable x y 9 v and primitives created with the extended model that take into account LLT We executes the tests with 16 values of orientation and where it is expected the set of velocities V 9 0 3 0 1 5 0 0 1 5 3 0 9 0 m s 0 1m s in stead of 0 0 m s in case of motion primitives created with the extended model and the set of steering angles A Soul F rad For the environment with three state variables we have used an average velocity of 3 0m s We have created the footprint of the robot using its real sizes The planning is done forward the behavior of the planner with forward or backward searches is really similar sometimes with backward searches it is faster We have set initial e for AD and ARA planners to 3 0 with a decreasing factor of 0 2 and we have set the maximum time to find the optima
10. inserting it in the OPEN queue and processing expanding it first The selection of the state to expand is done selecting in the OPEN queue the state with the lowest value of f s g s h s and extracting it from the queue in order to expand it In this way the states that look promising to reach early the goal state are extracted before the others and this is more efficient than expand states only considering g s In case of tie of f s between two states different criteria can be used to break tie for example extract the state from the queue in FIFO order During the expansion of a state s all successors s of s are checked if one or more have a g cost greater than the cost of the path passing from s the g cost is updated doing g s g s c s 5 and the path to reach s pass through s Moreover the state s is inserted into the OPEN queue with priority g s h s and become a candidate for the next expansion This process of selection expansion is iterated until the goal state is not expanded and the OPEN queue is not empty When the goal state is expanded the solution is found but if the OPEN queue is empty and the goal state has not been expanded yet a solution does not exist When the search algorithm has finished if it is successful there is a path composed of states with g values equal to g values That path is the solution Therefore the solution can be composed backward starting from the goal state and s
11. loss of completeness Indeed we loose all the primitives that lead directly from an angle to another greater than the maximum rotation allowed starting from the first angle From a theoretical point of view it is possible to compose every movement considering the minimum space needed to rotate of an angle To do this however we should have a very small resolution increasing a lot the time of primitive construction and the memory requirements during planning due to the map dimensions Consequently taking a resolution value that is approxi mately equal to the length of the vehicle it is possible to combine the primitives calculated with a maximum rotation of gt rad resulting in a good compromise To explain better the problem we can do an example supposing to have a resolution of 1 meter we can create primitives that pass from an orientation to the next orientation but in this way the minimum space needed for each prim itive could be 1 cell in x direction and 1 cell in y direction If we combine all primitives to do a rotation of 5 rad the movement ends in cell 4 4 Instead calculating directly the primitives that rotate the vehicle of 5 rad it could end in a nearer cell An example similar to the previous one is shown in Figure 4 3 On the contrary it is really difficult that the vehicle can turn back or turn of an angle major of 5 rad in a nearer cell than the one obtained by combining a 5 rad turn with another movement Now considering
12. 8 v s lt g s and g s gt v s c s s it holds that key s gt key s 3 v and g values of all states are initialized in such a way that all the v values are non negative g Sstart 0 and for every state s Sstart g s min v s c s s s Epred s CLOSED OPEN all inconsistent states function UPDATESETMEMBERSHIP s if v s g s then if s not in CLOSED then insert update s in OPEN with key s end if else if s in OPEN then remove s from OPEN end if end if end function function COMPUTEPATH while key Sgoai gt amin key s OR v Sgoat lt g Sgoat do remove s with the smallest key s from OPEN if v s gt g s then v s g CLOSED CLOSED s for all successor s of s do if g s gt g s c s 8 then g 5 g s c s 8 UpdateSetMembership s end if end for else propagating underconsistance v s 00 UpdateSetMembership s for all successor s of s do if sl Sstart then g s min v s e s 8 s Epred s UpdateSetMembership s end if end for end if end while end function 46 Chapter 2 State of Art changes one or more edge costs in the graph The Anytime D algorithm is able to do both types of changes simultaneously improving a solution by decreasing e even when the model of a problem changes slightly as reflected in the edge cost changes The pseudo code of AD is reported in A
13. E suboptimal solution Update keys e s in OPEN and prune states if g s h s gt G end while end function Denoting g s c Sstart 5 the cost of the optimal path between the start state Sstart and s if the optimal solution has not yet been found there must be a state s OPEN that is part of the optimal solution and whose g value is optimal Moreover each time a state s is selected for expansion in the second line of ImproveSolution function its e s value bounds the suboptimality of the current G A oculi solution to sb e s gt G The track of the suboptimality bound of the current best solution is stored in the variable E The proof on the suboptimality bounds of the ANA algorithm and some 2 4 Main Search Based Planning Algorithms 43 Figure 2 12 Example of ANA algorithm execution experimental results are reported in 11 In Figure 2 12 is reported an example of ANA execution lt shows the difference of the algorithm with respect to ARA during its first execution it found a solution rapidly not the best solution but a solution with a subopti mality bound After the algorithm expands other states using previous search effort and it can find the best solution The goodness of the algorithm is the ability to generate always solutions with some state expanded so the search increase the progresses without overload the s
14. XYOVO XYOV6_LLT 270 329 151 575 121 806 123 367 123 204 AD Table A 15 Average of the suboptimality bound reached at the best solution found when is found in the 70 problems of the second test set ESC XYOV XYOV_LLT XYOV XYOVO_LLT 1 00 1 25 1 75 1 96 1 63 AD Table A 16 Average of the time in seconds used to find the best solution found when is found in the 70 problems of the second test set xvo XYOV XYOV_LLT XYOVO XYOV _LLT 69 00 691 32 1093 88 1142 58 1059 47 AD Appendix B User manual The software is provided on the AirLAB svn From that repository it is possible to find all software sources developed for this thesis They was developed and used under a Linux operating system in particular the compatbility is granted for buntu based distribution from 12 04 however it should be compatible with all Linux distributions Moreover it should work correctly also on Mac OS and Windows if the dependencies are satisfied and gcc 4 7 compiler and cmake are installed and configured correctly B 1 Maps B 1 1 Map generation First of all in order to use the terrain generation tool is needed a Matlab instal lation After is necessary download the Automatic Terrain Generation toolbox from the Mathworks website Once the download is finished from the root of the cloned folder using a shell move into Maps TerrainGeneration In that folder there is a Matlab script called myTerrainGenerationScript m Cop
15. affrontare quest ultimo problema sono stati utilizzati due software per la soluzione di problemi di controllo ottimo ACADO e Bocop Le primitive sono state infatti generate risolvendo il problema di controllo ottimo che porta da una configurazione del veicolo a un altra Inoltre per tenere in considerazione la morfologia del terreno stato utilizzato un metodo particolare per creare delle mappe di costo che tiene conto dell altezza o della pendenza del terreno nei vari punti la seconda modalit quella usata in via definitiva partendo da una rappresentazione DEM Digital Elevation Model del luogo nel quale la pianificazione avviene Infine stata utilizzata una particolare funzione di costo per tenere in considerazione sia le caratteristiche del terreno sia il tempo impiegato per percorrere il percorso trovato Durante il lavoro sono state analizzate due tipologie di veicoli un veicolo Ackermann e un veicolo differential drive che utilizzano due modelli cinematici differenti Considerando entrambe le tipologie di veicoli possibile vedere come la difficolt di passaggio da un veicolo all altro non molto elevata quindi possibile creare vari ambienti per poter pianificare con una vasta scelta di vei coli a patto di generare le primitive di moto opportune Inoltre utilizzando un algoritmo di ricerca anytime e dinamico con i dovuti accorgimenti possibile ottenere un pianificatore efficace in grado di ripianificare velocemente u
16. and R Motwani Path Planning In Expansive Configuration Spaces International Journal of Computational Geometry amp Applications vol 9 no 4 amp 5 pp 495 512 1999 17 J J Knuffer and S M LaValle RRT Connect An Efficient Approach to Single Query Path Planning Robotics and Automation 2000 Proceed ings ICRA 00 IEEE International Conference on vol 2 pp 995 1001 April 2000 18 R Bohlin and L E Kavraki A Randomized Approach to Robot Path Planning Based on Lazy Evaluation Handbook on Randomized Comput ing 2001 19 L E Kavraki P vestka J C Latombe and M H Overmars Prob abilistic Roadmaps for Path Planning in High Dimensional Configura tion Spaces Robotics and Automation IEEE Transactions on vol 12 pp 566 580 August 1996 20 S Karaman and E Frazzoli Sampling based algorithms for optimal mo tion planning The International Journal of Robotics Research vol 30 no 7 pp 846 894 2011 21 L Jaillet J Cort s and T Simeon Sampling Based Path Planning on Configuration Space Costmaps Robotics IEEE Transactions on vol 26 pp 635 646 August 2010 22 S Russel and P Norvig Intelligenza Artificiale Un Approccio Moderno vol 1 Pearson 3 ed September 2010 BIBLIOGRAPHY 141 23 24 25 26 27 28 29 30 31 32 33 S Koenig and M Likhachev Fast replanning for navigation in unknow
17. assumptions for the velocity First of all to pass from a positive velocity to a negative velocity one have to pass through 0 m s velocity For this reason it is not useful to generate primitives that pass from a positive velocity to a negative velocity or vice versa The change of velocity 82 Chapter 4 Vehicle Models and Lattice Primitives sign is done combining two primitives the first that arrives to 0 m s and the second that starts from 0 m s Further if the velocities chosen during the discretization design phase are symmetric it is possible to generate only positive primitives and from that obtain the primitives with negative velocities In fact using the model expressed by Equation 4 1 we can negate both the controls obtaining tan L _ tand L tan v 4 v 4 6 d v tan is an odd function y Therefore the state equation for 0 is unchanged Instead the state equations of the Cartesian positions change their signs because the sign of the velocity is changed As a consequence the trajectory is the same of the one with positive velocity changing the Cartesian coordinate signs This observation can be done in the same way in the model expressed by Equation 4 5 changing the signs of the controls the transition of the velocity and steering states changes in sign but not in module and the same considerations done before still holds Finally the last assumption to do before analyzing the BVP constrai
18. be imposed on the elements of the problem in order to create a good primitive Furthermore for each BVP we would optimize the solution minimizing an objective function This kind of BVP is called OCP Therefore solving a series of OCPs we can construct a set of lattice primitives An important issue is how to create this set of lattice primitives Funda mentally we must create simple stackable trajectories therefore exploiting the translational invariance property of the state lattice it is possible to approach the problem in two ways The first solution is based on creating all feasible lattice primitives for each cell around the origin and for each pair of start and end state variable values until a predetermined distance is reached Once the primitives are created it is possible to analyze them looking for primitives that can be composed by smaller primitives and deleting the longer ones This approach however often generates an enormous number of primitives and although it leads to a good completeness of the state lattice and it increases the amount of admissible movements of the vehicle it affects also the time needed by the planner to find a plan and the required computational effort Another approach is aimed to create a connected lattice avoiding that some primitives lead to blind points The lattice generated is probably less complete with respect to the previous approach but the number of primitives generated is not so high This a
19. c s 8 if s not in CLOSED then insert update s in OPEN with key s end if end if end for end while end function Now all elements to have an efficient version of anytime search with provable suboptimality bounds were introduced and ARA can be described entirely To complete the algorithm is necessary a new set INCONS of all inconsistent states that are not in OPEN So the union of the INCONS and OPEN sets is exactly the set of all inconsistent states and can be used as a starting point for the inconsistency propagation before each new search iteration In Algorithm 4 there is pseudo code of Anytime Repairing A ARA The detailed analysis of the algorithm and some experimental results are reported in 10 A small delta to decrease e is suggested to have more benefit from the al gorithm Moreover as suggested in 10 ARA can be used to have a fast plan to execute and meanwhile the robot follow the first path generated the algo rithm compute better solutions To do this the search must be done backward in this way the g cost do not change between a search and another changing the start position of the robot and the only thing needs to do is the heuristic recomputation In Figure 2 11 is reported an example of ARA algorithm execution It starts from an inflation factor of 3 0 and decrease it of 0 5 each iteration The images show that initially a reasonable number of states are expanded but the solution found is
20. cmake make cd Now in the folder we have three executables e dd_time_vpos e genprimsvpos sh e dd_time_parallel Usage dd_time_vpos This executable in the same way of the executables for an ackermann vehicle allow to create a lattice primitive for a differential drive vehicle as explained in Chapter 4 to use them with the environment for differential drive vehicles explained in Chapter 5 The syntax to launch this program is dd_time_vpos w w_i w_f v v_i v_f t t_i t_f S s_x s_y e e_x e_y 162 Appendix B User manual where w_i is the initial discrete angular velocity value of the vehicle w_f is the final discrete angular velocity value of the vehicle v_i is the initial discrete linear velocity value of the vehicle v_f is the final discrete linear velocity value of the vehicle t_i is the initial discrete 0 orientation value of the vehicle t f is the final discrete 0 orientation value of the vehicle s_x is the start discrete position x of the vehicle s_y is the start discrete position y of the vehicle e_x is the end discrete position x of the vehicle e_y is the end discrete position y of the vehicle As output this executable gives two files one formatted with the lattice primitives and the others containing the continuous values to plot Usage genprimsvpos sh This executable accepts as arguments the start discrete orientation value the end discrete orientation value and the interval of cells in which try to g
21. constant of linear and angular velocity experimentally and use that values to simulate the dynamics of these two velocities In this way the model becomes the one represented in Equation 4 16 a v cos Y y sin aw 4 16 t wT dw 1 1 d To Ww Tw In the model that considers actuator dynamics linear and angular velocities become states and two controls u and wu are introduced representing the desired velocities Moreover the two vehicle dependent time constant T and T are also introduced Thanks to the turn in place ability it is possible to make many more maneuvers with respect to the ackermann vehicle As a consequence a slightly different approach is considered a small interval of cells is chosen and all combinations of start and end v and w are tried The only assumption that can be taken is that the vehicle during a primitive has to remain into an interval of coordinates otherwise to solve a BVP it can go everywhere making longer and not useful trajectories as shown in Figure 4 10 Otherwise it is possible to use the approach explained for the ackermann vehicle but the risk is the possibility that all the movements are done in the nearest cells though this is not the best solution for example because the vehicle executes a turn in place when it could simply go straight to reach the goal in a cell slightly farther Finally a BVP for a differential drive vehicle can 4 4 Creation of primitives for another veh
22. folder we have the executables e ackermann_space e ackermann_time_vpos e genprimsvpos sh e genminprimsvpos sh e ackermann_time_parallel e ackermann_ time parallel min e rotate_all_motions e negative_velocities_generation 156 Appendix B User manual whereas in the second folder there are the executables e ackermann_space e ackermann_steerinit_vpos e genprimsvpos sh e genminprimsvpos sh e ackermann_steerinit_parallel e ackermann steerinit_parallel_min e rotate_all_motions e negative_velocities_generation Usage ackermann_space This executable solve the simple kinematic problem preparing the solution found into some files for the executable that solve the problem with the model com prising the actuators dynamics The file is common for both the folders and the syntax is ackermann_space t t_i t_f s s_x s_y e e_x e_y vposlvneg where t_i is the initial discrete 0 orientation value of the vehicle t f is the final discrete orientation value of the vehicle s_x is the start discrete position x of the vehicle s_y is the start discrete position y of the vehicle e_x is the end discrete position x of the vehicle e_y is the end discrete position y of the vehicle vpos this flag is used to consider the assumptions on the final cells in accord ing on explanations done in Chapter 4 considering the velocity used to generate the primitive more or equal to 0 vneg this flag is used to consider the assumptions on
23. is composed by the triple x y 0 This environment accepts also a motion primitives file to simu late the real robot movement If a motion primitives file is not passed to the class generic motion primitives are used but is preferrable the use of ad hoc motion primitives EnvironmentNAVXYTHETALAT this class extends the functionality of the previous class for instance in this environment there is the possibility to set the start and goal with apposite method expressing the parameters in meters and radians there is a method that return the complete x y 0 path from the ids path and is the class used for 3D navigation EnvironmentNAVXYTHETAMLEVLAT this class extends the functionality 2 3 Planning Algorithms Definitions and Libraries 27 VIPlanner PPCPPlanner RSTARPlanner Figure 2 4 Hierarchy of existing Planner classes of sbpl ARAPlanner ADPlanner anaPlanner of the previous class in order to navigate in x y 2 0 environment The z coordinate is managed as a level index So for example for 1 level of z the environment behavior is the same of a simple x y 0 navigation instead increasing the number of levels of z there is a projection of the footprint of the robot in height to avoid collisions In addition to environment files many pieces of code as well as data struc tures pre processed macro functions and other stuffs are also provided to use easily the environments or extend them to create
24. is the same explained before but change the code because here we have more informations Usage negative_velocities_generation Also in this case the syntax is the same explained before and the work done is the same transform the primitives saved to create a set of primitives with positive and negative velocities The syntax is the same of the executable developed for the previous model used but change the code because here we have more informations B 2 5 Lattice Primitives considering LLT This kind of primitives was generated using Bocop as solver The proce dure to solve the two kind of BVPs one save only x y 0 v the other save x 4 0 v 6 is similar so in the same way of the previous case the explanations are given in parallel Build The two folders taken into account in this case are MotionPrimitives 0207_bocop_single_track and MotionPrimitives 0212_bocop_single_track_steer Into this folder are presents many files All the files with tpp extension are C template files and are useful to define the model Some other files are used by Bocop to define the problem start conditions end conditions and all things needed to solve the Optimal Con trol Problem Between all the files there is a file named problem def that contain B 2 Lattice Primitives 159 informations on the problem and it is needed to create the executable Now open the Bocop GUI launching the file lt BOCOP_ROOT gt qtgui BocopGui and load one o
25. maneuvers and it does not consider the speed and the feasibility of that maneuvers AII the Tables with data extracted from these tests are in Appendix A 6 mainly because AD and ANA does not expands duplicate states in one iteration ANA can expand some duplicates 122 Chapter 5 Experimental tests 2 5 15 Number of expansions 0 5 x10 ARAXY _ ARAXY V ARAXY V_LLT 1 ARAXY V5 ARAXYOV6_LLT s ANAXY ANAXYOV ANAXYOV_LLT FI ANAXY V6 ANAXY V6_LLT gt _ ADXY0 A ADXYOV ADXY V_LLT ADXYOV5 ADXY V6_LLT 600 800 1000 Time s 1201 1400 1600 a Expansions done by the various search algorithms during planning over time in seconds Number of expansions ARAXYO S ARAXY a ANAXY ADxXYe A4 ADXYOV lt ADXYOV5 ANAXYOV 7 ANAXY V_LLT ANAXYOV5 ANAXY VS LLT x ADXY V LLT ARAXY V_LLT ARAXY V ARAXY V LLT ADXYOV_LLT Y 600 800 Time s L 1000 1200 1400 1600 b Expansions done by the various search algorithms during planning over time in seconds with focus on ARA and AD Figure 5 13 Expansions done in a problem of the first test set 5 3 Planner Evaluation 123
26. many trajectories cannot be generated To solve this issue one can set a parameter of the OCP and use the exact matrix not only an approximation to the detriment of the solving time needed Moreover 5 2 BVP Solvers 99 Hi fo Primitives lt I FOrRFrTNWAUD N CO OO 10 9 8 7 6 5 4 3 2 1 012345678910 x Figure 5 2 Example of primitives taking into account the four state variables and actuators dynamics 100 Chapter 5 Experimental tests Hi fo lt I FOrRrTNWAUD N CO OO 10 9 8 7 6 5 4 3 2 101234567 8910 x Figure 5 3 Example of primitives taking into account the five state variables and actuators dynamics 5 2 BVP Solvers 101 there is another problem indeed in the extended model in some of the state equations the velocity parameter appears in the denominator This becomes a problem when the velocity starts or ends to 0 m s A possible workaround is to constraint the minimum velocity to 0 1 m s However also with this new value ACADO have some difficulties and it was not able to solve the problem despite some tries was done for example we have tried to solve the problem in the space domain then the problem with the model that includes actuator dynamics using its solution as initial input of some states of the extended model problem Other tries was done changing the order of the Runge Kutta integrator changing the integrator type increasing the number of the integrator steps or increa
27. methods used by the planners to commu nicate with every type of environment lt cannot be instantiated directly in code it works only as interface between Environments and Planners EnvironmentXXX this class is given as a template class from the creators of SBPL to the developers it represents a simple and naked environment so a user can copy and paste the code from that class to create a personalized environment AdjacencyListSBPLEnv this class represents an SBPL environment as an ad jacency list graph EnvironmentROBARM this class implements an environment for a planar kinematic robot arm with variable number of degrees of freedom 26 Chapter 2 State of Art I Coords i AdjacencyListSBPLEnv EnvironmentXXX EnvironmentNAV2D EnvironmentROBARM EnvironmentNAV2DUU EnvironmentNAVXYTHETALAT TICE EnvironmentNAVXYTHETAMLEVLAT Figure 2 3 Hierarchy of existing Environment classes of sbpl EnvironmentNAV2D this class is used to model an environment with the shape of a grid and a robot state in this problem type have 2 coordi nates x y in fact these are 2D problems This type of environment is useful for simple navigation problems EnvironmentNAV2DUU this class will be completed in the following version of the library Up to now it is indicated as not completed and is not explained the use in the code EnvironmentNAVXYTHETALATTICE this is the base class to execute a 3D planning in particular in this class the state
28. preliminary definitions necessary to understand the algorithms the main families of planning algorithms and the libraries that use them 2 3 1 State Space and Action Space preliminary definitions The State Space is the set containing all the configurations reachable by the robot i e if we take into account the Cartesian position x y of a robot the State Space contains all the admissible configurations x y of the robot Every element of the State Space admissible configuration is called state The State Space is important because in the following algorithms when a robot executes an action according to the plan generated it passes from a state to another state The first state of the robot is the Initial State or Start State and the Goal State is the desired configuration The State Space is often a continuous set with an infinite number of elements but in some algorithms it is discretized to reduce the complexity obtaining good solutions anyway 2 3 Planning Algorithms Definitions and Libraries 23 The Action Space or Control Set is the set of all the executable actions i e a rover could go straight steer left steer right etc The Action Space is used to lead a robot from a state to another state executing a particular action so given a particular state A and the Action Set we can find all the possible states reachable by the robot from the state A The Action Space is an important thing to define because its card
29. rectangle form with x axis we can write y f a l d cosy size of one side lg d siny size of other side x1 l cosa YI li sina T wry lz cos 7 a y2 l2 sin 7 a 4 4 80 Chapter 4 Vehicle Models and Lattice Primitives Rect angl e Trajectory Figure 4 6 Sample trajectory inscribed in a rectangle highlighting limit points Now we have found all the four coordinates of the rectangle and finding between them the minimum x minimum y maximum x and maximum y we can delimit an area in which our trajectory must stay The values found can be increased or decreased in order to relax the problem and the solver can find a solution easier Besides these assumptions one have to define the constraints applied to the control in the BVP therefore minimum and maximum speed and minimum and maximum steer The final problem can be reported as pT min fo dt t v cos 0 y v sin 0 j tan d tan InitialState 0 0 0 FinalState xf yf 0f or Lmin SX Tmax Ymin SY lt Ymax Umin V lt Umax Omin lt 0 lt max b GSS min I ds dz cos 0 ay sin 0 d _ y tan ds E InitialState 0 0 0 FinalState xf yf OF Lmin L T Tmax Ymin SY Ymar Omin S OL Omax i 5 Sk S 4 3 2 Actuators dynamics and extended primitives The first problem to face as previously wrote is the actuators dynamic In fact the changes of veloc
30. representation and the low computational effort required to solve BVPs However it has also many limitation in fact the velocity and the steering angle can jump between values in a discontinuous way leading to primitives that sometimes are not doable by the real vehicle However the previous model can be changed if we want to exclude the velocity and describe only the geometrical path traveled by the vehicle In fact the model can be transformed in the one represented in Equation 4 2 where the velocity does not appear anymore and the parameter is now the space traveled in the movement S expressed in m by the vehicle Both the parameters T and S are different from the independent variable t and s The parameters indicate the time and the space respectively for a maneuver found 4 3 Ackermann vehicle 75 Figure 4 2 Frames for the state variables and for the control variables for an Ackermann kinematic that allow to understand better their signs and their meaning The upper frame is with respect to the world the arrows on the vehicle are related to the vehicle frame 76 Chapter 4 Vehicle Models and Lattice Primitives solving a BVP The independent variables indicate for which variables are the derivatives in the differential equations Solving the BVPs often the object to minimize is the parameter so in the previous model we minimized the time needed by the vehicle to execute a primitive instead in this last model we minimi
31. search must be done forward or backward cfg file this parameter is needed and is the path to the environment configu ration file Some sample of configuration file are in sbp myenvfiles xy thetav_env subdivided for number of orientation values mot prims this parameter specifies the motion primitives file In the folder sbpl myprimitives there are some primitive samples usable To show a detailed help launch the executable with h flag The solution found is saved into so Continuous txt file If the executable is launched with m flag every solution found is saved into so Continuous N txt where in place of N there is the e value multiplied for 10 Usage xythetavsteer_test This program allow to test the environment with five state variables that we have developed The syntax to launch it is xythetavsteer_test s planner lt planner_t gt search dir lt search_t gt lt cfg file gt lt mot prims gt xythetavsteer_test h where h is the flag to show the help s is the flag to simulate a robot navigation in completely unknown environment cells occupation is discovered moving the robot planner allow to specify the planning algorithm to use In our case we are interested in one of adstar arastar or anastardouble default anastar does not work search dir allow to specify if the search must be done forward or backward cfg file this parameter is needed and is the path to the environment configu ration file
32. test_adjacency_list but for the our objectives is not useful Usage test_sbpl This program is the default test program shipped with SBPL We have changed it a little to test the environment that take into account 3 state variables that we have modified The syntaxes to launch this software are test_sbpl s env lt env_t gt planner lt planner_t gt search dir lt search_t gt lt cfg file gt lt mot prims gt test_sbpl h where h is the flag to show the help 164 Appendix B User manual s is the flag to simulate a robot navigation in completely unknown environment cells occupation is discovered moving the robot env allow to specify the environment used to do planning In our case we are interested in xytheta for this executable that is also the default environ ment if none is specified planner allow to specify the planning algorithm to use In our case we are interested in one of adstar arastar or anastardouble default anastar does not work If it is not specified the default planning algorithm value is arastar search dir allow to specify if the search must be done forward or backward By default the value is set to backward cfg file this parameter is needed and is the path to the environment configu ration file Some sample of configuration files for this executable are in sbpl myenvfiles xytheta_env subdivided for number of orientation val ues Other samples of environments are into sb
33. that found more optimal solutions The Table 5 11 reports the average suboptimality value reached at the best solution found by the planner This table confirms the considerations done in for the previous table In fact for the environment with three state variables the optimal solution is always found when a solution exists For the others environments the suboptimality increase its value despite the average costs of the solution that is good anyway as seen in Table 5 10 The Table 5 12 reports the average time in second used by the planner to find the best solution if a solution exists From this table we can observe that increasing the number of state variables the time needed increase a lot However we are planning on a relatively big cost map and the times reported are times needed to find the best solution found A suboptimal solution can be found in less time 126 Chapter 5 Experimental tests Table 5 11 Average of the suboptimality bound reached at the best solution found when is found in the 70 problems of the second test set ESC XYOV XYOV_LLT XYOV XYOV6_LLT 1 00 1 25 1 75 1 96 1 63 AD Table 5 12 Average of the time in seconds used to find the best solution found when is found in the 70 problems of the second test set xvo XYOV XYOVLLT XYOV6 XYOV LLT 69 00 691 32 1093 88 1142 58 1059 47 AD Moreover we report in Figure 5 15 the cost of the solutions along the y axis and the time along the x
34. the difficulties of the vehicle to across a particular part of the terrain In this thesis we represent the cost map with a matrix of numbers This matrix can be a division of the terrain into cells of fixed dimensions Each cell of the matrix represents a portion of the terrain and a cost is assigned to that cell between a minimum cost value and a maximum cost value The cost assigned is proportional to a feature we are interested in when planning In our case the costs varies between 0 and 1 where 0 means free plain zone 1 means not traversable terrain and the values between 0 and 1 are costs related to the risk in traversing that cell During planning the cost map is overlayed to the lattice on which the robot move finding the unreachable states and finding which states are simpler to reach than others The first step is find a good terrain representation the second step is the transformation of that representation in a cost map In this thesis we present two possible methods to obtain valid terrain rep resentations and two possible way to create cost map starting from the terrain representation 68 Chapter 3 Environments 3 4 1 Obtain terrain maps A realistic representation of a terrain is needed to develop a proper plan A first possible approach is the generation of the terrain using a tool to generate synthetic landscapes One of this tools for instance is a tool for Matlab that given some parameters generates a terrain
35. the final cells in accord ing on explanations done in Chapter 4 considering the velocity used to generate the primitive less or equal to 0 B 2 Lattice Primitives 157 Usage ackermann_time_vpos and ackermann_steerinit_vpos These two executables solve the problem that take into account the actua tors dynamics The first executable creates primitives file that considers as state x y 0 v whereas the second executable consider in the primitives saved x y 0 0 6 The velocities specified in these files must be both positive veloc ities and the initial values is given by ackermann_space execution The syntaxes are ackermann_time_vpos v v_i v_f t t_i t_f Ss S_X S_y e e_x e_y ackermann_steerinit_vpos st st_i v v_i v_f t t_i t_f s s_x s_y e e_x e_y where st_i is the initial discrete steering angle value of the vehicle v_i is the initial discrete velocity value of the vehicle v_f is the final discrete velocity value of the vehicle t_i is the initial discrete 0 orientation value of the vehicle t f is the final discrete orientation value of the vehicle s_x is the start discrete position x of the vehicle s_y is the start discrete position y of the vehicle e_x is the end discrete position x of the vehicle e_y is the end discrete position y of the vehicle At the end of the solving phase a file with the lattice primitive generated is created and another file containing continuous pose to plot them is created Us
36. using an algorithm The relevant parameters to specify for terrain generation are e size of the output terrain e initial elevation e roughness of the terrain e roughness of the roughness In this way is possible to obtain a plain terrain an hilly terrain or a mountain terrain depending on the specified parameters The result of synthetic terrain generation is a matrix containing values that indicate the height for each point x y Using algorithmic terrain generation it is really simple to obtain terrains for tests with appropriate resolution and to specify the features we want to test the planner on a particular scenario However a possible problem of algorithmic terrain generation is the nonexistence of that particular terrain in practice so to try the planner effectiveness and the planner performance on a real terrain the algorithmic terrain generation is not practical Another possible approach to have possible test terrains involves the use of a DEM Digital Elevation Model A DEM is digital model of terrain elevation gained with some techniques such as radar or laser Digital elevation models are subdivided into two main categories Digital Terrain Models These are models of the terrain surface without ob jects naturals and artificials of all types Therefore in DTMs vege tations buildings and all other objects in relief from the terrain are not considered Digital Surface Models These are models of the terrain comprisi
37. x y and Different considerations must be done for the last two variables indeed the movements of the robot are composed by the combinations of those Five linear velocities and five angular velocities could be sufficient for both null velocity small speed and high speed positive and negative Combining all of these speeds we obtain 25 combinations 24 if we remove 0 0 The cost functions described before are still valid however it is important to consider that the time taken by an action in differential drive platform is not always given by the traversed space over the linear velocity because the robot can move only with angular velocity In this case the space traveled could be also equal to 0 but the time needed to do the action can be different from 0 because a differential drive vehicle can execute turn in place maneuvers So when the time needed to execute an action is computed it is important to take into account both the linear space traveled with a linear velocity and the angular space traveled with an angular velocity Regarding the cost map it is possible to keep valid the same considerations done for the Ackermann vehicle Analogue considerations must be done for the heuristic function because computing the heuristic values we should take into account all the elements of space and velocity of the vehicle In this case however the function must 3 2 Considerations on environment extensions 65 underestimate the time
38. 1 the position x 2 the position y 3 the orientation 0 4 the velocity of the vehicle distinguished in positive and negative velocity With the addition of velocity sign is possible to distinguish the movement taken by the vehicle that are different if the velocity is positive or negative Moreover in 4 are taken into account some considerations to optimize the performance using a state lattice for example multi resolution lattice the lattice is discretized Digital Elevation Models 2 6 Lattice Primitives 53 with an high resolution near the robot and the goal and a lattice with a poor resolution is used in other parts of the state space and possibility to make plans over large areas Many considerations on the lattice are done also by Mihail Pivtoraiko and Alonzo Kelly in 28 and by Mikhail Pivtoraiko and Ross Alan Knepper and Alonzo Kelly in 7 2 6 Lattice Primitives Before explain what is a lattice primitive is necessary explain what is a motion primitive A robot can move in several way so a possibility is the creation of some minimal movement that combined together lead to a complex trajectory Each of the simple movements is called motion primitive The problem of using simple motion primitives is the difficulty to create a relation between the control of the vehicle and the state space if the robot is subjected to kinematics and dynamic constraints 29 As previously written a node in the lattice corresponds to a
39. 11 b darker pixels corresponds to higher costs black pixels are obstacles The starting point is shown with a red dot on the map the starting orien tation was of 0 rad the starting velocity was 0 m s and the starting steering angle was 0 rad 5 3 2 Experimental Results First set of tests results search algorithm evaluation We report and comment here some data extracted from the first set of tests executed We have put the data into tables reporting on the rows the different planning algorithms and on the column the different pairs of environment and lattice primitives In particular the column contains in order the following elements XY0 Environment with 3 state variable x y 9 and primitives created with the simple kinematic model XYOV Environment with 4 state variable x y 9 v and primitives created with the model that take into account actuators dynamics XYOV_LLT Environment with 4 state variable x y 0 v and primitives created with the extended model that take into account LLT XYOV Environment with 5 state variable x y v 6 and primitives created with the model that take into account actuators dynamics XYOV_LLT Environment with 5 state variable x y 9 v d and primitives cre ated with the extended model that take into account LLT The attention is drawn on the search algorithms We can start to analyze the number of solutions found also suboptimal solutions by the planner They are reported in Table
40. 141 166 February 2007 Rudolph Ackermann Notes http en wikipedia org wiki Rudolph_Ackermann Rudolph Ackermann Biography http www spartacus schoolnet co uk Jackermann htm M Zago Modellistica e controllo del servomeccanismo di sterzo di un atv Master s thesis Politecnico di Milano Scuola di Ingegneria Industriale e dell Informazione Corso di Laurea Magistrale in Ingegneria Elettronica AA 2010 2011 142 BIBLIOGRAPHY Appendix A Data extracted from tests The tables from Table A 1 to Table A 9 are referred to the first test set whereas tables from Table A 10 to Table A 16 are referred to second test set The costs in the tables are expressed in milliseconds multiplied for 103 for a better comprehension incremented in percentage of a portion of not flat terrain the times are expressed in seconds and suboptimalities are pure numbers Table A 1 Number of solutions found also suboptimal in the 100 problems of the first test set XY XYOV XYOV_LLT XYOVd XYOV6_LLT ARA 95 95 95 76 95 ANA 95 95 95 76 95 AD 95 95 95 76 95 144 Appendix A Data extracted from tests Table A 2 Number of solutions not found in the 100 problems of the first test set ARA ANA AD XYO XYOV XYOV_LLT XYOV6 XYOV6_LLT 5 5 5 24 5 5 5 5 24 5 5 5 5 24 5 Table A 3 Number of optimal solutions suboptimality equal to 1 found when is found in 1500 seconds in the 100 problems of the first tes
41. 4 a ee ae dA RS 3 1 1 Environment with three state variables x y 0 3 1 2 Environment with four state variables x y 0 v 11 13 15 17 21 21 21 22 23 24 28 29 33 33 35 37 41 43 46 53 55 55 56 60 3 1 3 Environment with five state variables x y 0 v 3 1 4 Environment with five state variables x y 0 v w 3 2 Considerations on environment extensions 3 3 Correlation between environments and lattice primitives 3 4 Considerations on the cost Map 3 4 1 Obtain terrainmaps se p eea kha ee ee 3 4 2 From the terrain representation to the cost map 4 Vehicle Models and Lattice Primitives 4 1 Introduction oo i eh Pek ep Oe ee eee eee 4 2 Methods for lattice primitives creation 4 3 Ackermann vehicle 020 00000022 pee 4 3 1 Ackermann kinematic model and y primitives 4 3 2 Actuators dynamics and extended primitives 4 3 3 Extended model and LLT index 2 4 4 Creation of primitives for another vehicle type 44 1 Differential drive vehicle 4 4 2 Differential drive model and primitives creation 5 Experimental tests 5I Introduction cpl be eee ph ee eee e Gees D2 BYP Solvers oo Bo ee he ee Oe ee ee g 5 2 1 ACADO an open source BVP solver 5 2 2 BOCOP an open source BVP solver 5 3 Planner Evaluation lt s eaa sasatna pada dedas 5 3 1 Experimental Set
42. 5 1 We can see that all the search algorithms find the same number of solutions considering the same pair of environment and lattice primitives Additionally it is easy to see how the environment that take into account five state variables and use the lattice primitives created using the 5 3 Planner Evaluation 117 a Real map of the zone used for the second set of tests the building must not be considered b Representation of the cost map used for the second set of tests cutting at a certain height Figure 5 11 Real map and cost map of the second test set 118 Chapter 5 Experimental tests Table 5 1 Number of solutions found also suboptimal in the 100 problems of the first test set XY XYOV XYOV_LLT XYOVd XYOV6_LLT ARA 95 95 95 76 95 ANA 95 95 95 76 95 AD 95 95 95 76 95 Table 5 2 Number of optimal solutions suboptimality equal to 1 found when is found in 1500 seconds in the 100 problems of the first test set XY XYOV XYOV_LLT XYOVd XYOV6_LLT ARA 95 90 92 22 69 ANA 95 79 35 19 39 AD 95 91 85 22 69 model with the actuators dynamics find less solution than the others This happen because generating random goals the plan can finish in a cell with a velocity different from 0 m s and a fixed steering angle but the primitives that lead to that state is missing constructing them as seen in Chapter 4 The solutions not found by the planner probably are placed in valid points but un
43. EPATH while key Sgoai gt min key s OR v Sgoat lt g Sgoal do remove s with the smallest key s from OPEN if v s Vi Linen v s CLOSED CLOSED s for all successor s of s do if s was never visited by AD before then de g 5 bp s null end if if g s gt g s c s s then bp s s g s g bp s c bp s s UpdateSetMembership s end if end for else propagating underconsistence v s 00 UpdateSetMembership s for all successor s of s do if s was never visited by AD before then v s 9 s 00 bp s null end if if bp s s then bp s argmin v s s 8 s Epred s g s v bp s c bp s8 5 UpdateSetMembership s end if end for end if end while end function 48 Chapter 2 State of Art Algorithm 9 AD Main function assuming heuristics consistent function KEY s if v s gt g s then return g s x h s g s else return v s h s v s end if end function function MAIN g Sgoal V Sgoat 00 v Sstart 00 bp Sgoat bp Sstart null g Sstart 0 OPEN CLOSED INCONS 0 insert Sstart into OPEN with key Sstart loop ComputePath publish e suboptimal solution if e 1 then wait for changes in edge costs end if for all all directed edges u v with changed edge costs do update the edge cost c u v if v vsta
44. Intuitively v values estimate the start distances just as g values However while g s is always the cost of the best path found so far from sstart to s v s is always equal to the cost of the best path found at the time of the last expansion of s Thus every v value is initially set to oo as with the corresponding g value except g Sstart and then it is reset to the g value of the state when the state is expanded A pseudo code of this procedure is shown in Algorithm 2 A state s with v s g s is inconsistent and a state with v s g s is consistent Thus OPEN always contains the inconsistent states Consequently since all the states for expansion are chosen from OPEN new defined A search expands only inconsistent states Moreover the g value of s is consistent with the g value of s in the following sense the cost of the found path from Sstart to s via state s given by g s c s s is already equal to g s and not lower than it The decrease in g s introduces an inconsistency between the g value of s 38 Chapter 2 State of Art Algorithm 2 A ComputePath function with v values all v values 00 g Sstart 0 other g values 00 OPEN Sstart function COMPUTEPATH while sgoai is not expanded do remove s with the smallest f s from OPEN v s g s for all successor s of s do if g s gt g s c s 8 then g 5 g 5 e s 8 insert update s in OPEN with f s
45. NA 0 2 6 14 2 AD 0 7 49 38 34 Tables 5 6 and 5 7 show how many times a search algorithm is the best in term of cost of the best solution found The first table considers a search algorithm the best search algorithm if its best solution found have a lower cost than the minimum solution cost found by the three search algorithms In the second table a search algorithm is considered the best if it found a solution that have a cost low or equal with respect to the minimum solution cost found by the three search algorithms From these tables is evident how ARA and AD performance are equivalent and ANA performance are lower with respect to the other two Moreover we report in Figure 5 12 the cost of the solutions along the y axis and the time along the x axis for a solution of this test set Instead in Table 5 7 This table shows how many times a given planner found a solution that cost less or equals lower or equal suboptimality respecting the other planners on the 100 problems of the first test set when a solution is found XYO XYOV XYOV_LLT XYOVd XYOVO_LLT ARA 95 93 89 62 93 ANA 95 88 45 38 61 AD 95 93 87 62 93 5 3 Planner Evaluation 121 ARAXYO S ARAXYOV e ARAXY V_LLT t ARAXY V6 ARAXY V6_LLT bd ANAXY 6 ANAXYOV ANAXY V_LLT ANAXYOVG ANAXY V6_LLT gt ADXY 4 ADXYOV ADXY V_LLT ADXYOV5 ADXY V LLT Solution cos
46. OS The node waits the map the start pose and the end pose and when it get all these elements it starts to plan First of all it sets up the environment then the search algorithm and finally executes a plan iteration To have a solution rapidly we have decided to start with a very big e value e 64 and to decrease it halving its value at each iteration of AD In this way the epsilon decrease rate is not linear but provides good results due to the fast search of the initial solutions and refines them in successive iterations At the end of a plan iteration the current solution is published onto a topic and if while planning the node has received different values of start position end position or map it consider these changes before continuing with the next iteration In particular if the map is changed it checks how many cells are changed and if they are less than 5 of the number of cells composing the map it notifies the environment and the planning algorithm of this changes to correct the planned path otherwise it forces the planner to restart from scratch because corrections of the current solution can be more expensive than a new plan The start position of the vehicle after one iteration might be different because it started moving with the previous solution The node then updates the start position and if the search is done backward it exploits the previous computation calculating the new solution since the g values of the state did
47. POLITECNICO DI MILANO Scuola di Ingegneria Industriale e dell Informazione Corso di Laurea Magistrale in Ingegneria Informatica Dipartimento di Elettronica Informazione e Bioingegneria On the Development of a Search Based Trajectory Planner for an Ackermann Vehicle in Rough Terrains AI amp R Lab Artificial Intelligence and Robotics Laboratory of Politecnico di Milano MERLIN Lab MEchatronics and Robotics Laboratory for INovation of Politecnico di Milano Relatore Prof Matteo Matteucci Correlatore Prof Luca Bascetta Tesi di Laurea di Andrea Conforto matricola 779840 Anno Accademico 2013 2014 Contents List of Figures List of Tables Abstract Estratto in lingua italiana Acknowledgements 1 Introduction 2 State of Art Zi Introduction di ee pew bee ee wee ee ees 22 Historical Notes 4 2464 4 fe 68 4 e204 5 Paw 4 2 3 Planning Algorithms Definitions and Libraries 2 3 1 State Space and Action Space preliminary definitions 2 3 2 Search Based Planning Algorithms 23 SER lho Pe te Be ee he ee i 2 3 4 Random Sampling Algorithms 2350 OMPELE o 4a odo Ren 2 4 Main Search Based Planning Algorithms 2A A ea es REATO DARE MESIA n ILS 243 ARAF Lene amp ae La at ws ZAR ANAF 3445 Lil nni CAS RO La a 25 State Lattice Lili See bee na 26 Lattice Primitives 2 6 lt co enee ae ha a i 3 Environments Sul iIlitroduetion s s eos 44 2
48. SCII GRID format as the DEM downloaded from SardegnaGeoportale into an occupancy map considering its slopes steepness and optionally convert ing it also into an image and into an environment configuration file template The syntax of the executable is demToOccupancySlope srcFile dstFile I imgFile eXYT xytEnvDstTemplate eXYTV xytvEnvDstTemplate eXYTVP xytvpEnvDstTemplate where srcFile is the source DEM file dstFile is the destination file where the occupancy map is saved imgFile is the destination png file where the map is saved using pixel colors in relation to the steepness in particular black pixels are unaccessible cells white pixel are free cells and the middle color corresponds to the risks xytEnvDstTemplate is the destination file where a template of configuration file for an environment with x y 0 as state variables is saved with the cost map of the DEM converted 150 Appendix B User manual xytvEnvDstTemplate is the destination file where a template of configuration file for an environment with x y 0 v as state variables is saved with the cost map of the DEM converted xytvpEnvDstTemplate is the destination file where a template of configuration file for an environment with x y 0 v 6 as state variables is saved with the cost map of the DEM converted If the slope threshold the negative height limit how much below sea level the distance between two cells horizontally and vertically and the dista
49. Some sample of configuration file are in sbp myenvfiles xy thetavsteer_env subdivided for number of orientation values 166 Appendix B User manual mot prims this parameter specifies the motion primitives file In the folder sbpl myprimitives there are some primitive samples usable To show a detailed help launch the executable with h flag The solution found is saved into so Continuous txt file Usage performance_test This program allow to launch a series of tests to face the performance and the effectiveness of the various environments and algorithms Before launch this executable is necessary change GLOBAL _PERFORMANCE_TEST value in sb pl src include sbpl config h setting it to 1 and after recompile the library In this way for each planning problem solved together the solution is saved into a file a recap of the performance as explained in Chapter 5 and the majority of the video outputs are suppressed All the files are stored in a folder struc ture with format Performance lt planner_used gt lt environment_used gt The start position of the robot the goal positions and the occupancy map are read from some files putted in the same folder of the executable called respectively startFile txt goalsFile txt and occupancyFile txt If the number of tests or the typology of tests must be changed it must be changed into the code The syntax to launch this executable is simply performance_test Usage xythetavomega_test T
50. aa a Map used for the search algorithms examples Example of A execution on the previous map Example of Weighted A withhe 3 Example of Weighted A withe 1 Example of ARA execution with decreasing inflation factor Example of ANA algorithm execution Example of AD algorithm execution Examples of discretization 2 004 Actions doable in 2D navigation State growth trend Example of construction of the cost map step by step Ackermann steering geometry LL Frames used by the Ackermann kinematic model Examples of primitive composition Example of excluded area Example of bad and good primitives ooo aaa Example of a trajectory inscribed in a rectangle Single track model represented graphically Vehicle representation with its suspended mass Frames used by differential drive model Examples of bad and good primitives x y 0 primitives generation ooa Y 0 v primitives generation o o Y 0 v primitives generation Y 0 v primitives generation considering LLT 5 5 5 6 5 7 5 8 5 9 5 10 5 11 52 Delo 5 14 515 5 16 5 17 5 18 6 1 6 2 x y 0 v primitives generation considering LLT Terrain generated and plotted with the surf Matlab function
51. ables but in this way the computational effort and memory requirements increase as well For instance to consider all state variable needed to plan according to the lateral load transfer of a vehicle the variables needed are 9 Assuming to take only 3 possible values for each variable with 16 values of orientation 500 values of x and 500 values of y we can have 500 16 3 2 916 10 possible states If some variable uses more than 3 state values this number becomes even greater and this increasing of states number is hard to manage especially from the memory point of view In Figure 3 3 we report some examples of state number growth over state variables growth supposing that every state variables can assume k different values Only from a theoretical point of view the state extension is always feasible and in a state there can be an arbitrary number of state variables In this way there is the possibility to manage a great number of vehicle physical parameters directly during the planning phase 66 Chapter 3 Environments 60000 50000 40000 30000 Number of states 20000 10000 0 2 4 6 8 10 Number of state variables Figure 3 3 Sample of growth of states number over state variable growth supposing for each example that all state variables can have k different values 3 3 Correlation between environments and lattice primitives 67 3 3 Correlation between environments and lattice prim itives A sta
52. age genprimsvpos sh and genminprimsvpos sh These two bash scripts use the same strategy of creation of the scripts explained for the kinematic model but in this case for each pair of 0 passed as arguments are created the primitives for each pair of start end positive velocities and with the model that save also the steering values also for each start steering value admissible 158 Appendix B User manual Usage ackermann_time_parallel ackermann_time_parallel_min acker mann_steerinit_parallel and ackermann_steerinit_parallel_min These executables carry out the same operations done by the executable xy theta_parallel considering the other models In particular the executables with out min at the end of the name launch in parallel the bash script genprimsv pos sh the others launch the script genminprimsvpos sh The only additional information to add during launch is the velocities signs so the syntaxes are ackermann_time_parallel t vposlvneg ackermann_time_parallel_min t vposlvneg ackermann_steerinit_parallel t vposlvneg ackermann_steerinit_parallel_min t vpos vneg For our problem set up we generate all the primitives with positive velocities and after we transform it creating the primitives with negative velocities Usage rotate_all_motions As explained for the previous model the executable make the same work chang ing the values of the states saved to obtain a rotation of the primitives The syntax
53. alone and why night after night you sit at your computer You re looking for him know because was once looking for the same thing And when he found me he told me wasn t really looking for him I was looking for an answer It s the question that drives us Neo It s the question that brought you here You know the question just as did Neo What is the Matrix Trinity The answer is out there Neo and it s looking for you and it will find you if you want it to The Matrix Path planning is a key activity in many fields of Robotics and Artificial Intelligence Path planning is the search of a path from a state A to a state B If an agent can make a high quality plan it will execute its work more easily Trajectory planning is a specified kind of path planning specialized in the construction of a trajectory travelable by an autonomous vehicle from a start configuration of the vehicle to a goal configuration of the vehicle The plan creation is important for autonomous vehicles because a vehicle can navigate also with a simple plan but it does not have infinite capabilities so it can collide with obstacles or makes some dangerous maneuvers e g for an ATV the risk of overturn for an UAV the risk of quote loss The vehicle encounters the previous problems because it generates a plan too approximate Consequently a good plan must take into account the capabilities of the vehicle during the planning phase and it would find
54. an find a cost value given by using the maximum slope between the slopes that connect the 4 cells In Figure 3 4 we have represented an example of what happen during the creation phase of a cost map The resultant map has one row and one column less than the original map with the heights Now it is simple to make a step ahead assigning a proportional cost to the resulting cell using the slope For instance if the maximum affordable slope is equal to k radiants it is possible to assign a cost of for each transition with the maximum slope less than k radiants and a cost of 1 to all transitions with the maximum slope greater or equal to amp radiants These costs are used by the planner to influence the cost of an action in base of the place where it is executed Chapter 4 Vehicle Models and Lattice Primitives An idea is like a virus Resilient Highly contagious And even the smallest seed of an idea can grow It can grow to define or destroy you Inception 4 1 Introduction This chapter introduces some methods we have used to create lattice primitives and shows the models we have used to represent the behavior of the vehicles We describe what is an Ackermann vehicle and the models both kinematic and dynamics that describes its behavior Then we focus the attention on a different vehicle type in particular we describes a differential drive vehicle writing about its kinematic model and introducing the actuators dynamics For eac
55. angle is not ensured but this is not required thanks to the fast change of the steering angle This set of primitives can be used with environments that consider x y 0 v as a state The second approach instead considers also the steering angle but it forces only the start steering value to one of the discretized values and the final value is rounded to the nearest 4 3 Ackermann vehicle 83 discretized value so it is not necessary to generate primitives for each pair of initial and final steering angles the number of primitives generated is less and final steering value is not binding The set of primitives generated in this way is used with x y 0 v 6 environment The constraints of the BVP are the same of the previous BVP regarding x y and apart from the constraints related to the rectangle that inscribes the trajectory In fact in the case of primitives that include steering angle the rectangle is enlarged to allow the vehicle to do a straightening maneuver when the initial steering value is different from 0 rad Moreover some constraints for the new variables are added In particular the velocity must assume values between the initial velocity and the final velocity and the steering angle must stay in the steering interval The control variable must stay in the same intervals of the state variables So the resulting BVP generalized for both the cases is min f dt t v cos 0 y v sin 0 tan o 10 sE nt
56. anning algorithms are good because they allow to find the optimal solution with respect to a cost function However they are usually computationally expensive and they need time and resources to find an optimal plan Random sampling algorithms are more rapid and less expensive in term of resources but it is really difficult impossible in most cases assign costs and find optimal paths with those Moreover random sampling al gorithms cannot determine if a solution exists or not meanwhile search based planning algorithms always know if a solution does not exist For our application we reputed search based planning algorithms as more appropriate To improve search based planning algorithm performance a par ticular graph structure can be used a lattice A lattice is a graph built from 19 a discretization of the state space it has regularity properties usable to make plans efficiently Moreover the actions executable by the robot can exploit the regularity properties of the lattice to reduce computational complexity when a plan is elaborated At the actual state SBPL and OMPL have already some classes to do simple generic planning Focusing on SBPL it is already able to plan for a differential drive vehicle using its Cartesian position and orientation as robot state More over it is also possible to generate plans for other kinds of robots such as robotic arms Various search algorithms are implemented in SBPL for example ARA ANA and AD
57. anning algorithms or can write another algorithm using the facilities provided by the library in the vari ous C files An important thing for a creation of a planner in SBPL is the inheritance from SBPLPlanner and the correct interface with a DiscreteSpace Information object because they are linked by the relation shown in Figure 2 5 Furthermore with the library are provided also some motion primitives files for some robots and the Matlab files used to generate them so if someone wants create the personal motion primitives could see how they are constructed On the contrary if the given primitives are enough everyone can use them Moreover are provided some configurations files to try 2D Environment 2DUU Environment 3D Environment and robot arm Environment Moreover some other useful functions are given with the library for example a little Matlab script to print a trajectory of a robot in a 2D grid world or some data structure such as MDP CHeap CList and other things used in the code Finally the library is commented with the Doxygen syntax so there is the possibility to generate the Doxygen documentation However tutorial and ex planations are not very rich and if someone would use or modify the library for the first time is not so simple the best way is to read and understand the code already written and read some tutorials on the website 2 3 4 Random Sampling Algorithms A second kind of planning algorithms is the random sa
58. another problem type Instead analyzing planners Figure 2 4 reports the classes that offer the graph search functionalities These are SBPLPlanner this is an abstract class used to communicate with environ ments Every planner inherit from this class VIPlanner this class offers functionalities of VI planning algorithm PPCPPlanner this class contains the code to execute the PPCP algorithm this introduces some probabilistic elements and it is used mainly for robotic arms 8 RSTARPlanner this class contains a code to execute the algorithm of the R planner 9 ARAPlanner in this class the algorithm of the ARA planner 10 is imple mented obtaining a result only at the end of the time given to the plan ner when a solution is found or when it is found that a solution does not exist anaPlanner in this class the algorithm of the AN A planner 11 is imple mented obtaining a result only at the end of the time given to the plan ner when a solution is found or when it is found that a solution does not exist ADPlanner in this class the algorithm of the AD planner 10 is implemented obtaining a result only at the end of the time given to the planner when a solution is found or when it is found that a solution does not exist 28 Chapter 2 State of Art 0 1 DiscreteSpacelnformation 4 SBPLPlanner environment_ Figure 2 5 Relation between Environments and Planners With these classes a user can use one of these pl
59. anytime and dynamic searches Currently some parallel versions of A algorithm exists PRA IDA but they are neither anytime nor dynamic and they are not available as components of SBPL If these algorithms became a reality it there will be also a realistic possibility of installing some network device on the vehicle compute the path on a supercomputer with many decades of cores and then send the solution back to the vehicle This could be done keeping the 138 Chapter 7 Conclusion and future works same planning software on the vehicle possibly oriented to small distance plans as fall back solution if something in the communications goes wrong With a cloud based planner we could plan on very large distances and we could use big states still having good time performance Another extension could be done to the SBPL library it could be useful to define a generic environment for autonomous vehicle navigation usable with an arbitrary number of state variables each one with an arbitrary number of values overriding for a specific case only the action cost definition and the heuristic used This is doable thinking to an appropriate configuration file or a method to pass parameters in a more generic way e g using dynamic vectors templates and other C features In this way changing the vehicle type in planning problem or changing the state representation could be as simple as changing the motion primitives An additional extensio
60. apter 2 State of Art the algorithm Algorithm 6 Forces all states to become either consistent or overconsistent function FIXINITIALIZATION Q slv 8 lt 9 s while Q is not empty do remove any s from Q v s 00 for all successor s of s do if s Sstart then g s min v s c s 8 s Epred s if v s lt g s and s not in Q then insert s into Q end if end if end for end while end function To implement the ComputePath function of LPA is necessary make some additional assumptions on the key These assumptions and the pseudo code of the ComputePath function of LPA are reported in Algorithm 7 A more detailed analysis of LPA algorithm is done in 10 and in 23 In the pseudocode reported in Algorithm 7 there is a significant optimiza tion to implement The re evaluation of g values is an expensive operation as it requires us to iterate over all predecessors of s We can decrease the number of times this re evaluation is done if we notice that it is invoked when state s is expanded as underconsistent and therefore its v value is increased to co There fore only those successors of s whose g values depending on s can be affected To keep track of these states we maintain back pointers it is set to null for the start state it is set to argmin v s c s s for the other states When s Epred s ever a state is expanded in addition to updating the g values of its successor
61. ar O U Q 1 5 Z 0 5 0 1 1 J 0 500 1000 1500 Time s Figure 5 16 Expansions done by the search algorithms in a problem of the second test set 5 3 Planner Evaluation 129 xvY0 XY6V xyev_LiT d A 1 AO GRE lt a Figure 5 17 Solution found for a problem of the second test set with a zoom on two part of the solution to see better the differences between them 130 Chapter 5 Experimental tests T_XY08 TT _XY0V XY8V_LLT XY0V6 XYOV6_LLT Figure 5 18 Solution found for a problem of the second test set reported on the real terrain representation Chapter 6 Integration with ROS l accept that every time get into my car there s 20 chance could die and could live with it but not one percent more Niki Lauda Rush 6 1 Introduction This chapter describes the current state of the system installed on the vehicle where we would initially use this planner Since the system is based mainly on the ROS middleware we have developed a ROS node for the planner In the second part of this chapter we explain that ROS node 6 2 Current system The last phase of the project consists in the integration of the developed planner in the ROS middleware to use it on the ATV vehicle currently it can only move following a manual built trajectory On the vehicle there is a hybrid system involving ROS and OROCOS middlewares The current evolution is making a tra
62. artesian position of the vehicle along the y axis expressed in m 0 orientation of the vehicle expressed in rad is the angle formed by the robot longitudinal axis and the x axis positive counterclockwise e Controls v linear velocity expressed in m s 4 4 Creation of primitives for another vehicle type 91 Figure 4 9 Frames for the state variables and for the control variables for a differential drive kinematic that allow to understand better their signs and their meaning The upper frame is with respect to the world Lower two vehicles are reported and the arrows on the vehicles are related to the vehicle frame 92 Chapter 4 Vehicle Models and Lattice Primitives w angular velocity expressed in rad s e Parameter T time to make a maneuver expressed in s Moreover it is possible to indicate with k the curvature of the trajectory and this variable can be inserted in the model substituting one of the two velocities The two velocities of the vehicle are given in Equations 4 15 7 rth 4 15a wa BS 4 15b where Vz and Vy are the two motor velocities Also with this kind of model there are some problems analyzed before for the ackermann model in fact the velocity is not reach instantaneously The simplest way is by determining the time constants of the two motors and use them to simulate the actuators dynamics As the vehicle is commanded with the linear and angular velocity however is better to determine the
63. ass Ms 366 7 Kg Suspended height hs 0 604 m Rolling rigidity ky 48900 0 Am Rolling damping factor br 1062 0 as Distance from front axial a 0 728 m Distance from rear axial b 0 565 m Front tyres cornering stiffness Cy 18137 7 Am Rear tyres cornering stiffness Cr 63601 5 Am Yaw moment of inertia I 86 5 Kg m Roll moment of inertia I TI Kg m Pitch moment of inertia Iy 41 39 Kg m
64. at solves a problem However this method is time consuming Another simpler solution consists in changing the main function of the program compiled from the GUI in such a way that it reads from files problem parameters and boundaries Then the executable can be compiled from the terminal or from the GUI and the problem parameter files can be automatically generated Finally the values used for angle speed and steer states are the same written above in the previous paragraph the controls y and 4 must be equal to 0 instead the values used for all other initial and final states was 0 1 rad for the angles and 0 01 rad s for the angular velocities As discretization type we used the implicit Euler method The last distinction concerns the various kind of constraints in fact start and end values of the problem are boundary conditions the interval of values that a state variable can assume are state constraints the interval of values between control variables must stay are control variable constraints the eventually constraints on time are parameter constraints and the LLT constraint is a path constraint The strategy of creation is the same used with ACADO so the script changes only the executable to launch Also this time are launched 36 generation processes in parallel one for each pair of 0 values At the end applying the rotations it is possible to have the final motion primitive file that is structured as the file created with the model that con
65. axis for a solution of this test set From this graph we can see how the environments paired with the lattice primitives that take into account the LLT index find the solution with a low costs Therefore when we can select primitives that considers LLT and primitives created taking into account only the actuators dynamics we can select the first one Moreover we can see also that the various suboptimal solution have not a big decrease of the cost for this reason we could start from an higher value of suboptimality and increase the decreasing factor to speed up the time in which the first solution is found Instead in Figure 5 16 we report the number of expansions done by the search algorithm during a planning along the y axis and the time along the x axis The trend of the various curves is not so different unless for the environment with three state variable that expands few states Finally to analyze the difference between the solutions we report in Fig ure 5 17 an example of the five solutions found in a problem of this set of tests In Figure 5 18 there is approximately the path found on the aerial map to understand where the vehicle pass Now we can define what is the best environment for our objectives From a computational point of view the best environment is surely the one with three state variables However it does not take into account dynamic constraints of the vehicle in the various maneuvers there is an high risks of overturn an
66. be in the interval 3 z rad To accelerate the computation of the primitives set the execution of problems was parallelized In particular with a bash script our C program was launched for given start 0 values end values and final cells this last is incremented to increase the distance from the origin every execution A C program launch in parallel many scripts In our cases we have used a 48 core computer with 64 GB of RAM Given the assumptions done before for the primitive generation for each start orientation it is necessary to solve the BVPs for 9 goal orientations The start orientation can be a value between 0 3 rad and the other orientations are obtained with the rotation of the primitives In this way all 36 problems can be launched together in parallel and all together they try to find a solution starting from the cell nearer to the origin and widen the distance if a solution is not found For each primitive the start discrete angle the end discrete pose a cost multiplier for the primitive in this case the space traveled in m the number of intermediate poses in our case 101 and the intermediate poses with values not discretized are saved on a file Once the Boundary Value Problems were solved the solutions were joined rotated four times of 5 rad to obtain all the primitives and saved on a file containing all useful information resolution used number of discrete values used to represent orientations total number of primit
67. by the vehicle If it is unfeasible or it is done without considering kinematic and dynamic constraints of the vehicle there are possibilities of collisions or fails in the execution phase The goal of this thesis is the creation of a planner for an autonomous ATV All Terrain Vehicle to be used in exploration missions integrated with ROS middleware installed on the vehicle and if possible easy to port to other vehi cles e g differential drive vehicles or UAVs Unmanned Aerial Vehicles It is important to consider that terrains traversed by an ATV vehicle can be irregular and faster path can be more dangerous Other important aspects to contemplate are the vehicle constraints and the possibility of overtaking of the vehicle All these factors influence the quality of the resulted trajectory because an unsafe or an unfeasible path planned can lead to a robot break or to a human intervention when possible In our work we have used and extended the SBPL Search Based Planning Library a planning library that exploits search algorithms to find an optimal path between two states into a graph The graph is obtained by making a discretization on the vehicle state space We have extended the library by creating new environments for the planner and good lattice primitives to reproduce accurately the behavior of the vehicle Two softwares have been used to face the latter problem ACADO and Bocop Moreover to take into account the morphology of the
68. can assign an infinite cost to every action that violate the LLT constraint and the avoid of overturns is practically certain BIBLIOGRAPHY 139 Bibliography 1 2 3 4 5 6 7 8 9 10 M Li Planning dynamic trajectories within the search based planning library Master s thesis Politecnico di Milano Scuola di Ingegneria In dustriale e dell Informazione Corso di Laurea Magistrale in Ingegneria dell Automazione AA 2011 2012 I E Paromtchik and C Laugier Motion generation and control for park ing an autonomous vehicle in Robotics and Automation 1996 Proceed ings 1996 IEEE International Conference on vol 4 pp 3117 3122 April 1996 M Likhachev Search based Planning with Motion Primitives Carnegie Mellon University M Likhachev and D Ferguson Planning Long Dynamically Feasible Ma neuvers for Autonomous Vehicles The International Journal of Robotic Research vol 28 pp 933 945 August 2009 M Pivtoraiko D Mellinger and V Kumar Incremental micro uav motion replanning for exploring unknown environments in Proceedings of the IEEE International Conference on Robotics and Automation 2013 M Pivtoraiko and A Kelly Kinodynamic motion planning with state lattice motion primitives in Proceedings of the IEEE RSJ International Conference on Intelligent Robots and Systems 2011 M Pivtoraiko R A Knepper and A Kelly Dif
69. common interface to various planner in order to find the nearest neighbor among the samples in the state space In the library many algorithms are already included to do this but a user could develop its own algorithm MotionValidator the MotionValidator class verifies if the motion of the robot between two states is valid or not At high level the MotionValidator must be able to evaluate whether the motion between two states is col lision free and respects all the motion constraints of the robot OMPL contains the DiscreteMotionValidator which uses the interpolated movements between two states to determine if a particular motion is valid This is an approximate computation as only a finite number of states along the motion are checked for validity ProblemDefinition a motion planning query is specified by the ProblemDef inition object Instances of this class define a start configuration and a goal con figuration for the robot The goal can be a single configuration or a region surrounding a particular state SimpleSetup with previous classes different types of planning are possible for example geometric motion planning and planning with controls The SimpleSetup provides a way to encapsulate the various objects necessary to solve a geometric or control query in OMPL OMPL is written mainly in C but some components and utility are written in Python Some of the planners supported by OMPL for planning under geometric constraints are e K
70. cop Files of our software and place it into the lt BOCOP_ROOT gt core replacing the existing main cpp Now all is ready to use Bocop to solve our BVPs B 2 3 Lattice Primitives with Kinematic Model Build To build these program in a terminal move the position from the root of the cloned folder into MotionPrimitives xytheta_not_unif folder and copy there the file FindACADO cmake from lt ACADO_ROOT gt cmake Now execute the commands mkdir build cd build cmake make cd Now in the folder are available all the executable files e xytheta e genprimsvpos sh e genminprimsvpos sh B 2 Lattice Primitives 153 e xytheta_parallel e rotate_all_motions e negative_velocities_generation Usage xytheta This program allow to create a set of primitives using the kinematics model of the vehicle The syntax to launch it is xytheta t t_i t_f s s_x s_y e e_x e_y where t_i is the initial discrete 0 orientation value of the vehicle t f is the final discrete 0 orientation value of the vehicle s_x is the start discrete position x of the vehicle s_y is the start discrete position y of the vehicle e_x is the end discrete position x of the vehicle e_y is the end discrete position y of the vehicle Launching the executable the problem is solved and the solution of the BVP is saved in two files one is formatted as a lattice primitives the other is done to print the trajectory with gnuplot or Matlab contains only the c
71. cribe to that topic to obtain the trajectory and all the rest of the information We report an example of communications between the nodes in Figure 6 1 6 3 Planner node We have developed a ROS node which allows to use the planner on the ATV vehicle We use AD as default planning algorithm because it has good perfor mance and is both anytime and dynamic as reported in Chapter 5 Moreover we have increase the capability of the search algorithm allowing it to publish a suboptimal solution every time it found a solution Also we can change its goal its start and the cost map between two iterations of AD algorithm The node can read from a file the parameters to plan In particular it can read e the file of motion primitives to use e the initial suboptimality value e the width of the vehicle footprint e the length of the vehicle footprint 6 3 Planner node 133 e the type of the environment to use among the one with 3 state variables x 4 0 the one with 4 state variables x y 0 v the one with 5 state variables x y 0 v 6 the one with 5 state variables x y 0 v w e the obstacle threshold from which cost value a cell has to be considered as an obstacle e the cell size i e the resolution of the map e the number of orientations used to initialize the environment e the maximum time allowed to find a feasible path e the flag indicating if the node must continue search until a solution is found if exists ignoring
72. ction must be added and the algorithm pseudo code become as shown in Algorithm 3 If the restriction of the key is satisfied then the cost of a greedy path after the search is at most e time larger than the cost of an optimal solution The set CLOSED added to algorithm is used to avoid the re expansion of the same state twice or more times Another change in Algorithm 3 is the initialization of the v and g values The only condition wanted is that no state is underconsistent and all g values satisfy the key restriction The arbitrary overconsistent initialization will allow us to re use previous search results when running multiple searches 2 4 Main Search Based Planning Algorithms 39 Algorithm 3 A ComputePath function with generalized priority function and generalized overconsistent initialization 1 for any two states s s S such that c 5 sgoal lt 00 v 8 gt g s u s gt g 8 and g s gt g s c s 5 it must hold that key s gt key s 2 v and g values of all states are initialized in such a way that v s gt g s dra 8 c s 5 forall s sstart and v Sstart gt g Sstart 0 CLOSED OPEN overconsistent state function COMPUTEPATH hile k de in k do while key sgoa gt min key s remove s with the smallest key s from OPEN v s g 5 CLOSED CLOSED s for all successor s of s do if g s gt g s c s 8 then g s g s
73. d in some cases the path found with that environment could be unfeasible path not executable maintaining certain velocities or dangerous For these reasons the environments to evaluate are the two indicated in the Tables as XYOV_LLT and XYOV6_LLT The last environment has only a problem during planning for performance reasons the memory needed for the states is pre allocated and with this environment the number of states is enormous So often the planning arrive to use also some decades of GB of memory and on the vehicle this is difficult to have So to use on the vehicle the best choice is the environment 5 3 Planner Evaluation 127 x 10 3 2 Reap e xve 3L A XYov x XYeV_LLT eal XYOVS i XYeV8_LLT i 2 6 F n lo 9 24 2 Ju L 5 2 2 N 24 1 8 1 67 AA AA Heth 1 2 I J 0 500 1000 1500 Time S Figure 5 15 Cost of the solution found over the time need to found them for a solution of the second test set with four state variables and the lattice primitives created with the extended model considering the LLT index If we have available a supercomputer we can also use the environment with five state variables and the lattice primitives created considering LLT index 128 Chapter 5 Experimental tests x 10 3 ci eT XYo si A xYev a x XY V_LLT Ko XY0VS un 4 2L XYeV8_LLT Q D i L
74. d the optimal solution a lot of states are expanded However A search with inflated heuristics known as Weighted A search can reduce the number of states examined before a solution is produced This is equivalent to process states in order of g s h s rather than g s h s Thus setting e 1 Wighted A results in a standard A and the resulting path is guaranteed to be optimal For e gt 1 the cost of the returned path is no larger than e times the cost of the optimal path Weighted A is important to construct an anytime algorithm with subop timality bounds in fact we could run a succession of these A searches with decreasing e In this way we have a series of solutions each with a subopti mality bound equal to the corresponding inflation factor and it has control over the suboptimality but each search iteration duplicates most of the efforts of 36 Chapter 2 State of Art Figure 2 9 Example of Weighted A with e 3 the previous searches so it wastes a lot of computation To solve this problem ARA has been introduced It is an efficient anytime heuristic search algorithm that runs a series of A searches with inflated heuristics reusing search efforts from previous executions while ensuring that the suboptimality bounds are still satisfied A problem of Weighted A is the possibility to fall in local minima with particular heuristics and particular environments In this case there is the chance that inc
75. d with the same name allow to generate a group of primitives given start orientation and final orientation All the files are created and deleted into the script and at the end of the script execution in the folder are present only the primitives files formalized to be joined and used in the planner and the files to make plots of the primitives If we would create primitives with negative velocities change the velocities used in this script and some conditions in create_files program is enough B 2 Lattice Primitives 161 Usage parallel_execution This program is launched in same way of the executable explained before that parallelize the creation process and make the same work in fact it parallelizes the execution of the genminprimsvpos sh script The syntax is parallel_execution t vpos Usage rotate_all_motions Also in this case this executable rotate the primitives generated to have at least one primitive for each start orientation The syntax is the same explained before for the other programs with the same name B 2 6 Differential Drive model primitives There is also the program to build the lattice primitives for differential drive vehicle Up to now the program develop exploits ACADO as solver Build First of move into the folder MotionPrimitives differential_drive 1213_grid11_TSTV In that folder copy the file FindACADO cmake as explained previously Now ex ecute the following commands mkdir build cd build
76. der GNU Lesser General Public License v3 ACADO offer a C library and a MATLAB interface to use it To solve a problem with ACADO one has to use the functions provided by the library set up a new problem and solve it using the method exposed by ACADO There is not a GUI shipped with the software however there are many examples freely available with the source code and there are tutorials and documentation on the website to understand how to use the toolkit We use ACADO to solve OCPs formalized in Chapter 4 in order to create a good set of lattice primitives 96 Chapter 5 Experimental tests The solving procedure of a BVP with ACADO consists in the creation of a source file C or Matlab that includes the problem definition defined using the ACADO library components and a call to the solver of ACADO library for the problem just defined Then the solution can be saved automatically on a file or can be stored in variables To generate a first set of primitives both models of Equations 4 1 and 4 2 can be used In our case we have used the second model because it is simpler to solve that kind of problem for the solver Moreover we have used a resolution of 1 meter so the vehicle must start and end in cells multiple of 1 meter and discretization of 16 angles not uniform in particular the values that orientation can take are arctan 0 3 rad arctan 3 k rad arctan 1 k 5 rad and arctan 3 k rad The steering value must
77. divided by 10 at the best solution found when is found in the 70 problems of the second test set XY0 XYOV XYOV_LLT XYOVd XYOV6LLT 270 329 151 575 121 806 123 367 123 204 AD this table we have not information on the quality of the solution found but only if the optimal solution is found In Table 5 10 we can see the average cost of the solutions found by the planner In this case the costs correspond to the time in milliseconds multiplied for 10 in the table for a better comprehension needed to travel the path found multiplied for a cost factor based on the terrain morphology We can see immediately how the costs of the three variables environment are greater than the others As in the previous set of tests this happens because the three variables environment does not take into account the exact velocity during the computation but it uses an average velocity without considers feasibility of certain maneuvers to a determined velocity In this case the average velocity was set at 3 0m s This is not good if we want consider the real feasibility of the path found We can analyze the results to give a judgment based on Table 5 9 and Table 5 10 indeed we can see that despite the optimal solutions found in the third fourth and fifth columns of the Table 5 9 are not so many the suboptimal solutions found are good anyway In fact the average of the costs of the third fourth and fifth columns are lower than one in the second column
78. dix B User manual v_i is the initial discrete velocity value of the vehicle v_f is the final discrete velocity value of the vehicle t_i is the initial discrete 0 orientation value of the vehicle t f is the final discrete orientation value of the vehicle s_x is the start discrete position x of the vehicle s_y is the start discrete position y of the vehicle e_x is the end discrete position x of the vehicle e_y is the end discrete position y of the vehicle The files created by this program are useful to solve our problems and to create all primitives Usage bocop This is the executable created by Bocop and allow to solve an OCP With the modified main cpp copied into Bocop folder instead to force the executable to read problem def and problem bound files is possible to pass them on the command line This feature is particularly important in parallel solving process To use this executable in order to solve a problem we can create the files needed with create_files executable and after launch this program with bocop file def file bound This generate some files but the important is the file with so extension In that file there are the results viewable with the Bocop GUI or converting it with create files executable in a file containing the primitives generated and in another file containing the continuous pose to plot the primitives Usage genminprimsvpos sh This script in the same way of the previous scripts explaine
79. e algorithm similarity on the 100 problems of the first test set when a solution is found XYO XYOV XYOV_LLT XYOV XYOV6_LLT ARA 0 7 50 38 34 ANA 0 2 6 14 2 AD 0 7 49 38 34 Table A 9 This table shows how many times a given planner found a solution that cost less or equals lower or equal suboptimality respecting the other planners on the 100 problems of the first test set when a solution is found XYO XYOV XYOV_LLT XYOV XY0VLLT ARA 95 93 89 62 93 ANA 95 88 45 38 61 AD 95 93 87 62 93 146 Appendix A Data extracted from tests Table A 10 Number of solutions found also suboptimal in the 70 problems of the second test set xvo XYOV XYOV_LLT XYOV XYOV6_LLT 55 55 55 38 56 AD Table A 11 Number of solutions not found in the 70 problems of the second test set ES XYOV XYOV_LLT XYOV XYOV6_LLT 15 15 15 32 14 AD Table A 12 Number of optimal solutions suboptimality equal to 1 found when is found in 1500 seconds in the 70 problems of the second test set ESC XYOV XYOV_LLT XYOV6 XYOV6_LLT 55 45 25 14 28 AD Table A 13 Number of solutions that not have reached the optimality when is found in 1500 seconds in the 70 problems of the second test set xvo XYOV XYOV_LLT XYOV XY0VLLT 0 10 30 24 28 AD Table A 14 Average of the costs divided by 10 at the best solution found when is found in the 70 problems of the second test set XY0 XYOV XYOV_LLT
80. e is followed positioning the front wheels in way so as to maintain a curvilinear trajectory The peculiarity of this steering model is that the axles of wheels converge to one point during a curve if the vehicle go straight axles converge in an infinite point but the front external wheel with greater turning radius than internal wheel has a steering angle lower than internal wheel Knowing that rear wheels are fixed the common point is placed on the extension of the rear wheels axle Moreover the front wheels do not steer together changing rotation around a common pivot of the arm that connects them as in the turntable vehicles but they steer moving a track rod and each wheel has its own pivot To obtain the correct effect track rod is often designed shorter than arm between two wheels if it is placed behind the arm instead it is designed longer if it is placed ahead the arm creating in both cases a little toe out effect on the front wheels As write before the two front wheels have a different steering angles and the mean of the two angles is called ackermann angle The tangent of the Acker mann angle affects the turning radius for an Ackermann vehicle In Figure 4 1 there is a sample of ackermann steering geometry The typology of the vehicle used to plan is important in lattice primitives creation phase because the motion must be executable by the vehicle For this reason it is important to create the correct primitives using the cor
81. e of the second approach is the computational effort because a lot of BVPs must be solved but a big advantage of primitives is the pre computation in fact lattice primitives are computed off line before its use in this way the computational effort to create primitives does not affect planning time An attempt to reach the minimal set of lattice primitives would be nice To Set containing only the primitives strictly necessary to generate every possible path in the lattice concatenating them 54 Chapter 2 State of Art do this there are fundamentally two possible ways generate the primitives from the origin to some radius 5 and after find and remove the lattice primitives that can be composed by other two or more lattice primitives e g Leave One Out Decomposition or D Decomposition 6 or try to generate a minimal set of lattice primitives immediately Unfortunately lattice primitives have also some drawbacks in particular ob jectives reached with a rich set of primitives conflict with other objectives e g small set of primitives is better for the planner but worst for an environment with many obstacles and narrow passages and a trajectory follower in many cases is necessary anyway to correct the imprecisions of the physical compo nents Finally a necessary consideration must be done a terrain should not be perfectly plain so the slopes on the terrain must be take into account This process was faced in the art c
82. earch expanding too much states as A Moreover it generates a great number of solution in little time in this way if the robot needs immediately a solution the path that algorithm sends to the robot is the best at the request time 2 4 5 AD In common real world applications is really difficult that the initial graph per fectly models the planning problem In fact the graph could change during the execution of the planning algorithm and this could be a problem if an edge cost decrease there are not too problems but if an edge cost is increased the states may become underconsistent The ComputePath function of A ARA ANA do not manage the underconsistence of the states For this reason a variant of A algorithm was introduced LPA Lifelong Planning A It is an incremental algorithm that can manage underconsistence of the states but LPA is not anytime The way the ComputePath function of LPA handles these states is based on a simple idea every underconsistent state s can be made either consistent or overconsistent by setting its v value to oo However setting v s co for each underconsistent state s the g values of successors of s may increase making these successors underconsistent Thus these successors need to have their v values set to oo In Algorithm 6 is reported the pseudo code to implement this idea In LPA the FixInitialization function is implemented during the ComputePath function to reduce the complexity of 44 Ch
83. edecessors and with all elements used by the planner which are related to the environment In our work we have extended SBPL library to use it with an Ackermann vehicle and to consider also dangerousness of the path To do this we can use the environment EnvironmentNAVXYTHETALAT given with the library One thing we have to change in this environment is the format of the map indeed SBPL uses a reference system similar to the image reference system with origin in the up left corner x axis right oriented and y axis down oriented instead we used a Cartesian reference system with the origin in low left corner x axis right oriented and y axis up oriented This is required to have the reference system of the cost map in agreement with the reference system used to develop motion primitives Furthermore as default the map used by the planner is a pointer to int so we have changed it to a pointer to double to allow the use of decimal costs if needed Also the cost function of that environment has been changed since as default it considers both linear time and angular time for a differential drive vehicle but this is not correct for an Ackermann vehicle We use only one time that is needed to perform the trajectory Finally the existing environment used a uniform discretization of the orientations and we have changed it as 110 Chapter 5 Experimental tests a Real terrain map get b Cost Map created starting from DTM cutting at det
84. ehicle finishes an action the speed must be one of the discrete values chosen About the cost function in this case we applied the same principle of the environment with three state variables but with the advantage that a velocity profile is associated to an action Between two states beside the trajectory and the orientation we know also the velocity maintained by the vehicle it becomes simple know the time needed to walk through two states In this way the 62 Chapter 3 Environments transition cost between two states with low velocity is higher than transition cost between two states with high velocity when it is possible the trajectory doable with highest velocity will be chosen for the plan Comparing the cost function with the previous one this has a t value more accurate and it can discriminate on the vehicle velocity Having velocity profile taken during a path between two position we could impede to go through mountainous terrain when the speed is over a certain value imposing an infinite cost to an action that satisfy the previous condition Also for the heuristic function previous considerations done for the environ ment with three state variables remain valid but a little change on the speed used is required Using the average velocity the heuristic could be admissible but surely not consistent so it is necessary to use the maximum admissible velocity to compute the heuristic function In particular to be sure about the con
85. electing the states to compose the solution in order of increasing g values until the start state is encountered From now on unless specified otherwise the term greedy path will refer to a greedy path constructed in respect of g values of states In Algorithm 1 there is the pseudo code of the A algorithm Some others details on A are explained in 10 22 Algorithm 1 ComputePath function of A g Sstart 0 other g values co OPEN Sstart function COMPUTEPATH while sgoai is not expanded do remove s with the smallest f s from OPEN for all successor s of s do if g s gt g s c s s then g s g s s 8 insert update s in OPEN with f s g s h s end if end for end while end function 2 4 Main Search Based Planning Algorithms 35 Figure 2 8 Example of A execution on the previous map In Figure 2 8 is reported an example of execution of A algorithm The Figure 2 8 shows that the optimal path is found but many cells are expanded and processed This fact becomes a problem when the environment becomes more complex with many state variables and many more cells because the memory requirements and the performance requirements become too big 2 4 2 Weighted A Normally the h values used by A search are consistent and therefore they do not overestimate the cost of the paths from the states to the goal state This property leads to an optimal solution but to fin
86. enerate the primitives At the end all the primitives with all combinations of positive velocities both linear and angular findable in the given interval for the start and end discrete orientation values are generated and stored in a file Usage dd time parallel This program allow to launch in parallel the previous explained bash script in order to generate all primitives needed If we want change the interval of orientation generated in parallels is necessary change the for cycle initialization end out conditions of the source code after recompile the program and execute The syntax of this executable is dd_time_parallel t vpos B 3 SBPL our modified planning library 163 B 3 SBPL our modified planning library Build and Install To build the library open a terminal and move from the root of our software into the folder sbpl From this folder execute some commands mkdir build cd build cmake make At this moment the library is compiled and is also usable however if we want use it in ROS is mandatory the installation of the library into the system so as superuser execute the command make install Now is all ready to test the library and for example in the build folder there are some executables that can be used to test the planner in particular the executables are e test_sbpl e xythetav_test e xythetavsteer_test e performance test e xythetavomega_ test Moreover there is an additional program called
87. ents to define graphs where it executes a search 3 1 1 Environment with three state variables x y 0 The first of the environments analyzed is an environment with three state vari ables x the Cartesian position x of the robot in the map considering the origin in the lower left point of the map it is a pure number because it is discretized as the x position of the vehicle in meters over the resolution used how wide is a cell in meters y the Cartesian position y of the robot in the map considering the origin in the lower left point of the map it is a pure number because it is discretized as the x position of the vehicle in meters over the resolution used how wide is a cell in meters 0 the orientation of the robot taken counterclockwise and discretized as integer value pure number a continuous 0 rad orientation indicate that the robot is oriented toward the x axis Taking into account only three state variables the plan is quite simple and a solution is usually found rapidly this gives the chance to make plans over large areas although some issues might arise if high speed and low speed maneuvers must be done in the same plan During the planning with only three variables speed is not taken into account so the trajectory generated could be invalid or could be difficult to drive through with the real vehicle Starting from the discretization for what concern x and y they are strictly dependent from the resolution used therefo
88. ermined height 5 3 Planner Evaluation 111 Re i di BA ase as Wess Gon Ries pe e Ce i TAN c Cost Map created using the slopes steepness of the DTM d Cost Map created using the slopes steepness of the DSM Figure 5 9 Example of map resulting from a DEM of the Sardinia 112 Chapter 5 Experimental tests explained in Chapter 3 Beside the previous variation the library has been extended with some en vironments that did not exist The structure of these environments has been explained in Chapter 3 and they take into account the velocities of the vehi cle and the start steering angle of the vehicle considering ackermann vehicles Additionally we build an advanced environment for differential drive vehicles We have made some significant changes in these environments with respect to the existing ones For instance in the existing environments actions were saved in a matrix and acceding to this matrix as ActionsV i j means ac ceding to the j th action for the i th source angle This structure is slightly too rigid so we have changed it using a vector of vectors making it dynamic We have made some changes also in the search algorithm in particular we have saved on a file some performance index to make tests A search algorithm at each iteration all the search algorithms tested are anytime so they execute many iterations save e The total execution time The execution time of that iteration The cost of
89. ers ANA Anytime Nonparametric A Instead of minimizing and controlling f s or key s depending from algorithm ANA expands the G g s open state s with a maximal value of e s he where G is the cost of the current best solution initially an arbitrarily large value The value of e s 42 Chapter 2 State of Art is equal to the maximal value of e such that f s lt Hence continually ex panding the node s with the maximal e s corresponds to the greediest possible search to improve the current solution It automatically adapts the value of as the algorithm progresses and path quality improves In addition the maximal value of e s provides an upper bound on the suboptimality of the current best solution which is hence gradually reduced while ANA looks for an improved solution In the Algorithm 5 is shown the pseudo code of ANA algorithm Algorithm 5 AN A ImproveSolution while OPEN Y do s s max e s scOPEN OPEN OPEN s if e s lt E then E e s end if if IsGoal s then G g 5 return end if for all successor s of s do if g s c s 8 lt g s then g s g s c s 8 pred s s if g s h s lt G then insert update s with key e 5 end if end if end for end while function MAIN G 00 E 00 OPEN g sstart 0 insert Sstart into OPEN with key e Sstart while OPEN do ImproveSolution Report current
90. es cre ated with the extended model that take into account LLT This time attention is drawn on the environment evaluation and on the lattice primitives comparison It is possible to start analyzing the number of solutions found also suboptimal solutions by the planner They are reported in Table 5 8 From the table emerges that with the first three environment and in the last environment a great number of solutions are found In particular in the last environment is found an additional solution with respect to first three environments This can happen due to the completeness of the primitives that is higher in the last pair of environment and primitives The environment that take into account five state variable with the primitives that consider only the actuators dynamics is the worst of all as in the previous tests done We can consider also the number of optimal solution found in time by the planner These results are reported in Table 5 9 Also in this set of tests as in the previous one we can see that increasing the number of variables the optimal solution is not found in time but only a suboptimal solution is found From 71500s as reported in test setup 5 3 Planner Evaluation 125 Table 5 9 Number of optimal solutions suboptimality equal to 1 found when is found in 1500 seconds in the 70 problems of the second test set xvo XYOV XYOV_LLT XYOV XYOV6_LLT 55 45 25 14 28 AD Table 5 10 Average of the costs
91. essary modify the value of RESOLU TION at the top of the source file defined with a define instruction Usage occupancyToRandomGoals The executable occupancyToRandomGoals allow to find many possible valid goals in the map maybe not reachable but surely valid the cells are not obsta cle The goals are created considering all five state variables taken into account in the environments the not useful variables must not be considered during en vironment initialization The goals are saved on a file goalsFileSlope txt The syntax of the executable is occupancyToRandomGoals occupancyFile where occupancyFile is the file containing the occupancy map If the number of goals to save would be changed before compile program is necessary modify the value of NUMGOALS at the top of the source file defined with a define instruction To change the discretization parameters up to now is mandatory make some modifies to the code Usage convertAll sh and convertAllSlope sh These two bash scripts do the same things but one launch the demToOccu pancy program the other launch demToOccupancySlope program These bash scripts take in input a range of value and convert sequentially a series of DEMs named as DIM asc or changing the name also DEM asc or DSM asc where the stands for digits from 0 to 9 into an im age to view graphically the terrain and choose what use Uncommenting a line in the script and commenting the actual p
92. execution in a file we have the primitive found formatted as lattice primitive in another files we have only the continuous pose to plot them in gnuplot or Matlab All the intermediate files generated during solving phase and not necessary at the end of the script are deleted Usage xytheta_parallel Launching this executable many processes are generated one for each 0 com bination in according on what explained in Chapter 4 and in Chapter 5 All of these process execute one bash script between the previous two explained in base on the strategy generation the second strategy is better how written in Chapter 4 specifying in the code the script to execute The syntax is xytheta_parallel At the end we have all the lattice primitives with 0 orientation start value in the first quadrant Pay attention that many processes are launched so if we have not a great number of processor to manage all them use this program is not a good idea Usage rotate_all_motions This program allow to rotate all primitives generated of 5 rad three times obtaining all primitives in according to what explained in Chapter 4 The syntax is rotate_all_motions d primitives_directory where primitives_directory is the folder containing all the primitives generated by the parallel_execution program or generated one by one with the previous pro grams As output is given a file xytheta_rotate txt in the same folder containing all the lattice primitives u
93. f the most desirable property to minimize is the time needed to reach the goal state from the starting state As cost function is thus possible to use the time necessary to execute the planned path In this way the g value of a state s is the time needed to reach the state s from the start state However in this environment we have no information about the speed maintained by the vehicle so to have a time we can fix an average velocity supposing that the vehicle maintains it and we can calculate the space traveled to reach a state and divide it for the average velocity The result consists in a planner that tries to minimize the space In this way the morphology of the terrain that is traversed during plan execution is not taken into account To deal with this when the cost of an action is computed the time needed to drive through the path can be multiplied for a cost factor based on the terrain morphology Some methods to compute it considering that during an action the vehicle traverses more cells are 1 get the maximum cell cost between the cells traversed by the vehicle during an action 2 get the multiplication of 1 plus the cell cost of the cells crossed by the vehicle during an action 58 Chapter 3 Environments Directions SEERA N ue ei 20 al LL A x N gt Z A 8 6 5 4 3 2 1 0 1 2 3 4 3 6 T 8 x a Uniform discretization 8 T 6 5
94. f the problem def file located into one of the folder listed before in base of which problem want solve From the GUI click on the hammer button to compile the project At this point in the folder of our problem an executable file is appeared Before launch that move the terminal into the problem folder and execute the following commands mkdir build cd build cmake make ed Now in the folder which folder depends on the problem chosen the fol lowing executables are available e create files e bocop e genminprimsvpos sh parallel_execution rotate_all_motions Usage create files This executable allow to create the file containing some problem informations and the file containing bounds of the problem Finally it allow to catch the data from the result file generated by bocop and use it to create our primitives files The syntaxes used to launch these programs are create_files ilr v v_i v_f t t_i t_f s S_X S_y e e_x e_y create_files ilr st st_i v v_i v_f t t_i t_f s S_x s_y e e_x e_y depending if we are solving the problem that save in the solution the steering informations or not In the previous commands the parameters are i indicate that this program creates the problem parameters file and the bound ary condition file r indicate that this program analyze the bocop results file and save the data we need into a file st_i is the initial discrete steering angle value of the vehicle 160 Appen
95. ferentially constrained mobile robot motion planning in state lattices Journal of Field Robotics vol 26 pp 308 333 March 2009 M Likhachev and A Stentz PPCP Efficient Probabilistic Planning with EnvironmentsClear Preferences in Partially Known Environments tech rep American Association for Artificial Intelligence 2006 R Search Association for the Advancement of Artificial Intelligence 2008 M Likhachev D Ferguson G Gordon A Stentz and S Thrun Anytime search in dynamic graphs Artificial Intelligence vol 172 pp 1613 1643 September 2008 140 BIBLIOGRAPHY 11 J van den Berg R Shah A Huang and K Goldberg ANA Any time Nonparametric A tech rep Association for the Advancement of Artificial Intelligence February 2011 12 Rice University Kavraki Lab Open Motion Planning Library A Primer January 2013 13 I A Sucan and L E Kavraki Kinodynamic Motion Planning by Interior Exterior Cell Exploration Workshop on the Algorithmic Foundations of Robotics December 2008 14 R Bohlin and L E Kavraki Path Planning using Lazy PRM in Robotics and Automation 2000 Proceedings ICRA 00 IEEE International Confer ence on pp 521 528 April 2000 15 G Sanchez and J C Latombe A Single Query Bi Directional Probabilis tic Roadmap Planner with Lazy Collision Checking Robotic Research vol 6 pp 403 417 2003 16 D Hsu J C Latombe
96. ge the behavior of the model ACADO had some problem to find a solution directly with this model so to improve the solver behavior the problem is initialized with the solution of the correspondent simple kinematic problem For the initialization of the two new states a maximum velocity value is set and the steering is taken from the controls of the kinematic problem The time parameter is initialized as the ratio between the euclidean distance to travel and the medium velocity between initial and final velocity values First of all the simple kinematic problem is solved then this solution is need to initialize the problem that includes the dynamics of the actuators To do this a bash script receives initial and final values of 0 with the same strategy seen for the kinematic model It solves the simpler problem and once a solution is found tries to solve the more complex problem At the end of an iteration if a result is found a new pair of speed values and eventually a new initial steer value are used to solve a new problem otherwise a wider cell is taken and the procedure is restarted At the end of the script all useless files are deleted automatically leaving only the files containing the primitives Also in this case all scripts are launched in parallel for each pair of values as in the previous case but each scripts try to solve the problem for each pair of selected velocities start and final values can take four positive values 0 m s 1 5
97. h algorithm and for a better comprehension for each algorithm is done an example Every example is based on a 2D navigation and uses the same map For each algorithm the example reported is selected to show the peculiarity of the algorithm In the examples the obstacles cells are colored in black the free cells are colored in white the expanded cells are colored in gray the start cell is colored in green and the goal cell is colored in blue The heuristic function used is the same in all the algorithms and it is the maximum distance in rows or in columns between a state and the goal state The cost assigned to the movements is equal to 1 for an up movement a down movement a left movement or a right movement and 1 01 for a diagonal move ment of 1 cell to give little advantage to horizontal and vertical movements The map used for the examples is reported in Figure 2 7 2 4 1 A A is one of the simplest search based algorithms and it is the base for the algorithms implemented in SBPL library its objective is to reach the goal state 34 Chapter 2 State of Art finding the optimal path using a cost function to express costs of the transitions between nodes of a data structure such tree or graph A maintains g values for each state it has visited so far where g s is always the cost of the best path found so far from sstart to s If no path to s has been found yet then g s is assumed to be oo A starts by setting g Sstart to 0
98. h model we explain it is also provided a detailed description of we have done to create the primitives with that model 4 2 Methods for lattice primitives creation A method to created primitives must be defined As said in the state of art there are two possible ways to create lattice primitives one samples a big action space and extract lattice primitives meanwhile another method creates directly lattice primitives solving BVPs For our work we have chosen the second method Therefore using a vehicle model we solve many BVPs one for each primitive that has to be created First of all it is necessary to define what a BVP is A Boundary Value Problem is defined as a differential equation with a set of 1Boundary Value Problems 72 Chapter 4 Vehicle Models and Lattice Primitives constraints called boundary conditions A solution of the problem is a solution of the differential equation that fulfills constraints imposed by the boundary conditions In our case there is not only a differential equation but there is a set of differential equations that describes the behavior of the vehicle This set of differential equations is composed by states controls and parameters The states are the parameter dependent variables that change following the relations expressed by the differential equations the controls are the variables on which we can act to affect the states and the parameters are independent variables of the system Constraints can
99. h space in fact we have a structure composed by nodes and edges as a graph Each node of the lattice represent a possible state of the robot state space and each node is connected to another node by an edge or a sequence of edges that represent a possible motion of the robot This method allow resolution completeness of the planning queries A motion that connect two nodes should satisfy kinematics and dynamic constraints of the vehicle in this case the motion is called feasible motion at the moment to describe state lattice we do not consider the obstacles in the environment where the plan is done An important property of the state lattice is the regularity that lend to the state lattice the translational invariant property This property allow to repeat the same motion in many nodes of the lattice to understand better why we can consider a state composed by cartesian position x y and some other state variables A motion that lead from a state A to a state B lead also from a state C to a state D if the state A have the same state variables value of the 2 5 State Lattice 47 Algorithm 8 AD ComputePath function function UPDATESETMEMBERSHIP s if v s g s then if s not in CLOSED then insert update s in OPEN with key s else if s not in INCONS then insert s into INCONS end if else if s in OPEN then remove s from OPEN else if s in INCONS then remove s from INCONS end if end if end function function COMPUT
100. he chapter there are the state of art concerning the state lattice and the lattice primitives 2 2 Historical Notes For many years the researchers have faced the problem of mobile robots nav igation providing much materials The firsts approaches were based on local planning the robot considers only the actions in the near future so it de cides step by step the next action to take In these group there was potential field based techniques where obstacles generate potential fields attractive in the goal point and repulsive where the robot cannot stay Moreover in this category there are curvature velocity and dynamic window approaches where the plan is done in control space to generate dynamically feasible actions Ma jor limitation of this kind of planners was the impossibility to generate complex maneuvers 2 as three point turn because they only reason locally meanwhile three point turn need three distinct maneuvers accurately planned Moreover this kind of planners fall often in local minima leading to an erroneous result 22 Chapter 2 State of Art To reduce the great influence of local minima of the previous approaches were developed some algorithms that incorporate global as well as local infor mations Often this approach computes a set of simple local actions and it evaluates them by using a global value function This approach works better but the local planner still causes some problems i e the robot cannot always fi
101. he heuristic function is a delicate choice because a bad heuristic function can slow down the planning phase For the previous reasons as heuristic function we have used the maximum between the Euclidean distance between two states and the distance computed as 2D navigation distance considering the world as a grid the previous value is divided for the average velocity to obtain a time incremented by 10 to avoid overestimates of the cost This choice is done because Euclidean distance between two states is a known consistent heuristic for navigation problems but it has a problem when we have u shaped obstacles because of local minima The Euclidean distance function is complemented by the 2D navigation distance function that computes the distance in terms of cells between two states To compute this value it considers only the position x y of the two states and the cost map The actions available to calculate this distance are the movements in the 8 cells adjacent to the actual position of the robot and a movement of 2 cells in one direction and 1 cell in the other direction as shown in Figure 3 2 The cost of an action in the 2D navigation case is given by the distance traveled by the action The cost map used to compute the heuristic values is a variation of the cost map used for planning where the cost of each cell is set to a threshold if the cell is not traversable and it is set to 0 in all other cases In this way the distance cost between
102. his program allow to test the environment with five state variables for the differential drive vehicles that we have developed The syntax to launch it is xythetavsteer_test planner lt planner_t gt search dir lt search_t gt lt cfg file gt lt mot prims gt xythetavsteer_test h where h is the flag to show the help planner allow to specify the planning algorithm to use In our case we are interested in one of adstar arastar or anastardouble default anastar does not work search dir allow to specify if the search must be done forward or backward cfg file this parameter is needed and is the path to the environment configu ration file mot prims this parameter specifies the motion primitives file B 4 ROS Nodes 167 To show a detailed help launch the executable with h flag The solution found is saved into so Continuous txt file B 4 ROS Nodes The ROS node developed to use the trajectory planner is created as part of the quadrivio package However it is usable anyway also to make plans for different vehicles respect the ATV involved in quadrivio project Build To use the planner node ROS must be installed and configured opportunely as explained in the on line wiki Moreover SBPL library with our changes must be installed into the system At this point two ROS node must be added to the ROS workspace These nodes are in the folders ROSNodes trajectoryPlanner and ROSNodes map_server_decimal_costs The f
103. i TW FU eae InitialState 0 0 0 vi d FinalState xf yp Of vs 4 7 Lmin SX Tmax Ymin SY lt Ymax min vi vf lt v lt max v vp Onin lt lt maz 6 5 lt 0 lt 0 P a min vj vf lt Uy lt max vi vs Omin lt ug lt maz 4 3 3 Extended model and LLT index From the previous models it emerges that it is impossible to check if a primitive is safe or not For this reason another model can be introduced a single track model 33 with a suspended mass This improved model takes into account more state variables in particular it starts from some considerations done on the single track model represented in Figure 4 7 This model starts from the assumption to collapse the left wheel and the right wheel in one wheel and making a study of the forces acting on x axis and y axis of the vehicle We can 84 Chapter 4 Vehicle Models and Lattice Primitives Figure 4 7 Single track model represented graphically identify the system of differential equations 4 8 ap E b Cr ar a C fap I r Cy apt Cr art m g sin y j p vm x 4 8 _ ba l ar b 7 an pda where p yaw angle expressed in rad B drift angle referring to the vehicle center of gravity expressed in rad a distance from front axial and center of mass expressed in m b distance from rear axial and center of mass expressed in m C front tyres cornering stiffness expressed in Nm C rear tyres c
104. icle type 93 Bad Good Figure 4 10 Example of bad primitive that is not within an interval and primitive that stay into an interval be formalized as follows min fi dt t v cos 0 y v sin 0 w p u AU InitialState 0 0 0 v wi 4 17 FinalState f yf Of Uf we Amin lt T lt Tmax Ymin lt YS lt Ymax min vi vf lt v lt max vi vp min vj vf lt Uy lt max v ve min w wp lt w lt max wi we min w wf lt Uy lt max wi we 94 Chapter 4 Vehicle Models and Lattice Primitives Chapter 5 Experimental tests A long time ago in a galaxy far far away Star Wars 5 1 Introduction This chapter shows the experiments and the tests we have done for this work First of all are presented two BVP solvers used to create the sets of lattice primitives Then we present our evaluations on the planner reporting the cost map construction the tests done and the experimental results 5 2 BVP Solvers In order to generate primitive sets we use a BVP solver The BVP solvers employed in our work are two ACADO and BOCOP Both are open source so their source code is freely available on the net and there are no fees to pay 5 2 1 ACADO an open source BVP solver ACADO is an open source toolkit for automatic control and dynamic opti mization freely available at http sourceforge net p acado wiki Home un
105. in a Y cos a Now applying a rotation of 5 rad to the two previous coordinates we obtain 0 0 T o gt 0 di 6 9 Tf Yf n ys Tf Of Of 5 Analyzing the coordinates of x and y after the transformation if the start coor dinates are into the lattice also the end coordinates are still valid because they 4 3 Ackermann vehicle TI are the same only a change of position and sign has occurred The orientation instead is increased of 3 rad but the discretization of the orientation can be done in a way that if the start angle is valid the angle incremented of 3 is valid as well Another simplification is doable considering that an ackermann vehicle can not turn in place as a consequence to do a maneuver it must travel along a path If we want that a vehicle turns back passing from an orientation 6 to the opposite orientation 0 7 it must go through all the values of orienta tion between the two previous one Considering the previous discussion about rotations of 3 rad it is possible to generate all the primitives that induce a maximum rotation of 5 of the vehicle obtaining all the others combining the primitives so far generated In this way the vehicle can reach every orientation belonging to the lattice The maximum rotation allowed is 5 rad and not a smaller value because if this value is reduced the computational effort in plan ning time is decreased due to a small number of primitives but we have a little
106. inality influences the branching factor of the graph search in the search based algorithms and despite the actions availability is important great attention must be posed on the computation tractability 2 3 2 Search Based Planning Algorithms A first kind of planning algorithms is the set of the search based planning algorithms They look for the sequence of actions that allow the robot to go from the start state to the goal state in an optimal way constructing a graph of states and searching a solution into the graph In the graph constructed each node corresponds to a state each edge corresponds to an action and it link two nodes All the algorithms in this category use a heuristic function indicated as h s that is an estimation of the cost to reach the goal state from a state s In order to use the heuristic function correctly it must be admissible and if we need to find an optimal solution the heuristic must be also consistent Find a consistent heuristic that is not admissible is very difficult so we can affirm that most of the time consistent heuristic is also an admissible heuristic An example of admissible and consistent heuristic function h s for robot navigation is the Euclidean distance from the state s to the goal state The search algorithms to take the decision on which action execute use a function A sample of function is the real cost of the path to reach the current state indicated as g s plus the heuristic function to
107. inematic Planning by Interior Exterior Cell Exploration KPIECE 13 e Bi directrional KPIECE BKPIECE 13 14 Lazy Bi directional KPIECE LBKPIECE 13 14 Single query Bi directional Lazy collision checking planner SBL 15 Parallel Single query Bi directional Lazy collision checking planner pSBL 15 2 3 Planning Algorithms Definitions and Libraries 31 Components for planning under differential constraints ControlSampler ControlSpace StateSpace StateSampler DirectedControlSampler ProjectionEvaluator StatePropagator MotionValidator Spacelnformation StateValidityChecker ValidStateSampler Path SimpleSetup Goal Planner A gt B Ais owned by B ProblemDefinition User code Figure 2 6 Components of OMPL e Expansive Space Trees EST 16 e Rapidly exploring Random Trees RRT 17 e RRT Connect RRTConnect 17 e Parallel RRT pRRT 17 e Lazy RRT LazyRRT 17 18 14 e Probabilistic RoadMaps PRM 19 e PRM 19 20 e RRT 20 e Transition based RRT T RRT 21 And with other external tools OMPL can support also e Inverse Kinematics with Hill Climbing e Inverse Kinematics with Genetic Algorithms Instead for planning under differential constraints the planner supported are e Kinodynamic Planning by Interior Exterior Cell Exploration KPIECE 13 32 Chapter 2 State of Art Rapidly exploring Random Trees RRT 17 e Expansive Space Trees EST 16 Syclop a meta pla
108. irst is the planner node the second is a node that gives a map with decimal costs to the planner These nodes can be added in two different packages Now from the root of the catkin workspace execute catkin_make roscore and the nodes are ready to be used Usage map_server_decimal_costs This node allow to publish on a topic the map used reading it from a png image To do this work somewhere in the filesystem we must have a gray scale png image and a correspondent yaml file containing the name of the image and the resolution used At this point the node can be launched with rosrun lt package gt map_server_decimal_costs lt yaml file gt where lt yaml file gt is the file explained before and lt package gt is the name of the package within the node is placed On a topic named map is published the map values The only attention must be posed to the map order format because it is the same of the original map server of ROS the first values of the vector published corresponds to the last row of the image instead the order of the column remain the same B 4 1 Usage trajectoryPlanner This node is the planner node It can be launched as rosrun lt package gt trajectoryPlanner 168 Appendix B User manual where package is trajectoryPlanner if the node is putted alone in a package called trajectoryPlanner otherwise if it is used into quadrivio package the lt package gt value is quadrivio The node read many parameter
109. ition and the state B have the same state variables value of the state D without consider cartesian position with euclidean distance between A and B equal to the euclidean distance from B and C Formally supposing to have a state composed by x y 0 where the distance between the position of the two cells is indicated with A is possible to write Ti T2 If exists a feasible path yi gt yo 0 02 TIT kA 2 kA exists also a feasible path yy kA gt yo kA Vk E Z 04 05 In this way it is possible to compute all the feasible motions for the states in a Cartesian position and repeat them for each other position Moreover a state lattice must satisfy two necessary conditions to respect the differential constraints 1 Every pair of node connected each other must be connected by a feasible motion 2 Every pair of node connected each other must guarantee the continuity of the state variables so two states are linked if and only if a feasible motion that join two states exist When a state lattice is designed three properties must be taken into account 7 Optimality how an optimal path in the lattice is near to the optimal path in the continuum state space Completeness how the discretized search space is able to approximate all pos sible motions Complexity how much computation is needed to solve a planning problem These three properties are critical because increasing the goodness of one the goodnes
110. ition x of the robot in the map considering the origin in the lower left point of the map it is a pure number because it is discretized as the x position of the vehicle in meters over the resolution used how wide is a cell in meters y the Cartesian position y of the robot in the map considering the origin in the lower left point of the map it is a pure number because it is discretized as the x position of the vehicle in meters over the resolution used how wide is a cell in meters 0 the orientation of the robot taken counterclockwise and discretized as integer value pure number a continuous 0 rad orientation indicate that the robot is oriented toward the x axis v the linear velocity of the vehicle expressed in m s discretized as integer value w the angular velocity of the vehicle expressed in rad s discretized as integer value With this kind of vehicle we have not a steering angle because a change of the orientation happens applying to the two motors of the vehicle two different velocities Therefore with these vehicle we have a linear velocity and an angular velocity We can control the vehicle movement controlling these two velocities For this reason the environment allows to take into account the two velocities of the vehicle to make plans so it is really important to have both the velocities in the state although this is computationally expensive The discretization of the variables is the same of the previous environments for
111. ity and steering angle are not instantaneous To consider 4 3 Ackermann vehicle 81 these changes in the vehicle model it is possible to extend the previous model as represented in Equation 4 5 t v cos 0 v sin 0 0 v palla 4 5 The model is composed by e States x Cartesian position of the vehicle along the x axis expressed in m y Cartesian position of the vehicle along the y axis expressed in m 0 orientation of the vehicle expressed in rad is the angle formed by the robot longitudinal axis and the x axis positive counterclockwise v linear velocity expressed in m s steering angle expressed in rad e Controls u desired velocity expressed in m s us desired steering angle expressed in rad e Parameter T time to make a maneuver expressed in s e Vehicle dependent constants L length of the vehicle from rear axis to front axis expressed in m T linear velocity time constant expressed in s the vehicle reach the 99 of the desired velocity in about 5 Ty Ts steering angle time constant expressed in s the vehicle reach the 99 of the desired steering angle in about 5 Ty With the previous model it is possible to simulate the acceleration time and deceleration time of the vehicle and the time needed to change the steering angle In this way the primitives generated are more accurate Now the considerations done for the simple kinematic model still holds Moreover we can add some
112. ives and all the primitives assigning an ID to each one In Figure 5 1 there is an example of primitive generation with the model that uses Equations 4 1 or 4 2 The second model used have introduced in our problem the actuators dy namics and is represented by Equation 4 5 In this case the problem has two more state variables and two different controls The problem set up is the same of the previous model with some additional constraints due to the new state variables and the new control variables Using this model two sets of lattice primitives were created one let free the steering angle state variable using only four state variables the other take into account also the steering angle value The constraints are formalized in Equation 4 7 with only a little change the values that controls can assume are increased in absolute value of 20 for the velocity control and of a very small constant 0 01 rad for the steering control This is done because as explained before in 5 T is reached the 99 of the 5 2 BVP Solvers 97 lt BART Lune ssi ASS A AA a Figure 5 1 Example of primitives taking into account three state variables 98 Chapter 5 Experimental tests desired value but the exact value is not reached for many other seconds and the solver could not consider reached the target value Increasing the control variables speeds up the actuators allowing to reach the target value without significantly chan
113. l we propose a different approach to deal with a DEM Knowing what is the distance that separates the cells it is possible to calculate the slope between two cells Given two cells x1 y1 21 and z2 21 V 2 21 yo 91 the position of the center of the cell and z its height In our case the previous relation is applied only to adjacent cells because we suppose that between two adjacent cells the steepness is regular and it is represented by a straight Applying the arctan operator to the previous ratio we can find the a angle that corresponds to the riptide of the slope Fixing a maximum slope travelable by the vehicle we can find what are the parts of the map accessible by it Taking 12 42 22 we have that tan a where x y indicate 70 Chapter 3 Environments Q12 Z Z2 Opre Qia Compute Compute all 0 3 23 ____ t a a between 23 Za di cells aly Vax Dy Q34 Find cost Y DEM max a Spread to all im map cells si Cost Map Figure 3 4 Example of construction of the cost map step by step We start with 4 adjacent cells and we compute the maximum slope between them Find that we create a cell from the previous 4 assigning to it a cost proportional to the maximum slope of the 6 found between the 4 cells Now we can iterate the process for the other map cells 4 near cells we c
114. l solution to 1500 seconds If the planner finds the optimal solution in time it continues with the next goal until all planning problems are executed If it does not find the solution in time it stops to the suboptimal solution found or notify a failure if it does not find any solution The cost map used in this tests is represented in Figure 5 10 b It is obtained starting from a DTM vegetations and buildings are not taken into account corresponding approximately to the place in Figure 5 10 a Its area is 1441 meters width and 1146 meters high Therefore using a resolution of 1 meter we have a map of 1441 x 1146 cells that is a relatively wide area This map does not consider the slope but we have thresholded it to an height of 5 meters 114 Chapter 5 Experimental tests regi pire i le Pmi i di a Real map of the zone used for the first set of tests the building must not be considered and water is not deep b Representation of the cost map used for the first set of tests cutting at a certain height Figure 5 10 Real map and cost map of the first test set 5 3 Planner Evaluation 115 The starting point is shown with a red dot on the map the starting orienta tion was of 3 14 rad the starting velocity was 0 m s and the starting steering angle was 0 rad Second set of tests setup We have made a second set of tests in order to evaluate the environments and the lattice primitives We have created a set
115. lanner for an autonomous vehicle the resulting planner is able to take into account vehicle constraints both kinematics and dynamics To reach this result we have extended successfully the SBPL library creating new planning environments and extending the functionalities of the AD planner to use it in a real application of an ATV ackermann vehicle with limited planning time We have considered two kind of vehicles an ATV vehicle and a differential drive vehicle and to consider the path safeness the terrain morphology was considered as a cost increment for the path A series of tests was done to verify the performance and the effectiveness of the software that we have developed The results of the tests are rather good despite the need of a considerable amount of time to find an optimal solution a suboptimal solution is found very rapidly and the quality of the solution is greater than the solution found with state of the art solution Up to now the software have been used only in simulation since the global system is under developed however simulation results are rather good and we are optimistic on the performance on the real vehicle Our work could be extended in many parts a first extension concerns the search algorithm nowadays computer have often many processors to use for computations so it would be nice if a search algorithm could exploit the paral lelism of the actual architectures maintaining in all cases the advantages of the
116. les Using a DEM the minimum height usually is not 0 sea level for this reason we find the minimum positive height in the map and reduce all heights exactly of that value In this way after the transformation only the parts of the map that exceed the threshold appear as obstacles Moreover if we are using a DEM it is necessary to pay attention to negative height cells below sea level and the unknown height cells and consider them as obstacles if needed This approach based on heights has many limitations indeed it considers only the heights so it could be good doing an assumption on the relatively flatness of the terrain Otherwise for instance between two cells at 1 meter distance there could be 2 meters of height difference if we fix the threshold at a height greater than the highest of the two both cells could be accepted but probably a great change of height in a small space means that in one of the two cells there is an obstacle On the other hand if a low height is chosen to avoid this problem in case we have a terrain with low constant slope at some point it might exceed the threshold selected although being traversable This approach can be used only when the terrain does not contain small objects it has no steep slopes and it does not present big differences in the height of the traversable cells In the real world it is really difficult to find terrains with these previous features To deal with a more general terrain mode
117. lgorithm 8 and Algorithm 9 The suboptimality bound for each solution AD publishes is the same as for ARA Some experimental results and some demonstrations are in 10 As said before this is a dynamic or incremental algorithm If this algorithm is executed with an all known map the result will be the same of ARA so to show the feature of this algorithm a particular example is done supposing that robot can view only the cells surrounding it during robot movements may be many changes on the map that affect the path planned moreover e is decreased of 0 5 each iteration till e 1 and the search is done backward to avoid the recomputation of all g costs changing the start state Figure 2 13 shows that AD correct the path when a new obstacle impede the path already planned and expands cells only if the expansion is strictly necessary This algorithm is a good choice for trajectory planning because it have all the advantages of ARA and extends it supporting changes on the map 2 5 State Lattice A state lattice is a discretization of the state space in an hyperdimensional grid converting the problem of planning in continuous function space in a problem of sequence decision generation choosing from distinct alternatives It is done sampling the state space with regularity and is based on the structure of the lattice Representing the state space in a state lattice is possible to perform a graph search state lattice is also called searc
118. lorazione integrato con il middleware ROS che installato sul veicolo Questo tipo di veicolo ha una cinematica Ackermann e durante la pianificazione necessario tenerne conto Inoltre se possibile il pianificatore dovrebbe essere facilmente portabile ad altre tipologie di veicolo ad esempio veicoli con cine matica differential drive o UAV Unmanned Aerial Vehicle come ad esempio droni a quattro rotori Nel caso di un ATV importante considerare che il terreno attraverso il quale il veicolo pu passare non regolare quindi percorsi pi brevi o con una velocit di percorrenza elevata possono essere pi pericolosi di altri percorsi Di conseguenza durante la pianificazione importante consi derare anche i limiti del veicolo tra cui la possibilit di ribaltamento Infatti un percorso non sicuro o non percorribile pu portare il robot al danneggiamento o alla necessit di un intervento umano Nel nostro lavoro per costruire un pianificatore efficace abbiamo utilizzato ed esteso una libreria chiamata SBPL Search Based Planning Library che affronta i problemi di pianificazione usando degli algoritmi di ricerca in grado di trovare il percorso ottimo tra due stati di un grafo Il grafo ottenuto attuando una discretizzazione dello spazio degli stati del robot Noi abbiamo esteso la libreria creando un nuovo ambiente per la pianificazione e un set di primitive di moto che riproducano accuratamente il comportamento del veicolo Per
119. lover Fri En LLT __ 4 12 Ha Fry Now generating primitives with this model is difficult because the model is complex has many state variables and many controls For this reason some additional assumptions are required First of all during the creation of the primitives we can suppose that the terrain is completely flat in this way two controls are invariant and are fixed to 0 Despite this simplification there are still 9 state variables to manage Saving and using all these state variables during the planning phase represents a problem from a computational and a memory point of view A possible solution can be found limiting at the start and at the end the new state variables and eventually the steering angle to values near to 3Lateral Load Transfer 4 3 Ackermann vehicle 89 0 not exactly 0 because it could be a too strict condition In this way the primitives are practically continuous and the vehicle at the start and at the end is surely stable So the constraints previously explained can be added to the one considered for the other models apart from the assumption that avoid genera tion of curvilinear trajectory at high speed because in this case the feasibility is controlled with LLT and the assumption that trajectories with negative velocity can be obtained from the trajectories with positive velocity With this model it is possible to generate both x y 0 v primitives and x y 0 v 6 primitives The fi
120. m s 3 0 m s and 9 0 m s and eventually for each value of start steering angle k k 2 1 0 1 2 Also in this case the solutions are saved in a file At the end of the execution all primitives can be obtained rotating four times of 5 rad the primitives found and for the negative velocities changing signs of the actual primitives as ex plained in Chapter 4 The final file saved contains some information resolution used number of angles number of velocity values velocities used number of steering values if in the BVP there is boundary conditions on steering angle total number of primitives and all the primitives assigning an ID to each of them With this approach discretizing in 32 values a set of primitives was generated with ACADO The results however were not so good because the number of primitives increased too much The additional values of 0 adopted in this case were arctan 3 5 rad arctan 2 5 rad arctan 3 5 rad and arctan 7 5 rad In Figure 5 2 and in Figure 5 3 there are two exam ples of primitives obtained solving BVP reported in Equations 4 7 in the first considering only the velocity in the second considering also the start steering angle At last some tries was done with the extended model but ACADO has shown some limitations First of all the solver during the solving process uses an approximation of a Hessian matrix but with this model the approximation becomes too ill conditioned and
121. mpling algorithms They concern the idea to sample the space of states of the robot starting from the continuous space of states and not from a finite set of configurations as it often occurs with search based methods This sampling operation is done in order to quickly and effectively answer planning queries especially for systems with many degrees of freedom Traditional approaches to solve these type of problems could be very slow and managing a continuous state space could be a problem Therefore the sampling process generally computes a uniform set of random configurations and at a later stage it connects these samples via collision free paths respecting the motion constraints of the robot verifying them using a simple model of the robot the previous procedure is done to answer the query the identification of a path to solve the request done to the planner Many sampling based methods provide probabilistic completeness This means that if a solution exists the probability to find a solution converges to one for the number of samples that tends to infinity Sampling based approaches cannot recognize a problem with no solution 2 3 Planning Algorithms Definitions and Libraries 29 A set of keywords used in sample based methods is composed by Workspace the physical space where the robot operates An assumption can be done for the boundaries of the workspace that represents obstacles for the robot State Space the parameter space for
122. n correlated with the previous one considers the ex tension to different kind of vehicle e g aerial vehicles water vehicles etc In this way the planner could be suitable for a great number of planning problems Another improvement of the library could be a memory usage optimization when the number of state variables and the number of states increases the planner becomes too cumbersome from a memory consumption point of view Analyzing more specifically our work some extensions could be done rela tively to the morphology of the terrain two cost maps could be used one that shows the slope in x direction positive and negative to consider both slope directions and the other that consider the slope in y direction again positive and negative to consider both slope directions Combining these two maps we could know the exact orientation of the terrain in a given point and this representation could be exploited to generate primitives that take into account also the terrain morphology using discrete values for the slope and generating them considering the various terrain morphology combinations An alternative approach to slope discretization could be the use of a set of parametric prim itives that fixed many parameters it uses the extended model of the vehicle leaving the inclination of the terrain as parameter where it appears So the LLT computation happens during planning having the exactly informations on the slope In this way we
123. n terrain Robotics IEEE Transactions on vol 21 pp 354 363 June 2005 Z Shiller S Sharma Stern and A Stern Obstacle Avoidance at High Speeds The International Journal of Robotic Research vol 32 pp 1030 1047 August September 2013 F Ambrosiani Interpolazione di mappe dem mediante algoritmi di dis tanza inversa pesata e funzioni a base radiale Master s thesis Politecnico di Milano Scuola di Ingegneria Industriale e dell Informazione Corso di Laurea Magistrale in Ingegneria Meccanica AA 2012 2013 C Hu Comparison of the interpolation methods on digital terrain mod els Master s thesis Politecnico di Milano Scuola di Ingegneria In dustriale e dell Informazione Corso di Laurea Magistrale in Ingegneria dell Automazione AA 2012 2013 DEM standards http nationalmap gov standards demstds html M Pivtoraiko and A Kelly Generating near minimal spanning control sets for constrained motion planning in discrete state spaces in Proceedings of the 2005 IEEE RSJ International Conference on Intelligent Robots and Systems IROS 05 pp 3231 3237 August 2005 Z Shiller and Y R Go Dynamic Motion Planning of Autonomous Ve hicles Robotics and Automation IEEE Transactions on vol 7 pp 241 249 April 1991 T M Howard and A Kelly Optimal Rough Terrain Trajectory Genera tion for Wheeled Mobile Robots The International Journal of Robotics Research vol 26 pp
124. n per corso se si rilevano ostacoli su quello attuale e inoltre possibile trovare una soluzione subottima molto velocemente e mentre il veicolo segue la traiettoria individuata si pu raffinare la soluzione trovata ed eventualmente modificare il percorso con uno migliore in una fase successiva L elaborato sviluppato su 7 capitoli In particolare nel Capitolo 1 presente un introduzione approfondita del lavoro svolto e una descrizione della struttura del documento nel Capitolo 2 viene analizzato lo stato dell arte quindi cosa gia stato fatto e pu essere utilizzato per raggiungere i nostri obiettivi nel Capitolo 3 si analizza il lavoro svolto ponendo l attenzione sui vari ambienti di pianificazione costruiti per le due tipologie di veicoli dove con ambienti si intende un insieme di caratteristiche specifiche utilizzate per la pianificazione come ad esempio la funzione di costo usata la funzione euristica la strut tura della mappa dei costi le discretizzazioni effettuate e le possibilit di un ambiente rispetto ad altri Nel Capitolo 4 si analizzano i modelli utilizzati per simulare i comportamenti dei veicoli presi in considerazione partendo dal mo dello puramente cinematico e integrando altri fattori in modo da simulare un comportamento accurato del veicolo questi modelli sono stati usati per gene rare le primitive di moto analizzando anche le diverse modalit di generazione possibili Nel Capitolo 5 si presenta il soft
125. n position x of the robot in the map considering the origin in the lower left point of the map it is a pure number because it is discretized as the x position of the vehicle in meters over the resolution used how wide is a cell in meters 3 1 Introduction 61 y the Cartesian position y of the robot in the map considering the origin in the lower left point of the map it is a pure number because it is discretized as the x position of the vehicle in meters over the resolution used how wide is a cell in meters 0 the orientation of the robot taken counterclockwise and discretized as integer value pure number a continuous 0 rad orientation indicate that the robot is oriented toward the x axis v the linear velocity of the vehicle expressed in m s discretized as integer value Adding the velocity we have the control of the vehicle speed in every state and in every transition In this way we can use the speed of the vehicle in planning and we can manage manage costs and risk factors in a better way About the discretization the previous considerations done for the environ ment with three state variables are still valid So x and y are discretized according to the environment resolution and the orientation is discretized in 16 non uniform values as explained for the previous environment However here we have also the velocity and it must be discretized In order to have a good control on the velocity of the vehicle the discretiza ti
126. nal BVP can be recap as min 9 dt V min fo 14 LLT dt di v cos 0 dy v sin 0 do __ tan 6 ded dv 1 1 dio Ty Yu To dd _ 1 1 di Ty Pe Ts dy _ b4 Cr a Cf b C a C a Cf dt Izv Y f Iz aes I dB _ Cp Cr Cr b Cpa m v ps Cy 9 6 dt mv B mv Y mv Sai de _ t _ 61401 dp _ fz 2 12 m b Ce Cr Cr b Cf a 3 E 09 ett v4 Sei e Y mh Msh InitialState 0 0 6 vi 0 1 0 1 0 01 0 01 0 1 0 1 0 1 0 1 0 01 0 01 FinalState x f yf 0p vp 0 1 0 1 x 0 01 0 01 0 1 0 1 0 1 0 1 0 01 0 01 Lmin LL lt Emar Ymin SY Ymax min vj vy lt v lt max yj vf Omin lt lt max bi 0 OES min vi vf lt Uy lt max vi vf Omin lt Us lt Imax 0 lt y lt 0 05750 0 8 lt LLT lt 0 8 4 13 With the previous BVP not only the time but also the LLT can be taken into account in the cost function in order to make the primitives safer 90 Chapter 4 Vehicle Models and Lattice Primitives 4 4 Creation of primitives for another vehicle type Once a problem is set up for a vehicle the question was how difficult is to adapt this approach to another vehicle For this reason a differential drive vehicle is taken into account and a generation of a set of primitives is done for this kind of vehicle 4 4 1 Differential drive vehicle A differential drive rob
127. nce be tween two cells diagonally would be changed before compile all programs is necessary modify the values e SLOPE_THRESHOLD e NEGATIVE _HEIGHT_THRESHOLD e R_DIST e D DIST at the top of the source file defined with a define instruction Usage image ToOccupancy The executable imageToOccupancy allow to transform a PNG gray scale image into an occupancy map or into an environment configuration file template as signing a decimal cost value proportioned on the color value of the pixel The syntax of the executable is imageToOccupancy srcFile 0 occFile eXYT xytEnvDstTemplate eXYTV xytvEnvDstTemplate eXYTVP xytvpEnvDstTemplate where srcFile is the source png image occFile is the destination file where the occupancy map is saved xytEnvDstTemplate is the destination file where a template of configuration file for an environment with x y 0 as state variables is saved with the cost map of the image converted xytvEnvDstTemplate is the destination file where a template of configuration file for an environment with x y 0 v as state variables is saved with the cost map of the image converted B 1 Maps 151 xytvpEnvDstTemplate is the destination file where a template of configuration file for an environment with x y 0 v 6 as state variables is saved with the cost map of the image converted If the resolution of the map how meters corresponds to a pixel would be changed before compile program is nec
128. nd in 4 5 6 7 With SBPL there is the possibility to implement particular planning modules within ROS design and drop in new environments using existing graph searches to solve it design and drop in it new graph searches and test their performance to solve existing environments or design and drop in new environments and new graph searches and test both the communication between the environments and the graph searches is shown in Figure 2 1 Moreover in some environments of SBPL motion primitives can be used to include kinodynamics constraints of the robot in Figure 2 2 there is a sample of graph expansion with motion primitives In this way the graph is sparse the paths are feasible and we can incorporate a variety of constraints To analyze the structure of the library we can divide it in some parts and analyze each part separately e Environments http www sbpl net 2 3 Planning Algorithms Definitions and Libraries 25 Figure 2 2 Sample of expansion graph with motion primitives e Planners e Motion Primitives Environment Configuration files and other stuffs Starting from the environments in Figure 2 3 it is possible to see a simple hierarchy graph of the classes of the environments given with the library We can rapidly analyze them starting from the super class and proceeding with the others DiscreteSpacelnformation this is the super class of all environments It is an abstract class that provides the
129. nd the optimal path but only a suboptimal path So successive approaches have improved the quality of the local planner to create a local planner that fol lows the global value function better and create more complex local maneuvers using a sequence of actions The most complex of these last hybrid approaches are able to generate very precise local maneuvering but they are limited by the approximate global planner Due to the inefficiency of the global planner the effort was addressed to create powerful global planners In this way the created path is followed easily by the vehicle Some algorithms developed to improve global planners are based on a construction of a graph composed by robot states After the construction a search of the solution is done in this graph An improvement of these graph search algorithms is a discretization of the space in which the search happens and the use of heuristic functions to increase plan performance this kind of planners are used in sbpl library Other kind of improved global planners are randomized planners and geometric planners With last discovers planning tasks have reached a good level of efficiency however they remain computational expensive and plan over large distances considering dynamic constraints is a challenging task so it is really important to find a way in order to plan complex feasible paths efficiently 2 3 Planning Algorithms Definitions and Libraries This section introduces some
130. nd white image where the white pixels correspond to the accessible cells whereas the black pixels represent the obstacles Instead the images generated with the second method slope thresholding are gray scale images where the gray intensity of the pixels is proportional to the slope of the terrain In Figure 5 9 there is an example of a piece of terrain represented with the real map and with the two methods used to create cost maps The second method is applied both on a DTM and on a DSM As it is possible to see from the images the second method give a cost map richer of informations SBPL Setup SBPL is an open source library for planning from the Search Based Planning Lab of the Carnegie Mellon University It allows plans with every kind of robot if an appropriate environment and a planner are defined More specifically it is com posed by a superclass of the environments called DiscreteSpaceInformation that is used as a variable of the SBPLPlanner superclass In this way the plan ner can interact with the environment to solve a problem without knowing the details of a particular environment The environment assigns a unique ID to a state composed by a combination of the state variables Then the planner uses that ID to identify a state during the search phase When the planner needs the successors of a given state it calls an environment function that given a state ID returns all the state IDs of its successors The same happens with the pr
131. needed by the actions thus similarly to the previous environment it is possible to use as heuristic the maximum distance to reach the goal state between Euclidean distance and 2D navigation distance and di vide it for the maximum linear velocity incremented by its 10 as shown in Equation 3 5 Despite we ignore the angular velocity in the heuristic computa tion the time is underestimated anyway because for instance in turn in place maneuvers the space traveled is null max de s Syoal dap s Sgoal h 3 5 s Umar k dg Euclidean distance dop 2D navigation distance Umax Maximum linear velocity in absolute value k Small constant about 0 1 Umax This environment can be a good choice if we are using a differential drive vehicle however having five state variables the search process could be slightly slow and a lot of memory might be needed An alternative to this environment obtaining worst results but in less time is the use of the environment with 3 state variables adopting the methods described to change the cost function and the heuristic function and using an arbitrary medium velocity instead of the real velocities used to make a particular action 3 2 Considerations on environment extensions Every problem can be faced the best with a suitable environment however there are some considerations to do on environment extensions Extending an environment we usually increase the number of state vari
132. ng also the objects on it Consequently in DSMs all object that influences the height of a zone are considered DSMs are more accurate and they are a superset of the DTMs if they are available they are perfect for planning but if they are not available a simple DTM can be used anyway with the insertion of new obstacles observed by the autonomous vehicle during its movement Nevertheless DEMs are not that available in fact many DEMs are available freely but they have a low horizontal resolution This means that an height reported in the DEM is related to a wide area this can be a problem using 3 4 Considerations on the cost map 69 planner because when the discretization of the map into cells is done many cells cells of the cost map must corresponds to the same cell of the DEM If the DEM horizontal resolution is higher with respect to the resolution used for the cell size during planning is necessary make elaborations of the DEM data to have a cost map of the same resolution The best would be found DEMs with the same resolution of the cost map used 3 4 2 From the terrain representation to the cost map Once we have the terrain representation we must represent a cost for a terrain part inscribed in a cell A first simple idea could be the selection of a critical height to be used as threshold In this way all the cells of the map that have a height under the threshold are considered free space while the others are considered obstac
133. nner that uses other planners at a lower level Syclop using RRT as the low level planner Syclop using EST as the low level planner About the state space instead some of the following are supported e R e SO 2 rotation in the plane e SO 3 rotation in 3D e SE 2 rotation and translation in the plane e SE 3 rotation and translation in 3D e Time representation of time e Discrete representation of discrete states e Dubins representation of a Dubins car state space e ReedsShepp representation of a Reeds Shepp car s state space e OpenDE representation of OpenDE states if the Open Dynamics Engine library is available For the control space are supported e R e Discrete e OpenDE this is an extension that is built only if the Open Dynamics Engine library is detected Logically if a user want the library could be extended with many other planners and spaces writing them starting on some classes provided by OMPL that simplify the work of planners creation and state spaces creation 2 4 Main Search Based Planning Algorithms 33 Figure 2 7 Map used for the search algorithms examples 2 4 Main Search Based Planning Algorithms Some of the main search algorithms are presented in this section with particular attention on the algorithms implemented in SBPL library The syntax used in the algorithm pseudo code is the same for eac
134. not change The goal position probably is the same until the vehicle reaches the planned point for this reason if that changes environment and planner are updated but with a backward search the planning restarts from scratch it is a reasonable choice because the destination might be changed with a completely different point If a solution is found the planner proceeds refining it until the optimal solu tion is reached each time publishing the best current solution Otherwise if a solution does not exist the planner notifies this fact and planning process end waiting for a new planning problem We have tested this node in ROS using RViz node to view the path computed and results are good It find a rapid solution which is not so bad and then it refines the solution until the optimal solution is found An example of solution found and its refinements are shown in Figure 6 2 where it is possible to see how the first solution is different from the optimal one and how at each iteration the trajectory becomes smoother and it passes through plain zones instead of hilly ones 6 3 Planner node 135 Figure 6 2 Example of solution refinement starting from a suboptimal solution reaching the optimal solution 136 Chapter 6 Integration with ROS Chapter 7 Conclusion and future works When you have excluded the impossible whatever remains however improbable must be the truth Sherlock Holmes In this work we have developed a p
135. not optimal Afterwards some other states are expanded and solution may improve its quality not in this case here it remain the same until last iteration Finally when the e became equals to 1 the optimal solution is found expanding only the unexpanded necessary states 40 Chapter 2 State of Art Algorithm 4 ARA function COMPUTEPATH while key Sgoai gt RiR ey s do remove s with the smallest key s from OPEN v s g s CLOSED CLOSED s for all successor s of s do if s was never visited by ARA before then v s g s 00 s gt g s c s 8 then g s g s e s 8 if s not in CLOSED then insert update s in OPEN with key s else insert s into INCONS end if end if end for end while end function assuming heuristic consistent function KEY s return g s ex h s end function function MAIN I Sgoat V Sgoar 00 U Sstart OO g Sstart 0 OPEN CLOSED INCONS insert Sstart into OPEN with key Sstart ComputePath publish current e suboptimal solution while e gt 1 do decrease Move states from INCONS into OPEN CLOSED ComputePath publish current e suboptimal solution end while end function 2 4 Main Search Based Planning Algorithms 41
136. nother ROS node OMPL is flexible consequently it does not represent explicitly the geometry of the workspace where the robot operates So the user must select a computa tional representation for the robot and provide an explicit state validity collision detection method Moreover there is not a default collision checker in OMPL and this allow the library to operate with few assumptions allowing it to plan for many different systems remaining compact and portable OMPL is supported on Linux and Mac OS X Windows installation is possible from the source code but is not a simply thing to do For OMPL as for SBPL it is possible to analyze the main classes and func tionalities The main components of OMPL are the following and some of these are shown in Figure 2 6 30 Chapter 2 State of Art StateSampler the StateSampler class implemented in OMPL provides the methods for uniform and Gaussian sampling in the most common state space configurations e g Euclidean spaces the space of 2D and 3D rotations Afterwards the ValidStateSampler take advantage from StateValidityChecker to find valid state space configurations StateValidityChecker the StateValidityChecker controls if a state config uration collides with an environment obstacle and respects the constraints of the robot In OMPL there is not a default checker so this component is an integral part of the chosen planner NearestNeighbor this is an abstract class that provides a
137. nsition in ROS direction wherever this is possible The modules actually integrated are various but the most relevant at least for our work are ROAMFREE and the trajectory follower ROAMFREE executes computations on the data gathered by the sensors to give information about the robot pose using it we can identify the actual position of the robot the orientation the current linear velocity and some others useful information Using ROAMFREE the planner can know the actual position of the robot to start the plan The trajectory follower is the ROS node that uses the trajectory generated by the planner to give commands to the vehicle and move it on the generated trajectory Two additional nodes give to the planner the map and the goal to reach in particular the node that provides the map 132 Chapter 6 Integration with ROS map_server decimal costs GoalPose goal RViz or node that gives goal pose Commands trajectoryController Figure 6 1 Example of communication between ROS nodes reads a png image and uses the gray scale values of the pixels publishing the corresponding cost values as a topic If something changes in the map the planner receives the updates so it can possibly change some edge costs and eventually make corrections to the plan The planner publishes on a topic the trajectory computed including a velocity profile and the steering angle at each pose in the trajectory the controller can subs
138. nts is about the stability of the vehicle With the model expressed by Equation 4 5 we have no information about the possibility of overturn of the vehicle so if we are sure that a trajectory is straight we can try to generate them with all velocities however if the trajectory have a change of orientation therefore there is a curvature in the path it is better avoid to compute the trajectory over a certain velocity Now before analyzing the BVP it is possible to do some considerations about steer and velocity Often the steer time constant is lower than the velocity time constant This means that changes of velocities can vary the shape and the length of the path significantly instead changes of steering are faster considering both initial and final value of steering angles we can generate better primitives but can lead to difficulty in trajectory generation e g the vehicle could reach final state but not with the perfect steering angle so the BVP fails to find a solution and generate primitives for each pair of initial and final steering angle leading to an enormous number of primitives To solve these problems two approaches are available The first approach does not consider the steering value at the beginning and ending of the primitives The model of the steering actuator is considered and primitives are generated ensuring that fast changes of steering do not happen In this way between two primitives the continuity of the steering
139. oblem components over parameter normalized between 0 and 1 e various parameters on the optimization process With the GUI it is possible to compile and execute a solution process and view the solution graphically Practically the BOCOP GUI is a front end for a software that exploit some C template files containing the model definition and then read some text files containing the dimensions and parameters of the problem the bound imposed the constants the variable initialization and all the others informations We can compile our problem from the GUI creating an executable to solve the problem We have used BOCOP to solve the problem with the extended model With respect to ACADO BOCOP has a wider selection of discretization methods and some of them work also with the extended model for example Mid Point Rule method and Euler methods both implicit and explicit In Figure 5 4 and in Figure 5 5 there are two examples of primitive generations obtained solving BVP reported in Equations 4 13 in the first considering only the velocity in the second considering also the initial steering angle In order to create a set of primitives we must solve many problems and collect the ones for which a solution is determined Solve many problems with BOCOP GUI is really uncomfortable so we had two ways to do this A first method consists in using BOCOP as ACADO therefore understanding the library and using every single component to create an executable th
140. of 70 goals We have generated randomly the values of x y and the orientation 0 We have set the goal velocity to 0 m s 0 1 m s in case of motion primitives created with the extended model and the goal steering angle to 0 rad We have executed these tests solving every planning problem using only AD as search algorithm because the first set of tests shown that AD is the best search algorithm for our scopes data are reported in the next subsection How ever we have used every environment and both the primitives generated with the model with actuator dynamics and with the extended model that considers LLT In particular we have used the following search algorithm e AD and we have used the following pairs of environment primitives e Environment with 3 state variable x y 0 and primitives created with the simple kinematic model e Environment with 4 state variable x y v and primitives created with the model that take into account actuators dynamics e Environment with 4 state variable x y v and primitives created with the extended model that take into account LLT e Environment with 5 state variable x y 9 v d and primitives created with the model that take into account actuators dynamics e Environment with 5 state variable x y 9 v d and primitives created with the extended model that take into account LLT We executes the tests with 16 values of orientation and where it is expected the set of velocities V
141. on must cover both low and high speeds paying attention to not use too many values to avoid an excessive increment in the computational effort Neverthe less using too few values we cannot cover admissible velocities of the vehicle and the resulting plan might not be the ideal one For instance if we use null velocity one positive velocity and one negative velocity if speeds are too low the plan could be improved simply increasing the vehicle speed on the other hand if the speeds are too high the plan could fail because the vehicle cannot maneuver at high speed A good compromise can be the use of 5 or 7 velocities subdivided between positives and negatives plus the null velocity In particular in our case we have used 7 velocities of which 3 negatives 3 positives and the null velocity The positive and negative values are opposites but the same in absolute value and are chosen to give a good range of speeds to the vehicle The speeds chosen are a very low one 1 5 m s to do some tricky maneuvers and to execute better the maneuvers when the vehicle must start and stop one low 3 0 m s for complex maneuvers when the vehicle is already in movement and one high 9 0 m s to navigate fast Moreover using also the null speed we can decide to impose that the vehicle must start from a state with null velocity and arrive in a state with null velocity To respect the lattice conditions every action must lead the vehicle into another state so when the v
142. onsidering obstacles all parts of non plain ter rain 7 or generating primitives considering terrain morphology in the primitives generation phase 30 Chapter 3 Environments Don t think you are know you are The Matrix 3 1 Introduction This chapter defines the concept of environment and describes the main kind of environment that we created and used in our work in particular three kind of environment for ATV and one environment for a differential drive vehicle Moreover it reports some considerations on environment expansion and some reflections about the correlation between environments and lattice primitives Finally the chapter reports some considerations on the cost map and describes the method that we used to create cost maps We use the term environment to indicate all the features required to con struct the state lattice The environment is thus composed by all the elements exposed to the planner to have it solving a planning problem In particular depending on the planning problem the environment defines e the composition of the states e the discretization technique e the cost function e the heuristic function e the start state e the goal state e the map cost 56 Chapter 3 Environments e all the elements necessary to provide a graph in particular a lattice to the planner In this way the planner does not distinguish an environment type from another The planner uses the environments as instrum
143. ontinuous coordinates Usage genprimsvpos sh This bash script allow to create in a file all the lattice primitives for a given pair of discrete orientation values in an interval of cells since a certain distance from the origin The syntax to launch it is genprimsvpos sh t_i t_f interval where t_i is the initial 0 orientation of the vehicle t_f is the final orientation of the vehicle interval is how far x and y must go from the origin for clarity if as interval is passed the value 3 all primitives in all combination of cells between x 3 3 and y 3 3 are tried to be generated At the end of the execution in a file we have the primitives found formatted as lattice primitives in another files we have only the continuous pose to plot them in gnuplot or Matlab All the intermediate files generated during solving phase and not necessary at the end of the script are deleted 154 Appendix B User manual Usage genminprimsvpos sh This bash script allow to create in a file all the lattice primitives for a given pair of discrete orientation values trying the generation from the cells nearer to the origin and increasing the distance at each iteration When a solution is found with this model the script ends finding one primitive The syntax to launch it is genminprimsvpos sh t_i t_f where t_i is the initial orientation of the vehicle t_f is the final orientation of the vehicle At the end of the
144. ornering stiffness expressed in Nm a front tyres drift angle expressed in rad a rear tyres drift angle expressed in rad I yaw moment of inertia expressed in Kg m terrain slope angle expressed in rad 4 3 Ackermann vehicle 85 Suspended mass Figure 4 8 Vehicle representation with its suspended mass m total mass of the vehicle expressed in Kg v linear velocity expressed in m s steering angle expressed in rad We can take into account also the suspended mass of the model where for suspended mass we want to indicate all the components to a different quote with respect to the suspensions An example of suspended mass model is reported in Figure 4 8 Analyzing the forces acting on the vehicle with this model we can extract the differential equation 4 9 a g siny 4 9 1 kp Dp Ms h where w yaw angle expressed in rad 6 drift angle referring to the vehicle center of gravity expressed in rad roll angle of the suspended mass expressed in rad N rad k rolling rigidity expressed in b rolling damping factor expressed in wats y terrain slope angle expressed in rad sum between terrain slope angle and roll angle of the suspended mass ex pressed in rad m suspended mass of the vehicle expressed in Kg 86 Chapter 4 Vehicle Models and Lattice Primitives h suspended height expressed in m v linear velocity expressed in m s g gravity acceleration exp
145. ot is composed by two driven wheels each wheel is commanded by a motor one for each side of the robot The two wheels are independent therefore the robot can change its orientation on the spot commanding different rotation velocity of the two wheels In particular if the two wheels have the same velocity in the same direction the robot go straight else if the wheels have the same velocity but in the opposite direction the robot perform a turn in place maneuver All the other combinations of the two wheels velocity result in a curvilinear trajectory depending on the combinations between the two velocities The center of rotation of a differential drive vehicle can be anywhere on the axis formed by the wheels contact points If the robot go straight the center of rotation is located to an infinite distance from the robot meanwhile if the vehicle perform a turn in place maneuver the center of rotation is in the middle between the wheels contact points Sometimes to increase the stability of the differential drive vehicles additional wheels or casters are added to the robot 4 4 2 Differential drive model and primitives creation First of all we can analyze the kinematic model of a differential drive vehicle shown in Equation 4 14 It respects the frames reported in Figure 4 9 de a GE V cosh d 3 v sin 4 14 d _ di Y In the model we have e States x Cartesian position of the vehicle along the x axis expressed in m y C
146. over the resolution used how wide is a cell in meters 3 1 Introduction 63 0 the orientation of the robot taken counterclockwise and discretized as integer value pure number a continuous 0 rad orientation indicate that the robot is oriented toward the x axis v the linear velocity of the vehicle expressed in m s discretized as integer value the steering angle of the vehicle expressed in rad discretized as integer value In this way is possible to grant very good continuity of the states of an Ackermann vehicle between the arriving in a state and the leaving from that state Adding the steering variable in the state during the plan allows to take into account also the steering of the vehicle and this can be used to avoid parts of the trajectory with rapid unfeasible changes of steer Thinking about the discretization is necessary to consider that an Ackermann vehicle steering angle has two limit values one that limits the right steer and one that limits the left steer It is possible to assume that the two limit values are the same A reasonable idea is to consider an equal division between the two limits and the division could be done in five values that corresponds to go straight little steer left and right and big steer left and right In this way we have enough intervals to grant the continuity of the motions between two motions passing in a state Therefore the discretization of the steer can be done simply taking the inter
147. ox function x y h hm xm ym generate_terrain n mesh_size hO rO rr that takes as input the following parameters n number of iterations of the algorithm used by the toolbox a number of itera tions over 7 brings some advantages that are not notable but an increment of 1 iteration leads to increase time increment of about 3 seconds so 7 can be a good compromise mesh_size size of the mesh given in output it is a square mesh of mesh_size x mesh_size hO initial height rO initial roughness how much the terrain can vary in a step rr roughness roughness how much the roughness can vary in a step and after computation the function get back some vectors x y h vectors of points comprising terrain xm ym hm meshes over landscape Using this function we can generate a terrain and visualize it in Matlab using the surf function as the one shown in Figure 5 6 Using the function c generate_terrain_colors hm of the Automatic Terrain Generation tool it is also possible to generate a color profile for the map created displaying in the surf function a colored map that simulates a real terrain map e g sea mountains etc as in Figure 5 7 In the previous function the input parameter is the the toolbox use an iterative algorithm to creates the terrain map and the number of iterations can be specified by the user increasing the number of iterations the result is better however the toolbox creator suggests a value of 7 itera
148. p env_examples mot prims this parameter specifies the motion primitives file and despite in the original test file this file is not necessary for our scope is mandatory specify a motion primitive file In the folder sbp myprimitives there are some lattice primitive samples Other primitives file are into the folder sbpl matlab mprim If we want a complete list of environments and planning algorithm to use with this executable we can show the help and all options are listed The executable save the solution in a file called sol txt Usage xythetav_test This program allow to test the environment with four state variables that we have developed The syntax to launch it is xythetav_test s m planner lt planner_t gt search dir lt search_t gt lt cfg file gt lt mot prims gt xythetav_test h where h is the flag to show the help s is the flag to simulate a robot navigation in completely unknown environment cells occupation is discovered moving the robot B 3 SBPL our modified planning library 165 m is the flag to execute manually managed iterations of planning publishing every suboptimal solution found using the changes done in adstar The only planning algorithm usable with this flag is adstar up to now planner allow to specify the planning algorithm to use In our case we are interested in one of adstar arastar or anastardouble default anastar does not work search dir allow to specify if the
149. particular state of the robot state space A lattice primitive is a motion primitive that lead exactly from a state of the state lattice to another state in the state lattice So if the robot executes an action in the lattice primitives set it cannot be in a state not contained in the lattice To obtain a set of lattice primitives there are some different ways but essentially there are two kind of methods and both use the translational property of the state lattice i e find in all primitives starting in x y position 0 0 and replicate it in every position of the lattice 7 1 Forward for certain systems there are methods to sample the control space set containing all motions so the resulting after sample process is a state lattice In this case first of all is generated a set of motion primitives and after is transformed in lattice primitives 2 Inverse first of all the state sampling method is chosen and after is solved a BVP Boundary Value Problem in order to find a feasible motion that lead from state to another state within the lattice The first of the previous approach is not always applicable and it is unsafe because the resultant lattice primitives set could be incomplete or incorrect The second approach is more suitable because the resultant set is surely a valid state lattice even though dependently on the generation program used it can be redundant or too big in this case some measure can be taken The disadvantag
150. patible library in particular the library extended is SBPL inserting in it the possibility to consider more vehicle features than those considered in its current version At the end of the work we obtained a planner usable with an ATV vehicle which reduces the risk of overturn and it can take into account many sources of information during planning In ROS there are many methods to create plans two of the main libraries available are SBPL Search Based Planning Library and OMPL Open Motion Planning Library These two libraries allow to make plans for various kind of robot such as robotic arms autonomous vehicles and every other robot which allows a state representation in these libraries Moreover SBPL and OMPL are open source so if something is missing everyone can add its own code to implement desirable features The main difference between the libraries is the type of algorithm used to plan SBPL uses search based algorithms whereas OMPL uses random sampling algorithms These categories of algorithm face the problem of planning in a slightly different matter search based planning algorithms build a graph of states linked together and then they search an optimal solution of the problem in the graph built random sampling algorithms sample some possible states of the robot and they connect those found to obtain a possible solution Every family of planning algorithms has its own advantages and disadvan tages in particular search based pl
151. pproach starts from the origin cells and for each pair of start and end state variable values tries to find a destination cell for which the problem is solved The destination cells are taken in order of distance from the origin This ap proach have again some limitations in fact some primitives instead of others more desirable could be created because the distance from the origin is equal for at least four cells therefore there could be another cell instead of the one chosen that allows a better solution of the BVP shorter path or faster path This problem is partially solved taking some assumptions during the primitive generation analyzing the vehicle type for which primitives are generated and the state variable that we take into account 4 3 Ackermann vehicle The main vehicle for which this planner is developed is an ATV in particular a Yamaha Grizzly 700 transformed in autonomous vehicle within the project 2Optimal Control Problem 4 3 Ackermann vehicle 73 Figure 4 1 Sample of ackermann steering geometry highlighting center point of radii and wheel axles quadrivio However more generally we could target it as an Ackermann ve hicle An Ackermann vehicle has the peculiarity of the steering geometry that is called Ackermann steering geometry This geometry was invented by Georg Lankensperger and was patented by Rudolph Ackermann 31 32 This inven tion was created to avoid the slip sideways of the tyres when a curv
152. quests costs requests heuristic Figure 2 1 Communication between planners and environments is denoted with 7 Sstart This path is a sequence of states s 51 Sk such that so Sstart Sk Sgoal and for each 1 lt i lt k si succ si 1 The cost of the path is the sum of the costs of the corresponding transitions DE c 84 1 8 For any pair of states s s S we denote c s s as the cost of the least cost path from s to s For s s we define c s 5 0 The goal of a search based planning algorithm is the research of a path from Sstart tO Sgoal Whose cost is minimal i e equal to c Sstart Sgoal Supposing for every state s S to know the cost of a least cost path from sstart to s that is c Sstart S we use g s to denote this cost 2 3 3 SBPL SBPL stands for Search Based Planning Library and is composed by a set of domain independent graph search algorithms and a set of environments plan ning problems that represents the problem as a graph search problem The library is designed to allow the use of the same graph searches to solve a variety of environments graph searches types and environments types are independent each other Finally SBPL is a standalone library that can be used with or without ROS and under Linux or Windows Many details of SBPL can be found in 3 in the official SBPL site and an example of utilization of the method exploited by this library can be fou
153. re AND v was visited by AD before then bp v argmin v s c s v s Epred v g v v bp v c bp v 0 UpdateSetMembership v end if end for if significant edge cost changes were observed then increase e or replan from scratch else if e gt 1 then decrease e end if move states from INCONS into OPEN update the priorities for all s in OPEN according to key s CLOSED end loop end function 2 5 State Lattice 49 a Example of AD algorithm execution part 1 2 50 Chapter 2 State of Art mr mT i Tp ETA b Example of AD algorithm execution part 2 2 Figure 2 13 Example of AD algorithm execution 2 5 State Lattice 51 state C without consider cartesian pos
154. re m is the angular coefficient of the straight line and is also equals to tan 0 Combining the two things if we want a straight line for each admissible orientation of the robot 0 must be chosen in a way that y tan 0 x where x y are the coordinates of the cell to have the measure in meters they must be multiplied for the resolution Since we would x and y as integer values for the lattice to be defined 9 must respect the relation 0 arctan with y and x possible integer values In our work we have chosen as orientations 0 k 3 arctan 1 3 k 7 arctan 1 k 3 and arctan 3 k 5 With this non uniform discretization for every couple of start and end angles we can find a straight trajectory that joins two states with the same orientation in some cell In Figure 3 1 is possible to see the different orientations with uniform dis cretization and with arctan discretization Moreover it is possible to see why with the uniform discretization it is not possible to find always straight lines between two states with the same 0 value Considering that the crosses of the background grid are the centers of the lattice cells in Figure 3 1 a some straight lines pass near them but do not intersect them instead in Figure 3 1 b every straight line intersects a center of a lattice cell Another important aspect to consider when designing an environment is the cost function to use We are writing about trajectory planning problems so one o
155. re they can assume values depen dently on the map size and on the resolution of the map Instead the 0 angle can be discretized in different ways First of all it is possible to decide how many values of orientation the vehicle can take Given that the number of values that orientation can assume should be a multiple of 4 to have a good equal division in all directions a good compromise could be 16 values because with 8 values the vehicle has too few angles in order to represent a state in a good plan and with 32 values the additional computational effort introduced is not really improving the goodness of the solution Once the number of values to use in discretizing is chosen it is important to decide how to make such discretization A first possible idea is to execute a uniform discretization starting from the first discrete value that corresponds to 0 radians each discrete value corresponds to an increase of 2a of the angle where 3 1 Introduction 57 N is the number of available discrete values This approach has a little problem when the vehicle starts with an orientation and arrives with the same orientation because it should follow a straight line but with respect to the lattice definition it should end in the center of a cell A straight line oriented with uniform discretization value could not pass in a center of a cell Figure 3 1 a To solve this issue we know that a straight line must respect the equation y m x whe
156. reach the goal state f s g s h s In these algorithms when a state is processed its possible destination states are found The term used to indicate this process is expanded For some algorithms another cost function is very important v s It is the best path cost to reach the state s from the initial state at the moment of the expansion of s As we have already introduced search based planning algorithms use a graph data structure Afterwards we use S to denote the finite set of states in the graph succ s to denote the set of the successors of a state s S and pred s to denote the set of predecessors of the state s For any pair of states s s S such that s succ s we require the cost of transitioning from s to s to be positive 0 lt c s s lt co The start state and goal state are indicated respectively as Sstart and Sgoal and a path from Sstart tO Sgoal 1The cost estimated by the heuristic function must be less or equal to the real cost to reach final state 2The cost estimated from a state to each other must be less or equals than real cost 3All the states reachable from a state doing available actions All the states that can reach a state doing available actions 24 Chapter 2 State of Art Mapping between IDs and States predecessors sucessors costs heuristics antenne n i constructed on the fly Graph search algorithm Plan done as sequence of state IDs requests IDs re
157. reachable by the vehicle maybe isolated points We can consider also the number of optimal solution found in time by the planner These results are reported in Table 5 2 From the table we can see how with the increasing of the number of state variables the optimal solutions found in time are less In fact using the environment with only three state variable all the solution found have reached the optimality instead in the other cases only a part of the solutions found have reached the optimality However the number of optimal solutions found with four state variable environment is near to the number of solutions found From Table 5 2 we can also see that ARA and AD are comparable and find both a good number of optimal solution Instead ANA finds less optimal solutions than the previous two In Table 5 3 we can see the average cost of the solutions found by the planner In this case the costs correspond to the time in milliseconds multiplied for 103 in the table for a better comprehension needed to travel the path found We can see immediately how the costs of the three variables environment are greater than the others This happens because the three variables environment does not take into account of the exact velocity during the computation but use an average velocity without considers feasibility of certain maneuvers to a 1500s as reported in test setup 5 3 Planner Evaluation 119 Table 5 3 Average of the costs divided by 10
158. reasing inflation factor rise the number of states expanded An example of this case is a 2D navigation with Euclidean distance used as heuristic and a presence of U shaped obstacles placed between robot and its goal In this case the robot can fall in a minima during the obstacle avoiding and unnecessary states are expanded until it has not exited from the local minima Many details on Weighted A algorithm are analyzed in 10 In Figure 2 9 is reported the result of an execution of Weighted A with an inflation factor of 3 The image shows that the solution is not the optimal solution found before from A algorithm but the state expanded compared to A are less This leads to faster executions and is more suitable for complex environments Instead Figure 2 10 shows an execution of Weighted A with equals to 1 and is possible to see that the result is the same of A 2 4 Main Search Based Planning Algorithms 37 Figure 2 10 Example of Weighted A with e 1 2 4 3 ARA A search can be re formulated to reuse the search results of its previous execu tions To do this we define the notion of inconsistent state and then formulate A search as the repeated expansion of inconsistent states Moreover will be also generalized the priority function that A uses to any function satisfying certain restrictions This generalizations allow to define the ARA algorithm Starting from the simple A we introduce a new variable v s
159. rect vehicle model Once the primitives are created they can be used in different environ ments and an environment can use many types of primitives For example if we have a turnable vehicle and an Ackermann vehicle we could use the previous explained environment with four state variables changing only the primitives 74 Chapter 4 Vehicle Models and Lattice Primitives used 4 3 1 Ackermann kinematic model and x y 0 primitives A first kind of primitives generated are those that take into account the kine matic model of the vehicle The kinematic model of the vehicle is reported in Equation 4 1 t v cos 6 y v sin 0 4 1 j tan d 12000 The model expressed by Equation 4 1 uses the frames shown in Figure 4 2 and is composed by three differential equations having as e States x Cartesian position of the vehicle along the x axis expressed in m y Cartesian position of the vehicle along the y axis expressed in m 0 orientation of the vehicle expressed in rad is the angle formed by the robot longitudinal axis and x axis positive counterclockwise e Controls v linear velocity expressed in m s steering angle expressed in rad e Parameter T time to make a maneuver expressed in s e Vehicle dependent constant L length of the vehicle from rear axis to front axis expressed in m Moreover we can indicate with k en the trajectory curvature The main advantage of this model is the simplicity of
160. ressed in m s We can join the previous equations with the model used in the previous section obtaining the complete model reported in Equation 4 10 da GF v cos 0 dy q v sin 6 de tan d di L dv _ I 1 da Ty UT Ta Y d 1 1 di T as T d b C a Ce b Cr a C a C F po WA ams a dB _ Cp Cr Cr b Cf a m v Al Cy 9 6 dt m v BA mv Y mv u SLY dp _ di TP dp _ m 2 2 m Cp Cr Cr b Cf a T e etate y t bt Se Cr kr p br p m h ms h 4 10 This model is composed by the previous state equations and other four equa tions In particular in this model we have States x Cartesian position of the vehicle along x axis expressed in m y Cartesian position of the vehicle along y axis expressed in m 0 orientation of the vehicle expressed in rad is the angle formed by the robot longitudinal axis and the x axis positive counterclockwise v linear velocity expressed in m s steering angle expressed in rad w yaw angular velocity expressed in rad s B drift angle referring to the vehicle center of gravity expressed in rad y roll angle of the suspended mass expressed in rad roll angular velocity of the suspended mass expressed in rad s Controls u desired velocity expressed in m s us desired steering angle expressed in rad y terrain slope angle expressed in rad 4 3 Ackermann vehicle 87 4 terrain slope angular velocity exp
161. ressed in rad s Parameters T time to make a maneuver expressed in s Vehicle dependent constants L length of the vehicle from rear axis to front axis expressed in m T linear velocity time constant expressed in s the vehicle reach the 99 of the desired velocity in about 5 Ty Ts steering angle time constant expressed in s the vehicle reach the 99 of the desired steering angle in about 5 Ts m total mass of the vehicle expressed in Kg ms suspended mass expressed in Kg h suspended height expressed in m k rolling rigidity expressed in Nm b rolling damping factor expressed in am a distance from front axial and center of mass expressed in m b distance from rear axial and center of mass expressed in m C front tyres cornering stiffness expressed in Nm C rear tyres cornering stiffness expressed in Nm I yaw moment of inertia expressed in Kg m g gravity acceleration expressed in m s To use this model it is important to make some hypothesis in particular velocity referred to the center of mass is approximated equal to the velocity of the rear axis we are in presence of small angles there is some inclination y of the street also 0 rad Y KP Using the previous model we have all elements to analyze the normal forces that act on the left wheels of the vehicle and on the right wheels of the vehicle The contact between the terrain and the wheels create the normal forces so when a
162. rmations named environment to make plan Chapter 4 the fourth chapter reports the vehicle model used to generate the motion primitives the precaution taken during the generation process for the various vehicle type and the various way in which is possible to generate the motions primitives Chapter 5 the fifth chapter reports the experimental phase of the work in particular considering the developed softwares the tools used and tests done Chapter 6 the sixth chapter presents the integration of the planner with the ROS middleware in order to use it within the quadrivio project 20 Chapter 1 Introduction Chapter 7 the final chapter presents the conclusion of our work and future directions of development The thesis includes also two appendices Appendix A this appendix reports some tables with the tests results Appendix B this appendix reports a manual on how use the developed software Appendix C this appendix reports a table with some parameters of the ATV Chapter 2 State of Art Come in close because the more you think you see the easier it ll be to fool you Now You See Me 2 1 Introduction This chapter shows the state of art starting from some historical notes After wards we introduce some important definitions and the two main families of planning algorithm showing also the two libraries that use them Furthermore the main search based planning algorithms are listed and explained and in the last part of t
163. rogram launch and the deleting of occupancy map file created is also possible create immediately the occupancy map and the environments configuration templates more than images for each DEM The syntax of the executables is convertAll sh startNumber endNumber convertAllSlope sh startNumber endNumber where startNumber and endNumber are the interval of numbers contained in the DEM file name to convert 152 Appendix B User manual B 2 Lattice Primitives In this folder are provided many pieces of code to generate the primitives Below are explained only the main program created and the more developed hence the file more suitable for an immediate use but if someone would see also the other source files they are leaved as example B 2 1 ACADO set up Many of these kind of primitives was generated with ACADO so first of all is necessary download it and follow the instruction of building present on the website After is necessary execute the instruction source lt ACADO_ROOT gt build acado_env sh or append the previous line at the end of the bashrc file and restart the terminal or source the bashrc file At this point is possible to compile all the programs that use ACADO B 2 2 Bocop set up Other primitives was generated using Bocop solver so it must be downloaded from the website and install it following the instructions reported on the website Once the program was installed pick the file main cpp from the folder Bo
164. ronments the suboptimality increase a little but the values are acceptable anyway Also in this case ANA search algorithm have worst performance than the others The Table 5 5 reports the average time in seconds used by the planner to find the best solution if a solution exists This table is full of meaning indeed we can see how the time are higher for the ANA search algorithm with respect to others search algorithms and we can see also how the time needed to find the best solution found is lower for the three state environment variables with respect to the other environments considering the best solution found not the first solution found This environment difference depends by the increasing of the state variables and by the great size of the map 120 Chapter 5 Experimental tests Table 5 5 Average of the time in seconds used to find the best solution found when is found in the 100 problems of the first test set XYO XYOV XYOV_LLT XYO Vd XYOV _LLT ARA 29 27 510 41 564 62 1196 17 914 31 ANA 152 10 687 60 1179 14 1262 05 1203 53 AD 29 12 483 55 688 28 1197 79 941 43 Table 5 6 This table shows how many times a given planner found a solution that cost less lower suboptimality in front of the other planners also equal are accepted only for ARA and AD due to the algorithm similarity on the 100 problems of the first test set when a solution is found XY XYOV XYOV_LLT XYOVd XYOV6_LLT ARA 0 7 50 38 34 A
165. s we now also need to update their backpointers In fact if a backpointer is set first then a g value is just set based on the new state the backpointer points to The optimization is that in case of an underconsistent state expansion the re evaluation of a g value is now only done for the state whose backpointer points to the state being expanded In addition a greedy path and hence the solution can be reconstructed in a backward fashion by just following backpointers from Sgoal tO Sstart We will refer to the path reconstructed in this way as the path defined by backpointers Now combining the anytime property of ARA and the incremental prop erty of LPA we obtain a single anytime incremental search algorithm AD Anytime Dynamic A AD can plan under time constraints just like ARA but is also able to reuse previous planning efforts in dynamic domains Both ARA and LPA reuse their previous search efforts when executing the ComputePath function The difference is that before each call to the ComputePath function ARA changes the suboptimality bound e while LPA 2 4 Main Search Based Planning Algorithms 45 Algorithm 7 LP A ComputePath function 1 for any two states s s S such that c 8 Sgoal lt 00 v s gt g 8 v s gt g s and g s gt g s c s s it must hold that key s gt key s 2 for any two states s s S such that c 8 Sgoal lt 00 v s gt g
166. s from a configuration file as write in Chapter 6 and it should be loaded when the node starts A sample of configuration file is putted in the node directory This node read from a topic map the occupancy map from a topic goal the goal pose and from a topic roamfree odometry the start state Once it have all the previous informations it starts to make plan in according on the features explained in Chapter 6 The goal and start pose can be given with some other ROS node for example the goal can be given with RViz the start with ROAMFREE or another node that give an Odometry object Finally the node publish two topic each time it found a solution One is named path its type is PathWithVelocity and contain the path composed by poses with a velocity associated to every pose The other is called testPath and its type is Path and does not contain a velocity profile This second topic published is useful to see the trajectory found in RViz together the map on which the planning is done Appendix C Vehicle parameters This appendix in Table C 1 reports some ATV parameters used in the Ackermann vehicle models 170 Appendix C Vehicle parameters Table C 1 Parameters of the ATV Parameter Symbol used Value Measurement unit Linear velocity time constant da 0 7 S Steering angle time constant Ts 0 13 S Vehicle length L 1 25 m Vehicle width W 0 96 m Steer angle interval Oa XF rad Total mass of the vehicle m 422 0 Kg Suspended m
167. s of another can decrease Therefore find a good compromise is important Use a state lattice to do searches have many advantages in fact searches can be done with search algorithms as AD 4 that take into account dynamic changes of the lattice so if during navigation some obstacles appear in front of the robot it can change the lattice and replan the path rapidly the fast change of the trajectory during vehicle movement is a critical task of autonomous vehi cles 24 Moreover the long computation due to motion creation can be done 52 Chapter 2 State of Art out of the planning phase The motion created can be used in many planning problems and the cost of each motion during planning is simple to assign taking into account that the motions and their costs are always the same for each pair of states with the same state variables value without consider position Finally the lattice is created sampling in the state space so only the state vari ables which are important for a problem are chosen for a particular planning problem However the state lattice has also some problems indeed it is slightly memory avid and adding a possible values of the state variables or adding a state variable the cardinality of the search space the memory used and the possible combination of connected state grow rapidly leading to a slower search To make planning with state lattice is important make some choices during the sample phase because it is
168. s simple to read and to create the correspond ing cost map With a simple C program we have read the DEM available on the portal and we can export various files e A text file containing the cost map as a matrix of number each number is in the interval 0 1 where 0 stands for free cell and 1 stands for obstacle e A PNG gray scale image representing the cost map using libPNG each pixel of the image corresponds to a cell of the DEM a white pixel is a free space and a black pixel is an obstacle e A configuration file template for the various environments containing the dimensions of the map all the specific parameters of the environments depending on environment type and the cost map Starting from the DEMs we can create the cost map in two different ways In the first we create a cost map cutting the DEM to a certain height and assigning to a cell a value corresponding to obstacle or free space In the second we analyze the slopes between the cells and assign a cost depending 3http www sardegnageoportale it http www sardegnageoportale it index php xsI 1594 amp s 40 amp v 9 amp c 8759 amp n 10 5 3 Planner Evaluation 109 on the maximum slope as explained in Chapter 3 We have used a slope limit of 7 rad and 10 interval of costs so every 75 rad the cost was increased of 0 1 and the costs value goes between 0 and 1 When the image is exported in the first case height thresholding it looks like a black a
169. sable in the planner and four other files containing the continuous values one for each start orientation value quadrant in order to plot the primitives B 2 Lattice Primitives 155 Usage negative_velocities_generation This executable takes in input the file generated by rotate_all_motions program and give in output the old primitives file more all primitives previously generated if the velocity take into account would be negative instead of positive making a transformation of the trajectories The syntax is negative_velocities_generation primitives_file B 2 4 Lattice Primitives considering Actuators Dynamics In this subsection are explained the software developed using the model that introduces the actuators dynamics This kind of primitives are generated both for the environment that takes into account four state variables during planning phase and the environment that takes into account five state variables during planning phase Due to the similarity of the executables are explained the procedure in parallel for both the program types developed Build Starting from the root of the software get from svn the folder in which we are interested in are MotionPrimitives 1030_theta_not_unif and MotionPrimi tives 1205 intro steer In this two folders is necessary copy the FindACADO cmake file as explained before After in both folders execute the following commands mkdir build cd build cmake make ed Now in the first
170. siders only the actuators dynamics 5 2 BVP Solvers 103 Primitives 12 10 8 6 4 2 0 2 4 6 8 10 12 x Figure 5 4 Example of primitives taking into account the four state variables actua tors dynamics and LLT value paying attention that the resolution is 1 meter but for graphical reasons on the graph is represented a cell every 2 meters so the primitives that finish in the middle of a cell are anyway valid 104 Chapter 5 Experimental tests 8 7 6 5 4 3 2 1 0 1 2 3 4 5 6 7 8 x Figure 5 5 Example of primitives taking into account the five state variables actuators dynamics and LLT value 5 3 Planner Evaluation 105 To solve a single OCP BOCOP is simple to use and is also effective and efficient enough however using this software directly it is slightly more difficult than ACADO Anyway in our experiment it was really useful because it allows us to create a set of safe primitives on plain terrains 5 3 Planner Evaluation 5 3 1 Experimental Setup Maps Evaluation We have used two different approaches to build the maps for tests In particular we have used a terrain generation toolbox for Matlab to try the planner on digitally generated terrains and we have interpreted DEMs from real terrains We have tried some terrains obtained with the Automatic Terrain Generation tool freely available on line This tool contains some Matlab scripts to create a terrain with some specification We have used the toolb
171. sing the value of the minimum speed Unfortunately the unique way to solve this problem was by increasing the speed value but the value at which the problem became solvable by using ACADO was about 0 7 m s that is too high to simulate a stopped vehicle Finally with this model considering all necessary constraints and LLT ACADO was really slow also when it was able to find a solution For all these reasons a try was done with another solver called BOCOP Despite the problems encountered during the primitives generation espe cially with the last model of the vehicle ACADO must be taken into account because it is simple to use in order to create personal software to solve optimal control problems Unfortunately it is slightly lacking from some points of view as write before 5 2 2 BOCOP an open source BVP solver BOCOP is an open source toolbox for Optimal Control Problems developed within the framework Inria Saclay and supported by the team Commands with support of industrial and academic partner BOCOP is freely available at http bocop org and is licensed with EPL BOCOP is given with a sim ple GUI that can be used in order to solve a BVP In that GUI we can set up a problem specifying e parameters and dimensions of the problem e variable names e functions e bounds e constant values e discretization method e number of time steps Eclipse Public License 102 Chapter 5 Experimental tests e starting values of the pr
172. sistence of the heuristic it is necessary to use the maximum admissible ve locity plus a little constant value to suppress numerical errors and be sure of the consistency In Equation 3 3 the heuristic function is reported max de s Sgoal d2D s Sgoal h 3 3 s as 3 3 dg Euclidean distance dop 2D navigation distance Umax Maximum velocity in absolute value k Small constant about 0 1 Umax Analyzing the environment we could write that the additional variable in crements the computational effort but it introduces also many advantages among which the consideration of a velocity profile associate to the trajectory Moreover in this way it is simpler to take into account some constraints of the vehicle such as dynamics of the actuators changes of velocity are not instanta neous and the trajectory planned is more accurate and easily followable by the follower 3 1 3 Environment with five state variables x y 0 v 6 This environment uses five state variables x the Cartesian position x of the robot in the map considering the origin in the lower left point of the map it is a pure number because it is discretized as the x position of the vehicle in meters over the resolution used how wide is a cell in meters y the Cartesian position y of the robot in the map considering the origin in the lower left point of the map it is a pure number because it is discretized as the x position of the vehicle in meters
173. so non li cito uno ad uno solo per evitare di omettere erroneamente qualche nome loro sanno chi sono non necessario citarli per le avventure vissute le uscite la compagnia tutto insomma dagli amici con cui ho condiviso il percorso di studi universitario a quelli con cui ho condiviso il divertimento delle uscite dagli amici che magari adesso vedo un po meno ma che hanno fatto parte della mia vita in questi anni agli amici che vedo ancora molto di frequente e che continuano ad essermi vicini mando un immenso grazie di cuore a tutti tutti coloro che hanno fatto parte della mia vita in questi anni e coloro che ancora ne fanno parte Infine come ultimi ma non ultimi in ordine di importanza ringrazio il mio relatore il Prof Matteo Matteucci per avermi aiutato nell impostazione del lavoro e per avermi dato delle dritte per procedere nel modo migliore e un ringraziamento al mio correlatore il Prof Luca Bascetta per avermi aiutato durante il lavoro soprattutto nella comprensione dei vari modelli dei veicoli e delle parti di impostazione dei problemi di controllo ottimo Nonostante la loro fitta agenda sono sempre stati disponibili a darmi delle indicazioni per portare a termine i compiti necessari e per vedere come procedeva l attivit coprendo un ruolo fondamentale nella realizzazione di questo lavoro Chapter 1 Introduction Trinity know why you re here Neo know what you ve been doing why you hardly sleep why you live
174. t L L L L L 0 200 400 600 800 1000 1200 1400 1600 Time s Figure 5 12 Cost of the solution found over the time need to found them for a solution of a first test set Figure 5 13 we report the number of expansions done by the search algorithms during a planning along the y axis and the time along the x axis This last graph is subdivided into Figure 5 13 a and Figure 5 13 b The second sub figure is a detail of the first sub figure to show the number of expansions of ARA and AD algorithm that in the Figure 5 13 a does not appear clearly In both the graph each point marked corresponds to a suboptimal solution found The last point corresponds to the best solution found These graphs are in according on the comments done on the tables Focusing on Figure 5 13 we can see that ANA make many more expansions than other two search algorithms and this is time and resource consuming From the data we can affirm that ARA and AD are the best search algo rithm However AD is also dynamic whereas ARA is only anytime For this reason the predefined search algorithm to use is AD Indeed the next set of test is done only with the AD search algorithm Finally we report in Figure 5 14 an example of the five solutions found in a problem of this set of tests In the image we highlight two parts of the solution to show how the solution with environment that take into account only three state variables make curvilinear
175. t set ARA ANA AD XYO XYOV XYOV_LLT XYOVO XYOV6_LLT 95 90 92 22 69 95 79 35 19 39 95 91 85 22 69 Table A 4 Number of solutions that not have reached the optimality when is found in 1500 seconds in the 100 problems of the first test set ARA ANA AD XYO XYOV XYOV_LLT XYOVO XYOV6_LLT 0 5 3 54 26 0 16 60 57 56 0 4 10 54 26 Table A 5 Average of the costs divided by 103 at the best solution found when is found in the 100 problems of the first test set XY XYOV XYOVLLT XYOVO XYOV6_LLT ARA 320 529 115 089 108 132 121 631 108 093 ANA 320 529 117 722 111 153 132 868 108 356 AD 320 529 115 089 108 278 121 631 108 093 145 Table A 6 Average of the suboptimality bound reached at the best solution found when is found in the 100 problems of the first test set XYO XYOV XYOV_LLT XYOV6O XYOV6_LLT ARA 1 00 1 01 1 01 1 17 1 05 ANA 1 00 1 04 1 07 1 25 1 13 AD 1 00 1 01 1 02 1 17 1 05 Table A 7 Average of the time in seconds used to find the best solution found when is found in the 100 problems of the first test set XYO XYOV XYOVLLT XYO Vd XYOV6 LLT ARA 29 27 510 41 564 62 1196 17 914 31 ANA 152 10 687 60 1179 14 1262 05 1203 53 AD 29 12 483 55 688 28 1197 79 941 43 Table A 8 This table shows how many times a given planner found a solution that cost less lower suboptimality in front of the other planners also equal are accepted only for ARA and AD due to th
176. te lattice planner with respect to differential constraints must move the robot between its states using lattice primitives The lattice primitives generated must be strictly correlated to the environment where they will be used For instance if we decide to plan in an environment with x y 9 as state variables it is not useful to have primitives with information about velocities On the other hand if the plan is done with the four state variable environment described before it is important to have velocity information in the primitives For these reasons the model used to generate each kind of primitives is slightly different and in some extended models it is possible to do some controls that in simplest model are impossible to do Moreover the motion generation process of the primitives must guarantee the connectivity of the lattice and the resolution used in the environment and in the primitives must be the same Primitives can be viewed as a tool that allows to connect all states of the lattice allowing to use at the best the environments described before 3 4 Considerations on the cost map The cost map has a great relevance on the cost function used during planning for this reason is necessary define how we construct it before start planning With the cost map we want represent the physical space where the vehicle must follow the trajectory planned therefore the maps must represent it in an accurate way basing the costs on the risks or on
177. terrain we have developed a method to create a cost map we take into account the slopes in the terrain using heights reported on a DEM Digital Elevation Model of the place where the plan is done Finally a particular cost function is used to consider both of the morphology of the terrain and the time to execute a plan found Estratto in lingua italiana Il processo di pianificazione molto importante nell ambito della Robotica e dell Intelligenza Artificiale Per pianificazione si intende la ricerca di un percorso da uno stato A a uno stato B Se un agente riesce a effettuare una pianificazione di alta qualit allora sar molto pi semplice effettuare il proprio lavoro La pianificazione di traiettoria un particolare processo di pianificazione che si occupa di costruire una traiettoria sotto forma di percorso attraversabile da un veicolo autonomo partendo da una configurazione iniziale Per generare un piano di alta qualit importante che si considerino tutti gli aspetti e i vincoli necessari affinch la traiettoria individuata sia effettivamente attuabile Se non fosse percorribile o comunque venisse costruita senza prendere in considerazione vincoli cinematici e dinamici del robot la pianificazione fatta potrebbe portare a collisioni o errori nella fase di esecuzione L obiettivo principale della tesi la creazione di un pianificatore utilizzabile su un ATV All Terrain Vehicle autonomo principalmente a scopo di missioni di esp
178. that the maximum rotation is 5 rad the vehicle cannot reach cells positioned at his back in one movement for this reason it is not useful to generate that primitives Moreover to have a set of minimal movements the final orientation has to be taken into account as well Indeed if the final orientation is incremented counterclockwise with respect to the start orientation the final cell is surely over the perpendicular of the final orientation straight line translated into the origin elsewhere is under that translated line An example to understand better this assumption is represented in Figure 4 4 Finally to generate a minimal set of primitives we can act on the BVP definition From the previous assumptions it follows that the vehicle do not 78 Chapter 4 Vehicle Models and Lattice Primitives ra Not composed i i 4 1 2 x Figure 4 3 Example to reach an orientation of 3 rad using primitives composition of too small primitives and directly Start Vv End F Figure 4 4 Example of areas where destination cannot be stripes area given start and end orientations considering the origin as starting point 4 3 Ackermann vehicle 79 X Bad R Better L Ll L i gt i i i i 0 1 2 3 0 1 2 3 x x Figure 4 5 Example of two primitives with the same start angle and goal angle but the first despite the end position nearer is worse to compose with other primitives and in terms of execution
179. the maximum time allowed to find a feasible path e the search direction flag true is forward false is backward e some parameters needed if the environment used is the three variables environment the nominal velocity the time to turn in place e some parameters if the environment used takes into account x y 0 v the number of linear velocities admitted the linear velocities admitted the number of steering angles if the environment take into account the steering angles the number of angular velocities admitted if the environment take into account angular velocities the angular velocities admitted if the environment take into account angular velocities This node subscribes to the odometry topic published by ROAMFREE to get the actual position and velocity of the vehicle to a topic that publishes the goal pose to know where the vehicle must go and to a topic that publishes a map to get the map of the zone in which it needs to navigate When the node receives the map it instantiates a new matrix to save it otherwise it checks its size If the current map size and the new map size does not correspond or some value into the cost map is changed it changes the map taking into account how many cells have changed their value Finally the node advertise a topic called path where it publishes the path with the velocity profile associated to each point for the follower 134 Chapter 6 Integration with R
180. the optimal solution to the planning problem considering a cost function to minimize Planning autonomous vehicles comprise two main components a global planner and a local planner These components are complementary in fact many times are used together The global planner creates a trajectory considering the map of the environment the constraints of the robot the risks in executing some trajectories and all others global informations 18 Chapter 1 Introduction A local planner also named trajectory follower decides the action to take locally considering the current state of the vehicle and generating the controls value to follow a trajectory decided by a global planner In particular the global planner generates a path to reach the goal state starting from the start state and this path is passed to the local planner that undertakes to follow the trajectory previously generated respecting admissible values of the controls of the autonomous vehicle In this work the main topic concerns the global planner often written simply as planner In particular the objective of this work is the creation of a planner that can take into account kinematic and dynamic constraints of an ATV to create feasible plans safe as much as possible in a reasonable computation time Some attentions are posed also on other vehicles for example what is necessary to do to create plans for a differential drive vehicle To reach these objectives we extended a ROS com
181. the robot This space represents all the possible configurations of a robot in the workspace A single point in the state space is a state The continuous state space is also called configuration space and often it is indicated as C Free State Space a subset of the state space in which each state corresponds to an obstacle free configuration of the robot Often it is indicated as C free Path a continuous mapping of states in the state space A path is collision free if each element of the path is an element of the free state space Configurations are often indicated with the letter q and the trees generated to find a path is indicated with T The last phase of random sample planning algorithms is the query phase and the goal of a sampling based motion planning query can be formalized as the task of finding a collision free path it should not be the minimum cost path in the state space of the robot from a distinct start state to a specific goal state utilizing a composition of paths that connects various configurations 2 3 5 OMPL OMPL stands for Open Motion Planning Library it provides an abstract repre sentation for all of the core concepts in motion planning including state space control space state validity sampling goal representations and planners OMPL is a counterpart of SBPL that uses random sample planners instead of search based planners 12 Furthermore OMPL could be integrated in ROS and it generates a correct output for a
182. the solution found The sub optimality bound for the solution found The number of expansions done in that iteration The indicator of the memory used saved as the dimension of the table that maps IDs to search states multiplied for the size of an integer In this way we can view the performance and the quality of the various search algorithms with the different environments The search algorithms in which we are interested in are ARA ANA and AD Since ANA search algorithm included in SBPL library had some problems before do all tests we have fixed It First set of tests setup We have made a first set of tests in order to evaluate the search algorithms and to have a more clear idea of what are the differences between the environments We have created a set of 100 random goals Only for primitives generated considering the LLT constraint there is an exception Indeed in this case we have set the goal steering angle to 0 rad due to the boundary problem formulation the final steering angle of the primitives is often 0 rad and to avoid a failure of many tests for the goal steering angle we have changed it We have executed these tests solving every planning problem for each pair of planning algorithm and environment using both the primitives generated with 5 3 Planner Evaluation 113 the model with actuator dynamics and with the extended model that considers LLT In particular we have used the following search algorithms e ARA
183. time change its orientation of more than 3 rad from the start value In fact if the vehicle changes its orientation more than 3 rad from its initial orientation it probably means that it goes around before reaching the destination At last considering always that the vehicle cannot turn more than 5 rad during a primitive another possibility to create better primitives is by inscribing the generated trajectory into a rectangle setting constraints on the x y co ordinates In fact without this expedient if we try to create the primitives in order of distance from the origin it can happen that the BVP is solved in a cell founding a long curvilinear solution meanwhile it can perform a straighter path going more distant of 1 cell An example to understand better what we think is reported in Figure 4 5 To face this problem one can find the vertexes of the rectangle where a primitive can be inscribed An example of the points found for a pair of start end coordinates is in Figure 4 6 To find the four points of the rectangle in which the primitive should be inscribed we can proceed in the following way Two of the four points are well known because are the origin and the end point of the trajectory Now indicating with d the euclidean dis tance between the two known points the diagonal of the rectangle with a the starting orientation of the robot angle that one rectangle side form with the x axis and with 6 the angle that the diagonal of the
184. tions 106 Chapter 5 Experimental tests Figure 5 6 Terrain generated and plotted with the surf Matlab function Figure 5 7 Terrain generated and plotted with realistic colors 5 3 Planner Evaluation 107 Figure 5 8 Example of map created with Automatic Terrain Generation The parts in black are obstacles meanwhile the white places compose the free space elevation map and the output parameter is the color profile From the map generated it is possible to take the height in various points and create a cost map using both the methods seen in Chapter 3 cutting to a determined height or considering the slopes These operations can be done directly from Matlab The advantages of this approach using Matlab are the simplicity and the velocity by which a terrain of desired size is generated however the terrain does not correspond to any existing terrain and if the parameters for the creation are wrong the terrain generated can be unrealistic An important aspect to take into account is represented by the values used the default ones are not expressed in a specific unit so it is really important to take care of the value for height thresholding or in case of slope computation to pay attention to the measure assumed for the cell distance and for the elevations In Figure 5 8 there is an example of map created with Automatic Terrain Generation The cost maps created with this toolbox are nice but for our tests we have not
185. tions found first test set A 2 Number of solutions not found first test set A 3 Number of optimal solutions found first test set A 4 Number of optimal solutions not found first test set A 5 Average cost first test Set A 6 Average suboptimality first test set A 7 Average time first test set A 8 Planner classification without equality first test set A 9 Planner classification first test set A 10 Number of solutions found second test set A 11 Number of solutions not found second test set A 12 Number of optimal solutions found second test set A 13 Number of optimal solutions not found second test set A 14 Average costs second test Set A 15 Average suboptimality second test set A 16 Average time second test set ooo C 1 Parameters of the ATVO 0 000 Abstract In many fields of Robotics and Artificial Intelligence path planning task is really important Path planning is the search of a path from a state A to a state B If an agent can make a high quality plan it will execute its work more easily Trajectory planning is a specific kind of path planning specialized in the construction of a trajectory travelable by an autonomous vehicle from a start configuration of the vehicle to a goal configuration of the vehicle To have a high quality plan it is important that the trajectory found is feasible
186. two states is decreased with respect to the original cost map and on the traversable cells there is no increments Once that the euclidean distance and the 2D navigation distance have been computed to take a value that does not underestimate too much the cost 60 Chapter 3 Environments IT ENI DAT EZANS EURO Figure 3 2 Actions doable in 2D navigation function the maximum value between the two is taken Finally to transform it in a time is possible divide it for an average velocity in this environment we do not have velocity informations so we take an arbitrary average velocity incremented by a small amount to avoid the overestimation of the costs in some rare particular cases The heuristic function can be written as reported in Equation 3 2 max dg s Sgoal dan s Sgoal h s L 32 dg Euclidean distance dop 2D navigation distance Vv Medium velocity k Small constant about 0 1 This environment can be reasonable for a vehicle that needs to move at low speed or at constant speed meanwhile if we need high speed maneuvers or we need to consider also velocity during planning or in our plan there are many changes of speed it is not so effective and we should take into account more state variables 3 1 2 Environment with four state variables x y 0 0 This environment has four state variables In particular it has the same previous three variables more one All variables are x the Cartesia
187. up 5 3 2 Experimental Results 6 Integration with ROS CI Intieduetione lt gt ago ail 6 2 Current system o 6 3 Planner node co s 4 oh a Re eae A 7 Conclusion and future works Bibliography A Data extracted from tests B User manual BL MAS ma s oe ee be ee ne eS GE ee B 1 1 Map generation o B 1 2 Map creation from DEM soore coe pac ase wsi B2 Lattice Pamitives o s rocca noa 4 oa e a B 2 1 ACADO set up 62 63 65 67 67 68 69 71 71 71 72 80 83 90 90 90 131 131 131 132 137 139 143 B22 Bocop Setup iii a a B 2 3 Lattice Primitives with Kinematic Model B 2 4 Lattice Primitives considering Actuators Dynamics B 2 5 Lattice Primitives considering LLT B 2 6 Differential Drive model primitives B 3 SBPL our modified planning library 2 2 B 4 ROS Nodes lt o e enea mad dae aa Pa Ra AUER R B 4 1 Usage trajectoryPlanner oaoa C Vehicle parameters List of Figures 2 1 22 2 3 2 4 2 5 25 2 7 2 9 2 9 2 10 ZII 2 12 2 13 I die SLA 3 4 4 1 4 2 4 3 4 4 4 5 4 6 4 7 4 8 4 9 4 10 Sl De 5 3 5 4 Communication between planners and environments Sample of expansion graph with motion primitives Hierarchy of existing Environment classes of sbpl Hierarchy of existing Planner classes of sbpl Relation between Environments and Planners Components of OMPL aaa a
188. used them We have not use them because the terrain generated is not equal to a real terrain and despite the accuracy of the parameters it is different from 108 Chapter 5 Experimental tests a real terrain e g some features repeat themselves in the terrain create a ter rain that represents cliffs is hard etc We have created the cost maps used in the tests starting from DEM from real terrains The DEM used for the experi ments can be found on the web portal SardegnaGeoportale The particularity of these DEM is the high horizontal resolution up to 1 meter Both DTM and DSM are available with the reference to which part of the region they are re ferred coordinates in the various DEM are provided in the WGS84 UTM32N reference system On the same web site if needed a coordinates converter is available The DEMs are available in simple ARC INFO ASCII GRID format representing a portion of terrain and the ASCII file contains NCOLS the number of columns of the matrix representing the elevation map NROWS the number of rows of the matrix representing the elevation map CELLSIZE the size of a cell of the matrix XLLCENTER x coordinate of the origin YLLCENTER y coordinate of the origin NOVALUE value representing a part of the terrain which is unknown Beside the previous values that compose the header the files contain a matrix of values Each cell of the matrix corresponds to an height From the previous file format i
189. val of steer and dividing it in smaller intervals The formula to compute the steering increment is reported in Equation 3 4 dinterval Ao nterva 34 esi 84 interval Steer ra nge N Number of discrete values including straight steer Cost function and heuristic remain the same of those in the 4 variables environment and all the considerations done for the previous environment are valid However we have the value of the steering angle so the primitives improve their continuity with respect to the previous case and the sum of the action costs is more precise Again with respect to the state lattice rules when the vehicle is in a state its state variables should have one of the values possible for the state this holds true also for the steering variable Considering this environment it is possible to obtain accurate trajectories but the computational effort and memory needs increase significantly the in crease of efforts is exponential with respect to the increasing of the number of state variables Since the planning could be really onerous it is necessary to see if the increment of quality in the result is worth compared to the increment of planning time and memory requirements 3 1 4 Environment with five state variables x y 6 v w This environment was developed for a differential drive vehicle Indeed it has five state variables adapted to a differential drive vehicle 64 Chapter 3 Environments x the Cartesian pos
190. very important choose a good state space rep resentation to avoid bad plan or bad start and goal states Once state variables are chosen a search in the state lattice is done considering also the obstacles in the environment states over obstacles are unreachable and using one of the previous search algorithm is possible to find the solution of the problem The time needed to find the solution is dependent from some factors cardinality of the lattice size of the map where planning is done number of state variables values assumed by the state variables and branching factor fanout number of admissible motions When a planning is executed a cost map can be overlaid to the lattice in this way is possible to consider the cost of an action on a particular map considering the cells crossed by the action The cost maps can be obtained in many way but an interesting approach could involve the use of DEMs 25 26 27 to build up a cost map exploiting the features of real terrains Planning using a state lattice means do a deterministic search so the advantages are more or less the same optimality with respect to a cost func tion and resolution completeness instead the main drawback is the exponential growth of complexity increasing the cardinality of the search space Using a state lattice Maxim Likhachev and Dave Ferguson have obtained amazing results in the DARPA Urban Challenge Competition 4 They have used a lattice with 4 variables
191. ware sviluppato e si considerano i test fatti per analizzare la qualit delle soluzioni trovate e le prestazioni raggiunte Nel Capitolo 6 viene spiegata l integrazione con l ambiente ROS quindi com stato creato e come funziona il nodo che utilizza il pianificatore mentre nel Capitolo 7 vengono riportate alcune conclusioni sul lavoro svolto e i possibili sviluppi futuri per aumentare le funzionalit di quanto fatto fino ad ora Acknowledgements Nonostante la stesura della tesi in lingua inglese trovo pi corretto scrivere la sezione dei ringraziamenti in italiano Lo trovo pi corretto perch la mia lingua madre la lingua con cui ho comunicato con la maggior parte delle persone che mi sono state vicine in questo percorso universitario le stesse persone che voglio ringraziare in questa sezione Da dove iniziare Sicuramente da casa un ringraziamento grandissimo a mia madre Milena e a mio padre Marzio che durante tutti questi anni mi hanno supportato e soprattutto sopportato anche in periodi in cui ero decisamente intrattabile sostenendo sempre le mie scelte anche in momenti difficili e aiutandomi moralmente a superare le difficolt Un ringraziamento anche a tutti i parenti nonne zii cugini che non cito per evitare ingiustizie nel dimenticare qualcuno erroneamente che si sono interessati al proseguimento dei miei studi incoraggiandomi sempre Inoltre un grandissimo ringraziamento va a tutti i miei amici e anche in questo ca
192. wheel loss the contact with the terrain the normal force disappears So using the previous elements it is possible to compute the sum and the difference between the normal forces wheel terrain on left and right side of the vehicle These forces are computed with Equation 4 11 88 Chapter 4 Vehicle Models and Lattice Primitives Fri Frag Ms 9 14 9 7 9 h p 9 m v Ce C Cy b Cr a Cy E i m m v C l d p g u 4 8 7 4 11a m faut z VG Bea rm Fn s P y h wv 0 U r r i Pn Cioe pal Cr at m h m h v Cs kr pt bp b oy e aa 5 sin 2 aig hops Fm Fro 4 11b 2 In Equation 4 11 there are some new vehicle dependent constants W width of the vehicle from right wheel to left wheel expressed in m I roll moment of inertia expressed in Kg m I pitch moment of inertia expressed in Kg m Using the previous forces it is possible to compute the LLT index as reported in Equation 4 12 A ratio between the sum and difference of the normal forces constructs the LLT index This index assumes values between 1 and 1 and when is equal to 1 means that one of the vehicle s wheel has lost contact with the terrain Indeed if the index assumes 1 value it means that one of the two normal forces is equal to 0 and the ratio has at numerator and at denominator the same force in absolute value For our scopes it is important to keep LLT lt 0 8 to avoid vehicle s rol
193. y also converting it into an image and into an environment configuration file template The syntax of the executable is demToOccupancy srcFile dstFile I imgFile eXYT xytEnvDstTemplate eXYTV xytvEnvDstTemplate eXYTVP xytvpEnvDstTemplate where srcFile is the source DEM file dstFile is the destination file where the occupancy map is saved B 1 Maps 149 imgFile is the destination png file where the map is saved and black pixel are parts of map higher than threshold white pixel are the lower ones xytEnvDstTemplate is the destination file where a template of configuration file for an environment with x y 0 as state variables is saved with the cost map of the DEM converted xytvEnvDstTemplate is the destination file where a template of configuration file for an environment with x y 0 v as state variables is saved with the cost map of the DEM converted xytvpEnvDstTemplate is the destination file where a template of configuration file for an environment with x y 0 v 6 as state variables is saved with the cost map of the DEM converted If the height threshold positive for mountains and negative for sea would be changed before compile all programs is necessary modify the two values e HEIGHT_THRESHOLD e NEGATIVE _HEIGHT_THRESHOLD at the top of the source file defined with a define instruction Usage demToOccupancySlope The executable demToOccupancySlope allow to transform a DEM given in AR C INFO A
194. y and paste it into the root folder of the Automatic Terrain Generation Toolbox Now using Matlab moving into the root folder of Automatic Terrain Generation and executing the script in the file writing the file name in the Matlab shell is possible to generate a terrain To change some terrain parameters is possible change some values into the Matlab scripts for example the generated terrain size the roughness the initial height in according on what explained in Chapter 5 about Automatic Terrain Generation toolbox 148 Appendix B User manual B 1 2 Map creation from DEM Build First of all to use these softwares is needed a valid installation of png library After have installed that library from the root of the cloned folder using a shell move into Maps DEMDTM and copy into that position the file FindPNG cmake installed within png library its position is dependent on the library installation place and staying into Maps DEMDTM folder execute the following commands mkdir build cd build cmake make cd Now in the folder are available all the executable files e demToOccupancy e demToOccupancySlope e image ToOccupancy e occupancy ToRandomGoals e convertAll sh e convertAllSlope sh Usage demToOccupancy The executable demToOccupancy allow to transform a DEM given in AR C INFO ASCII GRID format as the DEM downloaded from SardegnaGeoportale into an occupancy map considering the heights and optionall
195. zed the space traveled by the vehicle de cos 0 sin 0 4 2 do tan 6 ds 7 L Using the kinematic model one can introduce some assumptions to increase performance and effectiveness of the generation process First of all we have not velocity as a state variable but it appears as a control in one model or disappear completely in another model Therefore we can consider that the velocity during a primitive cannot change its sign To change velocity sign the vehicle should compose two primitives So the considerations done afterwards are valid considering trajectories obtained moving the vehicle with a positive velocity All the primitives that involve negative velocities can be obtained from the ones generated with positive velocities Now the primitives can be created for an initial value of 9 between 0 rad and 5 rad Then they can be rotated of 5 rad t rad and 3 m rad In fact a primitive remains valid also if it is rotated of 5 rad despite there could be doubts about the validity of the trajectory created and about state lattice condition start and end in the center of a cell with exact discretized value To demonstrate that rotating a lattice primitive of 3 rad one obtains a valid lattice primitive we can take a primitive that starts in 0 0 0 and ends in x yy 0 A rotation of an angle a of a reference frame is represented in the relation reported in Equations 4 3 i X cos a Y sin a 4 3 y X s

On the Development of a Search-Based Trajectory

Contents

Download Pdf Manuals

Related Search

Related Contents