36 Structures in Space - WIT Press Structures in Space include higher order system dynamics, especially those between the actuator and sensor, as the non-collocated …

Force control of kinematically constrained

manipulators in the presence of contact point

disturbances

Y. Ohkami, S. Matsunaga, H. Lakhani

Department of Mechano-Aerospace Engineering, Tokyo Institute of

Technology, 2-12-1 O-okayama, Meguro-ku, Tokyo 152, Japan

Abstract

It is envisioned that in the future long-arm manipulators such as the Space ShuttleRMS and the ETS-VII Remote Manipulator Arm will be required to performvarious types of constrained motion tasks, such as work-surface inspection,assembly, and cleanup. In the presence of contact point disturbances, due to eitherunknown contour profiles or movement of the work surface relative to the robot,the robot must ensure a minimum amount of contact force error while at the sametime maintaining the desired constraint. This paper discusses the disturbancerejection properties of several commonly-used explicit force control strategiesand highlights the control issues particular to force control of kinematicallyconstrained mechanisms. A general disturbance-based model for constrainedmanipulators which is based on previous work on the simulataneous position/force control of constrained robots is also presented. Simulation results of thismodel elucidate the problems and issues relevant to the force control task ofkinematically constrained manipulators.

1 Introduction

Force control has been an active field of robotics for the past 30 years. Duringthis time, several force and compliant motion strategies have been proposed,including hybrid force/position controlfl, 2], stiffness control [3], admittancecontrol [4], impedance control [5], and integral control [6, 7].

Previous work has mainly focused on stability and performance, as well as onthe development of simple plant models to understand the inherent instabilitiesof force controlled systems [6, 8]. Through the latter effort, it was demonstratedthat high contact-stiffness force control degenerates to high-gain— thus,increasingly unstable— position control when the end point force is fed back. Inaddition, Eppinger and Seering showed in [6] that it is sometimes necessary to

Transactions on the Built Environment vol 19, © 1996 WIT Press, www.witpress.com, ISSN 1743-3509

36 Structures in Space

include higher order system dynamics, especially those between the actuator andsensor, as the non-collocated nature of many implementations can sometimesexplain the undesired stability characteristics of the force controlled system.

This paper deals with two aspects of force control which have received relativelylittle attention up until now. Once is force control in the presence of disturbances —namely contact point disturbances — and the second is force control in closed-loop kinematically constrained mechanisms.

Contact point disturbances can be considered to be either force disturbancesor position disturbances. This distinction is closely related to the nature of thecontact. When there are significant inter-robot-environment dynamics, thedisturbance can be adequately treated as a force disturbance, while for extremelystiff contacts these disturbances may be better modelled as position disturbances.In this case, because of the closed-loop nature of force controlled systems, theposition disturbances are closely related kinematically to the robot motion, andan understanding of this relationship is essential in designing force controllersfor such systems.

This paper deals specifically with force controlled systems in which the robot-environment is extremely stiff and in which disturbances acting at the contactpoint can be treated as position disturbances. The first assumption is not extremelylimiting since many practical force control applications such as grinding, scraping,and painting involve relatively stiff contacts. Even if the contact is somewhatsoft, then depending on the contact stiffness, and thus the natural frequency ofthe contact dynamics, it is generally possible to define a frequency band overwhich the contact can be effectively assumed to be rigid. This is illustrated by theBode plot in Figure 1 of the transfer function between endtip motion andenvironment motion for a force controlled system with simple second order contactdynamics. Here, over the frequency range 0. 1 < \v^ < 10 [rad/sec], not only is themagnitude of the transfer function near unity, but the phase is close to zero,meaning that over this frequency range the endtip motion almost perfectly tracks

_ 20

2. <D 0I -10& -20^ -30

-401

_, 0CD2 -50O)2.-1000)% -150:°" -200

10°

nr

10' 10=freq [rad/sec]

10* 10*

10"freq [rad/sec]

Figure 1. Bode plot of transfer function between contact point motionand endtip motion for a typical 2nd-order force controlled system.


Structures in Space 37

Figure 2. Typical Force Control Application.

the environment motion in both magnitude and phase; the contact is, in a sense,perfectly rigid.

The second assumption seems restrictive at first, but in fact is representativeof many common force control aplications in which the motion of the environmentis not constant. For example, consider the application depicted in Figure 2 inwhich the manipulator is asked to apply a coat of paint to an unknown and unevensurface. In order to achieve a consistent layer of paint, the applied normal forcemust be carefully regulated, and the surface variations are seen by the manipulatoras position disturbances in the vertical direction.

Tracking a constant force in the presence of such unknown environment motionsis a difficult problem, and because the nature of the disturbance is unknown itmay be impossible to completely overcome the force error resulting from thedisturbance. Thus the problem becomes one of minimizing the effect of thedisturbance, rather than pure cancellation. Another possibility is to somehowmeasure or predict the the contour variations ahead of time and then to force themanupulator to track these variations, but this is in general difficult and in additionmay require the use of additional sensors (vision, for example) mounted at theendpoint. There is one case, however, in which a similar approach can be applied.Specifically, it is the case when the contact is considered infinitely rigid. We willdiscuss this in further detail later in the paper.

Since the ultimate goal of this paper is to investigate precise force regulationstrategies, we will assume that the controller will have an outer force feedbackloop in which sensed contact force is compared with a desired force to form thecontrol error which should be driven to zero. Note that this structure does notpreclude the use of other strategies such as stiffness or admittance control since,for example, an inner stiffness loop can be easily incorporated into the structure.In this case, the outer force control loop serves to adjust the inner loop setpoints.

2 Relation to Previous Work

As stated earlier, the issue of force control in the presence of contact pointdisturbances has received relatively little attention in the robotics literature. Oneof the earliest mentions is in [8] where a simple experiment with a one-link direct-drive arm and a sinusoidal position disturbance generated by a cam revealed thatfor even low frequency disturbances, the force error could exceed 10% of thedesired force.

De Shutter [9] was the first to explicitly include position disturbances in his



generic force control model. In his paper, he defines conditions on the error-driven compensator for achieving zero steady-state error in the face of positiondisturbances. The analysis in this paper will take a much broader look at theeffect of disturbances, not just in terms of their steady-state effects, but also theireffects across a predefined input bandwidth.

Modelling of force controlled systems in the framework of closed-loopstructures has been studied by many researchers— [10,11,12,13,14]— in thecontext of simultaneous position/force control during contour followingmaneuvers. In these analyses, however, it is generally assumed that the contactsurface variations are known a priori. This prescribed path is then incorporatedinto the dynamic model as a time-varying kinematic constraint, or is injected asan open loop input bias. In this paper, we will take a similar approach, but willassume that the surface variations are unknown— although it might be that theycan be measured— and will study a simple one-link constrained system to helpshed better light on the underlying issues.

3 Disturbance Rejection

The analysis in this section assumes the model shown on the left-hand side ofFigure 3. Note that here we have explicitly included the environment motion .Y,.Next to it is a feedback control block diagram of the system, where the positiondisturbance enters the system at just one port. The transfer function Q(s) representsthe mapping between the position disturbance and the associated force disturbanceacting on the internal system and is given by [15]

£>•#r-

^ ch

1

-v^/eI

•i'" T

G/.f) w ,.

|0(J)

*e'/.r + bs + /"

?s'{J

Figure 3. Model of 1-link force controlled system with contact pointdisturbances.

We will now proceed to evaluate the disturbance transfer function— or in otherwords, the closed-loop transfer function between the environment motion .Y andthe contact force F^— for several common force control strategies.

Table 1 lists the associated disturbance transfer functions for P, PI, and PDcontrol. Here, Kj , A',, and K^ are, respectively, the proportional, integral, andderivative gains, and the error state is given by e = (/^ - /v). Figures 4, 5, and6 show the Bode magnitude plots of these transfer functions for several values ofthe relevant gains. In the plots for PI and PD control, the proportional gain Kyhas been set to 2.0. The values for the parameters /, b, and kg were selected tomatch those of an actual experimental force control testbed discussed later in the



Table 1. Disturbance response transfer functions for three typical controllers.

Controller Control Law F (s)/.\

/.v" + b.s +

PI u = Kje + K^t *"_,Is +hs~ +kj(\

PD+ fcs)

paper.Note that, as expected, the DC gain of all three transfer functions is zero and

the response exhibits a gain-dependent resonant peak at the natural frequency ofthe system. The high frequency asymptote is finite and equal to the contact stiffnessk ,. Thus at high frequencies, the controller essentially shows no response to thedisturbance; the robot becomes decoupled from the environment.

Also, compared with the pure proportional controller, the low frequency slopeof the transfer function for PI control is greater. This is due to the additional zerowhich adds 20 dB to the slope at zero frequency. In addition, the integral controllerresults in a Type II transfer function, whereas the transfer function withproportional control is Type I. For low frequencies, the PD controller exhibits aBode plot similar to that of the P controller. At higher frequencies, the damping

10* 10* 10*freq [rad/sec]

Figure 4. Bode magnitude plot of transfer function between contactpoint disturbance and contact force for P control.

-5010" 10'

freq [rad/sec]

Figure 5. Bode magnitude plot of transfer function between contactpoint disturbance and contact force for PI control.



"10"' 10° 10' 10* 10* 10*freq [rad/sec]

Figure 6. Bode magnitude plot of transfer function between contactpoint disturbance and contact force for PD control.

helps to decrease the resonant gain.From a simple glance at the three transfer functions above, we can see that the

only way to significantly reduce the force error due to the position disturbanceacross a wide frequency range is to somehow reduce the magnitude of the termsin the numerator, and this can be accomplished only by feeding back someinformation about the environment motion or by using a higher order controller.This is in fact the basis for the nonlinear control strategies presented in the nextsection.

4 Force Control in Closed-Loop Mechanisms

As stated earlier, for very stiff contact systems there is a finite frequency rangeover which the contact can be safely considered to be infinitely rigid. In this case,the endpoint motion nearly exactly tracks the environment motion in bothmagnitude and phase, and thus the contact relationship can be effectivelyconsidered to be kinematic. For such a system, we propose to study the modelshown on the left-hand side of Figure 7. Here, the outer link is used to apply aconstant force at the contact point, but there is a position disturbance injectedinto the system due to the motion of the cam. The aft links are assumed to be heldrigid (for example with a high-gain position servo). On the right is a free-bodydiagram of the outer link. Here, /-, /+j, /?,, and /z,+i represent the forces andtorques acting at the link ends. F and N are, respectively, the inertial force andtorque acting at the link center of mass. In our problem, the force at the link

Figure 7. Closed-loop force control with position disturbance (left),and free-body diagram of outer link (right).



endtip acts only in the direction of the unit vector d? and since there is no torqueacting at the endtip, /i,.+j = 0. Now summing the torques about the pivot point,we get

-%,,, + /,0 + (r/,x[o F, O]).d3 + (r^x(_mr^)).d3=0 (2)

where /<. is the moment of inertia of the link about its mass center, and r<.,,, andr/, are vectors (defined in reference framed) locating, respectively, the link masscenter and the link endpoint. Solving the above equation for the contact force, weget

We can see here that the contact force is a function of not only the actuatortorque, but also the link inertial force. Thus, in order to regulate the force in thepresence of contact point position disturbances, we must take into account theinertial force generated by the disturbance-imposed motion. In this example, thelink is completely constrained; that is, there is no degree of freedom because thecam displacement completely determines the link angle. The cam motion imposesa kinematic constraint on the end position of the link which can be written as

y = -/sin0 (4)

v = -/0cos0 (5)

y = /0*sin0-/0cos0 (6)

Substituting the above equation for the link angular acceleration into the expressionfor the contact force eqn (3), we find

Thus, as expected, the contact force is related to the contact point disturbancethrough a highly nonlinear relationship which derives itself specifically from thekinematic nature of the robot-environment contact.

Let us now consider a control strategy for regulating the force in such a system.It can be seen from eqn (7) that in the presence of contact point disturbances theforce can not be perfectly regulated without considering the nonlinear terms inthe equation. We rewrite this equation as

F = bT,.,.. +£ (8)V (1C i " V V)

where b is a multiplying coefficient so that bT ., constitutes the part of the forcedue to the actuator torque. The term# represents the remaining nonlinear terms.Now consider the following control law

_. . _ r \(9), = T k, + f/ + / + f, j f) -

which is expressed in terms of the error state, e = F^ - /y The control lawconstitutes a variation of feedback linearization in which through feedback, wecancel the nonlinear terms and thus leave ourselves with a linear system for which



controllers can be designed using classical techniques [16]. Substituting the abovecontrol law into eqn (7) we get as a result the following 2nd order error dynamics

-1^4-^ = 0 (10)

The feedback gains Kp, K^, and K; can be chosen freely to obtain the desirederror dynamics. Figure 8 shows some simulation results of the plant under forcecontrol for both the case without the feedback linearization (pure proportional-integral control) and for the case with the feedback linearization. The input positiondisturbance is a 1 [Hz] sine input of amplitude 0.05 [m].

In the above control law, we assume implicitly that we can precisely measureall the quantities to be fed back. Specifically, we require a measurement of thecontact point disturbance motion. Note, however, that by our original assumptionthat the contact is infinitely rigid, we are in effect saying that the contact pointdisturbance motion is equated with the endtip motion (there is no additionaldynamics in between) and so this disturbance motion can be measured simply bymeasuring the link endtip motion. Even so, the feedback linearization control lawof eqn (9) is extremely ideal, and as such is subject to the same problems as allfeedback linearization control strategies — namely, the fact that perfect cancellationof the nonlinear terms assumes perfect knowledge and measurement of the requiredquantities, in addition to zero actuator and sensor delays.

Kp = .3. Ki = 2.0

0.4 0.6time [sec]

0.2 0.4 0.6time [sec]

0.8

Figure 8. Simulation of one-link force controlled system with positiondisturbance. The plots show the contact force (left) and torque (right)responses for both a pure PI controller and a PI controller withnonlinear disturbance feedback.

5 Experimental Verification

Figure 9 shows a photograph of the experimental setup used to test the conceptspresented above. A 1:10 gear-ratio current-controlled DC motor is used to drivea rigid link, the endtip of which is equipped with a load cell sensor and anaccelerorneter. A low friction bearing is mounted at the end to ensure that thecontact force vector is dominated by the component normal to the contact platesurface— in other words, we assume there are no tangential forces acting on theendtip. A potentiometer is mounted at the output of the revolute drive shaft to



Figure 9. Photograph of experimental testbed.

measure the link angle. The load cell is mounted inside the bearing mount and ispreloaded prior to experiments. The accelerometer is used to measure accelerationsat the link endtip.

A second linear-drive actuator is used to drive an aluminum contact plate onwhich the link applies a force. Thus with the re volute drive we have a simple 1-link rigid torque-controlled system, and with the linear drive we can inject positiondisturbances at the contact point, much the same way as with the cam in Figure 7.The linear drive is position controlled using a laser range sensor to measure theactual motion of the contact plate. Additional testbed parameters are listed inTable 2. In addition, Table 3 gives the results of an open-loop system identificationexperiment which was used to find a linear third-order model (one pole for theactuator dynamics and two complex poles for the robot-environment dynamics)that best represents the actual plant behavior.

Next, we evaluate the reponse of this system to contact point disturbances forvarious controllers. The results are shown in Figure 10 for P, PI, and thedisturbance feedback linearization controller of eqn (9). Shown on the top left isthe actual motion of the contact plate. We see that the P and PD controllers exhibit

Table 2. Experimental testbed parameters.

Link drive :

Contact plate drive :

Link length :Link inertia :

Load cell :Accelerometer :Contact surface :

100V DC motor, 1:10 Planetary gearheadcurrent controlled12V DC motor, linear ball-bearing driveouter-loop position servo0.23 [mj0.0036 [kgrrr ]0-5 [kgf]max Ig8mm thick 5052 aluminum plate



Table 3. System parameter identification results.

poles :

zeros :

w,,:

C:

b:

k,:

-1320, - 54.7 ±

744.57, -237.03

340.8 [rad/sec]

0.1604[kg/rad]

0.394 [kgfmse<

1818[kgf/m]

336.4,"

:/rad]

significant force error in response to the disturbance. The PI controller performsbetter but this is largely due to the fact that although the input reference signal tothe linear drive is sinusoidal, the actual motion trajectory of the platemore closelyresembles a ramp, and as shown earlier, since the PI controller results in a Type IItransfer function between contact plate motion and contact force, it exhibits zerosteady-state error to a ramp disturbance input. The nonlinear feedback controllersucceeds in reducing the magnitude of the spikes where the acceleration is thegreatest, but it should be noted that, as predicted by simulation results, even smallvariations in parameter values as well as actuator and sensing delays (due to

0.8

0.6

0.4

0.2

0

-0.2

-0.4

-0.6

P control: Kf = 0.2, Ki = Kd = 0.0

2 3 4time [sec]

PI control: Kf = 0.2, Ki = .05, Kd = 0.0

3 4 5time [sec]

PI control with nonlinear feedback:

„ 3.5

I 38 2.5S. 21 1.5I 1U 0.5

0

I I I I I I

: : :.!....: J...1 !I t ', _-. ....._lwWV -A/ww W/A rJvvJ Vyy x—

i i i I i i

*B>

0)

i

Contact

3

2

1

0

535

2c;

150

I i I I ! I

" n ! ! l : -r"t"i PIT"; rj, r

li • Lx/L ArjJuJ VVtWV AJULiJ Vut ....: T%/\r r<V ff\/VYYV VY\^""--

\ I \ i ! •T ' ; ' I ' "|

~ i ! ; : ; i -"- : ' : : ' :i i i i i i

1 2 3 4 5 6time [sec]

2 3 4 5 6time [sec]

Figure 10. Experimental results of disturbance response for several controllers.



filtered sensor outputs) result in significantly poorer performance. Unfortunately,with the actual motion of the contact plate being ramp-like, the acceleration iszero over most of the curve, and thus it is difficult to adequately compare thenonlinear feedback controller with the PI controller.

6 General Modelling of Constrained Systems with Contact PointDisturbances

Consider an /i-link constrained manipulator whose end-effector moves along arigid frictionless surface as shown in Figure 11. The equations of motion of theconstrained system are given by[12]

M(q)q + H(q,q) = T + J F, (11)

where q = [<7i,#2»•••#/?] is the vector of generalized coordinates, which in thiscase represents the joint angles; M is the symmetric, positive-definite massmatrix; His a matrix containing Coriolis and centrifugal terms;! is the vector ofgeneralized input joint torques; F^ is the contact force in workspace coordinates;and J is the nonsingular Jacobian matrix which relates instantaneous motions inworkspace coordinates to motions in joint space coordinates.

Figure 11. General /z-link constrained manipulator.

6.1 Constrained Motion DynamicsAssume that the constraints on the end-effector motion can be written as

= 0 (12)

where O is assumed to be twice differentiable andp is the position vector of theend-effector in a fixed coordinate system. Note the explicit dependence on time,indicating that this is a time-varying constraint. In terms of the above constraintspecification, the normal contact force can be written as [17]

(13)

where

(14)



and X is the vector of Langrange multipliers.Equation (11) represents a highly nonlinear system of equations in which the

controlled variables, q and X, appear explicitly. In addition, due to theconstraints, O, the generalized coordinates q are not all independent. Our goal isto control both the end-effector position and contact force. Thus, we would liketo arrive at an integrable set of equations in which q and X appear explicitly andwhich guarantee at the same time that the constraints given byO are satisfied.

We follow a development similar to Hemami [10] and MacClamroch [11],except that we will retain the time-varying portion of the constraint equation andtreat this term explicitly as an unknown disturbance. First, differentiate theconstraint equation <& twice to arrive at an expression for p

, = Dp + <f», = 0 (15)

Dp + Dp + <£„=() (16)

from which we get

p = D-i[-bp-OJ (17)

Using the Jacobian relationship, we can write

p = Jq (18)

p = Jq + Jq (19)

q = J-'[p-jq] (20)

Substituting (17) into (20) and using (18), we get

q = J-*[D \-DJq -*J-jq] (21)

Next, we substitute the above equation for q into the original equations ofmotion and solve for the Langrange multiplier, X. This yields

X = P"' VM"'[H -T]- P"' Vq - P (<% (22)

where, similarly to MacClamroch, we have

V = DJ (23)

P = VM-'jV (24)

Equations (11) and (22), which can be written in the following form,

q = 0(q, q, T, X, *„(») (25)

X = F(q, q, T, *,,(r)) (26)

represent two equations in the controlled variables q and X. The equation for Xhas been derived by taking into account the constraint O. Thus by using thisresult in the equation for q and integrating, we are assured that the system will atthe same time satisfy the constraint equation.

We now present the linearization of the above two equations (25) and (26)around an equilibrium point (q , X,,) [10, 14]. This will permit us to express theequations in state-space form. Proceeding with the linearization, we have



MiSq + HiSq + HoSq^T+EiSJt (27)

8A. -f- F;Sq + Fi8q + G5x + N8<&,, (28)

where 6q = q - q,,, Sq = q ,5q = q, 5X = A.-A.,,, 81 = i , &#„ = <&„ and thecoefficient matrices are given by

Mi=M(q,,)

H,=—H(q,q)dq

F = -

_8_

3q

We can now write equations (27) and (28) in state-space form as follows

B5u + ESX

5X = F8x•

where Sx = [8q, 8q] , 8u = ST^ 8d =

A.I

and

0 I

M M:T»

M;

o

(29)

(30)

(31)

(32)

(33)

(34)

(35)

(36)

(37)

(38)

Introducing a new state z = J 8A., we can then write

L 0 z

+ EG1 .

G HN|8d (39)

6.2 Inclusion of DisturbancesWe would like to include the disturbance d in our state-space model in a formwhich enables us to apply state-space control techniques. Assume that thedisturbance can be described as the solution of the following differentialequation.

d = A,yd (40)



For example, a sine-wave input disturbance of frequency co rad/sec andamplitude d^ can be expressed as the solution of a differential equation of theform

6/ + m^ = 0 W(0) = a*/,,) (41)

which can be written in the form given by equation (40) where d = [d, d] and

o 11w: Oj (4

If we now expand our original state vector to include the disturbance state,

x = [Sx. z, d] (43)

we can rewrite equation (39) with the disturbance state included as follows

(44)

~Sx~

z

d

=

"A + EF o EN"

F O N

0 0 A.

"6x~

z

d

+

B + EG

G

0

Su

or

x = Ax + Bu

y = C x + Du

(u = 8u)

. ri o oi _ roi

'' • ' oJHo]

(45)

(46)

(47)

6.3 Optimal Control with Disturbance InputConsider the application of LQR optimal control to equations (45) and (46). Weapply the following quadratic minimization criteria in which both the outputs andthe states are weighted.

(48)

Note that we cannot directly weight the state, since the state vector includes thedisturbance state, and it will generally not be possible to drive the unknowndisturbance to zero while at the same time forcing the input to zero [18].

Minimizing the above integral yields the optimal state feedback matrix, K,given by

K = R 'B L (49)where for infinite control intervals, L is given by the solution of the followingAlgebraic Riccati Equations (ARE) [18].

0 = C^QC - LBR-'lFL + LA + A^L

Once K is determined, the joint torque input is easily computed as

(50)

(51)



6.4 Numerical SimulationsNumerical simulations of a two-link constrained manipulator were carried outusing the above formulation. As shown in Figure 12, the manipulator is asked totrack a straight-line trajectory along the v-axis while at the same timemaintaining a constant normal contact force. The dynamics are linearized aroundan operating point ((.Y,),,, (yj,,, Aj = (0 [m], 1.3 [m], 6), where ( , yj arethe coordinates of the end-effector with respect to the inertial, base-fixed frame.These nominal values correspond to link angles (0 , 0iJ = (.7064, 1.726)[rad]. For simplicity of calculation, the link lengths are set at 1 [m] each. Themass of each link is set at 1 [kg] and is assumed to be concentrated at the distalend of the link. The mass matrix for this manipulator is thus given by

M = /£(52)

ii/ii

Figure 12. Simulation Model.

The contact surface variation is assumed to be a sinusoid of amplitude 0.05[m] and frequency .5 [Hz]. Therefore the constraint equation can be expressed as

0(.v,f) = x - 0.05sin(/rr) = 0 (53)

The linearized equations (41) and (42) are used to construct an optimal feedbackgain matrix, K. Next, an error is injected into the system by specifying initialconditions that are different from the nominal operating point. For thesesimulations, the initial condition was taken to be (*,, », A) = (0 [m], 1.25 [m],5.5), which in terms of the joint angles is (0,, &,) =(6.6751, 1.791) [rad]. Thusthe initial error is (80,, 80i, 8A) = (-.0325 [rad], .0649 [rad], 0.5).

Figure 13 shows the initial condition response of the closed-loop system fortwo cases: (1) No disturbance signal, (2) .5 [Hz] sinusoidal disturbance asdescribed above. The initial condition represents a deviation from the nominalvalues around which the equations were linearized. Thus the simulations are anindication of the error convergence dynamics of the previously derived optimalcontroller.

For the case with no disturbance (left-hand side), we see that all the errorsconverge to zero within 2-3 seconds. The contact force regulation is particularly



— 0.06- 0.04§ 0022 0--002-004

. thlerr"":"" — — . th2err

\ ; i^ ^ i i

x r T • : i^ \ \ \ i i3 0.5 1 15 2 2.5

time fsecl

I !r

i i3 3.5 4

thdot_err [rad/sec] 02

0.10

-0.2

&[ th1dot_err I

* " "'. ~ ~l : : i

1 | • ; : : i '.4f : : i : i •- —

\ i i i i i0.5 1 15 2 2.5 3

time [sec]35 4

01n

^ -0.1i

E "0-3-G-0.4

-n^

_ :^ : I j

/ ye

i i i

13' T i ^

...: ! : _ 1281: : 127

• 126...I j ; _ 125

i I i 1240.5 1 1.5 2 2.5 3 35

time [sec]

Fz0)3g-n

4U30'

20100

-10°c

' .joint

k"Z-' ri i i

) 02 0.4 0.6time [sec]

rn —1J

i0.8 1

0.08=o 0.0603— 0.04£ 0.02m oCD£-0.02-0.04

th 1 err\_ — — - th2err

-\ ' i i '

J i i i !) 0.5 1 1.5 2 2.

I

I5 3

I

I3.5 A™"0 0.5

o 02 i 1 —_w j !m jj\ ;

®, OH L/j•8 n?L( j(Q -0-2 r, :0 i', It=^0 0.5

1 15 2 2.5 3time [sec]

th 1 dot err

i i i i i1 1.5 2 25 3

time [sec]

35 ^

j35 A

tl

«

0.10

-0.2

-0.5-06

. . ,

-i- i \ 4

t — : • : ; • \ —j ! i ! ; 1

1.311 31291281 971261251?4

FZCD

O

0 0.5 1 1.5time

30 _

10 L :

o Z L.-m I

0 02 0.4time

2 2.5 3ec]

iointl 1joint2 1

|0.6 C

[sec]

3.5 4

j_|_j

1

1.8 1

Figure 13. Simulation results of two-link constrained manipulator withdisturbance input (right) and without (left).

fast, taking less than a tenth of a second, and \\ reaches its appropriate nominalvalue of 1.3 [m]. The fact that the two joints shoot off with opposite velocities iscorrect given the initial configuration, in which the end-effector is locatedbeneath the target position and the applied force is less than the nominal. Thus,link 1 pushes in (into the wall) to increase the contact force, while link two drivesforward (up the wall) toward the desired position y^.

In the case with contact surface variation (right-hand side), again the errorsare regulated to zero within 2-3 seconds, after which there is a steady-statefluctuation in the link angles due to the sinusoidal disturbance. The end-effectorposition, >v, settles to within 5 [cm], and no appreciable error is apparent in thecontact force response despite the presence of the disturbance. It should be notedthat the above results are only a rough indication of the behavior of the system, asdifferent responses can be obtained by changing the quadratic weighting



matrices Q and R in equation (48). In addition, the state feedback controllerdepends on some knowledge of the disturbances. In practice, this information isoften not available, but in some instances, it may be possible to use forward-looking vision sensing to measure the contour variations and then formulate amathematical description of this disturbance which can then used in the abovecontrol formulation.

7 Conclusions

This paper has presented a discussion of the issues related to the force control ofclosed-loop kinematically constrained systems. Through the analysis of a simpleone-link kinematically determined mechanism, it was shown that in order toachieve force control in the presence of contact point disturbances, it is necessaryto consider inertial accelerations imposed by the disturbance. Controllers de-signed around the feedback of contact point accelerations can potentially lead tobetter force regulation, although actuator and sensor delays can result indetrimental phase lag effects. In addition, larger actuator torques are required inorder to cancel the inertial effects.

One of the principal drawbacks of current approaches to solving thesimultaneous contour tracking/force control problem is that they generally relyon a priori knowledge of the constraint surface variation. These knownvariations are then fed forward in order to generate the open loop portion of theinput signal [12].

For the force control example depicted in Figure 2, however, it is generallynot possible to know the contact surface variations ahead of time. In this case, itmay be possible to either measure or estimate the variations on-line. Barring this,the variations must me modelled explicitly as disturbances, and feedback controllaws must be derived based on the disturbance model. This paper has presentedone such approach to incorporating contact point disturbances in the frameworkof constrained system modeling.

References

1. Raibert, M.H. & Craig J.J. Hybrid position/force control of manipulators,ASME Journal of D\namic Systems, Measurement and Control 1981, 102126-133.

2. Luh, J.Y.S., Walker M.W., & Paul R.P.C. Resolved-acceleration control ofmechanical manipulators, IEEE Transactions on Automatic Control 1980AC-25, 468-474.

3. Salisbury, J.K. Active stiffness control of a manipulator in cartesiancoordinates, Proceedings of the IEEE Conference on Decision and Controlpp. 95-100, 1980.

4. Whitney, D.E. Historical perspective and state of the art in robot forcecontrol. Proceedings of the IEEE International Conference on Robotics andAutomation, pp. 262-268, 1985.



5. Hogan, N. Impedance control: An approach to manipulation, Parts I, II, III,ASM E Journal of Dynamic Svsterns, Measurement, and Control, 1985,107-1, 1-24.

6. Eppinger, S.D. & Seering, W.P. Understanding bandwidth limitations inrobot force control, Proceedings of the IEEE Conference on Robotics andAutomation, pp. 904-909, 1987.

7. Volpe, R. & Khosla, P. An experimental evaluation and comparison ofexplicit force control strategies for robotic manipulators, Proceedings of theIEEE Conference on Robotics and Automation, pp. 1387-1393, 1992.

8. An, C.H. & Hollerbach, J.M. Dynamic stability issues in force control ofmanipulators. Proceedings of the IEEE Conference on Robotics andAutomation, pp. 890-896, 1987.

9. De Shutter, J. A study of active compliant motion control methods for rigidmanipulators based on a generic scheme, Proceedings of the IEEEConference on Robotics and Automation, pp. 1060-1065, 1987.

10. Hemami, H. & Wyman, B.F. Indirect control of the forces in constraineddynamic systems, ASME Journal ofD\namic Systems, Measurement, andControl, 1979,101, 355-360.

11. McClarnroch, N.H. & Huang, H.P. Dynamics of a closed chain manipulator,American Control Conference, pp. 50-54, 1985.

12. Cai, L. & Goldenberg, A.A. An approach to force and position control ofrobot manipulators, Proceedings of the IEEE Conference on Robotics andAutomation, pp. 86-91, 1989.

13. Mills, J.K. & Goldenberg, A.A. Force and position control of manipulatorsduring constrained motion tasks, IEEE Journal of Robotics and Automation,1989, 5-1, 20-46.

14. Krishnan, H. & McClamroch, N.H. A new approach to position and contactforce regulation in constrained robot systems. Proceedings of the IEEEConference on Robotics and Automation, pp. 1344-1349, 1990.

15. Ohkami, Y., Matsunaga, S., & Lakhani, H. Force control in the presence ofcontact point disturbances (In Japanese), Proceedings of the 13th AnnualJapan Robotics Society Fall Conference, pp. 519-520, 1995.

16. Slotine, J.J.E. & Li W. Applied Nonlinear Control, Prentice Hall,Englewood Cliffs, 1991.

17. Goldstein, H. Classical Mechanics, Addison-Wesley. Reading,Mass., 1950.

18. Friedland, B. Control System Design, McGraw-Hill, New York, 1986.


36 Structures in Space - WIT Press Structures in Space include higher order system dynamics, especially those between the actuator and sensor, as the non-collocated …

Documents