- Email: [email protected]
Simultaneous localisation and mapping in a complex field environment
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
Available online at www.sciencedirect.com
ScienceDirect journal homepage: www.elsevier.com/locate/issn/15375110
Research Paper
Simultaneous localisation and mapping in a complex field environment Peter Lepej a,*, Jurij Rakun b a b
Faculty of Electrical Engineering and Computer Science, Smetanova ul. 17, Maribor, Slovenia Faculty of Agriculture and Life Sciences, Pivola 10, Hoce, Slovenia
article info
The usefulness of image registration techniques in mapping and localising a robot in an
Article history:
agricultural environment by using readings from a laser range scanner was investigated.
Received 4 October 2015
The proposed approach used frequency domain and correlation. Translational and rota-
Received in revised form
tional differences that occur between successive readings of the scanner and that corre-
21 June 2016
spond to the movement of the robot were used. The approach was tested on 9 test runs,
Accepted 2 August 2016
with a total of 252 m in length, recorded in an apple orchard and in a vineyard. The results
Published online 20 August 2016
were then compared to results from the Hector mapping algorithm. It was shown that the present approach performed very well compared to Hector mapping. On average achieved
Keywords:
an 4.24% ± 2.9% error rate and the present approach 0.16% ± 0.1%. Hector mapping on the
Image processing
other hand proved better in cases where rotational differences were looked for, reaching an
Distance transformation
error rate of 1.69% ± 0.7% in comparison to present approach with an error rate of
Phase correlation
4.19% ± 3.1%. © 2016 IAgrE. Published by Elsevier Ltd. All rights reserved.
SLAM Agriculture Mobile robot
1.
Introduction
As in other research areas, mobile robot applications are rapidly expanding into different areas of agriculture. Tasks such as arable farming require standard, periodic operations that are increasing in demand. The high demand for diverse food products has led food producers to use a wide range of fertilisers and pesticides in order to achieve optimal crop growth, but they have a negative impact on the environment. In order to minimise unwanted, potentially damaging effects, agricultural tasks should be examined and carried out more precisely, selectively and with care for the safety of people and the environment. This was one reason that we decided to
develop a mobile robot that could fit and operate between the crop rows of different agricultural cultures. The use of such mobile robots in agriculture relies on the hypothesis that they can selectively spray plants that need to be treated. The motivation is to use potentially dangerous pesticides as little as possible and to make farming more nature friendly. In addition, such robots could carry out a number of other tasks, such as mechanical weed killing, selective fertilisation, crop assessment, plant damage assessment, detection pest, after spraying or even during the process. Mobile robots that can perform these tasks need to be independent; this means there should be no intermediate control of human intervention in an individual robot's performance. In addition, a fleet of such robots should be able
* Corresponding author. E-mail addresses: [email protected] (P. Lepej), [email protected] (J. Rakun). http://dx.doi.org/10.1016/j.biosystemseng.2016.08.004 1537-5110/© 2016 IAgrE. Published by Elsevier Ltd. All rights reserved.
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
Nomenclature lrfdata angleðiÞ I1 I2 M N I1* S1 S2 res rangemax W dx dy qc ux ux q
an array of laser range finder readings, mm i-th angle of the lrf reading, first 2D signal e an reference, second, successive 2D signal e a template, the width of signal I, the height of signal I, a complex conjugate of I1, frequency transforms of signal I1, frequency transforms of signal I2, resolution of an image, pixels the scaling factor used in interpolation step, a template image with Ww Wh in size, pixels temporal shift along the X axis, pixels temporal shift along the Y axis, pixels temporal angular shift, position of the robot along X axis, m position of the robot along Y axis, m orientation of the robot,
to form a swarm and work towards a common collective goal, reaching it faster, making the work more precise, while yielding lower production costs. This idea is based on an autonomous field robot system that is able to perform given tasks. Such as basic autonomous system for a mobile field robot is shown in Fig. 1. Localisation and mapping are the means by which it is able to achieve autonomy. Mapping is a procedure indicating out the affected areas and present observations on 2D or 3D maps by taking into account laser range finder (LRF) readings. Here the focus is on building a reliable, odometry free mapping system. Mapping is used in autonomous systems at the medium level (Fig. 1) and provides the system with information about the environment. High control levels take care of all basic application controls, whereas the low level represents the drivers for actuators and sensors. The system takes decisions based on the given environmental status in the form of a map. Odometry is not considered in this study because wheel encoders can produce a lot of undesirable error caused by complex ground structure.
161
Here the focus is not on a low-cost sensor such as wheel encoders, because of the common problems described in Ohno, Tsubouchi, Shigematsu, Maeyama, and Yuta (2003). Oksanen, Linja, and Visala (2005) discussed a low-cost system for outdoor positioning which cannot only rely on odometry. For this reason, the Hector mapping algorithm from TU Darmstadt (Kohlbrecher et al., 2013), since it does not require additional information from odometry. The initial tests of Hector mapping were conducted on ordinary, complex crop rows and they showed that in some cases the result is incorrect and mapping fails. This drove us to develop a new approach for complex structure mapping in natural environments with the goal of achieving the best possible precision on larger areas of crop rows. The algorithm provides a map of the current environment and localisation based on a reference coordinate system that is established at the start of the algorithm. This approach was compared to a Hector mapping algorithm by using the same data sets. Localisation and mapping is commonly known as the SLAM (simultaneous localisation and mapping) algorithm (Thrun, Burgard, & Fox, 2005). Widely used algorithms for localisation and mapping include Hector mapping and Gmapping. Gmapping is a standard method described in Grisetti, Stachniss, and Burgard (2007) and Grisetti, Stachniss, and Burgard (2005), which is based on use of a RaoBlackwellised particle filter to create a map and provide localisation information. The method's purpose is to decrease the number of particles with filtering, where precise calculation and exact deviation are considered. It uses the latest information provided by a laser scanner, reduces locational uncertainty and predicts the robot's position in the environment. Gmapping is optimised for long-range laser scanners and environments with large areas. In order to function, it needs laser scan data and, for best results additional odometry. In contrast to Gmapping, Hector mapping as described in Kohlbrecher et al. (2013), Kohlbrecher, Meyer, von Stryk, and Klingauf (2011) and hector_slam software package (2014) is optimised for narrow corridors and the kind of complex environment found in rescue scenarios. Hector mapping does not need additional information regarding odometry and this does not affect its performance. For localisation, it uses the scan-matching method and a Gauss-Newton filter to define
Fig. 1 e Overview of a mobile robot autonomous system.
162
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
the robot's current position based on particle distribution. It also considers a multi-layer map and a map server to save all available maps and trajectories. Hector mapping is a very efficient and reliable algorithm, which can be used for handheld devices to map certain areas. State of the art studies of an autonomous guidance system based on a mapping algorithm as described in Barawid, Mizushima, Ishii, and Noguchi (2007) or computer vision (Hiremath, van der Heijden, van Evert, Stein, & Braak, 2014, Hiremath, van Evert, Stein, & van der Heijden, 2014) shows the need for a reliable and fast mapping algorithm that could lead to completely autonomous agricultural systems. Hiroi and Niitsuma, (2012) describe a map building approach in dynamic environments. They use LRF in an environment including human presence. Their work is also interesting from the agricultural point of view, since changing conditions can be present on the field, as well, for example, due to wind. They proved that the method could correctly detect static and moving landmarks. Chung, Hou, and Lu (2013) developed map building system, where the authors use distance differences were used, which were taken from the LRF measurements. The authors presented a mapping experiment with adaptable threshold parameters that affected map building performance. Auat Cheein, Steiner, Perez Paina, and Carelli (2011) developed a precision SLAM algorithm based on extended information filter (EIF-SLAM) for agricultural environments, in their case olive groves. The most significant features in the agricultural environment were used for the optimisation and pose estimation process. The algorithm was shown to be robust and the resulting map consistent with the environment. Hiremath, van der Heijden et al. (2014) and Hiremath, van Evert, Stein et al. (2014) pointed out the disadvantages of visual mapping in fields. The mapping algorithm was built by a probabilistic navigation method based on a particle filter. The authors claimed that mapping with a particle filter enables robust robot navigation in agricultural environments such as a maize field. This paper is organised as follows. An existing mobile robot setup and its hardware system are described in order to acquaint the reader with the agricultural mobile robot that is used for map building. The third section summarises the details of the proposed approach. In the fourth section, the results our mapping method and the Hector mapping algorithm are compared using pre-recorded data about the environment. In the last section a conclusion is drawn with suggestions for potential possible future improvements.
made up the sensory part of the robot. The electronics of the robot included an embedded computer, a peripheral circuit and a power supply. An image of the robot is shown on the left side of Fig. 2, showing all the crucial components of the robot, while the right side of the figure the robot is shown in action while spraying roses. The mechanical part of the robot was built as a four-wheel drive unit, which allowed it to move over rough terrain. It was driven by a high-performance, brushless motor X-power eco A4130-06BL (Der Schweighofer, Deutschlandsberg, Austria), controlled by an x-power professional 70-3P BEC controller. The motor and the controller were powered by a two-cell LiPo battery pack capable of providing a 75 A constant and 100 A peak current, with 7.4 V voltage and a 5000 mAh capacity. The speed of the motor is 510 rpm per Volt (without gears) which droves the robot with a top speed of 2 m per second (through gears). Two electromagnetic valves, nozzles and a reservoir, coupled with PVC pipes, offered the possibility of spraying plants in selected locations. When plants were not affected by disease, or were simply missing from the crop row, the valves were shut, and they opened only when the robot identified a weed or passed a potential area in need of spraying. The electronic part of the robot was made up of an embedded computer, Nitrogen6x (Boundary Devices Inc., Arizona, USA), and a peripheral expansion board built around an AVR ATmega 128 microcontroller (Farnell, Leeds, UK). The computer provided a number of ports. A USB was used for the camera and laser range sensors, while an UART port was used to communicate with the expansion board. Ubuntu Linux (Linaro) was selected for the operating system and was uploaded to an SD card, where a custom version of the kernel was compiled to support the USB-based camera.
2.1.
The sensory part of the robot includes an industrial camera DBK31BU03 (The Imaging Source Europe GmbH, Bremen, Germany) and an LRF TIM 310 (SICK, Ljubljana, Slovenia). The camera captured Bayer encoded images with a resolution of 1024 768 pixels at 30 fps, and the laser range sensor captures distances between a sensor and an obstacle in a 270 radius from 0.05, up to 4 m distance. The sampling frequency of the laser scanner is 15 Hz with 1 resolution. Both sensors are connected to the embedded computer via USB connection.
2.2.
2.
Mobile robot sensors
Robotic operating system
Mobile robot
The mobile robot was built as an agricultural test platform and consists of four major parts: mechanical components, actuators, embedded electronics and sensors. The mechanical parts include a four-wheel drive, two axles that can be turned individually by servomotors, an automatic gearbox with forward and reverse two-speed transmission and a three-phase motor. The actuators included a pressurised reservoir, two electro-magnetic valves and nozzles that enable the robot to spray. The robot could carry a range of tools such as an automatic fertiliser applicator or mechanical forks for weed removal. The on-board digital camera and a laser range sensor
In order to control the low-level hardware, the mobile robot used a robotic operating system (ROS) (Quigley et al., 2009), which is a meta operating system. ROS provides a number of libraries and tools for various robotic applications; it includes hardware drivers, device drivers, visualisers, message passing and package management as open source software. For the mobile robot, device drivers for the SICK laser scanner, camera and other hardware, provided by ROS were used. In order to connect the embedded computer with the mobile base, a custom program was created for command interpretation and communications protocol. All software and dataset handling was done in ROS.
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
163
Fig. 2 e Two images of the robot. Left; All crucial components and, right; the mobile robot in action spraying roses.
3.
The algorithm
The main focus was to build a map of the environment, based only on measurements from the LRF. The scan area had to be parallel to the earth's horizon in order to provide a 2D projection of a given environment. Only LRF is used in this work, with no other sensors. We choose only to use LRF alone because of the errors that can be produced using wheel odometry is used. The cost of the required hardware setup was considered. Figure 3 shows the procedure for taking measurements in a cropped line with LRF from a bird's eye view, where lrfdata are the acquired distances based on the angleðiÞ. The red dot represents an LRF, and the green areas are the plants in a crop row. The mapping approach was based on phase correlation (described in Section 3.1), LRF laser data filtering (described in Section 3.2) and transformations (for linear and rotational pose estimation) described in the following paragraphs. Pose estimation was calculated as shown in Subsection 3.3.
3.1.
Phase correlation
To calculate displacement between each consecutive measurement, phase correlation was chosen as described in
Gonzales and Woods (2001) and Stone (2001). The correlation compared two images, where the first was a reference and the subsequent one was a template. Since the measurements were semi-identical, they had almost the same energy distribution. The correlation reached a maximum value when both images were registered. In addition, if the correlation was computed in the frequency domain, the translation and rotation were separated and presented in the phase (translation) and amplitude (rotation) spectra, without having effecting each other as in the spatial domain. Hence the problem of finding the right translation and/or rotational parameters could be solved separately. The translation of the semi-identical signals in spatial space only affects the phase spectrum of the frequency transform. As the signals are semi-identical their phase compensates but preserves their linear part. This produces a maximum global value with coordinates (cx ; cy ) that correspond to translation on the X and Y axes. If the log-polar transform is used before phase correlation, phase correlation can also be used to calculate rotation. To calculate the correlation the following equation is applied: I1 ðx; yÞ I2 ðx; yÞ ¼
1 X N1 X 1 M I* ðm; nÞI2 ðx þ m; y þ nÞ MN m¼0 n¼0 1
Fig. 3 e LRF scan presentation from a bird's eye perspective.
(1)
164
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
where I1(x,y) and I2(x,y) represent a 2D signal (image) with M x N pixels in size and I1* a complex conjugate of I1. By applying the convolution theorem, Eq. (1) can be written as: I1 ðx; yÞ I2 ðx; yÞ⇔S*1 ðu; vÞS2 ðu; vÞ
(2)
where S*1 and S2 correspond to frequency transforms of the input signals I1 and I2. Based on the convolution theorem the correlation in frequency domain can simply be calculated by multiplying the complex conjugate of the frequency transform of the first by the frequency transform of the second signal. By transforming measurements, such as the one from Fig. 3, and depicting them in the spatial domain, the consecutive reading shows up as translated outlines of the objects in the scene. Once these displacements were calculated with the help of Eq. (2), they were used to calculate real displacements on both sides. Both displacements were then simply used to compute new vectors with an origin starting at (0,0) located at the beginning of the row, approximately half the distance from each side of the row. Both vectors are then used in simple vector summation which revealed the true movement vector, that is the difference along the X and Y axes, along with the new coordinates of the origin for new iteration.
3.2.
Data filtering
In real environments the distance between the crop rows may vary, and if the robot is located too close to one side, it can pick up distance readings from the neighbouring row. This can be eliminated in some cases by setting the maximum valid distance reading in ROS's SICK laser finder node. However, in order to eliminate it completely, a pre-processing filtering step is recommended. A statistical approach was chosen to eliminate this kind of reading. By calculating a median distance for each crop row, along with the first and fifth sixtiles, an interval of valid readings was computed. It comprised of 66.66% of all readings that were closest to the median and. Since the majority of the distances in all readings belong to the first row, the marginal distances were filtered out. Of course, some valid readings were also filtered out, but this had no negative effect on the calculations.
3.3.
Position and rotation estimation
This section describes our approach to mapping crop rows. Different crop plant lines can have versatile structures that produce very different environmental conditions. Our approach to this problem was based on image registration, where phase correlation as described in Section 3.1 was chosen. It computed the information about the template position on a corresponding original image. Our goal was to estimate an angular shift in successive measurements that correspond to the differences in translation and rotation on images. These two problems were split, we needed to calculate phase correlation separately for estimation of position and orientation. Therefore, two different data scan transformations between two distinct successive readings were required. Firstly, the laser data transformation, which is essential preparation for the phase correlation that is used to estimate the pose, is described.
3.3.1.
Laser data transformation
This transformation adds an additional dimension to the laser scan data. The first transformation from LRF scans to a 2D image was done as shown in Fig. 4, which is a projection of laser scan data into a two-dimensional scaled grid map. Equation (5) describes the grid map transformation for linear movement estimation. Transformation was performed for every measured distance set captured by the LRF, where only valid data was considered that was greater than 0 and smaller than the maximum range of the LRF. For each piece of LFR data, the xl and yl coordinates of the 2D image were calculated (see Fig. 4 upper). The same conditions were considered for rotational estimation, where for the rotational estimation the data scan transformation was performed as can be seen in Fig. 4 below, and the whole transformation was described by Eq. (7). Laser data transformation uses threshold limits, which are defined by user settings. Users can define the maximum and the minimum laser range, which should be LRF specific and depending on the environment.
xl yl
2 6 ¼6 4
3 roundððcosðbÞÞ*lrf ðiÞÞ þ Iw 7 res 7 5 roundððsinðbÞÞ*lrf ðiÞÞ þ Ih res
b ¼ deg2radði þ 45Þ
(5)
(6)
Pose estimation is linear translation dependant and it was calculated with the help of a parameter that represents a current laser beam angle. To obtain the correct orientation of the LRF sensor, 45 was added. In our case, a 270 degree LRF measurement could be obtained, but 180 was used, owing to occluded areas where the wheels of the robot obscured the view. The number of readings from the LRS depended on the hardware. In our case, the system had an LRF with 1 degree angular resolution, which meant that variable i, was actually an angle presented in degrees. Each angle i had a corresponding distance measurement noted with lrf ðiÞ. The parameter res is the resolution of an image I with a size ofðIw ; Ih Þ: For angular estimation, xa and ya image positions were calculated, where the parameter rangemax was used to scale the given LRF range into an image, which had a size twice the LRF angular resolution. The y component was presented as angle data.
xa ya
3 roundðlrf ðiÞ*Ih Þ 7 6 rangemax ¼4 5 2
(7)
i
By applying Eq. (5) and Eq. (7) to the original data sets, produced by the LRF, two 2D images were constructed, and two more for the next measured data sets. The set of images used to calculate the translations was scaled to 1024 1024 pixels, corresponding to a 0.07 m resolution. The resolution was based on empirical tests where images were interpolated to gain sub-pixel accuracy, with as little effect as possible on the execution times of phase correlation. To determine the difference in rotation, Eq. (7) was used to produce the other two images. This time given the angular resolution of the LRF, the original resolution was used, which was 270 270 pixels and LRF type dependant.
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
165
Fig. 4 e An example of two successive scans that were transformed into 2D space, as binary images, where the measurements are depicted in Euclidian space (upper pair of images), and an example of two successive scans depicted as angle (horizontal axis) vs. distance (vertical axis) (lower pair of images).
3.3.2.
Pose estimation
The first pair of images was used as a reference and a template (Fig. 4 e upper pair), with almost the same content but with present translation that corresponds to the angular shift from the original data sets. The first image was used as a reference and part of the other (successive measurement) as a template. In order to guarantee the same content in both images, the template was cropped; in our case, the template image was cropped by 100 pixels on each side. This was more than enough, as shifts larger than 50 pixels were not expected. Usually the shifts were very small since the laser scanner generates new data rapidly, up to 15 Hz. The result of the correlation were presented as a position on the base image, which was converted to an image shift based on the centre of the base image, because measurements were transformed on the 2D image relative to the image centre. For a small template shift, Eq. (8), as shown, was used:
c þ ðWh Iw Þ=2 dx ; ¼ x cy þ ðWh Ih Þ=2 dy
(8)
where W is a template image that has a size of ðWw ; Wh Þ pixels. To calculate an angular shift, a similar approach can be used. Instead of converting the reference and a template to log-polar space and then using the phase correlation, the measurements were transformed to (r, theta) space. In this way, the angular shifts showed up as translational shifts that
were calculated with the help of phase correlation. Equation (9) shows how the current angular shift was calculated: qc ¼ cy þ ðWh Ih Þ=2;
(9)
where cy is the result of phase correlation. Because resolution of the LRF was limited, that some readings might not have their pair in the next iteration. This can cause some additional errors, which can be minimised if an additional step in image processing is applied. To do so, dilatation (Gonzales & Woods, 2001) was used on both image pairs, which stretches the neighbouring regions, so the readings in question in most cases had their pair in the next iteration. Figure 5 shows an example of phase correlation's intermediate result. The white rectangular border is a template that has been matched with the original image. The relative differences calculated by Eq. (10) and Eq. (11) were calculated based on the centre position of the window. The previous step of phase correlation returned information regarding the shifts on both axes as well as the shift in angle that corresponds to rotation. The translation was then used to determine the movement of the robot, which was calculated by using the following equations: X dx*Iw =2 (10) ux ¼
166
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
movements were based on phase correlation as described in Section 3.1., where different data sets were used.
3.4.
Map building
To complete the SLAM functionality, a map must be built. A map has a standard form as in ROS (Quigley et al., 2009), where different colours represent distinct environmental states. Dark grey is an unknown space, light grey is known space, and black locations are obstacles or landmarks. The map can be updated each time a position update is available, if needed. Laser distances were projected on a 2D plane considering translation and rotation; this meant that the new locations on the map were relative to the current robot position. An ROS publisher calculated a map update every 5 s in order to provide the latest available map.
4. Fig. 5 e An example of a template (white border rectangle) that is compared with an original image.
uy ¼
X
dy*Ih =2
(11)
The robot or LRF pose is represented in (ux ; uy ), where ðdx; dy) are calculated shifts in two dimensions. The result can have a positive or a negative value, the former means that the robot moved forward and the latter that the robot moved backwards. By summing up all the small changes from the original a path, it can be seen how the robot moves in 2D space. Multiplying the result with the map resolution provides a metric result. In order to compute the robots final orientation, Eq. (12) was used, as shown: q¼
X
qc
(12)
Current robot rotation was estimated in qc which is given in degrees, and the q represented the result of current robot orientation. In order to compute the final orientation, all temporary orientations were summed together, that were computed as rotational differences between the second two images from Eq. (12). Both translation and rotation
Experimental results
In order to test the algorithm, crop rows were recorded and stored as rosbag data format. These datasets were used to evaluate our approach and compare it to the results of the Hector mapping algorithm. The algorithm comparison was performed on the same recorded datasets. The following subsection describes in detail the experimental setup, as well as the results of the mapping for both procedures.
4.1.
Experimental setup
To objectively grade the algorithms, a set of data recordings was captured. They were made in an apple orchard and a vineyard, as both represent challenging complex structures. Eight different recordings were captured, four in the apple orchard and four between the vineyard rows. In both cases, the first two were done at 16 m and 32 m long runs. The first of each pair was captured at 0.4 m from the ground and the other two 1.2 m from ground, with the aim of evaluating the influence of plant structure on the output. Figure 6 shows the apple crop rows on the left, and on right, the vineyard crop rows where the experimental data sets were taken. By recording data sets, we assured that in all subsequent tests, comparing Hector mapping and the present algorithm, the input data was precisely the same. The quality of the algorithms could then be simply evaluated by comparing the
Fig. 6 e Left, apple tree crop row, and, right, vineyard crop row where the experimental data was recorded.
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
167
Fig. 7 e Maps produced by Hector mapping (left) and the present algorithm (right) in apple crop rows. The environmental setup, from first to last as follows; 1. row: 16 m long, 0.4 m high, 2. row: 16 m long, 1.2 m high, 3. row: 32 m long, 0.4 m high and 4 row: 32 m long, 1.2 m high.
Fig. 8 e Map produced by Hector mapping (left) and the present algorithm (right) in vineyard crop rows. The environmental setup from first to last as follows; 1. row: 16 m long, 0.4 m high, 2. row: 16 m long, 1.2 m high, 3. row: 32 m long, 0.4 m high and 4 row: 32 m long, 1.2 m high.
168
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
output values and comparing them to referential ones that were measured manually when recording the data sets.
4.2.
Mapping results
The recorded data sets were tested with Hector mapping and compared to the results produced by the present algorithm. In both examples, the map of the environment was based on the LRF measurements depicted by the output maps in Fig. 7, for the apple tree rows, and in Fig. 8, for the vineyard rows. The black dots on the maps are the plants, and the light grey area is free space between the crop rows, where the robot can move. In all cases the distance was also measured manually and compared to the distance in both algorithms for evaluation.
Table 1 gives an overview of the mapping results. The results of Hector mapping are summarized in the second to the fifth columns, and the results for our algorithm, in columns six to nine. As shown, the presented algorithm was less accurate where measurements were taken of tree trunks, due to the low density of the readings. It performed better in a vineyard environment, as with the Hector mapping. When comparing the two methods, the algorithm produced better results overall. Error was calculated based on the whole combined area, that including 192 m of crop rows. For large agricultural environments, this represents a small error and is low enough to accurately locate the robot between the crop rows. When examining the Hector mapping algorithm's results from Table 1, Figs. 7 and 8, it can be seen that Hector mapping failed in
Table 1 e Comparison between the manual and automatic distance measurements. Plant distance/Height
Apple 16 m 0.4 m Apple 16 m 1.2 m Apple 32 m 0.4 m Apple 32 m 1.2 m Vineyard 16 m 0.4 m Vineyard 16 m 1.20 m Vineyard 32 m 0.4 m Vineyard 32 m 1.2 m Absolute average error [m]
Hector Translation Hector Rotational Presented Translation Presented Rotational mapping absolute mapping absolute algorithm [m] absolute algorithm absolute [m] error [m] [deg] error [deg] error [m] [deg] error [deg] 3.72 15.29 0.71 13.50 16.48 14.31 32.17 31.87
12.28 0.71 31.29 18.50 0.48 1.69 0.17 0.13 8.1 ± 11.6
0 13 144 12 0 7 33 0
0 13 144 12 0 7 39 0 26.8 ± 49.04
12.73 15.80 31.79 31.68 15.54 15.2 32.06 31.07
0.72 0.2 0.21 0.22 0.46 0.8 0.06 0.93 0.45 ± 0.33
3 6 18 19 11 6 16 6
3 6 18 19 11 6 16 6 10.62 ± 6.3
Fig. 9 e A picture of the ROVITIS robot (upper image), the map built by the present algorithm (bottom left image) and the map from Hector mapping (bottom right image).
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
several scenarios and reached an overall 8.1 m ± 11.6 m for translation and 26.8 ± 49.04 for rotation. The algorithm, on the other hand, was accurate in all cases and produced a maximum of 0.54 m of error on a 16 m long run, with a best performance of 0.2 m error while estimating position. When estimating the distance error, the angle was calculated by both methods. The accuracy of rotation proved to be a challenge for both Hector and the algorithm, with an average error of around 10 . This was because of the complex structure of the environment as well as a lack of good reference points where the rows ended. Results were acquired by testing the algorithm on another, bigger agricultural robot e ROVITIS (Pantano, 2014), with the aim of further testing the utility of the algorithm. In contrast to the platform described in Section 2, this platform was diesel powered rather than electric and could produce vibrations that affect the results. Another factor is the spraying application, which might also influence the performance. Again, the datasets were recorded, so they could be tested and the results compared from the present algorithm and for Hector mapping. Figure 9 shows the robot as well as both sets of results. To test the algorithms, the ROVITIS mobile robot was used with a spraying application on 60 m long vineyard rows. The LRF data scans were recorded and maps for both SLAM algorithms were constructed. The present algorithm calculated 58.89 m in length, while the Hector mapping managed only 12.86 m.
5.
Conclusions
The aim of this study was able to provide stable and reliable information about the robot's position while moving in parallel with crop rows. The mobile robot's autonomy in agriculture is based on a map that needs to be exact in order to provide localisation. The method depended only on the information from the laser data because LRF provided exact distance information. The distance measurements in the crop rows detect landmarks, which are used to iteratively determine the robot's new location. Therefore, the LRF's limitations need to be considered. We believe that this study will contribute to the field of autonomous robot applications in agriculture. Currently we see this algorithm as a basis for field navigation for applications such as crop assessment, missing plant detection, poor crop location detection and weed detection. Future studies need to combine this algorithm with visual SLAM, where a classic LRF can be used as a 3D scanner. The LRF needs to be mounted on a tilt mechanism, which, given the right time sequence, can provides a 3D image of an environment that might produce even better results.
Acknowledgement We would like to thank Dr. Giorgio Pantano for letting us use the ROVITIS robot to test and evaluate our algorithm. The authors also acknowledge the vital contributions made by Dr. Michelle Gadpaille by proofreading the manuscript.
169
references
Auat Cheein, F., Steiner, G., Perez Paina, G., & Carelli, R. (2011). Optimized EIF-SLAM algorithm for precision agriculture mapping based on stems detection. Computers and Electronics in Agriculture, 78, 195e207. Barawid, O. C., Jr., Mizushima, A., Ishii, K., & Noguchi, N. (2007). Development of an autonomous navigation system using a two-dimensional laser scanner in an orchard application. Biosystems Engineering, 96(2), 139e149. Chung, H. Y., Hou, C. C., & Lu, Z. R. (2013). Wheeled robot navigation and local-map building in unknown environments. In CACS international automatic control conference (pp. 350e355). Gonzales, R. C., & Woods, R. E. (2001). Digital image processing (2nd ed.) Prentice Hall, Upper Saddle River, New Jersey. Grisetti, G., Stachniss, C., & Burgard, W. (2005). Improving gridbased SLAM with Rao blackwellized particle filters by adaptive proposals and selective resampling. In Proceedings of the IEEE international conference on robotics and automation (pp. 2432e2437). Grisetti, G., Stachniss, C., & Burgard, W. (2007). Improved techniques for grid mapping with rao-blackwellized particle filters. IEEE Transactions in Robotics, 23, 34e46. Hiremath, A. S., van der Heijden, G. W. A. M., van Evert, F. K., Stein, A., & Braak, C. J. F. (2014). Laser range finder model for autonomous navigation of a robot in a maize field using a particle filter. Computers and Electronics in Agriculture, 100, 41e50. Hiremath, S., van Evert, F. K., Braak, C., Stein, A., & van der Heijden, G. (2014). Image-based particle filtering for navigation in a semi-structured agricultural environment. Biosystems Engineering, 121, 85e95. Hiroi, S., & Niitsuma, M. (2012). Building a map including moving objects for mobile robot navigation in living environments. In IEEE networked sensing systems (INSS) international conference, Antwerp (pp. 1e2). Kohlbrecher, S., Meyer, J., Graber, T., Petersen, K., Klingauf, U., & Stryk, O. (2013). Hector open source modules for autonomous mapping and navigation with rescue robots. TU Darmstadt, Germany: Department of Computer Science. Kohlbrecher, S., Meyer, J., von Stryk, O., & Klingauf, U. (2011). A flexible and scalable SLAM system with full 3D motion estimation. In Proceedings of the 2011 IEEE international symposium on safety, security and rescue robotics, Japan (pp. 155e160). Ohno, K., Tsubouchi, T., Shigematsu, B., Maeyama, S., & Yuta, S. (2003). Outdoor navigation of a mobile robot between buildings based on DGPS and odometry data fusion. In Proceedings. ICRA '03. IEEE international conference on robotics and automation, 2 (pp. 1978e1984). Oksanen, T., Linja, M., & Visala, A. (2005). Low-cost positioning system for agricultural vehicles. In International symposium on computational intelligence in robotics and automation (pp. 297e302). Pantano, G. (2014, 08, 21). Rovitis e Robotics in viticulture. Retrieved from: http://www.exponentialtimes.net/videos/rovitisrobotics-viticulture. Quigley, M., Conley, K., Gerkey, B., Faust, J., Foote, T., Leibs, J., et al. (2009). ROS: An open-source robot operating system. ICRA Workshop on Open Source Software, Willow Garage, Menlo Park, California. ROS hector_slam software stack (17th of April 2014). Retrieved from: http://wiki.ros.org/hector_slam. Accessed 2014 April 18. Stone, H. S. (2001). A fast direct fourier-based algorithm for subpixel registration of images. IEEE Transactions on Geoscience and Remote Sensing, 39(10), 2235e2242. Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic robotics. Cambridge, USA: The MIT Press.
Available online at www.sciencedirect.com
ScienceDirect journal homepage: www.elsevier.com/locate/issn/15375110
Research Paper
Simultaneous localisation and mapping in a complex field environment Peter Lepej a,*, Jurij Rakun b a b
Faculty of Electrical Engineering and Computer Science, Smetanova ul. 17, Maribor, Slovenia Faculty of Agriculture and Life Sciences, Pivola 10, Hoce, Slovenia
article info
The usefulness of image registration techniques in mapping and localising a robot in an
Article history:
agricultural environment by using readings from a laser range scanner was investigated.
Received 4 October 2015
The proposed approach used frequency domain and correlation. Translational and rota-
Received in revised form
tional differences that occur between successive readings of the scanner and that corre-
21 June 2016
spond to the movement of the robot were used. The approach was tested on 9 test runs,
Accepted 2 August 2016
with a total of 252 m in length, recorded in an apple orchard and in a vineyard. The results
Published online 20 August 2016
were then compared to results from the Hector mapping algorithm. It was shown that the present approach performed very well compared to Hector mapping. On average achieved
Keywords:
an 4.24% ± 2.9% error rate and the present approach 0.16% ± 0.1%. Hector mapping on the
Image processing
other hand proved better in cases where rotational differences were looked for, reaching an
Distance transformation
error rate of 1.69% ± 0.7% in comparison to present approach with an error rate of
Phase correlation
4.19% ± 3.1%. © 2016 IAgrE. Published by Elsevier Ltd. All rights reserved.
SLAM Agriculture Mobile robot
1.
Introduction
As in other research areas, mobile robot applications are rapidly expanding into different areas of agriculture. Tasks such as arable farming require standard, periodic operations that are increasing in demand. The high demand for diverse food products has led food producers to use a wide range of fertilisers and pesticides in order to achieve optimal crop growth, but they have a negative impact on the environment. In order to minimise unwanted, potentially damaging effects, agricultural tasks should be examined and carried out more precisely, selectively and with care for the safety of people and the environment. This was one reason that we decided to
develop a mobile robot that could fit and operate between the crop rows of different agricultural cultures. The use of such mobile robots in agriculture relies on the hypothesis that they can selectively spray plants that need to be treated. The motivation is to use potentially dangerous pesticides as little as possible and to make farming more nature friendly. In addition, such robots could carry out a number of other tasks, such as mechanical weed killing, selective fertilisation, crop assessment, plant damage assessment, detection pest, after spraying or even during the process. Mobile robots that can perform these tasks need to be independent; this means there should be no intermediate control of human intervention in an individual robot's performance. In addition, a fleet of such robots should be able
* Corresponding author. E-mail addresses: [email protected] (P. Lepej), [email protected] (J. Rakun). http://dx.doi.org/10.1016/j.biosystemseng.2016.08.004 1537-5110/© 2016 IAgrE. Published by Elsevier Ltd. All rights reserved.
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
Nomenclature lrfdata angleðiÞ I1 I2 M N I1* S1 S2 res rangemax W dx dy qc ux ux q
an array of laser range finder readings, mm i-th angle of the lrf reading, first 2D signal e an reference, second, successive 2D signal e a template, the width of signal I, the height of signal I, a complex conjugate of I1, frequency transforms of signal I1, frequency transforms of signal I2, resolution of an image, pixels the scaling factor used in interpolation step, a template image with Ww Wh in size, pixels temporal shift along the X axis, pixels temporal shift along the Y axis, pixels temporal angular shift, position of the robot along X axis, m position of the robot along Y axis, m orientation of the robot,
to form a swarm and work towards a common collective goal, reaching it faster, making the work more precise, while yielding lower production costs. This idea is based on an autonomous field robot system that is able to perform given tasks. Such as basic autonomous system for a mobile field robot is shown in Fig. 1. Localisation and mapping are the means by which it is able to achieve autonomy. Mapping is a procedure indicating out the affected areas and present observations on 2D or 3D maps by taking into account laser range finder (LRF) readings. Here the focus is on building a reliable, odometry free mapping system. Mapping is used in autonomous systems at the medium level (Fig. 1) and provides the system with information about the environment. High control levels take care of all basic application controls, whereas the low level represents the drivers for actuators and sensors. The system takes decisions based on the given environmental status in the form of a map. Odometry is not considered in this study because wheel encoders can produce a lot of undesirable error caused by complex ground structure.
161
Here the focus is not on a low-cost sensor such as wheel encoders, because of the common problems described in Ohno, Tsubouchi, Shigematsu, Maeyama, and Yuta (2003). Oksanen, Linja, and Visala (2005) discussed a low-cost system for outdoor positioning which cannot only rely on odometry. For this reason, the Hector mapping algorithm from TU Darmstadt (Kohlbrecher et al., 2013), since it does not require additional information from odometry. The initial tests of Hector mapping were conducted on ordinary, complex crop rows and they showed that in some cases the result is incorrect and mapping fails. This drove us to develop a new approach for complex structure mapping in natural environments with the goal of achieving the best possible precision on larger areas of crop rows. The algorithm provides a map of the current environment and localisation based on a reference coordinate system that is established at the start of the algorithm. This approach was compared to a Hector mapping algorithm by using the same data sets. Localisation and mapping is commonly known as the SLAM (simultaneous localisation and mapping) algorithm (Thrun, Burgard, & Fox, 2005). Widely used algorithms for localisation and mapping include Hector mapping and Gmapping. Gmapping is a standard method described in Grisetti, Stachniss, and Burgard (2007) and Grisetti, Stachniss, and Burgard (2005), which is based on use of a RaoBlackwellised particle filter to create a map and provide localisation information. The method's purpose is to decrease the number of particles with filtering, where precise calculation and exact deviation are considered. It uses the latest information provided by a laser scanner, reduces locational uncertainty and predicts the robot's position in the environment. Gmapping is optimised for long-range laser scanners and environments with large areas. In order to function, it needs laser scan data and, for best results additional odometry. In contrast to Gmapping, Hector mapping as described in Kohlbrecher et al. (2013), Kohlbrecher, Meyer, von Stryk, and Klingauf (2011) and hector_slam software package (2014) is optimised for narrow corridors and the kind of complex environment found in rescue scenarios. Hector mapping does not need additional information regarding odometry and this does not affect its performance. For localisation, it uses the scan-matching method and a Gauss-Newton filter to define
Fig. 1 e Overview of a mobile robot autonomous system.
162
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
the robot's current position based on particle distribution. It also considers a multi-layer map and a map server to save all available maps and trajectories. Hector mapping is a very efficient and reliable algorithm, which can be used for handheld devices to map certain areas. State of the art studies of an autonomous guidance system based on a mapping algorithm as described in Barawid, Mizushima, Ishii, and Noguchi (2007) or computer vision (Hiremath, van der Heijden, van Evert, Stein, & Braak, 2014, Hiremath, van Evert, Stein, & van der Heijden, 2014) shows the need for a reliable and fast mapping algorithm that could lead to completely autonomous agricultural systems. Hiroi and Niitsuma, (2012) describe a map building approach in dynamic environments. They use LRF in an environment including human presence. Their work is also interesting from the agricultural point of view, since changing conditions can be present on the field, as well, for example, due to wind. They proved that the method could correctly detect static and moving landmarks. Chung, Hou, and Lu (2013) developed map building system, where the authors use distance differences were used, which were taken from the LRF measurements. The authors presented a mapping experiment with adaptable threshold parameters that affected map building performance. Auat Cheein, Steiner, Perez Paina, and Carelli (2011) developed a precision SLAM algorithm based on extended information filter (EIF-SLAM) for agricultural environments, in their case olive groves. The most significant features in the agricultural environment were used for the optimisation and pose estimation process. The algorithm was shown to be robust and the resulting map consistent with the environment. Hiremath, van der Heijden et al. (2014) and Hiremath, van Evert, Stein et al. (2014) pointed out the disadvantages of visual mapping in fields. The mapping algorithm was built by a probabilistic navigation method based on a particle filter. The authors claimed that mapping with a particle filter enables robust robot navigation in agricultural environments such as a maize field. This paper is organised as follows. An existing mobile robot setup and its hardware system are described in order to acquaint the reader with the agricultural mobile robot that is used for map building. The third section summarises the details of the proposed approach. In the fourth section, the results our mapping method and the Hector mapping algorithm are compared using pre-recorded data about the environment. In the last section a conclusion is drawn with suggestions for potential possible future improvements.
made up the sensory part of the robot. The electronics of the robot included an embedded computer, a peripheral circuit and a power supply. An image of the robot is shown on the left side of Fig. 2, showing all the crucial components of the robot, while the right side of the figure the robot is shown in action while spraying roses. The mechanical part of the robot was built as a four-wheel drive unit, which allowed it to move over rough terrain. It was driven by a high-performance, brushless motor X-power eco A4130-06BL (Der Schweighofer, Deutschlandsberg, Austria), controlled by an x-power professional 70-3P BEC controller. The motor and the controller were powered by a two-cell LiPo battery pack capable of providing a 75 A constant and 100 A peak current, with 7.4 V voltage and a 5000 mAh capacity. The speed of the motor is 510 rpm per Volt (without gears) which droves the robot with a top speed of 2 m per second (through gears). Two electromagnetic valves, nozzles and a reservoir, coupled with PVC pipes, offered the possibility of spraying plants in selected locations. When plants were not affected by disease, or were simply missing from the crop row, the valves were shut, and they opened only when the robot identified a weed or passed a potential area in need of spraying. The electronic part of the robot was made up of an embedded computer, Nitrogen6x (Boundary Devices Inc., Arizona, USA), and a peripheral expansion board built around an AVR ATmega 128 microcontroller (Farnell, Leeds, UK). The computer provided a number of ports. A USB was used for the camera and laser range sensors, while an UART port was used to communicate with the expansion board. Ubuntu Linux (Linaro) was selected for the operating system and was uploaded to an SD card, where a custom version of the kernel was compiled to support the USB-based camera.
2.1.
The sensory part of the robot includes an industrial camera DBK31BU03 (The Imaging Source Europe GmbH, Bremen, Germany) and an LRF TIM 310 (SICK, Ljubljana, Slovenia). The camera captured Bayer encoded images with a resolution of 1024 768 pixels at 30 fps, and the laser range sensor captures distances between a sensor and an obstacle in a 270 radius from 0.05, up to 4 m distance. The sampling frequency of the laser scanner is 15 Hz with 1 resolution. Both sensors are connected to the embedded computer via USB connection.
2.2.
2.
Mobile robot sensors
Robotic operating system
Mobile robot
The mobile robot was built as an agricultural test platform and consists of four major parts: mechanical components, actuators, embedded electronics and sensors. The mechanical parts include a four-wheel drive, two axles that can be turned individually by servomotors, an automatic gearbox with forward and reverse two-speed transmission and a three-phase motor. The actuators included a pressurised reservoir, two electro-magnetic valves and nozzles that enable the robot to spray. The robot could carry a range of tools such as an automatic fertiliser applicator or mechanical forks for weed removal. The on-board digital camera and a laser range sensor
In order to control the low-level hardware, the mobile robot used a robotic operating system (ROS) (Quigley et al., 2009), which is a meta operating system. ROS provides a number of libraries and tools for various robotic applications; it includes hardware drivers, device drivers, visualisers, message passing and package management as open source software. For the mobile robot, device drivers for the SICK laser scanner, camera and other hardware, provided by ROS were used. In order to connect the embedded computer with the mobile base, a custom program was created for command interpretation and communications protocol. All software and dataset handling was done in ROS.
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
163
Fig. 2 e Two images of the robot. Left; All crucial components and, right; the mobile robot in action spraying roses.
3.
The algorithm
The main focus was to build a map of the environment, based only on measurements from the LRF. The scan area had to be parallel to the earth's horizon in order to provide a 2D projection of a given environment. Only LRF is used in this work, with no other sensors. We choose only to use LRF alone because of the errors that can be produced using wheel odometry is used. The cost of the required hardware setup was considered. Figure 3 shows the procedure for taking measurements in a cropped line with LRF from a bird's eye view, where lrfdata are the acquired distances based on the angleðiÞ. The red dot represents an LRF, and the green areas are the plants in a crop row. The mapping approach was based on phase correlation (described in Section 3.1), LRF laser data filtering (described in Section 3.2) and transformations (for linear and rotational pose estimation) described in the following paragraphs. Pose estimation was calculated as shown in Subsection 3.3.
3.1.
Phase correlation
To calculate displacement between each consecutive measurement, phase correlation was chosen as described in
Gonzales and Woods (2001) and Stone (2001). The correlation compared two images, where the first was a reference and the subsequent one was a template. Since the measurements were semi-identical, they had almost the same energy distribution. The correlation reached a maximum value when both images were registered. In addition, if the correlation was computed in the frequency domain, the translation and rotation were separated and presented in the phase (translation) and amplitude (rotation) spectra, without having effecting each other as in the spatial domain. Hence the problem of finding the right translation and/or rotational parameters could be solved separately. The translation of the semi-identical signals in spatial space only affects the phase spectrum of the frequency transform. As the signals are semi-identical their phase compensates but preserves their linear part. This produces a maximum global value with coordinates (cx ; cy ) that correspond to translation on the X and Y axes. If the log-polar transform is used before phase correlation, phase correlation can also be used to calculate rotation. To calculate the correlation the following equation is applied: I1 ðx; yÞ I2 ðx; yÞ ¼
1 X N1 X 1 M I* ðm; nÞI2 ðx þ m; y þ nÞ MN m¼0 n¼0 1
Fig. 3 e LRF scan presentation from a bird's eye perspective.
(1)
164
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
where I1(x,y) and I2(x,y) represent a 2D signal (image) with M x N pixels in size and I1* a complex conjugate of I1. By applying the convolution theorem, Eq. (1) can be written as: I1 ðx; yÞ I2 ðx; yÞ⇔S*1 ðu; vÞS2 ðu; vÞ
(2)
where S*1 and S2 correspond to frequency transforms of the input signals I1 and I2. Based on the convolution theorem the correlation in frequency domain can simply be calculated by multiplying the complex conjugate of the frequency transform of the first by the frequency transform of the second signal. By transforming measurements, such as the one from Fig. 3, and depicting them in the spatial domain, the consecutive reading shows up as translated outlines of the objects in the scene. Once these displacements were calculated with the help of Eq. (2), they were used to calculate real displacements on both sides. Both displacements were then simply used to compute new vectors with an origin starting at (0,0) located at the beginning of the row, approximately half the distance from each side of the row. Both vectors are then used in simple vector summation which revealed the true movement vector, that is the difference along the X and Y axes, along with the new coordinates of the origin for new iteration.
3.2.
Data filtering
In real environments the distance between the crop rows may vary, and if the robot is located too close to one side, it can pick up distance readings from the neighbouring row. This can be eliminated in some cases by setting the maximum valid distance reading in ROS's SICK laser finder node. However, in order to eliminate it completely, a pre-processing filtering step is recommended. A statistical approach was chosen to eliminate this kind of reading. By calculating a median distance for each crop row, along with the first and fifth sixtiles, an interval of valid readings was computed. It comprised of 66.66% of all readings that were closest to the median and. Since the majority of the distances in all readings belong to the first row, the marginal distances were filtered out. Of course, some valid readings were also filtered out, but this had no negative effect on the calculations.
3.3.
Position and rotation estimation
This section describes our approach to mapping crop rows. Different crop plant lines can have versatile structures that produce very different environmental conditions. Our approach to this problem was based on image registration, where phase correlation as described in Section 3.1 was chosen. It computed the information about the template position on a corresponding original image. Our goal was to estimate an angular shift in successive measurements that correspond to the differences in translation and rotation on images. These two problems were split, we needed to calculate phase correlation separately for estimation of position and orientation. Therefore, two different data scan transformations between two distinct successive readings were required. Firstly, the laser data transformation, which is essential preparation for the phase correlation that is used to estimate the pose, is described.
3.3.1.
Laser data transformation
This transformation adds an additional dimension to the laser scan data. The first transformation from LRF scans to a 2D image was done as shown in Fig. 4, which is a projection of laser scan data into a two-dimensional scaled grid map. Equation (5) describes the grid map transformation for linear movement estimation. Transformation was performed for every measured distance set captured by the LRF, where only valid data was considered that was greater than 0 and smaller than the maximum range of the LRF. For each piece of LFR data, the xl and yl coordinates of the 2D image were calculated (see Fig. 4 upper). The same conditions were considered for rotational estimation, where for the rotational estimation the data scan transformation was performed as can be seen in Fig. 4 below, and the whole transformation was described by Eq. (7). Laser data transformation uses threshold limits, which are defined by user settings. Users can define the maximum and the minimum laser range, which should be LRF specific and depending on the environment.
xl yl
2 6 ¼6 4
3 roundððcosðbÞÞ*lrf ðiÞÞ þ Iw 7 res 7 5 roundððsinðbÞÞ*lrf ðiÞÞ þ Ih res
b ¼ deg2radði þ 45Þ
(5)
(6)
Pose estimation is linear translation dependant and it was calculated with the help of a parameter that represents a current laser beam angle. To obtain the correct orientation of the LRF sensor, 45 was added. In our case, a 270 degree LRF measurement could be obtained, but 180 was used, owing to occluded areas where the wheels of the robot obscured the view. The number of readings from the LRS depended on the hardware. In our case, the system had an LRF with 1 degree angular resolution, which meant that variable i, was actually an angle presented in degrees. Each angle i had a corresponding distance measurement noted with lrf ðiÞ. The parameter res is the resolution of an image I with a size ofðIw ; Ih Þ: For angular estimation, xa and ya image positions were calculated, where the parameter rangemax was used to scale the given LRF range into an image, which had a size twice the LRF angular resolution. The y component was presented as angle data.
xa ya
3 roundðlrf ðiÞ*Ih Þ 7 6 rangemax ¼4 5 2
(7)
i
By applying Eq. (5) and Eq. (7) to the original data sets, produced by the LRF, two 2D images were constructed, and two more for the next measured data sets. The set of images used to calculate the translations was scaled to 1024 1024 pixels, corresponding to a 0.07 m resolution. The resolution was based on empirical tests where images were interpolated to gain sub-pixel accuracy, with as little effect as possible on the execution times of phase correlation. To determine the difference in rotation, Eq. (7) was used to produce the other two images. This time given the angular resolution of the LRF, the original resolution was used, which was 270 270 pixels and LRF type dependant.
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
165
Fig. 4 e An example of two successive scans that were transformed into 2D space, as binary images, where the measurements are depicted in Euclidian space (upper pair of images), and an example of two successive scans depicted as angle (horizontal axis) vs. distance (vertical axis) (lower pair of images).
3.3.2.
Pose estimation
The first pair of images was used as a reference and a template (Fig. 4 e upper pair), with almost the same content but with present translation that corresponds to the angular shift from the original data sets. The first image was used as a reference and part of the other (successive measurement) as a template. In order to guarantee the same content in both images, the template was cropped; in our case, the template image was cropped by 100 pixels on each side. This was more than enough, as shifts larger than 50 pixels were not expected. Usually the shifts were very small since the laser scanner generates new data rapidly, up to 15 Hz. The result of the correlation were presented as a position on the base image, which was converted to an image shift based on the centre of the base image, because measurements were transformed on the 2D image relative to the image centre. For a small template shift, Eq. (8), as shown, was used:
c þ ðWh Iw Þ=2 dx ; ¼ x cy þ ðWh Ih Þ=2 dy
(8)
where W is a template image that has a size of ðWw ; Wh Þ pixels. To calculate an angular shift, a similar approach can be used. Instead of converting the reference and a template to log-polar space and then using the phase correlation, the measurements were transformed to (r, theta) space. In this way, the angular shifts showed up as translational shifts that
were calculated with the help of phase correlation. Equation (9) shows how the current angular shift was calculated: qc ¼ cy þ ðWh Ih Þ=2;
(9)
where cy is the result of phase correlation. Because resolution of the LRF was limited, that some readings might not have their pair in the next iteration. This can cause some additional errors, which can be minimised if an additional step in image processing is applied. To do so, dilatation (Gonzales & Woods, 2001) was used on both image pairs, which stretches the neighbouring regions, so the readings in question in most cases had their pair in the next iteration. Figure 5 shows an example of phase correlation's intermediate result. The white rectangular border is a template that has been matched with the original image. The relative differences calculated by Eq. (10) and Eq. (11) were calculated based on the centre position of the window. The previous step of phase correlation returned information regarding the shifts on both axes as well as the shift in angle that corresponds to rotation. The translation was then used to determine the movement of the robot, which was calculated by using the following equations: X dx*Iw =2 (10) ux ¼
166
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
movements were based on phase correlation as described in Section 3.1., where different data sets were used.
3.4.
Map building
To complete the SLAM functionality, a map must be built. A map has a standard form as in ROS (Quigley et al., 2009), where different colours represent distinct environmental states. Dark grey is an unknown space, light grey is known space, and black locations are obstacles or landmarks. The map can be updated each time a position update is available, if needed. Laser distances were projected on a 2D plane considering translation and rotation; this meant that the new locations on the map were relative to the current robot position. An ROS publisher calculated a map update every 5 s in order to provide the latest available map.
4. Fig. 5 e An example of a template (white border rectangle) that is compared with an original image.
uy ¼
X
dy*Ih =2
(11)
The robot or LRF pose is represented in (ux ; uy ), where ðdx; dy) are calculated shifts in two dimensions. The result can have a positive or a negative value, the former means that the robot moved forward and the latter that the robot moved backwards. By summing up all the small changes from the original a path, it can be seen how the robot moves in 2D space. Multiplying the result with the map resolution provides a metric result. In order to compute the robots final orientation, Eq. (12) was used, as shown: q¼
X
qc
(12)
Current robot rotation was estimated in qc which is given in degrees, and the q represented the result of current robot orientation. In order to compute the final orientation, all temporary orientations were summed together, that were computed as rotational differences between the second two images from Eq. (12). Both translation and rotation
Experimental results
In order to test the algorithm, crop rows were recorded and stored as rosbag data format. These datasets were used to evaluate our approach and compare it to the results of the Hector mapping algorithm. The algorithm comparison was performed on the same recorded datasets. The following subsection describes in detail the experimental setup, as well as the results of the mapping for both procedures.
4.1.
Experimental setup
To objectively grade the algorithms, a set of data recordings was captured. They were made in an apple orchard and a vineyard, as both represent challenging complex structures. Eight different recordings were captured, four in the apple orchard and four between the vineyard rows. In both cases, the first two were done at 16 m and 32 m long runs. The first of each pair was captured at 0.4 m from the ground and the other two 1.2 m from ground, with the aim of evaluating the influence of plant structure on the output. Figure 6 shows the apple crop rows on the left, and on right, the vineyard crop rows where the experimental data sets were taken. By recording data sets, we assured that in all subsequent tests, comparing Hector mapping and the present algorithm, the input data was precisely the same. The quality of the algorithms could then be simply evaluated by comparing the
Fig. 6 e Left, apple tree crop row, and, right, vineyard crop row where the experimental data was recorded.
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
167
Fig. 7 e Maps produced by Hector mapping (left) and the present algorithm (right) in apple crop rows. The environmental setup, from first to last as follows; 1. row: 16 m long, 0.4 m high, 2. row: 16 m long, 1.2 m high, 3. row: 32 m long, 0.4 m high and 4 row: 32 m long, 1.2 m high.
Fig. 8 e Map produced by Hector mapping (left) and the present algorithm (right) in vineyard crop rows. The environmental setup from first to last as follows; 1. row: 16 m long, 0.4 m high, 2. row: 16 m long, 1.2 m high, 3. row: 32 m long, 0.4 m high and 4 row: 32 m long, 1.2 m high.
168
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
output values and comparing them to referential ones that were measured manually when recording the data sets.
4.2.
Mapping results
The recorded data sets were tested with Hector mapping and compared to the results produced by the present algorithm. In both examples, the map of the environment was based on the LRF measurements depicted by the output maps in Fig. 7, for the apple tree rows, and in Fig. 8, for the vineyard rows. The black dots on the maps are the plants, and the light grey area is free space between the crop rows, where the robot can move. In all cases the distance was also measured manually and compared to the distance in both algorithms for evaluation.
Table 1 gives an overview of the mapping results. The results of Hector mapping are summarized in the second to the fifth columns, and the results for our algorithm, in columns six to nine. As shown, the presented algorithm was less accurate where measurements were taken of tree trunks, due to the low density of the readings. It performed better in a vineyard environment, as with the Hector mapping. When comparing the two methods, the algorithm produced better results overall. Error was calculated based on the whole combined area, that including 192 m of crop rows. For large agricultural environments, this represents a small error and is low enough to accurately locate the robot between the crop rows. When examining the Hector mapping algorithm's results from Table 1, Figs. 7 and 8, it can be seen that Hector mapping failed in
Table 1 e Comparison between the manual and automatic distance measurements. Plant distance/Height
Apple 16 m 0.4 m Apple 16 m 1.2 m Apple 32 m 0.4 m Apple 32 m 1.2 m Vineyard 16 m 0.4 m Vineyard 16 m 1.20 m Vineyard 32 m 0.4 m Vineyard 32 m 1.2 m Absolute average error [m]
Hector Translation Hector Rotational Presented Translation Presented Rotational mapping absolute mapping absolute algorithm [m] absolute algorithm absolute [m] error [m] [deg] error [deg] error [m] [deg] error [deg] 3.72 15.29 0.71 13.50 16.48 14.31 32.17 31.87
12.28 0.71 31.29 18.50 0.48 1.69 0.17 0.13 8.1 ± 11.6
0 13 144 12 0 7 33 0
0 13 144 12 0 7 39 0 26.8 ± 49.04
12.73 15.80 31.79 31.68 15.54 15.2 32.06 31.07
0.72 0.2 0.21 0.22 0.46 0.8 0.06 0.93 0.45 ± 0.33
3 6 18 19 11 6 16 6
3 6 18 19 11 6 16 6 10.62 ± 6.3
Fig. 9 e A picture of the ROVITIS robot (upper image), the map built by the present algorithm (bottom left image) and the map from Hector mapping (bottom right image).
b i o s y s t e m s e n g i n e e r i n g 1 5 0 ( 2 0 1 6 ) 1 6 0 e1 6 9
several scenarios and reached an overall 8.1 m ± 11.6 m for translation and 26.8 ± 49.04 for rotation. The algorithm, on the other hand, was accurate in all cases and produced a maximum of 0.54 m of error on a 16 m long run, with a best performance of 0.2 m error while estimating position. When estimating the distance error, the angle was calculated by both methods. The accuracy of rotation proved to be a challenge for both Hector and the algorithm, with an average error of around 10 . This was because of the complex structure of the environment as well as a lack of good reference points where the rows ended. Results were acquired by testing the algorithm on another, bigger agricultural robot e ROVITIS (Pantano, 2014), with the aim of further testing the utility of the algorithm. In contrast to the platform described in Section 2, this platform was diesel powered rather than electric and could produce vibrations that affect the results. Another factor is the spraying application, which might also influence the performance. Again, the datasets were recorded, so they could be tested and the results compared from the present algorithm and for Hector mapping. Figure 9 shows the robot as well as both sets of results. To test the algorithms, the ROVITIS mobile robot was used with a spraying application on 60 m long vineyard rows. The LRF data scans were recorded and maps for both SLAM algorithms were constructed. The present algorithm calculated 58.89 m in length, while the Hector mapping managed only 12.86 m.
5.
Conclusions
The aim of this study was able to provide stable and reliable information about the robot's position while moving in parallel with crop rows. The mobile robot's autonomy in agriculture is based on a map that needs to be exact in order to provide localisation. The method depended only on the information from the laser data because LRF provided exact distance information. The distance measurements in the crop rows detect landmarks, which are used to iteratively determine the robot's new location. Therefore, the LRF's limitations need to be considered. We believe that this study will contribute to the field of autonomous robot applications in agriculture. Currently we see this algorithm as a basis for field navigation for applications such as crop assessment, missing plant detection, poor crop location detection and weed detection. Future studies need to combine this algorithm with visual SLAM, where a classic LRF can be used as a 3D scanner. The LRF needs to be mounted on a tilt mechanism, which, given the right time sequence, can provides a 3D image of an environment that might produce even better results.
Acknowledgement We would like to thank Dr. Giorgio Pantano for letting us use the ROVITIS robot to test and evaluate our algorithm. The authors also acknowledge the vital contributions made by Dr. Michelle Gadpaille by proofreading the manuscript.
169
references
Auat Cheein, F., Steiner, G., Perez Paina, G., & Carelli, R. (2011). Optimized EIF-SLAM algorithm for precision agriculture mapping based on stems detection. Computers and Electronics in Agriculture, 78, 195e207. Barawid, O. C., Jr., Mizushima, A., Ishii, K., & Noguchi, N. (2007). Development of an autonomous navigation system using a two-dimensional laser scanner in an orchard application. Biosystems Engineering, 96(2), 139e149. Chung, H. Y., Hou, C. C., & Lu, Z. R. (2013). Wheeled robot navigation and local-map building in unknown environments. In CACS international automatic control conference (pp. 350e355). Gonzales, R. C., & Woods, R. E. (2001). Digital image processing (2nd ed.) Prentice Hall, Upper Saddle River, New Jersey. Grisetti, G., Stachniss, C., & Burgard, W. (2005). Improving gridbased SLAM with Rao blackwellized particle filters by adaptive proposals and selective resampling. In Proceedings of the IEEE international conference on robotics and automation (pp. 2432e2437). Grisetti, G., Stachniss, C., & Burgard, W. (2007). Improved techniques for grid mapping with rao-blackwellized particle filters. IEEE Transactions in Robotics, 23, 34e46. Hiremath, A. S., van der Heijden, G. W. A. M., van Evert, F. K., Stein, A., & Braak, C. J. F. (2014). Laser range finder model for autonomous navigation of a robot in a maize field using a particle filter. Computers and Electronics in Agriculture, 100, 41e50. Hiremath, S., van Evert, F. K., Braak, C., Stein, A., & van der Heijden, G. (2014). Image-based particle filtering for navigation in a semi-structured agricultural environment. Biosystems Engineering, 121, 85e95. Hiroi, S., & Niitsuma, M. (2012). Building a map including moving objects for mobile robot navigation in living environments. In IEEE networked sensing systems (INSS) international conference, Antwerp (pp. 1e2). Kohlbrecher, S., Meyer, J., Graber, T., Petersen, K., Klingauf, U., & Stryk, O. (2013). Hector open source modules for autonomous mapping and navigation with rescue robots. TU Darmstadt, Germany: Department of Computer Science. Kohlbrecher, S., Meyer, J., von Stryk, O., & Klingauf, U. (2011). A flexible and scalable SLAM system with full 3D motion estimation. In Proceedings of the 2011 IEEE international symposium on safety, security and rescue robotics, Japan (pp. 155e160). Ohno, K., Tsubouchi, T., Shigematsu, B., Maeyama, S., & Yuta, S. (2003). Outdoor navigation of a mobile robot between buildings based on DGPS and odometry data fusion. In Proceedings. ICRA '03. IEEE international conference on robotics and automation, 2 (pp. 1978e1984). Oksanen, T., Linja, M., & Visala, A. (2005). Low-cost positioning system for agricultural vehicles. In International symposium on computational intelligence in robotics and automation (pp. 297e302). Pantano, G. (2014, 08, 21). Rovitis e Robotics in viticulture. Retrieved from: http://www.exponentialtimes.net/videos/rovitisrobotics-viticulture. Quigley, M., Conley, K., Gerkey, B., Faust, J., Foote, T., Leibs, J., et al. (2009). ROS: An open-source robot operating system. ICRA Workshop on Open Source Software, Willow Garage, Menlo Park, California. ROS hector_slam software stack (17th of April 2014). Retrieved from: http://wiki.ros.org/hector_slam. Accessed 2014 April 18. Stone, H. S. (2001). A fast direct fourier-based algorithm for subpixel registration of images. IEEE Transactions on Geoscience and Remote Sensing, 39(10), 2235e2242. Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic robotics. Cambridge, USA: The MIT Press.