Path Planning with Obstacle Avoidance Based on Normalized R -Functions

. Existing methods for path planning with obstacle avoidance need to check having the interference between a moving part and an obstacle at iteration and even to calculate their shortest distance in the case of given motion parameters. Besides, the tasks like collision-checking and minimum-distance calculating themselves are complicated and time-consuming. Rigorous mathematical analysis might be a practical way for dealing with the above-mentioned problems. An R -function is a real-valued function whose properties are fully determined by corresponding attributes of their parameters, which is usually applied to express a geometrical object. Thus, a signed distance function based on R -functions is created to represent whether two objects intervene and their level of intervention or separation. As the signed function is continuous and differentiable, the gradient information of the objective function guides a moving part to avoid its obstacles and to approach its target position rapidly. Therefore, a path planning approach with obstacle avoidance based on normalized R -functions is proposed in this paper. A discrete convex hull approach is adopted to solve the problem that R -function is inappropriate to represent a geometric object with some curves or surfaces, and pendent points and edges are generated in Boolean operations. Besides, a normalized approach ensures accuracy calculation of signed distance function. Experimental results have shown that the presented approach is a feasible way for path planning with obstacle avoidance.


Introduction
In recent years, technologies of path planning with obstacle avoidance have received extensive attention in the fields of robotics, computer simulation, and game animation.In the product design process, path planning with obstacle avoidance is an essential element of the simulations of product assemblies and disassemblies.Path planning with obstacle avoidance is in essence an optimization problem, which usually introduces the evaluation criteria (such as the shortest path or the shortest planning time) to plan an optimal path with obstacle avoidance from a given initial position to the destination in high-interference environments [1].
To avoid obstacles, existing approaches for path planning are necessary to frequently detect whether two objects intervene or to estimate the shortest path between a moving part and an obstacle at the given location.Therefore, there is not a unified form for planning path with obstacle avoidance.Moreover, collision-checking and minimum-distance estimating themselves are complex and time-consuming tasks.To deal with the above-mentioned problems, rigorous mathematical analysis might be a practical way for path planning with obstacle avoidance.
The attributes of an R-function are fully determined by corresponding attributes of their argument.Thus, the sign of a real function is fully determined by the sign of its argument.R-function is usually applied to represent a geometric object [2].If g is an R-function that expresses a geometric object, we can use a signed distance function g(x)≤0 to describe interference constraints between this geometric object and a space point .Thus, g(x) <0, g(x) =0, or g(x)>0, respectively, represents that space point  is the internal, the boundary, or the external of the geometric object.
Based on the above-mentioned analysis, a path planning method with obstacle avoidance based on normalized Rfunctions is presented in this paper.Firstly, Boolean operations based on R-functions are introduced to generate an implicit function for expressing a geometric object.Secondly, a discrete convex hull method is adopted to solve problems that R-function is inappropriate to represent a geometric object with some curves or surfaces, and there are some pendent points and edges in Boolean operations.Then, a normalized method is introduced to transform the implicit function into the m-th order normalization function so that the calculation of signed distance function is accurate.Finally, planning paths with obstacle avoidance for a single-point and a polygonal region are conducted to validate the presented approach.Experimental results have shown that the proposed method is a feasible way for path planning with obstacle avoidance.
The rest of this paper is organized as follows.First, related works are introduced in Section 2.Then, the theory of Rfunction is presented in detail in Section 3. In Section 4, a discrete-convex hull method for generating CSG models of geometric objects is introduced.Section 5 adopts the normalization method to transform g into the m-th order normalization function.And path planning with obstacle avoidance based on the normalization function is given in Section 6.Following this section, experimental results applying the proposed method are presented in Section 7. Finally, the paper ends up with some conclusions in Section 8.

Related Works
Path planning with obstacle avoidance has been extensively applied for the robot navigation.For a robot, path planning carries out a series of translational motions and rotational motions from an initial position to the target position while obstacles are avoided in its working environment [3].Most of path planning methods with obstacle avoidance can be classified into three categories: potential field method (PFM), sampling-based method, and heuristic-based algorithm.
Artificial PFM appeared earlier in [4], which is simple and highly efficient.However, disadvantages like local minima, nonsmooth movement, and oscillations also appear in artificial PFM.To escape from the location of the local minima, Kim [5] presented a new artificial potential function for path planning.Lee et al. [6] proposed new potential function with random force algorithm using PFM for solving the risks of a robot-obstacle collision and local minima trap.To overcome artificial PFM with slow progress and system instability in the presence of obstacles and narrow passages, a modified Newton's approach was applied to potential field [7].Sfei et al. [8] proposed a new formula of repelling potential for reducing oscillations and avoiding collisions when the target approaches obstacles.To solve problems where a robot only follows the destination while obstacles are rejected in conventional artificial PFMs, Cosio et al. [9] proposed a path planning method with obstacle avoidance based on improved artificial PFM and genetic algorithm.Yin et al. [10] and Zhang et al. [11] adopted modified PFMs to solve path planning problem in dynamic environments.
The advantage of sampling-based path planning methods (SPPMs) has abilities to deal with planning problems in complex and/or time critical real world.At present, most influential SPPMs are probabilistic road-maps (PRMs) and rapidly exploring random trees (RRTs) [12].Although PRMs and RRTs both randomly sample connecting points from their state space, their graphs connecting points are generated in different ways [13].More work related to SPPMs can be found in [14].PRMs have better performance for path planning in high-dimensional state space.As curves or straight lines are used for constructing PRMs, a robot may go anywhere in its free space.Visibility graph [15][16][17] and Voronoi diagram [18][19][20] are two typical road-map approaches, which have achieved better results with dramatically various types of roads.RRT algorithms randomly build a space-filling tree to efficiently handle problems with obstacles and differential constraints by searching nonconvex and high-dimensional spaces [21].Kothari et al. [22] proposed a RRT method to handle path planning problem of unmanned aerial vehicles with uncertainty.The probability of constraint violation and uncertain dynamic obstacles are, respectively, limited by a chance constraint and its extended framework.To further improve search efficiency, Bi-RRT algorithms with bidirectional search have appeared [23,24].Two RRTs, respectively, root at the initial and the target state, and algorithms seek states that are "common" for two trees.In [24], a Bi-RRT * approach with the intelligence is adopted to handle path planning problem with complex cluttered environments.
A complete overview to heuristic-based algorithms for path planning, which can be found in [3].

Theory of R-Functions
3.1.Terminology Definition.The sign of an R-function is completely determined by the sign of its variable and has nothing to do with the value of its variable.For example, a function g 1 =x 1 x 2 x 3 is negative, if the number of its negative variables is odd or positive otherwise.In general, the domain of variable not just has positive or negative but has the form ≥a or <a (a is a real number).In a practical application, the real domain is usually divided into (-∞, -0] and [+0, +∞).Then, the definition of an R-function can be expressed as follows.
Definition 1 (Rfunction y).Give a function S(x) with real parameter x, which has the forms A real function  = ( ( A group of R-functions with the same adjoint function is called an R-function branch.Key characteristics of an Rfunction are given in the following section (more characteristics can be found in [2]).
(1) The set of R-functions has closure properties under a composition operator.Thus, a function can be obtained by compositing some R-functions, and it is also an R-function.
(2) If  1 and  2 are R-functions from the same branch, then the function  1 + 2 is an R-function belonging to the same branch.

Sufficiency
Completeness.The characteristics of an Rfunction show that a complex R-function may be generated from simple R-functions.If there is a group of R-functions, which can generate R-functions belonging to any branch by using composition operators, we define this group of Rfunctions as a function system with sufficiency completeness.
Here, a( . If a=0 or a=1, we call a   system an  0 system or an  1 system.For function systems    and   , their logical operations can be defined as follows: And their "¬" operations have the same handling rule with   -function system.Although the above-mentioned  0 -function system and  1 -function system have the same logic function, their differential characteristics are various.For instance,  0 -function system is nondifferentiable at the origin of coordinate while other points are differentiable; but  1 -function system is nondifferentiable at the point  1 = 2 or  1 =- 2 ; and   function system has m-order differential operators at the origin of coordinate while other points have infinite differential operators.Differential characteristics of   -function system have great influence in practical applications.  function system has better differential characteristics while the calculation of  0 -function system is simple.

R-Function Representation for a Geometric Object.
Boolean operations (such as "intersection" and "union") between basic primitives can be used to express a CSG model [26].Then, a geometric object  has the following form: If we define the signed distance function  i (x) for a basic primitive   as (6) can be rewritten as follows: Given a continuous function g(x) and its adjoint function G( 1 , . . .,   ), a geometric object  in ( 6) can be expressed as the following form [16]: In the above formula,   (i=1, 2, . .., m) is a real continuous function while  is an implicit function of a geometric object.In the following, we give an example to illustrate the definition of implicit function .In Figure 1(a), basic regions  1 and  2 , respectively, have the following equations: It can be seen from Figure 1(a), rectangular region  has logic function B= 1 ∧  2 .Then, using  0 -function system to represent region  has the following implicit function g: If  1 in (10) and  2 in (11) are plugged into (12), (2) has

Discrete-Convex Hull Method
In Section 3.3, using R-function to express a geometric object is based on its CSG model.However, a geometric object usually has various CSG representations.For example, rectangular region  in Figure 1(a) has logic function B= 1 ∨  2 in Figure 1(b).CSG representations usually have various logic functions and implicit functions, but they have the same logic functionality.In fact, it is an intractable task to automatically generate a CSG representation for a complex object.Even so, existing algorithms can automatically generate CSG representation for polygon regions and polyhedrons.
In [27], a polygon region is automatically divided into some Sidechains with the intersection of convex hull vertices.Then, the intersection of the half-space defined by Sidechains can be adopted to express a polygon region while the half-space  is represented by the intersection or the union of the edges.
If the intersection of two edges is a concave, the half-space is the union; otherwise, the half-space is the intersection.For instance, polygonal region  in Figure 2 has logic function , and  4 = 7 ∧  8 .Besides, convex hull method has been applied to polyhedrons in a three-dimensional space [28,29].However, the above-mentioned method is unsuitable for a geometric object with some curves or surfaces.Then, the surface is approximately discretized into planes while the curve is approximately discretized into straight lines.Thus, plane region  in Figure 3 The above-mentioned approach for dealing with a geometric object is called a discrete-convex hull method.If the geometric object is a polygon without a curve or a polyhedron without a surface, the implicit function is accurate; otherwise, the implicit function approximately expresses the geometric object.
Discrete-convex hull method not only automatically generates CSG representation for a geometric object but also solves the problem of Point Membership Classification (PMC) [30].PMC problem might lead to the implicit function with 0 value for a nongeometric object on its boundary.There are the following two aspects.First, irregular Boolean intersection ∩ for a basic primitive might produce pendent points, pendent edges, or pendent planes (see Figure 4).As the operation ∧ for an R-function is irregular, the implicit function of a geometric object may has pendent points on its outside part.Second, rectangular region  has logic function B=  1 ∨ 2 in geometry (see Figure 1(b)).But for mathematical analysis, the operation ∨ of an R-function cannot ensure that the implicit function has 0 value on B's boundary.As PMC exists, the implicit function cannot define the interference constraint between a geometric object and a space point.Fortunately, a discrete-convex hull method avoids the abovementioned problems.The reasons are that two adjacent lines (planes) of a polygon (polyhedron) just intersect in one point (straight line), and this point (straight line) must belong to the boundary of this polygon (polyhedron).For the operation Boolean union, the discrete-convex hull method will not produce the PMC problem.
Therefore, the implicit function of a geometric object based on discrete-convex hull method has 0 value on the object's boundary.And the internal point and external point, respectively, have a value < 0 or > 0.Then, the sign of the function determines whether a space point and a geometric object have the interference.If g(x) represents an implicit function of a geometric object, g(x)≤0 shows that there is no interference between the space point  and the object.

Normalized R-Function
The implicit function based on the above-mentioned approach cannot accurately calculate the signed distance between a space point and an object's boundary.For instance, the function has value of -0.84 at the point (1, 0) for the rectangular region given by the implicit function equation (13) in Section 3.But the minimum signed distance between the point (1, 0) and the boundary of the rectangular region is -1.0 in practices.
Let  be the boundary of a geometric object and  be a space point; the signed distance (, ) between the point  and the boundary  can be expressed as follows:  (, ) ≡ inf  (, ) ,  ∈ . ( where (, ) is the signed distance between the point  and the point q; if  ∈ , the signed distance (, ) is "+" (there is the interference between the point  and the point q); otherwise, it is "-".If all space points satisfy the condition () = (, ), the function  is called G's normal function.For example, let (a, b) and , respectively, be the centre and radius of a circle; the normal function  of this circular region has the following form: As most of geometric objects have some nonsmooth adjacent surfaces, their normal functions usually do not exist.Thus, we introduce the concept of normalized function for a geometric object.A similar normal function  with essential   differential attributes for a complex object is introduced, which satisfies the following conditions: where  is a regular point on the boundary L and V is the normal unit vector of the point .Then,  is the morder normalized function of a geometric object.There are very small differences between normalized function near the boundary and signed distance function.And normalized function far from the boundary is gradually, continuously, and smoothly similar to signed distance function.In general, many R-functions have the above-mentioned normalized properties.For an R-function without normalized properties, the following method can be adopted to normalize.( Then, g can use the following formula to normalize into the first-order form. Moreover, higher-order normalized method for the function  can be found in [31].M-order normalized functions  1 and  2 have the same logical operations  1 ∨  2 and  1 ∧  2 with  function.Figure 5 lists the distribution of first-order normalized function (it is also called similar signed distanced field;   -function (p=2) system is adopted) for the polygonal region in Figure 2.
The generation of normalized function of a geometric object can be summarized below.
Firstly, a discrete-convex hull method is adopted to generate the implicit function  using the following three steps: (1) A geometric object is transformed into a polygon (polyhedral).Then, a half-space implicit function that points to the interior of a geometric object is created based on its polygon (polyhedral) and edges (planes).(2) A convex hull method is introduced to generate CSG representation and logic function for this geometric object.
(3) The logic function, the implicit function of an edge (plane), and appropriate R-function system are adopted to construct the implicit function  for whole geometric object.
Secondly, the implicit function  is normalized into .

Path Planning with Obstacle Avoidance
In obstacle environments, path planning with obstacle avoidance is to find an obstacle avoidance path for a moving part from a given initial posture to the final target posture.It is an optimization problem with noninterference between moving parts, noninterference between a moving part and a barrier, or kinematics constraints.For simplicity, we only discuss path planning with a moving part and noninterference between a moving part and a barrier in this paper.For the environments with  obstacles, path planning with obstacle avoidance based on typical optimization can be expressed as follows: min  () , In the above formula, x is a motion variable and   (x) ≤0 represents noninterference between a moving part and the ith obstacle and f (x) is an objective function.
For existing path planning approaches with obstacle avoidance based on numerical analysis, most of interference constraints cannot be explicitly defined using the abovementioned inequality.Thus, two procedures are adopted to solve these problems.First, the interference checking is used to qualitatively determine whether there is interference between a moving part and an obstacle.Second, if it is noninterference, the minimum distance between a moving part and an obstacle and gradient direction of motion parameters are quantitatively calculated, which are used to  guide next iteration.Although existing algorithms can deal with them, they usually have heavy computing burden.Fortunately, normalized R-function of a geometric object is not only applied to represent interference constraints between two objects, but also there are essential differential properties.Then, (19) can be rewritten as the following forms: min  () , ..  (  ()) ≤ 0 Here,   and DPt j , respectively, express normalized Rfunction of the i-th obstacle and a discrete point of moving part M's surface.
For path planning with obstacle avoidance, we must ensure that planning paths have noninterference between a moving part and a barrier.Then, it requires that each iteration point will not default for solving (20).The reason is that interference constraint conditions cannot be strictly ensured for some artificial potential field methods [11].Therefore, a feasible SQP method in [32] is adopted to solve (20).SQP is a local optimum algorithm based on the gradient with the superliner convergence.
Based on the above analysis in Sections 3.3, 4, 5, and 6, the flow diagram of the proposed method is given in Figure 6.

Experimental Results
To validate the proposed approach in this paper, some experiments have been performed for path planning with obstacle avoidance on the following three conditions: (1) The generation of the implicit function is similar in a two-dimensional space and a three-dimensional space, and the representation of the implicit function is relatively complex in a three-dimensional space.
For simplicity, examples are conducted in a twodimensional space.
(2) Compromise differential properties and the computational complexity for a normalized function; an  function system (p=2) is adopted to create the implicit function for an obstacle.
(3) The tests are conducted on a computer with DELL OPTIPLEX 760 and Windows XP and Matlab (R2001a).
First, a single-point moving part  is moved to the target position  from an initial position  through the translation movement in  direction and  direction, and the obstacles  1 ,  2 , . ..,  5 have been avoided (see Figure 7).Besides, the obstacle  5 has an arc .Then, it is easy to get normalized function of an obstacle and to create interference constraints for (20).
In Figures 7(b) and 7(c), O 5 's arc  is, respectively, discretized into 4 and 6 straight-line segments, and their corresponding planning paths are listed.As there is a higher discrete approximation for O 5 's arc  in Figure 7(c), its planning path is more reasonable.In general, the curve of an obstacle has more discrete straight-line segments; normalized function is more complex, which will lead to higher computational complexity.The time is spent on planning paths for O 5 's arc  with various discrete approximations is given in Table 1.
Figure 7(d) is partial enlargement of dashed boxes in Figure 7(c), which is listed to further illustrate the role of normalized function in path planning.Red and blue plots in   Second, a moving part  is a polygonal region, which has three degrees of freedom (the translation movement in  and  directions, and the rotation motion in  direction), and  1 ,  2 ,  3 , and  4 are obstacles (see Figure 8).To generate interference constraints between a moving part and an obstacle, M's boundary is uniformly discretized into 142 points.And M' is the target posture of a moving part.The translation and rotation motions of a moving part  make bold edges of  and the obstacle  1 are aligned, and their midpoints  1 and  2 are coincident in Figure 8(a).Therefore, the objective function for (20) where  1 (x) is a function of external normal vector  1 of M's bold edge for motion parameter x and  2 ≡(1 0 0) expresses external normal vector of O 1 's bold edge and  1 (x) represents a function of the midpoint  1 of M's bold edge for the parameter x and  1 and  2 describe weight coefficients.As the cosine value of the vectorial angle is smaller (≥-1 and ≤1), we, respectively, set  1 =100 and  2 =1. Figure 8(b) has listed obstacle avoidance planning paths for a polygonal region M, and dashed lines represent M's posture.To observe clearly, solid lines are adopted to express M's typical postures.And Figure 8(c) is partial enlargement of dashed boxes in Figure 8(b).It can be seen from Figure 8, moving part  successfully avoids obstacles  2 and  3 on the basis of posture adjustments.Experimental results have shown that normalized functions of the obstacles can well approximate their signed distanced fields, and their gradient information may guide obstacle avoidance planning paths.
The proposed approach for path planning with obstacle avoidance is based on semianalytic geometry with Rfunctions, which has rigorous mathematical analysis.In general, the characteristics of the algorithm are that the results usually have higher accuracy while the processing speed of the algorithm is relatively slow.In particular, the processing time for various instances has greater diversity.

Conclusions
A path planning method with obstacle avoidance based on normalized R-functions is presented in this paper.First, an R-function is introduced to represent a geometrical object, which ensures that normalized R-function near the boundary of a geometrical object has little difference with signed distance functions, and other parts are continuously and smoothly similar to signed distance functions.Thus, normalized R-function can be applied to define interference constraints between a space point  and a geometrical object.As for the generation of interference constraint between two geometrical objects, this needs to plug discrete points of a geometrical object's boundary into normalized R-function of another object.Finally, path planning with obstacle avoidance for a single-point and polygonal region is conducted to validate the presented approach.Experimental results have shown that the proposed method is a feasible way for path planning with obstacle avoidance.

Figure 1 :
Figure 1: The rectangular region  and its logic function .

Figure 2 :
Figure 2: Convex hull method for a polygon region.

Figure 3 :Figure 4 :
Figure 3: Plane region  with a curve is discretized.
If the function  ∈   , a point  on the boundary  satisfies the following conditions:      = 0,  V         > 0.

Figure 5 :
Figure 5: The normalized function and its similar signed distanced field.

Figure 6 :
Figure 6: The flow diagram of the proposed method.
A planning path for O 5 's arc c with 4 discrete straight-line segments A planning path for O 5 's arc c with 6 discrete straight-line segments Normalized functions of obstacles affect the iteration direction

Figure 7 :
Figure 7: Planning path with obstacle avoidance for a single-point moving part.

Figure 8 :
Figure 8: Path planning with obstacle avoidance for a polygonal region moving part.

Figure 7 (
Figure 7(d), respectively, express outer distance field of obstacles  2 and  3 .At an iteration point I,  2 and  3 , respectively, express negative gradient direction of normalized function of obstacles  2 and  3 , and  o represents negative gradient direction of the objective function.To avoid obstacles  2 and  3 ,  2 and  3 make final search direction be revised to  at iteration point I.Second, a moving part  is a polygonal region, which has three degrees of freedom (the translation movement in  and  directions, and the rotation motion in  direction), and  1 ,  2 ,  3 , and  4 are obstacles (see Figure8).To Here,   (i=1, 2, . .., m) is a basic primitive; and  is a regular Boolean function whose operations have forms ∩ * , ∪ * , and - * .If we use logical operators ∧, ∨, and ¬ to displace Boolean operations, G will become a logical function.Combined with the definition of R-function, a geometric object  can be expressed as follows: [ ( 1 ) ,  ( 2 ) , . . .,  (  )] = 1

Table 1 :
The time is spent on planning paths for O 5 's arc  with various discrete approximations.