A geometric tracking of rank-1 manipulability for singularity-robust collision avoidance

Abstract

For safe operation of cooperative robots, we propose a real-time collision avoidance algorithm that is robust to kinematic singularities of serial robots. The main idea behind the algorithms is to generate control inputs that increase the directional manipulability of a robot along the object direction by reducing directional safety measures. While existing directional safety measures show undesirable behaviors in the vicinity of the kinematic singularities of the robot, the proposed geometric safety measure, which is a distance on the space of positive semi-definite matrices with rank one, robustly generates control input that drives the robot to safer posture, even at the kinematic singularities. By adding the control input from the geometric safety measure to the conventional repulsive input for collision avoidance, a hierarchical collision avoidance algorithm that is robust to kinematic singularity is implemented. The proposed method is demonstrated and validated with a set of numerical experiments that consist of manipulability tracking and collision avoidance examples for a serial manipulator.

Appendix: Proof of triangle inequality when \(p=1\)

Given three matrices $A_1, A_2, A_3\in S^+(1,n)$ in form of $A_i=\lambda _iu_iu_i^T$, the length (14) satisfies the triangle inequality

$$\begin{aligned} l_{1,2} + l_{2,3} \ge l_{3,1} \end{aligned}$$

(37)

where

$$\begin{aligned} l_{i,j} = \sqrt{\theta _{i,j}^2 + k\left( \log \frac{\lambda _i}{\lambda _j}\right) ^2} \end{aligned}$$

(38)

and $\theta _{i,j}=\cos ^{-1}(u_i^Tu_j)$.

Proof

For simplicity, we denote that $\sqrt{k}\log \frac{\lambda _1}{\lambda _2}=X$ and $\sqrt{k}\log \frac{\lambda _2}{\lambda _3}=Y$. Then above equation become

$$\begin{aligned} \sqrt{\theta _{1,2}^2 + X^2} + \sqrt{\theta _{2, 3}^2 + Y^2} \ge \sqrt{\theta _{3, 1}^2 + (X+Y)^2}. \end{aligned}$$

(39)

Since the angle part and the logarithm part are the distance on each space, i.e., $\mathrm{Gr}(1,n)$ and P(1), the following triangle inequality holds.

$$\begin{aligned}&\theta _{1, 2} + \theta _{2, 3} \ge \theta _{3, 1} \end{aligned}$$

(40)

$$\begin{aligned}&2\theta _{1, 2}\theta _{2, 3} \ge \theta _{3, 1}^2 - \theta _{1, 2}^2 - \theta _{2, 3}^2 \end{aligned}$$

(41)

$$\begin{aligned}&|X| + |Y| \ge |X+Y|. \end{aligned}$$

(42)

Taking square on both sides of (39) and rearranging the equation give

$$\begin{aligned}&2\sqrt{\left( \theta _{1, 2}^2 + X^2\right) \left( \theta _{2, 3}^2 + Y^2\right) } \nonumber \\&\quad \ge 2XY + \theta _{3, 1}^2 - \theta _{1, 2}^2 -\theta _{2, 3}^2 \end{aligned}$$

(43)

From (41), it becomes

$$\begin{aligned} 2\sqrt{\left( \theta _{1, 2}^2 + X^2\right) \left( \theta _{2, 3}^2 + Y^2\right) } \ge 2XY + 2\theta _{1, 2}\theta _{2, 3}. \end{aligned}$$

(44)

Taking another square on both sides gives

$$\begin{aligned} \left( \theta _{1, 2}Y - \theta _{2, 3}X \right) \ge 0 \end{aligned}$$

(45)

$\square$