# Experimental Evaluation of Collision Avoidance Techniques for Collaborative Robots
## 2 Obstacle Avoidance Algorithm  
This section invokes the obstacle avoidance algorithm in the formulation for a generic manipulator operated by a number of actuators greater or equal to 6 (depending on its degree of redundancy). The position of the joints can be described by the vector \(\mathbf{q}=\begin{bmatrix}q_{1}&q_{2}&\dots&q_{n}\end{bmatrix}^{T}\), in which the \(n\) components give the \(n\)-DOF. The pose of the end-effector in the Cartesian space, according to the Euler angle ZYZ convention, is defined by the vector \(\mathbf{x}=\begin{bmatrix}x&y&z&\alpha&\beta&\gamma\end{bmatrix}^{T}\). The forward velocity kinematics can be written as:  
\[\dot{\mathbf{x}}=\mathbf{J}\dot{\mathbf{q}}=\begin{bmatrix}\mathbf{I}_{p}\\ \mathbf{I}_{r}\end{bmatrix}\dot{\mathbf{q}} \tag{1}\]  
where \(\mathbf{J}\) is the \(6\times n\) arm Jacobian (\(n\geq 6\)), with \(\mathbf{I}_{p}\) the submatrix of the position and \(\mathbf{J}_{r}\) of the rotation. To obtain inverse kinematics, it is necessary to use a damped least-square strategy to avoid singularity problems. The inverse of the Jacobian \(\mathbf{J}\) can be obtained as  
\[\mathbf{J}^{*}=\mathbf{J}^{T}(\mathbf{J}\mathbf{J}^{T}+\lambda^{2}\mathbf{I})^{-1} \tag{2}\]  
where \(\lambda\) symbolizes the damping factor, which depends on the minimum singular value of the Jacobian matrix, and by the value \(\epsilon\), which is a tunable parameter of the algorithm to which it is compared. It can be seen as a compromise between accuracy, low value, and numerical robustness, high value. In particular, for a full \(\mathbf{J}\) rank matrix, \(\lambda\) is set to 0.  
The algorithm detects the manipulator link where the risk of collision with obstacles is greatest. This requires calculating the distance of each obstacle from each robot body, bounded by two control points located at the extremes of the link. The center of the obstacle is called \(\mathbf{P}_{o}\) and its distance from the body is calculated according to the diagrams of Figure 1, in which \(\mathbf{d}_{l}\) is the vector representing the segment; \(\mathbf{d}_{p}\) and \(\mathbf{d}_{d}\) are the distances between the obstacle and the proximal and distal ends of the link, respectively. In addition, \(\mathbf{P}_{r}\) is the point of the segment closest to the obstacle. The Jacobian \(\mathbf{J}_{0}\) associated with the linear velocity of \(\mathbf{P}_{r}\) is defined as  
\[\mathbf{J}_{0}=\begin{bmatrix}\mathbf{I}_{0p}\\ \mathbf{I}_{0r}\end{bmatrix} \tag{3}\]  
The motion of the robot is influenced by the presence of the obstacle if the distance \(d_{o}\) is lower than the radius \(r\), which is a parameter of the algorithm. Thus, the region of interest of each link can be visualized as a cylinder with two hemispheres at its extremities, all of radius \(r\). The repulsive velocity \(\dot{\mathbf{x}}_{0}\) is applied to \(\mathbf{P}_{r}\). In detail:  
(a) \(\cos\beta<0\); the point \(\mathbf{P}_{r}\) is localized to the distal extremity of the link:  
\[\mathbf{d}_{o}=\mathbf{d}_{d}\quad x=1 \tag{4}\]  
Figure 1: Obstacle-link distance calculation; cases (**a–c**) refer to Equations (4)–(6) respectively.  
2. \(\cos\alpha\geq 0\) and \(\cos\beta\geq 0\); the distance \(\mathbf{d}_{o}=\mathbf{P}_{r}-\mathbf{P}_{o}\) is orthogonal to the link, and the position of \(\mathbf{P}_{r}\) along the link is defined by the scalar parameter \(x\): \[d_{o}=\frac{|\mathbf{d}_{l}\times\mathbf{d}_{p}|}{d_{l}}\quad x=\frac{d_{p}\cos\alpha}{ d_{l}},\quad 0<x<1\] (5) 3. \(\cos\alpha<0\); the point \(\mathbf{P}_{r}\) is localized to the proximal extremity of the link: \[\mathbf{d}_{o}=\mathbf{d}_{p}\quad x=0\] (6) Once the distance and position of the most critical obstacle are determined, three methods are proposed to avoid the collision [23].