Sysem I, Singular Mission. Sysem I, Long Distance Mission. No Convergence. y [mm] y [mm] x [mm] x [mm]

Transcription

1 Time-Inariant Stabilization of a Unicycle-Tye Mobile Robot: Theory and Exeriments ByungMoon Kim 1 and Panagiotis Tsiotras 2 School of Aerosace Engineering Georgia Institute of Technology, Atlanta, GA, Abstract In this aer we deelo two time-inariant control laws for a unicycle-tye mobile robot. A mobile robot of this tye is an examle of a system with a nonholonomic constraint. Similarly to the majority ofresultsin the literature thus far, the controllers are based on the robot's kinematic model. They do not directly address realistic factors such as motor dynamics, quantization, sensor noise or delay which may affect the robot stability and erformance. We use a Kheera robot to comare the erformance of these controllers in a realistic situation that includes all the reious factors. Introduction An underactuated system is one with fewer control inuts than indeendent generalized coordinates. Tyically, underactuated systems arise because these generalized coordinates are subject to some non-integrable motion constraint, decreasing the degrees of freedom of the system, while the number ofcontrol inuts remains the same. These systems are called nonholonomic. Seeral examles which inole nonholonomic constraints can be found in real world alications, such as mobile robots, bicycles, cars, underactuated axisymmetric sacecraft, underwater ehicles, etc. Seeral control laws hae been roosed for stabilizing such systems. One aroach is to use time-arying controllers [6, 8, 5]. Howeer, most of time-arying controllers hae slow conergence rates [4]. Moreoer, time-arying controllers may be non-robust to certain model erturbations [3]. Exerimental alidation of time-arying controllers has been erformed in [6]. An alternatie aroach isto use time-inariant, non-smooth controllers, such as those in [9, 2, 1, 1]. These control laws ensure exonential conergence rates. Their robustness roerties, howeer, is still a toic under inestigation. This aer roides a small ste towards this goal by roiding some indication on the robustness roerties of time-inariant, discontinuous controllers by imlementing these controllers on a unicycle-tye robot called Kheera. Ways to imroe the controller erformance are also discussed. 1 Graduate Student, gte342k@rism.gatech.edu, Tel: (44) , Fax: (44) Associate Professor, .tsiotras@ae.gatech.edu, Tel: (44) , Fax: (44) Corresonding author. y! Figure 1: Definition of configuration ariables. x Kinematic Equations Consider a unicycle-tye robot with two wheels, as shown in Fig. 1. The kinematic equations are _x = cos fl; _y = sin fl; _fl =! (1) The kinematic model of the mobile robot has two control inuts: elocity and angular elocity! (see Fig. 1). Equation (1) can be transformed to a normal chained (or ower) form by a state and inut transformation. Using the state transformation x 1 = x cos fl + y sin fl; x 2 = fl (2) x 3 = x sin fl y cos fl (3) one arries at two slightly different systems, deending on the inut transformation used. Systems I and II are gien by ρ _x1 = u (I) 1 ; _x 2 = u 2 ; _x 3 = x 1 u 2 = u 1 + x 3 u 2 ;! = (4) u ρ 2 _x1 = u (II) 1 x 3 u 2 ; _x 2 = u 2 ; _x 3 = x 1 u 2 = u 1 ;! = (5) u 2 The two systems are comletely equialent when then inuts are and! but are different when the inuts are u 1 and u 2. In articular, the two systems differ by the term x 3 u 2 in the _x 1 equation. Our exerimental results show that this term has a significant effect in the transient resonse of the robot. This can be seen by obsering that for System I angular elocity always generates forward elocity. In other words, for any control law u 1 fl. 1

2 and u 2 that stabilizes System I, forward and angular elocities are couled, a rather awkward condition. Also, for System I, large elocity commands may be generated when x 3 is large. Although it is easier to design a control law u 1 and u 2 using System I, its ractical imlementation faors System II. This fact has also been obsered by McCloskey and Murray [6, 7] where they imlemented their controllers (deried based on System I) using System II. Luckily, it turns out that the extra term x 3 u 2 does not destroy stability. A formal roof of the statement can be found in Proosition 2 below. Our exeriments actually showed that working with System II was always beneficial. Controller 1 The first controller is gien below u 1 = kx 1 + fi 1 (x; s); u 2 = kx 2 + fi 2 (x; s) (6) where fi 1 =+μ s(x) x x2 2 x 2 and fi 2 = μ s(x) x 2 + x 1. 1 x2 2 Proosition 1 The control law gien by Eq. (6) globally asymtotically stabilizes System I for all initial conditions such that x 1 () 6= and x 2 () 6=,ifμ> and k >. Moreoer, if μ > 2k > the control inuts u 1 and u 2 are bounded along the trajectories of the closed-loo system. Proof: roof. The reader may refer to [2, 9] for the comlete The deriation of the reious control law makes used of ideas from inariant manifold theory. Secifically, it can be shown that M = fx 2 R 3 : s(x) = g where s(x) = x 3 x 1 x 2 =2 is an inariant manifold for the closed-loo system. The nonlinear terms in (6) render M attractie and the linear terms drie any trajectory on M to the origin [9]. Singularity aoidance The controller gien by Eq. (6), is not defined on the x 3 axis. For initial conditions near the x 3 axis, large control inuts may result. A modification of controller (6) is needed for this singular case. To this end, let j j fi1 2 + fi2 2 =μ = x jsj= x2 2. Since the control inut increases with j j, this is a direct measure of the magnitude of the control inut. Let us now denote the following neighborhood of the x 3 axis D b = f(x 1 ;x 2 ;x 3 ):j j b g. The alue b is chosen by exerimentation in order to achiee a reasonable control inut and transient resonse. The set where j j < b will be denoted by D c b. For initial conditions in the region D b (where j j is large), we aly the following simle control law u 1 = k s sgn(s); u 2 = (7) to escae from D b, where k s is some constant. Asimle argument shows that with the control law (7) the trajectories leae D b in finite time. To comlete the design of Controller 1, one must make sure that the control law gien in Eq. (6) will not bring the closedloo trajectory to the region D b. A simle calculation shows that _ = (μ 2k)=2 and j j is decreasing always under this control law. Thus, once the system leaes D b, it will neer enter this region again. We now turn our attention to System II. In this case, s is not inariant under the simle feedback and one cannot aly the reious analysis to roe global asymtotic stability. Howeer, local asymtotic stability can be shown using the roerties of homogeneous systems. Before doing that, we show that the control law (6) achiees boundedness for the system in (5). Consider the radially unbounded, ositie definite function V = x x x 2 3. Its deriatie along (5) and (6) is calculated as V _ = 2k(x x 2 2)». This shows that x 1!, x 2! ast!1and x 3 is bounded. To show local stability we need the following mathematical reliminaries. Definition 1 For any set of ositie scalars r i >, i = ;::: ;n, the dilation oerator r is defined as r x = [ r1 x 1 r2 x 2 ::: rn x n ] T ; >. The homogeneous norm associated with the dilation r is a continuous function ρ : R n!r + if (i) ρ(x), and ρ(x) =if and only if x =, and (ii) ρ( r x)= ρ(x). Such a norm always exists. The next roosition basically states that the extra term x 3 u 2 in the _x 1 equation for System II does not destroy asymtotic stability. Proosition 2 The control law gien by Eq. (6) locally asymtotically stabilizes the System II, if k > and μ >. Moreoer, if μ > 2k the control law is bounded along the closed-loo trajectories. Proof: Let f(x) = [u 1 u 2 x 1 u 2 ] T and g(x) = [ x 3 u 2 ] T, then the equation for System I is gien by _x = f(x) and the equation for System II is gien by _x = f(x) + g(x). The fact that the control law in (6) globally exonentially stabilizes the System I can be shown using the Lyauno function V (x 1 ;x 2 ;x 3 )= 1 2 (x2 1 + x 2 ) 2 + s 2. The deriatie ofv along the closed-loo trajectories of System I is gien by V _ = Lf V (x) = 2k(x x 2 2) 2 μs 2» flv (x) < for all x 2 R 3, where fl = minf4k; μg >. The deriatie of V along the closed-loo trajectories of System II is gien by V _ = Lf V (x) +L g V (x), where L g V (x) = 2(x x 2 2)x 1 x 3 u 2 (x)+sx 2 x 3 u 2 (x). Consider the dilation r x = [ x 1 x 2 2 x 3 ] T. Using this dilation, one obtains that s( r x) = 2 s(x), fi 1 ( r x) = fi 1(x) and fi 2 ( r x) = fi 2(x). Therefore, u 1 ( r x) = k( x 1)+ fi 1 (x) = u 1 (x) and u 2 ( r x) = k( x 2)+ fi 2 (x) = u 2 (x). Thus, the control law in Eq. (6) is homogeneous of degree one with resect to the dilation r x. Moreoer, since L f V ( r x)= 4 L f V (x) and L g V ( r x)= 6 L g V (x), then L f V and L g V are homogeneous functions of degrees four and six, resectiely. Consider now the homogeneous norm ρ(x) associated with the gien dilation r.1 We claim that there exists 1 Choose, for instance, ρ(x) =(x x4 2 + x2 3 )

3 c> such that for all ρ(x)» c, jl f V (x)j > jl g V (x)j. To this end, let Λ = min ρ(x)»1 s jlf V (x)j jl g V (x)j (8) If Λ > 1, let c = 1 and we are done. Otherwise, let a ositie scalar c Λ such thatc Λ < Λ» 1 and consider the set ρ(x)» c Λ. Noticing that f x : ρ(x)» c Λ g = f r y : ρ(y)» 1;»» cλ g, one obtains that for any x such that ρ(x)» c Λ s s s jlf V (x)j jl g V (x)j = jlf V ( r y)j jl g V ( r y)j = 1 jlf V (y)j jl g V (y)j where»» c Λ and ρ(y)» 1. From (8) we hae that s 1 jlf V (y)j jl g V (y)j Λ > 1 Since x was arbitrary, this roes the assertion. Therefore, there always exist a neighborhood of the origin such that jl f V (x)j > jl g V (x)j. Since L f V (x) <, this imlies that V _ = Lf V (x) +L g V (x) < and System II is (at least) locally asymtotically stable. To show that the control law remains bounded for μ>2k > it suffices to show that remains bounded in a neighborhood of the origin. Along the trajectories of (5) with control law (6) we hae that _s = (μ=2) s + x 2 x 3 u 2 =2andthus _ = [(μ 2k)=2] + u 2 x 3 (4ν 2 + x 1 )=(4ν 2 )= [(μ 2k)=2] + ffi(x). Next, notice that is homogeneous of degree one, whereas, ffi(x) is homogeneous of degree three. Using a similar argument as before, we conclude that for a small enough neighborhood of the origin _ < and the control law remains bounded around the origin. Controller 2 The ond controller is based on a control law that was originally deeloed for the stabilization roblem of an underactuated axisymmetric sacecraft [1]. It is modified here for the case of a mobile robot. In this controller, the control inut is bounded by some finite alue regardless of the initial conditions. Consider the following controller, u i = k x i ν μ sat i(s; ν); i =1; 2 (9) Where ν = x x2 2 and the saturation functions sat 1 and sat 2 are defined as sat 1;2 (s; ν) = ( sat s x2;1 ν ν sgn(s); ; if ν ffl if ν<ffl (1) with ffl a small number to aoid chattering in the numerical imlementation of the controller on a digital comuter. Proosition 3 The control law gien by Eqs. (9) and (1) globally asymtotically stabilizes the System I for k and μ satisfying μ > 2k > for j j < 1 and μ > 2k > for j j 1 2. Moreoer, the control inut is bounded byju 1;2 j»jkj + μ. Proof: The roof inoles two regions deending on the alue of = s=ν. We inestigate the closed-loo trajectories in the two regions of the state sace j j < 1 and j j 1. If j j < 1, then sat 1;2 (s; ν) =(s=ν 2 ) x 2;1 and the time deriatie ofsis _s = (μ=2)s. The time deriatie of the radially unbounded, ositie definite function V s x 2 1+x 2 2+s 2 = ν 2 +s 2 is therefore gien by _V s = 2kν 2 = ν 2 +1 μs 2 <. The time deriatieof = s=ν, is comuted as _ = (1=2)(μ 2k= ν 2 +1). It follows that if μ > 2k the region where j j < 1 is inariant. It follows that for all initial conditions in this region the state will remain in this region and conerge to zero. This result shows that the closed-loo system will go to origin if the initial conditions satisfy j j < 1. For j j 1, it is sat 1;2 (s; ν) = sgn(s)(x 2;1 =ν) and the time deriatie of s is comuted as _s = (1=2)μν sgn(s). The time deriatie of = s=ν is thus comuted as _ = (μ=2) sgn(s) + k = ν Since k<, when j j 1, and since sgn(s) = sgn( ) it follows that j j will always decrease and the trajectory will enter the region where j j < 1 in finite time. Combining the reious results, we see that the controller in Eq. (9) makes the closed loo trajectories conerge to the origin for all initial conditions. The fact that the control inuts are bounded by jkj+ μ follows directly from (9). Next, we aly Controller 2 to System II. For System II consider the radially unbounded, ositie definite function V = 1 2 (x2 1 + x x 2 3). Its deriatie is calculated as V _ = k(x x 2 2)= x x This result holds regardless whether j j < 1 or j j 1. If k >, V is non-increasing for all x 2 R 3. Thus, we conclude that x 1 and x 2 go to zero and x 3 is bounded. Moreoer, since k(x x 2 2)= x x k(x2 1 + x x 2 3)= x x kv,itfollows that V is bounded below by an exonentially decaying function. Therefore, x x 2 2 go to zero asymtotically (not in finite time). Now, we hae the following roosition. Proosition 4 For k> and μ>, the control law in Eq. (9) and (1) globally asymtotically stabilizes the System II. Moreoer, the control inuts are bounded by ju 1;2 j»k + μ. Proof: The roof is similar to the one of Proosition 3 and thus, omitted. It is based on the obseration that V _» onlyifx 3 conerges to zero. Controller Imlementation The imlementation of the controllers discussed reiously was done on a Kheera mobile robot. 2 The bound 1 is taken only for conenience. It could be any ositie number.. 3

4 Kheera is a small-size robot deeloed for educational and research uroses by K-Team (htt:// It has seeral roximity sensors (not used in this work) and can work in semi-autonomous (serer-client) or comletely autonomous mode. In the serer-client mode the robot is controlled through an RS-232 serial ort by a host comuter. A C++ alication running under Windows NT was deeloed to imlement the reious algorithms and control the robot. 1Hz because of the seed limitation of the RS-232 serial communication (maximum is 4.8kbytes/s for the Kheera robot). For all exeriments in this aer, we hae chosen 5Hz for the samling frequency. The software also features a Sensors dislay tab, and a Console tab. The motor can be tested/configured ia the Performance tab. For conenience, all gains of the controller and other arameters are stored in a.ini file. The software comes also with an ini editor so that the user can change the settings online. To record the history of the control inut and robot resonse without recording time limitations, a double-buffered data storage algorithm was deeloed. The robot can also be isualized by an indeendent OenGL Window that suorts 6DOF camera naigation using the keyboard. Figure 2: The Kheera robot (htt:// The Kheera Robot The Kheera mobile robot uses two DC motor-drien wheels. The DC motors are connected to the wheels through a 25:1 reduction gear box. Two incremental encoders are laced on the motor axes. The resolution of the encoder is 24 ulses er reolution of motor axis. This corresonds to = 6 ulses er reolution of the wheels or 12 ulses er millimeter of wheel dislacement. The algorithm to estimate the elocity from the encoder outut is imlemented on the robot. For the DC motor seed control,anatie PID controller is also imlemented on the Kheera robot. All one needs to do in order to control Kheera, is to read osition signals and issue elocity commands ia the RS-232 serial ort. Imlementation of Controllers on a Windows NT Enironment C/C++ is used to imlement the reious control algorithms with a nice-looking, multi-tabbed dialog box interface, shown in Fig. 3. From the Realtime tab, one can click the target osition and orientation. The software automatically sets u a stabilizing roblem by transforming the target osition/orientation to origin and current osition/orientation to the initial conditions. For the discrete imlementation of the continuous controller, a 32bit multimedia timer serice in NT is used and all other alications are closed to minimize the timer latency. The software roides a combo-box interface to select the samling frequency of the controller in the Configuration tab. The maximum samling rate can, theoretically, be slightly oer Figure 3: Robot control rogram interface. Quantization in the Velocity Outut The elocity command of the Kheera robot is quantized by 8 mm/. At the origin, quantization is manifested as a deadzone roblem. If we aly a control law, the small elocity command will be ignored by the robot and we will get large steady state errors. For examle, the steady state error in fl was about 15 ο 6 deg for both controllers. One simle but effectie aroach is to use an inerted deadzone to handle this roblem. The resonses of the robot with and without the inerted deadzone are shown in Fig. 4. A dramatic imroement in steady-state resonse (esecially for fl) is achieed using this method. The inerted dead-zone is imlemented in software. Scaling Before imlementing the controllers to the real robot, we need to choose the units of seeral ariables, or more generally, to scale the states. By choosing the scaling alues, we can adjust the magnitude of the states, i.e., x 1 ;x 2 and x 3. For each controller, state scaling was chosen to gie the best transient resonse.. 4

5 no I.D.Z. I.D.Z Without Inerted Deadzone no I.D.Z. I.D.Z. 5 γ [deg] no I.D.Z. I.D.Z With Inerted Deadzone Exerimental Results The summary of the exeriments are shown in Table 2. In this table, `E', `N', `S' and `L' stand for easy, normal, singular and long distance missions, resectiely. `G' stands for `Good', which means that the conergence is fast enough, i.e., within 1 onds. `O' stands for oscillatory, which means that the trajectory did not conerge nor dierge but oscillated around the origin Table 2: Exerimental results. Ctr. Sys. E N S L Note 1 I G O O O Oscillatory 1 II G G G G Good 2 I G G G O Oscillatory 2 II G G G G Good Figure 4: Effect of inerted deadzone on the steady state error meter millimeter meter millimeter Figures 6 and 7show lots of some selected trajectories. As it is shown in these figures, Controllers 1 and 2 may fail to achiee conergence for System I for some cases. In System II, they achieed stability for all missions γ [deg] meter 1 millimeter Sysem I, Easy Mission 5 5 Sysem I, Normal Mission No Conergence Meter Millimeter 1 Conergence : x,y,γ < System II, Normal Mission Sysem II, Singular Mission Figure 5: Comarison of resonses with different units (scaling). 1 Conergence : x,y,γ < Conergence : x,y,γ < Exerimental Results Mission Design To comare the two controllers, we designed four missions: easy, normal, singular and long distance. We defined the difficulty of each mission by the ratio between forward and sideways motions. The initial alue of fl is chosen to be zero. A long distance mission was deised to demonstrate the motor saturation due to ossible large elocity commands. During the long distance mission the adantage of Controller 2 which has bounded inut was eident. The initial conditions for these missions are shown in Table 1. Starting from this osition, the robot is commanded to moe totheorigin. Table 1: Mission secifications. Mission x (mm) y(mm) fl (deg) Easy Normal -1-1 Singular -1 Long Distance -5-5 Figure 6: Selected Trajectories of Controller Sysem I, Singular Mission Conergence : x,y,γ < System II, Singular Mission Conergence : x,y,γ < Sysem I, Long Distance Mission No Conergence Sysem II, Long Distance Mission Conergence : x,y,γ < Figure 7: Selected Trajectories of Controller 2. Figure 8 shows a comarison between the actual and commanded elocities for Controllers 1 and 2. The. 5

6 bounded Controller 2 behaes as exected, whereas, Controller 1 exhibits large magnitude in. Figure 9 shows the commanded and actual forward and angular elocity resonses for Systems I and II using the same controller (Controller 1) for the Normal mission. It is seen that a great imroement results in the commanded elocities using System II for controller imlementation. In this case, Controller 1 exhibits highly oscillatory behaiour when alied to System I. [mm/] [mm/] Long Distance, Controller 1, System II Commanded Velocities [deg/] [deg/] [mm/] [mm/] 2 1 Long Distance, Controller 2, System II Commanded Velocities Figure 8: Selected Trajectories of Controller 2. [mm/] [mm/] Normal, Controller 1, System I x 14 Commanded Velocities [deg/] [deg/] [mm/] [mm/] 2 1 Normal, Controller 1, System II Commanded Velocities Figure 9: Commanded and Actual Velocities for Systems I and II. More detailed lots of the results are aailable from the authors uon request. Conclusion We hae exerimentally tested two time-inariant controllers for a unicycle-tye mobile robot. These controllers are discontinuous at the origin so their exerimental imlementation/alidation oses seeral interesting challenges. In articular, deadzone, quantization errors, sensor noise, oor measurements and motor dynamics can affect the resonse and een the stability of the system. We deised seeral techniques, such as inut transformation, scaling and inerted deadzone to handle some of these issues. By alying these techniques, the erformance imroed significantly and the 4 2 [deg/] [deg/] [deg/] [deg/] exerimental results matched retty closely to the theoretical redictions. Motor dynamics turned out to be a major roblem for some tasks. Oerall, the exeriments showed that discontinuous, time-inariant controllers can be used successfully in ractice if care is taken when imlementing these controllers. References [1] M. Aicardi, G. Casalino, A. Bicchi, and A. Balestrino, Closed loo steering of unicycle-like ehicles ia Lyauno techniques," IEEE Robotics and Automation Magazine, Vol. 2, , [2] H. Khennouf and C. Canudas de Wit, On the construction of stabilizing discontinuous controllers for nonholonomic systems," in IFAC Nonlilear Control Systems Design Symosium, , Tahoe City, CA. [3] D. A. Lizárraga, P. Morin, and C. Samson, Nonrobustness of continuous homogeneous stabilizers for affine control systems," in 38th IEEE Conference on Decision and Control, , Phoenix, AZ. [4] R. T. M'Closkey and R. M. Murray, Conergence rates for nonholonomic systems in ower form," in Proceedings of the American Control Conference, , San Francisco, California. [5] R. T. M'Closkey and R. M. Murray, Exonential conergence of nonholonomic systems: some analysis tools," in Proceedings of the 31st Conference on Decision and Control, , San Antonio, Texas. [6] R. T. M'Closkey and R. M. Murray, Exeriments in exonential stabilization of a mobile robot towing a trailer," in Proceedings of the American Control Conference, , Baltimore, Maryland. [7] R. T. M'Closkey and R. M. Murray, Exonential stabilization of driftless nonlinear control systems using homogeneous feedback," IEEE Transactions on Automatic Control, Vol. 42, No. 5, , [8] A. R. Teel, R. M. Murray, and G. Walsh, Nonholonomic control systems : From steering to stabilization with sinusoids," in Proceedings of the 31st Conference ondecision and Control, , Tuckson, Arizona. [9] P. Tsiotras and J. Luo, Inariant manifold techniques for control of underactuated mechanical systems," in Proceedings of the Worksho Modeling and Control of Mechanical Systems, Ed: A. Astolfi, P. J. N. Limebeer, C. Melchiorri, A. Tornambe, R. R. Vincer, , London, World Scientific Publishing Co. [1] P. Tsiotras and J. Luo, Stabilization and tracking of underactuated axisymmetric sacecraft with bounded control," in IFAC Symosium on Nonlinear Control Systems Design (NOLCOS'98), Enschede, The Netherlands.. 6