- Email: [email protected]
VIRTUAL TELEOPERATION AND REACTIVE MOTION OF A WHEELED MOBILE MANIPULATOR
VIRTUAL TELEOPERATION AND REACTIVE MOTION OF A WHEELED MOBILE MANIPULATOR
Vin ent Padois, Jean-Yves Fourquet, Pas ale Chiron
Laboratoire Génie de Produ tion, ENIT, Tarbes, Fran e vpadois,fourquet,p hironenit.fr
Abstra t: The work presented in this paper is devoted to the oordinated ontrol of wheeled mobile manipulators in the parti ular ase where operational traje tory is not planned but generated in real-time during the mission. We present a kinemati
ontrol s heme that an be used for operational ontrol and that also gives a
ess to the redundan y of the system. Redundan y as well as set-point ltering and a tuators' input s aling are used to take into a
ount all the se ondary onstraints
2006 IFAC asso iated to mobile manipulation mission. Copyright Keywords: : wheeled mobile manipulators, redundan y, rea tive motion generation
1. INTRODUCTION Roboti missions are getting more and more omplex. From manipulators exe uting highly spe i and simple tasks in stru tured environments, they have evolved to missions implying larger workspa e than before. This workspa e extension is due to bigger expe tations in terms of what a robot should be able to do : explore planets and o eans, manipulate dangerous produ ts, move in dynami al environments; examples are numerous. These new types of missions are hara terised by poorly alibrated environment and goals of dierent nature : operational traje tories to follow, operational for e to exert or ombinations of both. Moreover, they require the ombination of both lo omotion and manipulation means and the term mobile manipulation mission is often used to des ribe these types of roboti missions. Robots used to ful l these missions are alled mobile manipulators and we here fo us on wheeled mobile manipulators that are systems ombining a wheeled mobile platform and one or several roboti arms.
The operational ontrol of wheeled mobile manipulators, i.e. the ontrol of the end-ee tor, requires to take into a
ount their inherent hara teristi s: • wheeled mobile platforms, and by extension wheeled mobile manipulators, are nonholonomi systems; • they are often kinemati ally redundant regarding the operational task to be a hieved; • sub-systems that ompose the whole systems present very dierent dynami s.
It also requires to identify onstraints asso iated to the mission itself : • • • •
obsta les avoidan e; joint limits avoidan e; rated inputs for the a tuators; numeri al singularities.
From this set of onstraints and hara teristi s, a rst lass of approa hes has been proposed to solve the operational ontrol of wheeled mobile manipulators. It is based on a theoreti al orpus widely explored for simple roboti manipulators
(a) Traje tory tea hing Fig.
(b) Collaborative obje t handling
1. Two examples where operational traje tories are generated on-line
and, onsidering their inherent hara teristi s, extended to the ase of those systems. This lass of approa h assumes that the only input of the
ontrol problem is the operational traje tory to be followed or the operational for e to be exerted by the end-ee tor of the system. Among the
ontribution to this lass of approa h, work in (Yamamoto and Yun, 1993) and (Seraji, 1993)
on erns modeling and oordinated ontrol of a parti ular type of wheeled mobile manipulator. Coordinated ontrol, i.e. ontrol of the system as a whole, is also handled in (Kang et al., 2001) and (Umeda et al., 1999) but from the operational for e point of view. Regarding rea tive obsta le avoidan e in the ase of an operational task to be a
omplished, it is treated in (Bro k et al., 2002) where redundan y of the system is used to allow the mobile platform to avoid obsta les. The uni ed kinemati modeling issue is treated in (Bayle et al., 2003). It is based on the lassi ation of wheeled mobile platforms presented in (Campion et al., 1996). The se ond lass of approa hes relies on the observation that the ontrollability properties of nonomnidire tional wheeled mobile platform indu e an expli it de nition of the platform traje tory that omes as a supplementary input to the operational ontrol problem. This is the ase of the work in (Fru hard et al., 2005) that is based on the transverse fun tion approa h introdu ed in (Morin and Samson, 2003). With regard to the realisation of mobile manipulation omplex missions, our previous work presented in (Padois et al., 2004b; Padois et al., 2004a) fo used on the operational ontrol of wheeled mobile manipulators by dynami sequen ing of lo al tasks whose nature is dierent from an operational point of view but also from the angle of the onstraints (listed hereinbefore) that have to be met at a given instant. Operational traje tories (movement and/or for e) are in that ase planned before the mission and adapted during the mission. This paper dis usses the ase of operational ontrol with operational
traje tories generated on-line. Teleoperation, traje tory tea hing, ollaborative obje t handling are example of appli ations of this parti ular topi illustrated on gure 1. This paper is organized in four se tions. The rst one introdu es models. Control laws used to
ompute a tuators set-points are also presented. In the se ond one, we propose solutions espe ially based on the system redundan y to respe t the dierent onstraints asso iated to the mission. The third se tion presents the prin iple of the developed appli ation. We nally show results asso iated to a parti ular example and analyse these results.
2. OPERATIONAL CONTROL OF A WHEELED MOBILE MANIPULATOR We onsider the ase of a wheeled mobile manipulator omposed of a wheeled mobile platform with two independent driving wheels and a planar serial manipulator with two revolute joints su h as depi ted on gure 2. A tuators are velo ity
ontrolled and thus, we rely on a kinemati ontrol s heme.
2.1 Kinemati s
The on guration of su h a system is ompletely T de ned using ve tor q = qTb qTp where qb = T T qb1 qb2 and qp = x y ϑ ϕr ϕl respe tively represents the manipulator on guration and the platform on guration. The situation of the end-ee tor is de ned as a m-dimensional ve tor ξ . We onsider in this paper that the orientation of the end-ee tor isnot to be ontrolled T and we have m = 2 and ξ = xEE yEE . From a geometri point of view, ξ an be expressed as a non linear fun tion f (q) of ve tor q. Dierentiating f (q), the relation between ξ˙ and q˙ is given by : ˙ (1) ξ˙ = J (q) q,
This term is asso iated to the system kinemati redundan y and orreponds to motions that do not provide any end-ee tor motion. From the dynami point of view, this is only true for a ♯ parti ular hoi e of J¯ (q) ( f. (Khatib, 1987)).
θEE x EE xEE
OEE
y EE
l2 qb2
qb1
yb
A parti ular set of generalized inverses of J¯ (q) is alled the set of weighted pseudo inverses. When m ≤ δmob and rank J¯ (q) = m, these generalized inverses are de ned as : h i−1 J¯ (q)⋆Mx = Mx−1 J¯ (q)T J¯ (q) Mx−1 J¯ (q)T , Remark 1 :
l1
xb Ob ϕl
where Mx ∈ Rδmob ×δmob is a positive de nite symmetri matrix. Repla ing J¯ (q)♯ by J¯ (q)⋆Mx in (3) leads to the solution minimizing the Mx -weighted eu lidean norm of (u − u0 ). For Mx = I , J¯ (q)⋆Mx is alled the pseudo-inverse of matrix J¯ (q). Detailed results and proof regarding generalized inversion are presented in (Doty et al., 1993) and (Ben Israel and Greville, 2003).
a a′ yp y
xp Op
b
ϕr y ϑ
O
x
2r
x
yEE
Figure 2. A planar mobile manipulator with J (q) the Ja obian matrix of f (q). Relation (1) does not take nonholonomi onstraints on the platform wheels (rolling without slipping) into a
ount. A tually omponents of ve tor q˙ annot vary independently. However, one an de ne a ve T tor u = uTb uTp of independent δmob ontrol parameters, related to ve tor q˙ by a on guration dependent matrix S (qp ) su h as: q˙ = S (qp ) u. ¯ De ning J (q) as: J¯ (q) = J (q) S (qp ) ,
equation (1) be omes : ξ˙ = J¯ (q) u.
2.3 Operational kinemati ontrol
Given a desired end-ee tor traje tory ξ(t)∗ and assuming that a tuators' low-level ontrol loops
an ensure reje tion of perturbations asso iated to unmodeled dynami s, the following operational kinemati ontrol law : ˙ ∗ + W (ξ(t)∗ − ξ(t)) u(t)∗ = J¯ (q)⋆Mx ξ(t) ⋆ + I − J¯ (q)Mx J¯ (q) u0 ensures the onvergen e of error eξ = ξ(t)∗ − ξ(t) to 0 if W is a positive de nite matrix.
(2)
3. MISSION CONSTRAINTS SATISFACTION
Equation (2) ompletely des ribes the system kinemati s. T T A natural hoi e for u is u = q˙ b v ω where v and ω are respe tively the linear velo ity of point Op along ve tor xp and the angular velo ity of the platform with respe t to ve tor z p . In our parti ular ase the system is said to be kinemati ally redundant of degree two. This property provides the ability to hoose a parti ular ve tor u among those giving the pres ribed end-ee tor velo ity ξ˙ . Solutions are given by the following relation : ♯ ♯ u = J¯ (q) ξ˙ + I − J¯ (q) J¯ (q) u0 , (3)
The listed onstraints are not always a tive during the mission. Thus one has to rst de ne a tivation/dea tivation threshold values for ea h onstraint. These threshold values are, for example, a distan e or a joint angle. Arbitration has to be done so that there is only one a tive onstraint at ea h time. This arbitration is based on the evolution of indi ators (distan e to obsta le, joint angle value) with respe t to their asso iated threshold value. It is also based on priority of ea h onstraint relatively to the others. Swit hings between a tive
onstraints is based on this arbitration.
♯ where J¯ (q) is any generalized inverse of J¯ (q) and u0 any δmob × 1 ve tor.
2.2 Kinemati Redundan y
A
ess to internal motions of the system is given by the null spa e proje tion term of equation (3).
In order to take advantage of the kinemati redundan y of the system, onstraints that are not dire tly asso iated to the operational movement
an be modeled as s alar fun tion P (q) alled potential fun tion. Using lo al optimisation methods su h as the steepest des ent method we an ensure the lo al optimisation of these fun tions. Considering a potential fun tion P (q) to be minimised,
one has to ensure P˙ (q) ≤ 0. To do so, u0 an be
hosen as : T u0 = −K ∇T P (q) S (qp ) I − J¯ (q)⋆Mx J¯ (q) , where K is a positive de nite matrix. To ensure rated a
eleration of the redundan y term, P(t) has to be of lass C 2 and to swit h smoothly between two potential fun tions, a transition fun tion an be reated. De ning tti and tte respe tively as the time when the transition begins and the time when it ends, we have : 0 if t < tti (a) 1 β(t) = if t ≤ t < t (t − t ) ti tf (b) , ti (ttf −tti ) 1 if t ≥ ttf (c) and : P(t) =
Pold (t) (a) Pnew (t)β(t)3 + Pold (t) 1 − β(t)3 (b) . Pnew (t) (c)
Regarding inputs for the a tuators, we must ensure rated a
eleration and speed. If the desired operational traje tory is de ned on-line, a solution is to lter this desired traje tory so that operational a
eleration and speed are rated. Their maximum values an be al ulated in the worst
ase in order to omply with the maximum values asso iated to ea h a tuator and by extension to
ontrol ve tor u. We also have to ensure that the sum of the term asso iated to the operational motion and the term asso iated to the kinemati redundan y of the system is su h that a tuators' inputs maximum values are respe ted. A solution is to s ale dynami ally the term asso iated to the kinemati redundan y. Given the following notations : ⋆ ˙ ∗ + W (ξ(t)∗ − ξ(t)) , up = J¯ (q)Mx ξ(t) ⋆ ured = I − J¯ (q)Mx J¯ (q) u0 , a s aling fa tor α is obtained using the relation:
small α (potential fun tion is slowly optimised), one an s ale the operational movement in order to allo ate more power of a tuation to the internal movement. A way to do this is to slow the operational movement. The geometri path followed by the end-ee tor remains the same but the speed of movement along this path is redu ed for a better optimisation of the a tive onstraint. The ontrol law is then given by : ⋆ ˙ ∗ + W ξ(t)∗ − ξ(t) u(t)∗ = J¯ (q)Mx ξ(t) f f , +α I − J¯ (q)⋆Mx J¯ (q) u0 ˙ ∗ an be written as a fun tion of : ξ(t) ˙ ∗, where ξ(t) f ξ˙ max (asso iated to umax ), ξ¨max (asso iated to u˙ max ), P(t) and α.
4. VIRTUAL TELEOPERATION OF A WHEELED MOBILE MANIPULATOR In this paper, we onsider the ase of on-line generated operational traje tory. This traje tory is ltered to omply with the a tuators' limits but also to slow down the operational movement to the bene t of the a tive se ondary onstraint optimization. We have developed a simulator using Matlab / software. This simulator allows the simulation of the system kinemati s and dynami s ( ontinuous) as well as the various ontrol loops (dis rete time) : low level PID of ea h a tuator, operational ontrol law, operational traje tory ltering. The operational traje tory is generated in real time by a user intera ting with Simulink via a software joysti k. A 3D visual feedba k of the s ene is generated using GDHE software. The prin iple of this appli ation is given on gure 3. Simulink
For ltering purpose, we have hosen a rst order lter (low pass) F (s):
α = min (αscale , αmax ) ,
where : αscale =
|umax,i .sign (ured,i ) − up,i | |ured,i |
F (s) = . i=1...δmob
In the worst ase, α = 0. It means that the operational task does not let any margin to optimise the potential fun tion asso iated to the a tive
onstraints (one or more omponents of u is fully dedi ated to the operational task). In the best
ase, α = αmax where αmax is a value hosen so that the term asso iated to the optimisation of the potential fun tion is not exaggeratedly magni ed (it is, for example, no use to generate large maneuver of the system in order to avoid an obsta le very far from the system, even if the a tuator limits are far from being rea hed). In ase of a
su h as :
Kf , 1 + τs
˙ ∗ = F (s)ξ(s) ˙ ∗, ξ(s) f
(4) (5)
˙ ∗ is the norm of the operational speed where ξ(t) set-point generated by the user using the joysti k ˙ ∗ the norm of the ltered operational and ξ(t) f speed set-point. Maximum operational speed is dynami ally set using Kf , a fun tion of ξ˙max , P(t) and α. Using relations (4) and (5) and hoosing ¨ ∗ to the maximum value ξ¨ ξ(t) , parameter τ is f max given by : τ=
˙ ∗ − ξ(s) ˙ ∗ Kf ξ(s) f . ¨ ξ max
Fig.
3. Prin iple of the virtual teleoperation of a wheeled mobile manipulator 5. RESULTS
Here are presented the results of a simulation. For real time intera tion purpose, simulation of the robot dynami s is simpli ed. Three dierent types of potential fun tions are onsidered: • one for low obsta les avoidan e; this potential fun tion aims at maximising the distan e between the wheeled mobile platform and the obsta les; it also aims at generating a tangential movement of the platform with respe t to the obsta le; • one for ea h joint limit avoidan e; these fun tions are bowl-shaped and take their maximum values around the joint limit values. They have to be minimised ; • one for numeri al singularities avoidan e; these singularities o
ur when J¯ (q) is rank de ient or has a bad ondition number ; maximising the manipulability of the system is a way to avoid singular on gurations.
Figure 4 presents the ee t of ltering the operational speed set-point issued from the joysti k. The ve time zones where the ltered speed is really lower than the un ltered one are those where the a tive onstraint is obsta les avoidan e. In this parti ular simulation no joint limit avoidan e was needed and when redundan y is not used to avoid low obsta les, it is used to maximise manipulabil˙ ∗− ity. This gure also presents signal e(t)∗f = ξ(t) ˙ ∗ . This signal shows the ee ts of operational ξ(t) f speed set-point redu tion as well as the dieren e indu ed by the respe t of maximum operational a
eleration. Figure 5 gives a planar view of the whole movement (blue urve : path of the end-ee tor, red
urve : path of point Op of the platform). In this example, obsta les (green points) are, without loss of generality, modeled as points (no spe i shape or size). These obsta les are e iently avoided using a totally rea tive ontrol s heme. This robust rea tive behaviour is also enlightened by the
platform traje tory that exhibits not expli itly planned usps. 6. CONCLUSION We propose an appli ation of oordinated ontrol for a wheeled mobile manipulator. Two types of
onstraints are present. The rst one is the respe t of the operational traje tory generated on-line by a user via a virtual joysti k. The se ond one
orresponds to all the se ondary onstraints that have to be respe ted when dealing with mobile manipulation. Using redundan y and lo al optimisation methods, we are able to swit h between those onstraints a
ording to their importan e at a given instant during the mission and thus to optimise the right onstraint at the right time. Respe t of a tuators' limits is also a hieved using set-point ltering and input s aling. REFERENCES Bayle, B., M. Renaud and J.-Y. Fourquet (2003). Nonholonomi mobile manipulators: kinemati s, velo ities and redundan ies. Journal of Intelligent and Roboti Systems 36, 4563. Ben Israel, A. and T.N.E. Greville (2003). Genralized Inverses : Theory and Appli ations, se ond edition. Springer. ISBN 0-387-00293-6.
Bro k, O., O. Khatib and S. Viji (2002). Task onsistent obsta le avoidan e and motion behavior for mobile manipulation. In: Pro eedings of the ICRA. Washington DC - USA. pp. 388 393. ISBN 0-7803-7273-5. Campion, G., G. Bastin and B. D'Andréa Novel (1996). Stru tural properties and lassi ation of kinemati and dynami models of wheeled mobile robots. IEEE Transa tions on Automati Control 12, 4762. Doty, K. L., C. Mel hiorri and C. Boniveto (1993). A theory of generalized inverses applied to Roboti s. The International Journal of Roboti s Resear h 12(1), 119.
Fig.
4. Ee ts of ltering operational speed set-points issued from joysti k
Fig.
5. Planar view of the movement of the system during the mission (grid size : 0.5 × 0.5 m)
Fru hard, M., P. Morin and C. Samson (2005). A framework for the ontrol of nonholonomi mobile manipulators. In: INRIA Resear h Report No 5556. Kang, S., K. Komoriya, K. Yokoi, T. Koutoku and K. Tanie (2001). Utilization of inertial ee t in damping-based posture ontrol of mobile manipulator. In: Pro eedings of the 2001 IEEE ICRA. Seoul, Korea. pp. 1277 1282. Khatib, O. (1987). A Uni ed Approa h for Motion and For e Control of Robot Manipulators: The Operational Spa e Formulation. IEEE Journal of Roboti s and Automation 3(1), 43 53. Morin, P. and C. Samson (2003). Pra ti al stabilization of driftless systems on Lie groups: the transverse fun tion approa h. IEEE Transa tions on Automati Control 48, 1496 1508. Padois, V., J-Y. Fourquet and P. Chiron (2004a). Motion Coordination of a mobile manipulator for onta t transition tasks. In: Pro eedings of the 10 th IEEE International Confer-
en e on Methods and Models in Automation and Roboti s. Vol. 2. Miedzyzdroje - Pologne.
pp. 967972. ISBN 83-88764-04-7. Padois, V., J-Y. Fourquet, P. Chiron and M. Renaud (2004b). On onta t transition for nonholonomi mobile manipulators. In: Pro eedings of the 9th ISER. Singapour. Seraji, H. (1993). An on-line approa h to oordinated mobility and manipulation. In: Pro eedings of the 1993 IEEE ICRA. Atlanta, USA. pp. 2835. Umeda, Y., D. Nakamura, T. Mukarami and K. Ohnishi (1999). Hybrid position/for e ontrol of a mobile manipulator based on ooperative task sharing. In: Pro eedings of the 1999 IEEE ISIE. Bled, Slovenia. Yamamoto, Y. and X. Yun (1993). Re ent trends in mobile robots (Y. F. Zheng). Chap. 6: Coordinating lo omotion and manipulation of a mobile manipulator, pp. 157181. World S ienti .
Vin ent Padois, Jean-Yves Fourquet, Pas ale Chiron
Laboratoire Génie de Produ tion, ENIT, Tarbes, Fran e vpadois,fourquet,p hironenit.fr
Abstra t: The work presented in this paper is devoted to the oordinated ontrol of wheeled mobile manipulators in the parti ular ase where operational traje tory is not planned but generated in real-time during the mission. We present a kinemati
ontrol s heme that an be used for operational ontrol and that also gives a
ess to the redundan y of the system. Redundan y as well as set-point ltering and a tuators' input s aling are used to take into a
ount all the se ondary onstraints
2006 IFAC asso iated to mobile manipulation mission. Copyright Keywords: : wheeled mobile manipulators, redundan y, rea tive motion generation
1. INTRODUCTION Roboti missions are getting more and more omplex. From manipulators exe uting highly spe i and simple tasks in stru tured environments, they have evolved to missions implying larger workspa e than before. This workspa e extension is due to bigger expe tations in terms of what a robot should be able to do : explore planets and o eans, manipulate dangerous produ ts, move in dynami al environments; examples are numerous. These new types of missions are hara terised by poorly alibrated environment and goals of dierent nature : operational traje tories to follow, operational for e to exert or ombinations of both. Moreover, they require the ombination of both lo omotion and manipulation means and the term mobile manipulation mission is often used to des ribe these types of roboti missions. Robots used to ful l these missions are alled mobile manipulators and we here fo us on wheeled mobile manipulators that are systems ombining a wheeled mobile platform and one or several roboti arms.
The operational ontrol of wheeled mobile manipulators, i.e. the ontrol of the end-ee tor, requires to take into a
ount their inherent hara teristi s: • wheeled mobile platforms, and by extension wheeled mobile manipulators, are nonholonomi systems; • they are often kinemati ally redundant regarding the operational task to be a hieved; • sub-systems that ompose the whole systems present very dierent dynami s.
It also requires to identify onstraints asso iated to the mission itself : • • • •
obsta les avoidan e; joint limits avoidan e; rated inputs for the a tuators; numeri al singularities.
From this set of onstraints and hara teristi s, a rst lass of approa hes has been proposed to solve the operational ontrol of wheeled mobile manipulators. It is based on a theoreti al orpus widely explored for simple roboti manipulators
(a) Traje tory tea hing Fig.
(b) Collaborative obje t handling
1. Two examples where operational traje tories are generated on-line
and, onsidering their inherent hara teristi s, extended to the ase of those systems. This lass of approa h assumes that the only input of the
ontrol problem is the operational traje tory to be followed or the operational for e to be exerted by the end-ee tor of the system. Among the
ontribution to this lass of approa h, work in (Yamamoto and Yun, 1993) and (Seraji, 1993)
on erns modeling and oordinated ontrol of a parti ular type of wheeled mobile manipulator. Coordinated ontrol, i.e. ontrol of the system as a whole, is also handled in (Kang et al., 2001) and (Umeda et al., 1999) but from the operational for e point of view. Regarding rea tive obsta le avoidan e in the ase of an operational task to be a
omplished, it is treated in (Bro k et al., 2002) where redundan y of the system is used to allow the mobile platform to avoid obsta les. The uni ed kinemati modeling issue is treated in (Bayle et al., 2003). It is based on the lassi ation of wheeled mobile platforms presented in (Campion et al., 1996). The se ond lass of approa hes relies on the observation that the ontrollability properties of nonomnidire tional wheeled mobile platform indu e an expli it de nition of the platform traje tory that omes as a supplementary input to the operational ontrol problem. This is the ase of the work in (Fru hard et al., 2005) that is based on the transverse fun tion approa h introdu ed in (Morin and Samson, 2003). With regard to the realisation of mobile manipulation omplex missions, our previous work presented in (Padois et al., 2004b; Padois et al., 2004a) fo used on the operational ontrol of wheeled mobile manipulators by dynami sequen ing of lo al tasks whose nature is dierent from an operational point of view but also from the angle of the onstraints (listed hereinbefore) that have to be met at a given instant. Operational traje tories (movement and/or for e) are in that ase planned before the mission and adapted during the mission. This paper dis usses the ase of operational ontrol with operational
traje tories generated on-line. Teleoperation, traje tory tea hing, ollaborative obje t handling are example of appli ations of this parti ular topi illustrated on gure 1. This paper is organized in four se tions. The rst one introdu es models. Control laws used to
ompute a tuators set-points are also presented. In the se ond one, we propose solutions espe ially based on the system redundan y to respe t the dierent onstraints asso iated to the mission. The third se tion presents the prin iple of the developed appli ation. We nally show results asso iated to a parti ular example and analyse these results.
2. OPERATIONAL CONTROL OF A WHEELED MOBILE MANIPULATOR We onsider the ase of a wheeled mobile manipulator omposed of a wheeled mobile platform with two independent driving wheels and a planar serial manipulator with two revolute joints su h as depi ted on gure 2. A tuators are velo ity
ontrolled and thus, we rely on a kinemati ontrol s heme.
2.1 Kinemati s
The on guration of su h a system is ompletely T de ned using ve tor q = qTb qTp where qb = T T qb1 qb2 and qp = x y ϑ ϕr ϕl respe tively represents the manipulator on guration and the platform on guration. The situation of the end-ee tor is de ned as a m-dimensional ve tor ξ . We onsider in this paper that the orientation of the end-ee tor isnot to be ontrolled T and we have m = 2 and ξ = xEE yEE . From a geometri point of view, ξ an be expressed as a non linear fun tion f (q) of ve tor q. Dierentiating f (q), the relation between ξ˙ and q˙ is given by : ˙ (1) ξ˙ = J (q) q,
This term is asso iated to the system kinemati redundan y and orreponds to motions that do not provide any end-ee tor motion. From the dynami point of view, this is only true for a ♯ parti ular hoi e of J¯ (q) ( f. (Khatib, 1987)).
θEE x EE xEE
OEE
y EE
l2 qb2
qb1
yb
A parti ular set of generalized inverses of J¯ (q) is alled the set of weighted pseudo inverses. When m ≤ δmob and rank J¯ (q) = m, these generalized inverses are de ned as : h i−1 J¯ (q)⋆Mx = Mx−1 J¯ (q)T J¯ (q) Mx−1 J¯ (q)T , Remark 1 :
l1
xb Ob ϕl
where Mx ∈ Rδmob ×δmob is a positive de nite symmetri matrix. Repla ing J¯ (q)♯ by J¯ (q)⋆Mx in (3) leads to the solution minimizing the Mx -weighted eu lidean norm of (u − u0 ). For Mx = I , J¯ (q)⋆Mx is alled the pseudo-inverse of matrix J¯ (q). Detailed results and proof regarding generalized inversion are presented in (Doty et al., 1993) and (Ben Israel and Greville, 2003).
a a′ yp y
xp Op
b
ϕr y ϑ
O
x
2r
x
yEE
Figure 2. A planar mobile manipulator with J (q) the Ja obian matrix of f (q). Relation (1) does not take nonholonomi onstraints on the platform wheels (rolling without slipping) into a
ount. A tually omponents of ve tor q˙ annot vary independently. However, one an de ne a ve T tor u = uTb uTp of independent δmob ontrol parameters, related to ve tor q˙ by a on guration dependent matrix S (qp ) su h as: q˙ = S (qp ) u. ¯ De ning J (q) as: J¯ (q) = J (q) S (qp ) ,
equation (1) be omes : ξ˙ = J¯ (q) u.
2.3 Operational kinemati ontrol
Given a desired end-ee tor traje tory ξ(t)∗ and assuming that a tuators' low-level ontrol loops
an ensure reje tion of perturbations asso iated to unmodeled dynami s, the following operational kinemati ontrol law : ˙ ∗ + W (ξ(t)∗ − ξ(t)) u(t)∗ = J¯ (q)⋆Mx ξ(t) ⋆ + I − J¯ (q)Mx J¯ (q) u0 ensures the onvergen e of error eξ = ξ(t)∗ − ξ(t) to 0 if W is a positive de nite matrix.
(2)
3. MISSION CONSTRAINTS SATISFACTION
Equation (2) ompletely des ribes the system kinemati s. T T A natural hoi e for u is u = q˙ b v ω where v and ω are respe tively the linear velo ity of point Op along ve tor xp and the angular velo ity of the platform with respe t to ve tor z p . In our parti ular ase the system is said to be kinemati ally redundant of degree two. This property provides the ability to hoose a parti ular ve tor u among those giving the pres ribed end-ee tor velo ity ξ˙ . Solutions are given by the following relation : ♯ ♯ u = J¯ (q) ξ˙ + I − J¯ (q) J¯ (q) u0 , (3)
The listed onstraints are not always a tive during the mission. Thus one has to rst de ne a tivation/dea tivation threshold values for ea h onstraint. These threshold values are, for example, a distan e or a joint angle. Arbitration has to be done so that there is only one a tive onstraint at ea h time. This arbitration is based on the evolution of indi ators (distan e to obsta le, joint angle value) with respe t to their asso iated threshold value. It is also based on priority of ea h onstraint relatively to the others. Swit hings between a tive
onstraints is based on this arbitration.
♯ where J¯ (q) is any generalized inverse of J¯ (q) and u0 any δmob × 1 ve tor.
2.2 Kinemati Redundan y
A
ess to internal motions of the system is given by the null spa e proje tion term of equation (3).
In order to take advantage of the kinemati redundan y of the system, onstraints that are not dire tly asso iated to the operational movement
an be modeled as s alar fun tion P (q) alled potential fun tion. Using lo al optimisation methods su h as the steepest des ent method we an ensure the lo al optimisation of these fun tions. Considering a potential fun tion P (q) to be minimised,
one has to ensure P˙ (q) ≤ 0. To do so, u0 an be
hosen as : T u0 = −K ∇T P (q) S (qp ) I − J¯ (q)⋆Mx J¯ (q) , where K is a positive de nite matrix. To ensure rated a
eleration of the redundan y term, P(t) has to be of lass C 2 and to swit h smoothly between two potential fun tions, a transition fun tion an be reated. De ning tti and tte respe tively as the time when the transition begins and the time when it ends, we have : 0 if t < tti (a) 1 β(t) = if t ≤ t < t (t − t ) ti tf (b) , ti (ttf −tti ) 1 if t ≥ ttf (c) and : P(t) =
Pold (t) (a) Pnew (t)β(t)3 + Pold (t) 1 − β(t)3 (b) . Pnew (t) (c)
Regarding inputs for the a tuators, we must ensure rated a
eleration and speed. If the desired operational traje tory is de ned on-line, a solution is to lter this desired traje tory so that operational a
eleration and speed are rated. Their maximum values an be al ulated in the worst
ase in order to omply with the maximum values asso iated to ea h a tuator and by extension to
ontrol ve tor u. We also have to ensure that the sum of the term asso iated to the operational motion and the term asso iated to the kinemati redundan y of the system is su h that a tuators' inputs maximum values are respe ted. A solution is to s ale dynami ally the term asso iated to the kinemati redundan y. Given the following notations : ⋆ ˙ ∗ + W (ξ(t)∗ − ξ(t)) , up = J¯ (q)Mx ξ(t) ⋆ ured = I − J¯ (q)Mx J¯ (q) u0 , a s aling fa tor α is obtained using the relation:
small α (potential fun tion is slowly optimised), one an s ale the operational movement in order to allo ate more power of a tuation to the internal movement. A way to do this is to slow the operational movement. The geometri path followed by the end-ee tor remains the same but the speed of movement along this path is redu ed for a better optimisation of the a tive onstraint. The ontrol law is then given by : ⋆ ˙ ∗ + W ξ(t)∗ − ξ(t) u(t)∗ = J¯ (q)Mx ξ(t) f f , +α I − J¯ (q)⋆Mx J¯ (q) u0 ˙ ∗ an be written as a fun tion of : ξ(t) ˙ ∗, where ξ(t) f ξ˙ max (asso iated to umax ), ξ¨max (asso iated to u˙ max ), P(t) and α.
4. VIRTUAL TELEOPERATION OF A WHEELED MOBILE MANIPULATOR In this paper, we onsider the ase of on-line generated operational traje tory. This traje tory is ltered to omply with the a tuators' limits but also to slow down the operational movement to the bene t of the a tive se ondary onstraint optimization. We have developed a simulator using Matlab / software. This simulator allows the simulation of the system kinemati s and dynami s ( ontinuous) as well as the various ontrol loops (dis rete time) : low level PID of ea h a tuator, operational ontrol law, operational traje tory ltering. The operational traje tory is generated in real time by a user intera ting with Simulink via a software joysti k. A 3D visual feedba k of the s ene is generated using GDHE software. The prin iple of this appli ation is given on gure 3. Simulink
For ltering purpose, we have hosen a rst order lter (low pass) F (s):
α = min (αscale , αmax ) ,
where : αscale =
|umax,i .sign (ured,i ) − up,i | |ured,i |
F (s) = . i=1...δmob
In the worst ase, α = 0. It means that the operational task does not let any margin to optimise the potential fun tion asso iated to the a tive
onstraints (one or more omponents of u is fully dedi ated to the operational task). In the best
ase, α = αmax where αmax is a value hosen so that the term asso iated to the optimisation of the potential fun tion is not exaggeratedly magni ed (it is, for example, no use to generate large maneuver of the system in order to avoid an obsta le very far from the system, even if the a tuator limits are far from being rea hed). In ase of a
su h as :
Kf , 1 + τs
˙ ∗ = F (s)ξ(s) ˙ ∗, ξ(s) f
(4) (5)
˙ ∗ is the norm of the operational speed where ξ(t) set-point generated by the user using the joysti k ˙ ∗ the norm of the ltered operational and ξ(t) f speed set-point. Maximum operational speed is dynami ally set using Kf , a fun tion of ξ˙max , P(t) and α. Using relations (4) and (5) and hoosing ¨ ∗ to the maximum value ξ¨ ξ(t) , parameter τ is f max given by : τ=
˙ ∗ − ξ(s) ˙ ∗ Kf ξ(s) f . ¨ ξ max
Fig.
3. Prin iple of the virtual teleoperation of a wheeled mobile manipulator 5. RESULTS
Here are presented the results of a simulation. For real time intera tion purpose, simulation of the robot dynami s is simpli ed. Three dierent types of potential fun tions are onsidered: • one for low obsta les avoidan e; this potential fun tion aims at maximising the distan e between the wheeled mobile platform and the obsta les; it also aims at generating a tangential movement of the platform with respe t to the obsta le; • one for ea h joint limit avoidan e; these fun tions are bowl-shaped and take their maximum values around the joint limit values. They have to be minimised ; • one for numeri al singularities avoidan e; these singularities o
ur when J¯ (q) is rank de ient or has a bad ondition number ; maximising the manipulability of the system is a way to avoid singular on gurations.
Figure 4 presents the ee t of ltering the operational speed set-point issued from the joysti k. The ve time zones where the ltered speed is really lower than the un ltered one are those where the a tive onstraint is obsta les avoidan e. In this parti ular simulation no joint limit avoidan e was needed and when redundan y is not used to avoid low obsta les, it is used to maximise manipulabil˙ ∗− ity. This gure also presents signal e(t)∗f = ξ(t) ˙ ∗ . This signal shows the ee ts of operational ξ(t) f speed set-point redu tion as well as the dieren e indu ed by the respe t of maximum operational a
eleration. Figure 5 gives a planar view of the whole movement (blue urve : path of the end-ee tor, red
urve : path of point Op of the platform). In this example, obsta les (green points) are, without loss of generality, modeled as points (no spe i shape or size). These obsta les are e iently avoided using a totally rea tive ontrol s heme. This robust rea tive behaviour is also enlightened by the
platform traje tory that exhibits not expli itly planned usps. 6. CONCLUSION We propose an appli ation of oordinated ontrol for a wheeled mobile manipulator. Two types of
onstraints are present. The rst one is the respe t of the operational traje tory generated on-line by a user via a virtual joysti k. The se ond one
orresponds to all the se ondary onstraints that have to be respe ted when dealing with mobile manipulation. Using redundan y and lo al optimisation methods, we are able to swit h between those onstraints a
ording to their importan e at a given instant during the mission and thus to optimise the right onstraint at the right time. Respe t of a tuators' limits is also a hieved using set-point ltering and input s aling. REFERENCES Bayle, B., M. Renaud and J.-Y. Fourquet (2003). Nonholonomi mobile manipulators: kinemati s, velo ities and redundan ies. Journal of Intelligent and Roboti Systems 36, 4563. Ben Israel, A. and T.N.E. Greville (2003). Genralized Inverses : Theory and Appli ations, se ond edition. Springer. ISBN 0-387-00293-6.
Bro k, O., O. Khatib and S. Viji (2002). Task onsistent obsta le avoidan e and motion behavior for mobile manipulation. In: Pro eedings of the ICRA. Washington DC - USA. pp. 388 393. ISBN 0-7803-7273-5. Campion, G., G. Bastin and B. D'Andréa Novel (1996). Stru tural properties and lassi ation of kinemati and dynami models of wheeled mobile robots. IEEE Transa tions on Automati Control 12, 4762. Doty, K. L., C. Mel hiorri and C. Boniveto (1993). A theory of generalized inverses applied to Roboti s. The International Journal of Roboti s Resear h 12(1), 119.
Fig.
4. Ee ts of ltering operational speed set-points issued from joysti k
Fig.
5. Planar view of the movement of the system during the mission (grid size : 0.5 × 0.5 m)
Fru hard, M., P. Morin and C. Samson (2005). A framework for the ontrol of nonholonomi mobile manipulators. In: INRIA Resear h Report No 5556. Kang, S., K. Komoriya, K. Yokoi, T. Koutoku and K. Tanie (2001). Utilization of inertial ee t in damping-based posture ontrol of mobile manipulator. In: Pro eedings of the 2001 IEEE ICRA. Seoul, Korea. pp. 1277 1282. Khatib, O. (1987). A Uni ed Approa h for Motion and For e Control of Robot Manipulators: The Operational Spa e Formulation. IEEE Journal of Roboti s and Automation 3(1), 43 53. Morin, P. and C. Samson (2003). Pra ti al stabilization of driftless systems on Lie groups: the transverse fun tion approa h. IEEE Transa tions on Automati Control 48, 1496 1508. Padois, V., J-Y. Fourquet and P. Chiron (2004a). Motion Coordination of a mobile manipulator for onta t transition tasks. In: Pro eedings of the 10 th IEEE International Confer-
en e on Methods and Models in Automation and Roboti s. Vol. 2. Miedzyzdroje - Pologne.
pp. 967972. ISBN 83-88764-04-7. Padois, V., J-Y. Fourquet, P. Chiron and M. Renaud (2004b). On onta t transition for nonholonomi mobile manipulators. In: Pro eedings of the 9th ISER. Singapour. Seraji, H. (1993). An on-line approa h to oordinated mobility and manipulation. In: Pro eedings of the 1993 IEEE ICRA. Atlanta, USA. pp. 2835. Umeda, Y., D. Nakamura, T. Mukarami and K. Ohnishi (1999). Hybrid position/for e ontrol of a mobile manipulator based on ooperative task sharing. In: Pro eedings of the 1999 IEEE ISIE. Bled, Slovenia. Yamamoto, Y. and X. Yun (1993). Re ent trends in mobile robots (Y. F. Zheng). Chap. 6: Coordinating lo omotion and manipulation of a mobile manipulator, pp. 157181. World S ienti .