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.
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 which is embedded in the Euclidean space [1, 7–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–12] for planning and replanning a path for mobile robot navigation on 3D terrain.
As will be stated formally later in Section 2, a geodesic linking the given initial position and terminal position has the following properties for navigation in addition to shortest length: it is continuously differentiable; it is unit speed with velocity in the tangent direction so that the maximum velocity constraint is easily handled. In other words, its acceleration is normal to the manifold so that the geodesic curvature is zero along the geodesic, and thus the two-point boundary value problem (TPBVP) arises from geodesic differential equations on Riemannian manifold. Given the terrain, the initial position, and the terminal position on the terrain, the initial geodesic connecting them, and the single obstacle therein, the proposed geodesic replanning algorithm consists of the steps of directional scanning, critical point selection, path generation, and collision checking. It relies on the properties of the geodesic and the directional scanning on the tangent plane for exploration of terrain. The outcome of this procedure is two distinct composite geodesic paths connecting the initial and the critical points and the terminal entirely confined on the terrain. Furthermore, the new composite path and the initial path form a triangular region, called geodesic triangle, which satisfies the local Gauss-Bonnet Theorem. This theorem serves as guidance for selecting a better replanned path and terrain at the planning stage.
Related Work on 3D Path Planning. There are a number of methods and concepts available for efficient 2D path and motion planning of mobile robot, which could be a mobile platform or a wheelchair, in planar maps. Some are interested in employing parametric curves other than circular arcs and line segments for planning a 2D path meeting smoothness requirements or for smoothing the paths, so that the obtained paths could be actually executed without unnecessary stops or velocity discontinuities by the robots with specific shapes, kinematics, and dynamics. Building on these progresses in path planning in planar maps, researchers and engineers have been increasingly interested in generating geometric representations of 3D terrain from raw point cloud obtained from range sensors without plane assumption (e.g., B-spline patches [13] and tensor voting framework in combination with -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 (dimensional)-manifold such as torus. Reference [7] designed a Riemannian metric on point cloud to reflect the surface for finding the most desirable direction (as flat as possible) for traversal with reduced energy consumption.
Suppose that an initial path is given in an environment. In case the initial route crosses the obstacle, the path should be replanned for obstacle avoidance. Visibility-based obstacle-avoidance approach is often employed for polygonal obstacles [2]. In order to bypass the obstacle of general shape encountered by the robot, one approach commonly used in 2D environment is to follow a path that is tangent to the circumference of the obstacle in front of the robot until there is a collision-free path toward the goal. In a 2D plane, there are only two directions of motion to follow: counterclockwise or clockwise. By contrast, in a 3D environment, there are an infinite number of tangent directions of motion on the surface of obstacle of general shape which causes planning difficulty [21, 22] if without a priori or online traversability analysis of terrain, since the terrain traversability may obstruct the robot to successfully traverse on the boundary surface of an obstacle of general shape in 3D environment. Reference [23] suggested the use of geodesic between two specified points on the obstacle surface as the boundary following path in 3D environment. Meanwhile, without considering the typical objective of path length and terrain traversability information, successful application of boundary following imposes a restriction on the shape of the obstacles and may often yield a longer time- and energy-consuming path and slow down the movement.
The remainder of the paper is organized as follows. Section 2 introduces the differential geometry of 3D terrain constructed by the Riemannian manifold and the system of geodesic differential equations which is used as a control system for navigating the mobile robot along a path composed of geodesics. Section 3 describes the geodesic replanning procedure accommodating the collision constraint of the robot, which is based on the representation of the obstacle as a circle with a hazard grade tunable radius. Associated with geodesic replanning procedure, local Gauss-Bonnet Theorem of the geodesic triangle formed by the initial geodesic and the replanned geodesics will also be presented for aiding traversability analysis of the terrain that the replanned path traverses in Section 3. Simulation result is given in Section 4 for demonstrating the geodesic replanning procedure. The conclusion is made in Section 5.
2. Shortest Path Planning on Manifold
The terrain on which the robot operates is assumed to be a smooth manifold. In this section, we will work on the geometric framework of manifold 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.
2.1. A Brief on Manifold
For path planning of a mobile robot modelled as a point, the terrain profile is modelled as the smooth surface embedded in the Euclidean three-dimensional space , called manifold. Intuitively, a manifold embedded in 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].
Definition 1. A topological space is called a topological 3-manifold if (1) is Hausdorff and second countable;(2) is locally Euclidean. Every point has an open ball that is homeomorphic to an open subset such that a homeomorphism is called a coordinate chart.An atlas is a family of charts for which constitutes open covering of ; that is, . 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. 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 , at each point , the partial velocities along the coordinate curves are linearly independent and thus could serve as a basis for , where , . Any tangent vector on can thus be represented by the basis vectors . 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 , a Riemannian metric is a symmetric, positive-definite quadratic form: + , 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 is the inverse matrix to the metric tensor (i.e., if or 1 if ).
Note that ; 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 and tangent to along at to produce a new vector field at as the rate of change of in direction in the basis .
Definition 3. Let for be two vector fields. Then the covariant derivative of the tangent vector field with respect to the tangent vector field is the operation , , .
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 denote the geodesic curvature of a curve at a point which measures how a curve deviates from being a geodesic [24, 30–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 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 in the direction of principal normal and a tangential component composed of the covariant derivative 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: .(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 . Given a curve and given a continuously differentiable mapping of coordinate chart , where is an open set parametrized by the local coordinate on , the path could be expressed in terms of the parametrization as with and , where is the path parameter. We are interested in computing the length-minimizing paths between a pair of points . Let , , and form a Riemannian metric tensor with as its component. The Riemannian metric induced distance function defined over is [30, 35]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, , and (recall the fact that the definition of Riemannian metric is the quadratic form , where is the arc length. In particular, is a unit speed curve parametrized by arc length). Any path satisfying 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 with fixed endpoints on Riemannian manifold, the two unknown coordinates and of geodesic satisfy the geodesic differential equations in terms of Christoffel symbols defined in Definition 2 [24, 30–32]: with the boundary conditions at 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 and denote the velocity by (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 , 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 (length-minimizing 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 .
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 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.
3.2. 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, denotes the set of natural numbers, and denotes the infimum of the length of all continuously differentiable paths from an arbitrary path point to a point . 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] holds.
In the presence of single obstacle, one strategy is as follows. The robot can first follow the initial geodesic from 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 , where 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 denote the collision-free region:(1)Input. We are given the terrain , the circular region of hazardous region on , and the points and on the terrain. Let be the initial geodesic connecting 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) 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 on and an value that determines the range of directional scanning along the direction. A line extending from in the direction is drawn. Choose two segments of equal length with opposite direction from this selected tangent line, where 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 and , respectively(3)Step 2 (critical point selection). Choose the projection of and 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 , go to Step (4)Step 3 (path generation). Connect via geodesics the initial position 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 and repeat the procedure of searching candidate critical points with new and/or a new value(6)Output. A new collision-free composite geodesic path
In what follows, for notational simplicity, we use subscripts and . Step can be formally described as follows. Let be parametrization at , and let be the initial geodesic. Denote as the arc length value of so that . The tangent plane , which contains the velocity vector of the initial geodesic at , can be decomposed into two independent (not necessarily orthonormal) unit tangent directions , . The vector (as Figure 1 shows) assigns to each a unit normal vector at . The trihedron , , represents a moving frame of along the initial geodesic. Let be a unit tangent vector of . The tangent line through along direction is . 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 onto the terrain along the normal of , as depicted in Figure 4, can be obtained simply aswhere there exists such that . 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 , where is step size and is the direction, and check if is on . The computation of in parameter space for one direction is continued until good enough 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 is composed by two geodesics, for the first interval and for the second interval, which are joined at the critical point . Thus, a new path continuous everywhere can be expressed as 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., ) is larger than the length of the initial geodesic ; that is, the shortest path going from to via geodesic directly is always shorter (and also faster for unit speed robot) than the path going from to via a waypoint .
Remark. Since 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 and 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 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, is uniquely determined [30, 32] and thus could be computed only after is computed. However, this , 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.
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 confined on the terrain. Let be discretized into a set of path points by a partition of the interval (e.g., with uniform arc length distribution) into a sequence of monotonically increasing discrete path parameter values with the boundary conditions , , and . Moreover, the sequences in the first interval and in the second interval are Cauchy sequences on , which is assumed to be a complete Riemannian manifold. Therefore, succession of robot states on the geodesic path will approach the geodesics and 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 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 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 . The travel time (efficiency) of constant speed navigation based on geodesic segments increases monotonically with the increase of the overall path length of . Note that the accuracy of navigation depends on the number of waypoints. The computation of 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 or 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 is formed by the sides composed of the initial geodesic and two replanned geodesics. Let denote the edges , , and , and let 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]: where is the Gaussian curvature, is an infinitesimal area element on ( in local coordinate chart ), and is the geodesic curvature of . Since the geodesic curvature on , the term in (20). We can thus rewrite (20) as the angle sum:We see that as everywhere on , then . 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 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 everywhere on , then the angle sum , which means the local terrain in the neighborhood of , appears to be a convex landform such as an elliptic surface. On the contrary, if everywhere on , then the angle sum . 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 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 or because the local terrain with the angle sum , such as a hyperbolic paraboloid, may be deemed less preferable since energy-consuming ascending and descending motions are required, and the motion on this type of terrain which involves curvature sign change would require that the mobile robot have better mobility and maneuverability.
3.5. Additional Design and Implementation Considerations
The replanning algorithm requires the computation of four new geodesics in each iteration, while the initial geodesic, the point , 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) by iterating Steps and . 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 directions at , the manifold may have different curvatures, indicating the way the manifold is bending in given 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 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 , 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 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 and the projection of to find is not needed, the computation is greatly reduced. Figure 7 summarizes the discussions made so far as a flowchart of 3D path planning and waypoint navigation via geodesics. It is noted that since only position continuity is ensured at the critical point of the composite geodesic, the robot temporarily stops at the critical point to change its heading toward the second geodesic while traveling at unit speed along each of the two geodesic segments.
4. Simulation
For simulation of wheeled mobile robot navigation, we assume that the robot is a point that moves at unit speed, moves in the direction of its velocity/tangent vector (so that the nonholonomic constraint of no-slip motion is satisfied), and is capable of switching its heading instantaneously to follow the continuous spatial curve confined on the 3D terrain accurately. In this section, a simulation aimed at demonstrating the applicability, instead of generality, of the proposed geodesic replanning procedure for mobile robot navigation on a smooth (implicit and parametric) surface in 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 at the point [32], where is the distance from the center of ellipsoid to and , , and 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 cubic unit shown in Figures 8 and 9. The initial and terminal points are and , 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 and center . The hazard grade for this obstacle is , so the hazard region on the ellipsoidal terrain is a circle of radius unit and center 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 , which is a good linear approximation of the ellipsoid nearby . The critical point chosen for this simulation is , 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 .
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 of six dimensions.
Competing Interests
The authors declare that there are no competing interests regarding the publication of this paper.
Acknowledgments
The authors are grateful to Kevin C.- W. Lo and J.-M. Chen for valuable discussions and Dr. T.-Y. Zing and Dr. H.-T. Hwang for constructive criticisms.