Obstacle Avoidance Algorithm for 7-DOF Redundant Anthropomorphic Arm

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.Themotion of that critical pointsmove away from the closest points on the obstacles is defined as obstacle avoidingmotion. Two transitioning variables were used tomake 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.


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][2][3][4][5][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.

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 Figure 1.
The coordinate frames of the redundant anthropomorphic arm are shown in Figure 2. The corresponding Denavit-Hartenberg (D-H) parameters are reported in Table 1.

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, the motion of the endeffector 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].

The Problem of Obstacle Avoidance Corresponds to the
Second Kind of Obstacle.For the second kind of problem of obstacle avoidance, as shown in Figure 4, 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.

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.

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 d 0 be the vector connecting the closest point on the obstacle and the critical point.The unit vector in the direction of d 0 is n 0 .Consider The Jacobian matrix associated with the critical point can be calculated as where J 0 is the Jacobian defined in the Cartesian space.Clearly, J d 0 ∈  1× .

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 velocity q in the proposed algorithm is defined as where the obstacle avoidance velocity vector ẋ d 0 and the matrix N  0 are given as where q is a -dimensional vector representing the joint velocity, the end-effector velocity is represented by the dimensional vector ẋ , J is the  ×  Jacobian matrix, J + is the generalized inverse of the Jacobian matrix J, V 0 is scalar representing the nominal avoiding velocity, and  1 and  2 are the transitioning variables defined as where ‖d 0 ‖ is the distance between the closest point on the obstacle and the critical point and   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.If ‖d 0 ‖ = ∞,  1 =  2 ≈ 0. Now, (3) can be rewritten in the form 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 ‖d 0 ‖ ≈   ,  1 =  2 ≈ 1. Equation ( 3) can be rewritten in the form However, the tracking accuracy is decreased due to the obstacle avoiding motion.If ‖d 0 ‖ ≈ 0,  1 ≈ 2 and  2 ≈ 1.
Then, (3) can be rewritten in the form 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.

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 sixdimensional vector, (, , , , , ).(, , ) 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 as  1 = 0.45 m,  2 = 0.327 m,  3 = 0.322 m, and  4 = 0.3 m.

Example 1.
Here, the initial angle of the robot is  10 and the initial configuration is  10 .The robot will move to the final configuration  1 in a straight trajectory and the time of the simulation is 5 sec: 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.
The simulation results of robot using the proposed approach are shown in Figure 6. 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.
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.Figure 8 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.

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  20 .The robot will move to the final configuration  2 in a straight trajectory and the time of the simulation is 5 sec: Ports of the open container appear in the desired trajectory, as shown in Figure 13.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 Figure 14.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 endeffector (Figure 12).Simulation results show that the actual trajectory is also smooth and continuous.
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.

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.

A. Forward Kinematics Analysis of the Robot
The homogeneous transformation matrix and the D-H parameters are used to analyze the kinematic problem of the robot.So, we can obtain the general form of transformation matrix −1  :  where,   = sin(  ) and   = cos(  ).
The transformation matrix from the coordinate frame of the end-effector to the 0th coordinate frame or the base coordinate frame can be obtained as

B. The Jacobian of the Robot
The Jacobian of the robot is as follows:

Figure 1 :
Figure 1: CAD model of the anthropomorphic arm.

Figure 2 :
Figure 2: D-H coordinate frames of the anthropomorphic arm.

Figure 3 :Figure 4 :
Figure 3: The obstacles do not disturb the motion of the endeffector.

Figure 9 :Figure 10 :
Figure 9: Closest distance using the traditional approach.

Figure 11 :Figure 12 :
Figure 11: The changes of the position of the end-effector.

Figure 17 :Figure 18 :
Figure 17: The changes of the position of the end-effector.

Table 1 :
D-H coordinate design parameters of the anthropomorphic arm.