Real-Time Kinematics-Based Self-Collision Avoidance Algorithm for Dual-Arm Robots

Abstract

Self-collisions of a dual-arm robot system can cause severe damage to the robot. To deal with this problem, this paper presents a real-time algorithm for preventing self-collisions in dual-arm systems. Our first contribution in this work is a novel collision model built using discrete spherical bounding volumes with different radii. In addition, we propose a sensitivity index to measure the distance between spheres with different radii in real time. Next, according to the minimal sensitivity index between different spheres, the repulsive velocity is produced at the centers of the spheres (control points), which the robot uses to generate new motion based on the robot kinematic model. The proposed algorithm offers the additional benefits of a decrease in the number of bounding spheres, and a simple collision model that can effectively decrease the computational cost of the process. To demonstrate the validity of the algorithm, we performed simulations and experiments by an upper-body humanoid robot. Although the repulsive velocity acted on the control points, the results indicate that the algorithm can effectively achieve self-collision avoidance by using a simple collision model.

1. Introduction

Dual-arm robotic systems are widely used in various settings to expand the working range compared with single-arm robotic systems. As described in [1,2], dual-arm systems are widely used in space robots and disaster environments. To perform complicated tasks, each arm is provided with different targets and trajectories. One of the challenges in dual-arm cooperation control is to avoid self-collisions, which dramatically constrain the working range and affect the continuous motion of each arm. In this paper, we propose a self-collision avoidance method, where a dual-arm robot has the ability to detect and avoid self-collisions, to deal with such problems.

For self-collision avoidance methods, the collision model is critical. In collision models, a model of the robot is approximated, and relative position information is obtained between the robot links. In a collision model, the distance between the two arms is calculated in real time. A function related to the minimum distance is used to prevent robot collisions.

A collision model is always formed by various convex hulls. In previous studies, a collision detection model was built using a large number of polygons [3,4]. To tightly fit the different volumes of the robot, researchers have developed algorithms to build each convex polyhedral model. Examples include the tight-fitting oriented bounding box trees (OBB) algorithm [5], and the axis-aligned bounding boxes (AABB) algorithm [6]. Many algorithms have been proposed to calculate the distance in convex polyhedral models such as the fast and robust polyhedral collision detection (V-Clip) algorithm [7]. As the features of polyhedral-based collision model are vertices, edges, and faces [8], the computational cost to determine if a pair of polyhedrons are disjointed and to compute the distance between them is high [7]. To reduce the computational cost between different collision models, researchers have developed sphere-based collision models to replace polygon-based collision models [9,10,11]. In [9,10], a sphere-based collision model was built using a sweep sphere model. For the sweep sphere line, the distance between two sweep sphere lines is calculated. The minimum distance may have a discontinuous gradient when the pair of closest points is on the two shapes [11]. In [11], an improved method was proposed, where a collision model was built using finite discretized spheres with the same radius. Compared with the sweep sphere model, the finite discretized sphere collision model algorithm [11] only needs to measure the distance between the center of each sphere (all possible distances between different spheres), which has a low computational cost. However, due to the difference in the links for each arm, the model of the origin robot may not be able to be approximated using spheres with the same radius.

Self-collision avoidance methods can be divided into two categories: (i) on-line (real-time) self-collision avoidance [3,6,12,13,14,15] and (ii) off-line planning of a collision-free trajectory [9,16,17] for the robot. Real-time self-collision avoidance methods always rely on the minimum distance between collision pairs. Earlier researchers proposed a collision model to check a collision pair in real time, where the robot abruptly stops when the collision model predicts a collision [4,6]. However, this behavior may influence the stability of the robot. To improve the results, researchers have proposed the skeleton algorithm [13,18,19,20], which establishes an artificial repulsive potential field to generate the virtual repulsive force on the robot links. Meanwhile, the skeleton algorithm has already been applied on the The German Aerospace Center(DLR) humanoid manipulator Justin [13]. Based on the dynamic model of the robot, the robot can generate a smooth trajectory to avoid self-collision. Although these algorithms can achieve self-collision avoidance, they do not consider the performance of the arm end-effector. Compared to these traditional methods, some researchers have proposed a unified framework for detecting collisions in real time by using a series of sensors capable of estimating the joint torque and acceleration [14]. However, using sensors always increases the cost of the robot due to the more expensive hardware.

In off-line methods, a collision-free trajectory is planned in advance. Based on the rapidly-exploring random tree (RRT) [21] algorithm, researchers have proposed an algorithm to plan a collision-free path [22] for each arm in advance. The extended stochastic trajectory optimization for motion planning (STOMP) algorithm, which is an improvement of the STOMP [23] algorithm, obtains a qualitatively collision-free trajectory according to user preferences [24]. Some researchers have presented a method for generating and synchronizing path-constrained trajectories for multi-robot systems [25]. Regarding the Asea Brown Boveri (ABB) company, they proposed a method to monitor the multi-arm, whether local at the collision space at the same time [26], before planning the path of each arm. With the development of machine learning, researchers can set different collision regions using efficient data-driven techniques to predict the joint collision position based on a support vector machine (SVM) algorithm [16,17], which may lead to higher computational costs. In particular, the method proposed in [16] has already been applied on the Istituto Italiano di Tecnologia (IIT) SCAFoI humanoid robot. Compared with on-line methods, off-line planning can be dangerous. For example, a robot can be misguided in planning a collision trajectory. In off-line planning, the collision-free trajectory must be planned before the arm moves to the target, so off-line planning also leads to higher computational costs.

In this paper, we propose an optimized collision model that decreases the computational cost as well as the robot hardware cost. In addition, it increases the reaction speed and accuracy simultaneously without any sensors to detect the relative distance of the two arms. To demonstrate the validity of the algorithm, two independent arms with conflicting tasks are considered, so that each arm is in danger of self-collision. The goal of this study is to develop a real-time self-collision avoidance method that consists of: (1) a collision model based on a finite number of spheres with different radii; and (2) a suitable self-collision avoidance strategy. The main contributions of this study are summarized as follows:

(1)

In this study, we developed a collision model by using multiple discretized spherical bounding volumes with different radii, which has fewer spheres and can enclose the robot with a low approximation error.

(2)

To calculate all possible distances between spheres with different radii, a sensitivity index was proposed to measure the distance between different spheres. According to the minimal sensitivity index, the magnitude of the repulsive velocity can be measured using the sigmoid function.

(3)

The repulsive velocity will generate on each control point of each link. Based on the kinematic model of each arm, the repulsive velocity on each control point is projected in the velocity direction of the end-effector, which helps the robot modify the magnitude of the end-effector velocity, while generating the repulsive velocity on the control points to avoid self-collision at the collision spheres.

The remainder of this paper has the following structure. In Section 2, the collision model of the dual-arm robot is developed, along with the method for calculating the minimum distance between the sphere bounding volumes. Section 3 describes the development of the real-time self-collision avoidance strategy of the robot. The repulsive velocity acts on the center of the spherical volumes to generate a smooth trajectory to avoid self-collision. In Section 4, the modeling parameters are identified in an example in which a robot is used. Simulations and experiments demonstrate the validity of the proposed algorithm. Section 5 concludes this work.

2. Collision Model of the Robot

2.1. The Dual-Arm Robot

The algorithm was implemented on the upper body of a humanoid dual-arm robot as illustrated in Figure 1a. The robot was designed by our group (the explosives ordnance disposal robot group of Shenyang Institute of Automation). The robot was equipped with two arms with seven degrees of freedom (7-DOF) and the torso with 2-DOF. Meanwhile, each arm was equipped with a gripper with 1-DOF as the end-effector.

To implement our algorithm, we first divided the robot into three parts: the left arm, the right arm, and the torso. Figure 1b shows the structure of the robot, where the dotted lines represent the joints of the robot; $q_{li}$, $q_{ri}$, and $q_{ti}$ are defined as the i-th joint angle of the left-arm, the right-arm, and the torso, respectively. To avoid collision of the links of the left arm, the angle limits of the i-th joint of the left arm were set as [$q_{li,l},q_{li,u}$]. Similarly, the angle limits of the i-th joint of the right arm and torso were set as [$q_{ri,l},q_{ri,u}$] and [$q_{ti,l},q_{ti,u}$], respectively.

The frames are defined as follows. The base frame $\left\{S\right\}$ is defined as the world frame in which the robot is located. Frame $\left\{S_{ki}\right\}$ is defined as the i-th link frame of the robot part k (k = t, l, r, where t indicates the torso part, l the left-arm, and r the right-arm). The positions of the control points (center of each spherical volume) are defined according to these link frames. In addition, the position vector ${}_{}{}^{ki}P\in R^3$ and the rotation matrices ${}_{ki}{}^{S}R$ are defined from $\left\{S\right\}$ to $\left\{S_{ki}\right\}$; $J_{kp}$ are Jacobian matrices for control points p with respect to the $\left\{S_{ki}\right\}$ frame, as illustrated in Figure 2. In this way, a point expressed as $P_0={\left(x_0,y_0,z_0\right)}^T$ in the $\left\{S_{ki}\right\}$ frame can be expressed in the base frame as ${}_{}{}^{s}P{}_0={}_k{}^{S}R{}_{}{}^{ki}P{}_0$ + ${}_{}{}^{S}P{}_{kiORG}$. Thus, all the control points are expressed in the base frame $\left\{S\right\}$ to detect a collision in real time.

Point a is expressed in frame B as ${}_{}{}^{B}P{}_a={\left(x_a,y_a,z_a\right)}^T$ and in frame A as ${}_{}{}^{A}P{}_a={\left(x_a^{\prime },y_a^{\prime },z_a^{\prime }\right)}^T$, whereas point b is expressed in frame C as ${}_{}{}^{C}P{}_b={\left(x_b^{},y_b^{},z_b^{}\right)}^T$ and in frame A as ${}_{}{}^{A}P{}_b={\left(x_a^{\prime },y_a^{\prime },z_a^{\prime }\right)}^T$, as illustrated in Figure 2b. These can be calculated as follows:

$${}_{}{}^{A}P{}_a={}_B{}^{A}R{}_{}{}^{B}P{}_a+{}_{}{}^{A}P{}_{BORG},$$

(1)

$${}_{}{}^{A}P{}_b={}_C{}^{A}R{}_{}{}^{C}P{}_a+{}_{}{}^{A}P{}_{CORG},$$

(2)

The distance between different points in each part can be computed as

$$d_{ab}=\sqrt{{\left(x_a^{\prime }-x_b^{\prime }\right)}^2+{\left(y_a^{\prime }-y_b^{\prime }\right)}^2+{\left(z_a^{\prime }-z_b^{\prime }\right)}^2}$$

(3)

2.2. Collision Model

The main propose of the collision model is to evaluate the minimum distance between two arms. After the model is built, the minimum distance between elements in the collision model must be evaluated. The function related to the minimum distance prevents the robot from colliding.

Based on the collision model using discretized spheres with the same radius [21], the method used in our robot arms is illustrated in Figure 3c. In this work, we proposed building a collision model using discretized finite spherical volumes with different radii at specific positions (control points) as illustrated in Figure 3d. Comparing Figure 3c with Figure 3d, the later collision model can be closer to the original arm. In this way, the collision model of each link of each arm can be described using a few different spheres. In general, we always used the same radius spheres at the same link and chose the size of each sphere according to the size of each link, which is illustrated in Figure 3a. The detection of all possible distances between different control points with different spheres is illustrated in Figure 3b.

The minimum distance between these bounding volumes produces the repulsive velocity. As illustrated in Figure 3, distance dab represents the distance between different spheres with different radii. To detect all the possible distances between different spheres and to calculate the minimum distance with different pairs of spheres, we propose the sensitivity index β (β > 0) and define it in Equation (4) to measure the collision between different spheres according to two spherical volumes with different radii r_a and r_b (r_a < r_b).

$${\beta }_{ab}=\frac{d_{ab}}{r_a+r_b},$$

(4)

$$\{\begin{array}{c}{\beta }_{ab}<1,\mathrm{collision}\\ {\beta }_{ab}=1,\mathrm{at}\mathrm{the}\mathrm{moment}\mathrm{of}\mathrm{collision}\\ {\beta }_{ab}>1,\mathrm{no}\mathrm{collision}\end{array},$$

(5)

The minimum distance between these bounding volumes produces the repulsive velocity. As illustrated in Figure 3, distance d_ab represents the distance between different spheres with different radii. To detect all the possible distances between different spheres and to calculate the minimum distance with different pairs of spheres, we proposed the sensitivity index β (β > 0) and defined it in Equations (4) and (5) to measure the collision between different spheres according to two spherical volumes with different radii r_a and r_b (r_a < r_b), as illustrated in Figure 4.

$${\beta }_{lr}=[\begin{array}{cccc}{\beta }_{11}& {\beta }_{12}& ..& {\beta }_{1m}\\ \dot{.}& \dot{.}& \dot{.}.& \dot{.}\\ {\beta }_{n1}& {\beta }_{n2}& ..& {\beta }_{nm}\end{array}],$$

(6)

In Equations (7)–(9), ${\beta }_{lrmin}^{}$ denotes the minimum element in the matrix:

$${\beta }_{lrmin}^{}=min\left({\beta }_{lr}^{}\right),$$

(7)

3. Self-Collision Avoidance Strategy

Our self-collision avoidance strategy relies on relationships between the minimal sensitivity index, the repulsive velocity, and the angular velocity of the arm. Section 3.1 illustrates the relationship between the minimal sensitivity index and the repulsive velocity, which shows how to use the repulsive velocity according to the minimal sensitivity index. Section 3.2 illustrates the relationship between the repulsive velocity and the velocity at the end-effector of each arm. Section 3.3 shows that to solve self-collision using our algorithm, each arm must be controlled at the joint velocity level.

3.1. Repulsive Velocity between Two Spheres

Each arm of the robot has multiple bounding volumes, and the minimal sensitivity index ${\beta }_{lrmin}^{}$ indicates the relationship between the closest bounding volumes. When ${\beta }_{lrmin}^{}\le 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^{\left(\alpha \times \left({\beta }_{lrmin}^{}-0.7\right)\right)}},$$

(8)

where Vmax is the maximum velocity allowed at the end of the robot and $\mathsf{\alpha }$ is the shape factor, as described in [27]. If Vmax = 0.25 and $\mathsf{\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 }_{lrmin}^{}$ resulted in a larger repulsive velocity, helping the robot avoid self-collision. Meanwhile, we can conclude that when ${\beta }_{lrmin}^{}=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 ${\mathrm{r}}_{\mathrm{cy},\mathrm{li}}^{}$, the radius $r_{cm}^{}$ of each sphere can be defined according to $\frac{r_{cy,li}^{}}{r_{cm}^{}}=0.4$. In this way, we can define the radius of each sphere. When ${\beta }_{lrmin}^{}=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.

As illustrated in Figure 5b, the position of the control point a, on the left, is defined as ${(x_a^{\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

$$\{\begin{array}{c}\overrightarrow{ab}=\frac{{x_a^{\prime },y_a^{\prime },z_a^{\prime }}^T-{x_b^{\prime },y_b^{\prime },z_b^{\prime }}^T}{d_{ab}}\mathrm{direction}\mathrm{on}\mathrm{point}a\\ \overrightarrow{ba}=\frac{{x_a^{\prime },y_a^{\prime },z_a^{\prime }}^T-{x_b^{\prime },y_b^{\prime },z_b^{\prime }}^T}{d_{ab}},\mathrm{direction}\mathrm{on}\mathrm{point}b\end{array}$$

(9)

The repulsive velocity produced at the control points can be computed as:

$$\{\begin{array}{c}{v}_{re,a}=\overrightarrow{ab}||v_{re}||,\mathrm{repulsive}\mathrm{velocity}\mathrm{on}\mathrm{point}a\\ v_{re,b}=\overrightarrow{ba}||v_{re}||,\mathrm{repulsive}\mathrm{velocity}\mathrm{on}\mathrm{point}b\end{array}$$

(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 ${\dot{\theta }}_{ki}$, where i represents the i-th joint of each arm. Thus, the joint velocities ${\dot{\theta }}_k=[{\dot{\theta }}_k{}_1\dots {\dot{\theta }}_k{}_{j-1}{\dot{\theta }}_k{}_j]\in R^j$ can be calculated as follows:

$$\dot{{\theta }_k^{}}=J_k^{\#}\left[\begin{array}{c}{v}_k\\ {\omega }_k\end{array}\right],k=l\mathrm{or}r,$$

(11)

where $J_k^{}={\left[J_v^{}{J}_{\omega }^{}\right]}^T$ are the Jacobian matrices with respect to the end-effector of the robotic arms, and $J_k^{\#}$ is the pseudo-inverse; $v_k\in R^3$ and ${\omega }_k\in R^3$ represent the linear and angular velocity of the end–effector of each arm, respectively.

3.2. Repulsive Velocity at the End-Effector

As illustrated in Figure 6, a bounding sphere is centered at point of the left arm and at point b of the right arm of the robot. These control points both have repulsive velocity $v_{rv,k}$(k = l or r) when ${\beta }_{ab}^{}={\beta }_{lrmin}^{}=\min \left({\beta }_{lr}^{}\right)\le 1$, which implies that these spherical bounding volumes will collide. The repulsive velocity acts on the centers of the spheres, as discussed in Section 2.

The repulsive velocity needs to be controlled at the joint velocity level. To determine the repulsive velocity of the control point, only the joints prior to this control point should be given a repulsive joint velocity. In the case shown in Figure 6, the repulsive joint velocity is generated on joints 1–5 of the left arm, and on joints 1–7 of the right arm.

Defining part of the repulsive joint velocity of the k arm from joint 1 to joint i (i ≤ j) as ${\dot{q}}_{rvp,k}={\left[{\dot{q}}_{1k}^{}\dots {\dot{q}}_{ik}^{}\right]}^T\in R^i$ (k = l or r) of the arm, the repulsive joint velocity of this part of the arm can be calculated as follows:

$${\dot{q}}_{rep,k}=J_{rv,k}^{\#}\left[\begin{array}{c}{v}_{rv,k}\\ {\omega }_{rv,k}\end{array}\right],(k=l\mathrm{or}r),$$

(12)

where $J_{re,k}={\left[J_{re,v}{J}_{re,\omega }\right]}^T\in R^{6\times i}$ are Jacobian matrices for the control points, as illustrated in Figure 2a in arm k (k = l or r). Here, only the positions of the two control points are controlled, so we set ${\omega }_{re,k}={\left[000\right]}^T\in R^3$ in arm k (k = l or r).

The velocity of the joints after/distal to the control point was set to zero. In this way, the repulsive joint velocity ${\dot{q}}_{re,k}$ (k = l or r) can be obtained as

$${\dot{q}}_{re,k}=[{\dot{q}}_{repar,k}0..0]\in R^j,$$

(13)

According to the repulsive joint velocity, the end-effector velocity of the arm is generated with an end-effector repulsive velocity $v_{re,k,end}$ as

$$v_{re,k,end}=J_k^{}{\dot{q}}_{re,k},(k=l\mathrm{or}r),$$

(14)

Due to the collision model, which was built using discretized spheres with different radii and detecting the minimum distance between the spheres, the minimum distance points could change instantaneously, causing discontinuous velocities and non-smoothness in the overall behavior of the system. In this way, we chose to project the velocity in the direction of the end-effector velocity, which only changes the magnitude of the end-effector velocity, to alleviate this situation. Meanwhile, this strategy can also generate the repulsive velocity.

3.3. Real-Time Self-Collision Avoidance

As illustrated above, the end-effector repulsive velocity $v_{re,k,end}$ calculated by (14) can have different directions from the original end-effector velocity $v_{end,k}$ (k = l or r), which is planned in advance for a dual-arm task. The end-effector repulsive velocity can drive the effector to an uncertain direction, and the desired task may not be finished. To solve this problem, the algorithm modifies only the magnitude of the original end-effector velocity and keeps the direction unchanged.

Using the end repulsive velocity $v_{re,k,end}$ calculated in (14), the end-effector repulsive velocity $v_{re,k,end}$ (k = l or r) is then projected to the original velocity of the end-effector $v_{end,k}$ (k = l or r), which is represented by the red lines in Figure 6.

The angle γ_k between $v_{end,k}$ and $v_{re,k,end}$ (k = l or r) and can be calculated as

$${\gamma }_k=\arccos \left({\left(\frac{v_{re,k,end}}{||v_{re,k,end}||}\right)}^T\frac{v_{end,k}}{||v_{end,k}||}\right),(k=l\mathrm{or}r),$$

(15)

The projected repulsive velocity $v_{pro,k}$ (k = l or r) can be calculated as

$$v_{pro,k}=\Vert v_{re,k,end}\Vert \cos {\gamma }_k\frac{v_{end,k}}{\Vert v_{end,k}\Vert },(k=l\mathrm{or}r),$$

(16)

Summing the projected repulsive velocity $v_{pro,k}$ and the original end-effector velocity $v_{end,k}$, the new velocity of the end-effector $v_{new,k}$ (k = l or r) is obtained as

$$v_{new,k}=v_{end,k}+v_{pro,k},(k=l\mathrm{or}r),$$

(17)

Next, the new joint velocity can be calculated using (18), where ${\dot{\theta }}_{new,k}=[{\dot{\theta }}_{k,1}..{\dot{\theta }}_{kj-1,}{\dot{\theta }}_{kj}]\in R^j$ (k = l or r) are the new joint velocities:

$${\dot{\theta }}_{new,k}=J_{rv,k}^{\#}\left[\begin{array}{c}{v}_{new,k}\\ 0\end{array}\right],(k=l\mathrm{or}r),$$

(18)

4. Simulation and Application of the Algorithm

This section presents the simulation and experiment for self-collision avoidance by a robot using the proposed algorithm. The robot has dual 7-DOF arms, as shown in Figure 1. To avoid collision of the links with one arm, the joints were set as indicated in

Table 1. Constraints of joints.

Joint	1	2	3	4	5	6	7
Joint limit (degrees)	−180, 25	−270, 90	−270, 90	−220, 45	−180, 180	0, 100	−90, 90

. According to the specifications of the robot, the maximum velocity of the end-effect was below 2.5 m/s.

The collision model was constructed with 10 spheres centered at 10 control points on each arm, as illustrated in Figure 7. The red lines in Figure 7 represent the distances between the control points on different arms. The diameters of the spheres are listed in

Table 2. Sphere diameters.

Point	1	2, 3	4	5, 6	7	8	9, 10
Link	1	2	3	4	5	6	7 (end-effector)

.

As the collision model is built, the sensitivity index β_lr is calculated according to Section 2. The new joint velocities ${\dot{\theta }}_{new,k}=[{\dot{\theta }}_{k,1}..{\dot{\theta }}_{kj-1,}{\dot{\theta }}_{kj}]\in R^j$ (k = l or r) are then calculated for self-collision avoidance when one spherical volume collides with another spherical volume.

4.1. Simulations

The simulation was performed using an Intel Atom^® E3845 processor (Quad Core, 1.91 GHz, 10 W) with 32 GB of RAM, and Ubuntu 16.04 using ROS Kinetic. The simulation results are shown in Figure 8.

The algorithm was simulated on a dual-arm robot with 10 control points on each arm. First, the system needs to detect and update the matrix in real time. Following the steps presented in Section 3, the new joint velocities were calculated, which changes the motion of the robot to avoid collision. These processes are illustrated in Figure 8.

In Figure 8(a.i, i = 1, 2), we intentionally set the same target positions for the two arms (i.e., without the self-collision avoidance algorithm, the two arms will collide as the end-effectors approach their targets). Actually, with the self-collision avoidance strategy, the repulsive velocities are calculated on the control points. In Figure 8(a.1), the repulsive velocity acted on the end-effector of the left arm and link 6 of the right arm; in Figure 8(a.2), the repulsive velocity acted on the end-effectors of the robotic arms. Both the left and right arms changed their motion to avoid collision. The repulsive velocity on each arm both act on the end-effectors, which makes each arm move away from their target.

In Figure 8(b.i, i = 1, 2, 3), we intentionally set the left and right end-effectors with different targets. Without the self-collision avoidance algorithm, the two arms will collide as the end-effectors approach their targets. With the self-collision avoidance strategy, the repulsive velocities were calculated on the control points. In Figure 8(b.1), the repulsive velocity acted on link 7 of the left arm and link 6 of the right arm; in Figure 8(b.2) the repulsive velocity acted on the end-effector of the left arm and link 6 of the right arm; in Figure 8(b.3) the repulsive velocity acted on link 6 of the left arm and link 6 of the right arm. The simulated process of avoiding self-collision is illustrated in Figure 8(b.i, i = 1, 2, 3). The right arm and left arm both changed their trajectories to avoid self-collision, but one of the dual-arms was also close to the target in the other trajectory. In Figure 8(b.1), the left arm was close to the target. In Figure 8(b.2), the left arm was close to the target. In Figure 8(b.3), the right arm was close to the target.

A common approach for self-collision avoidance is the artificial potential field in which one of the arms is modeled with an obstacle. However, these strategies often result in a (topologically unnecessary) minimum [28]. Meanwhile, we completed two simulations using the traditional artificial potential field method. These methods do not consider the velocity vector of the end-effector. In Figure 9a, two arms of dual-arm had a common target and in Figure 10a, two arms of dual-arm had different targets. The trajectories of each arm illustrated that the two arms both easily fell into the local minimum and vibrated around the local minimum points, as illustrated in Figure 9b, Figure 10b, respectively.

Compared with the traditional artificial potential field method, the method of this paper improved the problems of the traditional method due as we considered the action of the end-effector. Meanwhile, the simulation results of the different tasks described above prove the effectiveness of this real-time self-collision algorithm based on the kinematic model of a robot. The algorithm uses a simple collision model and the distance calculation method to decrease the computational costs of the algorithm.

4.2. Experimental Results

The algorithm was implemented on a dual-arm robot in an office environment, and demonstrated the change of the end-effector velocity. The sampling frequency of the control loop was set at 50 Hz.

Figure 9 and Figure 10 show the results of the two tasks. In Figure 9, the two arms of the dual-arm robot had a common target, while in Figure 10, each arm had a different target.

Figure 11a shows the snapshots of the experiment. The velocity profile and the trajectory of the end-effect of the arms are shown in Figure 11b,c, respectively. As the two arms had the same target, both arms changed trajectory to avoid self-collision. In fact, both arms moved away from the target.

In the second experiment, the robotic arms had different targets, as illustrated in Figure 12a. The velocity profile of the end-effector and the trajectories of the two arms are shown in Figure 12b,c, respectively.

As the two arms had different targets, each arm changed its motion to find a collision-free trajectory. As illustrated in Figure 12, the repulsive velocity produced the end-effect on the left arm and acted on link 6 of the right arm. In this way, the repulsive velocity was projected onto the end-effect of the left arm. In other words, both arms in the dual-arm system changed their motion to avoid self-collision: the right arm changed its motion to approach the target, whereas the left arm changed its motion by moving away from the target and the right arm.

Meanwhile, as illustrated in Figure 11b, Figure 12b, respectively, although there were fluctuations in the end-effector velocity, the fluctuation did not affect the results of the experiment.

These experiments were similar to the simulations, which are described in Section 4.1, and demonstrated the effectiveness of our algorithm in simulation A (robotic arms with the same target) and simulation B (robotic arms with different targets). Meanwhile, these demo of the experiments were upload on the web as illustrate in Supplementary Materials).

5. Conclusions

This paper proposed an algorithm based on the kinematic model of a robot to enable self-collision avoidance in real time. The collision model was based on a finite number of discrete spherical bounding volumes with different radii. In addition, the algorithm proposed a sensitivity index for measuring the collisions between spheres. When the bounding spheres collide, two repulsive velocities are generated on the control points, and the motion of the arms are changed accordingly to avoid collisions between the two arms. Simulations and experiments were conducted on a dual-arm robot to verify the effectiveness of the algorithm. The results indicate that the algorithm can effectively avoid self-collision between arms using a simple collision model. The method optimizes the collision model, decreases the computational cost, and increases the reaction speed as well as accuracy. This algorithm can be used in tele-operation and fully autonomous control systems.

In the future, we aim to devise a strategy/framework that can avoid self-collision and accomplish the desired tasks in concert, similar to human motion. Meanwhile, we aim to develop this strategy to avoid obstacles when external environment changes are detected to deal with more complicated tasks. In addition, a strategy for self-collision avoidance of multi-arm (>2) robots is also a direction worth investigating.