Special case: $m=n$

Here, ${J}\in \mathbb{R}^{n\times n}$ is square. Consider

(10) \begin{equation} \bar {\mathbf{F}}_{\square }(\bar {\mathbf{q}})= \begin{bmatrix} \operatorname {adj}{J}(\mathbf{q})\,\mathbf{S}(p)\\[2pt] \det {J}(\mathbf{q}) \end{bmatrix}\in \mathbb{R}^{n+1}. \end{equation}

where the subscript $\square$ emphasizes that this form applies specifically to the square-Jacobian case ( $m = n$ ). Using ${J}\,\operatorname {adj}{J}=\det\!({J})\,\mathbf{I}$ ,

\begin{equation*} \mathbf{H}\,\bar {\mathbf{F}}_{\square } ={J}\,\operatorname {adj}{J}\,\mathbf{S}-\det\!({J})\,\mathbf{S} =\big (\det\!({J})\,\mathbf{I}-\det\!({J})\,\mathbf{I}\big )\mathbf{S}=\mathbf{0}, \end{equation*}

thus $\bar {\mathbf{F}}_{\square }\in \ker \mathbf{H}$ . Define the singularity-consistent first-order step

(11) \begin{equation} d\bar {\mathbf{q}} = b\,\bar {\mathbf{F}}_{\square }(\bar {\mathbf{q}}),\qquad b\in \mathbb{R}\ \text{small}, \end{equation}

which automatically satisfies (7). As $\det {J}\to 0$ , (11) yields $dp=b\,\det {J}\to 0$ , avoiding blow-ups in $d\mathbf{q}$ or $dp$ – the desired “singularity-consistent” behavior.

Properties. If $\operatorname {rank}{J}=n-1$ , then $\operatorname {adj}{J}$ has rank $1$ and $\det {J}=0$ ; (11) reduces to $dp=0$ with a nonzero $d\mathbf{q}\in \ker {J}$ . If $\operatorname {rank}{J}\le n-2$ , then $\operatorname {adj}{J}=\mathbf{0}$ and a numerical null space (SVD) of $\mathbf{H}$ is required.

General case: $m\neq n$

To avoid the square-Jacobian assumption behind $\det\!({J})$ , define

(12) \begin{equation} \mathbf{A}(\mathbf{q})={J}(\mathbf{q})\,{J}^\top (\mathbf{q})\in \mathbb{R}^{m\times m},\qquad \alpha (\mathbf{q})=\det \big (\mathbf{A}(\mathbf{q})\big )\ (\ge 0), \end{equation}

and construct

(13) \begin{equation} \bar {\mathbf{F}}(\bar {\mathbf{q}})= \begin{bmatrix} {J}^\top (\mathbf{q})\,\operatorname {adj}\!\big (\mathbf{A}(\mathbf{q})\big )\,\mathbf{S}(p)\\[2pt] \det \big (\mathbf{A}(\mathbf{q})\big ) \end{bmatrix}\in \mathbb{R}^{n+1}. \end{equation}

Since $\mathbf{A}\,\operatorname {adj}\mathbf{A}=\det\!(\mathbf{A})\,\mathbf{I}_m$ ,

\begin{equation*} \mathbf{H}\,\bar {\mathbf{F}} ={J}\,{J}^\top \operatorname {adj}\!(\mathbf{A})\,\mathbf{S}-\det\!(\mathbf{A})\,\mathbf{S} =\big (\mathbf{A}\operatorname {adj}\mathbf{A}-\det\!(\mathbf{A})\mathbf{I}_m\big )\mathbf{S}=\mathbf{0}, \end{equation*}

hence $\bar {\mathbf{F}}\in \ker \mathbf{H}$ and

(14) \begin{equation} d\bar {\mathbf{q}} = b\,\bar {\mathbf{F}}(\bar {\mathbf{q}}),\qquad b\in \mathbb{R}\ \text{small}. \end{equation}

Consistency with the square case. When $m=n$ , using $\operatorname {adj}\!({J}{J}^\top )=\operatorname {adj}\!({J})^\top \operatorname {adj}\!({J})$ together with ${J}^\top \operatorname {adj}\!({J}{J}^\top )=\det\!({J})\,\operatorname {adj}\!({J})$ and $\det\!({J}{J}^\top )=\det\!({J})^2$ , we obtain

\begin{equation*} \bar {\mathbf{F}} = \det\!({J})\, \begin{bmatrix}\operatorname {adj}{J}\,\mathbf{S}\\ \det {J}\end{bmatrix}, \end{equation*}

which is colinear with (10) and thus identical as a kernel direction.

Computation and robustness

1. (i) If $\mathbf{A}$ is nonsingular, use

   \begin{equation*} \bar {\mathbf{F}}= \begin{bmatrix} \det\!(\mathbf{A})\,{J}^\top \mathbf{A}^{-1}\mathbf{S}\\[2pt] \det\!(\mathbf{A}) \end{bmatrix} \end{equation*}
   to avoid forming adjugates;

2. (ii) at generic singularities with $\operatorname {rank}{J}=m-1$ , $\operatorname {adj}\mathbf{A}$ has rank $1$ and $\det \mathbf{A}=0$ , yielding $dp=0$ and a nonzero feasible $d\mathbf{q}$ ;

3. (iii) at higher-order singularities ( $\operatorname {rank}{J}\le m-2$ ), $\operatorname {adj}\mathbf{A}=\mathbf{0}$ , and one should compute a null vector of $\mathbf{H}$ via SVD;

4. (iv) with a joint space metric/weight $\mathbf{W}\succ 0$ , take $\mathbf{A}={J}\mathbf{W}{J}^\top$ and replace (13) accordingly.

Remark. The method only relies on the null-space relation $\mathbf{H}\,\bar {\mathbf{F}}=\mathbf{0}$ and never requires $\det\!(\mathbf{H})$ . If a scalar conditioning index is desired, use $\mathbf{H}\mathbf{H}^\top ={J}{J}^\top +\mathbf{S}\mathbf{S}^\top$ and inspect its singular values or $\det\!(\mathbf{H}\mathbf{H}^\top )$ .

By analyzing the structure of $ \bar {\mathbf{F}}(\bar {\mathbf{q}})$ , it becomes clear that $ \mathbf{F}(\bar {\mathbf{q}})$ combines the joint angle variations and the path parameter $ p$ through the augmented parameter vector $ \bar {\mathbf{q}}$ . This coupling enables $ \mathbf{F}(\bar {\mathbf{q}})$ playing a singularity-consistent role in the solution process. When the manipulator’s joint configuration approaches a Jacobian singularity (i.e., $ \det {J}(\mathbf{q}) \to 0$ ), the singularity is reflected in the augmented Jacobian matrix $ \mathbf{H}(\bar {\mathbf{q}})$ . Consequently, the system automatically adjusts the Cartesian sampling along the path parameter $ p$ , ensuring consistent behavior even in the presence of singularities.

By splitting Eq. (9) into two components, we obtain the following expressions for the joint differential motion and the path parameter differential:

(15) \begin{align} d\mathbf{q} = b \, \mathbf{f}(\bar {\mathbf{q}}), \\[-30pt] \nonumber \end{align}

(16) \begin{align} dp = b \, \det {J}(\mathbf{q}) \, or\, b \, \det {A}(\mathbf{q}). \\[2pt] \nonumber \end{align}

To ensure that the manipulator can traverse through the singular region while respecting the joint’s maximum speed limits, we define the joint velocity adjustment factor $ b^*$ as the maximum allowable scaling factor for the joint velocities. Based on the current joint configuration and velocity limits, $ b^*$ is computed as:

(17) \begin{equation} b^* = \max \left \{ b \, \middle | \, b \cdot \|\mathbf{F}(\bar {\mathbf{q}})\| \leq dq_{\text{max}} \right \}, \end{equation}

where:

• • $ dq_{\text{max}}$ represents the maximum allowable joint velocity, determined by the physical constraints of the manipulator.

• • $ \|\mathbf{F}(\bar {\mathbf{q}})\|$ is the norm of the joint velocity component derived from the mapping $ \mathbf{F}(\bar {\mathbf{q}})$ , which reflects the influence of both the Jacobian singularity and the path parameter $ p$ .

The adjustment factor $ b^*$ ensures that the joint velocities remain within their feasible limits, even as the manipulator approaches a singular configuration. When $ \|\mathbf{F}(\bar {\mathbf{q}})\|$ becomes large near a singularity, $ b^*$ is automatically reduced to prevent the joint velocities from exceeding $ dq_{\text{max}}$ . This mechanism allows the manipulator to pass through the singular region in a controlled and safe manner.

2.2.1. Path arc-length parameterization

While curves can be mathematically represented using path parameters, the variation of the cumulative arc length with respect to the path parameter is generally non-uniform. As illustrated in Figure 2, the arc length $L(t)$ of both Bézier and Lissajous curves is not linearly related to the parameter $t$ (the red curve deviates significantly from the line $y=x$ ). The cumulative arc length $L(t)$ is defined as:

(18) \begin{equation} L(t) = \int _{0}^{t} ||\mathbf{C}'(u)|| \, du \end{equation}

where $\mathbf{C}(t)$ represents the parametric equation of the curve. Due to the integral, uniform variation in $t$ does not guarantee a uniform increase in $L(t)$ . Consequently, uniform sampling of the path parameter $t$ may result in irregular variations of the differential speed, approximated as $\frac {\Delta L}{\Delta t}$ . As shown in the right subplots of Figure 2, the speed curves demonstrate significant and non-smooth fluctuations, indicating that uniform sampling in the parameter domain does not guarantee uniformity in the arc-length domain.

Figure 2.

Path arc length and speed profiles. Top row: Bézier curve example. Bottom row: Lissajous curve example. Left: The geometric paths (blue solid line) and their control points (red dashed line for the Bézier curve). Middle: The arc-length function $L(t)$ (red solid line) and the ideal linear relationship $y=x$ (black dashed line). Right: The corresponding sampled differential speed profile. The results demonstrate that the arc-length function $L(t)$ deviates from the ideal linear relationship, indicating a non-uniform parameterization with respect to arc length. This non-uniformity is also reflected in the fluctuating speed profiles.

To address the non-uniformity challenge of the curve’s arc length, a natural approach is to introduce the path arc length as the path parameter. This involves reparameterizing the path using the arc length $s$ as the new parameter. For a path parameterized by $\sigma$ in joint space, the arc length $s$ is given by:

(19) \begin{equation} s = \int _0^\sigma \|\mathbf{q}'\|_M \, d\xi , \end{equation}

where $\|\mathbf{q}'\|_M = \sqrt {\mathbf{q'}^T M \mathbf{q}'}$ denotes the weighted Euclidean norm of the joint velocity, $M$ is a metric matrix (assumed to be the identity matrix for simplicity), and $\mathbf{q}' = \frac {d\mathbf{q}}{d\sigma }$ represents the derivative of the joint coordinates with respect to the original path parameter.

By parameterizing the path with respect to arc length, we have:

(20) \begin{equation} \frac {d s}{d \sigma } = \|\mathbf{q}'\|_M. \end{equation}

This implies that equal increments in $s$ correspond to equal distances traversed along the curve, regardless of the original parameterization. Consequently, sampling uniformly in $s$ yields points that are equidistant along the path in joint space. This method, known as arc-length parameterization, effectively transforms a non-uniformly parameterized path into a uniformly parameterized one with respect to arc length, ensuring consistent and predictable motion along the curve. The resulting parameterization $\mathbf{q}(s)$ provides a more natural and intuitive representation of the path, facilitating tasks such as trajectory planning and control.

It is worth noting that for curves where analytical integration is difficult, a discrete sampling approach combined with numerical integration can be employed to approximate the arc length. Common methods for equidistant sampling include Simpson’s rule, the Trapezoidal rule, and other numerical integration techniques, allowing us to obtain approximately uniformly spaced points with respect to arc length.

From this, we derive:

(21) \begin{equation} \frac {\partial s}{\partial \sigma } = \|\mathbf{q}'\|_M, \end{equation}

and consequently:

(22) \begin{equation} \frac {\partial \sigma }{\partial s} = \frac {1}{\|{J}^+ \mathbf{h}\|_M} = \frac {1}{\|\mathbf{q}'\|_M}, \end{equation}

(23) \begin{align} \frac {\partial \mathbf{q}}{\partial s} = \frac {{J}^+ \mathbf{h}}{\|{J}^+ \mathbf{h}\|_M} = \frac {\mathbf{q}'}{\|\mathbf{q}'\|_M}, \\[8pt] \nonumber \end{align}

These equations describe how the arc length, the original parameter, and the joint space coordinates change with respect to each other. They are fundamental to understanding and implementing arc-length parameterization.

2.2.2. Curvature modeling

While arc-length parameterization provides a uniform representation of the geometric path in Cartesian space, another crucial geometric variable must be considered: the path curvature. Although sampling uniformly with respect to arc length ensures equidistant points along the path, neglecting the path’s curvature can lead to subtle deformations in the robot’s motion during trajectory tracking. This is because curvature directly influences the required joint accelerations and velocities. Consequently, ignoring curvature can compromise the robot’s ability to precisely adhere to the intended geometric path, especially in regions with high curvature. Therefore, a comprehensive understanding of the path’s behavior requires considering both arc length and curvature to ensure accurate and compliant motion.

For a given path parameterized by $\mathbf{q}(\sigma )$ , the curvature $\kappa$ in joint space can be defined using the second derivative of the path with respect to $\sigma$ :

(24) \begin{equation} \kappa = \frac {\|\mathbf{q}' \times \mathbf{q}''\|_M}{\|\mathbf{q}'\|_M^3}, \end{equation}

where $\mathbf{q}'$ and $\mathbf{q}''$ are the first and second derivatives of $\mathbf{q}$ with respect to $\sigma$ , respectively, and $\|\cdot \|_M$ denotes the norm induced by the metric matrix $M$ . The numerator $\|\mathbf{q}' \times \mathbf{q}''\|_M$ represents the magnitude of the cross product of the tangent and acceleration vectors, capturing the “twist” of the curve, while the denominator $\|\mathbf{q}'\|_M^3$ normalizes this value relative to the cube of the tangent vector’s magnitude.

The curvature $\kappa$ provides a quantitative measure of how sharply the path bends at any point $\sigma$ . High curvature values indicate regions of sharp turns or singularities, which may lead to large joint accelerations and potentially unstable behavior. Conversely, low curvature values correspond to smoother, more gradual transitions along the path.

To analyze the curvature effectively, it is important to examine its value range (curvature value domain) over the entire path. Let $\kappa _{\text{min}}$ and $\kappa _{\text{max}}$ denote the minimum and maximum curvature values along the path, respectively. The range $[\kappa _{\text{min}}, \kappa _{\text{max}}]$ provides critical information about the path’s geometric complexity and the potential for dynamic instabilities. For practical implementations, it is often desirable to normalize the curvature to a unitless quantity for easier interpretation and comparison across different paths.

A curvature normalization function $\kappa _{\text{norm}}$ can be defined as follows:

(25) \begin{equation} \kappa _{\text{norm}}(\sigma ) = \frac {\kappa (\sigma ) - \kappa _{\text{min}}}{\kappa _{\text{max}} - \kappa _{\text{min}}}, \end{equation}

where $\kappa _{\text{norm}}(\sigma ) \in [0, 1]$ . This normalization maps the curvature values to a standardized range, enabling consistent evaluation and visualization of the path’s geometric properties. Regions with $\kappa _{\text{norm}} \approx 0$ correspond to relatively straight segments, while $\kappa _{\text{norm}} \approx 1$ indicates areas with the sharpest bends.

Furthermore, the normalized curvature can be incorporated into path planning and motion control algorithms to adaptively adjust speed or smooth transitions in regions of high curvature, thereby mitigating the risk of instability. By coupling curvature analysis with arc-length parameterization, a comprehensive understanding of the geometric and dynamic characteristics of the path can be achieved, leading to more robust and efficient motion planning strategies.

Figure 3.

Limitations of arc-length uniform sampling strategy. The figure illustrates potential issues arising from solely relying on arc length for sampling. Top : A planned path with uniform arc-length sampling. Middle: In regions with gentle curvature (circled area), the mapping between joint space and Cartesian space can lead to joint configurations near singularities, causing large joint movements for small end-effector displacements. Bottom: In areas of high curvature (circled area), uniform arc-length sampling may result in under-sampling, potentially missing crucial details of the path and compromising tracking accuracy.

2.2.3. Weights adaptive sampling

As illustrated in Figure 3, path curvature significantly impacts trajectory tracking, particularly concerning the robot’s ability to accurately follow the planned path. Neglecting curvature can lead to issues such as large joint movements near singularities in regions of low curvature, or under-sampling in areas of high curvature, ultimately compromising the robot’s ability to adhere to the intended trajectory. To address these challenges and improve path fidelity, we introduce a curvature-adaptive weighted sampling algorithm. This algorithm is designed to ensure more accurate path adherence by dynamically adjusting the sampling density based on the curvature.

First, given a path represented as a sequence of discrete points $\mathbf{p}_i \in \mathbb{R}^2$ , the curvature at each point is computed using three consecutive points. Let $\mathbf{p}_{\text{prev}}$ , $\mathbf{p}_{\text{curr}}$ , and $\mathbf{p}_{\text{next}}$ denote the previous, current, and next points, respectively. The vectors between these points are defined as:

(26) \begin{align} \mathbf{v}_1 &= \mathbf{p}_{\text{curr}} - \mathbf{p}_{\text{prev}}, \end{align}

(27) \begin{align} \mathbf{v}_2 &= \mathbf{p}_{\text{next}} - \mathbf{p}_{\text{curr}}. \\[6pt] \nonumber \end{align}

The curvature $\kappa$ is then calculated as:

(28) \begin{align} \kappa = \begin{cases} \frac {2 \cdot \| \mathbf{v}_1 \times \mathbf{v}_2 \|}{\| \mathbf{v}_1 \| \cdot \| \mathbf{v}_2 \| \cdot \| \mathbf{v}_1 + \mathbf{v}_2 \|}, & \text{if } \| \mathbf{v}_1 \| \neq 0 \text{ and } \| \mathbf{v}_2 \| \neq 0, \\ 0, & \text{otherwise}. \end{cases} \end{align}

Here, $\|\cdot \|$ denotes the Euclidean norm, and $\times$ represents the cross product. If either $\mathbf{v}_1$ or $\mathbf{v}_2$ has zero magnitude, the curvature is set to zero.

To ensure that curvature values are within a standard range, they are normalized as follows:

(29) \begin{equation} \kappa _{\text{norm}} = \begin{cases} \frac {\kappa }{\max\!(\kappa )}, & \text{if } \max\!(\kappa ) \neq 0, \\ 0, & \text{if } \max\!(\kappa ) = 0. \end{cases} \end{equation}

Here, $\max\!(\kappa )$ represents the maximum curvature value across all points.

Then we design the weight function to integrate a base arc-length weight factor $\alpha$ with a curvature weight factor $\beta$ , effectively combining these two crucial aspects.

(30) \begin{align} w \;=\; \alpha \;+\; \beta \,\kappa _{\text{norm}}, \qquad \kappa _{\text{norm}}\in [0,1]. \end{align}

Here, $\alpha$ governs the baseline density along arc length (favoring more uniform spacing), whereas $\beta$ increases sampling density in higher-curvature regions. Without loss of generality, we set $\alpha ,\beta \ge 0$ and $\alpha +\beta =1$ (the subsequent normalization makes only the relative balance matter). In the subsequent experiments, we adopt $\boldsymbol{\alpha }=0.6$ and $\boldsymbol{\beta }=0.4$ as a balanced default. In practical engineering applications, the parameters can be adjusted according to the specific task requirements and trajectory characteristics. For relatively flat trajectories where a smoother and more optimal velocity profile is desired, $\alpha$ can be tuned within [0.6, 0.8]; for tasks demanding high trajectory-following accuracy, such as precision machining or robotic path tracking in high-curvature segments, $\beta$ can be tuned within [0.4, 0.6]. Nearby choices lead to similar qualitative behavior.

The cumulative weight $W(s)$ along the path is computed as the integral of the weight function with respect to the arc length $s$ :

(31) \begin{equation} W(s) = \int _0^s w(s') \, ds'. \end{equation}

The cumulative weight is then normalized to the range $[0, 1]$ :

(32) \begin{equation} W_{\text{norm}}(s) = \frac {W(s)}{\max\!(W(s))}. \end{equation}

To perform sampling based on the computed weights, we define a set of target weights uniformly distributed in $[0, 1]$ :

(33) \begin{equation} W_{\text{target}} = \left\{0, \frac {1}{N-1}, \frac {2}{N-1}, \ldots , 1\right\}, \end{equation}

where $N$ is the number of desired sampling points. For each target weight, the closest cumulative weight is identified, and the corresponding path point is selected.

The following pseudocode summarizes the entire process:

Algorithm 1

Curvature – Based Weight Adaptive Sampling

To evaluate how well an executed path conforms to the planned trajectory, we employ the Fréchet distance as a metric for assessing their similarity. This metric provides the measure of adherence between the two paths. When the paths are represented as discrete sequences of points, the discrete Fréchet distance is commonly used. Let $P = \{p_1, p_2, \ldots , p_m\}$ and $Q = \{q_1, q_2, \ldots , q_n\}$ denote two discrete curves in $\mathbb{R}^d$ , representing the planned and executed paths, respectively. The discrete Fréchet distance is defined as:

(34) \begin{equation} d_F^d(P, Q) = \min _{\pi } \max _{(i,\, j) \in \pi } \|p_i - q_j\|, \end{equation}

where $\pi$ represents all possible alignments (or couplings) of the two curves and $\|p_i - q_j\|$ is the Euclidean distance between points $p_i \in P$ and $q_j \in Q$ . An alignment $\pi$ is a sequence of index pairs $(i, j)$ satisfying the following conditions:

1. $(1, 1) \in \pi$ and $(m, n) \in \pi$ (the alignment starts at the first points and ends at the last points of the curves).

2. The alignment is monotonic; that is, if $(i, j) \in \pi$ , then $(i+1, j)$ , $(i, j+1)$ , or $(i+1, j+1)$ must also be in $\pi$ .

The discrete Fréchet distance can be computed efficiently using a dynamic programming approach. Let $C[i, j]$ represent the discrete Fréchet distance between the sub-curves $\{p_1, \ldots , p_i\}$ and $\{q_1, \ldots , q_j\}$ . The recursive formula for $C[i, j]$ is given by:

(35) \begin{equation} \begin{aligned} C[i, j] = &\ \max \Big ( \|p_i - q_j\|, \\ &\ \min \big ( C[i-1, j], \, C[i, j-1], \, C[i-1, j-1] \big ) \Big ) \end{aligned} \end{equation}

with the following base cases:

(36) \begin{gather} C[1, 1] = \|p_1 - q_1\|, \end{gather}

(37) \begin{gather} C[i, 1] = \max\!(C[i-1, 1], \|p_i - q_1\|), \quad \text{for } i = 2, \ldots , m, \end{gather}

(38) \begin{gather} C[1, j] = \max\!(C[1, j-1], \|p_1 - q_j\|), \quad \text{for } j = 2, \ldots , n. \end{gather}

The final discrete Fréchet distance is given by:

(39) \begin{equation} d_F^d(P, Q) = C[m, n]. \end{equation}

The following pseudocode outlines the dynamic programming algorithm for computing the discrete Fréchet distance.

Algorithm 2

Compute Discrete Fréchet Distance

The time complexity of this algorithm is $O(m \cdot n)$ , where $m$ and $n$ are the number of points in the curves $P$ and $Q$ , respectively. This makes it efficient for practical applications involving discrete curves.