Keywords

1 Introduction

Human drivers display remarkable abilities when controlling vehicles in a highly reactive manner and with impeccable precision. Even more impressive is the implicit consideration of the vehicle and road parameters. Researchers have shown that humans use specific visual cues to identify road curvature and endeavor to match it. Analysis that drew inspiration from the steering commands, used by operators of varying driving experience, is attempted to generate likewise natural paths. This approach can be combined with a path planning algorithm to generate paths with natural smooth trajectories for autonomous vehicles. This will circumvent the need to rely on computationally intensive planning algorithms that are based on forward model integration. Humans have been controlling the steering wheel for the past century. Currently self-driving is emerging as technology that promises to improve our lives greatly. Cars are underactuated systems designed to facilitate their control for operators. This simplification of actuation has adverse effects when attempting to automate such systems, leading to the appearance of nonholonomic constraints (Jazar 2008). Researchers have shown that drivers rely on certain visual landmarks to assess the path curvature prior to attempting to steer toward it (Land and Lee 1994; Land and Horwood 1995). The majority of studies, conducted on human steering, focus on modeling and predicating it, using control system theories (Donges 1978; MacAdam 1981; Prokop 2001). The aim of this chapter is to employ an efficient spline parameterization method to synthesize paths that mimic human steering controls, in order to generate motions that feel natural and familiar to human passengers. It is not uncommon for robotics researchers to draw inspiration by observing natural behavior (Donghyun et al. 2014; Lentink 2014). We hope that the results, presented in this chapter, can be employed in robotics to efficiently plan spline-based paths, similar to Yang et al. (2014), in addition to mimicking human steering, improving passenger comfort, and explicitly considering the limitations of the car.

Advances in sensing technology, computer vision, communications, and computational power have contributed toward the development of autonomous agents in a wide range of fields. Self-driving ground vehicles are used in military, urban transportation, and industrial and agricultural applications. Unmanned aerial vehicles (UAVs ) and micro aerial vehicles (MAVs) are considered as a cost-effective, safe, and efficient choice for several military and civil applications. Robotic platforms are currently equipped with multiple sensors, which enable them to sense their surroundings and localize themselves in reference to their environment, goals, and obstacles. Path planning is a widely studied, fundamental task for mobile robots. Robot navigation mandates a strategy that steers it from its current location, through the environment while avoiding obstacles toward its goal.

Classical planning algorithms , such as A* algorithm (Hart et al. 1968), Voronoi diagrams (Canny 1985), visibility graphs (Asano et al. 1985), and cell decompositions (Brooks and Lozano-Perez 1985) produce piecewise linear paths. These paths consist of subsequent waypoints joined by straight lines. Potential field methods guide the robot toward its goal by applying attractive forces, toward the goal, and repulsive, away from obstacles (Khatib 1986). Potential field methods tend to produce oscillating paths in narrow passages (Koren and Borenstein 1991). Sampling-based motion planning algorithms, such as rapidly exploring random trees (RRT) (LaValle 2000) and probabilistic roadmap method (PRM) (Kavraki et al. 1996), rely on stochastic sampling to efficiently explore the search space. Resulting paths from randomization are suboptimal and require post-processing to improve their quality (Elbanhawi and Simic 2014c). Motion planning using state lattices is disadvantaged by discretization (Pivtoraiko et al. 2009; Pivtoraiko and Kelly 2011). Coarse discretization leads to loss of completeness, while high-fidelity subdivision increases the computational time of the planner, especially in highly dimensional scenarios. Homotopy class optimization of trajectories is proposed (Zucker et al. 2013). These methods do not discuss curvature continuity, and the performance is dependent on the optimization algorithm. Optimization methods are not immune from local minima and are not guaranteed to converge.

Agile robots , such as omnidirectional, differential-drive robots and quadrotors, are capable of traversing piecewise linear paths. Such paths require stationary turns, at every waypoint, to change heading toward the subsequent waypoint. This approach is inefficient with regard to time, energy, and jerk considerations. The motion of some robots, such as car-like vehicles and fixed-wing UAVs, is highly constrained. Nonholonomic robots must be considered in the planning procedure, as they cannot follow piecewise linear paths. Minimum turning radius constraints impose further limitations on the path, which is often represented by maximum curvature restrictions.

Traditionally, Dubins paths or Reed’s and Shepp’s (Xuan-Nam et al. 1994; Reeds and Shepp 1990) are used in path smoothing for vehicles with minimum turning radius constraint in a two-dimensional space. Configurations are joined by sets of primitives consisting of circular arcs and straight lines. The amalgamation of circular arcs and lines results in discontinuities in curvature. Clothoids may appear to be suitable for path smoothing, as they are characterized by continuous curvature (Fraichard and Scheuer 2004). However, clothoid generation is challenging, as they have no closed-form expression. High-order splines (11th order) and polynomials (26th order) have been proposed for clothoid approximation (Wang et al. 2001; Meek and Walton 2004; Walton and Meek 2005; McCrae and Singh 2009; Montes et al. 2008). Recent work has enabled the real-time approximation of clothoids under bounded length and orientation limitations (Brezak and Petrovic 2013). Consequently, they are still not suited for real-time replanning and highly dimensional scenarios.

Curvature discontinuities result in overactuation, slipping, localization errors (Magid et al. 2006), passenger discomfort (Gulati and Kuipers 2008), mechanical wear and failure (Berglund et al. 2010; Maekawa et al. 2010), and control instability (Lau et al. 2009; Roth and Batavia 2002). Subsequently, achieving continuous curvature is advantageous in applications that involve carrying sensitive cargos such as human passengers (Gulati and Kuipers 2008) or heavy loads in mining applications (Berglund et al. 2010; Maekawa et al. 2010) and those which require precise localization such as agricultural applications to minimize the impact of the vehicle on crops (McPhee and Aird 2013; Sabelhaus et al. 2013; Alshaer et al. 2013) or energy loss minimization for MAVs with battery-size restrictions (Myung et al. 2007).

In our earlier work, we proposed an evaluating and bounding B-spline paths approximate solution. We show that humans control vehicles with continuous commands and generate paths that obey the vehicles kinematic constraints. We propose the premise of using a single B-spline curve to generate paths that resemble human driving and obey the vehicle’s constraints. This is achieved by defining the curvature of a B-spline segment in terms of the parameters of its corresponding control polygon, which in this case is assumed to be a linear path generated by a path planning algorithm.

In this work we improve B-spline-based motion planning by proposing efficient methods for segment curvature evaluation and analytical bounding. The characteristics of B-splines are exploited to present two solutions for continuous curvature bounding, which can be combined together or used separately. The novelty of our proposal is that it is not limited to a plane or a dimension; it is not subject to orientation, length, or control polygon restrictions. It guarantees continuity throughout the path while preserving real-time performance. We also show that it is possible to plan the trajectory of a robot with nonholonomic constraints and maintain parametric continuity.

This chapter is organized as follows: Sect. 9.2 lists the current related research in path smoothing. The problem is formally described in Sect. 9.3. B-spline curve synthesis is introduced in Sect. 9.4. We address curvature continuity, segment curvature formulation, and curvature bounding in 2D in Sect. 9.5. Our findings are validated and compared with previous work using simulation experiments, as given in Sect. 9.6. The chapter is concluded in Sect. 9.7.

2 Related Work

There are two separate problems addressed in this chapter: firstly, planning a geometric curve with curvature bounds given in Sect. 9.4 and, secondly, maintaining parametric continuity of the generated trajectory. The authors could not ascertain any literature that combined these two problems. There are only approaches that address each issue separately. The benefits of synthesizing kinodynamically feasible and continuous paths are well studied in robotics (McPhee and Aird 2013; Magid et al. 2006; Gulati et al. 2009; Gulati and Kuipers 2008; Maekawa et al. 2010; Lau et al. 2009; Myung et al. 2007; Sabelhaus et al. 2013; Alshaer et al. 2013). However, current solutions given in the literature fail to guarantee C2 continuity with curvature bounds for nonholonomic mobile robots.

Similarly, in trajectory generation literature, there are multiple solutions to optimally generate bounded trajectories for given geometric curves with regard to time (Balkcom and Mason 2002; Wu et al. 2000) and jerk considerations (Guarino Lo Bianco 2013). Other approaches considered curvature and acceleration bounds as parameters in optimization problems (Johnson and Hauser 2012; Kunz and Stilman 2013; Sachin et al. 2014). However these methods, adversely, provided no discussion on the parametric continuity problem and often led to C1 continuity only. The approach developed by Velenis and Tsiotras (2008) for vehicles is limited to velocity continuity and acceleration bounds and ignored acceleration continuity which will undoubtedly lead to a jerky and uncomfortable ride (Gulati and Kuipers 2008; Guarino Lo Bianco 2013). Control laws proposed for unicycle robots ignored acceleration and curvature bounds (Lapierre et al. 2007; Sgorbissa and Zaccaria 2010; Morro et al. 2011).

Dubins paths and circular arcs were commonly used for robot planning despite their curvature discontinuity. Dubins paths were generated under the assumption that the vehicle maintains constant linear velocity. Multiple circular segments have been proposed for UAV path smoothing (Anderson et al. 2005). This approach was limited to planar scenarios and produces discontinuous paths. Bézier curves were commonly used for path smoothing. The order of Bézier is dependent on the number of control points, which resulted in limiting them to maintain a low-order curve (Jolly et al. 2009; Lau et al. 2009; Kwangjin and Sukkarieh 2010). A comparative navigational-based analysis showed that Bézier curves poorly interpolate a linear path as opposed to B-splines, (Elbanhawi et al. 2014). Limiting the number of control points of Bézier curves results in the need to join multiple curve segments. Discontinuities arise at the joint of two Bézier segments .

A condition for Bézier curve geometric , G 2, continuity was presented in (Walton et al. 2003). However, this condition had no closed-form solution. Kwangjin et al. (Kwangjin and Sukkarieh 2010; Kwangjin et al. 2013) provided a solution for a particular case of the G 2 planar condition. This approach was used for upper-bounded curvature smoothing algorithms. It is still limited to a plane and, in fact, incapable of considering different curvature bounds in horizontal and vertical planes. Fixed-wing UAVs have different turning angle (horizontal plane) and climbing angle (vertical plane) limitations. Barsky and Derose (1990) proposed geometric continuity, Gk, as condition for ensuring that curve endpoints had the same directions, not the values. The work in (Kwangjin and Sukkarieh 2010; Kwangjin et al. 2013) fails to guarantee velocity and acceleration continuity, which are more realistic for robotics than geometric continuity. Pan et al. (2012) have shown that only C2 parametric continuities of acceleration and velocity are suitable for real robots and provided a shortcutting algorithm that guarantees continuity in most scenarios but fails to address the maximum curvature constraint. Recent studies investigated trajectory planning for trailer cars with continuous velocities (Ghilardelli et al. 2014).

The advantages of B-splines for real-time planning have been shown (Dyllong and Visioli 2003; Elbanhawi and Simic 2012). A genetic algorithm was employed to select the location of a fixed number of control points, for a single B-spline curve (Nikolos et al. 2003). This guaranteed the continuity of the curve. However, having a constant number of control points reduced the robustness of the generated path. B-splines were used for generating smooth paths for passenger transporting robots (Gulati and Kuipers 2008). That approach is limited to a 2D setting and robots with no curvature bounds. Similarly, a 3D B-spline smoothing algorithm was presented that did not consider curvature continuity or upper bounds (Koyuncu and Inalhan 2008). Several optimization algorithms are limited to 2D offline B-spline smoothing and curvature bounding (Berglund et al. 2010; Maekawa et al. 2010). A B-spline shortcutting algorithm was proposed, which used multiple segments; however, it did not guarantee continuity in all segments and did not consider the maximum curvature (Pan et al. 2012). In our earlier work, we provided an approximate B-spline-based approach to the presented problem and did not consider acceleration and velocity bounds (Elbanhawi et al. 2014). Research findings were implemented, in real time, on experimental vehicle (Elbanhawi and Simic 2014b). On the other hand, in here we present solution of the problem analytically and consider kinodynamic constraints.

3 Problem Statement

We consider front wheel-steered vehicles , referred to as car-like robots. It is common to model the vehicle using the bicycle model, which assumes identical steering angles on both sides (Jazar 2008), as shown in Fig. 9.1. This model has been shown to be adequate for modeling the global kinematic motion of front wheel-steered vehicles (Campion et al. 1996). The advantage of our approach is in solving two closely related problems simultaneously, which are generally decoupled in robotic literature. We aim to synthesize a curve that satisfies conditions given by Eqs. (9.1, 9.2, 9.3, and 9.4) and maintain parametric continuity. The vehicle’s Cartesian coordinates (x, y) and heading angle θ are measured from the center of the rear axle relative to a global frame. The length between the front and back wheel is referred to as wheel base, W. The two actuation commands are linear velocity, v, and steering angle Φ. It is clear that the vehicle is underactuated as it has two controls and three degrees of freedom, i.e., it is not fully controllable (Ogata 2010). The velocity components in the x and y directions, v x and v y , are constrained as given in Eq. 9.1. This nonholonomic condition is often referred to as the rolling without slipping constraint.

$$ {v}_x\sin \left(\theta \right)-{v}_y\cos \left(\theta \right)=0 $$
(9.1)
Fig. 9.1
figure 1

Joining two path segments

Full size image

Consider a planning algorithm that produces a path consisting of, n−1, straight lines joining successive, n, waypoints, P = [P 1,  P 2…P n ], where P i  = (P xi , P yi , P zi ) for i = [1, 2, ..,n−1, n]. It is required to generate a curve, c(u), which closely follows straight-line path, where u is the normalized path length parameter. It is an independent variable in the range of [0,1] for any curve, c(u). Parameter u takes the value u = 0 at the beginning of the segment and reached the value u = 1 at the end. The generated curve must satisfy the following imposed constraints.

Path continuity at the endpoints of two curve segments must be addressed; such a situation is illustrated in Fig. 9.1. For two consecutive curve segments c j (u) and c j + 1 (u), Ck parametric continuity could be then defined as shown in Eq. 9.2, according to Farin (2002), where k is a positive integer denoting the order of the parametric continuity.

$$ \frac{\partial^i{c}_{j+1}(0)}{\partial {u}^i}=\frac{\partial^i{c}_j(1)}{\partial {u}^i},\operatorname{}\forall i=1,2,3..k $$
(9.2)

The curvature of the path must not exceed the maximum curvature, K max , at any point. The curvature, k(u), along a path is defined as Eq. 9.3, where c(u) = [x(u), y(u)] and the first- and second-order derivatives with respect to u are c’(u) = [x’(u), y’(u)] and c”(u) = [x”(u), y”(u)].

$$ k(u)=\frac{x^{\prime }(u){y}^{{\prime\prime} }(u)-{x}^{{\prime\prime} }(u){y}^{\prime }(u)}{{\left({x}^{\prime }{(u)}^2+{y}^{\prime }{(u)}^2\right)}^{3/2}} $$
(9.3)

The minimum radius of curvature, r min , in a plane restricts the curvature of the path to K max in that plane. For car-like robots, the curvature constraint is a result of the maximum steering Φ max angle due to the mechanical construction of the vehicle, as shown in Eq. 9.4. In three-dimensional scenarios for aerial vehicles, the maximum yaw and pitch angles in the horizontal and vertical planes must be considered separately (Fig. 9.2).

$$ {k}_{\mathrm{max}}=\frac{1}{r_{\mathrm{min}}}=\frac{\tan \left({\phi}_{\mathrm{max}}\right)}{W} $$
(9.4)
Fig. 9.2
figure 2

Bicycle model for front wheel-steered vehicles

Full size image

4 Spline Primitives

B-splines are vector-valued parametric curves, initially proposed by Schoenberg (1946). B-splines and nonuniform rational B-splines (NURBs) are commonly used for computer-aided design (CAD) applications as a result of their efficient synthesis and robustness (Farin 1992). In addition to CAD, they have been utilized in reverse engineering (Ma and Kruth 1998; Piegl and Tiller 2001), finite element analysis (Hughes et al. 2008), machining (Cheng et al. 2002; Sungchul and Taehoon 2003), medical imaging (Zhang et al. 2007), computer vision (Biswas and Lovell 2008), bio-inspired data fitting (Jones and Adamatzky 2014), and signal processing (Unser et al. 1993). As discussed in earlier sections, their use in robotics is fairly recent.

A p-th degree B-spline curve, c(u), is defined by n control points and a knot vector \( \widehat{u} \), evaluated by Eq. 9.5. The length of the one-dimensional knot vector, m, is equal to n + p + 1. Normalized path length parameter, u, is simply referred to as the path parameter (Farin 2002).

$$ c(u)=\sum \limits_{i=0}^n{N}_{i,p}(u){P}_i $$
(9.5)

P i is the i-th control point, which is in turn influenced by a corresponding basis functions. The number of basis functions therefore mirrors the number of control points, n. N n,i (u) is the i-th B-spline basis function, which is defined using the Cox-de Boor recursive algorithm (De Boor 1972). First-order basis functions are evaluated using Eq. 9.6 based on the predefined knot vector. Higher-order functions are computed by the recursive substitution in Eq. 9.7.

$$ {N}_{i,0}(u)=\left\{\begin{array}{ll}1& u\in \left[{\widehat{u}}_i,{\widehat{u}}_{i+1}\right)\\ {}0& \mathrm{else}\end{array}\right. $$
(9.6)
$$ {N}_{i,p}(u)=\frac{u-{\widehat{u}}_i}{{\widehat{u}}_{i+p}-{\widehat{u}}_i}{N}_{i,p-1}(u)+\frac{{\widehat{u}}_{i+p+1}-u}{{\widehat{u}}_{i+p+1}-{\widehat{u}}_{i+1}}{N}_{i+1,p-1}(u) $$
(9.7)

We have previously shown B-spline properties that render them as superior to other parametric curves, for the task of robot navigation (Elbanhawi et al. 2014). The curve’s degree, p, is independent of the number of control points, n. This allows the possibility of using a single curve for the entire path smoothing without imposing limitations on the number of control points. It is in contrast to Bézier curve methods (Jolly et al. 2009; Kwangjin and Sukkarieh 2010; Lau et al. 2009; Kwangjin et al. 2013) where the number of control points is predefined. Modification of control points affects the curve shape locally and does not change the rest of the path. This enables the local control of the path for smoothing or obstacle avoidance purposes. A clamped B-spline curve follows its control polygon more closely in comparison to a Bézier curve of the same order. Clamping is achieved by having (p + 1) multiplicity of the initial and final knots, \( \widehat{u} \) (Farin 2002). Knot multiplicity ensures that the curve passes through the initial and final control points.

Despite the beneficial properties that characterize B-splines, maintaining path continuity and controlling its curvature are nontrivial issues. They continue to challenge the use of B-splines in robotic path planning applications (Lau et al. 2009; Elbanhawi and Simic 2014a; Pan et al. 2012). It must be noted that both Beziers and B-spline are essentially combinations of polynomials. In principal, there should exist control laws, or conditions, that are capable of generating parametrically continuous trajectories using Bezier curves as well. The authors could not identify such methods in literature. Consequently, we have utilized the existing benefits of B-splines for motion planning.

5 Curvature Bounding

5.1 Parametric Continuity

The challenge of path continuity stemmed from the linking of two separate path segments. Primitives such as circular arcs, polynomials, and clothoids were not flexible enough to represent a path using a single segment. The number of control points, which were usually predefined prior to smoothing, governs the order of a Bézier curve. Consequently, multiple Bézier curves must be linked for smoothing a single piecewise linear path.

The order of a B-spline curve is independent of the number of control points in the path, as already mentioned. In theory, it is possible to smooth a path using a single curve of a predefined order. The single B-spline curve approach was adopted for UAV planning; however, the number of control points was fixed (Nikolos et al. 2003). The region in which planning is conducted and path shape robustness are significantly limited by fixing the number of control points. The work by Jolly et al. (2009) is based on rapid replanning with a short planning horizon and relies on four control points. We did not pose any restrictions on the number of control points, apart from that the number of control points must exceed the degree of the curve, p. The local control property of B-spline enables the modification of a curve segment without changing the entire path. The necessity for rerouting commonly results from obstacle detection or smoothing purposes.

Despite the superiority of B-splines over a Bézier curve of the same order, in closely following the shape of a path, they still deviate from the original path (control polygon). Ideally, the curve would follow the original linear path and smoothly cut corners when turning is needed. It is desired to maintain proximity to the originally planned straight-line path as it is more likely to be collision-free. This was achieved by forcing the tangency of the curve to the sides of the control polygon. B-spline tangency to collinear control points is leveraged to ensure the close following of the original path. Systematic midpoint insertion, between every two successive points, effectively transformed control polygon edges into lines connecting three control points, thus forcing the curve’s tangency to the edges. The effect of midpoint insertion is illustrated in Fig. 9.3. It is worth highlighting that in both cases, a single curve segment was used for smoothing. That guarantees continuity along the path. This avoids the need to address parametric continuity at union points, as illustrated in Fig. 9.1. The curvature and higher-order derivatives do not exhibit any abrupt changes after adding midpoints. This can be validated by comparing the resulting trajectories given in Fig. 9.4.

Fig. 9.3
figure 3

Midpoint insertion improves the path proximity of B-splines without compromising parametric continuity. It forces the curve (blue) tangency to the edge of the control polygon (black) unlike the unmodified B-spline curve (red)

Full size image
Fig. 9.4
figure 4

Parametric continuity was maintained before (left) and after (right) midpoint insertion as a result of using a single B-spline segment

Full size image

5.2 Curvature Evaluation

5.2.1 Path Segmentation

Our aim was to control the curvature, k, of a B-spline curve. Specifically, it was required to maintain the curvature below the maximum curvature bound, K max. Systematic midpoint insertion allowed for the definition of a repeated segment throughout the path (see for Fig. 9.5(a) illustration). The segment consists of two intersecting control edges and a total of five control points (including two midpoints). It was required to define B-spline paths curvature in terms of their corresponding segment parameters. This enabled the isolation of each segment and local modifications of its parameters by leveraging the local support property of B-splines. Smoothing modifications will be proposed to ensure maximum curvature bounds are obeyed.

Fig. 9.5
figure 5

(a) The notion of a reoccurring control segment through the path. A segment consists of two intersecting straight lines and five control points. (b) The parameters of a single segment

Full size image

The parameters of the reoccurring control segment are the side length, L, the angle between segment sides, α, and the length ratio of both sides, r, as illustrated in Fig. 9.5(b). In our earlier work (Elbanhawi et al. 2014), segments of equal sides were assumed, r = 1, which overestimated the curvature of the path and resulted in attaining approximate solutions. The use of the length ratio parameter, r, is presented to enable a more precise evaluation of the curvature. Position vectors describing the five control points of the segment can be defined with respect to the parameters of the same segment and are given in Eq. 9.8.

$$ P\left(\begin{array}{l}{P}_x\\ {}{P}_y\end{array}\right)=\left(\begin{array}{l}L,\frac{L}{2},0,r\frac{L}{2}\cos \left(\alpha \right), rL\cos \left(\alpha \right)\\ {}0,\kern0.36em 0,\kern0.36em 0,r\frac{L}{2}\sin \left(\alpha \right), rL\sin \left(\alpha \right)\end{array}\right) $$
(9.8)

The cubic B-spline curve, p = 3, has five control points, n = 5, and, m = 9, knots with four initial and final multiplicity for clamping, \( \widehat{u} \) = [0,0,0,0,0.5,1,1,1,1]. Initial order basis functions were evaluated using Eq. 9.6. Following that, basis functions, N(u), were computed, using the Cox-de Boor algorithm by recursive evaluation of Eq. 9.7. The following third-order basis functions can be defined as given in the set of Eq. 9.9.

$$ {N}_{0,3}=\frac{{\left(1-2u\right)}^3}{2} $$
(9.9a)
$$ {N}_{1,3}=6{u}^3-6{u}^2+1 $$
(9.9b)
$$ {N}_{2,3}=-6{u}^2+6u-1 $$
(9.9c)
$$ {N}_{3,3}=-6{u}^3+12{u}^2-6u+1 $$
(9.9d)
$$ {N}_{4,3}=\frac{{\left(2u-1\right)}^3}{2} $$
(9.9e)

In order to define the curvature of a segment in terms of its parameters, k = f(r, L, α)¸ the position vectors of the segment, Eq. 9.8, and basis functions, Eq. 9.9, were substituted in the curve Eq. 9.5. The curve was defined as a function of its corresponding segment parameters, \( c(u)=\left[\begin{array}{c}x(u)\\ {}y(u)\end{array}\right]=\left[\begin{array}{c}x\left(r,L,\propto \right)\\ {}y\left(r,L,\propto \right)\end{array}\right] \); x(u) and y(u) are given in Eq. 9.10.

$$ {\displaystyle \begin{array}{ll}x(u)=& {\frac{{\left(1-2u\right)}^3}{2}}^{\ast }L+{\left(6{u}^3-6{u}^2+1\right)}^{\ast}\frac{L}{2}\\ {}& +{\left(-6{u}^3+12{u}^2-6u+1\right)}^{\ast}\frac{rL\cos \left(\alpha \right)}{2}{\frac{{\left(2u-1\right)}^3}{2}}^{\ast } rL\cos \left(\alpha \right)\end{array}} $$
(9.10a)
$$ y(u)={\left(-6{u}^3+12{u}^2-6u+1\right)}^{\ast}\frac{rL\sin \left(\alpha \right)}{2}+{\frac{{\left(2u-1\right)}^3}{2}}^{\ast } rL\sin \left(\alpha \right) $$
(9.10b)

For a given segment, its parameters, r, L, and α, are constant and known prior to a curvature query. The first- and second-order derivatives with respect to the path parameter, u, are derived below from equations set (Eq. 9.11).

$$ {x}^{\prime }(u)=3L\left({u}^2\left(r\cos \left(\alpha \right)-1\right)+2u+1\right) $$
(9.11a)
$$ {x}^{{\prime\prime} }(u)=6L\left(u\left(r\cos \left(\alpha \right)-1\right)+1\right) $$
(9.11b)
$$ {y}^{\prime }(u)=3 Lr\sin \left(\alpha \right){u}^2 $$
(9.11c)
$$ {y}^{{\prime\prime} }(u)=6 Lr\sin \left(\alpha \right)u $$
(9.11d)

The curvature expression, k = f(r, L, α), in Eq. 9.12 was obtained by substituting the curve and its first- and second-order derivatives from Eq. 9.11 into Eq. 9.3. It can be noted that when substituting by r = 1, in Eq. 9.12 we get the same expression derived in Elbanhawi et al. (2014). Prior to introducing the parameter, r, curvature evaluations were approximate, and the accuracy of the maneuvers could not be ascertained.

$$ k(u)=\frac{2 ru\left(u-1\right)\sin \left(\alpha \right)}{3L{\left({u}^4\left({r}^2-2r\cos \left(\alpha \right)+1\right)+4{u}^3\left(r\cos \left(\alpha \right)-1\right)-2{u}^2\left(r\cos \left(\alpha \right)-3-4u+1\right)\right)}^{3/2}} $$
(9.12)

5.2.2 Segment Curvature Evaluation

Midpoint insertion ensured the curve’s tangency to the polygon edges, which resembled paths generated by human operators in the experiments conducted by Elbanhawi et al. (2014). Subsequently, the curvature of the path started at u = 0 and finished at u = 1, with k = 0. Curvature peaked to k peak at some point, u peak, in between, u = [0,1]. In order to limit path curvature to the maximum value of K max, the peak curvature, k peak, of the segments must be evaluated first. The point, u peak, along the parametric path length, u, where the curvature peaks, was found by solving Eq. 9.13. Then k peak was computed by substituting u peak in Eq. 9.12.

$$ \frac{dk(u)}{du}=0 $$
(9.13)

For every path segment, there exists a singular curvature peak, as shown in Fig. 9.6. The red profiles show the influence of changing the segment angle while maintaining fixed length and ratio. The location of the peak curvature was entirely dependent on the length and ratio. For a large angle (blue) and fixed length, the ratio changed both the position and value of the peak curvature. Similarly, for a much smaller segment angle (gray), the length ratio was still influential on both the peak value and position.

Fig. 9.6
figure 6

Changing segment parameters shifts the position of the curvature peaks. In all cases, curvature profile is continuous with a singular peak

Full size image

Solving Eq. 9.13 for u peak can prove to be a computationally intensive task, particularly when k peak had to be evaluated multiple times during each query of the path planning procedure. One useful observation is that the location of u peak is dependent on the segment angle, α, and length ratio, r, as highlighted in Fig. 9.7. We note that, while u peak is dependent on r and α only, the peak curvature value, k peak, is still dependent on r, α, and L. It was possible to store u peak values in a lookup table of equal intervals from r = 1 to 10 and α = 0 to π. The required values can be interpolated. To maintain a sparse lookup table, we use the property in Eq. 9.14, which can be observed from Fig. 9.7. In our case, retaining a lookup table (less than 10kB in size) produced curvature values of 10−3 accuracy.

$$ \mathrm{If}\;0<r<1,\mathrm{then}\;{u}_{\mathrm{peak}\left(r,\alpha \right)}=1-{u}_{\mathrm{peak}\left(1/r,\alpha \right)} $$
(9.14)
Fig. 9.7
figure 7

Parametric length location, u peak, of the peak curvature, k peak, is dependent on the segment angle, α, and the length ratio, r. It can be noted that when length ratio is 0 < r < 1, u peak > 0.5 and when r > 1, u peak < 0.5. This results from the observation that u peak is shifted toward the shorter segment edge

Full size image

5.3 Curvature Bounding

In this section, two analytical solutions for curvature bounding are presented. They ensured peak segment curvature does not exceed the maximum curvature, k peak ≤ K max. This confirms that the path is feasible, having shown in the previous section that each path segment has a single peak. The first solution was relaxed ensuring a smooth curvature. The second solution was strict to minimize deviation from the original control polygon. It was possible to combine both conditions in different segments, on account of B-spline local support property, with minimal effect on other segments. Both conditions were designed to make certain that the path was contained within the convex hull of the original control polygon to reduce the probability of the obstacle collision. Both solutions are essential homotopy class transformation to ensure feasibility. Nonetheless, the guarantee that the path is collision-free was not addressed in this work. We assume that this work will eventually be combined within a planning framework and will not be restricted to path smoothing.

5.3.1 Single-Peak Solution

Consider the single control segment, shown in Fig. 9.8, whose corresponding B-spline curvature violates the maximum curvature condition. The segment consists of two lines l n,n + 2 , joining point (n) and point (n + 2), and l n + 2, n + 4 , joining point (n + 2) and point (n + 4), shown as solid black lines. Point (o) is the intersection point between l n, n + 4 (thin gray line) and line l o,n + 2 (dotted blue line) which is passing through point (n + 2) and is orthogonal to l n, n + 4 .

Fig. 9.8
figure 8

First smoothing solution; it is required to find the point (P) along the line (dotted blue line), joining point (n + 2) and point (o), that ensures the curvature, k peak, does not exceed K max

Full size image

The current curvature, k n + 2, and segment angle, αn + 2, are known, and k n + 2 > K max. Assume that point (n + 2) is shifted toward point (o), along the line, l o,n + 2 , while points (n) and (n + 4) are unchanged and the midpoints (n + 1) and (n + 3) are recomputed accordingly. Finally at α n + 2  = π, k o  = 0. It is required to find the nearest point (p), at which k p  = K max, as point (n + 2) is being shifted toward (o) along l o,n + 2 . The minimum angle αp lies between αo = π and α n + 2 as given by Eq. 9.15. We define \( \overline{li,j\ } \) as the Euclidean distance between two points (i) and (j) whose Cartesian coordinates are known.

Assuming line l o,n + 2 is parameterized between P n + 2 and P o using \( \widehat{l} \) =[0,1], the value of \( \widehat{l} \) is required where the point (p) satisfies the curvature requirement. Firstly, P p is given as follows:

$$ {P}_p={P}_{n+2}\left(1-\widehat{l}\right)+{P}_0\widehat{l} $$
(9.15)

In every iteration, the curvature is evaluated until the k p  = K max condition is satisfied. To optimize the search, we can estimate the initial point where the curvature may be equal to K max. This is achieved by knowing that, at \( \widehat{l} \) = 0, k = k p and, at \( \widehat{l}=1, \) k = 0.

$$ {\widehat{l}}_{\mathrm{init}}=1-\left|\frac{K_{\mathrm{max}}}{K_p}\right| $$
(9.16)

An example of curvature bounding is shown in Fig. 9.9 using this solution. The resulting curvature has a single segment as shown in Fig. 9.10 and was bound to 0.14 m−1. Curvature continuity was maintained in both cases.

Fig. 9.9
figure 9

Bounding using single-peak solution. The original path is blue and new path is red

Full size image
Fig. 9.10
figure 10

Resulting curvature profiles before (blue) and after (red) bounding

Full size image

5.3.2 Double-Peak Solution

In this section we proposed a different approach for the same problem considered in the previous section. The curvature of a control segment, P 1 , P 0 , P 5 and their midpoints in Fig. 9.1 exceeds K max. Segment P 1 , P 0 , P 5 is decomposed into two segments, P 1 , P 2 , P 4 (segment 1) and P 2 , P 4 , P 5 (segment 2). Line segment \( \overline{P_2{P}_4\ } \) is constructed to be parallel to edge \( \overline{P_1{P}_5\ } \). As a result, triangles ΔP 1 P 0 P 5 and ΔP 2 P 0 P 4 are similar, and the ratio between their side lengths is (1-β), where 0 < β < 1 (Fig. 9.11). Segment 1 and 2 parameters can be described in terms of β, where segment angles are constant, as given in Table 9.1.

Fig. 9.11
figure 11

Second smoothing solution; it is required to find the value of β that ensures curvature bounding in both segments and minimizes the total path length

Full size image
Table 9.1 Comparing related methods
Full size table

By substituting the segment parameters, given in Table 9.2, in Eq. 9.12, it is possible to find a range for β, subset of set [0,1), in which both segment curvatures are less than K max . Firstly, we compute a separate range for each segment 1 and 2 [βmin1, βmax1] and [βmin2, βmax2]. These computations are efficient by virtue of using the lookup table in the previous section. The allowable range for β is [max(β min1, β min2), min(β max1, β max2)].

Table 9.2 Segment parameter
Full size table

We nominate the β value that minimizes the total length. Now, new segment control points P 2 , P 3 , P 4 can be computed, where for any control point we have P i  = (x i , y i ).

$$ \beta =\mathrm{argmin}\left(\beta \left({L}_1+{L}_2-{L}_0\right)+{L}_0\right),\beta \kern0.24em \upepsilon\;\left[{\beta}_{\mathrm{min}},{\beta}_{\mathrm{max}}\right] $$
(9.17)
$$ {P}_2=\beta {P}_1+\left(1-\beta \right){P}_0 $$
(9.18)
$$ {P}_4=\beta {P}_5+\left(1-\beta \right){P}_0 $$
(9.19)

A midpoint is inserted between the two added points based on the ratio between the lengths of both, such that if both lines are equal, r = 1; the midpoint is equidistant between them.

$$ {P}_3=\frac{r}{r+1}\left({P}_4-{P}_2\right)+{P}_2 $$
(9.20)

An example of curvature bounding is shown in Fig. 9.12 using this solution. The resulting curvature has two segments as shown in Fig. 9.13 and was bound to 0.14 m−1. Curvature continuity was maintained in both cases.

Fig. 9.12
figure 12

Bounding using double-peak solution. The original path is blue and the feasible path is red

Full size image
Fig. 9.13
figure 13

Resulting curvature profiles before (blue) and after (red) bounding

Full size image

6 Results

6.1 Curvature Evaluation

To efficiently evaluate a segment’s curvature, we proposed storing the peak curvature position u peak in a sparse lookup table and evaluating the curvature using the segment parameters. We conducted 1000 queries, for a range of segment parameters where r and L = [1 m, 10 m] in steps of 1 m and α was = [30°, 180°] in steps of 15°. The time performance of this evaluation method was compared with solving Eq. 9.13. From the results, given in Table 9.3, it is clear that this method is more efficient. Comparing with previously published research results Elbanhawi et al. (2014), which assumed equal segment length, we show that this approach has better accuracy. The results are illustrated in Fig. 9.14 and given in Table 9.4.

Table 9.3 Curvature evaluation time performance for 1000 queries
Full size table
Fig. 9.14
figure 14

Curvature evaluation errors of proposed lookup table compared to Elbanhawi et al. (2014)

Full size image
Table 9.4 Curvature evaluation errors
Full size table

6.2 Curvature Bounding

In this section we compared the presented bounding solutions to our earlier work in Elbanhawi et al. (2014). Two different examples were used as shown in Figs. 9.15 and 9.18. The linear reference paths are assumed to result from a planning algorithm. It can be noted that the proposed solutions maintain the curve within the convex hull of the original reference path. In both cases, the curvature is successfully bounded to 0.2 and 0.15 m−1 successively, and its continuity is maintained as shown in Figs. 9.16, 9.17, and 9.18. The proposed solutions reduce the deviation from the original path and the total path length, outperforming our earlier work as detailed in Tables 9.5 and 9.6. Solution (1) results in a low-frequency single-peak curvature profile as opposed to solution (2), which may have a better impact on passenger comfort in autonomous cars, as suggested in (Gulati and Kuipers 2008; Turner and Griffin 1999). On the other hand, solution (2) minimizes deviation from the reference paths and as a consequence minimizing the risk of collision.

Fig. 9.15
figure 15

Example 1: Bounding paths using different methods

Full size image
Fig. 9.16
figure 16

Example 1: Resulting curvature profiles

Full size image
Fig. 9.17
figure 17

Example 2:Bounding paths using different methods

Full size image
Fig. 9.18
figure 18

Example 2:Resulting curvature profiles

Full size image
Table 9.5 Example 1: Resulting path lengths and deviation
Full size table
Table 9.6 Example 2: Resulting path lengths and deviation
Full size table

Example (3) highlights the ability of the proposed method to generate a feasible path among obstacles. The benefit of maintaining the curve within the convex hull of the path is apparent in this example. The linear path was generated from a rapidly exploring random tree (RRT) algorithm (Elbanhawi and Simic 2014b). The resulting B-spline path among obstacles is illustrated in Fig. 9.19. Post-processing RRT algorithms have been shown to improve path quality and produce fairly consistent results. Nonetheless these methods do not guarantee that the path is collision-free. The resulting trajectory is given in Fig. 9.20. It is clear that the multi-segment path maintains curvature and parametric continuity.

Fig. 9.19
figure 19

Example 3: Kinodynamic motion among obstacles

Full size image
Fig. 9.20
figure 20

Example 3: Resulting path maintains parametric continuity

Full size image

7 Conclusion

An approach to continuous curvature robot path smoothing that satisfies the maximum curvature bounds and parametric continuity is presented here. B-spline curves have been proposed for this task. In this chapter we offer the following contributions:

  • Maintaining path parametric C2 continuity, by using a single B-spline curve segment with midpoint insertion, to generate more realistic robot paths (Pan et al. 2012). No limitations were posed on the number of control points, for the B-spline curve, enabling a more robust representation of the path, unlike the work in (Nikolos et al. 2003; Jolly et al. 2009).

  • Two analytical solutions are offered, formulating the path curvature in terms of a predefined path segment’s parameters. They modify the path to limit its curvature to the maximum kinodynamic curvature and satisfy the vehicle’s constraints. Our previous publications presented an introduction to the more advanced solutions (Elbanhawi et al. 2014; Elbanhawi and Simic 2014b).

Based on the presented numerical and experimental results, we show that this approach:

  • Improved accuracy of segment curvature evaluation

  • Accelerated segment curvature evaluation

  • Decreased path length compared to reference spline and linear path

  • Decreased deviation from reference path

  • Bounded curvature to desired value while maintaining parametric continuity

The proposed method results in paths that lie within the convex hull of the linear path, with no undesirable oscillations in the path. This produced realistic commands with continuous velocity and acceleration.

This approach relies on smoothing a path defined by successive waypoints, which are generated by a planning algorithm. As presented here, smoothing is considered as a post-planning procedure. Consequently, obtaining an obstacle-free smooth path cannot be guaranteed. In many cases, when collision is detected, replanning is required (Koyuncu and Inalhan 2008). Several researchers examine collision detection for parametric curves (Kwangjin and Sukkarieh 2010; Pan et al. 2012). Circumventing the need for replanning can be achieved by incorporating the smoothing process within the planning framework. The benefits of guiding the search by the reachability of the robot have been revealed (Shkolnik et al. 2009; Jaillet et al. 2011). In this context, the reachable set can be computed using the efficient curvature evaluation method presented here. Several comments have to be made regarding the presented contributions.

The benefit of solving C2 parametric continuity problem with maximum curvature constraint and employing the results to mimic human steering is the possibility to combine this parameterization, within any planning framework, such as an RRT or A* algorithm for autonomous vehicles. We predict improvements in human comfort as a result of mimicking human steering, in addition to other claims made by researchers, with regard to continuous curvature paths.

The proposed midpoint insertion algorithm is used to simplify the smoothing algorithm; a more generalized approach would include the location of the inserted point, as a function of the segment angle, but the benefits of doing that are not clear. That could be the subject of other investigation.

In the practical implementation through experiments, we demonstrated that the closely following control polygon method is advantageous over the other algorithms. This benefit comes from the assumption that the path planning algorithm generates a collision-free piecewise linear path, which is then used by our smoothing algorithm, as shown in (Elbanhawi and Simic 2014b).The results can be developed within the context of a recently developed sampling-based algorithm (Elbanhawi and Simic 2014c), which employs efficient collision-checking procedures.

We expect that the outcomes of presented research can be integrated within an efficient planning framework, in which the spline-parameterized motions feel natural to passengers and improve their comfort. Passenger comfort and natural paths are obviously subjective terms that require a large sample of human volunteers for validation. The promising simulations’ results, presented here, will be followed by field tests using prototype ground vehicles and UAVs. We plan to validate the concept of graceful motions and curvature continuity, with regard to passenger comfort, by conducting full-scale field experiments.