1

Important Notice: This is the authors’ pre-publication version. This paper does not include changes and revisions arising from the peer review and publishing processes. The final definitive copy, which should be used for all referencing, is published at: http://www.sciencedirect.com/science/journal/15375110

2

Randomised kinodynamic motion planning for an autonomous vehicle in semi-structured agricultural areas

Mohamed Elbanhawi*, Milan Simic School of Aerospace, Mechanical and Manufacturing Engineering, RMIT University, Bundoora, VIC 3083, Australia

Abstract A randomised motion planner is presented that operates within a suitable timeframe for constrained mobile robots in agricultural environment. The core of this approach relies on splitting planning into two efficient phases to reduce its computational time. The effectiveness of sampling based planners is combined with the robustness of parametric vector-valued splines. The first phase involves relaxed two-dimensional path planning using rapidly-exploring random trees (RRT). Recent advances in sampling based planning are leveraged to improve the performance of the planner. Detailed implementation of the RRT approach and parameter selection are highlighted using comprehensive analysis and simulations. Feasible continuous paths with bounded curvature for nonholonomic robots are generated using B-spline curves. Curve segment parameters are formulated with respect to vehicle specifications. Manoeuvres satisfying maximum curvature constraints and path continuity are designed based on the segment parameters. Numerical experiments are used to validate the practicality of the proposed two-phase planner in solving kinodynamic motion queries, in real-time and replanning under limited sensing conditions.

Research Highlights •  Two-­‐phase  method  to  motion  planning  for  vehicles  in  agricultural  areas  is  proposed. • Parameter effects on randomized planners are analyzed to select appropriate values. • Vehicle constraints are formulated in terms of B-spline curve parameters. • Heading maneuvers are continuous and account for vehicle kinematics. • Planning algorithm is capable of running at 25Hz.

Nomenclature Cfree Free configuration space Cobs Obstacle configuration space q Vehicle configuration (pose) q.rand Random configuration q.new New configuration q.failure Node failure rate d Extension step distance x,y Vehicle position coordinates θ Vehicle heading r Vehicle turning radius w Vehicle wheel base δ Vehicle steering angle v Vehicle velocity vf Maximum forward vehicle velocity

* Corresponding author. Email: mohamed.elbenhawi@rmit.edu.au (M. Elbanhawi)

3

vr Maximum reverse vehicle velocity a Vehicle acceleration ω Vehicle angular velocity d(i,j) RRT node distance metric between nodes (i) and (j) h(i,j) RRT node heading metric between nodes (i) and (j) m(i,j) RRT metric between nodes (i) and (j) gd RRT distance metric weight gh RRT heading metric weight gf RRT node failure rate metric weight n B-spline number of control point p B-spline curve degree m B-spline number of knots 𝒖 B-spline knot vector u B-spline parametric length  k B-spline curvature Kmax Maximum B-spline Curvature u B-spline normalized length parameter N(u) B-spline basis function Px, Py B-spline control polygon coordinates c(u) B-spline curve x(u) B-spline curve x-coordinates y(u) B-spline curve y-coordinates L B-spline segment length α B-spline segment angle Lmin B-spline minimum segment length αmin B-spline minimum segment angle Abbreviations RRT Rapidly-exploring random trees PRM Probabilistic roadmap method UAV Unmanned aerial vehicle MAV Micro aerial vehicle CAD Computer-aided design FCFS First come first serve CLOOK Circular Look SSTF Shortest seek time first C-space Configuration space RC-RRT Resolution complete rapidly-exploring random tree ERRT Execution extended rapidly-exploring random tree

1. Introduction Autonomous robots are capable of performing tasks independent of human intervention. They have properties beneficial for a broad range of applications. Contemporary developments in computing, sensor technology and electronics, have led to widespread robots use in military, industrial and commercial applications. Self-driving passenger cars, fork lifts, medical robots, mining trucks, domestic robots, planetary rovers and rescue robots are amongst the existing applications of autonomous ground vehicles. The agility, size and cost-effectiveness of Micro Aerial Vehicles (MAV) have drawn interest

4

towards their use for commercial purposes such as monitoring, aerial photography, document delivery, mapping and exploration in addition to their military uses. As in the case of any novel technology, autonomous robots will be embraced by industry, as researchers develop more efficient, reliable, safe and intelligent robots.

There is much potential in agriculture for the application of autonomous systems / robots. The ever-growing global demand for food, together with climate change, poses challenges for the farming industry (Bennett, Bending, Chandler, Hilton, & Mills, 2012; Chakraborty & Newton, 2011; Karp & Richter, 2011; Mueller et al., 2012). The deployment of autonomous robots is an opportunity for enhancing tasks such as weeding, spraying, harvesting and mowing (Bakker, van Asselt, Bontsema, Müller, & van Straten, 2011; De-An, Jidong, Wei, Ying, & Yu, 2011; McPhee & Aird, 2013; Van Henten, Hemming, Van Tuijl, Kornet, & Bontsema, 2003; Van Henten et al., 2006). Monitoring and sampling are also important to improve crop yields and pest control (Slaughter, Giles, & Downey, 2008). Autonomous mobile robots have the potential to ultimately revolutionise agriculture. Small, cost-effective robot swarms, which can operate in various conditions and are easily maintained, can eventually replace heavy expensive machinery.

Operations of autonomous robots can be categorised into four main procedures, which are perception, localisation, planning and execution (Siegwart, Nourbakhsh, & Scaramuzza, 2011). Multisensory, vision-guided, perception systems are well studied and commonly used for different robotic platforms (Atanacio-Jiménez et al., 2011; Corke, Lobo, & Dias, 2007; Geiger, Lenz, Stiller, & Urtasun, 2013; Pandey, McBride, & Eustice, 2011). Data gathered by perception systems is further analysed to extract localization, mapping and obstacle information (Durrant-Whyte & Bailey, 2006; Endres, Hess, Sturm, Cremers, & Burgard, 2014; Rone & Ben-Tzvi, 2013). Planning is the process of computing a feasible, collision-free route towards the goal, given the current knowledge of the environment. Feedback control loops are commonly used for plan execution (Antonelli, Chiaverini, & Fusco, 2007; Wallace et al., 1985).

Fig. 1 – A photo of the experimental autonomous ground vehicle

1.1 Path Planning Path planning is a primary task for a robot to achieve mobility. Arguably, it is considered to be the most widely researched topic in robotics. Planning has extended well beyond robotics into computer animation, games and biology (Latombe, 1999). Planning in a fully observable, deterministic environment for an unrestrained robot is referred to as the piano movers problem in three dimensions or sofa movers in two dimensions. Deterministic solutions for, the aforementioned, simplified problems

5

have proven to be computationally extensive (Reif, 1979). Path planning algorithms produce simple paths consisting of subsequent waypoints linked by straight lines. Agile robots, such as omnidirectional robots and multi-rover aerial vehicles, are capable executing similar paths. This strategy involves performing stationary turns at all waypoints to change heading towards the subsequent waypoint. Following such paths will lead to increased traversal time. Nonholonomic robots, similar to car-like robots and fixed wing UAVs, are incapable of performing stationary turns, as their turning radius is bounded.

Roadmaps algorithms attempt to capture the connectivity of the environment (Asano, Asano, Guibas, Hershberger, & Imai, 1985; Brooks & Lozano-Perez, 1985; Canny, 1985; Keil & Sack, 1985). Graph search algorithms such as A* (Hart, Nilsson, & Raphael, 1968) and AD* (Likhachev, Ferguson, Gordon, Stentz, & Thrun, 2008) rely on the search space discretisation. It is undesirable, as coarse cell size can fail to capture the search space and lead to loss of completeness. High fidelity discretization will cause a surge in computation especially in large search spaces and dimensions. Potential fields methods employ artificial forces to guide robots to their goals (Khatib, 1986). They are, however, prone to falling into local minima and generating oscillating paths (Koren & Borenstein, 1991).

1.2 Randomized Planning Sampling-based approaches are effective algorithms in rapid search space coverage and path planning. Unlike graph search algorithms they operate in a continuous search space. They scale well in large search spaces. Rapidly-exploring Random Trees (RRT) and probabilistic roadmaps method (PRM) are the most widely adopted sampling-based methods (Kavraki, Svestka, Latombe, & Overmars, 1996; S. M. LaValle, 1998).

Kinodynamic planning is the process of planning under nonholonomic and dynamic constraints (Donald, Xavier, Canny, & Reif, 1993). Consequently, the search is more complex (Canny, Reif, Donald, & Xavier, 1988), as it is conducted in a large and more constrained space than that of the piano movers problem. Decouple motion planning is a common approach to handling nonholonomic constraints (Laumond, Sekhavat, & Lamiraux, 1998). An initial plan is generated that relaxes the nonholonomic constraints, followed by feasible trajectory modification.

Randomised kinodynamic planning algorithms have been proposed (Steven M. LaValle & Kuffner, 2001). Kinodynamic RRTs operations mostly resemble path planning RRT algorithms apart from the local connection procedure. In path planning a simple straight line is used to connect configurations or samples. A configuration is used to describe the robot motion and will be further explained. In the case of kinodynamic planning, trajectories are generated between samples by integrating the robot’s equation of motion. This approach causes inaccuracies when implemented for real-time planning scenarios (Srinivasa et al., 2010). Real-time, nonholonomic sampling-based planning algorithms are still intensively investigated (Elbanhawi & Simic, 2014c; Karaman & Frazzoli, 2013).

1.3 Related Work Circular arcs and straight lines are often combined to generate feasible paths between configurations (Anderson, Beard, & McLain, 2005; Reeds & Shepp, 1990; Xuan-Nam, Boissonnat, Soueres, & Laumond, 1994). Discontinuous curvature arises at the points where two path primitives, arc or lines, are joined. Overactuation, localisation errors, control instability, mechanical wear failure and passenger discomfort have been attributed to curvature discontinuity (Berglund, Brodnik, Jonsson, Staffanson, & Soderkvist, 2010; Gulati & Kuipers, 2008; Lau, Sprunk, & Burgard, 2009; Maekawa, Noda, Tamura, Ozaki, & Machida, 2010; Magid, Keren, Rivlin, & Yavneh, 2006).

Clothoids appear to be suited for continuous curvature path smoothing (Fraichard & Scheuer, 2004). They have been used in robot manoeuver design and path planning for agricultural purposes (Alshaer, Darabseh, & Alhanouti, 2013; Sabelhaus, Röben, Meyer zu Helligen, & Schulze Lammers, 2013). Their use is limited by the ability to synthesise them, as they have no closed form expression and high order curves are commonly needed for their approximation (McCrae & Singh, 2009; Montes,

6

Herraez, Armesto, & Tornero, 2008). Real time approximations have been proposed but they are limited in length and orientation (Brezak & Petrovic, 2013).

Parametric curves such as Beziers and B-splines are commonly used in computer-aided design (CAD) applications. They are capable of representing complex shapes whilst being easily synthesized (Farin, 2002). They have also been proposed for path smoothing. Several approaches ignore continuity constraints (Koyuncu & Inalhan, 2008; Lau et al., 2009; Nikolos, Valavanis, Tsourveloudis, & Kostaras, 2003) or maximum curvature constraint (Gulati & Kuipers, 2008; Jolly, Sreerama Kumar, & Vijayakumar, 2009; Koyuncu & Inalhan, 2008; Lau et al., 2009; Nikolos et al., 2003; Zhou, Song, & Tian, 2011). Geometric continuity is guaranteed by some approaches (Huh & Chang, 2014; Kwangjin, Jung, & Sukkarieh, 2013; Kwangjin & Sukkarieh, 2010). Fuzzy parametric smoothing generated heading discontinuities which cannot be realistically executed (Huh & Chang, 2014). B-spline methods that satisfy the maximum curvature constraint are limited to offline optimization (Berglund et al., 2010; Maekawa et al., 2010). Pan, Zhang, and Manocha (2012) argue that geometric continuity is not suitable for robotic applications and acceleration and velocity continuity must be ensured. We have previously proposed a B-spline smoothing solution that uses a single curve segment to guarantee continuity along the path and provided solutions for the maximum curvature condition (Elbanhawi & Simic, 2014a). This work is further extended to a real-time motion planning applications as shown in this paper.

1.4 Contribution In this work we propose a two-phase planning algorithm for a constrained car-like vehicle, shown in Fig. 1. Path planning is achieved using a sampling based algorithm. Some recent research in sampling approaches and RRT-variants is utilised to design an efficient algorithm for planning and replanning in dynamic changing environments. The performance of sampling based planners is sensitive to their implementation parameters of the algorithm and the metric function used (Amato, Bayazit, Dale, Jones, & Vallejo, 2000; Elbanhawi & Simic, 2013). Sucan and Kavraki (2010) note that implementation parameters for sampling planner are often left unmentioned with no guidance for selecting the appropriate values. We rigorously analyze parameter selection process for our planner. A metric function is also presented to minimize traversal time of the path without increasing the planner runtime. The second phase makes use of our previous work on B-spline smoothing, to obtain a path that obeys nonholonomic curvature and continuity constraints imposed on the robot (Elbanhawi & Simic, 2014a). B-spline smoothing is used as a secondary phase, succeeding an RRT-based algorithm, to generate an efficient two-phase kinodynamic planning algorithm.

2. Method An autonomous ground vehicle operating in a semi-structured agricultural area, as shown in Fig. 2, is considered. It is assumed that the positions of the landmarks in environment, grey boxes in Fig. 2., are fully known. However, the environment is partially observable and dynamic; changes maybe occur during the execution stage. This will require replanning of the remaining path. In this case, both phases of the algorithm must be repeated to generate a new solution from the current configuration.

2.1 Global planning Motion planning requires that the start and finish configurations, often referred to as queries, must be defined. Configurations, where tasks are to be carried out by the robot, are referred to as task configurations. It is assumed that all task configurations are defined and the distances between them are known, shown as red dots in Fig. 2. In some cases the tasks sequencing is predetermined. Otherwise, the optimal sequence of tasks, to be processed, can be determined by considering the scenario as a travelling salesperson problem (Russell & Norvig, 2010). The matter of job sequencing and task processing arrangement is well studied in industrial engineering (Nawaz, Enscore Jr, & Ham,

7

1983) and computer operating systems theory. The most applicable approach is to use some of the disk scheduling algorithms. The obvious ones could be First Come First Served (FCFS), or Shortest Seek Time First (SSTF). FCFS has no starvation problem, which means that all requests will be serviced. At the same time both of them seek the next destination regardless of the motion direction, which might be the problem because of the kinematics constraints of the robot vehicle. The second method reduces the total seek time compared to the first one, however both are suboptimal. Probably the best choice, for the agriculture scenarios, would be application of circular LOOK (CLOOP) algorithm. CLOOK does not have starvation problem and takes care of the motion direction. In addition, since agriculture depends on many external factors as weather, configuration of terrain and other, a priority algorithm might be applied together with CLOOK. That will add one more dimension to consider in the waypoints coordinate. A more comprehensive investigation of this scheduling algorithms applied in agriculture, with the goal of seeking the optimal one, could be subject of further research. Here a motion planner that favours shorter routes to just shorter ones if external priorities are not considered.

Fig. 2 – An example of a semi-structured environment. Obstacles, illustrated as grey boxes, are fully known prior to execution. The environment is dynamic and partially observable during execution. Task configurations are shown as solid red circles.

2.2 RRT-based path planning Sampling based planners operate in the configuration space (C-space). It contains all possible configurations of the robot positions (Lozano-Perez, 1983). A robot with n-degrees of freedom has a C-space of n-dimensions. A single point in C-space describes the robot at an instance. C-space is subdivided into free and obstacle configurations, Cobs and Cfree. A planning problem can be defined as finding a set of subsequent connected configurations in Cfree from the initial to the goal configurations. RRT incrementally builds a tree of connected configurations in Cfree by randomly sampling nodes in the C-space and connecting them to the nearest node in the tree. Algorithms such as RRT and PRM are probabilistically complete i.e. they guarantee a solution will be found in sufficient planning time.

Despite the effectiveness of RRT in rapidly exploring the search space, there are some implementation challenges. Reliance on stochastic sampling results in suboptimal paths that need further processing, or optimisation. Narrow Cfree regions arise in C-space as a result of the robot constraints. In cases of uniform sampling, the probability of selecting a sample in a narrow region is lower than other regions, leading to difficulties in solving a query that requires navigation through such regions. The performance of RRTs is affected by their parameters (Elbanhawi & Simic, 2014c) and the metric used of nearest node search (Peng & LaValle, 2001). Albeit the significance of specifying parameters of the designed planner, they are seldom defined in literature (Sucan & Kavraki, 2010). S.M. LaValle (2006) argues that finding an optimal metric is as difficult as solving the piano mover’s problem.

8

2.2.1 Planning Algorithm RRT efficiently explore C-space, as shown in Fig. 3 after 1000 iterations. The robot is represented as a circle with a 5-metre diameter. It can be noted that,

• All regions are uniformly sampled regardless of the goal configuration, leading to exploring areas that do not contribute to the solution.

• Near obstacle regions can be favoured, based on the metric value, despite the high probability of failure in those regions, highlighted in yellow in Fig. 3

• No configurations are sampled in the narrow region, highlighted in red in Fig. 3.

Fig. 3 – Exploring C-space (500x500 m) using RRT (after 1000 iterations)

Research is conducted to overcome previously listed gaps. However, leveraging the superior aspects of the aforementioned planners, in combination to produce a single efficient planner, has not been presented yet. We propose an RRT approach as follows:

• The planner operates in a 2D C-space that relaxes all constraints on the robot to overcome the appearance of narrow regions in the search space. It is opposite to the process of conducting search in a 3D space (x, y, θ) with nonholonomic constraints, which will be handled in the next phase of planning.

• Goal biased (Kuffner & LaValle, 2000): The planner attempts to connect to the goal every ith-iteration, effectively steering the search towards it. Lighting biasing (less than 10%) is recommended to maintain the randomness of the search.

• Resolution complete, RC-RRT, (Peng & LaValle, 2001): The failure rate of a node is considered when attempting to extend. This improves the effectiveness of the search, as the node selection is not simply reliant on metric function. As a result more promising nodes are selected and others disregarded.

• Execution extended, ERRT, (Bruce & Veloso, 2006): An efficient strategy for replanning in dynamic environment. It assumes that small changes occur in the environment provided the planner is capable of running with a high replanning rate. A new tree is constructed with a higher probability of expanding towards the original tree, stored in waypoint cache and the goal region. We will show in ERRT planning in dynamic environments can reduce planning time in some cases to a tenth of the standard RRT planning time.

9

2.2.2 Parameters Utilising the aforementioned RRT variants (section 2.2.1) concurrently is not sufficient. The parameters of the planner must be appropriate for the given task. The aim of this study is to propose the planner that minimises the metric function, for different planning queries, in a shortest time frame. Ultimately a planner that minimises path traversal time and operates within a timeframe suitable for real time operation is desirable. The parameters of the planner are:

• Step size extension: This refers to the maximum length of incremental growth allowed for each planning step. If a length between a random node, q.rand, and its nearest node, q.near, exceeds the step size, the nearest node is extended towards a node, q.new, in the same direction with a distance equal to the step size, d. A greedy step size will improve the planner performance but will adversely affect the path cost.

Fig. 4 – The RRT extend procedure with limited step size

• Biasing: Increasing the goal biasing of the planner will guide the tree growth towards the goal. In some environments this is undesirable, as the planner must move away from the goal before moving towards it. A biasing ratio must be selected to maintain the randomness whilst guiding the path towards the goal.

• Metric function: The planner connects a random sample to the nearest node in the tree. The nearest node is determined based on the metric function. It is common to utilize Euclidean distance as the metric. Shorter distances can be covered in less time. We aim to reduce the traversal time and curvature as explained earlier. The turning angle between two nodes should also be considered. A small turning angle is an indication of a smoother path. As the curvature of the path increases the vehicle is required to slow down thus a smoother path will eventually lead to reduction in the total traversal time of the path. The planner favours smoother paths over shorter ones. This is illustrated in Fig. 5 where the new node is connected to a further node that requires less heading change. Metric function, m(i,j), between the nodes qi and qj is given in equation (1),where q = q(x, y, θ). It combines distance between nodes, d(i,j), heading difference, h(i,j), and the node failure rate, q.failure. All metrics in the cost function have been scaled and have equal weights, gd, gh, gf, which can be altered to tune the planner behaviour.

m(i,j) = gd d(i,j) + gh h(i,j) + gf q.failure , where 𝑑 𝑖, 𝑗 =   (𝑥! − 𝑥!)! + (𝑦! − 𝑦!)! ℎ 𝑖, 𝑗 =   |𝜃! − 𝜃!| (1)

10

Fig. 5 – The planner will favour smoother connections over shorter ones by including the turning angle in the metric function

The advantages of the proposed algorithm will be shown in the experiments section of this paper. For now we shed a light on some differences between the proposed algorithm, shown in Fig. 6, and standard RRT, as given in Fig. 3. Comparing the resulting search trees, from both planners, it can be noted that proposed approach generates more practical and direct search tree with less unnecessary exploration and sampling around obstacles.

2.3 Continuous Path Smoothing In second phase of planning, the simple RRT path is modified to obey the constraints on the vehicle and generate a continuous path. A typical path obtained from the RRT planner is shown red in Fig. 6. It is widely suboptimal and involves several redundant motions. Optimal randomised planners are characterised with asymptotic optimality (Karaman & Frazzoli, 2011), which is not suited for real time planning. The first part of smoothing is trimming the path by shortcutting it, as shown green in Fig. 6. As previously explained, it is required to transform the linear path to a feasible continuous path that satisfies the maximum curvature constraint.

Fig. 6 – Typical suboptimal path obtained from randomised planning (red path) and the corresponding shortcut path (green)

2.3.1 Path Continuity Clamped cubic B-spline curves were shown to be advantageous to robot navigation (Elbanhawi & Simic, 2014a). Local support property enables the modification of a single region of the path with minimal effect to the entire shape of the path. A single B-spline segment can be used for the entire path thus ensuring path continuity. Midpoint insertion between successive waypoints was proposed to

11

ensure the tangency of the curve to the original linear path. The resulting path from B-spline smoothing and its corresponding curvature are shown in Fig. 7.

A p-th degree B-spline curve, c(u), that is defined by n control points and m = n+p+1 knots 𝑢 is given by equations (2-4) . Control Points Pi are obtained from path shortcutting. They are shown as green nodes in Fig. 6.

𝑐 𝑢 = 𝑁!,! 𝑢 𝑃!          !

!!!

    2

Nn,i (u) is the B-spline basis function which is defined using the Cox-de Boor algorithm as given by equations (3) and (4) (De Boor, 1972).

𝑁!,! =  1        𝑢  𝜖  [𝑢!, 𝑢!!!)  0                                        𝑒𝑙𝑠𝑒

   (3)

𝑁!,! 𝑢 =  𝑢 − 𝑢!𝑢!!! − 𝑢!

𝑁!,!!! 𝑢 +𝑢!!!!! − 𝑢𝑢!!!!! − 𝑢!!!

𝑁!!!,!!! 𝑢    (4)

Fig. 7 – Continuous B-spline path and its corresponding curvature

2.3.2 Maximum Curvature Constraint A segment consisting of five control points is repeated throughout the path as a result of midpoint insertion. The five control points construct two control polygon edges of length L, referred to as B-spline segment length, separated by a segment angle α  as  shown  in  Fig. 8.  

Fig. 8 – Path segment parameters

The maximum curvature, Kmax, can be satisfied locally in each segment by relating the segment parameters with the corresponding B-spline path curvature. This can be formulated, as shown in (Elbanhawi & Simic, 2014a) equation (6), by substituting with the B-spline curve, c(u) = [x(u) y(u)]T, in Eq. (5). The maximum curvature for a car-like vehicle can be calculated from its minimum turning radius.

12

𝑘 =  𝑥!𝑦!! − 𝑦!𝑥!!

(𝑥!" + 𝑦!")! !              (5)

𝑘 =2(𝑢 sin ∝)(1 − 𝑢)

3𝐿(2𝑢!(1 − cos ∝)(𝑢! − 2𝑢 + 1) + 2𝑢 − 1 !)! !      (6)

Assuming equal length path segment edges, the maximum curvature of the B-spline curve can be

calculated by substituting by u=0.5 as it will be reached at the centre of the segment. A minimum segment angle αmin  is  set.  The  corresponding  minimum  segment  length,  Lmin,  can  be  found  for  a  given  maximum  allowable  curvature  of  the  vehicle,  Kmax.  The  path  planning  algorithm  step  size  is  selected  to  be  at  least  equal  to  Lmin  thus  ensuring  feasible  paths  are  generated.  Segments  are  modified,  as  shown  in  Fig. 9,  by  adding  another  segment  edge  with  minimum  segment  angle  and  length.    This  ensures  that  the  curvature  in  that  segment  does  not  exceed Kmax.    

Fig. 9 – Modifying a segment to satisfy the maximum curvature constraint

2.3.3 Initial and Final Heading Curvature bounds on the robot motion must be handled at the initial and final configurations. Clamped B-splines are characterised by their tangency to the initial and final control polygon edges as a result of knot multiplicity (Farin, 2002). Subsequently, the headings of the initial and final edges of the control polygon must correspond to the required initial and final headings respectively for the vehicle. This is rarely the case and the control polygon edges must be modified to match the desired headings. Algorithms that rely on clothoids fall short in handling non-zero initial and final headings (Sabelhaus et al., 2013).

Two novel heading manoeuvres are proposed. The difference between the current planned heading and the desired heading will determine the manoeuvre needed to reach the desired configuration. Small differences, less than αmin,  can  be  corrected  by  extending  a  segment  of  length  Lmin  in  the  desired  heading,  either  at  the  start,  or  the  end  of  the  path,  and  then  turning  towards  the  path,  as  shown  in  Fig. 10(a).  The  dashed  line  shows  the  original  path  heading,  while  the  solid  red  line  shows  the  modified  path.  Larger  differences,  exceeding αmin,  require  reversing  and  turning  manoeuvres.  The  manoeuvres  are  shown  in  in  Fig. 10(b).    Vehicle  starts  at  A  and  reverses  by  a  distance  of  twice  Lmin  to  B.  This  is  followed  by  a  turning  manoeuvre  with  αmin  where  the  length  of  both  line  segments  𝐵𝐶  and  𝐷𝐶  is  Lmin.  

13

Fig. 10 – Manoeuvres for heading differences (a) less than or equal αmin (b) larger than αmin  

2.4 Algorithm Planning, smoothing and replanning procedures, are combined in this section, to present a planning algorithm. A flow chart illustrating the algorithm is given in Fig. 11. As already mentioned, there are two main procedures; i) Initial planning and B-spline smoothing, and ii) Replanning and B-spline smoothing. Each of the main procedures in the algorithm is a two-phase planning algorithm that combines randomized planning with B-spline smoothing. As for any planning algorithm, partial or full knowledge of the environment and locations are given from localisation and mapping algorithms (Durrant-Whyte & Bailey, 2006; Endres et al., 2014).

An initial plan is generated offline covering motion from the start to the current position. In order to generate this plan, full knowledge of the environment is assumed. The initial plan is generated offline prior to execution. The path is shortcut and modified to match the start and final headings. B-spline smoothing is then applied to generate a continuous path with maximum bounded curvature. During execution when any changes in the environment are detected, the remaining path is replanned from the current position using greedy waypoint cache biasing. Subsequently, the regenerated path is shortcut, i.e. modified to match headings and B-spline smoothing. We refer to this operation as B-spline post processing. The replanning cycle is repeated until the goal configuration is reached.

14

Fig. 11 – Two-phase planning algorithm flow chart

3. Results and Discussion In this section two sets of experiments are presented. Firstly d the RRT planner is tested with different parameters to find the appropriate values that improve the planning time and resulting paths. This is followed by a numerical experiment to analyse the performance of the proposed planner in a variety of scenarios. All experiments were conducted in a 500×500 m environment, shown in Fig. 2. The planner is implemented using Python running on a machine with a 2.9 GHz i7 Processor with 8GB of RAM.

3.1 RRT Parameters Experiments in this section are used to select the appropriate RRT planner step size, see Fig. 12, and goal-biasing ratio, see Fig. 13. The effect of considering the turning angle in the metric function on the path traversal time is shown in Fig. 14. The advantage of using a goal-biased RRT in conjunction with a resolution complete (RC-RRT) and execution extended (ERRT) are highlighted in Fig. 15 and Fig. 16. RRT relies on random sampling and as a result pathological and lucky solutions can be attained. To validate the results each planner is run 30 times for two different queries referred to as easy, see Fig. 19(a), and difficult, see  Fig. 17(a). Averaged results are analysed to specify the appropriate planning parameters for the specified problem.

The vehicle shape, for planning purposes, is assumed to be a circle of 5 m diameter as shown in Fig. 3. The planner was first implemented with a step size of 5 m, which is the size of the vehicle and 1% of the environment side length. The average number of iterations, needed to a find a solution, decreases as the step size increases, see Fig. 12 (top). The planner was capable of covering more Cfree

with a large step size. At some stage this will degrade the path quality as the distances between nodes increase, see Fig. 12 (bottom). A step size of 10m was selected as it balances the exploration speed with the solution quality. A similar analysis was performed with goal biasing ratio. The step size was not allowed to go below Lmin to ensure the path satisfies the maximum curvature constraint.

15

As previously mentioned, light biasing is required to maintain randomness in RRT and prevent greedy planner behaviour. The results shown in Fig. 13 highlight the effect of light biasing on steering the search towards the goal, whilst maintaining randomness. Improvements on both planning time and path cost were achieved by biasing. The difficult query requires the path to move away from the goal and then towards the goal as illustrated in Fig. 17(a). Increasing biasing ratio in that case provided no improvements in planning time and path cost increased. The path was being pulled towards the wrong direction as a result of increased goal biasing. A goal-biasing ratio of 5% was selected as it operates well in both queries without affecting the randomness of the planner. The planner attempted a direct connection to goal configuration once every twenty loops.

Fig. 12 – Average path length and number of iterations for different extension step sizes

Fig. 13 – Average path length and number of iterations for different extension goal biasing ratio

Fig. 14 – Average path traversal time using different metrics

We have shown that curvature of a segment is a function of both segment length and angle in equation (6). Accordingly, the proposed metric, in Eq. (1), accounts for both distance and heading. Subsequently, the planner favours smoother connections to just shorter ones. An average 5 s improvement, using the proposed metric, has been achieved in the difficult environment, as shown in

16

Fig. 14. In contrast, no improvements were achieved in the easy environment due to the absence of turns in the solution, as shown in Fig. 19 (a). It worth noting that the B-spline continuous trajectory was generated using the algorithm proposed in (Elbanhawi & Simic, 2014b). However, this planning algorithm can be implemented with other trajectory generation algorithms (Jolly et al., 2009; Lau et al., 2009).

The metric function was further modified by including the nodes failure rate. The search was guided to expand nodes that are more likely to succeed, i.e. away from obstacles. This prevents the planner from attempting connections in areas that are not likely to expand the search based on their attractive metric values. Improvements in the average planning iterations are needed in both environments and are shown in Fig. 15. The advantage of considering the failure rate is apparent in more obstacle-cluttered queries, as the planner will avoid expanding from near-obstacle regions.

During the planned path execution, the robot gathers information and updates its belief state. An efficient planning method is required to modify the path based on the perceived observation. ERRT assumes minor changes in the environment as a result of the high frequency of update and replanning cycle. Replanning using a standard RRT algorithm was not considered an efficient approach, as the entire search space will unnecessarily be explored in every iteration. ERRT stores the discarded path in a cache and attempts to reconnect to that path. A biasing ratio of 50% has been utilised for the waypoint cache and 5% for the goal region. Residual planning time is used for random sampling. The results in Fig. 16 show that utilising waypoint cache improved both the average planning time and path quality in easy and difficult planning scenarios.

Fig. 15 – RC-RRT improves the algorithm runtime compared to Goal Biased RRT

Fig. 16 – RRT, with 50% waypoint cache biasing, average runtime and path length solution compared to 5% Goal Biased RRT

3.2 Numerical Experiments The proposed two-phase planning algorithms was implemented for two different motion planning problems. The parameters of the experimental vehicle, shown in Fig. 1, are given in Table 1. The first

17

planning phase was path planning using an efficient RRT variant. The planner had a step size of 10 m, 5% goal biasing. The metric considers node failure rate, heading change and distance. In dynamic environments, replanning was conducted with 50% waypoint cache biasing ratio. The resulting paths were optimised using linear shortcutting. Significant improvements in the average path length were achieved: 8.6% in easy and 18.5% in difficult queries. Shortcutting also circumvents redundant actions that result from stochastic sampling. This effect can be observed in Fig. 17 and Fig. 19, where the RRT path is the coarse black path and the final path consists of green straight lines.

The second planning phase uses of B-spline curves to generate a continuous path with bounded curvature. In our earlier work it was shown that, for the continuous B-spline smoothing and bounded trajectory, average processing time is 0.047 s (Elbanhawi & Simic, 2014a). Based on the vehicle parameters, the minimum segment angle, αmin, is set to 90o and the segment length, Lmin, was calculated to be 9.5 meters by Eq. (6). The step size was selected as 10 m, which exceeds Lmin. The algorithm was tested for two scenarios, planning an initial path for the difficult query assuming a fully observable environment, and obstacle detection replanning for the easy query assuming partially observable obstacles. Table 1 – Experimental vehicle parameters

Parameter Value

Minimum turning radius (r) 3 m

Maximum curvature (k) 0.3 m-1

Wheel base (w) 1.9 m

Maximum steering angle (δ) 30o

Maximum acceleration (a) 2 m s-2 Maximum angular velocity (ω) 1 rad s-1

Maximum forward velocity (vf) 5.5 m s-1

Maximum reverse velocity (vr) 1 m s-1

3.2.1 Experiment one: Initial planning The average processing time for planning a path in the difficult environment is 0.76 s. The resulting path is shown in Fig. 17(a). The corresponding continuous B-spline path and its bounded curvature are shown in Fig. 18. The initial and final headings were set to 180o and 210o. The vehicle started by reversing and then performing two successive turns to execute the planned path, see Fig. 17(c). The planned path was modified in the last corner, shown as dotted red line in Fig. 18(left), as it exceeded the allowable turning angle, αmin. Considering at the path’s curvature, Fig. 18(right) at normalised length 0.1 and 0.7, the reversing and path modification manoeuvres satisfied the maximum curvature constraint (|k|≤0.3 m-1).

18

Fig. 17 – (a) Planning difficult query using the proposed RRT planner (b) Final heading, 210o, manoeuvre (c) Initial heading, 180o, reversing manoeuvre.

Fig. 18 – Experiment one: resulting B-spline path and corresponding bounded continuous curvature

3.2.2 Experiment two: replanning in a dynamic environment The execution of the path with partial knowledge of the environment was considered. The hidden obstacle, shown as a dotted box in the centre of the environment in Fig. 19, was not known. It could only be sensed when been approached by the vehicle. To calculate the initial plan, given in Fig. 19(a), an average time of 0.33 s is required. Once an obstacle is detected the path must be regenerated from the current position with the updated knowledge of the environment. The planner is biased towards the existing solution to improve the planning time. The average replanning time, with 50% waypoint cache biasing, was 0.038 s (a tenth of the initial average planning time). At this replanning rate it is reasonable to assume only minor changes in the dynamic environment will take place. This validates our choice to greedily bias towards the cached waypoints. The efficient replanning can be attributed to the fact that the RRT algorithm is greedily biased to explore around the already existing path, which only requires minor changes. This is clear in Fig. 19(b) where most of the tree growth is in the vicinity of the initial path. Marginal planning time was wasted on C-space exploration. The new plan required

19

the vehicle to steer away from the obstacle. The corresponding B-spline path executed the manoeuvre whilst satisfying the maximum curvature constraint and path continuity, as shown in Fig. 20.

Fig. 19 – (a) Planning the easy query using the proposed RRT planner (b) efficient replanning under obstacle detection using waypoint cache from ERRT. The sensor detection range is depicted in yellow

Fig. 20 – Experiment two: resulting B-spline path and corresponding bounded continuous curvature

3.2.3 Experiment three: planning in static structured environments The proposed algorithm was designed for semi-structured and partially known dynamic environments. It can also be implemented for fully known structured static environments, as shown in Fig. 21. The start position is at the bottom right corner (0,0) and the goal position is in the top left corner (500,500). The heading at both positions is 90o. The resulting B-spline path and its continuous curvature profile are given in Fig. 22. Maximum curvature constraints were satisfied along the entire path especially at the initial and final heading manoeuvres.

20

Fig. 21 - Planning in a structured environment with initial and final headings of 90o; the environment is photographed on the right and the model is shown on the left

Fig. 22 - Experiment three: Resulting B-spline path and corresponding bounded continuous curvature

4. Conclusion and Future Work The development of a two-phase randomized kinodynamic motion planner is presented here. Planning with kinodynamic constraints in real time planning scenarios is challenging due to the size of the search space and the restrictions imposed by the various constraints. Several aspects of RRTs planner into one efficient RRT algorithm are combined. Simulations highlight the effect of RRT step size, metric and biasing ratio on the planning performance. This is followed by rigorous experimentation and analysis of the planner parameters. Large step size allows the algorithms coverage of the search space but can have adverse effect on the path cost. In certain situations, it was shown increasing the goal biasing ratio will have no effect on the planning time. Including the heading change in the metric function allows the algorithm to favour smoother rather shorter connections. This ultimately results in reducing the path traversal time. As the curvature of the path increases the vehicle must slow down when turning. The planner parameters

21

were selected based on the simulation results to minimize planning time and path costs. Further enhancements were introduced by incorporating the node failure rate in the planning metric and waypoint cache biasing in replanning case. Including failure rate improved the planning time by preventing the planner from exploring nodes near obstacles and reduced the influence of metric functions on the planner performance. Waypoint cache bias focuses the search to explore the area in the vicinity of the initial path not the entire C-space. An initial path, calculated in an average of 0.76 s, and the remaining path was replanned on average in under 0.04 s. The efficiency of the replanning procedure allows the algorithm to run at a high frequency. As a result, the assumption that every time the replanning is run (up to 25 Hz), minor changes occur in the environment is justified. Mapping algorithms with off the shelf vision systems can be implemented at a high frequency (Endres et al., 2014). In such cases, greedy waypoint cache biasing will prove to be an effective approach.

In the second phase the algorithm generated a feasible plan under kinodynamic constraints. B-splines curves were utilised for path smoothing. A single B-spline curve is utilised to ensure path continuity. Curve parameters were formulated with respect to the path segment. Smoothing, initial and final headings manoeuvres were designed using B-spline to satisfy the maximum curvature constraint. The minimum segment parameters were selected based on the vehicle’s parameters. Two numerical experiments highlighted the effectiveness of such manoeuvres in obeying the vehicle constraints.

In the future, it is hoped to gain more insight about the metric function’s effect on the resulting path. Possible tuning factors or adaptive factors can be introduced based on the task of the vehicle and environment. This proposed planner could be implemented in conjunction with any future advancement in sampling based planning. They are yet to be implemented for real-time kinodynamic purposes. It is possible to extend the presented methods to reduce the replanning load for the partially observable markov decision process (POMDP) problem, which deals with uncertainties in robotics. Finally, it is hoped that common claims on the significance of continuous curvature with regards to mechanical wear, overshooting and passenger comfort can be experimentally validated. Continuous curvature algorithm can now be implemented on a robotic platform for real time planning.

Acknowledgements M. Elbanhawi is the recipient of the Australian Postgraduate Award (APA) and the Research Training Scheme (RTS). The authors would like to thank Tesla Robotics Team at RMIT University for setting up the experimental vehicle.

References Alshaer, B. J., Darabseh, T. T., & Alhanouti, M. A. (2013). Path planning, modeling and

simulation of an autonomous articulated heavy construction machine performing a loading cycle. Applied Mathematical Modelling, 37(7), 5315-5325. doi: http://dx.doi.org/10.1016/j.apm.2012.10.042

Amato, N. M., Bayazit, O. B., Dale, L. K., Jones, C., & Vallejo, D. (2000). Choosing good distance metrics and local planners for probabilistic roadmap methods. Robotics and Automation, IEEE Transactions on, 16(4), 442-447. doi: 10.1109/70.864240

Anderson, E. P., Beard, R. W., & McLain, T. W. (2005). Real-time dynamic trajectory smoothing for unmanned air vehicles. Control Systems Technology, IEEE Transactions on, 13(3), 471-477. doi: 10.1109/TCST.2004.839555

Antonelli, G., Chiaverini, S., & Fusco, G. (2007). A fuzzy-logic-based approach for mobile robot path tracking. IEEE Transactions on Fuzzy Systems, 15(2), 211-221.

22

Asano, Takao, Asano, Tetsuo, Guibas, Leonidas, Hershberger, John, & Imai, Hiroshi. (1985, 21-23 Oct. 1985). Visibility-polygon search and euclidean shortest paths. Paper presented at the Foundations of Computer Science, 1985., 26th Annual Symposium on.

Atanacio-Jiménez, G., González-Barbosa, J. J., Hurtado-Ramos, J. B., Ornelas-Rodríguez, F. J., Jiménez-Hernández, H., García-Ramirez, T., & González-Barbosa, R. (2011). LIDAR velodyne HDL-64E calibration using pattern planes. International Journal of Advanced Robotic Systems, 8(5), 70-82.

Bakker, Tijmen, van Asselt, Kees, Bontsema, Jan, Müller, Joachim, & van Straten, Gerrit. (2011). Autonomous navigation using a robot platform in a sugar beet field. Biosystems Engineering, 109(4), 357-368. doi: http://dx.doi.org/10.1016/j.biosystemseng.2011.05.001

Bennett, Amanda J., Bending, Gary D., Chandler, David, Hilton, Sally, & Mills, Peter. (2012). Meeting the demand for crop production: the challenge of yield decline in crops grown in short rotations. Biological Reviews, 87(1), 52-71. doi: 10.1111/j.1469-185X.2011.00184.x

Berglund, T., Brodnik, A., Jonsson, H., Staffanson, M., & Soderkvist, I. (2010). Planning Smooth and Obstacle-Avoiding B-Spline Paths for Autonomous Mining Vehicles. Automation Science and Engineering, IEEE Transactions on, 7(1), 167-172. doi: 10.1109/TASE.2009.2015886

Brezak, M., & Petrovic, I. (2013). Real-time Approximation of Clothoids With Bounded Error for Path Planning Applications. Robotics, IEEE Transactions on, PP(99), 1-9. doi: 10.1109/TRO.2013.2283928

Brooks, R. A., & Lozano-Perez, T. (1985). A subdivision algorithm in configuration space for findpath with rotation. Systems, Man and Cybernetics, IEEE Transactions on, SMC-15(2), 224-233. doi: 10.1109/TSMC.1985.6313352

Bruce, J. R., & Veloso, M. M. (2006). Safe Multirobot Navigation Within Dynamics Constraints. Proceedings of the IEEE, 94(7), 1398-1411. doi: 10.1109/JPROC.2006.876915

Canny, John. (1985, Mar 1985). A Voronoi method for the piano-movers problem. Paper presented at the Robotics and Automation. Proceedings. 1985 IEEE International Conference on.

Canny, John, Reif, John, Donald, B., & Xavier, P. (1988, 24-26 Oct. 1988). On the complexity of kinodynamic planning. Paper presented at the Foundations of Computer Science, 1988., 29th Annual Symposium on.

Chakraborty, S., & Newton, A. C. (2011). Climate change, plant diseases and food security: an overview. Plant Pathology, 60(1), 2-14. doi: 10.1111/j.1365-3059.2010.02411.x

Corke, Peter, Lobo, Jorge, & Dias, Jorge. (2007). An Introduction to Inertial and Visual Sensing. The International Journal of Robotics Research, 26(6), 519-535. doi: 10.1177/0278364907079279

De Boor, C. (1972). On calculating with B-splines. Journal of Approximation Theory, 6(1), 50-62. De-An, Zhao, Jidong, Lv, Wei, Ji, Ying, Zhang, & Yu, Chen. (2011). Design and control of an

apple harvesting robot. Biosystems Engineering, 110(2), 112-122. doi: http://dx.doi.org/10.1016/j.biosystemseng.2011.07.005

Donald, Bruce, Xavier, Patrick, Canny, John, & Reif, John. (1993). Kinodynamic motion planning. Journal of the ACM, 40(5), 1048-1066.

Durrant-Whyte, H., & Bailey, Tim. (2006). Simultaneous localization and mapping: part I. Robotics & Automation Magazine, IEEE, 13(2), 99-110. doi: 10.1109/MRA.2006.1638022

Elbanhawi, M., & Simic, M. (2013, 20-22 Nov. 2013). On the Performance of Sampling-Based Optimal Motion Planners. Paper presented at the Computer Modeling and Simulation (EMS), 2013 Seventh UKSim/AMSS European Symposium on (in press).

Elbanhawi, M., & Simic, M. (2014a). Continuous Bounded Curvature Path Smoothing Using Parametric Splines. IEEE Transactions on Robotics (Under Review).

Elbanhawi, M., & Simic, M. (2014b). Continuous-Curvature Bounded Trajectory Planning Using Parametric Splines. Paper presented at the 7th International KES conference on Intelligent Interactive Multimedia Systems and Services (IIMSS) (in press), Chania, Greece.

Elbanhawi, M., & Simic, M. (2014c). Sampling-Based Robot Motion Planning: A Review. Access, IEEE, 2, 56-77. doi: 10.1109/ACCESS.2014.2302442

Endres, F., Hess, J., Sturm, J., Cremers, D., & Burgard, W. (2014). 3-D Mapping With an RGB-D Camera. Robotics, IEEE Transactions on, 30(1), 177-187. doi: 10.1109/TRO.2013.2279412

Farin, Gerald. (2002). Curves and Surfaces for CAGD: Morgan Kaufmann. Fraichard, T., & Scheuer, Alexis. (2004). From Reeds and Shepp's to continuous-curvature paths.

Robotics, IEEE Transactions on, 20(6), 1025-1035. doi: 10.1109/TRO.2004.833789

23

Geiger, A, Lenz, P, Stiller, C, & Urtasun, R. (2013). Vision meets robotics: The KITTI dataset. The International Journal of Robotics Research, 32(11), 1231-1237. doi: 10.1177/0278364913491297

Gulati, S., & Kuipers, B. (2008, 19-23 May 2008). High performance control for graceful motion of an intelligent wheelchair. Paper presented at the Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on.

Hart, P. E., Nilsson, N. J., & Raphael, B. (1968). A Formal Basis for the Heuristic Determination of Minimum Cost Paths. Systems Science and Cybernetics, IEEE Transactions on, 4(2), 100-107. doi: 10.1109/TSSC.1968.300136

Huh, Uk-Youl , & Chang, Seong-Ryong (2014). A G² Continuous Path-smoothing Algorithm Using Modified Quadratic Polynomial Interpolation. International Journal of Advanced Robotic Systems, 11(25). doi: 10.5772/57340

Jolly, K. G., Sreerama Kumar, R., & Vijayakumar, R. (2009). A Bezier curve based path planning in a multi-agent robot soccer system without violating the acceleration limits. Robotics and Autonomous Systems, 57(1), 23-33. doi: http://dx.doi.org/10.1016/j.robot.2008.03.009

Karaman, Sertac, & Frazzoli, Emilio. (2011). Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7), 846-894. doi: 10.1177/0278364911406761

Karaman, Sertac, & Frazzoli, Emilio. (2013, 6-10 May 2013). Sampling-based optimal motion planning for non-holonomic dynamical systems. Paper presented at the Robotics and Automation (ICRA), 2013 IEEE International Conference on.

Karp, Angela, & Richter, Goetz M. (2011). Meeting the challenge of food and energy security. Journal of Experimental Botany, 62(10), 3263-3271. doi: 10.1093/jxb/err099

Kavraki, L. E., Svestka, P., Latombe, J. C., & Overmars, M. H. (1996). Probabilistic roadmaps for path planning in high-dimensional configuration spaces. Robotics and Automation, IEEE Transactions on, 12(4), 566-580. doi: 10.1109/70.508439

Keil, J Mark, & Sack, Jorg-R. (1985). Minimum decompositions of polygonal objects. Computational Geometry, 197-216.

Khatib, O. (1986). Real-Time Obstacle Avoidance for Manipulators and Mobile Robots. International Journal of Robotics Research, 5(1), 90-98. doi: 10.1177/027836498600500106

Koren, Y., & Borenstein, J. (1991, 9-11 Apr 1991). Potential field methods and their inherent limitations for mobile robot navigation. Paper presented at the Robotics and Automation, 1991. Proceedings., 1991 IEEE International Conference on.

Koyuncu, E., & Inalhan, G. (2008, 22-26 Sept. 2008). A probabilistic B-spline motion planning algorithm for unmanned helicopters flying in dense 3D environments. Paper presented at the Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on.

Kuffner, J. J., & LaValle, S. M. (2000, 2000). RRT-connect: An efficient approach to single-query path planning. Paper presented at the Robotics and Automation, 2000. Proceedings. ICRA '00. IEEE International Conference on.

Kwangjin, Yang, Jung, Daehan, & Sukkarieh, Salah. (2013). Continuous curvature path-smoothing algorithm using cubic Bezier spiral curves for non-holonomic robots. Advanced Robotics, 27(4), 247-258. doi: 10.1080/01691864.2013.755246

Kwangjin, Yang, & Sukkarieh, S. (2010). An Analytical Continuous-Curvature Path-Smoothing Algorithm. Robotics, IEEE Transactions on, 26(3), 561-568. doi: 10.1109/TRO.2010.2042990

Latombe, Jean-Claude. (1999). Motion Planning: A Journey of Robots, Molecules, Digital Actors, and Other Artifacts. The International Journal of Robotics Research, 18(11), 1119-1128. doi: 10.1177/02783649922067753

Lau, B., Sprunk, C., & Burgard, W. (2009, 10-15 Oct. 2009). Kinodynamic motion planning for mobile robots using splines. Paper presented at the Intelligent Robots and Systems, 2009. IROS 2009. IEEE/RSJ International Conference on.

Laumond, J. P., Sekhavat, S., & Lamiraux, F. (1998). Guidelines in nonholonomic motion planning for mobile robots. In J. P. Laumond (Ed.), Robot Motion Planning and Control (Vol. 229, pp. 1-53): Springer Berlin Heidelberg.

LaValle, S. M. (1998). Rapidly-exploring random trees: A new tool for path planning (C. S. Department, Trans.): Iowa State University.

LaValle, S.M. (2006). Planning Algorithms: Cambridge University Press.

24

LaValle, Steven M., & Kuffner, James J. (2001). Randomized Kinodynamic Planning. The International Journal of Robotics Research, 20(5), 378-400. doi: 10.1177/02783640122067453

Likhachev, Maxim, Ferguson, Dave, Gordon, Geoff, Stentz, Anthony, & Thrun, Sebastian. (2008). Anytime search in dynamic graphs. Artificial Intelligence, 172(14), 1613-1643. doi: http://dx.doi.org/10.1016/j.artint.2007.11.009

Lozano-Perez, T. (1983). Spatial Planning: A Configuration Space Approach. Computers, IEEE Transactions on, C-32(2), 108-120. doi: 10.1109/TC.1983.1676196

Maekawa, Takashi, Noda, Tetsuya, Tamura, Shigefumi, Ozaki, Tomonori, & Machida, Ken-ichiro. (2010). Curvature continuous path generation for autonomous vehicle using B-spline curves. Computer-Aided Design, 42(4), 350-359. doi: http://dx.doi.org/10.1016/j.cad.2009.12.007

Magid, E., Keren, D., Rivlin, E., & Yavneh, I. (2006, 9-15 Oct. 2006). Spline-Based Robot Navigation. Paper presented at the Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on.

McCrae, James, & Singh, Karan. (2009). Sketching piecewise clothoid curves. Computers & Graphics, 33(4), 452-461. doi: http://dx.doi.org/10.1016/j.cag.2009.05.006

McPhee, John E., & Aird, Peter L. (2013). Controlled traffic for vegetable production: Part 1. Machinery challenges and options in a diversified vegetable industry. Biosystems Engineering, 116(2), 144-154. doi: http://dx.doi.org/10.1016/j.biosystemseng.2013.06.001

Montes, N., Herraez, A., Armesto, Leopoldo, & Tornero, Josep. (2008, 19-23 May 2008). Real-time clothoid approximation by Rational Bezier curves. Paper presented at the Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on.

Mueller, Nathaniel D., Gerber, James S., Johnston, Matt, Ray, Deepak K., Ramankutty, Navin, & Foley, Jonathan A. (2012). Closing yield gaps through nutrient and water management. Nature, 490(7419), 254-257. doi: http://www.nature.com/nature/journal/v490/n7419/abs/nature11420.html - supplementary-information

Nawaz, Muhammad, Enscore Jr, E. Emory, & Ham, Inyong. (1983). A heuristic algorithm for the m-machine, n-job flow-shop sequencing problem. Omega, 11(1), 91-95. doi: http://dx.doi.org/10.1016/0305-0483(83)90088-9

Nikolos, I. K., Valavanis, K. P., Tsourveloudis, N. C., & Kostaras, A. N. (2003). Evolutionary algorithm based offline/online path planner for UAV navigation. Systems, Man, and Cybernetics, Part B: Cybernetics, IEEE Transactions on, 33(6), 898-912. doi: 10.1109/TSMCB.2002.804370

Pan, Jia, Zhang, Liangjun, & Manocha, Dinesh. (2012). Collision-free and smooth trajectory computation in cluttered environments. The International Journal of Robotics Research, 31(10), 1155-1175. doi: 10.1177/0278364912453186

Pandey, Gaurav, McBride, James R, & Eustice, Ryan M. (2011). Ford Campus vision and lidar data set. The International Journal of Robotics Research, 30(13), 1543-1552. doi: 10.1177/0278364911400640

Peng, Cheng, & LaValle, S. M. (2001, 2001). Reducing metric sensitivity in randomized trajectory design. Paper presented at the Intelligent Robots and Systems, 2001. Proceedings. 2001 IEEE/RSJ International Conference on.

Reeds, J. A., & Shepp, L. A. (1990). Optimal Paths For A Car That Goes Both Forward And Backward. Pacific Journal of Mathematics, 145(2), 367-393.

Reif, J. H. (1979, 29-31 Oct. 1979). Complexity of the mover's problem and generalizations. Paper presented at the Foundations of Computer Science, 1979., 20th Annual Symposium on.

Rone, William, & Ben-Tzvi, Pinhas. (2013). Mapping, localization and motion planning in mobile multi-robotic systems. Robotica, 31(01), 1-23. doi: doi:10.1017/S0263574712000021

Russell, S.J., & Norvig, P. (2010). Artificial Intelligence: A Modern Approach: Pearson Education/Prentice Hall.

Sabelhaus, Dennis, Röben, Frank, Meyer zu Helligen, Lars Peter, & Schulze Lammers, Peter. (2013). Using continuous-curvature paths to generate feasible headland turn manoeuvres. Biosystems Engineering, 116(4), 399-409. doi: http://dx.doi.org/10.1016/j.biosystemseng.2013.08.012

Siegwart, R., Nourbakhsh, I.R., & Scaramuzza, D. (2011). Introduction to Autonomous Mobile Robots: Mit Press.

25

Slaughter, D. C., Giles, D. K., & Downey, D. (2008). Autonomous robotic weed control systems: A review. Computers and Electronics in Agriculture, 61(1), 63-78. doi: http://dx.doi.org/10.1016/j.compag.2007.05.008

Srinivasa, SiddharthaS, Ferguson, Dave, Helfrich, CaseyJ, Berenson, Dmitry, Collet, Alvaro, Diankov, Rosen, . . . Weghe, MichaelVande. (2010). HERB: a home exploring robotic butler. Autonomous Robots, 28(1), 5-20. doi: 10.1007/s10514-009-9160-9

Sucan, I. A., & Kavraki, L. E. (2010, 3-7 May 2010). On the implementation of single-query sampling-based motion planners. Paper presented at the Robotics and Automation (ICRA), 2010 IEEE International Conference on.

Van Henten, E. J., Hemming, J., Van Tuijl, B. A. J., Kornet, J. G., & Bontsema, J. (2003). Collision-free Motion Planning for a Cucumber Picking Robot. Biosystems Engineering, 86(2), 135-144. doi: http://dx.doi.org/10.1016/S1537-5110(03)00133-8

Van Henten, E. J., Van Tuijl, B. A. J., Hoogakker, G. J., Van Der Weerd, M. J., Hemming, J., Kornet, J. G., & Bontsema, J. (2006). An Autonomous Robot for De-leafing Cucumber Plants grown in a High-wire Cultivation System. Biosystems Engineering, 94(3), 317-323. doi: http://dx.doi.org/10.1016/j.biosystemseng.2006.03.005

Wallace, R., Stentz, A., Thorpe, C., Moravec, H., Whittaker, W., & Kanade, T. (1985). First Results in Robot Road Following.

Xuan-Nam, Bui, Boissonnat, Jean-daniel, Soueres, P., & Laumond, J. P. (1994, 8-13 May 1994). Shortest path synthesis for Dubins non-holonomic robot. Paper presented at the Robotics and Automation, 1994. Proceedings., 1994 IEEE International Conference on.

Zhou, F., Song, B., & Tian, G. (2011). Bézier curve based smooth path planning for mobile robot. Journal of Information and Computational Science, 8(12), 2441-2450.