Transcript thesis (pptx)

Natchanon Wongwilai
Advisor: Asst. Prof.Nattee Niparnan, Ph.D.
1
•
•
•
•
Introduction
Objective
Overview
Our work
• System Calibration
• Fingertips placement planning
• Experiment
• Conclusion
2
“The most common failure mode I've seen is that the closing
fingers bump the object so that the fingers don't touch the
intended contact points. Then the fingers knock the object
completely out of the grasp. I think the causes are localization
errors from the perception system and asking the robot to carry
out an inherently dynamic task that was planned with static
analysis tools.”
Jeff Trinkle
GRSSP Workshop 2010
3
Modeling
Grasp planning
Localization
Grasping
4
Grasping
5
Grasp planning
Modeling
Localization
• To implement the accurate fingertip placement system
• System calibration error
• Hand(fingertips)-object localization error
• Object modeling error
6
• System calibration
• Fingertip placement planning
7
RGB image
Depth image
For DepthSense 325 and Katana 6M180
8
Without our calibration
With our calibration
9
• DepthSense 325 calibration
• Intrinsic & Extrinsic calibration (Depth & RGB camera)
• Depth value correction
• Extrinsic calibration (Katana 6M180 & DepthSense 325)
10
• Using canonical calibration process by OpenCV
(Both depth & RGB camera)
RGB image
Depth image
Confidence level image
11
𝐷𝑖 𝑥 = 𝑎𝑖 𝑥 + 𝑏3𝑖 cm
0 cm
สัมประสิทธิ์
𝑎𝑖
𝑏𝑖
ค่ าเฉลี่ย ค่ าเบี่ยงเบน
มาตรฐาน
1.045
0.022
0.016
0.004
ค่ าต่าสุด
ค่ าสูงสุด
0.920 1.145
-3 cm
-0.031 0.028
12
• Calibration of an Eye-in-Hand System using SoftKinetic
DepthSense and Katana Robotic Arm (N.Wongwilai, N. Niparnan, and A.
Sudsang, 2014)
13
14
15
16
17
18
• 𝑁𝑎𝑣𝑔 times average of depth image
• Plane remover
• RANSAC
• Outliers remover
• Statistical model
• Sampling
• Voxel grid
19
20
• Key frame addition
4
1
1
1
2
3
2
1
3
2
21
• Converted to graph-based optimization problem
• Nodes = Key frames (robot poses)
• Edges = Association between nodes (using 3D object data)
4
4
4
4
1
3
2
• Using TORO
(Grisetti, G., et al, 2007) as
optimizer
22
• 𝑖𝑡ℎ key frame addition
• Robot pose 𝑥𝑖
• Current robot pose related to world frame (form robot’s encoders)
• Measurement 𝑧𝑚,𝑖
• Calculate from 𝑇 ∗ that minimize this error function (ICP point to plane)
𝑀𝑖
𝑐𝑜𝑟𝑟(𝑘)
𝑇 ∗ 𝑝𝑖𝑘 − 𝑝𝑚
𝐸𝑚,𝑖 (𝑇 ∗ ) =
𝑐𝑜𝑟𝑟(𝑘)
2
∙ 𝒏𝑚
𝑘=1
23
𝑤𝑡
𝐸𝑚,𝑖
0
0
0
0
0
0
𝑤𝑡
𝐸𝑚,𝑖
0
0
0
0
0
0
𝑤𝑡
𝐸𝑚,𝑖
0
0
0
0
0
0
𝑤𝑟
𝐸𝑚,𝑖
0
0
0
0
0
0
𝑤𝑟
𝐸𝑚,𝑖
0
0
0
• 𝑤𝑡 and 𝑤𝑟 are weighting factor
0
0
0
𝑤𝑟
𝐸𝑚,𝑖
• Information matrix 𝑚,𝑖 =
• Optimize with TORO
• Input:
• Output:
a graph (a set of nodes 𝒙 and set of edges 𝑧𝑚,𝑛 with
information matrix 𝑚,𝑛 )
a new set of robot pose 𝒙
24
25
KD-Tree
KD-Tree
..
.
Closest point
averaging
..
.
KD-Tree
26
• Object model: Bounding box using PCA
• 𝑝𝑐𝑚 (Center of bounding box)
• 𝑑𝑥 , 𝑑𝑦 and 𝑑𝑧
• Rotation (related to world frame)
z
x
y
27
Out of scope
28
• Input
• Object model (bounding box, …)
• Gripper parameter (number of fingers, …)
• Output
• Grasping points (respect to object frame)
• Approach direction
29
30
• Calculate target robot pose (respect to world frame)
• Calculate target end effector frame (related to object frame) from
calculated grasping points
• Object frame (related to world frame) is known
• Navigate robot to target pose
• Using robot’s SDK
• Linear movement
31
32
• Depth camera: Minimum distance is 15 cm
Depth image
33
• Project 3D points to image plane
• Pyramids Lucas-Kanade optical flow (Bouguet, J.-y., 2000)
34
• Estimate 𝑇 ∗ that minimize projection error
𝑢𝑖′ − 𝑝𝑟𝑜𝑗(𝑇 ∗ ∙ 𝑝𝑖 )
𝐸𝑝𝑟𝑜𝑗 (𝑇 ∗ ) =
𝑢𝑖′ 𝜖𝑈 ′
• Using RANSAC
• with Levenberg-Marquardt optimization
• Loop (with same 3D points data) until
• Reached to target pose
• Optical flow tracking points < 20 points
35
Implementation
Method
1
Object model update
2
3
4
5
(Ours)









Robot pose correction
Robot pose optimization

Robot pose update using RGB
camera
Robot navigation
1
step
n
step
n
step
n
step
n
step
36
37
• Localization error
• Object modeling error
• Fingertip placement error
Implementation 1-4
Implementation 5
38
39
40
41
42
43
วิธกี าร
วิธีการที ่ 1
วิธีการที ่ 2
วิธีการที ่ 3
วิธีการที ่ 4
วิธีการที ่ 5
วิธกี าร
วิธีการที ่ 1
วิธีการที ่ 2
วิธีการที ่ 3
วิธีการที ่ 4
วิธีการที ่ 5
ความผิดพลาดของตาแหน่ ง (มิลลิเมตร)
กล่อง
ทรงกระบอก
มุมฉาก
กล่องรูปตัวที
20.52
13.59
9.25
9.14
7.33
กล่อง
24.16
14.31
9.52
9.48
8.40
30.35
28.56
8.98
8.91
7.35
21.85
14.72
7.71
7.55
6.42
ความผิดพลาดของทิศทาง (องศา)
ทรงกระบอก
มุมฉาก
กล่องรูปตัวที
4.22
2.47
1.31
1.21
1.26
5.65
3.93
1.55
1.52
1.57
8.82
6.93
1.04
0.95
0.72
4.01
2.23
1.02
0.94
0.99
44
วิธกี าร
วิธีการที ่ 1
วิธีการที ่ 2
วิธีการที ่ 3
วิธีการที ่ 4
และ วิธีการที5่
ความผิดพลาดของแบบจาลองวัตถุ (ร้ อยละ)
กล่อง
ทรงกระบอก
มุมฉาก
กล่องรูปตัวที
52.72
39.73
23.79
57.76
50.39
29.45
62.83
60.61
21.40
58.53
45.75
17.53
18.47
20.34
12.87
16.91
45
วิธกี าร
วิธีการที ่ 1
วิธีการที ่ 2
วิธีการที ่ 3
วิธีการที ่ 4
และ วิธีการที5่
62.83 %
ความผิดพลาดของแบบจาลองวัตถุ (ร้ อยละ)
กล่อง
ทรงกระบอก
มุมฉาก
กล่องรูปตัวที
52.72
39.73
23.79
57.76
50.39
29.45
62.83
60.61
21.40
58.53
45.75
17.53
18.47
20.34
12.87
16.91
20.34 %
46
วิธกี าร
วิธีการที ่ 1
วิธีการที ่ 2
วิธีการที ่ 3
วิธีการที ่ 4
และ วิธีการที5่
ความผิดพลาดของแบบจาลองวัตถุ (ร้ อยละ)
กล่อง
ทรงกระบอก
มุมฉาก
กล่องรูปตัวที
52.72
39.73
23.79
57.76
50.39
29.45
62.83
60.61
21.40
58.53
45.75
17.53
18.47
20.34
12.87
16.91
47
48
49
วิธกี าร
วิธีการที ่ 1
วิธีการที ่ 2
วิธีการที ่ 3
วิธีการที ่ 4
วิธีการที ่ 5
กล่อง
10
16
20
20
20
วิธกี าร
วิธีการที ่ 1
วิธีการที ่ 2
วิธีการที ่ 3
วิธีการที ่ 4
วิธีการที ่ 5
ความผิดพลาดของตาแหน่ ง (มิลลิเมตร)
ทรงกระบอก
มุมฉาก
20.12
13.70
8.54
8.04
6.01
27.03
19.52
9.73
9.13
7.04
15
18
20
20
20
39.36
29.43
8.27
8.07
5.84
9
14
20
20
20
ความผิดพลาดของทิศทาง (องศา)
ทรงกระบอก
มุมฉาก
กล่อง
10
16
20
20
20
8
14
19
20
20
กล่องรูปตัวที
5.14
3.58
2.40
2.31
2.38
8
14
19
20
20
5.94
4.12
2.49
2.49
2.52
15
18
20
20
20
22.28
16.86
9.51
8.79
6.39
กล่องรูปตัวที
4.92
3.79
1.65
1.49
1.45
9
14
20
20
20
3.20
1.85
1.06
1.05
1.10
50
51
52
• Proposed fingertip placement planning method
• More accurate than native grasping approach
• Localization
• Modeling
• Fingertip placement
• Can be applied to grasping real objects showed in pick-and-place task
53
54
ECTI-CON 2014 (N.Wongwilai, N. Niparnan, and A. Sudsang, 2014)
55
• 𝒖 = (𝑢𝑥 , 𝑢𝑦 , 𝑢𝑧 )𝑇
• 𝒖/ 𝒖 ∙ 𝒗𝑖 = cos(𝜃)
• 𝐴𝒖 = 𝑏
• 𝐴 = (𝒗0 , 𝒗1 , … , 𝒗𝑖 )𝑇
• 𝑏 = (1,1, … , 1)𝑇
• Finding 𝒖 with length of
1/ cos(𝜃) using linear least
square
56
• 𝑎𝑖 𝑥 + 𝑏𝑖 𝑦 + 𝑑𝑖 = 0
• 𝒄 = (𝑝, 𝑞, 𝑟)
• 𝐴𝒄 = 𝑏
• 𝐴 is a 𝑛 × 3 matrix where each
row is (𝑎𝑖 , 𝑏𝑖 , −1)
• 𝑏 = (𝑑1 , 𝑑2 , … , 𝑑𝑛 )
• Finding 𝒄 using linear least
square
57
𝑖
• 𝐷𝑖 = (𝒑4 − 𝑐 ) ×
(𝑖)
𝒛5
• Finding 𝒑4 that minimize
•
𝑑(𝐷𝑖2 )
𝑑𝒑4
= 2 𝒑4 − 𝑐
𝑖
𝑚
2
𝐷
𝑖=1 𝑖
𝑖
− 2𝒛5
𝒑4 − 𝑐
𝑖
𝑖
∙ 𝒛5
=0
58
• Calculate the intersection of a line passing through 𝒑4 in the
(𝑖)
direction of each 𝒛5 with chessboard plane
• These points should lie on a line 𝑙𝑥4
(𝑖)
• The projection of 𝑙𝑥4 on a plane whose normal is 𝒛5 is the
direction of 𝑥4
59
วัตถุ
ทรงกลม
ทรงกระบอก
กล่อง
RMSE (เซนติเมตร)
ทั ้งหมด พารามิเตอร์ ภายนอก ค่าความลึก
0.426
0.515
0.692
0.714
0.928
1.533
1.248
2.041
3.670
ไม่มี
0.726
1.998
4.986
60
• Robot build up a map and localize itself simultaneously while
traversing in an unknown environment
• Robot location
Hand(Fingertips) location
• Environment map
Object model
61
• Node: Robot pose (𝑥𝑖 )
• Edge: Relative transform (𝑧𝑖𝑗 ) with
information matrix (𝑖𝑗 )
• Error:
𝑒𝑖𝑗 𝑥𝑖 , 𝑥𝑗 = 𝑧𝑖𝑗 − 𝑧𝑖𝑗 𝑥𝑖 , 𝑥𝑗
• Goal: Finding 𝒙 = (𝑥1 , … , 𝑥𝑖 )𝑇 that
minimize error likelihood
𝑇
𝑒𝑖𝑗
𝑖𝑗 𝑒𝑖𝑗
𝐹 𝒙 =
𝑖,𝑗 ∈𝐶
62
• Tree-based network optimizer (TORO) (Grisetti, G., et al, 2007)
• Optimizing with tree parameterization (Fast)
• Solved error distributing problem in 3D (Accurate)
• Ease of implementation (Available source code)
63
• Before 𝑖𝑡ℎ key frame addition
• Robot pose: 𝒙 = 𝑥1 , … , 𝑥𝑖−1 𝑇
• Measurement: 𝑧𝑚,𝑛 with information matrix 𝑚,𝑛
n\
m
1
2
3
𝟒
𝟓
…
1
…
2
…
3
…
𝟒
…
𝟓
…
…
…
…
…
…
…
…
𝒊−𝟑
…
𝒊−𝟐
…
𝒊−𝟏
…
𝒊−𝟑
𝒊−𝟐
𝒊−𝟏
…
…
…
64
65
66
67
68