Vector Distance Transform Maps for Autonomous Mobile Robot Navigation

Vector Distance Transform Maps for
Autonomous Mobile Robot Navigation
Janindu Arukgoda
20 December 2019

Content
Background
List of Contributions
Contribution 1 → Contribution 6
Conclusions
List of Publications
1

Content
Background
Conclusions
2

Background
Fundamental questions of autonomous mobile robot
navigation :
Where am I?
Where am I going?
How do I get there?
(Hugh Durrant-Whyte and John Leonard, 1989)
common requirement : Map
3

Background
Map
Mathematical representation of the environment
Machine understandable
Required to answer all three questions
Examples
Feature maps (landmark maps, point cloud maps)
Occupancy grid maps (OGM)
Distance transform (DT) maps
4

Background
Property
Feature
Maps
OGM DT
Clearly deﬁned occupied
and free spaces

Analytical observation
equation

Continuous representa-
tion

No feature extraction /
loss of information from
sensor measurement

5

Background
Distance Transforms
Originated in the image processing domain
For a 2D binary image where pixels belong either to the set S,
the set of 1’s that belong to objects, or the set ¯S, the set of
0’s that belong to the background, a distance map or a
distance transform L(S) is an image deﬁned as
L(x) = min(d[(i, j), ¯S]) ∀x = (i, j) ∈ S (1)
The distance function d is a positive deﬁnite, symmetric and
triangular function
6

Background
Distance Transform Variants
Unsigned DT : distance to the closest object
Signed DT : distance assigned a sign based on the
concept of inside vs outside
Vector DT : distance described using orthonormal vectors
7

Content
Background
Conclusions
8

1. Vector distance transforms (VDT) for environment
representation
2. Sensor model on VDT maps
3. Optimization framework for localization on VDT maps
4. EKF framework for localization on VDT maps
5. Active localization using sparse range measurements
6. Capturing the uncertainty of VDT maps
9

Content
Background
Contribution 1 : VDT for Environment Representation
Conclusions
10

VDT for Environment Representation
Figure: Box shaped environment
11

(a) (b)
(c) (d)
Figure: (a) UDT, (b) SDT, (c) x component of VDT and (d) y
component of VDT for box shaped environment
12

Behavior of Distance Transforms
(a) (b)
(c) (d)
Figure: (a) UDT, (b) SDT, (c) x component of VDT and (d) y
component of VDT along the dotted line in box shaped
environment
13

Behavior of Interpolated Distance Transforms
(a) (b)
(c) (d)
Figure: The behaviour of cubic spline interpolations of (a) UDT,
(b) SDT, (c) x component of VDT and (d) y component of VDT
close to the boundary 14

Gradients of Distance Transforms at the Boundary
(a) (b)
(c) (d)
Figure: Cubic spline interpolations of the gradients along the x
direction of (a) UDT, (b) SDT, (c) VDT x and (d) VDT y close to
the boundary 15

Behavior of Distance Transforms at Cut Locus
(a) (b)
(c) (d)
Figure: Cubic spline interpolations of the gradients along the x
direction of (a) UDT, (b) SDT, (c) VDT x and (d) VDT y close to
the cut locus 16

Property UDT SDT VDT
Capable of represent-
ing complex 2D environ-
ments

Continuous gradients on
boundaries

Continuous function on
cut locus

17

Content
Background
Contribution 2 : Sensor Model for VDT Maps
Conclusions
18

Sensor Model for VDT Maps
Sensor Model associates sensor measurements
with the map
given robot state
Likelihood of an observation given the robot state
P(z | x, m) (2)
z : observation x : robot state m : map / internal
representation of the environment
19

Given a VDT map, an observation zi ∈ Z; zi = {ri, θi} and
an estimate for robot pose XR = (xR, yR, φR) , what is the
disparity between the actual and the expected observation?
Xo =
xoi
yoi
=
xR + ri sin(θ + φi)
yR + ri cos(θ + φi)
(3)
20

dV DT = V DT(Xo) =














DTx(Xo1 )
DTy(Xo1 )
.
.
DTx(Xoi
)
DTy(Xoi
)
.
DTx(Xon )
DTy(Xon )














(4)
21

Assuming
1. Each range measurement is independent within a laser
scan
2. Range measurement noise σr is the only contributing
factor to the sensor noise
Sensitivity of dV DT due to sensor noise is
ΣdV DT
= diag(σ2
DTx,r1
, σ2
DTy,r1
, ..., ..., σ2
DTx,ri, σ2
DTy,ri, ..., ) (5)
22

σ2
DTx,ri
= JDTx,ri
· σ2
r · JDTx,ri
σ2
DTy,ri
= JDTy,ri
· σ2
r · JDTy,ri
(6)
JDTx,ri
=
∂dDTx
∂r
ri
=
∂DTx
∂xoi
xoi
·
∂xoi
∂r ri
+
∂DTx
∂yoi
yoi
·
∂xoi
∂r
ri
JDTy,ri
=
∂dDTy
∂r
ri
=
∂DTy
∂xoi
xoi
·
∂xoi
∂r ri
+
∂DTy
∂yoi
yoi
·
∂xoi
∂r
ri
(7)
23

A scalar disparity measurement between the expected and the
actual observations, inspired by the Chamfer distance
dV CD =
2n
i=1
dV DT (i)2
=
n
i=1
DTx(Xoi
)2
+ DTy(Xoi
)2
(8)
24

Content
Background
Contribution 3 : Optimization Framework for Localization
Conclusions
25

Localization
“Where am I?” == Localization
Given
a map
sensor measurements
prior information
ﬁgure out where I am.
26

Optimization Framework for Localization
Given
S : sensor measurement
M : map
XR = (xR, yR, φR) : hypothesized robot pose
if a function f can evaluate the disparity between XR and the
observations, localization becomes a minimization problem.
ˆXR = argmin
xR,yR,φR
f(S, M) (9)
Is dV CD scalar disparity measurement suitable for this?
27

Figure: Behavior of dV CD around the true robot position with the
orientation ﬁxed at true value
28

Figure: Behavior of dV CD around the true robot orientation with
the position ﬁxed at true value
29

ˆXR = argmin
xR,yR,φR
dV CD(Xo, DTv) (10)
Initial guess required
Similar to C-LOG
30

Uncertainty estimation
Sensitivity of the pose estimate to the error in sensor
measurement Σz = diag(σ2
r )
Pose derived using non linear least squares optimization
Use implicit function theorem
cov( ˆXR) = J · Σz · J (11)
31

J = −H−1
∗



∂2dV CD
∂xr∂r
∂2dV CD
∂yr∂r
∂2dV CD
∂φr∂r


 (12)
H =



∂2dV CD
∂x2
r
∂2dV CD
∂xr∂yr
∂2dV DT
∂xr∂φr
∂2dV CD
∂xr∂yr
∂2dV CD
∂y2
r
∂2dV CD
∂yr∂φr
∂2dV CD
∂xr∂φr
∂2dV CD
∂yr∂φr
∂2dV CD
∂φ2
r


 (13)
Calculating these derivatives is possible because of the
good behavior of the VDT at the boundary
32

Simulation Dataset : Ground truth available
Figure: Trajectory estimate of the optimization framework
33

Figure: Optimization pose estimate error
34

Localization of a UAV in Kentland, VA, USA for MBZIRC 2017
Flight controller estimates roll and pitch
Height obtained from a range sensor
3DOF localization using monocular camera
Edge map of the ﬂat ground terrain represented using a
VDT
35

Edge pixels (λi, µi) projected to the ground using
hypothesized robot position (xr, yr, zr) and orientation R
and focal length f.
xoi
=
xoi
yoi
=
xr + zr
λiR1,1+µiR1,2−fR1,3
λiR3,1−µiR3,2+fR3,3
yr + zr
λiR2,1+µiR2,2−fR2,3
λiR3,1−µiR3,2+fR3,3
(14)
Figure: Bogey 5
36

Figure: UAV trajectory
37

Figure: Optimization pose error
38

Content
Background
Contribution 4 : EKF Framework for Localization
Conclusions
39

EKF Framework for Localization
Prediction using motion model
ˆXk|k−1 = g( ˆXk−1, Uk)


ˆxk|k−1
ˆyk|k−1
ˆφk|k−1

 =


ˆxk−1 + ∆T · υk · cos(ˆφk−1)
ˆyk−1 + ∆T · υk · sin(ˆφk−1)
ˆφk−1 + ∆T · ωk

 (15)
ˆXRk−1
: robot pose at step k − 1
Uk = [υk, ωk] : odometry
∆T : time between steps k and k − 1
40

Covariance propagation
Pk|k−1 = Gx · Pk−1 · Gx + Gu · Q · Gu (16)
Gx =
∂g
∂X ˆXk−1,Uk
=


1 0 −∆T · υk · sin(φk−1)
0 1 ∆T · υk · cos(φk−1)
0 0 1

 (17)
Gu =
∂g
∂U ˆXk−1,Uk
=


∆T · cos(φk−1) 0
∆T · sin(φk−1) 0
0 ∆T

 (18)
Pk−1 : pose covariance at step k − 1
Q : odometry noise
41

Observation model
Standard EKF calculates an expected measurement
Innovation is the disparity between expected and actual
measurements
In a VDT map, this disparity is implicitly captured in the
map
h( ˆXRk|k−1
, zk) = dV DT (Xok
) = 0 (19)
42

Innovation :
νk = h( ˆXRk|k−1
, zk) = dV DT (Xok
) (20)
A vector of distance values at the projected laser end points
43

Innovation covariance
Sk = Hz · Σz · Hz + Hx · Pk|k−1 · Hx (21)
Hz =
∂h
∂z ˆXk|k−1,z
(22)
Hx =
∂h
∂X ˆXk|k−1,z
(23)
44

Calculation of Kalman gain and update equations follow the
standard EKF.
Kk = Pk|k−1 · Hx · S−1
k (24)
ˆXk = ˆXk|k−1 + Kk · νk (25)
Pk = Pk|k−1 − Kk · Sk · Kk (26)
45

Simulation Dataset : Ground truth available
Figure: Trajectory estimate of the EKF framework
46

Figure: EKF pose estimate error
47

Algorithm
Mean Absolute Error
P osition(m) Orientation(rad)
Optimization (proposed) 0.0181 ± 0.0126 0.0024 ± 0.0037
EKF (proposed) 0.0235 ± 0.0223 0.0161 ± 0.0280
Optimization (C-LOG) 0.0099 ± 0.0141 0.0042 ± 0.0167
EKF (UDT) 0.0227 ± 0.0251 0.0157 ± 0.0211
Particle Filter (AMCL) 0.0296 ± 0.0243 0.0252 ± 0.0431
Table: Mean Absolute Errors : Simulation
Algorithm
Mean Squared Error
P osition(m2
× 10−3
) Orientation(rad2
× 10−3
)
Optimization (proposed) 0.4847 ± 1.700 0.0192 ± 0.2636
EKF (proposed) 1.1000 ± 2.6000 1.0000 ± 2.5000
Optimization (C-LOG) 0.299 ± 2.167 0.2975 ± 1.7000
EKF (UDT) 1.146 ± 4.492 1.4000 ± 3.7000
Particle Filter (AMCL) 1.464 ± 2.078 2.5000 ± 7.1000
Table: Mean Squared Errors : Simulation
48

Localization of a mobility scooter
Ground surface structure observations
Assume locally ﬂat terrain
Tilted depth camera
Landmark based bearing-only EKF localization
implemented by Maleen Jayasuriya
49

Figure: PMD localization framework
50

(a) (b)
Figure: (a) Ground Surface Image (b) the corresponding HED
Output Image
51

Figure: Stitched ground surface map
52

Figure: Trajectory estimate in Glebe, Sydney
53

Content
Background
Contribution 5 : Active Localization
Conclusions
54

Active Localization
LiDARs are high frequency, high resolution sensors
The EKF framework can handle sparse measurements -
even a single range measurement
Using a rotating single point laser to localize
Active localization - determining the direction of the
single point laser
Based on information gain
55

Active Localization
Figure: Active localization framework
56

Active Localization
Prediction step
standard
Observation step
similar to the previously presented step
The observation vector contains only two elements
Update step
standard
57

Active Localization
The sensor rotation range is divided into n segments
Once a segment is selected as the goal, the sensor rotates
from current position to the farthest end of the selected
segment in a sweeping motion
Sensor continuously detect environment
Once the sensor reaches the goal, calculate a new goal
58

Active Localization
Calculating the goal
Given current position estimate and uncertainty
Ray trace at ﬁxed angular resolution for hypothetical
observations
Update pose uncertainty using hypothetical observations
Pick the segment that produces the best cost function
value
For the ith
segment, the cost function is deﬁned as
cost(i) = trace(Pki) + (λ.d) (27)
Pki - Uncertainty of the pose estimate using hypothetical
observations in the ith
segment
d - angular distance between current position and ith
segment
λ - tuning parameter
59

Active Localization
Experiment
Willow Garage environment in Gazebo
8 segments covering 360 °
Range limited to 3m
Video
60

Content
Background
Contribution 6 : Uncertain VDT Maps
Conclusions
61

Uncertain VDT Maps
Desired properties of a map built from point cloud data
Continuous
Captures uncertainty of map building process
Simpler Sensor Model
62

Uncertain VDT Maps
Problem deﬁnition :
Given
a set of noisy robot poses
noisy observations obtained at said poses
how to represent the environment using the observations while
capturing the uncertainties?
63

Uncertain VDT Maps
If u(x), x ∈ 2
is the distance from point x to a set of points
S,
u(x) =⇒ UDT
u satisﬁes the Eikonal equation
| u| = 1, u|S = 0 (28)
64

Uncertain VDT Maps
Given the relationship between UDT and VDT
DTu(x, y)2
= DTx(x, y)2
+ DTy(x, y)2
(29)
A PDE can be derived as
(DTx
∂DTx
∂x
+ DTy
∂DTy
∂x
)2
+ (DTx
∂DTx
∂y
+ DTy
∂DTy
∂y
)2
DT2
x + DT2
y
= 1
(30)
With boundary conditions
DTx(x, y) = 0, DTy(x, y) = 0, ∀(x, y) ∈ S (31)
65

Uncertain VDT Maps
Parametric appriximation of a solution to a PDE
L u = f, x ∈ Ω (32)
Bu = g, x ∈ ∂Ω (33)
f, g - given functions
L , B - diﬀerential operators
∂Ω boundary of bounded open domain Ω
If a parametric function ua(x, βi) exists that is arbitrarily close
to u, βi can be optimized to obtain u = ua(x, βi) using
objective function h
h =
Ω
||L ua − f||2
dV +
∂Ω
||Bua − g||2
dS (34)
66

Uncertain VDT Maps
Cubic B-Spline approximation of distance transoforms
P(u, w) = (
1
6
)2
u3
, u2
, u, 1 MPMT




w3
w2
w
1



 (35)
M =




−1 3 −3 1
3 −6 3 0
−3 0 3 0
1 4 1 0



 (36)
P =




P00 P01 P02 P03
P10 P11 P12 P13
P20 P21 P22 P23
P30 P31 P32 P33



 (37)
67

Uncertain VDT Maps
hv =
Ω
(DTx
∂DTx
∂x
+ DTy
∂DTy
∂x
)2
+ (DTx
∂DTx
∂y
+ DTy
∂DTy
∂y
)2
DT2
x + DT2
y
− 1
2
dV +
∂Ω
DT2
x + DT2
y dS
(38)
Optimize for the control points Pij using hv
Neural networks may also be candidates for the approximation
function
68

Uncertain VDT Maps
Integrating uncertainty
Each point in a point cloud Xo is
Xoi =
xoi
yoi
=
xr + ri cos(θi − φr)
yr + ri sin(θi − φr)
(39)
ˆxR = (xr, yr, φr)T
- robot pose
cov( ˆxR) - robot pose uncertainty
Srθ = {(ri, θi}) - laser range - bearing observations
σr - laser range measurement noise
69

Uncertain VDT Maps
Uncertainty of each point in the point cloud
cov(Xoi) =
∂Xoi
∂ ˆxR
cov( ˆxR)
∂Xoi
∂ ˆxR
T
+
∂Xoi
∂r
σ2
r
∂Xoi
∂r
T
(40)
70

Uncertain VDT Maps
Weights for observations based on uncertainty ρoi calculated
as :
ρoi =
1
∂(DT2
x +DT2
y )
∂Xoi
cov(Xoi)
∂(DT2
x +DT2
y )
∂Xoi
T
(41)
Weights for Eikonal equation ρei are empirically determined
71

Uncertain VDT Maps
Objective function with weights incorporated :
hv∗ =
Ω
ρei
(DTx
∂DTx
∂x
+ DTy
∂DTy
∂x
)2
+ (DTx
∂DTx
∂y
+ DTy
∂DTy
∂y
)2
DT2
x + DT2
y
− 1
2
dV +
∂Ω
ρoi DT2
x + DT2
y dS
(42)
72

Uncertain VDT Maps
Estimating map uncertainty
Px∗
ij - optimized values of the control points for DTx
Py∗
ij - optimized values of the control points for DTy
cov(Px∗
ij) = Jx ∗ cov(Xoi) ∗ JT
x (43)
cov(Py∗
ij) = Jy ∗ cov(Xoi) ∗ JT
y (44)
Jx = −H−1
∗
∂2hv∗
∂Px∗
ij∂rk
(45)
(implicit function theorem)
73

Uncertain VDT Maps
Experiment 1
Simulated Robot
Equipped with 180° FOV LiDAR
Figure: Environment and robot poses 74

Uncertain VDT Maps
Measurement Noise Parameter
Position Estimates σx = 0.01m σy = 0.01m
Orientation Estimates σφ = 0.005rad
Range Measurements σr = 0.02m
Table: Noise Parameters used in Experiment 1
75

Uncertain VDT Maps
Figure: Noisy point cloud
76

Uncertain VDT Maps
(a) DTx (b) DTy
Figure: Optimal VDT at 0.2m control point resolution
77

Uncertain VDT Maps
Figure: Experiment 1 : Zero contour of the approximated VDT,
observations and true boundary
78

Uncertain VDT Maps
Figure: Error of the VDT approximation bounded by uncertainty
79

Uncertain VDT Maps
Experiment 2
Turtlebot in Gazebo equipped with a 270° FOV LiDAR
Navigating through Willow Garage oﬃce
Measurement Noise Parameter
Position Estimates σx = 0.05m σy = 0.05m
Orientation Estimates σφ = 0.005rad
Range Measurements σr = 0.02m
Table: Noise Parameters for Experiment 2
80

Uncertain VDT Maps
Figure: A corridor in the Gazebo Willow Garage environment
81

Uncertain VDT Maps
Figure: Point Cloud Map 82

Uncertain VDT Maps
Figure: OGM of the oﬃce space corridor derived from VDT 83

Uncertain VDT Maps
Demonstrate the use of Eikonal equation and boundary
condition to build a map
What the best approximation function is, is an open
question
Need improvements for practical use such as handling cut
locus eﬃciently, reduce computational burden using
submaps
84

Content
Background
Conclusions
85

Conclusion
Vector Distance Transform
Continuous
Implicitly captures the geometry
Behavior along the boundary makes it preferable to UDT
Preferable over SDT for unstructured environments
Localization using optimization or EKF on diﬀerent types
of sensors
Uncertainty of mapping data can be embedded
86

Content
Background
Conclusions
87

Publication Matrix
Contribution
ACRA
2017
ICIIS
2017
AIM
2019
CASE
2019
ICRA
2020
VDT Maps
Sensor
Model

Optimization
Localization

EKF Local-
ization

Active Local-
ization
Uncertain
VDT Maps

88

Main Publications
Arukgoda, J., Ranasinghe, R., Dantanarayana, L.,
Dissanayake, G. and Furukawa, T., 2017. Vector Distance
Function Based Map Representation for Robot
Localisation. In The Australian Conference on Robotics
and Automation (ACRA), (Vol. 12). ARAA. ISBN:
978-0-9807404-8-6 ISSN: 1448-2053
Ranasinghe, R., Dissanayake, G., Furukawa, T.,
Arukgoda, J. and Dantanarayana, L., 2017, December.
Environment representation for mobile robot localisation.
In 2017 IEEE International Conference on Industrial and
Information Systems (ICIIS), (pp. 1-6). IEEE. doi:
10.1109/ICIINFS.2017.8300384
89

Main Publications ctd...
Arukgoda, J., Ranasinghe, R. and Dissanayake, G., 2019,
July. Robot Localisation in 3D Environments Using
Sparse Range Measurements. In 2019 IEEE/ASME
International Conference on Advanced Intelligent
Mechatronics (AIM), (pp. 551-558). IEEE. doi:
10.1109/AIM.2019.8868466
Arukgoda, J., Ranasinghe, R. and Dissanayake, G., 2019,
August. Representation of Uncertain Occupancy Maps
with High Level Feature Vectors. In 2019 IEEE 15th
International Conference on Automation Science and
Engineering (CASE), (pp. 1035-1041). IEEE. doi:
10.1109/COASE.2019.8842965
90

Under Review
Jayasuriya, M., Arukgoda, J., Ranasinghe, R. and
Dissanayake, G., 2020, May. Localising PMDs through
CNN Based Perception of Urban Streets. Under review in
2020 International Conference on Robotics and
Automation (ICRA).
91

Other Publications During Candidature
Perera, A., Arukgoda, J., Ranasinghe, R. and
Dissanayake, G., 2017, September. Localization System
for Carers to Track Elderly People in Visits to a Crowded
Shopping Mall. In 2017 International Conference on
Indoor Positioning and Indoor Navigation (IPIN), (pp.
1-8). IEEE. doi: 10.1109/IPIN.2017.8115936
Unicomb, J., Dantanarayana, L., Arukgoda, J.,
Ranasinghe, R., Dissanayake, G. and Furukawa, T., 2017,
September. Distance function based 6DOF localization
for unmanned aerial vehicles in GPS denied environments.
In 2017 IEEE/RSJ International Conference on Intelligent
Robots and Systems (IROS) (pp. 5292-5297). IEEE.
10.1109/IROS.2017.8206421
92

Other Publications During Candidature ctd...
Hodges, J., Attia, T., Arukgoda, J., Kang, C., Cowden,
M., Doan, L., Ranasinghe, R., Abdelatty, K., Dissanayake,
G. and Furukawa, T., 2019. Multistage Bayesian
Autonomy for High-precision Operation in a Large Field.
Journal of Field Robotics, (Vol 36 (1)), (pp.183-203).
doi: 10.1002/rob.21829
93

Vector Distance Transform Maps for Autonomous Mobile Robot Navigation

More Related Content

What's hot (20)

Similar to Vector Distance Transform Maps for Autonomous Mobile Robot Navigation (20)

Recently uploaded (20)

Vector Distance Transform Maps for Autonomous Mobile Robot Navigation