Robot Singularity Avoidance, Analysis and Comparison

Feb 25, 2023 | Tech Robotics

Motivation

Mathematically, singularities happen when robot Jacobian loses rank, resulting robot loses its ability to move its end-effector along certain directions. Or, in other mathematically words, when the Jacobian is singular, it cannot map robot joint velocity to certain Cartesian directions.

Practically, when robot is close to a singularity zone, to achieve some Cartesian motion, the computed joint velocity or joint torque would become infinitely large, causing instability. There are many ways to handle it in the literature. Some are more robust than others.

This post summarizes some results in 2017@Carmichael . The paper discusses the application of its singularity handling approach to velocity kinematics-based control. This post extends the discussion to dynamics-based control in the operational space and image-based visual servo control.

Nomenclature

In addition to nomenclature used in Fundamentals of Dynamics and Control of Flexible Joint Robots and Operational Space Framework, Hybrid Motion-Force Control:

  • $U, V$ - unitary matrices, i.e., $A^{-1} = A^T$, in SVD as in Wikipedia@Svd
  • $\Sigma$ - rectangular diagonal singular value matrix as in Wikipedia@Svd
  • $\Sigma^\star$ - damped inverse of $\Sigma$
  • $J^\star$ - singular value damped generalized inverse of robot Jacobian
  • $\Lambda^\star$ - SVD processed operational space mass matrix
  • $\mathcal{X}^{\dagger}$ - generalized inverse of a matrix in a broader sense, not tied to a specific type, where $\mathcal{X}$ is a wildcard symbol that could represent $J$, $\Sigma$, etc.

With redundant robots, $J$ is a fat matrix, i.e., $J \in \mathcal{m \times n}$ with $m < n$, meaning there are more joints than task DOF to compose end-effector velocity. And $J^\star$ and $\Sigma^\star$ are tall matrices.

Singularity at the Kinematic Level

Robot kinematics model shows the following velocity mapping between joint and Cartesian spaces $$ \dot{x} = J \dot{q} \label{eq:system-kinematics}$$ When designing motion controller with velocity inputs, one typically employs $$ \dot{q} = J^{\dagger} u \label{eq:inverse-kinematics} $$ $$ u = \dot{x}_d + K_p(x_d - x) + K_i \int_0^t (x_d - x) dt $$ as discussed in Sec. 11.3.3 in 2017@Lynch .

When singularity occurs, $J^{\dagger}$ becomes infinitely large, and the control effort $u$ will go unbounded after getting mapped by $J^{\dagger}$ causing instability.

Basic Analysis

To avoid singularity, a typical technique is to perform SVD on the Jacobian, regulate its singular values before inverting it.

That is, given $J\in\mathbb{R}^{m \times n}$, there exists $$ J = U \Sigma V^T $$ and $$ J^{\dagger} = V \Sigma^{\dagger} U^T $$

Since $V$ and $U$ are unitary matrices, singularity issue can be contained by not letting $\Sigma^{\dagger}$ go unbounded. That is, rewriting control law in \eqref{eq:inverse-kinematics} as $$ \dot{q} = V \Sigma^{\dagger} U^T u $$ it is clear that if $\Sigma^{\dagger}$ is well-handled, the joint velocity will not go unbounded. And the commanded end-effector velocity is given by \begin{align} \dot{x} &= J \dot{q} = J J^{\dagger} u \nonumber \\ &= \Bigl( U \Sigma V^T \Bigl) \Bigl( V \Sigma^{\dagger} U^T \Bigl) u \nonumber \\ &= U \Bigl( \Sigma \Sigma^{\dagger} \Bigl) U^T u \label{eq:end-effector-velocity} \end{align}

Singularity Avoidance Techniques

Damped Least Square

One of the most common singularity avoidance techniques is to have $$ \Sigma^\star = \begin{bmatrix} \text{diag}\{ \frac{\sigma_i}{\sigma_i^2 + \lambda^2} \} \\ \hline \mathbf{0} \end{bmatrix} \label{eq:damped-inverse} $$ where $\lambda \in \mathbb{R}^+ $ is a small damping term that prevents $1/ \sigma_i$ from going unbounded near singularities.

While \eqref{eq:damped-inverse} certainly works. But observing \eqref{eq:end-effector-velocity}, the drawback of this method is quite obvious. Because in this approach leads to $$ \Sigma \Sigma^{\star} \neq I $$ that is, even not in singularities, the introduction of $\lambda$ will result in the end-effector motion trajectory slightly deviating from the desired trajectory.

Exponentially Damped Least Square

The method introduced in 2017@Carmichael handles such problems with another formulation that chooses $$ \Sigma^\star = \begin{bmatrix} \text{diag}\{ \frac{s(\sigma_i)}{\sigma_i} \} \\ \hline \mathbf{0} \end{bmatrix} \label{eq:edls-inverse} $$ where $$ s(\sigma_i) = \begin{cases} 1 - \beta^{(\sigma_i - \sigma^-)/(\sigma^+ - \sigma^-)} &, \sigma_i > \sigma^- \\ 0 &, \text{otherwise} \end{cases} $$ Notice that in with \eqref{eq:edls-inverse}, the following relationship $$ \dot{x} = U \Sigma \Sigma^{\star} U^T u = u$$ is preserved when Jacobian is not close to singularity, hence offers better trajectory tracking quality.

Extension to Operational Space Control and IBVS Control

In the operational space framework, the control torque to track a Cartesian trajectory is given by $$ \tau = J^T (\Lambda F^* + \mu + \rho) $$ where $$ \Lambda = (J M^{-1} J^T)^{-1} $$ and Jacobian singularity could cause this term to go unbounded as well. One can apply \eqref{eq:edls-inverse} when computing $\Lambda$ to prevent the negative effect of singularity on the operational space mass matrix.

The same applies to image-based visual servo control as well, as mentioned in Image-Based Visual Servo Control, which has a camera to image space feature velocity kinematics relationship governed by $$ \dot{U} = L \mathcal{V}_c $$ A control input can be designed as $$ \mathcal{V}_c = L^{\star} \mathcal{F} $$ with an image space force $\mathcal{F}$ driving features to desired targets safely with the singularity handling technique given in \eqref{eq:edls-inverse}. And it achieves $$ \dot{U} = L L^{\star} \mathcal{F} $$

Further Generalization

When not inverting a matrix, a similar approach can also be used the cap the maximum singular value to prevent quantities getting mapped by the matrix become too large.

In some Cartesian control cases, one may want to prioritize singularity protection between orientation and position subspaces. In such situations, a straightforward approach is to divide a 6-DOF Cartesian control task into a 3-DOF orientation control task and a 3-DOF position control task, and prioritize them using the whole-body control approach as mentioned in Analytical Whole Body Control, Prioritized Recursive Nullspace. That way, the tasks will avoid singularity based on their priorities.