Lecture Note 8: Inverse Kinematics

Transcription

1 ECE5463: Introduction to Robotics Lecture Note 8: Inverse Kinematics Prof. Wei Zhang Department of Electrical and Computer Engineering Ohio State University Columbus, Ohio, USA Spring 2018 Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 1 / 14

2 Outline Inverse Kinematics Problem Analytical Solution for PUMA-Type Arm Numerical Inverse Kinematics Outline Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 2 / 14

3 Inverse Kinematics Problem Inverse Kinematics Problem: Given the forward kinematics T (θ),θ R n and the target homogeneous transform X SE(3), find solutions θ that satisfy T (θ) =X Multiple solutions may exist; they are challenging to characterize in general This lecture will focus on: - Simple illustrating example - Analytical solution for PUMA-type arm - Numerical solution using the Newton-Raphson method Inverse Kinematics Problem Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 3 / 14

4 Example: 2-Link Planar Open Chain 2-link planar open chain: considering only the end-effector position and ignoring its orientation, the forward kinematics is [ ] [ ] x L1 cos θ 1 + L 2 cos(θ 1 + θ 2 ) = = f(θ y L 1 sin θ 1 + L 2 sin(θ 1 + θ 2 ) 1,θ 2 ) Inverse Kinematics Problem: Given (x, y), find (θ 1,θ 2 )=f 1 (x, y) Inverse Kinematics Solution: { Righty Solution: θ 1 = γ α, θ 2 = π β Lefty Solution: θ 1 = γ + α, θ 2 = β π x 2 + y 2 β α γ L 2 θ 2 L 1 θ 1 (x, y) L 2 L 1 θ 2 where ( L γ = atan2(y, x), β=cos L 2 2 x2 y 2 ) 2L 1 L 2 ) Workspace θ 1 α =cos 1 ( x 2 + y 2 + L 2 1 L2 2 2L 1 (x 2 + y 2 ) Inverse Kinematics Problem Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 4 / 14

5 Analytical Inverse Kinematics: PUMA-Type Arm 6R arm of PUMA type: Two shoulder joint axes intersect orthogonally at a common point Joint axis 3 lies in ˆx 0 ŷ 0 plane and is aligned with joint axis 2 Joint axes 4,5,6 (wrist joints) intersect orthogonally at a common point (the wrist center) For PUMA-type arms, the inverse Kinematics problem can be decomposed into inverse position and inverse orientation subproblems ˆx 0 ẑ 0 θ 1 a 2 a 3 θ 2 p y r θ 3 p z p x ŷ 0 Analytical Solution Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 5 / 14

6 PUMA-Type Arm: Inverse Position Subproblem Given desired configuration X =(R, p) SE(3). Clearly, p =(p x,p y,p z ) depends only on θ 1,θ 2,θ 3. Solving for (θ 1,θ 2,θ 3 ) based on given (p x,p y,p z ) is the inverse position problem. ẑ 0 d 1 Assume that p x,p y not both equal to zero. They can be used to determine two solutions of θ 1 Solutions: ŷ 0 ŷ 0 p y p y r d 1 r α φ θ 1 θ 1 d 1 p x ˆx 0 p x ˆx 0 Analytical Solution Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 6 / 14

7 PUMA-Type Arm: Inverse Position Subproblem Determining θ 2 and θ 3 is inverse kinematics problem for a planar two-link chain cos(θ 3 )= p 2 d 2 1 a 2 2 a 2 3 = D 2a 2 a 3 ( θ 3 = atan2 ± ) 1 D 2,D The solutions of θ 3 corresponds to the elbow-up and elbow-down configurations for the two-link planar arm. Similarly, we can find: ( ) θ 2 = atan2 p z, p 2 x + p 2 y d 2 1 atan2 (a 3 sin θ 3,a 2 + a 3 cos θ 3 ) Analytical Solution Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 7 / 14

8 PUMA-Type Arm: Inverse Orientation Subproblem Now we have found (θ 1,θ 2,θ 3 ), we can determine (θ 4,θ 5,θ 6 ) given the end-effector orientation The forward kinematics can be written as: e [S 4]θ 4 e [S 5]θ 5 e [S 6]θ 6 = e [S 1]θ 1 e [S 2]θ 2 e [S 3]θ 3 XM 1 = 3 where the right-hand side is now known (denoted by R) For simplicity, we assume joint axes of 4,5,6 are aligned in the ẑ 0, ŷ 0 and ˆx 0 directions, respectively; Hence, the ω i components of S 4, S 5, S 6 are ω 4 =(0, 0, 1),ω 4 =(0, 1, 0),ω 6 =(1, 0, 0) Therefore, the wrist angles can be determined as the solution to Rot(ẑ, θ 4 )Rot(ŷ, θ 5 )Rot(ˆx, θ 6 )= R This corresponds to solving for ZY X Euler angles given R SO(3), whose analytical solution can be found in Appendix B.1.1 of the textbook. Analytical Solution Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 8 / 14

9 Numerical Inverse Kinematics Inverse kinematics problem can be viewed as finding roots of a nonlinear equation: T (θ) =X Many numerical methods exist for finding roots of nonlinear equations For inverse kinematics problem, the target configuration X SE(3) is a homogeneous matrix. We need to modify the standard root finding methods. But the main idea is the same. We first recall the standard Newton-Raphson method for solving x = f(θ), where θ R n and x R m. Then we will discuss how to modify the method to numerically solve the inverse kinematics problem. Numerical Inverse Kinematics Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 9 / 14

10 Newton-Raphson Method Given f : R n R m,wewanttofindθ d such that x d = f(θ d ). Taylor expansion around initial guess θ 0 : x d = f(θ d )=f(θ 0 )+ f θ θ 0 (θ d θ 0 )+h.o.t. Let J(θ 0 )= f θ θ 0 and drop the h.o.t., we can compute Δθ as Δθ = J (θ 0 )(x d f(θ 0 )) - J denotes the Moore-Penrose pseudoinverse - For any linear equation: b = Az, the solution z = A b falls into the following two categories: 1. Az = b 2. Az b Az b, z R n Numerical Inverse Kinematics Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 10 / 14

11 Newton-Raphason Method Algorithm: Initialization: Given x d R m and an initial guess θ 0 R n,seti =0and select tolerance ɛ>0. Set e = x d f(θ i ). While e >ɛ: 1. Set θ i+1 = θ i + J (θ i )e 2. Increment i. x d f(θ) x d f(θ 0 ) slope = f θ (θ 0) θ 0 θ 1 θ d Δθ = f θ (θ 0) 1 (x d f(θ 0 )) θ If f(θ) is a linear function, the algorithm will converge to solution in one-step If f is nonlinear, there may be multiple solutions. The algorithm tends to converge to the solution that is the closest to the initial guess θ 0 Numerical Inverse Kinematics Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 11 / 14

12 From Newton Method to Inverse Kinematics Solution Given desired configuration X = T sb SE(3), wewanttofindθ d R n such that T sb (θ d )=T sd At the ith iteration, we want to move towards the desired position: - In vector case, the direction to move is e = x d f(θ i ) - Meaning: e is the velocity vector which, if followed for unit time, would cause a motion from f(θ i ) to x d - Thus, we should look for a body twist V b which, if followed for unit time, would cause a motion from T sb (θ i ) to the desired configuration T sd. - Such a body twist is given by [V b ]=logtt bd (θ i ), where T bd (θ i )=T 1 sb (θ i )T sd To achieve a desired body twist, we need the joint rate vector: Δθ = J b (θi )V b Numerical Inverse Kinematics Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 12 / 14

13 Numerical Inverse Kinematics Algorithm Algorithm: - Initialization: Given: T sd and initial guess θ 0 Set i =0and select a small error tolerance ɛ>0 - Set [V b ]=logt bd (θ i ). While V b >ɛ: 1. Set θ i+1 = θ i + J b (θi )V b 2. Increment i An equivalent algorithm can be developed in the space frame, using the space Jacobian J s and the spatial twist V s =[Ad Tsb ] V b Numerical Inverse Kinematics Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 13 / 14

14 More Discussions Numerical Inverse Kinematics Lecture 8 (ECE5463 Sp18) Wei Zhang(OSU) 14 / 14