A new approach to optimal smooth path planning of mobile robots with continuous-curvature constraint

ABSTRACT Smooth path planning is very important to mobile robots with continuous-curvature constraint, but there are still some limitations and drawbacks on traditional planning approach. To deal with this problem, a new approach combined with parametric cubic Bezier curve (PCBC) and particle swarm optimization with adaptive delayed velocity (PSO-ADV), is developed to plan the smooth path of mobile robots. Unlike the traditional smooth path consisting of several linear and curve segments with discontinuous curvature at the joints, the smooth path composed of PCBC segments has equivalent curvature at the segment joints, thereby it is able to attain continuous curvature along the whole smooth path. In terms of the mathematical formulation of PCBC, the smooth path planning is essentially an optimization problem to seek the optimal control points and parameters of PCBC segments. To handle this intractable problem and some frequently encountered troubles (e.g. premature convergence and local trapping), a new PSO-ADV algorithm is developed by blending the term of adaptive delayed velocity, and its superiority can be confirmed by several simulation experiments. The new approach is finally applied to produce the smooth path with continuous-curvature constraint, and can achieve superior performance in comparison with traditional method.


Introduction
With the rapid development of robotics in recent years, autonomous mobile robots have been widely used for various service tasks (Liu et al., 2018), and path planning (PP) is one of the most critical technics for the autonomy of mobile robots (Lin et al., 2017). The main task of path planning is to produce an optimal and feasible path for a mobile robot from the start to the destination, and till now, a large amount of algorithms have been developed to tackle the issues of mobile robot path planning, see e.g. Gonzalez et al. (2016), Fong et al. (2015), Elbanhawi and Simic (2014), Atyabi and Powers (2013), and the references therein.
Among these algorithms, the heuristic intelligent optimization algorithms have been extensively investigated to deal with the path planning problem, which is subject to several performance criteria and constraints (e.g. the shortest path and collision avoidance) (Atyabi & Powers, 2013;Fong et al., 2015;Qu et al., 2013;. For example, a hybrid algorithm has been proposed for the path planning of a mobile robot in Contreras-Cruz et al. (2015), where the artificial bee CONTACT Baoye Song songbaoye@gmail.com; Maoyong Cao my-cao@263.net College of Electrical Engineering and Automation, Shandong University of Science and Technology, Qingdao 266590, People's Republic of China colony algorithm is firstly utilized to plan a set of feasible paths in the local searching procedure, and then the evolutionary programming algorithm is applied to refine the local results in the searching space. The firefly algorithm has been applied to navigate the mobile robot in an uncertain environment, in which the uncertainty is introduced by the variation of the environmental conditions from static to dynamic (Patle et al., 2018). In , a novel PSO (particle swarm optimization) algorithm combined with the DE (differential evolution) algorithm and a non-homogeneous Markov chain has been presented for the path planning of intelligent robot, and the effectiveness and feasibility of the algorithm are verified by several simulation experiments. A hybrid algorithm combining the two-dimensional cellular automata with the ant colony has been put forward to plan a feasible path for the mobile robots in the environment with both convex and concave obstacles (Akbarimajd & Hassanzadeh, 2012), where the working environment of the mobile robots is decomposed into a great deal of rectangular grids, and the four states of the automata updated by the evolutionary rules are used to guide the mobile robots towards their destination. In Nazarahari et al. (2019), a hybrid approach combining the artificial potential field algorithm with an enhanced genetic algorithm has been developed to deal with a multi-objective path planning problem in the environment with several mobile robots, and some comprehensive evaluations on several benchmark path planning problems are carried out to demonstrate the superiority of the proposed novel approach in comparison with some well-known algorithms. In B. Tang et al. (2016), the PSO algorithm is hybridized with the DE algorithm to tackle the issues such as stagnation and premature in the global path planning of mobile robots, and the superior performances of the proposed algorithm are verified by comparing with several state-of-the-art intelligent optimization algorithms.
However, there are still several limitations in the aforementioned works, and one of them is the multi-objective path planning problem (Nazarahari et al., 2019), which usually includes not only the minimization of the path length, but also several other objectives such as smoothness and safety of the path (Song et al., 2016), etc. Among others, the smoothness of a planning path, which has proven to be an imperative criterion that is closely related to other criteria (Castillo et al., 2007), has attracted increasing research attentions in recent years. For example, an A * algorithm has been employed to seek for a feasible path from the start to the destination on the directed graph (On & Yazici, 2011, july), and the generated optimal path is smoothed by using an arc-line approach. A parallel PSO algorithm has been proposed to plan the feasible linear path in Huang (2014), and then the path is smoothed by using a cubic B-spline curve. In Zhou et al. (2011), the Dijkstra algorithm has been combined with the Voronoi digram to devise a piecewise linear path, whose endpoints are regarded as the control points of the smooth Bezier curve path. A linear path has been generated by using a hybrid approach combining the Dijkstra algorithm with the MAKLINK Graph, then the η 3 -splines are employed to smoothly interpolate the endpoints of the path (S. Zhang et al., 2014, december). A genetic algorithm has been implemented to achieve an optimal obstacle-avoidance path connecting the initial and final positions of the mobile robots, and then the piecewise cubic Hermite interpolating polynomials are exploited to produce a smooth optimal path (Bakdi et al., 2017).
Nevertheless, it is worth noting that the smooth path planning methodologies in the aforementioned works often include two steps, i.e. a linear path is usually firstly generated by using an intelligent search algorithm (e.g. PSO) upon some criteria and constraints such as the length and feasibility of the path, and then the linear path is smoothed via a parametric curve (e.g. Bezier curve). The two-step methodology would undoubtedly bring some performance degradations and other undesirable results, because the path smoothing is not directly linked to the path optimization criteria (Song et al., 2016). To overcome the drawbacks identified above, a few approaches have been put forward to directly produce an optimal smooth path for the mobile robots. In Chang and Liu (2009, february), an IPGA algorithm (island parallel genetic algorithm) has been combined with the η 3 -splines to plan a continuous collision-avoidance smooth path with differentiable curvature for autonomous mobile robots, but only the minimization of path length is considered for the optimization criteria. In Song et al. (2016), a new approach combining Bezier curve with genetic algorithm has been developed to design a smooth path for mobile robots, however, the control points of the Bezier curve are limited to the centre of the predefined grids rather than arbitrary positions of the whole workspace. To combat this obstacle issue, an improved PSO combined with Bezier curve segments has been investigated to devise a smooth path for mobile robots , but it is subject to the trouble of discontinuous curvature on the connecting points of the curve segments. Thus, a new scheme has been proposed to plan the smooth path of mobile robots using a continuous high-degree Bezier curve in combination with an improved PSO algorithm in Xu et al. (2017, october), yet, the computational burden is huge because of the complexity of the path optimization problem, and some constraints such as maximum curvature and maximum curvature-derivative are not simultaneously considered. Additionally, a new method to optimal curvature smoothing has been presented for the trajectory generation of flying robots in Dong et al. (2019), while it is to deal with the issue of smooth transition between straight linear segments rather than smooth path planning of mobile robots.
To handle the above-mentioned problems in the smooth path planning of mobile robots, a new approach combined with the parametric cubic Bezier curve (PCBC) and a novel PSO with adaptive delayed velocity (PSO-ADV) is developed to plan an optimal smooth path for the mobile robots with continuous-curvature constraint. The main contributions of the current paper can be outlined from the following aspects. (1) A new approach based on parametric cubic Bezier curve is presented to overcome the drawbacks of the traditional smooth paths composed of several curve segments, thereby the continuous curvature can be achieved at the joints of the curve segments.
(2) A modified PSO with adaptive delayed velocity is developed to deal with some obstacles encountered frequently in the smooth robot path optimization, e.g. premature convergence and local trapping, and the performance of the modified PSO is evaluated via some simulation experiments on several famous benchmark functions. (3) The problem of smooth path planning with continuous curvature constraint is formulated mathematically as an optimization problem, which is resolved by the new approach combined with PCBC and PSO-ADV algorithms. (4) The superiority of the new approach on the smooth path planning is confirmed on the basis of several comparisons with traditional studies.
The remaining part of the present paper is organized as follows. Section 2 firstly presents the preliminary of the parametric cubic Bezier curve in detail, and then delineates the problem statement of the smooth path planning of mobile robots. Section 3 elaborates the newly developed PSO algorithm with adaptive delayed velocity, and its superiority is also validated by the comparisons with some famous PSOs on several benchmark functions. Section 4 reports the results of the simulation experiments on smooth path planning based on the new approach combined with PCBC and PSO-ADV algorithms. In the last section, the paper is concluded and some future works are pointed out.

Preliminary on parametric cubic Bezier curve
The parametric cubic Bezier curve (PCBC) is an analytical continuous-curvature path-smoothing algorithm proposed by Yang and Sukkarieh (2010). By regulating the parameter of PCBC, the straight linear segments can be smoothly connected and the constraint of continuouscurvature can also be satisfied.
The cubic Bezier curve can be expressed as follows: where u is subject to 0 ≤ u ≤ 1; B(u) indicates the cubic Bezier curve; B i denotes the ith control point. With this definition, the Bezier transition curves illustrated in Figure 1 can be built by using two symmetric cubic Bezier curves that are expressed as follows: where B j0 (j = 1, 2) denotes the connecting point bet ween the Bezier curve and the straight line, and B j3 (j = 1, 2) denotes the connecting point of the two Bezier curves. On the straight linear segments, the so-called transition length is usually evaluated as d 1 = P 2 − B 10 and d 2 = P 2 − B 20 . If the constraints hold for d = d 1 = d 2 and β = θ/2 (see Figure 1 for more details), the Bezier transition curves can be obtained analytically (Dong et al., 2019), where the control points can be determined as and where It should be highlighted that the quality of the Bezier transition curves are fully depended on the transition length d, a control parameter of PCBC that can be linked to the optimization criteria of smooth path planning. It has proven that the continuous-curvature can be achieved at the connecting point B j3 of the two Bezier transition curves B 1 (u) and B 2 (u) via the PCBC algorithm (Yang & Sukkarieh, 2010). Besides, the connecting point B j3 is the point that has the maximum curvature, which can be computed as In view of Equation (12), the maximum curvature κ max has an inverse relation with the control parameter d. So that, if the maximum curvature κ max of the mobile robot is given, the satisfying control parameter d of PCBC should satisfy the following constraint: Therefore, the control parameter d of PCBC is a critical variable to be optimized for each Bezier transition curve in the following smooth path planning of mobile robots.

Problem statement of mobile robot smooth path planning
To compare the performance of the smooth path planning algorithms conveniently, the workspace of the mobile robot is supposed to be a two-dimensional plane like (Song et al., 2016. As shown in Figure 2, the workspace is a square plane cut into 2 n × 2 n grids, each of which is a square with 10 × 10 units. Each grid of the workspace is assigned a grid number, where the white grids represent the feasible space for the mobile robots, while the infeasible area with obstacles is marked with black grids. It is obvious that the workspace can be described more detailedly using more grids (i.e. a larger parameter n), which also means more computational cost. In this paper, the parameter n is set as 4 in accordance with the performance requirement of the smooth path planning.
It should be noted that the obstacles in the workspace have been extended according to the size of the mobile robot, which can be regarded as a unit point of the workspace regardless of their true sizes. Thus, a feasible unit point can be determined by the following defined criterion.
Definition 2.1: A unit point is feasible if and only if it is included in the feasible white grids, which should not contain any part of the extended obstacles, i.e. N B(u) ∈ N w , where N B(u) represents the number of the grid containing the unit point on the Bezier path B(u), and N w represents the set of the numbered feasible white grid, and the conversion from the coordinates of a unit point to the corresponding numbered grid can be formulated as: where x(u) and y(u) stand for the X-and Y-coordinate components of the unit point; · indicates the rounding down operation. Now, we are in the position to describe the problem to be solved in this paper. On account of the aforementioned workspace of the mobile robot, the objective of this paper is to plan a feasible and optimal smooth path between the given start and destination of the mobile robot, where the obtained smooth path should satisfy the optimization criteria and constraints as follows: (1) the smooth path devised in this paper should be an obstacleavoidance route for the movement of the mobile robot; (2) the planned smooth path should satisfy the requirement of continuous-curvature along the path, i.e. a G 2path (see Piazzi et al., 2007 for more details); (3) the maximum curvature along the smooth path should be minimized for the mobile robot with limited turning radius; and (4) the full length of the path should be minimized except for the above-mentioned criteria and constraints.
To accomplish this smooth path planning task, the segmented parametric cubic Bezier curves are employed to produce a satisfying smooth path by optimizing their control points and control parameters. Thus, the planning of smooth path in this paper is essentially an optimization problem to seek a series of control points P i (i = 1, 2, . . . , m + 1) and control parameters d i (i = 1, 2, . . . , m), which can lead to a feasible and optimal smooth path for the mobile robot. Consequently, considering some constraints of the parametric cubic Bezier curves, the objective function of the optimization problem can be formulated as follows: arg min where B ij (u; d i ) is one of 2m segmented parametric cubic Bezier curves, which are used for m transitions of straight linear segments; G 2 denotes the G 2 -path that has continuous curvature; κ max is the given constrained curvature for the smooth movement of the mobile robot; κ max,i denotes the maximum curvature on the ith transition curve; l i = P i+1 − P i is the length of the ith straight linear segment; and d i is the ith control parameter of the transition. It is apparent that the issue of smooth path planing is a rather complicated optimization problem that is apt to encounter some obstacle issues, e.g. local trapping and premature convergence. To handle these obstacle issues, a novel PSO algorithm is developed and will be discussed later.

Classical PSO and its variants
The PSO, which was developed by Kennedy and Eberhart (1995, november) to simulate the behaviours of animal swarms (e.g. the flocks of birds and fishes), is a metaheuristic intelligent optimization algorithm that has been widely and successfully utilized to search for the optimum solutions of various optimization problems. In PSO, the particle of the swarm acts as a candidate optimal solution of a specific optimization problem, which determines the dimension of the particle's velocity and position vectors. By flying around to explore the solution space of the optimization problem, an optimal solution can be achieved and the updating functions of the velocity and position vectors at the kth iteration can be expressed as follows: where V i (k) and X i (k) represent the ith particle's velocity and position vectors, respectively; P ib (k) and G b (k), respectively, indicate the best position that the ith particle has ever experienced and the best position of the global swarm till the kth iteration; w denotes the inertia weight of the velocity; c 1 and c 2 are defined as the cognitive and social parameters which can also be called the acceleration coefficients; r 1 and r 2 are the random numbers uniformly distributed on [0, 1].
By now, a large amount of improved PSO algorithms have been presented to advance the performance of the above-mentioned classical PSO, e.g. PSO-LDIW (Shi & Eberhart, 1998, may), PSO-CK (Clerc & Kennedy, 2002), PSO-TVAC (Ratnaweera et al., 2004), FIPS (Mendes et al., 2004), CLPSO (Liang et al., 2006), PS-ACO (Shuang et al., 2011), and PSO-SA (Behnamian & Fatemi Ghomi, 2010), to name but a few. These improved PSOs are usually developed based on diverse methodologies, e.g. the parameter regulation, local swarm division, and hybridation with other intelligent algorithms, etc. However, to the best of our knowledge, the idea about adaptively regulating the updating functions of the particle swarm is still scarcely considered, except for a few scattered works. For instance, the velocity updating function has been adaptively switched among several evolutionary states depending on the evolutionary factor of the particle swarm (Z. Zhan et al., 2009). An SPSO (switching PSO) has been developed in Y. Tang et al. (2011) to switch the updating function in accordance with the predicted evolutionary states determined by the calculated evolutionary factor and a Markov chain. Afterwards, an SDPSO (switching PSO with random delay) has been put forward to update the velocity function based on the evolutionary state of the particle swarm (Zeng, Wang, et al., 2016). Besides, an MDPSO (PSO with multimodel delayed velocities) has been developed in Song et al. (2017) to overcome some obstacle issues, such as the local trapping and the premature convergence. Nevertheless, the aforementioned improved PSOs are usually subject to several rather severe requirements, e.g. the powerful computational capacity and the large memory storage, etc.

PSO-ADV algorithm and its performance evaluation
In this paper, a novel PSO with adaptive delayed velocity (denoted by PSO-ADV) is proposed to overcome the obstacle issues identified above and promote the searching performance of the above-mentioned improved PSO algorithms, and hence effectively tackle the smooth path planning problem of mobile robots. The updating functions of the proposed novel PSO algorithm can be formulated as follows: where c 1 and c 2 are computed by where k max denotes the maximum iteration; c 1i (c 2i ) and c 1f (c 2f ) are, respectively, the initial and final values of the acceleration coefficient c 1 (c 2 ); w 1 , which is inspired by the FOPSO algorithm (Pires et al., 2010), stands for the inertia weight of the additional delayed velocity V i (k − 1), and can be computed depending on the inertia weight w of the current velocity V i (k) by where w is linearly and adaptively regulated depending on the evolutionary state of the swarm in this paper, and can be computed by where E f (k) indicates the evolutionary factor to reflect the evolutionary state of the swarm, and can be defined by where D g b indicates the average Euclidean distance from the global best particle to the other particles of the swarm; D max (k) and D min (k) indicate the maximum and minimum average Euclidean distances between one particle and the others in the swarm; the average Euclidean distance of the ith particle at the kth iteration (denoted by D i (k)) can be computed by where D denotes the dimension of the particle; and S denotes the size of the particle swarm.

Remark 3.1:
In the PSO-ADV algorithm, the velocity of the next iteration is updated according to not only the velocity of the current iteration but also the velocity of the previous iteration, i.e. the delayed velocity. This scheme can bring some 'useful' disturbances to the convergence process of the algorithm, and hence make the particle more likely to fly out of the local optima of the searching space. Besides, the inertia weights of V i (k) and V i (k − 1) are adaptively regulated depending on the evolutionary state of the swarm, which makes it possible for the particle to exploit and explore the whole searching space more thoroughly so as to obtain an optimum of the global space.
In order to appraise the performance of the PSO-ADV algorithm, several simulation experiments have been implemented in this paper upon a few standard benchmark functions, which can be expressed by , and all of them are frequently used evaluation functions that are fairly difficult to attain the optimum solutions. TABLE 1 shows the configuration information of the benchmark functions, including the name of each function, dimension of each particle swarm, searching range of each dimension, theoretical optimal solution of each benchmark function, and threshold of the successful optimum solution.
100(x i − 10) 4 , x i > 10, To verify the superiority of the novel PSO-ADV algori thm, its performance is compared with some well-known   standard or modified PSOs, including PSO-LDIW (Shi & Eberhart, 1998, may), PSO-TVAC (Ratnaweera et al., 2004), SDPSO (Zeng, Wang, et al., 2016), FOPSO (Pires et al., 2010), and MDPSO . For all simulation experiments, their parameters are taken as follows:    As shown in Figures 3-8, the logarithmic average fitness values of each iteration are clearly demonstrated with diverse line and mark styles for all benchmark functions. Besides, the statistical results of the optimal fitness values, including the minimum (MIN), average value (AVG), standard deviation (SD), and success rate (SR) of the results, have also been illustrated in TABLE 2. Obviously, it can be concluded from the performance comparisons that PSO-ADV is superior to other standard or modified PSOs. For the Sphere function, which is a typical unimodal function to test the convergence ability of an intelligent optimization algorithm, PSO-ADV can outperform all other PSOs on the obtained optimum. For the Rosenbrock function, which is often regarded as a multimodal benchmark function fairly difficult to obtain the optimal solution because of its famous banana-valley, PSO-ADV is superior to others on two aspects of the convergence ability, i.e. obtainable optimum and convergence rate. For the Rastrigin function, which is a typical complex multimodal benchmark function hard to converge, the performance of PSO-ADV can further verify its superiority in comparison with other algorithms. Moreover, the superior performance of PSO-ADV can also be confirmed by the optimization results of the Schwefel 2.22, Schwefel 1.2 and Schwefel 2.21 functions, all of which are severe benchmark functions very hard to achieve the optimum. Moreover, it is worth noting that the PSO-ADV can attain 100% convergence success rate and better optimization result than others for all benchmark functions. This is benefited by the introduction of the adaptive delayed velocity, which can more likely make the particles jump out of the local trapping and robustly converge to the optimum, rather than some other PSOs that can trap into several local minima or diverge to the boundary of the searching space sometimes.

Simulation experiments
In this section, several simulation experiments are carried out to affirm the effectiveness and superiority of the presented smooth path planning approach, which is combined with the PCBC and the PSO-ADV algorithms. For the sake of contrast, the path planning algorithm is implemented in the workspace sketched in Figure 2, and the configuration of the experiments are as follows: N = 7 for the amount of control points; S = 50 for the size of particle swarm; and T = 100 for the maximum iteration of each simulation experiment.
Figures 9 and 10 demonstrate two of the representative simulation results, whose starts and destinations are respectively set as Grid 15 (respectively, Grid 0) to Grid 240 (respectively, Grid 255), and their X-Y-coordinate values are specified as (155, 5) for Grid 15, (5, 5) for Grid 0, (5, 155) for Grid 240,and (155,155) for Grid 240. In these figures, the control points of the Bezier curves are indicated by the blue hollow circles; the corresponding convex hulls are depicted by the blue solid lines; and the red solid curves represent the obtained optimal smooth path.
To compare the smooth paths produced via different approaches, Figures 11 and 12 illustrate the counterpart results generated by using square Bezier curve (SBC) segments and MDPSO algorithm. Essentially, the smooth paths in Figure 9 (respectively, Figure 11) and Figure 10 (respectively, Figure 12) have different properties, though they look like each other. For example, both of the smooth paths in Figures 9 and 10 are composed of many cubic Bezier transition curve segments, which have  equal curvatures at the segment joints. Thus, the G 2 -path can be acquired along the whole Bezier curve paths. In contrast, the smooth paths in Figures 11 and 12 consist of square Bezier curve segments, which can not satisfy the curvature continuity at the joints of the curve segments. Undoubtedly, this will cause discontinuities of the velocity and acceleration of the mobile robot, and can result in over actuation and slippage in the case of fast moving. The aforementioned properties can also be confirmed by the curvature curves of these smooth paths described in Figures 13 and 14, where the curvatures of the smooth paths are computed for every segments and the curvature of each segment is sampled on 100 uniformly distributed path points. It is apparent that there are several 'steps' on the curvature curves of the smooth paths devised by using SBC and MDPSO algorithms (see e.g. the points 200, 400, 600, and 800), which can not satisfy the requirement of continuous movement of the mobile robot and will lead to inevitable frequent switches on the motion states of the mobile robot. While, the continuous curvature of the smooth paths (which have the same curvature '0' at the segment joints) presented in this paper can ensure the movement continuity of the mobile  robot, and therefore make the trajectory tracking of the mobile robot more easier.
Remark 4.1: It should be highlighted that it is really a challenging issue to plan the smooth path in the cases of this paper. The difficulty and complexity come from at least the following two aspects. Firstly, there is only a narrow valley surrounded by obstacles for the optimizer to find a feasible smooth path for the mobile robot, which is a global optimization problem that it prone to trap into the local optima, i.e. infeasible smooth paths. Secondly, several criteria and constraints are linked to the smooth path optimization, which is actually a multiobjective optimization problem that is difficult to obtain the global optimum. It is fortunate that the challenging  task can be fully accomplished via the approach presented in this paper, which can acquire superior performance comparing with the results of our previous papers.

Conclusions
In this paper, a new approach combined with PCBC and PSO-ADV algorithms is developed to plan the smooth path of mobile robots. The smooth path composed of PCBC segments can achieve equivalent curvature at the joints of the segments, and thus it is able to attain continuous curvature along the whole smooth path. Based on the mathematical formulation of the smooth path planning problem, the optimal smooth path can be obtained by regulating the control points and control parameters of PCBC, which is essentially an intractable optimization problem. To tackle this optimization problem and some frequently encountered troubles (e.g. premature convergence and local trapping), a novel PSO-ADV algorithm is developed by blending an adaptive delayed velocity term that can give the particles more 'power' to jump out of the local minima, and the validity of PSO-ADV can be affirmed on account of some simulation experiments on several famous benchmark functions. The new approach is finally applied to the smooth path planning of mobile robots, and its effectiveness and superiority can be verified through several simulation experiments.
In the future work, we will pay more attentions to some interesting topics including: (1) the convergence analysis of the PSO-ADV algorithm; (2) the new schemes that can be employed to boost the performance of PSO; (3) the application of the new smooth path planning approach to some more complicated cases, e.g. 3-D path planning (Sun et al., 2018), multi-robot path planning (Nazarahari et al., 2019), and path planning in severe environment (Berglund et al., 2010), etc.

Disclosure statement
No potential conflict of interest was reported by the author(s).