Mathematical Problems in Engineering

Volume 2016 (2016), Article ID 2539761, 12 pages

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

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

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

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

Academic Editor: Mustapha Zidi

Copyright © 2016 Kun-Lin Wu et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

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