Abiko, Satoko

Hirzinger, Gerd

Lampariello, Roberto

English

Institute of Transport Research:Publications

Dynamics modeling of structure-varying kinematic chainsfor free-flying robotsRoberto Lampariello, Satoko Abiko, Gerd HirzingerInstitute of Robotics and MechatronicsGerman Aerospace Center (DLR)82234 Weßling, GermanyRoberto.Lampariello@dlr.deAbstract— A new method for the computation of the dynamicsof structure-varying kinematic chains is proposed. This is basedon the complete redefinition of the system connectivity derivingfrom a given structural change. The derived computationalefficiency is then described with examples of typical motionplanning tasks and structure changes for free-flying robots.These include open branched chains and closed loops, in freeand grappled conditions. The method may then contribute tothe efficiency of motion planning for robots which may requiredifferent kinematic structures for a given task.I. INTRODUCTIONThis paper presents an algorithm for the efficient compu-tation of the dynamics of structure-varying kinematic chains.As major application, multiple-arm free-flying space robotsare considered. Other applications may however also be ofinterest. Structure variants include open branched chains andclosed loops, in free and grappled conditions (see Fig. 1).The aim of this work is to support motion planningmethods, to be implemented on robots which may requiredifferent kinematic structures for different tasks, or evenfor single complex tasks. As is already argued in [1], itis generally useful to be able to handle structure changeswithout switching between different models and algorithms.Furthermore, the alternative of locking non-active joints,leads to computational inefficiency.The goal of computing the dynamics of structure-varyingkinematic chains was previously treated in [1], where theproposed method was based on two important features: firstly,changes to the connectivity, which describes the kinematicstructure of the system, are minimized; secondly, the use ofvirtual links is made. Examples are given for handling closedloops and grappling of a free-floating robot to an inertiallyfixed structure. It is then argued that the advantage in thisapproach is that it is simple to implement and that it can beeasily parallelized.The method proposed here instead, is based on full con-nectivity variation. In fact, despite the extra implementationcomplexity, it is shown that this approach present an advan-tage in the computational efficiency.As mentioned above, another advantage of the structure-varying modeling is to avoid unnecessary computations. ThisFig. 1. Free-flying robot with four arms: free open branched chain (above);grappled closed loop (below)is because the dynamic model of a given system can bereduced to only possess the degrees of freedom which arenecessary to plan or perform a given task. Other degrees offreedom, which would normally have to be locked, are thusomitted in the computation. This point is demonstrated withsome examples.Structural changes are handled on-line, which practicallymeans that they can take place without necessity to recompilethe algorithm. This is a mandatory feature for treating tasksduring which the structure of the system varies. It also opensthe possibility to eventually tackle the problem of optimizingthe kinematic structure for a given system and a given task.The modeling of the free-flying base is derived from basicprinciples of free rigid-body dynamics, as opposed to the al-ternative six degree-of-freedom virtual link approach. Closedloops are also treated in [2], where Lilly and Bonaventuraproposed a dynamic formulation for a space robot with onerobot arm, constrained at the end-effector to be fixed with thebase. In this paper, we consider a more general formulationfor free-flying space robots, to allow for one or more closedloops between any of the system links and also to treatbranching.The paper is then structured as follows: in section II themathematical model used for a free-flying robotic system ispresented. In section III, the necessary functions to performstructure changes are described. Examples are then treated insection IV, including one discussed in [1] and others for 3Dfree-flying robots. Section V gives the conclusions.II. MODELINGA. Open branched chainsIn this section the modeling of the dynamic equationsis introduced. We first consider the simplest case, that ofthe open kinematic chain. We assume a free-flying base, asopposed to one fixed to an inertial point. We will later arriveat this latter case, by virtue of the proposed modeling method.The system of interest in depicted in Fig. 2.1) Equations of motion: The equations of motion are thefollowing:[Hb HbmHTbm Hm] [x¨bφ¨]+[cbcm]=[Fbτ]+[JTbJTm]Fe,(1)where xb and φ are the generalized coordinates of thebase body and the robot joints respectively. The remainingTABLE IMAIN NOTATION IN THE DYNAMIC EQUATIONSHb ∈ R6×6 : inertia matrix of the baseHm ∈ Rn×n : inertia matrix of the robot armsHbm ∈ R6×n : coupling inertia matrix betweenthe base and the armscb ∈ R6×1 : non-linear velocity dependentterm on the basecm ∈ Rn×1 : non-linear velocity dependentterm of the armsFb ∈ R6×1 : force and moment exerted on thebaseFe ∈ R6×1 : force and moment exerted on theend-effectorτ ∈ Rn×1 : torque on the jointsJb ∈ R6×6 : Jacobian matrix for the baseJ im ∈ R6×n : Jacobian matrix for the ith armJC ∈ R6×n : Jacobian matrix for the closed loopΣIΣeφ 1FbFeφ nLink 1Link 2Basexb=(rb,    )ΩFig. 2. Open Tree StructureΣI( Base )01234567Arm  AArm  B∆Pe123Fig. 3. Closed Loop Chainvariables are described in table I. A gravity-free environmentis assumed.In implementing these, the Articulated Body Algorithm(ABA) proposed by Featherstone [3] is used. This is wellapplicable, since it directly addresses free-floating systems.The ABA provides O(n) in both the forward and the inversedynamics.2) Connectivity: The connectivity is described as follows,following the standard approach. The i-th body is attributedan index, parent(i) which defines its parent. The first bodyis generally the base body, with index 0.Furthermore, one or more bodies are defined as an end-effector, by means of a second index, end − effector(i),which is non-zero if affirmative and 0 otherwise. For moreend-effectors, the index is also used to number them.For the case of branching, an example of which is shownat link 1 in figure 2, a parent simply possesses two (or more)children.B. Closed loopThe dynamic model for the closed loop stems from that ofthe open chain structure, subject to the closed loop kinematicconstraints. In a simple example in Fig. 3, the constraintbetween the arm A and the arm B can be described withthe Jacobian matrices:[JAm −TJBm] [ φ˙Aφ˙B]= JCφ˙ = O, (2)whereT =[E ∆˜P eO E](3)and where ∆P e is the vector between the two end-effectors.Furthermore, if m independent kinematic constraints exist,which may relate to N ≥ m linearly dependent closed loops,the constraint equation is expressed by:JCφ˙ =[JG JD] [ φ˙Gφ˙D]= O, (4)where the joint vector φ is divided into the indepen-dent(active) joints vector φG ∈ R(n−m) and the depen-dent(passive) joints vector φD ∈ Rm. JG ∈ Rm×(n−m)and JD ∈ Rm×m denote the Jacobian matrix with respectto the joints φG and φD, respectively.By virtue of d’Alembert’s principle and the above kine-matic constraints, the dynamic equations of the closed loopsystem can be deduced from that of the tree-structure system(1) as follows:M[x¨bφ¨G]+C =[FbτG]+ΠT[JTbJTm]Fe, (5)whereM = ΠTHΠ,C = ΠT[cbcm]+ΠTHΠ˙[x˙bφ˙G],H =[Hb HbmHTbm Hm],Π = E6×6 O6×(n−m)O(n−m)×6 E(n−m)×(n−m)Om×6 −J−1DJG .M and C denote the inertia matrix and the non-linearvelocity dependent term for the closed loop, respectively. Πrepresents the transform matrix from the tree-structure to theclosed loop system. Eq. (5) is a general formulation for thedynamics of the free-flying robot with closed loops. A moredetailed derivation can be found in [4].1) Connectivity Representation for Closed Loop Chains:To represent the connectivity for the closed loop chains, weintroduce one additional index, namely cflag. The cflag indexindicates that the i-th link forms a closed loop with the j-thlink as cflag(j) = i.III. STRUCTURAL CHANGESIn this section we describe the method for treating struc-tural changes. The latter comprise the following possibilities:1521043baseend-effector67891512340baseend-effector67891152346baseend-effector0789end-effectorend-effectorend-effectorend-effectorend-effectorend-effectorFig. 4. Examples for chain inversion of a branched system• given a system with n joints, to lock p of them, resultingin a reduction of DOF;• given a free-flying system, to fix its base, or its end-effector, to an inertially fixed point;• given an open kinematic chain, to close one or moreloops.To accomplish these changes, the following four functionsare constructed.A. New number of jointsThis function calculates the new parameters which describethe system for its n − p DOF. The parameters for the ithlink consist of the inertial and the DH parameters. Theseparameters are of course known for the complete system.The required transformation is function of the position ofthe locked joints. Practically, each locked joint is omittedin the new modeling description and the parameters relativeto the links before and after it are updated, in function ofits position. Mathematical rules for the updating are hereomitted, but can be found in [5], for example, for thecalculation of the new composite inertia.In this process, the connectivity of the system is alsoupdated. To represent the locked joints, we introduced an ad-ditional index, namely connect, which expresses the numberof links which are connected together. Examples are given insection IV.The equations of motion remain structurally identicalto (1), whereas the dimension of φ is reduced or increasedto represent the new number of joints of the system.TABLE IILINK CONNECTIVITY REPRESENTATION FOR FIGURE 3Link No. parent end-effector connect cflag1 0 0 0 -12 1 1 0 -13 1 0 0 -14 3 2 0 -15 0 0 0 -16 6 0 0 -17 7 3 2 2B. Invert chainThis function inverts the order of the system connectivity,such that a chosen end-effector becomes the new base. Thisis a simple procedure for an open kinematic chain.For a branched system the procedure is more involved.Consider the example shown in Fig. 4. From it, the followingrules follow:• given a new base, construct the connectivity of an openchain moving along the system, until an end-effector isreached;• then continue the numbering of the remaining branches,such that the first child belonging to a branch is con-nected to the new parent which has the same numberingas that which its old parent receives.The equations of motion remain structurally identical to (1).For the closed loop case, the same procedure applies, asshown in the example in Fig. 6.C. Extract fixed-base equationsThis function extracts the fixed-base system equations fromthe equations of the equivalent free-flying system. Whenreferring to Eq. (1), the outcome is then simply the secondrow with x¨b = x˙b = 0.D. Close loopAn example is shown in switching from Fig. 2 to Fig. 3.The system has in total n DOF. When the open kinematicstructure changes to the closed loop, two links are connectedtogether, as shown in Fig. 3.As mentioned in section II-B, the i-th link forms theclosed loop with the j-th link by expressing cflag(j) = i. Thecorresponding model description for the system depicted inFig. 3 is shown in Table II. In the example, link 2 and link7 are connected to form the closed loop and cflag(7) = 2 isset, as shown in the table.The equations of motion are extended to those describedin eqn. (5), by means of the transformation matrix Π.IV. EXAMPLESA. Example 1: Open chain grappling to fixed inertial pointIn this example, the first computational advantage ofthe proposed method can be seen. Consider the structurevariation shown in Fig. 5, where a one joint free-flying robotgrapples to a fixed inertial point by means of a one DOFfixture. In alternative to the approach proposed in [1], wherethe connectivity is preserved and a subsidiary link betweenthe end-effector and ground is introduced, the followingprocedure is instead suggested, where the system is convertedto a fixed-base system and its connectivity inverted:• in order to allow for an extra joint at the end-effector,an extra link is first added to the system, by means ofthe New number of joints function;• the system is inverted, by means of the Invert chainfunction;• finally, the fixed-base equations are derived, by meansof the Extract fixed-base equations function.This example shows how this approach allows to representa free-flying system and a fixed-base system with the samemodel. Furthermore, a fixed-base two-degree-of-freedom sys-tem results. This has a clear numerical advantage on the5 degree-of-freedom system obtained in [1] (with also theadditional geometric constraints on the end-effector). Thenumerical burden for calling the structure varying functions isof the order of milliseconds. This is neglegible, if comparedto the efficiency gained in the thousands of function calls,necessary in motion planning procedures.B. Example 2: Closed loop grappling to fixed inertial pointConsider the structure variation shown in Fig. 6, wherea three-arm free-flying system grapples to a fixed inertialpoint forming two closed loops. The following procedure isapplied:• the first loop is closed, by imposing the necessary con-straint on end-effector 2, to be connected to end-effector1. This is achieved by setting the index cflag(5) = 3;• the system is inverted, such that end-effector 1 becomesthe base, by means of the Invert chain function. Thechange in connectivity can be seen in Fig. 6;• the fixed-base equations are obtained by means of theExtract fixed-base equations function;• finally, the third arm is connected to the base, byapplying the closed loop constraint on its end-effectorand by setting the index cflag(7) = 0. The appropriatechoice of the vector ∆P e (defined in figure 3) ensuresthe correct positioning of the point of connection to thefixed structure.C. Example 3: DOF reduction for an open chainA free-flying robot may consist of a multibody system withup to 37 DOF, when considering 6 DOF for the base, 7 DOFfor each arm and 3 DOF for reaction wheels (see Fig. 1). ItFree-Flying RobotFixed-Base RobotΣIΣIΣI01 1212Base0Base Invert Chain &Extract Fixed-Base EquationsAdd Extra Joint atEnd EffectorFig. 5. Simple Example for Varying StructureOpen Tree StructureClosed Loop StructureFixed-Base StructureClose LoopInverte Chain, Extract Fixed-BaseEquations &Close Second Loop012345671230123456712301234567Fig. 6. Free-Flying Space Robot Applicationmay however need to perform tasks which only require partof these DOF.As a first example, maneuvers where discussed in [6],where three joints of one arm were used to induce motions ofthe free-floating base for identification of the latter’s inertialparameters, or those of the load (it was assumed that theinertial parameters of the arms were known from CAD data).This process involved solving a motion planning problem,based on the integration of the system equations of motion.The necessary DOF for this task however are only 6 DOF forthe base + 3 DOF for the actuated arm. The model reductionis then from 37 to 9 DOF. For this example, the respectiveconnectivity indexes before and after the structural change aredescribed in table III. A simplified version of this operationis shown in Fig. 7.The computational advantage between using a 9 DOFTABLE IIILINK CONNECTIVITY REPRESENTATION FOR EXAMPLE IV-C: 2 ARMSYSTEM WITH 2 ACTIVE JOINTS IN ARM 1Link No. parent end-effector connect cflagBEFORE1 0 0 0 -12 1 0 0 -13 2 0 0 -14 3 0 0 -15 4 1 2 -16 1 0 4 -17 6 0 4 -18 7 0 4 -19 8 2 4 -1AFTER1 0 0 0 -12 1 0 0 -13 2 1 0 -111Fig. 7. System description for Example IV-C: 2 arm system with 2 activejoints in one armsystem and using a 37 DOF system with 28 locked joint isevident. Note that the computational burden between actuatedand locked joints is equivalent, due to the recursive natureof the dynamics computation. The computation of the pathplanning problem is then linear in the number of joints.This is because it involves the computation of the systemJacobian matrices for the base and for the end-effector,and of the actuation forces, through the inverse dynamics.These are both linear functions in the number of joints forthe algorithms applied here, as described in section II. Assuch, for this example the time for the reduced system isapproximately one quarter of that for the full system. Typicalrunning times for our current implementations range between15 and 30 seconds on an Intel Pentium 4 (3.2 GHz), for thereduced system.An example for a grasping maneuver of a tumbling target isgiven in Fig. 8. Here, the task is divided into: a waiting phase,where the target approaches the stationary robot; an approachphase, i.e. a point-to-point maneuver of the robot towards thetarget; a tracking phase, where the robot tracks the movingtarget to avoid impacts; a grasping and stabilization phase.The computational time for this optimal motion planning taskis in the order of 30 seconds on an Intel Pentium 4 (3.2 GHz),for a 13 DOF robot system.Fig. 8. Grasping maneuver of tumbling target: wait, approach, track andgraspFor the above motion planning examples, the structure-varying approach may give a clear computational advantage.Furthermore, note that the same approach allows, in principle,to tackle the optimization problem of determining whichstructure may be best for performing a desired task.V. CONCLUSIONA new method for the computation of the dynamicsof structure-varying kinematic chains is proposed. A firstcomputational advantage with respect to existing approaches,validated with examples, is gained by the complete redef-inition of the system connectivity deriving from a givenstructural change. A second advantage is pointed out, for thecase of locked joints, where these are simply omitted fromthe computation.Since structure variations are handled on-line, there is noneed to prepare every possible kinematic chain in advance,for a given system. This constitutes an important practicaladvantage. Furthermore, the method improves the efficiencyof motion planning.Examples are given for the application of the proposedmethod, which include typical motion planning tasks andstructure changes for free-flying robots. Free-flying robotsmodels are thus switched between open branched chains,closed loop, in the free and in the grappled states.REFERENCES[1] Nakamura, Y., Yamane, K.: Dynamics Computation of Structure-VaryingKinematic Chains and Its Application to Human Figures, IEEE Trans.on Robotics and Automation, Vol. 16, No. 2, pp. 124 – 134, April 2000.[2] Lilly, K.W., Bonaventura, C.S.: A Generalized Formulation for Simula-tion of Space Robot Constrained Motion, The Proc. of the 1995 IEEEInternational Conference on Robotics and Automation, vol. 3, pp. 2835– 2840, Nagoya, Japan, May 1995.[3] Featherstone, R.: Robot Dynamics Algorithms, Kluwer Academic Pub-lishers, 1987.[4] Park, F.C., Choi, J., Ploen, S.R.: Symbolic Formulation of ClosedChain Dynamics in Independent Coordinates, Mechanism and MachineTheory, No. 34, pp. 731–751, 1999.[5] Walker, M.W., Orin, D.E.: Efficient Dynamic Computer Simulation ofRobotic Mechanisms, Journal of Dynamic Systems, Measurement andControl, vol. 104, pp. 205–211, September 1982.[6] Lampariello, R., Hirzinger, G.: Modeling and Experimental Design forthe On-Orbit Inertial Parameter Identification of Free-Flying SpaceRobots, ASME 2005, Long Beach, California, September 2005.[7] Lampariello, R., Deutrich, K.: Simplified Path Planning for Free-Floating Robots, Technical Report 515-99-04, Institute of Robotics andMechatronics, German Aerospace Center (DLR), 1999.

C.S.: A Generalized Formulation for Simulation of Space Robot Constrained Motion,

Dynamics Computation of Structure-Varying Kinematic Chains and Its Application to Human Figures,

Efﬁcient Dynamic Computer Simulation of Robotic Mechanisms,

Modeling and Experimental Design for the On-Orbit Inertial Parameter Identiﬁcation of Free-Flying Space Robots,

Robot Dynamics Algorithms,

S.R.: Symbolic Formulation of Closed Chain Dynamics

Simpliﬁed Path Planning for FreeFloating Robots,

Dynamic Modeling of Structure-Varying Kinematic Chains for Free-Flying Robots

Dynamic Modeling of Structure-Varying Kinematic Chains for Free-Flying Robots

Abstract

Similar works

Full text

Available Versions

Institute of Transport Research:Publications