RECURSIVE INVERSE DYNAMICS

Transcription

1

2 We assume at the outset that the manipulator under study is of the serial type with n+1 links including the base link and n joints of either the revolute or the prismatic type. The underlying algorithm consists of two steps: Kinematic Computations: required to determine the twists of all the links and their time derivatives in terms of θ, θ, θ Dynamic Computations: required to determine both the constraint and the external wrenches. Henceforth, revolute joints are referred to as R, prismatic joints as P.

3 KINEMATICS COMPUTATIONS: OUTWARD RECURSIONS We will use the Denavit-Hartenberg (DH) notation. Moreover, every 3-D vector-component transfer from the F i frame to the frame F i +1 requires a multiplication by Q it. Likewise, every component transfer from the frame F i +1 to the F i frame requires a multiplication by Q i. If we have: r i = r 1, r 2, r 3 and we need:[r i ] i+1 then we proceed as follows: r i+1 = Q i T r i

4 KINEMATICS COMPUTATIONS: OUTWARD RECURSIONS If we recall the form of Q i we then have: r i+1 = cosθ i senθ i 0 λ i sinθ i λ i cosθ i μ i μ i sinθ i μ i cosθ i λ i r 1 r 2 r 3 = Where: λ i = cosα i and μ i = senα i r 1 cosθ i + r 2 cosθ i λ i r + μ i r 3 μ i r + λ i r 3 While: r = r 1 sinθ i r 2 cosθ i Likewise, if we have v i+1 = v 1, v 2, v T 3 and we need [v] i, we use the component transformation given below: v i = cos θ i λ i sinθ i μ i sin θ i sin θ i λ i cosθ i μ i cosθ i 0 μ i λ i v 1 v 2 v 3 = v 1 cosθ i + vsinθ i v 1 sinθ i + vcosθ i v 2 μ i + v 3 λ i

5 KINEMATICS COMPUTATIONS: OUTWARD RECURSIONS It is now apparent that every coordinate transformation between successive frames, whether forward or backward, requires eight multiplications and four additions. We indicate the units of multiplications and additions with M and A, respectively.

6 KINEMATICS COMPUTATIONS: OUTWARD RECURSIONS The angular velocity and acceleration of the ith link are computed recursively as follows: ω i = ω i 1 + ω i θ i e i, ω i = ω i 1 + ω i 1 ω i 1 θ i e i + ith joint R ith joint P θ i e i, ith joint R ith joint P for i = 1, 2,..., n, where ω 0 and ω 0 are the angular velocity and angular acceleration of the base link. This equations are valid in any coordinate frame.

7 KINEMATICS COMPUTATIONS: OUTWARD RECURSIONS In view of outwards recursive nature of the kinematic relations it is apparent that a transfer from F i to F i+1 coordinate is needed, which can be accomplished by multiplying either ei or any other vector withe (i-1) subscript by matrix Q it. Hence, the angular velocities and accelerations are computed recursively, as indicated below: ω i = Q i T ω i 1 + Q i T θ i e i, ω i = Q i T ( ω i 1 + ω i 1 Q T i ω i 1 θ i e i + θ i e i ith joint R ith joint P ith joint R ith joint R

8 KINEMATICS COMPUTATIONS: OUTWARD RECURSIONS If the base link is an inertial frame, then: ω 0 = 0 ω 0 = 0 Furthermore, in order to determine the number of operations required to calculate ω i in F i+1 when ω i 1 is available in F i, we note that: ω i 1 θ i e i i = θ i ω y θ i ω x 0 Where w x, w y e w z are the w i-1 components in F i-1

9 KINEMATICS COMPUTATIONS: OUTWARD RECURSIONS Furthermore, let c i be the position vector of C i, the mass center of the ith link, ρ i being the vector directed from O i to C i, as shown in Figures

10 KINEMATICS COMPUTATIONS: OUTWARD RECURSIONS The position vectors of two successive mass centers thus observe the relationships: if the ith joint is R: δ i 1 = a i 1 ρ i 1 c i = c i 1 + δ i 1 + ρ i if the ith joint is P: δ i 1 = d i 1 ρ i 1 c i = c i 1 + δ i 1 + b i e i + ρ i Note that in the presence of a revolute pair at the ith join, the difference a i 1 ρ i 1 is constant in F i. Likewise, in the presence of a prismatic pair at the same joint, the difference d i 1 ρ i 1 is constant in F i.

11 KINEMATICS COMPUTATIONS: OUTWARD RECURSIONS We derive the corresponding relations between the velocities and accelerations of the mass centers of links i - 1 and i, namely, if the ith joint is R: c i = c i 1 + ω i 1 δ i 1 + ω i ρ i c i = c i 1 + ω i 1 δ i 1 + ω i 1 (ω i 1 δ i 1 ) + ω i (ω i ρ i ) if the ith joint is R: ω i ρ i + u i = δ i 1 + ρ i + b i e i v i = ω i u i c i = c i = c i 1 + v i + c i 1 + b i e i ω i u i + ω i (v i + 2 b i e i + b i e i )

12 KINEMATICS COMPUTATIONS: OUTWARD RECURSIONS For i = 1, 2,..., n, where c 0 and c 0 are the velocity and acceleration of the mass center of the base link. If the latter is an inertial frame, then ω 0 = 0 ω 0 = 0 c 0 = 0 c 0 = 0 The expressions above are invariant. They hold in any coordinate frame, as long as all vectors involved are expressed F i in that frame. However, we have vectors in the frame, and hence a coordinate trasformation is needed. This coordinate trasformation is taken into account in the following algorithm whereby the logical variable R is true if the ith joint is R; otherwise is false

13 ALGORITHM OUTWARD RECURSION Read Q n 1 i 0, c 0, ω 0, c 0, ω 0, c 0, ρ n n 1 i 1, δ 1 0 For i=1 till n step 1 do: Update Q i if R then: c i Q T i (c i 1 + δ i 1 ) + ρ i ω i Q T i ω i 1 + θ i e i u i 1 ω i 1 δ i 1 v i ω i ρ i c i Q T i c i 1 + u i 1 v i ω i Q T i ω i 1 + ω i 1 + θ i e i + θ i e i c i Q T i c i 1 + ω i 1 δ i 1 + ω i 1 u i 1 + ω i ρ i + ω i v i

14 ALGORITHM OUTWARD Else u i Q i T δ i 1 + ρ i + b i e i c i Q i T c i 1 + u i ω i Q i T ω i 1 v i ω i u i w i b i e i c i Q T i c i 1 + v i + w i ω i Q T i ω i 1 c i Q T i c i 1 + Endif Enddo RECURSION ω i u i + ω i v i + w i + w i + b i e i

15 ALGORITHM OUTWARD RECURSION If, moreover, we take into account that the cross product of two arbitrary vectors requires 6M and 3A, we then have the operation counts given below: R joint P joint M A M A Q i c i ω i c i ω i c i

16 DYNAMICS COMPUTATIONS: INWARD RECURSIONS Free-body diagram of the ith link

17 DYNAMICS COMPUTATIONS: INWARD RECURSIONS A free-body diagram of the EE appears in figure. Note that the link is acted upon by a nonworking constraint wrench, exerted though the nth pair, and a working wrench; the latter involves both active and dissipative forces and moments. Although dissipative forces and moment are difficult to model. Since this forces and moment depend only on joint variable and joint rates, the can be calculated one the cinematic variable are known.

18 DYNAMICS COMPUTATIONS: INWARD RECURSIONS Hence, the force and the moment that (i-1)st link excert on th ith link throught the ith joint produce non working constraint and active wrences. That is for a revolute pair: n i P = n i x n i y τ i f P i = in which n ip and f ip are the nonzero Fi-components of the nonworking constraint moment exerted by the (i 1)st link on the ith link; obviously, this moment lies in a plane perpendicular to Z i, whereas τ i is the active torque applied by the motor at the said joint. f i x f i y f i z

19 DYNAMICS COMPUTATIONS: INWARD RECURSIONS For a prismatic pair, one has: n i P = n i x n i y n i z f i P = f i x f i y τ i Whre vector n ip contains only nonworking constraint torques, while τ i is now the active force exerted by ith motor in the Z i direction, f ix and f iy being the nonzero Fi-components of the nonworking constraint force excerted by the ith joint on the ith link, which is perpendicular to the Zi axis.

20 DYNAMICS COMPUTATIONS: INWARD RECURSIONS Free-body diagram of the end-effector

21 DYNAMICS COMPUTATIONS: INWARD RECURSIONS From the figure above, the Newton-Euler equations of the end-effector are: f P n = m n c n f n P i = I n ω n + ω n I n ω n n + ρ n fp n where f and n are the external force and moment, the former being applied at the mass center of the EE. The Newton- Euler equations for the remaining links are derived based on the free-body diagram namely, f i P = m i n P i = I i ω i + ω i I i ω i + n i+1 P P c i f i+1 + a i ρ i f P i+1 + ρ i f i P

22 DYNAMICS COMPUTATIONS: INWARD RECURSIONS The vectors n P i and f P i indicate the couples and active forces, denoted by t i. In fact, if the i-th joint is rotationally has: τ i = e i T n i P if the i-th joint is prismatic then the actuator force reduces to: τ i = e i T f i P The foregoing relations are written in invariant form. In order to perform the computations involved, transformations that transfer coordinates between two successive frame are required. In taking these coordinate transformations into account, we derive the Newton-Euler algorithm from the above equation.

23 ALGORITHM INWARD RECURSIONS f P n m n c n f n P P n I n ω n + ω n I n ω n n + ρ n f n If R then P τ n n n z Else P τ n f n z For i=n-1 till 1 step-1 do P φ i+1 Q i f i+1 f P i m i c i φ i n P i I i ω i + ω i I i ω i + ρ i f P P i + Q i n i 1 If R then P τ i n i else τ i f i P enddo z z P + (a i ρ i ) Q i f i+1

24 COMPLEXITY OF DYNAMICS COMPUTATIONS A summary of all the calculations is shown in Table: Row # M A (n-1) 4(n-1) 6 3(n-1) 3(n-1) 7 44(n-1) 37(n-1) Total 55n-22 44n-14 The total number of additions and moltiplications For M d can be calculated with the following formulas: M d = 55n 22 A d = 44n 14