9.

2 Dynamic Model of Flexible Joint Robot in Contact with Different Environment

The dynamic model of a flexible joint robot have n degrees of freedom and interact- ing with the environment can be written as: M(q)q + V (q, q) + K(q ) = F Im + K( q) = m , where q: (9.1) (9.2)

Generalized coordinates representing the angle of the robot links, q n ; : Generalized coordinates representing the angle of the actuators rotors (shafts) n ; M(q): Inertia matrix associated with the rigid links, M(q) nn ; V (q, q): Vector of Coriolis, centrifugal and gravity generalized force, V (q, q) n1 ; Im : K: F : m : diag[Imi ], the positive nn definite diagonal matrix of the moments of inertia of the motors, Im ; diag[Ki ], the positive definite diagonal matrix of stiffness of the rotors, K nn ; Joint torque contributed from the contact force with environment, F n ; input torque from the motors, n .

Note that Imi and Ki (i = 1, 2,.. ., n) are the inertia and the stiffness of the ith joint. n is degrees of freedom of the robotic manipulator. The joint torque, F , contributed by the contact force with the environment can be formulated depending on the type of contact surface. In order to derive the expression for the contact force, lets first define some variable. u: JT : position vector of the robot end-effector expressed in the task space or constraint frame, u n ; Task space Jacobian.
T

We can partition vector u into the following from, u = ut un (ut nm, m un ), where ut and un are vectors of coordinates in the tangent and normal space, respectively. Assume that the robot is subject to m holonomic, frictionless T and deformable constraint surfaces characterized by (q) = 1 (q) m (q) = un une, m n, where une is the equilibrium position in the normal direction. The constraint force in the joint space, F , can be expressed by F = J T f , where
n

f m Jn (q) .

is m-dimensional vector of represents normal contact force components, is the jacobian of the holonomic constraints, i.e., Jn = q mn

It is also found that matrix Jn is the m row of matrix JT , or Jn = [ O I ]JT , where I = unity matrix mm and O = zero matrix m(nm) . When the robot is in contact with stiff environment, contact force is generated as a result of environmental stiffness. If we assume that the direction of the contact force f is normal to the un-deformed environment, then the contact force in joint coordinates can be obtained as f = k (un une) , (9.3)

where k represents the environment stiffness and is a constant positive definite matrix, k mm . When the robot is interacting with the dynamic environment, the contact force expressed in the compliance frame can be given by the following model: f = mun + bun + k (un une) , (9.4)

where m, b, and k are m m matrices of equivalent inertia, damping constant, and stiffness of the environment.

9.3 Decoupled Controller Design

Let qd (t ) be the desired trajectory of the link position, d (t ) the desired magnitude of the constraint force with a rigid surface and fd (t ) the desired contact force with a deformable surface. The control objective is to make sure that the manipulators end-effector follows a desired trajectory and at the same time maintains a desired contact force between the end-effector and the surface even when the surface com- pliance changes along the trajectory. When the robotic system is subject to m environmental constraints, the original n degrees of freedom will be left with only nm degrees of freedom. Thus the sys- tems motion is governed by a set of nm independent equations (position/velocity relations) and a set of m dependent equations (force relations). Here, we are going to perform the coordinate reduction on the vector u in the constraint frame. First we need to obtain the explicit expression between u and torque input m .

9.3.1 Contact with Rigid Surface

Differentiating the link Eq. 9.1 of the dynamic model twice yields, Mq(4) + 2M q(3) + M q + V + K(q n ) = n T f + 2JT f + J T f . (9.5) J n

Note the notion, q(i) = d i q/dt i . Substituting obtained from Eq. 9.2 into Eq. 9.5 gives Mq(4) + 2M q(3) + M q + V + Kq KI 1 m + KI 1K( q) m m = J T f + 2JT f + J T f .
n n n

(9.6)

The velocity of the end-effector in the constraint frame is related to the joint velocities by u = JT q. The relationship between the fourth derivative of joint variables q and the end-effector position vector u (in the task space) is
(4) q(3) = J 1 u(4) 3(JT q(3) + JT q) J q T T

(9.7)

Substituting Eq. 9.7 into Eq. 9.6 gives 3 M u(4) M (JT q(3) + JT q) + J (3)q + V = KI 1m + J T f , m n T (9.8)

where M (q) = MJ 1 and T V (q, q, q, q(3) , , f , f) = 2M q(3) + M q + V + Kq + KI 1K( q) J T f 2JT f . m n n Since u = ut un and further, the end-effector is in contact with the rigid environment, no motion is allowed in the directions that are normal to the constraint n (4) hyper-surfaces, hence, u = 0. With some manipulators, Eq. 9.8 becomes Mr ut f
(4) T

+ Vr = KI 1 m ,
m

(9.9)

T I where Mr = M Jn ; Vr = 0 M

3) 3(JT q + JT q) + JT ( q

(3)

+ V .

A generalized computed torque control law can be chosen, m = Im K 1 (Mr yr + Vr) ,

T

(9.10)
(4)

where yr = yut yf . This control law leads to two closed-loop systems ut = yut and f = yf . Finally, controller design is completed on the linear side of the problem by choosing
(4) (3) (3)

yut = utd + K3

utd ut

+ K2 (utd ut )

+ K1 (utd u t ) + K0 (utd ut ) yf = fd + Kv fd f + Kp ( fd f ),

(9.11)

where K3 , K2 , K1 , K0 and Kv , Kp are constant diagonal feedback gain matrices, re- spectively.

9.3.2 Contact with Stiff Environment

Since the contact surface is rigid any more, motion is allowed in the normal direction. Hence, un = Jn q. Similarly, differentiating Eq. 9.1 twice and combining with the solution for obtained from Eq. 9.2, one can obtain the following expression between u and torque input m . MJ 1
T (4) 1 (3) (3) (3)

MJT

3(JT q
1

+ JT q) + JT q

+ 2M q

+ M q + V

(9.12)

+ Kq + KI m

T T T 1 K( q) J f 2J n J fm = KI m . f n n

In brief, the above equation can be expressed as Ms u(4) + Vs(q, q, q, q(3) , , f , f, f) = KI 1 m ,

m

(9.13)

where Ms and Vs can easily be identified from Eq. 9.12. Now the corresponding feedback linearizing control for the dynamics given by Eq. 9.13 can be chosen as m = Im K 1 (Ms ys + Vs ) . (9.14)

A globally linearized and decoupled equation of motion: u(4) = ys is obtained. Similarly linear position and force controllers can be designed for the corresponding task T space variables u. Let ys = yut yun , where y is partitioned corresponding to free and constrained motion. This leads to u y
t (4)

= yu and u
t

(4)

=
un

. To ensure asymptot-

ical stability for the unconstrained direction of motion, yuf is chosen the same as in Eq. 9.11. Since the desired contact force is achieved by regulating the end-effector (4) in the constrained direction uc and u = 1 f (4) , then is designed as y
n k un

yun =

1 k

(4) fd + Kc3 fd f

(3)

(3)

+ Kc2 fd f .

(9.15)

+Kc1 fd f + Kc0 ( fd f )

With proper choice of diagonal matrices for Kc0 , Kc1 , Kc2 , Kc3 , asymptotic force tracking in the constrained directions is guaranteed.

9.3.3 Contact with Dynamic Environment

Now the decoupled controller for a robot interacting with the dynamic environment may be developed. Substitute Eq. 9.4 into Eq. 9.1, we have M(q)q + V (q, q) + K(q ) T mJn q + mJnq + bun + k(un . = Jn une ) (9.16)

Rearrange the above equation and solve for q, yields

T q = M nJ T mJn 1 [Jn mJn q + bun + k(un une )

(9.17)

V (q, q) K(q )] . As can be seen from Eq. 9.17, the dynamics of the environment is included in the dynamics of the robot. Differentiate Eq. 9.1 twice to get Mq(4) + 2M q(3) + M q + V + Kq + KI 1 K( q) m T T T 1 J f 2J f J f = KI m .
n n n m

(9.18)

Obtain f , f and f from Eq. 9.4, substitute into Eq. 9.18 and solve for q(4) . Then using Eq. 9.7, the expression between the end-effector task space position vector and the motor input torque m is obtained Md u(4) + Vd = KI 1 m , m where Md = M J T mJn J 1 and n T
(3) Vd = M J T mJn J 1 (JT q(3) + JT q) + J q 3 V + Kq n T T m + KI 1K( J (3) q n

(9.19)

+ 2M q(3) + M q +
n n

q) J T m

(Jn q(3) + Jn q) + + bu
(3)

J T f 2JT f .
n n

+ ku

Partitioning u according to tangent and normal space, and making use of the ex(4) (4) pression for un obtained from differentiate twice of Eq. 9.4, which is u n = (3) m1 f m1 bu + ku, then Eq. 9.19 can be rewritten as n n Md ut Md m1 f
(4)

0 (3) bun + kun

1 + Vd = KIm m .

(9.20)

A generalized computed torque control law can be chosen as follows, m = Im K 1 M y d Md 0

(3) bun

+ Vd ,
n

(9.21)

ku

where y = yut yun

. This control law leads to two decoupled linear closed-loop

systems, ut

(4)

= yut and m1 f = yun . Finally, the controller design is completed on

the linear side of the problem by choosing

(4) (3) (3)

yut = utd + Kt3

1

utd ut

+ Kt2 (utd ut ) + Kt1 (utd u t) + Kt0 (utd ut )

yun = m

fd + Kn1 fd f + Kn0 ( fd f ) ,

(9.22)

where Kt3 , Kt2 , Kt1 , Kt0 and Kn1 , Kn0 are constant diagonal feedback gain matrices, respectively.

Reconfigurable Control of Constrained Flexible Joint Robots

171

9.4 Reconfigurable Control Scheme

When the flexible joint robot is in contact with a surface containing both rigid, stiff and dynamic environment, it can be stated that the manipulator dynamics belong to either the invariant manifolds Sr (rigid surface), Ss (stiff surface), and Sd (dynamic surface). A switching control scheme is adopted for flexible joint robot moving in contact with surfaces of different compliance. Appropriate control law should be used depending on the type of contact surface. The following summarizes the switching logic. If q Sr , the motion of the robot remains on the manifold defined by Sr , then Control Law: m = Im K 1 (Ms ys + Vs ) . If q Ss , the motion of the robot remains on the manifold defined by Ss , then Control Law: m = Im K 1 (Ms ys + Vs ) . If q Sd , the motion of the robot remains on the manifold defined by Sd , then Control Law: m = Im K 1 M y d Md 0 + + Vd .
n

(3) bun

ku A schematic diagram of this reconfigurable controller is shown in Fig. 9.2. Cr , Cs and Cd represent the position/force controller for rigid, stiff and dynamic environment, respectively. Supervisory module is the switching decision making module, which is triggered by the change of the type of the surface (or environment). Then the corresponding force controller is selected to continue the task. The supervisory module also monitors the performance of the system and it can possess learning and self-organizing capabilities as well. Detailed framework of this supervised control switching system can be found in Cao and de Silva (2006).

Supervisory

Cr r(t) e(t) Cs Cd Switch

u(t) Control

Constrained Robot

y(t)

Fig. 9.2 Reconfigurable force control scheme

9.5 Simulation Study

A two-link flexible joint robot (Fig. 9.3) is chosen. The following system parameters, initial conditions, and desired values were used in the numerical simulation: System parameters: m1 = 5 kg, m2 = 5 kg, l1 = 1 m, l2 = 1 m, K1 = K2 = 500 N/m, k = 500 N/m Constraint Surface: x + y = 2 or (q) = l1 cos 1 + l2 cos(1 + 2 ) + l1 sin 1 + l2 sin(1 + 2 ) 2 . Initial Conditions: Coordinate of point A in constrain frame is: ( 2, 2 + 0.05). Desired Maneuver: Move from A B; with a sine-on-ramp profile, i.e. und (td ) un(0) und (t ) = td t td 2 sin 2 td t + un(0) ,

where und is the desired trajectory; t is time; and td is the time required for the maneuver. Note that un (0) refers to the coordinate of initial location A in the tangent space, i.e. un (0) = 2 + 0.05. At the same time, it is also desired to maintain a constant normal force of 10 N in the task space. The contact surface is assumed to be rigid when un < 0 and stiff when un 0. Control law is switched at the boundary of the two types of surfaces. Values of the gains for the decoupled linear position controller are selected as K3 = 168, K2 =

Fig. 9.3 Simulation example

of the switched force control

Fig. 9.4 Time history of the Contact force

10

Force applied on the environment (N)

10

Time (s)

7696, K1 = 53 760, and K0 = 102 400, which are essentially the coefficients of the fourth polynomial (s + 80)2 (s2 + 8s + 16) = 0. The roots of the second-order system (s2 + 8s + 16) are dominant ones since they are less than one tenth of the third and fourth roots (80). Hence, with this set of gains, it is expected that the endeffectors position exhibit critically damped response with natural frequency of n = 4 rad/s corresponding to a step-input. Values of the gains in the force controller for the rigid environment (Eq. 9.11) are selected as Kv = 100, Kp = 250. Values of the gains in the force controller for the stiff environment (Eq. 9.15) are selected as Kc0 = 409 600, Kc1 = 163 840, Kc2 = 10 304, Kc3 = 184, which is essentially the coefficients of the fourth polynomial, (s + 80)2 (s2 + 16s + 64) = 0. Similarly, response of the contact force with the stiff environment is expected to be critically damped with natural frequency of n = 8 rad/s corresponding to a stepinput. The time history of the contact force is plotted in Fig. 9.4. Figure 9.5 presents the time history of the system variables including response of the links (1 , 2 ), tracking error of the links angle (e1 , e2 ) and the control inputs (m1 , m2 ). With the decoupled nonlinear position/force controller, first, the desired trajectory tracking and contact force is achieved when the end-effector is on the rigid surface. Control switching happens at the same time when the end-effector is in contact with the soft surface. Switching of the control law happens at t = 4 s. Tracking error occurs when the end-effector is in contact with the soft surface. This is necessary since the contact force is generated due to the deformation of the surface. There is also a short period of variation of the contact force from the desired value. This can be explained by the robotic dynamics at the time of the surface type change. When the end-effector moves from rigid surface to soft one, the end-effector dips into the soft surface resulting in a sudden change of the angular position of the links in order to maintain the 10 N desired contact force. Since the control law switches while the end-effector is still moving, dramatic change of the control inputs is observed. The maximum control input goes to around 400 Nm. This sudden change in the control inputs may not be acceptable in practice depending on the capacity of the motor. In order to prevent damage to the hardware, limits on the control inputs can be imposed.

Fig. 9.5 Response of the system variables

Force applied on the environment (N)
10

Fig. 9.6 Contact force with torque input limit 50 Nm

Time (s)

10

The thresholds are set to 50 to 50 Nm. Figures 9.6 and 9.7 show the updated system response. No significant change can be observed in the response of either the tracking error or the contact force. At the time of control switching, the variation of

120

Link 1 motion (deg)

Link 2 motion (deg)

90

-30

60 30

-60

-90 0 2 4 6 8 10 0 2 4 6 8 10

Tracking Error of Link 1 (deg)

0 4 -1

Tracking Error of Link 2 (deg)

e1

-2

e2

-3 0 50 2 4 6 8 10

0 0 50 2 4 6 8 10

Torque Input for Joint 1 (Nm)

Torque Input for Joint 2 (Nm)

-50

10

-50

10

Time [s]

Time [s]

Fig. 9.7 Response of the system variables with torque input limit 50 Nm

the contact force is only slightly higher than the no threshold case. Setting limit on the torque input or voltage to the actuator is a very common practice in reality. However, the value of the threshold cannot be lower than what is needed to achieve the desired contact force. Results indicate that as long as such limits do not interfere with the minimum torque required to hold the system at desired location and desired contact force. As can be seen in Fig. 9.7, the control input for joint 1 is about 12 Nm at stable state. If the threshold is set to be lower than 12 Nm, the system is unstable.
Acknowledgements This research was conducted at the Intelligent Manufacturing Systems (IMS) Center where the first author held a Post Doctoral Fellowship. The research support from The Natural Science and Engineering Research Council (NSERC) of Canada is greatly appreciated.

References
Arimoto S., Liu Y., Naniwa T., 1993, Model-based adaptive hybrid control for geometrically con- strained robots. Proc IEEE International Conference on Robotics and Automation. Atlanta GA USA 16373. Cao Y., de Silva C.W., 2006, Supervised switching control of a deployable manipulator system. International Journal of Control and Intelligent System 34/2:153165 De Luca A., Manes C., 1991, Hybrid force/position control for robots in contact with dynamic environments. Proc of Robot Control SYROCO 377382. Featherstone R., 2003, A dynamic model of contact between a robot and an environment with un- known dynamics. In: Jarvis R.A., Zelinsky A. (eds) Robotics Research: The Tenth International Symposium. Springer, Berlin Heidelberg Bew York, pp 433446 Hogan N., 1985, Impedence Control: An approach to manipulation: Part I Theory; Part II Implementation; Part III-Applications. J Dyn Syst Meas Control 107:124 Jankowski K.P., ElMaraghy H.A., 1991, Nonlinear decoupling for position and force control of constrained robots with flexible joints. Proc of IEEE Inter Conf on Robotics and Automation. Sacramento California USA 2:12261231 Karan B., 2005, Robust position-force control of robot manipulator in contact with linear dynamic environment. Robotica 23:799803 McClamroch N.H., 1989, A singular perturbation approach to modeling and control of manipu- lators constrained by a stiff environment. Proc of the 28th IEEE Conference on Decision and Control. Tampa FL USA 24072411 McClamroch N.H., Wang D., 1988, Feedback stabilization and tracking of constrained robots. IEEE Trans Auto Control 33/5:419426 Raibert M.H., Craig J.J., 1981, Hybrid position/force control of manipulators. J Dyn Syst Meas Control 102:126133 Siciliano B., Villani L., 1996, A passivity-based approach to force regulation and motion control of robot manipulators. Automatica 32/3:443447 Villani L., deWit C.C., Brogliato B., 1999, An exponentially stable adaptive control for force and position tracking of robot manipulators. IEEE Trans Rob Auto 44:778802 Vukobratovic M., Stojic R., Ekalo Y., 1998, Contribution to the position/force control of manipu- lation robots interacting with dynamic environment- a generalization. Automatica 34/10:1219 1226 Vukobratovic M., Ekalo Y., 1996, New approach to control of robotic manipulators interacting with dynamic environment. Robotica 14:3139 Whitcomb L.L., Arimoto S., Naniwa T., Ozaki F., 1997, Adaptive model based hybrid control of geometrically constrained arms. IEEE Trans Rob Auto 13:105116 Yoshikawa T., Sugie T., Tanaka M., 1988, Dynamic hybrid position force control of robot mani- pulators: controller design and experiment. IEEE Trans Rob Auto 4/6:699705 Yoshikawa T., Zheng X.Z., 1993, Coordinated Dynamic Hybrid Position/Force Control for Multi- ple Robot Manipulators Handling One Constrained Object. Int J Robot Res 12/3:219230