SUGISAKAMASANORI

Journal of Robotics, Networking and Artificial Life
Vol. 9(2); September (2022), pp. 177–182
ISSN (Online):2352-6386; ISSN (Print):2405-9021
https://alife-robotics.org/jrnal.html

Research Article

Online SLAM for Forestry Robot
Sylvain Geiser1, Sakmongkon Chumkamon2, Ayumu Tominaga3, Takumi Tomokawa2, Eiji Hayashi2

Department of Interdisciplinary Informatics, Graduate School of Computer Science and Systems Engineering, Kyushu Institute of Technology, 680-4,Kawazu, Iizuka-City, Fukuoka, 820-8502, Japan
Department of Mechanical Information Science and Technology, Kyushu Institute of Technology, 680-4, Kawazu, Iizuka-City, Fukuoka, 820-8502, Japan
3
National Institute of Technology, Kitakyushu College, 5-20-1 Shii, Kokuraminamiku, Kitakyushu, Fukuoka, 802-0985 Japan
12ARTIC LE INFO
Article History
Received 24 November 2021
Accepted 31 May 2022

Keywords
Field robot
Online SLAM
Forestry

ABSTRACTLess and less Japanese are working in the forestry sector. Due to this lack of workforce, dangerous
tasks could be delegated to robots. In order to perform this mission, the latter need embedded
localization and mapping systems. This paper discusses the implementation of a particle filter
based SLAM solver on a dedicated mobile robot called SOMA, developed at Hayashi Laboratory.
The proposed approach has been evaluated in a realistic forest simulation and the first results
suggest that it could be used in real time

© 2022 The Author. Published by Sugisaka Masanori at ALife Robotics Corporation Ltd
This is an open access article distributed under the CC BY-NC 4.0 license(http://creativecommons.org/licenses/by-nc/4.0/).

Particle filter

localization. The first one is performed offline and oncefor all, whereas the second one is carried out online. Thismethod tends to be very efficient when the environment
where the robot evolves remains unchanged. However,when it is frequently changing, this approach becomesinappropriate and online mapping, worthwhile. This is
the reason why, forests being constantly transformed byexploitation, online SLAM is proposed in this article as
the problematic to address for this specific application.
Numerous algorithms have been developed in thisdomain, but FastSLAM has the advantage of being ableof handling multimodal beliefs, owing to its logic basedon particle filters.4 This is the main reason why
FastSLAM was selected to be implemented on SOMA.
This article is organized in three parts. The first one
explains the realized implementation and the undertaken
choices. Then, the simulation experiments andparameters are described. Finally, the results arepresented and an analysis of them is performed, leading
to the conclusion which reviews achievements andremaining goals to reach.

1. Introduction
In Japan, owing to the combination of aging workers andunattractivity, the forestry sector is facing a significant
decrease in terms of workforce, while the demand isconversely increasing.1,2 In this tense context, mobilerobots have been proposed to replace lacking humans,
especially for hazardous jobs. The Hayashi Laboratory isbuilding a dedicated prototype for this purpose. Thisrobot, called SOMA,3 is based on the platform of an AllTerrain Vehicle (ATV). Many additional sensors
including odometers, GPS (Global Positioning System),
IMU (Inertial Measurement Unit), RGB-D cameras andlidar have been attached to the structure.
In mobile robotics, navigation is an essential topic todeal with, and field robots are not exceptions to the rule.
For a long time, the several problems to solve in thiscontext have been unified in the framework of
Simultaneous Localization and Mapping (SLAM), butcurrent applications mostly treat separately mapping andCorresponding author’s E-mail: geiser.nathan-sylvain778@mail.kyutech.jp, m-san@mmcs.mse.kyutech.ac.jp, tominaga@kct.ac.jp,
tomokawa.takumi163@mail.kyutech.jp, haya@.mse.kyutech.ac.jp, http://www.kyutech.ac.jp/177

2. FastSLAM for Forestry Robotis the nature of landmarks. Here, the choice of trees
seems obvious, due to their concentration and their easily
identifiable shape. Moreover, specific motion andobservation models also have to be chosen. Their
description is the topic of the following sections.

The core of the FastSLAM algorithm is a RaoBlackwellized particle filter and the environment is
described with a feature map. The latter consists of thelist of the locations of easy distinguishable elements
called features or landmarks. In order to estimate the poseof the robot and the map at the same time, the particles
used include, apart from a weight, a pose and a collectionof Kalman filters. The latter store the position andassociated uncertainty of features.
Each time new sensory data becomes available, two
steps are performed: prediction and correction, which arecommon to every classical Bayes filter used in robotics.
During prediction step, the poses of the particles arechanged by applying the captured motion of the realrobot, corrupted with the modelled motion noise. Then,
two different updates are applied during correction step.
First, according to received observation and associatednoise, the Kalman filters corresponding to features in thesensor visibility scope are updated for each particle andnew filters are initialized for previously unseen
landmarks, using Kalman equations. In addition, theweights of particles are updated in accordance withobservation likelihood.

2.1. Motion
The robot motion is captured through the odometry
delivered by wheels encoders and the selected model
breaks the movement in three steps: a first rotation, thena translation, and finally a second rotation. Each of themis supposed to be corrupted with linear gaussian noise(Eq. 1, 2 and 3).

(1)

(2)

(3)

where 𝛿𝛿𝛿𝛿𝛿𝛿𝛿𝛿1, 𝛿𝛿𝛿𝛿𝛿𝛿𝛿𝛿𝛿𝛿𝛿𝛿 and 𝛿𝛿𝛿𝛿𝛿𝛿𝛿𝛿2 are noisy values of firstrotation, translation and second rotation respectively,��������
𝛿𝛿𝛿𝛿𝛿𝛿𝛿𝛿1 , ���������𝛿𝛿𝛿𝛿𝛿𝛿𝛿𝛿𝛿𝛿𝛿𝛿 and ��������
𝛿𝛿𝛿𝛿𝛿𝛿𝛿𝛿2 are corresponding noiselessvalues, and 𝑎𝑎𝑟𝑟𝑟𝑟𝑟𝑟 , 𝑏𝑏𝑟𝑟𝑟𝑟𝑟𝑟 , 𝑐𝑐𝑟𝑟𝑟𝑟𝑟𝑟 , 𝑎𝑎𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡 , 𝑏𝑏𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡 and 𝑐𝑐𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡𝑡 areconstants.

Fig. 1. Steps of Fast SLAM algorithm

2.2. Observation

The Figure 1 is illustrating this process. In thisdiagram, the real robot is represented by a red disk
indicating its real position and a black line in front of it
revealing its heading. Concerning the particles, they aredepicted by green points with variable size, located at theposition they contain. The bigger a particle, the higher its
weight, the more probable it is to embody the true poseof the robot and the true map of the environment. The
particle with the highest weight before the new iterationof the algorithm is highlighted in black. Finally, the blue
crosses represent the real features, whereas the green
crosses are the landmarks in the map of the most probableparticle. The green circles around green crosses represent
the uncertainty over the positions of features.
Since this algorithm can be used in different
situations, our implementation has to adjust to the forest
environment particularities. One major parameter to set

Fig. 2. Clusters made from lidar pointcloud

178

Concerning observation, the range-bearing model was
taken on, because the sensor uses to perceive theenvironment around the robot is a lidar. Consequently,
each individual observation is composed of the distanceand the azimuth between the feature and the robot, which
defines coordinates in the moving polar reference frame
attached to the robot. In order to get them for each tree inthe visibility scope, the raw pointcloud has to beprocessed. First, the ground and the canopy are removed
by clipping it in height. Then, 3D euclidian clustering is
performed to locate trunks, turning their representationfrom discrete and noisy points into simple
parallelepipeds (Fig. 2). Finally, the centers of gravity of
the clusters are taken as the positions of the trees. The
returned length of each laser ray is assumed to include
linear gaussian noise. Therefore, given someapproximations, the extracted coordinates of the trees
follow the same noisy distribution (Eq. 4 and 5).

algorithm.5 This choice prevents a common issue
encountered using the usual splitting technique: wrong
fusion of landmarks. Indeed, in this case, distinct featuresin reality can be linked to the same feature in the map.
For each observed tree, the likelihood toward eachfeature in the visibility scope is computed and ordered ina list. At the beginning, the first-ranked landmarks areassociated with the corresponding tree. Then, if multipleobservations share the same feature, the latter is onlyappaired with the most likely one, and the others are
assigned to the following landmark in their list. This
process is repeated until each tree differs from others on
its linked feature. If the likelihood of the next landmark
to be associated is not greater than some threshold or if alist comes to its end, a new feature is initialized.
2.3.2. Features deletion
Sometimes, previously added features become orphaned,that is they do not correspond to any real tree. To preventthese features to stay in the map, their deletion is carriedout together with correspondences establishment. Thisaction is subject to a threshold and the presence ofconcerned landmarks in the lidar visibility scope.

(4)

2.4. Particles handling(5)

The common particle deprivation issue of particle filters
is counteracted by applying conditional resampling. Thisshrinking of diversity among particles is also limited byadding gaussian noise to the poses of resampled ones, atthe same level as motion noise.

where 𝑑𝑑 and 𝜑𝜑 are the noisy values of distance and azimuth
respectively, 𝑑𝑑̅ and 𝜑𝜑� are the corresponding noiselessvalues, and 𝑎𝑎𝑑𝑑 , 𝑏𝑏𝑑𝑑 , 𝑎𝑎𝜑𝜑 and 𝑏𝑏𝜑𝜑 are constants.

3. Experiments

2.3. Correspondences and map management

A model of the SOMA robot, along with a realistic forest
environment made of a 30 by 30 meters ground and 16
pine trees have been created, with the aim of conducting
simulation experiments (Fig. 3). The software used areRviz and Gazebo, integrated with ROS.

The correspondence problem, which consists ofassociating each observation in reality with a feature in
the map, can be tackled in various ways. The maximumlikelihood approach has been chosen, as it is often usedand has proven its efficiency. In FastSLAM, each particle
having its own correspondences, a large set of different
data associations cohabit throughout the experiment.
This diversity tends to lead to better results.
2.3.1. Multiple observations
A tree is rarely alone in the lidar visibility scope. Hence,since several trees are seen at the same time, observations
are called multiple. Instead of decomposing these
multiple observations into individual ones and executing
successive updates for each of them, they are treated as awhole, by using a slightly modified Gale-ShapleyFig. 3. Robot model in Gazebo environment

179

The motor noise experienced in reality is virtuallyemulated by a ROS node inserted between the steering
controller and Gazebo. It consists of a linear gaussian
noise added to linear and angular velocity commands
respectively.
During all experiments, the robot is manuallyoperated to go straight from one side of the forest to theopposite one. The comparison of the accuracy and update
rate of each simulation enables to evaluate how thesemeasures are impacted by the number of particles and thenumber of trees.
4.

Final position error (m)2.39

Final map average error (m)Redundant features

0.81
6Orphan features
Update rate (Hz)11

Table 1. Simulation quantitative results

As it can be seen on Figure 5, even though correct
tracking of the robot is performed on the whole, anincreasing distance between the estimated and real poses
can be noticed. More precisely, considering the directionof movement, the estimate stays behind the real pose,accumulating late. The delay before taking new odometry
data into account can explain some of this gap, but themain reason is most probably to be found in the poor
estimation of the coordinates of the trees.

Discussion

The accuracy of pose and map estimates has been
evaluated with a simulation using 100 particles andrealistic motion and observation noises. The Figure 4
shows the current pose and map estimation after 42
seconds of simulation. The same elements arerepresented as in Figure 1 and the same color code is used.
Additionally, the inner space between the two dotted red
circles defines the lidar visibility scope.

Fig. 5. Display of pose and map estimates at different times

Indeed, these locations are assumed to coincide withthe centers of the clusters extracted from the lidar
pointcloud. However, since each tree is only seen partly
at one time, the former statement does not hold. Besides,
the centers of the clusters are constantly changing basedon the pose of the robot, breaking the fundamental staticworld assumption (Fig. 6). Since motion noise is much
more significant than observation noise, this is the poseestimate and not the map one which bears theconsequences of this issue. Indeed, the estimated pose isupdated with the compensation of the virtual movementof trees, moving it in the opposite direction. Because therobot and the extracted centers of trees move in the same
direction, the pose estimate tends to be late. To prevent
this issue in the future, a circle pattern recognition should
be applied to each cluster to get the real centers of thetrees.

Fig. 4. Display of pose and map estimates at time t = 42 sSome metrics have been used to measure theefficiency of the implementation. In the experimental
conditions described above, the distance between thefinal real and estimated poses is 2.39 m, and the average
distance between each tree and its best corresponding
feature in the map is about 0.81 m (Table 1). Five trees
over 16 are overrepresented since several landmarks lie
around their locations, and one feature in the map is far
away from closest trees, leaving it orphaned.
Furthermore, an update rate of 1 Hz has been measured
during this experiment.

1804.3. Future research
A countermeasure should be taken in order to prevent thepreviously described issue regarding the late of poseestimation. With this in mind, performing a circle patternrecognition on each cluster can be considered as aneffective remedy.
Even with randomization during the resampling stepof FastSLAM, particles deprivation still tarnishes theresults and can lead to failures. Since motion noise issurely dominating observation noise in this situation,using the mixture Monte-Carlo Localization (MCL)
method,6 suitable for reducing deprivation in these cases,
could be contemplated. This technique derives from theidea of dual MCL where the roles of motion andobservation are inversed. The latter is used to set newparticles and the former is used to update the weights ofthe particles. Dual MCL alone yields poor results, but
combining classical and dual approaches with some ratio
in mixture MCL significantly improves stability.
Finally, only simulations having been realized so far,real experiments should be conducted as well in order tocompare the results and entirely assess the presented
approach.

Fig. 6. Centers of clusters depending on the pose of the robot

4.1. Effect of the number of particlesThe overall accuracy of the proposed approach does notshow a substantial dependence with the number ofparticles used. Actually, only five particles seem to beenough to output a precise pose and map at the end of theexperiment. Nevertheless, the update rate suffers from a
too high number of particles (Table 2). Finally, theextension of the particles set does not alleviate thesystematic shift detailed in the last section.

Effect of increasing thenumber of particles

Accuracy
NoneUpdate rate
Decrease

References

Table 2. Effect of the number of particles on accuracy andupdate rate

1. O. Ikuo, “Declining situation of Japanese forestry today
and its challenges toward the 21st Century”, Kyoto
University Bioresource Economic Research, 1999
2. M. Matsumoto, H. Oka, Y. Mitsuda, S. Hashimoto, C.
Kayo, Y. Tsunetsugu, M. Tonosaki, “Potentialcontributions of forestry and wood use to climate change
mitigation in Japan”, Journal of Forest Research, Vol. 21,
2016
3. A. Mowshowitz, A. Tominaga, E. Hayashi, “RobotNavigation in Forest Management,” Journal of Roboticsand Mechatronics, Vol.30, No.2, pp. 223-230, 2018
4. M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit,“FastSLAM: A Factored Solution to the SLAM Problem”,
2002
5. D. Gale, L. S. Shapley, “College Admissions and theStability of Marriage”, The American Mathematical
Monthly, Vol. 69, No. 1 (Jan., 1962), pp. 9-15
6. S. Thrun, D. Fox, W. Burgard, “Monte carlo localization
with mixture proposal distribution”, Proceedings of theAAAI National Conference on Artificial Intelligence,
2000

4.2. Effect of the number of trees
The late of pose estimate is more considerable when therobot is close to trees, as a smaller part of them iscaptured by the lidar and therefore the centers of theclusters are far from the centers of the trees. Thus, a highdensity of the forest lowers the accuracy. In addition, asa well-known issue when solving SLAM, a large numberof landmarks tends to make data association harder,
involving more frequent errors in this process. Regarding
update rate, while the total number of trees does not have
any impact on it, a denser forest induces a lower update
rate, because only the features in the visibility scope arehandled (Table 3).
Effect of increasing thesize of the forest
Effect of increasing thedensity of the forest

Accuracy
Decrease

Update rate
NoneDecrease

Decrease

Table 3. Effect of the number of trees on accuracy and updaterate

181

Prof. Eiji Hayashi

Authors Introduction
Mr. Sylvain Geiser

Prof. Eiji Hayashi is a professor in theDepartment of Intelligent and Control
Systems at Kyushu Institute ofTechnology. He received the Ph.D.
(Dr. Eng.) degree from Waseda
University in 1996. His researchinterests
includeIntelligent
mechanics, Mechanical systems andPerceptual information processing. He is a member of The
Institute of Electrical and Electronics Engineers (IEEE)and The Japan Society of Mechanical Engineers (JSME).

He
studiedEngineering andComputer Science in Ecole desMines de Nancy, France, between
September 2018 and June 2020. He iscurrently a Master student at Kyushu
Institute of Technology, Japan, andconducts research at Hayashi
Laboratory.

Dr. Sakmongkon Chumkamon
Dr. Sakmongkon Chumkamon
received Doctor of Engineering
degree from Kyushu Institute ofTechnology in 2017. He was a
postdoctoral
researcheratGuangdong
UniversityofTechnologyin
2017-2019.
Presently he is a postdoctoral
researcher in Kyushu Institute of Technology since 2019.
His research interests include factory automation robots
and social robots.
Prof. Ayumu Tominaga
Prof. Ayumu Tominaga is aprofessor in the Department ofIntelligent and Control Systems atKyushu Institute of Technology. He
received the Ph.D. (Dr. Eng.) degreefrom Kyushu Institute of Technologyin 2021. His research interests
includeIntelligent
mechanics,
Mechanical systems and Perceptual information
processing.
Mr. Takumi Tomokawa
He received bachelor degree inEngineering in 2021 from mechanicalsystem engineering, Kyushu Instituteof Technology in Japan. He is aquiring
the Master degree in Kyushu Instituteof Technology.

182