Upload
posy-pitts
View
213
Download
0
Embed Size (px)
Citation preview
Dynamics of Articulated Robots
Rigid Body Dynamics
The following can be derived from first principles using Newton’s laws + rigidity assumption
Parameters CM translation x(t) CM velocity v(t) Rotation R(t) Angular velocity (t) Mass m, local inertia tensor HL
Kinetic energy for rigid body
Rigid body with velocity v, angular velocity KE = ½ (m vTv + T H )
World-space inertia tensor H = R HL RT
v
T
v
H 0 0 m I
1/2
Kinetic energy derivatives
KE/v = m v Force (@CM) f = d/dt (KE/v) = m v’
KE/ = H d/dt H = []H – H[] Torque = d/dt (KE/) = [] H H ’
Summary
f = m v’ = [] H H ’
Robot Dynamics
Configuration q, velocity q’ Rn
Generalized forces u Rm
Joint torques/forces If m < n we say robot is underactuated
How does u relate to q and q’?
Articulated Robots
Treat each link as a rigid body Use Langrangian mechanics to
determine dynamics of q, q’ as a function of generalized forces u
(Derivation: principle of virtual work)
Lagrangian Mechanics
L(q,q’) = K(q,q’) – P(q)
Lagrangian equations of motion:d/dt L/q’ - L/q = u
Kinetic energy Potential energy
Lagrangian Approach
L(q,q’) = K(q,q’) – P(q)
Lagrangian equations of motion:d/dt L/q’ - L/q = u
L/q’ = K/q’ L/q = K/q - P/q
Kinetic energy Potential energy
Kinetic energy for articulated robot K(q,q’) = i Ki(q,q’) Velocity of i’th rigid body
vi = Jit(q) q’
Angular velocity of i’th rigid bodyi = Ji
r(q) q’ Ki = ½ q’T(miJi
t(q)TJit(q) + Ji
r(q)THi(q)Jir(q))q’
K(q,q’) = ½ q’T B(q) q’Mass matrix
Derivative of K.E. w.r.t q’
/q’ K(q,q’) = B(q) q’ d/dt (/q’ K(q,q’)) = B(q) q’’ + d/dt B(q) q’
Derivative of K.E. w.r.t q
/q K(q,q’) = ½ q’T /q1 B(q) q’…
q’T /qn B(q) q’
Potential energy for articulated robot in gravity field
P/q = i Pi/q
Pi/q = mi (0,0,g)T vi = mi (0,0,g)T
Jit(q) q’
-G(q)Generalized gravity
Putting it all together
d/dt K/q’ - K/q - P/q = u
B(q) q’’ + d/dt B(q) q’ – ½ + G(q) = uq’T /q1 B(q) q’
…q’T /qn B(q) q’
Putting it all together
d/dt K/q’ - K/q - P/q = u
B(q) q’’ + d/dt B(q) q’ – ½ + G(q) = uq’T /q1 B(q) q’
…q’T /qn B(q) q’
Final canonical form
B(q) q’’ + C(q,q’) + G(q) = u
Mass matrix Centrifugal/coriolis forces
Generalized gravity
Generalized forces
Forward/Inverse Dynamics
Given u, find q’’q’’ = M(q)-1 (u - C(q,q’) - G(q) )
Given q,q’,q’’, find uu = M(q) q’’ + C(q,q’) + G(q)
Newton-Euler Method (Featherstone 1984)
Explicitly solves a linear system for joint constraint forces and accelerations, related via Newton’s equations
Faster forward/inverse dynamics for large chains (O(n) vs O(n3))
Lagrangian form still mathematically handy
Software
Both Lagrangian dynamics and Newton-Euler methods are implemented in KrisLibrary
Application: Feedforward control
Joint PID loops do not follow joint trajectories accurately
Include feedforward torques to reduce reliance on feedback
Estimate the torques that would compensate for gravity and coriolis forces
Application: Feedforward control
Joint PID loops do not follow joint trajectories accurately
Include feedforward torques to reduce reliance on feedback
Estimate the torques that would compensate for gravity and coriolis forces
Feedforward Torques
Given q,q’,q’’ of trajectory 1. Estimate M, C, G 2. Compute u
u = M(q) q’’ + C(q,q’) + G(q) 3. Add u into joint PID loops