A G³-Continuous Extend Procedure for Path Planning of Mobile Robots with Limited Motion Curvature and State Constraints

Featured Application

The proposed method can be applied to planning of reference paths for autonomous wheeled robots and intelligent vehicles maneuvering in cluttered environments.

Abstract

Provably correct and computationally efficient path planning in the presence of various constraints is essential for autonomous driving and agile maneuvering of mobile robots. In this paper, we consider the planning of ${\mathbb{G}}^3$-continuous planar paths with continuous and limited curvature in a motion environment that is bounded and contains obstacles modeled by a set of (non-convex) polygons. In practice, the curvature constraints often arise from mechanical limitations for the robot, such as limited steering and articulation angles in wheeled robots, or aerodynamic constraints in unmanned aerial vehicles. To solve the planning problem under those stringent constraints, we improve upon known path primitives, such as Reeds–Shepp (RS) and CC-steer (curvature-continuous) paths. Given the initial and final robot configuration, we developed extend-procedure computing paths that can approximate RS paths with arbitrary precision, but guaranteeing ${\mathbb{G}}^3$-continuity. We show that satisfaction of all stated path constraints is guaranteed and, contrary to many other methods known from the literature, the method of checking for collisions between the planned path and obstacles is given by a closed-form analytic expression. Furthermore, we demonstrate that our approach is not conservative, i.e., it allows for precise maneuvers in tight environments under the assumption of a rectangular robot footprint. The presented extend procedure can be integrated into various motion-planning algorithms available in the literature. In particular, we utilized the Rapidly exploring Random Trees (RRT*) algorithm in conjunction with our extend procedure to demonstrate its feasibility in motion environments of nontrivial complexity and low computational cost in comparison to a ${\mathbb{G}}^3$-continuous extend procedure based on ${\eta }^3$-splines.

1. Introduction

In this paper, we focus on the development of a path primitive and the so-called extend procedure (i.e., a local planning algorithm generating a path connecting two robot configurations), which is crucial for many path-planning algorithms utilized in the navigation of mobile robots. Despite a large body of work concerning path planning for mobile robots and autonomous vehicles (e.g., References [1,2]), this problem remains a challenge, especially in the presence of various constraints arising in practical scenarios. On one hand, mechanical limitations of the robot, such as limited steering and articulation angles in wheeled robots, or aerodynamic constraints in unmanned aerial vehicles, result in path curvature limits. On the other hand, the presence of a bounded-motion environment with obstacles and forbidden areas leads to state constraints imposed on the robot, which reduces the set of feasible paths. It is also important to maintain a high degree of path continuity to achieve smooth control of the robot and increase the comfort of passengers or the safety of the payload. To account for all these constraints, we built upon our approach introduced in Reference [3], where we proposed the extend procedure generating ${\mathbb{G}}^3$-continuous planar paths (that is, paths with continuous curvature derivative with respect to curve arc length) taking into account a limited curvature of motion in cluttered environments. In contrast to Reference [3], we present the new extend procedure for the carlike kinematics taking into account vehicle-body dimensions in planning collision-free paths, admitting the nonconvex polygonal obstacles present in the operational space. As a consequence, the motion-planning strategy presented in the current paper inherits beneficial properties of the original approach presented in Reference [3], but extends its potential applications to more practical path-planning scenarios.

2. Prerequisites and Problem Statement

While our considerations are quite general, meaning that the planar paths planned with our approach can be applied to various systems and planning tasks, let us consider a rear-driven carlike-vehicle kinematics as an illustratory example used throughout this paper. This is shown in Figure 1. Using results from Reference [4], one can model this system by decomposition into unicycle vehicle-body kinematics $\dot{\overline{\mathit{q}}}=\mathit{G}(\theta )\mathit{v}$ and steering dynamics $\dot{\beta }=u_1$ as follows:

$$\begin{array}{cc}\hfill \dot{\overline{\mathit{q}}}& =\left[\begin{array}{cc}1& 0\\ 0& cos\theta \\ 0& sin\theta \end{array}\right]\left[\begin{array}{c}{v}_1\\ v_2\end{array}\right]=\mathit{G}(\overline{\mathit{q}})\mathit{v},\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill \dot{\beta }& =u_1,\mathit{v}=\left[v_1{v}_2\right]\triangleq {\left[\begin{array}{cc}\frac{u_2}{L}tan\beta & u_2\end{array}\right]}^{\top }\in {\mathbb{R}}^2,\hfill \\ \hfill \mathit{u}& ={\left[u_1{u}_2\right]}^{\top }\in {\mathbb{R}}^2,\hfill \end{array}$$

(2)

where $\mathit{q}={\left[\beta \theta xy\right]}^{\top }={\left[\beta {\overline{\mathit{q}}}^{\top }\right]}^{\top }={\left[\beta \theta {\tilde{\mathit{q}}}^{\top }\right]}^{\top }\in \mathcal{Q}=[-{\beta }_m,{\beta }_m]\times \mathbb{R}\times \mathcal{P}$ denotes a configuration vector with ${\beta }_m<\pi /2$ being a steering angle limit, $\mathcal{P}\subseteq {\mathbb{R}}^2$ denotes a position space, $\mathit{v}$ corresponds to vehicle-body kinematics-control input, with $v_1$ being vehicle-body angular velocity, and $v_2$ corresponding to longitudinal velocity of the guidance point $\tilde{\mathit{q}}={\left[xy\right]}^{\top }$ illustrated in Figure 1, while u is the control input of carlike kinematics comprising steering angle rate $u_1$ and longitudinal velocity $u_2\equiv v_2$. As a result of limited steering angle $\beta \in [-{\beta }_m,{\beta }_m]$, the following constraint is present:

$$\begin{array}{cc}\hfill \left|\kappa \right|& \le \frac{1}{L}tan{\beta }_m=:{\kappa }_B,\hfill \end{array}$$

(3)

where

$$\begin{array}{cc}\hfill \kappa & \triangleq \frac{v_1}{v_2}=\frac{1}{L}tan\beta \hfill \end{array}$$

(4)

is the motion curvature of the robot, whereas L denotes the distance between the rear and the front axle (see Figure 1).

Let us assume that control input u is continuous, which is often desirable in practical applications. According to Equation (2), this implies that $\dot{\beta }$ is continuous. Since $\dot{\beta }$ is related to curvature $\kappa (t)$ by Equation (4), one concludes that admissible trajectories of the carlike kinematics must have curvature $\kappa (t)$ of class $C^1$ (with a continuous time derivative), which satisfies Equation (3). As a consequence of such requirements, admissible paths for the carlike kinematics must be ${\mathbb{G}}^3$-continuous, that is, for a path ${\tilde{\mathit{q}}}_d(s)={\left[x_d(s)y_d(s)\right]}^{\top }$, its curvature ${\kappa }_d(s)$ must be at least of class $C^1$. In this paper we consider three problems of planning such ${\mathbb{G}}^3$-continuous paths. They shall be solved under the following assumptions:

A1.

Planned positional path ${\tilde{\mathit{q}}}_d(s)$ starts at $s=0$ and finishes at $s=s_{fin}$, i.e., $s\in [0,s_{fin}]$, $s_{fin}\ge 0$, where s corresponds to the arc-length parameter.

A2.

Initial reference steering angle ${\beta }_d(s=0)$ and final reference steering angle ${\beta }_d(s=s_{fin})$ are fixed to 0, i.e., ${\beta }_d(s=0)={\beta }_d(s=s_{fin})=0$.

A3.

Initial steering rate $\frac{d{\beta }_d}{ds}(s=0)$ and final steering rate $\frac{d{\beta }_d}{ds}(s_{fin})$ are fixed to 0, i.e., $\frac{d{\beta }_d}{ds}(0)=\frac{d{\beta }_d}{ds}(s_{fin})=0$.

A4.

${\mathcal{P}}_f\subseteq \mathcal{P}$ is a free subset of position space bounded by a single (nonconvex) polygon and containing (nonconvex) polygonal obstacles.

Assumption A1 is a consequence of not knowing the resultant path length in advance. Assumptions A2 and A3 have been taken for simplicity of considerations since they help to fix the path structure. Assumption A2 can be lifted by simply changing the initial order of path segments, as explained in the description of the proposed extend procedure. Assumption A4 is used to efficiently check for satisfaction of state constraints by the reference path.

Problem 1.

Feasible path planning in free space. Given a collision-free initial and vehicle-body configurations ${\overline{\mathit{q}}}_{dinit}$ and ${\overline{\mathit{q}}}_{dfin}$ plan a ${\mathbb{G}}^3$-continuous path ${\tilde{\mathit{q}}}_d(s)$ admissible for the car-like kinematics, such that ${\overline{\mathit{q}}}_d(s=0)={\overline{\mathit{q}}}_{dinit}$ and $\exists s_{fin}\overline{\mathit{q}}(s_{fin})={\overline{\mathit{q}}}_{dfin}$. Curvature ${\kappa }_d(s)$ of path ${\tilde{\mathit{q}}}_d(s)$, is limited as follows

$$\begin{array}{cc}\hfill \forall s\in [0,s_{fin}]\left|{\kappa }_d(s)\right|& \le {\kappa }_B.\hfill \end{array}$$

(5)

Problem 2.

Feasible path planning. Solve Problem 1 by planning a collision-free path, that is, the additional constraint

$$\forall s\in [0,s_{fin}]{\tilde{\mathit{q}}}_d(s)\in {\mathcal{P}}_f$$

shall be satisfied.

Problem 3.

Feasible path planning with rectangular footprint. Solve Problem 1 by planning a collision-free path for a robot with rectangular footprint, that is, the reference path shall additionally satisfy

$$\forall s\in [0,s_{fin}]\mathcal{V}({\overline{\mathit{q}}}_d(s))\in {\mathcal{P}}_f,$$

(6)

where ${\overline{\mathit{q}}}_d(s)={\left[{\theta }_d(s){\tilde{\mathit{q}}}_d(s)\right]}^{\top }$ is the reference vehicle-body configuration along the path, and ${\theta }_d(s)$ is the reference robot orientation tangent to path ${\tilde{\mathit{q}}}_d(s)$, while $\mathcal{V}({\overline{\mathit{q}}}_d(s))⊊{\mathbb{R}}^2$ is a position space subset occupied by rectangular footprint of the robot (see Figure 1). Rectangular footprint $\mathcal{V}({\overline{\mathit{q}}}_d(s))$ can be expressed in local coordinate frame $\left\{L\right\}$ of reference vehicle-body configuration ${\overline{\mathit{q}}}_d(s)$ as follows:

$${\mathcal{V}}^L\triangleq \{x^L,y^L:-c\le x^L\le a\wedge |y^L|\le b/2\}.$$

Note that even though we only plan a position path ${\tilde{\mathit{q}}}_d(s)$, orientation component ${\theta }_d(s)$ is known due to the differential flatness of vehicle-body kinematics. The foundations for solving Problem 1 are given in Section 4.1, Section 4.2 and Section 4.3, whereas the final solution is presented in Section 4.4. In Section 4.5, we extend our solution to Problem 1 with collision checking, such that it is capable of solving Problem 2 when coupled with a global motion-planning algorithm (e.g., sampling-based planner). Similarly, in Section 4.6 we extended the collision-checking method to rectangular robots. In Section 4.6 we discuss applications of the proposed extend procedure, and show how it can be applied to solve Problem 3. Before fully explaining our approach, let us briefly survey current path-planning primitives in the next section.

3. Related Work

Planning paths of limited curvature has been addressed with various methodologies. The most relevant properties of selected known approaches have been gathered in

Table 1. Comparison of the proposed ${\mathbb{G}}^3$-continuous path primitive with known path primitives.

Path Primitive	Continuity	Bounded $\mathit{\kappa }$	Collision Checking	Length Computation
Reeds–Shepp [5]	${\mathbb{G}}^1$	yes	analytic	analytic
${\eta }^3$-splines [11]	${\mathbb{G}}^3$	no	numerical	numerical
${\eta }^4$-splines [12]	${\mathbb{G}}^4$	no	numerical	numerical
Clothoid-based	${\mathbb{G}}^2$	yes	numerical	analytic
Fermat’s spiral [21]	${\mathbb{G}}^2$	yes	numerical	numerical
Low-order B-Splines [14,15]	${\mathbb{G}}^2$	yes	numerical	numerical
Cubic curvature splines [8]	${\mathbb{G}}^3$	yes	numerical	numerical
High-order B-splines	${\mathbb{G}}^{3+}$	no	approx. analytic	numerical
HC-Steer [10]	${\mathbb{G}}^1$	yes	analytic	numerical
${\mathbb{G}}^3$-continuous path primitive	${\mathbb{G}}^3$	yes	analytic	numerical

. A fundamental result from Reference [5] characterizes the shortest paths of bounded curvature as particular combinations of line segments and circle arcs, i.e., Reeds–Shepp (RS) paths. Note that this result does not consider obstacles, which have been partially taken into account for Dubins paths in Reference [6]. The drive toward paths with continuous curvature and a continuous-curvature arc-length derivative has led to the development of the CC-steer method [7] and its ${\mathbb{G}}^3$-continuous variant introduced in Reference [8]. Our work can also be viewed as a ${\mathbb{G}}^3$-continuous extension of the CC-steer method. However, contrary to the approach from Reference [8], our transition segments are not represented by curvature profiles. We propose transition segments with explicit representation of x as a function of y, which can easily be used with known path-following controllers (e.g., Reference [9]). We also devised a closed-form expression utilized to check for collisions between our ${\mathbb{G}}^3$-continuous path primitive and an obstacle, which leads to efficient and exact collision checking. There has also been some work on improving the computational cost of finding RS-like paths in Reference [10]; however, path-continuity and collision-checking issues for paths of high continuity were not explicitly addressed, to the best of our knowledge.

One can also find various methods of generating paths with a different structure to RS paths. For example, polynomial spline-based path primitives such as ${\eta }^3$-splines [11] and ${\eta }^4$-splines [12] have been developed. Their advantages are generality, conceptual simplicity, and high continuity. However, collision and curvature-constraint checking must usually be done numerically in the case of such primitives, which leads to significant computational cost. Furthermore, the parameters of such primitives can be hard to tune, even though this was partially addressed in Reference [13]. Another group of path primitives and extend procedures relies on B-splines [14,15,16] providing a limited curvature, curvature continuity (but not ${\mathbb{G}}^3$-continuity), and parameters that are easy to tune. Collision checking is done numerically in this case as well.

There are also various application-specific methods using the path primitives mentioned above, such as planning algorithms for environments with a roadlike structure similar to References [17,18], a method used to design curvature profiles of paths passing through waypoints [19], a spline-based approach exploiting sum-of-squares optimization to handle exact collision checking, or an elastic-band-like algorithm [20]. Those methods share the various benefits and drawbacks of different path primitives. However, thanks to the proposed path primitive, our approach combines ${\mathbb{G}}^3$-path continuity with analytically guaranteed curvature limits, as well as fast and exact analytic collision checking. To the best of our knowledge, such a combination of features is not exhibited by most known path primitives, as shown in Table 1.

4. The ${\mathbb{G}}^3$-Continuous Extend Procedure

4.1. Main Concept

We introduce the so called ${\mathbb{G}}^3$-continuous path primitive, which consists of a transition segment (further explained in Section 4.2), a circle arc, a reversed transition segment, and a line segment. Similarly to the form of path encoding taken from Reference [5], we introduce a language for encoding paths with three words corresponding to path segments:

• $T({\mathit{w}}_1,{\mathit{w}}_2,\mu )$ denotes a transition segment connecting ${\mathit{w}}_1$ with ${\mathit{w}}_2$ (defined in Section 4.2),
• $C({\mathit{w}}_1,{\mathit{w}}_2)$ is a circular arc of radius $1/{\kappa }_c$ connecting ${\mathit{w}}_1$ with ${\mathit{w}}_2$,
• $S({\mathit{w}}_1,{\mathit{w}}_2)$ corresponds to a straight line connecting ${\mathit{w}}_1$ with ${\mathit{w}}_2$,

where ${\mathit{w}}_k\triangleq {\left[w_{k\theta }{w}_{kx}{w}_{ky}\right]}^{\top }={\left[w_{k\theta }{\mathit{w}}_k^{\top }\right]}^{\top }$ for $k=1,2,3,4,5$ correspond to the reference vehicle-body configurations at endpoints of path segments. Using this encoding, one can describe the proposed ${\mathbb{G}}^3$-continuous path primitive connecting ${\mathit{w}}_1$ with ${\mathit{w}}_5$ as

$$T({\mathit{w}}_1,{\mathit{w}}_2,{\mu }_1)C({\mathit{w}}_2,{\mathit{w}}_3)T({\mathit{w}}_4,{\mathit{w}}_3,{\mu }_2)S({\mathit{w}}_4,{\mathit{w}}_5).$$

The geometric interpretation of our ${\mathbb{G}}^3$-continuous path primitive is shown in Figure 2 (please, note the intentionally reversed order of the arguments in $T({\mathit{w}}_4,{\mathit{w}}_3,{\mu }_2)$ in the above formula; it simply corresponds to a reversal of the segment’s endpoints). Note that, by taking transition segments of zero length, one can obtain an RS path. Depending on a choice of parameters ${\mu }_1,{\mu }_2$, one can compromise between path length and smoothness resulting from longer transition segments. Since properties of RS paths are well known and problems such as collision checking or curvature limit checking are trivial in their case, we focused on transition segments in the sequel, and show how solutions to Problems 1–3 can be obtained with their help.

The extend procedure utilizing the ${\mathbb{G}}^3$-continuous path primitive consists of the following main stages:

• Choose parameters ${\mu }_1$, ${\mu }_2$, and curvature ${\kappa }_c\ne 0$.
• Find a sequence of ${\mathbb{G}}^3$-continuous path primitives connecting two prescribed vehicle-body configurations.
• Check for collisions.
• Return computed path or a collision signal.

4.2. Transition Segments and ${\mathbb{G}}^3$-Continuous Path Primitives

Since computing the endpoints of a line segment and a circle arc given its radius and length is straightforward, we explicitly only define relations concerning transition segments $T({\mathit{w}}_1,{\mathit{w}}_2,\mu )$. Let us denote variables expressed in a local coordinate frame fixed at point ${\mathit{w}}_1$ by ${(·)}^1$, that is, ${\mathit{w}}_2^1$ corresponds to ${\mathit{w}}_2$ expressed in coordinates of ${\mathit{w}}_1$. A transition segment connecting vehicle-body configuration ${\mathit{w}}_1$ with ${\mathit{w}}_2$ is defined by the following curve, expressed in the coordinates of endpoint ${\mathit{w}}_1$:

$$\begin{array}{cc}\hfill x^1& =f(y^1)=\left\{\begin{array}{cc}\frac{-\mathrm{sgn}\left(w_{2x}^1\right)\left|y^1\right|}{2}\left[{\left(\frac{y^1}{p}\right)}^{\mu }-{\left(\frac{y^1}{p}\right)}^{-\mu }\right]\hfill & \mathrm{for}{y}^1\ne 0,\hfill \\ 0\hfill & \mathrm{for}{y}^1=0,\hfill \end{array}\right.\hfill \end{array}$$

(7)

$$\begin{array}{cc}\hfill p& \triangleq w_{2y}^{1}exp\left(\frac{\left|\mathrm{arsinh}\left(w_{2x}^1/w_{2y}^1\right)\right|}{\mu }\right),w_{2y}^1=\frac{K}{{\kappa }_c}{y}^{\ast },w_{2x}^1=\frac{K}{{\kappa }_c}{x}^{\ast },\hfill \end{array}$$

(8)

with

$$\begin{array}{cc}\hfill K& =\frac{y^{\ast }\left(-\mu +{\mu }^2\frac{x^{\ast }}{r}\right)}{-{\left(r\right)}^2{\left(1+{\mu }^2-2\mu \frac{x^{\ast }}{r}\right)}^{3/2}},r\triangleq \sqrt{{\left(x^{\ast }\right)}^2+{\left(y^{\ast }\right)}^2},x^{\ast }=f(y^{\ast }),\hfill \\ \hfill y^{\ast }& ={\left({\left(\frac{4}{3}{p}_5-p_3\right)}^{1/3}-\frac{-6{\mu }^3+{\mu }^2+2\mu +3}{\left(6\mu +3\right){\left(\mu +1\right)}^2}+p_2\right)}^{1/2\mu },\hfill \end{array}$$

whereas

$$\begin{array}{cc}\hfill p_1& =-6{\mu }^4-5{\mu }^3+3{\mu }^2+5\mu +3,\hfill \\ \hfill p_2& =\frac{4{\mu }^2\left(18{\mu }^4+3{\mu }^3-17{\mu }^2-11\mu +7\right)}{9p_1{\left(\mu +1\right)}^4{\left(2\mu +1\right)}^2},\hfill \\ \hfill p_3& =\frac{{\left(p_1\right)}^3}{27{(2\mu +1)}^3{(\mu +1)}^9}+\frac{(2\mu -1){(\mu -1)}^3}{(4\mu +2){(\mu +1)}^3}-p_4,\hfill \\ \hfill p_4& =\frac{p_1(-6{\mu }^4+5{\mu }^3+3{\mu }^2-5\mu +3)}{6{(2\mu +1)}^2{(\mu +1)}^6},\hfill \\ \hfill p_5& =\sqrt{\frac{{\mu }^6{(\mu -1)}^3(-36{\mu }^4+33{\mu }^2-29)}{{(2\mu +1)}^4{(\mu +1)}^9}},\hfill \end{array}$$

where $y^{\ast }$ is a rational function of $\mu \in (0.5,1)$, ${\kappa }_c\in [-{\kappa }_B,{\kappa }_B]\setminus \left\{0\right\}$ denotes the curvature of adjacent circle arc C, while $\mu \in (0.5,1)$ is a design parameter influencing the supremum value of curvature arc-length derivative $|d{\kappa }_d(s)/ds|$ during the transition segment and its length, as shown in Figure 3 and Figure 4.

Given a particular value of $\mu$ and transition segment endpoint ${\mathit{w}}_1$, one can compute coordinates of the other endpoint ${\mathit{w}}_2$ from Equation (8). The curve representing a transition segment is given by Curve (7) with particular value of parameter p resulting from Equation (8). Curve (7) was derived and analyzed in Reference [22]. It corresponds to an integral curve of the convergence vector field from the Vector Field Orientation (VFO) control law for the waypoint-following task. Its beneficial properties, proved in Reference [22], are instrumental in the construction and analysis of the ${\mathbb{G}}^3$-continuous path primitive. Given such a ${\mathbb{G}}^3$-continuous path primitive structure, we analyze its properties in the sequel.

Remark 1.

Transition segments are not explicitly parameterized by arc-length s, but they can immediately be used with path-following controllers utilizing level curves, such as the one from Reference [9].

4.3. Path Continuity and Curvature Limit Satisfaction Analysis

Let us begin by showing that Curvature Limit (5) is satisfied. Since ${\kappa }_c\in [-{\kappa }_B,{\kappa }_B]\setminus \left\{0\right\}$, circle arcs and line segments satisfy the path curvature limit by construction. The curvature limit is also satisfied by both transition segments, since their curvature is limited by ${\kappa }_c$, as proven in Property 2 in Reference [22].

We now turn to path continuity. To ensure ${\mathbb{G}}^3$-continuity of the proposed primitive, one must guarantee that, for every transition segment T, the following relations hold:

$$\begin{array}{c}\hfill \underset{s\to s_1}{lim}{\kappa }_d(s)=0\underset{s\to s_1}{lim}\frac{d{\kappa }_d}{ds}(s)=0,{\kappa }_d(s_2)={\kappa }_c,\frac{d{\kappa }_d}{ds}(s_2)=0,\end{array}$$

where $s_1$ and $s_2$ are the values of s, such that ${\overline{\mathit{q}}}_d(s_1)={\mathit{w}}_1$ and ${\overline{\mathit{q}}}_d(s_2)={\mathit{w}}_2$. Note that a limit is sufficient to ensure ${\mathbb{G}}^3$-continuity in the case of point $s_1$, since at this point only a connection with line segments or boundary conditions of the path can occur, which guarantees continuity on the other side of the transition segment connection.

Condition ${\kappa }_d(s_2)={\kappa }_c$ is immediately satisfied since point $w_{2y}^1$ is defined in such a way, that it corresponds to the point of maximal curvature for the transition segment (assuming $\mu \in (0.5,1)$, which is satisfied in our case) as shown in the proof of Property 2 in Reference [22]. Since a curvature maximum occurs at $s_2$, it also implies that $\frac{d{\kappa }_d}{ds}(s_2)=0$ holds, because $s_2$ is a stationary point of ${\kappa }_d(s)$. In the proof of Property 2 from Reference [22], we have also shown that ${lim}_{y^1\to 0}{\kappa }_f(y^1)=0$ for $\mu \in (0.5,1)$, where ${\kappa }_f$ denotes curvature of the transition segment as a function of $y^1$. We conclude that this implies ${lim}_{s\to s_1}\kappa (s)=0$.

Therefore, it remains to show that ${lim}_{s\to s_1}\frac{d{\kappa }_d}{ds}(s)=0$. Let us recall that, by the definition of curve’s curvature, a transition segment curve T has a curvature that can be represented as

$${\kappa }_f(y^1)=\frac{\frac{d^2{f}^1}{d{(y^1)}^2}}{{\left(1+{\left(\frac{df}{d{y}^1}\right)}^2\right)}^{3/2}}.$$

(9)

One can also compute the arc-length derivative of curvature $\frac{d{\kappa }_f}{ds}$ as follows:

$$\begin{array}{cc}\hfill \frac{d{\kappa }_f}{ds}& =\frac{d{\kappa }_f(y^1)}{d{y}^1}\frac{d{y}^1}{ds}=\frac{d{\kappa }_f(y^1)}{d{y}^1}sin{\theta }_d(s)=\frac{d{\kappa }_f(y^1)}{d{y}^1}\frac{m\mathrm{sgn}\left(\mu \right)y^1}{\sqrt{f{(y^1)}^2+{(y^1)}^2}},\hfill \end{array}$$

(10)

where $m=\mathrm{const}>0$, whereas the second equality results from the consideration of Equation (1) expressed in terms of parameter s (i.e., $t=s$), and the last equality results from the definition of ${\theta }_d(s)$ tangent to the transition segment T, which can be found from the definition of the VFO convergence vector field as shown in, e.g., Reference [22]. Then, differentiation of Equation (9) with respect to s and substitution of the result into the final form of Equation (10) results in a relation, which, after some tedious algebra, can be written as follows

$$\begin{array}{cc}\hfill \frac{d{\kappa }_f}{ds}& =\frac{8\mu \mathrm{sgn}\left(w_{2x}^1\right)(1-5\mu +{(y^1)}^{2\mu }{h}_1+{(y^1)}^{4\mu }h2+{(y^1)}^{6\mu }{h}_3)}{\left({(y^1)}^{1-2\mu }{({(y^1)}^{2\mu }+1)}^{7/2}{(1-{\mu }^2)}^5\right)},\hfill \\ \hfill h_1& =-6{\mu }^4+6{\mu }^3+3{\mu }^2-5\mu +3,\hfill \\ \hfill h_2& =-6{\mu }^4-5{\mu }^3+3{\mu }^2+5\mu +3,\hfill \\ \hfill h_3& =2{\mu }^4+7{\mu }^3+9{\mu }^2+5\mu +1.\hfill \end{array}$$

This clearly implies that, for $\mu >0.5$, limit ${lim}_{y^1\to 0}\frac{d\kappa }{ds}(s)=0$. As a consequence, ${lim}_{s\to s_1}\frac{d\kappa }{ds}(s)=0$, which concludes our analysis and proves that the proposed primitive is ${\mathbb{G}}^3$-continuous.

4.4. Computing Reeds–Shepp-Like Paths Using a ${\mathbb{G}}^3$-Continuous Path Primitive

In our extend procedure, we assume that parameters ${\mu }_1$, ${\mu }_2$, and ${\kappa }_c$ are selected or randomly sampled by the global planning algorithm. For simplicity of considerations, we also assume ${\mu }_1={\mu }_2$, but the proposed procedure can be trivially adapted for the case of ${\mu }_1\ne {\mu }_2$. Finding parameters of two ${\mathbb{G}}^3$-continuous primitives $T({\mathit{w}}_1,{\mathit{w}}_2,{\mu }_1)C({\mathit{w}}_2,{\mathit{w}}_3)T({\mathit{w}}_4,{\mathit{w}}_3,{\mu }_2)S({\mathit{w}}_4,{\mathit{w}}_5)$ and $T({\mathit{w}}_5,{\mathit{w}}_6,{\mu }_3)C({\mathit{w}}_6,{\mathit{w}}_7)T({\mathit{w}}_8,{\mathit{w}}_7,{\mu }_4)S({\mathit{w}}_8,{\mathit{w}}_9)$ connecting prescribed vehicle-body configurations ${\mathit{w}}_1$ and ${\mathit{w}}_5$ is a nontrivial task. However, one can leverage extensions of the procedure devised by Reeds and Shepp, which were presented in Reference [7] and later Reference [8]. This path-construction procedure requires knowledge of auxiliary circles, on which endpoints ${\mathit{w}}_4$ and ${\mathit{w}}_8$ of the transition segments must lie. One must also know the difference between an orientation tangent to this circle and orientation tangent to the transition segment at endpoints ${\mathit{w}}_4$ and ${\mathit{w}}_8$. This difference is denoted by $\nu$ in Figure 2.

The reference path is computed during an extend procedure as follows:

• The four possible transition segments $T_1,T_2,T_3,T_4$ starting at the prescribed initial vehicle body configuration are computed (Curve (7)). They correspond to forward motion with curvature ${\kappa }_c$, forward motion with curvature $-{\kappa }_c$, backward motion with curvature ${\kappa }_c$, and backward motion with curvature $-{\kappa }_c$. See Figure 5 for visual interpretation.
• Step 1 is repeated for the four possible transition segments, $T_5,T_6,T_7,T_8$, ending at a prescribed final vehicle-body configuration.
• For every transition segment $T({\mathit{w}}_1,{\mathit{w}}_2,\mu )$ computed up to this step, find center ${\tilde{\mathit{q}}}_c={\left[x_c{y}_c\right]}^{\top }$ of the auxiliary circle, on which the next transition segment must lie according to a simple geometric formula:

  $${\tilde{\mathit{q}}}_c={\left[w_{2x}{w}_{2y}\right]}^{\top }+1/{\kappa }_c{[-sin{w}_{2\theta }cos{w}_{2\theta }]}^{\top }.$$
• For every transition segment $T({\mathit{w}}_1,{\mathit{w}}_2,\mu )$ computed up to this step, find auxiliary circle radius R as follows:

  $$R=∥{\tilde{\mathit{q}}}_c-{\left[w_{1x}{w}_{1y}\right]}^{\top }∥.$$
• For every transition segment $T({\mathit{w}}_1,{\mathit{w}}_2,\mu )$ without a fixed value of ${\mathit{w}}_1$, find $\nu$, which is straightforward given the knowledge of transition segment Curve (7) and the auxiliary circle.
• For every circle segment $C({\mathit{w}}_1,{\mathit{w}}_2)$, compute its remaining unknown endpoint using the algorithm from Reference [7].
• If motion cost J is defined, compute the cost for all the paths and choose the optimal path. Otherwise, return a random path, or all found paths (depending on the utilized global planning algorithm).

Note that the choice of $\mu$ should depend on the assumed motion costs. For example, if smooth paths are desired, then our computational studies summarized in Figure 4 show that choosing $\mu =0.82$ leads to the paths with minimal $|d{\kappa }_d(s)/ds|$. On the other hand, for the shortest paths, choose $\mu$ close to $0.5$. The proposed extend procedure solves Problem 1; however, to solve the other problems one must account for obstacles. This is solved in the next sections.

4.5. Satisfaction of State Constraints for Point Robots

Thanks to Assumption A4, set ${\mathcal{P}}_f$ can be described by all the obstacle edges and environment boundary edges (see Figure 2) gathered in the set of N line segments:

$$\mathcal{E}\triangleq {\left\{({\mathit{p}}_i,{\mathit{r}}_i)\right\}}_{i=0}^N,{\mathit{p}}_i={\left[p_{ix}{p}_{iy}\right]}^{\top },{\mathit{r}}_i={\left[r_{ix}{r}_{iy}\right]}^{\top },$$

(11)

where ${\mathit{p}}_i$ and ${\mathit{r}}_i$ are endpoints of an i-th line segment. Condition (2) from Problem 2 is satisfied if no line segments from set $\mathcal{E}$ are crossed by the reference path. It is straightforward to analytically check this condition for circle arcs C and line segments S. We now show how to check this condition analytically for a transition segment $T({\mathit{w}}_1,{\mathit{w}}_2,\mu )$ with $w_{2x}^1>0$, since the proposed approach is easy to generalize for other cases.

After expressing edge ${\mathit{p}}_i$ and ${\mathit{r}}_i$ in the coordinates of a transition segment endpoint ${\mathit{w}}_1$, one concludes that a transition segment does not collide with line segment $({\mathit{p}}_i,{\mathit{r}}_i)$ if

$$\forall k\in [0,1]\mathrm{and}{d}_x^1\mathrm{such}\mathrm{that}{d}_x^1\in [0,w_{2x}^1]\mathrm{sgn}\left(f(d_y^1)-d_x^1\right)=\mathrm{const},$$

(12)

where

$$\mathit{d}={\left[d_x{d}_y\right]}^{\top }=k{\mathit{p}}_i+(1-k){\mathit{r}}_i.$$

This condition is illustrated in Figure 2. To explain, we consider curve f as a function and conclude that all points from the i-th line segment must lie either entirely above or below its graph. However, since the transition segment is bounded by its endpoints ${\mathit{w}}_1$ and ${\mathit{w}}_2$, we only consider such points from the i-th line segment that lie in subdomain $[0,w_{2x}^1]$ of curve f corresponding to the transition segment.

Condition (12) can be checked for the whole i-th line segment by simply checking it just for the maximal and minimal value of $d_x^1$ satisfying the left-hand-side conditions from Equation (12) and an additional critical point ${\mathit{n}}_i\triangleq {\left[n_{ix}{n}_{iy}\right]}^{\top }$ computed according to Equation (42) from Reference [22]. At critical point ${\mathit{n}}_i$, the orientation tangent to curve f is also tangential to the i-th line segment; thus, ${\mathit{n}}_i$ is the point closest to or farthest from a line containing the i-th line segment. Note that, contrary to the approach from Reference [22], we check point ${\mathit{n}}_i$ only if $\exists k\in [0,1]$, such that $d_x=n_{ix}$.

4.6. Satisfaction of State Constraints for Robots with Rectangular Footprint

Following Reference [15], we only perform collision checking for the key points of a rectangular robot footprint. Namely, it was proven in Reference [15] that, apart from initial and final configuration, it is sufficient to check for the clearance of $0.5b$ m (see Figure 1) around the path and check for collisions of point ${\mathit{p}}_o$ on the robot footprint (see Figure 2). The collision-checking procedure is performed as follows:

• Using a simple algebra check if all footprint edges in the initial and final vehicle-body configurations are collision free.
• Inflate the obstacles by $0.5b$ m. Perform the collision checking using our proposed fast method verifying condition (12).
• Upon the instantaneous center of rotation compute the orientation ${\theta }_o$ tangent to instantaneous velocity of the point ${\mathit{p}}_o$ (see Figure 2). Check for collisions of circle arcs and transition segments connected to a vehicle-body configuration ${\left[{\theta }_o{\mathit{p}}_o^{\top }\right]}^{\top }$ with modified curvature ${\kappa }_a={\kappa }_o({\kappa }_c)$ instead of ${\kappa }_c$, where

  $${\kappa }_o(\kappa )=1/\sqrt{{\left(1/\kappa +b/2\right)}^2+L^2},$$

  (13)

  where ${\kappa }_o$ is the motion curvature of the point ${\mathit{p}}_o$ which is furthest from the path, whereas $\kappa$ is the curvature of robot motion.

As shown in Figure 6, the approach taken in Step 3 is conservative for the transition segments because by taking ${\kappa }_a$ for collision checking, one assumes the motion curvature of point ${\mathit{p}}_0$ to change linearly with respect to the curvature of the robot motion. However, it is also computationally efficient, and, as shown by our computational examples, its conservativeness does not hinder maneuverability of the robot in tight environments due to the ability to plan short transition segments.

One can reason about the completeness of the presented extend procedure as follows. The proposed ${\mathbb{G}}^3$-continuous extend procedure leads to the solution of Problem 3 for its application to every complete global planning algorithm if there exists a collision-free RS path with $ϵ$-clearance for $ϵ>0$ solving this problem without the constraint of ${\mathbb{G}}^3$-continuity of the reference path. To illustrate why this is the case, let us consider that Equation (13) is not conservative for $\kappa (s)=0$ and $\kappa (s)={\kappa }_B$, that is, ${\kappa }_o={\kappa }_a=0$ and ${\kappa }_o={\kappa }_a={\kappa }_B$, respectively. Furthermore, due to Property 2 from Reference [22] and continuity of $y^{\ast }$, one concludes that $y^{\ast }\to 0$ as $\mu \to 0.5$. This means that, as $\mu \to 0.5$, the length of the transition segments tends to 0, and paths obtained from the concatenation of our ${\mathbb{G}}^3$-continuous path primitives approximate RS paths arbitrarily closely. Therefore, if there exists a feasible RS path that is no closer to obstacles than $ϵ$, one can always find such $\mu$ sufficiently close to 0.5, so that the maximal distance between the RS path and corresponding ${\mathbb{G}}^3$-continuous path is less than $ϵ$, meaning that the ${\mathbb{G}}^3$-continuous path is feasible.

5. Computational Results

To verify feasibility and effectiveness of planning with the proposed ${\mathbb{G}}^3$-continuous path primitive, it was investigated how it impacts computation times for the most crucial primitive operations in path planning, such as collision checking, checking of curvature-constraint satisfaction ($\kappa$ checking), and extension procedure computation. Similarly, we tested the computational performance of our approach in path-planning scenarios S1–S3, shown in Figure 7, Figure 8 and Figure 9. The proposed extend procedure was integrated with the RRT* motion-planning algorithm (see Reference [23] for details). All simulations were performed with the following parameters: $L=5.7$ m, $b=2.5$ m, $a=5.9$ m, ${\beta }_m=0.96$ rad. We used motion cost $J\triangleq \overline{l}+{\int }_0^{s_{fin}}{\left(\frac{d\kappa }{ds}(s)\right)}^{2}ds$, where $\overline{l}$ corresponds to the total path length. Note that the planning procedure was finished when the obtained motion cost was within $5\%$ of the value precomputed over the time of 600 s.

The computation times obtained in MATLAB are presented in

Table 2. Average computation times of primitive operations for 1000 random scenarios.

Path Primitive	Collision Checking ($\mathsf{\mu }$s)	$\mathit{\kappa }$ Checking ($\mathsf{\mu }$s)	Extend Procedure ($\mathsf{\mu }$s)
Reeds–Shepp	24	0	27
${\eta }^3$-splines	1397	1264	63
${\mathbb{G}}^3$-continuous path primitive	124	0	67

and

Table 3. Average computation times in MATLAB for 10 planning trials in Scenario S2.

Path Primitive	Planning Time of S2 (S)
Reeds-Shepp	27
${\eta }^3$-splines	86
${\mathbb{G}}^3$-continuous path primitive	39

. One can observe that the ${\mathbb{G}}^3$ path continuity of our approach comes with the price of increased computational cost in comparison to the classic RS paths with curvature discontinuities. This was expected, since one has to specifically account for additional transition segments during collision checking, and also check a larger amount of paths due to availability of additional combinations of transition segments and circle arcs. However, we note that the computational cost for the more general ${\eta }^3$-splines, which are also ${\mathbb{G}}^3$-continuous, is significantly higher than in our approach. This observation holds even if one assumes that numerical checking of curvature constraints and collision checking can be performed in parallel. Unsurprisingly, since collision-checking procedures can account for over $90\%$ of planning time, those computational performance tendencies propagate to computation times of a full path-planning procedure for example Scenario S2. Such results suggest that our approach can represent a viable alternative when the planning of ${\mathbb{G}}^3$-continuous paths is necessary due to task-specific constraints or the mechanical construction of the robot.

Figure 7 and Figure 8 illustrate the results of path planning with the proposed ${\mathbb{G}}^3$-continuous extend procedure for parking Scenarios S1 and S2, whereas in Figure 9 we show a challenging lane-change-like maneuver in Scenario S3. During Scenario S3, only forward robot motion was assumed admissible. Obstacles are shown in black, whereas the planned path is in blue. The magenta rectangles correspond to robot footprints at the endpoints of path segments comprising ${\mathbb{G}}^3$-continuous path primitives, whereas the green and red rectangles illustrate the initial and final robot configuration, respectively. It can be seen that planning is successfully performed in a severely constrained environment despite the conservative approximation utilized in our collision-checking algorithm. Curvature profiles and curvature arc-length derivative profiles are continuous; however, in some cases, e.g., in Scenario S3, relatively low $\mu$ values corresponding to relatively high curvature arc-length derivative values have been planned. This is due to the environment boundaries, which prohibited smoother curvature profiles, since those would lead to collision between the environment boundaries and the robot footprint, specifically point ${\mathit{p}}_o$ of the footprint. One can also find that, in some cases, paths contain more reversals (changes in motion strategy), because additional space is needed for transition segments, which allow smooth evolution of path curvature. Such a tendency can be eliminated by putting a bigger emphasis on path length in the planning motion cost; however, this inevitably leads to low $\mu$ values and less smooth paths.

6. Conclusions

The ${\mathbb{G}}^3$-continuous path primitive proposed in this paper allows for an extension of the well-known RS paths, and constitutes an easy-to-implement component for various path planners available in the literature. The proposed method guarantees that planned paths are collision-free, satisfy curvature constraints, and preserve continuity of the curvature arc-length derivative. It is worth emphasizing the computational efficiency of the method due to the fact that distance between the robot with a rectangular footprint and obstacles can be effectively checked in a continuous domain using the derived analytical formulas. As opposed to other solutions (e.g., those using clothoid-based approaches), the introduced ${\mathbb{G}}^3$-continuous path primitives are represented by closed-form expressions, which can be conveniently utilized by path-following feedback controllers. Upon the results included in the paper, one may conclude that the proposed planning strategy provides all the mentioned beneficial properties under a reasonable computational cost when compared to other methods known from the literature.