Path Planning and Singularity Avoidance for Robotic Manipulators in Vision-Constrained Confined Environments Based on GMM and Improved RRT*

Abstract

In complex and confined industrial environments—such as aerospace component assembly and narrow cabin maintenance—robotic front-end visual perception systems frequently suffer from severe structural occlusions, localized target feature loss, and field-of-view (FOV) clipping during deep aperture traversal, creating a representative “vision-constrained” operational regime where continuous real-time visual feedback is physically unavailable. Concurrently, robotic manipulators face low search efficiency in highly constrained narrow passages and severe risks of Cartesian kinematic singularities. To resolve these coupled challenges without relying on online visual servoing, this paper proposes an improved RRT* algorithm based on a Gaussian Mixture Model (GMM-IRRT*) integrating a singularity-aware barrier cost function, built upon pre-calibrated 6D target poses acquired from front-end perception outside the passage. Specifically, a continuous probabilistic guidance field is constructed via GMM learning from prior demonstration paths, transforming uniform random exploration into targeted sampling along feasible topological corridors. Furthermore, a nonlinear singularity penalty term based on the Yoshikawa manipulability measure is incorporated into the underlying algorithmic cost function, establishing a physical barrier that compels the planning tree to autonomously select non-singular, dexterous inverse kinematic configuration branches (e.g., an “elbow-up” posture). Finally, quasi-uniform cubic B-spline curves optimize the discrete waypoints to guarantee C² continuity and bounded joint jerk. Simulation results demonstrate that under constrained benchmark tests, the proposed method achieves a planning success rate exceeding 98.0%, shortens the average planning time by 15.1% in narrow passages and up to 37.3% in complex cluttered environments (with planning time variance reduced by over 66%), and completely eliminates explosive joint velocity spikes. Extensive physical experiments across 20 repeated trials on an AUBO-i5 manipulator platform validate the zero-failure execution, superior trajectory smoothness (tracking RMSE of 1.82 mm), and industrial transferability of the proposed framework.

1. Introduction

Under the macro-context of Industry 4.0 and intelligent manufacturing, collaborative robotic manipulators are increasingly transitioning from traditional, pre-programmed repetitive tasks toward autonomous decision-making in constrained industrial workspaces. Particularly in high-precision aerospace component assembly, deep cabin inspection, and precision sorting, manipulators must execute complex traversals through narrow, enclosed passages. However, during operations in such confined spaces, onboard or external eye-in-hand cameras frequently encounter severe structural physical occlusions, limited fields of view, and optical blind zones. This leads to substantial point cloud degradation and localized target feature loss inside the passage. Under such harsh “vision-constrained” conditions, relying on continuous real-time visual reconstruction fails to support reliable obstacle avoidance and closed-loop servoing during aperture traversal. Consequently, acquiring spatial target priors prior to entering the confined space and utilizing model-driven prior knowledge for offline, collision-free path planning becomes an indispensable necessity. Cartesian space motion planning is widely adopted in these applications because it directly regulates the geometric Euclidean trajectory of the end-effector Tool Center Point (TCP), exhibiting superior predictability in pose-constrained tasks such as window crossing, welding, and dispensing.

Nevertheless, Cartesian space path planning still confronts two intractable bottlenecks in practical deployments. The first is the well-known Narrow Passage Paradox, whose essence lies in the extremely diminutive volume of the feasible manifold within high-dimensional configuration spaces (C−space). Traditional sampling-based algorithms, such as standard RRT*, exhibit uniform spatial sampling characteristics, causing the search tree to generate substantial invalid sample nodes and resulting in exponential search stagnation as obstacle constraints intensify. The second critical challenge is kinematic singularity. When a manipulator moves along Cartesian paths, its analytical Jacobian matrix is prone to rank deficiency, causing minute Cartesian end-effector displacements to induce explosive angular velocity spikes in individual joints. This phenomenon triggers mechanical vibration, severe trajectory deviations, actuator saturation, and instantaneous motor current overloads. Conventional singularity alleviation strategies predominantly employ control-level Damped Least Squares (DLS) methods; however, DLS unavoidably sacrifices path tracking precision and frequently induces physical collisions in highly restricted narrow passages. Crucially, for a standard six-degree-of-freedom (6-DOF) manipulator executing a fully constrained 6D Cartesian pose task, the system possesses zero degrees of kinematic redundancy (n − m = 0). Therefore, singularity evasion cannot be accomplished via null-space projection. Instead, it requires the motion planner to autonomously optimize configuration branch selection across the finite inverse kinematic (IK) manifold during the global tree expansion phase.

To enhance sampling efficiency, the academic community has witnessed an evolution from geometric heuristics to distribution learning. The RRT* algorithm proposed by Karaman and Frazzoli established the theoretical foundation of asymptotic optimality by introducing the rewire operator [1]. Building upon this, the BIT* algorithm utilized heuristic cost functions to prune the search space [2], while the FMT* algorithm accelerated convergence through a fast-marching strategy [3]. Furthermore, perception networks such as YOLOv7 and YOLOv8 provide accurate 6D pose orientation for initial task initialization [4,5], mitigating search blindness in constrained task spaces [6,7]. Recent studies in advanced manipulation have explored combining information-theoretic distance measures, such as Kullback–Leibler (KL) divergence, with rapidly exploring random trees. By modeling robot manipulability and structural constraints as continuous probability density fields, KL divergence serves as an information-theoretic metric to quantify the divergence between candidate sampling nodes and singular states. However, computing high-dimensional continuous KL divergence during online tree expansion requires non-negligible numerical integration or variational approximations, which often compromises the sub-millisecond search efficiency required in highly restricted passages. On the other hand, Gaussian Mixture Models (GMM) have emerged as an effective probabilistic representation tool capable of extracting spatial corridor distributions from demonstration paths [8,9], smoothly parameterizing configuration manifolds via mean vectors and covariance matrices [10,11]. Nonetheless, existing GMM-guided planners focus primarily on geometric clearance, ignoring the dynamic coupling with intrinsic kinematic singularity boundaries [12,13,14].

To address these coupled challenges, this paper proposes an improved RRT* algorithm based on a Gaussian Mixture Model (GMM-IRRT*) integrating a singularity-aware potential barrier cost function, building upon target poses pre-acquired from lightweight visual grasping [15]. First, a GMM is trained on prior feasible paths to establish an anisotropic probabilistic guidance field, transforming uniform exploration into targeted sampling along narrow apertures. Second, an artificial potential barrier cost function derived from Yoshikawa’s manipulability index is embedded directly into the RRT* cost evaluation, penalizing singular postures with infinite cost and compelling the search tree to autonomously converge onto non-singular, high-dexterity inverse kinematic branches (such as the “elbow-up” posture). Finally, quasi-uniform cubic B-splines interpolate discrete waypoints to yield second-order continuous (C²) trajectories with strictly bounded joint jerk.

The main contributions of this paper are summarized as follows:

• Prior-Guided Anisotropic Sampling Strategy: We formulate a continuous probabilistic guidance field using a 4-component GMM trained on prior feasible paths. By exploiting spatial covariance anisotropy, the algorithm concentrates 70% of random samples within the topological centerline corridor of narrow apertures while retaining a 30% uniform exploration margin, fundamentally resolving search stagnation in narrow passages.
• Singularity-Aware Potential Barrier Cost Mechanism: We introduce an analytical singularity barrier cost function based on the Yoshikawa manipulability measure (w(q)) into the underlying RRT* cost metric. By establishing an infinite penalty barrier (w(q) < 0.02 → ∞), the planner proactively evades singular manifolds at the search source, autonomously selecting non-singular inverse kinematic configuration branches and eliminating explosive joint velocity spikes.
• High-Order Trajectory Optimization and Comprehensive Physical Validation: We integrate quasi-uniform cubic B-spline smoothing to guarantee C² continuity and eliminate actuator jerk impulses. The proposed methodology is comprehensively validated through multi-scenario stress simulations and 20 repeated physical trials on an industrial AUBO-i5 manipulator, demonstrating 100% physical execution success and a 49.2% reduction in trajectory tracking error.

The remainder of this paper is organized as follows: Section 2 establishes the manipulator kinematic model, manipulability metric, GMM prior modeling theory, and the configuration space framework; Section 3 details the proposed GMM-IRRT* algorithm architecture, including adaptive step size, GMM fusion sampling, and singularity barrier cost design; Section 4 provides comparative simulation results and mechanism analyses under various constrained benchmark environments; Section 5 presents the physical experimental setup and validation on the AUBO-i5 manipulator platform; and Section 6 concludes the paper.

2. Theoretical Foundations and Methodology

This section establishes a comprehensive mathematical framework for manipulator motion planning in confined spaces. First, the geometric kinematics of the manipulator are formulated using the Modified Denavit–Hartenberg (MDH) parameter method, and a full-body capsule bounding volume model is introduced to establish a rigorous physical benchmark for collision detection across all links. Subsequently, the analytical Jacobian matrix is derived via differential kinematics, and the motion dexterity metric is quantified based on Yoshikawa manipulability to establish a physical evaluation criterion for singularity avoidance. Finally, the probabilistic representation mechanism of the configuration space using the Gaussian Mixture Model (GMM) is explored, providing continuous heuristic guidance for subsequent sampling.

2.1. Kinematic Modeling and Representation of the Robotic Manipulator

An accurate kinematic description is a prerequisite for achieving autonomous obstacle avoidance. This study adopts the Modified Denavit–Hartenberg (MDH) parameter method to describe the kinematic chain of the AUBO-i5 manipulator [16,17,18]. By attaching the coordinate frame to the proximal joint axis of the link, the MDH method effectively resolves the kinematic singularity issue caused by parameter infinitesimal variations when adjacent joint axes are parallel. As shown in Figure 1, the spatial mapping relationship between adjacent link i − 1 and link i is uniquely determined by four kinematic parameters: link length a_i−1, twist angle α_i−1, link offset d_i, and joint angle θ_i.

Figure 1. AUBO-i5 robotic manipulator structure and establishment of the Modified D-H (MDH) coordinate frames.

Based on the topological geometric relationships shown above, the homogeneous transformation matrix ${}_i{}^{i-1}T$ from coordinate frame {i − 1} to frame {i} can be regarded as the composite mapping of rotation and translation operators around the x-axis and the z-axis. The construction process of the transformation matrix is defined as follows:

$$\begin{array}{l}i-1\hfill \\ i\hfill \end{array}T=\mathit{Rot}(x_{i-1},{\alpha }_{i-1})\cdot \mathit{Trans}(x_{i-1},a_{i-1})\cdot \mathit{Rot}(z_i,{\theta }_i)\cdot \mathit{Trans}(z_i,d_i)$$

(1)

Expanding into the standard matrix form yields:

$$\begin{array}{l}i-1\hfill \\ i\hfill \end{array}T=\left[\begin{array}{cccc}\mathit{cos}{\theta }_i& -\mathit{sin}{\theta }_i& 0& a_{i-1}\\ \mathit{sin}{\theta }_i\mathit{cos}{\alpha }_{i-1}& \mathit{cos}{\theta }_i\mathit{cos}{\alpha }_{i-1}& -\mathit{sin}{\alpha }_{i-1}& -d_i\mathit{sin}{\alpha }_{i-1}\\ \mathit{sin}{\theta }_i\mathit{sin}{\alpha }_{i-1}& \mathit{cos}{\theta }_i\mathit{sin}{\alpha }_{i-1}& \mathit{cos}{\alpha }_{i-1}& d_i\mathit{cos}{\alpha }_{i-1}\\ 0& 0& 0& 1\end{array}\right]$$

(2)

For a serial-link manipulator, by cascading the local transformation matrices from the base coordinate frame {0} to the end-effector frame {6}, the non-linear mapping equation from the joint vector $q={[{\theta }_1,{\theta }_2,\dots ,{\theta }_6]}^T$ to the Cartesian pose of the end-effector is established:

$${}_6{}^{0}T\left(q\right)={\displaystyle \prod _{i=1}^6\begin{array}{l}i-1\hfill \\ i\hfill \end{array}T\left(q_i\right)}=\left[\begin{array}{cccc}n\left(q\right)& o\left(q\right)& a\left(q\right)& p\left(q\right)\\ 0& 0& 0& 1\end{array}\right]$$

(3)

where p(q) = [x, y, z]^T denotes the Cartesian position vector of the end-effector Tool Center Point (TCP) in the base coordinate frame.

To resolve the issue of geometric interference in narrow apertures, collision detection in this paper is performed over the full robot body rather than merely the end-effector point. Each manipulator link i ∈ {1, 2, …, 6} is modeled as a swept-sphere cylinder (capsule bounding volume) $ℬ$_i(q), defined by the line segment connecting adjacent joint origins with an envelope radius r_link,i. To ensure absolute operational safety, a uniform safety margin d_margin = 20 mm is augmented to the capsule radius. The full-body collision-free condition in free configuration space $𝒞$_free is formulated as follows:

$𝒞$_free= {q ∈ ℝ⁶ | min_{i∈{1,…,6}}dist($ℬ$_i(q),$𝒪$_obs) > d_margin}

(4)

where $𝒪$_obs represents the set of environmental obstacles, and dist(⋅) evaluates the minimum Euclidean distance between the link capsule and obstacle geometries.

2.2. Jacobian Matrix and Motion Dexterity Metric

When conducting path planning, the manipulator’s intrinsic kinematic dead zones must be avoided alongside external physical obstacles. The analytical Jacobian matrix J(q) ∈ ℝ^6×6 connects the joint space velocity $\dot{q}\in {\mathbb{R}}^6$ with the end-effector Cartesian spatial velocity $\dot{x}={[v^T,{\omega }^T]}^T\in {\mathbb{R}}^6$:

$$\dot{x}=\left[\begin{array}{c}v\\ w\end{array}\right]=J(q)\dot{q}=\left[\begin{array}{c}{J}_v(q)\\ J_w(q)\end{array}\right]\dot{q}$$

(5)

The physical essence of kinematic singularity is the rank deficiency of the Jacobian matrix (rank(J(q)) < 6), leading to the instantaneous loss of motion degrees of freedom along specific Cartesian directions.

To evaluate posture dexterity quantitatively without retaining redundant basic schematics, this study characterizes motion capacity via the Manipulability Ellipsoid concept. Mapped into Cartesian space from a unit joint velocity hypersphere $\parallel \dot{q}{\parallel }_2\le 1$, the principal semi-axes lengths of the ellipsoid correspond to the singular values ${\sigma }_i=\sqrt{{\lambda }_i\left(J{J}^T\right)}$ of the Jacobian matrix (i = 1,…,6). The volume of this ellipsoid reflects the multidirectional motion capacity of the manipulator under configuration q. This study employs the Yoshikawa manipulability index w(q) to quantify ellipsoid volume [19]

$$w\left(q\right)=\sqrt{\det \left(J\left(q\right)J^T\left(q\right)\right)}={\sigma }_1{\sigma }_2\dots {\sigma }_6$$

(6)

When the manipulator approaches a singular boundary, w(q) → 0, causing minute Cartesian displacements to demand near-infinite joint angular velocities. To quantify this boundary condition, the singular region is defined as follows:

Singularity Region:={q∣w(q) < ϵ}

(7)

where the safety threshold is configured as ε = 0.02. This index directly supports the singularity penalty cost formulated in Section 3.4.

2.3. Configuration Space Prior Distribution Modeling Theory Based on GMM

To tackle the low search efficiency of uniform sampling in narrow passages, this paper utilizes a statistical learning model to extract topological corridor features from prior feasible trajectories.

Crucially, to eliminate the need for computationally intensive online inverse kinematics (IK) solvers during sampling, the Gaussian Mixture Model (GMM) is trained directly within the six-dimensional configuration space ($𝒞$-space, q ∈ ℝ⁶). The input training dataset $\mathcal{D}={\left\{q_n\right\}}_{n=1}^N$ consists of discrete joint configuration sequences extracted from prior collision-free demonstrations.

By taking a weighted linear combination of K multivariate Gaussian components, the discrete configuration manifold is transformed into a continuous probability density field p(q) [20]:

$$p(q)={\displaystyle \sum _{k=1}^K{\pi }_k}\mathcal{N}(q|{\mu }_k,{\Sigma }_k)$$

(8)

where π_k is the mixture weight satisfying ${\sum }_{k=1}^K{\pi }_k=1$, and μ_k ∈ ℝ⁶ and Σ_k ∈ ℝ^6×6 represent the mean vector and covariance matrix of the k-th Gaussian component, respectively:

$$\mathcal{N}(q|{\mu }_k,{\Sigma }_k)={\displaystyle \frac{1}{\sqrt{{(2\pi )}^6\left|{\Sigma }_k\right|}}}\mathit{exp}\left(-{\displaystyle \frac{1}{2}}{(q-{\mu }_k)}^T{{\Sigma }_k}^{-1}(q-{\mu }_k)\right)$$

(9)

The Expectation–Maximization (EM) algorithm optimizes model parameters iteratively:

• E-step: Compute the responsibility γ_nk of the k-th Gaussian component for configuration sample q_n:

  $${\gamma }_{\mathit{nk}}={\displaystyle \frac{{\pi }_k\mathcal{N}(q_n|{\mu }_k,{\Sigma }_k)}{{\displaystyle {\sum }_{j=1}^K{\pi }_j\mathcal{N}(q_n|{\mu }_j,{\Sigma }_j)}}}$$

  (10)
• M-step: Update mixture weights, mean vectors, and covariance matrices:

  $${\mu }_k^{\mathit{new}}={\displaystyle \frac{{\displaystyle {\sum }_{n=1}^N{\gamma }_{\mathit{nk}}}{q}_n}{{\displaystyle {\sum }_{n=1}^N{\gamma }_{\mathit{nk}}}}}$$

  (11)

  $${\Sigma }_k^{\mathit{new}}={\displaystyle \frac{{\displaystyle {\sum }_{n=1}^N{\gamma }_{\mathit{nk}}}\left(q_n-{\mu }_k^{\mathit{new}}\right){\left(q_n-{\mu }_k^{\mathit{new}}\right)}^T}{{\displaystyle {\sum }_{n=1}^N{\gamma }_{\mathit{nk}}}}}$$

  (12)

  where γ_nk denotes the posterior probability of sample q_n belonging to component k, characterizing its contribution weight; q_n is the discrete pose sample obtained from pre-planning; and N is the total sample count. Equation (11) locks the mean μ onto the geometric centerline of the feasible path via weighted averaging, while Equation (12) dynamically adjusts the search envelope Σ based on sample dispersion. Through this iterative process, the GMM adaptively fits narrow passages and scales the probability coverage based on the environmental spatial clearance.

2.4. Principles of the Standard RRT* Algorithm

To resolve notation ambiguities across the paper, all sampling, distance evaluations, tree branching, and rewiring operations are formulated strictly in the 6D configuration space ($𝒞$-space, q ∈ ℝ⁶).

The Rapidly exploring Random Tree (RRT) and its asymptotically optimal variant RRT* [21,22] explore non-convex configuration spaces through incremental randomized tree construction, as shown in Figure 2. As the sample count n → ∞, the planned path cost converges to the theoretical optimum with probability 1.

Figure 2. Execution logic and operational mechanisms of the standard RRT* algorithm operators: (a) Random configuration sampling and steer extension; (b) determination of neighborhood node set V_near; (c) selection of optimal parent node based on minimum cumulative cost; (d) rewire mechanism for local tree topology optimization.

In each iteration, a random configuration q_rand ∈ ℝ⁶ is generated incorporating a goal-bias factor P_{bias_goal}:

$$q_{\mathit{rand}}=\left\{\begin{array}{cc}{q}_{\mathit{goal}},& \mathit{if}\mathit{rand}(0,1)<P_{\mathit{bias}\_\mathit{goal}}\\ \mathit{Sample}(U(C_{\mathit{free}})),& \mathit{otherwise}\end{array}\right.$$

(13)

The nearest node q_nearest in the existing tree V is identified via weighted joint-space Euclidean distance dist(q_a, q_b) = ∥q_a − q_b∥₂. The planner then extends a step size λ toward q_rand via the Steer operator to generate a candidate configuration q_new:

$$q_{\mathit{new}}=q_{\mathit{nearest}}+\lambda {\displaystyle \frac{q_{\mathit{rand}}-q_{\mathit{nearest}}}{{‖q_{\mathit{rand}}-q_{\mathit{nearest}}‖}_2}}$$

(14)

If the edge connecting q_nearest and q_new is collision-free under the capsule model (4), existing tree vertices within the rewire radius r_n form the neighborhood set V_near = {q ∈ V∣∥q − q_new∥₂ ≤ r_n}. The optimal parent q_parent is selected based on minimum cumulative path cost:

$$q_{\mathit{parent}}=\mathit{arg}\underset{q\in V_{\mathit{near}}}{\mathit{min}}\left\{\mathrm{Cost}(q)+\mathrm{dist}\left(q,q_{\mathit{new}}\right)\right\}$$

(15)

Finally, the tree topology is dynamically optimized via the Rewire operator. For any neighbor q_near ∈ V_near, if routing through q_new yields a strictly lower cumulative cost:

Cost(q_new) + dist(q_new, q_near) < Cost(q_near)

(16)

the algorithm severs the existing parent edge and reconnects q_near with q_new as its parent, asymptotically driving path quality toward the global optimum.

3. Proposed GMM-IRRT* Path Planning and Obstacle Avoidance Algorithm

Building upon the kinematic modeling and configuration space theory established in Section 2, this section elaborates on the proposed GMM-IRRT* motion planning framework. Rather than a simplistic concatenation of independent modules, the proposed framework establishes a hierarchical architecture coupling prior demonstration learning, physical constraint avoidance, kinematic singularity evasion, and high-order trajectory optimization, aimed at overcoming the search efficiency and singularity bottlenecks of manipulators operating in vision-degraded confined spaces.

3.1. Improved RRT* Framework with Adaptive Step Size and Dynamic Radius

To mitigate the high-dimensional search burden faced by 6-DOF manipulators in confined configuration spaces, targeted improvements are implemented on the core operators of RRT*.

Regarding tree expansion, the fixed step size λ in conventional algorithms exhibits a prominent performance trade-off: an excessively large step size triggers frequent collisions near narrow passage boundaries, whereas an overly small step size severely retards exploration in open spaces. To address this, an environment-feedback-based adaptive step size operator is designed. The planner dynamically modulates the expansion gain by monitoring the distance to obstacle boundaries and kinematic manipulability:

$${\lambda }_{\mathit{adj}}=\left\{\begin{array}{ll}{\lambda }_{\mathit{max}},\hfill & \mathit{if} \mathrm{dist}(q,\mathit{Obs})>d_{\mathit{safe}}\hfill \\ {\lambda }_{\mathit{min}}\cdot \ln \left(1+w\left(q\right)\right),\hfill & \mathit{otherwise}\hfill \end{array}\right.$$

(17)

where w(q) denotes the Yoshikawa manipulability index defined in Equation (6), and d_safe is the predefined safety distance threshold. This adaptive mechanism allows the algorithm to maintain a large step size during initial global exploration; once a branch approaches an operational window or enters a singularity-sensitive zone, the step size automatically contracts, enabling high-resolution probing of physical constraints.

Furthermore, to optimize computational efficiency while preserving asymptotic optimality, a dynamic radius constraint is imposed on the Rewire operator. The neighborhood search radius r_n decays dynamically with the tree scale n:

$$r_n=\mathit{min}\left\{\gamma {({\displaystyle \frac{\mathit{lnn}}{n}})}^{1/d},r_{\mathit{max}}\right\}$$

(18)

where d denotes the dimension of the configuration space (d = 6 in this work), and γ is a planning constant. This ensures broad neighborhood rewiring during early iterations for macroscopic path trimming, transitioning to localized refinement in later iterations.

3.2. Deep Fusion Mechanism of GMM-Guided Distribution and Improved RRT*

The primary sampling innovation lies in the architectural coupling between prior environmental features provided by the GMM and the stochastic exploration of RRT* [23,24,25]. Rather than unconstrained global random exploration, this fusion constructs a heuristic sampling decision operator that dynamically translates the continuous probability density field into directional guidance for tree growth.

At the start of each sampling iteration, a decision factor r ∈ [0, 1] is drawn from a standard uniform distribution. The candidate configuration q_rand ∈ ℝ⁶ is generated via a hybrid sampling strategy:

$$\begin{array}{l}\\ q_{\mathit{rand}}=\left\{\begin{array}{ll}{q}_{\mathit{sample}}~p(q),\hfill & \mathrm{if}\mathrm{rand}(0,1)<P_{\mathit{bias}}\hfill \\ q_{\mathit{sample}}~U({\mathcal{C}}_{\mathit{free}}),\hfill & \mathrm{otherwise}\hfill \end{array}\right.\end{array}$$

(19)

where p(q) represents the 6D GMM probability density function defined in Equation (8), U($𝒞$_free) denotes the uniform distribution over free configuration space, and P_bias = 0.7 is configured as the empirical guidance bias factor. When r < P_bias, the planner operates in prior-guided sampling mode, drawing samples directly from the K = 4 component GMM distribution. A component k is selected via roulette wheel selection based on mixture weight π_k, and the Box-Muller transform generates a configuration adhering to $𝒩$(μ_k, Σ_k). Because the covariance matrix Σ_k exhibits pronounced spatial anisotropy along narrow apertures, generated samples mathematically stretch along the corridor’s longitudinal depth while remaining tightly compressed orthogonal to obstacle boundaries. In practice, this concentrates 70% of candidate nodes within the feasible centerline corridor. Conversely, when r ≥ P_bias, the planner reverts to uniform random exploration in $𝒞$_free with a 30% margin, preserving probabilistic completeness and enabling escape from local minima if environmental perturbations occur.

3.3. Overall Closed-Loop Flow of the GMM-IRRT* Framework

To clearly delineate system boundaries, the proposed framework operates as an offline prior-guided trajectory generation and execution pipeline designed for vision-degraded confined spaces. The system workflow is partitioned into three sequential stages, as illustrated in Figure 3:

Figure 3. Structured execution framework for robotic motion planning and obstacle avoidance in vision-constrained environments (target pose initialization is adapted from [15]).

• Stage 1: Environmental Prior Learning and Initialization: The system receives the target 6D object pose and static boundary maps pre-acquired outside the aperture via the external eye-to-hand RGB-D perception pipeline. A set of collision-free demonstration paths is loaded to execute the EM iterations (Equations (10)–(12)), fitting discrete joint sequences into a continuous 6D GMM topological navigation field.
• Stage 2: Heuristic Path Planning with Dual Validation: The GMM-IRRT* planner enters its core exploration cycle using hybrid sampling (Equation (19)). During tree expansion, every candidate configuration q_new must pass dual physical validations:
  • Full-body capsule collision check under Equation (4) with safety margin d_margin = 20 mm;
  • Real-time Jacobian manipulability validation (w(q_new) ≥ 0.02). Only collision-free and non-singular nodes execute rewiring optimization. The iteration count is bounded at 800 to ensure bounded computational convergence.
• Stage 3: Trajectory Smoothing and Hardware Execution: For the extracted polyline waypoints, the quasi-uniform cubic B-spline interpolation algorithm (Section 3.5) generates C² continuous trajectories, converting discrete geometric waypoints into smooth joint trajectory profiles for the AUBO-i5 motion controller.

3.4. Singularity-Aware Cost Function Design

In Cartesian space motion planning, standard RRT* evaluates expansion solely through Euclidean distance, which frequently drives the manipulator into kinematic singular boundaries in pursuit of the shortest geometric path. To couple geometric optimality with kinematic safety, a non-linear cost function reconstruction is formulated.

Let u denote the parent node and v denote the candidate node. The cumulative cost Cost(v) from the root to node v is formulated as follows:

Cost(v) = Cost(u) + dist(u,v) + f_sing(v) + f_cfg(v)

(20)

where dist(u, v) = ∥u − v∥₂ is the joint-space distance increment, f_sing(v) is the singularity barrier penalty term, and f_cfg(v) is the configuration optimization term.

Based on the Yoshikawa manipulability index w(q), a threshold-triggered infinite potential barrier function is designed:

$$f_{\mathit{sing}}(v)=\left\{\begin{array}{ll}0,\hfill & w(v)\ge 0.02\hfill \\ \infty ,\hfill & w(v)<0.02\hfill \end{array}\right.$$

(21)

Regarding information-theoretic alternatives, existing studies have utilized Kullback–Leibler (KL) divergence to measure the probabilistic distance between candidate configurations and singular manifolds. While KL divergence offers smooth distributional gradients, evaluating high-dimensional continuous divergence online demands iterative numerical quadrature or variational approximations, which introduces computational bottlenecks during sub-millisecond rewiring. In contrast, the threshold-triggered infinite barrier function (21) evaluates in near O(1) time, establishing a physical barrier at the boundary of singular zones (w(q) < 0.02). Crucially, because a standard 6-DOF manipulator executing a 6D pose task possesses zero degrees of kinematic redundancy (n − m = 0), this barrier penalty compels the planning tree to autonomously optimize configuration branch selection across the finite inverse kinematic (IK) manifold, steering tree growth away from singular joint-alignment branches and autonomously selecting the dexterous “elbow-up” non-singular branch.

Additionally, an elevation configuration penalty f_cfg(v) = λ_z ⋅ z is incorporated (where λ_z is a weighting coefficient and z represents the end-effector Cartesian height), encouraging the manipulator to maintain an optimal elevation profile for obstacle avoidance.

3.5. Trajectory Optimization via Quasi-Uniform Cubic B-Splines

Although the GMM-IRRT* planner yields collision-free paths, the sampling-based nature results in a polyline path composed of discrete linear segments with velocity discontinuities at waypoints. To eliminate actuator impulse shocks, quasi-uniform cubic B-spline curves (k = 3) are employed to achieve second-order continuous (C²) optimization [26,27,28,29].

The continuous trajectory curve P(u) is defined as follows:

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

(22)

where D_i represents the control vertices extracted from path waypoints, and N_i,k(u) denotes the k−th degree B-spline basis function defined by the Cox–de Boor recursion:

$$N_{i,0}(u)=\left\{\begin{array}{ll}1,& u_i\le u<u_{i+1}\\ 0,& \mathit{otherwise}\end{array}\right.$$

(23)

$$N_{i,k}(u)={\displaystyle \frac{u-u_i}{u_{i+k}-u_i}}{N}_{i,k-1}(u)+{\displaystyle \frac{u_{i+k+1}-u}{u_{i+k+1}-u_{i+1}}}{N}_{i+1,k-1}(u)$$

(24)

To ensure that the trajectory strictly interpolates the initial and terminal control vertices D₀ and D_n, a quasi-uniform knot vector U with boundary multiplicity k + 1 = 4 is configured:

$$U=\{\underset{k+1}{\underbrace{a,\dots ,a,}}{u}_{k+1},\dots ,u_n,\underset{k+1}{\underbrace{b,\dots ,b}}\}$$

(25)

As illustrated in Figure 4, Selecting cubic basis functions (k = 3) with zero initial and terminal acceleration derivatives guarantees that position, velocity, and acceleration profiles remain smooth and continuous, strictly bounding the actuator jerk (the time derivative of acceleration) within industrial servo limits and preventing gearbox wear.

Figure 4. Schematic of cubic B-spline trajectory optimization and smoothing.

4. Simulation Experiments and Result Analysis

4.1. Simulation Setup and Evaluation Metrics

To validate the performance of the proposed GMM-IRRT* algorithm, a high-fidelity simulation platform was developed within the Robot Operating System (ROS) environment running on Ubuntu 18.04 LTS. The simulation object is an AUBO-i5 6-DOF collaborative robotic manipulator. The geometric constraints and MDH kinematic parameters of the manipulator are listed in Table 1. All simulation tasks in this study are executed based on this kinematic model. The step size extension gain is set to λ = 0.15 rad, the goal-bias probability is P_{bias_goal} = 0.1, and the computing hardware configuration is an Intel Core i7-10700 CPU @ 2.90 GHz with 16 GB RAM.

Table 1. Modified Denavit–Hartenberg (MDH) parameters of the AUBO-i5 manipulator.

i	ai−1 (mm)	αi−1 (°)	di (mm)	θi
1	0	0	121.5	θ1
2	0	90	0	θ2
3	408	0	0	θ3
4	376	0	140.5	θ4
5	0	90	102.5	θ5
6	0	90	94	θ6

In this study, key quantitative metrics are evaluated:

• Average Planning Time (s): Measures the isolated algorithmic execution time of tree search;
• Planning Time Variance: Assesses the stochastic stability of the sampling process;
• Success Rate (%): Evaluates algorithmic robustness under bounded iteration limits;
• Cartesian Path Length (m): Formally defined to quantify geometric trajectory directness in Cartesian workspace. While sampling is conducted in 6D configuration space ($𝒞$-space), path length is evaluated as the cumulative Cartesian Euclidean distance traversed by the end-effector Tool Center Point (TCP) via forward kinematics (FK):

$$\mathrm{Path}\mathrm{Length}={\displaystyle \sum _{j=1}^{M-1}{‖{\mathrm{FK}}_{\mathit{pos}}(q_{j+1})-{\mathrm{FK}}_{\mathit{pos}}(q_j)‖}_2}$$

(26)

where M is the number of discrete waypoints along the planned trajectory, and FK_pos(⋅) ∈ ℝ³ extracts the Cartesian position from Equation (3).

4.2. Comparative Analysis in Narrow Passage Constrained Environments

First, a representative narrow passage benchmark was designed. The obstacle represents a window-like structural aperture with an inner opening dimension of 35 cm × 50 cm (width × height), an outer frame of 70 cm × 80 cm, a plate thickness of 10 mm, and its geometric centroid positioned at (0.55, 0.0, 0.35) m in the robot base frame. The initial and goal configurations are configured as q_start = [0.0, −0.26, 1.74, 0.52, 1.57, 0.0]^Trad and q_goal = [0.85, 0.35, 1.05, −0.17, 1.57, 0.0]^Trad, respectively.

The proposed algorithm (Ours) was benchmarked against Standard RRT* and Bidirectional RRT* (Bi-RRT*). To evaluate algorithmic robustness under stringent real-time constraints, the maximum iteration budget was set to 450 iterations across 50 independent trials, with random boundary pose perturbations of ±0.015 rad.

As illustrated in Figure 5, In-depth analysis of the data in Table 2 reveals that under highly constrained conditions, standard RRT* achieves a success rate of 96.0%. In contrast, the proposed method achieves a superior success rate of 98.0% while reducing average planning time by 15.1% (from 0.0833 s to 0.0707 s). A more prominent advantage is observed in computational stability: the time variance of our algorithm (0.00217) is reduced by 66.2% compared to standard RRT* (0.00642), effectively eliminating long-tail stochastic stagnation.

Figure 5. Continuous probability guidance field distribution constructed from environmental prior knowledge via GMM.

Table 2. Performance comparison under the narrow passage constrained scenario.

Algorithm	Avg Time (s)	Time Var	Avg Length (m)	Success Rate
Standard RRT*	0.0833	0.00642	1.3456	96.0%
Bi-RRT*	0.0777	0.00390	1.3781	92.0%
Ours (GMM-IRRT*)	0.0707	0.00217	1.2635	98.0%

Figure 6 illustrates the planning time distributions across 50 independent trials using boxplots, where the vertical axis reflects the isolated core algorithmic search time (calibrated to 0∼0.25 s). The median search times for Standard RRT*, Bi-RRT*, and Ours are 0.082 s, 0.076 s, and 0.070 s, respectively. Standard RRT* exhibits substantial dispersion due to blind uniform sampling in $𝒞$-space, whereas the proposed method demonstrates an extremely compact interquartile range with variance 0.00217, confirming rapid and deterministic convergence.

Figure 6. Boxplot of isolated algorithmic planning time stability across 50 test trials (calibrated scale: 0~0.25 s).

Figure 7 presents the 3D trajectory and full-body link posture comparison in the simulation workspace. Here, collision detection operates over the full robot arm using capsule bounding volumes (Equation (4)). While the end-effector trajectory (solid red line) traverses the window center smoothly, the robot’s upper arm (Link 2) and elbow (Joint 3) operate in close proximity to the window inner edges. Under the proposed framework, the minimum link-to-obstacle physical clearance during passage crossing is measured at 32.4 mm, safely exceeding the 20 mm safety margin. In contrast, Bi-RRT* exhibits redundant spatial oscillations with a narrow clearance margin of only 12.1 mm.

Figure 7. 3D trajectory and full-body manipulator posture comparison under the narrow passage simulation environment.

4.3. Generalization Performance in Complex Cluttered Environments

To evaluate generalization in unstructured cluttered environments, a rock array benchmark was constructed comprising three spherical obstacles: O₁ centered at (0.45, −0.15, 0.30) m with radius R₁ = 0.18 m; O₂ centered at (0.55, 0.20, 0.40) m with radius R₂ = 0.18 m; and O₃ centered at (0.65, 0.0, 0.55) m with radius R₃ = 0.14 m.

As indicated in Table 3, as environmental complexity escalates, baseline search efficiency degrades significantly. Benefiting from GMM guidance, our method maintains a 98.0% success rate while reducing average planning time by 37.3% (from 0.0926 s to 0.0581 s) and shortening average Cartesian path length to 1.2894 m.

Table 3. Performance comparison under the complex cluttered scenario across 50 independent trials.

Algorithm	Avg Time (s)	Time Var	Avg Length (m)	Success Rate
Standard RRT*	0.0926	0.00251	1.4563	94.0%
Bi-RRT*	0.0856	0.00680	1.4794	86.0%
Ours (GMM-IRRT*)	0.0581	0.00027	1.2894	98.0%

The learned continuous probability guidance field under the multi-obstacle environment is illustrated in Figure 8. As shown in Figure 9, the end-effector trajectory of our method safely circumvents the obstacle cluster while maintaining a minimum link-obstacle clearance of 28.6 mm across the entire kinematic chain, confirming robust full-body collision avoidance.

Figure 8. Probability guidance field learned and generated under the complex multi-obstacle environment.

Figure 9. Comparison of generated 3D paths and manipulator arm link envelopes under the complex multi-obstacle scenario.

4.4. Mechanism Analysis of Kinematic Singularity Avoidance

Kinematic safety in Cartesian motion planning requires maintaining the Jacobian matrix at full rank (rank(J) = 6) [30,31]. To elucidate the configuration optimization mechanism of the proposed singularity barrier cost function, the geometric morphology of the manipulability ellipsoid is analyzed in Figure 10.

Figure 10. Comparison of manipulability ellipsoid morphology between singular and non-singular configurations.

As illustrated in Figure 10, the dashed red ellipsoid characterizes an extreme configuration generated by standard RRT* near q_singular = [0, π/2, 0, 0, 0, 0]^Trad, where the manipulability index collapses to w(q) = 0.008 (well below the safety threshold ε = 0.02). Conversely, guided by the barrier penalty cost f_sing(v), our planner steers tree expansion toward the dexterous region depicted by the solid green ellipsoid (w(q) ≥ 0.02), preserving isotropic volume and ensuring kinematic dexterity across the selected non-singular IK branch.

This configuration optimization directly translates into autonomous end-effector path adjustments in the Cartesian workspace, as evidenced by the position profiles in Figure 11.

Figure 11. End-effector Cartesian X, Y, Z position profiles over time.

During a 5.0 s simulation cycle with a control period Δt = 0.033 s, the Z-axis elevation profile demonstrates that within the obstacle crossing interval (t = 1.8 s∼3.3 s, gray shaded region), the algorithm detects configuration collapse risks at the window boundary and autonomously commands an end-effector elevation of approximately 7 cm (rising from 0.35 m to 0.42 m). This adaptive elevation successfully prevents base joint deadlocks.

Figure 12 records joint position and angular velocity responses across all six joints.

Figure 12. Joint motion profiles: (a) Joint angle variations; (b) Joint angular velocity profiles.

Under the barrier constraint ε = 0.02, Figure 12b demonstrates that all joint angular velocities remain remarkably smooth, bounded strictly within ±0.8 rad/s. Transient velocity spikes common in standard planners near singularities are completely eliminated.

As observed in Figure 13a, standard RRT* forces the fifth joint (Wrist 2) near 0^∘ to minimize geometric Euclidean path length, causing collinearity between Joint 4 and Joint 6 axes. Conversely, as shown in Figure 13b, under w(q) ≥ 0.02, our planner autonomously selects the non-singular “elbow-up” configuration branch among the finite inverse kinematic manifold, ensuring ample kinematic margin.

Figure 13. Visualized manipulator postures in RViz: (a) Singular posture generated by standard RRT*; (b) dexterous posture generated by the proposed algorithm.

4.5. Quantitative Evaluation of Trajectory Smoothness Based on Jerk Metric

To ensure dynamic stability on industrial servo lines, high-order derivatives are evaluated. Figure 14 presents the full-cycle joint jerk (the time derivative of acceleration) profiles following quasi-uniform cubic B-spline optimization.

Figure 14. Joint Jerk profiles demonstrating high-order trajectory smoothness.

Using cubic basis functions (k = 3) to interpolate waypoints, Figure 14 reveals that the jerk profiles of the primary joints (Base, Shoulder, Elbow) remain strictly continuous and bounded within ±150 rad/s³. This C² continuous dynamic characteristic completely suppresses actuator torque surges, eliminates tip vibrations, and guarantees physical execution safety.

5. Physical Experimental Validation and Application

To evaluate the operational feasibility and dynamic robustness of the proposed GMM-IRRT* framework in real-world deployments, physical experiments were conducted on an industrial AUBO-i5 6-DOF collaborative robotic manipulator platform under high-constraint and high-singularity risk conditions.

5.1. Hardware Platform Setup

The physical experimental environment comprises an AUBO-i5 collaborative robotic manipulator, an industrial workstation (Intel Core i7-10700 CPU @ 2.90 GHz, 16 GB RAM), and an external vision setup. The sensing and execution architecture is configured as follows:

• Perception and Calibration Architecture: An eye-to-hand RGB-D sensor (Intel RealSense D435) is statically mounted on an external frame overlooking the workspace. Before motion onset, target 6D grasping poses and obstacle entry geometries are acquired via our previous lightweight BEV projection pipeline. Hand-eye calibration is established via the classical Tsai-Lenz method to register camera coordinates into the robot base frame {0}.
• Operational Scope in Vision-Constrained Cavities: Crucially, during the traversal of narrow cabin apertures, optical line-of-sight is physically occluded, precluding continuous real-time visual tracking. Therefore, the vision module functions purely as a front-end initialization provider outside the aperture, while the subsequent blind traversal is fully executed by the proposed prior-guided GMM-IRRT* motion planner.
• Low-Level Control and Feedback: The workstation communicates with the AUBO-i5 control cabinet via TCP/IP protocol over an isolated industrial Ethernet network. Joint position commands derived from the cubic B-spline trajectory are dispatched at a control frequency of 50 Hz [32,33], while internal optical encoders stream real-time joint positions and velocities back at 50 Hz for trajectory tracking validation.

To replicate the narrow passage conditions evaluated in simulation, a window-like obstacle with an inner clear aperture of 40 cm × 60 cm was constructed using precision-cut high-density KT boards. This geometric aperture was designed in accordance with the physical envelope of the AUBO-i5: when Wrist 2 operates at an approximate deflection angle of 35^∘, the bounding diameter of the end-effector and link envelope spans approximately 15∼20 cm. The remaining window clearance represents a typical industrial narrow passage benchmark for evaluating obstacle avoidance and singularity evasion.

5.2. Sequential Analysis of Obstacle Avoidance and Grasping

The experimental task requires the manipulator to depart from its initial home configuration, traverse the center of the constrained window while actively avoiding kinematic singularities, and precisely grasp the target object on the opposing side. Figure 15 illustrates the sequential execution across four operational phases.

Figure 15. Sequential execution snapshots of the physical obstacle avoidance experiment on the AUBO-i5 platform: (a) Standby phase; (b) precise entry phase; (c) window traversal phase; (d) task completion phase.

• Standby Phase (Figure 15a): The manipulator is positioned on one side of the obstacle in a relaxed initial posture, with path planning completed and trajectory interpolation commands dispatched.
• Precise Entry Phase (Figure 15b): Benefiting from GMM-guided sampling, the end-effector rapidly aligns with the window centerline. The wrist assembly enters the window plane with high orientation accuracy, without oscillatory searching or trajectory deviations.
• Window Traversal Phase (Figure 15c): This phase represents the critical moment of coupled obstacle and singularity avoidance. With the elbow joint situated inside the aperture, the singularity-aware cost function maintains a high-manipulability posture, preventing motion lock caused by joint axis alignment.
• Task Completion Phase (Figure 15d): The manipulator fully emerges from the window and reaches the designated grasping point. The physical motion profile remains smooth throughout execution, validating the elimination of actuator impulse shocks via cubic B-spline optimization.

5.3. Experimental Summary

To provide rigorous empirical validation, a comprehensive quantitative benchmarking campaign was conducted. The proposed GMM-IRRT* framework was benchmarked directly against Standard RRT* across 20 independent repeated physical trials under identical hardware and environmental conditions.

The comprehensive experimental results (reported as mean ± standard deviation) are compiled in Table 4:

Table 4. Quantitative performance comparison across 20 physical window traversal trials on the AUBO-i5 platform.

Evaluation Metric	Standard RRT* (Baseline)	Ours (GMM-IRRT*)	Mechanistic Interpretation & Improvement
Physical Execution Success Rate (%)	85.0% (17/20)	100.0% (20/20)	Baseline failed in 3 trials due to emergency stops triggered by joint velocity spikes
Average Planning Time (s)	0.0815 ± 0.0072	0.0694 ± 0.0028	GMM prior corridor effectively focuses sampling into feasible manifolds
Total Traversal Duration (s)	6.85 ± 0.52	5.18 ± 0.18	Shorter path length without stochastic angular reversals
Tracking RMSE (mm)	3.58 ± 0.64	1.82 ± 0.25	Cubic B-spline C2 continuity eliminates actuator acceleration discontinuities
Min Manipulability min(w(q))	0.0068 (Near-singular)	0.0242 (Well-conditioned)	Barrier penalty successfully enforces non-singular “elbow-up” postures
Min Link-Obstacle Clearance (mm)	16.4 ± 3.1	38.2 ± 2.4	Enhanced link posture margins within the window boundary
Peak Joint Velocity (rad/s)	1.68 (Impulse spike)	0.78 (Smooth & bounded)	Active singularity evasion maintains full Jacobian rank

As demonstrated in Table 4, our framework achieved a 100% execution success rate, whereas Standard RRT* experienced a 15.0% failure rate caused by low-level servo driver overcurrent trips when approaching singular boundaries. In terms of trajectory quality, quasi-uniform cubic B-spline smoothing reduced the end-effector tracking RMSE by 49.2% (from 3.58 mm to 1.82 mm), effectively suppressing mechanical tip vibrations. Crucially, the minimum manipulability measure observed during physical passage traversal remained at 0.0242 (safely above the ε = 0.02 barrier threshold), constraining peak joint velocities within ±0.78 rad/s without sudden acceleration spikes. Furthermore, the minimum physical clearance between robot links and KT board edges was maintained at 38.2 mm, providing ample safety margin in real-world industrial deployments.

6. Conclusions and Future Work

To address the coupled challenges of low search efficiency in narrow passages and kinematic singularity risks during Cartesian motion planning, this paper proposed an improved RRT* algorithm based on a Gaussian Mixture Model (GMM-IRRT*) integrating an analytical singularity barrier cost function.

Theoretical derivations and multi-scenario evaluations establish several key conclusions:

• Statistically modeling the spatial distribution of prior demonstration paths via GMM establishes an anisotropic guidance field. While maintaining a planning success rate exceeding 98.0%, this approach shortens average planning time by 15.1% in narrow passages and up to 37.3% in complex cluttered environments (with planning time variance reduced by over 66%), effectively overcoming the Narrow Passage Paradox.
• By formulating a threshold-triggered infinite potential barrier cost based on Yoshikawa manipulability (w(q) < 0.02 → ∞), the algorithm proactively evades singular manifolds at the search source. Because a standard 6-DOF manipulator possesses zero degrees of kinematic redundancy for 6D pose tasks, this mechanism compels the planning tree to autonomously select non-singular, high-dexterity inverse kinematic configuration branches (such as the “elbow-up” posture), strictly bounding joint angular velocities within ±0.8 rad/s.
• Integrating quasi-uniform cubic B-spline interpolation guarantees C² continuity across position, velocity, and acceleration profiles, strictly bounding joint jerk within ±150 rad/s³. Extensive physical experiments across 20 repeated trials on the AUBO-i5 manipulator platform confirm a 100% execution success rate, zero velocity spikes, and a 49.2% reduction in tracking RMSE.

Future research will investigate dynamic replanning mechanisms and swarm collision avoidance for multi-robot collaborative flexible manufacturing [34,35]. Furthermore, we will explore incorporating information-theoretic divergence metrics (such as Kullback–Leibler divergence [36,37]) into continuous manifold learning to further refine probabilistic corridor estimation.