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

In this paper, mobile robot navigation on a 3D terrain with a single obstacle is addressed. The terrain is modelled as a smooth, complete manifold with well-defined tangent planes and the hazardous region is modelled as an enclosing circle with a hazard grade tuned radius representing the obstacle projected onto the terrain to allow efficient path-obstacle intersection checking. To resolve the intersections along the initial geodesic, by resorting to the geodesic ideas from differential geometry on surfaces and manifolds, we present a geodesic-based planning and replanning algorithm as a newmethod for obstacle avoidance on a 3D terrain without using boundary following on the obstacle surface. The replanning algorithm generates two new paths, each a composition of two geodesics, connected via critical points whose locations are found to be heavily relying on the exploration of the terrain via directional scanning on the tangent plane at the first intersection point of the initial geodesic with the circle. An advantage of this geodesic path replanning procedure is that traversability of terrain on which the detour path traverses could be explored based on the local Gauss-Bonnet Theorem of the geodesic triangle at the planning stage. A simulation demonstrates the practicality of the analytical geodesic replanning procedure for navigating a constant speed point robot on a 3D hill-like terrain.


Introduction
Path 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 may be 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 collisionfree geometric path connecting the initial and the terminal location, with the typical objective of minimizing a userdefined 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 higherdimensional) terrain to have certain characteristics quite different from the path planning on the plane, as noted in [4][5][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 twodimensional surface.Suppose that the terrain that the robot traverses is constructed by a smooth, complete Riemannian manifold  which is embedded in the Euclidean space E 3 [1,[7][8][9].Regarding the obstacle, we recharacterize the obstacle as its enclosing circle on the terrain with radius   = , where   is denoted as radius of hazardous region,  is regarded as hazard grade, and  is the radius of enclosing circle.We are particularly interested in using the geodesic [8,[10][11][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 twopoint 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 -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 threedimensional 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 gridbased elevation map for mobile robot obstacle avoidance.Reference [20] proposed AtlasRRT * to extend the popular, state-of-the-art path planning algorithm RRT (rapidly exploring random tree) [1] on the plane to  (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 energyconsuming 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.

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  without reference to a specific, explicit coordinate frame and give a fundamental comprehension of geodesic on manifold .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.

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 threedimensional space E 3 , called manifold.Intuitively, a manifold  embedded in E 3 could be considered as a Euclidean space [1,24].Examples of manifolds are sphere, torus, and parametric surfaces.Here, let  denote a manifold and we give some most general definition of a manifold as follows [24]. .With an atlas defined on , given a point  in  and a coordinate chart about , a tangent vector to  at  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 , the tangent vector at  is also the velocity vector of  at  with the speed and direction of the motion the same as the length and direction of the tangent vector., where  is the arc length.A Riemannian metric reflects the nonlinearity of the space).Given a Riemannian metric  = (  ), the Christoffel symbol Γ   is uniquely defined in terms of the metric tensor (  ) composed by the first and second differentials of the Riemannian metric.

Definition 2. Consider
where (  ) = (  ) −1 is the inverse matrix to the metric tensor Note that Γ   = 0; (  ) is an identity matrix in Cartesian coordinate.An operator ∇ called the covariant derivative along a curve  on the manifold  is generalization of directional derivative of vector fields.It operates on a pair of vector fields X = (X 1 , X 2 ) and Y = (Y 1 , Y 2 ) tangent to  along  at  to produce a new vector field ∇X Y at  as the rate of change of Y in X() direction in the basis (  1 ,   2 ).Definition 3. Let X, Y ∈    for  ∈  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 The tangent plane at a path point  ∈  has a surface normal vector .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.

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][28][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   denote the geodesic curvature of a curve  at a point  ∈  which measures how a curve  deviates from being a geodesic [24,[30][31][32][33]. Definition 4. A path  : [, ] ⊂  →  is called a geodesic if and only if the tangent vector   to  is parallel along , where ()  denotes derivative with respect to the path parameter : where ∇ denotes the covariant derivative defined in Definition 3 with X replaced by a tangent vector   .
As shown in Figure 1, in general, the acceleration   of a path  can be decomposed to two orthogonal components: a normal component   nor in the direction of principal normal and a tangential component composed of the covariant derivative   tan = ∇    of the velocity   in the direction of tangent   along the path  (also called geometric/intrinsic acceleration) [24,[30][31][32].This tangential component of acceleration is caused by the rotation of the basis vectors spanning the tangent plane due to the movement in   direction.The following are equivalent characterizations of a geodesic [30,31,34]: (1)  is the locally shortest path.
(3) The acceleration   is normal to    (i.e., the angle between its acceleration and velocity is 90 degrees everywhere on  or a coincidence of the principal normal of  with the surface normal).The geodesic has purely normal/centripetal acceleration.
Therefore, from the characterization that ‖  ‖ is constant for geodesic , the notion of a geodesic implies a curve  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 locally shortest path for a point robot linking two distinct locations on the manifold  without obstacles.Denote the set of all piecewise continuously differentiable paths on  which start at the point  and end at the point  by C   ∈ .Given a curve  ∈ C    init and given a continuously differentiable mapping of coordinate chart  :  → , where  ⊂ R 2 is an open set parametrized by the local coordinate ( 1 (),  2 ()) on [, ], the path  could be expressed in terms of the parametrization ( 1 (),  2 ()) as () = ( 1 (),  2 ()) : [, ] →  with () = ( 1 (),  2 ()) =  init and () = ( 1 (),  2 ()) =   , where  is the path parameter.We are interested in computing the length-minimizing paths between a pair of points  init ,   ∈ .
Here the length of  is defined as the integral of the parametrized speed, expressed in terms of (  ) as where summation convention for repeated indices, that is, a sum over repeated indices, is used,  1 = ()/, and  2 = V()/ (recall the fact that the definition of Riemannian metric is the quadratic form ( , where  is the arc length.In particular, () = ( init ,   ) =  −   () is a unit speed curve parametrized by arc length).Any path  ∈ C    init satisfying ( init ,   ) = () is called a geodesic [24,[30][31][32].Using the calculus of variation for minimization of the functional (5) of the length over all curves in the class of C    init with fixed endpoints on Riemannian manifold, the two unknown coordinates  1 () and  2 () of geodesic satisfy the geodesic differential equations in terms of Christoffel symbols defined in Definition 2 [24,[30][31][32]: with the boundary conditions at  init and   .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  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   ∈ R 2 and denote the velocity by ]  ∈ R 2 (and the speed is denoted as ]  ).Then, the geodesic equation composed by the two second-order differential equations can be rewritten as four first-order differential equations [33] by defining and derivative of ]  : In terms of the robot state   , ]  =    , we obtain where  1 ,  2 are the control inputs [30,36].Equation ( 9) is the system of first-order differential equations (state equations) for generating the route  linking two distinct locations on the manifold , which is locally the shortest path (lengthminimizing geodesic) on .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 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.

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 collisionfree 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 Φ   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   which are assumed to be nonintersecting and not containing the initial and the terminal location on the terrain .Each   is finite size with its projection onto  enclosed by a connected set Φ   .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 Φ   is characterized by the radius as follows: where   is the radius of circle which encloses the obstacle projection onto the terrain and   is a hazard grade of th 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 , which can be made larger or smaller.

The Geodesic Replanning.
In Section 2, we consider the environment without any obstacle; the manifold  without potential danger guarantees that the robot following the initial geodesic path  will be safe during movement.In the sequel, it is assumed that there exists a single prohibited area Φ  which intersects the initial geodesic path  to simplify the presentation.The intersection condition is expressed as or where int means the interior of a compact set, N denotes the set of natural numbers, and ((  ), x) denotes the infimum of the length of all continuously differentiable paths from an arbitrary path point (  ) to a point x ∈ int(Φ  ).Since the path also intersects the boundary Φ  of the compact set of the obstacle, we have which is equivalent to the existence of intersection points between  and Φ  : 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]  ( (  ) , x) ≥ 0, ∀x ∈ Φ  , ∀ = 1, . . ., holds.
In the presence of single obstacle, one strategy is as follows.The robot can first follow the initial geodesic from  init to .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  from  to the other intersection point  closest to the terminal with curvature velocity V  = √ max nor , where  max nor is the maximal normal acceleration [6] or from  to  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   .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  free =  \ Φ  denote the collision-free region: (1) Input.We are given the terrain (, ), the circular region Φ  of hazardous region on , and the points  init and   on the terrain.Let () be the initial geodesic connecting  init and   parametrized by the arc length.Based on () and Φ  , check if there is any intersection of the initial geodesic with the obstacle.Suppose that (13)  In what follows, for notational simplicity, we use subscripts  = ,  and  = , .Step 1 can be formally described as follows.Let ( 1 ,  2 ) be parametrization at , and let () = ( 1 (),  2 ()) be the initial geodesic.Denote   1 as the arc length value of  so that  = (  1 ) = ( 1 (  1 ),  2 (  1 )).The tangent plane   , which contains the velocity vector   (  1 ) of the initial geodesic  at , can be decomposed into two independent (not necessarily orthonormal) unit tangent directions   1 ,   2 .The vector  = (  1 ×   2 )/‖  1 ×   2 ‖ (as Figure 1 shows) assigns to each ( 1 (),  2 ()) a unit normal vector at ( 1 (),  2 ()).The trihedron   1 ,   2 ,  represents a moving frame of  along the initial geodesic.Let k ∈    be a unit tangent vector of   .The tangent line through  along k direction is  + k  .Thus, According to the property of geodesic, the acceleration of initial geodesic at  is parallel to the normal  of the tangent plane at .Therefore, the projection of  tan onto the terrain along the normal  of   , as depicted in Figure 4, can be obtained simply as where there exists ℎ such that   ∈  free .Here note that small  yields   in close proximity to , and large  yields   whose location is less predictable (e.g., outside the manifold), since    is a good approximation to  for points on    nearby .In addition, ℎ 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 ( And the boundary conditions 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.,  new ) is larger than the length of the initial geodesic ; that is, the shortest path going from  init to   via geodesic directly is always shorter (and also faster for unit speed robot) than the path going from  init to   via a waypoint   .
Remark.Since  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   1 and   2 are tangentially connected; that is, their velocities are in the same direction at the critical point   they connect: Therefore, depending on the continuity requirement imposed on the critical point, computation of   2 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,   2 is uniquely determined [30,32] and thus could be computed only after   1 is computed.However, this   2 , 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  passes through the compact set of the single obstacle.Then with an appropriately chosen tangent direction and   , at least one of two replanned composite geodesics generated by the replanning procedure is collision-free.

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  new () confined on the terrain.Let  new () =   2 ∘   1 () be discretized into a set of path points  new ( 1 ),  new ( 2 ), . . .,  new (  ), . . .,  new (  ) ∈  free by a partition of the interval (e.g., with uniform arc length distribution) into a sequence of monotonically increasing discrete path parameter values {  }  =1 with the boundary conditions  new ( 1 =   ) =  init ,  new (  =   ) =   , and  new (  =   ) =   .Moreover, the sequences {  1 (  )}  =1 ∈  free in the first interval and {  2 (  )}  = ∈  free in the second interval are Cauchy sequences on (, ), which is assumed to be a complete Riemannian manifold.Therefore, succession of  robot states   ∈ { new (  )}  =1 on the geodesic path will approach the geodesics   1 () and   2 () within the intervals of parameter  between the initial position, the critical point, and the terminal position as the number of points  used to discretize the path  new () 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  init =  new ( 1 ),  new ( 2 ), . . .,   =  new (  ), . . .,   =  new (  ) 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 , 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  new (  ).The travel time (efficiency) of constant speed navigation based on geodesic segments increases monotonically with the increase of the overall path length of  new .Note that the accuracy of navigation depends on the number of waypoints.The computation of ( − 1) geodesic segments between two consecutive waypoints could be performed simultaneously; however, the computational load increases as the number of waypoints increases.Since the geodesic is parametrized by arc length, the number of path points of each segment of   could be determined on the basis of the length of   1 or   2 to avoid too sparse or too crowded path points (e.g., using equidistantly allocated path points) for uniform navigation performance.

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 △ ⊂  is formed by the sides composed of the initial geodesic and two replanned geodesics.Let { △  } =1,2,3 denote the edges ,   1 , and   2 , and let {  } =1,2,3 denote the interior angles of the geodesic triangle △, 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 △.The geodesic triangle △ satisfies the local Gauss-Bonnet Theorem for total curvature [24,[30][31][32]: where  is the Gaussian curvature,  is an infinitesimal area element on  ( = √det(  ) 1  2 in local coordinate chart ( 1 ,  2 )), and   is the geodesic curvature of △.
Since the geodesic curvature   ≡ 0 on { △  } =1,2,3 , the term (20).We can thus rewrite (20) as the angle sum: We see that as  = 0 everywhere on △, then ∑ 3 =1   = .This means that the neighborhood of △ is isometric to a plane (e.g., a plane or a cylinder) [24,30], and  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  = 0 everywhere on △.Let the initial geodesic  pass through the center of the circle.Then the two replanned composite geodesic paths,   and   , would be symmetric with respect to the initial geodesic, and the lengths of   and   are identical.Furthermore, there exists  * that makes   and   the shortest.
The symmetric routes generated by the path replanning procedure are illustrated in Figure 6.
If  > 0 everywhere on △, then the angle sum ∑ appears to be a convex landform such as an elliptic surface.
On the contrary, if  < 0 everywhere on △, then the angle sum ∑ 3 =1   < .This means the local terrain in the neighborhood of △ may look like a hyperbolic paraboloid landform such as a saddle.The local Gauss-Bonnet Theorem can thus aid to assess the traversability of △ on which the replanned geodesics traverse since the terrain  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 ∑ 3 =1   >  or ∑ 3 =1   =  because the local terrain with the angle sum ∑ 3 =1   < , 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.

Additional Design and Implementation Considerations.
The replanning algorithm requires the computation of four new geodesics in each iteration, while the initial geodesic, the point , and the tangent plane    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    and the availability of (at least one)   ∈  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  has a well-defined tangent plane   .In different k directions at , the manifold may have different curvatures, indicating the way the manifold is bending in given k directions.Furthermore, it should be noted that, depending on the Gaussian curvature of the terrain at , the intersection of tangent plane    with the terrain  can be a (asymptotic) curve, a pair of intersecting (asymptotic) curves through , or a cusp other than an isolated point .The determination of critical points becomes complicated, if the geometry of the manifold adjacent to  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 k and decreasing or increasing  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 ∑ 3 =1   < , 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 ∑ 3 =1   ≥  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  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 , 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   may be the same as  tan and the projection of   to find  tan 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.

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 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  on the terrain, and the terrain is bending away from    in all tangent directions at any  ∈ , and thus the intersection of    with  is a single point .In addition, the ellipsoidal surface is a typical manifold with positive Gaussian curvature  = ℎ 4 /( 1  2  3 ) 2 > 0 at the point  [32], where ℎ is the distance from the center of ellipsoid to    and  1 ,  2 , and  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 × 160 × 160 cubic unit shown in Figures 8 and 9.The initial and terminal points are  init = (77, 0, 0.6) and   = (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  = 2 and center (41, 14, 4).The hazard grade for this obstacle is  = 4, so the hazard region on the ellipsoidal terrain is a circle of radius   = 8 unit and center (41, 14, 4) depicted in Figure 9.The first intersection point  of initial geodesic with the hazardous circle is (47.89,12.22, 3.797), and the tangent plane at this point is 4 +  + 44 = 2, which is a good linear approximation of the ellipsoid nearby .The critical point chosen for this simulation is   = (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 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 "⋆", "," 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.
(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 3 .

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.

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

Figure 3 :
Figure 3: An environment with obstacles.The hazardous region or prohibited area Φ   is the circle with the hazard grade tuned radius derived from the obstacle   .

Figure 5 :
Figure 5: The general case.The nonsymmetric routes   =   2 ∘   1 (purple) and   =   2 ∘   1 (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 .

Figure 7 :
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.
is locally Euclidean.Every point has an open ball  ⊂  that is homeomorphic to an open subset  ⊂ R 3 such that a homeomorphism  :  →  is called a coordinate chart.An atlas is a family of charts {  ,   } for which   constitutes open covering of ; that is,  = ∪ ∈ The tangent plane at a point  ∈  on a curve is denoted by   , which is the vector space formed by the set of all tangent vectors to  at ; it is determined by its basis vectors on    and does not depend on the choice of coordinate chart.In a given local coordinate chart ( 1 ,  2 ), at each point , the partial velocities (  1 ,   2 ) along the coordinate curves are linearly independent and thus could serve as a basis for   , where    = /  ,  = 1, 2. Any tangent vector on    can thus be represented by the basis vectors (  1 ,   2 ).Let (, ) denote a Riemannian manifold, where  = (  ) is a Riemannian metric which defines the inner product of tangent vectors on    (note that, in a Riemannian manifold, each tangent vector has to be attached with a point  of the manifold.In a given local coordinate chart ( 1 ,  2 ), a Riemannian metric  = (  ) is a symmetric, positive-definite quadratic form: () 2 =  11 ( 1 ,  2 ) 2 1 +2 12 ( 1 ,  2 ) 1  2 +  22 ( 1 ,  2 ) 2 2 [30,35]d  12 =  21 = ⟨  1 ,   2 ⟩ form a Riemannian metric tensor (  ( 1 ,  2 )) with   :  →  as its component.The Riemannian metric induced distance function defined over C   init is[30,35] ,   ] is a constant that represents the amount of displacement from  within a range of directional scanning.Here, the bound [  ,   ] is to limit the directional scanning within a local neighborhood of .Let the endpoints of two segments in the opposite direction be denoted as  tan and  tan , respectively (3) Step 2 (critical point selection).Choose the projection of  tan and  tan along the normal of the tangent plane onto the terrain  as the critical points (waypoints)   and   .If neither   nor   is on the collision-free region Figure 4: Directional scanning on the tangent plane to search a critical point.The projection of  tan ∈    onto  along a surface normal vector  to    as the critical point   .
holds for ().Denote by  the first intersection point at Φ  (2) Step 1. Directional scanning a small portion of the terrain around . Construct the tangent plane    at . Select a tangent direction k on    and an  value that determines the range of directional scanning along the k direction.A line extending from  in the direction k is drawn.Choose two segments of equal length   with opposite direction from this selected tangent line, where  ∈ [ free , go to Step 1 (4) Step 3 (path generation).Connect via geodesics the initial position  init to two critical points (  and   ) and then to the terminal position   , 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 k and/or a new  value (6) Output.A new collision-free composite geodesic path  new () ( 1 (  ),  2 (  ))), where   is step size and ( 1 (  ),  2 (  )) is the direction, and check if  =  ( 1 (  ) ,  2 (  ))(18)is on  free .The computation of   in parameter space for one direction is continued until good enough   ∈  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  new =   is composed by two geodesics,   1 : [  ,   ] →  for the first interval and   2 : [  ,   ] →  for the second interval, which are joined at the critical point   .Thus, a new path  new =   =   2 ∘   1 : [  ,   ] →  free continuous everywhere can be expressed as 1 (  ),  2 (  )) = ( 1 (  1 ),  2 (  1 ) + 3=1   > , which means the local terrain in the neighborhood of △, Figure6: The symmetric case in the plane.The initial geodesic is simply the straight line  from the initial position ∘ to the terminal position ⬦.The circle designates a hazardous region with a userdefined hazard grade  = 1.As the initial geodesic cuts the circle in half (i.e., passes through the center of the circle), the two symmetric routes   =   2 ∘   1 (purple) and   =   2 ∘   1 (green) are piecewise linear line segments connected via critical points.