Experimental Comparison of Adaptive Controllers for Trajectory Tracking in Agricultural Robotics

Transcription

1 Experimental Comparison of Adaptive Controllers for Trajectory Tracking in Agricultural Rootics Jarle Dørum, Tryve Utstumo, Jan Tommy Gravdahl Department of Engineering Cyernetics Norwegian University of Science and Technology NO-749 Trondheim, Norway Astract This paper descries the development of several controllers to handle a trajectory tracking prolem for a differentially wheeled root. Both simulations and tests on a real root were performed. A simple kinematic controller has een implemented to calculate desired velocities ased on current position and trajectory. In order to also consider the current velocities, i.e. the dynamics of the system, the output of this controller was used as input to a dynamic controller derived from a nonlinear model. The dynamic controller was made adaptive y using an on-line parameter estimation scheme to estimate the unknown parameters of the nonlinear model. Lastly, a direct model erence adaptive controller (MRAC) ased on a linear model was derived and implemented as an alternative to the adaptive dynamic controller. Index Terms Moile roots, differentially wheeled roots, trajectory tracking, adaptive control, system identification, agricultural rootics, I. INTRODUCTION This paper presents part of the ongoing research for developing an agricultural root that autonomously navigates in row crops while identifying and precision spraying individual weed leaves with hericide. The root is a differentially steered root with two rear mounted caster wheels, and may e modeled as a unicycle-like root. A picture of the prototype during testing in row crops is shown in Fig.. Previous research on the project includes development of a precision drop-on-demand nozzle for hericide application [], a model predictive row controller [] to minimize potential crop damage during operation and attitude estimation in agricultural rootics [3]. The nozzle array presented in [] is intended to only e slightly wider than the row crops, meaning that the root has to follow the row crops precisely. A small offset could mean that the weed is out of reach for the nozzles, leaving the weed untreated. This motivates the research in this paper to find a trajectory tracking controller that minimizes the tracking error. Another aspect to consider is changing physical properties of the root. For example, the weight of the root will change as hericide and fuel is consumed. To ensure satisfactory performance at all times, several adaptive approaches that update the controller gains continuously have een tested. Unicycle-like roots are used extensively in all kinds of fields and numerous models and controllers have een descried in pulications. In this paper a nonlinear model proposed in [4] has een used for simulations. The same Fig.. A picture of the prototype root on a field test. model was also used in [5] to develop an adaptive dynamic controller, which has een implemented and tested here ut with a different adaptation law. In [6] an adaptive controller using adaptive ackstepping is presented. [7] developed a model erence adaptive controller (MRAC) for the tracking prolem, ut only simulations were performed. A similar direct MRAC has een derived here and implemented on the root for testing. The most important contriution of this paper is the comparison of two different adaptive controllers implemented on the same root. The author is not aware of any previous implementations of the MRAC controller presented here on a real root. Different approaches to row crop guidance systems has een thoroughly explored and reviewed in [8]. However, this paper focuses merely on tracking a smooth and well defined trajectory without considering how to otain the trajectory. The results otained should e applicale to most unicyclelike roots. II. MATHEMATICAL MODEL The model used for simulations and some of the controller designs in this paper was presented in [4]. It is given as ẋ u cos ψ aω sin ψ δ x ẏ u sin ψ + aω cos ψ ψ u ω 3 ω 4 u + u δ y + δ u ω 5 uω 6 ω δ ω () where h [ x y ] T is the position, ψ is the heading angle, u is forward velocity, ω is angular velocity and a is the distance

2 III. CONTROLLER DESIGN ω G B a h ψ u d In many cases unicycle-like roots operate at low speeds and often inhiit low moment of inertia. In other words, the dynamics of u and ω are so fast that in many cases one may simplify u u, ω and only study the kinematic model given y ẋ ẏ u cos ψ a sin ψ u sin ψ + a cos ψ (4) ψ For larger roots operating at higher speeds the dynamics cannot simply e ignored. In the next sections, various controller designs are considered. y x Fig.. Drawing of unicycle-like root with centered rear mounted caster wheel, similar to the root all tests were performed on. from center of wheel axis to h as shown in Fig.. Motor inputs are given as velocities instead of torque values, which means that the motor controller is assumed to have a PID controller or similar. is a collection of physical parameters derived in [4] and included here for erence: ( ) Ra /(rk P T ) (mr t r + I e ) + rk DT k ( a Ra (I e d + R t r(i z + m )) + rdk DR k a 3 R a mr t /(k P T ) k a 4 R ( ) a ka k + B e /(rk P T ) + k a R a c ) /(rdk P R ) 5 R a mr t /(dk P R ) k a 6 R ( ) a ka k + B e d/(rk P R ) + k a R a () where R a is motor resistance, k a motor torque multiplied y gear ratio, k motor voltage multiplied y gear ratio, r wheel radius, I e motor moment of inertia, B e motor viscous friction coefficient, k P T, k DT, k P R, k DR are PID motor controller gains, I z moment of inertia aout vertical axis at center of mass, m mass. δ [ δ x δ y δu δω ] T (3) represent the uncertainties of the system caused y wheel slips and forces exerted y the caster wheel. For the purpose of this paper it has een assumed that δ. A. Trajectory Tracking Controller Let h d (t) [ x d (t) y d (t) ] T denote the time varying erence trajectory for the root. Only the erence position is considered, he the The tracking error is defined as h [ x d x y d y ] T. The kinematics from () may e written as ẋ cos ψ a sin ψ [ ẏ sin ψ a cos ψ u (5) ψ Recalling that h [x y] T [ẋ ] [ [ cos ψ a sin ψ u u ḣ A (6) ẏ sin ψ a cos ψ Multiplying (6) with the inverse of A gives [ [ẋ ] [ẋ ] u A cos ψ sin ψ ẏ a sin ψ a cos ψ (7) ẏ A controller ased on inverse kinematics is proposed in [5]: ( ) u c k cos ψ sin ψ + l x tanh x ω c a sin ψ a cos ψ ẋd lx x ( ) ky (8) ẏ d + l y tanh l y ỹ where k x, k y > are controller gains and l x, l y > are saturation constants. u c and ωc are desired forward and angular velocities, respectively. The controller is shown in [5] to have an asymptotically stale equilirium at the origin h [ ] T under the assumption of u u c and ω ω c. The name trajectory tracking controller and kinematic controller oth er to the same controller for the rest of this paper. Note that it is not necessary to explicitly control the desired heading, as discussed in [5]. Due to the non-holonomic nature of a differentially wheeled moile root, the heading will e tangent to the trajectory path given small position errors. Any heading deviation from the tangent will cause a change in position errors, so proving staility for the position will e sufficient. B. Dynamic Controller The kinematic controller will work adequately as long as the dynamics of the system are fast enough, i.e. the assumption of u u c and ω ωc is reasonale. In cases where the dynamics are too slow to e ignored or high precision tracking

3 is required, the kinematic controller alone may no longer e sufficient. reducing () to ν K ν () Consider the dynamic part of () [ u 3 ω [ 4 u ] + u 5 uω 6 ω Rearranging gives [ u ] Which may e written as u u + ω u 3 ω + 4 u ω + 5 uω + 6 ω (9) () ω uω ω () Motivated y the inverse dynamics in (), [5] proposes the controller given as u σ ω + σ uω ω () where σ [ σ σ ] u c + k ũ ω c + k, ω ũ u c u ω ω c ω (3) and k, k > are constant gains. In order to implement (), the values of must e known. Measuring or otherwise otaining the parameters needed to calculate may prove hard, thus the need to estimate ecomes a necessity. Replacing with the estimate ˆ in () gives ν ˆDσ + E ˆ (4) where ω ν, E, uω ω ] (5) [ˆ ˆD, ˆ [ˆ ] T ˆ ˆ3 ˆ4 ˆ5 ˆ6 ˆ Following is a staility analysis similar to what was done in [5]. () may e written as Similarly, (4) is written ν D ν + E (6) ν ˆDσ + E ˆ G G Dσ + E G (7) where ˆ and σ ω G, D (8) σ uω ω (3) may e written as σ σ ν c σ + K ν (9) where K diag(k, k ). Comining (6), (7) and (9) D ν + E D ν c + DK ν + E G () ν K ν + D G () where ν ν c ν descries the error dynamics of the system. For this analysis, is considered known, i.e. ˆ, Consider the following Lyapunov-like function V νt P ν (3) where P P T >. Differentiating (3) along the solution of () gives V ν T P K ν < ν (4) Which means that V is negative definite and gloal asymptotic staility can e concluded. C. On-line Parameter Estimation The dynamic controller given y (4) needs a good estimate ˆ in order to perform well. One approach to estimate ˆ is to log a test run with sufficiently excited input signal and use an off-line system identification technique, e.g. least-squares method. Another approach is to estimate ˆ on-line using an adaptation law ˆ. This section shows the derivation of ˆ using the gradient method, which is motivated y the minimization of a cost function. Consider () written on the form ν ϕ T u ω (5) ω uω ω where ν [u ] T. Filtering oth sides gives ν Λ(s) ϕt Λ(s) su ω Λ(s) sω uω ω (6) Which may e written as the parametric model where z ν Λ(s), ΦT ϕt Λ(s) z Φ T (7) and Λ(s) is chosen to e a Hurwitz polynomial of degree one, e.g. Λ(s) s +. Note that z and Φ are availale measurements, while is unknown. An estimate of z denoted ẑ is generated as ẑ Φ T ˆ (8) where ˆ is the currently est estimate of. A normalized estimation error is defined as ɛ (M T M) (z ẑ) (M T M) (z Φ T ˆ) (9) where M T M I + N s T N s is a diagonal matrix that normalizes the estimation error, and N s T N s is another diagonal matrix for design of the normalized signal. The reason for this normalization is to ensure oundedness, i.e. ΦM L (3) If Φ L, then M I is sufficient. If it is not, choosing M T M I + Φ T Φ (3) will ensure (3) is satisfied [9, p. 7]. An instantaneous cost function J(ˆ) is defined as J(ˆ) ɛt M T Mɛ (z ΦT ˆ) T (M T M) (z Φ T ˆ) (3)

4 Trajectory xd y d Kinematic Controller u c ω c u Reference Model Plant Adaptive Controller um ωm [ u e Kinematics x y ψ Fig. 3. Block diagram of the model erence adaptive controller. The gradient of (3) is J(ˆ) Φ(M T M) (z Φ T ˆ) Φɛ (33) Motivated y this, the following adaptation law for generating ˆ(t) is proposed ˆ Γ J(ˆ) ΓΦɛ (34) where Γ Γ T > is a diagonal gain matrix. According to [9, p. 75], (34) ensures that ) ˆ, ɛ L ) ɛ, N s T ɛ, ˆ L independent of the oundedness properties of Φ. In other words, oth parameters and estimation errors should remain ounded. It does not, however, ensure that (t) (t) ˆ(t) as t. To ensure that the parameters ˆ do in fact converge to their actual value, Φ must e persistently excited (PE), i.e., it satisfies [9, p. 54] α I t+t Φ(τ)Φ T (τ) dτ α I, t (35) T t for some T, α, α. It is in general difficult to show that Φ is PE for an input signal ν, and especially in a case like this where Φ has some nonlinear elements. D. Adaptive Dynamic Controller The results from section III-B and section III-C may e comined to form an adaptive dynamic controller. The on-line parameter estimation operates independently from the dynamic controller and vice versa, making it a modular design. This may prove eneficial in cases where parameter estimation is only needed parts of the time, or if it is desirale to run parameter estimation without running the dynamic controller. The control laws are given y ν ˆDσ + E ˆ, ˆ ΓΦɛ (36) where the notation is the same as in section III-B and section III-C. E. Direct Model Reference Adaptive Controller In this section, a simple direct Model Reference Adaptive Controller (MRAC) scheme as shown in Fig. 3 is derived. The concept is to design a model of similar structure to the plant (root), let the tracking erence e an input to the model, and make the output of the plant track the output of the model. For the direct MRAC approach, this is made possile y developing adaptation laws for the controller gains directly without having to identify actual system parameters. Consider a simplified, linear model of the dynamics of () given y [ u au + u (37) cω + d where a,, c, d are unknown system parameters (not the same as those introduced in Fig. ). In this case, u and ω are considered decoupled and will e analyzed separately. A erence model u m for u is chosen to e u m a m u + m u c (38) Laplace transforming (37) and (38) gives u s a u, u m m u c (39) The following control law is proposed Inserting (4) into (39) gives u k uu + l uu c (4) l u u s a + ku u c, u m m u c (4) It is desirale to make the transfer functions of (4) equal. Choosing lu m, k u a + a m (4) ensures equal transfer functions. However, it is not possile to implement since the values of a and are unknown. Instead of using the control law (4), a control law using estimates of ku and lu is proposed u k u (t)u + l u (t)u c (43) where k u (t) and l u (t) are the currently est estimates of k u and l u, respectively. Adding and sutracting ( k uu+l uu c ) to u yields u au + u + ( k uu + l ur) ( k uu + l ur) (44) which, after comining with (4), may e written as u a m u + m u c u + (k uu l uu c + u ) (45) Laplace transforming (45) gives u m u c } {{ } u m + (k uu l uu c + u ) (46)

5 Define the tracking error e u u u m to otain e u (k s + a uu luu c + u ) (47) m Since ku and lu are unknown, our est estimate of the tracking error ê u is ê u (k u (t)u l u (t)u c + u ) (48) Inserting u from (43) into (48) simply gives ê u, i.e. the estimated tracking error is zero. Note that the estimation error ɛ u e u ê u e u u u m is equal to the tracking error e u. Comining (43) and (47) while defining the gain parameter estimation errors k u (t) k u (t) ku, l u (t) l u (t) lu gives e u ( k u u + s + a l u u c ) (49) m ė u a m e u + ( k u u + l u u c ) (5) Consider the Lyapunov-like function V e u + k γ u + l γ u (5) with γ, γ >. Differentiating (5) along the solution of (49) gives V a m e u + k u ( e u u sgn() + ) k γ + l u (e u u c sgn() + ) (5) l γ Choosing k u γ e u u sgn(), lu γ e u u c sgn() (53) ensures V a m e u (54) Thus it is shown that V is negative semi definite, V has an upper ound V () and ounded elow y zero, i.e. V (t) V (). From the oundedness of V (t) and (5), it is clear that e u, k u, l u L. u c, the output of the kinematic controller (8), is ounded, so u c L. The transfer functions of (4) are in L, and it follows from [9, p. 8] that u, u m L. This means that all signals of (49) are ounded and ė u L. From [9, p. 74] it follows that since V (t) is ounded from elow and non-increasing, it has a finite limit as t, denoted V. It can also e seen that ( / ( e u eu(τ)dτ) a m V (τ)dτ ) / ( ) / (V () V ) a m (55) which is clearly ounded, so that e u L. Finally, [9, p. 8] shows that since ė u, e u L and e u L, then e u (t) as t. The results otained show that the tracking ojective of making the output of the plant u(t) track the output of the erence model u m (t) is achieved. It does not, however, guarantee that k u (t), l u (t) k u, l u as t, i.e. the poles of the plant may differ from those of the erence model. This should e of less concern, since k u (t), l u (t) are ounded and the true values of k u, l u are not of any real importance. In a very similar manner the same results are found for ω. A summary of the control laws are given in Tale I. A modification that was done to provide for a more roust implementation was to add a small feedack loop to (53) to get k u γ e u u sgn() αk u, lu γ e u u c sgn() βl u (56) where < α << and < β <<. TABLE I. u ω Control Laws for The MRAC Plant Reference Model Control Law s a u u m m s+a m u c d s c ω m dm s+c m ω c u k u(t)u + l u(t)u c, k u γ e uu sgn() l u γ e uu c sgn() e u u u m k ω(t)ω + l ω(t)ω c, k ω γ 3 e ωω sgn(d) l ω γ 4 e ωω c sgn(d) e ω ω ω m IV. SIMULATIONS AND REAL RUNS Simulations were performed using Matla/Simulink, while real runs were performed on the root shown in Fig. 4. The dimensions of the root are approximately c. m and d. m using the notation from Fig.. The root is running Root Operating System (ROS) and all controllers were implemented in C++. The motor controller has a low level PID controller that uses individual motor velocities as setpoints, and motor acceleration can e saturated to ensure slower dynamics. Without limits on acceleration the dynamics were so fast that all controllers had equal performance. For oth simulations and tests on the real root the following figure eight trajectory was used: x d (t) r e sin(ω e t) (57) y d (t) r e (cos(ω e t) ) For oth simulations and real runs r e.6 m and ω e.3 rad/s. The distance from wheel axle to h was chosen to e a. m. The simulated system uses T and initial estimates ˆ T.... are used for the dynamic and adaptive dynamic controllers. The on-line parameter estimation method was tested in simulations where is known to ensure that the estimated ˆ converges to its actual value. It was found that ˆ does indeed converge correctly while attempting to track the figure eight, which means that the input signal is sufficiently excited to ensure convergence. A comparison of all controllers is shown in Fig. 8. The estimated parameters during a real run are shown in Fig. 5 and Fig. 6. Controller inputs during a real run are shown in Fig. 7. It is clear from Fig. 8 (a) and (e) that the kinematic controller alone does not provide sufficient performance in this case. The dynamic controller shows good performance given

6 Fig. 4. A picture of the root used for the tests. The root uses a rear caster wheel. ku lu kω lω Fig. 6. Estimated system parameters during a real run with the adaptive dynamic controller with [ ]T and Γ diag(.,.,.,.,.,.). uc, c, ω uc, c, ω adap adap MRAC MRAC Fig. 5. Controller gains for the MRAC during a real run. All controller gains have initial values of.5. fairly accurate estimates. The adaptive dynamic controller appears to e ale to improve upon the performance of the dynamic controller as adapts (shown in Fig. 6). In Fig. 8 (i) and (j) the motor acceleration saturation limit was increased to make the system a it faster. It is interesting that the MRAC improves greatly when the dynamics are faster, while the adaptive dynamic controller has almost identical performance to the case with slower dynamics. V. C ONCLUSION Two different adaptive dynamic controllers for tracking a trajectory were implemented on a differentially wheeled root and compared with non-adaptive kinematic and dynamic controllers. The MRAC configuration, which the author has een unale to find previous papers presenting real implementations of, delivered the est performance of all controllers on a system with fairly slow dynamics, while the other adaptive dynamic controller had equal or etter performance on a very slow system. R EFERENCES [] F. Urdal, T. Utstumo, S. A. Ellingsen, J. K. Vatne, and T. Gravdahl, Design and control of precision drop-on-demand hericide application in agricultural rootics, in The 3th international Conference on Control, Automation, Rootics and Vision, Singapore. IEEE, 4. [] T. Utstumo, T. Berge, and J. T. Gravdahl, Non-linear model predictive control for constrained root navigation in row crops, in the Proceedings of the 5 IEEE International Conference on Industrial Technology (ICIT 5), Seville, Spain, March Fig. 7. Motor controller inputs from the MRAC and the adaptive dynamic controller during a test run with the slow dynamics. The inputs ecome smoother as the parameters adapt. [3] T. Utstumo and J. T. Gravdahl, Implementation and comparison of attitude estimation methods for agricultural rootics, Agricontrol. Vol. 4. No.. 3, pp. 5 57, 3. [4] C. De La Cruz and R. Carelli, Dynamic Modeling and Centralized Formation Control of Moile Roots, IECON 6 3nd Annual Conference on IEEE Industrial Electronics, pp , Nov. 6. [Online]. Availale: [5] F. N. Martins, W. C. Celeste, R. Carelli, M. Sarcinelli-Filho, and T. F. Bastos-Filho, An adaptive dynamic controller for autonomous moile root trajectory tracking, Control Engineering Practice, vol. 6, no., pp , Nov. 8. [Online]. Availale: [6] T. Fukao, H. Nakagawa, and N. Adachi, Adaptive tracking control of a nonholonomic moile root, Rootics and Automation, IEEE Transactions on, vol. 6, no. 5, pp , Oct. [7] E. Canigur and M. Ozkan, Model erence adaptive control of a nonholonomic wheeled moile root for trajectory tracking, in Innovations in Intelligent Systems and Applications (INISTA), International Symposium on, July, pp. 5. [8] F. Dong, W. Heinemann, and R. Kasper, Development of a row guidance system for an autonomous root for white asparagus harvesting, Computers and Electronics in Agriculture, vol. 79, no., pp. 6 5,. [Online]. Availale: [9] P. Ioannou and J. Sun, Roust Adaptive Control. Dover Pulications, 3. [Online]. Availale:

7 sim (a) Simulation with kinematic controller. () Simulation with dynamic controller real.5 (c) Simulation with adaptive dynamic controller...5 (d) Simulation with MRAC (e) Real run with kinematic controller. (f) Real run with dynamic controller (g) Real run with adaptive dynamic controller. (h) Real run with MRAC (i) Real run with adaptive dynamic controller on a faster system (j) Real run with MRAC on a faster system Fig. 8. Comparison of all the controllers with a relatively slow system and two selected runs on a faster system.