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

Authors: Kun-Lin Wu; Ting-Jui Ho; Sean A. Huang; Kuo-Hui Lin; Yueh-Chen Lin; Jing-Sin Liu
Journal: Mathematical Problems in Engineering
 (2016)

---

## Abstract

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 new method 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.


---

## Body

## 1. 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 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 E3 [1, 7–9]. Regarding the obstacle, we recharacterize the obstacle as its enclosing circle on the terrain with radius rO=Or, where rO is denoted as radius of hazardous region, O 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 Section2, 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 k-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⁎ 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. Section2 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 manifoldM 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 spaceE3, called manifold. Intuitively, a manifold M embedded in E3 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 spaceM 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⊂M that is homeomorphic to an open subset V⊂R3 such that a homeomorphism φ:U→V is called a coordinate chart.An atlas is a family of charts {Uα,φα} for which Uα constitutes open covering of M; that is, M=∪α∈IUα. 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 γ, the tangent vector at P is also the velocity vector of γ 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∈M on a curve is denoted by TPM, 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 TPM and does not depend on the choice of coordinate chart. In a given local coordinate chart u1,u2, at each point P, the partial velocities (∂u1,∂u2) along the coordinate curves are linearly independent and thus could serve as a basis for TPM, where ∂uj=∂/∂uj, j=1,2. Any tangent vector on TPM can thus be represented by the basis vectors (∂u1,∂u2). Let (M,g) denote a Riemannian manifold, where g=(gij) is a Riemannian metric which defines the inner product of tangent vectors on TPM (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 (u1,u2), a Riemannian metric g=(gij) is a symmetric, positive-definite quadratic form: ds2=g11(u1,u2)du12+2g12u1,u2du1du2 + g22(u1,u2)du22, where s is the arc length. A Riemannian metric reflects the nonlinearity of the space). Given a Riemannian metric g=(gij), the Christoffel symbol Γijk is uniquely defined in terms of the metric tensor gij composed by the first and second differentials of the Riemannian metric.Definition 2.
Consider(1)Γijk=gkl∂ujgil+∂uigjl-∂ulgij2, where gij=gij-1 is the inverse matrix to the metric tensor gij (i.e., gikgkj=0 if i≠j or 1 if i=j).
Note thatΓijk=0; gij is an identity matrix in Cartesian coordinate. An operator ∇~ called the covariant derivative along a curve γ on the manifold M is generalization of directional derivative of vector fields. It operates on a pair of vector fields X=(X1,X2) and Y=(Y1,Y2) tangent to M along γ at P to produce a new vector field ∇~XY at P as the rate of change of Y in XP direction in the basis ∂u1,∂u2.Definition 3.
LetX,Y∈TPM for P∈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 X→∇~XY, ∇~XY=Xm(∂Yi/∂um)∂ui+XmΓmkiYk∂ui,  i,j,k,m=1,2.


### 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κg denote the geodesic curvature of a curve γ at a point P∈γ which measures how a curve γ deviates from being a geodesic [24, 30–33].Definition 4.
A pathγ:ε,δ⊂I→M is called a geodesic if and only if the tangent vector γ′ to γ is parallel along γ, where ()′ denotes derivative with respect to the path parameter s:(2)κg=∇~γ′γ′=0,where ∇~ denotes the covariant derivative defined in Definition 3 with X replaced by a tangent vector γ′.As shown in Figure1, 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–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.(2)
γ has vanishing geodesic curvature: κg≡0.(3)
The accelerationγ′′ is normal to TγM (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.Figure 1
The tangent plane at a path pointP∈γ 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.To begin with, we consider the planning of locally shortest path for a point robot linking two distinct locations on the manifoldM 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 Cab∈M. Given a curve γ∈Cqinitqt and given a continuously differentiable mapping of coordinate chart X:U→M, where U⊂R2 is an open set parametrized by the local coordinate u1s,u2s on ε,δ, the path γ could be expressed in terms of the parametrization X(u1(s),u2(s)) as γs=Xu1s,u2s:ε,δ→M with γε=Xu1ε,u2ε=qinit and γδ=Xu1δ,u2δ=qt, where s is the path parameter. We are interested in computing the length-minimizing paths between a pair of points qinit,qt∈M. Let g11=Xu1,Xu1, g22=〈Xu2,Xu2〉, and g12=g21=Xu1,Xu2 form a Riemannian metric tensor giju1,u2 with gij:M→R as its component. The Riemannian metric induced distance function defined over Cqinitqt is [30, 35](3)dqinit,qt=infqinit,qt∈M⁡Lγ∣γ∈Cqinitqt.Here the length of γ is defined as the integral of the parametrized speed, expressed in terms of (gij) as(4)s=Lγ=∫εδdγdsds=∫εδgijduidujds,∀i,j=1,2, where summation convention for repeated indices, that is, a sum over repeated indices, is used, du1=dus/ds, and du2=dvs/ds (recall the fact that the definition of Riemannian metric is the quadratic form ds2=g11(u1,u2)du12+2g12u1,u2du1du2+g22(u1,u2)du22, where s is the arc length. In particular, Lγ=dqinit,qt=δ-εifγs is a unit speed curve parametrized by arc length). Any path γ∈Cqinitqt satisfying dqinit,qt=Lγ 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 Cqinitqt with fixed endpoints on Riemannian manifold, the two unknown coordinates u1s and u2s of geodesic satisfy the geodesic differential equations in terms of Christoffel symbols defined in Definition 2 [24, 30–32]:(5)u1′′+Γ111u1′2+2Γ121u1′u2′+Γ221u2′2=0,(6)u2′′+Γ112u1′2+2Γ122u1′u2′+Γ222u2′2=0, with the boundary conditions at qinit and qt. 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 qr∈R2 and denote the velocity by νr∈R2 (and the speed is denoted as ν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(7)νri=duisds,i=1,2 and derivative of νri:(8)σi=dνrids=-∑j,k=12Γijku1s,u2sdujsdsduksds,i=1,2. In terms of the robot state qr, νr=qr′, we obtain(9)qri′s=νri,i=1,2,νri′=σi=-∑j,k=12Γijku1s,u2sdujsdsduksds,i=1,2, 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 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 E3.Figure 2
The geodesic from an initial position to a terminal position on the terrain modelled as a hypersurface inE3.


## 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 spaceE3, called manifold. Intuitively, a manifold M embedded in E3 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 spaceM 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⊂M that is homeomorphic to an open subset V⊂R3 such that a homeomorphism φ:U→V is called a coordinate chart.An atlas is a family of charts {Uα,φα} for which Uα constitutes open covering of M; that is, M=∪α∈IUα. 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 γ, the tangent vector at P is also the velocity vector of γ 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∈M on a curve is denoted by TPM, 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 TPM and does not depend on the choice of coordinate chart. In a given local coordinate chart u1,u2, at each point P, the partial velocities (∂u1,∂u2) along the coordinate curves are linearly independent and thus could serve as a basis for TPM, where ∂uj=∂/∂uj, j=1,2. Any tangent vector on TPM can thus be represented by the basis vectors (∂u1,∂u2). Let (M,g) denote a Riemannian manifold, where g=(gij) is a Riemannian metric which defines the inner product of tangent vectors on TPM (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 (u1,u2), a Riemannian metric g=(gij) is a symmetric, positive-definite quadratic form: ds2=g11(u1,u2)du12+2g12u1,u2du1du2 + g22(u1,u2)du22, where s is the arc length. A Riemannian metric reflects the nonlinearity of the space). Given a Riemannian metric g=(gij), the Christoffel symbol Γijk is uniquely defined in terms of the metric tensor gij composed by the first and second differentials of the Riemannian metric.Definition 2.
Consider(1)Γijk=gkl∂ujgil+∂uigjl-∂ulgij2, where gij=gij-1 is the inverse matrix to the metric tensor gij (i.e., gikgkj=0 if i≠j or 1 if i=j).
Note thatΓijk=0; gij is an identity matrix in Cartesian coordinate. An operator ∇~ called the covariant derivative along a curve γ on the manifold M is generalization of directional derivative of vector fields. It operates on a pair of vector fields X=(X1,X2) and Y=(Y1,Y2) tangent to M along γ at P to produce a new vector field ∇~XY at P as the rate of change of Y in XP direction in the basis ∂u1,∂u2.Definition 3.
LetX,Y∈TPM for P∈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 X→∇~XY, ∇~XY=Xm(∂Yi/∂um)∂ui+XmΓmkiYk∂ui,  i,j,k,m=1,2.


## 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κg denote the geodesic curvature of a curve γ at a point P∈γ which measures how a curve γ deviates from being a geodesic [24, 30–33].Definition 4.
A pathγ:ε,δ⊂I→M is called a geodesic if and only if the tangent vector γ′ to γ is parallel along γ, where ()′ denotes derivative with respect to the path parameter s:(2)κg=∇~γ′γ′=0,where ∇~ denotes the covariant derivative defined in Definition 3 with X replaced by a tangent vector γ′.As shown in Figure1, 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–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.(2)
γ has vanishing geodesic curvature: κg≡0.(3)
The accelerationγ′′ is normal to TγM (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.Figure 1
The tangent plane at a path pointP∈γ 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.To begin with, we consider the planning of locally shortest path for a point robot linking two distinct locations on the manifoldM 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 Cab∈M. Given a curve γ∈Cqinitqt and given a continuously differentiable mapping of coordinate chart X:U→M, where U⊂R2 is an open set parametrized by the local coordinate u1s,u2s on ε,δ, the path γ could be expressed in terms of the parametrization X(u1(s),u2(s)) as γs=Xu1s,u2s:ε,δ→M with γε=Xu1ε,u2ε=qinit and γδ=Xu1δ,u2δ=qt, where s is the path parameter. We are interested in computing the length-minimizing paths between a pair of points qinit,qt∈M. Let g11=Xu1,Xu1, g22=〈Xu2,Xu2〉, and g12=g21=Xu1,Xu2 form a Riemannian metric tensor giju1,u2 with gij:M→R as its component. The Riemannian metric induced distance function defined over Cqinitqt is [30, 35](3)dqinit,qt=infqinit,qt∈M⁡Lγ∣γ∈Cqinitqt.Here the length of γ is defined as the integral of the parametrized speed, expressed in terms of (gij) as(4)s=Lγ=∫εδdγdsds=∫εδgijduidujds,∀i,j=1,2, where summation convention for repeated indices, that is, a sum over repeated indices, is used, du1=dus/ds, and du2=dvs/ds (recall the fact that the definition of Riemannian metric is the quadratic form ds2=g11(u1,u2)du12+2g12u1,u2du1du2+g22(u1,u2)du22, where s is the arc length. In particular, Lγ=dqinit,qt=δ-εifγs is a unit speed curve parametrized by arc length). Any path γ∈Cqinitqt satisfying dqinit,qt=Lγ 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 Cqinitqt with fixed endpoints on Riemannian manifold, the two unknown coordinates u1s and u2s of geodesic satisfy the geodesic differential equations in terms of Christoffel symbols defined in Definition 2 [24, 30–32]:(5)u1′′+Γ111u1′2+2Γ121u1′u2′+Γ221u2′2=0,(6)u2′′+Γ112u1′2+2Γ122u1′u2′+Γ222u2′2=0, with the boundary conditions at qinit and qt. 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 qr∈R2 and denote the velocity by νr∈R2 (and the speed is denoted as ν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(7)νri=duisds,i=1,2 and derivative of νri:(8)σi=dνrids=-∑j,k=12Γijku1s,u2sdujsdsduksds,i=1,2. In terms of the robot state qr, νr=qr′, we obtain(9)qri′s=νri,i=1,2,νri′=σi=-∑j,k=12Γijku1s,u2sdujsdsduksds,i=1,2, 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 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 E3.Figure 2
The geodesic from an initial position to a terminal position on the terrain modelled as a hypersurface inE3.


## 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 ΦOi could be modelled as a circle enclosing the obstacle projected onto the terrain.Figure 3
An environment with obstacles. The hazardous region or prohibited areaΦOi is the circle with the hazard grade tuned radius derived from the obstacle Oi.Suppose that there exists a finite number of (clusters of) static obstaclesOi which are assumed to be nonintersecting and not containing the initial and the terminal location on the terrain M. Each Oi is finite size with its projection onto M enclosed by a connected set ΦOi. 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 ΦOi is characterized by the radius as follows:(10)rOi=Oiri, where ri is the radius of circle which encloses the obstacle projection onto the terrain and Oi 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 Section2, we consider the environment without any obstacle; the manifold M 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 ΦO which intersects the initial geodesic path γ to simplify the presentation. The intersection condition is expressed as(11)γsl∩int⁡ΦO≠⌀,∃l∈N or(12)dγsl,x<0,∀x∈int⁡ΦO,∃l∈N, where int means the interior of a compact set, N denotes the set of natural numbers, and dγsl,x denotes the infimum of the length of all continuously differentiable paths from an arbitrary path point γsl to a point x∈intΦO. Since the path also intersects the boundary ∂ΦO of the compact set of the obstacle, we have(13)γsl∩∂ΦO≠⌀,∃l∈N, which is equivalent to the existence of intersection points between γ and ∂ΦO:(14)dγsl,x=0,∃x∈∂ΦO,∃l∈N. 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](15)dγsj,x≥0,∀x∈∂ΦO,∀j=1,…,n holds.In the presence of single obstacle, one strategy is as follows. The robot can first follow the initial geodesic fromqinit 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 vc=ranormax, where anormax 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. LetMfree=M∖ΦO denote the collision-free region:(1)
Input. We are given the terrain M,g, the circular region ΦO of hazardous region on M, and the points qinit and qt on the terrain. Let γs be the initial geodesic connecting qinit and qt parametrized by the arc length. Based on γs and ΦO, check if there is any intersection of the initial geodesic with the obstacle. Suppose that (13) holds for γs. Denote by P the first intersection point at ∂ΦO(2)
Step  1. Directional scanning a small portion of the terrain around P. Construct the tangent plane TPM at P. Select a tangent direction v on TPM and an α 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 αrO with opposite direction from this selected tangent line, where α∈αlb,αub is a constant that represents the amount of displacement from P within a range of directional scanning. Here, the bound αlb,α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 qFtan and qGtan, respectively(3)
Step  2 (critical point selection). Choose the projection of qFtan and qGtan along the normal of the tangent plane onto the terrain M as the critical points (waypoints) qF and qG. If neither qF nor qG is on the collision-free region Mfree, go to Step 1(4)
Step  3 (path generation). Connect via geodesics the initial position qinit to two critical points (qF and qG) and then to the terminal position qt, 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 α value(6)
Output. A new collision-free composite geodesic path γnew(s)In what follows, for notational simplicity, we use subscriptsC=F,G and c=f,g. Step 1 can be formally described as follows. Let Xu1,u2 be parametrization at P, and let γs=Xu1s,u2s be the initial geodesic. Denote sβ1 as the arc length value of P so that P=γsβ1=X(u1sβ1,u2sβ1). The tangent plane TPM, which contains the velocity vector γ′sβ1 of the initial geodesic γ at P, can be decomposed into two independent (not necessarily orthonormal) unit tangent directions ∂u1, ∂u2. The vector N=∂u1×∂u2/∂u1×∂u2 (as Figure 1 shows) assigns to each u1s,u2s a unit normal vector at Xu1s,u2s. The trihedron ∂u1, ∂u2, N represents a moving frame of M along the initial geodesic. Let v∈TPM be a unit tangent vector of TPM. The tangent line through P along v direction is P+vαrO. Thus,(16)qFtan=P+vαrO,qGtan=P-vαrO.According to the property of geodesic, the acceleration of initial geodesic atP is parallel to the normal N of the tangent plane at P. Therefore, the projection of qCtan onto the terrain along the normal N of TPM, as depicted in Figure 4, can be obtained simply as(17)qC=qCtan+γ′′sβ1h,h∈R,where there exists h such that qC∈Mfree. Here note that small α yields qC in close proximity to P, and large α yields qC whose location is less predictable (e.g., outside the manifold), since TPM is a good approximation to M for points on TPM 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 u1sc,u2sc=(u1sβ1,u2sβ1+αc(u1(sd),u2(sd))), where αc is step size and (u1sd,u2sd) is the direction, and check if(18)qC=Xu1sc,u2sc is on Mfree. The computation of qC in parameter space for one direction is continued until good enough qC∈Mfree 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.Figure 4
Directional scanning on the tangent plane to search a critical point. The projection ofqCtan∈TPM onto M along a surface normal vector N to TPM as the critical point qC.Using the geodesic as path primitive, a composite geodesic pathγnew=γC is composed by two geodesics, γc1:εc,ρc→M for the first interval and γc2:ρc,δc→M for the second interval, which are joined at the critical point qC. Thus, a new path γnew=γC=γc2∘γc1:εc,δc→Mfree continuous everywhere can be expressed as(19)γC=γc2∘γc1=γc1s,εc≤s≤ρc,γc2s,ρc≤s≤δc. And the boundary conditions γc1εc=qinit, γc1ρc=γc2ρc=qC, γc2δc=qt. 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 qinit to qt via geodesic directly is always shorter (and also faster for unit speed robot) than the path going from qinit to qt via a waypoint qC.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 γc1 and γc2 are tangentially connected; that is, their velocities are in the same direction at the critical point qC they connect: γc1′(sc)=γc2′(sc). Therefore, depending on the continuity requirement imposed on the critical point, computation of γc2 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, γc2 is uniquely determined [30, 32] and thus could be computed only after γc1 is computed. However, this γc2, 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 Figure5 for an illustration).Figure 5
The general case. The nonsymmetric routesγF=γf2∘γf1 (purple) and γG=γg2∘γg1 (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.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 αrO, 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 γnews confined on the terrain. Let γnew(s)=γc2∘γc1(s) be discretized into a set of path points γnew(s1),γnew(s2),…,γnew(sc),…,γnew(sn)∈Mfree by a partition of the interval (e.g., with uniform arc length distribution) into a sequence of monotonically increasing discrete path parameter values sjj=1n with the boundary conditions γnews1=εc=qinit, γnewsc=ρc=qC, and γnewsn=δc=qt. Moreover, the sequences γc1sjj=1c∈Mfree in the first interval and γc2sjj=cn∈Mfree 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 qr∈γnewsjj=1n on the geodesic path will approach the geodesics γc1s and γc2s 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 γnews 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 qinit=γnews1,γnews2,…,qC=γnewsc,…,qt=γnewsn 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 γnewsc. 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 (n-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  γc could be determined on the basis of the length of γc1 or γc2 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△⊂M is formed by the sides composed of the initial geodesic and two replanned geodesics. Let γi△i=1,2,3 denote the edges γ, γc1, and γc2, and let θii=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–32]:(20)∬△KdA+∑i=13π-θi+∑i=13∫γi△κgds=2π, where K is the Gaussian curvature, dA is an infinitesimal area element on M (dA=detgijdu1du2 in local coordinate chart (u1,u2)), and κg is the geodesic curvature of ∂△. Since the geodesic curvature κg≡0 on γi△i=1,2,3, the term ∑i=13∫γi△κgds=0 in (20). We can thus rewrite (20) as the angle sum:(21)θ1+θ2+θ3=π+∬△KdA.We see that as K=0 everywhere on △, then ∑i=13‍θi=π. 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 thatK=0 everywhere on △. Let the initial geodesic γ pass through the center of the circle. Then the two replanned composite geodesic paths, γF and γG, would be symmetric with respect to the initial geodesic, and the lengths of γF and γG are identical. Furthermore, there exists α∗ that makes γF and γG the shortest.The symmetric routes generated by the path replanning procedure are illustrated in Figure6.Figure 6
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 user-defined hazard grade O=1. As the initial geodesic cuts the circle in half (i.e., passes through the center of the circle), the two symmetric routes γF=γf2∘γf1 (purple) and γG=γg2∘γg1 (green) are piecewise linear line segments connected via critical points.IfK>0 everywhere on △, then the angle sum ∑i=13θi>π, which means the local terrain in the neighborhood of △, appears to be a convex landform such as an elliptic surface. On the contrary, if K<0 everywhere on △, then the angle sum ∑i=13θi<π. 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 Mfree 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 ∑i=13θi>π or ∑i=13θi=π because the local terrain with the angle sum ∑i=13θi<π, 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 pointP, and the tangent plane TPM 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 TPM and the availability of (at least one) qC∈Mfree 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 pointP has a well-defined tangent plane TPM. 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 TPM 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 differentv 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 ∑i=13θi<π, 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 ∑i=13θi≥π 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 pointP 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 qC may be the same as qCtan and the projection of qC to find qCtan 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.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.


## 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 ΦOi could be modelled as a circle enclosing the obstacle projected onto the terrain.Figure 3
An environment with obstacles. The hazardous region or prohibited areaΦOi is the circle with the hazard grade tuned radius derived from the obstacle Oi.Suppose that there exists a finite number of (clusters of) static obstaclesOi which are assumed to be nonintersecting and not containing the initial and the terminal location on the terrain M. Each Oi is finite size with its projection onto M enclosed by a connected set ΦOi. 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 ΦOi is characterized by the radius as follows:(10)rOi=Oiri, where ri is the radius of circle which encloses the obstacle projection onto the terrain and Oi 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 Section2, we consider the environment without any obstacle; the manifold M 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 ΦO which intersects the initial geodesic path γ to simplify the presentation. The intersection condition is expressed as(11)γsl∩int⁡ΦO≠⌀,∃l∈N or(12)dγsl,x<0,∀x∈int⁡ΦO,∃l∈N, where int means the interior of a compact set, N denotes the set of natural numbers, and dγsl,x denotes the infimum of the length of all continuously differentiable paths from an arbitrary path point γsl to a point x∈intΦO. Since the path also intersects the boundary ∂ΦO of the compact set of the obstacle, we have(13)γsl∩∂ΦO≠⌀,∃l∈N, which is equivalent to the existence of intersection points between γ and ∂ΦO:(14)dγsl,x=0,∃x∈∂ΦO,∃l∈N. 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](15)dγsj,x≥0,∀x∈∂ΦO,∀j=1,…,n holds.In the presence of single obstacle, one strategy is as follows. The robot can first follow the initial geodesic fromqinit 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 vc=ranormax, where anormax 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. LetMfree=M∖ΦO denote the collision-free region:(1)
Input. We are given the terrain M,g, the circular region ΦO of hazardous region on M, and the points qinit and qt on the terrain. Let γs be the initial geodesic connecting qinit and qt parametrized by the arc length. Based on γs and ΦO, check if there is any intersection of the initial geodesic with the obstacle. Suppose that (13) holds for γs. Denote by P the first intersection point at ∂ΦO(2)
Step  1. Directional scanning a small portion of the terrain around P. Construct the tangent plane TPM at P. Select a tangent direction v on TPM and an α 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 αrO with opposite direction from this selected tangent line, where α∈αlb,αub is a constant that represents the amount of displacement from P within a range of directional scanning. Here, the bound αlb,α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 qFtan and qGtan, respectively(3)
Step  2 (critical point selection). Choose the projection of qFtan and qGtan along the normal of the tangent plane onto the terrain M as the critical points (waypoints) qF and qG. If neither qF nor qG is on the collision-free region Mfree, go to Step 1(4)
Step  3 (path generation). Connect via geodesics the initial position qinit to two critical points (qF and qG) and then to the terminal position qt, 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 α value(6)
Output. A new collision-free composite geodesic path γnew(s)In what follows, for notational simplicity, we use subscriptsC=F,G and c=f,g. Step 1 can be formally described as follows. Let Xu1,u2 be parametrization at P, and let γs=Xu1s,u2s be the initial geodesic. Denote sβ1 as the arc length value of P so that P=γsβ1=X(u1sβ1,u2sβ1). The tangent plane TPM, which contains the velocity vector γ′sβ1 of the initial geodesic γ at P, can be decomposed into two independent (not necessarily orthonormal) unit tangent directions ∂u1, ∂u2. The vector N=∂u1×∂u2/∂u1×∂u2 (as Figure 1 shows) assigns to each u1s,u2s a unit normal vector at Xu1s,u2s. The trihedron ∂u1, ∂u2, N represents a moving frame of M along the initial geodesic. Let v∈TPM be a unit tangent vector of TPM. The tangent line through P along v direction is P+vαrO. Thus,(16)qFtan=P+vαrO,qGtan=P-vαrO.According to the property of geodesic, the acceleration of initial geodesic atP is parallel to the normal N of the tangent plane at P. Therefore, the projection of qCtan onto the terrain along the normal N of TPM, as depicted in Figure 4, can be obtained simply as(17)qC=qCtan+γ′′sβ1h,h∈R,where there exists h such that qC∈Mfree. Here note that small α yields qC in close proximity to P, and large α yields qC whose location is less predictable (e.g., outside the manifold), since TPM is a good approximation to M for points on TPM 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 u1sc,u2sc=(u1sβ1,u2sβ1+αc(u1(sd),u2(sd))), where αc is step size and (u1sd,u2sd) is the direction, and check if(18)qC=Xu1sc,u2sc is on Mfree. The computation of qC in parameter space for one direction is continued until good enough qC∈Mfree 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.Figure 4
Directional scanning on the tangent plane to search a critical point. The projection ofqCtan∈TPM onto M along a surface normal vector N to TPM as the critical point qC.Using the geodesic as path primitive, a composite geodesic pathγnew=γC is composed by two geodesics, γc1:εc,ρc→M for the first interval and γc2:ρc,δc→M for the second interval, which are joined at the critical point qC. Thus, a new path γnew=γC=γc2∘γc1:εc,δc→Mfree continuous everywhere can be expressed as(19)γC=γc2∘γc1=γc1s,εc≤s≤ρc,γc2s,ρc≤s≤δc. And the boundary conditions γc1εc=qinit, γc1ρc=γc2ρc=qC, γc2δc=qt. 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 qinit to qt via geodesic directly is always shorter (and also faster for unit speed robot) than the path going from qinit to qt via a waypoint qC.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 γc1 and γc2 are tangentially connected; that is, their velocities are in the same direction at the critical point qC they connect: γc1′(sc)=γc2′(sc). Therefore, depending on the continuity requirement imposed on the critical point, computation of γc2 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, γc2 is uniquely determined [30, 32] and thus could be computed only after γc1 is computed. However, this γc2, 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 Figure5 for an illustration).Figure 5
The general case. The nonsymmetric routesγF=γf2∘γf1 (purple) and γG=γg2∘γg1 (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.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 αrO, 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 γnews confined on the terrain. Let γnew(s)=γc2∘γc1(s) be discretized into a set of path points γnew(s1),γnew(s2),…,γnew(sc),…,γnew(sn)∈Mfree by a partition of the interval (e.g., with uniform arc length distribution) into a sequence of monotonically increasing discrete path parameter values sjj=1n with the boundary conditions γnews1=εc=qinit, γnewsc=ρc=qC, and γnewsn=δc=qt. Moreover, the sequences γc1sjj=1c∈Mfree in the first interval and γc2sjj=cn∈Mfree 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 qr∈γnewsjj=1n on the geodesic path will approach the geodesics γc1s and γc2s 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 γnews 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 qinit=γnews1,γnews2,…,qC=γnewsc,…,qt=γnewsn 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 γnewsc. 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 (n-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  γc could be determined on the basis of the length of γc1 or γc2 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△⊂M is formed by the sides composed of the initial geodesic and two replanned geodesics. Let γi△i=1,2,3 denote the edges γ, γc1, and γc2, and let θii=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–32]:(20)∬△KdA+∑i=13π-θi+∑i=13∫γi△κgds=2π, where K is the Gaussian curvature, dA is an infinitesimal area element on M (dA=detgijdu1du2 in local coordinate chart (u1,u2)), and κg is the geodesic curvature of ∂△. Since the geodesic curvature κg≡0 on γi△i=1,2,3, the term ∑i=13∫γi△κgds=0 in (20). We can thus rewrite (20) as the angle sum:(21)θ1+θ2+θ3=π+∬△KdA.We see that as K=0 everywhere on △, then ∑i=13‍θi=π. 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 thatK=0 everywhere on △. Let the initial geodesic γ pass through the center of the circle. Then the two replanned composite geodesic paths, γF and γG, would be symmetric with respect to the initial geodesic, and the lengths of γF and γG are identical. Furthermore, there exists α∗ that makes γF and γG the shortest.The symmetric routes generated by the path replanning procedure are illustrated in Figure6.Figure 6
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 user-defined hazard grade O=1. As the initial geodesic cuts the circle in half (i.e., passes through the center of the circle), the two symmetric routes γF=γf2∘γf1 (purple) and γG=γg2∘γg1 (green) are piecewise linear line segments connected via critical points.IfK>0 everywhere on △, then the angle sum ∑i=13θi>π, which means the local terrain in the neighborhood of △, appears to be a convex landform such as an elliptic surface. On the contrary, if K<0 everywhere on △, then the angle sum ∑i=13θi<π. 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 Mfree 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 ∑i=13θi>π or ∑i=13θi=π because the local terrain with the angle sum ∑i=13θi<π, 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 pointP, and the tangent plane TPM 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 TPM and the availability of (at least one) qC∈Mfree 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 pointP has a well-defined tangent plane TPM. 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 TPM 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 differentv 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 ∑i=13θi<π, 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 ∑i=13θi≥π 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 pointP 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 qC may be the same as qCtan and the projection of qC to find qCtan 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.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.


## 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 inE3 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 TPM in all tangent directions at any P∈M, and thus the intersection of TPM with M is a single point P. In addition, the ellipsoidal surface is a typical manifold with positive Gaussian curvature K=h4/(a1a2a3)2>0 at the point P [32], where h is the distance from the center of ellipsoid to TPM and a1, a2, and a3 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 is6×160×160 cubic unit shown in Figures 8 and 9. The initial and terminal points are qinit=(77,0,0.6) and qt=(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 O=4, so the hazard region on the ellipsoidal terrain is a circle of radius rO=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 qC=(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 E3.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 “▽” denote the initial and the terminal locations, respectively.Figure 9
Geodesic replanning. The initial geodesic in Figure8 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.


## 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.



---

*Source: 2539761-2016-10-23.xml*