Learning Inverse Kinematics

Transcription

1 D Souza, A., Vjayakumar, S., Schaal, S. Learnng nverse knematcs. In Proceedngs of the IEEE/RSJ Internatonal Conference on Intellgence n Robotcs and Autonomous Systems (IROS 21). Mau, HI, USA, Oct 21. Learnng Inverse Knematcs Aaron D Souza, Sethu Vjayakumar and Stefan Schaal Computer Scence and Neuroscence, HNB-13, Unv. of Southern Calforna, Los Angeles, CA Kawato Dynamc Bran Project (ERATO/JST), 2-2 Hkarda, Seka-cho, Soraku-gun, Kyoto, Japan {adsouza,sethu,sschaal}@usc.edu Abstract Real-tme control of the endeffector of a humanod robot n external coordnates requres computatonally effcent solutons of the nverse knematcs problem. In ths context, ths paper nvestgates nverse knematcs learnng for resolved moton rate control (RMRC) employng an optmzaton crteron to resolve knematc redundances. Our learnng approach s based on the key observatons that learnng an nverse of a non unquely nvertble functon can be accomplshed by augmentng the nput representaton to the nverse model and by usng a spatally localzed learnng approach. We apply ths strategy to nverse knematcs learnng and demonstrate how a recently developed statstcal learnng algorthm, Locally Weghted Projecton Regresson, allows effcent learnng of nverse knematc mappngs n an ncremental fashon even when nput spaces become rather hgh dmensonal. The resultng performance of the nverse knematcs s comparable to Legeos [9] analytcal pseudo-nverse wth optmzaton. Our results are llustrated wth a 3 degree of freedom humanod robot. 1 Introducton Most movement tasks are defned n coordnate systems that are dfferent from the actuator space n whch motor commands must be ssued. Hence, movement plannng and learnng n task space [1, 2, 11] requre approprate coordnate transformatons from task to actuator space before motor commands can be computed. We wll focus on the case where movement plans are gven as external knematc trajectores as opposed to complete task-level control laws on systems wth many redundant degrees of freedom (DOFs), as typcal n humanod robotcs (Fgure 1). The transformaton from knematc plans n external coordnates to nternal coordnates s the classc nverse knematcs problem, a problem that arses from the fact that nverse transformatons are often ll-posed. If we defne the ntrnsc coordnates of a manpulator as the n-dmensonal vector of jont angles θ R n, and the poston and orentaton of the manpulator s end effector as the m-dmensonal vector x R m, the forward knematc functon can generally be wrtten as: x = f(θ) (1) Fgure 1: Humanod robot n our laboratory. whle what we need s the nverse relatonshp: θ = f 1 (x) (2) For redundant manpulators,.e., n > m, solutons to Eq. (2) are usually non-unque (excludng the degenerate case where no solutons exst at all), and even for n = m multple solutons can exst (e.g., [6]). Therefore, nverse knematcs algorthms need to address how to determne a partcular soluton to (2) n face of multple solutons. Heurstc methods have been suggested, such as freezng DOFs to elmnate redundancy. However, redundant DOFs are not necessarly dsadvantageous as they can be used to optmze addtonal constrants, e.g., manpulablty, force constrants, etc. Thus t s useful to solve the nverse problem (2) by mposng an optmzaton crteron: g = g(θ) (3) where g s usually a convex functon that has a unque global optmum. There are two generc approaches to solvng nverse knematcs problems wth optmzaton crtera ([4]). Global methods fnd an optmal path of θ wth respect to the entre trajectory, usually n computatonally expensve off-lne calculatons. In contrast, local methods, whch are feasble n real tme, only compute an optmal change n θ, θ, for a small change n x, x and then ntegrate θ to generate the entre jont space path. Resolved Moton Rate Control (RMRC) ([15]) s one such local method. It uses the Jacoban J of the forward knematcs to descrbe a change of the endeffector s poston as ẋ = J (θ) θ (4) Ths equaton can be solved for θ by takng the nverse of J f t s square.e. m = n, and non-sngular. For a

2 redundant manpulator n s greater than m, e.g., n = 26 and m = 3 for our humanod reachng for an object (neglectng the 4 DOFs for the eyes), whch necesstates the use of addtonal constrants, e.g., the optmzaton crteron g n Eq. (3), to obtan a unque nverse. For nstance, Legeos [9] suggested a pseudo-nverse soluton by mnmzng g n the null space of J: θ = J # ẋ α ( I J # J ) g θ (5) whch, for certan cost functons g, s a specal case of Balleul s extended Jacoban method [3, 13] whch has the general form θ = J 1 E ẋaug (6) where ẋ aug s an augmented nput vector [3]. The goal of our research s to accomplsh solutons to RMRC nverse knematcs wth statstcal learnng approaches that approxmate (5) and (6). In the next sectons, we wll frst dscuss the problems of nverse knematcs learnng and how they can be overcome. Afterwards, we brefly descrbe a learnng algorthm that we developed that s deally suted for the nverse knematcs learnng. In the last secton wll provde evaluatons of nverse knematcs learnng algorthm wth a humanod robot. 2 Learnng Inverse Knematcs Learnng of nverse knematcs s useful when the knematc model of a robot s not accurately avalable, when Cartesan nformaton s provded n uncalbrated camera coordnates, or when the computatonal complexty of analytcal solutons becomes too hgh. Learnng methods are nherently self-calbratng, preventng an accumulaton of errors from analytcal nverse knematcs computatons due to sensor offsets or naccurate knowledge of the robot knematcs. An addtonal appealng feature of learnng nverse knematcs s that t avods problems due to knematc sngulartes learnng works out of experenced data, and such data s always physcally correct and wll not demand mpossble postures as can result from an ll-condtoned matrx nverson. The major obstacle n learnng nverse knematcs les n the problem that the nverse knematcs of a redundant knematc chan has nfntely many solutons. Thus, the learnng algorthm has to acqure a partcular nverse, and moreover, has to make sure that the nverse s actually a vald soluton. Ths latter ssue was characterzed n Jordan and Rumelhart [8] as the problem of non-convex mappngs. In the context of Eq. (4), the forward knematcs of a redundant system maps multple θ to the same ẋ. When learnng an nverse mappng ẋ θ, learnng algorthms average over all the solutons θ, assumng that dfferent θ for the same ẋ are due to nose. Thus, for ẋ θ to be a vald nverse, t s requred that all θ encountered durng tranng form a convex set-otherwse the average θ could become an nvald soluton to the nverse problem. Unfortunately, as shown n Jordan and Rumelhart [8], nverse knematcs has the non-convexty property and therefore, does not permt drect learnng of the nverse mappng. As noted by Bullock et al. [5], t s possble to transform the non-convex problem of nverse knematcs learnng nto a convex problem by spatally localzng the learnng task: wthn the vcnty of a partcular θ, nverse knematcs s actually convex. Ths can be proven easly by averagng equaton (4) for multple θ that map to the same ẋ: ẋ = J (θ) θ ẋ = J (θ) θ = J (θ) θ (7) Eq. (7) smply demonstrates that a local average θ over θ wthn the vcnty of a partcular θ wll stll result n a vald soluton to the nverse knematcs problem. Thus, nverse knematcs learnng for a redundant system can theoretcally be accomplshed properly by learnng a mappng (ẋ, θ) θ f a spatally localzed learnng algorthm s employed. 3 Locally Weghted Projecton Regresson Locally weghted projecton regresson (LWPR) [14] s a supervsed learnng algorthm that s well suted for learnng the nverses knematcs mappng (ẋ, θ) θ. The key concept of LWPR s to approxmate nonlnear functons by means of pecewse lnear models. The regon of valdty, called a receptve feld, of each lnear model s computed from a Gaussan functon: ( w k = exp 1 ) 2 (x c k) T D k (x c k ) (8) where c k s the center of the ckth lnear model, and D k corresponds to a dstance metrc that determnes the sze and shape of regon of valdty of the lnear model. Gven an nput vector x, each lnear model calculates a predcton y k. The total output of the network ( s the weghted mean of all lnear models K ) /( ŷ = k=1 w K ) ky k k=1 w k. In order to avod numercal problems due to matrx nversons and to mnmze the computatonal complexty, the lnear models n each receptve feld are not computed by lnear regresson but rather by applyng a sequence of one-dmensonal regressons along selected projectons u r n nput space (note that we wll drop the ndex k from now on unless t s necessary to dstngush explctly between dfferent lnear models): Intalze: y = β, z = x x For = 1 : r s = u T z; z z p s y = y + β s (9)

3 In order to determne the open parameters n Eq. (9), the technque of partal least squares (PLS) regresson can be adapted from the statstcs lterature [16]. The mportant ngredent of PLS s to choose projectons accordng to the correlaton of the nput data wth the output data. The followng algorthm, Locally Weghted Projecton Regresson (LWPR), uses an ncremental locally weghted verson of PLS to determne the lnear model parameters: Gven: A tranng pont (x, y) Update means of nputs and outputs: x n+1 = λw n x n + wx W n+1 β n+1 = λw n β n+1 + wy W n+1 where W n+1 = λw n + w Update the local model: Intalze: z = x, res = y β n+1 For = 1 : r, a) u n+1 b) s = z T u n+1 = λu n + wz res c) SS n+1 = λss n + ws 2 d) SR n+1 = λsr n + ws res e) SZ n+1 = λsz n + wzs f) β n+1 = SR n+1 /SS n+1 g) p n+1 = SZ n+1 /SS n+1 h) z z sp n+1 ) res res sβ n+1 j) MSE n+1 = λmse n + w res 2 (1) In the above equatons, λ [, 1] s a forgettng factor that determnes how much older data n the regresson parameters wll be forgotten, smlar as n recursve system dentfcaton technques [1]. The varables SS, SR, and SZ are memory terms that enable us to do the unvarate regresson n step f) n a recursve least squares fashon,.e., a fast Newton-lke method. Step g) regresses the projecton p from the current projected data s and the current nput data z. Ths step guarantees that the next projecton of the nput data for the next unvarate regresson wll result n a u +1 that s orthogonal to u. The above update rules can be embedded n an ncremental learnng system that automatcally allocates new locally lnear models as needed [12]: Intalze the LWPR wth no receptve feld (RF); For every new tranng sample (x, y): For k=1 to #RF: calculate the actvaton from (8) update accordng to (1) end; If no lnear model s actvated more than w gen; create a new RF wth r = 2, c = x, D = D def end; end; In ths pseudo-code algorthm, w gen s a threshold that determnes when to create a new receptve feld, and D def s the ntal (usually dagonal) dstance metrc n Eq. (8). The ntal number of projectons s set to r = 2. The algorthm has a smple mechansm of determnng whether r should be ncreased by recursvely keepng track of the mean-squared error (MSE) as a functon of the number of projectons ncluded n a local model,.e., Step j) n (1). If the MSE at the next projecton does not decrease more than a certan percentage of the prevous MSE,.e. MSE +1 /MSE > φ, where φ [, 1], the algorthm wll stop addng new projectons to the local model. As shown n [12], t s even possble to learn the correct parameters for the dstance metrc D n each local model based on an ncremental cross valdaton technque. Ths algorthm s drectly applcable to LWPR, and s strongly smplfed, as t only needs to be done n the context of unvarate regressons. Due to space lmtatons, we wll not provde the update rules n ths paper as they can be derved from [12]. 3.1 Inverse Knematcs Learnng wth LWPR By usng spatally localzed receptve felds, LWPR has all the prerequstes to learn nverse knematcs. The nputs to the learnng system are z = (ẋ, θ), and the outputs are y = θ. ẋ can be n Cartesan coordnates f a calbrated 3D trackng system for the endeffector exsts, but t could also be n uncalbrated mage coordnates of two or more cameras snce LWPR can handle redundant nputs there s no restrcton on the dmensonalty of ẋ. For our humanod robot, the dmensonalty of the nput z s 29 (26 DOFs neglectng the 4 DOFs for the eyes, plus 3 Cartesan nputs), whle the dmensonalty of the output y s 26. By movng the robot whle readng values for z and y from the sensors, tranng data s generated that can be added ncrementally to the learnng system ths process s often termed self-supervsed learnng Creatng a cost functon. In the ntroducton we mentoned that the resoluton of redundancy requres creatng an optmzaton crteron that allows the system to choose a partcular soluton to the nverse knematcs problem. Gven that our robot s a humanod robot, we would lke the system to assume a posture that s as natural as possble. Our defnton of natural corresponds to the posture beng as close as possble to some default posture θ opt, as advocated by behavoral studes [7]. Addtonally, each DOF s gven a weght, whch determnes the extent of ts contrbuton to the cost functon. Hence the total cost

4 functon for tranng LWPR can be wrtten as follows: Q = 1 ( ˆ θ) T ( ) θ θ ˆ θ (ˆ θ 2 α θ t ) T W ) θ (ˆ θ t (11) where θ = θ opt θ represents the dstance of the current posture from the optmal posture θ opt, W s a dagonal weght matrx, and ˆ θ s the current predcton of LWPR for z = (ẋ, θ). Mnmzng Q can be acheved by presentng LWPR wth the target values: θ target = θ ) αw (ˆ θ θ (12) These targets are composed of the self-supervsed target θ, slghtly modfed by a component to enforce the null space optmzaton crteron. Note that the null space optmzaton wll sacrfce some performance n trackng accuracy to accomplsh the desred null space moton towards the optmal posture θ opt Learnng on the task. Our emphass n ths paper s towards learnng the nverse knematcs on the fly,.e., whle attemptng to perform the task tself. The problem however, s that ntally, LWPR has very lttle (or no) data upon whch to base ts regresson. We stll however, requre a command to be sent to create an output moton of the robot. As an exploraton strategy, we ntally bas the output of LWPR wth a term that creates a moton towards θ opt : θ = ˆ θ 1 + θ (13) n r The strength of the bas decays wth the number of data ponts n r seen by the largest contrbutng local model of LWPR. Ths addtonal term allows creatng meanngful (and mportantly, data-generatng) moton even n regons of the jont space that have not yet been explored. LWPR learns extremely quckly from even very sparse data. Ths can result n jerky and naccurate movement durng the ntal stages of exploraton and learnng. In order to ensure smoother trajectores durng the learnng process, we ntalze the SS varable of each local model n Eq. (1)c wth a value of 1 1. By nspectng (1)c, t can be seen that ths bas causes the regresson coeffcents to have very small values ntally, whch results n very slow movement of the robot. Whle makng these slow movements however, data s contnuously added to the LWPR algorthm, and eventually the ntal bas s overcome due to the forgettng factor λ, whch effectvely bases the statstcs computed n Eq. (1) on the last (1 λ) 1 data ponts. As the system acqures more data, t gradually ncreases ts trust n ts own approxmaton to the nverse knematcs, eventually allowng the full strength of the regresson to command the output Localzaton space vs. regresson space. An mportant aspect of our formulaton of the nverse knematcs problem s that although the nputs to the learnng problem comprse ẋ and θ, the localty of the local model s a functon of only θ, whle the lnear projecton drectons (gven ths localty n θ) are solely dependent on ẋ. We encode ths pror knowledge nto LWPR s learnng process by settng the ntal values of the dagonal terms of the dstance metrc D n Eq. (8) that correspond to the ẋ varables to zero. Ths bas ensures that the localty of the receptve felds n the model s solely based on θ. LWPR has the ablty to determne and gnore nputs that are locally rrelevant to the regresson, but we also provde ths nformaton by normalzng the nput dmensons such that the varance n the relevant dmensons (n ths case the dmensons correspondng to ẋ) s large. We use ths feature to create larger correlatons of the relevant nputs wth the output varables and hence bas the projecton drectons wthn each local model towards the relevant subspace. 4 Expermental Evaluatons In the followng experments, we use a smple Cartesan controller to generate the desred acceleratons n Cartesan space for trackng a target x t. Gven the poston, velocty and acceleraton nformaton of the target, the control law s: ẍ = ẍ t + k v (ẋ t ẋ) + k p (x t x) (14) where k p = 125, and k v = 7. Ths desred acceleraton s numercally ntegrated to obtan a desred Cartesan velocty: ẋ n+1 = ẋ n + ẍ t (15) where t = 1/42 n our experments. It s ths value of ẋ that we use as the Cartesan space nput to our algorthm to generate θ f 1 ( ẋ, θ), whch s then ntegrated and dfferentated to obtan θ and θ respectvely, as nputs to the nverse dynamcs controller. Tranng data, on the other hand, s created from Eq. (12). 4.1 Experments The goal task n each of the experments was to track a fgure-eght trajectory n Cartesan space created by smulated vsual nput to the robot. In each of the fgures n ths secton, the performance of the system s plotted along wth that of an analytcal pseudo-nverse (c.f. Eq. (5)) that was avalable for our robot from prevous work [13].

5 z z Learnng from motor babblng. We frst traned the system Analytcal Motor-Babblng on data collected from.2 motor babblng. We.1 created small snusodal motons of each DOF about a randomly chosen -.1 mean n θ space. Every few seconds, ths mean s repostoned wthn x the workspace. After Fgure 2: System performance after beng manner for approxmately tranng the system n ths traned on data collected 1 mnutes, we tested ts from motor babblng. performance on the fgureeght task. The trajectory followed by the system s shown n Fg. 2. The trackng naccuraces seen n the fgure are not surprsng, snce gven the hgh dmensonalty of the jont space, the data obtaned from motor babblng s sparse n the regon requred by the fgure-eght task. Thus, LWPR s predctons are based on too few ponts to acheve hgh accuracy Improvng performance on the task Analytcal Task-Specfc x Fgure 3: System performance after 1 mnute of executng the fgure-eght task, wth learnng on the task enabled. In order to demonstrate that more task-specfc tranng data leads to better nverse knematcs learnng, our second experment traned the nverse knematcs on the task. The robot executed the fgure-eght agan, usng the traned LWPR from the frst experment. In ths case however, the system was allowed to mprove tself wth the data collected whle performng the task. As shown n Fg. 3, after merely 1 mnute of addtonal learnng, the system performs as accurately as the analytcal soluton Learnng from scratch on the task. The fnal experment started wth an untraned system, and learned the nverse knematcs from scratch, whle performng the fgure-eght task tself. Fg. 4 shows the progresson of the system s performance from the begnnng of the task to about 3 mnutes nto the learnng. One can see that the system ntally starts out makng slow naccurate movements. As t collects data, however, t rapdly converges towards the desred trajectory. Wthn a few more mnutes of tranng on the task, the performance approached that seen n Fgure 3. z Analytcal Onlne Learnng x Fgure 4: Trajectory followed n the frst 3 mnutes when learnng the nverse knematcs from scratch whle attemptng to perform the fgure-eght task. 4.2 Consstency of the learned nverse knematcs For redundant manpulators, followng a perodc trajectory n operatonal space does not mply consstency n jont space,.e., the trajectory followed n jont space may not be cyclc snce there could be aperodc null space moton that does not affect the accuracy of the tracked trajectory n operatonal space. Fgures 5(a), 5(b), and 5(c) show phase plots of three DOFs shoulder flexon and extenson (SFE), humeral rotaton (HR), and elbow (EB) flexon and extenson respectvely plotted over about 3 cycles of the fgure-eght trajectory after learnng had converged. The presence of a sngle loop for the phase plot over all cycles n each case shows that the nverse knematcs soluton found by our algorthm s ndeed consstent. Comparng the analytcal soluton wth LWPR s soluton n the above fgures, t s clear that we learn an nverse knematcs soluton that s qualtatvely smlar to that obtaned by an analytcal pseudo-nverse. The quanttatve dscrepances n the two solutons are due to an mperfect approxmaton of the null space, whch s a result of enforcng the null space optmzaton only mplctly n the cost functon (Eq. (11)). 5 Dscusson Ths paper presented how nverse knematcs for redundant manpulators can be learned wth modern statstcal learnng algorthms. The key element of our approach was to augment the nput space to the learnng system such that averagng over redundant solutons of the nverse mappng could be done safely wthout creatng physcally mpossble results. Usng a specfc optmzaton crteron for tranng the learnng system, performance comparable to Legeos analytcal pseudonverse could be accomplshed [9]. We demonstrated the functonalty of our learnng methods on a full body humanod robot learnng to trace a fgure-eght

6 .4 Analytcal (SFE) Learned (SFE) 1.1 Analytcal (HR) Learned (HR) References jont velocty jont poston (a) Shoulder flexon and extenson (SFE). jont velocty Analytcal (EB) jont velocty jont poston jont poston (b) Humeral rotaton (HR). Learned (EB) (c) Elbow (EB) flexon and extenson. Fgure 5: Phase plots n Cartesan space after only a few mnutes of tranng. Despte these encouragng results, we need to address a varety of ssues n future work. Most mportantly, our suggested algorthm only fnds an approxmate soluton to optmzng the null space moton of the robot due to a cost functon that causes a slght amount of nterference between task goals and null space optmzaton. We noted that under unfortunate tranng data dstrbutons, ths nterference can cause a slght amount of unwanted movement n unconstraned endeffectors of our humanod, e.g., the left hand moved whle only the rght hand was supposed to track a target. Addtonally, t s not favorable to hard code the optmzaton crteron for the null space moton n the learnng system, as s currently the case n our approach. Dfferent tasks may favor dfferent optmzaton crtera for for the resoluton of redundancy, and the learnng system should be flexble enough to accommodate ths requrement. Our future work wll address learnng algorthms that learn the null space and range space of the local nverse knematcs mappng explctly n order to allow for such flexblty. 6 Acknowledgements Ths work was made possble by Award # of the Natonal Scence Foundaton, the ERATO Kawato Dynamc Bran Project funded by the Japanese Scence and Technology Cooperaton, and the ATR Human Informaton Processng Research Laboratores. [1] E. W. Aboaf, C. G. Atkeson, and D. J. Renkensmeyer. Task-level robot learnng. In Proceedngs of the IEEE Internatonal Conference on Robotcs and Automaton, Phladelpha, PA, [2] E. W. Aboaf, S. M. Drucker, and C. G. Atkeson. Tasklevel robot learnng: Jugglng a tenns ball more accurately. In Proceedngs of the IEEE Internatonal Conference on Robotcs and Automaton, Scottsdale, AZ, [3] J. Balleul. Knematc programmng alternatves for redundant manpulators. In Proceedngs of the IEEE Internatonal Conference on Robotcs and Automaton, pages , [4] J. Balleul and D. P. Martn. Resoluton of knematc redundancy. In Proceedngs of Symposa n Appled Mathematcs, volume 41, pages Amercan Mathematcal Socety, 199. [5] D. Bullock, S. Grossberg, and F. H. Guenther. A selforganzng neural model of motor equvalent reachng and tool use by a multjont arm. Journal of Cogntve Neuroscence, 5(4):48 435, [6] J. J. Crag. Introducton to Robotcs. Addson-Wesley, Readng, MA, [7] H. Cruse and M. Brüwer. The human arm as a redundant manpulator: The control of path and jont angles. Bologcal Cybernetcs, 57: , [8] M. I. Jordan and Rumelhart. Supervsed learnng wth a dstal teacher. Cogntve Scence, 16:37 354, [9] A. Legeos. Automatc supervsory control of the confguraton and behavor of multbody mechnsms. IEEE Transactons on Systems, Man, and Cybernetcs, 7(12): , [1] L. Ljung and T. Söderström. Theory and practce of recursve dentfcaton. Cambrdge MIT Press, [11] E. Saltzman and S. J. A. Kelso. Sklled actons: A taskdynamc approach. Psychologcal Revew, 94(1):84 16, [12] S. Schaal and C. G. Atkeson. Constructve ncremental learnng from only local nformaton. Neural Computaton, 1(8): , [13] G. Tevata and S. Schaal. Inverse knematcs for humanod robots. In Proceedngs of the Internatonal Conference on Robotcs and Automaton (ICRA2), San Francsco, CA, Apr. 2. [14] S. Vjayakumar and S. Schaal. Locally weghted projecton regresson: An O(n) algorthm for ncremental real tme learnng n hgh dmensonal spaces. In Proceedngs of the Seventeenth Internatonal Conference on Machne Learnng (ICML 2), Stanford, CA, 2. [15] D. E. Whtney. Resolved moton rate control of manpulators and human prostheses. IEEE Transactons on Man-Machne Systems, 1(2):47 53, [16] H. Wold. Soft modelng by latent varables: The nonlnear teratve partal least squares approach. In J. Gan, edtor, Perspectves n Probablty and Statstcs, Papers n Honour of M. S. Bartlett, pages Academc Press, London, 1975.