Force/Position Tracking of a Robot in Compliant Contact with Unknown Stiffness and Surface Kinematics

Transcription

1 27 I International Conerence on Robotics an Automation Roma, Italy, -4 April 27 FrC.2 Force/Position Tracking o a Robot in Compliant Contact with Unknown Stiness an Surace Kinematics Zoe Doulgeri Member, I, an Yiannis Karayianniis Stuent Member, I Abstract This work eals with the problem o orce/position trajectory tracking uner uncertainties arising rom surace position an orientation. A robotic inger with a sot hemispherical tip o uncertain compliance parameter is consiere in contact with a rigi lat surace. A novel aaptive controller is esigne using online estimates o the unknown parameters an is prove to achieve orce an position tracking by ensuring the convergence o the estimate normal to the surace irection to its actual value. The perormance o the propose controller is emonstrate by a simulation example. I. INTRODUCTION Many applications o robots involve tasks in which the robot en-eector is in contact with the environment. In such tasks, the en-eector position an the interaction orce between the robot an the environment, have to be simultaneously controlle to achieve either setpoint targets, or esire trajectories. In the orce an motion control problems, uncertainties may arise rom both the robot moel an the environment. Robot moel uncertainties are mainly owing to the lack o knowlege o system parameters. Moreover, kinematic uncertainties aecting the robot an the contact kinematics or surace Jacobian may appear whenever the shape o the contacte surace an its position are unknown. For a compliant contact, there are urther uncertainties regaring the stiness parameter. The majority o the works ealing with surace kinematic uncertainties consier rigi contact between the en eector an the environment [] [6]. In the rigi contact case, the en-eector can not move along the surace normal; hence, en-eector velocities lie on the plane tangent to the surace at the contact point. This act allows the use o tip velocities in the ientiication o the constraint surace []. For a rictionless point contact, the contact orce lies on the surace normal an can be irectly use to etermine the constraint Jacobian [2], [3], [5]. In the more practical case o contact with riction both measurements o velocities an orce are use to calculate the surace normal irection [6]. In orer to avoi calculation errors an the use o orce erivatives in the control law, an aaptive orce-motion controller with estimates o the constraint Jacobian is propose in [4] exploiting the geometrical relationship between orce an en-eector velocity. Regaring uncertainties on surace position, vision systems have been aitionally use to ientiy the esire position [], [5]. Z. Doulgeri an Y. Karayianniis are with the Aristotle University o Thessaloniki, Department o lectrical an Computer ngineering, Thessaloniki 5424, Greece. -mail: oulgeri@eng.auth.gr, yiankar@auth.gr In the case o compliant contact, the previous methoologies cannot be applie because the orce error is couple with the tip velocities. The problem o orce/position regulation oes not require an online aaptation o the stiness parameter to guarantee system stability [7] [9]. On the other han, surace kinematic uncertainties aect the convergence o the position error [9]. Nevertheless, the problem o orce/position regulation has been successully solve uner both stiness an kinematic uncertainties by an online aaptation o the surace normal irection that converges to its actual value [8]. There are some works in orce an position tracking uner stiness parameter uncertainties none o which however eals with surace kinematic uncertainties [] [3]. In act in [] aaptive orce control is applie to estimate unknown parameters uner the unrealistic assumption o a constant robot inertia matrix while in [] aaptive control is applie or the unknown stiness ater ully linearizing an ecoupling system ynamics. The works o Yao et al. [2] an Villani et al. [3] involve the use o the orce erivative in the control law through the reerence velocity erivative an hence they require extra control eort to overcome the problem o this noisy signal. At our knowlege there are no works that eal with surace kinematic uncertainties in compliant contact. This work consiers surace kinematic an stiness uncertainties as in [8] but eals with the problem o orce an position tracking as oppose to regulation [8]. An aaptive controller is propose an is prove to achieve the control target by ensuring the convergence o the estimate surace orientation to its actual value. II. PROBLM DSCRIPTION Consier a robot inger with sot hemispherical ingertip o raius r in contact with a rigi lat surace. Let q R nq be the vector o the generalize joint variables an {B} the inertia rame attache at the inger base (Fig. ). Let the surace rame {s} be attache at some point on the surace; its position is enote by p s an its orientation by matrix R s = [n s o s a s ] such that n s R 3 is the unit vector normal to the contact surace pointing inwars. Consier also the rame {t} at the inger rigi tip with position p t R 3 an rotation matrix R t that can be parameterize by three rotation angles ϕ t R 3 aroun the axes o the inertia rame. Let the generalize rigi tip position be p = [ ] p T t ϕ T T t R 6. The generalize velocity ṗ = [ ] ṗ T t ωt T T R 6 is relate to the joint velocity q through the rigi tip Jacobian J = /7/$2. 27 I. 49

2 FrC.2 [ ] J T v Jω T T R 6 n q as ollows: ṗ = J(q) q () At the center point o the contact area, we attach the contact rame {c}, escribe by the position vector p c an the orientation matrix R c = R s. For a sot hemispherical ingertip, the contact point ientiies with the rigi tip projection on the surace i.e., Q s p c = Q s p t where Q s = I n s n T s is the projection matrix. Moreover, or a lat surace the projections o p c an p s along the surace normal are equal (n T s p s = n T s p c ) an hence we can erive the material eormation as a unction o p s, p t : B Δx = r n T s (p s p t ) (2) Sot Hemispherical Fingertip p t t p c c p s nˆs Rigi Surace x s Surace stimate Fig.. A robot inger with a sot hemispherical ingertip in contact with a rigi surace We reasonably assume that the integral o the stresses that are istribute nearly semi-circularly over the contact area o the eorme ingertip arises as an aggregate orce perpenicular to the surace at the contact point. The concentrate orce can be expresse as F c = n s, where the orce magnitue is assume to be measurable by a sensor attache at the ingertip [4] an is in general a monotonically increasing, continuously ierentiable nonlinear unction o Δx : (Δx) i Δx >, =i Δx. A typical orceeormation relationship is (Δx) =k s Δx μ, μ R, where k s is the elasticity moel constant. The contact orce F c is mappe to the rigi tip as a generalize orce F = n R 6, with n =[n T s T 3 ] T R 6 enoting the generalize normal irection. In this work, we consier the orce/position trajectory ollowing problem uner uncertainties arising rom surace position an orientation. We urther assume that although the orce eormation relationship takes the typical structure given above the elasticity moel constant maybe unknown. n s s Uncertainties on the surace position an orientation aect the accurate einition o the esire position trajectory p c (t) in the three imensional space. Furthermore, measurements o the contact point position can not be easily obtaine in the case o an area contact. These problems can be easily overcome i the surace orientation is known. Then, a trajectory eine in the three-imensional space can be projecte on the surace Q s p c to annihilate errors that may exist along the surace normal irection an since the rigi tip projection on the surace ientiies with the contact point i.e. Q s p c = Q s p t, the error Q s (p t p c ) can be use or control purposes. Since the surace orientation is uncertain in this work, the basic iea is to esign a controller to achieve both slope ientiication an orce position tracking. In act, the controller is esigne using online estimates o the unknown parameters o surace orientation n s (Q s ), surace istance θ = n T s p s an the inverse o elasticity moel constant θ = ks an achieves esire orce (t) R + an position p (t) =[p T c (t) ωt t (t)]t R 6 tracking by ensuring the convergence o n s estimate to its actual value. We assume that joint positions an velocities are measure an that the rigi tip Jacobian J is known; hence, rigi tip position an velocity can be calculate by using the robot orwar kinematic relationships. We urther assume that the robot ynamic moel is known. The ynamic moel o the robot ignoring riction orces can be written as ollows: ( ) M(q) q + q) q +g(q)+j T (q)n = u (3) 2Ṁ(q)+S(q, where M(q) R nq nq is the robot inertia matrix that is positive einite, S(q, q) R nq nq is a skew symmetric matrix, g(q) R nq enotes the gravity vector an u enotes the input control law. III. CONTROLLR DSIGN For simplicity we consier the case o a linear elasticity moel that implies = k s Δx, = k s Δẋ. Hence, estimates o the eormation trajectory an its erivative can be expresse with respect to the orce trajectory, an the online estimate o parameter θ, ˆθ as ollows: Δˆx = ˆθ (4) Δˆẋ = ˆθ (5) The estimate o the eormation using (2) can be given by: Δˆx = r ˆθ +ˆn T s p t (6) where ˆθ an ˆn s are online estimates o θ, n s respectively. The generalize estimate o normal irection enote by ˆn is eine accoring to the einition o n i.e. ˆn =[ˆn T s T 3 ] T. The ucliean norm o ˆn enote by ˆn epens on the aaptation law an can take non-unit values. However, i the aaptation law is such that ˆn, the estimate o the generalize projection matrix Q = I 6 nn T is eine as ˆQ = I 6 ˆnˆnT ˆn 2 (7) 49

3 FrC.2 Notice that the estimate ˆQ correspons to a projection matrix that enjoys the basic properties: ˆQ = ˆQT, ˆQ = ˆQ2 an ˆQˆn =. Let us eine the reerence velocity vector ṗ r R 6 in the rigi tip operational space: ṗ r = ˆQ (ṗ αδp)+ ˆn ( ) ˆn 2 Δˆẋ βδˆx γδf (8) where α, β, γ are positive control gains, Δp = p p is the position error, δˆx =Δˆx Δˆx is the estimate eormation error an ΔF = t Δτ is the integral o the orce error. Using (4), (5) an (6), the reerence velocity vector can be written as ollows: ṗ r = ˆQ (ṗ αδp)+ ˆn ˆn 2 (Δẋ βδx γδf ) + ˆn ( ) (9) ˆn 2 ζ T θ + βpt ñ where δx = Δx Δx is the actual eormation error, ñ =[ñ T s T 3 ] T with ñ s = n s ˆn s is the normal irection error, ζ T = [ + β β ] is a regressor vector an θ T =[ θ θ ] is the parameter error vector with θ = θ ˆθ, θ = θ ˆθ. The reerence acceleration p r can be calculate as ollows: p r = [ ˆQ] (ṗ αδp)+ ˆQ( p αδṗ) + [ ] ˆn ˆn 2 (Δˆẋ βδˆx γδf ) + ˆn ˆn 2 { [ ] [ ] Δˆẋ β( Δˆx ˆn ˆn 2 γδ [ ] } Δˆx ) () where [ ] enotes the time erivative o a matrix, vector or scalar. The erivatives o the quantities appearing in () are calculate as ollows: [ ˆQ] = [ ˆn 4 2ˆn T ˆnˆnˆn ( )] T ˆn 2 ˆnˆn T T +ˆn ˆn [ ] ˆn ˆn 2 = [ ] ˆn ˆn 2 ˆn 4 2ˆnˆn T ˆn [ ] Δˆẋ = ˆθ + ˆθ [ ] Δˆx = ˆθ + ˆθ [ ] Δˆx = ˆθ + ˆn T s p t +ˆn T s ṗ t where ˆn [ ˆn T s T 3 ] T, ˆθ an ˆθ are given by upate laws that will be eine through the subsequent stability analysis. We also eine the error: s p =ṗ ṗ r () that can be written as ollows: s p = ˆQṗ + ˆn ˆn 2 Δẋ ˆn ˆn 2 ñt ṗ ṗ r (2) Substituting (9) in (2) we get: s p =ŝ Q +ŝ n (3) where ŝ Q = ˆQ(Δṗ + αδp) (4) ŝ n = ˆn [ δẋ + βδx + γδf + ζ T θ (ṗ + βp)t ˆn 2 ñ ] (5) On the other han s p can be expresse as ollows: s p =ṗ + A(ˆn)p v(ṗ, Δẋ,p, Δx, θ, ˆn, ΔF ) (6) where A(ˆn) is a uniormly positive einite matrix or all ˆn given by: A(ˆn) =α ˆQ + β ˆnˆnT ˆn 2 (7) an v = ˆn { Δẋ ˆn 2 + βδx γδf ζ T θ + β(θ r) } + ˆQ(ṗ + αp ) (8) In the non-reunant case, we can also eine the reerence joint velocity vector q r as: an the error s as ollows: q r = J ṗ r (9) s = q q r = J s p (2) The reerence joint acceleration vector can be calculate by: q r = J J q r + J p r (2) using () an (9). Now, we can propose the ollowing moel base input control law: u = J T ˆn( k Δ) Ds+M q r + ( 2Ṁ(q)+S(q, q)) q r +g (22) The controller consists o a eeorwar term o the esire orce trajectory magnitue an a proportional term o Δ through the positive control gain k, that lie on the estimate normal irection as well as a eeback o s through the positive einite gain matrix D. Substituting the input control law (22) into the robot ynamic moel (3) we obtain the close loop system: ( ) Mṡ + 2Ṁ + S s + J T ˆnk Δ + J T ñ + Ds = (23) where k = k +. Taking the inner prouct o the close loop system (23) with s we get: { 2 st Ms+ k I(δx)+ 2 k γδf 2} + k Δζ T [ θ + sp k Δ(ṗ + βp) ] T ñ (24) + βk δxδ + s T Ds = where I(δx) = δx {(ξ +Δx ) } ξ is the potential owing to eormation error with I(δx) > or δx.for the case o linear orce-eormation relationship the potential 492

4 FrC.2 is given by I(δx) = 2 k sδx 2. The parameter upate laws are chosen as ollows: ˆθ = Γζk Δ (25) ˆn s = P { Γ n [ I3 O 3 3 ]{ sp k Δ(ṗ + βp) }} (26) where Γ = iag [γ i ] i=2 i=, Γ n = iag [γ ni ] i=3 i= are iagonal matrices o parameter upate gains an P is an appropriately esigne projection operator [5] with respect to a convex set: S = {ˆn s R 3, ˆn T s ()ˆn s ε }, ε R + (27) that contains the actual normal irection i.e. ε<cos φ, φ < 9 where φ enotes the angle o the initial estimate with the actual normal vector (Fig. 2). Hence, the positive constant ε is a measure o the allowe uncertainty. The projection operator ensures that ˆn s. Fig. 2. T nn ˆ ˆ () s s nˆs ˆ () n s n s The projection operator convex set Notice that the control law given by (22) an the upate laws (25), (26) o not require the use o orce erivative. Using (25) an (26) in (24) we get: V + W = (28) where V = 2 st Ms+ k I(δx)+ 2 k γδf 2 + 2ñT s Γ n ñ s + 2 θ T Γ θ (29) W = βk δxδ + s T Ds (3) Function V is positive einite with respect to s, δx, ΔF an parameter errors θ, ñ s while unction W is positive einite with respect to s, δx. Hence, unction V has a negative erivative i.e. V = W an can be regare as a Lyapunov-like caniate unction in orer to prove the ollowing theorem. Theorem: The input control law (22) with the upate laws (25), (26), applie in (3) achieves the convergence to zero o orce, position an velocity tracking errors as well as slope ientiication i.e. Δ, Δ, QΔp, QΔṗ an ñ s. V Proo: = W implies V (t) V () an hence s (s p, ŝ Q, ŝ n ), δx (Δ), ΔF, θ, ñ s (ñ) L. Hence, given p, ṗ,, are boune trajectories, v L an consequently (6) implies ṗ + A(ˆn)p L.SinceA is boune an positive einite, it can be easily prove that ṗ, p L an in turn δẋ L. I the robot moves away rom singular positions, the bouneness o ṗ implies that q is boune. Hence, ñ s, θ are boune. From (23), ṡ can be expresse as a sum o boune quantities an hence ṡ (ṡ p ) L. From (28)-(3), δx, s L 2.Further,δx, s are uniormly continuous because o the bouneness o δx, s an their erivatives. Hence, it ollows that δx (Δ ), s (s p, ŝ n, ŝ Q ). From (6), the bouneness o ṡ p implies that p is boune as a unction o boune quantities an hence ṗ (δẋ) is uniormly continuous. Furthermore, (23) implies that ṡ is uniormly continuous as a unction o uniormly continuous quantities. The uniorm continuity o δẋ, ṡ as well as the convergence o δx, s to zero implies δẋ, ṡ an in turn J T (t) (t)ñ. Using the act that (t) is strictly positive an the assumption that J is non-singular we get t+t t J T (τ)j(τ) 2 (τ)τ α T, t or some α, T > that implies ñ 6 (ˆn s n s, ˆQs Q s ). The convergence o ˆn s to the actual normal irection as combine with ŝ Q implies Q(Δp + αδṗ) an consequently QΔp, QΔṗ. Notice that the convergence o θ is not require. Remark : The theorem hols even when k = an γ =. However, the use o a proportional an integral term or the orce error in (22) an (8) respectively provies an extra egree o reeom to the esigner or improving the perormance o the system response through the appropriate tuning o gains k an γ. Remark 2: For the case o the nonlinear orce-eormation relationship = k s Δx μ, the estimate parameter is θ = µ k an the regressor ζ becomes: s [ ζ T = µ ( μ + β) β Remark 3: In the presence o unknown riction an robot ynamics the propose controller can be extene to inclue a regressor term or the unknown parameters. In this case the convergence o the normal vector to its actual value woul require persistent excitation o the involve signals. Remark 4: The above analysis is vali even i the source o compliance is the surace or both the ingertip an the surace. Furthermore, the controller will operate satisactorily even in suraces with small curvature. Notice that the theorem is vali provie that the contact between the robotic inger an the surace is maintaine i.e. δx > Δx, t. This correspons to an upper boun or the potential owing to eormation error, i.e. I(δx) < Δx Δx ( (ξ)ξ, t (Fig. 3) an in turn to V < k Δx ) Δx (ξ)ξ, t. Since ] 493

5 FrC.2 V is a ecreasing unction i.e. V (() V, t, starting within the region V () <k Δx ) Δx (ξ)ξ ensures contact maintenance. For the case o linear orceeormation relationship the conition can be written as: V () < 2 k Δx an ensures both contact maintenance an less than % orce overshoot. x ( ) x x 2 x 2 x (N) ata ata time (sec) x x a) Nonlinear orce-eormation relation x 2 x b) Linear orce-eormation relation x Fig. 5. Desire an actual orce trajectories Fig. 3. Contact maintenance an potential owing to eormation error IV. SIMULATION RSULTS Consier a two-o planar manipulator with link lengths l =.3 m, l 2 =.2 m masses m =.8 kg, m 2 =.6 kg an inertias I i = mil2 i 2. The surace is moele by a line with slope ϕ s = 5 with a normal vector n s = [ ] T sin ϕ s cos ϕ s. Normal orce magnitue is simulate by = k s Δx where k s =,. The en-eector initial contact point position is p c () = [ ] T (m) an exerts a normal orce o () = 4 N. The control purpose is to exert a time-variant normal orce with magnitue (t) =9+3cos(t) (N) that correspons to an initial step o 8 N an to track the esire position trajectory p (t) = P + A sin(t) where P = [ ] T (m) an A = [. ] T (m) that correspons to an initial error o o T Δp =.2 m, where o is the tangential unit vector, without loosing contact. The initial estimate o the line slope given in the aaptive law is ˆϕ s () = 9 that correspons to an initial angle error o (m).5 o T p o T p Fig. 6. Desire an actual tangent position trajectories Δ (N) n s.95 ˆn s n s2. o T Δp (m) ˆn s Fig. 7. Normal vector estimate response Fig. 4. Force an position error response 494

6 FrC.2 ˆθ.8 x θ ˆθ u 5 (N m) 2.5 u 2 2 (N m) Fig. 8. Parameters response ˆθ(t) time (sec) Fig. 9. Control input response The estimate o elasticity moel constant is ˆk s =9, 5, that is 5% smaller than the actual, an correspons to ˆθ () = ˆk s. The initial estimate o θ is ˆθ () =.36 that is 8% smaller than the actual. The orce integrator is initially set to.45. The gains o the controller have the ollowing values: α =2, β =.3, γ =.6, k =., D = iag[7.5,.9], Γ=iag[2 5, 8 3 ], Γ n = iag[2.5,.25]. Fig. 4 shows the transients o orce an position error while Figs. 5, 6 show the esire an actual orce an tangent position trajectories. Fig. 7 shows the convergence o the estimate normal vector coorinates to their actual values. Force error converges in.5 s while the position error convergence ollows the convergence o the normal vector parameters an is achieve in 4 s. Fig. 9 shows the response o the estimate elasticity an surace position parameters ˆθ, ˆθ together with their actual values. Note that espite the lack o convergence o the estimate parameters to their actual values, the control target is achieve. Input torques, shown in Fig. 9, remain small although the initial orce an position values o not belong to the esire trajectories. θ V. CONCLUSIONS This work proposes an aaptive controller or the problem o orce/position tracking to cope with parametric uncertainties in surace kinematics an ingertip compliance. The control scheme requires measurements o joint variables an orce magnitue. Future works will investigate the eect o riction an ynamic moel uncertainties. ACKNOWLDGMNT Y.K. is supporte by the State Scholarships Founation o Greece. RFRNCS [] Y. Zhao an C. C. Cheah, Hybri vision-orce control or robot with uncertainties, in Proc. I 24 International Conerence on Robotics an Automation, 24, pp [2] C. C. Cheah, S. Kawamura, an S. Arimoto, Stability o hybri position an orce control or robotic kinematics an ynamics uncertainties, Automatica, vol. 39, pp , 23. [3] C. C. Cheah, Y. Zhao, an J. Slotine, Aaptive jacobian motion an orce tracking control or constraine robots with uncertainties, in Proc.I 26 International Conerence on Robotics an Automation, May 26, pp [4] M. Namvar an F. Aghili, Aaptive orce-motion control o coorinate robot interacting with geometrically unknown environments, I Trans. Robot. Automat., vol. 2, no. 4, pp , 25. [5] D. Xiao, B. Ghosh, N. Xi, an T. Tarn, Sensor-base hybri position/orce control o a robot manipulator in an uncalibrate environment, I Trans. Contr. Syst. Technol., vol. 8, no. 4, pp , 2. [6] T. Yoshikawa an A. Suou, Dynamic hybri position/orce control o robot manipulators on-line estimation o unknown constraint, I Trans. Robot. Automat., vol. 9, no. 2, pp , 993. [7] S. Chiaverini, B. Siciliano, an L. Villani, Force/position regulation o compliant robot manipulators, I Trans. Automat. Contr., vol. 39, no. 3, pp , 994. [8] Y. Karayianniis an Z. Doulgeri, An aaptive law or slope ientiication an orce position regulation using motion variables, in Proc.I 26 International Conerence on Robotics an Automation, May 26, pp [9] Z. Doulgeri an S. Arimoto, A position/orce control or a robot inger with sot tip an uncertain kinematics, Journal o Robotic Systems, vol. 9, no. 3, pp. 5 3, 22. [] R. Careli, R. Kelly, an R. Ortega, Aaptive orce control o robot manipulators, Int. J. Control, vol. 52, pp , 99. [] S. Chiaverini, B. Siciliano, an L. Villani, Force an position tracking: Parallel control with stiness aaptation, I Control Syst. Mag., vol. 8, no., pp , 998. [2] B. Yao, S. Chan, an D. Wang, Variable structure aaptive motion an orce control o robot manipulators, Automatica, vol. 3, no. 9, pp , 994. [3] L. Villani, C. C. e Wit, an B. Brogliato, An exponentially stable aaptive control or orce an position tracking o robot manipulators, I Trans. Automat. Contr., vol. 44, no. 4, pp , 999. [4] G. Ambrosi, A. Bichi, D.D.Rossi, an.p.scilingo, The role o contact area sprea rate in haptic iscrimination o sotness, in Proc. I 999 International Conerence on Robotics an Automation, 999, pp [5] P. A. Ioannou an J. Sun, Robust Aaptive Control. Upper Sale River, NJ:Prentice Hall,