Differential Kinematics. Robotics. Differential Kinematics. Vladimír Smutný

Transcription

1 Differential Kinematics Robotics Differential Kinematics Vladimír Smutný Center for Machine Perception Czech Institute for Informatics, Robotics, and Cybernetics (CIIRC) Czech Technical University in Prague ROBOTICS: Vladimír Smutný Slide, Page

2 Infinitesimal rotation vector Infinitesimal rotations Infinitesimal rotations have completely different behaviour from the rotation of the finite size. We can write: R x (dφ x ) = R y (dφ y ) = R z (dφ z ) = cos dφ x sin dφ x 0 sin dφ x cos dφ x cos dφ y 0 sin dφ y 0 0 sin dφ y 0 cos dφ y cos dφ z sin dφ z 0 sin dφ z cos dφ z dφ x 0 dφ x 0 dφ y 0 0 dφ y 0 dφ z 0 dφ z () (). () We used here an approximation cos(dφ) = and sin(dφ) = dφ. Further we will use dφdφ = 0. R x (dφ x )R y (dφ y ) = 0 dφ y dφ x dφ y dφ x dφ y dφ x = 0 dφ y 0 dφ x dφ y dφ x = R y (dφ y )R x (dφ x ). () Composition of infinitesimal rotations is comutative and it is possible to interchange them. This is not possible for rotations of finite size. The matrix of infinitesimal rotations is: dφ z dφ y R x (dφ x )R y (dφ y )R z (dφ z ) = dφ z dφ x. () dφ y dφ x Infinitesimal rotations can be summed: ROBOTICS: Vladimír Smutný Slide, Page

3 R(dφ x, dφ y, dφ z )R(dφ x, dφ y, dφ z) = We could also introduce notation: dφ z dφ y dφ z dφ x = E + dφ y dφ x (dφ z + dφ z) (dφ y + dφ y) (dφ z + dφ z) (dφ x + dφ x) (dφ y + dφ y) (dφ x + dφ x) = R(dφ x + dφ x, dφ y + dφ y, dφ z + dφ z). 0 dφ z dφ y dφ z 0 dφ x dφ y dφ x 0 = = E + [d Φ], () where [ Φ] is antisymmetric matrix with zeros on diagonal composed of the elements of the vector Φ. Then it is easy to show, that commutativity and additivity of composition holds: R(d Φ )R(d Φ ) = (E + [d Φ ] )(E + [d Φ ] ) = E + [d Φ ] + [d Φ ] + [d Φ ] [d Φ ] = () = E + [d Φ + d Φ ] ) = R(d Φ + d Φ ) () The representation of the orientation in axis angle system of infinitesimal rotation is even more clear. We can use Rodrigues rotation formula and modify it: r = r cos θ + (s r ) sin θ + s(s r )( cos θ)(s x, s y, s z ) = I () dφ = dφ x dφ y () dφ z r (s, dφ) = r cos dθ + (s r ) sin dθ + s(s r )( cos dθ) = r + (s r )dθ () dr = r r = (s r )dθ = (sdθ r ) () Composition of rotations (s x, dφ x ) (s y, dφ y ) (s z, dφ z ) will produce drdφ = (s x dφ x + s y dφ y + s z dφ z ) r = IdΦ r = dφ r () Vectors of infinitesimal rotations together with operations of vector addition and scalar multiplication form a vector space. We will use properties of vector space of infinitesimal rotations for analysis of differential kinematics of robots. ROBOTICS: Vladimír Smutný Slide, Page

4 Differential kinematics - DOF manipulator Diferential kinematics Diferential kinematics analyzes the robot motion in small neighbourhood of the end effector. It analyzes the velocity and acceleration of the end effector and its relationship to the velocity and acceleration in the joints and vice versa. The analysis motivation can be demonstrated on the shown manipulator. The coordinates of the gripper depend on the joint coordinates (forward kinematics): x(θ, θ ) y(θ, θ ) φ(θ, θ ) = x y φ = l cos θ + l cos(θ + θ ) l sin θ + l sin(θ + θ ) θ + θ We can calculate the infinitesimal change of the gripper position based on the infinitesimal change of the joint coordinates: = dx dy dφ = x(θ,θ ) x(θ,θ ) θ θ y(θ,θ ) y(θ,θ ) θ θ φ(θ,θ ) φ(θ,θ ) θ θ [ dθ dθ l sin θ l sin(θ + θ ) l sin(θ + θ ) l cos θ + l cos(θ + θ ) l cos(θ + θ ). ] = () [ dθ dθ ]. The matrix J = x(θ,θ ) x(θ,θ ) θ θ y(θ,θ ) y(θ,θ ) θ θ φ(θ,θ ) φ(θ,θ ) θ θ we call the manipulator Jacobian. Its value depends on the actual position of the joints (and thus of the gripper). We write dx = Jdθ. We can easily prove that for the velocity holds dx dt = Jdθ dt, v = J θ. The manipulator Jacobian can be divided into subjacobians of the coordinates: v = J θ = J θ + J θ. Each of the subjacobians holds the influence of one joint on the the resulting speed of end effector. Note that as the vector dx describing end effector position and vector of joint coordinates dq have elements with different units (meters, radians) so Jacobian has elements with various units, namely:, m/rad, rad/m. ROBOTICS: Vladimír Smutný Slide, Page

5 Calculation of manipulator Jacobian - Prismatic joint. - - i-,e e e Manipulator Jacobian can be calculated using Denavit- Hartenberg notation using following reasoning (see figure). Denote the vector of infinitesimal displacement of end effector dx e and a vector of infinitesimal rotations of end effector dφ e. Denote the composition of these vectors dp. ( ) dxe dp =. dφ e The velocity can be written as: ( ) ve ṗ = = J q. ω e Manipulator Jacobian, which is in D space of dimension n, where n is a number of manipulator joints can be written: [ ] JL J J = L... J Ln. () J A J A... J An The velocity can be written in the form: v e = J L q + J L q + + J Ln q n. For prismatic joint the infinitesimal translation is directly transformed into infinitesimal translation of the end effector: J Li q i = b i d i. The prismatic joint does not cause any rotation, so: J Ai = 0. Summary for prismatic joint: [ JLi J Ai ] = [ bi 0 ]. () ROBOTICS: Vladimír Smutný Slide, Page

6 Calculation of manipulator Jacobian - Prismatic joint Oi- bi- x d z y y x dxi z Note that the cyan arrow points in the direction of i-th, prismatic joint contribution to the linear velocity of the endefector. ROBOTICS: Vladimír Smutný Slide, Page

7 Calculation of manipulator Jacobian - Revolute joint. - i-,e e e Revolute joint causes infinitesimal motion of the end effector, its size is given by the vector multiplication of the vector of joint axis b i and a vector from the joint axis to end effector r i,e : J Li q i = ω i r i,e = (b i r i,e ) θ i. The rotation of the end effector caused by the joint rotation can be directly translated to the end effector: ω i = b i θi. Summary for revolute joint: [ ] [ ] JLi bi r = i,e. () J Ai b i Using Denavit-Hartenberg notation we can derive how to calculate b i. The vector of the joint i in the coordinate system i is b = [0, 0, ] T. We can transform it into coordinate system of the base by multiplying by rotational matrices: b i = R 0 (q ) R i i (q i ) b. Vector r i,e can be calculated as a vector between points of end effector and a base by the transformation into base coordinate system. Both point are the orgins of their respective coordinate systems and can be written as X = [0, 0, 0, ] T. Let the function, converting hommogenious coordinates to the Euclidean is caller e. Then: r i,e = e(a 0 (q ) A n n (q n ) X) e(a 0 (q ) A i i (q i ) X). ROBOTICS: Vladimír Smutný Slide, Page

8 Calculation of manipulator Jacobian - Revolute joint bi- Oi- x z y ri-,e y dxi dphii z Note that the cyan arrow points in the direction of i-th, revolute joint contribution to the linear velocity of the endefector. Purple (poorly visible) arrow in the end effector shows the contribution of i-th, revolute joint to the angular velocity of end effector. Its vector is parallel to the vector b i ROBOTICS: Vladimír Smutný Slide, Page

9 Differential kinematics Example. Exercise: Find the manipulator Jacobian of the robot from above figure. ROBOTICS: Vladimír Smutný Slide, Page

10 Velocity near singular point - example DOF planar manipulator ROBOTICS: Vladimír Smutný Slide, Page

11 Velocity near singular point - example ROBOTICS: Vladimír Smutný Slide, Page

12 Dexterity ROBOTICS: Vladimír Smutný Slide, Page

13 Dexterity ROBOTICS: Vladimír Smutný Slide, Page

14 Dexterity Dexterity /cond(j) as function of θ 0 0 ROBOTICS: Vladimír Smutný Slide, Page

15 Dexterity defined as D = cond(j) (dexteritycondxy.ud; this could be previewed only in the Acrobat Reader) ROBOTICS: Vladimír Smutný Slide, Page

16 Dexterity ROBOTICS: Vladimír Smutný Slide, Page

17 Dexterity Dexterity prod(σ i ) as function of θ 0 0 ROBOTICS: Vladimír Smutný Slide, Page

18 Dexterity defined as D = n Πσ i (dexterityprodsingular.ud; this could be previewed only in the Acrobat Reader) ROBOTICS: Vladimír Smutný Slide, Page