G² Continuity Smooth Path Planning using Cubic Polynomial Interpolation with Membership Function

Abstract

Path planning algorithms are used to allow mobile robots to avoid obstacles and find ways from a start point to a target point. The general path planning algorithm focused on constructing of collision free path. However, a high continuous path can make smooth and efficiently movements. To improve the continuity of the path, the searched waypoints are connected by the proposed polynomial interpolation. The existing polynomial interpolation methods connect two points. In this paper, point groups are created with three points. The point groups have each polynomial. Polynomials are made by matching the differential values and simple matrix calculation. Membership functions are used to distribute the weight of each polynomial at overlapped sections. As a result, the path has $G^2$ continuity. In addition, the proposed method can analyze path numerically to obtain curvature and heading angle. Moreover, it does not require complex calculation and databases to save the created path.

1. Introduction

The aim of path planning for mobile robots and vehicles is finding a path from the start to the target point with avoiding obstacles. The potential field method [1-3], ‘A* and D* algorithm’ [4-6] and ‘RRT algorithm’ [7, 8] are proposed for searching path.

The purposes of global path planning are the avoidance of stationary obstacles and solving mazes. On the other hand, considering the continuity, the path can be improved to use real movement, stable motion, smooth movement and optimal path. In addition, it can be used to analyze a planned path before driving.

The continuity of the path is defined as G0, G1, G2 … Gn in geometry [9]. G0 continuity is just contacted at each point by a straight line. The G1 continuity path is equal to the primary differential value at each point. The path will curve from G1 continuous. The G2 continuity path has an identical second-order differential value at each point. This means the same curvature at each point. In particular, the Gn continuity means continuity of the nth order differential value at each point [10].

In order to use smooth path in the real environment, the continuity should be moreover G2 continuity. It means the G2 continuous path has a continuous velocity functions and acceleration functions. So this path has continuous-curvature. As a result, if the mobile robot and vehicles follow the smooth path, the velocity is not changed abruptly in the path. Thus, they can be moved with the continuous velocity and acceleration without stop motion. This movement is efficient with time, energy and dynamics.

Kanayama and Hartman documented a definition of path curvature and the derivative path curvature [11]. Fraidchard and Ahuactzin suggested the continuouscurvature path planning for cars [12]. Lamiraux and Lammond proposed steering method for a car-like vehicle providing smooth paths [13]. Kito et al. described the path planning with maximum curvature, curvature derivative and curvature continuity [14]. Yang and Sukkarieh reported the continuous-curvature path-smoothing algorithm with efficient and analytical [15]. Villagra et al. indicated the efficiency and comfort of the smooth path and speed planning for the vehicle transportation system [16].

The obtained path from ‘RRT connect algorithm’ [7, 8] consists of ‘ qinit (the initial configuration)’, waypoints, ‘ qgoal (the goal configuration)’ and a line connecting the points in consecutive order [7, 17, 18]. A line connecting path is G0 continuity. ‘RRT connect algorithm’ [7, 8] can only solve mazes and stationary obstacle avoidance problems. The paths required to improve the G2 continuity to apply at the real environment.

Generally, in order to improve the continuity of path, the path should be reconstructed using an interpolation method considering the continuity. The interpolation method estimates f(x j+m) at xj+m with only given x and f(xi) [19, 20]. Polynomial interpolation and spline interpolation are used in general. Polynomial interpolation aims to find a polynomial including whole points [19]. The order of the polynomial is increased by the number of points. A high order polynomial occurs ‘Runge’s phenomenon’ [21]. This type of polynomial is unsuitable for applying a large number of points. Spline interpolation produces a polynomial between points by the differential value at the points [19]. Cubic spline interpolation is widely used and satisfies the C2 continuous [10].

Polynomial interpolation and spline interpolation involve numerical analysis to determine the function of connecting characteristic points that satisfy the ‘x0 < x1 < x2 < ⋯ < xn’ condition. In other words, they can be used under the condition of an always increasing x [19]. Therefore, normally interpolation methods are unsuitable for mobile robots and vehicles with complex movement. Therefore modified interpolation method has applied a mobile robots and vehicles. Piazzi et al. attempted to smooth path planning using the quantic G2 -splines [22], the η3-splines [23, 24] and the G3-splines [25]. However, methods of Piazzi have complex calculations such as high order polynomial and trigonometric functions which require the high performance hardware. Yang et al. proposed a continuous-curvature path-smoothing algorithm using cubic Bézier curves [26]. Bézier curve is a popular method to create curves for computer graphics [9]. Many smooth path studies have used this method, but the planned path is not visited entire waypoints because this method requires control points to decide the curvature. So some waypoints are used to the control points or require an algorithm to decide the control points. In the path planning method, the planned path must visit every waypoint. Therefore, a method using Bézier curves is not suitable to apply the smooth path planning.

On the movement of 2D plane, x is not changed in horizontal movement and y shows no variations in vertical movement on the 2D map. The parametric method is used in the present paper. The parametric method expresses x and y by u. The parameter u is the accumulation value of the distance between waypoints. This parameter is always a positive, increasing function and does not require the ‘x0 < x1 < x2 < ⋯ < xn ’ condition. The polynomial of x and y can be obtained using parameter u in any movement.

Spline interpolation makes a polynomial containing two points using point values and differential values. In addition, it makes another polynomial of the next two points in the same method. In this paper, the point group is made by three points. The quadratic polynomial and cubic polynomial include three points obtained by point values and differential values. The next group is made by two previous points and one new point. The quadratic polynomial and cubic polynomial about the new group is derived and extended to the next groups. The neighboring polynomial occurs in the overlapped section. Membership functions distribute the weight of each polynomial to make an improved continuity path in an overlapped section by parameter u. In addition, the planned smooth path could visit every waypoint unlike any other smooth path planning methods.

The proposed method provides the difference values. The curvature and heading angle can be calculated using the differential values of x and y . These data can be applied to an analysis of the planned path and real movement. In addition, a controller and control algorithm use these data. The curvature is concerned with the dynamics. In other words, the robots and vehicles have the maximum velocity about curvature. If the controller and control algorithm use this data, the user can check a driving time with stable movement and the maximum velocity before real driving.

This paper is organized as follows: section 2 describes a method for obtaining a quadratic polynomial and cubic polynomial with three points. The difference between the proposed method and the general spline interpolation method is explained. Section 3 proposes a method for making a rotated polynomial using the parametric method. In section 4, the distribution of weight is explained in a polynomial overlapped section. In section 5, the continuity, curvature and heading angle functions are described. Section 6, the proposed method is simulated to find the alternation of the primary differential values, second-order difference values. These values prove that the proposed method can make a G2 continuity path. In addition, the curvature and heading angle are obtained by the proposed method. These provide a data for real driving and path analysis

 

2. Spline Interpolation and Polynomial Interpolation

2.1 Spline interpolation

Spline interpolation connects each point by a polynomial. A cubic polynomial interpolation is used in general. The method for obtaining the polynomial involves matching the primary differential value and second-order differential value at each point [19, 20]. The planned path is the matched curvature at each point that satisfies the G2 continuity.

The G2 continuity requires the matching of primary and second-order differential values at each point. Matching of the differential value is difficult in a quadratic polynomial because the second-order differential value is constant in a quadratic polynomial. Therefore, a quadratic polynomial can only use the G1 continuity graph. Thus, the minimum order for satisfying over G1 continuity is three, which means a cubic polynomial. This is why a cubic polynomial used generally.

The widely used spline interpolation requires a change in x. x should be increased.

On the other hand, the paths of the robots or vehicles are not satisfied (1). They have a variety of movements. x or y is not variable in vertical or horizontal movement. Therefore, general spline interpolation is unsuitable for expressing the path trajectory.

2.2 Quadratic polynomial interpolation

The quadratic polynomial interpolation is a method to connect three points continuously. There are three points like as P1 , P2 and P3 The polynomial connecting these three points can be expressed as equation (2).

The primary and second-order differential values of quadratic polynomial are (3) and (4).

The curvature k is obtained using equation (5) by (3) and (4).

The graph of the quadratic polynomial has a concave or convex shape (Fig. 1). To use the quadratic polynomial interpolation, (1) should be satisfied. If (1) is not satisfied, the graph can be expressed, as shown in Fig. 2. In Fig. 2, the line is a rotation polynomial graph of connecting P5 , P4 and P3 in serial order, and the dot line is the rotation polynomial graph with P2 , P1 and P6 in consecutive order.

Fig. 1.Quadratic polynomial with 3 points (P1 → P2 → P3 and P4 → P5 → P6)

Fig. 2.Graph of the rotated quadratic polynomial (P2→ P1→P6 and P5→P4→P3)

2.3 Cubic polynomial interpolation

The cubic polynomial is the third-order function. The basic form can be expressed as (6).

The second-order differential function is needed to check the G2 continuity. The primary and second-order differential functions are (7) and (8).

The curvature k (9) can be calculated by (7) and (8).

Fig. 3 shows the basic graph of cubic polynomial.

Fig. 3.Graph of rotated cubic polynomial

More than four conditions are needed to use a cubic polynomial interpolation by (6). On the other hand, a normal polynomial graph is unconformable to apply the path of a mobile robots or vehicles. The path should be shortest and realizable. In Fig. 3, there are convex and detour paths when connecting P1 and P2. In particular, the path should be as short and realizable as possible with continuity. Therefore, if done correctly, the general cubic polynomial interpolation is unsuitable for application to mobile robots or vehicles.

The proposed method is the rotated form of the cubic polynomial. Fig. 4 is the rotated graph of cubic polynomial. The graph can be expressed unsatisfied (1).

Fig. 4.Cases of three points connected quadratic polynomial graph

2.4 Rotated polynomial graph

The center point is set to obtain a polynomial of three points connected in serial order. Three points exist Pn–1, Pn and Pn+1 . Pn is the center point. If the three points connecting the graph are rotated centered on Pn, two cases exist. The first case is a straight line on Pn–1 and Pn+1. In this case, the curvature is unlimited and is the simplest way of movement. The second case is the curve on Pn–1 and Pn+1. In this case, the graph will be convex or concave. The optimal smooth path is a convex or a concave graph by maintaining G2 continuity.

A quadratic polynomial is a polynomial including three points with continuity. If it is rotated, the center point will approach the vertex.

(10) are the general rotation functions. θ is the rotation angle.

(10) are impossible to obtain a graph that connects each point in consecutive order. In addition, it is difficult to express with a quadratic polynomial or cubic polynomial, such as (11) and (12). (11) and (12) should be used to find a , b , c , d and θ. On the other hand, trigonometric functions hinder solutions from being found.

Fig. 5 shows three cases, where P1 , P2 and P3 are connected. The sequential path cannot be derived by (11) or (12). Therefore, another method is needed to connect the points sequentially and rotated graph. This method should be able to apply a quadratic polynomial and cubic polynomial.

Fig. 5.Graph of the cubic polynomial with three points

 

3. Parametric Quadratic Polynomial

3.1 Quadratic polynomial with parametric method

In this section, the quadratic polynomial and cubic polynomial are expressed by parameter u . If the three points are existed, the center point should be approached the vertex of a graph to rotate the polynomial and the points should be connected in serial order. To rotate the polynomial, x and y are separated by parameter u. The parameter u is the accumulation value of the straight line distance between points. As a result, parameter u is always a positive and increasing value and the graph has rotated shape. u can express to (13) with three points.

(2) can change to (14) and (15) using parameter u.

ax , bx , cx , ay , by and cy can be calculated by (16) and (17).

Primary and second-order differential value of (16) and (17) are expressed as (18) and (19).

(16) and (17) make a polynomial with three points. The three points are then applied to (16) and (17) in more than three points path. Pn–1 , Pn and Pn+1 are used to obtain nth polynomial. (n + 1)th polynomial is made by Pn, Pn+1 and Pn+2. (13) is changed to the general function as (20). (16) and (17) can express to (21) and (22) with (n + 1)th points.

nth points are needed in a (n − 2)th quadratic polynomial. Every polynomial contacts the whole points and each polynomial had G2 continuity including three points Pn–1 , Pn and Pn+1.

For example, two polynomials are the existence with four points. Each polynomial has the overlapped section (Fig. 6).

Fig. 6.Two quadratic polynomial graph including four points

3.2 Cubic polynomial with parametric method

Parameter u consists of quadratic polynomial obtained by (16) and (17). The quadratic polynomial can be expressed using parameter u. The polynomial is in contact with three points in serial order and has a rotated graph form. Each polynomial has G2 continuity. On the other hand, it is unsure in more than three points. To make G2 continuity, a second-order differential value is matched at each point. Therefore a higher order polynomial is needed.

(14) and (15) can be expanded to a cubic polynomial. (23) and (24) are the basic form of cubic polynomial using parameter u.

(23) and (24) will be (25) and (26) in more than three points case. Pxn (u) is nth x-axis polynomial including xn-1 , xn and xn+1 . Pyn (u) is nth y-axis polynomial including yn-1, yn and yn+1.

(25) and (26) are cubic polynomials including Pn-1, Pn, Pn+1 in consecutive order.

a, b, c and d of (25) and (26) cannot be obtained by three points because another condition is needed to match the second-order differential value at each point. (27) and (28) are added to the conditions.

(27) and (28) apply to (25) and (26). (29) and (30) are then obtained.

(25), (26), (27) and (28) can be combined into matrix (31).

(31) can change to (32) via an inverse matrix.

axn-1 , bxn-1 , ayn-1 , byn-1 and un-1 can be obtained by Pn. On the other hand, they do not exist if (n = 2). Therefore, polynomial of P2 must use a quadratic polynomial by (21) and (22). The range of n values is changed, and (32) can be expressed as (33).

As a result, the Px2 and Py2 polynomials are connecting P1, P2 and P3 by a quadratic polynomial. In addition, n is expanded to n + 1, n + 2, ⋯, m − 1 to obtain a cubic polynomial with matching second-order differential value at each point.

The parametric method provides a variation of x and y. They are connected with parameter u. This method is useful to express complex movements.

 

4. Polynomial Interpolation with Membership Function

Polynomials of mth points are consist of quadratic polynomials and cubic polynomials with a matching second-order differential value. On the other hand, there are overlapped sections between nth polynomial and (n + 1)th polynomial. Membership functions distribute the weight of the polynomial in overlapped sections. They are functions of the parameter u.

X(u) is a group of Pxn and Y(u) is a group of Pyn. The smooth path is denoted as (34),

The weight of the nth polynomial and ( n + 1 )th polynomial are adjusted by the distance between the points in overlapped sections. In particular, the nth polynomial has the largest weight at the nth point and decreases as it approaches the (n + 1)th point. The weight of the (n + 1)th polynomial is zero at nth point, but it increases by approaching the ( n + 1 )th point. (35) and (36) are the membership functions expressing parameter u.

In Fig. 7, μr is zero at Pn and 1 at Pn+1. At the same time, μf is 1 at Pn and zero at Pn+1 . (37) is (34) combined (35) and (36).

Fig. 7.Graph of the membership functions

The general form can be expressed as (38).

Px2 (u) and Py2 (u) effect to the first section (P1 to P2) and Pxm-1 (u) and Pym-1 (u) make the path in the last section (Pm-1 to Pm ). On the other hand, memberships function and polynomials are combined in the other sections.

Fig. 8 is the weight variation of the polynomial by parameter u.

Fig. 8.Membership functions and polynomials

The membership functions change the weight of each polynomial by a parameter u in the overlapped section. As a result, separated smooth paths are combined to one smooth path that maintain the continuity.

 

5. Continuity, Curvature and Heading Angle

The proposed method can calculate the curvature and heading angle. This method can be used to analyze the planned path and background data of real movements.

The primary differential value and second-order differential value are needed to obtain the curvature. They can be expressed as (18) and (19) in the quadratic polynomial. (39) are differential values of P(X(u), Y(u)) using (18) and (19) in case of the quadratic polynomial.

(40) are the primary and second-order differential values of the cubic polynomial by (29) and (30).

(38) can change to (41) and (42) to find differential values.

Px2 (u) and Py2 (u) are expressed as the quadratic polynomial connecting P1 , P2 and P3 . The cubic polynomials are shown in other sections. This can maintain the G2 continuity by (31) and (32).

The curvature k can be obtained using (41) and (42).

The heading angle can be calculated using (44).

The differential values are used to analysis of the planned path for the velocity and acceleration control. The curvature concerns the maximum velocity. In addition, the heading angle indicates the direction of robots and vehicles, and they use to obtain an angular speed.

 

6. Simulation

In this section, two simulations are shown. The first simulation is a comparison with a ‘Quintic G2-splines’ [22]. This spline method connects each waypoint using the fifthorder functions and trigonometric functions. The proposed method created a smooth path using given waypoints. The second simulation shows the smooth path from an ‘RRT algorithm’ searched path. This path has G0 continuity. The proposed method improved the continuity to G2 continuity.

6.1 Case of [22] ‘Quintic G2-splines for the iterative steering of vision-based autonomous vehicles’

[22] reported motion planning using the quintic G2 - spline. The G2 continuity path is shown with the five points using the proposed method. Table 1 is the given waypoints of [22].

Table 1.Points of [22]

The proposed method can be applied to [22]. Fig. 9 shows the results by (38). In [22], paths between each waypoint consist of quntic function and trigonometric functions. However, the proposed method can create the path by using the cubic polynomial.

Fig. 9.[22] (black line) and the cubic polynomial interpolation with a membership function (red line)

Figs. 10 and 11 show the variation of x and y by u. These are continuous shape. To check continuity, the differential values should be observed.

Fig. 10.u vs. X(u)

Fig. 11.u vs. Y(u)

Figs. 12 and 13 show the primary differential values of X(u) and Y(u) by (41). The graph has continuous shape. This means that the path has G1 continuity at least. Therefore, the planned path is continuous on the velocity.

Fig. 12.u vs. dX(u)

Fig. 13.u vs. dY(u)

To confirm G2 continuity, the second-order differential values are checked. Figs. 14 and 15 show the second-order differential values by (42).

Fig. 14.u vs. d2X(u)

Fig. 15.u vs. d2Y(u)

Figs. 14 and 15 have not open sections with any points. These are show that path is G2 continuity. These graphs indicate that the planned path has acceleration continuity.

Fig. 16 shows the variation of curvature k by (43). This graph is continuous and closed in whole sections. These are features of the G2 continuity path. This data can be applied to control of the velocity and acceleration for real driving.

Fig. 16.u vs. curvature k

Fig. 17 shows the variation of heading angle θ using (41) and (44). θ can be important data of the driving simulation, real driving and analyze of planned path.

Fig. 17.u vs. heading angle θ

Figs. 10-15 show that the path is the G2 continuity. Fig. 9 compares path of [22] with the proposed path. The figure shows some difference between the path of [22] and the proposed path in P1 to P2 section. This is because path of [22] has an internal heading angle at P1, but the proposed path has no internal heading angle. The internal heading angle is the constraint of simulation. On the other hands, it is not needed in real driving.

The curvature graph (Fig. 16) is continuous, and Fig. 14 and Fig. 15 are not open in whole sections. In addition, the variation of heading angle can be obtained.

As a result, proposed path has G2 continuity with checking variation of differential values and curvature.

6.2 Case of the roadmap

This simulation is an improving continuity. The proposed method uses only waypoints. If searching algorithm obtains a collision-free linear path and waypoints, the proposed method can improve to the G2 continuity using searched waypoints.

Fig. 18 shows a path from ‘RRT algorithm’ [7, 8]. This is a simply connected path at each point with G0 continuity. The proposed method was applied to Figs. 16 and 18 can be express as Fig. 19. This figure shows the G2 continuity smooth path including every waypoint. In order to check the continuity of the path, the differential values were obtained.

Fig. 18.The path from ‘RRT algorithm’ [18], an example of how to solve a query with the roadmap

Fig. 19.The path from ‘RRT algorithm’ [18] vs. G2 continuity path using proposed method

Figs. 20 and 21 show the variation in X(u) and Y(u) by (38).

Fig. 20.u vs. X(u)

Fig. 21.u vs. Y(u)

Figs. 22-25 show the variation in the primary differential values and second-order differential values by (41) and (42). They have not open sections in every point. As a result, the proposed path is G2 continuity that just checking these graphs.

Fig. 22.u vs. dX(u)

Fig. 23.u vs. dY(u)

Fig. 24.u vs. d2X(u)

Fig. 25.u vs. d2Y(u)

Fig. 26 shows the change in curvature by (43).

Fig. 26.u vs. curvature k

Fig. 27 shows the variation of the heading angle by (44).

Fig. 27.u vs. heading angle θ

The G0 continuity path using the ‘RRT algorithm’ [7, 8] change to the G2 continuity path by the proposed method. The proposed method could obtain the change in curvature and heading angle data. This simulation indicates the combination with the searching algorithm. The searching algorithm cannot create the smooth path. However, it can provide waypoints. The proposed method uses these waypoints and the process of finding a smooth path is simple.

 

7. Conclusion

Most searching algorithms do not consider the continuity of a path. The proposed method aimed to improve the continuity. Polynomials derived using point groups consisted of three points. They were extended to other groups and used to create polynomials. The point groups considered the differential values at the points. Differential value matching improved the continuity, and a parametric method was used to obtain a polynomial with complex movement. The parameter functions consisted of a linear distance variable u. Membership functions were used to distribute the weight of each polynomial’s influence. As a result, the G2 continuity path was obtained in the whole sections. The proposed method could analyze the path numerically. This shows that the curvature and heading angle can be found using differential values. The curvature and heading angle are useful data to design a control algorithm considering dynamics.

The proposed method was verified by two simulations. The first simulation compared with the quintic G2-spline. A similar result was obtained and could analyze variation of x, y, the curvature and the heading angle. The second simulation produced a G2 continuity path from ‘RRT algorithm’ path. The ‘RRT algorithm’ could obtain only a G0 continuity path. On the other hand, with the ‘RRT algorithm’, a path could change to the G2 continuity path using the proposed method.

When there were waypoints by the searching algorithms, the continuous path between waypoints could be obtained calculations using the proposed method. An increase in the volume of data is not necessary and the path can be updated upon a change in environment. This is because the proposed method uses a simple matrix form and does not use trigonometric functions or high order functions which require high performance hardware and complex calculations compare with other smooth path planning algorithms. As a result, the proposed method can apply to simple system. In addition, the control algorithm could be stable and simple.

The proposed method can be applied to mobile robots, vehicles, game algorithms and computer graphics. The method can be used in flying robots and aircraft if it can be expanded to 3D space. The method of the expanding points group can be used to improve the continuity of the path in local path planning.