An Explicit Formulation of Singularity-Free Dynamic Equations of Mechanical Systems in Lagrangian Form Part Two: Multibody Systems

Transcription

1 Moeng, Ientification an Control, Vol 33, No 2, 2012, pp An Expcit Formulation of Singularity-Free Dynamic Equations of Mechanical Systems in Lagrangian Form Part Two: Multiboy Systems Pål Johan From 1 1 Department of Mathematical Sciences an Technology, Norwegian University of Life Sciences, 1432 Ås, Norway pafr@umbno Abstract Th paper presents the expcit ynamic equations of multiboy mechanical systems Th the secon paper on th topic In the first paper the ynamics of a single rigi boy from the Boltzmann Hamel equations were erive In th paper these results are extene to also inclue multiboy systems We show that when quasi-velocities are use, the part of the ynamic equations that appear from the partial erivatives of the system inematics are ientical to the single rigi boy case, but in aition we get terms that come from the partial erivatives of the inertia matrix, which are not present in the single rigi boy case We present for the first time the complete an correct erivation of multiboy systems base on the Boltzmann Hamel formulation of the ynamics in Lagrangian form where local position an velocity variables are use in the erivation to obtain the singularity-free ynamic equations The final equations are written in global variables for both position an velocity Keywors: Lagrangian mechanics, singularities, implementation, Lie theory 1 Introuction Multiboy ynamics a research fiel with many an iverse appcations The most common example of a multiboy ynamical system a robotic manipulator which consts of several ns connecte through joints an an amsible set of motions associate with each joint The geometry of the ns an the amsible motions of the joints characterize the motion of the robotic en effector In th paper the ynamic equations of multiboy systems base on a Lie theoretical approach will be erive an the main ifferences between multiboy systems an single rigi boies are pointe out Multiboy systems are quite ifferent from single rigi boies First of all the inematics more involve as the positions of the joints appear in the mapping from the joint velocities to the velocities of the ns an en-effector Kinematically the n positions an velocities are therefore couple Th ifferent from single rigi boies where the velocity space not configuration epenent It important to note, however, that the joint positions an velocities are not couple, ie, the joint velocities are inepenent of the positions of the joint Furthermore, on a inematic level the joint velocity also inepenent of all of all the other joint positions an velocities in the mechani Th always the case when using generaze coorinates or other similar sets of variables We will use th property frequently when eriving the multiboy ynamics Another ifference an an important one in y- ISSN c 2012 Norwegian Society of Automatic Control

2 Moeng, Ientification an Control namic moeng the configuration-epenent inertia matrix In other wors, the inetic energy oes not only epen on the velocity state of the system, but also the positions of the boies relative to each other A irect results of th that the partial erivatives of the inertia matrix with respect to the joint variables o not appear from the ynamics, which they i for single rigi boy ynamics In multiboy ynamics several terms that o not are in single boy ynamics therefore appear in the ynamic equations Lie groups have been appe to represent the configuration space of mechanical systems by several researchers, for example Seg 2000; Par et al 1995; Bullo an Lew 2000; Arnol 1989; Bullo an Murray 1999; Murray et al 1994 However, most formulations focus on single rigi boies, an the extension to multiboy systems often ealt with using an abstract formulation of the ynamics without expcitly referring to what the specific equations loo e, which not always a straight forwar erivation In th paper we erive the ynamics of multiboy systems with Lie group topologies, which nees to be treate somewhat ifferently from the single rigi boy formulations Th the the secon paper on th topic The first paper From 2012 erive the ynamic equations for single rigi boies using the Boltzman-Hamel equations In th paper th wor extene to multiboy systems The formulation base on From 2012 where the single rigi boy ynamics was erive an the expcit equations for several ifferent configuration spaces were shown In th paper we will show that when the ynamics erive in terms of quasivelocities in th way we can simply pic the appropriate transformation from the ynamics of single rigi boies that we foun in From 2012 an stac these into one big bloc-iagonal matrix to obtain the multiboy ynamics The ynamic coupng will then are naturally from the erivation In th paper we erive for the first time the correct expcit equations of multiboy systems using the approach first presente in Duinam an Stramigio 2007, an also use in Duinam an Stramigio 2008, From et al 2010a, From et al 2010b, an From et al 2011 In the next section we will see how to erive the ynamics of multiboy systems an we point out the main ifferences from the single rigi boy case We will also show what the equations loo e expcitly 2 Multiboy Dynamics In th section the ynamics of multiboy mechanical systems in terms of a general configuration vector x an velocity vector v are erive Serial chains of rigi boies only, for example robotic manipulators, are consiere We will see that the approach follows more or less the same reasoning as From 2012 but we also nee to tae into account the configuration-epenent inertia matrix an the inematic an ynamic coupng between the rigi boies in the system We will therefore point out where these multiboy terms are in the equations We will write v = [ v T 1 v T 2 v T n] T R N for velocity an x = [ x T 1 x T 2 xn] T T R N for position In th case we thus have n rigi boies, or ns, an N the total imension of the system, ie, N = n i=1 imv i The inetic energy of each n given by K i = 1 2 = 1 2 V B TIi 0i V0i B V S TA T 0i gib I i A gib V0i S 1 where V0i S the velocity of n i in the spatial frame an A g the ajoint transformation that transforms the spatial velocity to the boy frame, enote V0i B For a robotic manipulator with Eucean joints we get the stanar formulation of the inetic energy of each n given by an thus K i = 1 2 J ixẋ T A T g ib I i A gib J i xẋ = 1 2ẋT J i x T A T g ib I i A gib J i xẋ = 1 2 vt M i xv 2 M i x = J T i A T g ib I i A gib J i R N N 3 for each n J i xẋ gives the velocity of n i in the spatial frame, enote V0i S, an J ix the geometric Jacobian of n i The total inetic energy of the multiboy system given by the sum of the inetic energies of the mechani ns, that, Kx,v = 1 n 2 vt M i x v i=1 } {{ } Mx = 1 2 vt Mxv 4 with Mx the inertia matrix of the total system We note that the inertia matrix epens on the configuration x of the multiboy system, which ifferent from the single rigi boy case presente in From

3 From, Expcit Dynamics of Mechanical Systems The mapping between the time erivative of the position variable ẋ an the velocity variable v given by Sx as From, 2012 v = Sxẋ 5 Th the velocity transformation matrix, an therefore singularity prone whenever the Euler angles appear in the transformation For transformations with a Lie group topology we can also write the state space in terms of local variables, in which case the transformation given by where an Sϕ = v = Sϕ ϕ 6 I 1 2 a ϕ+ 1 6 a2 ϕ R m m 7 a V = [ ] ˆω b 0b ˆvb 0b 0 ˆω b 0b 8 the ajoint matrix efining the Lie bracet We refer to Duinam an Stramigio 2008 an From 2012 for more etail on local velocity an position variables 21 Quasi-velocities In th section we will use the relation in 5 to eminate ẋ from the equations The Lagrangian of the vehicle-manipulator system written in terms of th general configuration space given by Lx,v = 1 2 vt Mxv Ux 9 We will fin the partial erivatives of the Lagrangian following the same train of thought as in From 2012, but appe to multiboy systems The erivatives of the Lagrangian in 9 for a configuration-epenent matrix Mx are foun with respect to v an x as t = Mxv, 10 = Mx v +Ṁxv, 11 = 1 2 T Mxv v Ux 12 We note that th ifferent from the single rigi boy case presente in From 2012 Using the relation v = Sxẋ we write the Lagrangian as a function of generaze coorinates an velocities as Lx,ẋ = 1 2ẋT Sx T MxSxẋ Ux 13 The ynamics then foun by Lagrange s equations as L L = Bxτ 14 t ẋ for some Bx To fin the expcit equations we nee the time erivatives of the Lagrangian L which are ifferent from the single rigi boy We fin the partial erivatives as t L ẋ = ST xmxsxẋ = S T xmxv }{{} L ẋ = S T x, 15 = ṠT xmxv +S T xṁxv +ST xmx v = ṠT xmxv+s T x Ṁxv +Mx v }{{}}{{} = ṠT x +ST x t L = T Sxẋ MxSxẋ = T Sxẋ t T Mxv Sxẋ Ux Mxv }{{}, T Mxv v Ux } 2 {{ } = + T Sxẋ 17 Th an important result that we will use frequently so we write it as a proposition: Proposition 21 The partial erivative of a Lagrangian in the form Lx,ẋ = 1 2ẋT Sx T MxSxẋ Ux 18 can be expresse in terms of the Lagrangian as t Lx,v = 1 2 vt Mxv Ux 19 L = ẋ ṠT x +ST x t L = + T Sxẋ

4 Moeng, Ientification an Control Proof The proof follows irectly from Equations We can thus conclue with the following proposition: Proposition 22 The partial erivatives of the Lagrangian Lx,ẋ in 13 for multiboy systems can be written in terms of the Lagrangian Lx,v in 9 in the same way as for single rigi boies Proof We see th by comparing the expressions in with the corresponing expressions in From 2012, which are ientical Th a very strong result because it tells us that the ynamic equations of multiboy systems have the same overall structure an form as for single rigi boies The Euler Lagrange equations are foun by the partial erivatives of the Lagrangian Lx,ẋ as S T x t t +ṠT x L L ẋ = Bxτ T Sxẋ = Bxτ 22 The torques τ = [ τ1 T τ2 T τn] T T are efine in the usual way so that they are collocate with v = [ ] v T 1 v2 T vn T T R N To fin the corresponing external forces that are collocate with ẋ we write W = v T τ = Sxẋ T τ = ẋ T S T xτ 23 an we have Bx = S T x as expecte Sx will be of the form S1 T S2 T 0 Sx = Sn T for the general case with non-eucean transformations, an simply given by the ientity matrixsx = I for Eucean transformations We note that we can simply stac the transformations of each joint in a bloc iagonal matrix representing the whole system The reason that the velocity transformation matrix becomes bloc iagonal in th way that the velocity transformation between two consecutive rigi boies in a serial chain inematically inepenent of the positions an velocities of the other transformations The best example of th a serial robotic manipulator For a robotic manipulator the matrices S i efine coorinate transformations of the velocity variables of each joint in the chain, an shoul not be confuse with the velocities of the ns represente by the Jacobian J i which woul not result in a bloc-iagonal matrix in th form, but rather a lower triangular matrix We finally pre-multiply 22 with S T x to get the require ynamic equations The ynamic equations of a multiboy system then given by t S T x +S T x Ṡ T x T Sxẋ = τ 25 If we substitute the expressions in into 25, the ynamics becomes Mx v +Ṁxv 1 2 S T x T Mxv v }{{ } Multiboy terms + γ v Mxv +S T x Ux where we recognize γ v = S T x Ṡ T x T Sxẋ = τ from From 2012 Note the terms that are ue to the configuration-epenent inertia matrix Mx which were not present for single rigi boies with a constant inertia matrix We woul e to write the equations in terms of x an v = Sxẋ, but not with ẋ expcitly present in the equations We can follow the same train of thought as we i for single rigi boy systems, but we notice that two new terms are in 26 These are terms that are ue to the ynamic coupng between the rigi boies in the system From the erivation of γ we see, however, that th will not change the expression in 27 in any way, an we can use the expression for γ that we foun in From 2012 also for multiboy systems Thus, the terms with the partial erivatives of the velocity transformation matrix Sx are ientical to the single rigi boy case It now only remains to loo at the part of the Corios matrix that ares as a result of a configurationepenent inertia matrix If we use that ẋ l = S 1 l v we first note that Ṁx can be written as Ṁ ij x = l =,l M ij x ẋ l l M ij x l v 28 l 64

5 From, Expcit Dynamics of Mechanical Systems Further, we write the matrices S T x an T Mxv S T x = T Mxv = N N 2N Mv 1 Mv 2 1 Mv 1 Mv 2 2 N2 NN 1 Mv N 1 Mv N 2 2 Mv 1 Mv 2 N N which gives S T x T Mxv l S 1 Mv 1 l1 l l S 1 Mv 1 l2 l l S 1 N2 Mv 1 l as Mv N N l S 1 Mv N l1 l l S 1 Mv N l2 l l S 1 Mv N ln l as an we obtain the require expression as S T x T Mxv = Mv j ij l l = M j v 32 l,l We have foun the ynamics of multiboy systems without ẋ expcit in the equations an we can conclue with the following important result: Theorem 21 The ynamic equations of a multiboy system with generaze coorinates x an quasivelocity coorinates v = Sxẋ can be written as Mx v +Cx,vv +Nx = τ 33 where the inertia matrix given by M i q = J T i A T g ib I i A gib J i R n n, 34 the Corios matrix given by where α ij x =,l Cx,v = l α v + l 1 2 S 1 β v, 35 M j, 36 l β ij x = γ M sj s = Ssl S m M sj, m l l,m,s 37 an the potential forces are given by Nx = S T Ux 38 Proof We first multiply 27 with the inertia matrix: = s = s = γ v M ij γ v l,m,s l,m M sj Ssl S m l Ssl m S l m m v M sj M sj v 39 where we have use the expression for γ that we foun in From 2012, given by γ ij q = Sjl S jm m 40 q m q l l,m which we have alreay seen the same for multiboy systems We rewrite 26 by substituting the multiboy terms foun in 28 an 32 an the inematic terms in 39 into the ynamic equations in 26 an the expressions in 41 are see next page, which efines the matrices in the theorem 22 Exponential Coorinates In th section we will erive the ynamics of multiboy systems in terms of the well-efine state variables Q i for position an v i for velocity The velocity state first written in terms of exponential coorinates ϕ an we tae the partial erivative with respect to the local coorinates, evaluate at ϕ = 0 an the current configuration foun by the exponential map as Q = Φ Q,ϕ From, 2012 The efinition of the state variables aopte in th section more general than in the previous sections because we o not force the state variables into a vector form For position we write the state space in terms of the global configuration states Q = {Q 1,Q 2,,Q n } where Q i enotes the configuration of rigi boy i ie, a matrix representation of SE3 or one of its subgroups For 1-DoF Eucean ns we can stac the joint positions in a vector q in the normal way, ie, Q i = q i R We can also write the velocity part of the state space as v = {v 1,v 2,,v n } where v i the velocity variable 65

6 Moeng, Ientification an Control of rigi boyi For the velocity state we can choose the time erivative of the joint positions q as the velocity variable if the transformations are Eucean, or else we will choose the velocity twt The Lagrangian given in terms of the velocity variable v an the configuration-epenent inertia matrix MQ as LQ,v = 1 2 vt MQv UQ 42 In the previous section we foun the ynamics of a general multiboy system with state variables x an v We also eminate the time erivative of the position variable ẋ from the equations We i th by introucing the velocity transformation matrix Sx However, these ins of transformations are not well-efine if we use the Euler angles to escribe the orientation of one or more of the transformations escribe by x In th section we use local state variables to eminate these singularities We will now use local coorinates to fin a singularity-free formulation of the ynamics Locally the position variables can be written in terms of the exponential map as ΦQ,ϕ where ϕ the local position variables in the vicinity of Q We can re-write the Lagrangian using the new variables ϕ an v as Lϕ,v = 1 2 vt MΦQ,ϕv UΦQ,ϕ 43 The partial erivatives of the Lagrangian then become = MΦQ,ϕ v +ṀΦQ,ϕv 44 t ϕ = 1 2 T MΦQ,ϕv v Uϕ ϕ ϕ 45 which follows from Equations We consier ϕ an ϕ as variables an woul e to ifferentiate with respect to these, so we nee the partial erivatives of the Lagrangian L ϕ ϕ, ϕ expresse in terms of the local coorinates ϕ an ϕ Lϕ ϕ, ϕ can be written as L ϕ ϕ, ϕ = 1 2 ϕt S T Q,ϕMΦQ,ϕSQ,ϕ ϕ UΦQ,ϕ 46 From Proposition 21 we fin the partial erivatives with respect to ϕ an ϕ as t Lϕ = ϕ ṠT Q,ϕ +ST Q,ϕ t L ϕ ϕ = ϕ + T SQ,ϕ ϕ ϕ Substituting th into Lagrange s equations t Lϕ L ϕ = Bϕτ 49 ϕ ϕ gives us the equations of motion in terms of local position an velocity variables ϕ an ϕ To obtain the ynamics we first use the relation v = SQ,ϕ ϕ to eminate the local velocity variables We then eminate the local position variables ϕ by representing the position as Φ Q,ϕ which, after ifferentiating an evaluating at ϕ = 0, taes us bac to Q through the relation Q = Φ Q,0 From, 2012 We then treat Q as a parameter uring the erivation, but get the final equations in terms of the esire variables Q an v which gives us the following important result: Theorem 22 Consier a general multiboy system with local position an velocity coorinates ϕ an ϕ an global position an velocity coorinates Q an v Write the inetic energy as Kv = 1 2 vt MQv with the inertia matrix MQ The ynamics of th system then satfies MQ v +CQ,vv +NQ = τ 50 M v +,l M v +,l M ij l l v 1 2 M ij l l 1 2 Mx v +Ṁxv 1 2 S T x T Mxv v + γ v Mxv +S T x Ux = τ M j v + Ssl S m M sj v +S T U l m l = τ,l l,m,s M j v + Ssl S m M sj v +S T U l m l = τ,l l,m,s 41 66

7 From, Expcit Dynamics of Mechanical Systems where M foun in the normal way, with τ the vector of external an control wrenches collocate with v, the matrix escribing the Corios an centrifugal forces given by C ij x,v = l, + l,m,,s l ϕ 1 M j l 2 S 1 Ssl S ϕ m v m M sj v, 51 an the potential forces are given by NQ = S T Uϕ ϕ 52 To compute the matrix CQ,v for a single rigi boy with configuration space SE3 or one of its subgroups, we can use 8 to simpfy CQ,v sghtly to C ij x,v = +,s ϕ 1 2 M j ϕ i Ssi ϕ S s ϕ i v M sj v 53 be written as β v = s = = ij = s l,m, s s γ v M sj Ssl S m ϕ m ϕ v M sj l ii Ssi S s ϕ ϕ i M sj v Ssi S s M sj 56 ϕ ϕ i v If we a the α an β terms in 35 we get the require expression for the Corios matrix of a general multiboy mechani as C ij v =,l +,l,m,s l 1 2 S 1 M j v Ssl S ϕ m m M sj v 57 Proof Recall that the velocity transformation matrix can be written as Sϕ = I 1 2 a ϕ+ 1 6 a2 ϕ R m m 54 where a X the ajoint map for a general Lie algebra X of imension m Because the expressions are to be evaluate at ϕ = 0 th expression non-zero only for the iagonal elements of S ij, ie, i = j We will start by writing the first part of the Corios matrix in 35 as α v =,l = = ij ϕ ϕ 1 2 l 1 2 S S 1 ii M j v M j v ϕ i M j v 55 ϕ i The secon part of the Corios matrix in 35 can The final expressions are then given by We recognize the expressions in 55 as the Chrtoffel symbols However, it important to note that the formulation in 55 more general than the stanar formulation of the Chrtoffel symbols The Chrtoffel symbols require the state space in generaze coorinates an corresponing generaze velocities ie, in vector form The formulation in 55 therefore more general because the ynamics can be erive in terms of the local coorinates an then substitute into global variables in a more general form We therefore avoi many of the artifacts that normally are in multiboy ynamics when forcing the state space into a vector form in th way We can further reuce the number of summation inexes by one by following the mathematics in From 2012 an eeping in min that the inertia matrix not constant, which gives C ij v = + ϕ 1 2 M j ϕ i v Si S j Mv 60 ϕ j ϕ i 67

8 Moeng, Ientification an Control C ij v = = ϕ ϕ S 1 ii M j + ϕ i v Ssi ii S s ϕ ϕ i,s M j + ϕ i v Ssi S s ϕ ϕ i,s M sj M sj v 58 v 59 3 Conclusion In th paper we presente the singularity-free ynamic equations of a multiboy system The ynamics were erive from the Boltzmann-Hamel equations in local position an velocity coorinates to avoi singularities in the representation Then, the global state variables were obtaine from the ifferential properties of the Lie algebras We have presente, for the first time the complete an correct erivation of the multiboy ynamic equations in th form We have also shown that the ynamics of a multiboy system can be obtaine by stacing the transformations of single rigi boies in the appropriate way an that the ynamic coupng ares naturally through the erivation Acnowlegement The author acnowleges the support of the Norwegian Research Council their continue funing an support References Arnol, V I Mathematical Methos of Classical Mechanics Springer-Verlag, 1989 Bullo, F an Lew, A D Geometric Control of Mechanical Systems: Moeng, Analys, an Design for Simple Mechanical Control Systems Springer Verlag, New Yor, USA, 2000 Bullo, F an Murray, R Tracing for fully actuate mechanical systems: a geometric framewor Automatica, :17 34 Duinam, V an Stramigio, S Singularity-free ynamic equations of open-chain mechan with general holonomic an nonholonomic joints IEEE Transactions on Robotics, : From, P J An expcit formulation of singularityfree ynamic equations of mechanical systems in lagrangian form-part one: Single rigi boies Moeng, Ientification an Control, :45 60 From, P J, Duinam, V, Pettersen, K Y, Gravahl, J T, an Sastry, S Singularity-free ynamic equations of vehicle-manipulator systems Simulation Moelng Practice an Theory, 2010a 186: From, P J, Pettersen, K Y, an Gravahl, J T Singularity-free ynamic equations of AUVmanipulator systems, lecce, italy Symposium on Intelgent Autonomous Vehicles, 2010b 71 From, P J, Pettersen, K Y, an Gravahl, J T Singularity-free ynamic equations of spacecraftmanipulator systems Acta Astronautica, : Murray, R M, Li, Z, an Sastry, S S A Mathematical Introuction to Robotic Manipulation CRC Press, Boca Raton, FL, USA, 1994 Par, F C, Bobrow, J E, an Ploen, S R A Lie group formulation of robot ynamics International Journal of Robotics Research, : Seg, J M Geometric funamentals of robotics Springer Verlag, New Yor, USA, 2000 Duinam, V an Stramigio, S Lagrangian ynamics of open multiboy systems with generaze holonomic an nonholonomic joints In Proceeings of the IEEE/RSJ International Conference on Intelgent Robots an Systems, San Diego, CA, USA 2007 pages