Trajectory Planning and Singularity Avoidance Algorithm for Robotic Arm Obstacle Avoidance Based on an Improved Fast Marching Tree

Abstract

The quest for efficient and safe trajectory planning in robotic manipulation poses significant challenges, particularly in complex obstacle environments where the risk of encountering singularities and obstacles is high. Addressing this critical issue, our study presents a novel enhancement of the Fast Marching Tree (FMT) algorithm, ingeniously designed to navigate the complex terrain of Cartesian space with an unprecedented level of finesse. At the heart of our approach lies a sophisticated two-stage path point sampling strategy, ingeniously coupled with a singularity avoidance mechanism that leverages geometric perception to assess and mitigate the risk of encountering problematic configurations. This innovative method not only facilitates seamless obstacle navigation but also adeptly circumvents the perilous zones of singularity, ensuring a smooth and uninterrupted path for the robotic arm. To further refine the trajectory, we incorporate a quasi-uniform cubic B-spline curve, optimizing the path for both efficiency and smoothness. Our comprehensive simulation experiments underscore the superiority of our algorithm, showcasing its ability to consistently achieve shorter, more efficient paths while steadfastly avoiding obstacles and singularities. The practical applicability of our method is further corroborated through successful implementation in real-world robotic arm trajectory planning scenarios, highlighting its potential to revolutionize the field with its robustness and adaptability.

1. Introduction

With the increasing use of robotic arms in various industrial applications, robotic arm trajectory planning has become a crucial topic of research. Trajectory planning for robotic arms can be performed either in Cartesian space or joint space. Compared to trajectory planning in joint space, Cartesian space trajectory planning offers more intuitive and feasible solutions with better motion repeatability [1]. Cartesian space trajectory planning is particularly useful for applications that require specific end-effector trajectories, such as welding and operations in confined spaces [2]. However, one of the challenges in Cartesian space trajectory planning is the presence of singularities, which are not encountered in trajectory planning in joint space [3].

Several approaches have been developed for obstacle avoidance planning for robotic arms in Cartesian space. Classic sampling-based methods include Rapidly Exploring Random Tree Star (RRT*) and the probabilistic roadmap method (PRM), and many algorithms have been developed based on these two algorithms. Janson et al. combined the respective advantages of RRT* and PRM algorithms and proposed a Fast Marching Tree (FMT) [4]. Gammell et al. introduced the Batch Informed Trees (BIT*) algorithm and applied it to dual-arm robot trajectory planning [5]. Rybus et al. used an improved version of BI-RRT* for planning Cartesian trajectories of robotic arms [6]. Chen et al. proposed BI-FMT* and applied it to trajectory planning for spatial robotic arms [7]. Optimization-based methods have also been utilized for trajectory planning. For example, Jin et al. used Chaos Particle Swarm Optimization (CPSO) combined with damping least squares (DLS) and feedback compensation to avoid singularity [8]. Zhao et al. proposed a dynamic damping least squares method based on the minimum singular value to adjust Cartesian space trajectories in singular regions [9]. Riboli et al. used the least squares method and optimized the trajectory using B-splines [10]. Ju et al. introduced a model-based approach for robotic arm trajectory planning. They developed a predictive obstacle avoidance model that takes into account the geometric configuration of the arm. By considering motion state cost, frontal collision cost, and approach time cost, they formulated a cost function. This method also accounts for the deformation of the model when approaching singular points [11]. Learning-based approaches have also been explored. Fujii et al. used a novel neural network for gap inference to accelerate the generation of robotic arm motion trajectories [12]. Liu et al. proposed a model-free supervisor executor deep reinforcement learning method for path planning of UR5 robotic arms [13].

In the above discussion, several methods for obstacle avoidance planning of robotic arms in Cartesian space were discussed. Among them, the sampling-based method is simple, intuitive, and easy to understand and implement. Due to the randomness of the sampling, it also performs well in complex spaces. With a moderate number of sampling points, the computational complexity is not high. The optimization-based method requires setting appropriate optimization objectives. It consumes a lot of computation in complex spaces and is prone to becoming trapped in local optima. The model-based method relies on mathematical models established based on prior knowledge, and the accuracy of the modeling has an impact on the results. The learning-based method requires a large amount of data for training and has poor interpretability. Therefore, the sampling-based method has the advantages of speed, robustness, and wide applicability. As for the treatment of singularities in robotic arms, the mentioned methods do not explicitly identify or avoid singular points. Instead, they handle the singular regions by applying certain treatments to the Jacobian matrix to ensure smooth passage through the singular points.

Processing the Jacobian matrix in a certain way can allow the robotic arm to work in the singular region, such as through methods like damping least squares. However, these methods have issues with error accumulation and multiple solutions [14]. Therefore, when it is not necessary to pass through the singular region, it is better to select a reasonable trajectory that avoids singular points. Liu Y et al. decomposed the Jacobian matrix and eliminated singular points by determining whether the determinant of the Jacobian matrix is zero, ensuring that the trajectory solutions obtained through genetic algorithms were feasible [15]. Lu L et al. used the condition number index as an optimization indicator and proposed a high-order joint smooth trajectory planning method for singularity avoidance [16]. Beck F et al. proposed an artificial potential field method based on known singular configurations, incorporating manipulability indicators as optimization criteria for singularity avoidance [17]. Haviland J et al. proposed a robotic arm control method that maximizes manipulability indicators [18]. Manavalan J et al. presented a robotic arm control strategy that autonomously learns task constraints and plans trajectories by maximizing manipulability indicators [19]. Hao J analyzed the singular configuration of robot trajectories using dexterity indicators and applied them to robotic arm path planning [20]. Cao B et al. constructed the workspace of a robotic arm using flexibility indicators and applied it to grasping tasks [21]. To better utilize the geometric shape of the manipulability ellipsoid, Petrovi ć L et al. used Riemann distance to measure the proximity between a given configuration and a singular configuration, proposing geometry-aware singularity avoidance costs [22,23]. These methods primarily utilize optimization-based trajectory planning approaches for singularity avoidance without combining their optimization objectives with sample-based path planning methods. Moreover, these methods focus on improving the motion performance of the robotic arm and rarely discuss the issues of obstacle avoidance and singularity avoidance simultaneously.

Table 1. Problems addressed and methods used by different scholars in literature reviews.

Reference	Authors	Problem Addressed	Methods Used
[4]	Janson et al.	Merging RRT* and PRM for obstacle avoidance planning.	FMT*
[5]	Gammell.	Enhancing FMT* for dual manipulator obstacle avoidance.	BIT*
[6]	Rybus.	Applying bidirectional RRT algorithm to plan collision-free trajectory of robotic arms.	BI-RRT*
[7]	Chen et al.	Apply bidirectional FMT* to handle spacecraft power failure and arm deployment.	BI-FMT*
[8]	Jin et al.	Applying CPSO to solve the problem of singular points in the trajectory of robotic arms.	CPSO with DLS
[9]	Zhao et al.	Adjusting robotic arm trajectories based on minimum singular values.	Dynamic DLS
[10]	Riboli et al.	Trajectory planning of gantry system using B-spline and least squares method.	Least squares with B-splines
[11]	Ju et al.	Triangular collision planes and cost functions for obstacle avoidance planning.	Geometric-based predictable obstacle avoidance
[12]	Fujii et al.	Real-time trajectory smoothing via shortcutting for manipulators.	Learning-based approaches
[13]	Liu et al.	Model-free deep learning for UR5 robotic arm path planning.	Deep reinforcement learning
[15]	Liu Y et al.	Genetic algorithm with singularity avoidance for trajectory optimization.	AEGA-SA
[16]	Lu L et al.	Differential vector optimization for smooth, singularity-free trajectory planning.	High-order joint smooth trajectory planning method
[17]	Beck F et al.	Custom potential functions for singular configuration avoidance.	Improved artificial potential field method
[18]	Haviland J et al.	Learning-based control strategy for robotic arm task constraints.	A purely reactive method for maximizing manipulability
[19]	Manavalan J et al.	Planning with maximized manipulability without known constraints.	Learning-based methods combine manipulability index
[20]	Hao J	Addressing singularities with dexterity index and trajectory replanning.	Optimization methods using dexterity index
[21]	Cao B	Optimizing humanoid robot vision and operation through torso positioning.	Trajectory planning method combined with a dexterity map
[22,23]	Petrovi ć L et al.	Introducing geometric perception singularity index for trajectory planning.	Geometry-aware Singularity Avoidance costs

shows the methods used and problems addressed by all the scholars mentioned above.

In response to the aforementioned issues, this paper integrates the geometry-aware singularity avoidance cost with the FMT* algorithm to generate a path plan for obstacle and singularity avoidance in the Cartesian space of the robotic arm. To ensure a more suitable quantity and distribution of sampling points in space, a two-stage sampling point generation strategy is proposed. To avoid interference between obstacles and the robotic arm’s joints, a geometric envelope method is used for collision detection. To achieve smoother generated paths, a quasi-uniform cubic B-spline curve is employed for path optimization. These improvements enable the proposed algorithm to simultaneously achieve good performance and acceptable computational complexity. Through simulation experiments and real-world robotic arm verification, the proposed algorithm demonstrates shorter path lengths and stable obstacle avoidance capabilities compared to other methods. These experimental results showcase the superiority and potential of the proposed algorithm in robotic arm obstacle avoidance planning.

2. Methods

The trajectory planning and singularity avoidance algorithm for robotic arm obstacle avoidance based on an improved FMT is shown in Figure 1. The system receives inputs of obstacle information in Cartesian space, along with the start and end coordinates of the robotic arm end effector. First, path points are sampled in the robotic arm workspace environment. After initial sampling, initial path generation, and secondary sampling, the sampling phase ends. The next phase is the path expansion phase, where the path points obtained from secondary sampling are connected. Starting from the root node, which is the start point of the robotic arm end effector, an improved cost function that includes singularity avoidance is used to expand the nodes. At each expansion, the path point with the minimum cost within the connection radius is searched. When there are no unconnected path points in the space, the path optimization phase is entered. Each complete path is checked for collisions to prevent collisions between the robotic arm links. Then, the cost of collision-free paths is calculated, and the path with the minimum cost is obtained. Finally, the trajectory is smoothed using B-spline curves to achieve a smoother motion.

The improved FMT algorithm is presented as shown in Algorithm 1. The inputs to the algorithm are the sampling space $X_{free}$, path search starting point $X_{init}$, path search goal point $x_{goal}$, number of sample points $n$, initial path expansion radius $R1$, and secondary path expansion radius $R2$. Initially, the standard FMT is used to generate a feasible path set $\pi$. Subsequently, the feasible path set is utilized to obtain a secondary sampling path point collection $V$. Afterward, the collection is used for secondary path expansion to obtain a candidate path set $\mathsf{\Pi }$. Finally, the paths in the set are filtered and smoothed to obtain the optimal path.

Algorithm 1 Improved FMT*

Data: Xfree, Xinit, Xgoal, n, R1, R2

Result: BestPath

1: π←STANDARD FMT*(Xfree, xinit, xgoal, n, R1)

2: V←SECONDARY SAMPLING(Xfree, π, n, width)

3: II←SECONDARY EXPANSION(V, xinit, xgoal, R2)

4: $\mathit{BestPath}\leftarrow {\mathrm{P}}_{\mathrm{ATH}}{\mathrm{O}}_{\mathrm{PTIMIZATION}}(\Pi$)

2.1. Principles of the FMT* Algorithm

The FMT algorithm searches for the optimal path by constructing a search tree. Firstly, a certain number of path points are randomly sampled from the global exploration area. Three sets are created to classify the path points: the $V_{unvisited}$ set consists of path points that have not been added to the tree, the $V_{open}$ set consists of path points that have been connected to the tree and need to be expanded, and the ${\mathrm{V}}_{close}$ set consists of path points that have been added to the tree and cannot be expanded.

One iteration of the algorithm is depicted in Figure 2. After the initial point $x_{init}$ is expanded in the first round, it is moved from the $V_{open}$ set to the $V_{close}$ set. In the subsequent steps, a point z is selected from the $V_{open}$ set. Within its connection radius, candidate path points (e.g., point x) are identified. All points in the $V_{open}$ set that fall within the connection radius of the candidate path point x are then connected. The algorithm calculates the cost of each candidate connection and selects the minimum cost collision-free path. The newly added point is then classified into the $V_{open}$, $V_{close}$, or $V_{unvisited}$ set, and the iteration is completed.

FMT* enhances the limitations of RRT* and PRM* by combining their features. Like RRT*, FMT* utilizes a search tree for path exploration. However, instead of simply sampling and connecting a path point to the nearest node, FMT* incorporates a cost calculation process to ensure higher-quality paths. In contrast, both FMT* and PRM* involve sampling a certain number of paths in space. However, PRM* requires constructing a complete path graph and searching for the optimal path within it, which demands substantial computational resources.

During the process of connecting path points, the FMT* algorithm relies on a loss function to evaluate the quality of the path and selects the path point with the minimum loss to expand the search tree. This process adopts the idea of dynamic programming and follows the following state transition equation:

$$c(x)=\min \{c(y)+\mathrm{Cos}t(y,x)\}\mathrm{where}‖y-x‖<r_n$$

(1)

In Equation (1), $y\in V_{opend}$, where $y$ is a point that has been connected on the search tree, $x\in V_{unvisited}$, where $x$ is a candidate path point within a radius $r_n$ of $y$. $c\left(x\right)$ represents the lowest cost from the point $x$ to the root node, and $c\left(y\right)$ represents the cost from node $y$ to the root node. $Cost(y,x)$ is the cost from node $y$ to node $x$, and the standard FMT* algorithm uses the Euclidean distance from a node to the root node as the cost function for path expansion.

The pseudocode for the standard FMT* is listed in Algorithm 2. The initialization phase of the standard FMT* starts by resetting the flag (lines 2–3), followed by generating a set of points using random sampling in the sampling space $X_{free}$ (line 4). Subsequently, a search tree $T$ is initialized with the start point and the set of points (line 5). Set $X_{init}$ as the initial frontier point $z$ (line 6). During the path expansion phase of the standard FMT*, if the conditions for continuing path expansion are met, the Expand function is used to iteratively generate the search tree in the search space, with the initial expansion connection radius set to R1, and only using path length as its loss function. If enough feasible paths are found, the path search is deemed successful, and the search tree $T$ and the path set $\pi$ are returned as results; otherwise, an empty set is returned and the program terminates (lines 7–15).

Algorithm 2 Standard Fast Marching Tree (FMT*)

1: function STANDARD FMT*(Xfree, xinit, xgoal, n, R1)

2:  success = FALSE

3:  stop_expansion = FALSE

4:  S←xinit ∪ xgoal ∪ PSEUDO RANDOM SAMPLING(Xfree, n)

5:  T←INITIALIZE(S, xinit)

6:   z→xinit

7:  while (stop_expansion = FALSE and success = FALSE) do

8:   {T, z, success, stop_expansion}←EXPAND(Xfree, xinit, xgoal, R1)

9:   if (success = TRUE) then

10:    π←PATH(xinit, xgoal, T)

11:   else

12:    π→Ø

13:   end if

14:  end while

15: return π, T

16: end function

2.2. Improved Path Point Sampling Strategy

Although FMT* has made improvements compared to RRT* and PRM*, there are still opportunities for optimization in certain aspects. For example, FMT* uses a random sampling of path points in the sampling space, resulting in paths that are heavily influenced by randomness. Additionally, the fixed connection radius leads to lower path search efficiency. To address these issues, this paper proposes an improved path point sampling strategy that is performed in two stages.

2.2.1. Initial Sampling

The original FMT* algorithm utilizes pseudo-random sampling to sample a certain number of path points in the search space. Sobol sampling, on the other hand, is a deterministic low-discrepancy sampling method. Compared to random sampling, Sobol sampling can better fill the search space, reducing overlapping and redundant samples, as shown in Figure 3. Therefore, Sobol sampling finds wide applications in numerical integration, signal processing, computer graphics, and other fields.

The Sobol sequence can be represented as the product of the Van der Corput sequence and the Sobol-generating matrix $C$. For a Sobol sequence $X_n$ with $n$ numbers, it can be expressed as follows:

$$X_{\mathrm{n}}=[\left({\phi }_2,C(0)\right),\left({\phi }_2,C(1)\right)\dots ,\left({\phi }_2,C(n)\right)]$$

(2)

where

$${\phi }_b,C(i)=(b^{-1}\dots ,b^{-M})[C{\left(a_0(i)\dots ,a_{M-1}(i)\right)}^T]$$

(3)

In the Equation (2), ${\phi }_b$ represents the Van der Corput sequence in base b. The variable i represents the i-th number in the Van der Corput sequence (i = 0, 1, 2…n). In Equation (3), $a_M\left(i\right)$ represents the value of the M-th digit of the i-th number in the Van der Corput sequence in base b. The Sobol-generating matrix C is calculated based on the generating polynomial and the direction numbers. For Sobol sampling in different dimensions, the generating matrix C is different. For example, a 2-dimensional Sobol sampling requires a 2 × k generating matrix, where k represents the number of direction numbers. The calculation of the generating matrix is complex and will not be further discussed here. Joe S. et al. have found the highest-dimensional Sobol-generating matrix of dimension 21,201, which can be referenced for interested readers [24,25]. In practical applications, pre-calculated generating matrices are used, eliminating the need for complex calculations and resulting in higher sampling efficiency.

Using Sobol sampling in the search space for initial sampling, followed by setting a larger connection radius $r_n$, the original FMT* algorithm is used to generate the initial feasible path. Sobol sampling ensures a more uniform distribution of sampling points, while using a larger connection radius allows for faster expansion of the feasible paths. As a result, collision-free paths from the start to the goal can be quickly generated.

2.2.2. Secondary Sampling

After completing the initial sampling, a certain number of feasible paths are obtained. These paths serve as reference paths for the secondary sampling. The initial sampling points are cleared, and then secondary sampling is performed within a certain range of the reference paths, as shown in Figure 4. The process of secondary sampling can be represented by pseudocode.

The pseudocode for secondary path point sampling is shown in Algorithm 3. The process starts by segmenting and sorting the path set, followed by clearing other path points in the sampling space (lines 2–5). The width of all path segments is then expanded (line 7), and the secondary sampling space is defined as the intersection of the path space and sampling space (line 8). Sobol sampling is conducted in the secondary sampling space (line 9). For each sample, it is necessary to determine whether the sampling point falls within an obstacle; if it does, the point is deleted; otherwise, it is retained and added to the point set V. Finally, set V is returned as the result of secondary sampling (lines 10–17).

Algorithm 3 Secondary Sampling

1: function SECONDARY SAMPLING(Xfree, π, n, width)

2:  path←INITAL PATH SEGMENTATION(π)//Segmenting the path formed by standard FMT*

3:  path_num←PATH SORT(π) //sort the path segments and return the total number

4:  CLEAR(Xfree) //clearing initial sampling points

5:  i = 0

6:  while (i < path_num) do

7:   XPath←RESIZE PATH WIDTH(width)//resize the width of the initial sampling paths

8:   XPath ∩ Xfree = XSample

9:   Secondary_Point←SOBOL SAMPLING(XSample, point_num)

10:   if OUTSIDE OBSTACLE(SecondaryPoint) == TRUE then

11:    DELETE(SecondaryPoint)

12:   else

13:    V←APPEND(SecondaryPoint)

14:   end if

15:   i = i + 1

16:  end while

17: return V

18: end function

From Figure 4, it can be seen that the secondary sampling path points are distributed around the initial feasible path, which effectively improves the quality of path searching.

2.3. Path Connection

The proposal adopts a two-stage approach for path point sampling and path connection. In the initial sampling stage, Sobol sampling is used throughout the sampling space. After the initial sampling, the Euclidean distance is used as the cost for the initial path connection. In the secondary sampling stage, the sampling is performed around the paths connected in the initial stage, and the secondary path connection uses both Euclidean distance and a geometric perceptual singularity metric as costs. Different connection radii are set for the two stages of path connection.

The pseudocode for the secondary path expansion is presented in Algorithm 4. The algorithm begins with the initialization phase, where all points in set $V$ except $X_{init}$ are placed into the unvisited set ${\mathrm{V}}_{unvisited}$ (line 2), $X_{init}$ is placed into ${\mathrm{V}}_{open}$ (line 3), and ${\mathrm{V}}_{close}$ is initialized as an empty set (line 4). The initial frontier point z is set to $X_{init}$ (line 5), and points within the connection radius R2 of the frontier point z are noted as $N_z$ (line 6). While the current frontier point z is not the goal point $x_{goal}$, for a point x in $N_z$, the set $N_x$ represents the points within the connection radius R2 (lines 7–9). The points in $N_x$ that belong to ${\mathrm{V}}_{open}$ are noted as $N_y$ (line 10), and then the point $y_{min}$ in $N_y$ with the minimum cost to expand is found (line 11). If the attempted connection path between $y_{min}$ and x is collision-free (line 12), the connection between $y_{min}$ and x is added to the tree T (line 13), and the cumulative cost c(x) is updated based on the minimum cost from $y_{min}$ to x (line 16). If node x successfully connects, it is moved to the open list ${\mathrm{V}}_{open}$, while node y is moved into ${\mathrm{V}}_{close}$ (lines 19–20). If ${\mathrm{V}}_{open}$ is empty, indicating there are no more points available for expansion, the stop_expansion flag is returned (line 22). Finally, the frontier point z is changed in preparation for the next expansion (line 23). After completing all loops, the path set $\mathsf{\Pi }$, search tree T, and the success flag are returned.

Algorithm 4 Secondary Expansion

1: function SECONDARY EXPANSION(V, xinit, xgoal, R2)

2:  Vunvisited←V\{xinit}

3:  Vopen←{xinit}

4:  Vdosed←Ø

5:  z←xinit

6:  Nz←Near(V\{z}, z, R2)

7:  while (z ≠ xgoal) do

8:   for x ∈ Nz do

9:    Nx←Near(V\{x})

10:    Ny←Nx ∩ Vopen

11:    ymin←argminy∈Ny {c(y) + Cost(y, x)}

12:    if CollisionFree(ymin, x) then

13:     T←T ∪ {(ymin, x)}

14:     Vopen,new←Vopen,new ∪ {x}

15:     Vunvisited←Vunvisited\{x}

16:     c(x) = c(ymin) + cost(ymin, x) + sigcost(x)

17:    end if

18:   end for

19:   Vopen←{Vopen ∪ Vopen,new}\{z}

20:   Vclosed←Vclosed ∪ {z}

21:   if Vopen = Ø then

22:   return stop_expansion

23:   end if

24:   z←argminy∈Vopen {z}

25:  end while

$26:\mathbf{return}\Pi$, T, success

27: end function

2.3.1. Two-Stage Connection Parameter Settings

In the path connection process of FMT, the connection radius $r_n$ is a constant that remains unchanged throughout, which limits the flexibility of path exploration. For instance, when the distance between the start and goal points is large, a small connection radius would require a significant number of iterations. Conversely, if the distance between the start and goal points is small, using a larger connection radius would result in overly linear optimal paths. Figure 5 depicts the search tree and optimal paths generated by FMT with different connection radii.

To enhance the flexibility and efficiency of path expansion, different connection radii are set for each stage using Equation (4):

$$r_n=\frac{\lambda V_{space}}{N_{sample}}$$

(4)

$N_{sample}$ represents the number of sampled points, $V_{space}$ represents the volume of the sampling space, and λ represents the connection coefficient. After the initial sampling, in order to find more feasible paths from the start to the goal point within a smaller number of iterations, λ is set to 5. This allows for larger connection step sizes in the search tree, resulting in fewer iterations needed to find feasible paths. After the secondary sampling, a set of path points is formed around the feasible paths. To explore more path points in the search tree, λ is set to 2.5. This restricts the search range for each step, leading to increased iterations, but it also brings diversity to the path search.

2.3.2. Avoiding Singularities Cost

The original FMT* algorithm only considers the Euclidean distance as the cost for path expansion, resulting in the generation of the shortest path. However, for trajectory planning of robotic arms in Cartesian space, avoiding singularities is an important consideration. Therefore, the singularity index can be incorporated into the cost function of the FMT* algorithm.

Defining the joint space $Q$ and the operational space $X$ (in Cartesian space), the following relationship holds:

$$\mathrm{f}:Q\to X$$

(5)

By using the forward kinematics matrix $f$, the joint space $Q$ of the robotic arm can be mapped to the operational space $X$.

$$J(q)=\frac{\partial f}{\partial q}$$

(6)

The Jacobian matrix $J\left(q\right)$ is the partial derivative of the forward kinematics matrix $f$ concerning the joint angle vector q, determined by the configuration of the robot’s joints. The singularity of the robotic arm can be determined by the determinant of the Jacobian matrix. If the determinant of the Jacobian matrix is zero, i.e., $J\left(q\right)$ = 0, it indicates that the robotic arm is in a singular configuration.

According to the theory of singular value decomposition of matrices, the Jacobian matrix of any joint configuration can be decomposed into singular values.

$$J(q)=U\sum V$$

(7)

where

$$\mathsf{\Sigma }=\left[\begin{array}{lllll}{\sigma }_1\hfill & 0\hfill & \dots \hfill & 0\hfill & 0\hfill \\ 0\hfill & {\sigma }_2\hfill & \dots \hfill & 0\hfill & 0\hfill \\ ⋮\hfill & ⋮\hfill & & ⋮\hfill & ⋮\hfill \\ 0\hfill & 0\hfill & \dots \hfill & {\sigma }_m\hfill & 0\hfill \end{array}\right]$$

(8)

$U\in {\mathfrak{R}}^{m\times m}$ and $V\in {\mathfrak{R}}^{n\times n}$ are both orthogonal matrices, Σ is a diagonal matrix with singular values of the Jacobian matrix J(q) as its elements, and ${\sigma }_1\ge {\sigma }_2\ge \dots \ge {\sigma }_m\ge 0$.

While the Jacobian determinant can qualitatively determine singularity, many singularity indices use the singular values of the Jacobian matrix to quantitatively measure the distance between the robotic arm and the singular configuration.

The condition number $K\left(J\right)$ is defined as the ratio between the maximum singular value ${\sigma }_1$ and the minimum singular value ${\sigma }_m$ of the Jacobian matrix J(q). The dexterity index $D\left(J\right)$ is derived from the condition number index and is related to the minimum condition number of the intermediate joint angles $q_i$(i = 2, 3, 4, 5). The manipulability index $W\left(J\right)$ is the determinant of the product of the Jacobian matrix and its transpose. When the manipulability index $W\left(J\right)$ = 0, the robotic arm is in a singular region, and the larger the value of $W\left(J\right)$, the further the distance from the singularity region.

The aforementioned singularity indices are all related to the manipulability ellipsoid. The manipulability ellipsoid can be described by the singular value matrix, and the principal axes of the manipulability ellipsoid are determined by the singular values ${\sigma }_i$ of the Jacobian matrix. Figure 6 shows the manipulability ellipsoid of a two-link robotic arm under different joint configurations.

From Figure 6, it can be observed that as the two-link robotic arm moves closer to the singularity point, the area of the manipulability ellipsoid becomes smaller and flatter in shape. Conversely, when the arm moves further away from the singularity point, the area of the manipulability ellipsoid becomes larger and approaches a more spherical shape. The shape of the manipulability ellipsoid is related to the singular values of the Jacobian matrix; thus, the singularity of the robotic arm can also be determined using the shape of the manipulability ellipsoid. Recently, geometrically aware singularity indices have been proposed [22,23].

For a trajectory in joint space, the geometrically aware singularity cost can be represented using Equation (9):

$$C[q(t)]={\displaystyle \sum _{i=0}^{n}d{\left(k,J(q_i)J^T(q_i)\right)}^2}={{\displaystyle \sum _{i=0}^n‖\log \left(k^{-\frac{1}{4}}J(q_i)J^T(q_i)\right)‖}}_F^2$$

(9)

$C\left[q\left(t\right)\right]$ represents the singularity cost function of the trajectory $q\left(t\right)$. $q_i$ (i = 0, 1 … n) represents each planning point on the trajectory. $J\left(q_i\right)$ represents the Jacobian matrix at the planning point $q_i$. $d(A,B)$ represents the Riemann distance between matrices A and B. ${‖\dots ‖}_F$ represents the Frobenius norm. $k$ is a diagonal matrix with elements ${\sigma }_k$ as given values, satisfying the following equation:

$$k=\left[\begin{array}{ccc}{{\sigma }_k}^2& 0& 0\\ 0& \ddots & 0\\ 0& 0& {{\sigma }_k}^2\end{array}\right]\mathrm{where}{\sigma }_k\ge {\sigma }_{\max }$$

(10)

Equation (9) uses the square of the manipulability index multiplied by a scaling factor and takes the square of the Frobenius norm as the cost. The essence of this equation is to use the Riemann distance between the matrix k and the matrix $J\left(q_i\right)J^T\left(q_i\right)$ as the cost. The matrix k represents the ideal case where all singular values of the Jacobian matrix of the robotic arm are greater than the maximum value ${\sigma }_{max}$. In this case, the manipulability ellipsoid is in its most ideal shape. On the other hand, the matrix $J\left(q_i\right)J^T\left(q_i\right)$ represents the manipulability ellipsoid at the planning point $q_i$. Therefore, the higher the Riemann distance between the manipulability ellipsoid at $q_i$ and the ideal manipulability ellipsoid, the higher the singularity cost.

For trajectory planning in Cartesian space, the trajectory can be transformed to joint space using inverse kinematics $f^{-1}$:

$$q(t)=f^{-1}\left(x(t)\right)$$

(11)

Thus, the equation for the geometrically aware singularity cost in Cartesian space is given by Equation (12):

$$C[x(t)]={\displaystyle \sum _{i=0}^{n}d{\left(k,J\left(f^{-1}(x_i)\right)J^T\left(f^{-1}(x_i)\right)\right)}^2}={{\displaystyle \sum _{i=0}^n‖\log \left(k^{-\frac{1}{4}}J\left(f^{-1}(x_i)\right)J^T\left(f^{-1}(x_i)\right)\right)‖}}_F^2$$

(12)

where $x\left(t\right)$ represents the end effector trajectory in Cartesian space, and $x_i$ represents the $i$-th end effector pose on the trajectory. When $\left|J\right|$ = 0, it is not possible to compute the inverse kinematics solution, so $C\left[x\left(t\right)\right]$ is set to infinity.

Finally, the singularity index mentioned above is incorporated into the cost function of FMT* path planning, forming a new cost function ${c\left(x\right)}^{*}$ with singularity avoidance, defined as Equation (13):

$$c{(x)}^{*}=\min \{c(y)+\mathrm{Cos}t(y,x)+sig\cos t(x)\}$$

(13)

where

$$sig\cos t(x)={‖\log \left(k^{-\frac{1}{4}}J\left(f^{-1}(x_x)\right)J^T\left(f^{-1}(x_x)\right)\right)‖}_F^2$$

(14)

$x_x$ represents the end effector pose of the robotic arm at path point $x$. Compared to the original cost function, which only used the Euclidean distance from the path point x to the root node as the cost, the improved cost function also added the singularity index of Riemannian geometric perception as the cost function, so it can avoid the singularity of the robotic arm.

2.4. Path Optimization

To obtain the optimal path from the end effector trajectories generated by the improved FMT* algorithm, the following steps are taken. Firstly, the geometric envelope method is employed to check for interference between the robotic arm links and obstacles. Trajectories that result in collisions are removed. Then, the cost of each remaining path is calculated, and the path with the lowest cost is selected. Finally, the chosen path is smoothed using the B-spline algorithm.

2.4.1. Collision Detection

In terms of collision detection, the end effector trajectories are first input into a simulation environment built based on the real working environment of the robotic arm. The simulated robotic arm moves along the planned path, and the interference between each link and obstacle is detected. This process can generally be carried out using a geometric envelope method. As shown in Figure 7, the robotic arm is wrapped with simple geometric shapes, and collision detection is performed using envelope models. The advantage of this method is that it requires less computational effort for collision detection, as it only needs to calculate the minimum distance between the envelope model and the obstacles to determine if a collision occurs. If a collision is detected at a certain point along the trajectory, the inverse kinematics of the robotic arm at that point can be traversed to check if all joint combinations lead to collisions. If a collision-free inverse solution cannot be found, the trajectory is discarded.

2.4.2. Path Cost Calculation

After completing the collision detection, the cost of all collision-free trajectories is calculated. Since the cost of each path point has already been calculated during the expansion of the path points, the total path cost is the sum of the costs of all path points on that path, as shown in Equation (15):

$$C_{path}={\displaystyle \sum _{i=0}^{n}c{(x_i)}^{*}}$$

(15)

${{c(x}_i)}^{*}$ represents the cost of the i-th path point in the improved FMT* algorithm. This cost is determined by the distance from the path point to the root node and the distance from the path point to the singularity region. Using this equation, the path with the minimum cost can be obtained.

2.4.3. Path Smoothing

To make the final path smoother, B-spline curves are used to make the robotic arm trajectory smoother. The B-spline curve can be represented using the Equation (16):

$$p(u)={\displaystyle \sum _{i=0}^n{P}_i{B}_{i,k}(u)}$$

(16)

where $P_i(i\in \left[0,n\right])$ represents the i-th control point of the B-spline curve, $B_{i,k}\left(u\right)$ represents the i-th k-th order B-spline basis function. The basis function can be defined using the de Boor–Cox recursion formula:

$$B_{i,k}(u)=\left\{\begin{array}{ll}\left\{\begin{array}{l}1,\hspace{1em}{u}_i\le u\le u_{i+1}\hfill \\ 0,\hspace{1em}\mathrm{otherwise}\hfill \end{array}\right.\hfill & k=0\\ \frac{u-u_i}{u_{i+k}-u_i}{B}_{i,k-1}(u)+\frac{u_{i+k+1}-u}{u_{i+k+1}-u_{i+1}}{B}_{i+1,k-1}(u)\hfill & k\ge 1\end{array}\right.$$

(17)

In Equation (17), if both the numerator and denominator are zero, then the term is assumed to be 0. If the denominator is zero and the numerator is not zero, then the term is assumed to be 1. $u_i\left(iϵ\left[0,n+k\right]\right)$ is called the knot vector, which is defined as Equation (18):

$$[u_0,u_1,\cdots ,u_k,u_{k+1},\cdots ,u_n,u_{n+1},\cdots ,u_{n+k}]$$

(18)

Based on the knot vector, $n$ + $k$ segments of the B-spline curve can be generated. The knot vector can control the shape of the B-spline curve and its local characteristics near the control points. Commonly used knot vectors include uniform knot vectors and quasi-uniform knot vectors. A uniform knot vector is a vector with equal intervals between the nodes, which can create a uniformly distributed curve. A quasi-uniform knot vector has duplicate elements at both ends of the knot vector, which can make the tangent of the spline curve at the beginning and end parallel to the line connecting the control points.

In this paper, the path points generated by the improved FMT* algorithm are used as control points, and quasi-uniform cubic B-spline curves are used as the curves for trajectory planning to make the planned trajectory smoother.

2.5. Computational Complexity

2.5.1. Time Complexity Analysis

The improved FMT algorithm proposed in this article first uses standard FMT* to generate feasible paths. For the standard FMT*, suppose the number of sampling points is n, and if the connection radius is set to the maximum distance between sample points, the time complexity is $O(n\log (n))$. Then, in the second sampling step, it is necessary to segment and sort the feasible paths, and expand the width of the feasible paths. The time complexity of this step is linearly related to the number of path points, so its time complexity can be seen as $O(n)$. In the subsequent collision detection phase, each path point needs to be filtered using a cycle, so its time complexity is also $O(n)$. For the secondary sampling path expansion, compared with the path expansion of the original FMT* algorithm, the improved FMT* uses a singularity avoidance cost function based on geometric perception, which needs to calculate the inverse kinematics solution of the inner point of the connection radius in each iteration cycle, so its time complexity is $O(n^2)$. Therefore, the total time complexity of improving FMT is as Equation (19):

$$O(n\log (n))+O(n)+O(n)+O(n^2)\approx O(n^2)$$

(19)

2.5.2. Space Complexity Analysis

At the beginning of the algorithm, it needs to be in free space $X_{free}$. Generate a set of sample points internally. Each sample point needs to store its coordinates in space, which is usually proportional to the dimension (d) of the problem. Therefore, the space required to store all sample points is $O(nd)$. However, when analyzing the overall spatial complexity, it is common to consider (d) as a constant, thus simplifying the spatial complexity of sample point storage $O(n)$. The FMT algorithm constructs a path by checking the potential connections between sample points, which requires storing connection information between point pairs. In the worst-case scenario, if each point is connected to all other points, the number of connections may be close to $O(n^2)$. However, due to the fact that the FMT algorithm uses a radius-based neighborhood search, not all pairs of points will form a connection, which determines whether points can be connected. In fact, as the number of sample points increases, the average number of connections at each point is influenced by the choice of connection radius, but it does not increase in a square relationship with the number of points. The spatial complexity of improving FMT* satisfies Equation (20):

$$O(n)<O<O(n^2)$$

(20)

3. Experiment and Result

This paper first verifies the obstacle avoidance trajectory planning performance and singularity avoidance performance of the improved FMT* algorithm through simulation experiments on a personal computer. Then, the algorithm proposed in this paper is applied to the obstacle avoidance trajectory planning of the AUBO-I5 robotic arm.

3.1. Obstacle Avoidance Trajectory Planning Simulation Experiment

In order to evaluate the obstacle avoidance trajectory planning performance of the improved FMT* algorithm in Cartesian space, a three-dimensional path planning simulation environment was built on a computer based on the actual scenario. The length, width, and height of this rectangular space were set to 20 m, 20 m, and 5 m, respectively. A green point was set as the starting point and a red point as the destination. The coordinates of the starting point and destination were (10, 8, 1) and (12, 14, 1), respectively. There was a wall-shaped obstacle with a height of 3 m between the starting point and the destination. In the same simulation environment, the algorithm proposed in this paper was compared with three other path-planning algorithms in a comparative experiment. The results of the path planning are shown in Figure 8, where the green line segments represent the sampled paths and the red line segments represent the optimal path. From Figure 8, it can be seen that the path generated by the algorithm in this paper has a shorter length compared to the other algorithms, and the sampled paths are more concentrated.

By conducting 20 experiments using each algorithm in the same environment, the experimental results shown in

Table 2. Experimental results of obstacle avoidance path planning under different algorithms.

Algorithm	Improved FMT*	Standard FMT*	RRT* Smart	Standard RRT*	Standard RRT-Connect	Informed RRT*
Parameters	Initial sample point: 1000 Secondary samp point: 500 Initial connection radius: 0.3 (m) Secondary connection radius: 0.15 (m)	Sample point: 1000 Connection radius: 0.3 (m)	Sample point: 1000 Connection radius: 0.3 (m)	Step length: 0.3 (m) Maximum iteration count: 4000	Step length: 0.3 (m) Maximum iteration count: 4000	Sample point: 1000 Connection radius: 0.3 (m)
Average time consumption	1.67 (s)	1.58 (s)	1.72 (s)	2.21 (s)	0.98 (s)	1.64 (s)
Time variance	$0.16({\mathrm{s}}^2$)	$0.11({\mathrm{s}}^2$)	0.13 (s)	$0.38({\mathrm{s}}^2$)	$0.09({\mathrm{s}}^2$)	$0.21({\mathrm{s}}^2$)
Average path length	18.8 (m)	21.7 (m)	22.1 (m)	23.6 (m)	25.2 (m)	20.8 (m)
Path length variance	$1.12({\mathrm{m}}^2$)	$1.20({\mathrm{m}}^2$)	$1.18({\mathrm{m}}^2$)	$1.14({\mathrm{m}}^2$)	$1.39({\mathrm{m}}^2$)	$1.15({\mathrm{m}}^2$)

were obtained. From the experimental results, it can be seen that the average path length of the improved FMT* algorithm is 18.8 m, which is 13.3% shorter than the standard FMT*, 8.3% shorter than the standard RRT*, and 22.3% shorter than the standard RRT-connect. This indicates that the path planned by the improved FMT* algorithm has a shorter length compared to the other three algorithms. Moreover, in terms of the variance of path length, the improved FMT* algorithm has better stability, as the path length does not vary significantly in each planning iteration, demonstrating good stability. In terms of the time consumed for path planning, the improved FMT* algorithm ranks second among the four algorithms. It does not have an advantage over the standard FMT* and standard RRT-connect, which may be due to the increased computational requirements of the algorithm. However, the increase in computation time compared to the standard FMT* is only 5.6%, which is acceptable.

3.2. Singular Point Avoidance Simulation Experiment

To validate the effectiveness of the improved FMT* algorithm in avoiding singular points, experiments were conducted using a 6-degree-of-freedom (6-DOF) robotic arm model in MATLAB (version R2023b). Firstly, the initial and final poses were set in the workspace of the robotic arm, and a straight-line trajectory was planned. The start and end points were (0.5, 0.5, 1.2) and (−0.5, −0.5, 1.2), respectively. This trajectory passes through a singular point at the shoulder of the robotic arm, with coordinates (0.18, 0.18, 1.2). The MATLAB simulation system framework is shown in Figure 9.

As shown in Figure 10a, it can be observed that without singular point avoidance, joint 1, joint 4, joint 5, and joint 6 undergo significant changes in joint angles after passing through the singular point. As illustrated in Figure 10b, by using the singular point avoidance trajectory planning algorithm proposed in this paper, the end effector of the robotic arm successfully avoids the singular point. This ensures that the joint configuration of the robotic arm remains closer to the initial configuration, avoiding drastic changes in joint angles and ensuring the stability and safety of the robotic arm’s operation.

Applying the proposed algorithm for singular point avoidance, the trajectory planning results for the given singular point are depicted in Figure 11. The red dashed line represents experimental group 1, which did not utilize the singular point avoidance algorithm, while the blue solid line represents experimental group 2, which employed the singular point avoidance algorithm. Since the cost function of this algorithm depends on the singularity and path length, the planned trajectory using the algorithm avoids the singular point and maintains the shortest path in the non-singular region.

The variation curves of joint angles and angular velocities for the robotic arm in the simulation experiment are shown in Figure 12. The red dashed line represents experimental group 1, while the blue solid line represents experimental group 2. In Figure 12a, the joint angle curves can be observed, and it is evident that the joint curves in experimental group 2 are smoother compared to those in experimental group 1, especially in joint 1. In experimental group 1, joint 1 rotates by 3.1 rad, which is equivalent to 177°, while in experimental group 2, joint 1 rotates by 0.3 rad, which is equivalent to 17.2°. In Figure 12b, the joint angular velocity curves are shown. In experimental group 1, significant angular velocity changes occur in joints 1, 4, 5, and 6, which can be attributed to passing through the shoulder singularity point. This result is consistent with theoretical analysis. In contrast, in experimental group 2, the trajectory does not pass through the shoulder singularity point, resulting in overall smoother changes in joint angular velocities.

3.3. Robotic Arm Trajectory Planning Experiment in the Real World

An improved FMT* trajectory planning algorithm was effectively implemented for the AUBO-I5 robotic arm. As for the hardware, a personal computer running served as the primary controller for the robotic arm, while the AUBO-I5 robotic arm and its associated control cabinet acted as the execution mechanism. On the software side, a C++ program utilizing the Moveit interface was developed to incorporate the enhanced FMT* algorithm, with visualization performed through RVIZ. The program was developed and executed within the ROS Melodic environment, specifically on Ubuntu 18.04.6 LTS, and was written adhering to the C++11 standard.

As shown in Figure 13, obstacles were created in the working environment of the robotic arm, and the initial and target poses of the robotic arm were set. Then, the improved FMT* algorithm was used to plan a collision-free trajectory from the start point to the endpoint. Finally, the robotic arm’s working environment and trajectory planning were visualized using RVIZ. Since the algorithm in this study also has singularity avoidance capabilities, the collision-free trajectory planner can avoid the singular points of the robotic arm, thus improving the stability of the robotic arm.

The generated robotic arm trajectory is converted into specific commands using Moveit and sent to the robotic arm control cabinet, thereby driving the robotic arm to follow the predetermined path. The actual motion of the robotic arm is illustrated in Figure 14.

4. Discussion

In the intricate field of path planning, the improved Fast Marching Tree (FMT*) algorithm introduced in our paper emerges as a notably efficient alternative to both optimization-based and learning-based methodologies. Its principal advantage lies in the elimination of the necessity for either a pre-constructed mathematical model, which demands extensive prior knowledge, or a data-intensive model that requires substantial training. This distinctive characteristic markedly reduces the computational resource footprint, enhancing the algorithm’s ease of deployment and broadening its application spectrum.

When scrutinized alongside analogous sampling-based path-planning methods, specifically, the standard FMT, Rapidly Exploring Random Tree (RRT), and RRT-connect algorithms. Our improved FMT* algorithm demonstrates a clear superiority in generating more concise paths, as quantitatively evidenced in Table 1. This outcome signifies not only an optimization of the path’s length but also an indirect contribution to the efficiency and safety of robotic operations. It is noteworthy, however, that the improved FMT* does not boast the fastest computation times among its peers. Yet, this slight concession in speed is counterbalanced by its unique capability for singularity avoidance, a critical feature absent in its counterparts that justifies the marginal increase in computational effort.

The algorithm’s efficacy, particularly in singularity avoidance, was rigorously validated through simulation experiments employing a 6-degree-of-freedom robotic arm model in MATLAB. The results, illustrated in Figure 10 and Figure 11, unequivocally demonstrate how our algorithm significantly elevates the robotic arm’s motion performance by skillfully navigating away from singularities. Furthermore, real-world applicability tests conducted on the AUBO-I5 robotic arm, as depicted in Figure 12 and Figure 13, confirmed the algorithm’s practical effectiveness. The smooth avoidance of obstacles and singular regions, coupled with successful target point acquisition, attests to the robustness and real-world viability of the improved FMT∗ algorithm.

This discussion affirms the improved FMT∗ algorithm’s status as a highly effective, computationally efficient solution for robotic arm trajectory planning, outperforming traditional models in both theoretical and practical arenas. Its ability to reduce path length while incorporating singularity avoidance into its computational framework represents a significant leap forward, paving the way for future advancements in robot arm trajectory planning.

5. Conclusions

In this study, we introduced an augmented version of the Fast Marching Tree (FMT*) algorithm, meticulously engineered for the sophisticated task of Cartesian space trajectory planning in robotic arms, with a special focus on evading both physical obstacles and the mathematical quagmires known as singularities. This enhancement is primarily anchored in the strategic application of Sobel sampling—a technique renowned for its capability to yield a uniform distribution of sampling points across the space. By integrating this technique into a novel two-stage sampling point generation strategy, we have significantly advanced the efficiency and effectiveness of trajectory planning in complex environments.

A pivotal innovation of our work is the refinement of the FMT* algorithm’s path connection strategy through the incorporation of a geometric perception singularity index. This modification imbues the algorithm with the dual capability to skirt around obstacles while meticulously avoiding singular regions, ensuring a safer and more reliable path for robotic arm movement. The path selection process is equally robust, employing a comprehensive approach that leverages collision detection and path cost analysis to identify the most optimal trajectory. The application of quasi-uniform cubic B-splines further enhances the trajectory, ensuring smoothness and continuity essential for the precise execution of tasks by robotic arms.

Notwithstanding these advancements, our improved FMT* algorithm is not devoid of limitations. A notable challenge is the potential discrepancy between the optimal path in Cartesian space and the efficiency of that path when translated into joint space—a critical consideration for the practical application of robotic arms. Moreover, the reliance on sampling for path generation introduces a degree of variability influenced by initial sampling parameters. This sensitivity can result in significant variations in outcomes, emphasizing the impact of the stochastic nature of sampling-based methods on the predictability and consistency of the optimal path.

Future endeavors to ameliorate these limitations could explore adaptive sampling techniques that dynamically refine based on environmental complexity or investigate more sophisticated metrics for joint space efficiency. Additionally, integrating predictive models or machine learning algorithms to optimize initial sampling parameters may offer a pathway to reducing variability and enhancing the consistency of the algorithm’s outcomes.