# An Obstacle Avoidance Method for Action Support 7-DOF Manipulators Using Impedance Control

Authors: Masafumi Hamaguchi; Takao Taniguchi
Journal: Journal of Robotics
 (2013)

---

## Abstract

An obstacle avoidance method of action support 7-DOF manipulators is proposed in this paper. The manipulators are controlled with impedance control to follow user's motions. 7-DOF manipulators are able to avoid obstacles without changing the orbit of the end-effector because they have kinematic redundancy. A joint rate vector is used to change angular velocity of an arbitrary joint with kinematic redundancy. The priority of avoidance is introduced into the proposed method, so that avoidance motions precede follow motions when obstacles are close to the manipulators. The usefulness of the proposed method is demonstrated through obstacle avoidance simulations and experiments.


---

## Body

## 1. Introduction

The application of robot technology extends from the manufacturing industry to our homes. In particular, applied research on robot technology is widely carried out in the field of medical treatment and welfare, such as operation support robots and meal support robots. The authors focus on an action support manipulator which supports humans with poor muscle strength. One application of this manipulator is a meal support manipulator. The manipulator grasps a spoon, and a user attaches the hand to the spoon. The spoon moves to the desired direction according to the minute force applied by the user. Obstacle avoidance is necessary to use the manipulator safely under the environment where humans and other objects exist.A 7-DOF manipulator is used as an action support in this paper. 7-DOF manipulators have kinematic redundancy. 7-DOF manipulators are able to avoid obstacles without changing the position and the attitude of the end-effector by using their redundant degree of freedom. Various methods on obstacle avoidance for redundant manipulators are proposed in [1–6]. A joint rate vector [1, 2] is adopted to avoid obstacles in this paper.Impedance control [7–10] is used to make the manipulator follow the user’s motion in this paper. Though the methods of impedance control for redundant manipulators are proposed in [7, 8], the obstacle avoidance is not handled. The methods of impedance control for redundant manipulators considering the obstacle avoidance are presented in [9, 10]. Though the manipulator avoids the obstacle while the end-effector follows the fixed reference path in [9, 10], cooperative works of manipulators and humans are not considered.An obstacle avoidance method for redundant manipulators using impedance control is proposed in this paper. Priority of avoidance is introduced into the proposed method, so that avoidance motions precede follow motions when an obstacle is close to the manipulator. There are few papers on obstacle avoidance by using the joint rate vector for redundant manipulators which follows the human’s actions with impedance control. The validity of the proposed method is demonstrated through obstacle avoidance simulations and experiments.


## 2. Experimental Equipment

Figure1 shows a manipulator used in this paper. This manipulator is the PA-10A-ARM made by Mitsubishi Heavy Industries, Ltd. The manipulator has seven degrees of freedom and the transportable weight is 98 N. The joints are driven by AC servo motors with brushless resolvers. The manipulator moves to support a user (see Figure 2).Figure 1
7-DOF manipulator.Figure 2
Image of action support manipulator.A force sensor is installed in the end-effector to measure forces and moments applied to the end-effector. The force sensor is the IFS-67M25A50-I40 made by Nitta Corp. The measurable maximum forces are 200 N onx- and y-axis and 400 N on z-axis, and the measurable maximum moment is 13 Nm around x-, y- and z-axis.The distance between obstacles and the manipulator may be measured with a distance sensor such as a PSD sensor, a laser sensor, an ultrasonic sensor, or a stereo camera. The distance is calculated under the situation that obstacles’ sizes and positions are known in this paper. This ideal situation is considered in experiment to evaluate the proposed method purely, because the experimental performance is too sensitive to the accuracy of the distance sensor.The seven joint angles of the manipulator and the output of the force sensor are transmitted to a personal computer. The command calculated in the computer is sent to the manipulator. The sampling period is 10 ms.


## 3. Obstacle Avoidance Method

The manipulator is controlled with impedance control to follow user’s motions. Kinematic redundancy is used to avoid obstacles. Priority of avoidance is introduced to combine the follow motions and the avoidance motions.


### 3.1. Impedance Control

The motion equation of the manipulator is expressed as(1)M(θ)θ¨+h(θ,θ˙)+g(θ)=τ+JT(θ)Fext,
where θ=[θ1,θ2,θ3,θ4,θ5,θ6,θ7]T is the joint angle vector; M(θ) is the inertia matrix (hereafter denoted by M); h(θ,θ˙) is the nonlinear term due to the centrifugal and Coriolis force; g(θ) is the gravitational term; τ is the joint torque vector; Fext is the external force exerted to the end-effector; and J(θ) is the Jacobian matrix (hereafter denoted by J).The desired impedance of the end-effector is described by(2)Mex¨+Bex˙+Ke(x-xd)=Fext,
where Me, Be, and Ke are the desired inertia, viscosity, and stiffness matrices of the end-effector, respectively; x is the displacement vector of the end-effector, and xd is the desired one. These are given as follows:
(3)Me=diag(me,me,me,Im,Im,Im),Be=diag(be,be,be,bn,bn,bn),Ke=diag(ke,ke,ke,kn,kn,kn),Fext=[fx,fy,fz,τϕ,τθ,τψ]T,x=[x,y,z,ϕ,θ,ψ]T.
The position and the attitude of the end-effector are denoted by (x,y,z) and (ϕ,θ,ψ), respectively. The moments τϕ, τθ, and τψ of Fext are set to zero to keep the initial attitude of the end-effector in this method. The values of the impedance parameters, me=1.0kg, Im=1.0 kgm2, be=20 Ns/m, bn=20 Nms/rad, ke=100 N/m, and kn=100 Nm/rad are used in this paper. These values should be optimized for users in the future work.PuttingFext in (2) as
(4)Fext=Mex¨d+Bex˙d,
the following equation is obtained:
(5)Me(x¨-x¨d)+Be(x˙-x˙d)+Ke(x-xd)=0.
The torque vector τ satisfying (5) is given as follows:
(6)τ=JT(JM-1JT)-1×[x¨d+Me-1{Be(x˙d-x˙)+Ke(xd-x)}-J˙q˙]-JTFext+h(θ,θ˙)+g(θ).
The desired displacement of the end-effector xd and its derivative x˙d is obtained from Fext by solving (4) with the 4-order Runge-Kutta method. The value of x¨d is calculated from x˙d by using the backward difference approximation. The end-effector follows the user’s hand by applying the torque τ in (6) to the manipulator. The desired displacement xd is generated in real time from the external force Fext. This point is greatly different from other studies on impedance control for manipulators.


### 3.2. Obstacle Avoidance Using Kinematic Redundancy

The inverse kinematics equation of redundant manipulators is expressed as(7)θ˙=J+x˙+P⊥(J)ξ,
where J+ is the pseudoinverse of J,P⊥(J) is the projection operator which projects arbitrary joint rates into the null-space of the end-effector’s Cartesian coordinates, and ξ is an arbitrary joint rate vector. J+ and P⊥(J) are calculated by the method in [11]. The displacement vector of the end-effector x is regulated by the impedance control to follow the external force Fext. When an obstacle approaches the manipulator, the manipulator avoids the obstacle by using the joint rate vector ξ.In this paper, two virtual spheres are set in the 4th and the 5th joints (see Figure3). The center of the virtual sphere is that of the joint. It is considered that these joints may collide against obstacles easily. The virtual sphere is shown in Figure 4. The inner sphere with the radius Lmin is an inelastic body and covers the joint of the manipulator. The purpose of the obstacle avoidance control is to avoid the collision between the inner sphere and obstacles. The outer sphere with the radius Lmax is an elastic body with a stiffness ka. When obstacles enter the outer sphere, a repulsive force is generated. The repulsive force is used to calculate the value of the joint rate vector ξ.Figure 3
Virtual spheres with radiusLmin in the 4th and 5th joints.Figure 4
Virtual sphere.A preliminary experiment was carried out to detect which element ofξ is effective for the obstacle avoidance. When a value was given to the 1st or 3rd element, the attitude of the manipulator greatly changed. The 1st and 3rd elements, ξ1 and ξ3, are therefore given values and the other elements are set to zero for the simplification of the problem. The values of ξ1 and ξ3 are calculated from
(8)ξ1,3={ka(Lmax-dmin)Lmax-Lmin:dmin≤Lmax,0:dmin>Lmax,
where ka is a positive constant, Lmin is the radius of the inelastic sphere, Lmax is the radius of the elastic sphere, and dmin is the distance between the nearest obstacle and the manipulator. In this paper, Lmin=0.15 m, Lmax=0.25m, and ka=0.3 rad/s. The value of ξ1 or ξ3 is shown in Figure 5. As the obstacle approaches the manipulator, the value of ξ1 or ξ3 increases and then the attitude of the manipulator changes to avoid the obstacle.Figure 5
1st/3rd element ofξ, ξ1,3.


### 3.3. Priority of Avoidance

Since the avoidance motions may conflict with the follow motions, the priority of avoidance is introduced. The priority of avoidance means the weight between the follow motions generated by (6) and the avoidance motions caused by (7) and (8). The value of the priority of avoidance is greater when an obstacle is close to the manipulator. The priority of avoidance α is defined as
(9)α={exp(-kbdmin-LminLmax-Lmin):dmin≥Lmin,1:dmin<Lmin,
where kb is a positive constant, kb=5.0 in this paper. The value of the priority α is shown in Figure 6. According to the priority of avoidance α, Fext in (4) is calculated as
(10)Fext=(1-α)Fact,
where Fact is the actual applied force measured with the force sensor. Fext=0 at α=1 (dmin < Lmin) means that only the avoidance motions are carried out without the follow motions, and Fext=Fact at α=0 (dmin≥Lmax) means that only the follow motions are carried out without the avoidance motions. When the manipulator is close to the obstacle, the avoidance motions should precede the follow motions for the safety. Otherwise, the manipulator should follow the user’s motion as much as possible. Therefore, an exponential function is used in (9), so that the value of α rapidly increases as the value of dmin approaches Lmin.Figure 6
Priority of avoidance,α.


## 3.1. Impedance Control

The motion equation of the manipulator is expressed as(1)M(θ)θ¨+h(θ,θ˙)+g(θ)=τ+JT(θ)Fext,
where θ=[θ1,θ2,θ3,θ4,θ5,θ6,θ7]T is the joint angle vector; M(θ) is the inertia matrix (hereafter denoted by M); h(θ,θ˙) is the nonlinear term due to the centrifugal and Coriolis force; g(θ) is the gravitational term; τ is the joint torque vector; Fext is the external force exerted to the end-effector; and J(θ) is the Jacobian matrix (hereafter denoted by J).The desired impedance of the end-effector is described by(2)Mex¨+Bex˙+Ke(x-xd)=Fext,
where Me, Be, and Ke are the desired inertia, viscosity, and stiffness matrices of the end-effector, respectively; x is the displacement vector of the end-effector, and xd is the desired one. These are given as follows:
(3)Me=diag(me,me,me,Im,Im,Im),Be=diag(be,be,be,bn,bn,bn),Ke=diag(ke,ke,ke,kn,kn,kn),Fext=[fx,fy,fz,τϕ,τθ,τψ]T,x=[x,y,z,ϕ,θ,ψ]T.
The position and the attitude of the end-effector are denoted by (x,y,z) and (ϕ,θ,ψ), respectively. The moments τϕ, τθ, and τψ of Fext are set to zero to keep the initial attitude of the end-effector in this method. The values of the impedance parameters, me=1.0kg, Im=1.0 kgm2, be=20 Ns/m, bn=20 Nms/rad, ke=100 N/m, and kn=100 Nm/rad are used in this paper. These values should be optimized for users in the future work.PuttingFext in (2) as
(4)Fext=Mex¨d+Bex˙d,
the following equation is obtained:
(5)Me(x¨-x¨d)+Be(x˙-x˙d)+Ke(x-xd)=0.
The torque vector τ satisfying (5) is given as follows:
(6)τ=JT(JM-1JT)-1×[x¨d+Me-1{Be(x˙d-x˙)+Ke(xd-x)}-J˙q˙]-JTFext+h(θ,θ˙)+g(θ).
The desired displacement of the end-effector xd and its derivative x˙d is obtained from Fext by solving (4) with the 4-order Runge-Kutta method. The value of x¨d is calculated from x˙d by using the backward difference approximation. The end-effector follows the user’s hand by applying the torque τ in (6) to the manipulator. The desired displacement xd is generated in real time from the external force Fext. This point is greatly different from other studies on impedance control for manipulators.


## 3.2. Obstacle Avoidance Using Kinematic Redundancy

The inverse kinematics equation of redundant manipulators is expressed as(7)θ˙=J+x˙+P⊥(J)ξ,
where J+ is the pseudoinverse of J,P⊥(J) is the projection operator which projects arbitrary joint rates into the null-space of the end-effector’s Cartesian coordinates, and ξ is an arbitrary joint rate vector. J+ and P⊥(J) are calculated by the method in [11]. The displacement vector of the end-effector x is regulated by the impedance control to follow the external force Fext. When an obstacle approaches the manipulator, the manipulator avoids the obstacle by using the joint rate vector ξ.In this paper, two virtual spheres are set in the 4th and the 5th joints (see Figure3). The center of the virtual sphere is that of the joint. It is considered that these joints may collide against obstacles easily. The virtual sphere is shown in Figure 4. The inner sphere with the radius Lmin is an inelastic body and covers the joint of the manipulator. The purpose of the obstacle avoidance control is to avoid the collision between the inner sphere and obstacles. The outer sphere with the radius Lmax is an elastic body with a stiffness ka. When obstacles enter the outer sphere, a repulsive force is generated. The repulsive force is used to calculate the value of the joint rate vector ξ.Figure 3
Virtual spheres with radiusLmin in the 4th and 5th joints.Figure 4
Virtual sphere.A preliminary experiment was carried out to detect which element ofξ is effective for the obstacle avoidance. When a value was given to the 1st or 3rd element, the attitude of the manipulator greatly changed. The 1st and 3rd elements, ξ1 and ξ3, are therefore given values and the other elements are set to zero for the simplification of the problem. The values of ξ1 and ξ3 are calculated from
(8)ξ1,3={ka(Lmax-dmin)Lmax-Lmin:dmin≤Lmax,0:dmin>Lmax,
where ka is a positive constant, Lmin is the radius of the inelastic sphere, Lmax is the radius of the elastic sphere, and dmin is the distance between the nearest obstacle and the manipulator. In this paper, Lmin=0.15 m, Lmax=0.25m, and ka=0.3 rad/s. The value of ξ1 or ξ3 is shown in Figure 5. As the obstacle approaches the manipulator, the value of ξ1 or ξ3 increases and then the attitude of the manipulator changes to avoid the obstacle.Figure 5
1st/3rd element ofξ, ξ1,3.


## 3.3. Priority of Avoidance

Since the avoidance motions may conflict with the follow motions, the priority of avoidance is introduced. The priority of avoidance means the weight between the follow motions generated by (6) and the avoidance motions caused by (7) and (8). The value of the priority of avoidance is greater when an obstacle is close to the manipulator. The priority of avoidance α is defined as
(9)α={exp(-kbdmin-LminLmax-Lmin):dmin≥Lmin,1:dmin<Lmin,
where kb is a positive constant, kb=5.0 in this paper. The value of the priority α is shown in Figure 6. According to the priority of avoidance α, Fext in (4) is calculated as
(10)Fext=(1-α)Fact,
where Fact is the actual applied force measured with the force sensor. Fext=0 at α=1 (dmin < Lmin) means that only the avoidance motions are carried out without the follow motions, and Fext=Fact at α=0 (dmin≥Lmax) means that only the follow motions are carried out without the avoidance motions. When the manipulator is close to the obstacle, the avoidance motions should precede the follow motions for the safety. Otherwise, the manipulator should follow the user’s motion as much as possible. Therefore, an exponential function is used in (9), so that the value of α rapidly increases as the value of dmin approaches Lmin.Figure 6
Priority of avoidance,α.


## 4. Simulation Model of Manipulator

A simulation model of the manipulator is constructed. The link parameters of the manipulator are shown in Figure7, and the values of the link parameters and the moment of inertia of the links are shown in Tables 1 and 2, respectively. These values are calculated by measuring the manipulator.Table 1
Values of link parameters.
l
b (m)
l
s (m)
l
e (m)
l
w (m)
l
h (m)
0.315
0.450
0.500
0.080
0.121
m
1 (kg)
m
2 (kg)
m
3 (kg)
m
4 (kg)
m
5 (kg)
m
6 (kg)
m
7 (kg)
m
8 (kg)
9.78
8.41
3.51
4.31
3.45
1.46
0.24
0.98
k
1 (m)
k
2 (m)
k
3 (m)
k
4 (m)
k
5 (m)
k
6 (m)
k
7 (m)
k
8 (m)
0.147
0.063
0.089
0.046
0.165
0.030
0.085
0.051Table 2
Values of moment of inertia.
Link
I
x (kgm2)
I
y (kgm2)
I
z (kgm2)
1
0.1528
0.1322
0.0646
2
0.0900
0.0266
0.0794
3
0.0462
0.0337
0.0183
4
0.0307
0.0081
0.0272
5
0.0392
0.0375
0.0081
6
0.0039
0.0008
0.0039
7
0.0001
0.0001
0.0002
8
0.0011
0.0013
0.0008Figure 7
Link parameters of 7-DOF manipulator.The manipulator with the initial joint angle vectorθ(0)=[0,π/6,0,π/3,0,0,0]T rad operates by giving some adequate reference path of the end-effector. The results are shown in Figures 8 and 9. These figures show the time history of the position of the end-effector, and the fine line and the bold one denote the simulation result and the experimental one, respectively. The simulation results coincide with the experimental ones. Though the details are omitted, the similar results are also obtained in other experimental situations. Therefore, this model is used in simulation.Figure 8
Verification of simulation model (1).Figure 9
Verification of simulation model (2).


## 5. Simulation Results of Obstacle Avoidance

The usefulness of the proposed method is demonstrated in simulation. Figure10 shows the initial state of the obstacle avoidance simulation. The pink sphere denotes the inner virtual sphere with the radius Lmin. The initial joint angle vector θ(0)=[0,π/4,0,π/2,0-π/4,0]T rad and the actual applied force Fact is shown in Figure 11. The obstacle is a sphere of 0.05 m radius and its center is located at [0.4,0.3,0.78]T m.Figure 10
Obstacle avoidance in simulation.Figure 11
Actual applied force,Fact.


### 5.1. Obstacle Avoidance Using Kinematic Redundancy

The distance between the obstacle and the manipulatordmin is shown in Figure 12, where only ξ1 is calculated by (8) in Case  1 and both ξ1 and ξ3 are done in Case  2. The priority of avoidance α is not used in both cases, that is, Fext=Fact. The minimum value of dmin in Case  2 is greater than that in Case  1. This means that Case  2 using both ξ1 and ξ3 is better than Case  1 using only ξ1 from the viewpoint of obstacle avoidance. The manipulators in Cases  1 and 2 collide with the obstacle, since the distance dmin is less than Lmin. The scenes in Cases  1 and 2 are shown in Figures 13 and 14, respectively. The collision that the obstacle enters the inner sphere is confirmed from the last scene in Figures 13 and 14.Figure 12
Distancedmin in Cases  1, 2, and 3.Figure 13
Scenes in Case  1 of obstacle avoidance simulation.Figure 14
Scenes in Case  2 of obstacle avoidance simulation.


### 5.2. Obstacle Avoidance Using Kinematic Redundancy and Priority of Avoidance

The priority of obstacleα is used in Case  3, where both ξ1 and ξ3 are calculated by (8), since the result in Case  2 is better than that in Case  1. The distance dmin in Case  3 is also shown in Figure 12. The value of dmin is never less than Lmin. This means that there is no collision between the obstacle and the manipulator. The scenes in Case  3 are shown in Figure 15. No collision between the inner sphere and the obstacle is confirmed in Figure 15.Figure 15
Scenes in Case  3 of obstacle avoidance simulation.


## 5.1. Obstacle Avoidance Using Kinematic Redundancy

The distance between the obstacle and the manipulatordmin is shown in Figure 12, where only ξ1 is calculated by (8) in Case  1 and both ξ1 and ξ3 are done in Case  2. The priority of avoidance α is not used in both cases, that is, Fext=Fact. The minimum value of dmin in Case  2 is greater than that in Case  1. This means that Case  2 using both ξ1 and ξ3 is better than Case  1 using only ξ1 from the viewpoint of obstacle avoidance. The manipulators in Cases  1 and 2 collide with the obstacle, since the distance dmin is less than Lmin. The scenes in Cases  1 and 2 are shown in Figures 13 and 14, respectively. The collision that the obstacle enters the inner sphere is confirmed from the last scene in Figures 13 and 14.Figure 12
Distancedmin in Cases  1, 2, and 3.Figure 13
Scenes in Case  1 of obstacle avoidance simulation.Figure 14
Scenes in Case  2 of obstacle avoidance simulation.


## 5.2. Obstacle Avoidance Using Kinematic Redundancy and Priority of Avoidance

The priority of obstacleα is used in Case  3, where both ξ1 and ξ3 are calculated by (8), since the result in Case  2 is better than that in Case  1. The distance dmin in Case  3 is also shown in Figure 12. The value of dmin is never less than Lmin. This means that there is no collision between the obstacle and the manipulator. The scenes in Case  3 are shown in Figure 15. No collision between the inner sphere and the obstacle is confirmed in Figure 15.Figure 15
Scenes in Case  3 of obstacle avoidance simulation.


## 6. Experimental Results of Obstacle Avoidance

The validity of the proposed method is verified in the experiment. Figure16 shows the initial state of obstacle avoidance experiment. The initial joint angle vector θ(0) is the same as that in simulation. The obstacle is a square pole with 0.1 × 0.1 × 0.35 m and its center is located at the same point in simulation. An experimenter who is a healthy person applies the force to the end-effector in y-axis direction.Figure 16
Obstacle avoidance in experiment.The applied force measured with the force sensor is plotted in Figure17. The experimenter applies the vibrational force to the positive direction of y-axis, the right direction in Figure 16. The vibration may be reduced by adjusting the impedance parameters Me, Be, and Ke in (3). The position of the 4th joint of the manipulator on y-axis and the distance dmin are plotted in Figures 18 and 19, respectively. The 4th joint moves to the positive direction of y-axis according to the applied force until about 4 s which causes the value of dmin to be small. When the value of dmin is small or the 4th joint is close to the obstacle, the avoidance motion predominates. The 4th joint moves to the negative direction of y-axis from about 4 s to 6 s in Figure 18. This motion causes the increase of the value of dmin in Figure 19. There is no collision between the manipulator and the obstacle, since dmin>Lmin during the experiment in Figure 19. The scenes of the experiment are shown in Figure 20. The follow motion and the avoidance motion are demonstrated.Figure 17
Actual applied force,fy.Figure 18
Position of 4th joint ony-axis.Figure 19
Distance,dmin.Figure 20
Scene in obstacle avoidance experiment.


## 7. Conclusions

In this paper, an obstacle avoidance method of action support 7-DOF manipulators has been realized by using impedance control and kinematic redundancy of the manipulator. A joint rate vector has been used to avoid obstacles in the way of changing the posture of the manipulators. The joint rate vector has been calculated from the distance between obstacles and the manipulator. The concept of the priority of avoidance has been introduced, so that avoidance motions precede follow motions when obstacles are close to the manipulator. The usefulness of the proposed method has been demonstrated through obstacle avoidance simulations and experiments.



---

*Source: 842717-2013-11-20.xml*