Efficient Collision Detection Oriented Motion Primitives for Path Planning

Path planning has been a subject of extensive research in the field of robotics, yielding a diverse array of approaches over the years [1, 2, 3]. In cases where stringent computational efficiency is a primary concern, classic reactive strategies, exemplified by methods like the Virtual Potential Field [4] and the Vector Field Histogram [5], as well as their subsequent developments [6, 7], may offer viable solutions.

Nonetheless, in numerous scenarios, these methodologies are unsuitable due to their susceptibility to entrapment in local minima. Alternative potential field formulations, which alleviate this concern, have been put forth in the literature [8, 9, 10]. However, it is important to note that the practical utility of these alternatives is often confined to specific contexts, or their adoption is associated with a substantial escalation in computational overhead [11].

To address the inherent challenge of limited path information in potential field algorithms, an array of local search approaches has emerged over the years. This category of methods typically involves the iterative adjustment of a path, frequently represented as splines [12, 13, 14, 15]. Some notable approaches within this realm include Elastic Bands [16], Reactive Path Deformation [17], Trajectory Deformer [18, 19], and Timed Elastic Band [20]. Although these methods are generally more robust and sufficiently efficient when compared to potential field algorithms, they tend to yield sub-optimal solutions.

Alternatively, there exist other techniques, such as the Dynamic Window Approach [21] and its continuous-space extensions [22, 23, 24], which conduct local searches within the velocity search space. These methods, particularly when combined with a global planner, often demonstrate a high degree of robustness in real life scenarios. In recent years, machine learning techniques, particularly deep learning algorithms, have demonstrated remarkable performance in complex environments without the need for exhaustive search processes, as evidenced by works such as [25, 26, 27]. However, a notable limitation is that, in the absence of dedicated hardware acceleration, even basic inference operations can incur significant computational expenses.

An alternative strategy for improving the planning results involves the transformation of the configuration space into a convex space, conducting the planning within this convex space, and subsequently mapping the solution back to the original configuration space. It is essential to note, however, that this approach is most suitable for static environments, wherein the computationally intensive mapping definition is executed only once [28].

Discretizing the configuration space serves to convert the intrinsically continuous problem of path planning into a graph search problem. A range of strategies for selecting the sampling approach has been thoroughly examined, encompassing widely recognized methods such as the Rapidly Exploring Random Tree (RRT) algorithm [29] and its derivatives [30], as well as the Probabilistic Road Map (PRM) approach [31]. The sampling procedures employed by these algorithms are geared towards reducing the set of configurations that must be explored before a path between the start and goal nodes is identified. In terms of the underlying graph, the primary objective is to minimize the number of nodes that constitute the graph. Aspects of the path, such as its smoothness, are often addressed through successive refinement steps [32] or are managed through deterministic optimizations integrated with the stochastic sampling process [33].

Another line of investigation has focused on minimizing the search cost within a given graph. In many motion planning scenarios, effective heuristics are available for A* to significantly reduce the number of nodes traversed during a search compared to Dijkstra’s algorithm. Additionally, in the majority of cases, obstacle positions are updated as new sensor data becomes available. Rather than conducting an entirely new search, prior computation results are leveraged to expedite the search process. This approach is adopted by algorithms such as D* [34], Lifelong Planning A* [35], and D* Lite [36].

An additional research direction has been dedicated to reducing the number of evaluated edges. Specifically, the actual cost of an edge is computed only when absolutely necessary, and heuristics are employed as substitutes for the actual computations whenever feasible. This approach is exemplified in algorithms such as Lazy PRM [37], LWA* [38], Lazy-PRM* and Lazy RRG* [39], LazySP [40], and Lifelong-GLS [41].

We employ an alternative approach to enhance the efficiency of edge cost evaluation. In the physical realm, the edges on the graph correspond to the robot’s movements, commonly referred to as motion primitives. While path smoothness and path length have traditionally been the primary design criteria for motion primitives, this paper prioritizes collision detection efficiency as the foremost design criterion. Through the precise design of motion primitives, we enable accelerated path planning without a significant compromise on path characteristics.

Section II briefly introduces biarcs, the curves utilized to generate motion primitives in this study. Section III examines the impact of a free parameter on path length and path smoothness and identifies an optimal value for achieving desirable paths. Section IV concentrates on replanning and discusses a heuristic for determining the free parameter in this context. Section V elaborates on the efficient execution of collision detection. Section VI presents the experimental results. Lastly, in Section VII, the paper concludes by assessing the limitations of the proposed approach and outlining potential avenues for future research.

Biarcs have been the subject of extensive investigation in multiple domains, including Mathematics [42, 43], Computer Graphics [44, 45], and, particularly, in the field of Computer-Aided Design [46, 47, 48, 49, 50, 51]. One significant motivation for the extensive exploration of biarcs in the latter domain is the common provision of circular interpolation algorithms in the control modules of early numerically controlled manufacturing systems. Biarc trajectories, as a result, hold appeal as readily usable tool paths.

Biarcs as paths in the field of robotics have received significantly less attention [52, 53, 54]. One contributing factor is that biarcs exhibit $G_{1}$ continuity, whereas $G_{2}$ continuity is a prerequisite for Ackermann steering. However, it is worth noting that while $G_{2}$ continuity is a desirable trait, it is not essential for many types of robots, including holonomic robots, differential drive robots, or skid steering robots. In fact, for these categories of robots, even $G_{0}$ continuity can be sufficient. Indeed, straight paths and in-place rotations are often employed in robotics because they typically represent the shortest path [55]. Nonetheless, this type of motion necessitates the robot to come to a complete stop at each waypoint, leading to frequent acceleration and deceleration, which can result in increased travel time and wear and tear. As a result, $G_{1}$ continuous curves, specifically biarcs, provide an appealing compromise between smoothness and path planning simplicity

To the best of our knowledge, there are no documented instances of the use of biarcs as motion primitives for graph-based planners. Additionally, despite the maturity of the biarc literature, it often overlooks factors crucial for robotics, such as path length and degenerate cases [56, 57]. This section seeks to address this gap by presenting an analysis concentrated on aspects pertinent to path planning.

A biarc is a curve comprising two arcs of circles, as illustrated in Figure 1b. The centers of rotation for these two arcs are denoted as $C_{A}$ and $C_{B}$, and the point at which the two arcs intersect is marked as joint point $J$. The choice of the two arcs is made so that their tangents at point $J$ are collinear. The centers of rotation for the two arcs can be located either on the same side of the curve (as depicted in Figure 1a) or on opposite sides of the curve (as depicted in Figure 1b). A special case arises when the radius of rotation is infinite, resulting in a straight segment (as shown in Figure 1c). It is worth noting that both arcs can become straight segments, and, therefore, line segments are a subset of the biarc family. Lastly, when the two centers of rotation coincide (as seen in Figure 1d), the biarc is equivalent to a single arc of a circle.

Biarcs represent one of the simplest curve types that enable the specification of both initial position and orientation as well as final position and orientation. Notably, in the case of a single arc, it is not feasible to define the arrival orientation once the arrival position is determined. For biarcs, when provided with an initial position $A$ and orientation $\theta_{A}$, and a final position $B$ and orientation $\theta_{B}$ there exists an infinite array of options for the selection of the joint point $J$. To elaborate, let us establish the following definitions (as illustrated in Figure 2):

• •

  $-\pi<\phi_{A}\leq\pi$ as the angle from the segment $AB$ to the orientation of the robot in $A$

• •

  $-\pi<\phi_{B}\leq\pi$ as the angle from the segment $AB$ to the orientation of the robot in $B$

• •

  $\gamma=\phi_{B}-\phi_{A}$

• •

  The unit vector $\bm{u}=\left[\begin{matrix}u_{x}\\ u_{y}\end{matrix}\right]$ as the direction from $A$ to $B$

• •

  A unit vector $\bm{v}$ orthogonal to $\bm{u}$ as $\bm{v}=\left[\begin{matrix}-u_{y}\\ u_{x}\end{matrix}\right]$

• •

  The length of $AB$ as $|AB|$

• •

  The midpoint of $AB$ as $M$, i.e. $M=A+\frac{|AB|}{2}\bm{u}$

• •

  The point $C$ as

  $\displaystyle C$

  $\displaystyle=$

  $\displaystyle\begin{cases}M+\frac{|AB|}{2}\frac{\bm{v}}{\tan\left(\frac{\gamma}{2}\right)}&\text{if }\gamma\neq k\pi$, $k\in\mathbb{Z}\\ M&\text{if }\gamma=\pi+k2\pi\end{cases}$

  (1)

If the initial and final directions are different, i.e., if $\gamma\neq k2\pi$, where $k\in\mathbb{Z}$, then $J$ can be selected from any of the points situated on the circle with its center at $C$ and a radius of $|AC|=|BC|$. An example of this is illustrated in Figure 3. Detailed proofs can be found in [56]. In case the directions are the same ($\gamma=k2\pi$), then $J$ can be chosen from any point along the line $AB$.

Figure 4a illustrates how various biarcs, corresponding to different choices of $J$, can be employed to transition from an initial position and orientation $A,\theta_{A}$ to a final position and orientation $B,\theta_{B}$ when $\gamma\neq k2\pi$. In contrast, Figure 4b demonstrates how different biarcs, corresponding to different choices of $J$, can be utilized to move from an initial position and orientation $A,\theta_{A}$ to a final position with the same orientation $B,\theta_{A}$.’

Next, we shall delve into the process of selecting the optimal $J$. To commence, we will introduce the following additional definitions:

• •

  $l_{A}=|\stackrel{{\scriptstyle\frown}}{{AJ}}|$ denotes the length of the arc from point $A$ to point $J$.

• •

  $l_{B}=|\stackrel{{\scriptstyle\frown}}{{JB}}|$ signifies the length of the arc from point $J$ to point $B$.

• •

  $l$ represents the total length of the biarc, i.e., $l=l_{A}+l_{B}$.

• •

  $r_{A}$ is defined as the signed radius of the arc from point $A$ to point $J$. A positive radius corresponds to counterclockwise rotation, while a negative radius corresponds to clockwise rotation.

• •

  $r_{B}$ is the signed radius of the arc from point $J$ to point $B$. A positive radius indicates counterclockwise rotation, and a negative radius indicates clockwise rotation.

• •

  $k_{A}$ is defined as the signed curvature of the arc from point $A$ to point $J$, calculated as $k_{A}=\frac{1}{r_{A}}$.

• •

  $k_{B}$ represents the signed curvature of the arc from point $J$ to point $B$, determined by the equation $k_{B}=\frac{1}{r_{B}}$.

Minimizing the curvature discontinuity at the joint point is important in order to mitigate the acceleration demands placed on the robot. Nevertheless, when determining the optimal placement of $J$ to achieve reduced curvature discontinuity, it is essential to concurrently consider the path length. Notably, choices of $J$ that yield decreased curvature discontinuity may inadvertently result in excessively lengthy paths. The ensuing analysis will comprehensively investigate the disparities in curvature and path lengths associated with different $J$ selections, and elucidate the underlying rationale for the choice of $J$ adopted in this study.

In Section III-A, we will scrutinize the non-degenerate scenario where $\gamma\neq k2\pi$. Subsequently, in Section III-B, we will delve into the analysis of the degenerate case, characterized by $\gamma=k2\pi$.

In most mobile robot applications, online replanning is a necessity due to various factors, including non-static obstacles in the environment and imperfections in the robot’s trajectory caused by sensing noise, slippage, terrain irregularities, and more. As a result, we assume periodic replanning from the robot’s current pose to the goal pose.

During replanning, it is highly desirable to keep the trajectory close to a previous plan. Drastic changes to the robot’s trajectory at each time step can lead to jerky movements. Therefore, the plan generated in the previous time step should serve as a reference for constructing the new plan.

At time $t_{i}$, a trajectory comprising a sequence of biarcs is generated. The first biarc, denoted as $b$, consists of two arcs, $\stackrel{{\scriptstyle\frown}}{{AJ}}$ and $\stackrel{{\scriptstyle\frown}}{{JB}}$, which guide the robot from its pose at $(A,\theta_{A})$ to its pose at $(B,\theta_{B})$. At time $t_{i+1}$, the robot is at pose $(A^{\prime},\theta_{A}^{\prime})$. The objective is to plan a new biarc, referred to as $b^{\prime}$, from $(A^{\prime},\theta_{A}^{\prime})$ to $(B,\theta_{B})$ in a manner that maintains proximity to the previous biarc $b$. Determining the closest biarc based on the squared distance between the two trajectories can be achieved through classical optimization techniques. However, this approach is computationally expensive. Therefore, we employ a heuristic method, as illustrated in Fig. 7.

Let $c_{A}$ represent the circle of which arc $\stackrel{{\scriptstyle\frown}}{{AJ}}$ is a part. If $AJ$ is a straight segment (with zero curvature), then $c_{A}$ denotes the line that contains $AJ$. Similarly, let $c_{B}$ denote the circle of which arc $\stackrel{{\scriptstyle\frown}}{{JB}}$ is a part or, in the case of $JB$ being a straight segment, the line defined by $J$ and $B$. In instances where $c_{B}$ represents a circle, refer to the center of $c_{B}$ as $C_{B}$.

Let us consider a potential candidate for a biarc $b^{\prime}$, designed to resemble the previous biarc $b$. This candidate biarc $b^{\prime}$ consists of arcs $\stackrel{{\scriptstyle\frown}}{{A^{\prime}J^{\prime}}}$ and $\stackrel{{\scriptstyle\frown}}{{J^{\prime}B}}$, with the condition that $J^{\prime}$ lies on $c_{B}$ and $J^{\prime}\neq B$. This approach represents a particular case of a heuristic introduced in the field of curve approximation by [58]. With this choice, the chosen biarc and the original biarc overlap in their second arc. In Fig. 7, the previously planned biarc $b$ is represented by a dashed line. Biarc $b^{\prime}$ consists of arc $A^{\prime}J^{\prime}$, illustrated by a solid line, and arc $J^{\prime}B$, which is a segment of the original arc $JB$.

Let $c^{\prime}$ represent the locus of points where $J^{\prime}$ can be situated. As explained in Section II, if $\theta_{A}^{\prime}\neq\theta_{B}+k2\pi$, then $c^{\prime}$ takes the form of a circle with the center $C^{\prime}$, defined in Eq. 1, passing through point $B$. However, if $\theta_{A}^{\prime}=\theta_{B}+k2\pi$, then $c^{\prime}$ is represented by the line $A^{\prime}B$. With our definition of a similar biarc, $J^{\prime}$ must lie within the intersection of $c_{B}$ and $c^{\prime}$. This intersection always includes at least one point, which is point $B$. In the case where both $c_{B}$ and $c^{\prime}$ are not degenerate and $C^{\prime}\neq C_{B}$, the position of $J^{\prime}$ can be calculated as follows:

The example depicted in Fig. 7 illustrates this situation. If both $c_{B}$ and $c^{\prime}$ are circular and concentric, then $c_{B}$ coincides with $c^{\prime}$, making any point $J^{\prime}\in c^{\prime}$ a valid choice. In this scenario, we select $J^{\prime}$ corresponding to $\alpha=0$.

If $c_{B}$ takes the form of a line, while $c^{\prime}$ is a circle, then $J^{\prime}$ corresponds to the reflection of point $B$ over an axis that passes through $C^{\prime}$ and is perpendicular to the line $JB$. This operation can be computed efficiently as

where $\bm{u_{b}}$ represents the unit vector in the direction of $JB$. An illustrative example can be found in Figure 8a.

In a similar fashion, if $c_{B}$ is a circle and $c^{\prime}$ is a line, then $J^{\prime}$ can be determined as the reflection of point $B$ over an axis that passes through $C_{B}$ and is orthogonal to the line $A^{\prime}B$. The computation for finding $J$ in this case is also straightforward:

where $\bm{u^{\prime}}$ is the unit vector in the direction $A^{\prime}B$. An example is presented in Figure 8b.

If both $c_{B}$ and $c^{\prime}$ are lines, their only point of intersection is $B$, rendering the existence of $b^{\prime}$ impossible. In this case, $J$ is determined using Eq. 23.

In most cases, selecting the biarc $b^{\prime}$ using the heuristic defined above results in good biarcs that resemble the original trajectory $b$. However, there are instances where the obtained biarc is excessively long or contains arcs with very high curvature, as illustrated in Fig 9. If the biarc $b=(\stackrel{{\scriptstyle\frown}}{{AJ}},\stackrel{{\scriptstyle\frown}}{{JB}})$ (depicted by a dashed line) is chosen as a reference for generating a biarc from $A^{\prime}$ to $B$, the solid line biarc $b^{\prime}=(\stackrel{{\scriptstyle\frown}}{{A^{\prime}J^{\prime}}},\stackrel{{\scriptstyle\frown}}{{J^{\prime}B}})$ is obtained. Conversely, when a biarc is planned from $A^{\prime}$ to $B$ using $\alpha=0$, akin to the initial planning, the much more favorable dotted biarc $b^{\prime\prime}=(\stackrel{{\scriptstyle\frown}}{{A^{\prime}J^{\prime\prime}}},\stackrel{{\scriptstyle\frown}}{{J^{\prime\prime}B}})$ is obtained.

Therefore, during the replanning process, a comparative analysis is conducted between the length and curvature difference of biarc $b^{\prime}$ and those of biarc $b^{\prime\prime}$. The length of $b^{\prime\prime}$ can be calculated using Equations 12 and 13 when $\gamma\neq k2\pi$, or Equation 25 when $\gamma=k2\pi$. Similarly, the curvature difference of $b^{\prime\prime}$ is determined using Equation 16 when $\gamma\neq k2\pi$, and Equation 28 when $\gamma=k2\pi$. To decide between choosing $b^{\prime}$ and $b^{\prime\prime}$, we introduce two parameters, $\eta_{l}$ and $\eta_{k}$, both greater than 1. The selection criterion is based on the conditions $l^{\prime}>\eta_{l}\cdot l^{\prime\prime}$ or $|k_{B}^{\prime}-k_{A}^{\prime}|>\eta_{k}\cdot|k_{B}^{\prime\prime}-k_{A}^{\prime\prime}|$. In our experimental evaluations, we determined that setting $\eta_{l}=\eta_{k}=2$ yields favorable results.

Biarcs were selected as motion primitives for a specific reason—this type of trajectory lends itself to efficient collision detection. When a generic curve is employed as a motion primitive, it necessitates sampling the robot’s position along the trajectory and subsequently calculating whether a collision occurs at each sampled point. As a result, the computational time scales roughly in proportion to the path length and the sampling resolution.

The primary benefit of using biarcs as motion primitives is the elimination of this sampling requirement. Collision detection with obstacles can be carried out through closed-form computations.

Specifically, collision detection for a biarc trajectory can be executed by checking for collisions in each of its two constituent arcs. Let’s consider a single arc commencing at position $A_{i}$ with orientation $\theta_{A_{i}}$ and terminating at position $B_{i}$ with orientation $\theta_{B_{i}}$. In the case of a non-degenerate arc, we define the center of rotation as $C_{i}$ and the spanned angle as $\theta_{d_{i}}=\theta_{B_{i}}-\theta_{A_{i}}$.

For the subsequent discussions, we assume that a convex polygonal hitbox is utilized to represent the robot. If the hitbox is concave, it can be subdivided into convex hitboxes, and the collision check can be performed for each of these components.

This hitbox can be defined by its vertices as $H_{j}$, where $0\leq j<N_{h}$ when the robot is located at position $A_{i}$. We can further define $H_{N_{h}}=H_{0}$, and denote each edge as $H_{j}H_{j+1}$.

We make the assumption that obstacles in the environment are static. This assumption is supported by the characteristics of our setup, where collision detection can be performed within a single LIDAR scan (taking 50ms), and the robot operates at relatively low speeds (6 km/h).

We investigate collision scenarios involving the movement of the robot’s hitbox along an arc trajectory and its interaction with two types of obstacles. Section V-A focuses on point obstacles, which are a natural representation for LIDAR data. Section V-B addresses collisions with line (and line segments) obstacles, serving as a foundation for assessing collision with polygonal obstacles, lane edges, walls, and similar structures.

It is worth noting that when the map is known in advance and static obstacles can be represented as polygonal objects, LIDAR data collected during robot navigation can be selectively omitted from collision detection if they fall inside or in close proximity to the edges of these obstacles. This optimization can significantly accelerate the collision evaluation process.