## 3 SPO-Based Collision Detection and Obstacle Avoidance Using APF
### Obstacle Avoidance Using APF  
The APF is a collision avoidance method that uses calculations of the repulsive (or attractive) forces between obstacles and the body of the manipulator. The force between body and obstacles is defined as a function of their distance [29]. Equations (16) and (17) define an attractive potential according to a distance \(d\left(q,q_{goal}\right)\) between a current position \(q\) of the robot and goal \(q_{goal}\). \[U_{att}(q)= \begin{cases}0.5*\zeta*a^{2}\left(q,q_{goal}\right),&d\left(q,q_{ goal}\right)\leq R\\ R*\zeta*d\left(q,q_{goal}\right)-0.5*\zeta*R^{2},&d\left(q,q_{goal}\right)>R \end{cases}  \tag{16}\]  
Figure 5: Comparison between calculated gravity and SPO. Figure 6: Contact force impact on joints. where \(d\Big{(}q,q_{goal}\Big{)}=\Big{|}q-q_{goal}\Big{|}\). \[\
abla U_{att}(q)=\left\{ \begin{array}{ll}\zeta*d\Big{(}q,q_{goal}\Big{)}\,&d\Big{(}q,q_{goal}\Big{)}\leq R\\ R*\zeta*\frac{\big{(}q-q_{goal}\big{)}}{d\big{(}q_{goal}\big{)}},&d\Big{(}q,q_{goal }\Big{)}>R\end{array} \right. \tag{17}\]  
\(U_{att}(q)\) is the \"attractive\" potential; \(\zeta\) = attractive constant. In Equation (16), when the attractive potential is less than or equal to a set threshold \(R\), the quadratic potential of the distance is applied to slowly reach the goal. When the distance is greater than the \(R\), a conical attractive potential is applied to approach the goal quickly. The gradient of this potential is the attractive force \(F_{att}=\
abla U_{att}(q)\), which is calculated in Equation (17). Similarly, Equation (18) shows that the repulsive potential is applied when the distance between a robot's current position and an obstacle position is less than or equal to a set threshold \(R\), otherwise it is zero. The gradient of this repulsive potential, which is the repulsive force \(F_{rep}=-\
abla U_{rep}(q)\), is calculated as follows:  
\[U_{rep}(q)=\left\{ \begin{array}{ll}0.5*\eta*\Big{(}\frac{1}{D(q_{obs})}- \frac{1}{R}\Big{)}^{2},&D(q,q_{obs})\leq R\\ 0,&D(q,q_{obs})>R\end{array} \right. \tag{18}\]  
where \(D(q,q_{obs})=|q-q_{obs}|\). \[\
abla U_{rep}(q)=\left\{ \begin{array}{ll}\eta*\Big{(}\frac{1}{D(q,q_{obs} )}-\frac{1}{R}\Big{)}\ *\frac{1}{D^{2}}*\frac{q-q_{obs}}{D},&D(q,q_{obs})\leq R\\ 0,&D(q,q_{obs})>R\end{array} \right. \tag{19}\]  
where \(R\) is the distance threshold and \(\eta\) is a repulsive constant. In Equations (18) and (19), the distance between the robot end-effector and obstacle is very important for calculating the repulsive force. The two-dimensional distance is considered in the RVI because the robot is assumed as moving in \(x\) and \(y\) directions only. The distance between them is calculated using the following formula. The same method is followed to calculate the distance from the goal position. \[D(q,q_{obs})=\sqrt{\big{(}x_{q}-x_{q_{obs}}\big{)}^{2}+\big{(}y_{q}-y_{q_{obs }}\big{)}^{2}} \tag{20}\]  
where \(x_{q}\),\(y_{q}\) are the coordinates of the robot current end-effector positions from the desired Cartesian trajectory and \(x_{q_{obs}}\), \(y_{q_{obs}}\) are detected obstacle position using the estimated external torque while performing cutting/scanning in the RVI. Figure 7 provides a better idea for understanding the artificial potential field. Total potential force \(F_{pet}\) is defined as  
\[F_{pot}=\
abla U_{att}(q)+\
abla U_{rep}(q). \tag{21}\]  
Figure 7: Attractive and repulsive potential field [30]. In this study, the attractive potential of an artificial potential field is replaced by Cartesian space trajectory. The polynomial trajectory from the initial position to goal is planned in Cartesian space. Cartesian space trajectories relate to the motions of a robot relative to the Cartesian reference frame. In Cartesian space, the joint values must be repeatedly calculated through the inverse kinematic equations of the robot.