A G2 Continuous Path-smoothing Algorithm Using Modified Quadratic Polynomial Interpolation

Path searching algorithm is one of the main topics in studies on path planning. These algorithms are used to avoiding obstacles and find paths from starting point to target point. There are dynamic problems that must be addressed when these paths are applied in real environments. In order to be applicable in actual situations, the path must be a smooth path. A smooth path is a path that maintains continuity. Continuity is decided by the differential values of the path. In order to be G2 continuous, the secondary differential values of the path must be connected throughout the path. In this paper, the interpolation method is used to construct continuous paths. The quadratic polynomial interpolation is a simple method for obtaining continuous paths about three points. The proposed algorithm makes a connection of three points with curves and the proposed path is rotated using the parametric method in order to make the path optimal and smooth. The polynomials expand to the next three points and they merge into the entire path using the membership functions with G2 continuity.


Introduction
Path planning is an important topic in robotics studies. One of the general purposes of path planning is to construct collision free paths. Typical approaches include the D* algorithm [1], the Probabilistic Roadmap Method (PRM) algorithm [2], the Rapidly-exploring Random Tree algorithm (RRT) [3], and the potential field algorithm [4]. These algorithms can construct collision free paths, but they cannot guarantee that the resulting paths will be able to apply driving with stability and optimality in real environments.
Collision free paths are designed for obstacle avoidance. However, a continuous path is also suitable for the actual movements of robots. The actual movements of robots are affected by various forces (i.e., gravity, centrifugal force and centripetal force). These forces contribute to dynamic problems such as the slip and over-actuation of wheels, limitations of velocity and acceleration, and the effects of inertia. The forces that affect real movements need to be taken into consideration when planning and improving paths.
Considering these forces, paths should be continuous paths. There are paths connecting points on a two dimensional plane. Continuity is defined using geometry [5]. A continuous path is simply a path that is connected for entire sections. These paths can be constructed by the path searching algorithms mentioned above. A continuous path matches the primary differential values at each point. This path shares a common tangent direction. A continuous path has the same secondary differential values at each point. This path also shares a common centre of curvature. Hence, a continuous path signifies the equality of the differential value at each point.
The primary differential values on the paths indicate velocity. In addition, the secondary difference values indicate acceleration. Thus, the continuous paths have continuous velocities. The continuous paths are guaranteed to have continuous accelerations. These can reduce dynamic problems.
In order to apply the mobile robot or vehicle, Suzuki et al. demonstrated smooth path planning with pedestrian avoidance for a wheeled robot [6]. Vaillagra et al. proposed smooth path and speed planning for smooth autonomous navigation [7]. Yang et al. reported a continuous curvature path-smoothing algorithm using cubic B zeir spiral curves for non-holonomic robots [8].
A continuous path is also called a smooth path and the construction of continuous path algorithms is defined as path smoothing algorithms. There are two kinds of path smoothing algorithms. The first type is a graphical method and the second is a functional method. The graphical method uses lines, arcs, circles and clothoids to construct smooth paths. It is possible to analyse the results from graphical methods. The Dubins method [9] is a typical method for doing this. The Dubins method finds the shortest curve that connects two points in the two dimensional plane using lines and arcs. This method can be expanded into another graphical method. However, methods such as these have problems with limitations on path expressions and discontinuities at junction points. In order to solve these problems, studies have focused on functional methods.
Functional methods use mathematical functions to express the path. These methods can be used to construct various types of paths. The spline interpolation method is widely used. This method finds functions for connections between two points using differential values at each point [10]. Komoriya et al. suggested the B-spline method for designing trajectory and controlling wheel-type mobile robots [11]. Magid et al. proposed a spline-based method for robot navigation [12] and Piazzi et al. demonstrated smooth path generation using splines for wheeled mobile robots [13]. The spline method uses trigonometric functions and high order functions. Therefore, the complexities of the calculations increase and the numerical analysis can be complicated.
The polynomial interpolation method [5] is a simple functional method. This method finds a polynomial that includes all of the points. The number of points that are needed to construct a order polynomial is . Polynomials can be used to find simple methods for constructing paths. However, high order polynomials lead to the Runge's phenomenon [14] and a sharp increase in computation.
The essential requirements of a path smoothing algorithm are as follows: it must be possible to obtain the path using simple methods and calculations; the functions should be expressed in differential form; continuity must be maintained throughout the proposed path; it must be possible to express the vertical and horizontal path; it must be able to obtain the heading angle and curvature in order to enable analysis of the planned path.
To satisfy these requirements, this paper used a functional method. The spline interpolation method is used to form connections between points. These connections were suitable for the path smoothing algorithm using the start point (SP), the way points (WP) and the target point (TP) (these points are the position that robots or vehicles must visit on the map). They are also suitable for converting paths from line connecting paths to smooth paths. However, the trigonometric functions and high order functions can be used with the advanced spline interpolation method [13]. These functions lead to a high level of complexity. It is possible to use the polynomial interpolation method for the smoothing algorithm along with the necessary requirements. However, the polynomial method does not give an optimal path for random points. Also this method cannot be used many WPs, because Runge's phenomenon can occur. Additionally, high order polynomials increase the calculation load.
Spline interpolation methods and polynomial interpolation methods can be used to interpolate a graph of the geometry. These methods should be used with the variations of or . The or values are always increased or decreased in order to obtain the functions by general polynomial interpolation methods (Newton's polynomial interpolation and Lagrange's polynomial interpolation) [5]. Therefore, general polynomial interpolation methods cannot express vertical or horizontal movements like those of robots and vehicles.
In this paper, the polynomial interpolation method is used. If the � or � values of points are located at equal distances, the quadratic polynomial is the minimum order polynomial that includes three points with � � continuity. However, another method is required in order to express a path of irregular points or a path of points that has the same value on axis. It is possible for the rotated polynomials to give the shortest and continuous path for three points; however, it is difficult to obtain the rotated polynomials using normal methods. The general rotation method uses a rotation matrix. This matrix relies on trigonometric functions and can therefore lead to complex calculations. In this paper, the parametric method is used to obtain the rotated polynomials.
To apply for three or more points, the quadratic polynomial is extended to the subsequent three points and they are combined with the previous polynomial. Therefore, the proposed method should be able to construct a � � continuous polynomial that includes the next three points using the quadratic polynomial. In this paper, the membership function is used to combine polynomials. The membership function is part of a fuzzy algorithm. This function is used to regulate the certainty of a fuzzy set. The proposed method uses the membership function for distributing the degree of the polynomials in the overlapping sections. The fuzzy set assumes the polynomials and the membership function is changed based on the accumulation value of the linear distance. This function controls the degree of each polynomial. Thus, the � �� polynomial has the maximum degree at the � �� WP, but this degree decreases closer to the �� � 1� �� WP. The �� � 1� �� polynomial has the minimum degree at the � �� WP. This degree increases closer to the �� � 1� �� WP. Hence, the polynomials are combined by the membership function controlling degree at each WP. This paper is organized as follows: section 2 explains the characteristics of quadratic polynomials and a conception of path smoothing using the quadratic polynomials. In section 3, the parametric method is proposed for constructing the rotated quadratic polynomials. Section 4 analyses the rotated quadratic polynomials. Section 5 describes how the membership function can be used to merge the polynomials. Section 6 reports the simulation results for the proposed path. Section 7 documents the conclusions.

Characteristics of quadratic polynomial
The quadratic polynomial is a second order function for connecting three points. If this function is used for the path, then the three points include the SP, WP and TP. If the WP is closer to the vertex, the path can be more appropriate for the shortest and continuous path.
If (3) and (4) are satisfied, the graph of (1) is shown in Figure 1.  (3) and (4)) In Figure 1, the graph can have one of two forms depending on the values of � � , � � and � � . This graph is unique and satisfies � � continuity.
If (3) and (4) are not satisfied, then � � is not the vertex. The resulting graph is shown in Figure 2.  (3) and (4)) The quadratic polynomial can be obtained from (1) depending on the location of the point. In the case of #1 (green curve) in Figure 2, is the vertex between and . However, in the case of #2 (blue curve) in Figure  2, , and are not sequential. This path can be a distorted path. Therefore, constraints (3) and (4) must be satisfied to obtain the shortest and continuous path with continuity in the general quadratic polynomial.
If constraints (3) and (4) do not hold or points , and are not sequential (5), the quadratic polynomial cannot be obtained for the following condition: . The values of must have variation. These constraints mean that general polynomial interpolation cannot apply to the path smoothing algorithm. (5)

Shortest and continuous path using quadratic polynomial
If the points do not satisfy constraint (5), then the values of are not sequential or the values of have not changed. Figure 3 shows the graph of the polynomial for an irregular . Points , and are located irregularly on the curve. The simple method (2) can be used to construct the quadratic polynomial that connects , and . However, it is impossible to use this method to produce a sequential path that connects , and . Furthermore, the other general methods (Lagrangian interpolation polynomials and Newton's interpolation polynomials) [5] require the variation of or . The path cannot be constructed using these methods for the following conditions: , , and . These conditions occurred for vertical and horizontal movements on the 2D-plane.
The shortest and continuous path for irregular values of is shown in Figure 4. The red curve is the shortest curve centred on (middle point) and includes , and . This curve is constructed using the quadratic polynomial. In this paper, this red curve is used to construct the smooth path with continuity.

Modified quadratic polynomial interpolation using the parametric method
In section 2, the quadratic polynomial graph is centred on and is rotated. However, (1) is changed by the rotation matrix, which is based on the trigonometric functions that are used to construct the rotated quadratic polynomial [15]. The trigonometric functions are occurred complex calculations and open sections. Vertical and horizontal paths cannot be constructed using the trigonometric functions. The key characteristics of trigonometric functions are that has an unlimited value at and .
In this paper, a parameter is used. The parameter is the linear distance between each point. is always zero. is the distance between and .
can be obtained by calculating and the distance between and . The functions are as follows: (1) can be transformed into (8) and (9) using .
Equations (8) and (9) are the quadratic polynomials of . The parameter always has a positive value and connects each point in the serial order with irregular and values. The function of is changed to the function of . The function of is seperate from that of and . Each of the functions has the same characteristics and the continuity that is derived from the quadratic polynomial.
Equation (12) is a generalized from of equation (6). The � parameter represents the number of points.

Analysis of quadratic polynomial
The primary and secondary differential values of (1) are described as follows: The curvature of (1) can be obtained using (18) Equations (8) and (9) can be differentiated using �.
�� �� � �� � � � � � , The curvature of the parametric quadratic polynomial can be described as follows: The primary differential function is a function of �. The secondary differential function takes on a constant value. The general quadratic polynomial (that connects only three points) has the same primary and secondary differential values at each point. Therefore, each polynomial has � � continuity as those in (21) and (22).
The heading angles of mobile robots and vehicles are defined as follows: The heading angles are important data for path analysis and driving. The primary differential values of � and � can be used to obtain these values (24).
However, the method for finding continuity and heading angles cannot be applied to the overlapping sections. Section 5 describes the method for finding these values in the overlapping sections.
5. Modified quadratic polynomial interpolation using the parametric method and membership function

Membership function
The membership function is used to control the degree of truth in the fuzzy control theory. The membership function ���� increases from 0 to 1 as � increases. ���� starts at 0 and increases to 1 until the centre of � . This function decreases to 0 again as � increases. In this paper, the membership function is used to control the degree of each polynomial's effect on the entire path. In Figure 6, � is changed to � and the membership function is extended to the � �� membership functions, as shown in Figure 7. The membership function of � �� has two types, a rising function and a falling function. The dotted lines in Figure  7 represent the rising function. This function has increased from 0 to 1 for the � ��� to � � section. The falling function is represented by the dashed lines in Figure 7. This function has decreased 1 to 0 for the � � to � ��� section. General function can be described as follows:
Hence, the quadratic polynomial will be the cubic polynomial in each of the sections �� � � � � �, �� � , � � �, ⋯, �� ��� , � ��� � . In this paper, the proposed algorithm is concerned with finding the quadratic polynomial using irregular � and � . However, these polynimals are changed into the cubic functions by combining the membership functions. The graph of the overlapping sections has a curve representing the degree distribution based on the membership functions. The proposed method changes the path in Figure 5 into the path in Figure 8.

Analysis of proposed path
To analyse the proposed path, the differential values should be obtained and checked. The primary differential value can be defined using (21), (22) and the membership functions. The primary differential values are decribed in (33) and (34). (33) The secondary differential values are described in (35) and (36).
continuity is related to the matching of the primary differential value at each point and continuity is related to the agreement of the secondary differential value at each point. Each polynomial has the maximum degree at each point. Thus, and have the maximum degree at the point. Therefore, the continuity at each point is decided by the differential values of and . The values of and have continuity at the , and points. The next set of polynomials, and are continuous at the , and points. Hence, each polynomial has continuity. If these polynomials are combined using the membership functions, then continuity can be maintained throughout the entire path, because the membership functions distribute the degree of the polynomials and the differentials of the polynomials.
The heading angle can be obtained by modifying (24) using the primary differential values from (33) and (34).

Smooth path planning of random points
There are random points on the 2D-plane. 8 7 2  11  11  45  45  25  12  25  5  15  10  10  14  30  12  45  45  25   Table 1. Random points for simulation The points of and have various values. and have the same value. Also, and have the same value. This means that the movement from to is a vertical movement and the movement from to is a horizonal movement.
The linear distance between the points can be obtained using (12). Table 2  The variations of and can be changed into a function of using (31) and (32). Figures 9 and 10 show the graphs of and vs. .  The entire path that is shown in Figure 11 is based on (31), (32), Figure 9 and  The proposed path is a continuous curve. To verify the continuity, the primary differential values should be checked. The primary differential values are obtained using (33) and (34). Figures 12 and 13 display the graphs of the primary differential values.  The and values are connected throughout each of the graphs. The connectedness of the primary differential values signifies that the proposed path is continuous.
The status for continuity can be decided by checking the secondary differential values. These values can be obtained using (35) and (36). The graphs are as follows : Figures 14 and 15 display the secondary differential values of proposed path. The values are also connected throughout each of these graphs. These graphs demonstrate that the path has continuity.  The curvature graph is shown in Figure 16. The curvature can be obtained using (23). The heading angle graph that is shown in Figure 17 is based on (37). This simulation shows that the proposed method can be used to construct continuous paths. This method can also use for the analysis of paths with random points. Figure 18 shows a path from the PRM planner [16]. This planner is used to construct collision free continuous paths. In this simulation, the proposed method will be applied to the planning process for continuous paths in order to improve continuity.       The curvature and heading angle are shown in the following graphs: In this simulation, the proposed method was used to improve a continuous path so that it was also continuous. The differential values, curvature and heading angle were also obtained. These values show that the proposed method can be used to construct continuous and analysable paths.

Conclusions
Continuous paths are important for driving mobile robots and vehicles in real environments. The spline interpolation method and polynomial interpolation methods have problems with tasks involving expressing and analysing paths. In this paper, the quadratic polynomial interpolation method was used to construct a continuous path. The quadratic polynomial can easily be obtained using a simple matrix. The proposed method is based on the rotated quadratic polynomial. This polynomial gives the shortest path with continuity and also the minimum order. To express the rotated quadratic polynomial, the parametric method was used. This method uses an accumulated linear distance value . The variations of and values are transformed into functions of . The polynomials of and can be expressed using the polynomial interpolation method. These functions are always increasing and make it possible to express vertical and horizontal movements. However, it is possible to use the proposed method only if there are three points. To apply the proposed method for three or more points, the membership functions are used to distribute the degrees of polynomials in overlapping sections. The quadratic polynomials are combined with the membership functions, which affect the whole path because of variations in the membership functions.
The differential values are obtained using the differential functions of and . These values can be used to prove the continuity of path and can also serve as the basis for the curvature and heading angle.
Simulation results show that proposed methods can construct a continuous path with WPs. The first simulation constructed a continuous path with ramdom points. The second simulation demonstrated that the proposed algorithm can be applied directly to the searched path from the PRM algorithm.
In order to use a real robot navigation system, the searched paths have WPs, or they are continuous paths. PRM algorithm planned path can change directly, as shown in the simuilation. However, the D* algorithm and potential field algorithm requires another algorithm to decide WPs.
If there are WPs and a sequential rule of visiting WPs, the proposed method can construct a continuous path using only a second order polynomial. In addition, this method is a new interpolation method. It is not a required database for saving entired path. Additionally, the proposed method can use local path planning. In local path planning, if mobile robots or vehicles know the following two WPs, the proposed method can construct a continuous path of three points. In addition, if robots or vehicles reach the next point, the proposed method can update the paths using the reached point and the next two points.