Auton Robot (2010) 29: 85–97DOI 10.1007/s1051401091823
A path following algorithm for mobile robots
Tijmen Bakker
·
Kees van Asselt
·
Jan Bontsema
·
Joachim Müller
·
Gerrit van Straten
Received: 8 December 2008 / Accepted: 18 March 2010 / Published online: 21 April 2010© The Author(s) 2010. This article is published with open access at Springerlink.com
Abstract
This paper considers path following control fora robotic platform. The vehicle used for the experimentsis a specially designed robotic platform for performing autonomous weed control. The platform is fourwheel steeredand fourwheel driven. A diesel engine powers the wheelsvia a hydraulic transmission. The robot uses a Real TimeKinematic Differential Global Positioning System to determine both position and orientation relative to the path. Thedeviation of the robot to the desired path is supplied to twohigh level controllers minimizing the orthogonal distanceand orientation to the path. Wheel angle setpoints are determined from inversion of the kinematic model. At lowlevel each wheel angle is controlled by a proportional controller combined with a Smith predictor. Results show thecontroller performance following different paths shapes includingastep,aramp,andatypicalheadlandpath.Areﬁned
T. Bakker (
)
·
K. van Asselt
·
G. van StratenSystems and Control Group, Wageningen University,P.O. Box 17, 6700 AA Wageningen, The Netherlandsemail: tijmen.bakker@tyker.com
Present address:
T. BakkerTyker Technology, P.O. Box 507, 6700 AM Wageningen,The NetherlandsJ. BontsemaWageningen UR Greenhouse Horticulture, P.O. Box 644,6700 AP Wageningen, The NetherlandsJ. MüllerFarm Technology Group, Wageningen University, P.O. Box 17,6700 AA Wageningen, The Netherlands
Present address:
J. MüllerInstitute for Agricultural Engineering, University of Hohenheim,70593 Stuttgart, Germany
tuning method calculatescontroller settingsthat let the robotdrive as much as possible along the same path to its setpoint,but also limit the gains at higher speeds to prevent the closedloop system to become unstable due to the time delay in thesystem. Mean, minimum and maximum orthogonal distanceerrors while following a straight path on a paving at a speedof 0.5 m/s are 0.0,
−
2.4 and 3.0 cm respectively and thestandard deviation is 1.2 cm. The control method for fourwheel steered vehicles presented in this paper has the uniquefeature that it enablescontrol of a user deﬁnableposition relative to the robot frame and can deal with limitations on thewheel angles. The method is very well practical applicablefor a manufacturer: all parameters needed are known by themanufacturer or can be determined easily, user settings havean easy interpretation and the only complex part can be supplied as a generic software module.
Keywords
Robot
·
Path following
·
4WS
·
RTKDGPS
1 Introduction
In organic farming there is a need for weeding robots thatcan replace manual weeding. The required labour for handweeding is expensive and often difﬁcult to obtain. In 1998in the Netherlands on average 73 hours of hand weedingwere spent on one hectare of sugar beet in organic farming(Van der Weide et al. 2002). In this paper a path followingcontrol system for a weeding robot is presented enabling therobot to navigate autonomously along a path.A common design for a control system for agriculturalvehicles is to split up the control system in a low level and ahigh level controller (Bendtsen et al. 2002; Bak and Jakobsen 2004). The low level electrohydraulic system is a systemwithdeadtime.Awellknownmethodtocompensatefor
86 Auton Robot (2010) 29: 85–97
time delays is the Smith predictor (Stephanopoulos 1984).Ge and Ayers (1991) applied this successfully to control anelectrohydraulic system on a hydraulic test bench. We useda Smith Predictor to compensate for time delays in the application of an electrohydraulic steering system in practice.The high level control system is partly inspired by work of Hague and Tillett (1996) and Bendtsen et al. (2002).
Bendtsen et al. (2002) used a model for a fourwheel steeredvehicle derived from Campion et al. (1996) and presentedsimulation studies applying feedback linearization as a control method. Hague and Tillett (1996) worked out a methodfor path following for a vehicle with two driven wheels andtwo free rolling wheels. For a simpliﬁed vehicle model theydevelopedacontroller.Fromtheoutputofthiscontrollerfollow the wheel speed setpoints by inversion of the kinematicvehicle model. In this paper this method is worked out fora four wheel steered robot, using the kinematic model derived from Campion et al. (1996) resulting in wheel angleand wheel speed setpoints for the low level control system.A reﬁned tuning method of the high level controller, adaptedfrom Skogestad (2003), lets the robot drive as much as possible along the same path to its setpoint independent fromspeed, but also limit the gains at higher speeds to preventthe closed loop system from becoming unstable at higherspeeds because of the time delay.
2 Robotic platform
2.1 PlatformThe vehicle used for the experiments is a specially designedrobotic platform for performing autonomous weed control(Fig. 1). The design of the platform was described earlier byBakker et al. (2008). The platform is fourwheel steered andfourwheel driven. There is no mechanical constraint on themaximum turning angle of a wheel around its vertical axis,
Fig. 1
Robot platform
but the wheel angles should be constrained to prevent twisting of the cables of the wheel speed sensors. Power is provided by a diesel engine that powers the wheels via an hydraulic transmission. The hydraulic transmission consists of a pump supplying oil to eight proportional valves, each connected to one ﬁxed displacement hydraulic motor. Four hydraulic motors are used to drive the wheels, the other four tosteer the wheels. Computer control of the valves is achievedusingpulsewidthmodulationviatwomicrocontrollersconnected to a CAN bus. The pump/valves combination is a‘loadsensing’ system: the pressure drop overthevalvescontrols the displacement of the pump via an hydraulic loadsensing connection and is limited to a small value, independent of load pressure. The platform is further equipped witha hitch that can be lifted hydraulically. A second hydraulicpump mounted in series with the ﬁrst, supplies oil to twovalves: one for lifting the hitch, one for control of auxiliaryimplements. Computer control of the valves is achieved alsovia a micro controller connected to the CAN bus.2.2 ElectronicsThe weeding robot electronics consists of 9 embedded controllers connected by a CAN bus using the ISO 11783 protocol. In the inside of every wheel rim a cogwheel is mountedfor wheel speed measurement. The two magneto resistivesensors per cogwheel are placed in such a way that thedirection of rotation can be resolved. The rotation of thewheels is measured by these sensors with a resolution of 100 pulses per wheel revolution. The wheel angle of eachwheel is measured by a Kverneland 180 degree sensor withan accuracy of one degree. Per wheel a micro controller ismounted transmitting wheel speed and wheel angle via theCAN bus. Two GPS antennas are used to measure both vehicle position and orientation. Both are connected to a Septentrio PolaRx2eH RTKDGPS receiver with a speciﬁed position accuracy of 1–2 cm and a speciﬁed orientation accuracyof 0.3 degrees (1
σ
). The two GPS antennas are mountedon a metal plate to prevent multipath errors. A base stationwith a Septentrio PolaRx2e RTKDGPS supplies the RTKcorrection signals via a radio connection to the SeptentrioPolaRx2eH receiver. The position of the base station itself can be conﬁgured by a correction supplied by the service of a company called 06GPS via GPRS. One embedded controller running a real time operating system (National Instruments PXI system) also connected to the CAN bus controlsthe vehicle. The GPS receiver, and a radio modem are connected to the PXI via RS232. The radio modem interfacesthe remote control used for manual control of the weedingrobot. The manual control is used for guaranteeing safetyduringﬁeldtrialsandfortransportationtoandfromtheﬁeld.Different colored lamps of the signal tower can be operatedvia a microcontroller to indicate the current status of the robot platform. The platform is further equipped with sensors
Auton Robot (2010) 29: 85–97 87
measuring diesel level, hydraulic oil level, engine temperature and hitch height. The PXI system gathers wheel angles, wheel speeds, GPS data, remote control data and hitchheight and controls the vehicle by sending messages to thethree micro controllers connected to the hydraulic valves.A safety system consisting of four red emergency switchesat the corners of the vehicle and a remote switch, controlsthe valves to neutral position on activation, overruling thecomputer control.
3 Path following structure
The vehicle control consists of two levels. At high level thewheel angle setpoints and wheel speed setpoints are determined in order to decrease the deviation from the path andthe error in orientation. At low level, controllers are used torealize the wheel angles and wheel speeds determined by thehigh level control.The deviation and the orientation error of the robot froma path are determined by a specially designed orthogonalprojection on the path using the measured orientation andthe GPS position. The orthogonal projection is designed tocalculate the deviation and the orientation error relative to aline of positions
y(x)
.
4 Low level control
4.1 Wheel angle process modelThe low level control realizes for each wheel the wheel angle and the wheel speed. The hydraulic valves used for steering the wheels of the weeding robot have a certain reactiontime, resulting in a time delay of the steering. Furthermore,if a valve has a commanded open time of less than the deadtime, a control does not have any effect. So the wheel angleprocess can be represented by:
˙
β
=
0 for
t
open
< t
dead
(1)
˙
β
=
K
p
·
u(t
−
t
d
)
for
t
open
> t
dead
(2)and:
u(t)
=−
1995 if
U <
2500
u(t)
=
U
−
4495 if 2500
≤
U
≤
4000
u(t)
=
0 if 4000
< U <
6000
u(t)
=
U
−
5405 if 6000
≤
U
≤
7500
u(t)
=
2095 if
U >
7500where:
˙
β
is the wheel steering angle speed
[
◦
/
s
]
.
K
p
is the gain of the process and equals 0.0712.
u
is the control corrected for the dead band.
U
is the control
[
%
U
DC
·
100
]
.
U
DC
is the power supply voltage and equals about 12 [V].
t
d
is the delay of the system and equals 0.25 [s].
t
open
is the time generated by a counter counting the timethat the commanded control is in the active band (outside the dead band where 4000
< U <
6000). It resetswhenthecommandedcontrolreturnsto the deadband.
t
dead
is the dead zone of the system and equals 0.15 [s].The value of
t
dead
was determined from tests in which theopen time of a valve was varied,
K
p
,
t
d
and the values thatrelate
u
to
U
followed from step responses of the system.4.2 Wheel angle controlTo compensate for the time delay a P controller withSmith predictor is used for the wheel steering control(Stephanopoulos 1984).The wheel angle control of the robot was tested by applying setpoint changes to one wheel while the robot wasstanding still on a ﬂat concrete ﬂoor. From some ﬁrst measurements it appeared that at large setpoint changes the variable pump controlled by the load sensing system could notreactfastenoughforthechangeintheﬂowrequiredtomaintain full pressure in the hydraulic system. Furthermore, if weimagine the robot driving over the ﬁeld, the ﬂow needed forsteering will require only small changes in the ﬂow alreadypresent for driving. So to simulate the presence of a continuous oil ﬂow for driving during the wheel angle control test,one wheel was lifted from the ﬂoor and a constant controlwas put on the valve controlling its speed.The average error of a series of 96 measurements on awheel angle setpoint change of 10 degrees decreased withinone second to zero plus or minus 2 degrees (see Fig. 2).
Fig. 2
Performance of the wheel angle control, average of 96 measurements. The setpoint changes at
t
=
0 s from 10 to 0 degrees (front left , — rear left,
·

·
rear right,
···
front right)
88 Auton Robot (2010) 29: 85–97
5 High level control
5.1 Vehicle modelThe point of the vehicle that should follow the path is the vehicle implement attached to the vehicle at a certain speed
v
.Consider a pathrelativecoordinate system
(x
P
,y
P
)
as illustrated in Fig. 3. The implement position is then completelydescribed by
ξ
= [
x y θ
]
T
where
x
denotes the distancealong the path,
y
the perpendicular offset from the path, and
θ
the heading angle of the platform relative to the path (seeFig. 3).Consider a coordinate system (
x
v
,y
v
) ﬁxed to the robotframe.Thepositionofawheelinthisvehiclecoordinatesystemischaracterizedbytheangle
γ
i
andthedistance
l
i
where
i
is the wheel index. The orientation of a wheel relative to
x
v
is denoted
β
i
. The model assumes pure rolling and nonslipconditions and driving in a horizontal plane. Therefore themotion of the robot can always be viewed as an instantaneous rotation around the instantaneous center of rotation(ICR). At each instant, the orientation of any wheel at anypoint of the robot frame must be orthogonal to the straightline joining its position and the ICR. The twodimensionallocation of the ICR is speciﬁed by the angles of two wheels.For convenience a virtual front wheel
β
f
and a virtual rearwheel
β
r
is introduced with corresponding
γ
f
,
l
f
,
γ
r
and
l
f
,respectively located right in between the front wheels andright in between the rear wheels. The motion of the vehicleimplement is described by the following statespace model
Fig. 3
Robot with ICR
derived from earlier work from Campion et al. (1996) andBendtsen et al. (2002):
˙
ξ
=
R
T
(θ)Σ(β
i
)η
(3)where
R(θ)
is the orthogonal rotation matrix:
R(θ)
=
cos
(θ)
sin
(θ)
0
−
sin
(θ)
cos
(θ)
00 0 1
(4)and:
Σ(β
i
)
=
l
f
cos
(β
r
)
cos
(β
f
−
γ
f
)
−
l
r
cos
(β
f
)
cos
(β
r
−
γ
r
)l
f
sin
(β
r
)
cos
(β
f
−
γ
f
)
−
l
r
sin
(β
f
)
cos
(β
r
−
γ
r
)
sin
(β
f
−
β
r
)
(5)The scalar
η
is a velocity input. The wheel orientation
β
3
and
β
4
follow from
β
f
and
β
r
as described by Bendtsen et al. (2002) and Sørensen (2002) and
β
1
and
β
2
can befound in a similar way:
β
1
=
arctan
L
sin
(β
f
)
cos
(β
r
)L
cos
(β
f
)
cos
(β
r
)
−
12
W
sin
(β
f
−
β
r
)
β
2
=
arctan
L
cos
(β
f
)
sin
(β
r
)L
cos
(β
f
)
cos
(β
r
)
−
12
W
sin
(β
f
−
β
r
)
β
3
=
arctan
L
cos
(β
f
)
sin
(β
r
)L
cos
(β
f
)
cos
(β
r
)
+
12
W
sin
(β
f
−
β
r
)
β
4
=
arctan
L
sin
(β
f
)
cos
(β
r
)L
cos
(β
f
)
cos
(β
r
)
+
12
W
sin
(β
f
−
β
r
)
(6)where
L
is the distance between the front and rear wheelsand
W
the distance between the left and right wheels.The wheel angular speeds
˙
φ
= [ ˙
φ
1
,
˙
φ
2
,
˙
φ
3
,
˙
φ
4
]
T
are controlled at low level, and follow from the vehicle model:
˙
φ
=
J
−
12
J
1
(β
i
)Σ(β
i
)η(t)
(7)where:
J
1
(β
i
)
=
cos
(β
1
)
sin
(β
1
) l
1
sin
(β
1
−
γ
1
)
cos
(β
2
)
sin
(β
2
) l
2
sin
(β
2
−
γ
2
)
cos
(β
3
)
sin
(β
3
) l
3
sin
(β
3
−
γ
3
)
cos
(β
4
)
sin
(β
4
) l
4
sin
(β
4
−
γ
4
)
(8)
J
2
=
r
1
0 0 00
r
2
0 00 0
r
3
00 0 0
r
4
and
r
1
,r
2
,r
3
,r
4
are the radii of the four wheels.