Car-Like Robot Path Following in Large ... - Semantic Scholar
Car-Like Robot Path Following in Large Unstructured Environments Shahram Rezaei, Jose Guivant, Eduardo M. Nebot ARC Centre of Excellence in Autonomous Systems School of Aerospace, Mechanical and Mechatronic Engineering University of Sydney NSW, Australia Email: s.rezaei/jguivant/[email protected]
Abstract— This paper addresses the problem of on-line path following for a car working in unstructured outdoor environments. The partially known map of the environment is updated and expanded in real time by a Simultaneous Localization and Mapping (SLAM) algorithm. This information is used to implement global path planning. A cost graph is initially constructed followed by a search to find the near-optimal path considering uncertainty in both vehicle location and map. Selected points in the global path are connected by continuous-curvature paths. An improved feedback linearization technique is presented to guide the car along the defined path. Experimental results are presented to demonstrate the performance of the algorithms.
I. I NTRODUCTION During the past few years there has been significant progress in navigation applied to outdoor robots with several industrial applications in well defined environments. At the same time there is still a need of fundamental breakthroughs in autonomous systems to make them reliable in much less structured environments. These systems will have applications in many areas such as search and rescue and fire fighting. Once a reasonable representation of the environment is obtained, the vehicle needs to be controlled to perform a certain path. Path following has three main stages: navigation, path planning and guidance. The navigation module is usually responsible for the localization of the vehicle within a given map. The path planning module deals with defining global as well as local paths and the guidance module is responsible for keeping the car on the defined path within acceptable errors. Generally speaking, there are two categories for path planners: deterministic and probabilistic. In the first category are cell decomposition, roadmap and artificial potential field. In 1998, Yahja et. al. [1] proposed a Framed Quadtrees concept which is basically a cell decomposition method. This work presents experimental results in outdoor environments but does not take into account uncertainty. Stentz improved this algorithm with generalization to globally constrained problems [2]. Incremental construction of Voronoi diagram was addressed by Choset [3]. Then Nagatani et. al. [4] proposed an optimization method to modify the original Voronoi graph to obtain a smooth and traversable path for
car-like robots. However, as noted in the conclusions, the algorithms to create the local path based on Bezier curves are computationally expensive and highly dependent on the tuning parameters. In general these algorithms are designed for the case of working with known static maps and not for cases where the map is built in real time. The first probabilistic motion planner was introduced by Kavraki et. al. [5]. This planner, called Probabilistic RoadMap (PRM), constructs a roadmap off-line during the learning phase that consists of a set of nodes and local path. Then during a query phase the path from a goal to a destination is found based on the nodes obtained in the learning phase. The algorithm is also extended and applied to Car-like robots but still requires complete knowledge of the map [6]. Recently, some new randomized motion planners were developed to perform the same process on-line and considering the vehicle dynamics [7] [8] [9]. The key assumption behind all these planners is that a perfect navigation loop exists, that is they assume a perfect knowledge of the environment, i.e no uncertainty. The real time operation of a car-like vehicle in a large unknown / uncertain environment is still a very challenging problem. So far, most applications consider the existence of a map to design the path that a vehicle should follow to perform a task based on a given optimization criterion. In an unstructured partially known environment, only incremental global path planners are useful since information about the map is acquired incrementally. The use of global path planner requires path re-planning from scratch every time new information is incorporated. There are very few works that considered uncertainty in path planning. In [10], [11], [12] they considered some type of uncertainty in well structured indoor environments. A near real time path follower was introduced by Lambert [13]. This work can be considered near real time, since it performs an off-line simulation in order to generate sensors measurements and calculate uncertainty in any possible vehicle pose. This work has recently been generalized to car-like robots [14] but still requires a map with the uncertainty representation.
In this paper the navigation module will be responsible not only for the localization of the vehicle but for building a map of the environment. This increases the complexity of the problem since in addition to the standard issues of computational cost, kinematics and dynamic constrains of the car, we also add all the problems associated with uncertainties in vehicle position and map. There is a significant number of papers describing SLAM algorithms using different approaches [15], [16], [17], [18]. More recently some works have appeared addressing the consistency aspect of SLAM in very large areas [19], [20] presenting approaches that use topological and features maps. In [21] a new approach is presented to maximize the information that can be incorporated into the maps. This approach is based on a Hybrid Metric Map (HYMM) structure that combines feature maps with any other metric representation such as occupancy grid in a consistent manner. This algorithm splits the global region into several local triangular regions (LTR’s). The advantage of this method is that the traversing cost of each LTR can be evaluated in advance for path planning purposes. In this paper we propose a new method to construct the cost graph. The search over the map, is based on the D* search algorithm presented in [22]. In this stage, uncertainty is incorporated in the cost function. Since continuity of path is crucial for car type robots we choose continuous-curvature local paths, proposed by Fraichard [23]. An improved feedback linearization control algorithm is used to guide the car along this computed reference path. The algorithm presented addresses the singularity and instability problems in the original method when only steering control is used [24]. Experimental results are presented to demonstrate performance of the algorithms. II. NAVIGATION
AND
M AP R EPRESENTATION
Figure (1) shows a flowchart of the tasks required to implement real time path following in unknown environments. The navigation box obtains data from sensors such as encoder, laser, and GPS and provides path planners with vehicle’s pose together with information about the surrounding area. The SLAM algorithm used for navigation utilizes the Hybrid Metric Map (HYMM) structure for mapping [21]. This approach can combine feature maps with any other metric representation. When working with feature based maps, a set of natural or artificial landmarks are used to partition the region covered by the map. These partitions are shown in Figure (2) and consist of a set of connected local triangular regions (LTR). Each LTR is defined by the position of three landmarks called vertices. However, not all the landmarks are needed as vertex points in the LTRs definition. Any point that belongs to an LTR can be defined by a convex linear combination of the three vertex points associated to that sub-region. Similarly, any function of the global position can also be locally defined as a function of its local representation. These functions can be very accurate since the strong correlation of the vertex points makes the position uncertainty of an observer very low in this local frame. For instance, location of
Fig. 1.
Fig. 2.
Flowchart of tasks in real time operation
Local triangular regions and landmarks
an obstacle will be very accurate with respect to the relevant LTR, although it might have a considerable error in the global map. To take advantage of this fact all the properties required for path planning such as obstacle presence, quality of the ground, etc are represented in the local frame. Thus, a function describing the cost of moving from one point to another can then be defined based on local properties. This function will remain constant if the local properties, shape and size of the LTR do not change. This will be the case of a SLAM process, where a set of close landmarks are strongly correlated and will define a triangle with shape and size well defined. Since the LTR’s are used in this work to implement the path planning algorithm the uncertainty in map is inherently considered in this process. III. PATH P LANNING Path planning involves the design and update of global and local trajectories. The global path planner usually implements an optimization algorithm to choose a set of consecutive points in the global environment that lead the vehicle from starting point to the destination point while avoiding obstacles. The path is designed to be ”near-optimal” according to a certain criterion. In this paper, we take into account uncertainty and kinematics constrains of the vehicle. Uncertainty of the
Path Planner
Path Planner 80
10
60 0
Destination 40
Start
20
−20
North (metres)
North (metres)
−10
−30
−40
0
−20
−50
Uncertainty Guider
−40
Relative Distance
−60
−60
Normal Distance
−70 −70
−60
−50
−40 −30 East (metres)
−20
−10
Fig. 3.
Quadtree representation in local triangular regions
0
vehicle pose at any possible location in the map is obtained using an information map (IM) function. The information map function specifies the amount of information that can be obtained by positioning the vehicle at any location in the map. The IM can be built with the information provided by the SLAM algorithm. For most practical purposes the path planner does not require an analytical function to represent IM, but benefits from having knowledge of the areas of good information, that is a set of level sets, each representing areas of at least a given information quality. The level sets will increase their volumes in all directions of the space as the SLAM map improves its quality and gains more information from the environment [21]. In this paper we use the QuadTree concept in order to discretize the map. Figure (3) shows a typical discretization of the space. In this Figure the primary triangles represent the LTRs provided by the SLAM algorithm. They are split into four sub-triangles if there is obstacle inside. This process is repeated until we get the minimum size triangle (maximum grid resolution) that enclose part of the object. Finally the sides of all the sub-triangles are split into small segments. The segment points form nodes of the cost graph. We discard those nodes in the list that are not on the perimeter of any obstacle-embedded triangle. Uncertainty in vehicle’s pose is included in the path planning in the following form: The information distribution function is used to select as uncertainty guides the middle point of the triangles that are known to have good information. Then the minimum normal and relative distance of the vehicle to the guides are determined, Figure (4). Note that normal distances to guides located behind or too far from the car are not selected. If any of the distances is greater than a corresponding threshold, then the path is penalized by an augmentation factor. This is due to the fact that different
−80 −100
−80
−60
−40
−20
0 20 East (metres)
40
60
80
100
Fig. 4. An example of a path generated using QuadTrees representation and A* search considering uncertainty
layers are independent from the probabilistic point of view. In fact, the cost function is modified in the same manner for all different property layers, where each layer represents value of that property as a function of local frames components. Kinematic constrains of the car is also incorporated in the global path planning. Our experimental car can not reverse autonomously and its turning angle is also bounded. In the search for optimum neighbors only those neighbors that are ahead of the line connecting previous pose to current pose are considered. The data structure built by the QuadTrees is searched by D* algorithm to perform re-planning once new information is acquired [22]. Results for a sample map in shown in Figure (4) where the asterisks represent the uncertainty guides. From this Figure it can be appreciated that the algorithm has found a path that passes through information regions and at the same time is near-shortest. In this case D* is equivalent to A* since re-planning has not been implemented. The waypoints defined by the global path planner are set as local start and goal points. To connect these points, we use the Continuous-Curvature path planner (CC-Steer) proposed by Fraichard [23]. Since the algorithm is implemented in a standard car, we have to consider the mechanical limit of steering angle as well as maximum possible rate of steering change. The algorithm has two key parameters to take into account the steering constrains. These parameters are: curvature K and sharpness σ. They are related to steering angle and its rate by ˙ K = tanφ/l ; σ = K˙ = (1 + tan2 φ)φ/(V l)
(1)
where l is length of the car and V is velocity. Since the only control input is steering angle, we chose a conservative smaller value for the maximum steering angle to allow for
X−Y Location 30
25
Y (in meter)
20
15
10
5
0
−5
0 Start Point5
Fig. 5.
10 X (in meter)
15
20
25
Local path planner
unknown modeling errors. Fig. 6.
where x, y (in meters) and θ (in degrees). The area that is swept by the car while moving from one point to another should also be computed. This is essential in order to avoid forbidden zones. We simplify these calculations by evaluating the worst case conditions. Figure (6) shows the sweeping area for two typical types of local paths used to connect global waypoints. Part (a) consists of left turn, straight line, and right turn (lsr) and part (b) includes the sequence left, right and left turns (lrl). The vehicle starts from point A and ends at point B with a different orientation. In practice, the path will have some deviation from the straight line AB. In this work we consider the rectangle parallel to AB and circumference to the path as the sweeping area, which is related to location of circles’ centers and maximum curvature. Figure (7) shows the continuous-curvature path after fitting local paths between waypoints of Figure (4).
Path Planner 80
60 Destination 40
Start
20 North (metres)
According to the CC-Steer method, they are at most six simple continuous-curvature paths between two configurations, defined respectively as lsl, lsr, rsl, rsr, lrl and rlr. In this case, s denotes a line segment and l or r denote a continuouscurvature turn to the left or right. Figure (5) shows a path generated using CC-Steer method for the following waypoints. 0 10 20 x y = 0 15 30 (2) θ 0 90 0
Sweeping area in a local path, (a)lsr, (b)lrl
0
−20
−40
−60
−80 −100
−80
−60
−40
x1 x2 x3 x4
=x = tan φ/(l cos3 θ) = tan θ =y
where, x and y represent the 2D position, φ is the steering angle and θ is the vehicle orientation. This results in the fol-
40
60
80
100
lowing canonical form representation of the motion equations x˙ 1 x˙ 2 x˙ 3 x˙ 4
= u1 = u2 = x 2 u1 = x 3 u1
(4)
when u2 = (v2 +
(3)
0 20 East (metres)
Fig. 7. The continuous-curvature path generated using QuadTrees representation and A* search considering uncertainty
IV. G UIDANCE We use Feedback Linearization theory for the guidance of the car. The following change of coordinates is used [24]
−20
u1 = v1 cos θ 3sinθ sin2 φ u1 )/(l l cos2 θ
cos3 θ cos2 φ)
(5)
v1 and v2 are velocity and steering rate, respectively. In path following, the only control input is steering angle since velocity control is not implemented. Assuming the desired states are ~xd = (xd1 , xd2 , xd3 , xd4 ) (6)
The control input command that linearize the model is u ˜2 = −k1 |u1 (t)|˜ x4 − k2 u1 (t)˜ x3 − k3 |u1 (t)|˜ x2
(7)
u ˜2 = ud2 − u2 ; x ˜i = xdi − xi i = 2, 3, 4
(8)
where
The index d means desired states. The gains ki are chosen to satisfy the Hurwitz stability criterion. In particular by assigning three coincident eigenvalues at −α|u1 | (with α > 0), we can solve for the three gains as [24] k1 = α3 , k2 = 3α2 , k3 = 3α
(9)
The main problems of this algorithm is the singularity at orientation of 90 degree and unstability in the of x component. This is due to the fact that velocity is not a control input. To address these two problems a new transformation is defined y1 y2 y3 y4
=y = tan φ/(l sin3 θ) = −1/ tan θ = −x
Fig. 8.
Error Vs. Time
(10)
2 X Based Y Based
This transformation yields the following motion equations = u01 = u02 = y2 u01 = y3 u01
(11)
u01 = v1 sinθ 3cosθ sin2 φ u01 )/(l l sin2 θ
(12)
Error Y 1
where u02 = (v2 −
1.5
sin3 θ cos2 φ)
In this case the control input is u ˜02 = −k1 |u01 (t)|˜ y4 − k2 u01 (t)˜ y3 − k3 |u01 (t)|˜ y2
Error x/y (meter)
y˙ 1 y˙ 2 y˙ 3 y˙ 4
The utility car used for the experiments
0.5
Error X
0
−0.5
(13) −1
The switching policy is designed is such a form that when cos θ approaches to zero the Y model becomes active. The error of the uncontrollable state can also be reduced with appropriate selection of the switching time. The floating gains technique is used to take into account the steering constrains. This is implemented by adjusting the three gains k1 to k3 to keep v2 below its limit value. V. E XPERIMENTAL R ESULTS The experimental tests are performed using a utility car equipped with a Sick laser range and bearing sensor, GPS, linear variable differential transformer (LVDT) sensor for the steering and back wheel velocity encoder. A photo of the vehicle is shown in Figure (8). More information regarding modeling aspect and other experimental information is available in Ref. [25]. The actual steering limit of this vehicle is 35 degrees in both sides. However, we used a conservative value of 20 degrees to account for other non-modeling errors. Then the value of the sharpness parameter becomes 0.08 (deg/msec), being 0.2 (deg/msec) the maximum value. Figure (9) presents the result for the path shown in Figure
0
2
4
6
8
10
12
14
Time (sec.)
Fig. 9.
Result for a path with initial offset
(5). It is assumed that at the beginning the vehicle position has 1 and -2 meter error along x and y axes, respectively. The value of the controller parameters are listed below v = 3 m/s , , l = 2.83 m Kmax = 0.128 1/m , σmax = 0.0883 1/ms
(14)
Figure (10) shows the actual steering commands versus time needed to follow the reference path while eliminating the errors. The steering curve for the ideal path (without any initial error) is also shown. As can be seen from the figure, the proposed algorithm has no longer singularity problems. Furthermore, it can eventually remove both x and y errors. In this experiment, we use the X-Based system while orientation is less than 45 degrees and the Y-Based system otherwise. The error in the uncontrollable states (y if using Y model and x if using X model) can be reduced adjusting the switching time according the orientation of the vehicle and actual error.
Steering Vs. Time 40 X Based Y Based Reference 30
Steering (deg.)
20
10
0
−10
−20
−30
0
2
4
6
8
10
12
14
Time (sec.)
Fig. 10.
Actual and reference steering commands
VI. C ONCLUSION In this paper we presented incremental path following and guidance algorithms for a car-like robot while considering kinematics and dynamic constrains and uncertainties in vehicle pose and map. The navigation algorithm uses an HMM representation to exploit the benefits of both, efficient feature based localization and rich occupancy grid information. The D* search algorithm was modified in order to consider uncertainty in the cost function. This resulted in a global path planner that finds the near-optimal path in terms of length, path feasibility, and uncertainty. The points selected by the global path planner are connected by the local continuous curvature curves. Feedback linearization technique is used to guide the car along the reference path. Two transformations are presented to overcome the singularity and instability problems for car type vehicles. VII. ACKNOWLEDGMENTS This work is supported by the ARC Centre of Excellence programme, funded by the Australian Research Council (ARC) and the New South Wales State Government. The real time software was developed using QNX Momentix Integrated development Environment. R EFERENCES [1] A. Yahja, S. Singth and A. Stentz, Efficient On-Line Path Planner for Outdoor Mobile Robots, Robotics and Autonomous Systems, 32(2): pp. 129-143, 2000. [2] A. Stentz, CD*: A Real-Time Resolution Optimal Re-Planner for Globally Constrained Problems, American Association for Artificial Intelligence :http://www.frc.ri.cmu.edu/ axs/doc/aaai02.pdf, 2002. [3] H. Choset and J. Burdick, Sensor-Based Exploration: The Hierarchical Generalized Voronoi Graph, International Journal of Robotics Research, 19(2): pp. 96-125, 2000. [4] K. Nagatani, Y. Iwai and Y. Tanaka, Sensor Based Navigation for CarLike Mobile Robots Using Genralized Voronoi Graph, IEEE Int. Conf. on Intelligent Robots and Systems, Hawaii, USA : pp. 1017-1022, 2001.
[5] L. E. Kavraki, P. Svestka and M. H. Overmars, Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces, UU-CS (Ext. r. no. 94-32), Utrecht, 1994. [6] P. Svestka, A Probabilistic Approach to Motion Planning for Car-Like Robots, RUU-CS (Ext. r. no. 93-18), Utrecht, 1993. [7] S. LaValle and J. Kuffner, Randomized Kinodynamic Planning, International Journal of Robotics Research, 20(5): pp. 378-400, 2001. [8] D. Hsu, R. Kindel, J. C. Latombe and S. Rock., Randomized Kinodynamic Motion Planning with Moving Obstacles, International Journal of Robotics Research, 21(3): pp. 233-255, 2002. [9] E. Frazzoli, M. A. Dahleh and E. Feron., Real-Time Motion Planning for Agile Autonomous Vehicles, Journal of Guidance, Control, and Dynamics, 25(1): pp. 116-129, 2002. [10] H. Takeda, C. Facchinetti and J. C. Latombe, Planning the Motions of a Mobile Robot in a Sensory Uncertainty Field, IEEE Transactions on PAMI, 16(10): pp. 1002-1017, 1994. [11] N. Le Fort-Piat,I. Collin and D. Meizel, Planning Robust Displacement Missions by Means of Robot-Tasks and Local Maps, Robotics and Autonomous Systems, 20(1): pp. 99-114, 1997. [12] B. Bouilly, T. Simeon and R. Alami, A Numerical Technique for Planning Motion Stategies of a Mobile Robot in Presence of Uncertainty, IEEE Int. Conf. on Robotics and Automation, Japan: pp. 1327-1332, 1995. [13] A. Lambert and N. Le Fort-Piat, Safe Task Planning Integrating Uncertainties and Local Maps Federations, Int. Journal of Robotics Research, 19(6): pp. 597-611, 2000. [14] A. Lambert and Th. Fraichard, Landmark-Based Safe Path Planning for Car-Like Robots, IEEE Int. Conf. on Robotics and Automation, San Francisco, CA: pp. 2046-2051, 2000. [15] J. Guivant and E. M. Nebot, Optimization of the Simultaneous Localization and Map Building Algorithm for Real Time Implementations, IEEE Trans. on Robotics and Automation, 17(3): pp. 242-257, 2001. [16] J. Guivant and E. M. Nebot, Improved Computational and Memory Requirements of Simultaneous Localization and Map Building, IEEE Int. Conf. on Robotics and Automation, Zurich, Switzerland: pp. 2731-2736, 2002. [17] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, Fastslam: A Factored Solution to the Simultaneous Localization and Map Building, American Association for Artificial Intelligence, 2002. [18] J. J. Leonard and H. J. S. Feder, A Computationally Efficient Method for Large-Scale Concurrent Mapping and Localization, Ninth International Symposium on Robotics Research, Utah: pp. 316-321, 1999. [19] M. Bosse, P. M. Newman, J. J. Leonard and S. Teller, An Atlas Framework for Scalable Mapping, MIT Marine Robotics Laboratory Technical memorandum 2002-04, http://oe.mit.edu/ jleonard/index.html. [20] T. Bailey, Mobile Robot Localisation and Mapping in Extensive Outdoor Environments, PhD Thesis, ACFR: University of Sydney, 2002. [21] J. Guivant, J. Nieto, F. Masson and E. Nebot, Navigation and Mapping in Large Unstructured Environments, Accepted Int. Journal of Robotics Research, [22] A. Stentz, Optimal and Efficient Path Planning for Partially-Known Environments, IEEE Int. Conf. on Robotics and Automation Munich, Germany: pp. 3310-3317, 1994. [23] Th. Fraichard, Continuous-Curvature Path Planning for Car-Like Vehicles, IEEE Int. Conf. on Intelligent Robots and Systems, Grenoble, France : pp. 997-1003, 1997. [24] A. De Luca, G. Oriolo and C. Samson, Feedback Control of a Nonholonomic Car-like Robot, Robot Motion Planning and Control, edited by J.-P. Laumond: Springer-Verlag, 1998. [25] E. Nebot, J. Guivant and J. Nieto, Experimental data set, http://www.acfr.usyd.edu.au/homepages/academic/enebot/dataset.htm.
Abstract— This paper addresses the problem of on-line path following for a car working in unstructured outdoor environments. The partially known map of the environment is updated and expanded in real time by a Simultaneous Localization and Mapping (SLAM) algorithm. This information is used to implement global path planning. A cost graph is initially constructed followed by a search to find the near-optimal path considering uncertainty in both vehicle location and map. Selected points in the global path are connected by continuous-curvature paths. An improved feedback linearization technique is presented to guide the car along the defined path. Experimental results are presented to demonstrate the performance of the algorithms.
I. I NTRODUCTION During the past few years there has been significant progress in navigation applied to outdoor robots with several industrial applications in well defined environments. At the same time there is still a need of fundamental breakthroughs in autonomous systems to make them reliable in much less structured environments. These systems will have applications in many areas such as search and rescue and fire fighting. Once a reasonable representation of the environment is obtained, the vehicle needs to be controlled to perform a certain path. Path following has three main stages: navigation, path planning and guidance. The navigation module is usually responsible for the localization of the vehicle within a given map. The path planning module deals with defining global as well as local paths and the guidance module is responsible for keeping the car on the defined path within acceptable errors. Generally speaking, there are two categories for path planners: deterministic and probabilistic. In the first category are cell decomposition, roadmap and artificial potential field. In 1998, Yahja et. al. [1] proposed a Framed Quadtrees concept which is basically a cell decomposition method. This work presents experimental results in outdoor environments but does not take into account uncertainty. Stentz improved this algorithm with generalization to globally constrained problems [2]. Incremental construction of Voronoi diagram was addressed by Choset [3]. Then Nagatani et. al. [4] proposed an optimization method to modify the original Voronoi graph to obtain a smooth and traversable path for
car-like robots. However, as noted in the conclusions, the algorithms to create the local path based on Bezier curves are computationally expensive and highly dependent on the tuning parameters. In general these algorithms are designed for the case of working with known static maps and not for cases where the map is built in real time. The first probabilistic motion planner was introduced by Kavraki et. al. [5]. This planner, called Probabilistic RoadMap (PRM), constructs a roadmap off-line during the learning phase that consists of a set of nodes and local path. Then during a query phase the path from a goal to a destination is found based on the nodes obtained in the learning phase. The algorithm is also extended and applied to Car-like robots but still requires complete knowledge of the map [6]. Recently, some new randomized motion planners were developed to perform the same process on-line and considering the vehicle dynamics [7] [8] [9]. The key assumption behind all these planners is that a perfect navigation loop exists, that is they assume a perfect knowledge of the environment, i.e no uncertainty. The real time operation of a car-like vehicle in a large unknown / uncertain environment is still a very challenging problem. So far, most applications consider the existence of a map to design the path that a vehicle should follow to perform a task based on a given optimization criterion. In an unstructured partially known environment, only incremental global path planners are useful since information about the map is acquired incrementally. The use of global path planner requires path re-planning from scratch every time new information is incorporated. There are very few works that considered uncertainty in path planning. In [10], [11], [12] they considered some type of uncertainty in well structured indoor environments. A near real time path follower was introduced by Lambert [13]. This work can be considered near real time, since it performs an off-line simulation in order to generate sensors measurements and calculate uncertainty in any possible vehicle pose. This work has recently been generalized to car-like robots [14] but still requires a map with the uncertainty representation.
In this paper the navigation module will be responsible not only for the localization of the vehicle but for building a map of the environment. This increases the complexity of the problem since in addition to the standard issues of computational cost, kinematics and dynamic constrains of the car, we also add all the problems associated with uncertainties in vehicle position and map. There is a significant number of papers describing SLAM algorithms using different approaches [15], [16], [17], [18]. More recently some works have appeared addressing the consistency aspect of SLAM in very large areas [19], [20] presenting approaches that use topological and features maps. In [21] a new approach is presented to maximize the information that can be incorporated into the maps. This approach is based on a Hybrid Metric Map (HYMM) structure that combines feature maps with any other metric representation such as occupancy grid in a consistent manner. This algorithm splits the global region into several local triangular regions (LTR’s). The advantage of this method is that the traversing cost of each LTR can be evaluated in advance for path planning purposes. In this paper we propose a new method to construct the cost graph. The search over the map, is based on the D* search algorithm presented in [22]. In this stage, uncertainty is incorporated in the cost function. Since continuity of path is crucial for car type robots we choose continuous-curvature local paths, proposed by Fraichard [23]. An improved feedback linearization control algorithm is used to guide the car along this computed reference path. The algorithm presented addresses the singularity and instability problems in the original method when only steering control is used [24]. Experimental results are presented to demonstrate performance of the algorithms. II. NAVIGATION
AND
M AP R EPRESENTATION
Figure (1) shows a flowchart of the tasks required to implement real time path following in unknown environments. The navigation box obtains data from sensors such as encoder, laser, and GPS and provides path planners with vehicle’s pose together with information about the surrounding area. The SLAM algorithm used for navigation utilizes the Hybrid Metric Map (HYMM) structure for mapping [21]. This approach can combine feature maps with any other metric representation. When working with feature based maps, a set of natural or artificial landmarks are used to partition the region covered by the map. These partitions are shown in Figure (2) and consist of a set of connected local triangular regions (LTR). Each LTR is defined by the position of three landmarks called vertices. However, not all the landmarks are needed as vertex points in the LTRs definition. Any point that belongs to an LTR can be defined by a convex linear combination of the three vertex points associated to that sub-region. Similarly, any function of the global position can also be locally defined as a function of its local representation. These functions can be very accurate since the strong correlation of the vertex points makes the position uncertainty of an observer very low in this local frame. For instance, location of
Fig. 1.
Fig. 2.
Flowchart of tasks in real time operation
Local triangular regions and landmarks
an obstacle will be very accurate with respect to the relevant LTR, although it might have a considerable error in the global map. To take advantage of this fact all the properties required for path planning such as obstacle presence, quality of the ground, etc are represented in the local frame. Thus, a function describing the cost of moving from one point to another can then be defined based on local properties. This function will remain constant if the local properties, shape and size of the LTR do not change. This will be the case of a SLAM process, where a set of close landmarks are strongly correlated and will define a triangle with shape and size well defined. Since the LTR’s are used in this work to implement the path planning algorithm the uncertainty in map is inherently considered in this process. III. PATH P LANNING Path planning involves the design and update of global and local trajectories. The global path planner usually implements an optimization algorithm to choose a set of consecutive points in the global environment that lead the vehicle from starting point to the destination point while avoiding obstacles. The path is designed to be ”near-optimal” according to a certain criterion. In this paper, we take into account uncertainty and kinematics constrains of the vehicle. Uncertainty of the
Path Planner
Path Planner 80
10
60 0
Destination 40
Start
20
−20
North (metres)
North (metres)
−10
−30
−40
0
−20
−50
Uncertainty Guider
−40
Relative Distance
−60
−60
Normal Distance
−70 −70
−60
−50
−40 −30 East (metres)
−20
−10
Fig. 3.
Quadtree representation in local triangular regions
0
vehicle pose at any possible location in the map is obtained using an information map (IM) function. The information map function specifies the amount of information that can be obtained by positioning the vehicle at any location in the map. The IM can be built with the information provided by the SLAM algorithm. For most practical purposes the path planner does not require an analytical function to represent IM, but benefits from having knowledge of the areas of good information, that is a set of level sets, each representing areas of at least a given information quality. The level sets will increase their volumes in all directions of the space as the SLAM map improves its quality and gains more information from the environment [21]. In this paper we use the QuadTree concept in order to discretize the map. Figure (3) shows a typical discretization of the space. In this Figure the primary triangles represent the LTRs provided by the SLAM algorithm. They are split into four sub-triangles if there is obstacle inside. This process is repeated until we get the minimum size triangle (maximum grid resolution) that enclose part of the object. Finally the sides of all the sub-triangles are split into small segments. The segment points form nodes of the cost graph. We discard those nodes in the list that are not on the perimeter of any obstacle-embedded triangle. Uncertainty in vehicle’s pose is included in the path planning in the following form: The information distribution function is used to select as uncertainty guides the middle point of the triangles that are known to have good information. Then the minimum normal and relative distance of the vehicle to the guides are determined, Figure (4). Note that normal distances to guides located behind or too far from the car are not selected. If any of the distances is greater than a corresponding threshold, then the path is penalized by an augmentation factor. This is due to the fact that different
−80 −100
−80
−60
−40
−20
0 20 East (metres)
40
60
80
100
Fig. 4. An example of a path generated using QuadTrees representation and A* search considering uncertainty
layers are independent from the probabilistic point of view. In fact, the cost function is modified in the same manner for all different property layers, where each layer represents value of that property as a function of local frames components. Kinematic constrains of the car is also incorporated in the global path planning. Our experimental car can not reverse autonomously and its turning angle is also bounded. In the search for optimum neighbors only those neighbors that are ahead of the line connecting previous pose to current pose are considered. The data structure built by the QuadTrees is searched by D* algorithm to perform re-planning once new information is acquired [22]. Results for a sample map in shown in Figure (4) where the asterisks represent the uncertainty guides. From this Figure it can be appreciated that the algorithm has found a path that passes through information regions and at the same time is near-shortest. In this case D* is equivalent to A* since re-planning has not been implemented. The waypoints defined by the global path planner are set as local start and goal points. To connect these points, we use the Continuous-Curvature path planner (CC-Steer) proposed by Fraichard [23]. Since the algorithm is implemented in a standard car, we have to consider the mechanical limit of steering angle as well as maximum possible rate of steering change. The algorithm has two key parameters to take into account the steering constrains. These parameters are: curvature K and sharpness σ. They are related to steering angle and its rate by ˙ K = tanφ/l ; σ = K˙ = (1 + tan2 φ)φ/(V l)
(1)
where l is length of the car and V is velocity. Since the only control input is steering angle, we chose a conservative smaller value for the maximum steering angle to allow for
X−Y Location 30
25
Y (in meter)
20
15
10
5
0
−5
0 Start Point5
Fig. 5.
10 X (in meter)
15
20
25
Local path planner
unknown modeling errors. Fig. 6.
where x, y (in meters) and θ (in degrees). The area that is swept by the car while moving from one point to another should also be computed. This is essential in order to avoid forbidden zones. We simplify these calculations by evaluating the worst case conditions. Figure (6) shows the sweeping area for two typical types of local paths used to connect global waypoints. Part (a) consists of left turn, straight line, and right turn (lsr) and part (b) includes the sequence left, right and left turns (lrl). The vehicle starts from point A and ends at point B with a different orientation. In practice, the path will have some deviation from the straight line AB. In this work we consider the rectangle parallel to AB and circumference to the path as the sweeping area, which is related to location of circles’ centers and maximum curvature. Figure (7) shows the continuous-curvature path after fitting local paths between waypoints of Figure (4).
Path Planner 80
60 Destination 40
Start
20 North (metres)
According to the CC-Steer method, they are at most six simple continuous-curvature paths between two configurations, defined respectively as lsl, lsr, rsl, rsr, lrl and rlr. In this case, s denotes a line segment and l or r denote a continuouscurvature turn to the left or right. Figure (5) shows a path generated using CC-Steer method for the following waypoints. 0 10 20 x y = 0 15 30 (2) θ 0 90 0
Sweeping area in a local path, (a)lsr, (b)lrl
0
−20
−40
−60
−80 −100
−80
−60
−40
x1 x2 x3 x4
=x = tan φ/(l cos3 θ) = tan θ =y
where, x and y represent the 2D position, φ is the steering angle and θ is the vehicle orientation. This results in the fol-
40
60
80
100
lowing canonical form representation of the motion equations x˙ 1 x˙ 2 x˙ 3 x˙ 4
= u1 = u2 = x 2 u1 = x 3 u1
(4)
when u2 = (v2 +
(3)
0 20 East (metres)
Fig. 7. The continuous-curvature path generated using QuadTrees representation and A* search considering uncertainty
IV. G UIDANCE We use Feedback Linearization theory for the guidance of the car. The following change of coordinates is used [24]
−20
u1 = v1 cos θ 3sinθ sin2 φ u1 )/(l l cos2 θ
cos3 θ cos2 φ)
(5)
v1 and v2 are velocity and steering rate, respectively. In path following, the only control input is steering angle since velocity control is not implemented. Assuming the desired states are ~xd = (xd1 , xd2 , xd3 , xd4 ) (6)
The control input command that linearize the model is u ˜2 = −k1 |u1 (t)|˜ x4 − k2 u1 (t)˜ x3 − k3 |u1 (t)|˜ x2
(7)
u ˜2 = ud2 − u2 ; x ˜i = xdi − xi i = 2, 3, 4
(8)
where
The index d means desired states. The gains ki are chosen to satisfy the Hurwitz stability criterion. In particular by assigning three coincident eigenvalues at −α|u1 | (with α > 0), we can solve for the three gains as [24] k1 = α3 , k2 = 3α2 , k3 = 3α
(9)
The main problems of this algorithm is the singularity at orientation of 90 degree and unstability in the of x component. This is due to the fact that velocity is not a control input. To address these two problems a new transformation is defined y1 y2 y3 y4
=y = tan φ/(l sin3 θ) = −1/ tan θ = −x
Fig. 8.
Error Vs. Time
(10)
2 X Based Y Based
This transformation yields the following motion equations = u01 = u02 = y2 u01 = y3 u01
(11)
u01 = v1 sinθ 3cosθ sin2 φ u01 )/(l l sin2 θ
(12)
Error Y 1
where u02 = (v2 −
1.5
sin3 θ cos2 φ)
In this case the control input is u ˜02 = −k1 |u01 (t)|˜ y4 − k2 u01 (t)˜ y3 − k3 |u01 (t)|˜ y2
Error x/y (meter)
y˙ 1 y˙ 2 y˙ 3 y˙ 4
The utility car used for the experiments
0.5
Error X
0
−0.5
(13) −1
The switching policy is designed is such a form that when cos θ approaches to zero the Y model becomes active. The error of the uncontrollable state can also be reduced with appropriate selection of the switching time. The floating gains technique is used to take into account the steering constrains. This is implemented by adjusting the three gains k1 to k3 to keep v2 below its limit value. V. E XPERIMENTAL R ESULTS The experimental tests are performed using a utility car equipped with a Sick laser range and bearing sensor, GPS, linear variable differential transformer (LVDT) sensor for the steering and back wheel velocity encoder. A photo of the vehicle is shown in Figure (8). More information regarding modeling aspect and other experimental information is available in Ref. [25]. The actual steering limit of this vehicle is 35 degrees in both sides. However, we used a conservative value of 20 degrees to account for other non-modeling errors. Then the value of the sharpness parameter becomes 0.08 (deg/msec), being 0.2 (deg/msec) the maximum value. Figure (9) presents the result for the path shown in Figure
0
2
4
6
8
10
12
14
Time (sec.)
Fig. 9.
Result for a path with initial offset
(5). It is assumed that at the beginning the vehicle position has 1 and -2 meter error along x and y axes, respectively. The value of the controller parameters are listed below v = 3 m/s , , l = 2.83 m Kmax = 0.128 1/m , σmax = 0.0883 1/ms
(14)
Figure (10) shows the actual steering commands versus time needed to follow the reference path while eliminating the errors. The steering curve for the ideal path (without any initial error) is also shown. As can be seen from the figure, the proposed algorithm has no longer singularity problems. Furthermore, it can eventually remove both x and y errors. In this experiment, we use the X-Based system while orientation is less than 45 degrees and the Y-Based system otherwise. The error in the uncontrollable states (y if using Y model and x if using X model) can be reduced adjusting the switching time according the orientation of the vehicle and actual error.
Steering Vs. Time 40 X Based Y Based Reference 30
Steering (deg.)
20
10
0
−10
−20
−30
0
2
4
6
8
10
12
14
Time (sec.)
Fig. 10.
Actual and reference steering commands
VI. C ONCLUSION In this paper we presented incremental path following and guidance algorithms for a car-like robot while considering kinematics and dynamic constrains and uncertainties in vehicle pose and map. The navigation algorithm uses an HMM representation to exploit the benefits of both, efficient feature based localization and rich occupancy grid information. The D* search algorithm was modified in order to consider uncertainty in the cost function. This resulted in a global path planner that finds the near-optimal path in terms of length, path feasibility, and uncertainty. The points selected by the global path planner are connected by the local continuous curvature curves. Feedback linearization technique is used to guide the car along the reference path. Two transformations are presented to overcome the singularity and instability problems for car type vehicles. VII. ACKNOWLEDGMENTS This work is supported by the ARC Centre of Excellence programme, funded by the Australian Research Council (ARC) and the New South Wales State Government. The real time software was developed using QNX Momentix Integrated development Environment. R EFERENCES [1] A. Yahja, S. Singth and A. Stentz, Efficient On-Line Path Planner for Outdoor Mobile Robots, Robotics and Autonomous Systems, 32(2): pp. 129-143, 2000. [2] A. Stentz, CD*: A Real-Time Resolution Optimal Re-Planner for Globally Constrained Problems, American Association for Artificial Intelligence :http://www.frc.ri.cmu.edu/ axs/doc/aaai02.pdf, 2002. [3] H. Choset and J. Burdick, Sensor-Based Exploration: The Hierarchical Generalized Voronoi Graph, International Journal of Robotics Research, 19(2): pp. 96-125, 2000. [4] K. Nagatani, Y. Iwai and Y. Tanaka, Sensor Based Navigation for CarLike Mobile Robots Using Genralized Voronoi Graph, IEEE Int. Conf. on Intelligent Robots and Systems, Hawaii, USA : pp. 1017-1022, 2001.
[5] L. E. Kavraki, P. Svestka and M. H. Overmars, Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces, UU-CS (Ext. r. no. 94-32), Utrecht, 1994. [6] P. Svestka, A Probabilistic Approach to Motion Planning for Car-Like Robots, RUU-CS (Ext. r. no. 93-18), Utrecht, 1993. [7] S. LaValle and J. Kuffner, Randomized Kinodynamic Planning, International Journal of Robotics Research, 20(5): pp. 378-400, 2001. [8] D. Hsu, R. Kindel, J. C. Latombe and S. Rock., Randomized Kinodynamic Motion Planning with Moving Obstacles, International Journal of Robotics Research, 21(3): pp. 233-255, 2002. [9] E. Frazzoli, M. A. Dahleh and E. Feron., Real-Time Motion Planning for Agile Autonomous Vehicles, Journal of Guidance, Control, and Dynamics, 25(1): pp. 116-129, 2002. [10] H. Takeda, C. Facchinetti and J. C. Latombe, Planning the Motions of a Mobile Robot in a Sensory Uncertainty Field, IEEE Transactions on PAMI, 16(10): pp. 1002-1017, 1994. [11] N. Le Fort-Piat,I. Collin and D. Meizel, Planning Robust Displacement Missions by Means of Robot-Tasks and Local Maps, Robotics and Autonomous Systems, 20(1): pp. 99-114, 1997. [12] B. Bouilly, T. Simeon and R. Alami, A Numerical Technique for Planning Motion Stategies of a Mobile Robot in Presence of Uncertainty, IEEE Int. Conf. on Robotics and Automation, Japan: pp. 1327-1332, 1995. [13] A. Lambert and N. Le Fort-Piat, Safe Task Planning Integrating Uncertainties and Local Maps Federations, Int. Journal of Robotics Research, 19(6): pp. 597-611, 2000. [14] A. Lambert and Th. Fraichard, Landmark-Based Safe Path Planning for Car-Like Robots, IEEE Int. Conf. on Robotics and Automation, San Francisco, CA: pp. 2046-2051, 2000. [15] J. Guivant and E. M. Nebot, Optimization of the Simultaneous Localization and Map Building Algorithm for Real Time Implementations, IEEE Trans. on Robotics and Automation, 17(3): pp. 242-257, 2001. [16] J. Guivant and E. M. Nebot, Improved Computational and Memory Requirements of Simultaneous Localization and Map Building, IEEE Int. Conf. on Robotics and Automation, Zurich, Switzerland: pp. 2731-2736, 2002. [17] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, Fastslam: A Factored Solution to the Simultaneous Localization and Map Building, American Association for Artificial Intelligence, 2002. [18] J. J. Leonard and H. J. S. Feder, A Computationally Efficient Method for Large-Scale Concurrent Mapping and Localization, Ninth International Symposium on Robotics Research, Utah: pp. 316-321, 1999. [19] M. Bosse, P. M. Newman, J. J. Leonard and S. Teller, An Atlas Framework for Scalable Mapping, MIT Marine Robotics Laboratory Technical memorandum 2002-04, http://oe.mit.edu/ jleonard/index.html. [20] T. Bailey, Mobile Robot Localisation and Mapping in Extensive Outdoor Environments, PhD Thesis, ACFR: University of Sydney, 2002. [21] J. Guivant, J. Nieto, F. Masson and E. Nebot, Navigation and Mapping in Large Unstructured Environments, Accepted Int. Journal of Robotics Research, [22] A. Stentz, Optimal and Efficient Path Planning for Partially-Known Environments, IEEE Int. Conf. on Robotics and Automation Munich, Germany: pp. 3310-3317, 1994. [23] Th. Fraichard, Continuous-Curvature Path Planning for Car-Like Vehicles, IEEE Int. Conf. on Intelligent Robots and Systems, Grenoble, France : pp. 997-1003, 1997. [24] A. De Luca, G. Oriolo and C. Samson, Feedback Control of a Nonholonomic Car-like Robot, Robot Motion Planning and Control, edited by J.-P. Laumond: Springer-Verlag, 1998. [25] E. Nebot, J. Guivant and J. Nieto, Experimental data set, http://www.acfr.usyd.edu.au/homepages/academic/enebot/dataset.htm.
