Robotics & Automation Lecture 25 Dynamics of Constrained Systems, Dynamic Control John T. Wen April 26, 2007
Last Time Order N Forward Dynamics (3-sweep algorithm) Factorization perspective: causal-anticausal (LDU decomposition) + state space (group) structure leads to order N inverse dynamics (matrix-vector multiplication). Inverse dynamics: Kalman filter interpretation, mass matrix inverse can be expressed in terms of Kalman filter (written as whitening filter). Today: Control of serial robot, dynamics of constrained mechanism (single constrained arm, parallel platform type of mechanism), dynamics of rolling wheel, dynamics of general mechanisms, joint flexibility. April 26, 2007Copyrighted by John T. Wen Page 1
Dynamics of Closed Kinematic Chain Consider the slider crank system: q 2 q 1 q 3 Kinematic constraints, φ(θ) = 0: θ T = θ 1 + θ 2 + θ 3 = 0 y T = l 1 s 1 +l 2 s 12 +l 3 s 123 = 0. April 26, 2007Copyrighted by John T. Wen Page 2
Differentiate to obtain the velocity constraints: θ T v T = ẋ T ẏ T = J(θ) θ = 0 1 0 }{{} A ẋ T. }{{} v C From the force perspective, if we regard the load as part of the arm, then f Tx = 0, or A T f T = 0 where f T = If we regard the load as separate, then f Tx causes motion of the load: m c ẍ T = f Tx = A T f T. τ T f Tx f Ty. April 26, 2007Copyrighted by John T. Wen Page 3
Complete Dynamics The arm dynamics can be determined from either Lagrange-Euler or Newton-Euler formulation. In addition to kinematic parameters (in this case just lengths of the links), we need to furnish dynamic parameters: mass, location of center of mass, and inertia about center of mass, for each link. The complete dynamics of the constrained mechanism is then M(θ) θ+c(θ, θ) θ+g(θ) = A T f T = 0 τ J T (θ) f T J(θ) θ = Av c, J(θ) θ+ J(θ) θ = Aα c + Ȧv c. If the load is considered separately, A T f T = 0 equation should be replaced by the load dynamics equation: Note that in this case, Ȧ = 0, b c = G c = 0. m c α c + b c v c + G c = A T f T. April 26, 2007Copyrighted by John T. Wen Page 4
For simulation, solve for α c and f T simultaneously: 0 AT α c 0 = A JM 1 J T f T JM 1 ( C θ G) Ȧv c + J θ+jm 1 τ. The leading matrix is invertible if and only if A T JM 1 JA is invertible. For simulation, the states are x c and ẋ c, which may be combined with the constraints to solve for θ and θ using the inverse kinematics. For a given τ, the right hand side of the differential equation can then be evaluated for ẍ c. The constraint force f T can also be solved based on θ, θ, and τ. Order N computation can also be extended to this case. If the load is considered separately, the equation of motion becomes: M c A T α c b = c v c G c A JM 1 J T f T JM 1 ( C θ G) Ȧv c + J θ+jm 1 τ. Again, the right hand side only depends on θ, θ, and τ. Therefore, the equation of motion can be used to propagate x c and v c, and solve for f T. April 26, 2007Copyrighted by John T. Wen Page 5
Dynamics of Multifinger grasp Multiple arms rigidly holding an object and platform type of parallel mechanisms are special cases. Strategy: Write down the dynamics of each finger and the payload, then add all the kinematic constraints. Dynamics of ith finger: M i (θ i ) θ i +C i (θ i, θ i ) θ i + G i (θ i ) = τ i J T i f i, where f i is the spatial force that ith finger applies to the load. Constraints: A i v c = J i θ i + H i W i, A i = φ ie = I 0 p ci I. H T i f i = 0 April 26, 2007Copyrighted by John T. Wen Page 6
Dynamics of payload: M c α c + b c v c + G c = A T f = m A T i f i. i=1 Putting everything together and stacked up variables and form block diagonal matrices, we have the complete dynamics: Substitute f = H T η, we have M c A T A HJM 1 J T H T M(θ) θ+c(θ, θ) θ+g(θ) = H T f T = 0 τ J T (θ) f T J(θ) θ+hw = Av c J(θ) θ+ J(θ) θ+hẇ + ḢW = Aα c + Ȧv c M c α c + b c v c + G c = A T f. α c η = 0 HJM 1 τ+ which can be used to propagate α c and solve for η (and hence f )... }{{} depends on θ, θ April 26, 2007Copyrighted by John T. Wen Page 7,
Summary Dynamics of constrained mechanisms (single constrained robot, parallel platform type of mechanism). General strategy: Express kinematic constraints in terms of unconstrained variables: Single constrained arm and multiple arm with rigid grasp: J θ = Av c Multi-finger grasp: J θ+hw = Av c. Differentiate the velocity constraint to obtain acceleration constraint: Single constrained arm and multiple arm with rigid grasp: J θ+ J θ = Aα c + Ȧv c Multi-finger grasp: J θ+ J θ+hẇ+ḣẇ = Aα c + Ȧv c. Corresponding force constraint: Single constrained arm and multiple arm with rigid grasp: A T f = 0 or M c α c + b c + g c = A T f Multi-finger grasp: H T f = 0 and M c α c + b c + g c = A T f. April 26, 2007Copyrighted by John T. Wen Page 8
Eliminate θ from acceleration constraint by using the arm dynamics: θ = M 1 (τ C θ G J T f). Obtain the equation of motion to solve for unconstrained acceleration and the constrained force: Single constrained arm and multiple arm with rigid grasp: M c A T α c b = c + g c. A JM 1 J T f JM 1 (τ C θ G)+ J θ Ȧv c Multi-finger grasp ( f = Hη): M c HA = A T H T HJM 1 J T H T α c η b c + g c H[JM 1 (τ C θ G)+ J θ Ȧv c ḢW]. April 26, 2007Copyrighted by John T. Wen Page 9
Additional Topics on Dynamics Dynamics of a wheel Dynamics of general complex multibody systems Robots with flexibility April 26, 2007Copyrighted by John T. Wen Page 10
Unicycle Model z y x h p r Recall kinematics model: Planar: p T z = p z = r No tilt: z T Ry = 0 No slip: r ωz+ ṗ = 0 April 26, 2007Copyrighted by John T. Wen Page 11
Write as a velocity constraint (note planar constraint follows from no-slip constraint): I 0 ω = z eẑφ y φ. rẑ I v 0 0 θ or JV = Av c. Differentiate to obtain acceleration constraint: Jα = Aα c + Ȧv c. Force constraint: A T f = 0 or z T τ c = 0,y T e ẑφ τ c = 0. Dynamics of wheel (Newton-Euler Equation): Mα+b = τ J T f, τ = 0 eẑφ τ y τ z, where τ y is the driving torque and τ z is the steering torque. April 26, 2007Copyrighted by John T. Wen Page 12
Overall dynamics: 0 A AT JM 1 J T α c f = 0 JM 1 (τ+b) Ȧv c. Kinematics must still be included: ẋ ẏ φ = rc φ 0 rs φ 0 0 1 θ. φ }{{} v c April 26, 2007Copyrighted by John T. Wen Page 13
General Multibody Systems How do we obtain the dynamics of a general multibody system with many constraints? Newton-Euler (also called body-coordinate) approach: Write down Newton-Euler equation for each body: M i α i + b i = φ T ji f j. j April 26, 2007Copyrighted by John T. Wen Page 14
Kinematic constraints: Force constraint: φ i j (V i + H i θ i ) = 0 j H T i f i = 0 (or applied force/torque τ i ). Acceleration constraint: j Stack all variables together: Eliminate α to solve for θ and f together. φ i j (α i + H i θ i )+ ( φ i j (V i + H i θ i )+φ i j Ḣ i θ i ) = 0. j Mα+b = A T f Aα+Ȧ(V + H θ)+ah θ+ AḢ θ = 0. General, but computationally cumbersome. April 26, 2007Copyrighted by John T. Wen Page 15
Alternative Approach Constrained coordinate approach: First remove constraints to make the system a tree structure. The dynamics of which can be obtained using Lagrange-Euler method: M θ+c θ+ G = τ J T f April 26, 2007Copyrighted by John T. Wen Page 16
where f contains all the constrained forces. Then impose kinematic constraints: J θ = Av c, J θ+ J θ = Aα c + α c, A T f = 0, we can solve α c and f simultaneously. There are also order N methods for general constrained multibody systems. Prof. Anderson in mechanical engineering is a world authority on this topic. April 26, 2007Copyrighted by John T. Wen Page 17
Joint Flexibility Suppose each joint also has a torsional spring (e.g., flexure joints, popular in micromechanisms). To obtain the dynamics, define the zero configuration to correspond to zero deformation of all the springs. The Lagrangian now needs to include an additional term in the potential energy (assume linear springs): P = K i 2 (θ i θ i 1 ) 2. Lagrangian equation of motion then becomes M θ+c θ+ G+Kθ = τ J T f, where K = K 1 + K 2 K 2... K 2 K 2 + K 3 K 3 K 3 K 3 + K 4......... If the links are also flexible and can be modeled based on finite element method (FEM), April 26, 2007Copyrighted by John T. Wen Page 18
the dynamic model can also be constructed. April 26, 2007Copyrighted by John T. Wen Page 19
Impedance Control Make the robot end effector appear as a specified mechanical impedance. Desired behavior ( f is the spatial force robot applies to the environment): M des α des + D des v des + K des (x des x re f ) = B( f f re f ). f ref f M des K des D des x des x ref If only K des is present, it s called a generalized spring. If only D des is present, it s called a generalized damper. April 26, 2007Copyrighted by John T. Wen Page 20
To achieve the desired behavior, any motion controller can be applied to track x des, v des, α des. Joint space control: f desired impedance x des v des a des inverse kinematics joint controller τ robot θ Task space control: forward kinematics f desired impedance x des v des a des task space controller τ robot θ April 26, 2007Copyrighted by John T. Wen Page 21
Feedback linearizing control: f desired impedance x des v des a des feedback linearization τ robot θ April 26, 2007Copyrighted by John T. Wen Page 22
How to Exert a Desired Amount of Force? Position Accommodation: ψ r p c For position control, choose R des = I, p des = p c + eĥψ r, where ψ = vt. How do we maintain a specified downward z force? Let s consider a generalized damper: D z ż des = ( f z f zd ). April 26, 2007Copyrighted by John T. Wen Page 23
If the pen is not contacting the table, f z = 0. Since f zd < 0, z des will continue to decrease until the pen hits the table. Then f z will become more and more negative until at the steady state, f z = f zd and ż des = 0. Stability analysis: suppose the surface is infinitely rigid. Then upon contact, f z = F z where F z is the applied force. Suppose PD position controller is used: F z = k p (z z des ) k v ż. Since the surface is rigid, z and ż are zero, then F z = k p z des = k p D z Closed loop system: f z = k p ( f z f zd )dt. ( f z f zd )dt, which is stable and robust with respect to the force measurement time delay. What happens if a generalized spring is used? i.e., K z (z des z re f ) = ( f z f zd ). April 26, 2007Copyrighted by John T. Wen Page 24
Now consider direct torque control: First consider the motion control portion. Dynamics of open serial chain: Motion Control τ = τ m + τ f. M θ+c θ+ G = τ. Most industrial controllers are independent joint level PID: τ i = k Pi (θ i θ ides ) k Ii (θ i θ ides )dt k Di θ i. There is sometimes friction compensation (for Coulomb friction), integral anti-windup, backlash compensation, in addition to the standard PID. Trajectory generation is used for θ des to ensure smooth motion. Linearized system about (θ d,0): M(θ d ) θ+ G θ (θ θ d ) = τ. θ=θd April 26, 2007Copyrighted by John T. Wen Page 25
Performance under constant PID will vary depending on configuration and speed. Gains are typically tuned for the outstretched configuration, and response is faster (but may overshoot) in other configurations (smaller inertia and gravity load). Feedback Linearization (cancelling all the nonlinearity): τ = C θ+g(θ)+m(θ)( θ des K p (θ θ des ) K d θ). This can be computed by using the recursive Newton-Euler algorithm. Feedback linearization can be performed in the task space also (useful for impedance control): τ = C θ+g(θ)+j T (JM 1 J T ) 1 ( J θ α des K p (x x des ) K d (v v des )). Passivity based controller (noting the open loop system is conservative, we can just add damping and reshape the potential energy): τ = K p (θ θ des ) K d θ+g(θ). April 26, 2007Copyrighted by John T. Wen Page 26
Now consider a closed kinematic chain: Let τ = τ m + τ f, τ f = J T F. Effect of F on α c : Direct Force Control θ = M 1 (τ J T f)+..., Aα c = J θ+..., A T f = 0. Aα c = JM 1 J T F +..., therefore A T (JM 1 J T ) 1 Aα c = A T F +... If we choose F N (A T ) (A T F = 0), then F does not affect α c, i.e., force control does not affect motion (e.g., for the project, force control only affects how hard the arm presses down against the table.) Effect of motion on force: Reparameterize force: f = à T η and F = à T ξ. Then Aα c = JM 1 (τ J T f)+... = JM 1 J T (F f)+jm 1 τ m +... Multiply à on both sides: ÃJM 1 J T à T (ξ η) = terms dependent on θ, θ, and τ m. April 26, 2007Copyrighted by John T. Wen Page 27
Force dynamics: η = ξ+d where d is the motion induced force in the constrained direction. Since constrained force and its control do not affect motion, d can be considered as a disturbance as far as the force control is concerned. Direct force control Open loop: ξ = η des (no disturbance rejection) Proportional force feedback: ξ = k p (η η des ) (not robust w.r.t. force measurement delay) Integral force feedback: ξ = k i (η ηdes ) (robust w.r.t. force measurement delay) April 26, 2007Copyrighted by John T. Wen Page 28