Mathematical Model for Motion Control of Redundant Robot Arms

Transcription

1 Mathematcal Model for Moton Control of Redundant Robot Arms EVGENIY KRASTEV Department of Mathematcs and Informatcs Sofa Unversty St. Klment Ohrdsky 5, James Boucher blvd., 1164 Sofa BULGARIA Abstract: - Moton plannng s an essental actvty n executng manpulaton tasks wth robot arms lke weldng, pantng or smple pck-and-place operatons. A typcal requrement for such tasks s to acheve flexblty of the robot arm moton smlar to that of a human hand. The objectve of the here proposed mathematcal model s to satsfy ths requrement n path plannng a robot arm moton wth redundant degrees of freedom. Ths model provdes an effcent procedure for the computaton of the moton n the jonts that makes the endeffector moton to trace a gven geometrcal curve wth prescrbed lnear and angular velocty. The novelty of the mathematcal model s the clear separaton of concerns related to plannng the geometrcal path n task space on the one hand and on the other hand, the control of moton along ths path. Ths gudng desgn prncple s mplemented through vector space methods. It enables a new nsght about dentfyng and processng Jacoban sngulartes by means of the Contnuty prncple. The applcaton of the model s enabled through a detaled algorthm for computng the Jacoban, equatons for knematc path control and a heurstc procedure for handlng the sngulartes of the Jacoban. The obtaned results can be extended to non- redundant robot arms. Key-Words: - robot moton, knematcs, separaton of concerns, redundant robot arm, contnuty prncple 1 Introducton Robot arms cover a broad range of applcatons. The lst of applcatons s constantly growng and t ranges from assembly operatons n outer space and ndustry to executng hghly specalzed actvtes n surgery [1, 2], for nstance. In most cases the robot arm operates as a replacement n an actvty usually done by means of a human hand. Research n bomechancs and boncs motvates engneers to provde more degrees of freedom than the dmenson of the task space n the desgn of modern robot arms. Such robot arms are referred to as redundant robot arm. A robot arm s useful n executng repettve work tasks. Therefore t s common to preplan and control the moton of ts end effector along a path n the task space. The dmenson of the task space depends on the specfc task. It s represented n terms of a set of parameters specfyng the desred poston and orentaton of the end- effector along the specfed path. The soluton of the nverse knematc problem allows to fnd the moton n the jonts that results n a desred moton of the end- effector. Ths paper presents a Jacoban based model for path plannng the moton of robot arm wth redundant degrees of freedom. It s common for ths knd of models to consder moton control mplctly coupled wth geometrcal characterstcs of moton. Such desgn approach ncreases the complexty of the model and requres powerful numercal methods and optmzaton technques, typcally, Sngular Value Decomposton or Damped Square Methods [3]. Most often the procedures for the computaton of the Jacoban are vald only for a selected robot arm and rely on the symbolc representaton of ths robot arm model. Heavy numercal methods are employed to dentfy and avod robot arm confguratons, where the rank of the Jacoban s not full [4]. The novelty of the here proposed model s the clear separaton of concerns related to plannng the geometrcal path n task space on the one hand and on the other hand, the control of moton along ths path [5]. Ths reduces the usual complexty of the Jacoban- based models and allows ts mplementaton n modern computatonal envronments wth multthreadng. The here proposed model provdes computatonally effcent procedures for computng the Jacoban and resolvng the sngulartes n moton plannng makng use of vector space methods. The paper s organzed as follows. The Problem statement and the major mathematcal notatons are ntroduced n Secton 2. Secton 3 presents a computatonally effcent algorthm for computng the Ja- E-ISSN: Volume 16, 2017

2 coban of a robot arm. The major results n ths paper are provded n Sectons 4 and 5. In the last secton we summarze the obtaned results. 2 Problem Formulaton The robot arm s assembled by n lnks connected serally wth revolute or prsmatc jonts n an open loop knematc scheme (Fg. 1). The last lnk s the end- effector. Usually t s of type grpper holdng a tool that s selected n accordance wth a work task. The work task s assgned wth respect to the base frame O o (x o, y o, z o ) n terms of geometrcal and moton parameters. The geometrcal parameters comprse the orentaton of the tool n the end- effector and the poston of a characterstc pont H of the tool along a geometrcal curve. The moton parameters determne the lnear speed of a characterstc pont H of the tool and the angular speed of rotaton of the tool n each pont of the geometrcal curve. The dmenson of the task space s determned by the number m (m 6) of geometrcal parameters used to descrbe the poston and the orentaton durng the executon of the work task. Fg.1 Moton plannng wth a robot arm. Denote by Int Q M the nteror of the set Q M = { q = (q 1,..., q n ): q [a, b ] for = 1, 2, 3,.., n} of confguratons of the jont varables. The mappng F Int Q M W defnes the forward knematcs of a robot arm wth workspace W R m. Note, that the workspace s a subset of the task space R m. For clarty, let the geometrcal parameters of the work task be represented n W by a smooth geometrcal curve γ, defned parametrcally n the nterval [ λ 1, λ 2 ]. Wthout loss of generalty we assume that the ponts of γ belong to the workspace W of the robot arm or n other words, γ [λ 1, λ 2 ] F(Int Q M ). The moton parameters of the work task are gven wth respect to the ponts of γ. The objectve s to fnd laws governng the moton n the jonts such that the end- effector moton satsfes the geometrcal and moton parameters of the work task. In the begnnng we note the exstng clear dstncton between geometrcal and moton parameters n task space. Obvously, the moton parameters can change wthout the need of changng γ. Once γ s gven, the end- effector can execute a varety of dfferent motons along ths path. These two groups of parameters reflect two dfferent concerns n executng the work task. It s natural to retan ths knd of separaton of concerns n the space of confguratons Int Q M of the robot arm. Then t would be possble to control the moton n the jonts n a way smlar to controllng moton over a geometrc curve. For ths reason, we add a n + 1 dmenson to the space of confguratons, where the parameter q n+1 represents parametrcally γ = γ(q n+1 ), q n+1 [λ 1, λ 2 ]. Defnton. The set Q = Int Q M x R we refer to as extended space of confguratons The ntroducton of Q provdes a new nterpretaton for the nverse knematc equaton. Consder the smooth manfold B = { q = (q, q n+1 ): F(q) γ(q n+1 ) = 0, q Q } (1) defned by means of the nverse knematcs equaton. It s n m + 1 dmensonal n the general case. Hence, all the motons q (t) = (q(t), q n+1 (t)), t [t 1, t 2 ] of the robot arm that satsfy the assgned geometrcal parameters of the work task, belong to B. Among these motons are also motons that satsfy the moton parameters of the assgned work task. Defnton. A smooth moton q (t) B that satsfes gven n advance moton parameters of the endeffector moton we refer to as admssble moton of the robot arm. It s noteworthy, that the ntroducton of the extended space of confguratons enables the separaton of concerns n the nverse knematc problem soluton. Smlarly to the curve γ, the manfold B s a fxed geometrcal structure. It s determned by the geometrcal structure of the robot arm and the assgned geometrcal parameters of the work task. On the other hand, the trajectory of the requred robot moton n Q les on B for any gven values of the moton parameters of the work task. Ths representaton allows to control the robot moton over B n a smlar way we control the moton over γ n task space. E-ISSN: Volume 16, 2017

3 The proposed mathematcal model allows to resolve the nverse knematc problem nto two separate types of concerns. The frst type of concerns nvolves modellng the mmutable geometrcal artefacts n moton control. The second part of the model deals wth constructs for moton control. In partcular, the separaton of concerns guarantees that cyclc motons n task space map cyclc motons n the extended space of confguratons. In the followng secton we present a unform model of the robot arm geometrcal structure and develop a detaled numercal algorthm for computng the Jacoban on ths bass. Next, we make use of the here ntroduced separaton of concerns to buld a model for moton control of redundant robot arms subject to gven moton parameters. The thus obtaned results allow us to address from a new pont of vew the mportant ssue of sngulartes n the space of confguratons. In contrast to exstng approaches, ths mathematcal model allows to nterpret the sngularty problem n terms of the Contnuty prncple [6]. As a result we propose an effcent heurstc procedure for dentfyng and passng through sngulartes of the Jacoban durng moton control wth prescrbed geometrcal and moton parameters of the work task. Ths procedure reles on. J(q)q γ q (q n+1 ) q n+1 = 0 (2) n+1 where J(q) = D(F)/D(q) s the Jacoban matrx of the forward knematcs F(q) and q n+1 s the tme dervatve of q n+1 (t). It s common to present the computaton of the Jacoban employng the symbolc representaton of the forward knematcs F(q). The dervaton of the symbolc formulas for F(q) and dfferentaton of the thus obtaned expressons s tme-consumng n most cases and t s justfed n only n some specal cases of robot arm nvestgaton. Here we wll develop an algorthm for computng the Jacoban wthout the knowledge for the forward knematcs F(q) n symbolc form. The nvestgaton of a robot arm starts by attachng a frame O (x, y, z ), = 1,2,, n to each one of the n lnks, where the coordnate axes are fxed accordng to the Denavt- Hartenberg (DH) rules [7]. The mutual dsposton of the local frames s defned by a set of fxed geometrcal parameters and the values of the jont varables q. Ths set of fxed geometrcal parameters we defne n a slghtly dfferent way than the one used by the DH notaton. It wll enable us to ntroduce a common representaton for the dsplacements n the jonts [8]. Frst we note that axes z and z +1 are fxed n lnks and + 1, respectvely (Fg. 2). The fxed geometrcal parameters that defne the mutual dsposton between frames and + 1 for = 1,2,, n 1 are as follows: - the dstance l between axes z and z +1 measured along the axs x Wthout loss of generalty we assume l n = 0. Corresponds to lnk length a n the DH notaton. - the dstance d +1 between ponts O,+1 and O +1 measured along the axs z +1. Wthout loss of generalty we assume d n = 0. Corresponds to lnk twst d +2 n the DH notaton. - the angle α +1 between axes z +1 and z measured n the postve drecton of axs x.. Corresponds to lnk twst α n the DH notaton. - the angle β +1 between axes x +1 and x measured n the postve drecton of axs z +1. In case jont + 1 s revolute, then angle β +1 concdes wth the jont varable q +1. Corresponds to lnk angle θ +2 n the DH notaton. 3 Computaton of the Jacoban The followng equaton represents the Jacobanbased model for solvng the nverse knematc problem n the extended space of confguratons: Fg.2 Structure of the local frames n the lnks. Then the transformaton matrx from frame nto frame 1 can be represented n terms of these notatons as follows. cosβ snβ 0 G, 1 = snβ cosα cosβ cosα snα (3) snβ snα cosβ snα cosα Accordngly, the matrx product T = G 1,0 G 2,1 G, 1 (4) defnes the transformaton of the unt vectors e 1, e 2, e 3 from frame O (x, y, z ), = 1,2,, n nto the base frame O o (x o, y o, z o ). The columns of matrx T are the coordnates of the unt vectors e 1, e 2, e 3 of O (x, y, z ) n frame O o (x o, y o, z o ). It allows us to express the rotatons and the transla- E-ISSN: Volume 16, 2017

4 tons that occur along the z axs n case of revolute and prsmatc jont n terms of a common notaton. Let μ, = 1,2,, n takes values 0 or 1 for revolute and prsmatc jonts, correspondngly. Then dependng on μ the followng expresson z = [(1 μ )d + μ q ]e 3 (5) determnes the dstance between ponts O,+1 and O +1 n case of a revolute and n case of prsmatc jont, as well. Smlarly, we represent the vector of the angular velocty ω n jont = 1,2,, n as follows: ω = (1 μ )q e 3 (6) where the scalar functon q denotes a generalzed jont velocty = 1,2,, n. Denote by v and ω the gven moton parameters of the work task moton defned along the path γ. Here v s the velocty vector of the characterstc pont H of the end- effector and ω s the angular velocty vector of the tool n the end effector. Usng the notatons on Fgure 1 we obtan the followng representaton for v v = ω ρ + r o (7) For shortness ρ denotes O n H and r o s the velocty vector of pont O n. Here we note that r o s the sum of the veloctes n the prsmatc jonts along ther unt vector e 3. Hence, from (5) we obtan: n r o = =1 μ q e 3 (8) Smlarly, vector ω s the vector sum of the angular veloctes ω (6): n ω = =1 (1 μ )q e 3 (9) Replacng (8) and (9) n (7) yelds the followng general expresson for the vector v n v = [(1 μ )e =1 3 ρ + μ e 3 ] q (10) Consder the general case when the task space has the maxmum dmenson of 6 (three parameters for poston and three parameters for orentaton). Then γ qn+1 (q n+1 ) q n+1 = (v, ω) T and from (2), (9) and (10) t follows that column = 1, 2, 3,.., n of the Jacoban can be represented by the followng vector row [((1 μ )e 3 ρ + μ e 3 ) T, ((1 μ )e 3 ) T ] (11) Note, that vectors e 3 and ρ must be n the same frame n order to compute e 3 ρ, = 1, 2, 3,.., n. Therefore, the coordnates of vector ρ must transformed from O n (x n, y n, z n ) down to frame O (x, y, z ), before computng the vector product e 3 ρ. The followng pseudocode explans n detals how to compute the Jacoban makng use of the expresson (11). For clarty of explanaton the pseudocode sequentally computes the rows J = 1, 2, 3,.., n of the transposed Jacoban, startng from row n. S1. Compute the transformaton matrxes T, = 1, 2, 3,.., n defned n (4). S2. Compute vector h = T n ρ, where ρ s defned n (7). S3. Set a counter = n. S4. Compute the sx coordnates of vector jcbrow = [ ((1 μ )T,3 h + μ T,3 ) T, ((1 μ )T,3 ) T ] where T,3 s the thrd column of matrx T. S5. Store jcbrow as row of the transpose of the Jacoban. S6. Set = 1. S7. If s equal to zero go to Step S11. Else contnue wth step 8. S8. Compute the scalar z z = [(1 μ +1 )d +1 + μ +1 q +1 ] S9. Compute vector h = h + l T,1 + z T +1,3, where T,1 s the frst column of matrx T and l s the dstance parameter. S10. Go to step S4. S11. End of algorthm. Ths algorthm s applcable for both redundant and non redundant robot arms (n = m). It can be easly adapted for computng the Jacoban n cases when the work task s defned n terms of a subset of the coordnates of the moton parameters v and ω. 4 Moton Control The separaton of concerns ntroduced n the problem statement enables the computaton of an admssble moton matchng gven moton crtera. The defnton (1) of the smooth manfold B mples that the vector q = (q T q n+1) T belongs to the tangent vector space at an arbtrarly selected pont q = (q, q n+1 ) of B. Furthermore, an arbtrary vector u = (u T, u n+1 ) T that satsfes J(q)u γ q (q n+1 )u n+1 = 0 (12) n+1 also belongs to the tangent vector space at pont q = (q, q n+1 ) of B. Ths way we can solve the nverse knematc problem by controllng the tangent vector feld over B wth respect to one or another qualty crteron. The manfold B and the functon controllng the vector feld reflect the geometry concern and the moton concern, respectvely. E-ISSN: Volume 16, 2017

5 Equaton (12) allows to defne explctly the control functon for the vector feld of B. For a fxed value of u n+1 the solutons u of ths equaton represent a lnear manfold L un+1 = u (0) + ℵ(J) (13) where u (0) s an arbtrary partcular soluton of (12) and ℵ(J) s the lnear subspace ℵ(J) = {x R n : Jx = 0} defned at any pont of the smooth manfold B qn+1 = {q Int Q M : F(q) γ(q n+1 ) = 0} (14) The manfold B qn+1 (Fg. 3) conssts of all the confguratons q of the robot arm for whch the end effector poston retans poston γ(q n+1 ), q n+1 [ λ 1, λ 2 ]. Therefore B qn+1 s known also n the lterature as Null- space. Ths s a specal feature of redundant robot arms (n > m). In partcular, ℵ(J) contans just the zero vector n case of nonredundant robot arms (n = m). Equaton (16) represents an arbtrary soluton of the nverse knematc problem for redundant robot arms (12). Hence, the vector q T = (q q n+1) T n equaton (12) can be represented explctly n the followng concse form q = KU t, t [t o, t 1 ] (18) where U t = (u T, u n+1 ) T B R n+1 s a gven contnuous, pecewse smooth vector functon and K s a (n + 1) x (n + 1) block matrx n the form K = [ P ξ 0 1 ] (19) The vector functon U t s projected on the tangent vector space on B by means of matrx K. Hence, the selecton of U t allows us to control the tangent vector feld on B and n partcular, the robot arm moton. Therefore we refer to U t as knematc path control. It s defned by the qualty crtera for computng an optmal robot moton. The knematc path control and the manfold B are the analogues for the moton parameters and the geometrc path n task space, respectvely. Fg. 3. The null space of a redundant robot arm. Let us defne the operator P = E J + J (15) where E s the unty n x n matrx and J + s the Moore- Penrose pseudonverse matrx of the Jacoban J. From the propertes of the pseudonverse matrx [9] t follows that for a gven q the operator P(q) s a symmetrc n x n matrx projectng R n onto ℵ(J(q)) n (13).e. Pu ℵ(J(q)) for an arbtrary u R n. Let us consder the vector ξ defned as follows: ξ = J + γ (q n+1 ) (16) The defnton of the operator P mples that the product Pξ s the zero vector. Furthermore, the vector u (0) = ξu n+1 s a partcular nonzero soluton of (12). In summary, vector ξu n+1 s orthogonal to the lnear space ℵ(J) and ξu n+1 L un+1. Moreover, ξu n+1 s the vector wth the mnmum norm possessng ths property. Thus, we obtan the followng common representaton for any element of L un+1 n an arbtrary pont q of the manfold B qn+1 (Fg. 3): L un+1 = Pu + ξu n+1 (17) 5 Processng sngulartes The applcaton of Jacoban- based models n nverse knematcs ncludng the here proposed mathematcal model reles on the assumpton that the rank of the Jacoban s equal to the dmenson of the task space for all the confguratons of jont varables. In practce, there exst confguratons q, where rank J(q) < m. These so called sngulartes of the Jacoban appear to be a major obstacle for the wde mplementaton of ths knd of models. Typcal approaches for resolvng sngulartes attempt to avod them by employng sngularty analyss based on a symbolc representaton of the Jacoban [10], Sngular Value decomposton [7] or manpulablty resoluton [11]. Here we wll present a soluton to ths problem n the context of the here proposed mathematcal model for moton control of redundant robot arms. The frst part of ths soluton provdes an effcent crteron to dentfy a sngularty and the second part deals wth processng of the dentfed sngularty. Consder the matrx P defned n (15). It s a symmetrc n n matrx and rank P(q) = n m for all q, where rank J(q) = m. On the other hand, the trace of P s the sum of ts egenvalues. Ths trace s nvarant wth respect to the change of bass. Now from the Spectral theorem n algebra [12] t follows that P has exactly n m orthonormal egenvectors. The sum of ther correspondng egenvalues s equal to n m. Ths proves the followng proposton. E-ISSN: Volume 16, 2017

6 Proposton. The Jacoban has full rank, f and only f, the trace of matrx P s equal to n m. Ths proposton provdes an effcent numercal crteron to dentfy a sngularty of the Jacoban. It s enough to establsh when the trace of matrx P becomes larger than n m. Corollary. A confguraton q s a sngularty of the Jacoban, f and only f, the trace of matrx P s larger than n m. Consder now an admssble moton q (t) = (q(t), q n+1 (t)) of the robot arm. Denote by γ(q n+1 ) the poston of the end- effector n work space W. Then all the confguratons q that allow the end- effector to reman n ths poston belong to B qn+1 (Fg. 3). The dmenson of manfold B qn+1 s n m for all q where rank J(q) = m. Assume that among these confguratons there s a sngularty q (s). Then rank P(q (s) ) > n m and the tangent vector feld on B at q (t) = (q (s), q n+1 ) s undefned n a sngular confguraton. It appears that the sngularty q (s) separates one set of non-sngular confguratons from another set of non-sngular confguratons. In terms of the Prncple of contnuty [6] we can nterpret the sngularty as a gate between sets of non- sngular confguratons on B qn+1. Ths allows a redundant robot arm to reconfgure tself wth contnuous nternal movements wthout changng the poston and orentaton of the endeffector even when passng through a sngular confguraton lke q (s) for nstance. Therefore we can replace the tangent vector feld at (q (s), q n+1 ) n B wth the tangent vector feld at pont (q, q n+1 ) n B selected n a suffcently small neghbourhood of (q (s), q n+1 ) such that rank J(q) = m. Consder now a small varaton n the confguraton δq (s) = (δq (s), δq n+1 ). The end- effector movement has a lmted set of ndependent manpulablty drectons n a sngularty. Recall that q (t) s admssble. Then the varaton δq n+1 of the parameter of γ produces varatons both n task space and n the extended space of confguratons, where the manpulablty drectons satsfy the gven geometrcal and moton parameters of moton control. These fndngs allow us to propose the followng heurstc procedure for passng through sngulartes n moton control n the context of the here consdered problem statement. When the ntegraton procedure of equaton (18) reaches a Jacoban sngularty, contnue the ntegraton procedure by computng the rght sde of equaton (18) for the last computed non- sngular confguraton. 6 Concluson Ths paper presents a Jacoban- based mathematcal model for moton control of redundant robot arms subject to gven geometrcal and moton parameters. A gudng desgn prncple n ths model s the separaton of concerns addressng geometry and moton n the nverse knematc problem. Ths approach enables to reduce the nherent complexty n exstng models and formulate results sutable for software mplementaton. Such results are effcent procedures for computng the Jacoban and the knematc control. The Prncple of contnuty provdes a new nsght about resolvng sngulartes through a heurstc procedure. In vew of ths prncple nonredundant robot arms are a partcular case of redundant robot arms. Therefore the obtaned results can be extended for non-redundant robot arms, as well. Acknowledgment Ths work was supported by the Fund for Scentfc Research at the Department of Mathematcs and Informatcs at Sofa Unversty St. Klment Ohrdsky. References: [1] J. Jang, H. Cho, H. Km and H. Kwak, "Knematc control of redundant arms based on the vrtual ncson ports for robotc sngle-port access surgery," n IEEE Internatonal Conference on Robotcs and Automaton (ICRA), [2] H. Km, L. Makao Mller, N. Byl, G. M. Abrams and J. Rosen, "Redundancy Resoluton of the Human Arm and an Upper Lmb Exoskeleton," IEEE Transactons on Bomedcal Engneerng, vol. 59, no. 6, pp , [3] D. Megherb, "Method and apparatus for controllng robot moton at and near sngulartes and for robot mechancal desgn". USA Patent US A, [4] S. Kucuk and Z. Bngul, "Robot Knematcs: Forward and Inverse Knematcs," n Industral Robotcs: Theory, Modellng and Control, S. Cubero, Ed., Pro Lteratur Verlag, Germany / ARS, 2006, pp [5] E. Krustev and L. Llov, "Knematc path Control of Robot Arms wth Redundancy," Technsche Mechank, vol. 6, no. 2, pp , [6] G. W. Lebnz and L. E. Loemker, Phlosophcal Papers and Letters, 2 ed., R. Arew and D. Garber, Eds., Indanopols & Cambrdge: E-ISSN: Volume 16, 2017

7 Hackett Publshng Company Dordrecht: D. Redel, 1989, p [7] F. C. Park and K. M. Lynch, Introducton to Robotcs. Mechancs, Plannng and Control, Cambrdge Unversty Press, [8] L. Llov and G. Bojaddzev, Dynamcs and Control of Manpulatve Robots, Sofa: St. Klment Ohrdsk Unversty Press, [9] G. Calafore and L. E. Ghaou, Optmzaton Models, Cambrdge Unversty Press, [10] M. J. Mrza, M. W. Tahr and N. Anjum, "On Sngulartes Computaton and Classfcaton of Redundant Robots," n Internatonal Conference on Computng, Communcaton and Control Engneerng (IC4E'2015), Duba (UAE), [11] B. Tondu, "A Theorem on the Manpulablty of Redundant Seral Knematc Chans," Engneerng Letters, vol. 15, no. 2, p. 362, [12] J. Solomon, Numercal Algorthms: Methods for Computer Vson, Machne Learnng, and Graphcs, CRC Press, E-ISSN: Volume 16, 2017