# Real-Time Kinematics-Based Self-Collision Avoidance Algorithm for Dual-Arm Robots
## 3 Self-Collision Avoidance Strategy
### Repulsive Velocity between Two Spheres  
Each arm of the robot has multiple bounding volumes, and the minimal sensitivity index \(\beta_{lmin}\) indicates the relationship between the closest bounding volumes. When \(\beta_{lmin}\leq 1\), the repulsive velocity \(v_{re}\) is produced at these control points. Meanwhile, inspired by the on-line method of human-robot collision avoidance algorithms in [27], we chose the sigmoid function to measure the magnitude of the repulsive velocity. By using the sigmoid function, the magnitude of the repulsive velocity \(\|v_{re}\|\) can be calculated as  
\[\|v_{re}\|=\frac{V_{max}}{1+e^{(\alpha\times(\beta_{lmin}-0.7))}}, \tag{8}\]  
where Vmax is the maximum velocity allowed at the end of the robot and \(\alpha\) is the shape factor, as described in [27]. If Vmax = 0.25 and \(\alpha=20\), the profile shown in Figure 5a is achieved, which illustrates the relationship between the minimal sensitivity index and the repulsive velocity. From the profile shown in Figure 5, the sigmoid function not only ensures the continuity of the speed change, but when the relative distance of the arm becomes smaller, the gradient of the repulsive change continues to be changed. In particular, in Figure 5, a shorter \(\beta_{lmin}\) resulted in a larger repulsive velocity, helping the robot avoid self-collision. Meanwhile, we can conclude that when \(\beta_{lmin}=0.4\), the magnitude of the repulsive velocity \(\|v_{re}\|=0.25\) is equal to the maximum velocity \(V_{max}\) of the end-effector. This can also define the radii of each sphere. Considering each link of the robot as the cylinder and the radius of each cylinder (link) is r\({}_{\text{cyl.li}}\), the radius \(r_{cm}\) of each sphere can be defined according to \(\frac{r_{cv,li}}{r_{cm}}=0.4\). In this way, we can define the radius of each sphere. When \(\beta_{lmin}=0.4\), it indicates a collision between the arms; when the magnitude of the repulsive velocity \(\|v_{re}\|=V_{max}=2.5\), it indicates that the robot will stop motion. Figure 5: (**a**) Relationship between minimal sensitivity index and repulsive velocity. (**b**) The repulsive velocity produced at the center of the spherical volumes. As illustrated in Figure 5b, the position of the control point \(a\), on the left, is defined as \((x_{b}^{\prime},y_{a}^{\prime},z_{a}^{\prime})^{T}\), which is expressed in the base frame, and the other point \(b\), on the right, is \(\left(x_{b}^{\prime},y_{b}^{\prime},z_{b}^{\prime}\right)^{T}\). Then, the repulsive velocity direction can be defined as  
\[\left\{ \begin{array}{l}\overset{\rightarrow}{ab}=\frac{x_{a}^{\prime}y_{a} z_{a}^{\prime T}-x_{b}^{\prime}y_{b}^{\prime}y_{b}z_{b}^{\prime T}}{d_{ab}}\text{ direction on point }a\\ \overset{\rightarrow}{ba}=\frac{x_{a}^{\prime}y_{a}^{\prime}z_{a}^{\prime T}-x _{b}^{\prime}y_{b}^{\prime}y_{b}^{\prime}z_{b}^{\prime T}}{d_{ab}}\text{, direction on point }b\end{array} \right. \tag{9}\]  
The repulsive velocity produced at the control points can be computed as:  
\[\left\{ \begin{array}{l}v_{r,a}=\overset{\rightarrow}{ab}\|v_{r}\|\text{, repulsive velocity on point }a\\ v_{r,b}=\overset{\rightarrow}{ab}\|v_{r}\|\text{, repulsive velocity on point }b\end{array} \right. \tag{10}\]  
Section 3.1 introduced the calculation of the repulsive velocity between two spheres. In reality, the strategy must be applied on the dual-arm system in 3D space. Our approach is illustrated on the dual-arm robot in Figure 6, which has a left arm (\(l\)) and a right arm (\(r\)). Each arm has a different original velocity on the end-effector, which is defined as \(v_{end,k}\), where \(k\) = \(l\) or \(r\) represents the left or right arm. The original joint velocity is defined as \(\hat{\theta}_{ki}\), where \(i\) represents the \(i\)-th joint of each arm. Thus, the joint velocities \(\dot{\theta}_{k}=[\dot{\theta}_{k1}\ \ldots\ \dot{\theta}_{kj-1}\dot{\theta}_{kj}]\in R^{j}\) can be calculated as follows:  
\[\dot{\theta_{k}}=I_{k}^{\#}\left[ \begin{array}{c}v_{k}\\ \omega_{k}\end{array}