Collision Detection of a HEXA Parallel Robot Based on Dynamic Model and a Multi-Dual Depth Camera System

Abstract

This paper introduces a Hexa parallel robot and obstacle collision detection method based on dynamic modeling and a computer vision system. The processes to deal with the collision issues refer to collision detection, collision isolation, and collision identification applied to the Hexa robot, respectively, in this paper. Initially, the configuration, kinematic and dynamic characteristics during movement trajectories of the Hexa parallel robot are analyzed to perform the knowledge extraction for the method. Next, a virtual force sensor is presented to estimate the collision detection signal created as a combination of the solution to the inverse dynamics and a low-pass filter. Then, a vision system consisting of dual-depth cameras is designed for obstacle isolation and determining the contact point location at the end-effector, an arm, and a rod of the Hexa robot. Finally, a recursive Newton-Euler algorithm is applied to compute contact forces caused by collision cases with the real-Hexa robot. Based on the experimental results, the force identification is compared to sensor forces for the performance evaluation of the proposed collision detection method.

1. Introduction

A six-degree-of-freedom parallel structure called the Hexa parallel robot was introduced by Pierrot et al. in [1,2]. Despite its complicated kinematics structure and limited workspace, the Hexa robot has remarkable advantages over the serial robot: high accuracy, high stiffness, high speed, and high carrying capability. However, working with this type of robot must imply solving complex and computationally not only inverse/forward kinematics problems but also dynamics ones, which are complicated. Meanwhile, the inverse kinematics issue is tractable by geometrical methods [3,4], but the forward kinematics problem involves a system of nonlinear equations that usually has no closed-form solution. Therefore, the artificial neural networks (ANNs) method is proposed for finding a unique closed-form analytic solution to the forward kinematics problem [5]. There have been few studies on the dynamic modeling of the experimental configuration Hexa parallel robot [6,7]. In-depth study of the Hexa robot dynamics is needed to take full advantage of its strengths for trajectory control and fault detection.

It is a critical issue to study the collisions of a robot manipulator due to diverse application scenarios. An approach for accurate and real-time monitoring of serial robot manipulator collisions is presented in [8], which uses proprioception and IMU sensing for velocity and acceleration estimation. Another observer method to adapt the erroneous dynamics model of the serial manipulators is introduced in [9], where the observer significantly improves the collision detection sensitivity. Sensorless collision detection approaches based on different methods are presented in [10,11,12]. Accelerometers are attached along the surface of the robot arm to detect and localize contact events [13]. Tactile sensors mounted with the robot manipulator are applied to provide a tactile perception of the robot [14,15,16]. In [17,18], a virtual force sensor has been proposed for collision detection and estimating external forces. Authors of [19] have used neural networks for collision detection and classification. Most of the above methods are extremely difficult to apply to parallel manipulators. However, some papers use the dynamics equations and first-order filter structure in combination to build an observer as the collision identification signal called a residual [20]. The residual equals zero up to noise and disturbances during free motion or higher than a suitable threshold in response to a collision. The performance of this method is suitable not only for robot manipulators, and many pieces of research related to humanoid robots have been carried out [21,22].

Recently, a vision-based control system has been widely utilized in robotics to provide information about the target objects or the robot’s pose as position and orientation. Besides that, many vision-based techniques during the isolation phase have been presented. The authors of [23,24] have analyzed the problem of sensor-based collision detection and isolation for a robot manipulator using a set of images taken from several stationary cameras. A system combining deep learning and stereo vision for object detection, classification, and distance calculation [24] can be effectively applied for not only autonomous robot navigation but also collision isolation. For moving object detection, a method of estimating the motion of objects and extracting shapes of detected objects from stereo video cameras has been proposed in [25]. Another system with dual-depth camera information and visual odometry was used to detect moving objects, and an adaptive thresholding method was adopted to enhance performance [26]. An algorithm based on background subtraction and edge detection was applied to detect and segment objects in video frames [27]. In the collision isolation phase, moving objects in the workspace should be detected based on the multi-camera system, using the advantages of image processing techniques. One of them is the interesting You-Only-Look-Once (YOLO) algorithm, firstly presented by Redmon et al. in [23,28], so the contact point location and the Hexa robot pose in this paper are determined with the improved algorithm.

After collision detection and collision isolation, it is essential to determine the suitable method for identifying the contact force. In [29], data from a 6D wrist Force/Torque sensor is combined with the probabilistic approach to link contact estimation based on geometric considerations and compliant motions. In [12,30], the time series model and fuzzy identification were used for determining contact location. Once the contact point is detected, the exchanged force estimation must be the next stage for a complete identification phase. Some dynamics algorithms are approached to perform the external force estimation, such as the recursive Newton–Euler algorithm (RNEA) for the inverse dynamics [31,32], the articulated body algorithm (ABA) for the forward dynamics, and the composite-rigid-body algorithm (CRBA) for calculating the joint-space inertia matrix (JSIM) [33,34]. A recursive Newton-Euler method for inverse dynamics is suitable to analyze external forces acting on the Hexa parallel robot. Based on the above discussion, this paper is motivated by the primary algorithms to solve the detected collision issues as follows:

• The analysis of implementing a dynamic model of the moving Hexa parallel robot for the collision detection problem is proposed.
• The obstacles and the Hexa robot are isolated by the proposed geometrical algorithm and the set of real-time images taken from the multi-cameras system is used to locate the contact point.
• Finally, the Cartesian external force at the contact point is estimated by the recursive Newton-Euler algorithm for the Hexa parallel robot, compared to the force sensor in different collision point cases.

The rest of this paper is organized as follows. We present the configuration, the kinematics and dynamics problems of the Hexa robot in Section 2. In Section 3, the proposed collision detection, isolation, and identification methods applied to the Hexa robot are presented. We show block diagrams to illustrate the entire Hexa robot system and introduce equipment used in this study in Section 4. Section 5 includes the experimental scheme of study cases where the proposed methods are validated based on the given trajectories. Finally, we conclude the paper in Section 6.

2. Modeling of the Hexa Parallel Robot

2.1. Configurations of the Hexa Robot

The model of the Hexa parallel robot with six serial kinematics chains is shown in Figure 1. Each kinematic chain is described as a revolute-universal-spherical chain link between the base platform and the active plate. The arms driven by six motors at revolute joints connect with the rods at the universal joints. These rods are associated with the active plate at spherical joints. There are two main coordinate frames when the Hexa robot is considered. The first fixed coordinate frame $\left\{O\right\}=\left\{OXYZ\right\}$ is located in the center of the base platform with the $Z$-axis perpendicular to the base platform upwards. The $X$-axis direction points from the origin $O$ to the midpoint of the edge $A_1{A}_2$. The $Y$-axis is determined by the right-hand rule. The second rotating coordinate frame $\left\{P\right\}=\left\{Pxyz\right\}$ is located in the center of the active plate with the $z$-axis perpendicular to the surface of the active plate. The $x$-axis direction points from the end-effector to the midpoint of the edge $C_1{C}_2$. The $y$-axis is determined in the same way as as the Y-axis. For the $i$th kinematic chain $\left(i=1,\dots ,6\right)$, the point $A_i$ represents the center of the revolute joint located in the $i$th arm. The point $B_i$ is the center of the universal joint that connects the $i$th rod with the $i$th arm. The point $C_i$ is the center of the spherical joint that links between the $i$th rod and the active plate. The length of the $i$th arm and the $i$th rod are $l_a$ and $l_d$, respectively. The angle ${\zeta }_i$ is the rotation angle of the revolute joint of the $i$th arm.

The point $P$ representing the end-effector and the Euler angles $\left(\varphi , \theta , \psi \right)$ of the active plate with respect to the fixed frame $\left\{O\right\}$ are given. Three angles $\left(\varphi , \theta , \psi \right)$ are defined in compliance with the intrinsic rotation convention of Euler angles. The coordinate with respect to the active frame $\left\{P\right\}$ can be transformed to the coordinate with respect to the fixed frame $\left\{O\right\}$ by using the rotation matrix ${\mathit{R}}_P^O$:

$${\mathit{R}}_P^O=\left[\begin{array}{ccc}\cos \varphi \cos \theta \cos \psi -\sin \varphi \sin \psi & -\cos \varphi \cos \theta \sin \psi -\sin \varphi \cos \psi & \cos \varphi \sin \theta \\ \sin \varphi \cos \theta \cos \psi +\cos \varphi \sin \psi & -\sin \varphi \cos \theta \sin \psi +\cos \varphi \cos \psi & \sin \varphi \sin \theta \\ -\sin \theta \cos \psi & \sin \theta \sin \psi & \cos \theta \end{array}\right]$$

(1)

The position and orientation of the active plate can be determined by three Euler angles and the position of point $P$:

$${\mathit{r}}_P^O={\left[x_P{y}_P{z}_P\right]}^T$$

(2)

The angular velocity ${\mathit{\omega }}_P$ of the active plate can be expressed as:

$${\mathit{\omega }}_P=\left[\begin{array}{c}\dot{\psi }\cos \varphi \sin \theta -\dot{\theta }\sin \varphi \\ \dot{\psi }\sin \varphi \sin \theta +\dot{\theta }\cos \varphi \\ \dot{\psi }\cos \theta +\dot{\varphi }\end{array}\right]$$

(3)

At the point $O$ on the base platform, three frames $\left\{O_i\right\}=\left\{O_i{X}_i{Y}_i{Z}_i\right\}$ $\left(i=1,3,5\right)$ are built with the $Z_i$-axis coinciding with the $Z$-axis. The $X_i$-axis direction points to the center of edge $A_i{A}_{i+1}$. The $Y_i$-axis is determined by the right-hand rule. Simultaneously, at the point $P$ on the active plate, three frames $\left\{P_i\right\}=\left\{P_i{x}_i{y}_i{z}_i\right\}$ $\left(i=1,3,5\right)$ are also constructed. The rotation matrix of frame $\left\{O_i\right\}$ relative to frame $\left\{O\right\}$ and frame $\left\{P_i\right\}$ relative to frame $\left\{P\right\}$ can be expressed as:

$${\mathit{R}}_{O_i}^O={\mathit{R}}_{P_i}^P=\left[\begin{array}{ccc}\cos {\alpha }_i& -\sin {\alpha }_i& 0\\ \sin {\alpha }_i& \cos {\alpha }_i& 0\\ 0& 0& 1\end{array}\right]$$

(4)

where

$${\left[{\alpha }_1{\alpha }_2{\alpha }_3{\alpha }_4{\alpha }_5{\alpha }_6\right]}^T={\left[00\frac{2\pi }{3}\frac{2\pi }{3}\frac{4\pi }{3}\frac{4\pi }{3}\right]}^T$$

(5)

At the point $A_i$ on the base platform, a frame $\left\{A_{1i}\right\}=\left\{A_{1i}{X}_{1i}{Y}_{1i}{Z}_{1i}\right\}$ $\left(i=1,\dots ,6\right)$ is built with the $Z_{1i}$-axis along with the rotation axis. The $Y_{1i}$-axis is perpendicular to the base platform upwards. The $X_{1i}$-axis is determined by the right-hand rule. The rotation matrix of frame $\left\{A_{1i}\right\}$ relative to frame $\left\{O_i\right\}$ is:

$${\mathit{R}}_{A_{1i}}^{O_i}=\left[\begin{array}{ccc}-1& 0& 0\\ 0& 0& 1\\ 0& 1& 0\end{array}\right]$$

(6)

At the point $A_i$, a frame $\left\{A_{2i}\right\}=\left\{A_{2i}{X}_{2i}{Y}_{2i}{Z}_{2i}\right\}$ $\left(i=1,\dots ,6\right)$ attached to the $i$th arm is established with the $Z_{2i}$-axis coinciding with the $Z_{1i}$-axis. The $X_{2i}$-axis coincides with the axial line of the $i$th arm. The $Y_{2i}$-axis is determined by the right-hand rule. The rotation matrix of frame $\left\{A_{2i}\right\}$ relative to frame $\left\{A_{1i}\right\}$ is:

$${\mathit{R}}_{A_{2i}}^{A_{1i}}=\left[\begin{array}{ccc}\cos {\zeta }_i& -\sin {\zeta }_i& 0\\ \sin {\zeta }_i& \cos {\zeta }_i& 0\\ 0& 0& 1\end{array}\right]$$

(7)

where ${\zeta }_i$ is the rotation angle of the revolute joint of the arm.

At the point $B_i$, a frame $\left\{B_{1i}\right\}=\left\{B_{1i}{x}_{1i}{y}_{1i}{z}_{1i}\right\}$ $\left(i=1,\dots ,6\right)$ attached to the $i$th arm and a frame $\left\{B_{2i}\right\}=\left\{B_{2i}{x}_{2i}{y}_{2i}{z}_{2i}\right\}$ $\left(i=1,\dots ,6\right)$ attached to the $i$th rod are built with the $x_{1i}$-axis pointing from $A_i$ to $B_i$. The $x_{2i}$-axis points from $B_i$ to $C_i$. The $z_{1i}$-axis and the $z_{2i}$-axis coincide with the two rotating axes of the universal joint, respectively. The $y_{1i}$-axis and the $y_{2i}$-axis are determined by the right-hand rule. The rotation matrix of frame $\left\{B_{1i}\right\}$ relative to frame $\left\{A_{2i}\right\}$ is:

$${\mathit{R}}_{B_{1i}}^{A_{2i}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$

(8)

The rotation matrix of the frame $\left\{B_{2i}\right\}$ relative to the frame $\left\{B_{1i}\right\}$ can be calculated by transformations. Initially, the points of $A_i$, $B_i$, and $C_i$ align on the same line. The frame $\left\{B_{2i}\right\}$ is obtained by rotating the frame $\left\{B_{1i}\right\}$ around the two rotating axes of the universal joint by the two rotation angles ${\beta }_i$ and ${\gamma }_i$, respectively. The rotation matrix of frame $\left\{B_{2i}\right\}$ relative to frame $\left\{B_{1i}\right\}$ is:

$${\mathit{R}}_{B_{2i}}^{B_{1i}}=\left[\begin{array}{ccc}\cos {\beta }_i\cos {\gamma }_i& -\cos {\beta }_i\sin {\gamma }_i& \sin {\beta }_i\\ \sin {\beta }_i\cos {\gamma }_i& -\sin {\beta }_i\sin {\gamma }_i& -\cos {\beta }_i\\ \sin {\gamma }_i& \cos {\gamma }_i& 0\end{array}\right]$$

(9)

Finally, the rotation matrix of the frame $\left\{A_{2i}\right\}$ relative to the frame $\left\{O\right\}$ and the frame $\left\{B_{2i}\right\}$ relative to the frame $\left\{O\right\}$ are:

$${\mathit{R}}_{A_{2i}}^O={\mathit{R}}_{O_i}^O{\mathit{R}}_{A_{1i}}^{O_i}{\mathit{R}}_{A_{2i}}^{A_{1i}}=\left[\begin{array}{ccc}-\cos {\alpha }_i\cos {\zeta }_i& \cos {\alpha }_i\sin {\zeta }_i& -\sin {\alpha }_i\\ -\sin {\alpha }_i\cos {\zeta }_i& \sin {\alpha }_i\sin {\zeta }_i& \cos {\alpha }_i\\ \sin {\zeta }_i& \cos {\zeta }_i& 0\end{array}\right]$$

(10)

$${\mathit{R}}_{B_{2i}}^O={\mathit{R}}_{O_i}^O{\mathit{R}}_{A_{1i}}^{O_i}{\mathit{R}}_{A_{2i}}^{A_{1i}}{\mathit{R}}_{B_{1i}}^{A_{2i}}{\mathit{R}}_{B_{2i}}^{B_{1i}}=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]$$

(11)

where

$$\begin{gathered} {\beta }_i=-{\zeta }_i-\arctan \left(\frac{R_{31}}{R_{11}\cos {\alpha }_i+R_{21}\sin {\alpha }_i}\right)\mathrm{with}-\frac{\pi }{2}<{\beta }_i+{\zeta }_i<\frac{\pi }{2} \\ {\gamma }_i=\arcsin \left(R_{21}\cos {\alpha }_i-R_{11}\sin {\alpha }_i\right)\mathrm{with}-\frac{\pi }{2}<{\gamma }_i<\frac{\pi }{2} \end{gathered}$$

(12)

and

$$\begin{gathered} R_{11}=-\sin {\alpha }_i\sin {\gamma }_i-\cos {\alpha }_i\cos \left({\beta }_i+{\zeta }_i\right)\cos {\gamma }_i \\ {R}_{12}=-\sin {\alpha }_i\cos {\gamma }_i+\cos {\alpha }_i\cos \left({\beta }_i+{\zeta }_i\right)\sin {\gamma }_i \\ {R}_{13}=-\cos {\alpha }_i\sin \left({\beta }_i+{\zeta }_i\right) \\ {R}_{21}=\cos {\alpha }_i\sin {\gamma }_i-\sin {\alpha }_i\cos \left({\beta }_i+{\zeta }_i\right)\cos {\gamma }_i \\ {R}_{22}=\cos {\alpha }_i\cos {\gamma }_i+\sin {\alpha }_i\cos \left({\beta }_i+{\zeta }_i\right)\sin {\gamma }_i \\ {R}_{23}=-\sin {\alpha }_i\sin \left({\beta }_i+{\zeta }_i\right) \\ {R}_{31}=\sin \left({\beta }_i+{\zeta }_i\right)\cos \left({\gamma }_i\right) \\ {R}_{32}=-\sin \left({\beta }_i+{\zeta }_i\right)\sin \left({\gamma }_i\right) \\ {R}_{33}=-\cos \left({\beta }_i+{\zeta }_i\right) \end{gathered}$$

(13)

2.2. Kinematics

2.2.1. Inverse Kinematics

In the inverse kinematics, the position and Euler angles of the active plate are given. The rotation angles of motors need to be determined. A kinematics chain of the Hexa robot is shown in Figure 2. The coordinate of point $A_i$ in frame $\left\{O\right\}$ and $C_i$ in frame $\left\{P\right\}$ are given. The vector ${\mathit{r}}_{C_i}^O$ can be expressed as follows:

$${\mathit{r}}_{C_i}^O={\mathit{r}}_{A_i}^O+\mathit{A}{\mathit{B}}_i+\mathit{B}{\mathit{C}}_i={\mathit{r}}_P^O+\mathit{P}{\mathit{C}}_i$$

(14)

The kinematics equations can be expressed as follows:

$$\begin{array}{l}{l}_r^2=l_a^2+{\left(x_{A_i}-x_{C_i}\right)}^2+{\left(y_{A_i}-y_{C_i}\right)}^2+{\left(z_{A_i}-z_{C_i}\right)}^2\\ \dots \dots +\left(-2l_a\cos {\alpha }_i\left(x_{A_i}-x_{C_i}\right)-2l_a\sin {\alpha }_i\left(y_{A_i}-y_{C_i}\right)\right)\cos {\zeta }_i+(2l_a(z_{A_i}\\ -z_{C_i}))\sin {\zeta }_i\end{array}$$

(15)

where

$$\begin{gathered} {\mathit{r}}_{C_i}^O={\left[x_{C_i}{y}_{C_i}{z}_{C_i}\right]}^T \\ {\mathit{r}}_{A_i}^O={\left[x_{A_i}{y}_{A_i}{z}_{A_i}\right]}^T \end{gathered}$$

(16)

$$\begin{gathered} a_i=l_r^2-l_a^2-{\left(x_{A_i}-x_{C_i}\right)}^2-{\left(y_{A_i}-y_{C_i}\right)}^2-{\left(z_{A_i}-z_{C_i}\right)}^2 \\ {b}_i=-2l_a\cos {\alpha }_i\left(x_{A_i}-x_{C_i}\right)-2l_a\sin {\alpha }_i\left(y_{A_i}-y_{C_i}\right) \\ {c}_i=2l_a\left(z_{A_i}-z_{C_i}\right) \\ \mathrm{with}i=1,\dots ,6 \end{gathered}$$

(17)

The rotation angle ${\zeta }_i$ can be calculated as the following expression:

$${\zeta }_i=\arctan \frac{a_i{c}_i^2-b_i\sqrt{-c_i^2\left(a_i^2-b_i^2-c_i^2\right)}}{c_i\left(a_i{b}_i+\sqrt{-c_i^2\left(a_i^2-b_i^2-c_i^2\right)}\right)}$$

(18)

2.2.2. Forward Kinematics

The procedure of neural network training is presented in Figure 3. In the forward kinematics problem, the rotation angles of the arms are given, and the pose of the end-effector is determined by solving kinematics equations. These equations are nonlinear, so analytically, finding their solutions is highly complicated. For the above reason, artificial neural networks (ANNs) are employed to calculate the approximate pose of the end-effector. Firstly, a data set consisting of the potential pose positions of the active plate is created randomly. This data set should be modified to reject the singular points and points outside the workspace. The modified data set is called the target data in ANN training. From the target data, the inverse kinematics problem is solved and the data set of angles ${\zeta }_i$ of six arms is created. This data set is called the input data in ANN training. The training results for weight and bias factors are used to calculate the forward kinematics problem.

2.3. Dynamics

The dynamics equations can be obtained by several methods, such as the Newton-Euler Formulation, Lagrange equations of the first type, and the Virtual Work Principle. In this study, the Lagrange equations of the first type are applied to derive dynamics equations. Those equations are arranged into two sets and expressed as follows:

$$\frac{d}{dt}\left(\frac{\partial L}{\partial {\dot{q}}_j}\right)-\frac{\partial L}{\partial q_j}={\tau }_j+{\displaystyle \sum }_{i=1}^6{\lambda }_i\frac{\partial f_i}{\partial q_j} \left(j=1,\dots , 6\right)$$

(19)

$$\frac{d}{dt}\left(\frac{\partial L}{\partial {\dot{q}}_j}\right)-\frac{\partial L}{\partial q_j}=Q_j+{\displaystyle \sum }_{i=1}^6{\lambda }_i\frac{\partial f_i}{\partial q_j} \left(j=7,\dots , 12\right)$$

(20)

where $\mathit{q}={\left[{\zeta }_1 {\zeta }_2 {\zeta }_3 {\zeta }_4 {\zeta }_5 {\zeta }_6 x_p y_p z_p \varphi \theta \psi \right]}^T$ presents the generalized coordinates; $L$ denotes the Lagrangian function of the whole system, which is the Lagrangian functions summation of the active plate, the arms, and the rods; ${\tau }_j=K_t{I}_j$ is the actuator torque; $K_t$ is the motor torque constant and $I_j$ is the motor armature current; $Q_j$ is the generalized force; ${\lambda }_i$ $\left(i=1,\dots ,6\right)$ is the $i$th Lagrangian multiplier; $f_i=\left(b_i\cos {\zeta }_i+c_i\sin {\zeta }_i-a_i\right)$ is the $i$th constraint equation; $a_i$, $b_i$, and $c_i$ are in Equation (17).

2.3.1. Inverse Dynamics

Twelve motion variables $q_j$ and six generalized forces ${\tau }_j$ contributed by the actuators are given. Solving the inverse dynamics problem is to find out the six generalized external forces.

The first six-equation set Equation (19) contains the six Lagrange multipliers ${\lambda }_i$ $\left(i=1,\dots ,6\right)$ as the unknowns. The first set can be written in the form:

$${\displaystyle \sum }_{i=1}^6{\lambda }_i\frac{\partial f_i}{\partial q_j}=\frac{d}{dt}\left(\frac{\partial L}{\partial {\dot{q}}_j}\right)-\frac{\partial L}{\partial q_j}-{\tau }_j\left(j=1,\dots ,6\right)$$

(21)

The six generalized external forces $Q_j$ $\left(j=7,\dots ,12\right)$ can be calculated by solving the second six-equation set Equation (20):

$$Q_j=\frac{d}{dt}\left(\frac{\partial L}{\partial {\dot{q}}_j}\right)-\frac{\partial L}{\partial q_j}-{\displaystyle \sum }_{i=1}^6{\lambda }_i\frac{\partial f_i}{\partial q_j}\left(j=7,\dots ,12\right)$$

(22)

2.3.2. Forward Dynamics

Six generalized external forces $Q_j$ and six generalized forces ${\tau }_j$ contributed by the actuators are given. Solving the forward dynamics problem is to find out the twelve motion variables $q_j$.

Analyzing the problem, we have eighteen equations, including twelve dynamics equations in Equations (19) and (20), and six kinematics equations in Equation (15). There are eighteen unknown variables, including twelve motion variables $q_j$ and six Lagrange multipliers ${\lambda }_i$ $\left(i=1,\dots ,6\right)$. This equation system is a differential-algebraic equations system containing differential equations and algebraic equations. The strategy to solve this system of equations is to reduce differential order, leading to an increment in the number of variables and equations. Then, this system of equations becomes algebraic equations. MATLAB has a function to solve differential algebraic equations (DAEs), but solving forward dynamics problems is still extremely complicated.

3. Collision Detection, Isolation and Identification of the Hexa Robot

3.1. Collision Detection

When robot manipulators move freely in the environment, an obstacle may come to the workspace of the robot manipulator at any time. If that obstacle collides with the robot, a collision has occurred. Implementing collision detection is to recognize whether a robot collision occurred or not. An observer calculated based on the dynamics equations can detect the occurrence of a collision. A suitable threshold is selected based on the requirement of the accuracy for this observer. A collision occurs if the observer signal is greater than the chosen threshold.

The generalized momentum of the Hexa robot is defined as:

$$p_j=\frac{\partial L}{\partial {\dot{q}}_j}\left(j=1,\dots ,12\right)$$

(23)

According to the second dynamics equations set Equation (19), the generalized force can be written in the form:

$$Q_j={\dot{p}}_j-\frac{\partial L}{\partial q_j}-{\displaystyle \sum }_{i=1}^6{\lambda }_i\frac{\partial f_i}{\partial q_j}\left(j=7,\dots ,12\right)$$

(24)

The collision identification signal, or simply the residual $\mathit{r}$, is:

$$\dot{\mathit{r}}=-\mathit{K}\mathit{r}+\mathit{K}\mathit{Q}$$

(25)

Substituting $Q_j$ from (24) into (25), we obtain:

$${\dot{r}}_j=-K_j{r}_j+K_j\left({\dot{p}}_j-\frac{\partial L}{\partial q_j}-{\displaystyle \sum }_{i=1}^6{\lambda }_i\frac{\partial f_i}{\partial q_j}\right)\left(j=7,\dots ,12\right)$$

(26)

Lagrange multipliers ${\lambda }_i$ $\left(i=1,\dots ,6\right)$ can be computed using the first dynamics equations set Equation (19). When a collision occurs, at least an element $r_j$ of vector $\mathit{r}$ raises with a time constant 1/$K_j$. The detection is performed as soon as $\left|r_j\right|>r_{low}$, where $r_{low}$is the chosen threshold.

3.2. Collision Isolation

After the residual indicates that a collision occurs on the Hexa robot, it is necessary to identify the collision location. Collision isolation aims at localizing the contact point. In our paper, a vision system consisting of dual depth cameras is applied to detect the contact point if a collision occurs. The first camera’s photos at the time of a collision case are analyzed to identify whether the contact point is at the active plate or the rods. Otherwise, the contact point is detected by processing the image captured by the second camera. Based on the multi-camera vision system, we propose the flowchart of the collision isolation procedure in Figure 4 and the mathematics processing for localization of the contact point in Figure 5.

In this study, every external object coming to the workspace should be from the outside and cause the Hexa robot obscuration. Because the whole Hexa parallel manipulator cannot be observed in only images captured by the first dual depth camera, this camera is responsible for the external object isolation on the active plate and the rods from these images. On the contrary, the external obstacle for the arms case is identified by the second dual depth camera. Both arm and rod situations are considered in the direction to see the obstacle obscuring them.

• Case 1: Contact points are determined by the first camera.

Firstly, a bounding box around the active plate is recognized using YOLOv3. Because the color of the rods is different from the background, the rods can be isolated from the image, so the RGB color image is converted to a YCbCr color image. Then, the Canny edge detector and Hough transform are applied to recognize straight-line profiles around rods. From these detected lines, the rods are identified in the image. The considered image and the background image are converted to grayscale images. A binary image is created by subtracting the grayscale background image from the grayscale considered image. After combining the image subtraction with depth conditions of the objects in the workspace, the external object is identified by removing the Hexa robot from the binary image.

To find the contact point on the active plate, all points belonging to the external object are checked to see if any of them touch the bounding box around the active plate. Then, the image depth is considered at these points. The difference between the depth of these points and the bounding box must be less than a chosen threshold. A point represents intersections of the external object and the bounding box around the active plate, which is obtained by taking an average. A line passes through the mentioned point and the end-effector. Finally, the contact point is located at the junction of the line and the active plate’s hexagonal border.

To find the contact point on a rod, the possible pairs of points belonging to the external object are checked respectively to see if the midpoint of these points is on the rod. Then, the image depth is considered at these points. The difference between these points’ depth and the midpoint’s depth on this rod must be less than the chosen threshold. All of these midpoints are probably the contact point. As a result, a point represents the contact point location, obtained by averaging all the midpoint locations.

• Case 2: Contact points are determined by the second camera.

Based on the image depth, only objects in a limited workspace are considered in the image captured from the second camera. Because the color of the arms is blue, the arms can be isolated from the image. An appropriate threshold for the HSV color scale is chosen for probable extracting of the blue region from the image. Then, the Canny edge detector and Hough transform are applied to recognize straight-line profiles along arms. The direction of arms can be identified from these lines. Two fixed points are chosen as the motor location where two arms are connected with two motors. According to the fixed points and the directions of the arms, all points belonging to the blue region in the arm directions are checked to see if any of them intersect the non-blue region. The intersections are probable contact point locations. The correct contact point location can be detected with the depth conditions.

After the RGB color image is converted to a YCbCr color image, the rods can be isolated from the image. The Canny edge detector and Hough transform are applied to recognize straight-line profiles along rods. The direction of rods is calculated from these profiles. Two points are computed as the connector location where two arms are connected with two rods. According to the computed points and the direction of the rods, the white region points in the rod direction are checked for the non-white intersect probability. The intersections are probable contact point locations. The correct location of the contact point is determined based on the depth conditions and the above junctions.

3.3. Collision Identification

After the collision occurs and the contact point location is known, the information on the contact force should be determined. This study applies a Recursive Newton-Euler Algorithm (RNEA) to calculate how much the contact force was instead of using more devices. The inputs for this algorithm are the current intensity measured from current sensors and the pose of the end-effector obtained from image processing.

Three cases correspond to three possibilities that a collision occurs at an arm, a rod, or the active plate. RNEA is developed separately for the application with the Hexa robot in each case.

$$\begin{gathered} {\mathit{F}}_{ai}=m_a{\dot{\mathit{v}}}_{ai} \\ {\mathit{M}}_{ai}={\mathit{I}}_{ai}{\dot{\mathit{\omega }}}_{ai}+{\mathit{\omega }}_{ai}\times \left({\mathit{I}}_{ai}{\mathit{\omega }}_{ai}\right) \\ {\mathit{F}}_{ri}=m_r{\dot{\mathit{v}}}_{ri} \\ {\mathit{M}}_{ri}={\mathit{I}}_{ri}{\dot{\mathit{\omega }}}_{ri}+{\mathit{\omega }}_{ri}\times \left({\mathit{I}}_{ri}{\mathit{\omega }}_{ri}\right) \\ {\mathit{F}}_p=m_p{\dot{\mathit{v}}}_p \\ {\mathit{M}}_p={\mathit{I}}_p{\dot{\mathit{\omega }}}_p+{\mathit{\omega }}_p\times \left({\mathit{I}}_p{\mathit{\omega }}_p\right) \end{gathered}$$

(27)

where ${\mathit{F}}_{ai}$ is the inertia force acting on the $i$th arm; ${\mathit{M}}_{ai}$is the inertia moment acting on the $i$th arm; $m_a$ is the mass of the arm; ${\mathit{v}}_{ai}$ is the linear velocity of the center of mass (CoM) of the $i$th arm; ${\mathit{I}}_{ai}$ is the tensor of mass moment of inertia of the $i$th arm; ${\omega }_{ai}$ is the angular velocity of the $i$th arm; ${\mathit{F}}_{ri}$ is the inertia force acting on the $i$th rod; ${\mathit{M}}_{ri}$ is the inertia moment acting on $i$th rod; $m_r$ is the mass of the rod; ${\mathit{v}}_{ri}$ is the linear velocity of the center of mass (CoM) of the $i$th rod; ${\mathit{I}}_{ri}$ is the tensor of mass moment of inertia of the $i$th rod; ${\mathit{\omega }}_{ri}$ is the angular velocity of the $i$th rod; ${\mathit{F}}_p$ is the inertia force acting on the active plate; ${\mathit{M}}_p$ is the inertia moment acting on the active plate; $m_p$ is the mass of the active plate; ${\mathit{v}}_p$ is the linear velocity of the center of mass (CoM) of the active plate; ${\mathit{I}}_p$ is the tensor of mass moment of inertia of the active plate; ${\mathit{\omega }}_p$ is the angular velocity of the active plate.

• Case 1: A collision occurs on the active plate.

First step: According to six kinematics chains, calculate ${\mathit{f}}_{B_i}$ and ${\mathit{f}}_{C_i}$ $\left(i=1,\dots ,6\right)$.

$$\mathit{A}{\mathit{B}}_i\times {\mathit{f}}_{B_i}={\mathit{n}}_{A_i}-{\mathit{n}}_{B_i}-\frac{1}{2}\mathit{A}{\mathit{B}}_i\times {\mathit{F}}_{ai}-{\mathit{M}}_{ai}$$

(28)

$${\mathit{f}}_{B_i}-{\mathit{f}}_{C_i}={\mathit{F}}_{ri}$$

(29)

$$\mathit{B}{\mathit{C}}_i\times {\mathit{f}}_{C_i}={\mathit{n}}_{B_i}-{\mathit{n}}_{C_i}-\frac{1}{2}\mathit{B}{\mathit{C}}_i\times {\mathit{F}}_{ri}-{\mathit{M}}_{ri}$$

(30)

where ${\mathit{f}}_{A_i}$ and ${\mathit{n}}_{A_i}$ are the reaction force and the reaction moment exerted on the ith arm, a quantity of ${\mathit{n}}_{A_i}$ can be computed by the ith motor torque; ${\mathit{f}}_{B_i}$ and ${\mathit{n}}_{B_i}=0$ are the force and the moment exerted on the rod by the arm; ${\mathit{f}}_{C_i}$ and ${\mathit{n}}_{C_i}=0$are the force and the moment exerted on the active plate by the ith rod.

Second step: Calculate the external force ${\mathit{F}}_{ep}$.

$${\mathit{F}}_{ep}=-{\displaystyle \sum }_{i=1}^6{\mathit{f}}_{C_i}+{\mathit{F}}_p$$

(31)

• Case 2: A collision occurs at an arm.

First step: Assume collision occurred on the first kinematics chain. According to the five remaining kinematics chains, calculate ${\mathit{f}}_{B_i}$ and ${\mathit{f}}_{C_i}$ $\left(i=2,\dots ,6\right)$ according to Equations (28)–(30).

Second step: Calculate ${\mathit{f}}_{C_1}$ based on the force balance equation at the active plate.

$${\mathit{f}}_{C_1}=-{\displaystyle \sum }_{i=2}^6{\mathit{f}}_{C_i}+{\mathit{F}}_p$$

(32)

Third step: Calculate the external force ${\mathit{F}}_{e1}$ on the first arm.

$$\mathit{A}{\mathit{D}}_1\times {\mathit{F}}_{e1}={\mathit{n}}_{B_1}-{\mathit{n}}_{A_1}+\mathit{A}{\mathit{B}}_1\times {\mathit{f}}_{B_1}+\frac{1}{2}\mathit{A}{\mathit{B}}_1\times {\mathit{F}}_{a1}+{\mathit{M}}_{a1}$$

(33)

where $\mathit{D}$ is the contact point, which is determined in the collision isolation.

• Case 3: A collision occurs at a rod.

First step: Assume collision occurred in the first kinematics chain. According to the five remaining kinematics chains, calculate ${\mathit{f}}_{B_i}$ and ${\mathit{f}}_{C_i}$ $\left(i=2,\dots ,6\right)$according to Equations (28)–(30).

Second step: Calculate $f_{C_1}$ based on the force balance equation at the active plate according to Equation (32).

Third step: Calculate the external force ${\mathit{F}}_{e1}$ on the first rod.

$$\mathit{A}{\mathit{B}}_1\times {\mathit{f}}_{B_1}={\mathit{n}}_{A_1}-{\mathit{n}}_{B_1}-\frac{1}{2}\mathit{A}{\mathit{B}}_1\times {\mathit{F}}_{a1}-{\mathit{M}}_{a1}$$

(34)

$${\mathit{f}}_{B_1}+{\mathit{F}}_{e1}={\mathit{f}}_{C_1}+{\mathit{F}}_{r1}$$

(35)

$$\mathit{B}{\mathit{D}}_1\times {\mathit{F}}_{e1}={\mathit{n}}_{C_1}-{\mathit{n}}_{B_1}+\mathit{B}{\mathit{C}}_1\times {\mathit{f}}_{C_1}+\frac{1}{2}\mathit{B}{\mathit{C}}_1\times {\mathit{F}}_{r1}+{\mathit{M}}_{r1}$$

(36)

where $\mathit{D}$ is the contact point, which is determined in the collision isolation.

4. Experimental Setup

Figure 6 is the block diagram of the Hexa robot system. Both the computer and control software are responsible for controlling the robot via RS232 and solving collision problems. The robot controller is an Arduino Uno R3 microcontroller board. The six Servo motors are used to drive the arms. A current sensor module measures and sends current signals to Arduino Mega 2560. The vision system, including dual stereo cameras, monitors the Hexa robot. Meanwhile, the computer and software perform image processing. The first camera is mounted under the base platform, and the second camera is fixed away from the workspace of the Hexa robot. A force sensor should be used to verify the accuracy of the algorithm. A force/torque sensor is attached to the active plate to measure forces and torques.

Figure 7 is the block diagram of the collision problems solving process. In the first step, the end-effector pose ${\left[x_p y_p z_p \varphi \theta \psi \right]}^T$ can be obtained from processed images captured by the first camera. Six rotation angles ${\left[{\zeta }_1 {\zeta }_2 {\zeta }_3 {\zeta }_4 {\zeta }_5 {\zeta }_6 \right]}^T$ and the actuator torque $\tau$ can be sequentially computed by solving the inverse kinematics problems based on the current ${\left[I_1 I_2 I_3 I_4 I_5 I_6 \right]}^T$ measured by current sensors. In the second step, with the above variables, Lagrangian multipliers ${\lambda }_i$ $\left(i=1,\dots ,6\right)$ can be calculated using the first dynamics equations set. In the third step, the residual $\mathit{r}={\left[r_1 r_2 r_3 r_4 r_5 r_6 \right]}^T$ can be computed with the end effector pose and Lagrangian multipliers. According to the residual $\mathit{r}$, it denotes whether a robot collision occurred or not. In the fourth step, the contact point location ${\mathit{r}}_D$ is determined based on images captured by two stereo cameras. In the final step, after collision isolation, the Recursive Newton-Euler Algorithm is used to identify the external force ${\mathit{F}}_e$. This system is like a sensor wherein the inputs are measured current and the pose of the end-effector, and the outputs are the external contact force and the contact point position. Further research will probably cover exploiting those outputs and regenerating the trajectory.

Figure 8 shows the Hexa robot system used in this study. Six arms are driven by six Servo motors fixed on the base platform. The first stereo camera is mounted under the base platform to monitor the workspace from above. The second stereo camera is fixed on an immovable bar to observe from the outside. The force/torque sensor is mounted on the active plate to evaluate the external force in the experiment.

5. Experimental Results

5.1. Case 1: Collision at the Active Plate

5.1.1. Collision Detection at the Active Plate

In this experiment, the Hexa robot is controlled to move the active plate in a circular trajectory in the workspace. A collision occurred due to an external force acting on the active plate. When the active plate moves in a circle trajectory, the first camera mounted under the base platform captures images. These images are processed to compute the pose of the end-effector, shown in Figure 9. The analog voltage signal from six current sensors is continuously sent to the computer via RS232. The measured current intensity is shown in Figure 10. Equations of the circular trajectory are presented as follows:

$$\{\begin{array}{cc}x=100\cos \left(0.4\pi t\right) & \left(\mathrm{mm}\right)\\ y=100\sin \left(0.4\pi t\right) & \left(\mathrm{mm}\right)\\ z=-550& \left(\mathrm{mm}\right)\\ \alpha =-6& \left(\mathrm{deg}\right)\\ \beta =1-30\cos \left(0.4\pi t\right) & \left(\mathrm{deg}\right)\\ \gamma =1+30\sin \left(0.4\pi t\right) & \left(\mathrm{deg}\right)\end{array}$$

(37)

Three angles $\left(\alpha , \beta , \gamma \right)$ are defined in compliance with the Z-Y-X extrinsic rotation convention of Tait–Bryan angles. The three angles $\left(\alpha , \beta , \gamma \right)$ can be converted to the three angles $\left(\varphi , \theta , \psi \right)$. The rotation matrix is expressed as follows:

$$\mathit{R}=\left[\begin{array}{ccc}\cos \alpha \cos \beta & \cos \alpha \sin \beta \sin \gamma -\sin \alpha \cos \gamma & \cos \alpha \sin \beta \cos \gamma +\sin \alpha \sin \gamma \\ \sin \alpha \cos \beta & \sin \alpha \sin \beta \sin \gamma +\cos \alpha \cos \gamma & \sin \alpha \sin \beta \cos \gamma +\cos \alpha \sin \gamma \\ -\sin \beta & \cos \beta \sin \gamma & \cos \beta \cos \gamma \end{array}\right]$$

(38)

According to Equation (26), the residuals are computed to detect a collision, shown in Figure 11. Because there is a model error less than 2 (N), a suitable threshold $r_{low}$ is chosen equal to 2 (N). When a collision occurs, the residual $r_j$ $\left(j=7,8,9\right)$ raises with a time constant 1/K. Detection is performed as soon as $\left|r_j\right|>r_{low}$, where $r_{low}$ is the chosen threshold. According to Figure 11, a collision occurred at 3.4 s because the residual in the $Z$-axis is greater than $r_{low}=2$ (N). The external force reached the maximum value at 3.8 s during the collision. When contact is lost, $\left|r_j\right|$ decreases quickly until $\left|r_j\right|<r_{low}$.

5.1.2. Collision Isolation at the Active Plate

The image captured when a collision started occurring is considered to find out the contact point location. According to the flowchart of the procedure of collision isolation shown in Figure 4, a bounding box around the active plate is recognized using YOLOv3, shown in Figure 12. The RGB color image shown above is converted to a YCbCr color image. Then, the Canny edge detector and Hough transform are applied to recognize straight-line profiles around rods. These straight lines are shown in Figure 12b. The rods can be identified in the image, shown in Figure 12c. The considered image and the background image are converted to grayscale images. A binary image is created by subtracting the grayscale background image from the grayscale considered image. After performing image subtraction, objects that are not in the workspace cannot be completely eliminated. Combined with the image depth, only objects in the workspace are considered in the image, which is shown in Figure 12d. Because the Hexa robot, including the active plate and the rods, is determined, the external object can be identified by removing the Hexa robot from the binary image. The image containing the external object is shown in Figure 12e. According to the flowchart of the mathematics processing shown in Figure 5, 180 points are determined to satisfy the conditions. All of them are intersections between the external object and the bounding box around the active plate, shown in Figure 12f. A point represents the above 180 pixels by taking an average. A line passes through the mentioned pixel and the end-effector. According to the marker’s location, the hexagonal profile of the active plate can be calculated. The contact point location is the intersection between the mentioned line and the hexagonal profile, shown in Figure 12g. Following the strategy mentioned above, the contact point location in the considered image is determined as ${\left[358.87 618.66\right]}^T$(pixels). According to the camera model, the contact point coordinate is also identified as ${\left[-10.65-48.46-578.45\right]}^T$ (mm) when a collision occurs at the active plate.

A collision occurred on the active plate, and the external force reached the maximum value at 3.8 s. Similarly, 155 points are intersections between the external object and the bounding box around the active plate. The contact point location in the considered image is determined as ${\left[406.37 580.09\right]}^T$ (pixels). The contact point coordinate is also identified as ${\left[15.96-71.62-579.72\right]}^T$ (mm) when the external force reaches the maximum value.

5.1.3. Collision Identification at the Active Plate

According to Equation (27), the inertia forces acting on the arms, the rods, and the active plate are calculated. According to Equations (28)–(31), the external force acting on the active plate can be calculated, which is shown in Figure 13.

Based on Figure 13, the external forces computed by Lagrange’s equations and RNEA are close to each other and acceptable when these results are compared with the force sensor. The collision started occurring on the active plate at 3.4 s, and the external force was about ${\left[-0.67-0.69 3.13\right]}^T$ (N). Then, the force reached the maximum value of ${\left[-5.36-2.27 11.036\right]}^T$ (N) at 3.8 s.

The root mean square error (RMSE) is applied to evaluate the accuracy of RNEA, expressed as follows:

$$RMSE=\sqrt{\frac{{{\displaystyle \sum }}_1^n{e}_i^2}{n}}$$

(39)

where $e_i$ $\left(i=1,2,\dots ,50\right)$ is the error between the computed force by RNEA and the force sensor; $n=50$ is the number of samples.

RMSE in the $X$-axis is equal to 0.819 (N), RMSE in the $Y$-axis is equal to 0.4762 (N), and RMSE in the $Z$-axis is equal to 1.2823 (N).

5.2. Case 2: Collision at an Arm

5.2.1. Collision Detection an Arm

In this experiment, when the active plate moved in a helix trajectory, a collision occurred due to an external force acting on this arm. The pose of the end-effector is shown in Figure 14. The measured current intensity is shown in Figure 15. Equations of the helix trajectory are presented as follows:

$$\{\begin{array}{cc}x=100\cos \left(0.4\pi t\right) & \left(\mathrm{mm}\right)\\ y=100\sin \left(0.4\pi t\right) & \left(\mathrm{mm}\right)\\ z=-550+5t& \left(\mathrm{mm}\right)\\ \alpha =-6& \left(\mathrm{deg}\right)\\ \beta =1-25\cos \left(0.4\pi t\right)& \left(\mathrm{deg}\right)\\ \gamma =1+25\sin \left(0.4\pi t\right)& \left(\mathrm{deg}\right)\end{array}$$

(40)

According to Figure 16, a collision occurred at 5.2 s because the residual in the $Z$-axis is greater than $r_{low}=2$ (N). The external force reached the maximum value at 5.6 s during the collision.

5.2.2. Collision Isolation an Arm

When a collision started occurring, no contact point was detected in the image captured from the first camera. The image captured from the second camera at 5.2 s is considered to find out the contact point location, shown in Figure 17a. According to the flowchart of the procedure of collision isolation, shown in Figure 4, objects in this limited workspace are considered in the image captured from the second camera, shown in Figure 17b. Because the color of the arms is blue, the arms can be isolated from the image. An appropriate threshold for the HSV color scale is chosen to extract the blue region in the image, which is shown in Figure 17c. Then, the Canny edge detector and Hough transform are applied to recognize straight-line profiles along arms. These straight lines are shown in Figure 17d. According to the flowchart of the mathematics processing, shown in Figure 5, a fixed pixel is chosen as a motor location where an arm is connected with that motor. According to the fixed pixel and the direction of the arm, all pixels belonging to the blue region in the arm direction are checked to see if any of them intersect the non-blue regions, shown in Figure 17e. The intersections are potential contact point locations. Two intersections should be considered, shown in Figure 17f. The first intersection coordinate is ${\left[145 811\right]}^T$ (pixels), the depth at this point is 440 (mm). This intersection is not the contact point location because a point in the non-blue region has a coordinate equal to ${\left[165 818\right]}^T$ (pixels), and its depth is 379 (mm). The difference between two point’s depth is larger than a chosen depth threshold. The second intersection coordinate is ${\left[262 850\right]}^T$ (pixels), and the depth at this point is 419 (mm). This intersection is the contact point location because a pixel in the non-blue region has a coordinate equal to ${\left[242 845\right]}^T$ (pixel), and its depth is 408 (mm). The difference between two point’s depth is smaller than a chosen depth threshold. The contact point coordinate is identified as ${\left[-113.25 263.67-111.3\right]}^T$ (mm), shown in Figure 17g.

At 5.6 s, a collision occurred on the arm, and the external force reached the maximum value. The intersection coordinate is ${\left[232 885\right]}^T$ (pixels), and the depth at this point is 409 mm. The contact point coordinate is identified as ${\left[-121.99 278.95-99.66\right]}^T$ (mm).

5.2.3. Collision Identification an Arm

According to Equation (27), the inertia forces acting on the arms, the rods, and the active plate are calculated. According to Equations (32) and (33), the external force acting on the arm can be calculated, shown in Figure 18.

Based on Figure 18, the collision started occurring on the arm at 5.2 s, and the external force was about ${\left[\begin{array}{ccc}1.976& -2.557& -2.254\end{array}\right]}^T$ (N). Then, the force reached the maximum value of ${\left[\begin{array}{ccc}4.941& -7.38& -5.785\end{array}\right]}^T$ (N) at 5.6 s.

According to Equation (39), root mean square error (RMSE) is applied to evaluate the accuracy of RNEA. RMSE in the X-axis is equal to 1.1193 (N), RMSE in the Y-axis is equal to 1.4974 (N), and RMSE in the Z-axis is equal to 1.1056 (N).

5.3. Case 3: Collision at a Rod

5.3.1. Collision Detection at a Rod

In this experiment, when the active plate moved in an ellipse trajectory, a collision occurred due to an external force acting on this rod. The pose of the end-effector is shown in Figure 19. The measured current intensity is shown in Figure 20. Equations of the ellipse trajectory are presented as follows:

$$\{\begin{array}{cc}x=80\cos \left(0.4\pi t\right) & \left(\mathrm{mm}\right)\\ y=110\sin \left(0.4\pi t\right) & \left(\mathrm{mm}\right)\\ z=-550& \left(\mathrm{mm}\right)\\ \alpha =-6& \left(\mathrm{deg}\right)\\ \beta =1-30\cos \left(0.4\pi t\right) & \left(\mathrm{deg}\right)\\ \gamma =1+30\sin \left(0.4\pi t\right) & \left(\mathrm{deg}\right)\end{array}$$

(41)

According to Figure 21, a collision occurred at 4.4 s because the residual in the $X$-axis is greater than $r_{low}=2$ (N). The external force reached the maximum value at 4.6 s during the collision.

5.3.2. Collision Isolation at a Rod

In a similar method to Case 1, the collision isolation results are shown in Figure 22a–e. According to the flowchart of the mathematics processing shown in Figure 5, 46 midpoints are determined to satisfy the conditions, as shown in Figure 22f. A point represents the above 46 pixels by taking an average. The contact point location is shown in Figure 22 g. Following the strategy mentioned above, the contact point location in the considered image is determined as ${\left[382.8 698.83\right]}^T$ (pixels). According to the camera model, the contact point coordinate is also identified as ${\left[122.93-144.08-411.59\right]}^T$ (mm) when a collision occurs at a rod.

At 4.6 s, a collision occurred on the rod, and the external force reached the maximum value. Similarly, 447 pixels are potential contact points on the rod. The contact point location in the considered image is determined as ${\left[399.56 702.97\right]}^T$ (pixels). The contact point coordinate is also identified as ${\left[125.49-137.89-406.5207\right]}^T$ (mm) when the external force reaches the maximum value.

5.3.3. Collision Identification at a Rod

According to Equation (27), the inertia forces acting on the arms, the rods, and the active plate are calculated. According to Equations (34)–(36), the external force acting on the rod can be calculated, which is shown in Figure 23.

Based on Figure 23, the collision started occurring on the rod at 4.4 s, and the external force was about ${\left[-2.487-0.925-1.121\right]}^T$ (N). Then, the force reached the maximum value of ${\left[-8.055-2.631-3.837\right]}^T$ (N) at 4.6 s.

According to (39), the root mean square error (RMSE) is applied to evaluate the accuracy of RNEA. RMSE in the $X$-axis is equal to 1.2268 N, RMSE in the $Y$-axis is equal to 1.1874 N, and RMSE in the $Z$-axis is equal to 1.2627 N.

6. Conclusions

This study has presented a strategy to solve collision problems for the Hexa parallel robot. The proposed virtual force observer is created by combining the inverse dynamic problem solution and a low-pass filter. When the Hexa robot controls the active plate move with the designed trajectory, the observer monitors and indicates the collision time in the cases of three main parts: active plate, rods, and arms. The multi-dual depth camera system performs effectively for monitoring the workspace and the external obstacle. The contact point location in each collision case is obtained based on the proposed flowchart for the collision phases. The Recursive Newton-Euler algorithm is applied to analyze the dynamics free-body diagram of the Hexa robot. With the determined contact point location, finally, the intensity and directions of the external force are identified based on the Recursive Newton-Euler algorithm. The comparison results for the experimental force sensor show the potential for solving the collision problem in the Hexa robot structure.