Get documents about "Obstacle Avoidance of Manipulators with Rate Constraints
Shared by: sdfgsg234
-
Stats
- views:
- 2
- posted:
- 7/5/2011
- language:
- English
- pages:
- 6
Document Sample
Obstacle Avoidance of Manipulators with Rate Constraints
Toshiharu Sugie∗ , Yutaka Kito∗ and Kenji Fujimoto∗
∗
Department of Systems Science, Graduate School of Informatics
Kyoto University, Uji, Kyoto 611-0011, Japan
sugie@i.kyoto-u.ac.jp, fujimoto@i.kyoto-u.ac.jp
Abstract Recently, from the viewpoint of nonlinear control the-
ory, Fujimoto and co-workers pointed out that there
The paper presents a new control method which exists extra freedom in coordinate transformation for
achieves autonomous obstacle avoidance for manipula- exact linearization [3], and showed that it is possible
tors with rate constraints. More precisely, in order to to utilize the extra freedom for the real-time obstacle
achieve the autonomous obstacle avoidance, we exploit avoidance of manipulators [2]. Though it is a quite dif-
the freedom of the coordinate transformation for exact ferent approach from the above robotics literature, its
linearization of nonlinear systems. At the same time, effectiveness has been evaluated by experiment. They,
we cope with the rate constraints by adopting the state- however, do not consider the rate constraint of robot
dependent time scale transformation. Furthermore, we manipulators. Therefore, it seems to be interesting to
apply this method to an actual 2-link robot manipula- develop the method so as to cope with the rate con-
tor, and evaluate its effectiveness by experiment, which straints.
is the most important part of the paper.
The purpose of this paper is to show how to achieve
autonomous obstacle avoidance of manipulators with
rate constraints based on Fujimoto’s method. More
1 Introduction precisely, we achieve the autonomous obstacle avoid-
ance by exploiting the freedom of coordinate trans-
Concerning to the motion planning or master-slave con- formation in exact linearization, and cope with the
trol of robot manipulators, if there exist obstacles in the rate constraints simultaneously by adopting the state-
work space, the manipulators has to avoid the obsta- dependent time scale transformation. Furthermore, we
cles while they are doing their jobs. In such a case, it apply this method to an actual 2-link robot manipula-
is desirable for the manipulators to avoid the obstacles tor, and evaluate its effectiveness by experiment.
autonomously, because it would reduce both the risk of
collision and the burden of planning or teaching.
2 Obstacle avoidance via coordinate
Lozano-Peres [9] proposed a method to generate the
transformation
trajectory which avoids the obstacle automatically,
and it has been followed by many other works (e.g.,
[10, 6]). However, unfortunately these are off-line meth- 2.1 Exact linearization
ods. Khatib [7] proposed an on-line obstacle avoid- We consider a rigid link manipulator described by the
ance method using some potential functions, and there following dynamic equation
are many papers which discussed the real-time obsta- ¨ ˙
M (θ)θ + k(θ, θ) = τ (1)
cle avoidance problem based on the artificial potentials
(e.g., [8, 1]). On the other hand, another important where θ ∈ Rm denotes the joint variable, τ ∈ Rm is
factor to consider in motion planning is the rate con- the input torque or force, k denotes the nonlinear term
straints, because manipulators may become unstable such as centrifugal forces, and M is a positive definite
or cannot trace the planned trajectory accurately when inertia matrix.
they move so fast. Hollerbach [4] tried to cope with
this problem by using constant time-scaling. Tanaka (A) Conventional linearization
and co-workers [14] considered the convergence time in
motion planning, where they utilize the idea of state- For this system, we usually use the linearizing feedback:
dependent time scaling which is originally proposed by ˙
τ = k(θ, θ) + M (θ)uθ (2)
Sampei and Furuta [11], and developed for the con-
troller design with rate constraints [12, 13]. However, where uθ is a newly defined input. Then, we obtain a
there are few which address both the on-line property linearized system
and rate constraints in obstacle avoidance so far. ¨
θ = uθ . (3)
(B) Extra freedom in linearization the phase ϕ of θ from θ 0 by
Now, it is known that we have extra freedom in lin- ϕ = atan2(θ2 − θ02 , θ1 − θ01 ) (8)
earization. Namely, all pairs of linearizing coordinate r= (θ1 − θ01 )2 + (θ2 − θ02 )2 (9)
transformations and feedbacks for the system (1) are
given by the following pairs with eq.(2) (see e.g. [3]). as depicted in the Figure 1, where the domain S1 repre-
sent the obstacle, and S2 corresponds to the domain in
x = ψ(θ) (4) which we deform the coordinate for the above purpose
uθ = [J(θ)] −1 ˙
(ux − h(θ, θ)) (5) (b). For notational convenience, we define the mapping
ψ 0 by
where ψ : θ → x is any diffeomorphism, J := ∂ψ/∂θ ϕ θ1
= ψ0 (10)
is the Jacobian matrix of ψ, and h is defined by r θ2
˙T ˙ ˙T ˙
h := ( θ H 1 θ, · · · , θ H m θ )T (6)
where H i := ∂/∂θ(∂ψi /∂θ)T is a Hessian matrix of ψi .
In this case, we have
¨
x = ux (7)
Then we can design a control system which au-
tonomously avoid the obstacles by using the extra free-
dom ψ. We only consider the case m = 2 in the follow-
ing for simplicity, but it can be essentially generalized
to the case m > 2. Figure 1: New coordinate
2.2 Design of coordinate transformation In order to obtain a ψ satisfying the two objectives, we
In this section, we describe how to design a control sys- proceed the following steps:
tem for autonomous obstacle avoidance. We will con-
struct such a system by choosing the coordinate trans- Step1. Define the boundary ∂S1 of the obstacle region
formation ψ in an appropriate way. The objectives of S1 as:
this design are:
cos ϕ
∂S1 = µ(ϕ) + θ 0 0 ≤ ϕ < 2π (11)
(a) The manipulator tracks the reference trajectory sin ϕ
precisely when it is far away from the obstacles. where µ > 0 is a smooth periodic function with period
2π.
(b) The manipulator autonomously avoids the collision
with the obstacles when the reference trajectory ap- Step2. Define the boundary ∂S2 of the deforming do-
proaches to the obstacles. main S2 which contains S1 as:
For the objective (a), we choose ψ as identity when cos ϕ
the manipulator is away from the obstacles. When ψ ∂S2 = k(ϕ)µ(ϕ) +θ0 0 ≤ ϕ < 2π (12)
sin ϕ
is identity, the manipulator simply tracks the reference
trajectory. where k > 1 is also a smooth periodic function with
period 2π.
For the objective (b), we choose ψ in such a way that it
maps a connected obstacle domain into a single point. Step3. Define the function ν that is monotonously
Then it looks like as if the obstacle consists of the increasing with respect to r such that
single point from the manipulator in the new coordi-
nate, so the manipulator achieves autonomous obstacle ν(r, ϕ) = 0 ( r = µ(ϕ) )
avoidance whenever the reference trajectory dose not 0 < ν(r, ϕ) < r ( µ(ϕ) < r < k(ϕ)µ(ϕ) ) (13)
ν(r, ϕ) = r ( r ≥ k(ϕ)µ(ϕ) )
go through the single point. Actually, the mapping ψ
plays a role to deform the reference trajectory in the
neighborhood of the obstacle region. Then we introduce the coordinate transformation ψ
which maps the joint angle θ to the modified polar co-
The obstacle domain can be shown in the joint space. ordinate x = (ϕ, ν)T as follows.
Let the center position of the obstacle be θ 0 =
(θ01 , θ02 )T in the joint space, and the manipulator po- ϕ θ1
sition be θ = (θ1 , θ2 )T , and define the distance r and =ψ (14)
ν(ϕ, r) θ2
At this stage, we can adopt any stabilizing linear con- where Vmax is a parameter to be designed subject to
troller K as V ≥ Vmax > 0. This implies that the modified reference
ux = K(xr − x) (15) xr (τ ) moves slowly when V > Vmax holds, and xr (τ ) =
where xr denotes the reference trajectory in the (ϕ, r) xr (t) holds otherwise.
coordinate. Then we obtain a control system which
In the ideal case, that is, if x(t) tracks xr (τ ) exactly, the
autonomously avoid the obstacle. ˙
angular velocity θi in the actual time scale t is always
However, since the above procedure dose not take the no greater than Vmax . This can be checked as follows:
joint velocity into account, it may yields very high speed dθi (t) dθri (τ ) dτ
movement of the manipulator, and as a result the sys- = ≤ Vmax
dt dτ dt
tem may go unstable. In order to avoid this situation,
we adopt the state-dependent time scaling which is pro-
posed by Sampei [11] in the next section. 3.2 Time scale transformation in the feedback
loop
(A) Linearization in new time scales
3 Rate compression via time scale
transformation As well as the reference trajectory, we introduce the
time scale transformation in the feedback loop. Here
In this section, we introduce two types of time scale we consider the two time scales τ1 and τ2 which are
transformation in the previous control system to cope defined by
with the rate constraints of manipulator joints. dt
˙
= si (x, x) > 0, i = 1, 2 (20)
dτi
3.1 Time scale transformation in feedforward
part where si ’s are state-dependent positive function to be
Suppose that the reference trajectory xr (t) in the (ϕ, r) designed. By straightforward calculation, the system
coordinate is given, and that we want to keep the angu- (7) with x = (x1 , x2 )T = (ϕ, ν)T is described in the
lar velocity of each joint within a specified upper bound new time scales by
¯
V . To this end, we adopt a new time scale τ which is
dx1 dϕ
defined by dτ1 dτ1 ˙
s1 ϕ
dx2 = dν = (21)
dt ˙
s2 ν
= s(xr ) > 0 (16) dτ2 dτ2
dτ d2 x 1
where s is a positive function to be designed. Then we dτ1 2 ¨
x1
d2 x 2
= a+B (22)
transform the time-scale of the reference, that is, we 2
¨
x2
dτ2
inject the modified reference xr (τ ) in stead of xr (t) to
the closed loop system. For example, if we set s ≡ 2, where a and B are given as follows:
the new reference xr (τ ) becomes xr (t/2). This implies
that the reference trajectory moves slowly (i.e., it takes s1 ( ∂s1 ϕ + ∂s1 ν)ϕ
∂ϕ ˙ ∂ν ˙ ˙
a = ∂s2 ∂s2 (23)
double time to do the same job). For notational conve- s2 ( ∂ϕ ϕ + ∂ν ν)ν
˙ ˙ ˙
nience, let η denote the transformation;
s2 + s1 ∂s˙1 ϕ
1 ∂ϕ ˙ s1 ∂s˙1 ϕ
∂ν ˙
xref (t) = η(xr (t)) (17) B = ∂s2 (24)
s2 ∂ ϕ ν
˙ ˙ s2 + s2 ∂s˙2 ν
2
∂ν ˙
where xref (t) = xr (τ ).
Now we show how to choose the function s so that the Therefore, from (22) and (7), choosing ux with the new
maximum joint rate does not exceed the specified value input v as
¯
V . First, we calculate ux = B −1 (v − a) (25)
θr1 we obtain the following linear system in the new time
θr = = ψ −1 (xr ) scales:
θr2
dx
which is the obstacle avoidance trajectory to be actually 1
x1
dτ
dx1 0 0 1 0 0 0
tracked by the manipulator joint θ. Let V be 2
dτ2 0 0 0 1 x2 0 0
d2 x 1 =
0 0 0 0 dx1 + 1 0
v
θri (τ )
dτ1 2 dτ1
V = max (18) dx2
i dτ d2 x 2
2
0 0 0 0 dτ2
0 1
dτ2
then we propose to choose the following positive func- (26)
tion. Note that the input torque τ is given by
1 (V ≤ Vmax )
s= V (19)
Vmax (V > Vmax ) τ = k + M J −1 {−h + B −1 {v − a}} (27)
from (2), (5) and (25). The resultant control system is shown in Figure 2.
-
θ
- - - - - - +-
- a K v + a B−1 + a J −1 - M -a τ P -
xr xref uθ ˙
θ, θ
+ x
(B) Choice of the time scales for rate compres- η p ψ
6 6 6 6
− − − −
k p
sion
h
a
What we have to do now is to choose the positive func- p
˙
tion si ’s in (20) in such a way that the rate θ remains
within the admissible range. If we compress the mag- Figure 2: Proposed control system
˙ ˙
nitude of ϕ and ν, we may expect to suppress the joint
˙
angle rate θ. Therefore we choose si ’s from the view-
point of rate suppression in the (ϕ, ν) coordinate. In
4 Experimental Evaluation
particular, the functions si ’s canceling the complexity
of J −1 are selected. One of such a choice is given by
The most important part of this paper is the experi-
2
rϕ + 1 mental evaluation shown in this section.
s1 = α1 + β1 (|ϕ| − 1)2
˙ (28)
rν
1 4.1 Description of the experimental setup
s2 = α2 + β2 (|ν| − 1)2
˙ (29) The procedure given in the previous section is now ap-
rν
plied to a 2-link robot manipulator. Each joint is driven
∂r ∂r
where rϕ := ∂ϕ , rν := ∂ν , the weighting scalars αi and by a direct drive motor, and each link rotates in the
βi (i = 1, 2) are defined as follows: horizontal plane. The dynamics (1) of this system is
γ1 ˙
(|ϕ| > 1)
β1 = (30) ρ1 +ρ2 +2l1ρ3 cos θ2 ρ2 +l1 ρ3 cos θ2 ¨
0 ˙
(|ϕ| ≤ 1) τ = θ
ρ2 +l1 ρ3 cos θ2 ρ2
γ2 ˙
(|ν| > 1) ˙2 ˙
β2 = (31) −l1 ρ3 sin θ2 θ2 −2l1ρ3 sin θ2 θ1 θ2 +d1 θ1
0 ˙
(|ν| ≤ 1) + ˙
˙2 +d2 θ2
l1 ρ3 sin θ2 θ1
∂rν
α1 = exp(−1.2 kµ−ν
µ
∂ν
rν ) 2 2 2
∂rν (32) where ρ1 := I1 + m1 lg1 + m2 l1 , ρ2 := I2 + m2 lg2 , ρ3 :=
α2 = exp(− kµ−ν
kµ
∂ν
rν ) T T
m2 lg2 , τ = (τ1 , τ2 ) and θ = (θ1 , θ2 ) . The parameters
In the above equations, γi ’s are positive scalars to be are defined as follows: τi [Nm] denotes the input torque
chosen by the designer. If γ1 is large, ϕ tends to behave for joint i, mi [kg] denotes the mass of link i li [m]
slowly in the actual time because s1 becomes large. The denotes the length of link i lgi [m] denotes the length
behavior of µ corresponds to the choice of γ2 similarly. from the center to joint of link i, Ii [kgm2 ] denotes
the inertia of link i, di [Nms/rad] denotes the friction
3.3 Design procedure of control system coefficient of joint i and θi [rad] denotes the rotation
Combining the coordinate transformation in Section 2 angle of link i. The concrete values of parameters are
and the time scale transformation shown in this section, l1 = 0.25, l2 = 0.30, ρ1 = 2.55, ρ2 = 0.72, ρ3 = 2.60,
we can construct the control system for autonomous d1 = 0.2415d2 = 0.2457.
obstacle avoidance.
As for the obstacle, we put the cylindrical obstacle in
The design procedure is summarized as follows: front of the manipulator as shown in Figure 3. Carte-
sian coordinate (x, y) is set on the horizontal plane,
Step1. Define µ to determine the boundary ∂S1 of the whose origin is located at the center of the first joint.
obstacle in (11). The obstacle is located at 0.7[m] above from the origin,
and its diameter is 0.18 [m].
Step2. Choose k(ϕ) to define the boundary ∂S2 of the
deformation domain in (12). 4.2 Design of control system
We construct a control system according to the pro-
Step3. Define the function ν(r, ϕ) in (13). posed procedure.
Step4. Choose Vmax in (19) to determine the new time (Step 1) First, we determine the obstacle domain S1 .
scale for the reference trajectory. Taking account of the link width, we set the region is
described by
Step5. Choose γi (i = 1, 2) in (20) to determine the
new time scales for the closed loop. x2 + (y − 0.70)2 = 0.302 (34)
Step6. Design a linear controller K
in the (x, y) coordinate. This domain is depicted as the
v = K(xref − x) (33) dark colored region in Figure 4 in the (ϕ, r) coordinate.
4.3 Experimental results
In the experiment, the reference trajectory is given by
θ1r = 0
(0 ≤ t < 1)
θ2r = 0.3t + 0.1
θ1r = 3.14(t−1)
3 (1 ≤ t < 4) (38)
θ2r = 0.4
θ1r = 3.14
(t ≥ 4)
θ2r = 0.4
Figure 4 shows the trajectory in the (ϕ, r) coordinate
and in the (x, y) coordinate, respectively. If the manip-
Figure 3: Location of the obstacle and the manipulator
ulator follows the given trajectory precisely, it collides
with the obstacle as shown in the figures. Note that
The smooth function µ(ϕ) which determines the bound- the manipulator moves from right to left in the figures.
ary ∂S1 is given by
4
ab 3.5
µ(ϕ) = (35)
a2 sin2 (ϕ − ϕ0 ) + b2 cos2 (ϕ − ϕ0 ) 3
2.5
where a = 1.78, b = 0.42 and ϕ0 = −1.03.
r[m] 2
(Step 2) We choose k(ϕ) as 1.5
1
k(ϕ) ≡ 2. (36)
0.5
The boundary ∂S2 is denoted as the thin broken line in
0
Figure 4. 0 0.5 1 1.5 2 2.5 3
φ[rad]
(Step 3) We choose the following C 2 function ν which
satisfies (13). Figure 4: Reference trajectory in the (ϕ, r) coordinate
ν(r, ϕ) :=
a1 a2 r Finally, we perform the experiment. Figures 5 and 6
a1 − a2 tan(a2 − a1 µ(ϕ) ) (µ(ϕ) ≤ r < kµ(ϕ)) ˙
show the time responses of θ1 (t) and θ1 (t), respectively,
r (r ≥ kµ(ϕ)) by solid lines. The dot and dashed lines correspond to
the case where no time scaling is used. From these fig-
where a1 = 3.56 and a2 = 2.33.
ures, we can see that the velocity remains within the
(Step 4) As for the admissible rate upper bound, the safety zone, and the manipulator reaches the destina-
manipulator moves stably when the angular velocity of tion without delay.
each joint is below 5.0 [rad/s]. So we take
8
Vmax = 5.0.
6
(Step 5) Concerning to the time scales in the feedback
θ1v[rad/sec]
4
loop, we use the parameter
2
(γ1 , γ2 ) = (10, 5).
0
(Step 5) The PD type control law is adopted here.
−2
0 1 2 3 4 5 6 7
v1 ˙ ˙
200(ϕ − ϕref ) + 30s1 (ϕ − ϕref ) time[sec]
= (37)
v2 ˙ ˙
200(ν − rref ) + 30s2 (ν − rref )
˙
Figure 5: Responses of θ1 (t) with (Vmax = 5.0,γ1 =
where xref = (ϕref , rref )T denotes the reference of the 10,γ2 = 5)
closed loop. Note that we have to use the derivative
with respect to the new time scales (τ1 , τ2 ) in the closed The solid lines in Figures 7 and 8 show the trajectory
loop. This explains why si ’s appear in the above control of the manipulator in this case. The other line in each
scheme. figure corresponds to the obstacle avoidance trajectory
3.5 0.6
3
0.4
2.5
0.2
2
θ1[rad]
y[m]
1.5 0
1
−0.2
0.5
0 −0.4
0 1 2 3 4 5 6 7 −0.4 −0.2 0 0.2 0.4 0.6
time[sec] x[m]
Figure 6: Responses of θ1 (t) with (Vmax = 5.0,γ1 = Figure 8: Manipulator configuration in the (x, y) coordi-
10,γ2 = 5) nate with (Vmax = 5.0,γ1 = 10,γ2 = 5)
4 References
[1] S. Akishita, et al. Velocity potential approach for path
3.5 planning to avoid moving obstacles. J. of Advanced Robotics,
3 7(5):463, 1993.
[2] K. Fujimoto, K. Kimura, and T. Sugie. Obstacle avoidance
2.5
of manipulators by using freedom in coordinate transformation
r[m]
2 for exact linearization. In A. Beghi, L Finesso, and G. Picci,
editors, Mathematical Theory of Networks and Systems, pages
1.5 1007–1010. Il Poligrafo, Padova, 1998.
1 [3] K. Fujimoto and T. Sugie. Freedom in coordinate trans-
formation for exact linearization and its application to transient
0.5 behavior improvement. Proc. 35th IEEE Conf. on Decision and
0
Control, pages 84–89, 1996.
0 0.5 1 1.5 2 2.5 3
[4] J. M. Hollerbach. Dynamic scaling of manipulator trajec-
φ[rad]
tories. Trans. ASME, 106:102–106, 1984.
[5] B. Jakubczyk and W. Respondek. On linearization of con-
Figure 7: Manipulator configuration in the (ϕ, r) coordi- trol systems. Bulletin de l’Academie Polonaise des Sciences, Se-
nate with (Vmax = 5.0,γ1 = 10,γ2 = 5) rie des Sciences Mathematiques, 18:517–522, 1980.
[6] N. Kawarazaki and K. Taguchi. Path planning for a ma-
nipulator using differential geometry. IEEE Int. Conf. on Intel-
ligent Robots and Systems, pages 130–137, 1995.
produced by the coordinate transformation ψ. The ma- [7] O. Khatib. Real-time obstacle avoidance for manipulators
nipulator trace the obstacle avoidance trajectory rea- and mobile robots. Int. J. of Robotics Research, pages 90–98,
sonably well. 1986.
[8] P. Khosla and R. Volpe. Super-quadratic potential for
These experimental results show the effectiveness of the obstacle avoidance and approach. IEEE Conf. on Robotics and
Automation, pages 1778–1784, 1988.
proposed method.
[9] T. Lozano-Perez. Automatic planning of manipulator
transfer movements. IEEE Trans. SMC, 11(10):681–698, 1981.
[10] T. Lozano-Perez. A simple motion-planning algorithm for
general robot manipulators. IEEE Trans. Robotics and Automa-
tion, 3(3):224–238, 1987.
5 Conclusion
[11] M. Sampei and K. Furuta. On time scaling for nonlin-
ear systems –application to linearization–. IEEE Trans. Autom.
Contr., AC-31(5):459–462, 1986.
In this paper, we have presented a control method of [12] M. Sampei and K. Furuta. On linearization and control
autonomous obstacle avoidance for manipulators with with transformation of the time scale. Proc. IFAC 10th World
rate constraints. The basic idea behind the method is Congress, pages 97–102, 1987.
to utilize the extra freedom in time scale and coordi- [13] M. Sampei and K. Furuta. Robot control in the neighbor-
nate transformation for exact linearization. First, we hood of singular points. IEEE J. of Robotics and Automation,
4(3):303–309, 1988.
have achieved the obstacle avoidance by using suitably
[14] Y. Tanaka, T. Tsuji, M. Kaneko, and P. G. Morrasso.
chosen coordinate transformation. Then, introducing Trajectory generation using time scaled artificial potential field.
the time scale transformation to the feedforward and Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems,
the feedback loop, we have succeeded in suppressing pages 223–228, 1998.
the velocity of manipulator joints to meet with the rate
constraints. Finally, we have shown the effectiveness of
the proposed method by extensive experiments, which
is most important contribution of the paper.
Related docs
Other docs by sdfgsg234
Selective hydrogenation of cyclopentadiene to form cyclopentene using Raney nickel catalyst and ammonium hydroxide in the reaction mixture
Views: 0 | Downloads: 0
Heated air dissipating device for motor use in a battery-powered forklift truck
Views: 0 | Downloads: 0
