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(x, is y) L 2 [ ] [ ] x L L1 cos θ 1 + L 2 cos(θ 1 + θ 2) 1 θ 2 = = f(θ y L 1 sin θ 1 + L 2 sin(θ 1 + θ 1, θ 2) 2) θ 1 Inverse Kinematics Problem: Given (x, y), find (θ 1, θ 2) = f 1 (x, y) Inverse Kinematics Solution: { configurations. Righty Solution: θ 1 = γ α, θ 2 = π β Lefty Solution: θ 1 = γ + α, θ 2 = β π where 220 Workspace x 2 + y 2 θ 1 L 1 (a) A workspace, and lefty and righty (b) Geometric solution. (x, y) L 2 Figure 6.1: Inverse kinematics of a 2R planar open chain. L 1 θ 2 We also recall the law of cosines, θ 1 ( L γ = atan2(y, x), β = cos L 2 2 x2 y 2 ) c 2 = a 2 + b 2 2ab cos C, where a, 2L b, 1 and L 2 c are the lengths of the three sides of a triangle and C is t ( interior angle of x α = cos y 2 + L 2 1 ) the triangle opposite the side of length Workspace c. Referring L2 to Figure 6.1(b), angle β, restricted to lie in the interval [0, π], c 2 be 2L 1 (x 2 determined + y 2 from the law of cosines, (a) A workspace, and lefty and righty ) L L 2 configurations. 2 2L 1L 2 cos β = x 2 + y 2, Figure 6.1: Inverse kinematics from which it follows that ( L β = cos L 2 2 x 2 y 2 ) We also recall the. law of cosines, 2L Inverse Kinematics Problem Lecture 8 (ECE5463 Sp18) 1L 2 Wei Zhang(OSU) 2 24 / α γ β L 2 θ 2

5 Analytical Inverse Kinematics: PUMA-Type Arm 6R arm of PUMA type: The first 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 Analytic Inverse Kin ˆx 0 ẑ 0 θ 1 a2 a 3 p y θ 2 r θ 3 p z px ŷ0 Figure 6.2: Inverse position kinematics of a 6R PUMA-type arm. ẑ 0 d 1 ŷ ŷ 0 0 p y 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. Assume that p x, p y not both equal to zero. They can be used to θ determine two solutions of θ 2 1 Solutions: Analytic Inverse Kinematics Figure 6.2: ˆx 0 ẑ 0 θ 1 a2 a 3 p y ẑ 0 d 1 Figure 6.2: Inverse position kinematics of a 6R PUMA-type arm. (a) Elbow arm with offset. ẑ 0 d 1 r θ 3 p z px ŷ0 ˆx 0 Figure ŷ ŷ 0 0 p y that these joint axe p y The lengths of links anr offset at the sho r PUMA-type α arms ca subproblems, φ d as we 1 θwe 1 first consider θ 1 p x ˆx 0 to Figure p x 6.2 ˆx0 and e d 1 denote the compone p onto the ˆx 0 ŷ 0 -pla (a) Elbow arm (b) Kinematic diagram. with offset. 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 2a 3 ( θ 3 = atan2 ± ) 1 D 2, D Analytic Inverse Kinem Figure 6.5: Four possible inverse kinematics solutions for the 6R PUMA-type with shoulder offset. and θ2 can be obtained in a similar fashion as ( ) θ2 = atan2 pz, r 2 d 2 1 atan2 (a3s3, a2 + a3c3) ( ) = atan2 pz, p 2 x + p 2 y d 2 1 atan2 (a3s3, a2 + a3c3), where s3 = sinθ3 and c3 = cosθ3. The two solutions for θ3 correspon the well-known elbow-up and elbow-down configurations for the two-link pl arm. In general, a PUMA-type arm with an offset will have four solution the inverse position problem, as shown in Figure 6.5; the postures in the u panel are lefty solutions (elbow-up and elbow-down), while those in the l panel are righty solutions (elbow-up and elbow-down). We now solve the inverse orientation problem of finding (θ4, θ5, θ6) given end-effector orientation. This problem is completely straightforward: ha found (θ1, θ2, θ3), the forward kinematics can be manipulated into the form 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) e [S4]θ4 e [S5]θ5 e [S6]θ6 = e [S3]θ3 e [S2]θ2 e [S1]θ1 XM 1, where the right-hand side is now known, and the ωi-components of S4, S5, May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 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 3]θ 3 e [S 2]θ 2 e [S 1]θ 1 XM 1 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, we want to find θ d such that x d = f(θ d ). Taylor expansion around initial guess θ 0 : x d = f(θ d ) = f(θ 0 ) + f θ Let J(θ 0 ) = f θ θ 0 θ 0 (θ d θ 0 ) + h.o.t. 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, set i = 0 and select tolerance ɛ > 0. Set e = x d f(θ i ). While e > ɛ: 1. Set θ i+1 = θ i + J (θ i )e 2. Increment i Numerical Inverse Kinemat xd f(θ) xd f(θ0) slope = f θ (θ0) θ0 θ1 θd θ = f θ (θ0) 1 (xd f(θ0)) Figure 6.7: The first step of the Newton Raphson method for nonlinear root-find for a scalar x and θ. In the first step, the slope f/ θ is evaluated at the po (θ 0, xd f(θ 0 )). In the second step, the slope is evaluated at the point (θ 1, xd f(θ and eventually the process converges to θd. Note that an initial guess to the left the plateau of xd f(θ) would be likely to result in convergence to the other root xd f(θ), and an initial guess at or near the plateau would result in a large ini θ and the iterative process might not converge at all. If f(θ) is a linear function, the algorithm will converge to solution in one-step Given an initial guess θ 0 which is close to a solution θd, the kinematics c be expressed as the Taylor expansion xd = f(θd) = f(θ 0 ) + f θ (θd θ 0 ) + h.o.t., (6 θ 0 }{{}}{{} θ J(θ 0 ) 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 where J(θ 0 ) R m n is the coordinate Jacobian evaluated at θ 0. Truncat the Taylor expansion at first order, we can approximate Equation (6.3) as J(θ 0 ) θ = xd f(θ 0 ). (6 Assuming that J(θ 0 ) is square (m = n) and invertible, we can solve for θ a θ = J 1 (θ 0 ) ( xd f(θ 0 ) ). (6 If the forward kinematics is linear in θ, i.e., the higher-order terms in Equ tion (6.3) are zero, then the new guess θ 1 = θ 0 + θ exactly satisfies xd = f(θ Numerical Inverse Kinematics Lecture 8 (ECE5463IfSp18) the forward kinematics is not linear in θ, Wei as iszhang(osu) usually the case, the 11 new / 14gu θ

12 From Newton Method to Inverse Kinematics Solution Given desired configuration X = T sd SE(3), we want to find θ 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 ] = log T 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 = 0 and select a small error tolerance ɛ > 0 - Set [V b ] = log T 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