# Obstacle Avoidance Algorithm for 7-DOF Redundant Anthropomorphic Arm

Authors: Haoyu Shen; Hongtao Wu; Bai Chen; Yanjie Jiang; Cheng Yan
Journal: Journal of Control Science and Engineering
 (2015)

---

## Abstract

To deal with the problem of obstacle avoidance for redundant robots, an obstacle avoidance algorithm based on the internal motion of the 7-DOF redundant anthropomorphic arm is presented. The motion of that critical points move away from the closest points on the obstacles is defined as obstacle avoiding motion. Two transitioning variables were used to make a smooth, continuous transition between the primary and the secondary tasks. Using this approach, the robot can get the target configuration while avoiding the obstacles. Finally, the validity of the obstacle avoidance algorithm based on transitioning between tasks is manifested by simulation. The results show that, for the obstacle avoiding problem, the redundant robot not only can realize the obstacle avoidance, but also prevents the conflict between tasks by the proposed approach.


---

## Body

## 1. Introduction

Robots are now on their way of diverging into various fields. In particular, in the field of services, the research has gained much interest. Differing from traditional industrial robots, the service robots in human environments like handling tools, objects, and equipment are still challenging issue, because human environments are complex, dynamic, uncontrolled, and difficult to be perceived reliably. One key requirement of service robots is the avoidance of collisions between robot and obstacles. A redundant robot has the ability to move the end-effector along the same trajectory using different configurations of the mechanical structure. This provides means for solving sophisticated motion tasks such as avoiding obstacles, avoiding singularities, optimizing manipulability, and minimizing joint torques. It is well recognized that collision avoidance for redundant robots is a significant issue in current robotics research, which has gained extensive attention recently [1–6].In general, the obstacle avoidance approaches can be divided into two categories: off-line and online methods. Off-line path-planning methods are very computationally demanding and the calculation times are significantly longer than the typical response time of a robot. This computational complexity limits their use for practical obstacle avoidance to just simple cases. Furthermore, off-line path-planning methods are restricted to structured environments and static obstacles. Online methods, on the other hand, are less computationally demanding, can handle unstructured environments as well as moving obstacles, and can be used for sensor-based low-level motion planning [7, 8]. These distinguishing characteristics make off-line path-planning methods more excellent than online collision avoidance, especially in unstructured environments. For the problem of the collision avoidance of redundant robots, the local methods proposed have been the focus of research [9, 10]. Maciejewski and Klein suggested to assign to the critical point a velocity with which the point is moved away from the closest obstacle [11]. Colbaugh et al. used gesture control to address the avoidance problem and defined a constraint function for avoiding obstacles [12]. Subsequently, McLean and Cameron raised the issue of using a potential function to resolve the obstacle avoidance problem [13]. Guo and Hsia defined the objective function of the distance between the obstacle and robot to deal with the obstacle avoidance problem [14]. Chyan used particle swarm optimization method to solve the obstacle avoidance problem of redundant robots.Using these traditional approaches, the obstacle avoidance problem of redundant robots is defined as how to control the robot to track the desired end-effector trajectory while simultaneously avoiding the obstacles [1]. In the unstructured environments, the obstacle position is not known in advance. The motion of the end-effector of robots can be disturbed by any obstacles, which may create a conflict between the obstacle avoiding motion and the trajectory tracking motion, and the traditional approaches will yield the erroneous results. In view of this problem, we propose a novel obstacle avoidance algorithm for 7-DOF redundant anthropomorphic arms. The proposed algorithm allows a smooth, continuous priority transition between the trajectory tracking motion of the end-effector and the obstacle avoiding motion using the real-time minimum distance. Finally, the validity of the obstacle avoidance algorithm is manifested by simulations.


## 2. The Structure of 7-DOF Redundant Anthropomorphic Arm

By studying the human skeleton and behavior, the redundant anthropomorphic arm has 7 degrees of freedom such the shoulder has three degrees of freedom, the wrist has three degrees of freedom, and the elbow has one degree of freedom, as shown in Figure1.Figure 1
CAD model of the anthropomorphic arm.The coordinate frames of the redundant anthropomorphic arm are shown in Figure2. The corresponding Denavit-Hartenberg (D-H) parameters are reported in Table 1.Table 1
D-H coordinate design parameters of the anthropomorphic arm.
i
α
i
-
1/(rad)
a
i
-
1/(m)
d
i/(m)
θ
i
1
0
0
L
1
θ
1
2
π
/
2
0
0
θ
2
3
-
π
/
2
0
L
2
θ
3
4
π
/
2
0
0
θ
4
5
-
π
/
2
0
L
3
θ
5
6
π
/
2
0
0
θ
6
7
-
π
/
2
0
L
4
θ
7Figure 2
D-H coordinate frames of the anthropomorphic arm.


## 3. The Problem of Obstacle Avoidance for Redundant Robots

According to the position relation between the obstacle and robot, the problem of obstacle avoidance for redundant robots can be divided into the following two categories.


### 3.1. The Problem of Obstacle Avoidance Corresponds to the First Kind of Obstacle

For the first kind of problem of obstacle avoidance, as shown in Figure3, the motion of the end-effector is not disturbed by any obstacle. In other words, the trajectory tracking motion of the end-effector of the robot is independent of the obstacle avoiding motion. Therefore, the redundant robot can realize the obstacle avoidance without affecting the trajectory tracking task of the end-effector. This problem can be solved by traditional obstacle avoidance algorithms. The obstacle avoidance strategy is to ensure all the links of the robot are away from obstacles, while the robot can track the desired trajectory [1].Figure 3
The obstacles do not disturb the motion of the end-effector.


### 3.2. The Problem of Obstacle Avoidance Corresponds to the Second Kind of Obstacle

For the second kind of problem of obstacle avoidance, as shown in Figure4, obstacle appears in the desired trajectory. Conflict will take place between the trajectory tracking motion of the end-effector of the robot and the obstacle avoiding motion. Thus, the trajectory tracking motion and the obstacle avoiding motion cannot be completed simultaneously. The traditional obstacle avoidance algorithms will fail in this case.Figure 4
The condition of obstacles appears in the desired trajectory.


## 3.1. The Problem of Obstacle Avoidance Corresponds to the First Kind of Obstacle

For the first kind of problem of obstacle avoidance, as shown in Figure3, the motion of the end-effector is not disturbed by any obstacle. In other words, the trajectory tracking motion of the end-effector of the robot is independent of the obstacle avoiding motion. Therefore, the redundant robot can realize the obstacle avoidance without affecting the trajectory tracking task of the end-effector. This problem can be solved by traditional obstacle avoidance algorithms. The obstacle avoidance strategy is to ensure all the links of the robot are away from obstacles, while the robot can track the desired trajectory [1].Figure 3
The obstacles do not disturb the motion of the end-effector.


## 3.2. The Problem of Obstacle Avoidance Corresponds to the Second Kind of Obstacle

For the second kind of problem of obstacle avoidance, as shown in Figure4, obstacle appears in the desired trajectory. Conflict will take place between the trajectory tracking motion of the end-effector of the robot and the obstacle avoiding motion. Thus, the trajectory tracking motion and the obstacle avoiding motion cannot be completed simultaneously. The traditional obstacle avoidance algorithms will fail in this case.Figure 4
The condition of obstacles appears in the desired trajectory.


## 4. Obstacle Avoidance Algorithm for Redundant Robots Based on Transitioning between the Primary and the Secondary Tasks

For the two kinds of obstacle avoidance problem, we propose a universal algorithm for redundant robots based on transitioning between the primary and the secondary tasks. The robot can realize the obstacle avoidance while the end-effector of the robot can get the target configuration accurately at the same time by the proposed approach.


### 4.1. The Operational Space of Obstacle Avoiding Motion

The points on the robot that are near obstacles will be identified as critical points. The obstacle avoiding motion is realized by assigning to the critical points a motion in the direction of the line connecting the critical point with the closest point on the obstacle. This is a one-dimensional constraint, so the operational space of obstacle avoiding motion can be defined as one-dimensional space [15]. Let d0 be the vector connecting the closest point on the obstacle and the critical point. The unit vector in the direction of d0 is n0. Consider
(1)n0=d0d0.The Jacobian matrix associated with the critical point can be calculated as(2)Jd0=n0TJ0,
where J0 is the Jacobian defined in the Cartesian space. Clearly, Jd0∈R1×n.


### 4.2. Obstacle Avoidance Algorithm Based on Transitioning between the Primary and the Secondary Tasks

For the obstacle avoidance problem of several kinds of obstacle existing simultaneously, the joint velocityq˙ in the proposed algorithm is defined as
(3)q˙=Jd0+x˙d0+N0'J+x˙,
where the obstacle avoidance velocity vector x˙d0 and the matrix N0′ are given as
(4)x˙d0=a1v0,N0′=I-a2Jd0+Jd0,
where q˙ is a n-dimensional vector representing the joint velocity, the end-effector velocity is represented by the m-dimensional vector x˙, J is the m×n Jacobian matrix, J+ is the generalized inverse of the Jacobian matrix J, v0 is scalar representing the nominal avoiding velocity, and a1 and a2 are the transitioning variables defined as
(5)a1=dmd0n,n=1,2,3,…d0≥dm2dmdm+d0d0<dm,a2=dmd0n,n=1,2,3,…d0≥dm1d0<dm,
where d0 is the distance between the closest point on the obstacle and the critical point and dm is the given threshold value.Two transitioning variables are used to make a smooth, continuous priority transition between the trajectory tracking motion of the end-effector of the robot and the obstacle avoiding motion. Ifd0=∞, a1=a2≈0. Now, (3) can be rewritten in the form
(6)q˙=J+x˙.
Thus, while no obstacle is near the robot end-effector can achieve very high tracking accuracy. When the robot is very close to an obstacle, the obstacle avoiding motion will transition into primary task. If d0≈dm, a1=a2≈1. Equation (3) can be rewritten in the form
(7)q˙=Jd0+v0+I-Jd0+Jd0J+x˙.
However, the tracking accuracy is decreased due to the obstacle avoiding motion. If d0≈0, a1≈2 and a2≈1. Then, (3) can be rewritten in the form
(8)q˙=Jd0+2v0+I-Jd0+Jd0J+x˙.In this case, the obstacle avoiding motion will remain as the primary task and the velocity of the obstacle avoiding motion will be much larger. Meanwhile, tracking error will increase further.


## 4.1. The Operational Space of Obstacle Avoiding Motion

The points on the robot that are near obstacles will be identified as critical points. The obstacle avoiding motion is realized by assigning to the critical points a motion in the direction of the line connecting the critical point with the closest point on the obstacle. This is a one-dimensional constraint, so the operational space of obstacle avoiding motion can be defined as one-dimensional space [15]. Let d0 be the vector connecting the closest point on the obstacle and the critical point. The unit vector in the direction of d0 is n0. Consider
(1)n0=d0d0.The Jacobian matrix associated with the critical point can be calculated as(2)Jd0=n0TJ0,
where J0 is the Jacobian defined in the Cartesian space. Clearly, Jd0∈R1×n.


## 4.2. Obstacle Avoidance Algorithm Based on Transitioning between the Primary and the Secondary Tasks

For the obstacle avoidance problem of several kinds of obstacle existing simultaneously, the joint velocityq˙ in the proposed algorithm is defined as
(3)q˙=Jd0+x˙d0+N0'J+x˙,
where the obstacle avoidance velocity vector x˙d0 and the matrix N0′ are given as
(4)x˙d0=a1v0,N0′=I-a2Jd0+Jd0,
where q˙ is a n-dimensional vector representing the joint velocity, the end-effector velocity is represented by the m-dimensional vector x˙, J is the m×n Jacobian matrix, J+ is the generalized inverse of the Jacobian matrix J, v0 is scalar representing the nominal avoiding velocity, and a1 and a2 are the transitioning variables defined as
(5)a1=dmd0n,n=1,2,3,…d0≥dm2dmdm+d0d0<dm,a2=dmd0n,n=1,2,3,…d0≥dm1d0<dm,
where d0 is the distance between the closest point on the obstacle and the critical point and dm is the given threshold value.Two transitioning variables are used to make a smooth, continuous priority transition between the trajectory tracking motion of the end-effector of the robot and the obstacle avoiding motion. Ifd0=∞, a1=a2≈0. Now, (3) can be rewritten in the form
(6)q˙=J+x˙.
Thus, while no obstacle is near the robot end-effector can achieve very high tracking accuracy. When the robot is very close to an obstacle, the obstacle avoiding motion will transition into primary task. If d0≈dm, a1=a2≈1. Equation (3) can be rewritten in the form
(7)q˙=Jd0+v0+I-Jd0+Jd0J+x˙.
However, the tracking accuracy is decreased due to the obstacle avoiding motion. If d0≈0, a1≈2 and a2≈1. Then, (3) can be rewritten in the form
(8)q˙=Jd0+2v0+I-Jd0+Jd0J+x˙.In this case, the obstacle avoiding motion will remain as the primary task and the velocity of the obstacle avoiding motion will be much larger. Meanwhile, tracking error will increase further.


## 5. Simulation Experiments and Analysis

In this section, two examples are given to demonstrate the obstacle avoidance algorithm proposed in the prevision section.The configuration matrix of the end-effector of the robot in the base frame of the robot can be expressed by a six-dimensional vector,(α,β,γ,x,y,z). (x,y,z) is the Cartesian position vector of the end-effector with respect to the base frame of the robot (Figure 11), and (α,β,γ) is the Roll-Pitch-Yaw angels of the end-effector with respect to the base frame.The obstacle avoidance algorithm proposed in the prevision section is verified by mathematics-based simulations with two examples. The link length of the anthropomorphic arm is chosen asL1 = 0.45 m, L2 = 0.327 m, L3 = 0.322 m, and L4 = 0.3 m.


### 5.1. Example 1

Here, the initial angle of the robot isθ10 and the initial configuration is G10. The robot will move to the final configuration G1d in a straight trajectory and the time of the simulation is 5 sec:
(9)θ10=0,-π5,-π8,π3,π15,π9,0T,G10=-0.418,-0.707,0.155,-0.116,0.199,1.206T,G1d=-0.702,-0.832,1.209,-0.66,-0.193,1.086T.The form of the obstacles is spherical and the radius of the spheres is 0.05 m. The coordinate of the first obstacle center is assumed as(-0.4968,-0.0745,1.1223) with respect to the base coordinate frame. Thus, the obstacle appears in the desired trajectory, as shown in Figure 5. It belongs to the second kind of obstacle. The coordinate of the first obstacle center is assumed as (-0.2168,0.028,1.0644) with respect to the base coordinate frame. This obstacle does not disturb the motion of the end-effector, so it belongs to the first kind of obstacle. The simulation results of robot before obstacle avoidance path planning are shown in Figure 5. Consequently, the robot is not able to avoid any of the obstacles.Figure 5
Trajectory of robot before obstacle avoidance trajectory planning.The simulation results of robot using the proposed approach are shown in Figure6. Figure 7 shows the simulation results of the obstacle avoidance using the traditional approach [5, 11]. By observing the top plot in Figures 8 and 9, where the minimal distance between the robot and the center of the obstacles is shown, we can see that, for the traditional approach, the robot comes closer to the obstacle compared to the approach proposed.Figure 6
Trajectory for obstacle avoidance using the approach based on transitioning between tasks.Figure 7
Traces for obstacle avoidance using the traditional approach.Figure 8
Closest distance using the approach based on transitioning between tasks.Figure 9
Closest distance using the traditional approach.The radius of the spheres is 0.05 m, so the minimal distance between the closest point on the obstacle and the robot is about 0.01 m using the approach proposed. The robot can avoid the obstacles and get the target configuration perfectly. Obviously, the traditional approach is not able to avoid the obstacles.Figure8 shows all the joint angles. Figures 9 and 10 show the simulation results of the changes of position and attitude coordinates of the end-effector using the approach proposed. Simulation results show that the actual trajectory is smooth and continuous. We can see that the task for obstacle avoidance smoothly takes over when the robot is very close to an obstacle. However, the tracking error is significant when the robot is avoiding the obstacle. Of course, when the robot is far away from obstacles, the robot end-effector can achieve very high tracking accuracy.Figure 10
Angles of all joints.Figure 11
The changes of the position of the end-effector.


### 5.2. Example 2

The task is the same as in Example 1, but the obstacle is an open container. The initial angle of the robot isθ20 and the initial configuration is G20. The robot will move to the final configuration G2d in a straight trajectory and the time of the simulation is 5 sec:
(10)θ20=π9,-π2.3,-π8,-π1.2,π5,π1.2,π9T,G20=-0.877,0.936,-0.894,0.37,-0.007,0.427T,G2d=-0.877,0.936,-0.894,0.87,-0.007,0.427T.Ports of the open container appear in the desired trajectory, as shown in Figure13. So, it belongs to the second kind of obstacle. The simulation results of robot before obstacle avoidance path planning are shown in Figure 13. Consequently, the robot is not able to avoid the obstacle.The simulation results of robot using the proposed approach are shown in Figure14. The robot can avoid the obstacle and get the target configuration perfectly. The plot in Figure 15 shows the minimal distance to the obstacle. We can see that the minimal distance between the closest point on the obstacle and the robot is about 0.02 m. So, the robot can avoid the container. Figure 16 shows all the joint angles. Figures 17 and 18 show the simulation results of the changes of position and attitude coordinates of the end-effector (Figure 12). Simulation results show that the actual trajectory is also smooth and continuous.Figure 12
The changes of the attitude of the end-effector.Figure 13
Trajectory of robot before obstacle avoidance trajectory planning.Figure 14
Trajectory for obstacle avoidance using the approach based on transitioning between tasks.Figure 15
Closest distance.Figure 16
Angles of all joints.Figure 17
The changes of the position of the end-effector.Figure 18
The changes of the attitude of the end-effector.The above examples show that the obstacle avoidance algorithm can be efficiently used in a general range of tasks. However, we can see that the tracking error is significant when the robot is avoiding the obstacle. This means that the proposed approach can only be used if tracking accuracy of the end-effector motion is not very important.


## 5.1. Example 1

Here, the initial angle of the robot isθ10 and the initial configuration is G10. The robot will move to the final configuration G1d in a straight trajectory and the time of the simulation is 5 sec:
(9)θ10=0,-π5,-π8,π3,π15,π9,0T,G10=-0.418,-0.707,0.155,-0.116,0.199,1.206T,G1d=-0.702,-0.832,1.209,-0.66,-0.193,1.086T.The form of the obstacles is spherical and the radius of the spheres is 0.05 m. The coordinate of the first obstacle center is assumed as(-0.4968,-0.0745,1.1223) with respect to the base coordinate frame. Thus, the obstacle appears in the desired trajectory, as shown in Figure 5. It belongs to the second kind of obstacle. The coordinate of the first obstacle center is assumed as (-0.2168,0.028,1.0644) with respect to the base coordinate frame. This obstacle does not disturb the motion of the end-effector, so it belongs to the first kind of obstacle. The simulation results of robot before obstacle avoidance path planning are shown in Figure 5. Consequently, the robot is not able to avoid any of the obstacles.Figure 5
Trajectory of robot before obstacle avoidance trajectory planning.The simulation results of robot using the proposed approach are shown in Figure6. Figure 7 shows the simulation results of the obstacle avoidance using the traditional approach [5, 11]. By observing the top plot in Figures 8 and 9, where the minimal distance between the robot and the center of the obstacles is shown, we can see that, for the traditional approach, the robot comes closer to the obstacle compared to the approach proposed.Figure 6
Trajectory for obstacle avoidance using the approach based on transitioning between tasks.Figure 7
Traces for obstacle avoidance using the traditional approach.Figure 8
Closest distance using the approach based on transitioning between tasks.Figure 9
Closest distance using the traditional approach.The radius of the spheres is 0.05 m, so the minimal distance between the closest point on the obstacle and the robot is about 0.01 m using the approach proposed. The robot can avoid the obstacles and get the target configuration perfectly. Obviously, the traditional approach is not able to avoid the obstacles.Figure8 shows all the joint angles. Figures 9 and 10 show the simulation results of the changes of position and attitude coordinates of the end-effector using the approach proposed. Simulation results show that the actual trajectory is smooth and continuous. We can see that the task for obstacle avoidance smoothly takes over when the robot is very close to an obstacle. However, the tracking error is significant when the robot is avoiding the obstacle. Of course, when the robot is far away from obstacles, the robot end-effector can achieve very high tracking accuracy.Figure 10
Angles of all joints.Figure 11
The changes of the position of the end-effector.


## 5.2. Example 2

The task is the same as in Example 1, but the obstacle is an open container. The initial angle of the robot isθ20 and the initial configuration is G20. The robot will move to the final configuration G2d in a straight trajectory and the time of the simulation is 5 sec:
(10)θ20=π9,-π2.3,-π8,-π1.2,π5,π1.2,π9T,G20=-0.877,0.936,-0.894,0.37,-0.007,0.427T,G2d=-0.877,0.936,-0.894,0.87,-0.007,0.427T.Ports of the open container appear in the desired trajectory, as shown in Figure13. So, it belongs to the second kind of obstacle. The simulation results of robot before obstacle avoidance path planning are shown in Figure 13. Consequently, the robot is not able to avoid the obstacle.The simulation results of robot using the proposed approach are shown in Figure14. The robot can avoid the obstacle and get the target configuration perfectly. The plot in Figure 15 shows the minimal distance to the obstacle. We can see that the minimal distance between the closest point on the obstacle and the robot is about 0.02 m. So, the robot can avoid the container. Figure 16 shows all the joint angles. Figures 17 and 18 show the simulation results of the changes of position and attitude coordinates of the end-effector (Figure 12). Simulation results show that the actual trajectory is also smooth and continuous.Figure 12
The changes of the attitude of the end-effector.Figure 13
Trajectory of robot before obstacle avoidance trajectory planning.Figure 14
Trajectory for obstacle avoidance using the approach based on transitioning between tasks.Figure 15
Closest distance.Figure 16
Angles of all joints.Figure 17
The changes of the position of the end-effector.Figure 18
The changes of the attitude of the end-effector.The above examples show that the obstacle avoidance algorithm can be efficiently used in a general range of tasks. However, we can see that the tracking error is significant when the robot is avoiding the obstacle. This means that the proposed approach can only be used if tracking accuracy of the end-effector motion is not very important.


## 6. Conclusions

On the basis of the self-motion of the redundant robot, the obstacle avoidance algorithm for 7-Dof redundant anthropomorphic arm based on transitioning between the primary and the secondary tasks is presented. The approach proposed can realize a smooth, continuous priority transition between the trajectory tracking motion of the end-effector of the robot and the obstacle avoiding motion when necessary.The effectiveness of the proposed approach was shown by the simulation of the anthropomorphic arm. The proposed approach is very convenient to solve the obstacle avoidance problem that the motion of the end-effector of robots is disturbed by any obstacles. Thus, it overcomes some defects of the traditional approaches and has universality.



---

*Source: 540259-2015-01-28.xml*