AMIR-SLAM: Autonomous Mobile Industrial Robot Simultaneous Localization and Mapping

National Taiwan University
Graduate Institute of Electrical Engineering
Autonomous Mobile Industrial Robot with Multi-Sensor
Fusion Based Simultaneous Localization and Mapping
for Intelligent Service Applications
Department of Electrical Engineering
College of Electrical Engineering and Computer Science
Master Thesis
Presenter: Shang Lun Lee 李尚倫
Advisor: Ren C. Luo
Date: July 27, 2020
1

 Motivation & Background & Objective
 Robotic System
 Methodology
 AMIR SLAM Architecture
 Autonomous Mobile Manipulation Architecture
 Experimental Result
 Experiment I: Ablation Study
 Experiment II: SLAM Evaluation on Public Dataset
 Experiment III: SLAM Evaluation on Our Robot
 Experiment IV: Station-to-Station Autonomous Mobile Manipulation Test
 Experiment V: Multi-Station Autonomous Mobile Manipulation Demo
 Conclusions & Contributions & Future Works
Outline
2

 Robotic System
 Methodology
Outline
3

Motivation (1/2)
4
 Recent studies and industrial applications have shown that mobile manipulators remain a
popular robotic platform because of its multifunctional ability
 The manipulator on the robot is changing from decorative to practical
Pr2 Pepper
Robotnik+UR Omron+TM Clearpath+UR Kuka iiwa

Motivation (2/2)
5
 They can do various tasks with vision, mobility and dexterity
 In factories, café, restaurant, laboratory, etc.
 The autonomous mobile manipulation skill plays a key role
 How to do that without colliding?
Laboratory: robotic chemist
Service Industry : robotic coffee waiter
Factory : robotic material delivery
https://www.youtube.com/watch?v=1F3VEXYnwZs
https://www.youtube.com/watch?v=NaQMfkmQIno https://www.youtube.com/watch?v=dRT3tepdMyI

 Hand-crafted everything ?
 2D SLAM with autonomous navigation
 Hand-crafted manipulation procedure
 Hand-crafted obstacles with autonomous manipulation
 There is a more elegant way :
3D SLAM with autonomous mobile manipulation
Background (1/2)
6
*SLAM: Simultaneous Localization and Mapping
Hand-crafted obstacles with auto manipulation
Hand-crafted manipulation procedure
2D SLAM with autonomous navigation

 3D Simultaneous Localization and Mapping (SLAM)
 RGB-D based – high uncertainty, cheap, small
 3D LiDAR based – accurate, expensive, taking up space
 There is no 3D SLAM algorithm specifically designed for mobile manipulators
Background (2/2)
7
RGBDSLAMv2 [1] BLAM [3]
RTAB-Map SLAM [2]
[1] F. Endres, J. Hess, J. Sturm, D. Cremers and W. Burgard,
"3-D Mapping With an RGB-D Camera," in IEEE Transactions
on Robotics, vol. 30, no. 1, pp. 177-187, Feb. 2014.
[2] M. Labbé and F. Michaud, "RTAB-Map as an Open-Source Lidar
and Visual SLAM Library for Large-Scale and Long-Term Online
Operation, " Journal of Field Robotics, vol. 36, no. 2, pp. 416–446. 2019.
[3] E. Nelson, BLAM: berkeley localization and mapping,
[online]. Available: https://github.com/erik-nelson/blam.

 Propose a new SLAM system for AMIRs that can generate both 2D & 3D map and
better than other available methods which can be adopted to AMIR.
 Adopt our SLAM system to autonomous mobile manipulation, having the good
performance than other available methods.
Objective
8

 Autonomous Mobile Industrial Robot (AMIR) with Robot Operating System (ROS)
 = Autonomous Mobile Robot (AMR) + Industrial Robot (IR)
Robotic System
9
Coordinate system Sensors on AMIR Sub robotic system

 Robotic System
 Methodology
Outline
10

The Backbone of SLAM
11
Pose Graph
Node i (pose i)
Error (residual)
(prediction ij) Node i (pose j)
𝑒𝑖𝑗 = 𝑧𝑖𝑗 − 𝑧𝑖𝑗
x𝑓𝑜𝑜𝑡𝑝𝑟𝑖𝑛𝑡
⋆
= arg min Ω𝑖𝑗
1
2𝑒𝑖𝑗
2

A simple example of pose graph optimization
12
1. Formula
3. Ans:
2. Solve in least square: 𝑥∗
, 𝑙∗
= arg min
𝑥,𝑙
𝑐(𝑥, 𝑙)
https://blog.csdn.net/heyijia0327/article/details/47686523

AMIR SLAM Architecture (1/2)
13
 Cartographer 2D SLAM
[1] W. Hess, et al. "Real-time loop closure in 2D LIDAR
SLAM." 2016 IEEE International Conference on Robotics
and Automation (ICRA). IEEE, 2016.
[1]
Edges (bright yellow)
Loop closure detection Scan-to-Submap egomotion
Pose graph:
Node: footprint poses & submap center
Edge: scan-to-submap matching
(a footprint pose coupled with a scan)

14
 Multi-sensor fusion
 Based on Cartographer 2D
SLAM and extended to 3D

Graduate Institute of Electrical Engineering 15
 In detailed structure:

Point Cloud Filter
16
RGB-D
Camera
𝑐𝑎𝑚𝑒𝑟𝑎 𝑡
𝑃𝑐𝑡
5cm*5cm*5cm
A single frame
=

 Using the kinematic data to transform point clouds to local submap coordinate,
then accumulated into a 3D submap by Iterative Closest Point (ICP)
Submap Builder and Refiner
17

Global Optimizer
18
x𝑓𝑜𝑜𝑡𝑝𝑟𝑖𝑛𝑡
⋆
= arg min Σ𝑖𝑗
−
1
2𝑒𝑖𝑗
2
𝑒𝑖𝑗 =
𝑅 𝑞𝑖
𝑇
𝑝𝑗 − 𝑝𝑖 − 𝑝𝑖𝑗
2.0𝑣𝑒𝑐 𝑞𝑖
−1
𝑞𝑗 𝑞𝑖𝑗
−1
𝑝𝑎,𝑎+1 = 𝑅 𝑞𝑎∗
𝑇
(𝑝 𝑎+1 ∗ − 𝑝𝑎∗)
𝑞𝑎,𝑎+1 = 𝑞𝑎∗
−1
𝑞 𝑎+1 ∗
𝑝𝑎,𝑏 = 𝑅 𝑞𝑎∗
𝑇
(𝑝𝑏† − 𝑝𝑎∗)
𝑞𝑎,𝑏 = 𝑞𝑎∗
−1
𝑞𝑏†
1
𝑁 𝑖=0
𝑁
𝑆𝑢𝑏𝑚𝑎𝑝𝑏
𝑖
−
1
𝑀 𝑗=0
𝑀
𝑆𝑢𝑏𝑚𝑎𝑝𝑎
𝑗
< 𝑑𝑠𝑖𝑚𝑖𝑙𝑎𝑟
𝑅𝑏, 𝑡𝑏 = SVD based ICP alignment (𝑆𝑢𝑏𝑚𝑎𝑝𝑎, 𝑆𝑢𝑏𝑚𝑎𝑝𝑏)
Approximate detection constraints
(submap-to-submap matching)
Cartographer poses constraints Least square optimization
Pose graph:
Node: footprint poses
Edge: submap-to-submap matching
& cartographer poses constraints
(a footprint pose coupled with a 3D submap)
Pose graph in
MIT Dataset
 To achieve global consistent, we construct a new pose graph to adjust our 3D submaps
Solve in Ceres solver (LM+Cholesky)

 Finally, we compose all the submaps by transforming them into world coordinate
according to the new optimized poses, getting global 2D and 3D occupancy grid
maps (octomap) or point cloud map.
Global Map Builder (AMIR Map Register)
19

 Robotic System
 Methodology
Outline
20

 Divide into two stages: navigation and manipulation. For the safety and simplicity, the
manipulator will keep in an idle pose during navigation
Autonomous Mobile Manipulation System Architecture
21

Navigation & Manipulation
22
Navigation with collision avoidance Manipulation with collision avoidance
(Moveit [2])
(move_base [1])
[1] M. E. Eitan, “move_base: ROS navigation stack”, [online]. Available: http://wiki.ros.org/move_base
[2] S. Chitta, I. Sucan and S. Cousins, "Moveit![ros topics]." IEEE Robotics & Automation Magazine , 19.1, pp.18-19, 2012.

 Robotic System
 Methodology
Outline
23

Experiment Overview
24
1.
2.
3. 4.
5.
6.
7. 8.

Experiment – SLAM Evaluation Metric
25
 Quantitative evaluation:
 Absolute Trajectory Error (ATE) [1]. The absolute distances between ground
truth trajectory 𝑇𝑔𝑡
1:𝑛
and estimated trajectory 𝑇𝑒𝑠𝑡
1:𝑛
at time step i. Rigid-body
transformation S is the least-squares solution that maps 𝑇𝑒𝑠𝑡 onto 𝑇𝑔𝑡 by method
of Horn
 Qualitative evaluation:
 We will do qualitative comparison on the map with specific scene and the whole
appearance of map.
[1] J. Sturm, et al. "A benchmark for the evaluation of RGB-D SLAM systems." 2012
IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2012.

Experiment – Public Dataset
26
 MIT Stata Center Dataset [1]
 Total distance: 361.75 (m). Duration: 667 (s). Recorded in the second floor
 The ground truth position is estimated by
several optimized small batch scans
which align to the floor plan (to typical
accuracy of 2cm)
 Including laser scan data, RGB-D data,
filtered odometry data, kinematic data
[1] M. Fallon, et al. "The mit stata center dataset." The International
Journal of Robotics Research 32.14 (2013): 1695-1699.

 2D floorplan and scene appearance
Experiment – Public Dataset
27

 Robotic System
 Methodology
Outline
28

Experiment I – Ablation Study (1/7)
29
 We will incrementally
decompose the components of
AMIR SLAM system and see
how they can help to improve
the performance.

 The mapF is the final result of using
the whole procedure in AMIR SLAM
 Giving the best performance
30

31
 For mapE, the final refiner in
global map register is removed
 Defects such as ghost point clouds
of human (3, 9, 10) and slightly
mismatching on the wall of room
and corridor (1, 2, 4, 5, 6, 7, 8)

32
 For mapD, the global optimizer is
further removed
 Only Updating the map with
cartographer maintained pose graph
 Hardly mismatching on the objects
and walls (1, 2, 3, 4, 5).

33
 Quantitative comparison with
cartographer on trajectory

34
 For mapC, the submap refiner is
further removed
 There are some walls which
strongly mismatching on the map
(1, 2, 3, 4).

35
 For mapB, the point cloud filter is
further removed.
 We can see that there is blur and
noisy everywhere
 The sensing range of RGB-D
camera is around 0.4m~5m

36
 For mapA, the submap builder is
further removed
 Naïve register point cloud to map
based on current pose
 The corridor and the rooms are not
overlapped together (1, 2, 3, 4)

 Robotic System
 Methodology
 SLAM System Architecture
Outline
37

 The quantitative result
 Our approach is the
best among other
RGB-D based methods
Experiment II –SLAM Evaluation on Public Dataset (1/6)
38

 The 2D & 3D map result of our
AMIR SLAM approach
 Using RGB-D data, laser scan data,
filtered odometry data and kinematic
data as input
39

 The result of RGBDSLAMv2 (RGB-D only) [1]
 Only using RGB-D point cloud as input
 Hardly drift in z direction, fail to close loop, noisy
40
[1] F. Endres, J. Hess, J. Sturm, D. Cremers and W. Burgard, "3-D Mapping With an RGB-D Camera," in
IEEE Transactions on Robotics, vol. 30, no. 1, pp. 177-187, Feb. 2014.

41
 The result of RGBDSLAMv2 [1]
 Using RGB-D data, filtered odometry data and kinematic data
 Drift in z direction, bad in loop closing, noisy
[1] F. Endres, J. Hess, J. Sturm, D. Cremers and W. Burgard, "3-D Mapping With an RGB-D Camera," in
IEEE Transactions on Robotics, vol. 30, no. 1, pp. 177-187, Feb. 2014.

42
 The result of RTAB-Map (RBG-D only) [1]
 Only using RGB-D data as input
 Defects such as the ghost human point
clouds (12, 13), the objects are blur and not
overlapped (1, 6, 11), the walls are
mismatching (2, 4, 5, 7, 9, 10).
 The egomotion is error on translation
[1] M. Labbé and F. Michaud, "RTAB-Map
as an Open-Source Lidar and Visual SLAM
Library for Large-Scale and Long-Term
Online Operation, " Journal of Field
Robotics, vol. 36, no. 2, pp. 416–446. 2019.

43
 The result of RTAB-Map [1]
filtered odometry data and kinematic
data as input. Same with ours.
 Defects such as the ghost human point
clouds (2,8), the objects are blur and
not overlapped (1, 7), the walls are
mismatching (6, 9, 11,12)
[1] M. Labbé, et, al.
"RTAB-Map as an Open-
Source Lidar and Visual
SLAM Library for Large-
Scale and Long-Term
Online Operation, "
Journal of Field Robotics,
vol. 36, no. 2, pp. 416–
446. 2019.

 Ours (scan matching based) v.s. RTAB-Map result (appearance based)
44

 Robotic System
 Methodology
Outline
45

Experiment – Mapping with Our Robot (1/2)
46
 Our AMIR can automatically rotate joint 0 and joint 4
repeatedly to sense the environment more widely
 It can also go the
assigned pose
through joystick
Automatically rotate joint 0 and joint 4 Sensors on our AMIR
Go to the assigned pose

Experiment – Mapping with Our Robot (2/2)
47
 Including laser scan data, RGB-D data, odometry data, kinematic data, 3D Lidar data
 The total distance: 50.33 (m). and duration: 972 (s). Recorded in our lab Room 304 at NTU
 The ground truth is computed from well fine-tuned 3D LiDAR based method in offline (Acc. < 5cm)

Experiment III – SLAM Evaluation on Our Robot (1/5)
48
 The quantitative result
 Our approach is the best among
other 3D based methods

49
 The 2D & 3D map
result of our AMIR
SLAM approach

 The map is the most
complete among all,
while the mapping
quality is not very
good. The point cloud
of objects are sparse,
and the surface is
unclear
[1] E. Nelson, BLAM: berkeley localization
and mapping, [online]. Available:
https://github.com/erik-nelson/blam.
 BLAM [1]
 Using 3D LiDAR data only
(Projected
2D map)
(With projected 2D map)

 RTAB-Map [1] (RGB-D only)
 Only using RGB-D data as input
 The completeness of the map is not very
good. The aggressive motion may cause
appearance-based RTAB-Map (RBG-D only)
method easily to get lost.
(With projected 2D map)
(Projected 2D map)
[1] M. Labbé and F. Michaud, "RTAB-Map as an Open-
Source Lidar and Visual SLAM Library for Large-Scale
and Long-Term Online Operation, " Journal of Field

 RTAB-Map [1]
odometry data and kinematic data as input.
Same with ours.
 There are several mismatches on walls
and noises around the objects.
 Appearance based
v.s.
Scan matching based (With projected 2D map)
(Original 2D map)
[1] M. Labbé and F. Michaud, "RTAB-Map
as an Open-Source Lidar and Visual SLAM
Library for Large-Scale and Long-Term
Online Operation, " Journal of Field

 Robotic System
 Methodology
Outline
53

 Goal: Collecting 1 product from conveyor to white desk with collision avoidance
 Comparing with 4 different configuration in Moveit, each one is tested by 4 times
Experiment IV: Station-to-Station Autonomous Mobile Manipulation Test (1/5)
54
Target Object

Our approach is obviously
the only one completing the
whole autonomous mobile
manipulation pipeline and
collision-free
55

If we use no information,
the task is interrupted
due to the collision on
conveyor
56

If we use hand-crafted
geometry on conveyor,
the task is interrupted
due to the collision on
white desk which is out
of define

If we use local perception
in Moveit, the task is
incomplete due to the
object is mapped to the
local map which making
the goal unreachable

 Robotic System
 Methodology
Outline
59

 Goal: Collecting 3 products from conveyor and deliver to 3 different stations
 Scenario: Pick 3 products from conveyor → Place on robot base
Navigate to white desk A → Pick 1 products from robot base → Place on white desk A
Navigate to white desk B → Pick 1 products from robot base → Place on white desk B
Navigate to machine tool → Pick 1 products from robot base → Place on machine tool
Experiment V: Multi-Station Autonomous Mobile Manipulation Demo (1/2)
60

 The plot of the trajectory of the end effector, the total duration is 287 sec.
 The task is successfully complete and the whole journey is collision-free.
Experiment V: Multi-Station Autonomous Mobile Manipulation Demo (2/2)
61

Video Demonstration
62

 We develop an extension which takes the advantage of the AMIR and enhances the 2D
SLAM algorithm to a 2D and 3D mapping system for the AMIR. Also, our ablation study
shows how it can improve the performance better than original one.
 We propose the AMIR SLAM system that specifically designed for AMIRs that is
experimentally better than other available methods on public dataset as well as our robot.
 We integrate AMIR SLAM system with the autonomous mobile manipulation system,
achieving more comprehensive and convenience on obstacle avoidance than other candidate
methods in the experiment.
 Our successful autonomous mobile manipulation demonstration shows that our 3D SLAM
system on the AMIR plays a key role in autonomous mobile manipulation which is the most
important foundation of many robotic applications.
Conclusions & Contributions
63

 Our system can be adapted to more applications in intelligent service applications not
limited to our demonstration such as the applications in household, laboratory, café,
restaurant, hospital, factory, etc.
Future works
64
 In pace with 5G network, with
argument, virtual and mixed reality
development, it is able to construct a
mixed world between reality and
virtuality with our map information.
Making our robot collaborate with
people more interactive.
Ref: https://www.nec.com/en/global/insights/article/2020022509/index.html

學歷：
1. 民國109年7月國立台灣大學電機工程學研究所畢業
2. 民國107年6月國立台灣大學機械工程學系畢業
3. 民國103年6月台北市立和平高級中學畢業
發表著作：
R. C. Luo, S. L. Lee, Y. C. Wen, and C. H. Hsu, " Modular ROS Based Autonomous Mobile Industrial Robot System
for Automated Intelligent Manufacturing Applications," 2020 IEEE/ASME International Conference on Advanced
Intelligent Mechatronics (2020 AIM), Boston, July 2020. (Accepted)
R. C. Luo and S. L. Lee, " Autonomous Mobile Industrial Robot with Multi- Sensor Fusion based Simultaneous
Localization and Mapping," in IEEE Access, 2020. (Submitted)
榮譽事蹟：
民國108年8月參加「2019年全國機器人智機化應用競賽」榮獲冠軍
參與開發：
自主移動工業機器人Autonomous Mobile Industrial Robot (AMIR)
VITA
65

Thank you :)
Q & A

Appendix
(Supplementary Material)

Study Case: A Mobile Robotic Chemist
 Robotic chemist in chemical laboratory
68
Burger, B., Maffettone, P.M., Gusev, V.V. et al. A mobile robotic chemist. Nature 583, 237–241
(2020). https://doi.org/10.1038/s41586-020-2442-2

Octomap (octree)
69
A.Hornung, K.M.Wurm, M.Bennewitz, C.Stachniss and
W.Burgard. "OctoMap: An efficient probabilistic 3D
mapping framework based on octrees." Autonomous
robots, 34.3, pp. 189-206, Apr. 2013.

Extend Kalman Filter (EKF)
70
T. Moore and D. Stouch, "A generalized extended kalman filter implementation for the robot
operating system." Intelligent autonomous systems 13. pp. 335-348, Springer, Cham, 2016.

Madgwick Filter
71
 An Orientation algorithm designed to support a computationally efficient.
 It is applicable to inertial measurement units (IMUs) consisting of tri-axis gyroscopes
and accelerometers, and magnetic angular rate and gravity (MARG) sensor arrays that
also include tri-axis magnetometers.
 The MARG implementation incorporates magnetic distortion compensation. The
algorithm uses a quaternion representation, allowing accelerometer and magnetometer
data to be used in an analytically derived and optimised gradient descent algorithm
to compute the direction of the gyroscope measurement error as a quaternion derivative
S.O.H.Madgwick, A.J.L.Harrison and R.V aidyanathan, "Estimation of IMU and MARG orientation using a gradient
descent algorithm," 2011 IEEE International Conference on Rehabilitation Robotics, Zurich, 2011, pp. 1-7.

Time Elastic Band Local Planner
The ”timed elastic band” approach optimizes robot trajectories by subsequent modification of an
initial trajectory generated by a global planner. The objectives considered in the trajectory
optimization include but are not limited to the overall path length, trajectory execution time,
separation from obstacles, passing through intermediate way points and compliance with the
robots dynamic, kinematic and geometric constraints. It is formulated as a scalarized multi-
objective optimization problem, solved by g2o.
72
C. Rösmann, F. Hoffmann and T. Bertram: Integrated online trajectory planning and optimization
in distinctive topologies, Robotics and Autonomous Systems, Vol. 88, 2017, pp. 142–153.

Dijkstra global planner
73
Shortest path Fast exploration
Dijkstra’s A* (A star)

RRTConnect Planner
 Implement in OMPL (open motion planning library) used by Moveit in default
 RRTConnect: a state-of-the-art sampling-based motion planning algorithms
 It incrementally builds two rapidly-exploring random trees rooted at the start
point and the goal point, and then find the feasible path (edges) from the start
point to the goal point without collision quickly.
74
J. J. Kuffner and S. M. LaValle, "RRT-connect: An efficient approach to single-query path planning," Proceedings 2000 ICRA. Millennium Conference. IEEE International
Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065), San Francisco, CA, USA, 2000, pp. 995-1001 vol.2, doi: 10.1109/ROBOT.2000.844730.
Rapidly-exploring random tree (RRT) RRTConnect

Odometry Fusion on MIT Dataset
75
Total distance: 361.75 (m). Duration: 667 (s)
recorded the data in the second floor

Odometry Fusion on Our Robot
Odometry comparison, having total
distance: 52.67 (m) and duration: 229 (s).
76

MIT Stata Dataset Ground Truth
 The dataset also includes ground truth position estimates of the robot at every
instance (to typical accuracy of 2cm). For a small batch of laser poses (e.g. 160
scans or 4 seconds), they align the start and end scans to the floor plan and carry
out incremental LIDAR scan matching in between. They then construct a small pose
graph optimisation problem. Relaxing the pose graph (using iSAM) produces the
final ground truth poses for the small batch. This process is repeated for each
subsequent batch of scans. The scan matching mentioned above uses the Fast and
Robust Scan Matching which produces very low drift rates in all situations.
77
M. Fallon, et al. "The mit stata center dataset." The International
Journal of Robotics Research 32.14 (2013): 1695-1699.

AMIR-SLAM: Autonomous Mobile Industrial Robot Simultaneous Localization and Mapping

Recommended

Recommended

More Related Content

Recently uploaded

Recently uploaded (20)

Featured

Featured (20)

AMIR-SLAM: Autonomous Mobile Industrial Robot Simultaneous Localization and Mapping

Editor's Notes