Constrained Manipulator Visual Servoing (CMVS)

2011 IEE E/R SJ Internat ional Conference on Inte l l igent Robots and Syste ms

Septe m ber 25-30, 2011. San Francisco, C A, US A

Constra ined M a nipulator Visual Servoing (C M V S): Rapid Robot Progra m ming in Cluttered W orkspaces

A mbroseChan ,E l izabe thA.Crof t ,JamesJ .L i t t l e

Abstrac t —This paper presents a model-free opt imizat ion

fra me workforthev isualservo ingofeye-in-hand manipulators inc lut teredenviron ments .Visua l feedbackisusedtoso lvefora se tof feas ib le trajector ies thatbr i ngtherobotend-ef fec tortoa targetobjectataprev ious lyuntaught locat ionunderanu m ber of chal lenging constra ints (i .e ., whole-ar m col l i s ions , object occ lus ions ,robot ’s jo int l imits ,cam era ’s sens ing l imits ).Anovel

contro l ler is proposed , which explo i t s the natura l by-products of theteach-by-showingprocess ,t ohelptherobotnavigate th is

non-convex space . Exa mining the user-de monstrated trajecto- r ies that leaduptothereference image ,weuseaco mbinat ion ofs tochast icopt imizat iontechniquesandclass ica lopt imizat ion techniquestoextract there levantcost funct ionsandconstra ints

for servo ing . W e hypothes ize that we can leverage the user ’s sensorycapabi l i t i e sandkno wledgeof theworkspacetoa l lev iate theburdenof model ingsystemcons tra intsexpl ic i t ly .Wever i fy

th i s hypothes i s via real i s t i c exper iments on a Barret t W A M 7- D O F manipulator equipped with a Sony X C-H R70 ca mera to

show the co mparat ive ef ?cacy of this approach.

Fig . 1. The cor rec t hand/gr ipper pos i t ion for pick ing up a cup (le f t ) and i t scor responding image(r igh t ).

I . I NT R O D U C TIO N

Thesoc ie ta l impac tofphys ica l r obot ic technologyisev i -

dent innu merousexamplesofhumandai lyac t iv i t i e s ,suchas

inmanufac tu r ing ,inmin ing ,andinmedica l surger ies .How-

W eproposeaso lu t ion to th i sc lu t t ered-workspacecont ro l ever , there are s t i l l s ign i ?can t bar r ie r s tha t preven t phys ica l problem via a teaching method ca l led Cons t rained Manip- robots f rom being ef fec t ive l y ut i l i zed in many appl ica t ions . ula tor Visua l Servo i ng (C M V S). An exa mple is shown in Notwi ths tand ing thecos tofa robotp la t fo rm,onepreva i l ing Figure1.Aca mera i smountedad jacen t to therobot ’s end - problemis thed i f ?cul ty inprogram mingarobot toper form effec tor tosense the ta rge tobjec t .Whenarobot ,loca teda t

phys ica l ly usefu l tasks in changing envi ronments . To be of basef rame,F b ,i s ins t ruc ted top ickupacupf romthef r idge ,

co m mon use , modern-day robots must beco me easy , safe , i t must be ?rs t taught how to move i t s end-ef fec tor f rame, andin tu i t ive toprogram. F e , to the cor rec t pos i t ion with respec t to the targe t objec t

f rame, F , so as to properly grasp the cup . To teach the

An exa mple of a non-t r iv ial program ming task is tha t of teach ing a robot how to get i t s end-e f fec tor in posi t ion

with respec t to a targe t obj ec t , wi thout vio la t ing workspace

cons t ra in t s .Robot teachpendant sa res luggishandawk ward

in te r facesevenfor t ra ined “exper t s ”.Moreover ,theyresu l t in

piecewisepre-program medpat hs tha tdonotgenera l izewel l with respec t to pos i t iona l uncer ta in t ies . In many indus t r i a l

and do mes t ic scenar ios – e.g ., when a robot is asked to re t r i evea too l f romabinofpar t s ,oracupf romacupboard –

the loca t ionof the ta rge tob jec tmaychanges ign i ?cant ly .An-

other co m mon approach is to use sensor -based loca l i za t ion

methods in conjunc t ion wi t h model -based path plann ing to

sa t i s fy the cons t ra in t s . Ho w ever , these methods requi re the o robot ,theusermoves therobot to t he ta rge tob jec twhi le the robot observes what the com ple t ion of the task looks l ike . Thede mons t ra t ion i s repea tedahandfu lof t imes(i .e .4to5 samples )inC M V S.Since thecameraf rame,F c ,hasa ?xed re la t ionsh ipwi th respec t toF ,th i s re fe rence imagecanbe

e used to encapsu la te and genera l ize the pos i t ion ing task. In visua l servo ing , the robot uses fea tures tha t are ext rac ted

f rom the images in a feedback cont ro l law, to generat e new mot ions to in te rac t with the targe t objec t a t previous ly untaught loca t ions . In C M V S, the robot does so wi thout caus inganypar to f i t sa rmtoco l l i dewi th theenvi ronment . Recent progress in visua l servo in

g researc

h has de mon- user to spend cons iderab le t ime and resources to ca l ibra te st ra ted a wide range of cont ro l schemes tha t opt imize the Car tes ian t ra jec to ry of the camera [1] [2] [3] [4], wh

i l e

ensur ing tha t camera ?eld-of -v iew cons t ra in t s are sa t i s ?ed

[5] [6] [7]. Ho wever , less emphas i s (t rad i t iona l ly ) has been placedonensur ing tha t the robot ’s jo in t l imi t sandco l l i s ion cons t ra in t sa rea l sorespec ted .I mage-basedpa thp lann ingvia andtomodel theenvi ronment forthe robot . A. Chan , E.A. Crof t , J .J . Li t t l e are wi th the Univers i ty of Bri t i sh Columbia , Canada (achan82@mech.ubc .ca , ecrof t@mech.ubc .ca ,

l i t t l e @cs .ubc .ca ). This work was suppor ted in par t by NSE R C, CFI and

ICICS.

978-1-61284-456-5/11/$26.00 ?2011 IEE E

2825

ho mography-based recons t ruc t ion and poten t ia l ?elds was Equat ion 2 descr ibes the w hole-a rm col l i s ion cons t ra in t s proposed in[8]to loca l lymanagerobot jo in t l imi t s .Of ?ine andl ine-of -s igh tcons t ra in t s .Q c is these tofcon ?gura t ions

opt imiza t ion methods were used in [9] [10] to rea l ize large which do not cause any par t of the arm to co l l ide wi th cameramot ions ,whi leavoid ingend-e f fec tor col l i s ions . St i l l , i t has been di f ?cul t t o incorpora te whole-arm col - cause the targe t objec t to be occ luded from the camera . obs tac les and Q o is the se t of con ?gura t ions which do not l i s ion cons t ra in t s for two reasons : (i ) visua l servo ing i s Equat ions 3 and 4 descr ibe the robot ’s jo in t pos i t ion and inheren t ly an end-e f fec tor feedback cont ro l scheme; (i i ) the veloc i ty cons t ra in t s , respec t ive ly , where [q mi n , q m a x ] are robot ’s whole-arm col l i s ion-f ree space is di f ?cul t to m odel vec tors of pos i t ion l i mits and [q ˙min , q ˙max ] are vec tors of in prac t ice . A volume sweep ing approach wi th an oct ree veloc i ty l imi t s . Equa t ions 5 and 6 descr ibe the camera ’s

represen ta t ion was proposed in [11] to al low an operato r ?eld-of -v iew cons t ra in t s for a targe t objec t wi th n fea tures , tomanua l lymapout theco l l i s ion-f reespace .Unfor tuna te ly , where [u mi n , u m a x ] and [v mi n , v m a x ] are the l imi t s in naviga t ingaroundobs tac les in thi snon-convexspace iss t i l l the hor izonta l and the ver t i ca l di rec t ions , respec t ive ly , of di f ?cul t for visua l servo ing cont ro l le r s . In th i s paper , we the imagingar ray .Equat ion7descr ibes therobot ’s dynamic show ho w the C M V S framework can be used to capt ure cons t ra in t sv iaa ?rs t -o rderdisc re t i za t ionwi th t imes tep δt . thev i sua l se rvo inggoa l ,whi les i m ul taneous lyex t rac t ing the

whole-armcol l i s ioncons t ra in t s withoutCA D models .

C. Convex i tyAnalys i s

For typ ica l manipu la to rs with revolu te jo in t s , the con-

s t ra ined visua l servoing problem can be shown to be non- I I . S YSTE M M O D E LIN G F O R C O N V E XIT Y A N A L Y SIS Visua l servo ing can be genera l i zed as an opt imiza t ion convex .Equa t ion1isanon-convexfunc t ion tha t resu l t s f rom problemtha tmin imizes thecur rent lyobserved imageer rors thesuccess ivemul t ip l ica t i onofho mogeneous t rans format i on

withrespec t to there fe rence image .Thepos i t ion ing task i s

and pro jec t ion mat r ices (i .e ., produc ts and su ms of sin(q i ) co mple tewhenthefea tures ,p(t),o f thecur ren t lyobserved and cos(q i )). The se t s Q and Q are also non-convex , c o

image are iden t ica l to those , p d , of the refe rence image . where two robot con ?gura t ions can be col l i s ion-f ree , but In manipula tor visua l servoing , th i s opt imiza t ion is subjec t so mel inear ly in te rpo la t edcon ?gura t ions resu l t inco l l i s i ons . to a se t of cons t ra in t s per t a in ing to the robot ’s jo in t l imi t s ,

D. Cons tra inedNon-l inearOpt imiza t ion

ac tua t ion l imi t s , the camera’s sens ing l imi t s , and obs tac les

In prev ious work [12], we addressed the prob lem of which l imi twhere therobotcanm ove .Thedec i s ionvar iab les cons t ra inedvisua l se rvo ingviaanon-l inearmodelpred ic t ive cont ro l (MP C) frame work . W e used a dyna mic model of

the plan t for onl ine plann i ng and se lec t ion of an optimal are thecont ro l inpu tsq(t).

A. Robot ,CameraandObjec t M odels W e use a gener ic objec t m odel co mpr ised of a nu m ber sequenceofcont ro lac t ions toach ieveades i redoutpu t ,whi l e of 3-D fea ture poin t s , whose coord ina tes (o X j ,o Y j ,o Z j ) co mpensa t ingforpred ic t ioner rorsus ingfeedbackprov ided are de ?ned wi th respec t to the objec t ’s canonica l f rame. by observa t ions of the rea l plan t . Such opt imiza t i on was For a targe t objec t made up of n fea ture poin t s , the objec t subjec t to thesys temdyna micsandthecons t ra in t s re la tedto

m odel is : o P j C be the 3 × 3 ca mera matr ix for a class ica l pin-hole o X j o Y j o Z j 1 ,j ∈ [1,n ]. Let the s ta tes and inputs of the sys tem (as descr ibed in Sect ion

= I I -B).Weusedacos t func t ionof t heL Q Rform:

camera .Theho mogeneous t ransfo rmat ion 2T expresses the

T 1 f 0(t)= p(t)?p d W p p(t)?p d +q ˙(t )T W q q ˙(t ), coord ina tes of F in F 2. b T e (q) is the robot kinemati c 1 (8) m odel .The imagecoord ina tesofthe fea turepoin t sp(q )=

where W and W p are w e igh ts for regula r iza t ion . The q T

l inear re la t ionsh ipwi th the ··c on t ro l · inputsq: (p u p ) (p u p ) (p u p ) have a non- v 1 v 2 v n onl ineso lu t ion to th i s ?ni te -hor i zonopt imalcont ro lprob lem was solved via Sequent ia l Quadra t ic Program ming (SQP). Using a piecewise approx i mat ion of Q c , we showed tha t inc reas ing thep lanninghor izonN p andcont ro lhor izon N c

of the M P C cont ro l le r kept the robot away from per i phera l obs tac les . St i l l , when the robot was requi red to s ide-st ep a prominent obs tac le , the cont ro l l e r had a high probabi l i ty of be ing t rappedin loca lmin ima,caus ing thev isua l se rvo ing to ha l t .Moreover ,model ing therobot ’sworkspace toca lcu la te Q c and Q o wascu mbersome.

p u p v =C[I 3×3|03×1]c T ee T b (q)b T o o P j . p(q)j = j

1

(1) B. Model ingo fSys temCons t rain t s

Thefo l lowingsys temcons t ra in t sapplyforamanipu la tor per formingavisua l se rvo ing task:

W e wish to der ive a heur i s t i c tha t can increase the probabi l i tyofse rvo ingsuccess i nth i snon-convexprob lem. Image-basedvisua l se rvo ing(IBV S)canbeseenasonesuch heur i s t i c –one tha t i sbasedonthecur ren t lyobservedfea ture

pos i t ions , the i r approx imate depths , and rough ca mera cal i - bra t ion . Ho wever , the probab i l i ty of fa i lu re due to col l i s ion or occ lus ion s t i l l remains high wi th IB VS, espec ia l ly if an expl ic i tmodelof theworkspace i sunava i lab le .

q ∈Q c ,Q o

(2)

(3) (4) (5) (6) (7)

m i n ,q m a x ], q mi n ,q m a x q ˙min ,q ˙max R N ,

R N ,

q ∈[q ∈

∈ q ˙ ∈[q ˙min ,q ˙max ],

p u (q)j ∈[u mi n ,u m a x ],

p (q) ∈[v mi n ,v m a x ],

?j ∈[1,n ],

?j ∈[1,n ],

v j

q =q k +

δt q ˙k . k +1 2826

I I I . R APID L E A R NIN G OF T AS K S A N D C O N ST R AINTS in tospa t ia land tempora lcons t i t uents :

A. RobotTeachingbyDemons t ra t ions

t

m tq Σ Σ Σ μm = μt m μq m , Σm = m q m . (9) qt Σ W e propose an approach ca l led C M V S, which does not requ i re expl ic i t model ing of the robot ’s workspace . As in thepre requis i t e teach ings tepforvisua lse rvo ing ,weask the approach chosen by [14] [15], we per form G M R along the user toob ta inare fe rence image .I nC M V S,theuser repea t s t ime index to recons t r uc t the des i red jo in t t ra jec t ory q d (t) m 3) Gauss ian Mix ture Regress ion (G M R): Simi la r to the th i s s tepsevera l (e .g .,4to5)t imeswith theobjec ta td i f fe ren t loca t ions ,so tha twecanext rac t s t a t i s t i ca l in format ionabout

the demons t ra ted image-space and jo in t -space t ra jec tor i es . By explo i t ing the by-produc ts of th i s visua l teach ing s tep , we leverage the user ’s knowledge and sensory capabi l i t i e s

tohe lpmodel theworkspacefor t herobot .

A pai r of jo in t -space and image-space t ra jec tor ies are ex t rac ted from each de mons t ra t ion . We explo i t the t r i a l -to - t r i a l var ia t ions tha t exis t s t o gauge how close ly the robot should t rackag iven t ra jec tory ineachdo main .Forexample , whentherobot i s inc loseprox imity toworkspaceobs tac les , Werepea t theprocess toob t a in thedes i redimaget ra jec tory

weexpec t th i sse to f jo in t t ra jec tor i es tohave l i t t l evar ia t ion p d (t)andi t s t ime-dependen tcovar iancemat r ix (i .e .,robo t i s re la t ive lycons t ra ined).S imi la r ly ,as thev isua l

se rvo ing tasknearscomple t ion ,weexpec t th i sse to f image

t r a jec tor ies tohave l i t t l evar ia t ion(i .e .,t a rge tob jec t ’s appear -

andi t s t ime-dependentcovar iancemat r ix Σq (t): X M X M q d (t )= d βm (t)q (t), Σq (t )= m βm (t)Σ , (10) q

m

m =1 m =1 where

t ,Σt m ) πm N(t ,μm βm (t)=P , t i

(11) M j =1 π N(t ;μ,Σ) t j i ?1 d q (t)=μ +Σ m q

qt m t m t (12) Σ (t ?μ

). m m Σp (t).

IV. C O N T R O L LE R D ESIG N

W e modify our cos t func t i on f 0(t) (f rom Equat ion 8) in anceshouldconverge).In theabsenceofsuchcons t ra in t s ,we two aspec ts : i ) ra ther than pena l iz ing jo in t ve loc i t i es , we expec tbo th typesof t ra jec tor ies t ohavevar ia t ions tha t resu l t penal ize jo in t dev ia t ions from the des i red t ra jec t ory ; i i ) we

f romnatura lhumanincons i s tenc ies .

se t theweigh ts W q and W p tobe t ime-dependentmat r ices , W e hypothes ize tha t the dem onst ra ted t ra jec tor ies can soas tocap ture thecons t ra in t sof thesys tem(whicharenot provide impl ic i t informat ion about the ava i lab le f ree space expl ic i t lymodeled inCM V S).Le tusde ?netheC M V Scos t and vis ib i l i ty cons t ra in t s , and can be used to dete rmine the func t ionf 1(t)tobeminimizedas : appropr ia te ga ins for visua l servo ing . We bel ieve tha t th is

T W q (t) q(t)?q d (t)

f 1(t)= q(t)?q d (t) type of cont ro l le r can be used to sa t i s fy the cons t ra int s of the rea lphys ica l sys tem,a l lev ia t in

g theburdenofmodel ing the workspace nu mer ica l ly and expl ic i t ly . We wi l l examine t

h

i shypothes i s inourexper iment s .

T + p(t)?p d (t) W p (t) p(t)?p d (t) , (13) where(q d (t),p d (t))a re thedes i r ed jo in tand imaget ra jec- to r ies tha ta reob ta inedf rom G M R,and(q(t),p(t))a re the

se tofcandida te t ra jec tor ies .Theweights W q (t)and W p (t)

shouldbechosen tog iveameasureof there la t ive impor tance of the t ra jec tor ies a t each t

ime s tep . To achieve th is , we explo i tour resu l t s f rom G M Rbyse t t ing :

B. ParameterEs t imat ion 1) Canonica lT ime Warping(CT W): Weassumetha t the

t r a jec tory incons i s tenc ies fo l low a gauss ian dis t r ibu t ion in t ime and in space . We wish to ext rac t the most robus t t r a jec toryf romase tofdemons t r a t ions .Inaddi t ion ,wewish to quant i fy any spat ia l var i a t ions tha t may exis t s be tween the t ra jec tor ies .Toremovetheeffec t sof temporalvar ia t ions ,

W q (t)= Σ (t) ?1, W p (t)= Σ (t) ?1. q p (14) W e only have one se t of dec i s ion var iab les (i .e ., q), since we use CT W [13] to so lve for the bes t tempora l a l ign ment q and p are re la ted to each other through non-l inear t rans -

fo rmat ions (Equa t ion 1). W e use an i te ra t ive , loca l ly linear so lu t ion to the inversek inemat icequa t ionus ing thefo l lowing disc re teapprox imat ionwi thsam ple t ime δt ,where be tween two tra jec to r ies (v i a dyna mic program ming) w hi le

adher ing to tempora lprecedenceandcont inu i tycons t ra in t s .

2) Gauss ian Mix ture Model (GM M): Giventha tas ing le

de mons t ra t ion can cons i s t of up to severa l thousand da ta poin t s , we wish to ?nd an ef ?cien t represen ta t ion such tha t

the cont ro l le r opt imiza t ion can be executed in rea l -t ime . Here , we represen t our da t ase t us ing a mul t ivar ia te GM M of M-co mponents wi th dimens iona l i ty N +1 (for a robot

q(t)?q(t ?δt ) p(t)?p(t ?δt ) (15) (16) q ˙ ≈ , p ˙ ≈ , δ δ t t δq (t ), q d (t)?q(t ?δt ) δp (t ), p d (t)?p(t ?δt ) , . δ δ t t

Re wri t ingEquat ion13as : ! !

withN-degreesof f reedo mandthe t imeindext ):P(q(t))= T

q(t)?q(t ?δt ) q(t)?q(t ?δt )

P ··· ··· M

m =1 where i s thepr io rprobabi l - f 1(t)=δt2

δ δ π N(q(t);μ

,Σ ) π t W q (t) t m m m m q d (t)?q(t ?δ ) q d (t)?q(t ?δt ) ? t ?

i tyontheGauss ianco mponent m and N(q(t);μ ,Σm )is δ δ m t ! t ! the (N +1)-d imens iona lGauss i andens i tyofcomponent m, with μm and Σm as themeanandcovar iancemat r ix ,respec- t ive ly .Theseparametersa rees t i matedus ing theExpec ta t ion M aximiza t ion(E M)algor i thm.μm and Σm canbesepara ted

T

p(t)?p(t ?δt ) p(t)?p(t ?δt )

··· ··· +δt2 δ δ t W p (t) t . p d (t)?p(t ?δ ) p d (t)?p(t ?δt ) ? t ?

δ δ t t (17)

2827