Pang 2019
Pang 2019
Pang 2019
* This paper is supported by National Key Research and Development 1, School of Control Science and Engineering, Shandong University, Jinan,
Program, China (No. SQ2017YB130411) Major Program of Shandong 250061, China
Province Natural Science Foundation, China (No.ZR2018ZC0437) and Key Rui Song*, School of Control Science and Engineering, Shandong
Research and Development Program of Shandong Province, China University, Jinan, 250061, China (e-mail: rsong@sdu.edu.cn)
(No.2016ZDJS02A07)
Authorized licensed use limited to: Fondren Library Rice University. Downloaded on May 18,2020 at 06:44:30 UTC from IEEE Xplore. Restrictions apply.
nonlinear least squares problem as Back-end, and based the error is risen because of the error created in scan matching.
method of Branch and Bound to improve the speed of Their method without loop closure means that accumulative
calculation with the accuracy keeping at 5cm [8]. Their errors cannot be corrected. The error lead to the inconsistencies
algorithm reduced the dependence on computing resources, and the distortion of map (Figure 2). Moreover, too large usage
meanwhile they ensured global consistency and achieved real- scenarios lead to the increasing demand for computing
time large scenarios 2D SLAM, made lightweight equipment a resources. Nowadays, space resources on mobile robots are
reality. Compared to single channel LIDAR, multichannel becoming more and more precious, emphasizing carrying
LIDAR provide a lot of information of the point clouds. With smaller hardware resources to reduce the space of robots
the point clouds have more detailed details, multichannel themselves.
provide more information reflecting more real environment’s C
B C
change. It means that we need the method with the lower B
computation to keep real time.
Iterative Closest Point (ICP) is a typical method for finding
pose transformation between adjacent frames of multichannel
A A
LIDAR [9]. The corresponding relationship is found in use of
point by point in two laser point clouds until the requirements Figure 2. : Left: map built from odometry. The map is homotopic to a long
are met. However, the number of point clouds in 3D LIDAR is corridor that goes from the starting position A to the final position B. Points
that are close in reality (e.g., B and C) may be arbitrarily far in the odometric
very large and cost too much time in use of ICP. But as a map. Right: map build from SLAM. By leveraging loop closures, SLAM
reliable and effective method in 2D SLAM, there are many estimates he actual topology of the environment, and “discovers” shortcuts
ways to improve the performances of ICP. Y. Chen et al. in the map
proposed a variant of point-to-line ICP [10], and Censi Andrea
et al. proposed a variant of point-to-plane ICP [11]. Therefore, our work is to improve a high-precision
lightweight laser SLAM method for outdoor large-scale scenes,
Though, it is difficult to extract features from a large
which is convenient to carry on embedded system to realize
number of points clouds. If point clouds can be divided and
real-time pose calculation and map construction of the robot,
classified in advance, the amount of computation will be
and optimize the pose error by searching loop closure. In order
reduced. Tixiao Shan et al. proposed it could be fixed by
to verify the effectiveness of the algorithm, we have carried out
preprocessing original point clouds from multichannel LIDAR
several experiments to test it. The rest of this paper is arranged
[12]. About the method of preprocessing point clouds, M.
as follows. Section II describes the overall architecture of the
Himmelsbach et al. proposed a method for fast large-scale
system. Then, section III introduces the contents of the system
long-range segment the point clouds robot running
separately. In section IV, we validate our system in school
environment into ground point clouds and non-ground point
environment to prove its accuracy and robustness. The section
clouds, meanwhile, segment non-ground cloud into several
V summarizes the paper.
different small point clouds based on Euclidean distance [13].
It is effective to divide a complex point clouds into several
small ones to increasing operating speed. I. Bogoslavskyi et al. II. SYSTEM DESCRIPTION
proposed a method that 3D point cloud could be mapped to 2D A. Hardware System
laser range images with clearly defined neighborhood relations
The paper presented scheme is tested by the LIDAR of
make the segmentation problem easier [14]. Instead of
Velodyne VLP-16, unmanned ground vehicle and controller.
generation of the 3D point cloud, the approach has the faster
Velodyne VLP-16 can provide 3D laser point clouds. The
compute speed. By calculating the Euclidean distance between
effective measuring distance of VLP-16 is 100 meters, and the
neighboring points, they judge whether the points belong to the
precision is ±3cm. The vertical field of view (FOV) is 30°
same object or not. And this approach is effective for sparser
(±15°) and the horizontal FOV is 360°. Velodyne VLP-16 has
point clouds, though the points of 16-beam LIDAR with the
16 channel sensor with vertical angular resolution of 2°.
longer Euclidean distance between two points. After
segmentation, the components have been found which belong The UGV we used to be with the maximum operating speed
to one object. The application of these methods enables us to of 6 km/h. The JROBOT Komodo-02’s design load is 80kg,
further improve the speed of feature extraction and remove and its suspension adopts the balanced suspension of Christie
noisy point clouds. four-wheeled group, which can be adapted to heavy load shock
absorber.
The approaches of feature-based matching are attracting
more and more attentions, because of depending on the less The controller used in this paper is Nvidia Jetson TX2. The
computational resources by extracting features from point Jetson TX2 is an embedded system-on-module (SoM) with
clouds to predictive motion model of robot. Bastian Steder et dual-core NVIDIA Denver2+quad-core ARM Cortex-A57,
al. proposed a method to place recognition by a variant of the 8GB 128-bit LPDDR4 and integrated 256-core Pascal GPU.
Laplacian of Gaussian approach to calculate interest points Useful for deploying computer vision and deep learning, Jetson
with high curvature [15]. Ji Zhang et al. perform the method of TX2 runs Linux and provides greater than 1TFLOPS of FP16
computing the roughness of a point in its local region [16]. compute performance in less than 7.5 watts of power.
They perform an approach to calculate robot’s state by
edge/plane feature correspondences between two adjacent In order to verify the correctness of the trajectory of the
scans and get the range of obstacle from robot. With increasing robot, we equipped NovAtel’s integrated GNSS+INS
of the map size and the distance of robot walking, accumulative navigation system SPAN-CPT. The built-in GNSS board
adopts NovAtel’s latest OEM6 hardware platform technology.
869
Authorized licensed use limited to: Fondren Library Rice University. Downloaded on May 18,2020 at 06:44:30 UTC from IEEE Xplore. Restrictions apply.
The IMU consists of three-axis fiber optic gyroscope (FOG) Back-end
and three-axis micro-electromechanical system (MEMS) Local Matching
State of
Robotics
accelerometer, which can adapt to a variety of environments.
SPAN-CPT uses NovAtel's leading satellite navigation Front-end Graph Optimization
technology to achieve centimeter -level positioning accuracy.
Segmentation
Transform
Velodyne VLP-16 Integration Map
Point Cloud Feature Extraction
SPAN-CPT
Key Frame Fixed State
Motion Model
Loop closure
CONTROLLERR
JROBOT Komodo-02
Figure 4. Whole Software System
870
Authorized licensed use limited to: Fondren Library Rice University. Downloaded on May 18,2020 at 06:44:30 UTC from IEEE Xplore. Restrictions apply.
C. Loop Closure P \ LP
where submap poses of the time I poses < ={ }
In the previous apart, we obtain the motion model of robot
with small accumulated error. And the information of key
<
V \ V
M
frame we obtain from Back-end each 0.5m. We stored key i=1,…,m and the new scan poses ={ }j=1,…,n with we
frame in a vector containing the index of key frames and pose \
information. But for large scale environment, constructing use in building the constraints. LM is the translation and the
global consistency map is difficult because of accumulated rotation between the new scan pose j and the submap pose i.
error. The approach to calculate the associated covariance matrices
¦ LM like [17]. About the residual E, we compute it as a
constraint for
§ 5 PW P W V ·
¨ L L ¸
\ \ \M
H\ L \ \ LM
P V
\ LM
M
¨ \ P \ V ¸
© LT M T ¹
The Huber loss, we use it as a loss function ρ to reduce the
Figure 5. graph optimization influence if the wrong constraints added to the optimization
problem, like fig.5. Each blue triangle represents the pose of
Large map is joint by many small sub-maps. For searching the LIDAR, and the black edge means the constraint between
loop closure, a loop matching is operating and the loop closure new scan pose and pose in the map. Because of this process,
is found, the new constraint will be added in the optimization related poses will be adjusted to the correct poses.
problem. But it is difficult to search loop closure in LIDAR
feature map because ICP costs a lot of time to calculate the III. EXPERIMENT AND ANALYZE
translation and rotation from two discontinuous scans. In [13], In the experiments of us, the system running on the ROS
they used the method of branch and bound and multiresolution (Robot Operating System) Kinetic and Ubuntu 16.04. The
grid map to improve the speed of searching loop closure. In the software program we developed in our algorithms using C++,
feature map, it is difficult to achieve. PCL 1.9, Eigen and Ceres Solver.
So it is necessary that take the method with small In the Qianfoshan campus of Shandong University, the
computation. The approach we took that search loop by two running trajectory we drawn in the Google map, based on the
steps, distance calculation and loop matching. SPAN-CPT, which provided the information of position in the
For ensure the real-time performance of the method, the accuracy of centimeter-level.
geometric relationship of the robot relationship of different We carried out two different experiments to test the
time. And the Euclidean distance are calculated between the performance of our system. We evaluate the quality of our
current position of robot and all the historical position in the algorithm by considering both in mapping and location. In this
memory. We set a threshold to determine whether to scan scenario, the path of the robot is 300m, and area of the map
matching. If the distance is less than 1.5m between the current built is approximately 2 million square meters. At the first
time position and the position at certain moment, the scan experiment, we test SLAM’s mapping ability to match point
match will be detected. clouds by reconstructing the environment. The second
By calculating the Euclidean distance, we find the point experiment, by comparing the effects of several groups of
which is nearest position from the current time. At time I, the trajectories, the positioning effect of SLAM is evaluated. The
), , trajectory got from SPAN-CPT, we consider it as the true
feature set of the robot is built as set { S , )H } which used in ground trajectory. And the trajectory we got from our
loop matching. In the previous feature extraction, at time J, we algorithm based on robot’s odometer and IMU (Inertial
)- )- Measurement Unit) as the reference path.
have got the feature set{ S , H }.By the ICP, the translation T
and the rotation R has been computed between the time J and A. 3-D reconstruction experiment
the time I. In order of distance, we build constraint with each By controlling our mobile robot surrounding with our
scan at the range of threshold to optimize trajectory we built. school square, we get the 3D map of the square. The buildings
For the optimization problem, we use Ceres Solver to build and streets are complete reconstruction. We get the picture of
least square problem to our school square photo from Baidu Map. By the method of us,
we reconstruct the map of the entire environment shown as
Figure 5.
DUJ PLQ
P
< < V
¦
LM
U( \ LP \ MV ¦LM \ LM
871
Authorized licensed use limited to: Fondren Library Rice University. Downloaded on May 18,2020 at 06:44:30 UTC from IEEE Xplore. Restrictions apply.
On the path, the trajectory of SLAM and the trajectory
based on odometer and IMU are different (fig.8). The
method is used an extended Kalman filter (EKF) with a 6D
model (3D position and 3D orientation) to combine
measurements from wheel odometry, IMU sensor as references.
This Robot Pose EKF package belongs to Navigation in the
ROS. Our algorithm performs better than Robot Pose EKF
package.
Figure 6. 3D Reconstruct (the first photo is the real environment, and the
second photo is the map of 3D reconstruction)
Though the direction and angle of the two pictures are Figure 8. the trajectory of SLAM compared with EKF-Odom
different, we found that the constructed point cloud map is not
distorted. The immobile landmark will be saved in the map as In the experiment of loop closure, we compare the SLAM
a apart (like trees and buildings). But other things that move performance without loop closure and the performance with
won’t be saved. loop cosure. The first figure of fig.9 shows the trajectory
without graph optimization, and the second figure of Figure9
B. location experiment shows the trajectory closure is almost achieved.
In a real environment, we real-time calculate the motion
trajectory of robot. For the accuracy of robot motion trajectory,
we want to measure and analysis. If the ground truth trajectory
can be observed, quantitative analysis can be performed. So the
position of robot we tracked by SPAN-CPT.
But SPAN-CPT belongs to WGS84 coordinate system.
For the coordinate system in the SLAM map, we defined the
forward direction of x-axis is the forward direction of the robot
initial pose, the origin is the initial position of the robot
(comply right hand rule). So, we transform robot coordinate
system and WGS84 coordinate system to the same coordinate
system. Firstly, converting WGS84 coordinate system into
station center coordinate system. Then, we align the two
coordinate systems.
SPAN-CPT provide the longitudinal and latitude
information position is in WGS84 frame coordinate system.
Our path was draw in the Google Map (Figure7).
IV. CONCLUSION
Our paper has described a 3D LIDAR SLAM with low cost
and high accuracy which used on mobile robot. Our method
based on the point cloud matching and loop closure performed
Figure 7. Our path in Google Map better than some method in large outdoor scenarios which
872
Authorized licensed use limited to: Fondren Library Rice University. Downloaded on May 18,2020 at 06:44:30 UTC from IEEE Xplore. Restrictions apply.
suitable for environmental perception and three-dimensional
reconstruction. The statute of robot we can also gotten from our
motion model in real-time. In the feature, we will continue to
improve the frequency and accuracy of the robot’s pose and the
performance of mapping.
REFERENCES
[1] Giorgio Grisetti, Cyrill Stachniss, and Wolfram Burgard: Improved
Techniques for Grid Mapping with Rao-Blackwellized Particle Filters,
IEEE Transactions on Robotics, Volume 23, pages 34-46, 2007
[2] Kohlbrecher, Stefan , et al. "A flexible and scalable SLAM system
with full 3D motion estimation." 2011 IEEE International Symposium
on Safety, Security, and Rescue Robotics IEEE, 2011.
[3] S. Agarwal, K. Mierle, and Others, “Ceres solver,” http://ceres-
solver.org
[4] Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat, Kurt Konolige,
and Wolfram Burgard: g2o: A General Framework for Graph
Optimization, IEEE International Conference on Robotics and
Automation (ICRA), 2011
[5] Biber, Peter, and W. Strasser. "The normal distributions transform: a
new approach to laser scan matching." Proc.of IEEE/RSJ Intl Conf.on
Intelligent Robots & Systems 3(2003):2743-2748 vol.3.
[6] Tully, S, et al. "Iterated filters for bearing-only SLAM." IEEE
International Conference on Robotics & Automation 2008.
[7] Konolige, Kurt, M. Agrawal, and J. Solà. "Large-Scale Visual
Odometry for Rough Terrain." Robotics Research-the International
Symposium 2010.
[8] Hess, Wolfgang , et al. "Real-Time Loop Closure in 2D LIDAR
SLAM." 2016 IEEE International Conference on Robotics and
Automation (ICRA) IEEE, 2016.
[9] P.J. Besl and N.D. McKay, “A Method for Registration of 3D Shapes,”
IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.
14(2): 239-256, 1992.
[10] Y. Chen and G. Medioni, “Object Modelling by Registration of
Multiple Range Images,” Image and Vision Computing, vol. 10(3):
145-155, 1992.
[11] Censi, Andrea . "An ICP variant using a point-to-line metric." IEEE
International Conference on Robotics & Automation IEEE, 2008.
[12] Tixiao Shan ; Brendan Englot漓瀡LeGO-LOAM: Lightweight and
Ground-Optimized LIDAR Odometry and Mapping on Variable
Terrain,瀢 2018 IEEE/RSJ International Conference on Intelligent
Robots and Systems (IROS) IEEE漓2018
[13] M. Himmelsbach, F.V. Hundelshausen, and H-J. Wuensche, “Fast
Segmentation of 3D Point Clouds for Ground Vehicles,” Proceedings
of the IEEE Intelligent Vehicles Symposium, pp. 560-565, 2010.
[14] I. Bogoslavskyi and C. Stachniss, “Fast Range Image-based
Segmentation of Sparse 3D Laser Scans for Online Operation,”
Proceedings of the IEEE/RSJ International Conference on Intelligent
Robots and Systems, pp. 163-169, 2016.
[15] Steder, Bastian , G. Grisetti , and W. Burgard . "Robust place
recognition for 3D range data based on point features." IEEE
International Conference on Robotics & Automation IEEE, 2010.
[16] Zhang, Ji , and S. Singh . "Low-drift and real-time LIDAR odometry
and mapping." Autonomous Robots 41.2(2017):401-416.
[17] Olson, Edwin B. "Real-time correlative scan matching." 2009 IEEE
International Conference on Robotics and Automation. IEEE, 2009.
873
Authorized licensed use limited to: Fondren Library Rice University. Downloaded on May 18,2020 at 06:44:30 UTC from IEEE Xplore. Restrictions apply.