Kinematics Inverse Kinematic Singularity, Redundancy. Centre for Robotics Research School of Natural and Mathematical Sciences King s College London

Transcription

1 Kinematics Inverse Kinematic Singularity, Redundancy Centre for Robotics Research School of Natural and Mathematical Sciences King s College London

2 Singularity As mentioned, The Jacobian in the differential kinematics equation of a manipulat or defines a linear mapping Rank-deficient of J is kinematic singularities. det J = 0 Interests of kinematic singularity in robotics. o Singularities represent configurations at which mobility of the structure is constrained o When the J is singular infinite solutions to o In the neighbourhood of a singularity, small velocities in the operational space may cause large velocities in the joint space. Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

3 Singularity Type of singularities Boundary singularities that occur when the manipulator is either outstretched or retracted. Internal singularities that occur inside the reachable workspace. Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

4 Singularity determinant of a 2 x 2 matrix a b det = ad bc c d Kinematic singularity example of two links arm, Jacobian matrix for the two link arm, For a, a - 0 What is condition to have singularity? Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

5 Singularity Rank-deficient of J is kinematic singularities. det J = 0 Kinematic singularity example of two links arm, Jacobian matrix for the two link arm, For a, a - 0 What is condition to have singularity? Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

6 Singularity Rank-deficient of J is kinematic singularities. det J = 0 Kinematic singularity example of two links arm, Jacobian matrix for the two link arm, What is condition to have singularity? Boundary singularity Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

7 Singularity Decoupling Computation of internal singularities via the Jacobian determinant may be tedious and of no easy solution for complex structures. However, it is possible to compute singularity by splitting of manipulation structure Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

8 Singularity Decoupling- Anthropomorphic arm with wrist Ø computation of arm singularities resulting from the motion of the first 3 or more links, Ø computation of wrist singularities resulting from the motion of the wrist joints Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

9 Singularity Decoupling- Anthropomorphic arm with wrist Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

10 Singularity Decoupling Spherical wrist singularity The wrist kinematic structure reveals that a singularity occurs when z3 and z5 are aligned, i.e., whenever Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

11 Singularity Decoupling Spherical wrist singularity The wrist kinematic structure reveals that a singularity occurs when z3 and z5 are aligne d, i.e., whenever Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

12 Singularity Decoupling Anthropomorphic arm singularity analysis Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

13 Singularity Decoupling Anthropomorphic arm singularity analysis T 5 4 q represents the position and orientati on of the end-effectorframe with respect to the base frame T 7 8 q = A, 8 A -, A 7 - x 0 = c, c -7 s, c -7 s -7 0 c, s -7 s, s -7 c -7 0 s, c, 0 0 c, (a - c - + a 7 c -7 ) s, (a - c - + a 7 c -7 ) a - s - + a 7 s -7 1 P 3 = Position vector in the T(q) P 2 = Set a 3 = 0 in P 3 P 1 = Set a 2 = 0 and a 3 = 0 in P 3 Rotation Position Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

14 Singularity Decoupling Anthropomorphic arm singularity analysis Z 3 = R(q) = s, c, 0 is given by the rotation of z-axis unit vector [0 0 1] T Z 3 =Z 2 =Z 1 Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

15 Singularity Decoupling Anthropomorphic arm Jp (3 x 3) Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

16 Singularity Decoupling Determinant of a Jacobian matrix for a three links arm is Anthropomorphic arm has two singularity situations. 1. Elbow Singularity For a -, a 7 0, the determinant =0 if s 7 = 0 and/or (a - c - +a 7 c -7 ) = 0, Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

17 Singularity Decoupling Anthropomorphic arm has two singularity situations. 2. Shoulder Singularity it occurs when (a - c - +a 7 c -7 ) =0 Recalling the direct kinematic equations p C = c, (a - c - + a 7 c -7 ) p D = s, a - c - + a 7 c -7 p E = a - s - + a 7 s -7 For a -, a 7 0, wrist point lies on axis z0 Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

18 Singularity Decoupling Anthropomorphic arm has two singularity situations. 1. Elbow Singularity Determinant of a Jacobian matrix for a three links arm is For a -, a 7 = 0, the determinant vanishes if s 7 = 0 and/or (a - c - +a 7 c -7 ) = 0, Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

19 Singularity Decoupling Anthropomorphic arm has two singularity situations. 2. Shoulder Singularity it occurs when the wrist point lies on axis z0 Recalling the direct kinematic equations For a -, a 7 0, p C = c, (a - c - + a 7 c -7 ) p D = s, (a - c - + a 7 c -7 ) p E = a - s - + a 7 s -7 Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

20 REDUNDANT Manipulators The differential kinematic equation is v G is (r 1) vector of end-effector velocity J is (r n) Jacobian matrix, q is (n 1) vector of joint velocity if r < n, the manipulator is kinematically redundant and there exist (n r) redundant DOFs. Tasks Position in the plane Position in 3D space Orientation in the plane Position and orientation in 3D space r < M (Task Space) Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

21 Uses of robot redundancy Avoid collision with obstacles(in Cartesian space) Avoid kinematic singularities (in joint space) Stay within the admissible joint ranges Increase manipulability in specified directions Uniformly distribute/limit joint velocities and/or accelerations Minimize energy consumption or needed motion torques Optimize execution time Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

22 Redundancy Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

23 Example of Null Space ω = 0 a 7 a - a 7 0 a, a - a, 0 a - a, 0 0 a 7 a Row exchange and LU elimination a - a, 0 0 a 7 a x y z = 0 T C = D U V U W D U W = E U X Null space of A is λ a, a - a 7 Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

24 Example of Null Space a - a, 0 0 a 7 a x y z = 0 Null space of A is λ a, a - a 7 A p 0 If p 0 = x y z λ a 1 a 2 a 3 Ap 8 0 Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

25 Redundancy q is a solution the above equation, P is the projector of the Null space of J. q 8 is an arbitrary joint space vector Pq 0 is in the Null Space of J Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

26 REDUNDANT When the manipulator is redundant (r < n), the Jacobian matrix has more columns than rows and infinite solutions exist. A viable solution method is to formulate the problem as a constrained linear optimization problem. Let s minimize the quadratic cost functional of joint velocity W is a suitable (n n) symmetric positive definite weighting matrix. It can be solve d with the method of Lagrange multipliers. λ is an (r 1) vector of unknown multipliers that allows the incorporation of the co nstraint in the functional to minimize. Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

27 REDUNDANT The necessary conditions in solutions are From the first condition, g q = Wq λ h J h y = a h x y x = a Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

28 REDUNDANT The necessary conditions in solutions are From the first condition, g q = Wq λ h J h Since As W is positive definite, this solution is a minimum Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

29 REDUNDANT The necessary conditions in solutions are From the first condition, From the second condition, gives the constraint Combining the two conditions gives Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

30 REDUNDANT If J has full rank, JW A, J h is an square matrix of rank r and thus can be inverted. Substituted into the equation from the first condition. A particular case occurs when the weighting matrix W is the identity matrix I and t he solution simplifies into The matrix Right pseudo inverse of J Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

31 REDUNDANT For 3-link planar arm case Input for differential inverse kinematic is only tool position(x G = p C p ) D x d =[p xd, p yd ] * δx x e =[p x, p y ] e = x d x e δq = J A, q δx e [0,0] δθ, δθ - = a,s, a - s,- a 7 s,-7 a - s,- a 7 s,-7 a 7 s,-7 δp C a δθ, c, +a - c,- +a 7 c,-7 a - c,- +a 7 c,-7 a 7 c,-7 δp D 7 Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

32 REDUNDANT Example of redundancy in 3 links arm Input for differential inverse kinematic is only tool position(x G = p C p D ) Differential Inverse kinematic a, s, a - s,- a 7 s,-7 a - s,- a 7 s,-7 a 7 s,-7 δp C a, c, +a - c,- +a 7 c,-7 a - c,- +a 7 c,-7 a 7 c,-7 δp D r = 2 and n = 3 r < n redundant Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

33 REDUNDANT Apply the pseudo inverse equation Differential Inverse kinematic q = θ, θ - θ 7 p C p D a, s, a - s,- a 7 s,-7 a - s,- a 7 s,-7 a 7 s,-7 a, c, +a - c,- +a 7 c,-7 a - c,- +a 7 c,-7 a 7 c,-7 J h (J h J) A, Become 3 2 Jacobian matrix Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

34 REDUNDANT Apply the pseudo inverse equation Differential Inverse kinematic q = θ, θ - θ 7 p C p D a, s, a - s,- a 7 s,-7 a - s,- a 7 s,-7 a 7 s,-7 a, c, +a - c,- +a 7 c,-7 a - c,- +a 7 c,-7 a 7 c,-7 J h (J h J) A, Become 3 2 Jacobian matrix r > n Avoid the redundant Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

35 REDUNDANT Test the algorithm Generate a trajectory by: Given joint inputs are θ,, θ - and θ 7 = 0 to π Increment of θ,,θ - and θ 7 is Given θ,,θ - and θ 7 = 0 to π Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

36 REDUNDANT The differential inverse kinematic wit h position and orientation The differential inverse kinematic with p osition only(redundant Jacobian) Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

37 REDUNDANT Given joint inputs are θ,, θ - and θ 7 = 0 to π Increment of θ,,θ - and θ 7 is Output of the differential inverse kinematic is joint angular velocity(θ ) Given θ,,θ - and θ 7 = 0 to π Output θ,, θ - and θ 7 = 0 to π Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

38 REDUNDANT The robot end-effector reaches target position with small error but different orientation Tracking error Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

39 REDUNDANT Robot redundancy can be used to avoid collusion with obstacle. We need to consider a new cost functional in the form It aimed to minimize the norm of vector q q 8 Solutions are sought which satisfy the constraint ( ) and are as close as possible to q 8. Proceeding in a way similar to the above yields Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

40 REDUNDANT Proceeding in a way similar to the above yields From the first necessary condition it is which, substituted into ( ), gives v G = J(J h λ + q 8) v G Jq 8 = JJ h λ Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

41 REDUNDANT Proceeding in a way similar to the above yields From the first necessary condition it is Finally, substituting λ back in ( ) gives = J h JJ h A, q = J h JJ h A, v G J h JJ h A, Jq 8 + q 8 Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

42 REDUNDANT P = With v G = 0, internal motion is generated by (I J { J)q 8 Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

43 REDUNDANT How to specify the vector q 8 for a convenient utilization of redundant DOFs k 8 > 0 and ω(q) is a objective function of the joint variables. Since the solution moves along the direction of the gradient of the objective fun ction, it attempts to maximize it locally compatible to the primary objective (kine matic constraint) Typical objective functions are: The manipulability measure, defined as By maximizing this measure, redundancy is exploited to move away from singularities. Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

44 REDUNDANT How to specify the vector q 8 for a convenient utilization of redundant DOFs The distance from an obstacle, defined as Where o denotes the position vector of a suitable point on the obstacle (its centre) a nd P is the position vector of a generic point along the structure. By maximizing this distance, redundancy is exploited to avoid collision of the manipulator with an obstacle Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

45 REDUNDANT The distance from mechanical joint limits, defined as Where M m) denotes the maximum (minimum) joint limit and the middle value of the joint range. By maximizing this distance, redundancy is exploited to keep the joint variables as c lose as possible to the centre of their ranges. Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

46 REDUNDANT How to specify the vector q 8 for a convenient utilization of redundant DOFs The manipulability measure, defined as By maximizing this measure, redundancy is exploited to move away from singularities. Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

47 Example: Use redundancy for object avoidance Recall the final equation Ø If v G = 0, it is possible to generate internal motion Goal is to keep v e =0, while adjust the internal motion to let the arm a s far as possible to the object The initial configuration with the object Object Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

48 REDUNDANT Object avoidance Example given, q = θ, θ - θ 7 = v G = Target point J q = Into J(q) The initial configuration with the object Object J { = Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

49 REDUNDANT Object avoidance Example J { = Calculate the distance from an obstacle The initial configuration with the object p 7 ω q for the three links arm P, O - ω q = P - O - P 7 O - Find the minimum distance Object p, o p - Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London

50 REDUNDANT Calculate the distance from an obstacle ω q for the three links arm P, O - ω q = P - O - P 7 O - Find minimum distance. The initial configuration with the object Object p 7 Calculate q 8 o p - Update q 8 p, Robotic Endovascular Interventions Hamlyn Workshop June King s 2015College London