Fast Path-Planning Algorithm for Mobile Robots via Straight Strategy

Abstract

RRT*, which augments the RRT with the $\mathrm{C}\mathrm{h}\mathrm{o}\mathrm{o}\mathrm{s}\mathrm{e}\mathrm{P}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t}$ and Rewire steps, is a widely used algorithm for path planning. RRT*-based algorithms are effective for improving the quality of paths in the Rewire step. However, as the expansion continues, there are more nodes to be inspected, which can slow down the path search. In addition, due to the parameters set in the rewire step, a trade-off issue between path quality and planning time arises. In this paper, we propose Straight-RRT to improve upon these limitations. Straight-RRT applies the Straight strategy to rapidly obtain an initial path and then returns a refined path using the MoveParent strategies. Accordingly, Straight-RRT adopts the following mechanisms. (1) The Straight strategy is applied for rapidly finding an initial path. This procedure quickly finds a feasible initial path. (2) MoveParent is a strategy inspired by parametric equations for path optimization. This complements the weaknesses of the Straight strategy and the limitations of the triangle inequality. The MoveParent strategy is applied bidirectionally. These procedures progressively refine the path and improve efficiency. We propose an algorithm that is faster than other algorithms using these strategies and minimizes the trade-off caused by parameter settings. In the experimental comparison results across most environments, our approach achieved shorter planning times than the compared baseline algorithms and produced paths of comparable quality.

1. Introduction

Driven by rapid advances in science and technology, the era of human–robot coexistence is approaching. In everyday life, mobile robots can be found in various forms, including patrol, service, delivery, inspection, and agricultural robots. Accordingly, path planning for mobile robots is becoming increasingly important. For a mobile robot, path planning refers to computing a path from a start position to a goal position. The path must be collision-free and of as low a cost as possible. In addition, mobile robots must respond to user commands and environmental changes without delay, and they must be able to plan paths rapidly to cope with unforeseen dynamic obstacles during navigation. In essence, path planning must be able to generate efficient paths quickly.

Path planning can be broadly classified into classical, intelligent, and other approaches [1]. The classical category includes A* search, artificial potential fields (APFs), probabilistic roadmaps (PRMs), and rapidly exploring random trees (RRTs). The proposed algorithm is a variant of RRT, one of the classical path-planning approaches. RRT-based algorithms have several advantages.

Since RRT* introduced the Rewire strategy, most variants have adopted a Rewire process because of its benefits. However, rewiring depends on the hyperparameter settings. As a result, it has the drawback of reduced generalization [2]. In addition, the Rewire strategy requires considering an increasing number of nodes as the tree grows, which causes the planning time to be delayed as time goes on. That is, the existing algorithm has limitations due to planning time delay caused by rewiring and due to hyperparameters. The planning time delay can be dangerous in environments where the robot must respond immediately. For example, if a fast-moving obstacle is approaching from the opposite side, the robot must be able to quickly plan a path to avoid it. In addition, there are many different environments, and if the hyperparameters must be set differently for each environment, generalization becomes weak.

Therefore, to address these issues, the objectives of this study are as follows. First, we achieve faster planning times than existing algorithms to enable rapid path generation in response to motion commands and unexpected events. Second, we minimize parameter-dependent trade-offs by eliminating the rewiring step. To achieve the above goals, we propose an RRT variant without rewiring that plans faster while producing paths of comparable quality. The proposed algorithm quickly computes a drivable path in the early stage. The path is then post-processed to shorten it. The existing method adds a node at a point that is a fixed distance from ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{t}}$ in the direction of ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$. However, if there is an obstacle in this process, the computational cost used for collision-checking and the computational cost used for sampling are wasted. Unlike the existing method, the proposed strategy finds the last collision-free node position toward ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ direction. Then, it adds the midpoint between that position and ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{t}}$ to the tree. One representative post-processing method used in existing approaches is based on the triangle inequality. However, it cannot be applied if there is even a very small obstacle between node [i] and node [i+2]. The proposed strategy uses a parametric equation such that, if a direct connection from node [i] to node [i+2] is not possible, it updates the position of node [i+1] to a location as close as possible to node [i+2]. With this strategy, we aim to reduce reliance on rewiring in prior studies and to generate a comparable or better path while computing it more efficiently.

To evaluate the validity of the proposed algorithm, we set “length of path,” “cumulative heading change,” and “planning time” as the evaluation criteria. Length of path is an indicator used to determine how short the travel distance is. Cumulative heading change represents the total amount of direction change required during the robot’s path-following process, thereby indirectly reflecting the burden of rotational motion. Planning time is an indicator used to assess the feasibility of a real-time application in an environment with limited computation time. Based on the experimental results, it was overall superior to all comparison algorithms. Although the path quality was slightly lower than that of other algorithms, the planning time was noticeably reduced. That is, it shows a reduced planning time while maintaining similar path quality, even as it alleviates parameter dependence and removes the rewiring process. This means that, when applied to a mobile robot, it can respond to user commands or environmental changes with much less delay than other algorithms.

The remainder of this paper is organized as follows. Section 2 introduces related work on existing RRT-based algorithms. Section 3 describes the mechanisms of the comparison algorithms, Q-RRT*, F-RRT*, and MQ-RRT*, the reasons for selecting these algorithms, and the limitations of each algorithm. Section 4 presents the core of the proposed algorithm, focusing on the mechanisms of the Straight strategy and the MoveParent strategy. Section 5 presents experiments conducted using two scenarios across three different environments. Specifically, it describes the comparative performance evaluation and results of the algorithms based on three evaluation metrics. Section 6 discusses the conclusion, the limitations of the study, and future directions.

2. Related Works

In this section, we introduce several variants of RRT-based algorithms. Compared with many path planners, RRT offers key advantages, such as rapid exploration, robustness in obstacle-dense environments, and efficiency in high-dimensional spaces [3]. Despite these advantages, RRT often yields high-cost, non-smooth paths. Consequently, there has been active research aimed at improving path quality and reducing cost.

In RRT–Connect, once a tree creates $x_{new}$, the opposite tree repeatedly extends toward it via Connect, which typically yields faster path-finding than the standard RRT [4]. In RRT–Connect, the extend strategy grows toward ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$, and when an obstacle is detected, it stops the tree expansion without adding the node. It then performs another sampling step and continues expanding the tree by extending again toward the newly sampled ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{w}}$. Encountering an obstacle or wall during this expansion causes the growth toward the sampled node to terminate, after which the algorithm resamples and resumes tree expansion. This repeated stopping and resampling process results in wasted computational cost. The connect strategy can rapidly meet the tree on the opposite side because it extends as far as possible only in the direction of ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{w}}$. However, this process can reduce exploration capability, leading to search bias and, consequently, degrading space coverage. To mitigate the computational cost issues of the extend strategy and the reduced exploration of the connect strategy, the proposed strategy adds the midpoint to the tree after extending as far as possible. In RRT–Connect, after one tree expands, the opposite tree executes the connect strategy. During this process, it repeatedly attempts to reach ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{w}}$ by extending toward it until it is reached or trapped. In contrast, the proposed strategy checks connectivity only at regular intervals for the expanding tree. This approach can reduce computational cost by avoiding repeated nearest-neighbor searches and collision-checking steps with the identified nearby node.

An Anytime RRTs approach has been proposed that first rapidly finds an executable path and then incrementally improves its cost within the permitted time. This yields paths of higher quality than the standard RRT [5]. Similarly, the RRT–Back method first finds a path and applies shortcutting based on the triangle inequality [6]. This applies the triangle inequality to the completed path starting from the goal position. The proposed MoveParent strategy using a parametric equation is performed bidirectionally, and it can also be applied to segments where the triangle inequality cannot optimize the path.

RRT* was proposed, applying the ChooseParent and Rewire strategies to RRT to lower the path cost. It has been shown to converge to the optimal path as the number of samples increases. This addresses the limitation of RRT, which does not guarantee convergence to an optimal solution [7]. However, although RRT* guarantees optimality, it often suffers from long planning times. Consequently, numerous RRT*-based variants have been proposed to improve path quality and shorten the planning time.

When the robot is moving, RT-RRT* improves real-time performance by adopting an online tree-rewiring strategy that relocates the tree’s root along with the robot [8]. AM-RRT*, which extends RT-RRT* by employing a diffusion-distance-based assisting metric to reduce path cost and planning time, was proposed [9]. Fast AM-RRT* extends AM-RRT* by adding distance and directionality weighting and adopting a weight-based node selection strategy and bidirectional tree expansion. This yields faster planning [10]. The method applies Ant Colony Optimization (ACO) to maintain an ant table that preserves promising samples and to generate the next samples from a Gaussian mixture model. This adaptively refines the sampling distribution [11]. To address planning time slowdowns and excessive node generation in RRT and RRT*, a strategy was proposed that guides tree growth using intermediate guide nodes derived from the Generalized Voronoi Diagram (GVD) [12]. To improve RRT’s low sampling efficiency, GEO-RRT* was proposed [13]. It combines explored-region rejection with obstacle-information-based expansion to reduce unnecessary sampling and expansion failures, thereby accelerating exploration and improving path quality. Direct-DRRT* applies a direct heuristic after the Rewire step to link the nearest node to the sampled node and the sampled node to the goal with straight lines when both segments are collision-free [14]. However, this method relies on rewiring. Accordingly, it is difficult to address the time delay that increases as the tree grows and the parameter dependence. Attempts have also been made to extend Direct-DRRT* from 2D to 3D [15]. Informed-RRT* was proposed [16]. It first quickly finds a path using RRT and then samples only within an admissible region to optimize the path.

RRT*–Smart was proposed [17]. It uses intelligent sampling and the triangle inequality to generate paths rapidly. RRT*–Smart showed that the triangle inequality can optimize paths quickly and effectively. After that, several algorithms based on the triangle inequality were proposed. Improved RRT–Connect was proposed. It introduces a rewiring strategy that applies the triangle inequality [18]. Q-RRT* was proposed [19]. During ChooseParent, it considers the ancestry of neighboring nodes to select a parent. F-RRT* was proposed, mentioning the drawbacks of the ChooseParent step and introducing the FindReachest and CreateNode strategies to improve it [20]. MQ-RRT* was proposed to improve sampling efficiency by introducing a sparse sampling strategy and a dynamically goal-biased strategy using a sigmoid function and adopting the RemoveTips and CreateNode strategies [21]. FHQ-RRT* was proposed as an extension of MQ-RRT*, assuming the availability of sensor-preprocessed information for $c_{obs}$ and $c_{free}$ [22]. It dynamically changes the sparse sampling, and it introduces a NewRewire strategy that excludes co-origin nodes from the optimization during the Rewire process. A summary of the principal algorithms is provided in

Table 1. Overview of major RRT-based algorithms.

Algorithms	Principle
RRT*–Smart	After finding an initial feasible path, the path is optimized using triangle inequality-based PathOptimization. Then, path optimization is carried out by repeatedly performing IntelligentSample using the nodes of the optimized path as biasing points.
Q-RRT*	We expand the parent candidate set beyond the RRT* near set (${\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$) by including the ancestry of nearby vertices. This enables improved parent selection. The same expansion is applied to the rewiring.
F-RRT*	This algorithm is intended to reduce the impact of $r_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$. For tree expansion, when adding ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$, it introduces FindReachest (finding a reachable parent via ancestor search) instead of $r_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$. In addition, it generates the parent of ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ using a bisection-based CreateNode strategy. However, this process introduces an additional parameter, ${\mathrm{D}}_{\mathrm{d}\mathrm{i}\mathrm{c}\mathrm{h}\mathrm{o}\mathrm{t}\mathrm{o}\mathrm{m}\mathrm{y}}$.
MQ-RRT*	Planning time performance was improved through sparse sampling and dynamic goal bias. A bisection-based CreateNode and RemoveTips were introduced to generate new parent nodes. As a result, the method reduces path cost and improves smoothness. However, an additional parameter ${\mathrm{R},}_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{m}\mathrm{i}\mathrm{t}}$ is introduced, and performance varies depending on its value.
Straight-RRT (Proposed)	The Straight strategy does not use a rewiring process during the tree expansion stage, thereby generating an initial feasible path very quickly. This reduces dependence on nearby-neighbor search parameters, such as $r_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$, which affect performance and computational cost in RRT*-based methods, and alleviates the issue that the computational burden increases over time due to repeated neighbor searches and collision checks. Subsequently, the MoveParent post-processing improves the path by reconstructing node connections using the principle of a parametric equation, effectively enhancing the path length.

.

3. Overview of Comparator Algorithms

This section provides a brief review of the comparator algorithms Q-RRT*, F-RRT*, and MQ-RRT*. We selected these algorithms because they commonly refine the path based on the triangle inequality, which is known to reduce planning time and improve path quality compared with standard RRT*. The notation used throughout the paper is summarized in

Table 2. The definition of notations.

Notation	Description
$x_{start}$	Start position
$x_{goal}$	Goal position
$C_{free}$	The collision-free region of the configuration space with respect to the map.
$C_{obs}$	The obstacle region of the configuration space with respect to the map
$T_{limit}$	Limit path planning time
$l_{cc}$	Collision-check step size
$x_{rand}$	Randomly sampled configuration in the configuration space
$x_{new}$	$\mathrm{New} \mathrm{node} (\mathrm{steered} \mathrm{from} x_{nearest}$ $\mathrm{toward} x_{rand}$)
$x_{nearest}$	$\mathrm{The} \mathrm{nearest} \mathrm{tree} \mathrm{node} \mathrm{to} x_{rand}$
$r_{near}$	Rewiring radius
$X_{near}$	$\mathrm{The} \mathrm{set} \mathrm{of} \mathrm{nodes} \mathrm{within} \mathrm{distance} r_{near} \mathrm{o}\mathrm{f} x_{new}$
$D_{ancestry}$	Depth parameter when use in Q-RRT*
${\mathrm{D}}_{\mathrm{d}\mathrm{i}\mathrm{c}\mathrm{h}\mathrm{o}\mathrm{t}\mathrm{o}\mathrm{m}\mathrm{y}}$	Dichotomy parameter used in F-RRT*
${\mathrm{R}}_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{m}\mathrm{i}\mathrm{t}}$	Sparse-sampling commit radius used in MQ-RRT*

.

3.1. Q-RRT*

Q-RRT* was proposed to address the trade-off between planning time and path quality induced by the rewiring radius ${\mathrm{r}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$ in RRT*. The key idea is to expand the candidate parent set in the ChooseParent and Rewire steps from only ${\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$ to include their ancestors as well, thereby reducing overall planning time while maintaining solution quality. The Q-RRT* pseudocode is presented in Algorithms 1 and 2.

Algorithm 1. Pseudocode of the Q-RRT* algorithm.

Input: $x_{start} , x_{goal} , C_{free} , C_{obs} ,T_{limit} , r_{near} , D_{ancestry}$ Output: Tree Initialize: $\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}\leftarrow \mathrm{N}\mathrm{u}\mathrm{l}\mathrm{l} \mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e} , \left(T_{now} , T_{start}\right) \leftarrow \mathrm{I}\mathrm{n}\mathrm{i}\mathrm{t}\mathrm{i}\mathrm{a}\mathrm{l}\mathrm{i}\mathrm{z}\mathrm{e}$ Begin Q-RRT* Procedure 1    $\mathrm{Tree} \leftarrow$ Insert Root Node $x_{start}$ $2 \mathbf{While} T_{now}-T_{start}\le T_{limit}$ do $3 x_{rand}\leftarrow$ RandomSampling() $4 x_{nearest}\leftarrow$ Nearest($x_{rand}$) $5 x_{new}\leftarrow$ Steer($x_{nearest}, x_{rand}$) 6        If CollisionFree$\left(x_{nearest} , x_{new}\right)$ then 7            $X_{near}\leftarrow$ Near($x_{new}, r_{near}$) 8            $X_{parent}\leftarrow$ Ancestry($X_{near}, D_{ancestry}$) 9            $x_{parent}\leftarrow$ ChooseParent$\left(X_{near}\cup X_{parent} , x_{nearest} , x_{new}\right)$ 10          $\mathrm{Tree} \leftarrow$ Connect(Tree,$x_{parent}, x_{new}$) 11          $\mathrm{Tree} \leftarrow$ Rewire-Q-RRT*(Tree, $x_{new}, X_{near}$) 12      If Distance$\left(x_{new}, x_{goal}\right)<goa{l}_{threshold}$ then 13          Return Tree 14      $T_{now} \leftarrow UpdateTime\left(\right)$ End Q-RRT*

Algorithm 2. Pseudocode of the Rewire-Q-RRT*.

Input: Tree,$x_{new} , X_{near}$ Output: Tree Begin Rewire-Q-RRT* Procedure $1 \mathbf{for} x_{near}$$\mathbf{in} X_{near}$ 2            $\mathbf{If} \mathrm{C}\mathrm{o}\mathrm{s}\mathrm{t}\left(x_{new}\right)+\mathrm{D}\mathrm{i}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}\left(x_{new} , x_{near}\right) <\mathrm{C}\mathrm{o}\mathrm{s}\mathrm{t}\left(x_{near}\right)$ then 3                $\mathbf{If} \mathrm{C}\mathrm{o}\mathrm{l}\mathrm{l}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}\mathrm{F}\mathrm{r}\mathrm{e}\mathrm{e}\left(x_{new}, x_{near}\right)$ then 4                    $\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}\leftarrow \mathrm{R}\mathrm{e}\mathrm{c}\mathrm{o}\mathrm{n}\mathrm{n}\mathrm{e}\mathrm{c}\mathrm{t}\left(\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e} , x_{new} , x_{near}\right)$ 5    Return Tree End Rewire-Q-RRT* from Q-RRT*

In RRT*, the ChooseParent step considers only ${\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$ when selecting a parent for ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{w}}$. In contrast, Q-RRT* considers not only ${\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$ but also their ancestors, with the maximum ancestry depth determined by ${\mathrm{D}}_{\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}\mathrm{s}\mathrm{t}\mathrm{r}\mathrm{y}}$. Once the candidate parent set ${\mathrm{X}}_{\mathrm{p}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t}}$ has been obtained through the preceding steps, ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{w}}$ selects the optimal parent ${\mathrm{x}}_{\mathrm{p}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t}}\in X_{parent}$ and performs connect. Subsequently, the Rewire step is performed on the neighborhood. For each node in ${\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$, if a lower-cost collision-free connection via ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{w}}$ or any of ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{w}}$’s ancestors exists, it updates its parent using Reconnect. We selected Q-RRT* as the baseline because it substantially mitigates the limitations of RRT*. An overview of the overall algorithmic pipeline is provided in the flowchart in Figure 1.

3.2. F-RRT*

F-RRT* reduces the trade-off caused by ${\mathrm{r}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$. It replaces the ${\mathrm{r}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$-based ChooseParent step with two strategies called FindReachest and CreateNode. It aims to optimize path quality by generating nodes near obstacle boundaries. The F-RRT* pseudocode is presented in Algorithms 3 and 4.

Algorithm 3. Pseudocode of the F-RRT* algorithm.

Input: ${\mathrm{x}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}},x_{goal},C_{Free},C_{obs},T_{limit}, r_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}, D_{dichotomy}$ Output: Tree Initialize: $\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}\leftarrow \mathrm{N}\mathrm{u}\mathrm{l}\mathrm{l}\mathrm{ }\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}$,$\left(T_{now} , T_{start}\right) \leftarrow \mathrm{I}\mathrm{n}\mathrm{i}\mathrm{t}\mathrm{i}\mathrm{a}\mathrm{l}\mathrm{i}\mathrm{z}\mathrm{e}$ Begin F-RRT* Procedure $1 \mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}\mathrm{ }\leftarrow \mathrm{I}\mathrm{n}\mathrm{s}\mathrm{e}\mathrm{r}\mathrm{t}\mathrm{ }\mathrm{R}\mathrm{o}\mathrm{o}\mathrm{t}\mathrm{ }\mathrm{N}\mathrm{o}\mathrm{d}\mathrm{e} x_{start}$ $2 \mathbf{While} T_{now}-T_{start}\le T_{limit}$ do $3 x_{rand}$←$\mathrm{R}\mathrm{a}\mathrm{n}\mathrm{d}\mathrm{o}\mathrm{m}\mathrm{S}\mathrm{a}\mathrm{m}\mathrm{p}\mathrm{l}\mathrm{i}\mathrm{n}\mathrm{g}\left(\right)$ $4 x_{nearest}\leftarrow \mathrm{N}\mathrm{e}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{t}\left(x_{rand}\right)$ $5 \mathbf{If} \mathrm{C}\mathrm{o}\mathrm{l}\mathrm{l}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}\mathrm{F}\mathrm{r}\mathrm{e}\mathrm{e}\left(x_{nearest} , x_{rand}\right)$ then $6 X_{near}\leftarrow \mathrm{N}\mathrm{e}\mathrm{a}\mathrm{r}\left( x_{rand} , r_{near}\right)$ $7 x_{reachest}\leftarrow \mathrm{F}\mathrm{i}\mathrm{n}\mathrm{d}\mathrm{R}\mathrm{e}\mathrm{a}\mathrm{c}\mathrm{h}\mathrm{e}\mathrm{s}\mathrm{t}(\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e} , x_{nearest},x_{rand})$ $8 x_{create}\leftarrow \mathrm{C}\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{t}\mathrm{e}\mathrm{N}\mathrm{o}\mathrm{d}\mathrm{e}\left(\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e},{\mathrm{x}}_{\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{c}\mathrm{h}\mathrm{e}\mathrm{s}\mathrm{t}} ,x_{rand} ,D_{dichotomy}\right)$ $9 \mathbf{If} x_{create}$ is not Null then $10 \mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}.\mathrm{I}\mathrm{n}\mathrm{s}\mathrm{e}\mathrm{r}\mathrm{t}\mathrm{N}\mathrm{o}\mathrm{d}\mathrm{e}\left(x_{create} , x_{rand}\right)$ 11          Else $12 \mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}.\mathrm{I}\mathrm{n}\mathrm{s}\mathrm{e}\mathrm{r}\mathrm{t}\mathrm{N}\mathrm{o}\mathrm{d}\mathrm{e}\left(x_{rand}\right)$ $13 \mathbf{If} \mathrm{D}\mathrm{i}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}\left(x_{new} , x_{goal}\right)<goa{l}_{threshold}$ then 14              Return Tree $15 \mathrm{Rewire}(\mathrm{Tree} ,x_{rand}$ $,X_{near}$) 16      $T_{now} \leftarrow UpdateTime\left(\right)$ End F-RRT*

Algorithm 4. Pseudocode of the CreateNode.

Input: $\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}, x_{reachest},x_{rand},D_{dichotomy}$ Output: $x_{create}$ Begin CreateNode Procedure $1 x_{allow}\leftarrow x_{reachest}$ $2 \mathbf{If} x_{allow} \mathrm{i}\mathrm{s} \mathrm{n}\mathrm{o}\mathrm{t} x_{start}$ then $3 x_{forbid}\leftarrow \mathrm{P}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t}\left(x_{reachest}\right)$ 4        While $\mathrm{Distance}\left(x_{allow},x_{forbid}\right)>D_{dichotomy}$ do $5 x_{mid}\leftarrow \mathrm{M}\mathrm{i}\mathrm{d}\mathrm{d}\mathrm{l}\mathrm{e}\mathrm{N}\mathrm{o}\mathrm{d}\mathrm{e}\left(x_{allow},x_{forbid}\right)$ $6 \mathbf{If} \mathrm{CollisionFree}(x_{rand},x_{mid})$ then $7 x_{allow}\leftarrow x_{mid}$ 8            Else $9 x_{forbid}\leftarrow x_{mid}$ $10 x_{forbid}\leftarrow x_{rand}$ $11 \mathbf{While} \mathrm{D}\mathrm{i}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}\left(x_{allow} ,x_{forbid}\right)>D_{dichotomy}$ do $12 x_{mid}\leftarrow \mathrm{M}\mathrm{i}\mathrm{d}\mathrm{d}\mathrm{l}\mathrm{e}\mathrm{N}\mathrm{o}\mathrm{d}\mathrm{e}\left(x_{allow} ,x_{forbid}\right)$ $13 \mathbf{If} \mathrm{CollisionFree}(x_{mid},\mathrm{P}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t}\left(x_{reachest}\right))$ then $14 x_{allow}\leftarrow x_{mid}$ 15          Else $16 x_{formid}\leftarrow x_{mid}$ $17 \mathbf{If} x_{allow} \mathrm{i}\mathrm{s} \mathrm{n}\mathrm{o}\mathrm{t} x_{reachest}$ then $18 x_{create}\leftarrow x_{allow}$ 19  Else $20 x_{create} \mathrm{i}\mathrm{s} \mathrm{N}\mathrm{u}\mathrm{l}\mathrm{l}$ $21 \mathbf{Return} x_{create}$ End CreateNode

FindReachest find ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{t}}$, the nearest neighbor of ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ in the tree. It then ascends the ancestor chain of ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{t}}$ while the straight-line segment to ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ remains collision- free. The highest ancestor reachable under this condition is returned and denoted as ${\mathrm{x}}_{\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{c}\mathrm{h}\mathrm{e}\mathrm{s}\mathrm{t}}$.

CreateNode locally optimizes the subpath $\mathrm{P}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t}\left({\mathrm{x}}_{\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{c}\mathrm{h}\mathrm{e}\mathrm{s}\mathrm{t}}\right)\to x_{reachest}\to x_{rand}$. This step sets the upper bound of ${\mathrm{x}}_{\mathrm{c}\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{t}\mathrm{e}}$ to ${\mathrm{x}}_{\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{c}\mathrm{h}\mathrm{e}\mathrm{s}\mathrm{t}}$ and, in turn, assigns the lower bound to $\mathrm{P}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t}\left({\mathrm{x}}_{\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{c}\mathrm{h}\mathrm{e}\mathrm{s}\mathrm{t}}\right)$ and ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$. And a binary search is applied until the distance between the upper and lower bounds is below ${\mathrm{d}}_{\mathrm{d}\mathrm{i}\mathrm{c}\mathrm{h}\mathrm{o}\mathrm{t}\mathrm{o}\mathrm{m}\mathrm{y}}$. If the interval is less than ${\mathrm{d}}_{\mathrm{d}\mathrm{i}\mathrm{c}\mathrm{h}\mathrm{o}\mathrm{t}\mathrm{o}\mathrm{m}\mathrm{y}}$, it assigns the upper bound to ${\mathrm{x}}_{\mathrm{c}\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{t}\mathrm{e}}$. Then the tree connects ${\mathrm{x}}_{\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{c}\mathrm{h}\mathrm{e}\mathrm{s}\mathrm{t}}$ to ${\mathrm{x}}_{\mathrm{c}\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{t}\mathrm{e}}$ and ${\mathrm{x}}_{\mathrm{c}\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{t}\mathrm{e}}$ to ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$. And if ${\mathrm{x}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}$ is not reached, it performs Rewire on ${\mathrm{X}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$. Although CreateNode boosts optimization, both sensitivity to ${\mathrm{d}}_{\mathrm{d}\mathrm{i}\mathrm{c}\mathrm{h}\mathrm{o}\mathrm{t}\mathrm{o}\mathrm{m}\mathrm{y}}$ and the trade-off remain [23]. In other words, attempts to reduce dependence on ${\mathrm{r}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$ resulted in a ${\mathrm{d}}_{\mathrm{d}\mathrm{i}\mathrm{c}\mathrm{h}\mathrm{o}\mathrm{t}\mathrm{o}\mathrm{m}\mathrm{y}}$ induced speed–quality trade-off. F-RRT* was selected as a comparison algorithm because it is faster than RRT* and produces higher-quality solutions. An overview of the overall algorithmic pipeline is provided in the flowchart in Figure 2.

3.3. MQ-RRT*

MQ-RRT* introduces sparse sampling, a sigmoid function-based dynamic goal bias, RemoveTips, and CreateNode, and yields significant speed improvements over the two earlier algorithms. Sparse sampling is a strategy that discards ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ if the tree already has a node within an ${\mathrm{r}}_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{m}\mathrm{i}\mathrm{t}}$ radius of ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$. This enables more efficient exploration of the search space. Although sparse sampling contributes to much faster performance than the two preceding algorithms, its performance can degrade in narrow passages depending on the choice of ${\mathrm{R}}_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{m}\mathrm{i}\mathrm{t}}$. Additionally, the goal bias is adjusted dynamically using a sigmoid function and $n_1$. $n_1$ indicates whether the region is obstacle-dense. $n_1$ increases when ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ is collision-free, and accordingly, the output of the sigmoid function increases. This, in turn, raises the probability of sampling ${\mathrm{x}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}$ in the next sampling step. Conversely, when collisions are frequent, $n_1$ decreases, the sigmoid function returns a lower value, and the probability of sampling ${\mathrm{x}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}$ decreases.

RemoveTips applies the principle of dichotomy to smooth sharp points along the path Parent(${\mathrm{x}}_{\mathrm{p}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t}}$)→${\mathrm{x}}_{\mathrm{p}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t}}$→${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$. After removing the sharp points and obtaining ${\mathrm{x}}_{\mathrm{c}\mathrm{o}\mathrm{r}\mathrm{n}\mathrm{e}\mathrm{r}\mathrm{A}}$ and ${\mathrm{x}}_{\mathrm{c}\mathrm{o}\mathrm{r}\mathrm{n}\mathrm{e}\mathrm{r}\mathrm{B}}$, it further applies the principle of dichotomy in the CreateNodes procedure to produce a more optimized path that stays close to obstacles. If successful, we obtain the path (parent(${\mathrm{x}}_{\mathrm{p}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t}}$), ${\mathrm{x}}_{\mathrm{c}\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{t}\mathrm{e}\mathrm{A}}$, ${\mathrm{x}}_{\mathrm{c}\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{t}\mathrm{e}\mathrm{B}}$, ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$) instead of the direct (parent(${\mathrm{x}}_{\mathrm{p}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t}}$), ${\mathrm{x}}_{\mathrm{p}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t}}$) segment. Because it computes paths much faster than RRT* by applying sparse sampling and a dynamic goal bias, we selected MQ-RRT* as a comparison algorithm. An overview of the overall algorithmic pipeline is provided in the flowchart in Figure 3. The MQ-RRT* pseudocode is presented in Algorithm 5.

Algorithm 5. Pseudocode of the MQ-RRT*.

Input: $x_{start},x_{goal},C_{Free},C_{obs},T_{limit}, r_{near},R_{commit}, D_{dichotomy},D_{ancestor}$ Output: Tree Initialize: $\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}\leftarrow \mathrm{N}\mathrm{u}\mathrm{l}\mathrm{l}\mathrm{ }\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}$ ${\mathrm{n}}_1\leftarrow 0$ $\mathrm{I}\mathrm{t}\mathrm{e}\mathrm{r}\mathrm{C}\mathrm{o}\mathrm{u}\mathrm{n}\mathrm{t}\leftarrow 0$ $\mathrm{S}\mathrm{i}\mathrm{g}\mathrm{m}\mathrm{o}\mathrm{i}\mathrm{d}\left({\mathrm{n}}_1\right)\leftarrow 1/(1+\mathrm{e}\mathrm{x}\mathrm{p}(-(n_1-5)\left)\right)$ $\left(T_{now} , T_{start}\right) \leftarrow \mathrm{I}\mathrm{n}\mathrm{i}\mathrm{t}\mathrm{i}\mathrm{a}\mathrm{l}\mathrm{i}\mathrm{z}\mathrm{e}$ Begin MQ-RRT* Procedure $1 \mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}\mathrm{ }\leftarrow \mathrm{I}\mathrm{n}\mathrm{s}\mathrm{e}\mathrm{r}\mathrm{t}\mathrm{ }\mathrm{R}\mathrm{o}\mathrm{o}\mathrm{t}\mathrm{ }\mathrm{N}\mathrm{o}\mathrm{d}\mathrm{e} x_{start}$ 2    $\mathbf{While} T_{now}-T_{start}\le T_{limit}$ do 3        $\mathbf{I}\mathbf{f} \mathrm{R}\mathrm{a}\mathrm{n}\mathrm{d}\left(\right)<\mathrm{S}\mathrm{i}\mathrm{g}\mathrm{m}\mathrm{o}\mathrm{i}\mathrm{d}\left({\mathrm{n}}_1\right)$ then 4            $x_{rand}\leftarrow x_{goal}$ 5        Else 6            $x_{rand}\leftarrow \mathrm{S}\mathrm{p}\mathrm{a}\mathrm{r}\mathrm{s}\mathrm{e}\mathrm{S}\mathrm{a}\mathrm{m}\mathrm{p}\mathrm{l}\mathrm{i}\mathrm{n}\mathrm{g}\left(\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e},R_{commit}\right)$ 7        $\mathrm{I}\mathrm{t}\mathrm{e}\mathrm{r}\mathrm{C}\mathrm{o}\mathrm{u}\mathrm{n}\mathrm{t}+=1$ 8        $\mathbf{I}\mathbf{f} \mathrm{I}\mathrm{t}\mathrm{e}\mathrm{r}\mathrm{C}\mathrm{o}\mathrm{u}\mathrm{n}\mathrm{t} \mathrm{m}\mathrm{o}\mathrm{d} 10 \mathrm{i}\mathrm{s} \mathrm{z}\mathrm{e}\mathrm{r}\mathrm{o}$ then 9            ${\mathrm{N}}_1\leftarrow 0$ 10       $x_{nearest}\leftarrow \mathrm{N}\mathrm{e}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{t}\left(\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}, x_{rand}\right)$ 11      $\mathbf{If} \mathrm{C}\mathrm{o}\mathrm{l}\mathrm{l}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}\mathrm{F}\mathrm{r}\mathrm{e}\mathrm{e}(x_{rand}, x_{nearest})$ then 12          ${\mathrm{N}}_1+=1$ 13          $X_{near}\leftarrow \mathrm{N}\mathrm{e}\mathrm{a}\mathrm{r}(x_{rand}, r_{near})$ 14          $X_{parent}\leftarrow \mathrm{A}\mathrm{n}\mathrm{c}\mathrm{e}\mathrm{s}\mathrm{t}\mathrm{r}\mathrm{y}(\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}, X_{near}, D_{ancestor})$ 15          $x_{parent}\leftarrow \mathrm{C}\mathrm{h}\mathrm{o}\mathrm{o}\mathrm{s}\mathrm{e}\mathrm{P}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t}\left(X_{near}\cup X_{parent} , x_{nearest} , x_{rand}\right)$ 16          ${(\mathrm{x}}_{\mathrm{c}\mathrm{o}\mathrm{r}\mathrm{n}\mathrm{e}\mathrm{r}\mathrm{A}}, x_{cornerB})\leftarrow \mathrm{R}\mathrm{e}\mathrm{m}\mathrm{o}\mathrm{v}\mathrm{e}\mathrm{T}\mathrm{i}\mathrm{p}\mathrm{s}(\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}, x_{parent}, x_{rand}, D_{dichotomy})$ 17          $({\mathrm{x}}_{\mathrm{c}\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{t}\mathrm{e}\mathrm{A}},x_{createB})\leftarrow \mathrm{C}\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{t}\mathrm{e}\mathrm{N}\mathrm{o}\mathrm{d}\mathrm{e}\mathrm{s}\left(\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e},{x_{parent},x}_{rand},{x_{cornerA},x_{cornerB},D}_{dichotomy}\right)$ 18          $\mathbf{If} {(x}_{createA} \mathrm{i}\mathrm{s} \mathrm{n}\mathrm{o}\mathrm{t} \mathrm{N}\mathrm{u}\mathrm{l}\mathrm{l}) \mathbf{a}\mathbf{n}\mathbf{d} (x_{createB }\mathrm{i}\mathrm{s} \mathrm{n}\mathrm{o}\mathrm{t} \mathrm{N}\mathrm{u}\mathrm{l}\mathrm{l})$ then 19              $\mathrm{Tree}.\mathrm{InsertNode}\left(\mathrm{P}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t}\left(x_{parent}\right), x_{createA}\right)$ 20              $\mathrm{Tree}.\mathrm{InsertNode}\left(x_{createA},x_{createB}\right)$ 21              $\mathrm{Tree}.\mathrm{InsertNode}\left(x_{createB},x_{rand}\right)$ 22          Else 23              $\mathrm{Tree}.\mathrm{InsertNode}\left(x_{parent},x_{rand}\right)$ 24          $\mathbf{If} \mathrm{D}\mathrm{i}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}\left(x_{rand},x_{goal}\right)<goa{l}_{threshold}$ then 25              Return Tree 26          Else 27              Tree ← $\mathrm{Rewire}\text{-}\mathrm{Q}\text{-}\mathrm{RRT}*\left(\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e} , x_{rand} , X_{near}\right)$ 28      $T_{now} \leftarrow UpdateTime\left(\right)$ End MQ-RRT*

4. Proposed Algorithm: Straight-RRT

In this section, we introduce the proposed Straight-RRT algorithm. The design goal of the algorithm is to achieve results that are as good as or better in path quality than those of other algorithms while being faster. The key idea of Straight-RRT is based on three observations. First, if the tree is expanded while being optimized through a process such as rewiring, it takes a long time to compute a feasible path. In other words, there is a problem in that the path cannot be computed when the path must be computed within a very short time. Second, existing algorithms have parameters that the user must set, such as ${\mathrm{r}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}$ and ${\mathrm{D}}_{\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}\mathrm{s}\mathrm{t}\mathrm{o}\mathrm{r}}$, when the tree is expanded, and accordingly, their computational speed and quality are affected by these parameter values. Third, the triangle inequality is efficient but cannot be applied if there is a very small obstacle at a point on the straight line between ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{w}}$ and Parent(${\mathrm{x}}_{\mathrm{p}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{t}}$). To complement this, we applied the following strategies. First, the Straight strategy finds a feasible path faster than other algorithms. Second, the MoveParent strategy complements the triangle-inequality strategy, which cannot be applied if there is even a very small obstacle between a node and its ancestor node. It was inspired by parametric equations.

The proposed algorithm excludes the Rewire process and accordingly minimizes dependence on parameters. However, since there is no rewiring, the problem remains that it is difficult to find an efficient path. Therefore, a path is quickly found using the Straight strategy, and the MoveParent strategy is applied bidirectionally to optimize the overall path. The proposed algorithm finds a good-quality path faster than other algorithms by using these strategies. The overall workflow of the algorithm is shown in Figure 4. The trees are alternately expanded using the Straight strategy. If the two trees can be connected, the MoveParent strategy is executed once from the start direction of the path and then once more from the goal direction. The overall procedure of the Straight strategy is shown in Figure 5. The Straight strategy returns whether the trees have been connected. The overall procedure of the MoveParent strategy is shown in Figure 6. MoveParent returns the optimized path.

The overall workflow of the algorithm is shown in Figure 4. The trees are alternately expanded using the Straight strategy. If the two trees can be connected, the MoveParent strategy is executed once from the start direction of the path and then once more from the goal direction. The overall procedure of the Straight strategy is shown in Figure 5. The Straight strategy returns whether the trees have been connected. The overall procedure of the MoveParent strategy is shown in Figure 6. MoveParent returns the optimized path.

The pseudocode of the Straight-RRT algorithm is given in Algorithm 6. In Algorithm 6, the RandomSampling function samples a node uniformly at random and returns ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$. The Nearest function finds the node in the given tree that is closest to ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$. Straight and MoveParent are described in Algorithms 7 and 8, respectively. The Reconstruct function returns a complete path from the connected trees.

Algorithm 6. Pseudocode of the Straight-RRT algorithm.

Input: $x_{start}$$, x_{goal}$$, C_{free}$$,{ C}_{obs}$$, T_{limit}, { D}_{connect}$$, t_{incre}$ Output: $\mathrm{P}\mathrm{a}\mathrm{t}\mathrm{h}$ ← Result of Algorithm Initialize: ${\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$ ← Null Tree ${\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}$ ← Null Tree ${\mathrm{E}\mathrm{x}\mathrm{p}\mathrm{a}\mathrm{n}\mathrm{d}\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$ ← True $\left(T_{now}, T_{start}\right) \leftarrow \mathrm{I}\mathrm{n}\mathrm{i}\mathrm{t}\mathrm{i}\mathrm{a}\mathrm{l}\mathrm{i}\mathrm{z}\mathrm{e}$ Begin Straight RRT Procedure $1 {\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$← Insert Root Node $x_{start}$$\mathrm{to} {\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$ $2 {\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}$← Insert Root Node $x_{goal}$$\mathrm{to} {\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}$ $3 \mathbf{While} T_{now}-T_{start} \le T_{limit}$ do $4 x_{rand}$← RandomSampling() $5 \mathbf{If} {\mathrm{E}\mathrm{x}\mathrm{p}\mathrm{a}\mathrm{n}\mathrm{d}\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$ then $6 x_{nearest}$ ← Nearest($x_{rand}$,${\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$) 7        Else $8 x_{nearest}$ ← Nearest($x_{rand}$, ${\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}$) $9$isConnect ← Straight($x_{rand}$, $x_{nearest}$, $D_{connect}$, $\mathrm{E}\mathrm{x}\mathrm{p}\mathrm{a}\mathrm{n}\mathrm{d}\mathrm{T}\mathrm{r}\mathrm{e}{\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$) 10      If isConnect then $11 \mathrm{P}\mathrm{a}\mathrm{t}\mathrm{h}\leftarrow {\mathrm{R}\mathrm{e}\mathrm{c}\mathrm{o}\mathrm{n}\mathrm{c}\mathrm{t}\mathrm{r}\mathrm{u}\mathrm{c}\mathrm{t}}_{\mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h}}({\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}},\mathrm{T}\mathrm{r}\mathrm{e}{\mathrm{e}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}})$ 12          MoveParent(Path, ${\mathrm{t}}_{\mathrm{i}\mathrm{n}\mathrm{c}\mathrm{r}\mathrm{e}}$) 13          Path ← Path.Reverse() 14          MoveParent(Path, $t_{incre}$) 15          Path ← Path.Reverse() 16          Return Path 17      ${\mathrm{E}\mathrm{x}\mathrm{p}\mathrm{a}\mathrm{n}\mathrm{d}\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$ ←$\mathrm{Not} {\mathrm{E}\mathrm{x}\mathrm{p}\mathrm{a}\mathrm{n}\mathrm{d}\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$ 18      $T_{now} \leftarrow UpdateTime\left(\right)$ 19    End While End Straight RRT

Algorithm 7. Pseudocode of Straight.

Input: $x_{start},x_{target},D_{connect},\mathrm{E}\mathrm{x}\mathrm{p}\mathrm{a}\mathrm{n}\mathrm{d}\mathrm{T}\mathrm{r}\mathrm{e}{\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$ Output: $\mathrm{i}\mathrm{s}\mathrm{C}\mathrm{o}\mathrm{n}\mathrm{n}\mathrm{e}\mathrm{c}\mathrm{t}$ Initialize: $\mathsf{\theta }$$\leftarrow \mathrm{a}\mathrm{t}\mathrm{a}\mathrm{n}2\left({\left(x_{target}-x_{start}\right)}_y,{\left(x_{target}-x_{start}\right)}_x\right)$ distance ← 0 Begin Straight Procedure 1      ${last}_x\leftarrow {(x}_{start}{)}_x$ $2 {last}_y\leftarrow {(x}_{start}{)}_y$ $3 \mathbf{While} {(last}_x,{last}_y)\in C_{free}$ do $4 \mathrm{distance}+=l_{cc}$ $5 {tmp}_x\leftarrow {(x}_{start}{)}_x+\mathrm{d}\mathrm{i}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}\times \cos \left(\mathsf{\theta }\right)$ $6 {tmp}_y\leftarrow {(x}_{start}{)}_y+\mathrm{d}\mathrm{i}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}\times \sin \left(\mathsf{\theta }\right)$ $7 \mathbf{If} \mathbf{not} \mathrm{C}\mathrm{o}\mathrm{l}\mathrm{l}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}\mathrm{F}\mathrm{r}\mathrm{e}\mathrm{e}(\left({\mathrm{l}\mathrm{a}\mathrm{s}\mathrm{t}}_{\mathrm{x}},{\mathrm{l}\mathrm{a}\mathrm{s}\mathrm{t}}_{\mathrm{y}}\right),\left(tm{p}_x,tm{p}_y\right))$ then 8              Break  $9 \mathbf{If} \mathrm{i}\mathrm{n}\mathrm{t}\left(\mathrm{d}\mathrm{i}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}\right) \mathrm{m}\mathrm{o}\mathrm{d} D_{connect}=0$ then $10 \mathbf{If} {\mathrm{E}\mathrm{x}\mathrm{p}\mathrm{a}\mathrm{n}\mathrm{d}\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$ then $11 x_{nearest}\leftarrow \mathrm{N}\mathrm{e}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{t}\mathrm{N}\mathrm{o}\mathrm{d}\mathrm{e}\mathrm{I}\mathrm{n}\mathrm{O}\mathrm{p}\mathrm{p}\mathrm{o}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{e}(\mathrm{T}\mathrm{r}\mathrm{e}{\mathrm{e}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}},\left(tm{p}_X,tm{p}_y\right),D_{connect})$ $12 \mathbf{If} x_{nearest}$ is not Null then $13 \mathbf{If} \mathrm{C}\mathrm{o}\mathrm{l}\mathrm{l}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}\mathrm{F}\mathrm{r}\mathrm{e}\mathrm{e}(\left({tmp}_x.tm{p}_y\right),x_{nearest})$ then $14 x_{new}\leftarrow \left({tmp}_x,tm{p}_y\right)$ $15 {\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}\leftarrow \mathrm{C}\mathrm{o}\mathrm{n}\mathrm{n}\mathrm{e}\mathrm{c}\mathrm{t}(\mathrm{T}\mathrm{r}\mathrm{e}{\mathrm{e}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}},x_{start},x_{new})$ 16                    Return True 17             Else $18 x_{nearest}\leftarrow \mathrm{N}\mathrm{e}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{t}\mathrm{N}\mathrm{o}\mathrm{d}\mathrm{e}\mathrm{I}\mathrm{n}\mathrm{O}\mathrm{p}\mathrm{p}\mathrm{o}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{e}\left(\mathrm{T}\mathrm{r}\mathrm{e}{\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}},\left(tm{p}_X,tm{p}_y\right),D_{connect}\right)$ $19 \mathbf{If} x_{nearest}$ is not Null then $20 \mathbf{If} \mathrm{C}\mathrm{o}\mathrm{l}\mathrm{l}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{o}\mathrm{n}\mathrm{F}\mathrm{r}\mathrm{e}\mathrm{e}(\left({\mathrm{t}\mathrm{m}\mathrm{p}}_{\mathrm{x}}.tm{p}_y\right),x_{nearest})$ then $21 x_{new}\leftarrow \left({tmp}_x,tm{p}_y\right)$ $22 {\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}\leftarrow \mathrm{C}\mathrm{o}\mathrm{n}\mathrm{n}\mathrm{e}\mathrm{c}\mathrm{t}(\mathrm{T}\mathrm{r}\mathrm{e}{\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}},x_{start},x_{new})$ 23                       Return True 24    End While 25    ${last}_x\leftarrow tm{p}_x$ 26    ${last}_y\leftarrow tm{p}_y$ 27    $x_{new}\leftarrow \mathrm{M}\mathrm{i}\mathrm{d}\mathrm{d}\mathrm{l}\mathrm{e}\mathrm{N}\mathrm{o}\mathrm{d}\mathrm{e}({last}_X, {last}_y)$ 28    $\mathbf{If} {\mathrm{E}\mathrm{x}\mathrm{p}\mathrm{a}\mathrm{n}\mathrm{d}\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}$ then 29        ${\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}\leftarrow \mathrm{C}\mathrm{o}\mathrm{n}\mathrm{n}\mathrm{e}\mathrm{c}\mathrm{t}(\mathrm{T}\mathrm{r}\mathrm{e}{\mathrm{e}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}},x_{start},x_{new})$ 30    Else 31        ${\mathrm{T}\mathrm{r}\mathrm{e}\mathrm{e}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}\leftarrow \mathrm{C}\mathrm{o}\mathrm{n}\mathrm{n}\mathrm{e}\mathrm{c}\mathrm{t}(\mathrm{T}\mathrm{r}\mathrm{e}{\mathrm{e}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}},x_{start},x_{new})$ 32    Return False End Straight

Algorithm 8. Pseudocode of the MoveParent.

Input: $\mathrm{P}\mathrm{a}\mathrm{t}\mathrm{h}$, $t_{incre}$ Output: ${\mathcal{P}}_{\mathrm{r}\mathrm{e}\mathrm{f}\mathrm{i}\mathrm{n}\mathrm{e}\mathrm{d}}$ Initialize: ${\mathcal{P}}_{\mathrm{r}\mathrm{e}\mathrm{f}\mathrm{i}\mathrm{n}\mathrm{e}\mathrm{d}}$ ← [·] i ← 0 $\mathrm{n}$ ← $\mathrm{l}\mathrm{e}\mathrm{n}\mathrm{g}\mathrm{t}\mathrm{h} \mathrm{o}\mathrm{f} \mathrm{P}\mathrm{a}\mathrm{t}\mathrm{h}$ isTZero $\leftarrow$ False //If the path is improved at t = 0.0, it is set to True. Begin $1 {\mathcal{P}}_{\mathrm{r}\mathrm{e}\mathrm{f}\mathrm{i}\mathrm{n}\mathrm{e}\mathrm{d}}$.add(Path[0]) 2    While i < (n − 2) do 3        a ← Path[i+2] 4        b ← Path[i+1] 5        c ← ${\mathcal{P}}_{\mathrm{r}\mathrm{e}\mathrm{f}\mathrm{i}\mathrm{n}\mathrm{e}\mathrm{d}}$[−1] //The value −1 indicates the last index. 6        (${\mathrm{n}\mathrm{e}\mathrm{w}}_{\mathrm{b}}$, isTZero) $\leftarrow \mathrm{P}\mathrm{a}\mathrm{r}\mathrm{a}\mathrm{m}\mathrm{e}\mathrm{t}\mathrm{r}\mathrm{i}\mathrm{c}\mathrm{E}\mathrm{q}\mathrm{u}\mathrm{a}\mathrm{t}\mathrm{i}\mathrm{o}\mathrm{n}(\mathrm{a},\mathrm{b},\mathrm{c},t_{incre})$ 7        If isTZero then 8            i ← i+1 9            Continue 10      ${\mathcal{P}}_{\mathrm{r}\mathrm{e}\mathrm{f}\mathrm{i}\mathrm{n}\mathrm{e}\mathrm{d}}$.add(${\mathrm{n}\mathrm{e}\mathrm{w}}_{\mathrm{b}}$) 11      i ← i+1 12  End While 13  ${\mathcal{P}}_{\mathrm{r}\mathrm{e}\mathrm{f}\mathrm{i}\mathrm{n}\mathrm{e}\mathrm{d}}$.add(Path[−1]) //The value −1 indicates the last index. $14 \mathbf{Return} {\mathcal{P}}_{\mathrm{r}\mathrm{e}\mathrm{f}\mathrm{i}\mathrm{n}\mathrm{e}\mathrm{d}}$ End MoveParent

4.1. Straight Strategy

Existing RRT and its variations can only be extended by the step-size parameter value in the ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ direction from ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{t}}$ to ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{w}}$. If there is an obstacle on the straight path from ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{t}}$ to ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{w}}$, the point is discarded. If it is discarded, the inspection cost used for sampling and collision-checking is wasted, increasing planning time. To address this issue, this section introduces the Straight strategy. Algorithm 7 presents the Straight strategy procedure. In Algorithm 7, the CollisionFree function checks whether there is an obstacle between the given nodes. The NearestNodeInOpposite function finds the nearest node in the opposite tree within a distance ${\mathrm{D}}_{\mathrm{c}\mathrm{o}\mathrm{n}\mathrm{n}\mathrm{e}\mathrm{c}\mathrm{t}}$ from the given node. If no such node exists, it returns null. The Connect function inserts ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{w}}$ into the opposite tree and connects the two trees.

The Straight strategy looks for points that can be extended in the direction of ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ until a collision with the boundaries of obstacles, walls, or the map is detected. (Figure 7a and Figure 8a). If a collision is detected, the point immediately before the collision and the midpoint between ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{t}}$ and that point are added to the tree as ${\mathrm{x}}_{\mathrm{n}\mathrm{e}\mathrm{w}}$ (Figure 7b and Figure 8b). Adding the midpoint rather than directly using the furthest extended point is intended to mitigate search bias and improve exploration capability. If the furthest-extended point is greedily added as a node, the node is more likely to be placed near obstacles, which reduces the clearance available in the next expansion step and can negatively affect tree growth. Therefore, the straight strategy inserts the midpoint into the tree to account for both expansion stability and exploration of the free space.

In addition, as it extends in the ${\mathrm{x}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}$ direction, it looks for nodes that can be connected to the opposite tree within a ${\mathrm{D}}_{\mathrm{c}onnect}$ radius. The inspection cycle is not performed every time for efficiency, but only when it has moved by a distance of ${\mathrm{D}}_{\mathrm{c}onnect}$ (Figure 9a). And if there is a connectable node within a ${\mathrm{D}}_{\mathrm{c}onnect}$ radius, it connects the two trees (Figure 9b). This process eliminates the need for step-size parameters, thereby minimizing parameter dependence and allows faster tree expansion. In addition, it reduced wasted inspection costs when sampling nodes where collisions occur.

4.2. MoveParent Strategy

The Straight strategy above can quickly find a path, but it may produce a path with many nodes and curves. Therefore, the MoveParent strategy is applied to the found path as a post-processing step. Many RRT-based algorithms use a triangle inequality-based strategy for post-processing. This performs computations on the found path. According to the triangle inequality principle, the length of the path moving from path[i] to path[i+1] to path[i+2] is longer than or equal to the length of the path going from path[i] to path[i+2]. Therefore, if the straight-line path between path[i] and path[i+2] is collision-free, the path length is improved, so the path is reconstructed to go directly from path[i] to path[i+2] rather than from path[i] to path[i+1]. However, a drawback of the triangle inequality is that it cannot be applied if there is a very small obstacle in the segment between path[i] and path[i+2].

To address this, MoveParent uses a parametric equation and places candidate positions of path[i+1] on the segment between path[i+1] and path[i+2], and it refines the segment when an improvement is expected. Starting with 0.0 as the initial value, candidate positions of path[i+1] are found based on the t value. Therefore, if the candidate position according to the t value has no collision with path[i], the position of path[i+1] is updated. Depending on the t value, the candidate position changes, and the closer it is to 0, the more it indicates a region where optimization performs better. The MoveParent strategy shows that if there is no collision when t is greater than 0 and less than 1.0, path[i+1] can be optimized even in segments where the triangle inequality cannot optimize. The meaning of this strategy is that it is called MoveParent because the position of path[i+1] is updated as if it moves along the segment between path[i+1] and path[i+2].

The MoveParent strategy improves the quality of the segment between path[i], path[i+1], and path[i+2] by using a parametric equation. First, it derives the parametric equation for moving from path[i+2] to path[i+1]. And it sets the starting point as path[i+2] and the goal point as path[i+1]. Based on Equations (1) and (2), a parametric equation like Equation (3) is obtained.

$${\mathrm{P}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}} = \mathrm{Path}[\mathrm{i}+2]$$

(1)

$${\mathrm{P}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}} = \mathrm{Path}[\mathrm{i}+1]$$

(2)

$${\mathrm{P}}_{\mathrm{n}\mathrm{e}\mathrm{w}\mathrm{P}\mathrm{o}\mathrm{s}\mathrm{i}\mathrm{t}\mathrm{i}\mathrm{o}\mathrm{n}}\left(\mathrm{t}\right)={\mathrm{P}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}+\mathrm{t}\left({\mathrm{P}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}-{\mathrm{P}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}\right), 0.0\le \mathrm{t}<1.0$$

(3)

The number of candidate positions of path[i+1] to be checked on the straight-line segment between path[i+1] and path[i+2] is determined by the increment of t. T always starts from 0.0. For example, if it is divided into 10 segments, t is varied within the range [0.0, 1.0) by incrementing it by 0.1 each time. If there is no collision between path[i] and the candidate position of path[i+1] at each increment of t, it is updated to that position, and the check proceeds to the next path segment.

Using Equation (3), candidate positions of path[i+1] according to the t value are calculated as shown in Figure 10a. If the candidate position for a given t value is collision-free with path[i], the candidate position is updated as the new position of path[i], as shown in Figure 10b. With the MoveParent strategy, as can be seen in Figure 11a, the updated path[i+1] also affects the next MoveParent iteration, further optimizing the path as shown in Figure 11b. Figure 12 shows that the path is improved at t = 0.0. In this case, path[i] is connected to path[i+2]. When t = 0.0, path[i+1] is removed, and path[i] is connected to path[i+2]. Then, instead of checking the next segment, the MoveParent process is performed again based on the current position of path[i].

This strategy is applied bidirectionally to the path found by the Straight strategy, excluding the start and goal positions. In addition, the number of potential candidate positions for path[i+1] is discussed in Section 5.

5. Experimental Results

In this section, we evaluate the performance of the algorithms on the map shown in Figure 13. The map size is 240 (width) × 240 (height) pixels, and the experiments are conducted in a Python 3.9 environment. The gray areas represent obstacles. For each map, the experiments are conducted separately for Case 1 and Case 2. In Case 1, the start position is start 1, and the goal position is goal 1. In Case 2, the start position is start 2, and the goal position is goal 2. Each start and goal position is shown in

Table 3. The start and goal position for each map.

	Start 1	Goal 1	Start 2	Goal 2
Map1	(1, 239)	(239, 1)	(120, 239)	(229, 229)
Map2	(150, 190)	(230, 150)	(150, 190)	(239, 239)
Map3	(10, 10)	(239, 239)	(1, 239)	(239, 1)

.

Each algorithm is executed 100 times in the given environment, and the mean values are compared. The evaluation metrics are the path length (pixels), the time required to compute the path (seconds), and cumulative heading change (radians).

The comparative experimental results are compared by normalizing the Q-RRT* result to 100, as in Equation (4). That is, values closer to 100 indicate performance similar to Q-RRT*. Values smaller than 100 indicate better performance. In contrast, values larger than 100 indicate worse performance.

$$\mathrm{N}\mathrm{o}\mathrm{r}\mathrm{m}\mathrm{a}\mathrm{l}\mathrm{i}\mathrm{z}\mathrm{e}\mathrm{d}={\displaystyle \frac{mean\left(Resul{t}_{others}\right)}{mean\left(Resul{t}_{Q-RRT\mathrm{*}}\right)}}\times 100$$

(4)

For the comparison parameters, ${\mathrm{D}}_{\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}\mathrm{s}\mathrm{t}\mathrm{o}\mathrm{r}}$ in Q-RRT* was set to 2. The dichotomy value in F-RRT* was set to 2. In MQ-RRT*, ${\mathrm{R}}_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{m}\mathrm{i}\mathrm{t}}$ was set to 10, and the dichotomy and ${\mathrm{D}}_{\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}\mathrm{s}\mathrm{t}\mathrm{o}\mathrm{r}}$ were set to 2. These values were all set to the recommended values suggested in the original papers. The proposed algorithm sets the t increment to 0.1. The experimental setup for the t-increment is presented in Section 5.4.

Table 4. Experimental environment.

Hardware	Specification
CPU	AMD Ryzen 7 3700X (8 cores, 3.60 GHz)
RAM	32.0 GB
VGA	NVIDIA GeForce RTX 2070 SUPER (8 GB)

shows the specifications of the computer used for the experiments.

5.1. Experimental Results on Map 1

Figure 14 and Figure 15 show the experimental results for Case 1 in the Map 1 environment. Figure 14 shows the experimental results of Q-RRT* and F-RRT*. Figure 15 shows the experimental results of MQ-RRT* and Straight-RRT.

The straight strategy can find an initial path very quickly, but it may generate an inefficient path. Therefore, when the MoveParent strategy is applied, the sharp segments are smoothed, and the path length is reduced, as can be seen in Figure 15b. In this way, we confirmed that an initial path can be obtained quickly using the Straight strategy and that the path quality can then be improved rapidly using the MoveParent strategy.

Figure 16 and Figure 17 show the experimental results for Case 2 in the Map 1 environment. Figure 16 shows the experimental results of Q-RRT* and F-RRT*. Figure 17 shows the experimental results of MQ-RRT* and Straight-RRT.

The values in

Table 5. Experimental results for Map 1 (normalized to Q-RRT = 100%).

		Q-RRT*	F-RRT*	MQ-RRT*	Proposed Algorithm
Case 1 (Start 1 to Goal 1)	Length of path (%)	100	89.5748	108.0243	96.5402
	Planning time (%)	100	57.3709	14.0567	12.4823
	Cumulative heading change (%)	100	90.4439	114.2131	85.389
Case 2 Start 2 to Goal 2	Length of path (%)	100	90.3169	103.5305	92.5256
	Planning time (%)	100	55.1641	20.0516	7.6401
	Cumulative heading change (%)	100	79.7509	115.2701	72.7848

are relative comparison results normalized by setting the result of Q-RRT* to 100. This means that smaller values indicate better performance. In Case 1, the path length of the proposed algorithm is 96.5402, which is improved compared to Q-RRT* (100) and better than MQ-RRT* (108.0243), but a longer path is generated than by F-RRT* (89.5748). The planning time is 12.4823, which is greatly reduced compared to Q-RRT* and F-RRT*. In addition, we confirmed that it is slightly faster than MQ-RRT* (14.0567). In addition, the cumulative heading change in the proposed algorithm in Case 1 is 85.389, which is improved compared to Q-RRT* (100) and lower than F-RRT* (90.4439) and MQ-RRT* (114.2131). This result indicates that the proposed algorithm can reduce the overall amount of direction change along the path.

In Case 2 as well, the path length of the proposed algorithm is 92.5402, which is improved compared to Q-RRT* and better than MQ-RRT* (103.5305), but the path quality is slightly lower than that of F-RRT* (90.3169). In contrast, the planning time is 7.6401, which is the fastest among the compared algorithms. For Case 2, the proposed algorithm also shows improved performance in terms of cumulative heading change. As shown in Table 5, the cumulative heading change is 72.7848, which is lower than Q-RRT* (100), F-RRT* (79.7509), and MQ-RRT* (115.2701). From the results for Map 1, it was confirmed that the proposed algorithm is overall faster than the other algorithms, while the path quality is good or comparable.

Table 6. Experimental results for Map 1 (mean (std.)).

		Q-RRT*	F-RRT*	MQ-RRT*	Proposed Algorithm
Case 1 (Start 1 to Goal 1)	Length of path (px)	730.53 (19.97)	654.37 (1.30)	789.15 (22.70)	705.26 (21.38)
	Planning time (s)	63.11 (29.25)	36.21 (11.30)	8.87 (1.16)	7.87 (2.74)
	Cumulative heading change (rad)	17.82 (0.73)	16.12 (0.13)	20.3612 (1.96)	15.22 (0.32)
Case 2 Start 2 to Goal 2	Length of path (px)	421.98 (15.78)	381.12 (1.23)	436.88 (20.50)	390.44 (11.57)
	Planning time (s)	30.19 (26.98)	16.65 (9.81)	6.05 (1.78)	2.30 (1.20)
	Cumulative heading change (rad)	8.5364 (0.66)	6.80 (0.12)	9.84 (1.35)	6.21 (0.31)

reports the experimental results as mean (std) over repeated runs. For planning time, the proposed method achieves low average computation time with relatively low variability (7.87 (2.74) in Case 1 and 2.30 (1.20) in Case 2). In comparison, Q-RRT* shows much larger average time and variability (63.11 (29.25) and 30.19 (26.98)), and F-RRT* also has larger mean and variability (36.21 (11.30) and 16.65 (9.81)). MQ-RRT* has a similar mean time but relatively small variability in Case 1 (8.87 (1.16)) and a higher mean time in Case 2 (6.05 (1.78)).

5.2. Experimental Results on Map 2

Figure 18 and Figure 19 show the experimental results for Case 1 in the Map 2 environment. Figure 18 shows the experimental results of Q-RRT* and F-RRT*. Figure 19a shows the experimental results of MQ-RRT*. Figure 19b shows the experimental results of Straight-RRT.

When only the straight strategy was applied, the resulting path was winding. By applying the MoveParent strategy to this path, the winding path became closer to a straight line. In addition, we confirmed that it was improved into a more efficient path overall. Figure 20 and Figure 21 show the experimental results for Case 2 in the Map 2 environment. Figure 20 shows the experimental results of Q-RRT* and F-RRT*. Figure 21 shows the experimental results of MQ-RRT* and Straight-RRT.

The values in

Table 7. Experimental results for Map 2 (normalized to Q-RRT = 100%).

		Q-RRT*	F-RRT*	MQ-RRT*	Proposed Algorithm
Case 1 (Start 1 to Goal 1)	Length of path (%)	100	96.8872	112.4546	103.8429
	Planning time (%)	100	41.7648	3.4434	1.0969
	Cumulative heading change (%)	100	96.2589	112.4961	85.9243
Case 2 Start 2 to Goal 2	Length of path (%)	100	95.2605	101.2851	97.7381
	Planning time (%)	100	89.7849	21.4039	4.9181
	Cumulative heading change (%)	100	98.4794	118.1316	98.2255

are relative comparison results normalized by setting the result of Q-RRT* to 100. In Case 1, the path length of the proposed algorithm was 103.8429, which was a slightly longer path than that generated by the baseline algorithm, Q-RRT*. However, the proposed algorithm achieved a planning time of 1.0969, and we confirmed that although the path was slightly inefficient, the planning time was fast enough to mitigate this drawback. The comparison results with F-RRT* are similar to those of Q-RRT*, but it is much faster in terms of planning time. We confirmed that, like Q-RRT*, it can mitigate the drawback in terms of path length. We confirmed that the proposed algorithm achieves better results than MQ-RRT* in terms of both planning time and path length.

A similar tendency can be observed in Case 2. The path length of the proposed algorithm is 97.7381, which is shorter than Q-RRT* (100) and MQ-RRT* (101.2851), and is at a similar level to F-RRT* (95.2605). The planning time is 4.9181, which is smaller than that of Q-RRT* (100), F-RRT* (89.7849), and MQ-RRT* (21.4039), showing a clear advantage in time performance. From the results for Map 2, the proposed algorithm maintains a path length comparable to or slightly longer than that of F-RRT* while achieving superior planning time. The path quality was comparable to that of Q-RRT*, but the planning time was much faster. Compared to MQ-RRT*, it showed better results in all aspects.

In addition, Table 7 shows the cumulative heading change (rad.) normalized to Q-RRT* (100). In Case 1, the proposed algorithm achieves 85.9243, which is lower than Q-RRT* (100), F-RRT* (96.2589), and MQ-RRT* (112.4961), indicating a reduced overall direction-change demand along the path. In Case 2, the proposed algorithm achieves 98.2255, which is slightly lower than Q-RRT* (100) and comparable to F-RRT* (98.4794), while being lower than MQ-RRT* (118.1316).

Table 8. Experimental results for Map 2 (mean (std.)).

		Q-RRT*	F-RRT*	MQ-RRT*	Proposed Algorithm
Case 1 (Start 1 to Goal 1)	Length of path (px)	528.98 (6.38)	512.52 (1.07)	594.87 (22.55)	549.31 (11.96)
	Planning time (s)	233.20 (94.06)	97.39 (28.30)	8.03 (1.13)	2.55 (0.78)
	Cumulative heading change (rad)	11.06 (0.26)	10.64 (0.13)	12.44 (1.15)	9.50 (0.15)
Case 2 Start 2 to Goal 2	Length of path (px)	716.66 (10.68)	682.69 (0.64)	725.87 (10.68)	700.45 (7.28)
	Planning time (s)	25.33 (34.88)	22.75 (25.22)	5.42 (2.80)	1.24 (0.34)
	Cumulative heading change (rad)	6.14 (0.07)	6.04 (0.02)	7.25 (1.13)	6.03 (0.02)

reports the experimental results as mean (std) over repeated runs. For planning time, the proposed algorithm achieves low mean values with relatively small variability (Case 1: 2.55 (0.78), Case 2: 1.24 (0.34)), while Q-RRT* and F-RRT* show much larger variability (Q-RRT*: 233.20 (94.06) and 25.33 (34.88), F-RRT*: 97.39 (28.30) and 22.75 (25.22)). For the length of the path, the proposed algorithm shows competitive mean values (Case 1: 549.31 (11.96), Case 2: 700.45 (7.28)) and remains more consistent than MQ-RRT* in both cases.

5.3. Experimental Results on Map 3

Figure 22 and Figure 23 show the experimental results for Case 1 in the Map 3 environment. The Map 3 environment contains many obstacles and is therefore unfavorable for tree expansion. Figure 22 shows the experimental results of Q-RRT* and F-RRT*. Figure 23 shows the experimental results of MQ-RRT* and Straight-RRT. The values in

Table 9. Experimental results for Map 3 (normalized to Q-RRT = 100%).

		Q-RRT*	F-RRT*	MQ-RRT*	Proposed Algorithm
Case 1 (Start 1 to Goal 1)	Length of path (%)	100	94.5312	102.7431	100.5065
	Planning time (%)	100	82.3475	48.8431	7.5980
	Cumulative heading change (%)	100	56.8343	125.5068	54.3261
Case 2 Start 2 to Goal 2	Length of path (%)	100	95.0389	100.9559	97.4838
	Planning time (%)	100	155.5519	87.5403	18.8191
	Cumulative heading change (%)	100	56.6577	127.7667	49.6513

are relative comparison results normalized by setting the result of Q-RRT* to 100.

In Case 1, the path length of the proposed algorithm is 100.5065, which is similar to Q-RRT* (100) and MQ-RRT* (102.7431), but tends to be slightly longer than F-RRT* (94.5312). In contrast, the planning time of the proposed algorithm is 7.5980, which is smaller than those of Q-RRT* (100), F-RRT* (82.3475), and MQ-RRT* (48.8431), showing the fastest planning time among the compared algorithms.

Figure 24 and Figure 25 present the experimental results for Case 2 of Map 3. Figure 25b shows the results of the Straight-RRT. A similar tendency can be observed in Case 2. The path length of the proposed algorithm is 97.4838, which is shorter than Q-RRT* (100) and MQ-RRT* (100.9559), but somewhat longer than F-RRT* (95.0389). However, the planning time is 18.8191, which is smaller than those of Q-RRT* (100), F-RRT* (155.5519), and MQ-RRT* (87.5403), exhibiting the best time performance. Therefore, although the proposed algorithm maintains a path length that is similar to or slightly longer than that of F-RRT*, it provides a much shorter planning time compared to the other algorithms, indicating a clear advantage in terms of speed.

In addition, Table 9 reports the cumulative heading change (rad.) normalized to Q-RRT* (=100). In Case 1, the proposed algorithm achieves 54.3261, which is lower than Q-RRT* (100), F-RRT* (56.8343), and MQ-RRT* (125.5068), indicating a reduced overall direction-change demand along the path. In Case 2, the proposed algorithm achieves 49.6513, which is lower than Q-RRT* (100), F-RRT* (56.6577), and MQ-RRT* (127.7667).

Table 10. Experimental results for Map 3 (mean (std.)).

		Q-RRT*	F-RRT*	MQ-RRT*	Proposed Algorithm
Case 1 (Start 1 to Goal 1)	Length of path (px)	403.12 (33.03)	381.08 (32.44)	414.18 (37.48)	405.17 (37.24)
	Planning time (s)	38.46 (35.13)	31.67 (20.33)	18.78 (8.03)	2.92 (1.33)
	Cumulative heading change (rad)	8.58 (2.62)	4.88 (1.66)	10.77 (3.00)	4.66 (1.60)
Case 2 Start 2 to Goal 2	Length of path (px)	390.57 (27.05)	371.20 (23.05)	394.31 (27.22)	380.75 (27.16)
	Planning time (s)	11.17 (8.82)	17.38 (8.58)	9.78 (4.94)	2.10 (0.35)
	Cumulative heading change (rad)	6.97 (2.56)	3.95 (1.62)	8.91 (2.96)	3.46 (1.42)

reports the experimental results as mean (std) over repeated runs. For planning time, the proposed algorithm shows a low mean with small variability (Case 1: 2.92 (1.33), Case 2: 2.10 (0.35)), while Q-RRT* and F-RRT* exhibit much larger variability (Q-RRT*: 38.46 (35.13) and 11.17 (8.82), F-RRT*: 31.67 (20.33) and 17.38 (8.58)). For the length of the path, the proposed algorithm shows a similar standard deviation across repeated runs (Case 1: 405.17 (37.24), Case 2: 380.75 (27.16)).

5.4. Experiment on Candidate Positions of path[i+1] by t Increment

In MoveParent, the increment of the t value indicates how finely the candidate positions of path[i+1] are examined. Therefore, we conducted experiments by dividing the total number of candidate checks according to the increment into 1, 3, 5, 10, 15, 20, and 30. In this case, each increment is 1, 0.34, 0.2, 0.1, 0.067, 0.05, and 0.034. As the increment of the t value decreases, the number of candidates to be checked increases. An increase in the number of candidates means that the inspection is performed more finely.

The results for Map 1 are shown in Figure 26 and Figure 27, which correspond to Case 1 and Case 2, respectively. As the number of candidates of path[i+1] increases, the mean value and dispersion of the length of path decrease, but after 10, the results show a similar level.

The results for Map 2 are shown in Figure 28 and Figure 29, which correspond to Case 1 and Case 2, respectively. Similar to Map 1, for Map 2, as the number of candidates to be checked increases, the mean and dispersion of the length of the path decrease. However, after 10 candidates, the results remain at a similar level.

The results for Map 3 are shown in Figure 30 and Figure 31, which correspond to Case 1 and Case 2, respectively. Since Map 3 is an obstacle-dense environment, we confirmed that the dispersion of the length of the path is improved only slightly, unlike in Map 1 and Map 2. The mean of the length of the path continues to decrease, but after 10 candidates, it shows a phenomenon similar to the results of Map 1 and Map 2. Therefore, based on this experiment, we set the increment of t to 0.1 and conducted the experiments. The values of each experimental result are shown in

Table 11. Results of the mean path length for the number of candidate positions of path[i+1] according to the t increment. The mean value is based on 100 repeated trials.

		1	3	5	10	15	20	30
Map 1	Case 1	828.26	822.27	785.00	705.26	703.44	700.47	703.07
	Case 2	439.20	429.99	418.86	390.44	389.34	390.10	390.16
Map2	Case 1	610.85	598.15	592.94	549.32	549.74	548.90	550.27
	Case 2	722.08	716.73	711.94	700.45	700.53	701.65	701.47
Map3	Case 1	438.18	436.87	427.34	405.17	407.00	410.90	403.85
	Case 2	405.61	403.06	399.20	380.75	383.38	381.37	378.94

.

6. Conclusions

This paper proposes the Straight-RRT algorithm, motivated by the observation that the Rewire procedure widely used in RRT-based path planning improves path quality but increases computational cost and implementation complexity due to hyperparameters, potentially becoming a bottleneck in planning speed. Straight-RRT rapidly obtains an initial solution without rewiring and subsequently improves the path through post-processing. The proposed method rapidly finds an initial path during the tree expansion phase using a straight strategy, and then efficiently improves the path length via MoveParent post-processing. The straight strategy provides an approach that fills the gap in achieving rapid acquisition of an initial path while reducing parameter dependence. In addition, MoveParent improves the limitations of the triangle-inequality-based method. Experimental results show that the Straight strategy generates an initial path very quickly, and that MoveParent also effectively improves the path.

From the results of six types of experiments, the proposed algorithm shows an average of 8.75% in planning time compared to the baseline algorithm Q-RRT*. This shows a much lower value than other algorithms, F-RRT* and MQ-RRT*, which show averages of 80.33% and 32.55%, respectively. This indicates that the proposed method is significantly more advantageous in terms of planning speed. In the length of the path, it also shows an average of 98.10%, showing that it is more efficient than MQ-RRT* (104.83%) and Q-RRT*. Although the proposed method computes a slightly longer path than F-RRT* (93.6%), it achieves a lower cumulative heading change, with an average of 74.38 compared to 79.73 for F-RRT*. This indicates that, even if the path is somewhat longer, the proposed method can sufficiently offset this disadvantage in terms of planning time and cumulative heading change. It also shows an advantage in cumulative heading change over Q-RRT* and MQ-RRT* (118.89).

Straight-RRT can be effectively applied in environments with weak curvature constraints on obstacles and few dynamic obstacles, such as humans. The scope of this paper is to plan paths faster than existing algorithms while generating shorter path lengths. In environments with stringent curvature constraints, by leveraging processing methods that take the dynamics into account [24,25], the proposed approach is expected to handle abrupt changes in heading angle. This is because the generated path may include segments that are sharp or involve abrupt turns, which may not adequately satisfy kinodynamic constraints. Although the evaluation metrics include the cumulative heading change, it simply represents the accumulation of angular changes. In other words, it does not consider physical criteria caused by abrupt changes in angles. In addition, realistic physical factors, such as the robot’s mass, acceleration, inertia, and energy consumption, were not considered. To address these limitations, future work needs to compute a realistic path by considering the physical world. Also, there is the aspect of applicability in a three-dimensional environment. The experiments were conducted as path planning of a mobile robot and were carried out on a two-dimensional environment map without a z coordinate. Therefore, the applicability in a three-dimensional environment with a z coordinate has not been confirmed. To compensate for these limitations, it is necessary to extend the study by considering not only two dimensions but also three dimensions.