- Email: [email protected]
Robust digital control for autonomous skid-steered agricultural robots
Computers and Electronics in Agriculture 153 (2018) 94–101
Contents lists available at ScienceDirect
Computers and Electronics in Agriculture journal homepage: www.elsevier.com/locate/compag
Original papers
Robust digital control for autonomous skid-steered agricultural robots Benjamin Fernandez a b
a,b,⁎
b
, Pedro Javier Herrera , Jose Antonio Cerrada
T
b
Research and Advanced Engineering, AGCO GmbH, Johann-Georg-Fendt-Str. 4, 87616 Marktoberdorf, Germany Depto. de Ingeniería de Software y Sistemas Informáticos, ETSI Informática, UNED, C/ Juan del Rosal, 16, 28040 Madrid, Spain
A R T I C LE I N FO
A B S T R A C T
Keywords: Skid-steered robot Robust digital control Guidance control RST-controller Digital pole placement Autonomous off-road vehicles Agricultural robotics
There are two main issues to consider when designing a controller for autonomous off-road vehicles: velocity and terrain irregularities. Whereas the first one is measurable, the second one is very difficult to determine. Solutions to cover these issues could be very complex and difficult to implement in an embedded system with limited resources. The results obtained in our previous research for an adaptive approach implemented in a tractor with varying hitch forces, lead to the improvements presented here. This paper proposes a robust digital RST pole placement controller design for the lateral position, with sensitivity functions tuned to cover uncertainties and non-linearities not considered in the model. Simulations were implemented to assess the performance of the system and the controller implementation was applied to a skid-steered agricultural robot with limited computational resources and a state-of-the-art navigation system, which delivered satisfactory results.
1. Introduction Skid-steered robots will play an important roll in agricultural robotics due to their flexibility and simple construction. They are suitable for applications such as seeding (Blender et al., 2016), scouting, fertilizing and weed control. Nevertheless, there are some challenges to be covered related not only to the steering system, but also to the interaction with the soil. There are intrinsic non-linearities related to the steering system of such vehicles that make the design of a controller a very complex task. Furthermore, the dynamics of the system are constantly changing due to irregularities on the terrain. There are different approaches for the representation of such systems (Caldwell and Murphey, 2011; Martínez et al., 2005; Wang et al., 2015; Maclaurin, 2011; Guo and Peng, 2013; Yi et al., 2009; Al-Milli et al., 2010; Xueyuan et al., 2013), where different non-linear controllers are applied (Caracciolo et al., 1999; Tchoń et al., 2015; Jun et al., 2014; Pazderskit et al., 2004; Yi et al., 2007; Pazderski and Kozlowski, 2008), some of them being even robust solutions (Arslan and Temeltas, 2011; Inoue et al., 2013). There are also general linear solutions in the literature (Derrick and Bevly, 2008; Derrick and Bevly, 2009; Derrick et al., 2008; Gartley and Bevly, 2008) for controlling the lateral position of off-road vehicles by adapting the controller of the yaw rate with the use of a Model Reference Adaptive System(MRAS). In contrast to the MRAS solution, which requires more effort and filtering, our previous work implements a self-tuning regulator since the error is directly
obtained from a least-square identification and a general 2nd-order model can be used in the closed loop. This allows the solution to be applied to different steering systems in a general way. (Fernandez et al., 2018). Nevertheless, this solution still needs a traditional PID to be tuned for the lateral position, which is a time consuming task. Besides, since the identification of this solution requires a rich set of measurements, it could not be ideal for small vehicles with a higher frequency response such a small skid-steered robot. Therefore, the motivation of this research is to find a linear and practical approach for agricultural skid-steered robots to control the lateral position, easy to be programmed into a digital real-time embedded system with limited resources and that also guarantees robustness against uncertainties due to changes in the dynamics of the system. This paper is divided in four sections. The first part of Section 2 presents an RST pole placement design with tracking and regulation. In the second part (Section 2.2), static parameters used in the controller design are introduced for shaping the sensitivity functions of the closed loop. An experimental setup of a skid-steered robot with its results are presented in Section 3. Finally, the conclusions can be found in Section 4. 2. Material and methods Generally speaking, the uncertainties in the dynamics of an off-road vehicle are related to the changes in speed and soil irregularities. A
⁎
Corresponding author at: Research and Advanced Engineering, AGCO GmbH, Johann-Georg-Fendt-Str. 4, 87616 Marktoberdorf, Germany. E-mail addresses: [email protected], [email protected] (B. Fernandez), [email protected] (P.J. Herrera), [email protected] (J.A. Cerrada). https://doi.org/10.1016/j.compag.2018.07.038 Received 30 May 2018; Received in revised form 24 July 2018; Accepted 28 July 2018 0168-1699/ © 2018 Elsevier B.V. All rights reserved.
Computers and Electronics in Agriculture 153 (2018) 94–101
B. Fernandez et al.
Bm Am
r(t)
y ∗ (t + d + 1)
T
u(t)
1 S
−
z −d B A
y(t)
Fig. 1. Digital canonical controller for tracking and regulation.
R
and
solution for that is to design a system that considers these uncertainties due to nonlinearities and time varying elements. Here we present a methodology for the design and implementation of a robust digital RST controller using pole placement. There are automatic methods for finding the parameters and shaping the sensitivity functions using convex optimization or H∞ optimization (Langer and Landau, 1999; Langer and Constantinescu, 1999; Landau and Karimi, 1998). Nevertheless, this section presents a method to control the lateral position of a skid-steered robot by manually shaping the sensitivity functions (Landau and Zito, 2006) due to its simplicity compared to the automatic methods. This also allows for more flexibility in the design and a deep understanding of how the system reacts to changes in the controller design.
⎡ 1 0 … 0 . ⎢ a1 1 ⎢a 0 2 ⎢ 1 ⎢ M=⎢ a1 ⎢ a2 ⎢ anA ⎢ 0 . ⎢ 0 … 0 a nA ⎣
nA = degA (z −1) nB = degB (z −1) nS = degS (z −1) = nB + d−1 nR = degR (z −1) = nA−1 nP = degP (z −1) ⩽ nA + nB + d−1.
A canonical digital controller called RST is shown in Fig. 1. This structure allows one to impose different dynamics by obtaining the polynomials R and S in order to satisfy the desired regulation performance. T introduces a tracking performance that filters at the same time a desired trajectory y∗ (t + d + 1) from a tracking model system Bm . −d
The process to be controlled is represented by z B and the closed A loop function from the desired trajectory y∗ to the output y is represented in Eq. (1):
z −dB (z −1) T (z −1) + z −dB (z −1) R (z −1)
(1)
where d is the time delay and
T (z −1) = G∗P (z −1)
A (z −1)
where
= 1 + a1
z −1+…+a
nA
z −nA
B (z −1) = b1 z −1 + b2 z −2+…+bnB z −nB
1/ B (1) if B (1) ≠ 0 G=⎧ if B (1) = 0. ⎨ ⎩ 1
S (z −1) = 1 + s1 z −1+…+snS z −nS R (z −1) = r0 + r1 z −1+…+rnR z −nR.
(2)
S (z −1) u (t ) + R (z −1) y (t ) = T (z −1) y∗ (t + d + 1).
P (z −1) = PD (z −1) PF (z −1) = A (z −1) S (z −1) + z −dB (z −1) R (z −1).
2.2. Robust pole placement design
(3)
The characteristic polynomial P contains dominant and auxiliary poles. The dominant poles PD are chosen from the digitalization of a second-order system defined by ω0 and ζ . The digital auxiliary poles PF improve the robustness of the controller and are normally smaller (faster) than the real part of the dominant poles. Typical values for the auxiliary poles are −0.05 ⩽ α1 ⩽ −0.5 and α2 either equals 0 or α2 = α1. By defining the characteristic equation as follows: (4)
(5)
where
= [1, p1 , …, pnP , 0, …, 0]
(11)
Syp (z −1) =
A (z −1) S (z −1) A (z −1) S (z −1) + B (z −1) R (z −1)
Sup (z −1) =
−A (z −1) R (z −1) A (z −1) S (z −1) + B (z −1) R (z −1)
Syb (z −1) =
−B (z −1) R (z −1) A (z −1) S (z −1) + B (z −1) R (z −1)
Syv (z −1) =
B (z −1) S (z −1) . A (z −1) S (z −1) + B (z −1) R (z −1)
(12)
As we can see, the common denominator happens to be the characteristic equation of the closed loop system of Eq. (1). The noise sensitivity function Syb with a negative sign is also known in the literature as
xT = [1, s1, …, snS , r0, …, rnR] pT
(10)
In order to consider uncertainties in our control design, the structure of Fig. 1 can be extended into Fig. 2 where p (t ) are disturbances, b (t ) is noise and v (t ) disturbances on the plant input. We can obtain then the following output, input, noise, and disturbance sensitivity functions respectively:
we obtain R and S by solving
x = M−1p
(9)
Having R, S, T and a desired trajectory y∗ we obtain a control law of the form:
2.1.1. Regulation To compute the coefficients R and S of the digital controller we solve a Bezout polynomial equation of the form:
P (z −1) = 1 + p1 q−1 + …+pnP q−nP
(8)
2.1.2. Tracking B The reference model Hm = Am can be used to have an output y that m ∗ follows a desired trajectory y each time a reference r is changed. This tracking model Hm can take the form of a second order system with desired ω0 and ζ . This leads to choose T (z −1) to have a unit static gain between y∗ and y and to compensate between regulation dynamics defined by P (z −1) and tracking dynamics defined by the poles of the reference model (Am):
Am
A (z −1) S (z −1)
(7)
Here bi′ = 0 for i = 0, 1, …, d and bi′ = bi − d for i ⩾ d + 1 and:
2.1. Pole placement: tracking and regulation
HCL (z −1) =
0 … … 0 ⎤ b1′ ⎥ b2′ b1′ ⎥ ⎥ . b2′ ⎥ . . . ⎥ ⎥ bn′B . ⎥ 0 . . . ⎥ 0 0 0 bn′B ⎥ ⎦
(6) 95
Computers and Electronics in Agriculture 153 (2018) 94–101
B. Fernandez et al.
y ∗ (t + d + 1) r(t)
Bm Am
T
p(t)
v(t) −
1 S
+
u(t)
B A
+
y(t)
+
R
b(t)
Fig. 2. Digital canonical controller with disturbances and noise.
Syp (z −1) = 1 + Syb (z −1) and with the use of triangle inequality impose an upper and lower delay margin defined by the following equation:
the complementary sensitivity function and from Eq. (12) we see that Syp−Syb = 1. Furthermore, the robustness of the closed loop system can be evaluated by the distance of the critical point [−1, j 0] to the Nyquist plot of the nominal plant and by the frequency characteristics of the sensitivity functions (Eq. (12)). The elements that help to evaluate this robustness are the following four margins: gain, phase, delay and modulus margin. Especially the modulus margin is very important because it defines the maximum admissible value of the output sensitivity function Syp and defines the tolerance with respect to nonlinear or time varying elements that may belong to the system. A good modulus margin also guarantees good values for the gain and phase margins. The modulus margin ΔM is the inverse of the maximum of the output sensitivity function Syp . Typical values for a good modulus margin are ΔM ⩾ 0.5(−6 dB) . Therefore, we use these margins to shape the frequency characteristics of the sensitivity functions Syp and Sup and guarantee both robustness and closed loop performance. To be able to shape the sensitivity functions using pole placement, we have to introduce fixed parts to the Bezout polynomial equation (Eq. (3)) as follows:
P (z −1) = A (z −1) HS (z −1) S′ (z −1) + z −dB (z −1) HR (z −1) R′ (z −1).
1−|Syb (z −1)| ⩽ |Syp (z −1)| ⩽ 1 + |Syb (z −1)|. By substituting Eq. (16) into (18) we obtain Eq. (19).
1−|1−z −1|−1 ⩽ |Syp (z −1)| ⩽ 1 + |1−z −1|−1 .
3. Experimental setup and results Fig. 3 shows a representation of a skid-steered robot where y is the lateral position to a desired path and is the variable to be controlled. The yaw rate of the robot (ω ) and its linear velocity (V) are produced by the difference between the velocities of the right and left axis (VR and VL respectively), where β represents the angle of the velocity vector to the center of gravity. The angular velocities of the right and left wheels are represented by ωR and ωL respectively and the distance between axes is 2c . The angle with respect to the desired path is represented by ν and r is the wheel radius.
(13)
3.1. Robot model (14)
From the kinematic point of view, we can define the yaw rate at the center of gravity and the linear velocity by the following equations:
2.2.1. Shaping the input sensitivity function Sup The input sensitivity function reflects the actuator stress, therefore, we should limit its magnitude at high frequencies. By plotting the inverse of its magnitude |Sup (e−jω)|, we check and impose and upper bound maximum by imposing the prefixed value HR (z −1) shown in Eq. (15), which e.g. makes |Sup | = 0 at 0.5fs by choosing β = 1, where fs is the sampling frequency.
HR (z −1) = 1 + βz −1;
0 < β ⩽ 1.
V = r∗
ωL + ωR 2
(20)
ω = r∗
−ωL + ωR . 2c
(21)
(15)
2.2.2. Shaping the noise sensitivity function Syb The noise sensitivity function is related to the delay margin Δτ and considering Δτ = Ts we impose a frequency template of a digital integrator of the form: −1 −1 |Syb (z )|dB < −20log|1−z −1|; z = e jw;
0 ⩽ ω ⩽ π.
(16)
2.2.3. Shaping the output sensitivity function Syp We already mentioned a typical value for a good modulus margin (ΔM ⩾ 0.5(−6 dB) ). Imposing that to the output sensitivity function we have that
|Syp (z −1)| ⩽ −ΔM ; z = e jw;
0 ⩽ ω ⩽ π.
(19)
Again, in order to find a closed loop function that fits inside of the upper and lower bounds of the template defined by Eqs. (17) and (18), we can pick specified parts for HS . For example making HS = 1−z −1 we introduce a zero at the zero frequency allowing perfect rejection of constant disturbances.
The solution to Eq. (13) is found in the same way Eq. (3) is solved with Eq. (5), after replacing A (z −1) by A′ (z −1) = A (z −1) HS (z −1) and B (z −1) by B′ (z −1) = B (z −1) HR (z −1) with the following new conditions:
nP = degP (z −1) ⩽ nA + n HS + nB + n HR + d−1 nS′ = degS′ (z −1) = nB + n HR + d−1 nR′ = degR′ (z −1) = nA + n HS−1.
(18)
(17) Fig. 3. Lateral position.
Furthermore, we can use the complementary function defined by 96
Computers and Electronics in Agriculture 153 (2018) 94–101
B. Fernandez et al.
By rewriting Eq. (21) into right and left velocities we obtain Eq. (22). And by defining a time delay τ we find a first oder yaw-rate transfer function shown in Eq. (23).
V −V ΔV . ω= R L = 2c 2c
Gω (s ) =
ω (s ) 1/2c = . ΔV (s ) τ·s + 1
Table 1 Technical data of a skid-steered robot. Robot data
Ts τ 2c V
(22)
(23)
Finally using the z-transform we obtain the following digital transfer function:
Gω (z −1) =
br 1 z −1 1 + ar 1 z −1
d=0 B (z −1) = 0.003473z −2 + 0.003473z −3 A (z −1) = 1−2.368z −1 + 1.736z −2−0.3679z −3.
(24)
br 1 = (1/2c ) ∗ (1−e−Ts / τ ) (25)
Generally speaking, the lateral position y with respect to a desired path is related to the yaw rate of the vehicle in the form of Eq. (26), where β is the side slip angle and ν the course angle with respect to the desired path (Fig. 3) (Gartley and Bevly, 2008).
y ̇ = V sin (ν ) ν ̇ = ω + β.̇
y (s ) V = 2. ω (s ) s
(26)
Gy
bl0 + bl1 z −1 = , 1 + al1 z −1 + al2 z −2
(27)
b2 z −2 + b3 z −3 , 1 + a1 z −1 + a2 z −2 + a3 z −3
(28)
R (z −1) = 1−0.4784z −1 + 0.04941z −2−0.005427z −3−0.01235z −4 S (z −1) = 8.788−6.796z −1−7.374z −2 + 6.903z −3−1.308z −4 T (z −1) = 144−409.7z −1 + 424.4z −2 + 189.1z −3−30.67z −4.
(36)
(37)
3.3. Results (29) With the results of Section 3.2 we can prove the robustness of the design for the different sensitivity functions. Fig. 4 shows the Nyquist −1 −1 in green. Since the critical plot of the open loop function Hol = Syp point [−1, j 0] is at the left of the plot, we conclude that the open loop system has no unstable poles. Here, the inverse of the gain margin 1/ΔG and the phase margin ΔΦ are stability margins with respect to the critical point. It can be seen that the modulus margin ΔM is the radius of a circle centered at the critical point and tangent to the Nyquist plot. It tells us the distance between Hol and the critical point. Therefore, a good modulus margin (e.g. ΔM ⩾ 0.5) implies stable gain and phase margins. Fig. 5 shows in blue the applied modulus and delay margins to the output sensitivity function Syp . The upper-left bound is built by a modulus margin ΔM = 0.5(−6 dB) (Eq. (17)) and the upper-right and lower bounds represent the limits of the delay margin (Eq. (19)) which is more restrictive at higher frequencies than the modulus margin. The output sensitivity function in green is the result of imposing the fixed part HS (z −1) to the pole placement design (Eq. (33) (Landau and Zito, 2006)). It can be seen that using different values for the fixed part HS will shape the output sensitivity function differently. It can also be seen that, although using HS = 1−1z −1 (dashed green line) will reject constant disturbances in steady state, the shaped Syp is closer to the upper margin. This will make the system less tolerant to changes on the system dynamics at lower frequencies and to fix that we should impose
(30)
where
b2 = br 1 ∗bl0 b3 = br 1 ∗bl1 a1 = al1 + ar 1 a2 = 1 + al1 ∗ar 1 a3 = ar 1.
(35)
Finally, using these results to solve Eqs. (5) and (9) we find the following control parameters R, S and T:
Multiplying Eqs. (24) and (28) we obtain the digital model of the lateral position with respect to a desired path of a skid-steered robot to be used for our RST controller design (Eq. (30)).
A
(34)
Bm (z −1) = 0.01752z −1 + 0.01534z −2 Am (z −1) = 1−1.637z −1 + 0.6703z −2.
bl0 = V ∗Ts2/2
GB =
B′ (z −1) = 0.003473z −2 + 0.006946z −3 + 0.003473z −4 A′ (z −1) = 1−2.868z −1 + 2.92z −2−1.236z −3 + 0.1839z −4.
For the tracking dynamics we use Ts = 0.1; ω0 = 2.0; and ζ = 1.0 to find the following parameters of the reference model Hm :
where
bl1 = V ∗Ts2/2 al1 = −2 al2 = 1.
(33)
P (z −1) = PD·PF = (1−1.846z −1 + 0.8521z −2)·(1−0.5z −1)2.
Again, using the z-transform we obtain the following digital transfer function of the lateral position with respect to the yaw rate:
(z −1)
HR (z −1) = 1 + 1.0z −1 HS (z −1) = 1−0.5z −1.
For the regulation dynamics we use Ts = 0.1, ω0 = 0.8, ζ = 1.0 and α1 = α2 = −0.5 and so the left part of the Bezout equation (13) appears as follows:
Neglecting side slip, linearizing and using the small angle approximation we find the transfer function of the lateral position with respect to the yaw rate in Eq. (27).
Gy (s ) =
(32)
To shape the sensitivity functions we choose the fixed parts represented in Eq. (33) and the new poles and zeros to be used in the right part of the Bezout equation (13) are expressed in Eq. (34).
where
ar 1 = −e−Ts / τ .
0.1 [ms] 0.1 0.455 [m] 0.5 [m/s]
(31)
3.2. Controller design For the controller design, the robot will be driving at a constant speed of 0.5 [m/s] and the sampling time of the digital system is 100 [ms]. The track-width of 0.455 [m] corresponds to the real robot described in Section 3.3.1, with a time constant of 100 [ms] (see Table 1). Substituting the data from Table 1 into Eqs. (25), (29) and (31) we get the following plant model parameters: 97
Computers and Electronics in Agriculture 153 (2018) 94–101
B. Fernandez et al.
Fig. 4. Nyquist plot.
the solid green line arrives faster at the steady state (around 4 [s]) whereas the dashed one needs more than 8 [s] and contains oscillations. As already mentioned, the oscillations can be fixed by picking slower regulation dynamics (ω0 ⩽ 0.8) or by driving faster. Nevertheless, we could reduce the error at the turning points by reducing the speed and to this end, it is desirable to have the system react faster to changes. A 3D real-time simulation using the results of Eqs. (32), (36) and (37) is shown in Fig. 9. It shows representative results which are also useful to prove the robustness of the behaviour in an interactive environment (Fernandez et al., 2016). For the simulation, a real path with geographic coordinates was used so we could compare these results with the results of a real robot using the same path. The simulation shows very satisfactory results where the biggest error (at the fifth turning point) was around 20 [cm] which is comparable to other more complex solutions (Hwang et al., 2013; Arslan and Temeltas, 2011; Inoue et al., 2013; Tchoń et al., 2015; Caracciolo et al., 1999; Jeon and Jeong, 2015). These errors at the turning points are expected since in order to be able to turn with zero curvature radius, the skid-steered robot should stop completely. In contrast, in the simulation the robot was reducing its velocity to 0.1 [m/s] to test its robustness.
slower regulation dynamics (e.g. ω0 < 0.8). On the other hand, using Hs = 1−0.5z −1 (solid green line) will decrease the gain, making the system more tolerant to uncertainties with faster regulation dynamics. Fig. 6 shows in green the noise sensitivity function Syb and in blue the bound related to the delay margin Δτ = Ts (Eq. (16)). The delay margin was already set for the margins of the output sensitivity function (Syp ) with good results (Fig. 5). Nevertheless, the delay margin is related to the noise sensitivity function and the fact that Syb is under the bound confirms the good choice of the fixed parts (Eq. (33)). Fig. 7 shows in green the input sensitivity function (Sup ) for a pole placement with the imposed fixed part HR = 1 + 1z −1 (Eq. (33)) which will impose a zero at 0.5fs reducing the actuator stress at higher frequencies. This has the effect of limiting not only the actuator stress but also the reaction to measurement noise and disturbances making the robot drive more smoothly and not react right away after every stone or irregularity in the soil. It will also reduce the energy consumed by the motors and increase their lifetime. The resulting step responses of the different shaping of the output sensitivity function is shown in Fig. 8. The dashed green line corresponds to the response using the fixed part HS = 1−1.0z −1, whereas the solid green one corresponds to the use of HS = 1−0.5z −1. It can be seen that
Fig. 5. Output sensitivity function. 98
Computers and Electronics in Agriculture 153 (2018) 94–101
B. Fernandez et al.
Fig. 6. Noise sensitivity function.
4. Conclusions
3.3.1. Real validation For the validation, this method was programmed in the Electronic Control Unit (ECU) of a 40kg-weight agricultural skid-steered robot (Fig. 10). Each wheel is equipped with an electric drive, able to reach a maximum speed of 1 [m/s], and hall sensors to measure the angle and angular velocity of the motors. For the position, a state-of-the-art GNSS receiver with RTK-correction is used. The ECU with a 400 [MHz] processor, includes an Inertial Measurement Unit (IMU), WLAN module and CAN interfaces for different internal communications. The same coordinates used for the simulation (Fig. 9) were sent to the real robot and the results are shown in Fig. 11. It can be seen that the robot was successfully followed the path with a similar type of error in the turning points as shown in the simulation with a maximum of 18 [cm] also at the fifth turning point. In contrast to the simulation, disturbances due to terrain irregularities can be appreciated in the real approach. Nevertheless, the controller was able to react very smooth to disturbances without producing an error of more than 2 [cm] once on the lane. The same path was tested in different terrains and the skid steered robot produced similar results.
This paper presented a robust digital method to control the lateral position of a skid-steered agricultural robot. The method is based on a digital RST pole placement and introduces fixed parts to the Bezout polynomial in order to shape the different sensitivity functions to guarantee robustness against uncertainties due to noise and disturbances, as well as non-linearities not considered in the model. The results presented showed the robustness of the controller and its ability to react nicely against terrain irregularities and disturbances. It was also possible to implement this linear robust digital solution into an ECU with limited resources and obtain similar results to more complex, even non-linear approaches. As future work, this method could be implemented for a MIMO state-space system to be able to include heading and speed in the same controller. Acknowledgment This work has been developed with the help of the research project DPI2016-77677-P of the MICINN. It also belongs to the activities
Fig. 7. Input sensitivity function. 99
Computers and Electronics in Agriculture 153 (2018) 94–101
B. Fernandez et al.
Fig. 8. Step response over 1 meters cross track.
Fig. 11. Lane-tracking control on the field. Fig. 9. 3D real time lane-tracking simulation with openframeworks.
References Al-Milli, S., Seneviratne, L.D., Althoefer, K., 2010. Track-terrain modelling and traversability prediction for tracked vehicles on soft terrain. J. Terrramech. 47, 151–160. https://doi.org/10.1016/j.jterra.2010.02.001. Arslan, S., Temeltas, H., 2011. Robust motion control of a four wheel drive skid-steered mobile robot. In: 2011 7th International Conference on Electrical and Electronics Engineering (ELECO), pp. II-415–II-419. Blender, T., Buchner, T., Fernandez, B., Pichlmaier, B., Schlegel, C., 2016. Managing a Mobile Agricultural Robot Swarm for a seeding task. In: IECON 2016 - 42nd Annual Conference of the IEEE Industrial Electronics Society. IEEE, pp. 6879–6886. https:// doi.org/10.1109/IECON.2016.7793638. URL.. Derrick, J., Bevly, D., 2008. Adaptive control of a farm tractor with varying yaw dynamics accounting for actuator dynamics and saturations. In: IEEE International Conference on Control Applications, 2008. CCA 2008, pp. 547–552. doi:https://doi.org/10. 1109/CCA.2008.4629591. Derrick, J., Bevly, D., Rekow, A., 2008. Model-reference adaptive steering control of a farm tractor with varying hitch forces. In: American Control Conference, 2008, pp. 3677–3682. doi:https://doi.org/10.1109/ACC.2008.4587065. Derrick, J.B., Bevly, D.M., 2009. Adaptive steering control of a farm tractor with varying yaw rate properties. J. Field Robotics 26, 519–536. https://doi.org/10.1002/rob. 20291. Fernandez, B., Cerrada, J., Gross, J., 2016. Tractor-implement real time interactive 3d simulation based on openframeworks and opengl. In: International Conference on Machine Control & Guidance, 2016. URL: .
Fig. 10. Skid-steered robot for corn seeding provided by AGCO GmbH | Fendt.
carried out within the framework of the research network CAM RoboCity2030 S2013/MIT-2748 of the Comunidad de Madrid. The authors would like to thank AGCO for the provision of the robot and the technical support of this research.
Appendix A. Supplementary material Supplementary data associated with this article can be found, in the online version, at https://doi.org/10.1016/j.compag.2018.07.038.
100
Computers and Electronics in Agriculture 153 (2018) 94–101
B. Fernandez et al.
method using convex optimization criteria. Automatica 35, 1111–1120. https://doi. org/10.1016/S0005-1098(99)00013-8.. Maclaurin, B., 2011. A skid steering model using the Magic Formula. J. Terrramech. 48, 247–263. https://doi.org/10.1016/j.jterra.2011.04.002. Martínez, J.L., Mandow, A., Morales, J., Pedraza, S., García-Cerezo, A., 2005. Approximating kinematics for tracked mobile robots. Int. J. Robot. Res. 24, 867–878. https://doi.org/10.1177/0278364905058239. Pazderski, D., Kozlowski, K.R., 2008. Trajectory tracking control of skid-steering robot experimental validation. IFAC Proc. Volumes (IFAC-PapersOnline) 17, 5377–5382. https://doi.org/10.3182/20080706-5-KR-1001.3252. Pazderskit, D., Kozlowski, K., Dixon, W.E., 2004. Tracking and regulation control of a skid steering vehicle. In: American Nuclear Society Tenth International Topical Meeting on Robotics and Remote Systems, pp. 369–376. URL: . Tchoń, K., Zadarnowska, K., Juszkiewicz, Arent, K., 2015. Modeling and control of a skidsteering mobile platform with coupled side wheels. Bull. Pol. Acad. Sci.: Tech. Sci. 63, 807–818. https://doi.org/10.1515/bpasts-2015-0092. Wang, T., Wu, Y., Liang, J., Han, C., Chen, J., Zhao, Q., 2015. Analysis and experimental kinematics of a skid-steering wheeled robot based on a laser scanner sensor. Sens. (Switz.) 15, 9681–9702. https://doi.org/10.3390/s150509681. Xueyuan, L., Yu, Z., Jibin, H., Chongbo, J., Jing, G., 2013. Lateral dynamic simulation of skid steered wheeled vehicle. In: 2013 IEEE Intelligent Vehicles Symposium (IV). IEEE, pp. 1095–1100. https://doi.org/10.1109/IVS.2013.6629612.
Fernandez, B., Herrera, P.J., Cerrada, J.A., 2018. Self-tuning regulator for a tractor with varying speed and hitch forces. Comput. Electron. Agric. 145, 282–288. https://doi. org/10.1016/j.compag.2017.12.027. Gartley, E., Bevly, D., 2008. Online estimation of implement dynamics for adaptive steering control of farm tractors. IEEE/ASME Trans. Mech. 13, 429–440. https://doi. org/10.1109/TMECH.2008.2001692. URL.. Langer, J., Landau, I., 1999. Combined pole placement/sensitivity function shaping
101
Contents lists available at ScienceDirect
Computers and Electronics in Agriculture journal homepage: www.elsevier.com/locate/compag
Original papers
Robust digital control for autonomous skid-steered agricultural robots Benjamin Fernandez a b
a,b,⁎
b
, Pedro Javier Herrera , Jose Antonio Cerrada
T
b
Research and Advanced Engineering, AGCO GmbH, Johann-Georg-Fendt-Str. 4, 87616 Marktoberdorf, Germany Depto. de Ingeniería de Software y Sistemas Informáticos, ETSI Informática, UNED, C/ Juan del Rosal, 16, 28040 Madrid, Spain
A R T I C LE I N FO
A B S T R A C T
Keywords: Skid-steered robot Robust digital control Guidance control RST-controller Digital pole placement Autonomous off-road vehicles Agricultural robotics
There are two main issues to consider when designing a controller for autonomous off-road vehicles: velocity and terrain irregularities. Whereas the first one is measurable, the second one is very difficult to determine. Solutions to cover these issues could be very complex and difficult to implement in an embedded system with limited resources. The results obtained in our previous research for an adaptive approach implemented in a tractor with varying hitch forces, lead to the improvements presented here. This paper proposes a robust digital RST pole placement controller design for the lateral position, with sensitivity functions tuned to cover uncertainties and non-linearities not considered in the model. Simulations were implemented to assess the performance of the system and the controller implementation was applied to a skid-steered agricultural robot with limited computational resources and a state-of-the-art navigation system, which delivered satisfactory results.
1. Introduction Skid-steered robots will play an important roll in agricultural robotics due to their flexibility and simple construction. They are suitable for applications such as seeding (Blender et al., 2016), scouting, fertilizing and weed control. Nevertheless, there are some challenges to be covered related not only to the steering system, but also to the interaction with the soil. There are intrinsic non-linearities related to the steering system of such vehicles that make the design of a controller a very complex task. Furthermore, the dynamics of the system are constantly changing due to irregularities on the terrain. There are different approaches for the representation of such systems (Caldwell and Murphey, 2011; Martínez et al., 2005; Wang et al., 2015; Maclaurin, 2011; Guo and Peng, 2013; Yi et al., 2009; Al-Milli et al., 2010; Xueyuan et al., 2013), where different non-linear controllers are applied (Caracciolo et al., 1999; Tchoń et al., 2015; Jun et al., 2014; Pazderskit et al., 2004; Yi et al., 2007; Pazderski and Kozlowski, 2008), some of them being even robust solutions (Arslan and Temeltas, 2011; Inoue et al., 2013). There are also general linear solutions in the literature (Derrick and Bevly, 2008; Derrick and Bevly, 2009; Derrick et al., 2008; Gartley and Bevly, 2008) for controlling the lateral position of off-road vehicles by adapting the controller of the yaw rate with the use of a Model Reference Adaptive System(MRAS). In contrast to the MRAS solution, which requires more effort and filtering, our previous work implements a self-tuning regulator since the error is directly
obtained from a least-square identification and a general 2nd-order model can be used in the closed loop. This allows the solution to be applied to different steering systems in a general way. (Fernandez et al., 2018). Nevertheless, this solution still needs a traditional PID to be tuned for the lateral position, which is a time consuming task. Besides, since the identification of this solution requires a rich set of measurements, it could not be ideal for small vehicles with a higher frequency response such a small skid-steered robot. Therefore, the motivation of this research is to find a linear and practical approach for agricultural skid-steered robots to control the lateral position, easy to be programmed into a digital real-time embedded system with limited resources and that also guarantees robustness against uncertainties due to changes in the dynamics of the system. This paper is divided in four sections. The first part of Section 2 presents an RST pole placement design with tracking and regulation. In the second part (Section 2.2), static parameters used in the controller design are introduced for shaping the sensitivity functions of the closed loop. An experimental setup of a skid-steered robot with its results are presented in Section 3. Finally, the conclusions can be found in Section 4. 2. Material and methods Generally speaking, the uncertainties in the dynamics of an off-road vehicle are related to the changes in speed and soil irregularities. A
⁎
Corresponding author at: Research and Advanced Engineering, AGCO GmbH, Johann-Georg-Fendt-Str. 4, 87616 Marktoberdorf, Germany. E-mail addresses: [email protected], [email protected] (B. Fernandez), [email protected] (P.J. Herrera), [email protected] (J.A. Cerrada). https://doi.org/10.1016/j.compag.2018.07.038 Received 30 May 2018; Received in revised form 24 July 2018; Accepted 28 July 2018 0168-1699/ © 2018 Elsevier B.V. All rights reserved.
Computers and Electronics in Agriculture 153 (2018) 94–101
B. Fernandez et al.
Bm Am
r(t)
y ∗ (t + d + 1)
T
u(t)
1 S
−
z −d B A
y(t)
Fig. 1. Digital canonical controller for tracking and regulation.
R
and
solution for that is to design a system that considers these uncertainties due to nonlinearities and time varying elements. Here we present a methodology for the design and implementation of a robust digital RST controller using pole placement. There are automatic methods for finding the parameters and shaping the sensitivity functions using convex optimization or H∞ optimization (Langer and Landau, 1999; Langer and Constantinescu, 1999; Landau and Karimi, 1998). Nevertheless, this section presents a method to control the lateral position of a skid-steered robot by manually shaping the sensitivity functions (Landau and Zito, 2006) due to its simplicity compared to the automatic methods. This also allows for more flexibility in the design and a deep understanding of how the system reacts to changes in the controller design.
⎡ 1 0 … 0 . ⎢ a1 1 ⎢a 0 2 ⎢ 1 ⎢ M=⎢ a1 ⎢ a2 ⎢ anA ⎢ 0 . ⎢ 0 … 0 a nA ⎣
nA = degA (z −1) nB = degB (z −1) nS = degS (z −1) = nB + d−1 nR = degR (z −1) = nA−1 nP = degP (z −1) ⩽ nA + nB + d−1.
A canonical digital controller called RST is shown in Fig. 1. This structure allows one to impose different dynamics by obtaining the polynomials R and S in order to satisfy the desired regulation performance. T introduces a tracking performance that filters at the same time a desired trajectory y∗ (t + d + 1) from a tracking model system Bm . −d
The process to be controlled is represented by z B and the closed A loop function from the desired trajectory y∗ to the output y is represented in Eq. (1):
z −dB (z −1) T (z −1) + z −dB (z −1) R (z −1)
(1)
where d is the time delay and
T (z −1) = G∗P (z −1)
A (z −1)
where
= 1 + a1
z −1+…+a
nA
z −nA
B (z −1) = b1 z −1 + b2 z −2+…+bnB z −nB
1/ B (1) if B (1) ≠ 0 G=⎧ if B (1) = 0. ⎨ ⎩ 1
S (z −1) = 1 + s1 z −1+…+snS z −nS R (z −1) = r0 + r1 z −1+…+rnR z −nR.
(2)
S (z −1) u (t ) + R (z −1) y (t ) = T (z −1) y∗ (t + d + 1).
P (z −1) = PD (z −1) PF (z −1) = A (z −1) S (z −1) + z −dB (z −1) R (z −1).
2.2. Robust pole placement design
(3)
The characteristic polynomial P contains dominant and auxiliary poles. The dominant poles PD are chosen from the digitalization of a second-order system defined by ω0 and ζ . The digital auxiliary poles PF improve the robustness of the controller and are normally smaller (faster) than the real part of the dominant poles. Typical values for the auxiliary poles are −0.05 ⩽ α1 ⩽ −0.5 and α2 either equals 0 or α2 = α1. By defining the characteristic equation as follows: (4)
(5)
where
= [1, p1 , …, pnP , 0, …, 0]
(11)
Syp (z −1) =
A (z −1) S (z −1) A (z −1) S (z −1) + B (z −1) R (z −1)
Sup (z −1) =
−A (z −1) R (z −1) A (z −1) S (z −1) + B (z −1) R (z −1)
Syb (z −1) =
−B (z −1) R (z −1) A (z −1) S (z −1) + B (z −1) R (z −1)
Syv (z −1) =
B (z −1) S (z −1) . A (z −1) S (z −1) + B (z −1) R (z −1)
(12)
As we can see, the common denominator happens to be the characteristic equation of the closed loop system of Eq. (1). The noise sensitivity function Syb with a negative sign is also known in the literature as
xT = [1, s1, …, snS , r0, …, rnR] pT
(10)
In order to consider uncertainties in our control design, the structure of Fig. 1 can be extended into Fig. 2 where p (t ) are disturbances, b (t ) is noise and v (t ) disturbances on the plant input. We can obtain then the following output, input, noise, and disturbance sensitivity functions respectively:
we obtain R and S by solving
x = M−1p
(9)
Having R, S, T and a desired trajectory y∗ we obtain a control law of the form:
2.1.1. Regulation To compute the coefficients R and S of the digital controller we solve a Bezout polynomial equation of the form:
P (z −1) = 1 + p1 q−1 + …+pnP q−nP
(8)
2.1.2. Tracking B The reference model Hm = Am can be used to have an output y that m ∗ follows a desired trajectory y each time a reference r is changed. This tracking model Hm can take the form of a second order system with desired ω0 and ζ . This leads to choose T (z −1) to have a unit static gain between y∗ and y and to compensate between regulation dynamics defined by P (z −1) and tracking dynamics defined by the poles of the reference model (Am):
Am
A (z −1) S (z −1)
(7)
Here bi′ = 0 for i = 0, 1, …, d and bi′ = bi − d for i ⩾ d + 1 and:
2.1. Pole placement: tracking and regulation
HCL (z −1) =
0 … … 0 ⎤ b1′ ⎥ b2′ b1′ ⎥ ⎥ . b2′ ⎥ . . . ⎥ ⎥ bn′B . ⎥ 0 . . . ⎥ 0 0 0 bn′B ⎥ ⎦
(6) 95
Computers and Electronics in Agriculture 153 (2018) 94–101
B. Fernandez et al.
y ∗ (t + d + 1) r(t)
Bm Am
T
p(t)
v(t) −
1 S
+
u(t)
B A
+
y(t)
+
R
b(t)
Fig. 2. Digital canonical controller with disturbances and noise.
Syp (z −1) = 1 + Syb (z −1) and with the use of triangle inequality impose an upper and lower delay margin defined by the following equation:
the complementary sensitivity function and from Eq. (12) we see that Syp−Syb = 1. Furthermore, the robustness of the closed loop system can be evaluated by the distance of the critical point [−1, j 0] to the Nyquist plot of the nominal plant and by the frequency characteristics of the sensitivity functions (Eq. (12)). The elements that help to evaluate this robustness are the following four margins: gain, phase, delay and modulus margin. Especially the modulus margin is very important because it defines the maximum admissible value of the output sensitivity function Syp and defines the tolerance with respect to nonlinear or time varying elements that may belong to the system. A good modulus margin also guarantees good values for the gain and phase margins. The modulus margin ΔM is the inverse of the maximum of the output sensitivity function Syp . Typical values for a good modulus margin are ΔM ⩾ 0.5(−6 dB) . Therefore, we use these margins to shape the frequency characteristics of the sensitivity functions Syp and Sup and guarantee both robustness and closed loop performance. To be able to shape the sensitivity functions using pole placement, we have to introduce fixed parts to the Bezout polynomial equation (Eq. (3)) as follows:
P (z −1) = A (z −1) HS (z −1) S′ (z −1) + z −dB (z −1) HR (z −1) R′ (z −1).
1−|Syb (z −1)| ⩽ |Syp (z −1)| ⩽ 1 + |Syb (z −1)|. By substituting Eq. (16) into (18) we obtain Eq. (19).
1−|1−z −1|−1 ⩽ |Syp (z −1)| ⩽ 1 + |1−z −1|−1 .
3. Experimental setup and results Fig. 3 shows a representation of a skid-steered robot where y is the lateral position to a desired path and is the variable to be controlled. The yaw rate of the robot (ω ) and its linear velocity (V) are produced by the difference between the velocities of the right and left axis (VR and VL respectively), where β represents the angle of the velocity vector to the center of gravity. The angular velocities of the right and left wheels are represented by ωR and ωL respectively and the distance between axes is 2c . The angle with respect to the desired path is represented by ν and r is the wheel radius.
(13)
3.1. Robot model (14)
From the kinematic point of view, we can define the yaw rate at the center of gravity and the linear velocity by the following equations:
2.2.1. Shaping the input sensitivity function Sup The input sensitivity function reflects the actuator stress, therefore, we should limit its magnitude at high frequencies. By plotting the inverse of its magnitude |Sup (e−jω)|, we check and impose and upper bound maximum by imposing the prefixed value HR (z −1) shown in Eq. (15), which e.g. makes |Sup | = 0 at 0.5fs by choosing β = 1, where fs is the sampling frequency.
HR (z −1) = 1 + βz −1;
0 < β ⩽ 1.
V = r∗
ωL + ωR 2
(20)
ω = r∗
−ωL + ωR . 2c
(21)
(15)
2.2.2. Shaping the noise sensitivity function Syb The noise sensitivity function is related to the delay margin Δτ and considering Δτ = Ts we impose a frequency template of a digital integrator of the form: −1 −1 |Syb (z )|dB < −20log|1−z −1|; z = e jw;
0 ⩽ ω ⩽ π.
(16)
2.2.3. Shaping the output sensitivity function Syp We already mentioned a typical value for a good modulus margin (ΔM ⩾ 0.5(−6 dB) ). Imposing that to the output sensitivity function we have that
|Syp (z −1)| ⩽ −ΔM ; z = e jw;
0 ⩽ ω ⩽ π.
(19)
Again, in order to find a closed loop function that fits inside of the upper and lower bounds of the template defined by Eqs. (17) and (18), we can pick specified parts for HS . For example making HS = 1−z −1 we introduce a zero at the zero frequency allowing perfect rejection of constant disturbances.
The solution to Eq. (13) is found in the same way Eq. (3) is solved with Eq. (5), after replacing A (z −1) by A′ (z −1) = A (z −1) HS (z −1) and B (z −1) by B′ (z −1) = B (z −1) HR (z −1) with the following new conditions:
nP = degP (z −1) ⩽ nA + n HS + nB + n HR + d−1 nS′ = degS′ (z −1) = nB + n HR + d−1 nR′ = degR′ (z −1) = nA + n HS−1.
(18)
(17) Fig. 3. Lateral position.
Furthermore, we can use the complementary function defined by 96
Computers and Electronics in Agriculture 153 (2018) 94–101
B. Fernandez et al.
By rewriting Eq. (21) into right and left velocities we obtain Eq. (22). And by defining a time delay τ we find a first oder yaw-rate transfer function shown in Eq. (23).
V −V ΔV . ω= R L = 2c 2c
Gω (s ) =
ω (s ) 1/2c = . ΔV (s ) τ·s + 1
Table 1 Technical data of a skid-steered robot. Robot data
Ts τ 2c V
(22)
(23)
Finally using the z-transform we obtain the following digital transfer function:
Gω (z −1) =
br 1 z −1 1 + ar 1 z −1
d=0 B (z −1) = 0.003473z −2 + 0.003473z −3 A (z −1) = 1−2.368z −1 + 1.736z −2−0.3679z −3.
(24)
br 1 = (1/2c ) ∗ (1−e−Ts / τ ) (25)
Generally speaking, the lateral position y with respect to a desired path is related to the yaw rate of the vehicle in the form of Eq. (26), where β is the side slip angle and ν the course angle with respect to the desired path (Fig. 3) (Gartley and Bevly, 2008).
y ̇ = V sin (ν ) ν ̇ = ω + β.̇
y (s ) V = 2. ω (s ) s
(26)
Gy
bl0 + bl1 z −1 = , 1 + al1 z −1 + al2 z −2
(27)
b2 z −2 + b3 z −3 , 1 + a1 z −1 + a2 z −2 + a3 z −3
(28)
R (z −1) = 1−0.4784z −1 + 0.04941z −2−0.005427z −3−0.01235z −4 S (z −1) = 8.788−6.796z −1−7.374z −2 + 6.903z −3−1.308z −4 T (z −1) = 144−409.7z −1 + 424.4z −2 + 189.1z −3−30.67z −4.
(36)
(37)
3.3. Results (29) With the results of Section 3.2 we can prove the robustness of the design for the different sensitivity functions. Fig. 4 shows the Nyquist −1 −1 in green. Since the critical plot of the open loop function Hol = Syp point [−1, j 0] is at the left of the plot, we conclude that the open loop system has no unstable poles. Here, the inverse of the gain margin 1/ΔG and the phase margin ΔΦ are stability margins with respect to the critical point. It can be seen that the modulus margin ΔM is the radius of a circle centered at the critical point and tangent to the Nyquist plot. It tells us the distance between Hol and the critical point. Therefore, a good modulus margin (e.g. ΔM ⩾ 0.5) implies stable gain and phase margins. Fig. 5 shows in blue the applied modulus and delay margins to the output sensitivity function Syp . The upper-left bound is built by a modulus margin ΔM = 0.5(−6 dB) (Eq. (17)) and the upper-right and lower bounds represent the limits of the delay margin (Eq. (19)) which is more restrictive at higher frequencies than the modulus margin. The output sensitivity function in green is the result of imposing the fixed part HS (z −1) to the pole placement design (Eq. (33) (Landau and Zito, 2006)). It can be seen that using different values for the fixed part HS will shape the output sensitivity function differently. It can also be seen that, although using HS = 1−1z −1 (dashed green line) will reject constant disturbances in steady state, the shaped Syp is closer to the upper margin. This will make the system less tolerant to changes on the system dynamics at lower frequencies and to fix that we should impose
(30)
where
b2 = br 1 ∗bl0 b3 = br 1 ∗bl1 a1 = al1 + ar 1 a2 = 1 + al1 ∗ar 1 a3 = ar 1.
(35)
Finally, using these results to solve Eqs. (5) and (9) we find the following control parameters R, S and T:
Multiplying Eqs. (24) and (28) we obtain the digital model of the lateral position with respect to a desired path of a skid-steered robot to be used for our RST controller design (Eq. (30)).
A
(34)
Bm (z −1) = 0.01752z −1 + 0.01534z −2 Am (z −1) = 1−1.637z −1 + 0.6703z −2.
bl0 = V ∗Ts2/2
GB =
B′ (z −1) = 0.003473z −2 + 0.006946z −3 + 0.003473z −4 A′ (z −1) = 1−2.868z −1 + 2.92z −2−1.236z −3 + 0.1839z −4.
For the tracking dynamics we use Ts = 0.1; ω0 = 2.0; and ζ = 1.0 to find the following parameters of the reference model Hm :
where
bl1 = V ∗Ts2/2 al1 = −2 al2 = 1.
(33)
P (z −1) = PD·PF = (1−1.846z −1 + 0.8521z −2)·(1−0.5z −1)2.
Again, using the z-transform we obtain the following digital transfer function of the lateral position with respect to the yaw rate:
(z −1)
HR (z −1) = 1 + 1.0z −1 HS (z −1) = 1−0.5z −1.
For the regulation dynamics we use Ts = 0.1, ω0 = 0.8, ζ = 1.0 and α1 = α2 = −0.5 and so the left part of the Bezout equation (13) appears as follows:
Neglecting side slip, linearizing and using the small angle approximation we find the transfer function of the lateral position with respect to the yaw rate in Eq. (27).
Gy (s ) =
(32)
To shape the sensitivity functions we choose the fixed parts represented in Eq. (33) and the new poles and zeros to be used in the right part of the Bezout equation (13) are expressed in Eq. (34).
where
ar 1 = −e−Ts / τ .
0.1 [ms] 0.1 0.455 [m] 0.5 [m/s]
(31)
3.2. Controller design For the controller design, the robot will be driving at a constant speed of 0.5 [m/s] and the sampling time of the digital system is 100 [ms]. The track-width of 0.455 [m] corresponds to the real robot described in Section 3.3.1, with a time constant of 100 [ms] (see Table 1). Substituting the data from Table 1 into Eqs. (25), (29) and (31) we get the following plant model parameters: 97
Computers and Electronics in Agriculture 153 (2018) 94–101
B. Fernandez et al.
Fig. 4. Nyquist plot.
the solid green line arrives faster at the steady state (around 4 [s]) whereas the dashed one needs more than 8 [s] and contains oscillations. As already mentioned, the oscillations can be fixed by picking slower regulation dynamics (ω0 ⩽ 0.8) or by driving faster. Nevertheless, we could reduce the error at the turning points by reducing the speed and to this end, it is desirable to have the system react faster to changes. A 3D real-time simulation using the results of Eqs. (32), (36) and (37) is shown in Fig. 9. It shows representative results which are also useful to prove the robustness of the behaviour in an interactive environment (Fernandez et al., 2016). For the simulation, a real path with geographic coordinates was used so we could compare these results with the results of a real robot using the same path. The simulation shows very satisfactory results where the biggest error (at the fifth turning point) was around 20 [cm] which is comparable to other more complex solutions (Hwang et al., 2013; Arslan and Temeltas, 2011; Inoue et al., 2013; Tchoń et al., 2015; Caracciolo et al., 1999; Jeon and Jeong, 2015). These errors at the turning points are expected since in order to be able to turn with zero curvature radius, the skid-steered robot should stop completely. In contrast, in the simulation the robot was reducing its velocity to 0.1 [m/s] to test its robustness.
slower regulation dynamics (e.g. ω0 < 0.8). On the other hand, using Hs = 1−0.5z −1 (solid green line) will decrease the gain, making the system more tolerant to uncertainties with faster regulation dynamics. Fig. 6 shows in green the noise sensitivity function Syb and in blue the bound related to the delay margin Δτ = Ts (Eq. (16)). The delay margin was already set for the margins of the output sensitivity function (Syp ) with good results (Fig. 5). Nevertheless, the delay margin is related to the noise sensitivity function and the fact that Syb is under the bound confirms the good choice of the fixed parts (Eq. (33)). Fig. 7 shows in green the input sensitivity function (Sup ) for a pole placement with the imposed fixed part HR = 1 + 1z −1 (Eq. (33)) which will impose a zero at 0.5fs reducing the actuator stress at higher frequencies. This has the effect of limiting not only the actuator stress but also the reaction to measurement noise and disturbances making the robot drive more smoothly and not react right away after every stone or irregularity in the soil. It will also reduce the energy consumed by the motors and increase their lifetime. The resulting step responses of the different shaping of the output sensitivity function is shown in Fig. 8. The dashed green line corresponds to the response using the fixed part HS = 1−1.0z −1, whereas the solid green one corresponds to the use of HS = 1−0.5z −1. It can be seen that
Fig. 5. Output sensitivity function. 98
Computers and Electronics in Agriculture 153 (2018) 94–101
B. Fernandez et al.
Fig. 6. Noise sensitivity function.
4. Conclusions
3.3.1. Real validation For the validation, this method was programmed in the Electronic Control Unit (ECU) of a 40kg-weight agricultural skid-steered robot (Fig. 10). Each wheel is equipped with an electric drive, able to reach a maximum speed of 1 [m/s], and hall sensors to measure the angle and angular velocity of the motors. For the position, a state-of-the-art GNSS receiver with RTK-correction is used. The ECU with a 400 [MHz] processor, includes an Inertial Measurement Unit (IMU), WLAN module and CAN interfaces for different internal communications. The same coordinates used for the simulation (Fig. 9) were sent to the real robot and the results are shown in Fig. 11. It can be seen that the robot was successfully followed the path with a similar type of error in the turning points as shown in the simulation with a maximum of 18 [cm] also at the fifth turning point. In contrast to the simulation, disturbances due to terrain irregularities can be appreciated in the real approach. Nevertheless, the controller was able to react very smooth to disturbances without producing an error of more than 2 [cm] once on the lane. The same path was tested in different terrains and the skid steered robot produced similar results.
This paper presented a robust digital method to control the lateral position of a skid-steered agricultural robot. The method is based on a digital RST pole placement and introduces fixed parts to the Bezout polynomial in order to shape the different sensitivity functions to guarantee robustness against uncertainties due to noise and disturbances, as well as non-linearities not considered in the model. The results presented showed the robustness of the controller and its ability to react nicely against terrain irregularities and disturbances. It was also possible to implement this linear robust digital solution into an ECU with limited resources and obtain similar results to more complex, even non-linear approaches. As future work, this method could be implemented for a MIMO state-space system to be able to include heading and speed in the same controller. Acknowledgment This work has been developed with the help of the research project DPI2016-77677-P of the MICINN. It also belongs to the activities
Fig. 7. Input sensitivity function. 99
Computers and Electronics in Agriculture 153 (2018) 94–101
B. Fernandez et al.
Fig. 8. Step response over 1 meters cross track.
Fig. 11. Lane-tracking control on the field. Fig. 9. 3D real time lane-tracking simulation with openframeworks.
References Al-Milli, S., Seneviratne, L.D., Althoefer, K., 2010. Track-terrain modelling and traversability prediction for tracked vehicles on soft terrain. J. Terrramech. 47, 151–160. https://doi.org/10.1016/j.jterra.2010.02.001. Arslan, S., Temeltas, H., 2011. Robust motion control of a four wheel drive skid-steered mobile robot. In: 2011 7th International Conference on Electrical and Electronics Engineering (ELECO), pp. II-415–II-419. Blender, T., Buchner, T., Fernandez, B., Pichlmaier, B., Schlegel, C., 2016. Managing a Mobile Agricultural Robot Swarm for a seeding task. In: IECON 2016 - 42nd Annual Conference of the IEEE Industrial Electronics Society. IEEE, pp. 6879–6886. https:// doi.org/10.1109/IECON.2016.7793638. URL.
Fig. 10. Skid-steered robot for corn seeding provided by AGCO GmbH | Fendt.
carried out within the framework of the research network CAM RoboCity2030 S2013/MIT-2748 of the Comunidad de Madrid. The authors would like to thank AGCO for the provision of the robot and the technical support of this research.
Appendix A. Supplementary material Supplementary data associated with this article can be found, in the online version, at https://doi.org/10.1016/j.compag.2018.07.038.
100
Computers and Electronics in Agriculture 153 (2018) 94–101
B. Fernandez et al.
method using convex optimization criteria. Automatica 35, 1111–1120. https://doi. org/10.1016/S0005-1098(99)00013-8.
Fernandez, B., Herrera, P.J., Cerrada, J.A., 2018. Self-tuning regulator for a tractor with varying speed and hitch forces. Comput. Electron. Agric. 145, 282–288. https://doi. org/10.1016/j.compag.2017.12.027. Gartley, E., Bevly, D., 2008. Online estimation of implement dynamics for adaptive steering control of farm tractors. IEEE/ASME Trans. Mech. 13, 429–440. https://doi. org/10.1109/TMECH.2008.2001692. URL.
101