Non Holonomic Motion Planning - Duke University

Download this Presentation

0

Presentation Transcript

  • 1.Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013
  • 2.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
  • 3.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
  • 4.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
  • 5.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
  • 6.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
  • 7.Dynamics of Rigid Bodies
  • 8.Kinetic energy for rigid body Rigid body with velocity v, angular velocity w about COM KE = ½ (m vTv + wT H w) World-space inertia tensor H = R HL RT w v T w v H 0 0 m I 1/2
  • 9.Kinetic energy derivatives 𝜕𝐾𝐸 𝜕𝑣 = 𝑚 𝑣 Force (@CM) 𝑓 = 𝑑 𝑑𝑡 ( 𝜕𝐾𝐸 𝜕𝑣 )= 𝑚 𝑣 𝜕𝐾𝐸 𝜕𝜔 =𝐻𝜔 𝑑 𝑑𝑡 H = [w]H – H[w] Torque t = 𝑑 𝑑𝑡 𝜕𝐾𝐸 𝜕𝜔 = [w] H w + H 𝜔
  • 10.Summary 𝑓 = 𝑚 𝑣 𝜏 = [𝜔] 𝐻 𝜔 + 𝐻 𝜔 Gyroscopic “force”
  • 11.Force off of COM x F
  • 12.Force off of COM x F Consider infinitesimal virtual displacement 𝛿𝑥 generated by F. (we don’t know what this is, exactly) The virtual work performed by this displacement is FT𝛿𝑥 𝛿𝑥
  • 13.Generalized torque f Now consider the equivalent force f, torque τ at COM
  • 14.Generalized torque f Now consider the equivalent force f, torque τ at COM And an infinitesimal virtual displacement of R.B. coordinates 𝛿𝑞 𝛿𝑥 𝛿𝑞
  • 15.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] 𝛿𝑞
  • 16.Principle of virtual work f 𝛿𝑥 𝛿𝑞 [fT,τT] 𝛿𝑞= FT 𝛿𝑥 Since 𝛿𝑥=𝐽 𝑞 𝛿𝑞 we have [fT,τT] 𝛿𝑞= FT𝐽 𝑞 𝛿𝑞 F
  • 17.Principle of virtual work f 𝛿𝑥 𝛿𝑞 [fT,τT] 𝛿𝑞= FT 𝛿𝑥 Since 𝛿𝑥=𝐽 𝑞 𝛿𝑞 we have [fT,τT] 𝛿𝑞= FT𝐽 𝑞 𝛿𝑞 Since this holds no matter what 𝛿𝑞 is, we have [fT,τT] = FTJ(q), Or JT(q) F = F f τ
  • 18.Articulated Robot Dynamics
  • 19.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 𝑞
  • 20.Lagrangian Mechanics 𝐿 𝑞, 𝑞 =𝐾 𝑞, 𝑞 −𝑃 𝑞 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 Kinetic energy Potential energy
  • 21.Lagrangian Mechanics 𝐿 𝑞, 𝑞 =𝐾 𝑞, 𝑞 −𝑃 𝑞 Minimum action condition => Euler-Lagrange equations of motion: 𝑑 𝑑𝑡 𝜕𝐿 𝜕 𝑞 − 𝜕𝐿 𝜕𝑞 =𝑢 Note that P is independent of 𝑞 , so 𝑑 𝑑𝑡 𝜕𝐾 𝜕 𝑞 − 𝜕𝐾 𝜕𝑞 + 𝜕𝑃 𝜕𝑞 =𝑢 A system of n partial differential equations
  • 22.Sanity check: Newton’s laws Example: Point Mass Coordinates q = (x,y) Potential field P(x,y) Lagrangian: 𝐿 𝑞, 𝑞 = 1 2 𝑚 𝑥 2 + 𝑦 2 −𝑃 𝑥,𝑦 Equations of motion 𝑑 𝑑𝑡 𝑚 𝑥 + 𝜕𝑃 𝜕𝑥 =𝑚 𝑥 + 𝜕𝑃 𝜕𝑥 = 𝑢 𝑥 𝑑 𝑑𝑡 𝑚 𝑦 + 𝜕𝑃 𝜕𝑦 = 𝑚 𝑦 + 𝜕𝑃 𝜕𝑦 =𝑢 𝑦
  • 23.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
  • 24.Derivative of K.E. w.r.t 𝑞 𝜕𝐾 𝜕 𝑞 𝑞, 𝑞 =𝐵 𝑞 𝑞 𝑑 𝑑𝑡 𝜕𝐾 𝜕 𝑞 =𝐵 𝑞 𝑞 + 𝑑 𝑑𝑡 𝐵 𝑞 𝑞 =𝐵(𝑞) 𝑞 + 𝑖 𝑞 𝑖 𝜕 𝜕 𝑞 𝑖 𝐵 𝑞 𝑞
  • 25.Derivative of K.E. w.r.t q 𝜕 𝜕𝑞 𝐾 𝑞, 𝑞 =½ 𝑞 𝑇 𝜕 𝜕 𝑞 1 𝐵(𝑞) 𝑞 … 𝑞 𝑇 𝜕 𝜕 𝑞 𝑛 𝐵(𝑞) 𝑞
  • 26.Potential energy for articulated robot in gravity field 𝜕𝑃 𝜕𝑞 = 𝑖 𝜕 𝑃 𝑖 𝜕𝑞 𝜕 𝑃 𝑖 𝜕𝑞 = 𝑚 𝑖 0 0 𝑔 𝑇 𝑣 𝑖 = 𝑚 𝑖 0 0 𝑔 𝑇 𝐽𝑖𝑝(𝑞) G(q) Generalized gravity
  • 27.Putting it all together 𝑑 𝑑𝑡 𝜕𝐾 𝜕 𝑞 − 𝜕𝐾 𝜕𝑞 + 𝜕𝑃 𝜕𝑞 =𝑢 𝐵 𝑞 𝑞 + 𝑑 𝑑𝑡 𝐵(𝑞) 𝑞 – ½ 𝑞 𝑇 𝜕 𝜕 𝑞 1 𝐵(𝑞) 𝑞 … 𝑞 𝑇 𝜕 𝜕 𝑞 𝑛 𝐵(𝑞) 𝑞 + 𝐺(𝑞) = 𝑢 Group these terms together
  • 28.Final canonical form 𝐵 𝑞 𝑞 +𝐶 𝑞, 𝑞 +𝐺(𝑞) = 𝑢 Generalized inertia Centrifugal/coriolis forces Generalized gravity Generalized forces (joint torques + external forces)
  • 29.Forward/Inverse Dynamics Given 𝑢, 𝑞, and 𝑞 , find 𝑞 From torques to accelerations 𝑞 = 𝐵 𝑞 −1 (𝑢−𝐶(𝑞, 𝑞 )−𝐺(𝑞) ) Given 𝑞, 𝑞 , and 𝑞 , find 𝑢 From desired accelerations to necessary torques 𝑢=𝐵 𝑞 𝑞 +𝐶(𝑞, 𝑞 )+𝐺(𝑞)
  • 30.Example: RP manipulator
  • 31.Application: Effective Inertia If a force 𝑓 is applied to a point 𝑝 on a robot, how much will 𝑝 accelerate?
  • 32.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 𝐽(𝑞) 𝑇 𝑓
  • 33.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 𝐽(𝑞) 𝑇 𝑓 𝑝 =𝐽 𝑞 𝑞 =𝐽(𝑞)𝐵 𝑞 −1 𝐽(𝑞) 𝑇 𝑓 The matrix 𝐽 𝑞 𝐵 𝑞 −1 𝐽 𝑞 𝑇 −1 is called the effective inertia matrix 𝑓= 𝐽 𝑞 𝐵 𝑞 −1 𝐽 𝑞 𝑇 −1 𝑝 Can be infinite at singular configurations!
  • 34.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
  • 35.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
  • 36.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…
  • 37.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)
  • 38.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
  • 39.Software Both Lagrangian dynamics and Newton-Euler methods are implemented in KrisLibrary Lagrangian form is usually most mathematically convenient representation
  • 40.Constrained Dynamics
  • 41.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?
  • 42.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!
  • 43.The Right Way… Constrained system of equations: 𝐵 𝑞 𝑞 +𝐶 𝑞, 𝑞 +𝐺 𝑞 =𝑢+𝐴 𝑞 𝑇 𝜆 (1) 𝑑 𝑑𝑡 𝐴 𝑞 𝑞 = 𝐴 𝑞 𝑞 +𝐴 𝑞 𝑞 =0 (2) Lagrange multipliers have been introduced 𝜆= 𝜆 1 ,…, 𝜆 𝑘 𝑇 𝜆 can be thought of as constraint forces Solve for n+k variables 𝑞 ,𝜆
  • 44.Solving… Constrained system of equations: 𝐵 𝑞 𝑞 +𝐶 𝑞, 𝑞 +𝐺 𝑞 =𝑢+𝐴 𝑞 𝑇 𝜆 (1) 𝑑 𝑑𝑡 𝐴 𝑞 𝑞 = 𝐴 𝑞 𝑞 +𝐴 𝑞 𝑞 =0 (2) Solve for n+k variables 𝑞 ,𝜆 A solution must satisfy 𝑞 = 𝐵 −1 𝑢+ 𝐴 𝑇 𝜆−𝐶−𝐺 (3) solve 1 for 𝑞 𝐴 𝑞 +𝐴 𝐵 −1 𝑢+ 𝐴 𝑇 𝜆−𝐶−𝐺 =0 (4) subst (3) in (2) 𝜆= 𝐴 𝐵 −1 𝐴 𝑇 −1 𝐴 𝑞 +𝐴 𝐵 −1 𝐶+𝐺−𝑢 (5) solve for 𝜆 in (4), use − 𝐴 𝑞 =𝐴 𝑞 from (2) 𝑃 𝑞 =𝑃 𝐵 −1 (𝑢−𝐶−𝐺) (6) more manipulations.. With 𝑃=𝐼− 𝐵 −1 𝐴 𝑇 𝐴 𝐵 −1 𝐴 𝑇 −1 𝐴
  • 45.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 (overconstrained) A+ = AT(AAT)-1 when AAT is invertible (underconstrained)
  • 46.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
  • 47.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
  • 48.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!
  • 49.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 𝐵 𝑞 𝑞 +𝐶 𝑞, 𝑞 +𝐺 𝑞 =𝑢+𝐴 𝑞 𝑇 𝜆 (1) 𝑑 𝑑𝑡 𝐴 𝑞 𝑞 = 𝐴 𝑞 𝑞 +𝐴 𝑞 𝑞 =0 (2) Solve for n+k variables 𝑞 ,𝜆 (1), (2) are sparse systems and are solved using specialized solvers More on frictional contact later…
  • 50.Next class Feedback control Principles App J