# Path Planning and Replanning for Mobile Robot Navigation on 3D Terrain: An Approach Based on Geodesic.

1. IntroductionPath planning is a complex problem whose aim is to find a suitable collision-free path which satisfies certain criteria and constraints. It has been of theoretical interests and practical relevance for decades [1, 2]. A lasting focus for path planning and replanning algorithms is on generating a safe path that minimizes the length metric, one of the most desirable performance metrics for diverse applications, since the length of a parametrized path is unchanged under reparametrization. This is because the distance traveled along a curve between two points is independent of the speed moving on it. Path planning has a wide range of applications. Vehicle navigation in different environments such as ground, air, space, and underwater requires the vehicle to autonomously move from one location to another. This autonomy is accomplished partly by the functions of path planning which generates a path for the vehicle to follow and the generation of steering commands for the vehicle to follow a given path. The generated path could also be combined with a velocity planning stage to produce a trajectory for the vehicle to track. There are other domains that require path planning as a part of the solutions or tasks that can be formulated as (or simplified as, transformed into, or reduced to) a variety of path planning problems. In wireless sensor networks, mobile robots that move along appropriately planned paths are used for monitoring the environment or collecting data efficiently. For industrial applications of robot arms in a manufacturing cell, path planning is an output of robotic task sequencing whose goal is to find an optimal sequence of multiple tasks to be completed by a robot (i.e., a travelling salesman problem with or without neighborhood) [3] and the path connecting the task sequence. The obtained path points are then converted into joint angles of robot arm via the inverse kinematics solver.

A key prerequisite for path planning is that the vehicle can find a traversable path and by closely following the path it can move safely within its task environment. In general, the path planning problem refers to the combination of three essentials. The first is that an initial location and a terminal location are given a priori. The second is the geometric or topological description and representation of task environment constructed from point cloud data. The (topological, graph, or metric) model of environment maybe known, partially known, or even unknown for robot motion planning and control. In fact, the terrain characteristics (such as variations in roughness, elevation/slope, or curvature) and the associated obstacle property such as the number, the size, the shape, and its motion or deformation are related to the environmental situation that should be analyzed in the planning stage. The last is algorithms to generate a collision-free geometric path connecting the initial and the terminal location, with the typical objective of minimizing a user-defined travel cost such as the path length. Motion constraints of vehicle mobility could also be incorporated into the algorithm to make the path kinematically or dynamically feasible.

Significant complexity of the geometry (manifold) structure of the terrain causes the path planning on 3D (or higher-dimensional) terrain to have certain characteristics quite different from the path planning on the plane, as noted in [4-6]. Compared to the numerous contributions to path planning on 2D planar environments, there is a clear need for further research of methods and algorithms for 3D path planning to apply to the autonomous ground/aerial/underwater vehicles or articulated or parallel robot manipulators.

Our Approach. In view of generality of 3D path arising from the nonlinear nature of the spatial curves and terrains in diverse applications, motivated from autonomous guided vehicles moving on nonflat ground, we restrict ourselves to a practical instance of geometric path planning, that is, the generation of a geometric path confined on a two-dimensional surface. Suppose that the terrain that the robot traverses is constructed by a smooth, complete Riemannian manifold M which is embedded in the Euclidean space [E.sup.3] [1, 7-9]. Regarding the obstacle, we recharacterize the obstacle as its enclosing circle on the terrain with radius [r.sub.[OMICRON]] = [OMICRON]r, where [r.sub.[OMICRON]] is denoted as radius of hazardous region, [OMICRON] is regarded as hazard grade, and r is the radius of enclosing circle. We are particularly interested in using the geodesic [8, 10-12] for planning and replanning a path for mobile robot navigation on 3D terrain.

As will be stated formally later in Section 2, a geodesic linking the given initial position and terminal position has the following properties for navigation in addition to shortest length: it is continuously differentiable; it is unit speed with velocity in the tangent direction so that the maximum velocity constraint is easily handled. In other words, its acceleration is normal to the manifold so that the geodesic curvature is zero along the geodesic, and thus the two-point boundary value problem (TPBVP) arises from geodesic differential equations on Riemannian manifold. Given the terrain, the initial position, and the terminal position on the terrain, the initial geodesic connecting them, and the single obstacle therein, the proposed geodesic replanning algorithm consists of the steps of directional scanning, critical point selection, path generation, and collision checking. It relies on the properties of the geodesic and the directional scanning on the tangent plane for exploration of terrain. The outcome of this procedure is two distinct composite geodesic paths connecting the initial and the critical points and the terminal entirely confined on the terrain. Furthermore, the new composite path and the initial path form a triangular region, called geodesic triangle, which satisfies the local Gauss-Bonnet Theorem. This theorem serves as guidance for selecting a better replanned path and terrain at the planning stage.

Related Work on 3D Path Planning. There are a number of methods and concepts available for efficient 2D path and motion planning of mobile robot, which could be a mobile platform or a wheelchair, in planar maps. Some are interested in employing parametric curves other than circular arcs and line segments for planning a 2D path meeting smoothness requirements or for smoothing the paths, so that the obtained paths could be actually executed without unnecessary stops or velocity discontinuities by the robots with specific shapes, kinematics, and dynamics. Building on these progresses in path planning in planar maps, researchers and engineers have been increasingly interested in generating geometric representations of 3D terrain from raw point cloud obtained from range sensors without plane assumption (e.g., B-spline patches [13] and tensor voting framework in combination with fc-nearest neighbor [7]). In [14], the terrain is represented by B-spline patches, while the B-spline path is generated by Hamilton-Jacobi-Bellman equation. Reference [5] selected a B-spline curve to link two distinct locations and avoid collisions with the obstacles in 3D environment or 3D terrain. The established techniques and novel concepts of 2D path planning like optimization, metaheuristic, grid search algorithms, and potential fields could be extended with some success to plan a 3D path ([4, 15-19] to cite a few). The potential field approach that navigates a robot to the target position by the sum of attractive force exerted at the goal and the repulsive forces from the obstacles is efficient and simple, which is applicable in two-dimensional and three-dimensional environments. Reference [16] applied the artificial potential field in combination with curve shortening flow method for avoiding collisions with obstacles in a dynamic three-dimensional environment. Reference [17] used grid-based elevation map for mobile robot obstacle avoidance. Reference [20] proposed [AtlasRRT.sup.*] to extend the popular, state-of-the-art path planning algorithm RRT (rapidly exploring random tree) [1] on the plane to k (dimensional)-manifold such as torus. Reference [7] designed a Riemannian metric on point cloud to reflect the surface for finding the most desirable direction (as flat as possible) for traversal with reduced energy consumption.

Suppose that an initial path is given in an environment. In case the initial route crosses the obstacle, the path should be replanned for obstacle avoidance. Visibility-based obstacle-avoidance approach is often employed for polygonal obstacles [2]. In order to bypass the obstacle of general shape encountered by the robot, one approach commonly used in 2D environment is to follow a path that is tangent to the circumference of the obstacle in front of the robot until there is a collision-free path toward the goal. In a 2D plane, there are only two directions of motion to follow: counterclockwise or clockwise. By contrast, in a 3D environment, there are an infinite number of tangent directions of motion on the surface of obstacle of general shape which causes planning difficulty [21, 22] if without a priori or online traversability analysis of terrain, since the terrain traversability may obstruct the robot to successfully traverse on the boundary surface of an obstacle of general shape in 3D environment. Reference [23] suggested the use of geodesic between two specified points on the obstacle surface as the boundary following path in 3D environment. Meanwhile, without considering the typical objective of path length and terrain traversability information, successful application of boundary following imposes a restriction on the shape of the obstacles and may often yield a longer time- and energy-consuming path and slow down the movement.

The remainder of the paper is organized as follows. Section 2 introduces the differential geometry of 3D terrain constructed by the Riemannian manifold and the system of geodesic differential equations which is used as a control system for navigating the mobile robot along a path composed of geodesics. Section 3 describes the geodesic replanning procedure accommodating the collision constraint of the robot, which is based on the representation of the obstacle as a circle with a hazard grade tunable radius. Associated with geodesic replanning procedure, local Gauss-Bonnet Theorem of the geodesic triangle formed by the initial geodesic and the replanned geodesics will also be presented for aiding traversability analysis of the terrain that the replanned path traverses in Section 3. Simulation result is given in Section 4 for demonstrating the geodesic replanning procedure. The conclusion is made in Section 5.

2. Shortest Path Planning on Manifold

The terrain on which the robot operates is assumed to be a smooth manifold. In this section, we will work on the geometric framework of manifold M without reference to a specific, explicit coordinate frame and give a fundamental comprehension of geodesic on manifold M. In order to maintain some focus in our work on path planning, we provide a very brief introduction of geometry setup related to geodesic path planning by defining several mathematical objects. Details are referred to the references cited in the texts. Thereby, this section is split into three subsections in the following.

2.1. A Brief on Manifold. For path planning of a mobile robot modelled as a point, the terrain profile is modelled as the smooth surface embedded in the Euclidean three-dimensional space [E.sup.3], called manifold. Intuitively, a manifold M embedded in [E.sup.3] could be considered as a Euclidean space [1, 24]. Examples of manifolds are sphere, torus, and parametric surfaces. Here, let M denote a manifold and we give some most general definition of a manifold as follows [24].

Definition 1. A topological space M is called a topological 3-manifold if

(1) M is Hausdorff and second countable;

(2) M is locally Euclidean. Every point has an open ball U [subset] M that is homeomorphic to an open subset V [subset] [R.sup.3] such that a homeomorphism [phi] : U [right arrow] V is called a coordinate chart.

An atlas is a family of charts {[U.sub.[alpha]], [[phi].sub.[alpha]]} for which [U.sub.[alpha]] constitutes open covering of M; that is, M = [[union].sub.[alpha][member of]I][U.sub.[alpha]]. With an atlas defined on M, given a point P in M and a coordinate chart about P, a tangent vector to M at P can be defined as generalization of the usual notion of tangent vector in Euclidean space using the directional derivative of a function or curve along the direction of tangent vector [24]. For a curve [gamma], the tangent vector at P is also the velocity vector of [gamma] at P with the speed and direction of the motion the same as the length and direction of the tangent vector. The tangent plane at a point P [member of] M on a curve is denoted by [T.sub.p]M, which is the vector space formed by the set of all tangent vectors to M at P; it is determined by its basis vectors on [T.sub.p]M and does not depend on the choice of coordinate chart. In a given local coordinate chart ([u.sub.1], [u.sub.2]), at each point P, the partial velocities ([mathematical expression not reproducible]) along the coordinate curves are linearly independent and thus could serve as a basis for [T.sub.p]M, where [mathematical expression not reproducible], j = 1, 2. Any tangent vector on [T.sub.p]M can thus be represented by the basis vectors ([mathematical expression not reproducible]). Let (M, g) denote a Riemannian manifold, where g = ([g.sub.ij]) is a Riemannian metric which defines the inner product of tangent vectors on [T.sub.p]M (note that, in a Riemannian manifold, each tangent vector has to be attached with a point P of the manifold. In a given local coordinate chart ([u.sub.1], [u.sub.2]), a Riemannian metric g = ([g.sub.ij]) is a symmetric, positive-definite quadratic form: [(ds).sup.2] = [g.sub.11] ([u.sub.1], [u.sub.2]) [du.sup.2.sub.1] + 2[g.sub.12] ([u.sub.1], [u.sub.2]) [du.sub.1] [du.sub.2] + [g.sub.22] ([u.sub.1], [u.sub.2]) [du.sup.2.sub.2], where s is the arc length. A Riemannian metric reflects the nonlinearity of the space). Given a Riemannian metric g = ([g.sub.ij]), the Christoffel symbol [[GAMMA].sup.k.sub.ij] is uniquely defined in terms of the metric tensor ([g.sub.ij]) composed by the first and second differentials of the Riemannian metric.

Definition 2. Consider

[mathematical expression not reproducible], (1)

where ([g.sup.ij]) = [([g.sub.ij]).sup.-1] is the inverse matrix to the metric tensor ([g.sub.ij]) (i.e., [g.sup.ik] [g.sub.kj] = 0 if i [not equal to] j or 1 if i = j).

Note that [[GAMMA].sup.k.sub.ij] = 0; ([g.sub.ij]) is an identity matrix in Cartesian coordinate. An operator [??] called the covariant derivative along a curve [gamma] on the manifold M is generalization of directional derivative of vector fields. It operates on a pair of vector fields X = ([X.sup.1], [X.sup.2]) and Y = ([Y.sup.1], [Y.sup.2]) tangent to M along [gamma] at P to produce a new vector field [[??].sub.X] Y at P as the rate of change of Y in X(P) direction in the basis ([mathematical expression not reproducible]).

Definition 3. Let X, Y [member of] [T.sub.p]M for P [member of] M be two vector fields. Then the covariant derivative of the tangent vector field Y with respect to the tangent vector field X is the operation [mathematical expression not reproducible].

2.2. Geodesic. Due to its locally shortest length and other nice properties (such as the induced near-minimum time property [6]), connecting (or interpolating) two configurations via geodesic has attracted much attention in robot arm and mobile robot path planning [6-8, 10-12, 25, 26] and nonlinear control of mechanical systems [27-29]. Nonlinear control based on contraction analysis requires that at any fixed time the actual system state on state manifold be driven via a geodesic toward a target state of a virtual system [27]. For this application, online computation of geodesic is essential for real-time nonlinear control. For control of redundant constrained mechanical system, such as human arm or human-like robotic arm, for the task of handwriting, grasping, and manipulation of objects, the desired arm path on the configuration manifold, modelled as a Riemannian manifold with appropriately defined Riemannian metric (the configuration-dependent inertia matrix), is a geodesic from the initial posture on a submanifold to the desired posture on the other submanifold [28]. The length of a path is the total energy from one configuration to the other configuration needed by following the geodesic between the two configurations. This implies that the geodesic is the path satisfying the least action principle of mechanics. Reference [29] considers the geodesics in the space of pointing directions of head/eye movement modelled as Euler-Lagrange equations.

Let [[kappa].sub.g] denote the geodesic curvature of a curve [gamma] at a point P [member of] [gamma] which measures how a curve [gamma] deviates from being a geodesic [24, 30-33].

Definition 4. A path [gamma] : [[epsilon], [delta]] [subset] I [right arrow] M is called a geodesic if and only if the tangent vector [gamma]' to [gamma] is parallel along [gamma], where ()' denotes derivative with respect to the path parameter s:

[[kappa].sub.g] = [[??].sub.[gamma]'] [gamma]' = 0, (2)

where [??] denotes the covariant derivative defined in Definition 3 with X replaced by a tangent vector [gamma]'.

As shown in Figure 1, in general, the acceleration [gamma]" of a path [gamma] can be decomposed to two orthogonal components: a normal component [y".sub.nor] in the direction of principal normal and a tangential component composed of the covariant derivative [y".sub.tan] = [[??].sub.[gamma]'] [gamma]' of the velocity [gamma]' in the direction of tangent [gamma]' along the path [gamma] (also called geometric/intrinsic acceleration) [24, 30-32]. This tangential component of acceleration is caused by the rotation of the basis vectors spanning the tangent plane due to the movement in [gamma]' direction. The following are equivalent characterizations of a geodesic [30, 31, 34]:

(1) [gamma] is the locally shortest path.

(2) [gamma] has vanishing geodesic curvature: [[kappa].sub.g] [equivalent to] 0.

(3) The acceleration [gamma]" is normal to [T.sub.[gamma]]M (i.e., the angle between its acceleration and velocity is 90 degrees everywhere on [gamma] or a coincidence of the principal normal of [gamma] with the surface normal). The geodesic has purely normal/centripetal acceleration.

Therefore, from the characterization that [parallel][gamma]'[parallel] is constant for geodesic [gamma], the notion of a geodesic implies a curve [gamma] with constant parametric speed, that is, parametrized by proportional to arc length, and zero tangential/longitudinal acceleration, while the normal/lateral acceleration is to keep the geodesic confined on the manifold. In addition, null geodesic curvature intuitively says that geodesics have no curvature other than the curvature of the manifold itself.

To begin with, we consider the planning of lo cally shortest path for a point robot linking two distinct locations on the manifold M without obstacles. Denote the set of all piecewise continuously differentiable paths on M which start at the point a and end at the point b by [C.sup.b.sub.a] [member of] M. Given a curve [gamma] [member of] [mathematical expression not reproducible] and given a continuously differentiable mapping of coordinate chart X : U [right arrow] M, where U [subset] [R.sup.2] is an open set parametrized by the local coordinate ([u.sub.1](s),[u.sub.2](s)) on [[epsilon], [delta]], the path [gamma] could be expressed in terms of the parametrization X([u.sub.1](s),[u.sub.2](s)) as [gamma](s) = X([u.sub.1](s),[u.sub.2](s)) : [[epsilon], [delta]] [right arrow] M with [gamma]([epsilon]) = X([u.sub.1](e),[u.sub.2](e)) = [q.sub.init] and [gamma]([delta]) = X([u.sub.1]([delta]), [u.sub.2]([delta])) = [q.sub.t], where s is the path parameter. We are interested in computing the length-minimizing paths between a pair of points [q.sub.init], [q.sub.t] [member of] M. Let [mathematical expression not reproducible] form a Riemannian metric tensor ([g.sub.ij] ([u.sub.1], [u.sub.2])) with [g.sub.ij] : M [right arrow] R as its component. The Riemannian metric induced distance function defined over [mathematical expression not reproducible] is [30, 35]

[mathematical expression not reproducible]. (3)

Here the length of [gamma] is defined as the integral of the parametrized speed, expressed in terms of ([g.sub.ij]) as

[mathematical expression not reproducible], (4)

where summation convention for repeated indices, that is, a sum over repeated indices, is used, [du.sub.1] = du(s)/ds, and [du.sub.2] = dv(s)/ds (recall the fact that the definition of Riemannian metric is the quadratic form [(ds).sup.2] = [g.sub.11] ([u.sub.1], [u.sub.2]) [du.sup.2.sub.1] + 2[g.sub.12] ([u.sub.1], [u.sub.2]) [du.sub.1] [du.sub.2] + [g.sub.22] ([u.sub.1], [u.sub.2]) [du.sup.2.sub.2], where s is the arc length. In particular, L([gamma]) = d([q.sub.init], [q.sub.t]) = [delta] - [epsilon] if [gamma](s) is a unit speed curve parametrized by arc length). Any path [gamma] [member of] [mathematical expression not reproducible] satisfying d([q.sub.init], [q.sub.t]) = L([gamma]) is called a geodesic [24, 30- 32]. Using the calculus of variation for minimization of the functional (5) of the length over all curves in the class of [mathematical expression not reproducible] with fixed endpoints on Riemannian manifold, the two unknown coordinates [u.sub.1] (s) and [u.sub.2] (s) of geodesic satisfy the geodesic differential equations in terms of Christoffel symbols defined in Definition 2 [24, 30-32]:

[u".sub.1] + [[GAMMA].sup.1.sub.11] [([u'.sub.1].sup.2] + 2[[GAMMA].sup.1.sub.12] [u'.sub.1] [u'.sub.2] + [[GAMMA].sup.1.sub.22] [([u'.sub.1].sup.2] = 0, (5)

[u".sub.2] + [[GAMMA].sup.2.sub.11] [([u'.sub.1].sup.2] + 2[[GAMMA].sup.2.sub.12] [u'.sub.1] [u'.sub.2] + [[GAMMA].sup.2.sub.22] [([u'.sub.2].sup.2] = 0, (6)

with the boundary conditions at [q.sub.init] and [q.sub.t]. The system of (5) and (6) together with the boundary conditions is known as the second-order differential equations of geodesics, whose solutions are geodesics. The appearance of Christoffel symbols in geodesic equations reflects the effect of geometry (e.g., curvature) of the terrain M on the curve, geodesic in particular, on it (since the Christoffel symbols are zero in Cartesian coordinate, it follows from the geodesic equations that the shortest path is the straight line in Cartesian coordinate). Denote the state by [q.sub.r] [member of] [R.sup.2] and denote the velocity by [v.sub.r] [member of] [R.sup.2] (and the speed is denoted as [v.sub.r]). Then, the geodesic equation composed by the two second-order differential equations can be rewritten as four first-order differential equations [33] by defining

[v.sub.ri] = [du.sub.i (s)/ds, i = 1, 2 (7)

[mathematical expression not reproducible]. (8)

In terms of the robot state [q.sub.r], [v.sub.r] = [q'.sub.r], we obtain

[mathematical expression not reproducible], (9)

where [[sigma].sub.1], [[sigma].sub.2] are the control inputs [30, 36]. Equation (9) is the system of first-order differential equations (state equations) for generating the route [gamma] linking two distinct locations on the manifold M, which is locally the shortest path (length-minimizing geodesic) on M. Due to the geometric characteristics of the geodesic derived from geodesic differential equations, the geodesic is twice continuously differentiable. Figure 2 shows an example of geodesic on a smooth surface in [E.sup.3].

3. Obstacle Avoidance

The introduction of obstacles into the terrain affects the area that is local to the obstacle, called the hazardous region or prohibited area. For example, the hazardous region could be a no-fly area such as mountains, a risk-sensitive area such as extreme weather locations, or guard zone for flight. If there exists an obstacle on the way of the initial path, the robot would be blocked from reaching the terminal point or may be trapped or even destroyed, if the obstacle happens to be a hole so that the mobile robot cannot move directly by following the initial path to reach the terminal. To resolve the collision, the obstacle representation and the obstacle avoidance constraint should first be expressed. Meanwhile, a new method of resolving collisions via geodesic replanning is presented along with the terrain traversability analysis using local Gauss-Bonnet Theorem.

3.1. Obstacle Recharacterization. Methods for describing the obstacles on the terrain for numerical or analytical study could be a signed distance function [23], a set of equalities or inequalities, or a cluster of point clouds from real world mapping data. The presence of obstacles causes many numerical difficulties in computation of a reasonably good collision-free path in an obstacle obstructing terrain, in addition to a trade-off between the conflicting objectives of path length minimization and obstacle avoidance. One aspect is that collision detection of path-obstacle interaction caused by the presence of obstacles consumes a large portion of computing time [33] in planning a safe path. Thus, a commonly used practical approach for reducing the computation that is also employed in this paper is to use a simpler geometric shape, called bounding volume, to enclose the obstacle. For the current treatment, as illustrated in Figure 3, we assume that the compact set [mathematical expression not reproducible] could be modelled as a circle enclosing the obstacle projected onto the terrain.

Suppose that there exists a finite number of (clusters of) static obstacles [[OMICRON].sub.i] which are assumed to be nonintersecting and not containing the initial and the terminal location on the terrain M. Each [[OMICRON].sub.i] is finite size with its projection onto M enclosed by a connected set [mathematical expression not reproducible]. It is used by the path planner for safe robot motion and risk minimization to avoid collision or invasion of guard zone. We propose that the circle[mathematical expression not reproducible] is characterized by the radius as follows:

[mathematical expression not reproducible], (10)

where [r.sub.i] is the radius of circle which encloses the obstacle projection onto the terrain and [[OMICRON].sub.i] is a hazard grade of ith obstacle which measures the threat due to a risk of collision or an intrusion into a guard zone and can be adapted to a specific application. This defines the maximum distance of influence of hazard region on M, which can be made larger or smaller.

3.2. The Geodesic Replanning. In Section 2, we consider the environment without any obstacle; the manifold M without potential danger guarantees that the robot following the initial geodesic path y will be safe during movement. In the sequel, it is assumed that there exists a single prohibited area [[PHI].sub.[OMICRON]] which intersects the initial geodesic path [gamma] to simplify the presentation. The intersection condition is expressed as

[gamma] ([s.sub.l]) [intersection] int ([[PHI].sub.[OMICRON]]) [not equal to] [empty set], [there exists]l [member of] N (11)

or

d ([gamma] ([s.sub.l]), x) < 0, [for all]x [member of] int ([[PHI].sub.[OMICRON]]), [there exists]l [member of] N, (12)

where int means the interior of a compact set, N denotes the set of natural numbers, and d([gamma]([s.sub.l]), x) denotes the infimum of the length of all continuously differentiable paths from an arbitrary path point [gamma]([s.sub.l]) to a point x [member of] int ([[PHI].sub.[OMICRON]]). Since the path also intersects the boundary [partial derivative][[PHI].sub.[OMICRON]] of the compact set of the obstacle, we have

[gamma] ([s.sub.l]) [intersection] [partial derivative][[PHI].sub.[OMICRON]] [not equal to] [empty set], [there exists]l [member of] N, (13)

which is equivalent to the existence of intersection points between [gamma] and [partial derivative][[PHI].sub.[OMICRON]]:

d ([gamma] ([s.sub.l]), x) = 0, [there exists]x [member of] [partial derivative][[PHI].sub.[OMICRON]], [there exists]l [member of] N. (14)

Therefore, no collision with the obstacle occurs if the constraint of obstacle avoidance for sampled path points along the path in terms of the distance function [1]

d ([gamma] ([s.sub.j]), x) [greater than or equal to] 0, [for all]x [member of] [partial derivative][[PHI].sub.[OMICRON]], [for all]j = 1,...,n (15)

holds.

In the presence of single obstacle, one strategy is as follows. The robot can first follow the initial geodesic from [q.sub.init] to P. Then perform a counterclockwise (or clockwise) 90-degree rotation on the spot to change its heading to upward (or downward); then follow the upper (or lower) circumference of the circle with radius r from P to the other intersection point Q closest to the terminal with curvature velocity [v.sub.c] = [square root of r[a.sup.max.sub.nor]], where [a.sup.max.sub.nor] is the maximal normal acceleration [6] or from P to Q via a geodesic on the obstacle surface [23] with unit speed to bypass the circle in front of the robot. Finally, follow the initial geodesic again to reach qt. This approach has the advantage of utilizing part of the initial geodesic without the need of replanning but has the disadvantage of boundary following as we mentioned before. Alternatively, we propose a geodesic replanning procedure for avoiding a single intersected obstacle with the aid of (10) which recharacterizes the obstacle.

Now we present the steps of geodesic replanning procedure, namely, directional scanning, critical point selection, path generation, and collision checking, in case the intersection of initial geodesic with the obstacle occurs. The procedure heavily relies on the tangent plane at the first intersection point for searching appropriate waypoints, called critical points, to bypass the obstacle in front of the robot. Let [M.sub.free] = M \ [[PHI].sub.[OMICRON]] denote the collision-free region:

(1) Input. We are given the terrain (M, g), the circular region [[PHI].sub.[OMICRON]] of hazardous region on M, and the points [q.sub.init] and [q.sub.t] on the terrain. Let [gamma](s) be the initial geodesic connecting [q.sub.i]nit and [q.sub.t] parametrized by the arc length. Based on [gamma](s) and [[PHI].sub.[OMICRON]], check if there is any intersection of the initial geodesic with the obstacle. Suppose that (13) holds for [gamma](s). Denote by P the first intersection point at [partial derivative][[PHI].sub.[OMICRON]]

(2) Step 1. Directional scanning a small portion of the terrain around P. Construct the tangent plane [T.sub.p]M at P. Select a tangent direction v on [T.sub.p]M and an a value that determines the range of directional scanning along the v direction. A line extending from P in the direction v is drawn. Choose two segments of equal length [alpha][r.sub.[OMICRON]] with opposite direction from this selected tangent line, where [alpha] [member of] [[[alpha].sub.lb], [[alpha].sub.ub]] is a constant that represents the amount of displacement from P within a range of directional scanning. Here, the bound [[[alpha].sub.lb], [[alpha].sub.ub]] is to limit the directional scanning within a local neighborhood of P. Let the endpoints of two segments in the opposite direction be denoted as [q.sub.Ftan] and [q.sub.Gtan], respectively

(3) Step 2 (critical point selection). Choose the projection of [q.sub.Ftan] and [q.sub.Gtan] along the normal of the tangent plane onto the terrain M as the critical points (way-points) [q.sub.F] and [q.sub.G]. If neither [q.sub.F] nor [q.sub.G] is on the collision-free region [M.sub.free], go to Step 1

(4) Step 3 (path generation). Connect via geodesics the initial position [q.sub.init] to two critical points ([q.sub.F] and [q.sub.G]) and then to the terminal position [q.sub.t], respectively

(5) Step 4 (collision checking). Check the collision of the new paths with the designated circle. If at least one path is found to be collision-free, the replanning is finished. Else go to Step 1 and repeat the procedure of searching candidate critical points with new v and/or a new a value

(6) Output. A new collision-free composite geodesic path [[gamma].sub.new](s)

In what follows, for notational simplicity, we use subscripts C = F, G and c = f, g. Step 1 can be formally described as follows. Let X([u.sub.1], [u.sub.2]) be parametrization at P, and let [gamma](s) = X([u.sub.1](s), [u.sub.2](s)) be the initial geodesic. Denote [mathematical expression not reproducible] as the arc length value of P so that [mathematical expression not reproducible]. The tangent plane [T.sub.p]M, which contains the velocity vector [gamma]'([mathematical expression not reproducible]) of the initial geodesic [gamma] at P, can be decomposed into two independent (not necessarily orthonormal) unit tangent directions [mathematical expression not reproducible]. The vector [mathematical expression not reproducible] (as Figure 1 shows) assigns to each ([u.sub.1](s), [u.sub.2](s)) a unit normal vector at X([u.sub.1](s), [u.sub.2](s)). The trihedron [mathematical expression not reproducible], N represents a moving frame of M along the initial geodesic. Let v e [T.sub.p]M be a unit tangent vector of [T.sub.p]M. The tangent line through P along v direction is P + v[alpha][r.sub.[OMICRON]]. Thus,

[q.sub.Ftan] = P + v[alpha][r.sub.[OMICRON]],

[q.sub.Gtan] = P + v[alpha][r.sub.[OMICRON]]. (16)

According to the property of geodesic, the acceleration of initial geodesic at P is parallel to the normal N of the tangent plane at P. Therefore, the projection of [q.sub.Ctan] onto the terrain along the normal N of [T.sub.p]M, as depicted in Figure 4, can be obtained simply as

[mathematical expression not reproducible], (17)

where there exists h such that [q.sub.C] [member of] [M.sub.free]. Here note that small [alpha] yields [q.sub.C] in close proximity to P, and large [alpha] yields [q.sub.C] whose location is less predictable (e.g., outside the manifold), since [T.sub.p]M is a good approximation to M for points on [T.sub.p]M nearby P. In addition, h may not be easy to find due to the nonlinearity of the manifold. Instead, we may work on the parametric space, without determining the tangent space, as follows. We can set ([u.sub.1]([s.sub.c]), [u.sub.2]([s.sub.c])) = [mathematical expression not reproducible] + [[alpha].sub.c]([u.sub.1]([s.sub.d]), [u.sub.2]([s.sub.d]))), where [[alpha].sub.c] is step size and ([u.sub.1]([s.sub.d]), [u.sub.2]([s.sub.d])) is the direction, and check if

[q.sub.C] = X ([u.sub.1] ([s.sub.c]), [u.sub.2] ([s.sub.c])) (18)

is on [M.sub.free]. The computation of [q.sub.C] in parameter space for one direction is continued until good enough [q.sub.C] [member of] [M.sub.free] is found (see "Additional Design and Implementation Considerations") or a predefined number of iterations are reached. Two critical points need to be generated from the procedure in either parametric space or configuration space: one critical point is on the left side, and the other is on the right side of the initial geodesic and the prohibited area.

Using the geodesic as path primitive, a composite geodesic path [[gamma].sub.new] = [[gamma].sub.C] is composed by two geodesics, [mathematical expression not reproducible] : [[[epsilon].sub.c], [[rho].sub.c]] [right arrow] M for the first interval and [mathematical expression not reproducible] : [[[rho].sub.c], [[delta].sub.c]] [right arrow] M for the second interval, which are joined at the critical point [q.sub.C]. Thus, a new path [[gamma].sub.new] = [[gamma].sub.C] = [mathematical expression not reproducible] : [[[epsilon].sub.c], [[delta].sub.c]] [right arrow] [M.sub.free] continuous everywhere can be expressed as

[mathematical expression not reproducible]. (19)

And the boundary conditions [mathematical expression not reproducible] ([[epsilon].sub.c]) = [q.sub.init], [gamma] ([[rho].sub.c]) = [mathematical expression not reproducible] ([[rho].sub.c]) = [q.sub.C],[mathematical expression not reproducible] ([[delta].sub.c]) = [q.sub.t]. It is easily seen from (19) that for each of the composite geodesic paths the critical point is the end point of the replanned geodesic path starting from the initial position and also the initial point of the other replanned geodesic ending at the terminal position. Furthermore, the sum of the lengths of two geodesics (i.e., [[gamma].sub.new]) is larger than the length of the initial geodesic [gamma]; that is, the shortest path going from [q.sub.init] to [q.sub.t] via geodesic directly is always shorter (and also faster for unit speed robot) than the path going from [q.sub.init] to [q.sub.t] via a waypoint [q.sub.C].

Remark. Since [[gamma].sub.new] is composed by the unit speed geodesics, the maximum velocity constraint could be handled easily. For velocity continuity, we require that the unit speed curves [mathematical expression not reproducible] are tangentially connected; that is, their velocities are in the same direction at the critical point [q.sub.C] they connect: [mathematical expression not reproducible]. Therefore, depending on the continuity requirement imposed on the critical point, computation of [mathematical expression not reproducible] is either a two-point boundary value problem if only position continuity is required or an initial value problem if the tangent continuity is also required at the critical point. In the former case, the two geodesics can be computed simultaneously, while, in the latter case of initial value problem for geodesic, [mathematical expression not reproducible] is uniquely determined [30, 32] and thus could be computed only after [mathematical expression not reproducible] is computed. However, this [mathematical expression not reproducible], though ensuring the tangential continuity at the critical point, very likely cannot reach the terminal point.

We have the following proposition from our construction (see Figure 5 for an illustration).

Proposition 5. Suppose that the initial geodesic path y passes through the compact set of the single obstacle. Then with an appropriately chosen tangent direction and [alpha][r.sub.[OMICRON]], at least one of two replanned composite geodesics generated by the replanning procedure is collision-free.

3.3. Waypoint Navigation. We assume the robot is modelled as a point. Discrete piecewise paths joining an ordered list of subtargets is a concise way for planning and navigation in a global environment, since a sequence of waypoints may be easier to plan and follow by a mobile robot in practice. For navigation via waypoints, we need to design a sequence of control inputs (i.e., normal acceleration) to the geodesic equation (9) for a unit speed robot to follow the path [[gamma].sub.new] (s) confined on the terrain. Let [[gamma].sub.new] (s) = [mathematical expression not reproducible] be discretized into a set of path points [[gamma].sub.new]([s.sub.1]), [[gamma].sub.new]([s.sub.2]),...,[[gamma].sub.new]([s.sub.c]),...,[[gamma].sub.n ew]([s.sub.n]) [member of] [M.sub.free] by a partition of the interval (e.g., with uniform arc length distribution) into a sequence of monotonically increasing discrete path parameter values [{[s.sub.j]}.sup.n.sub.j=1] with the boundary conditions [[gamma].sub.new]([s.sub.1] = [[epsilon].sub.c]) = [q.sub.init], [Y.sub.new]([s.sub.c] = [[rho].sub.c]) = [q.sub.C], and [[gamma].sub.new]([s.sub.n] = [[delta].sub.c]) = [q.sub.t]. Moreover, the sequences [mathematical expression not reproducible] [member of] [M.sub.free] in the first interval and [mathematical expression not reproducible] [member of] [M.sub.free] in the second interval are Cauchy sequences on (M, g), which is assumed to be a complete Riemannian manifold. Therefore, succession of n robot states [q.sub.r] [member of] {[[gamma].sub.new]([s.sub.j])}.sup.n.sub.j=1] on the geodesic path will approach the geodesics [mathematical expression not reproducible] within the intervals of parameter s between the initial position, the critical point, and the terminal position as the number of points n used to discretize the path [[gamma].sub.new](s) tends to infinity. This generates a number of approximately shortest path segments defined by solving a TPBVP for the geodesic differential equations between two consecutive path points. In each point-to-point movement the robot passes through succession of waypoints [q.sub.init] = [[gamma].sub.new]([s.sub.1]), [Y.sub.new]([s.sub.2]),...,[q.sub.C] = [[gamma].sub.new]([s.sub.c]),...,[q.sub.t] = [[gamma].sub.new]([s.sub.n]) along a locally shortest geodesic from the initial to the terminal position. Notice that this path does not guarantee the globally shortest path on the manifold M, since a geodesic is defined to be locally shortest. This shows that the robot could navigate accurately from the initial to the terminal position sequentially along a sequence of geodesic segments, which are twice continuously differentiable, with position continuity at the junction [y.sub.new]([s.sub.c]). The travel time (efficiency) of constant speed navigation based on geodesic segments increases monotonically with the increase of the overall path length of [[gamma].sub.new]. Note that the accuracy of navigation depends on the number of waypoints. The computation of (n - 1) geodesic segments between two consecutive waypoints could be performed simultaneously; however, the computational load increasesasthe number of waypoints increases. Since the geodesic is parametrized by arc length, the number of path points of each segment of [[gamma].sub.c] could be determined on the basis of the length of [mathematical expression not reproducible] to avoid too sparse or too crowded path points (e.g., using equidistantly allocated path points) for uniform navigation performance.

3.4. Local Traversability Analysis. Finally, we point out an additional advantage of geodesic planning and replanning process which is the fact that terrain traversability could be analyzed with the aid of local Gauss-Bonnet Theorem of geodesic triangle. Recall the definition of geodesic triangle: a triangle formed by the arcs of three geodesics on a smooth surface. It is noted that a geodesic triangle [DELTA] [subset] M is formed by the sides composed of the initial geodesic and two replanned geodesics. Let {[[gamma].sup.[delta].sub.i]}.sub.i=1,2,3] denote the edges [gamma], [mathematical expression not reproducible], and let {[[theta].sub.i]}.sub.i=1,2,3] denote the interior angles of the geodesic triangle [DELTA], respectively. A measure of total curvature (i.e., the integral of Gaussian curvature with respect to the geodesic triangle) is a measure of traversability of [DELTA]. The geodesic triangle [DELTA] satisfies the local Gauss-Bonnet Theorem for total curvature [24, 30-32]:

[mathematical expression not reproducible], (20)

where K is the Gaussian curvature, dA is an infinitesimal area element on M (dA = [square root of det([g.sub.ij])][du.sub.1][du.sub.2] in local coordinate chart ([u.sub.1],[u.sub.2])), and [[kappa].sub.g] is the geodesic curvature of [partial derivative][DELTA]. Since the geodesic curvature [[kappa].sub.g] = 0 on [[gamma].sup.[delta].sub.i]}.sub.i=1,2,3], the term [[summation].sup.3.sub.i=1] [mathematical expression not reproducible] [[kappa].sub.g]ds = 0 in (20). We can thus rewrite (20) as the angle sum:

[[theta].sub.1] + [[theta].sub.2] + [[theta].sub.3] = [pi] + [[integral][integral].sub.[DELTA]] K dA. (21)

We see that as K = 0 everywhere on [DELTA], then [[summation].sup.3.sub.i=1] [[theta].sub.i] = [pi]. This means that the neighborhood of [DELTA] is isometric to a plane (e.g., a plane or a cylinder) [24, 30], and [gamma] is simply the straight line from the initial position to the terminal one. Due to symmetry in the plane, we have the following.

Proposition 6. Suppose that K = 0 everywhere on [DELTA]. Let the initial geodesic [gamma] pass through the center of the circle. Then the two replanned composite geodesic paths, [[gamma].sub.F] and [[gamma].sub.G], would be symmetric with respect to the initial geodesic, and the lengths of [[gamma].sub.F] and [[gamma].sub.G] are identical. Furthermore, there exists [[alpha].sup.*] that makes [[gamma].sub.F] and [[gamma].sub.G] the shortest.

The symmetric routes generated by the path replanning procedure are illustrated in Figure 6.

If K > 0 everywhere on [DELTA], then the angle sum [[summation].sup.3.sub.i=1] [[theta].sub.i] > [pi], which means the local terrain in the neighborhood of [DELTA], appears to be a convex landform such as an elliptic surface. On the contrary, if K < 0 everywhere on [DELTA], then the angle sum [[summation].sup.3.sub.i=1] [[theta].sub.i] < [pi]. This means the local terrain in the neighborhood of [DELTA] may look like a hyperbolic paraboloid landform such as a saddle. The local Gauss-Bonnet Theorem can thus aid to assess the traversability of [DELTA] on which the replanned geodesics traverse since the terrain [M.sub.free] may have complex curvature profiles. For example, we prefer choosing the replanned path as the one that traverses the local terrain with the angle sum [[summation].sup.3.sub.i=1] [[theta].sub.i] > [pi] or [[summation].sup.3.sub.i=1] [[theta].sub.i] < [pi] because the local terrain with the angle sum [[summation].sup.3.sub.i=1] [[theta].sub.i] < [pi], such as a hyperbolic paraboloid, may be deemed less preferable since energy-consuming ascending and descending motions are required, and the motion on this type of terrain which involves curvature sign change would require that the mobile robot have better mobility and maneuverability.

3.5. Additional Design and Implementation Considerations. The replanning algorithm requires the computation of four new geodesics in each iteration, while the initial geodesic, the point P, and thetangent plane [T.sub.p]M only need to be computed once. We mention that in general a closed-form solution to TPBVP for the geodesic differential equations is not available, or the derivation of geodesic differential equations is nearly impossible, so that the geodesic can only be numerically defined. Hence, to improve the efficiency of the algorithm, it is important to employ an efficient algorithm to numerically compute the geodesic with acceptable accuracy based on the aforementioned several equivalent characterizations of geodesic on a case-by-case basis in accordance with the geometry of the terrain [10, 33]. Despite the importance of stable and efficient algorithms to numerically compute geodesic, note that the feasibility of the proposed geodesic replanning procedure depends on the existence of [T.sub.p]M and the availability of (at least one) [q.sub.C] [member of] [M.sub.free] by iterating Steps 1 and 2. There are other concerns listed in the following which should be taken into account in the path planning and replanning process:

(1) We remark that if there are multiple, disjoint prohibited areas, there can be multiple numbers of intersection points along the initial geodesic, and narrow passages that introduce curvature constraints for turning around may complicate the replanning. However, in principle, the same replanning procedure could be sequentially applied to each (cluster of) prohibited area (one at a time, e.g., with ordering set as from the closest one to the farthest one away from the initial position [6]), so that a collision-free path composed of a number of local geodesics is generated from the initial to the terminal position.

(2) Since the terrain is assumed to be smooth, every point P has a well-defined tangent plane [T.sub.p]M. In different v directions at P, the manifold may have different curvatures, indicating the way the manifold is bending in given v directions. Furthermore, it should be noted that, depending on the Gaussian curvature of the terrain at P, the intersection of tangent plane [T.sub.p]M with the terrain M can be a (asymptotic) curve, a pair of intersecting (asymptotic) curves through P, or a cusp other than an isolated point P. The determination of critical points becomes complicated, if the geometry of the manifold adjacent to P has a lot of variations or has more than one obstacle.

(3) There exists flexibility of directional scanning which allows the planner to iteratively search alternative critical points by choosing different v and decreasing or increasing [alpha] in multiple runs at the cost of computational complexity. In such a way, the replanning algorithm generates a collection of paths better fulfilling the multiple criteria of safety, short length, traversability considered here, and alternative objectives such as energy consumption. This feature is interesting for multiobstacles scenarios and for dynamic environments. For example, in case the replanned path obtained at one iteration is safe but unfortunately traverses the local terrain with the angle sum [[summation].sup.3.sub.i=1] [[theta].sub.i] < [pi], it is advised to refine the critical points in the intermediate stages of replanning so as to make the replanned geodesics meet the traversability condition of angle sum [[summation].sup.3.sub.i=1] [[theta].sub.i] [greater than or equal to] [pi] in addition to meeting the primary safety requirement (15). Meanwhile if both replanned composite geodesics are safe and traversable, we propose choosing the one which is farther or has a larger minimum clearance from the obstacle and if possible passes through a terrain with less difference in curvatures, yielding a safe path passing through a flat terrain with larger clearance from obstacle at the price of longer length.

(4) In dynamic environment that consists of an unforeseen or moving obstacle on the initial geodesic, the initial point is the point that the robot detects the presence of obstacle, and the subpath from this initial point to the goal is a geodesic segment of the initial geodesic. In order to make the replanning approach amenable to real-time applications such as dynamic environment, it is desirable to reduce the computation time as much as possible. Real-time limitation of the replanning algorithm is addressed based on looking at the geometric operations in a coordinate chart such as algorithms used for geodesic computation, intersection checking and the computation of intersection point P of geodesic with circle (see (14)), determination of the tangent direction, critical points determination either via tangent lines to make a directional sweep of the terrain or via search in coordinate parameter space nearby P, projection of the point on tangent plane onto the manifold, collision checking of the new composite geodesic path with the obstacle, and computation of the angle sum of geodesic triangle for applying Gauss-Bonnet Theorem. In case [q.sub.C] may be the same as [q.sub.Ctan] and the projection of [q.sub.C] to find [q.sub.Ctan] is not needed, the computation is greatly reduced.

Figure 7 summarizes the discussions made so far as a flowchart of 3D path planning and waypoint navigation via geodesics. It is noted that since only position continuity is ensured at the critical point of the composite geodesic, the robot temporarily stops at the critical point to change its heading toward the second geodesic while traveling at unit speed along each of the two geodesic segments.

4. Simulation

For simulation of wheeled mobile robot navigation, we assume that the robot is a point that moves at unit speed, moves in the direction of its velocity/tangent vector (so that the nonholonomic constraint of no-slip motion is satisfied), and is capable of switching its heading instantaneously to follow the continuous spatial curve confined on the 3D terrain accurately. In this section, a simulation aimed at demonstrating the applicability, instead of generality, of the proposed geodesic replanning procedure for mobile robot navigation on a smooth (implicit and parametric) surface in [E.sup.3] is given in more detail. The simulation setup is as follows. Firstly, the ellipsoidal surface is often used to approximate the region of the earth on which the mobile robot moves. Hence, a terrain for simulation is given as an ellipsoidal surface. Then the initial and terminal positions on the terrain are given and the initial geodesic without considering the obstacle is generated first. It is noted that the tangent plane exists for any point P on the terrain, and the terrain is bending away from [T.sub.p]M in all tangent directions at any P [member of] M, and thus the intersection of [T.sub.p]M with M is a single point P. In addition, the ellipsoidal surface is a typical manifold with positive Gaussian curvature K = [h.sup.4]/[([a.sub.1] [a.sub.2] [a.sub.3]).sup.2] > 0 at the point P [32], where h is the distance from the center of ellipsoid to [T.sub.p]M and [a.sub.1], [a.sub.2], and [a.sub.3] denote the semiaxis lengths of the ellipsoid. For this terrain with positive Gaussian curvature everywhere, any safe composite geodesic path generated by the replanning algorithm is traversable according to the local Gauss-Bonnet Theorem. Then a static obstacle that intersects the initial geodesic is deployed on the terrain to form new terrain geometry for simulation of the geodesic path replanning algorithm.

The data for simulation is as follows, and all of our computations and plots are handled by Matlab. The dimension of the simulated hill-like terrain is 6 x 160 x 160 cubic unit shown in Figures 8 and 9. The initial and terminal points are [q.sub.init] = (77, 0, 0.6) and [q.sub.t] = (11, 27, 5), respectively. Figure 8 shows the shortest path in the absence of obstacles, in which we resort to Bessel's method to solve for a geodesic between two points on an ellipsoidal surface that involves elliptic functions [37]. Regarding the obstacle represented by (10), we set the enclosing circle with a radius r = 2 and center (41, 14, 4). The hazard grade for this obstacle is [OMICRON] = 4, so the hazard region on the ellipsoidal terrain is a circle of radius [r.sub.[OMICRON]] = 8 unit and center (41, 14, 4) depicted in Figure 9. The first intersection point P of initial geodesic with the hazardous circle is (47.89, 12.22, 3.797), and the tangent plane at this point is 4x + y + 44z = 2, which is a good linear approximation of the ellipsoid nearby P. The critical point chosen for this simulation is [q.sub.C] = (50, 28, 3), which induces a corner (velocity discontinuity) of the composite geodesics with two unit tangents ((-0.9955, -0.0275, 0.0908) and (-0.9173, 0.3768, 0.1287)) at this point (as seen clearly in Figure 9 that shows the path returned by the replanning algorithm). The simulation in Figure 9 serves to demonstrate that the path replanning procedure works well in practice for a unit speed mobile robot on a terrain with arbitrary smooth surface and well-defined tangent planes in [E.sup.3].

5. Conclusion

We have presented an offline geodesic path planning and replanning procedure to produce a continuous path that a point robot with constant speed satisfying the maximum velocity constraint would follow on a 3D terrain without using boundary following on the obstacle surface as an integral portion of the path. The terrain is modelled as a Riemannian manifold with a single obstacle represented by a circle whose radius could be tuned according to the hazard grade of the obstacle to meet the requirement of specific applications. The replanning approach heavily relies on the properties associated with the geodesics which ties in the differential geometry information of the manifold and directional scanning on the tangent plane at the first intersection point of the initial geodesic with the circle for exploration of terrain. The replanning process iteratively searches critical points to generate a composition of two geodesics joined at a critical point. Thanks to the geodesic that ties in the differential geometry information of the terrain, additional exploration of traversability of the terrain could be performed at the planning stage by the local Gauss-Bonnet Theorem of geodesic triangle to aid a selection of more maneuverable path and terrain. Simulation demonstrates the practicability of the proposed geodesic planning and replanning procedure for mobile robot navigation on 3D terrain. Our method of resolving the collision could be integrated into AtlasRRT* to effectively fix the infeasibility of connecting two nodes via geodesic in tree extension or update on a Riemannian manifold. Despite the properties of the geodesic exploited by the replanning algorithm, we may compare the geodesic with other parametric path primitives such as B-spline [5, 14] with local shape control property for 3D path planning. Furthermore, a natural extension we may investigate in the future is to consider certain criterion other than the path length to make the planned path respect the differential constraints of the robot such as mobility (e.g., climbing ability and maximum turning curvature constraint for rollover) [26, 38] in addition to the constraints imposed by the terrain. This requires that the state of robot include the pose compatible with the normal of terrain so that the state space of the robot is a submanifold of the Lie group SE(3) of six dimensions.

http://dx.doi.org/10.1155/2016/2539761

Competing Interests

The authors declare that there are no competing interests regarding the publication of this paper.

Acknowledgments

The authors are grateful to Kevin C.- W. Lo and J.-M. Chen for valuable discussions and Dr. T.-Y. Zing and Dr. H.-T. Hwang for constructive criticisms.

References

[1] S. M. LaValle, Planning Algorithms, Cambridge University Press, 2006.

[2] P. K.-C. Wang, Visibility-Based Optimal Path and Motion Planning, Springer, New York, NY, USA, 2015.

[3] S. Alatartsev, S. Stellmacher, and F. Ortmeier, "Robotic task sequencing problem: a survey," Journal of Intelligent and Robotic Systems: Theory and Applications, vol. 80, no. 2, pp. 279-298, 2015.

[4] L. Yang, J. Qi, J. Xiao, and X. Yong, "A literature review of UAV 3D path planning," in Proceedings of the 11th World Congress on Intelligent Control and Automation (WCICA '14), pp. 2376-2381, IEEE, Shenyang, China, July 2014.

[5] O. Hachour, "A three dimensional collision-free path planning," International Journal of Systems Applications, Engineering & Development, vol. 3, no. 4, pp. 117-126, 2009.

[6] K. G. Shin and N. D. Mckay, "Selection of near-minimum time geometric paths for robotic manipulators," IEEE Transactions on Automatic Control, vol. 31, no. 6, pp. 501-511, 1986.

[7] M. Liu, "Robotic online path planning on point cloud," IEEE Transactions on Cybernetics, vol. 46, no. 5, pp. 1217-1228, 2016.

[8] L. D. Zhang and C. J. Zhou, "Robot optimal trajectory planning based on geodesics," in Proceedings of the IEEE International Conference on Control and Automation (ICCA '07), pp. 2433-2436, June 2007.

[9] P. Coelho and U. Nunes, "Lie algebra application to mobile robot control: a tutorial," Robotica, vol. 21, no. 5, pp. 483-493, 2003.

[10] Y. Chen, L. Li, and X. Ji, "Smooth and accurate trajectory planning for industrial robots," Advances in Mechanical Engineering, vol. 6, Article ID 342137, 8 pages, 2014.

[11] L. Zhang and C. Zhou, "Kuka youBot arm shortest path planning based on geodesics," in Proceedings of the IEEE International Conference on Robotics and Biomimetics (ROBIO '13), pp. 2317-2321, IEEE, Shenzhen, China, December 2013.

[12] Y. Hu, F. Bao, B. Li, and Z. Gu, "Path planning based on geodesic for mobile robots," in Proceedings of the 27th Chinese Control and Decision Conference (CCDC '15), pp. 4315-4320, IEEE, Qingdao, China, May 2015.

[13] R.-J. Yan, J. Wu, J. Y. Lee, and C.-S. Han, "Representation of 3D environment map using B-spline surface with two mutually perpendicular LRFs," Mathematical Problems in Engineering, vol. 2015, Article ID 690310, 14 pages, 2015.

[14] Z. Shiller and Y.-R. Gwo, "Dynamic motion planning of autonomous vehicles," IEEE Transactions on Robotics and Automation, vol. 7, no. 2, pp. 241-249, 1991.

[15] M. Huptych and S. Rock, "Online path planning in dynamic environments using the curve shortening flow method," Production Engineering, vol. 9, no. 5-6, pp. 613-621, 2015.

[16] Y. Kitamura, T. Tanaka, F. Kishino, and M. Yachida, "3-D path planning in a dynamic environment using an octree and an artificial potential field," in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, vol. 2, pp. 474-481, August 1995.

[17] Y. Wang and W. Cao, "A global path planning method for mobile robot based on a three-dimensional-like map," Robotica, vol. 32, no. 4, pp. 611-624, 2014.

[18] M. Ataei and A. Yousefi-Koma, "Three-dimensional optimal path planning for waypoint guidance of an autonomous underwater vehicle," Robotics and Autonomous Systems, vol. 67, pp. 23-32, 2015.

[19] S.-R. Chang and U.-Y. Huh, "Curvature-continuous 3D path-planning using QPMI method," International Journal of Advanced Robotic Systems, vol. 12, 2015.

[20] L. Jaillet and J. M. Porta, "Efficient asymptotically-optimal path planning on manifolds," Robotics and Autonomous Systems, vol. 61, no. 8, pp. 797-807, 2013.

[21] A. Aalbers, Obstacle avoidance using limit cycles [M.S. thesis], Delft University of Technology, Delft, Netherlands, 2013.

[22] S. M. Khansari-Zadeh and A. Billard, "A dynamical system approach to realtime obstacle avoidance," Autonomous Robots, vol. 32, no. 4, pp. 433-454, 2012.

[23] J. Lu, Y. Diaz-Mercado, M. Egerstedt, H. Zhou, and S.-N. Chow, "Shortest paths through 3-dimensional cluttered environments," in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '14), pp. 6579-6585, Hong Kong, June 2014.

[24] M. P. do Carmo, Riemannian Geometry, Birkhouser, 1992.

[25] H. Yu, J. J. Zhang, and Z. Jiao, "Geodesics on point clouds," Mathematical Problems in Engineering, vol. 2014, Article ID 860136, 12 pages, 2014.

[26] I. Arvanitakis, A. Tzes, and M. Thanou, "Geodesic motion planning on 3D-terrains satisfying the robot's kinodynamic constraints," in Proceedings of the 39th Annual Conference of the IEEE Industrial Electronics Society (IECON '13), pp. 4144-4149, Vienna, Austria, November 2013.

[27] I. R. Manchester and J. J. E. Slotine, "Control contraction metrics: convex and intrinsic criteria for nonlinear feedback design," https://arxiv.org/abs/1503.03144.

[28] S. Arimoto, "Modeling and control of multi-body mechanical systems: part i a riemannian geometry approach," in Advances in the Theory of Control, Signals and Systems with Physical Modeling, vol. 407 of Lecture Notes in Control and Information Sciences, pp. 3-16, Springer, Berlin, Germany, 2011.

[29] B. K. Ghosh and I. B. Wijayasinghe, "Dynamics of human head and eye rotations under Donders' constraint," IEEE Transactions on Automatic Control, vol. 57, no. 10, pp. 2478-2489, 2012.

[30] M. P. do Carmo, Differential Geometry of Curves and Surfaces, Prentice-Hall, 1976.

[31] V. A. Toponogov, Differential Geometry of Curves and Surfaces: A Concise Guide, Birkhaauser, 2006.

[32] B. O'Neill, Elementary Differential Geometry, Academic Press, Amsterdam, The Netherlands, 2nd edition, 2006.

[33] N. M. Patrikalakis and T. Maekawa, Shape Interrogation for Computer Aided Design and Manufacturing, Springer, 2002.

[34] D. M. Morera and P. C. P. Carvalho, Geodesic-Based Modeling on Manifold Triangulations, Instituto Nacional de Matematica Pura e Aplicada, Rio de Janeiro, Brazil, 2006.

[35] Z. T. Zheng and S. G. Chen, "The general orthogonal projection on a regular surface," in Proceedings of the 5th European Conference on European Computing Conference (ECC '11), pp. 116-119, World Scientific and Engineering Academy and Society (WSEAS), Orlando, Fla, USA, 2011.

[36] K.-L. Wu, C.-W. Lo, Y.-C. Lin, and J.-S. Liu, "3D Path planning based on nonlinear geodesic equation," in Proceedings of the 11th IEEE International Conference on Control and Automation, pp. 342-347, Taichung, Taiwan, June 2014.

[37] R. E. Deakin and M. N. Hunter, Geodesics on an Ellipsoid-Bessels Method, School of Mathematical Geospatial Sciences, RMIT University, Melbourne, Australia, 2009.

[38] N. Ganganath, C.-T. Cheng, and C. K. Tse, "A constraint-aware heuristic path planner for finding energy-efficient paths on uneven terrains," IEEE Transactions on Industrial Informatics, vol. 11, no. 3, pp. 601-611, 2015.

Kun-Lin Wu, Ting-Jui Ho, Sean A. Huang, Kuo-Hui Lin, Yueh-Chen Lin, and Jing-Sin Liu

Institute of Information Science, Academia Sinica, Nangang, Taipei 11529, Taiwan

Correspondence should be addressed to Jing-Sin Liu; liu@iis.sinica.edu.tw

Received 31 December 2015; Revised 11 April 2016; Accepted 26 May 2016

Academic Editor: Mustapha Zidi

Caption: Figure 1: The tangent plane at a path point P [member of] [gamma] has a surface normal vector N. The acceleration of the path is in the direction of principal normal to the path, not necessarily orthogonal to the tangent plane. In general, the acceleration has a normal component normal to the tangent plane and a tangential component in the tangent plane.

Caption: Figure 2: The geodesic from an initial position to a terminal position on the terrain modelled as a hypersurface in [E.sup.3].

Caption: Figure 3: An environment with obstacles. The hazardous region or prohibited area [mathematical expression not reproducible] is the circle with the hazard grade tuned radius derived from the obstacle [[OMICRON].sub.i].

Caption: Figure 4: Directional scanning on the tangent plane to search a critical point. The projection of [q.sub.Ctan] [member of] [T.sub.p]M onto M along a surface normal vector N to [T.sub.p]M as the critical point [q.sub.C].

Caption: Figure 5: The general case. The nonsymmetric routes [mathematical expression not reproducible] (purple) and [mathematical expression not reproducible] (green) are generated. The circle designates a hazardous region. The tangent direction for scanning shown in the figure is orthogonal to the radial direction of the circle at P.

Caption: Figure 6: The symmetric case in the plane. The initial geodesic is simply the straight line y from the initial position [omicron] to the terminal position [??]. The circle designates a hazardous region with a user-defined hazard grade [OMICRON] = 1. As the initial geodesic cuts the circle in half (i.e., passes through the center of the circle), the two symmetric routes [mathematical expression not reproducible] (purple) and [mathematical expression not reproducible] (green) are piecewise linear line segments connected via critical points.

Caption: Figure 7: The flowchart for 3D geodesic replanning and navigation for obstacle avoidance. The robot could move from the initial position to the terminal position sequentially along a composition of two continuously differentiable geodesic segments, thus guaranteeing a shortest path segment in each movement.

Caption: Figure 8: The terrain is an ellipsoid with positive Gaussian curvature everywhere. The geodesic is depicted on the simulated 3D terrain without obstacles, where the signs "*" and "[DELTA]" denote the initial and the terminal locations, respectively.

Caption: Figure 9: Geodesic replanning. The initial geodesic in Figure 8 is in collision with hazardous region (the dashed red circle) detected while following it. A new path is obtained by the geodesic replanning algorithm via directional scanning on the tangent plane. In the figure, the signs "*", "[nabla]," and "[??]" denote the initial, the terminal, and the critical point, respectively. The initial geodesic and replanned collision-free composite geodesics are denoted by magenta and black lines, respectively. The cusp point is the critical point, where the robot temporarily stops to switch its moving direction.

Printer friendly Cite/link Email Feedback | |

Title Annotation: | Research Article |
---|---|

Author: | Wu, Kun-Lin; Ho, Ting-Jui; Huang, Sean A.; Lin, Kuo-Hui; Lin, Yueh-Chen; Liu, Jing-Sin |

Publication: | Mathematical Problems in Engineering |

Article Type: | Report |

Date: | Jan 1, 2016 |

Words: | 11156 |

Previous Article: | Flow Simulation of Suspension Bridge Cable Based on Lattice-Boltzmann Method. |

Next Article: | Mining the IPTV Channel Change Event Stream to Discover Insight and Detect Ads. |

Topics: |