Reference trajectory tracking for a multi-dof robot arm

Transcription

1 Archives of Contro Sciences Voume 5LXI, 5 No. 4, pages Reference trajectory tracking for a muti-dof robot arm RÓBERT KRASŇANSKÝ, PETER VALACH, DÁVID SOÓS, JAVAD ZARBAKHSH This paper presents the probem of tracking the generated reference trajectory by the simuation mode of a muti-dof robot arm. The kinematic transformation between task space and joint configuration coordinates is noninear and configuration dependent. To obtain the soution of the forward kinematics probem, the homogeneous transformation matrix is used. A soution to the inverse kinematics is a vector of joint configuration coordinates cacuated using of pseudoinverse Jacobian technique. These coordinates correspond to a set of task space coordinates. The agorithm is presented which uses iterative soution and is simpified by considering stepper motors in robot arm joints. The reference trajectory in Cartesian coordinate system is generated on-ine by the signa generator previousy deveoped in MS Exce. Dynamic Data Exchange communication protoco aows sharing data with Matab-Simuink. These data represent the reference tracking trajectory of the end effector. Matab-Simuink software is used to cacuate the representative joint rotations. The proposed agorithm is demonstrated experimentay on the mode of 7-DOF robot arm system. Key words: inverse kinematics, rea-time reference tracking, signa generator, muti-dof, dynamic data exchange.. Introduction Robotics represents one of the main discipines in the industry. The synergy of robotics with other appications ike car assemby operations, vision systems or artificia inteigence aows not ony for innovations but aso reduces the manufacture costs. In the kinematic anaysis of a robot arm position, two separate probems have to be soved: forward kinematics and inverse kinematics. Forward kinematics incudes soving the forward transformation equation to find the ocation of the hand in terms of the anges and dispacements between the inks of the robot arm. R. Krasňanský, P. Vaach and D. Soós are with Facuty of Eectrica Engineering and IT, Sovak University of Technoogy in Bratisava, Ikovicova 3, 89 Bratisava, Sovak Repubic. E-mais:robert.krasnansky, peter.vaach, david.soos@stuba.sk. J. Zarbakhsh is with Carinthia University of Appied Science, Europastrasse 4, 954 Viach, Austria. E-mai: zarbakhsh@cuas.at. Received This work has been supported by Grant N. /4/ of the Sovak Scientific Grant Agency.

2 54 R. KRASŇANSKÝ, P. VALACH, D. SOÓS, J. ZARBAKHSH In order to find the corresponding sets of joint ange given the desired position and orientation of the end effector it is necessary to sove the inverse kinematics probem. Inverse kinematics, on the other hand, incudes soving the inverse transformation equations in order to find the reationships between the inks of the robot arm from its ocation in space. There has been an increasing amount of work in recent years to derive the inverse kinematics of the robot arm [], [9], [6]. Many schemes as we as profound comparative anaysis for the inverse kinematic probem of redundant robot arm are proposed in previous robotics works. Some schemes provide essentia approach for manipuator geometries with unknown inverse kinematic functions. However, for a continuous path motion contro probem, the inverse of the Jacobian matrix is required. A generaized inverse of Jacobian matrix, so-caed pseudoinverse is widey appied for a redundant robot. The present work is concerned with the inverse kinematics soution for a seven- DOF redundant manipuator. The trajectory panning of robot arms is a very active area since many tasks require specia characteristics to be satisfied. Using of MS Exce and MS VBA Visua Basic for Appications software, the appication for reference signa generation has been deveoped. It aows to determine arbitrary reference trajectory and to change it during runtime. By using of the Dynamic Data Exchange DDE, the communication between the appication and Matab-Simuink is secured in rea-time. The paper is organized as foows. In section, the genera robot kinematics as we as the inverse kinematics anaysis is presented. In section 3, the kinematics mode of the seven-dof robot arm is presented and the inverse kinematics soution is proposed. Section 4 describes a rea-time signa generator deveoped in MS Exce to generate an arbitrary reference signa for the end effector. Subsequenty, computer simuation and experimenta resuts of the proposed methodoogy are presented. Finay, the concusions are drawn in Section 5.. Robot kinematics.. Homogeneous transformation matrix The anaysis the motion, a robot arm can be arranged by assigning certain coordinate frames to each ink, in order to obtain a transformation reating joint to space coordinates as the position and orientation of the end effector with respect to a reference frame [3]. The reative position and orientation of transation and rotation of the robot arm in 3D space can be represented by the 4x4 homogeneous transformation matrix in the foowing form: R Q Notation and symbos are given in the end of this paper. T.

3 REFERENCE TRAJECTORY TRACKING FOR A MULTI-DOF ROBOT ARM 55 Homogeneous transformation is used to cacuate the new coordinate vaues for each robot part. The rotation for each axis in the space is represented by the 3x3 matrix, which may change with respect to rotation vaue: r r r3 x 3x3 3x r r r y Q x3 perspective goba scae rotation matrix transation. r7 r8 r9 z The resuting motion is generated by composition of eementary motions of each ink with respect to the previous one. The joints are supposed to be controed individuay. The transation matrix has dimension 3x and introduces the changing vaue between the coordinate systems. Using rotation matrices brings a compact representation of the rotationa movement of a rigid body. Consider the rotation of a given reference frame with respect to the word reference frame rotation about z-axis as depicted in Fig.. Figure. Rotation of frame x, y, z respect to the x,y,z reference frame Since the both frames have the common origin, the reationship between them foows ony from the rotation. Rotation is then represented by the foowing matrix: R z cos -sin sin cos. In the same way, we can define other rotations with respect to the axis y and x as foows: 3

4 56 R. KRASŇANSKÝ, P. VALACH, D. SOÓS, J. ZARBAKHSH R y cos sin, - sin cos Rx cos sin -. sin cos The transation about x, y, and z axes of is then in the form: T x, T y, T z Consider the -DOF robot arm as depicted in Fig.. Figure. -DOF robot arm Equation representing the dependence of the end effector EE coordinates on joint coordinates and vice versa can be obtained according to the basic trigonometry in the foowing way: x y EE Rz Ry Tz Ry 3 z 7

5 REFERENCE TRAJECTORY TRACKING FOR A MULTI-DOF ROBOT ARM 57 where cos - sin sin cos R z 8 cos sin R y 9 - sin cos T z R y cos 3 sin sin 3 cos 3 Resuting position of the end effector is described by the foowing equations x cos sin + cos sin + 3 y sin sin + sin sin z cos + cos Inverse kinematics Inverse kinematics uses the kinematics equations of a robot to determine the joint parameters providing a desired position of the end effector. For exampe, using these formuas aows cacuation of the joint parameters which set a robot arm to pick up an object. Simiar formuas might determine the positions of the skeeton of an animat-

6 58 R. KRASŇANSKÝ, P. VALACH, D. SOÓS, J. ZARBAKHSH ed character which is supposed to move in a particuar way. However, the soution of the inverse kinematics probem is not aways unique, since the same end effector pose might be reached within severa configurations corresponding to distinct joint position vectors. There are many methods of modeing and soving inverse kinematics probems. Besides of anaytic methods, most fexibe methods typicay rey on iterative optimization in order to find an approximate soution. The main reasons are the difficuty of inverting the forward kinematics equation and the possibiity of an empty soution space [5], [8], []. The inverse Jacobian technique is a simpe yet effective way of impementing inverse kinematics. The veocities in joint space are being mapped to veocities in Cartesian space. It contains the first order partia derivatives of the joint ange parameters: x x n y y J. n z z n 5 Cacuation the inverse Jacobian aows the inverse kinematics to be directy cacuated for a sma time step as foows: - J x. 6 According to 6, the rea function can be ineary approximated for sufficienty sma steps. For kinematicay redundant robots with n > 6 DOF, the Jacobian matrix is non-square and its inverse can be obtained using a pseudoinverse J + [4], [] + T - T J J J J. 7 However, by using the pseudoinverse, the probems of singuarities in the Jacobian sti remain. These singuarities might occur for instance, when mutipe inks become aigned in the same direction and subsequenty, identica derivatives for severa joints of the robot arm are obtained. Inversion of neary singuar matrices resuts in excessivey arge veocities and causes unreaistic behavior with osciations around a singuar configuration. There are many aternative approaches which address the inverse kinematics probem, such as Jacobian transpose methods [3], quasi-newton and conjugate gradient methods [], [4] or Levenberg-Marquardt damped east squares methods [], [7].

7 REFERENCE TRAJECTORY TRACKING FOR A MULTI-DOF ROBOT ARM Modeing and contro of muti-dof robot arm 3.. Mode of 7-DOF robot arm The 7-DOF robot arm is composed of four rotationa joints and five inks with engths to 5 as depicted in Fig. 3. We consider seven stepper motors for rotation and transation of individua inks. Joints two and four can rotate about two axes. A the others can rotate just about one axis. We introduce the foowing notation: i joint variabe generaized coordinates, i,,,7; i ength of i-th ink, i,,,5. Figure 3. 7-DOF robot arm 3.. Inverse kinematics anaysis The contro task was to determine the joint parameters that provide a desired position of the end effector. This position is determined by the reference vaues of x, y, z coordinates and anges 6, 7, which determine the rotation settings of the end ink. The end effector position was determined according to 7 in the foowing way: EE Tz Rz Ry Tz Ry 3 Tz 3 Rz 4 T R T R R [ ] y 5 z 4 y 6 x 7 5 8

8 5 R. KRASŇANSKÝ, P. VALACH, D. SOÓS, J. ZARBAKHSH where T z, T z 9, 3 3 T z 4 4 T z, cos sin sin cos - R z - cos sin sin cos z R, cos sin sin cos - R y - cos sin sin cos y R, cos sin sin cos R y - cos sin sin cos y R cos sin. sin cos x R - 4

9 REFERENCE TRAJECTORY TRACKING FOR A MULTI-DOF ROBOT ARM 5 Since the rotation of the end ink of the robot arm is determined by the reference trajectory, it stays in determined position unti the reference vaues of anges 6, 7 wi change. The cacuation of joint parameters is reaized according to Agorithm 3.. Resuting Jacobian matrix has a size of 3 x 5. Agorithm 3. Inverse kinematics Inputs: ength of inks 5, reference coordinates x, y, z, reference anges 6, 7. Outputs: joints parameters Δ i. Step : Determine the position of the end effector s according to 8 4 Step : Determine the Jacobian matrix according to 5 Step 3: for k : n do : Evauate Jacobian with actua parameters i and the inverse Jacobian using pseudoinverse J + 7; : Obtain the vector Δ according to 6; 3: Compute the joints parameters i k+ Δ i + i k; 4: Check the position error of the end effector according to reference trajectory point err x ref x i ; if err satisfy certain condition, go to step 4, otherwise go back to step 3; end for Step 4: Update i k i k+. In this way, we obtained suitabe joint rotations in order to achieve the desired position of the end effector Rea-time reference trajectory generator In the motion contro we consider the trajectory of the end effector which is updated through the time, so these information can be stored in a tabe or in a text fie. Consequenty, they are sent to Matab-Simuink software in rea-time. In Matab-Simuink we are abe to compute exact joint rotation to achieve required reference trajectory by using the inverse kinematics. In this paper, we consider the trajectory data obtained from tabe stored in Microsoft Exce MS Exce, where the so-caed signa generator SG is deveoped. Thus, the signas for the end effector motion in x, y, z coordinates as we as the exact rotation of the end effector using the anges 6 and 7 Fig. 3 are generated in rea-time. The communication between Microsoft Exce and Matab-Simuink needed for data exchange is estabished by using DDE Dynamic Data Exchange communication deveoped by Microsoft. Thus, the goa of using Microsoft Exce is to generate anaogue processing variabes in rea-time, which are used to generate trajectory of the end effector. In order to generate different kinds of signas in rea-time, it was needed to deveop the automatic generator on-ine. By using appropriate functions in MS Exce, it was possibe to obtain the vaue of rea system time. In order to update data sheet continuousy, it was needed to create a macro using Visua Basic software, which woud per-

10 5 R. KRASŇANSKÝ, P. VALACH, D. SOÓS, J. ZARBAKHSH form these updates in the shortest time interva. The macro can be run by using the start/ stop button, as can be seen in Fig. 4 ony a part of the main window of the appication is shown in Fig. 4. Figure 4. Rea-time signa generator The task of the macro is to run the procedure containing infinite update cyces in MS Exce data sheets. In Fig. 4 it can be aso seen how the ces are generated in the rea-time. In order to create required trajectory of the motion of the end effector, different kinds of mathematica functions and operations for the coordinates and anges were used and stored in other ces of the data sheet. In this way, we obtained the vector of desired reference vaues in the foowing form: x, y, z, r 6, 7 5 The vector r is updated continuousy in MS Exce Fig. 4. The vaues of the vector r are consequenty sent via DDE to Simuink software, where the inverse kinematics probem is soved according to 5 7 and the resuting joint parameters are obtained. From these parameters the position of each robot ink are cacuated according to 8 for simuation purposes. The cacuated position of the end effector is sent for potting as depicted in Fig. 5. Obtained data can be used to contro of the rea robotic system by simpe interconnection of Arduino patform and Matab-Simuink software. In the next section, the simuation resuts of the motion of individua robot inks are shown.

11 REFERENCE TRAJECTORY TRACKING FOR A MULTI-DOF ROBOT ARM 53 DDE [A] u fcn y -8.4 Theta6 DDE Theta7 DDE GotoA [B] GotoB Jpseudo Matrix Mutipy s End_Efector mutipy Integrator XYZ_End_Efector MATLAB Function Anges_rad FromA [A] FromB X DDE joint XYZ_4 MATLAB Function [B] Y XY_ref DDE Z joint XYZ_3 MATLAB Function x joint.3 XYZ_ [;;] joint XYZ_ MATLAB Function XY_EE.5 y End_Efector_Z Figure 5. Rea-time contro scheme in Matab-Simuink 4. Simuation resuts The simuations were carried out in the Matab-Simuink software. The ink ength parameters of the 7-DOF robot arm are as foows:.3 m, m, 3 m, 4.5 m, 5. m. The rotationa range of motion of individua joints is as foows: Î<, 7 >, Î<-6, >, 3 Î<-, 5 >, 4 Î<-8, 8 >, 5 Î<-9, 9 >, 6 Î<-9, 9 >, 7 Î<-9, 9 >, where the vaues of the anges 6 and 7 are determined by the user via signa generator. According to these parameters we were abe to evauate and pot the workspace of the end effector in 3D space as depicted in Fig. 6. The simuation resuts of the robot arm tracking the desired circe-shape trajectory generated in rea-time via signa generator in MS Exce are presented in Figs 6 8. Each ink is represented by a unique coor.

12 54 R. KRASŇANSKÝ, P. VALACH, D. SOÓS, J. ZARBAKHSH Figure 6. Workspace of the end effector in 3D space The circe with the radius of.5 m is situated in the workspace of the end effector considering the midpoint coordinates: x, y, z,,. Simuated trajectories projected on the XY and XZ panes are shown in Fig. 7. Figure 7. Trajectory of the end effector s motion bue dots and reference trajectory red dots in A XY pane B XZ pane The robot arm s configuration during the simuation is depicted in Fig. 8. For better iustration of the motion of each ink, every sixth step of motion is considered.

13 REFERENCE TRAJECTORY TRACKING FOR A MULTI-DOF ROBOT ARM 55 x z Figure 8. Robot arm configuration The time responses of the end effector s position in X and Y axis are shown in Fig. 9. These resuts verify the sufficient tracking of the reference trajectory by the end effector. y position reference y-axis end effector y-axis end effector x-axis reference x-axis time [s] Figure 9. Time response of the end effector position in X, Y axes As can be seen from the figures, the simuation experiments have shown desired resuts. 5. Concusions This paper presents a compete soution to the inverse kinematics of a 7-DOF robot arm. In order to obtain the soution, the iterative procedure based on cacuating of pseudoinverse Jacobian was proposed. The procedure requires that the Jacobian is computed numericay. The main contribution of this work has been the deveopment of the signa generator in order to generate the reference trajectory in rea-time. This too has been deveoped using macros in MS Exce and it enabes moduation of the shape of the reference trajectory continuousy during runtime. The simuation resuts prove the

14 56 R. KRASŇANSKÝ, P. VALACH, D. SOÓS, J. ZARBAKHSH efficiency of the proposed soution with the deveoped too for the rea-time reference trajectory tracking by the robot arm. Notation and symbos DOF DDE VBA EE SG degree of freedom dynamic data exchange visua basic for appications end effector signa generator k time instant R rotation matrix T transation matrix Q homogeneous transformation matrix q rotation ange between the ink and the respective reference frame ink of the robot arm J Jacobian J + pseudoinverse of Jacobian J T transpose of Jacobian x, y, z coordinates in Cartesian coordinate system r desired reference vaue Δ time derivative References [] J. Angees, F. Ranjbaran and R.V. Pate: On the design of the kinematic structure of seven-axes redundant manipuators for maximum conditioning. Proc. of the IEEE Int. Conf. Robotics and Automation, Nice, France,99, [] J.G.P. Barnes: An agorithm for soving non-inear equations based on the secont method. Computer J., 8, 965, [3] J. Denavit and R.S. Hartenberg: A kinematic notation for ower-pair mechanisms based on matrices. Journa of Appied Mechanics, 955, 5-. [4] R.G. Fenton, B. Benhabib and A.A. Godenbereg: Optima point-to-point motion contro of robots with redundant degrees of freedom. J. of Manufacturing Science and Engineering, 8, 986, -6. [5] R. Fetcher: Generaized inverse methods for the best east squares soution of systems of non-inear equations: Computer J., 968,

15 REFERENCE TRAJECTORY TRACKING FOR A MULTI-DOF ROBOT ARM 57 [6] C.A. Kein and C.H. Hung: Review of pseudoinverse contro for use with kinematicay redundant manipuators. IEEE Trans. on Systems, Man, and Cybernetics, 3, [7] Y. Nakamura and H. Hanafusa: Inverse kinematics soutions with singuarity robustness for robot manipuator contro. J. of Dynamic Systems, Measurement, and Contro, 8 986, [8] P. Rabinowitz: Numerica Methods for Non-inear Agebraic Equations. New York: Gordon and Breach, 97, [9] J. De Schutter, T. De Laet and J. Rutgeerts: Constraint-based task specification and estimation for sensor-based robot systems in the presence of geometric uncertainty. The Int. J. Robotics Research, 65, 7, [] B. Siciiano: Kinematic contro of redundant robot manipuators: A tutoria. J. of Inteigent and Robotic Systems, 33, 99, -. [] C.W. Wamper: Manipuator inverse kinematic soutions based on vector formuations and damped east squares methods. IEEE Trans. on Systems, Man and Cybernetics, 6 986, 93-. [] L.C.T. Wang and C.C. Chen: A combined optimization method for soving the inverse kinematics probem of mechanica manipuators. IEEE Trans. on Robotics and Automation, 7 99, [3] W. A. Woovichand and H. Eiot: A computationa technique for inverse kinematics. 3rd IEEE Conf. on Decision and Contro, 984. [4] J. Zhao and N. I. Bader: Inverse kinematics positioning using noninear programming for highy articuated figures. ACM Trans. on Graphics, 3 994,