q 1 F m d p q 2 Figure 1: An automated crane with the relevant kinematic and dynamic definitions.

Transcription

1 Robotics II March 7, 018 Exercise 1 An automated crane can be seen as a mechanical system with two degrees of freedom that moves along a horizontal rail subject to the actuation force F, and that transports a swinging link connected with a passive and frictionless revolute joint, as sketched in Fig 1 With reference to the kinematic variables and dynamic parameters defined therein: derive the dynamic model of this system using a Lagrangian formalism; provide a linear parameterization of the obtained model in terms of a minimal number of dynamic coefficients; provide a linear approximation of the nonlinear model for small variations around the unforced equilibrium state x 0 = T q 1 q q 1 q = 0; find the nonlinear state feedback law for the force F = F x, a that linearizes exactly the dynamics of the first coordinate as q 1 = a F m q 1 passive joint d p g 0 q!mp, Ip Figure 1: An automated crane with the relevant kinematic and dynamic definitions Exercise The end-effector of a PPR robot moving on a horizontal plane and equipped with a D force sensor should follow a stiff and frictionless linear surface tilted by α > 0 wrt the absolute y axis, starting at time t = t 0 in the position p s = x s y s T, with a tangential speed Vt = V t t 0 + A t t t 0 with V t t 0 > 0 and a constant A t > 0, and applying a constant normal force F n > 0 see Fig Assuming full knowledge of geometric, kinematic, and dynamic parameters, provide the symbolic expressions of the initial robot state and explicitly of all terms in the force/torque commands at the joints that will guarantee perfect execution of the desired task in nominal conditions Is the solution unique? If not, provide the simplest one y d c1 m! q q 1! m 1 d c D force sensor m d!!,i! c3 q 3 l 3 stiff and frictionless linear surface S p s! x Figure : A PPR robot should move in contact with a stiff and frictionless linear surface [150 minutes; open books] 1

2 Exercise 1 Solution March 7, 018 The crane is an underactuated mechanical system with n = degrees of freedom, but with only a single control command p = 1 To derive its dynamic model, we can follow a Lagrangian approach For this, the position and velocity of the center of mass of the swinging link are 1 q1 + d p sin q p c = d p cos q The kinetic energy of the system is T = T m +T p = 1 m q 1+ 1 m p v c + 1 I p q = 1 and thus q1 + d p cos q q, v c = ṗ c = d p sin q q m + mp q 1 + I p + m p d p q + m p d p cos q q 1 q, T = 1 m + mp m p d p cos q qt Mq q Mq = m p d p cos q I p + m p d p Using the Christoffel s symbols, we found only a centrifugal term mp d p sin q q cq, q = 0 The potential energy and the associated gravity vector are T Uq 0 U = U 0 m p g 0 d p cos q gq = =, q m p g 0 d p sin q with g 0 = 981 [m/s ] Assuming the possible presence of a viscous friction term with a viscous coefficient f v 0 on the movement along the rail, the dynamic equations take the scalar form m + m p q 1 + m p d p cos q q m p d p sin q q + f v q 1 = F m p d p cos q q 1 + I p + m p d p q + m p g 0 d p sin q = 0 1 Equations 1 can be rewritten in the linearly parametrized form m + m p q1 0 cos q q sin q q q 1 I p + m p d p = Y q, q, qπ = 0 q cos q q 1 + g 0 sin q 0 m p d p f v The linear approximation of the dynamic equations of the crane around the stable equilibrium state x 0 = q T 0 q T T T 0 = q1 q q 1 q = 0, which satisfies 1 with F = F0 = 0 unforced, is obtained by setting in 1 q = q 0 + q = q, q = q 0 + q = q, q = q 0 + q = q, F = F 0 + F = F, 1 We have taken the x-axis along the rail, and the y-axis in the vertical upward direction F 0

3 and neglecting second- and higher-order increments eg, setting sin q q and cos q 1: m + mp m p d p m p d p I p + m p d p q1 q fv 0 q1 0 0 q1 F + + = 0 0 q 0 m p g 0 d p q 0 Finally, partial feedback linearization of the crane dynamics as concerns the motion of q 1 is obtained as follows Solve globally! for the revolute joint acceleration q from the second equation in 1, and substitute it in the first one, yielding m + m p m pd p cos q I p + m p d p 1 q = I p + m p d m p d p cos q q 1 + m p d p g 0 sin q p q 1 m pd p g 0 sin q cos q I p + m p d p From this, it is immediate to see that the nonlinear state feedback law F = m + m pi p + m m p d p + m pd p sin q I p + m p d p a m pd p g 0 sin q cos q I p + m p d p m p d p sin q q + f v q 1 = F yields again, globally q 1 = a Accordingly, the second equation in 1 becomes I p + m p d p q + m p g 0 d p sin q = m p d p cos q a m p d p sin q q + f v q 1 Exercise Noting that q 1 affects the y-coordinate and q the x-coordinate, the direct/differential kinematics of the end-effector position, velocity, and acceleration are given respectively by q + l 3 cos q 3 p = = fq, q 1 + l 3 sin q 3 and ṗ = fq q 0 1 l3 sin q 3 q = Jq q = q 1 0 l 3 cos q 3 p = Jq q + Jq 0 0 l3 cos q 3 q 3 q = Jq q + q, l 3 sin q 3 q 3 where the Jacobian Jq has been introduced The robot has n = 3 joints and the hybrid planar task has dimension m = one in force, the other in motion/velocity In the presence of n m = 1 degree of redundancy, the task can be executed in an infinite number of ways, beginning right from the different initial choices of an inverse kinematic configuration qt 0 at time t 0, among those associated to the initial Cartesian point pt 0 = p s, and of the initial joint velocity qt 0, among those associated to the initial end-effector velocity sin α ṗt 0 = V t t 0 cos α 3

4 It is then possible to parametrize the joint-space motion in terms of one variable In this case, the easy choice is to pick the third joint variable q 3 as the parametrizing one We set an arbitrary but sufficiently smooth time profile for it and thus q 3 t = βt, q 3 t = βt, t t 0, q 3 t 0 = βt 0, q 3 t 0 = βt 0 As a result, the two prismatic joints will be initialized at q1 t 0 = q t 0 ys l 3 sin βt 0 x s l 3 cos βt 0, with initial velocity q1 t 0 = q t 0 V t t 0 cos α l 3 βt0 cos βt 0 V t t 0 sin α l 3 βt0 sin βt 0 Moreover, we can also invert the second-order differential kinematics 3 in a parametrized way as q 1 p y + l 3 cos β β + sin β β q = q = p x + l 3 sin β β + cos β β 4 q 3 β With this in mind, the dynamic model of the planar PPR robot in the absence of gravity and without dissipative effects, when in contact with a stiff environment environment takes the form Mq q + cq, q = τ + J T qf 5 where F R is the contact force applied by the environment on the robot end-effector equal and opposite to the one applied by the robot on the environment and J has been defined in We provide next the explicit symbolic expressions of the dynamic terms appearing in 5 Note first that the position and velocity of the center of mass of the third link are q + d c3 cos q 3 p c3 = q 1 + d c3 sin q 3, v c3 = ṗ c3 = q d c3 sin q 3 q 3 q 1 + d c3 cos q 3 q 3 Following a Lagrangian approach, we compute the total kinetic energy T = T 1 + T + T 3 as: T 1 = 1 m 1 q 1, T = 1 m q 1 + q, T 3 = 1 m 3 q 1 + q + d c3 q 3 + d c3 q 3 cos q 3 q 1 sin q 3 q + 1 I 3 q 3 T = 1 qt Mq q, with m 1 + m + m 3 0 m 3 d c3 cos q 3 Mq = 0 m + m 3 m 3 d c3 sin q 3 m 3 d c3 cos q 3 m 3 d c3 sin q 3 I 3 + m 3 d c3 The Coriolis and centrifugal terms cq, q are derived using the Christoffel s symbols, ie, for each component c i q, q = q T C i q q, C i q = 1 T m i q mi q + Mq, i = 1,, 3, q q q i 4

5 being m i the ith column of the inertia matrix M We obtain C 1 q = c 1 q, q = m 3 d c3 sin q 3 q 3, 0 0 m 3 d c3 sin q 3 C q = m 3 d c3 cos q 3 c q, q = m 3 d c3 cos q 3 q 3, C 3 q = 0 c 3 q, q = 0 We note that there are only centrifugal terms and no Coriolis torques Applying now to 5 the feedback linearizing control law τ = Mqa + cq, q J T qf 6 will transform the system into a set of decoupled input-output integrators q = a For the specified hybrid task, the desired end-effector acceleration and contact force are respectively p d = A t sin α cos α cos α, F d = F n sin α From the desired end-effector acceleration, using 4, we obtain also the desired joint acceleration in parametrized form A t cos α + l 3 cos β β + sin β β q d = A t sin α + l 3 sin β β + cos β β β Substituting a = q d and F = F d in the feedback linearizing law 6, yields the desired nominal control commands A t cos α + l 3 cos β β + sin β β τ d = Mq A t sin α + l 3 sin β β + cos β β + cq, q + J T Fn cos α q, 7 F n sin α β where the dependence of the inertia matrix M and of the Jacobian J is actually only on q 3 = β and that of the centrifugal terms c is only on q 3 = β and q 3 = β This can be made more explicit by rewriting 7 in extended form as m 1 +m +m 3 A t cos α + l 3 cos β β + sin β β + m 3 d c3 cos β β sin β β + F n sin α A t sin α + l 3 sin β β + cos β β m 3 d c3 sin β β + cos β β τ d = m 1 +m 3 I3 + m 3 dc3 β + m3 d c3 A t cosα β l 3 β + F n l 3 sinα β + F n cos α Note that the first two components of τ d are forces the units of all terms are [N] = [kg m/s ], while the last component is a torque units in [Nm] Moreover, thanks to the fact that the initial 5

6 robot state is matched with the task at the initial time t = t 0, there will be no need of a feedback action on task errors in the nominal control commands 7 in order to execute the entire task in ideal conditions The above parametrized control law is one of the many realizing the desired task, depending on the choice of the time evolution βt for the variable q 3 of the revolute joint Indeed, simplifications arise for specific choices The simplest one is choosing to keep q 3 at a constant value β, with β = β = 0 We obtain A t m 1 + m + m 3 cos α + F n sin α τ d = = A t m 1 + m 3 sin α + F n cos α A t m 3 d c3 cosα β + F n l 3 sinα β τ d1 τ d τ d3 Having chosen to keep the third joint at rest for the entire motion, the robot behaves kinematically as a P robot In particular, when placing the third robot link normal to the frictionless surface, we have β = α and the third control component reduces to τ d3 = A t m 3 d c3 6