Dynamics of rigid bodies and of articulated robots
advertisement
Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013 Agenda • Basic elements of simulation • Derive the standard form of the dynamics of an articulated robot in joint space • Also works for humans, biological systems, non-actuated mechanical systems … • Featherstone algorithm: fast method for computing forward dynamics (torques to accelerations) and inverse dynamics (accelerations to torques) • Constrained dynamics Rigid Body Dynamics • The following can be derived from first principles using Newton’s laws + rigidity assumption • Parameters • • • • • CM translation c(t) CM velocity v(t) Rotation R(t) Angular velocity w(t) Mass m, local inertia tensor HL Rigid body ordinary differential equations • We will express forces and torques in terms of terms of H (a function of R), π, π£ and π • π = ππ£ • π = [π] π» π + π» π • Rearrange… • π£ = π/π • π = π» −1 (π − π π» π ) • So knowing f(t) and τ(t), we can derive c(t), v(t), R(t), and w(t) by solving an ordinary differential equation (ODE) • dx/dt = f(x) • x(0) = x0 • With x=(c,v,R,w) the state of the system • Numerical integration, also known as simulation Articulated body ODEs • We will express joint torques π in terms of terms of π, π, and π and external forces f • π΅ π π + πΆ π, π + πΊ π = π + π½π π π • Rearrange… • π=π΅ π −1 π + π½π π π − πΆ π, π − πΊ π • An ODE in the state space x=(π, π) • ππ₯ ππ‘ = ππ ππ‘ ππ ππ‘ = π π, π = π΅ π −1 π π + π½π π π − πΆ π, π − πΊ π • Solve using numerical integration Numerical integration of ODEs • If dx/dt = f(x) and x(0) are known, then given a step size h, • x(kh) ο» xk = xk-1 + h f’(xk-1) • gives an approximate trajectory for k ο³1 • Provided f is smooth • Accuracy depends on h • Known as Euler’s method • Better integration schemes are available • (e.g., Runge-Kutta methods, implicit integration, adaptive step sizes, etc) • Beyond the scope of this course DYNAMICS OF RIGID BODIES Kinetic energy for rigid body • Rigid body with velocity v, angular velocity w • KE = ½ (m vTv + wT H w) • World-space inertia tensor H = R HL RT 1/2 w v T H 0 0 mI w v Kinetic energy derivatives • ππΎπΈ ππ£ = ππ£ • Force (@CM) π = • • ππΎπΈ ππ π H ππ‘ π ππΎπΈ ( ) ππ‘ ππ£ = ππ£ = π»π = [w]H – H[w] • Torque t = π ππΎπΈ ππ‘ ππ = [w] H w + H π Summary •π = ππ£ • π = [π] π» π + π» π Gyroscopic “force” Force off of COM F x Force off of COM F πΏπ₯ x Consider infinitesimal virtual displacement πΏπ₯ generated by F. (we don’t know what this is, exactly) The virtual work performed by this displacement is FTπΏπ₯ Generalized torque f Now consider the equivalent force f, torque τ at COM Generalized torque f πΏπ₯ πΏπ Now consider the equivalent force f, torque τ at COM And an infinitesimal virtual displacement of R.B. coordinates πΏπ Generalized torque f πΏπ₯ πΏπ Now consider the equivalent force f, torque τ at COM And an infinitesimal virtual displacement of R.B. coordinates πΏπ Virtual work in configuration space is [fT,τT] πΏπ Principle of virtual work f F πΏπ₯ πΏπ [fT,τT] πΏπ= FT πΏπ₯ Since πΏπ₯ = π½ π πΏπ we have [fT,τT] πΏπ= FTπ½ π πΏπ Principle of virtual work f F πΏπ₯ πΏπ [fT,τT] πΏπ= FT πΏπ₯ Since πΏπ₯ = π½ π πΏπ we have [fT,τT] πΏπ= FTπ½ π πΏπ Since this holds no matter what πΏπ is, we have [fT,τT] = FTJ(q), f Or JT(q) F = τ ARTICULATED ROBOT DYNAMICS Robot Dynamics • Configuration π, velocity π ο Rn • Generalized forces u ο Rm • π’ = π + π π½π π πππ • Joint torques π and external forces ππ • How does u relate to π and π? • Use Langrangian mechanics to find a link between u and π Lagrangian Mechanics • πΏ π, π = πΎ π, π − π π Kinetic energy Potential energy • The trajectory between two states (π0 , π0 ), ππ , ππ is the one that minimizes the “action” π π = 0 πΏ π, π ππ‘ • πΏ π, π is defined such that the path minimizing S is equivalent to the one produced by Newton’s laws, subject to the constraints that the system only moves along coordinates q Lagrangian Mechanics • πΏ π, π = πΎ π, π − π π • Minimum action condition => Euler-Lagrange equations of motion: • π ππΏ ππ‘ ππ − ππΏ ππ =π’ A system of n partial differential equations • Note that P is independent of π, so • π ππΎ ππ‘ ππ − ππΎ ππ + ππ ππ =π’ Example: Point Mass • Coordinates q = (x,y) • Potential field P(x,y) 1 2 • Lagrangian: πΏ π, π = π π₯ 2 + π¦ 2 − π π₯, π¦ • Equations of motion • • π ππ‘ π ππ‘ ππ ππ₯ ππ + ππ¦ ππ ππ₯ ππ + ππ¦ ππ₯ + = ππ₯ + = π’π₯ ππ¦ = ππ¦ = π’π¦ Sanity check: Newton’s laws Kinetic energy for articulated robot • πΎ(π, π) = π πΎπ (π, π) • Velocity of i’th rigid body π • π£π = π½π π π • Angular velocity of i’th rigid body • ππ = π½ππ(π)π • πΎπ = 1 π π (πππ½ππππ½ππ 2 + π½ππππ»ππ½ππ)π • πΎ(π, π) = ½π π π΅(π)π Mass matrix: symmetric positive definite Derivative of K.E. w.r.t π ππΎ • ππ π, π = π΅ π π π ππΎ • ππ‘ ππ =π΅ π π+ • = π΅(π) π + π ππ π π΅ ππ‘ π π΅ πππ π π π π Derivative of K.E. w.r.t q π πΎ π, π = ½ ππ π π΅(π)π ππ1 … π π π π΅(π)π πππ ππ Potential energy for articulated robot in gravity field ππ • ππ πππ • ππ = πππ π ππ = ππ 0 0 0 π£π = ππ 0 π½ππ(π) π π • G(q) Generalized gravity Putting it all together π ππΎ • ππ‘ ππ ππΎ − ππ • π΅ π π+ + ππ ππ π π΅(π)π ππ‘ =π’ –½ π π π π΅(π)π ππ1 ππ … + πΊ(π) = π’ π π΅(π)π πππ Group these terms together Final canonical form • π΅ π π + πΆ π, π + πΊ(π) = π’ Generalized inertia Centrifugal/ coriolis forces Generalized gravity Generalized forces (joint torques + external forces) Forward/Inverse Dynamics • Given π’, π, and π, find π • From torques to accelerations • π = π΅ π −1 (π’ − πΆ(π, π) − πΊ(π) ) • Given π, π, and π, find π’ • From desired accelerations to necessary torques • π’ = π΅ π π + πΆ(π, π) + πΊ(π) Example: RP manipulator Application: Effective Inertia • If a force π is applied to a point π on a robot, how much will π accelerate? Application: Effective Inertia • If a force π is applied to a point π on a robot, how much will π accelerate? • Assume a stationary system, no acceleration when no force is applied • π0 = π΅ π −1 π − πΆ π, π − πΊ π =0 • With the force: •π= π΅ π −1 π½(π)π π Application: Effective Inertia • If a force π is applied to a point π on a robot, how much will π accelerate? • Assume a stationary system, no acceleration when no force is applied • π0 = π΅ π −1 π − πΆ π, π − πΊ π =0 • With the force: • π = π΅ π −1 π½(π)π π • π = π½ π π = π½(π)π΅ π • The matrix π½ π π΅ π inertia matrix • π= π½ π π΅ π −1 π½ −1 π½ π π −1 π½(π)π π π −1 is called the effective π −1 π Can be infinite at singular configurations! Application: Feedforward control • Feedback control: let torques be a function of the current error between actual and desired configuration • Problem: heavy arms require strong torques, requiring a stiff system • Stiff systems become unstable relatively quickly Application: Feedforward control • Solution: include feedforward torques to reduce reliance on feedback • Estimate the torques that would compensate for gravity and coriolis forces, send those torques to the motors Feedforward Torques • Given current π, π, desired π • 1. Estimate B, C, G • 2. Compute u • π’ = π΅ π π + πΆ(π, π) + πΊ(π) • 3. Apply torques u • How to compensate for errors in B,C,G? Combine feedforward with feedback. More in later classes… Newton-Euler Method (Featherstone 1984) • Explicitly solves a linear system for joint constraint forces and accelerations, related via Newton’s equations • No matrix larger than 6x6 • Faster forward/inverse dynamics for large chains (O(n) vs O(n3) for direct matrix computations) Forward Dynamics: Basic Intuition • Downward recursion: Starting from root, compute “articulated body inertia matrix” for each link • 6x6 matrix πΌππ΄ relating (π, π) vectors to translational/angular accelerations (a, πΌ) respectively • Also need a “bias force” ππ • π, π = πΌππ΄ πΌ, π + ππ • Upward recursion: Starting from leaves, compute accelerations on links • Given (π, π) acting on i’th link, compute acceleration of joint i and the joint constraint forces on the i-1’th link • (π, π) includes external forces + joint constraint forces from downward links Software • Both Lagrangian dynamics and Newton-Euler methods are implemented in KrisLibrary • Lagrangian form is usually most mathematically convenient representation CONSTRAINED DYNAMICS Constrained Systems • Suppose the system is constrained by π΄ π π = 0 • E.g., closed-chains, contact constraints, rolling constraints • A is a k x n matrix (k constraints) • How does π evolve over time? The Wrong Way • Suppose the system is constrained by π΄ π π = 0 • E.g., closed-chains, contact constraints, rolling constraints • A is a k x n matrix (k constraints) • How does π evolve over time? • Wrong way: • π ππ‘ π΄ π π =π΄ π π+π΄ π π =0 • Solve for π as usual, then project it onto the subspace that satisfies this equation, obtaining πππππ π‘ • The correct answer will be a projection, but a very specific one! The Right Way… • Constrained system of equations: • π΅ π π + πΆ π, π + πΊ π = π’ + π΄ π π π • π ππ‘ π΄ π π =π΄ π π+π΄ π π =0 • Lagrange multipliers have been introduced • π = π1 , … , ππ π • π can be thought of as constraint forces • Solve for n+k variables π, π (1) (2) Solving… • Constrained system of equations: • π΅ π π + πΆ π, π + πΊ π = π’ + π΄ π π π • π ππ‘ π΄ π π =π΄ π π+π΄ π π =0 (1) (2) • Solve for n+k variables π, π • A solution must satisfy • π = π΅−1 π’ + π΄π π − πΆ − πΊ • π΄π + π΄π΅−1 π’ + π΄π π − πΆ − πΊ = 0 • π = π΄π΅−1 π΄π −1 (3) solve 1 for π (4) subst (3) in (2) π΄π + π΄π΅−1 πΆ + πΊ − π’ (5) solve for π in (4), use −π΄π = π΄π from (2) • ππ = ππ΅−1 (π’ − πΆ − πΊ) • With π = πΌ − π΅−1 π΄π π΄π΅−1 π΄π (6) more manipulations.. −1 π΄ Back to Pseudoinverses • A pseudoinverse A# of the matrix A is a matrix such that • A = AA#A • A# = A#AA# • Generalizes the concept of inverse to non-square, noninvertible matrices • Such a matrix exists (in fact, there are infinitely many) • The Moore-Penrose pseudoinverse, denoted A+, can be derived as • A+ = (ATA)-1AT when ATA is invertible • A+ = AT(AAT)-1 when AAT is invertible (overconstrained) (underconstrained) Properties • Note connection to least-squares formula • Ax=b => x = A+b • If system is overconstrained, this solution minimizes ||b-Ax||2 • If system is underconstrained, this solution minimizes ||x||2 • Note that (I-AA+)Ay = 0 is always satisfied • (I-AA+) is a projection matrix Weighted Pseudoinverse • If (AAT)-1 exists, given any positive definite weighting matrix W, we can derive a new pseudoinverse • A# = W-1AT(AW-1AT)-1 • This is a weighted pseudoinverse • Has the property that x=A#b is a solution to Ax = b such that • x minimizes xTWx – a weighted norm Weighted Pseudoinverse • If (AAT)-1 exists, given any positive definite weighting matrix W, we can derive a new pseudoinverse • A# = W-1AT(AW-1AT)-1 • This is a weighted pseudoinverse • Has the property that x=A#b is a solution to Ax = b such that • x minimizes xTWx – a weighted norm • Revisiting constrained dynamics… • The P projection matrix solves for π such that ππ» π©π is minimized • Constraint forces dissipate kinetic energy in a minimal fashion! Rigid Body Simulators • Articulated robots are often simulated as a set of connected rigid bodies (Open Dynamics Engine, Bullet, etc) • Connections give rise to constraints in the dynamics • π΅ π π + πΆ π, π + πΊ π = π’ + π΄ π π π • π ππ‘ π΄ π π =π΄ π π+π΄ π π =0 (1) (2) • Solve for n+k variables π, π • (1), (2) are sparse systems and are solved using specialized solvers • More on frictional contact later… Next class • Feedback control • Principles App J
