Real-Time Control of Six-DOF Serial Manipulators via Learned Spherical Kinematics

Abstract

Achieving reliable and real-time inverse kinematics (IK) for 6-degree-of-freedom (6-DoF) spherical-wrist manipulators remains a significant challenge. Analytical formulations often struggle with complex geometries and modeling errors, and standard numerical solvers (e.g., Levenberg–Marquardt) can stall near singularities or converge slowly. Purely data-driven approaches may require large networks and struggle with extrapolation. In this paper, we propose a low-latency, polynomial-based IK solution for spherical-wrist robots. The method leverages spherical coordinates and low-degree polynomial fits for the first three (positional) joints, coupled with a closed-form analytical solver for the final three (wrist) joints. An iterative partial-derivative refinement adjusts the polynomial-based angle estimates using spherical-coordinate errors, ensuring near-zero final error without requiring a full Jacobian matrix. The method systematically enumerates up to eight valid IK solutions per target pose. Our experiments against Levenberg–Marquardt, damped least-squares, and an fmincon baseline show an approximate 8.1× speedup over fmincon while retaining higher accuracy and multi-branch coverage. Future extensions include enhancing robustness through uncertainty propagation, adapting the approach to non-spherical wrists, and developing criteria-based automatic solution-branch selection.

1. Introduction

Inverse kinematics (IK) for 6-DoF robotic manipulators remains challenging. Analytical methods struggle with modeling errors and uncertainties, often requiring extensive calibration for real-world deployment. Numerical solvers depend heavily on initial joint-angle guesses and may fail to converge within practical time limits, especially near singularities. Meanwhile, purely data-driven neural networks demand large training datasets and exhibit poor generalization outside their sampled workspace.

We detail a hybrid polynomial–analytical IK method tailored for 6-DoF robots with a spherical wrist. We approximate the first three joints (positional subsystem) using polynomial fits in spherical coordinates, then solve the final wrist orientation through a closed-form technique. A short iterative process using partial-derivative evaluations from the polynomial fits refines each candidate solution to high accuracy. The algorithm updates each angle component-wise, avoiding the need for a full Jacobian-based method, and discards out-of-limit or divergent solutions. The approach systematically enumerates up to eight valid solutions, giving flexibility in workspace planning. Empirical results show superior speed, accuracy, and multi-solution coverage compared to a Levenberg–Marquardt baseline.

2. Literature Review

2.1. Classical IK Methods

Classical inverse kinematics is split between analytic closed-form solutions and iterative numerical schemes. While efficient, analytical methods typically require specialized manipulator geometry and can become infeasible with additional offsets or unusual link arrangements [1,2]. Numerical approaches such as Jacobian transpose/inverse or Levenberg–Marquardt (LM) are widely applicable but may struggle near singularities due to ill-conditioned equations [3,4,5], prompting refinements such as stabilized variants [6].

2.2. Computational Intelligence Methods

Data-driven approaches, including neural networks and fuzzy systems, address IK’s inherent nonlinearity by learning pose-to-joint mappings [7,8,9,10]. However, large datasets and memory footprints are often required to cover the manipulator’s entire workspace [11,12,13]. Extrapolation beyond the training domain remains a challenge, which can be especially problematic near singularities or mechanical limits.

2.3. Enhanced Numerical and Iterative Techniques

Refinements to classical solvers like the Crank-Nicolson approach [3] or advanced Jacobian-based formulations [5] improve stability and convergence speed; still, repeated matrix inversions can become computationally heavy for real-time tasks, and local minima or singular points can disrupt performance if no robust fallback is present [6].

2.4. Optimization and Meta-Heuristic Methods

Global search approaches such as firefly, particle swarm, or evolutionary algorithms attempt to mitigate local minima issues [14,15]. They can find solutions under complex constraints but often require significant compute time, making them less appealing for high-frequency control loops.

2.5. Hybrid Approaches for Complex IK

Research into hybrid methods combining partial geometric insights with optimization or data-driven techniques highlights possible synergy. Jin et al. [16] fused swarm optimization with partial analytics to handle singularities, while Rathnam et al. [17,18] used neural networks integrated with analytical solvers to reduce training overhead in 3D tasks. These strategies motivate our work, which pairs polynomial fittings for the first three joints with a direct wrist solver. Unlike regression-only IK mappers, the proposed approach retains analytical structure in the wrist and uses low-degree polynomials only where the kinematic decoupling is strongest, thereby reducing model size and improving extrapolation. Compared with prior hybrid schemes that still rely on global Jacobians or heavy numerical back-ends, our refinement is component-wise and avoids matrix inversions, trading a small offline fit for lighter online computation.

2.6. Task-Specific IK and Spherical Wrists

Many industrial arms adopt a spherical wrist layout, enabling simpler closed-form orientation solvers [1]. Additional specialized solutions target hardware constraints such as FPGA-based IK [19] or anchor-point modeling [20], or alternatively target specific tasks such as generative visual servoing [12]. Our approach extends this niche by providing a polynomial-based solution for the arm portion while relying on the well-understood wrist geometry.

3. Problem Statement

A robust, fast, and memory-efficient IK framework is critical for real-time 6-DoF manipulation. Classical analytical methods often fail when offsets or redundancies exist, while purely numerical solvers risk slow or non-convergent behavior near singularities. Data-driven neural networks add flexibility but can require large memory footprints and may degrade when extrapolating beyond their training domain.

Our goal is to develop an IK method that:

• Runs in Real-Time: Achieves millisecond-level solve times.
• Handles Singularity Neighborhoods: Avoids indefinite iteration or instability in kinematically challenging poses.
• Enumerates Multiple Solutions: Accounts for elbow-up/down and wrist flips, yielding up to eight distinct IK branches.

Hence, we propose a hybrid polynomial–analytical solution: the first three positional joints are solved using computationally efficient polynomial fitting on lower-dimensional spherical manifolds, whereas the wrist joints are resolved analytically. The solution is further refined using partial derivative-based iteration to ensure kinematic consistency and precision.

4. Proposed Methodology

This section outlines our hybrid polynomial–analytical inverse kinematics (IK) method, showing how four distinct polynomials address the first three (arm) joints $({\theta }_0,{\theta }_1,{\theta }_2)$. We further explain the iterative refinement strategy, which applies partial-derivative corrections for final precision. Joints 4–6 (the spherical wrist) are solved through a standard closed-form process.

4.1. Offline Phase: Data Collection and Polynomial Fitting

The offline phase systematically collects and prepares data to create four polynomial models, capturing the relationship between the first three joint angles $({\theta }_0,{\theta }_1,{\theta }_2)$ and the end-effector spherical coordinates (azimuth, elevation, and radial distance). The collected data ensure accurate polynomial fits, enabling multiple valid solutions during the subsequent online inverse kinematics phase.

Rationale for Targeted Sweeps in Reduced Manifolds

By decomposing the workspace into lower-dimensional manifolds (azimuth, elevation, radial distance) and conducting targeted sweeps rather than a brute-force 3D grid search, we achieve the following:

• Data efficiency:

  –

  Fewer samples required ($O\left(3N\right)$ vs $O\left(N^3\right)$)

  –

  Higher resolution per parameter with sparse sampling

• Improved joint-to-workspace mapping, resulting in:

  –

  Noise reduction through decoupled 1D/2D polynomial fits

  –

  Explicit coverage of elbow-up/down configurations

• Manifold-aware sampling:

  –

  Natural alignment with spherical kinematics

  –

  Built-in singularity detection via $\mathsf{\partial }r/\mathsf{\partial }{\theta }_i$

Outcome. This approach yields a compact dataset that enables:

• Numerically stable polynomial inversion during online IK
• Multiple valid solutions with guaranteed physical feasibility
• Real-time performance through reduced computational complexity

Observation. For a robot with n joints, the minimum required samples scale as $O\left(n\right)$ per manifold rather than $O\left(k^n\right)$ for grid sampling, while still spanning the solution space through

$${\mathcal{M}}_{\mathrm{reduced}}=\bigcup _{i=1}^k{\mathcal{M}}_i,\mathrm{where}dim\left({\mathcal{M}}_i\right)\le 2$$

(1)

under the assumption that the positional subspace decouples as in (2). The refinement process in the online stage then converges to the same solutions as grid-based initialization for the sampled workspace.

Algorithm 1 summarizes the offline data collection and polynomial fitting procedure.

Algorithm 1 Data Collection and Polynomial Fitting

1: Denavit–Hartenberg Modeling and Robot Setup Define Denavit–Hartenberg (DH) parameters for joints ${\theta }_0,{\theta }_1,{\theta }_2$, specifying link lengths, twists, and joint limits $({\theta }_{\min },{\theta }_{\max })$. Apply base transformation if the robot’s base is offset from the global origin for consistent spherical reference coordinates $(az,el,r)$. Use sign-modification array to capture elbow-up/down and negative angle configurations. 2: Targeted Sweeps for Data Collection Azimuth vs. ${\theta }_0$: vary ${\theta }_0$ within $[-{\theta }_{az},{\theta }_{az}]$, keeping other joints nominal. Elevation vs. ${\theta }_1$: sweep ${\theta }_1$ between $[-{\theta }_{el},{\theta }_{el}]$. Radial distance vs. ${\theta }_1,{\theta }_2$: systematically vary between $(r_{min},r_{max})$. Compute forward kinematics for each configuration to get Cartesian $(x,y,z)$. Convert Cartesian coordinates $(x,y,z)$ to spherical $(az,el,r)$. Adjust offsets for continuity and exclude invalid or discontinuous data points. Typical sampling used in this study: ${\theta }_0\in [-\pi ,\pi ]$ (about 2° steps); ${\theta }_1\in [-\pi /2,\pi /2]$ (about 2° steps); ${\theta }_2$ swept to span the robot’s feasible radii between $r_{min}$ and $r_{max}$. 3: Polynomial Fitting Organize data subsets: – Azimuth subset: ${\theta }_0$ vs. $az$ – Elevation subset: ${\theta }_1$ vs. $el$ – Radial subsets: ${\theta }_1$ vs. r, and ${\theta }_2$ vs. r Fit polynomials: $$p_{az}\left({\theta }_0\right)\approx az,p_{el}\left({\theta }_1\right)\approx el,p_{r1}\left({\theta }_1\right)\approx r,p_{r2}\left({\theta }_2\right)\approx r$$ Compute real roots with MATLAB’s roots function; discard complex or out-of-limit candidates before refinement. Store polynomial coefficients. 4: Polynomial Derivatives and Singularity Identification Compute polynomial derivatives ${\displaystyle \frac{d}{d{\theta }_i}}\left[p_{ri}\left({\theta }_i\right)\right]$. Mark derivative roots as singularities to manage numerical stability during runtime. 5: Validation and Data Storage Save polynomial coefficients, derivatives, and joint limits in a structured file.

Algorithm 2 summarizes the online inverse kinematics procedure (polynomial root finding, partial-derivative refinement, and solution filtering).

Algorithm 2 6-DoF Spherical-Wrist IK with Four Polynomials, Partial-Derivative Refinement, and Multi-Solution Handling

1: Input: Target position $(x,y,z)$, orientation $R_{\mathrm{target}}$, and tolerance $\mathtt{tol}$. 2: Local Spherical Preprocessing:  Compute $(az,el,r)$ via cart2sph($x,y,z$).  Apply base transform so that ${\theta }_0$ (azimuth) can be set to 0 reference.  If $r\ge$ max_reach, no solutions. 3: Root Finding for Polynomials:  Solve $p_{az}\left({\theta }_0\right)-az=0$ for valid ${\theta }_0$ roots (via MATLAB roots).  Solve $p_{el}\left({\theta }_1\right)-el=0$ for primary ${\theta }_1$ candidates (via roots).  Solve $p_{r1}\left({\theta }_1\right)-r=0$ for $∆{\theta }_1$ (radial correction), add to each ${\theta }_1$ candidate (i.e., ${\theta }_1+∆{\theta }_1$).  Solve $p_{r2}\left({\theta }_2\right)-r=0$ for valid ${\theta }_2$ roots (via roots).  Form all {${\theta }_0,{\theta }_1,{\theta }_2$} combos in mechanical limits. 4: Iterative Refinement (Partial-Derivative Method): 5: for each $({\theta }_0,{\theta }_1,{\theta }_2)$ combo do 6:     loop up to n iterations:  Compute forward kinematics $\to (x_f,y_f,z_f)$, convert to local $(a{z}_f,e{l}_f,r_f)$.  $\mathbf{e}\leftarrow (az-a{z}_f,el-e{l}_f,r-r_f)$  Update ${\theta }_0+=\mathrm{err}\left(\mathrm{az}\right)$  Update ${\theta }_1+=\mathrm{err}\left(\mathrm{el}\right)+{\displaystyle \frac{\mathrm{err}\left(r\right)}{\frac{dr}{d{\theta }_1}}}$ (if derivative is nonzero)  Update ${\theta }_2+={\displaystyle \frac{\mathrm{err}\left(r\right)}{\frac{dr}{d{\theta }_2}}}$ (if derivative is nonzero)  If $\parallel \mathbf{e}\parallel <\mathtt{tol}$, mark solution as valid and break 7:     Store solution if converged; else discard 8: end for 9: Wrist Solutions:  Compute up to 2 solutions $({\theta }_4,{\theta }_5,{\theta }_6)$ from $f_{\mathrm{wrist}}^{-1}\left(R_{\mathrm{target}}\right)$.  Discard any out-of-limit wrist angles. 10: Combine & Filter:  For each valid arm solution, pair with each valid wrist solution  Discard solutions that fail final checks or exceed joint limits 11: Output: Return up to $2\times 2\times 2=8$ 6-DoF solutions (when multiple real roots arise).

4.2. Mathematical Validity of Reduced Sampling

4.2.1. Chain Rule in Spherical Coordinates

The decomposition is valid because spherical coordinates $(az,el,r)$ naturally separate the kinematic relationships:

$${\displaystyle \frac{\mathsf{\partial }(az,el,r)}{\mathsf{\partial }({\theta }_0,{\theta }_1,{\theta }_2)}}=\left[\begin{array}{ccc}{\displaystyle \frac{\mathsf{\partial }az}{\mathsf{\partial }{\theta }_0}}& 0& 0\\ 0& {\displaystyle \frac{\mathsf{\partial }el}{\mathsf{\partial }{\theta }_1}}& 0\\ 0& {\displaystyle \frac{\mathsf{\partial }r}{\mathsf{\partial }{\theta }_1}}& {\displaystyle \frac{\mathsf{\partial }r}{\mathsf{\partial }{\theta }_2}}\end{array}\right]$$

(2)

This triangular structure emerges because:

• Azimuth ($az$) depends only on the base rotation ${\theta }_0$.
• Elevation ($el$) depends primarily on the shoulder joint ${\theta }_1$ for the spherical wrist considered here; coupling terms are negligible in the sampled workspace.
• Radius (r) depends on the planar arm configuration (${\theta }_1,{\theta }_2$).

4.2.2. Block-Diagonal Jacobian Structure

The Jacobian matrix in $\theta$-space has the following block-diagonal form:

$$J=\left[\begin{array}{ccc}{J}_{az}& 0& 0\\ 0& J_{el}& 0\\ 0& J_{r1}& J_{r2}\end{array}\right],\mathrm{where}\left\{\begin{array}{c}{J}_{az}={\displaystyle \frac{\mathsf{\partial }az}{\mathsf{\partial }{\theta }_0}}\hfill \\ J_{el}={\displaystyle \frac{\mathsf{\partial }el}{\mathsf{\partial }{\theta }_1}}\hfill \\ J_{ri}={\displaystyle \frac{\mathsf{\partial }r}{\mathsf{\partial }{\theta }_i}}\hfill \end{array}\right.$$

(3)

For a spherical wrist, the orientation subspace decouples from the first three joints, justifying this block-diagonal layout. This structure guarantees that:

• Joint corrections can be computed independently per subspace
• Null spaces do not interact across different joints

4.2.3. Completeness of Spherical Basis Functions

The spherical-coordinate basis vectors form a complete basis for ${\mathbb{R}}^3$:

$$\mathcal{B}=\{\hat{r},\widehat{az},\widehat{el}\}\mathrm{satisfies}\mathrm{span}\left(\mathcal{B}\right)={\mathbb{R}}^3.$$

(4)

With $\varphi =az$ and $\theta =el$, the basis vectors are:

• Azimuthal basis: $\widehat{az}=(-sin\varphi ,cos\varphi ,0)$
• Elevation basis: $\widehat{el}=(cos\theta cos\varphi ,cos\theta sin\varphi ,-sin\theta )$
• Radial basis: $\hat{r}=(sin\theta cos\varphi ,sin\theta sin\varphi ,cos\theta )$

This ensures all reachable workspace points can be expressed through the reduced coordinates.

Conclusions: The reduced sampling is mathematically equivalent to 3D grid sampling because:

• The chain rule justifies independent parameter sweeps.
• The Jacobian structure permits separate error correction.
• The spherical basis guarantees complete workspace coverage.

4.3. Online Phase: Polynomial Inversion + Partial-Derivative Refinement + Wrist Solutions

• Partial-Derivative Refinement.

During the online loop, a global Jacobian is neither formed nor inverted. Instead, each joint angle is updated based on spherical-coordinate errors. For instance, ${\theta }_0$ uses the azimuth error and ${\theta }_1,{\theta }_2$ incorporate partial derivatives ${\displaystyle \frac{\mathsf{\partial }r}{\mathsf{\partial }{\theta }_i}}$ to correct the radial dimension. If any derivative is near zero, the code omits or clamps that update to avoid numerical instability.

• Overall Flow.

Offline: (1) sample $({\theta }_0,{\theta }_1,{\theta }_2)$ on the spherical manifolds; (2) fit low-order polynomials $p_{az},p_{el},p_{r1},p_{r2}$; (3) store coefficients and partial derivatives. Online: (1) convert the target pose to spherical coordinates; (2) solve the four polynomials for the arm roots; (3) refine each arm root with the per-joint partial-derivative updates (clamping/skipping near singularities); (4) solve the wrist in closed form; (5) filter joint limits, validate via forward kinematics, and return up to eight solutions. This flow keeps the online loop lightweight and hardware-friendly.

4.4. Final Multi-Solution Output

If multiple real roots exist for ${\theta }_0,{\theta }_1,{\theta }_2$, the solver can produce up to four valid arm configurations; coupled with two wrist solutions, up to eight full 6-DoF solutions may emerge. Any out-of-range angles or nonconvergent results are discarded, leaving the final set of valid solutions. In many typical poses, fewer than eight survive; still, the framework is designed to systematically account for all potential branches.

5. Results and Performance Analysis

To evaluate the performance of the proposed inverse kinematics (IK) method, a comprehensive comparison was conducted against a standard numerical optimization-based IK solver. This section details the experimental setup for the benchmark and presents a detailed analysis of the results.

5.1. Benchmark: Optimization-Based IK Solver Setup

The benchmark solver was designed to find the optimal joint configuration for the 6-DOF KUKA Rhino manipulator. The optimization problem is formulated to find the joint configuration $q\in {\mathbb{R}}^6$ that minimizes a weighted sum of the squared position and orientation errors. Given a desired end-effector pose $T_d=(R_d,p_d)\in SE\left(3\right)$ and the robot’s forward kinematics map $T\left(q\right)=\left(R\right(q),p(q\left)\right)$, the objective function is defined as

$$\underset{q}{\mathrm{minimize}}{\lambda }_p{\parallel p\left(q\right)-p_d\parallel }_2^2+{\lambda }_o·{\theta }^2(R\left(q\right),R_d),$$

(5)

where $\parallel p\left(q\right)-p_d{\parallel }_2^2$ is the squared Euclidean distance for position and $\theta (R\left(q\right),R_d)$ is the angular error in radians, calculated as the angle of the relative rotation matrix $R\left(q\right)R_d^T$. As such, the target pose $T_d$ contains the desired rotation $R_d$ and position $p_d$. The weighting factors ${\lambda }_p$ and ${\lambda }_o$ balance the contribution of the position and orientation components. This minimization is subject to box constraints defined by the KUKA Rhino’s joint limits ($q_{min}\le q\le q_{max}$) and a nonlinear inequality constraint to ensure solution smoothness by limiting the maximum joint displacement between consecutive configurations to 0.2 radians ($\parallel q_k-q_{k-1}{\parallel }_{\infty }\le 0.2$). The problem was solved using MATLAB’s fmincon function with a sequential quadratic programming (SQP) algorithm configured with an optimality tolerance of ${10}^{-8}$ and a constraint tolerance of ${10}^{-6}$. A multi-start strategy was employed, initiating five searches per trajectory point: one warm start from the previous solution and four from random configurations. All computations were performed on a workstation equipped with an AMD Ryzen 5 7000 series processor (six cores, twelve threads, up to 4.5 GHz) and 16 GB of DDR5 RAM while running MATLAB R2023b.

5.2. Trajectory Tracking Performance

The proposed custom IK method demonstrated exceptional precision, achieving a mean and maximum tracking error of effectively zero (<10⁻⁶ m) across all 200 trajectory points, as illustrated in Figure 1 and Figure 2. In contrast, the optimization-based solver, while maintaining a low median error of $1.67\times {10}^{-8}$ m, exhibited significant spikes, with a maximum deviation of 0.2223 m whenever it fell into a local minimum. The LM baseline achieved very small medians ($1.55\times {10}^{-9}$ m) but occasionally diverged to 2.62 m near singularities; the DLS baseline was stable and low-error (max $9.74\times {10}^{-7}$ m) but returned only a single branch. These observations highlight the superior accuracy and reliability of the proposed solver across the workspace.

Figure 1. Three—dimensional visualization of the target trajectory versus paths achieved by all IK solvers (Custom, fmincon, LM, DLS).

Figure 2. Comparative IK tracking accuracy over the trajectory (custom, fmincon, LM, DLS).

5.3. Computational Efficiency

As shown in Figure 3, the proposed custom IK was substantially more efficient. It recorded a mean computation time of 10.48 ms per point (median 9.01 ms; 95th 14.07 ms). The optimization-based solver averaged 85.32 ms per point (median 77.01 ms; 95th 125.09 ms). DLS was the fastest (median 3.49 ms; 95th 7.07 ms), but is single-branch; LM showed wide timing dispersion (median 7.90 ms; 95th 172.49 ms) and intermittent large errors. The custom method remains the best balance of speed, accuracy, and multi-branch coverage.

Figure 3. Computation time comparison of custom, fmincon, LM, and DLS.

5.4. Complexity and Baseline Context

The offline stage fits low-order polynomials (degree 3–4) per joint manifold. Online, each candidate arm solution is refined with a small and fixed number of iterations (typically <10) using only per-joint partial derivatives and no matrix inversions, meaning that runtime scales linearly with the number of candidate branches. We also tested Levenberg–Marquardt (LM) and a simple damped least-squares (DLS) baseline; LM remained sensitive near singularities (wide error and time dispersion), while DLS was fast but returns only a single branch and has higher residuals than the custom solver. A closed-form solver for this arm exists and aligns with our wrist solution; for completeness, it is discussed as a future comparative baseline.

5.5. Error Variability and Singularities

Across the 200-point trajectory, the positional error distribution for the custom IK had a standard deviation below ${10}^{-4}$ m, with a maximum error below ${10}^{-6}$ m; the optimization baseline showed a standard deviation on the order of ${10}^{-2}$ m, consistent with the 0.2223 m spikes. The refinement loop omits or clamps updates when $\mathsf{\partial }r/\mathsf{\partial }{\theta }_i\approx 0$, preventing numerical blow-up near singularities; near such poses, the method may return fewer valid branches (reflected in Figure 4) but retains finite errors.

Figure 4. Distribution of solution counts for the custom inverse kinematics solver.

5.6. Joint Space Behavior and Continuity

An examination of the joint-space paths reveals notable differences in smoothness and continuity. The custom IK produced smooth and continuous joint trajectories, characteristic of a solution that implicitly minimizes joint velocities. The optimization-based solver’s trajectories were less smooth and had occasional oscillations, suggesting solutions that, while locally optimal for the pose, may be suboptimal in terms of joint-space smoothness.

5.7. Custom IK Solver Characteristics

As depicted in Figure 4, the proposed IK solver consistently identified a small number of distinct inverse kinematics solutions for each evaluated point. With a mean solution count of 2.6, this finding suggests a robust and well-defined analytical process, yielding a predictable and manageable set of kinematic configurations for a given end-effector pose.

5.8. Optimization Solver Diagnostics and Solution Quality

Analysis of the optimization solver’s diagnostics (Figure 5) provides key insights into its behavior. The solver successfully found a solution for all 200 points. While gradient norms were generally low, indicating convergence, the solver converged to a suboptimal local minimum (i.e., a solution with higher tracking error than the custom IK’s) in 3.5% of cases (7 out of 200). In these same instances, the solver failed to meet the optimality tolerance, indicated by high final gradient norms. Encouragingly, no constraint violations were observed, demonstrating the solver’s effectiveness at adhering to joint limits.

Figure 5. Optimization solver diagnostics: solution quality, local minima, constraint adherence, and multi-start results.

5.9. Time–Error Tradeoff

The time-=error tradeoff plot in Figure 6 provides a clear summary of the performance comparison. The custom IK occupies the ideal region of the plot, consistently delivering high accuracy (low error) at very low computational cost. The optimization-based solver’s results are scattered, showing both higher computation times and wide variance in the tracking error. For LM, the points are concentrated near low error, but include several high-error and high-time outliers. DLS clusters at low time and modest error, but only yields one branch. Overall, the custom method remains the best choice for this application.

Figure 6. Time—error tradeoff: performance comparison of custom, fmincon, LM, and DLS.

5.10. Summary of Performance Metrics

Clarifications. Custom positional errors are bounded by the solver tolerance (${10}^{-6}$ m); the mean/median/percentile values reported come from forward-kinematics validation over 200 points (max $4.14\times {10}^{-6}$ m). “Mean solutions/point” counts distinct feasible IK branches per pose after removing out-of-limit or duplicate configurations. “Validated solutions” denotes the fraction of targets producing a feasible branch that passes the final FK check (200/200 for all methods; LM statistics exclude candidate solves with >${10}^{-2}$ m residual before validation). Timings measure solver runtime only (no plotting or I/O) and use a warm-started optimizer (previous solution plus four random initializations) on the AMD Ryzen 5/16 GB workstation noted above; MATLAB’s default PRNG was used without a fixed seed. Optimization median errors are very small but exhibit rare spikes to 0.2223 m, captured by the max entry. Levenberg–Marquardt (RVC ikine) remains sensitive near singularities (max error 2.62 m despite low median), while a simple damped least-squares baseline converged quickly (median 3.49 ms) but with higher residuals than the custom solver while returning only one branch. Local-minima flags for fmincon, LM, and DLS indicate cases in which the final residual exceeded the custom solution; constraint checks were instrumented for fmincon and replayed on the returned LM/DLS trajectories without detecting limit violations.

The overall performance metrics are summarized in Table 1.

Table 1. Summary of performance metrics for the 200-point trajectory.

Metric	Custom	fmincon (Opt)	LM	DLS
Mean Positional Error (m)	$1.64\times {10}^{-6}$	0.0045	$1.55\times {10}^{-8}$	$8.40\times {10}^{-7}$
Median Positional Error (m)	$1.52\times {10}^{-6}$	$1.67\times {10}^{-8}$	$1.55\times {10}^{-9}$	$8.47\times {10}^{-7}$
95th-Percentile Error (m)	$3.16\times {10}^{-6}$	$2.84\times {10}^{-8}$	$7.55\times {10}^{-5}$	$9.74\times {10}^{-7}$
Max Positional Error (m)	$4.14\times {10}^{-6}$	0.2223	2.62	$9.74\times {10}^{-7}$
Mean Time/Point (ms)	10.48	85.32	79.00	3.49
Median Time/Point (ms)	9.01	77.01	7.90	3.49
95th-Percentile Time (ms)	14.07	125.09	172.49	7.07
Total Time (s)	2.10	17.06	15.80	0.70
Mean Solutions/Point	2.6	1 (single branch)	1	1
Validated Solutions	200/200	200/200	200/200 (after filtering)	200/200
Local Minima Cases	0/200	7/200 (3.5%)	48/200 (24.0%)	42/200 (21.0%)
High Gradient Cases	0/200	7/200 (3.5%)	0/200	0/200
Constraint Violations	0/200 (0.0%)	0/200 (0.0%)	0/200	0/200

6. Discussion

The results demonstrate the superior performance of the proposed custom IK solver over the generalized optimization-based approach for the KUKA Rhino manipulator. The custom method’s ability to achieve near-perfect tracking accuracy with significantly lower computational overhead highlights its efficiency and reliability. While the optimization solver is robust in its ability to find a valid solution for every point, its performance is inconsistent: rare local minima produce error spikes up to 0.22 m despite sub-centimeter medians, and mean computation time is about eight-fold higher. The custom solver’s roughly 8.1× speedup, tight timing dispersion (median 9.01 ms, 95th percentile 14.07 ms), and predictable multi-branch coverage (mean of 2.6 distinct solutions per pose) make it better suited for real-time control and motion planning. DLS attains slightly lower residuals at a single branch, but lacks branch diversity, while LM occasionally matches the custom accuracy but diverges near singularities. Furthermore, the smoother joint-space trajectories generated by the custom solver are highly desirable for practical robotics, leading to reduced mechanical stress and more fluid robot motion. In summary, for this specific manipulator, the proposed method provides a faster, more accurate, and more reliable solution.

Limitations and Future Work

This study is focused on a specific robotic manipulator and trajectory. Future work could investigate the performance of the solvers on different robot configurations, more complex trajectories, and in the presence of noise or uncertainties. Further research could also explore methods to improve the robustness and efficiency of optimization-based IK solvers, such as incorporating problem-specific knowledge into the optimization algorithm or developing more efficient global optimization techniques. Extension to non-spherical wrists or redundant manipulators would require coupling terms in (2), and may motivate additional manifolds or lightweight Jacobian regularization; we leave this to a follow-up paper focused on non-spherical arms. Near singularities, the refinement step clamps or skips updates when $\mathsf{\partial }r/\mathsf{\partial }{\theta }_i$ is small and relies on multiple root candidates to switch branches safely; incorporating a damped least-squares wrist fallback is a planned robustness extension. For deployment, a lightweight embedded/FPGA implementation is feasible because the online loop uses only polynomial evaluations, partial-derivative updates, and a closed-form wrist solver; porting and benchmarking on embedded hardware are planned as future work.

7. Conclusions

The custom IK solver significantly outperformed the optimization-based solver in terms of accuracy, computational efficiency, and joint space smoothness for the tested robotic system. While the optimization-based solver demonstrated robustness in finding solutions, its limitations in terms of accuracy, smoothness, and speed highlight the advantages of a specialized custom IK algorithm that leverages the specific kinematic structure of the robot.