Asymptotically Optimal Path Planning and
Surface Reconstruction for Inspection
O
by
OCT 16 201
Georgios Papadopoulos
LIBRARIES
S.M., Massachusetts Institute of Technology (2010)
Diploma, National Technical University of Athens (2007)
Submitted to the Department of Mechanical Engineering
in partial fulfillment of the requirements for the degree of
Doctor of Philosophy at the Department of Mechanical Engineering
at the
MASSACHUSETTS INSTITUTE OF TECHNOLOGY
September 2014
@ Massachusetts Institute of Technology 2014. All rights reserved.
Signature redacted
A uthor .......................
...........
D'epartment of Mechanical Engineering
June 16, 2014
Certified by..................
Signature redacted
Nicholas M. Patrikalakis
Kawasaki Professor of Engineering
Thesis Supervisor
Accepted by ......................
Signature redacted
SigDavideE.
.d
David E. Hardt
Chairman, Department Committee on Graduate Students
Department of Mechanical Engineering
Asymptotically Optimal Path Planning and Surface
Reconstruction for Inspection
by
Georgios Papadopoulos
Submitted to the Department of Mechanical Engineering
on June 16, 2014, in partial fulfillment of the
requirements for the degree of
Doctor of Philosophy at the Department of Mechanical Engineering
Abstract
Motivated by inspection applications for marine structures, this thesis develops algorithms to enable their autonomous inspection. Two essential parts of the inspection
problem are (1) path planning and (2) surface reconstruction. On the first problem,
we develop a novel analysis of asymptotic optimality of control-space sampling path
planning algorithms. This analysis demonstrated that asymptotically optimal path
planning for any Lipschitz continuous dynamical system can be achieved by sampling the control space directly. We also determine theoretical convergence rates for
this class of algorithms. These two contributions were also illustrated numerically
via extensive simulation. Based on the above analysis, we developed a new inspection planning algorithm, called Random Inspection Tree Algorithm (RITA). Given a
perfect model of a structure, sensor specifications, robot dynamics, and an initial configuration of a robot, RITA computes the optimal inspection trajectory that observes
all surface points on the structure. This algorithm uses of control-space sampling
techniques to find admissible trajectories with decreasing cost. As the number of
iterations increases, RITA converges to optimal control trajectories. A rich set of
simulation results, motivated by inspection problems for marine structures, illustrate
our methods. Data gathered from all different views of the structure are assembled to
reconstruct a 3D model of the external surfaces of the structure of interest. Our work
also involved field experimentation. We use off-the-shelf sensors and a robotic platform to scan marine structures above and below the waterline. Using such scanned
data points, we reconstruct triangulated polyhedral surface models of marine structures based on Poisson techniques. We have tested our system extensively in field
experiments at sea. We present results on construction of various 3D surface models
of marine structures, such as stationary jetties and slowly moving structures (floating
platforms and boats). This work contributes to the autonomous inspection problem
for structures and to the optimal path, inspection and task planning problems.
Thesis Supervisor: Nicholas M. Patrikalakis
Title: Kawasaki Professor of Engineering
3
4
Acknowledgments
I would like to first thank my advisor, Prof. N. M. Patrikalakis, for his continuous
support and direction. He was always available to discuss ideas and has provided me
with the necessary materials and guidance to successfully pursue my research. I also
would like to express my appreciation to my thesis committee members Professors M.
S. Triantafyllou and F. S. Hover of MIT and Dr. H. Kurniawati (of the University of
Queensland, Australia and formerly of SMART
/
CENSAM of Singapore) for their
guidance and comments. Dr. Kurniawati has co-authored several publications with
me and has always been willing to share ideas in robotics research and to provide
important technical input.
I would also like to thank my S.M. thesis advisor, Prof. J. J. Leonard of MIT,
for his support and direction during my first years at MIT and my undergraduate
advisor, Prof. E. G. Papadopoulos (of the National Technical University of Athens,
Greece), who introduced me to different aspects of robotics. I also wish to express
special appreciation to Professors E. Frazzoli, S. Karaman, and T. P. Sapsis of MIT
for useful technical discussions. Especially Prof. Karaman, as an expert in optimal
sampling-based path planning, was always available to discuss topics of common
interest.
Last but not least, I would like to thank my wife Tina and my parents for their
support during the course of my studies.
This work was supported by the Singapore-MIT Alliance for Research and Technology (SMART), Center for Environmental Sensing and Modeling (CENSAM).
5
6
Contents
21
1 Introduction
21
1.1
Motivation .......................................
1.2
Challenges in the Inspection Planning Problem
. . . . . . . . . . . .
23
1.3
Optimality Through the Control Space . . . . . . . . . . . . . . . . .
29
1.4
Challenges in Surface Reconstruction by Marine Vehicles . . . . . . .
29
1.5
Publications ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
31
1.6
Outline of the Thesis . . . . . . . . . . . . . . . . . . . . . . . . . . .
32
2 Related Work
33
2.1
Inspection Planning Problem . . . . . . . . . . . . . . . . . . . . . . .
33
2.2
Coverage Planning Problem . . . . . . . . . . . . . . . . . . . . . . .
36
2.3
Path Planning .....................................
37
2.4
Surface Reconstruction ..........................
40
2.4.1
3D Model Reconstruction using Ground Robots . . . . . . . .
40
2.4.2
3D Model Reconstruction using Marine Vehicles . . . . . . . .
43
3 Surface Reconstruction Using Marine Vehicles
47
3.1
Introduction ........
. . . . . . . . . . . . . . . . . . . . . . . .
48
3.2
Robotic Platform for Marine Field Experiments . . . . . . . . . . . .
48
3.3
Surface Reconstruction Algorithms . . . . . . . . . . . . . . . . . . .
50
3.3.1
Registration Algorithm . . . . . . . . . . . . . . . . . . . . . .
51
3.3.2
Above-Water Surface Reconstruction Algorithm . . . . . . . .
52
3.3.3
Combined Map ..........................
56
7
3.4
Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . .
60
3.5
Summary
74
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4 Asymptotically Optimal Planning for Systems with Differential Constraints
77
4.1
Problem Definition ............................
78
4.2
Planners that Sample the Control Space ....................
79
4.2.1
Algorithm Description .......................
79
4.2.2
Complexity ......
80
4.3
4.4
4.5
Analysis ...........
............................
.......................
82
4.3.1
Optimality ............................
4.3.2
The Lyapunov Approach ..........................
91
4.3.3
Convergence Rate ..............................
93
4.3.4
Different Sampling Strategies
97
Simulation Results ......
. 83
..................
............................
4.4.1
Case Study: Pendulum on a Cart . . . . . . . . . . . . . . . .
4.4.2
Additional Simulation Results ......................
97
97
100
Summary and Discussion .........................
102
5 Asymptotically Optimal Inspection Planning for Systems with Differential Constraints
109
5.1
Problem Definition ............................
110
5.2
Proposed Algorithms ................................
112
5.3
Analysis ........................................
115
5.3.1
Optimality of RITA .......................
116
5.3.2
Completeness of RITA ..........................
117
5.4
Results .........................................
120
5.4.1
2D Workspace Results ......................
121
5.4.2
TSP-Art-gallery ...............................
128
5.4.3
3D Workspaces ...............................
130
5.4.4
Implementation ...............................
137
8
5.5
Summary and Discussion .........................
137
6 Robustness of Approximate Optimal Trajectories
139
Introduction ................................
140
6.2
Deterministic Theoretical Bounds ........................
141
6.3
Probabilistic Theoretical Bounds ........................
142
6.4
Monte-Carlo Framework for General Trajectory Evaluation . . . . .
144
6.5
Summary and Discussion . . . . . . . . . . . . . . . . . . . . . . . .
145
.
.
6.1
7 Conclusions and Future Work
7.1
147
. . . . . . . . . . . . . . . . . . . . .
147
7.1.1
Surface Reconstruction
. . . . . . . . . . . . . . . . . . . . .
147
7.1.2- Path Planning . . . . .
. . . . . . . . . . . . . . . . . . . . .
148
7.2
Contributions ..........
. . . . . . . . . . . . . . . . . . . . .
151
7.3
Future Work ............
. . . . . . . . . . . . . . . . . . . . .
153
7.3.1
Path Planning ......
. . . . . . . . . . . . . . . . . . . . .
153
7.3.2
Inspection Planning. .
. . . . . . . . . . . . . . . . . . . . .
154
7.3.3
Surface Reconstruction
. . . . . . . . . . . . . . . . . . . . .
155
.
.
.
.
Conclusions ...........
A Probability to Get at Least one Trajectory that is e-close to Any
Approximation of the Optimal Trajectory
157
B Convergence Rate for the First Milestone for the Discrete System 161
9
10
List of Figures
1-1
Thunder Horse oil platform after the Hurricane Dennis in 2005 [15J. .
1-2
Diver inspecting a platform in the Gulf of Mexico; the picture is taken
from "TheOceanCorporation" [1]. . . . . . . . . . . . . . . . . . . . .
23
24
1-3 An illustration of a TSP-art-gallery approach: In this example, even if
the observation points are chosen correctly the resulting trajectory is
not close to optimal because the TSP algorithm computes the visiting
order ignoring the obstacles. . . . . . . . . . . . . . . . . . . . . . . .
25
1-4 Different planning problems: the path planning problem, the optimal
path planning problem, the optimal inspection planning problem and
the optimal task planning problem. . . . . . . . . . . . . . . . . . . .
1-5
The simple planning problem is a PSPACE-hard and a PSPACEcomplete problem (adapted from [1001). . . . . . . . . . . . . . . . . .
2-1
27
28
An illustration of greedy strategy that always chooses to move to a
configuration within R radius from its current position, that can see
the largest unseen structure. The environment contains two structures,
i.e., the "U" shaped at the bottom and the line at the top. In this
example, the greedy strategy may fluctuate moving down and up, while
the shortest trajectory would move to cover the structure on the top
first, before going down. . . . . . . . . . . . . . . . . . . . . . . . . .
11
35
3-1
(a) The ASV with Velodyne LiDAR mounted on top of it. Two pontoons are attached to the ASV to improve its stability. (b) The Velodyne LiDAR and the mounting platform for placing the LiDAR in an
inverted configuration on the ASV. (c) The BlueView MB2250 micro. . . . . .
49
. . . . . . . . . .
53
bathymetry sonar is mounted in a sideways configuration.
3-2
The online map: we use big simplification cell size.
3-3 The off-line map: Less simplified data are used to get the vehicle's
trajectory (a set of transformation matrices) and then the vehicles'
trajectory is used to project raw data resulting in a dense point cloud.
54
3-4 Trajectories generated during the first 100 seconds of one of the experiments: Trajectories corresponding to different At form a sequence
with decreasing difference. . . . . . . . . . . . . . . . . . . . . . . . .
3-5
The coordinate systems of LiDAR and sonar. b, and
8R,
55
denote the
translation vector and the rotation matrix that transform the sonar
data to the Velodyne coordinate system . . . . . . . . . . . . . . . . .
57
3-6 Sonar coordinate system: Sonar gives returns up to a range of 10 m
with an angle of 45 degrees. . . . . . . . . . . . . . . . . . . . . . . .
58
3-7 Surface reconstruction of both parts of marine structures, above and
below waterline. The ICP algorithm is used to find the transformation matrices between different poses (using the laser scanner data).
Then we use the transformation matrices to register the laser scanner
data under the same coordinate frame and register the unregistered
2D sonar data as 3D data to the same coordinate frame. Then we use
a probabilistic framework such as the occupancy grid based maps to
clean up the point clouds, and then we use known surface reconstruction algorithms (ball pivoting algorithm and the Poisson reconstruction
algorithm) to reconstruct a 3D surface model of the marine structure.
59
3-8 Filtering the sonar data. . . . . . . . . . . . . . . . . . . . . . . . . .
61
12
3-9 Reconstruction of a jetty in Pandan Reservoir, Singapore. (a) The
target jetty. (b) Side view of the jetty model constructed by our algorithm. (c) Top view of multiple frames of the 3D LiDAR data before
processing. (d) Top view of the constructed 3D model based on GPS
and compass information. (e) Top view of the constructed 3D model
generated by our algorithm. . . . . . . . . . . . . . . . . . . . . . . .
63
3-10 Reconstruction of a slowly moving boat in rough sea water environment, in Selat Pauh, Singapore. (a) Our operating area. (b) The
target boat. (c) Side view of multiple frames of the 3D LiDAR data
before processing. (d) Side view of the constructed 3D model based on
GPS and compass information. (e) Side view of the constructed 3D
model generated by our method. (f) Top view of multiple frames of
the 3D LiDAR data before processing. (g) Top view of the constructed
3D model based on GPS and compass information. (h) Top view of
the constructed 3D model generated by our method . . . . . . . . . .
64
3-11 Low resolution "real-time" model. . . . . . . . . . . . . . . . . . . . .
66
3-12 The mesh-based high quality map for the above-water part of the jetty,
using a high probability threshold for occupied cells and the ball pivoting surface reconstruction algorithm. Cell-size=8cm.
. . . . . . . .
67
3-13 The mesh-based high quality map for the above-water part of the jetty,
using a low probability threshold for occupied cells and the ball pivoting
surface reconstruction algorithm. Cell-size=8cm. . . . . . . . . . . . .
67
3-14 The mesh-based high quality map for the above-water part of the jetty,
using a low probability threshold for occupied cells and the Poisson
surface reconstruction algorithm. Cell-size=8cm. . . . . . . . . . . . .
13
68
3-15 Zoom in of the mesh-based high quality map for the above-water part
of the jetty, using a low probability threshold for occupied cells and
the Poisson reconstruction algorithm. Upper part, shows the pillars
of the structure. Below left part, shows a surface of a human body
(during the experiments we have people sitting and possibly moving
on the jetty). Below right, shows two large buoys that are used to
avoid direct collision of boats on the jetty. Cell-size=8cm.
. . . . . .
69
3-16 Combined map: a) Picture of the front part of the structure. b) Point
cloud-based map, front view. c) Point cloud-based map, side view. d)
Above- and below-water mesh-based map. . . . . . . . . . . . . . . .
70
3-17 Upper part: A point cloud representation of the above- and belowwater parts of the structure. The color indicates the density of the
points. Lower part: An occupancy grid based map of the above- and
below-water parts of the structure. The color represents the probability
of the cells to be occupied. Low probability gives blue colors and high
probability gives red colors.
. . . . . . . . . . . . . . . . . . . . . . .
73
3-18 Comparison between our results and pictures of the actual jetty taken
from google maps. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
73
3-19 Different types of oil platforms, the majority of the platforms are floating and slowly moving. The picture is from the Office of Ocean Exploration and Research . . . . . . . . . . .
. . . . . . . . . . . . . . . .
75
4-1 We can see the optimal control input (I*(t)), a representation of the
optimal control input (u*(t)) and a control input that is close to the
representation of the optimal control input (u(t) e.g. a control an
algorithm that samples the control space returns). . . . . . . . . . . .
84
4-2 Pendulum on a cart . . . . . . . . . . . . . . . . . . . . . . . . . . . .
98
4-3 Pendulum on a cart: A typical close-to-optimal trajectory. . . . . . .
99
14
4-4
Cart and pole simulation results.
Upper:
Simple-uniform without
pruning. Middle: Simple-uniform with pruning. Bottom: ExpandTreeRRT
with pruning. The iterations shown are the valid iterations (do not
count collisions), the actual iterations are approximate 2 times more.
4-5
101
Pendulum on a cart, statistics (results obtained by choosing a node to
expand uniformly and using pruning). Left hand side: the statistics for
the case constant integration time At = is is used. Right hand side:
the case the integration time is uniformly sampled from [0, 3s] is used.
The iterations shown are the valid iterations (do not count collisions),
the actual iterations are approximate 2 times more.
. . . . . . . . . .
102
. . . . . . . . . . . . . .
103
4-6
Close-to-optimal path as a function of time.
4-7
Close-to-optimal trajectory, in this case the integration time is uniformly sampled from [0, 3]sec. . . . . . . . . . . . . . . . . . . . . . .
4-8
104
Holonomic agent in a space without obstacles: We can see the optimal
tree and the optimal trajectory at certain numbers of iterations. The
green area in the upper left part of each figure indicates the goal region. 105
4-9
Holonomic agent in a space without obstacles: the statistics for 50 runs
in logarithmic scale . . . . . . . . . . . . . . . . . . . . . . . . . . . .
106
4-10 The second case: two trajectories the algorithm returns for certain
number of iterations and the statistics.
4-11 The second case: the statistics.
. . . . . . . . . . . . . . . . .
106
. . . . . . . . . . . . . . . . . . . . .
107
4-12 Dubin's airplane: Different views of a close-to-optimal trajectory (preliminary results).
5-1
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
108
For the biased-RITA, each node to be expanded is chosen with probability proportional to the inverse of the density of the nodes within its
neighborhood, here the first 3 nodes would be sampled with probability
5-2
2/8 and the last 2 with probability 1/8. . . . . . . . . . . . . . . . . .
115
A cartoon illustration of v-partitioning.
119
15
. . . . . . . . . . . . . . . .
5-3 First case study: we can see the average cost as a function of the
running time in linear scale (left part) and in log-log scale (right part).
In the upper part of the figure we can see results generated using
Weighted-RITA, and in the lower part of the figure we can see results
generated using Uniform-RITA. . . . . . . . . . . . . . . . . . . . . . 123
5-4 First case study, one run: The algorithm finds the first admissible
trajectory (cost 120.6 units) after 0.7 seconds. The first admissible
solution found does not belong to the same homotopy class as the optimal trajectory. We keep running the algorithm, and at 71.5 seconds
it switches to a different homotopy class and finds another admissible
trajectory with cost 114.9 units. The algorithm improves the current
solution within the same homotopy class (at time 117.3 seconds and
cost 100.85 units). At time 171.4 seconds it finds another admissible
trajectory with cost 92.5 units. After 400.3 seconds, it finds a better trajectory (this particular one of cost 68.7 units), which belongs
to the same homotopy class as the optimal trajectory, and eventually
this trajectory converges to a near-optimal one (63.8 units after 951.1
seconds). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
5-5
First case study: Above part) Comparison between the trajectory that
the algorithm converges to and the optimal holonomic trajectory. We
see that the optimal holonomic trajectory is close to the one the algorithm generates. Upper right and below part) The effect of additional
obstacles (gray color) close to the structure of interest. . . . . . . . ..126
5-6 First case study: we can see the average cost as a function of the
running time in linear scale (left part) and in log-log scale (right part).
In the upper part of the figure we can see results generated using
Weighted-RITA, and in the lower part of the figure we can see results
generated using Uniform-RITA. . . . . . . . . . . . . . . . . . . . . . 127
16
5-7 Second case study, (a-d) the algorithm finds the first admissible solution after running for 43.5 seconds (cost 219 units). It improves
the current solution within the same homotopy class (113.7 seconds
for cost 200 units). Then the algorithm explores few other homotopy
classes and finally it converges to a trajectory with cost close to the
optimal one (177.6 units in 51 min). (e-f) Different homotopy classes
of trajectories with cost close to the optimal one. (i-j) Results with
additional obstacles (obstacles in gray color). . . . . . . . . . . . . . . 129
5-8 TSP-art-gallery approach: upper left shows the best inspection trajectory out of 40 runs (cost 212 units), upper right: shows a typical
trajectory (cost 265 units), lower right: the TSP solver failed to compute a good visiting order because the obstacles were not considered
for the TSP problem, lower right: It fails to find a feasible trajectory
because one of the observation points was chosen to be close to the
obstacles.
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131
5-9 The size of marine structures and their mooring lines is huge. . . . . . 133
5-10 Top and isometric views of typical runs for each one of the missions.
a-b) close-to-optimal trajectory for the "ring" mission, c-d) close-tooptimal trajectory for the "knot" mission and in e-f) close-to-optimal
trajectory for the "maze" mission.
. . . . . . . . . . . . . . . . . . . 135
5-11 Several views of typical runs for each one of the missions. a-b) closeto-optimal trajectory for the "oil rig" mission, c) a 3D CAD model of a
real oil rig, d-f) close-to-optimal trajectory for the "Submarine" mission. 136
6-1
Effect of additive control and state noise . . . . . . . . . . . . . . . . 146
7-1
The set of all path planning problems is a subset of the set of all
inspection planning problems and the set of all the inspection planning
problems is a subset of the set of all the task planning problems.
17
. . .
149
18
List of Tables
5.1 Weighted-RITA: Average running times and iterations to get the first
feasible inspection trajectory and close-to-optimal inspection trajectories134
5.2
Uniform-RITA: Average running times and iterations to get the first
feasible inspection trajectory and close-to-optimal inspection trajecto-
.................................134 ries
. .
19
20
Chapter 1
Introduction
1.1
Motivation
Autonomous inspection of structures is a complex problem with several applications
to the marine, terrestrial and space environments. Efficient methods for structural
inspection have become increasingly important to ensure the safety and functionality
of structures. Higher shipping traffic and increased terrorist threats demand faster
and more frequent ship and harbor inspection. Likewise, greater demands on land
infrastructure create the need for faster bridge and road network inspection.
The
need for faster and more frequent structural inspection has increased the demand for
structural inspection using autonomous robots.
Two essential parts of the autonomous (robotic) inspection problem are path
planning and surface reconstruction. The inspection (path) planning is the problem
of finding an inspection trajectory that is informative enough to gather data (such as
sonar and LiDAR data) from different views of a given structure that guarantee that
all surfaces of the structures would be visible at a later time -during the algorithm
execution.
Given data gathered from all different views of a given structure, the
surface reconstruction problem is the problem of registering all data under the same
coordinate system to get a point based representation of the structure and then use
meshing algorithms to reconstruct a 3D CAD model of the structure of interest.
In this thesis, we focus on both the inspection planning problem and the surface
21
reconstruction problem. Another essential part of the autonomous inspection problem
is the inference problem. In this part of the problem, the method performs inference
over the obtained model to identify features of interest; in this work we do not focus
on the inference part of the problem. Contributions (e.g. algorithms and theoretical
results) developed in this thesis can be applied for any kind of structure such as
terrestrial structures, road networks, bridges, space stations and marine structures.
However, this thesis is motivated by the inspection of marine structures, and therefore
this is the focus of this work.
The inspection problem of marine structures is a very important problem with
several applications including, evaluation and re-evaluation of the structures' condition, identification of missing or broken parts, detection of foreign objects attached
to the structures (e.g. mines or explosives), and navigation purposes. Depending on
the application, the required resolution can vary from about 7cm (e.g. mine detection
tasks) to 0.5m (missing or broken parts) or even to 1m for navigation tasks.
Marine structures are exposed to challenging conditions such as water currents,
water waves, corrosion, and frequently several hurricanes per year. This exposure
results in unpredictable damages to the marine structure, which could be life threatening for the people on and around such structures.
Therefore, to ensure safety,
thorough structural inspections are required regularly or after a platform is affected
by natural disasters. For example, after Hurricane Dennis in 2005
[15], structural
inspection of Thunder Horse oil platform (Fig. (1-1)) was needed as soon as possible
to identify damages on the platform and re-evaluate the platform's condition.
Currently, such inspections are performed by human workers through visual observation. They inspect the above-water part of the structures aboard small boats,
and use diving equipment for the submerged portions of structures. In Figure (1-2),
we can see a diver inspecting the underwater part of a platform in the Gulf of Mexico
[1]. In both situations, inspectors often lack the time and comfort to inspect the
structures and re-evaluate structures' status properly. In addition, visual examination
does not give inspectors the ability to track important structural changes. Scanning
the structures, using robots, and automatically reconstructing 3D models from the
22
Figure 1-1: Thunder Horse oil platform after the Hurricane Dennis in 2005 [15].
scanned data would give the inspectors the opportunity to inspect structures from
their offices, thereby reducing the safety risks of the inspectors and increasing the
accuracy of the inspection results.
1.2
Challenges in the Inspection Planning Problem
Efficient methods for structural inspection have become increasingly important to
ensure safety. Higher shipping traffic and increased terrorist threats demand faster
and more frequent ship and harbor inspection. Likewise, greater demands on land
infrastructure create the need for faster bridge and road network inspection. The
need for faster and more frequent structural inspection has increased the demand for
structural inspection using autonomous robots.
An efficient inspection planning algorithm is critical for developing such autonomous
robots. Inspection planning is the problem of finding an inspection trajectory, i.e.,
23
Figure 1-2: Diver inspecting a platform in the Gulf of Mexico; the picture is taken
from "TheOceanCorporation" [1].
a trajectory for the robot to scan all points on the structures.
In this thesis, we
focus on the inspection planning problem where the model of the structure and the
environment are perfectly known prior to planning, and the robot has no control nor
sensing error. More specifically, we would like to compute optimal or close to optimal
inspection trajectories for robots with non-linear dynamics.
Although many inspection planning methods have been proposed, most are not
suitable for robots with differential constraints. Most methods [29, 36, 37, 38] separate
the inspection planning problem into two NP-hard problems and then solve each subproblem sequentially. The first sub-problem is the art gallery problem, which finds
the smallest set of observation points that the robot needs to visit to guarantee
100% coverage. The second sub-problem is the traveling salesman problem (TSP),
which finds the shortest trajectory to visit all observation points that have been
generated by the art gallery solver. This separation approach is difficult to apply
when the robot is non-holonomic and operates in cluttered environments, as the
observation points generated by the art gallery problem may not be reachable from
other observation points in the set. Furthermore, even if we generate the smallest set
of observation points and even if we find the shortest trajectory to visit all points in the
set of observation points, the generated inspection trajectory may not be the shortest
trajectory for scanning all points on the boundary of the structure.
24
In addition,
because most of the existing TSP solvers do not take into account neither vehicle
dynamics, nor obstacles in the configuration space (e.g. the cost for connecting each
one of the nodes is based on the euclidian distance), then the resulting visiting order
results in trajectories that are far from optimal. In Fig. (1-3) we can see an example
where the observation points are chosen correctly however the resulting trajectory is
not close to optimal because the TSP algorithm computes the visiting order ignoring
the obstacles. There is ongoing work on the multi-goal path planning community that
tries to address the TSP problem in the presence of obstacles using heuristics (e.g.
[114]), and in the presence of differential constraints without obstacles (e.g. [115]).
Algorithms, presented later in this thesis can be used to address (in a probabilistic
way, using randomized algorithms) the TSP problem in the presence of differential
constrains and obstacles, however, this is not within the scope of our work. In short,
most existing inspection planning methods are often not suitable to find a short
inspection trajectory for robots with non-holonomic constraints.
9I
Figure 1-3: An illustration of a TSP-art-gallery approach: In this example, even if the
observation points are chosen correctly the resulting trajectory is not close to optimal
because the TSP algorithm computes the visiting order ignoring the obstacles.
In summary, because the computation of the observation points and the visiting
order cannot be computed independently, even for holonomic systems, by separating
the inspection planning problem to the TSP and the art-gallery problem optimal or
close to optimal inspection planning may be impossible to be achieved. Instead, we
model the problem as an optimization planning problem and we propose a solution
based on tools and ideas developed by the path planning community and novel ideas
25
we present in this thesis. As we show later in this thesis this approach achieves
optimal inspection planning.
A general way to define a planning problem is as follows. Given a dynamical
system, the obstacle free space (e.g. a subset of the state space that is obstacle
free) and task specifications, we define an admissible path to be any obstacle free
path that represents a solution to the system dynamics and does not violate the task
specifications. Then an optimal task planning problem computes among all admissible
paths the one that minimizes a cost function. The simplest category of task planning
problems is the path planning problem; for the simple path planning problem an
admissible path is an obstacle free path that starts from a given state, reaches the
goal region and represents a solution to the system dynamics. Similarly, among all
admissible paths the optimal path planning problem computes the one that minimizes
a given cost function. In addition, for the optimal inspection planning problem we
define an admissible path to be an obstacle free path that observes all details on a
given structure. Figure (1-4) gives an overview of the path planning problem, the
optimal path planning problem, the optimal inspection planning problem and the
general task planning problem (for more details refer to the next chapters).
It is important to notice that it is extremely difficult to study the above problems
in the state space since each planning problem (e.g. path planing, inspection planning,
task planning) needs a separate analysis. In fact, the path planning community treats
the inspection planning problem and the path planning problem in a different way
separating it into the TSP and the art-gallery problems. Instead, of searching for
an admissible path, if we search the control space for an admissible trajectory (path
in the control space) similar analysis holds for each one of the planning problems.
For instance, we assume that there is an optimal trajectory (control input) and our
focus is to compute it (or approximate it). In this work, we will first study the simple
path planning problem and then we extend our approach to the inspection planning
problem.
Unfortunately, even the simple path planning problem is proven to be a PSPACEhard problem, in fact it was proven to be a PSPACE-complete problem (with re26
Path Planning
Optimal Path Planning
Optimal Inspection Planning
Given:
Given:
Given:
[11 Dynamical system:
' (t) = f 0(t), U(t), t)
[1] Dynamical system:
;Y(t) = f(-IM, u(t), t)
[1]
X0t
=
[21 Free space: Si,
[2}
ree
[31 Initial state: Sin
[2 Pree space: St
[3} Initial state: Sin
[41 Goal region:
GoalfRegion c Stra
[4] Goal region:
GoalRegion C Sfee
141
Compute:
[1] Path y : [0, Tc --+ S
Such that:
Y(O)
=
Sin
7(TG) E GoalRegion
7(t)
E SfreeVt E [0, Tc
Dynamical system:
0~(t), u(t), 0)
space: Se.
[3J Initial state: Sin
Structure Primitives:
STR
Optimal Task Planning
Given:
[1] Dynamical system:
Wt = f(-Y(t), u(t), 0)
[21 ree space: Sje
[31 Initial state: Sin
[41 'ask specifications:
TASK
Define admissible path:
-y : [0, TJ - S such that
Define admissible path:
Y : [0, TGJ -+ S such that
y(O) = Sin
'Y(0) = Sin
-y(Tc) E GoalRegion
-y(t) E Str.Vt E [0,TGI
7(t) E Sfr.Vt E [0,Tcl
= STR,
where V(-y(t)) C STR
Compute:
-y verifies all TASK speci-
-(t) E Sfre.Vt E 10, Tca
Compute:
y* = argmaxmEr(D(y)),
where:
D(y): Cost function
r: Set of admiss. paths
UtCJ0TorG V(-y(t))
*-= argmax_,Er(D(7)),
where:
D( 7 ): Cost function
F: Set of admiss. paths
Define admissible path:
y : [0, Tel - S such that
()=
Sin
fications
Compute:
y*
= arg max1 tr(D(-y)),
where:
D(y): Cost function
F: Set of admiss. paths
Figure 1-4: Different planning problems: the path planning problem, the optimal
path planning problem, the optimal inspection planning problem and the optimal
task planning problem.
spect to the number of dimensions) [111, 117, 18]. PSPACE-complete problems are
the hardest problems among all PSPACE problems, and PSPACE problems may be
harder than NP problems (see Figure (1-5)). Because the dimensionality of the inspection problem for agents with differential constraints can be large, this thesis introduces
the use of sampling-based motion planners [21] to compute close-to-optimal inspection
trajectories. Significant scientific work has been dedicated on sampling-based motion
planners: Probabilistic RoadMap (PRM) [69], Expansion Space Tree algorithm (EST)
[61], Rapidly-exproring Random Trees algorithm (RRT) [80]; however, until today the
problem of asymptotically optimal motion planning for sampling-based motion planners still remains an open problem for systems with differential constraints.
Sampling-based approaches [21], [81], are the most successful approaches for solving simple path planning problems for high-dimensional systems. Up to a few years
ago, sampling-based motion planners were only known to be probabilistically com27
PSPACE
-^, NPC-
LOG
S ace
O%
Time%
Figure 1-5: The simple planning problem is a PSPACE-hard and a PSPACE-complete
problem (adapted from [100J).
plete, in the sense that given enough time, they will find an admissible trajectory
with probability one whenever such a trajectory exists. The question of optimality
was addressed recently in [671. They developed sampling-based algorithms that are
asymptotically optimal, such as PRM* and RRT*.l
Although PRM* and RRT* perform well for robots with simple dynamics, such
as holonomic robots, extending asymptotically optimal sampling-based planners to
systems with complex dynamics remains an open problem. The difficulty lies in the
fact that both RRT* and PRM* perform their sampling in the configuration space
and therefore require a steering function that drives the robot from one configuration
to the other. Unfortunately, steering functions are often difficult to find and may not
even exist for systems with complex dynamics, such as systems with non-holonomic
constraints. We believe that for the general case, where we have non-linear underactuated robots, sampling the control space directly may be a better choice. By
sampling the control space directly, the difficulty of dealing with challenging dynamics
and the necessity of the use of a steering function is alleviated. In fact, in this thesis
we show that asymptotic optimality in sampling-based motion planning, inspection
planning and general task planning (given the optimal admissible trajectory is a finite
1
where asymptotically optimal means, given enough time, the probability the planner returns a
trajectory close to the optimal trajectory, is one.
28
time trajectory) can be achieved without the use of a steering function. Our analysis
only requires Lipschitz continuity of the differential constraints and the cost function.
1.3
Optimality Through the Control Space
Currently, the optimality property for sampling-based motion planning algorithms
that sample the state-space (e.g. RRT* [67]) is established only for linear-fullycontrollable systems and a small subset of non-linear systems. The extension of
the optimality proof for algorithms that sample directly the control space (without a
steering function) is not obvious and it has been an open problem in robotics since
1997.
One of the key differences is that in the case we have a steering function and we
sample points uniformly at random from the state space, e.g. the RRT* algorithm,
each iteration of the RRT* algorithm is independent of the previous one. As a result
the Borel-Cantelli lemma [67] can be used to show that the probability of sampling
arbitrarily close to the optimal path approaches to 1 as the number of iterations
increases. In the case the control space is sampled uniformly at random the above
does not hold, and as a result Borel-Cantelli lemma cannot be used. In addition, the
metric space used for the RRT* case (for fully controllable systems) is based on the
Euclidean distance in the state space. In contrast, the approach we present in this
thesis is general enough to use any cost function that is continuous.
1.4
Challenges in Surface Reconstruction by Marine Vehicles
There is a plethora of scientific work [95, 92] dedicated to the reconstruction of 3D
models for land-based structures, however very few works have been dedicated to
marine environments. Unpredictable disturbances such as water currents, and the
marine environment itself pose significantly more challenges compared to the challenges posed by terrestrial (shore) environments. Water currents generate disturbance
29
forces on marine vehicles, resulting in large roll and pitch angles. Furthermore, many
marine structures are floating platforms, which move under strong currents. On top
of that, GPS (Global Positioning System) access close to big marine structures is not
reliable and other localization sensors developed for marine vehicles lag behind the
ones developed for ground robots.
The 3D surface reconstruction problem often requires gathering 3D point clouds
and registering them under the same coordinate frame. In the case of stationary
structures, 2D laser scanners and localization sensors (GPS, Inertial Measurement
Unit (IMU), Doppler Velocity Logger (DVL) or wheel encoders) can be used to gather
3D data from stationary environments [57]. In addition, advanced SLAM techniques
can be used to optimize the estimated trajectory and the resulting map [131, 116].
On the other hand, in the case of slowly moving structures special care should be
taken in both gathering the 3D point clouds and registering them under the same
coordinate frame. In the absence of knowledge over the motion of the structure, data
gathered using 2D laser scanners combined with localization units cannot be used to
reconstruct point cloud representations of structure views.
From our discussion above, it is clear that when dealing with moving structures,
3D laser scanners with a wide field of view that allows significant overlap between
subsequent laser scans should be used to gather point clouds. Then, registration algorithms that are invariant to motion, such as the Iterative Closest Point (ICP) [14],
can be used to register all data gathered under the same coordinate frame resulting
in a point cloud representation of the structure of interest. In the case of stationary structures, researchers commonly use estimates of vehicle positions to initialize
the ICP algorithm, [95], however this is not always feasible in the case of moving
structures.
Part of this thesis (Chapter (3)) presents the hardware and software designs for
scanning and reconstructing 3D surface models of marine structures. We alleviate the
difficulty caused by disturbances, moving structures, and sensor errors by hardware
and software design. In terms of hardware, we select sensors that would ease construction of 3D models from scanned data when no positioning sensors are available.
30
Furthermore, we develop a software system that constructs 3D point cloud models of
marine structures from the scanned data and by using known surface reconstruction
techniques reconstructs 3D surface models of marine structures. We have successfully
tested our system in various conditions in Singapore waters between 2009 and 2011,
including operating in the water with up to 2m/s water currents, and constructing
3D models of slowly moving structures. All the structures are constructed without
any information from positioning sensors, such as GPS and DVL.
The problem of reconstructing surfaces from registered point clouds still remains
an open problem. There are several algorithms that reconstruct surfaces from point
clouds, some of them reconstruct exact surfaces (interpolating surface) by a triangulation that uses a subset of the input point cloud as vertices [13], [5]. These approaches
perform well in clean point clouds and present problems when the point clouds are
noisy. Some other approaches reconstruct approximating surfaces by using best-fit
techniques [55] [70], [3]. These approaches are robust to noise in the point clouds,
however they tend to over-smooth the surfaces and thus important part of the structure geometry may be lost. A comparison between several state-of-the-art surface
reconstruction techniques can be found at [12].
1.5
Publications
Our surface reconstruction work was presented to several referee conference papers
e.g. [76, 108] and published to Journal of Field Robotics (JFR) [107]. Our inspection
planning work was presented to IEEE ICRA 2013 [102] and submitted to TRO [104].
Our path planning work is currently under-review to International Journal of Robotics
Research [105], preliminary results can be found online in the "arXiv.org" [101].
An important part of robotics and especially marine robotics is the localization
problem. It is well-known that there is no GPS access available underwater, this makes
navigation of underwater vehicles a challenging problem. This thesis does not focus
on the localization problem, however we have worked on the localization problem in
previous contributions including several referee conference papers [106, 42, 41], a book
31
chapter published by Springer-Verlag [43] , and an International Journal of Robotics
Research paper [44].
1.6
Outline of the Thesis
In Chapter (2) we describe related work on both the path planning part of the problem and the surface reconstruction problem. Regarding the path planning problem we
will start with early results that explore the complexity of the problem and then focus
on sampling-based motion (path) planners. In addition, we will also describe previous
work on the inspection planning problem that use greedy methods, heuristics, and
art-gallery-based approaches. We start the main part of this thesis with Chapter (3)
where we describe hardware and software designs for scanning and reconstructing 3D
surface models of marine structures. The proposed system was tested extensively in
Singapore's coastal waters and a rich set of experimental results is presented. Doing
the surface reconstruction experiments in Chapter (3), we realized how difficult is to
compute robot trajectories that guarantee 100% structure coverage. The above open
question motivated us to focus on the inspection planning problem which is modeled
as a path planning optimization problem. In Chapter (4) we focus on the simple path
planning problem; we provide an analysis that indicates that asymptotically optimal
path planning can be achieved using sampling in the control space and thus asymptotically optimal path planning for non-linear systems can be achieved without the
use of a steering function. We also provide simulation results. Taking advantage of
the analysis presented in Chapter (4), in Chapter (5) we provide a novel inspection
planner that achieves asymptotically optimal inspection planning using sampling in
the control space. A rich set of simulation results is provided. The planner developed in Chapter (5) does not take into account uncertainty and noise. However, in
Chapter (6) we study the robustness of close-to-optimal trajectories in the presence
of control and state noise. In Chapter (7) we summarize our work with conclusions,
contributions and future work. In the Appendix (A) and Appendix (B) we derive
some probability equations that are useful for Chapter (4).
32
Chapter 2
Related Work
2.1
Inspection Planning Problem
Given a perfect model of a structure, sensor specifications, robot's dynamics, an environment map and an initial configuration of a robot, an inspection planning algorithm
computes an inspection trajectory (or path) that observes the entire surface of the
structure of interest. The problem of inspection planning has attracted the interest of
several researchers over the last 20 years. Choset's group proposed planners for coverage, exploration and sensor-based coverage using cell decomposition methods [20, 22],
Voronoi diagrams [2], and various heuristics [6]. Approaches based on cell decomposition methods yield good results but are restricted to low-dimensional state spaces.
In this subsection we will concentrate on related work concerning inspection planning
approaches (e.g. sensor-based coverage and structure coverage) whereas in the next
subsection we will concentrate in the Coverage Planning Problem (CCP).
We first discuss early methods that discretize the free space, use graph representations of the free space and search methods and heuristics to compute admissible
paths. One of the first sensor-based coverage approaches for surfaces in 3D environments utilizes the use of 2D terrain-covering algorithms in successive horizontal
planes laying at different depths and several heuristics. The proposed method can be
used to map the seabed using AUVs (simulation results are provided) [51, 82]. The
underling 2D terrain-covering algorithm uses cell decomposition methods and zig-zag
33
patterns to cover each one of the cells and their projected volume with respect the
depth. Atkar et al. [8, 7] proposed the use of coverage algorithms for generating
paths for autonomous/robotic painting applications. This approach utilizes a Morse
decomposition method and a Reeb graph representation of the space that captures
the topology of the surface of interest. It gives good results for the target application
however it cannot handle additional obstacles in the configuration space and it is not
scalable for different coverage problems.
Recently Cheng et al. [19] proposed a method that uses fine discretization optimization methods to compute time-optimal trajectories that provide complete 3dimensional coverage with applications to urban environments with 2.5-dimensional
features. The basic approach is to approximate the features of interest with a set of
non-planar coverage surfaces and to design a motion plan that guarantees the coverage surface is swept completely with a conical-field-of-view sensor. Simulation and
experimental results are provided for a sensor attached to an unmanned aerial vehicle
(UAV).
Most of today's inspection planning methods can be divided into two approaches.
One approach
[29, 36, 37, 38] separates the problem into two sub-problems, the
art-gallery problem and the TSP. Although most art-gallery solvers in computational
geometry [99] do not consider sensor limitations, such as range limit, Gonzalez-Banos
et.al. [48] have developed a method that takes into account sensor limitations. They
use a randomized algorithm to select observation points that provide 100% coverage
of the environment [47]. Danner and Kavraki [29] proposed the first randomized inspection planning algorithm that uses a similar method to the one Gonzalez-Banos
proposed to compute the guard points and then connects them to form inspection
paths. The visiting order of the guard points is obtained by approximating the TSP
solution; Danner and Kavraki also expand their work to simple 3D environments. A
similar approach was taken by Englot and Hover [38] with application to complex
3D structures such as ship hulls. Their approach was probably the first one that
handles large structure sizes. Methods based on this separation approach often have
difficulties to plan trajectories for robots with non-holonomic constraints operating
34
in cluttered environments, because some observation points generated by art-gallery
solvers may be difficult or even impossible to reach from other observation points.
Furthermore, these methods do not provide optimality guarantees, even if each subproblem is solved optimally. Englot and Hover [38] provide a probabilistic completeness analysis of the algorithm and propose a smoothing algorithm that shortens the
inspection paths by taking advantage of the asymptotically optimal RRT* algorithm;
without guarantee of global optimality.
Another approach (e.g., [53]) relies on a sub-modular objective function to guarantee that greedy algorithms generate near-optimal solutions. However, when the
objective is to minimize the length of the inspection trajectory, as in our case, the
problem is not sub-modular, and a greedy method may have difficulties as described
in Figure (2-1).
404
R
L
Figure 2-1: An illustration of greedy strategy that always chooses to move to a
configuration within R radius from its current position, that can see the largest unseen
structure. The environment contains two structures, i.e., the "U" shaped at the
bottom and the line at the top. In this example, the greedy strategy may fluctuate
moving down and up, while the shortest trajectory would move to cover the structure
on the top first, before going down.
35
2.2
Coverage Planning Problem
A very similar problem to the inspection planning problem (e.g. the structure cov-
erage problem, the sensor-based structure coverage problem) is the Coverage Path
Planning (CCP) problem. For a given visibility sensor, environment map and robot
dynamics, the Coverage Path Planning is the task of determining an obstacle-free path
that covers all the parts of the obstacle free space. An admissible solution to the CPP
problem could also represent an admissible solution to the inspection planning problem, however the opposite does not hold. Most of the previous CCP approaches deal
with 2D workspaces, holonomic robots and assume disk-shaped sensor foot-prints
with diameter of the same size of the robot physical diameter.
Choset and his group have studied the CPP problem extensively [23]. Among the
first approaches they proposed were approaches based on cell decomposition methods.
All the cell decomposition based methods require:
" Exact decomposition of the free space into cells.
" Computation of the adjacency graph.
" Search methods to compute a path that visits all of the cells at least once.
" Local planning strategies to cover each one of the cells (e.g. zig-zag motion
pattern or mowing the lawn pattern).
The trapezoidal decomposition is the simplest cell decomposition approach that
guarantees a complete coverage of the space [79]. Because it decomposes the space
into trapezoidal shaped cells, each cell can be covered by simple back-and-forth motion [98]. On the other hand, because of the simplicity of the trapezoidal shaped cells,
this approach generally results in a huge number of cells increasing the difficulty of
the problem. To address this problem, Choset and Pignon proposed the boustrophedon cell decomposition method that takes into account the shape of the obstacles in
the space and generates convex and non-convex shaped cells [241. Both the trapezoidal cell decomposition and the boustrophedon cell decomposition can only handle
36
planar polygonal spaces. To address this issue, Acar et at. proposed a cell decomposition approach based on critical points on Morse functions [2]. The Morse-based cell
decomposition approach can handle arbitrarily shaped obstacles in the state space.
By choosing different Morse functions arbitrarily shaped cells can be obtained (e.g
circular, trapezoidal etc.).
Another family of CPP approaches is the grid-based method that discretizes the
free space into uniform grid-cells. As opposed to cell decomposition methods that
result in complete coverage, grid based approaches result in resolution-complete cov-
erage. Gabriely and Riman [45] developed the Spiral-Spanning Tree Coverage gridbased approach that discretizes the free space into uniform grid-cells and uses algorithms (from graph-theory) to compute a tree that "visits" all grids. Zelinsky et at.
[135] proposed another grid based coverage approach; the proposed approach uses a
wavefront algorithm and gradient following methods to compute a path that passes
from all grid-cells. Additional related work for the CPP problem can be found in
[23, 46].
2.3
Path Planning
Given a dynamical system, the obstacle-free space (e.g. a subset of the state space
that is obstacle-free) and task specifications, we define an admissible path to be any
obstacle-free path that represents a solution to the system dynamics and does not
violate the task specifications.
Then an optimal task planning problem computes
an admissible path that minimizes a cost function.
The simplest category of task
planning problems is the path planning problem; for the simple path planning problem
an admissible path is an obstacle-free path that starts from a given state, reaches the
goal region and represents a solution to the system dynamics. Similarly, among all
admissible paths the optimal path planning problem computes the one that minimizes
a given cost function. In addition, for the optimal inspection planning problem we
define an admissible path to be an obstacle-free path that observes all details on a
given structure.
37
Early scientific work on the path planning problem has illustrated that the simple
path planning problem is an intractable problem [111, 117, 181. Studying the "piano mover's" problem (a simple path planning problem) using differential geometry
methods Reif [111] has shown that the simple "piano mover's" problem (and therefore the simple path planning problem) is a PSPACE-hard problem. Few years later,
Schwartz and Sharir [117] proposed the first complete algorithm that solves the "piano mover's" problem with doubly exponential running time. Given that Reif [1111
has shown that the simple "piano mover's" problem is a PSPACE-hard problem, it
was unknown for about ten years whether the problem is PSPACE-complete problem
or much harder. In his Ph.D dissertation Canny [18] proposed an exponential-time
and polynomial-space algorithm illustrating that the path planning problem is in fact
a PSPACE-complete problem.
Since the path planning problem is a PSPACE-complete problem, all practical
algorithms that solve the path planning problem relax the completeness requirement
one way or another. There are two families of practical path planning algorithms:
the deterministic path planning algorithms and the sampling-based path planning
algorithms. Deterministic algorithms often give up the completeness property, on the
other hand, sampling-based planners relax the completeness requirement to the probabilistic completeness requirement. A sampling-based planner is called a probabilistic
complete algorithm if, given that an admissible trajectory exists, the probability will
return an admissible trajectory approaches to one as the number of iterations increases
[21].
Much scientific work has been dedicated to the path/motion planning problems
[79, 21, 81]. Among the first path planning algorithms developed were the "bugi"
and "bug2" algorithms [89].
very simple actions (e.g.
The bug algorithms use workspace information and
the agent moves only on straight lines and follows the
boundaries of the workspace). The introduction of the "configuration space" and the
"free space" by Lozano-Perez [88] has enabled researches to study the problem in
its correct base. Several approaches have been proposed including: potential fields
[71, 9], discretization methods [54, 97] and sampling-based approaches [69, 68].
38
Planners based on potential fields construct a potential field on the configuration
space in a such way that obstacles push away the agent and the goal configuration attracts the agent [71]. Thus an admissible trajectory can be computed using
gradient-following methods. However, until today the construction of a potential field
over a general configuration space that has a unique global minimum remains an open
problem, thus these methods easily get trapped in local minima. On the other hand,
discretization methods such as cell decomposition methods and decompositions based
on Voronoi diagrams yield good results but are restricted to low-dimensional spaces.
Over the past two decades, several sampling-based motion planners have been proposed. Sampling-based planners can be divided into two broad categories, i.e., graphbased approaches (multi-query) and tree-based approaches (single-query). Graphbased approaches first construct a random graph in the configuration space and then
use the graph to find admissible paths. This approach includes Probabilistic RoadMap
(PRM)[69], [681, and it variants [125],[741,[35],[60], [4].
Tree-based approaches construct a random tree and stop construction whenever
the constructed tree contains a path from a given initial configuration to the goal
region.
This approach includes the widely used Rapidly-exploring Random Trees
algorithm (RRT) [801 and its variants e.g. [134]. Another type of tree-based sampling
based planner is the Expansive Space Tree (EST)[61]. In terms of handling differential
constraints, the main difference between the RRT planner and the EST planner is that
the former performs its sampling in the configuration space and the latter performs
its sampling in the control space.
Recently, Karaman and Frazzoli showed that RRT and PRM converge to a nonoptimal trajectory and they developed asymptotical optimal planners e.g. RRT* and
PRM* [67],[66]. However, a steering function is required to guarantee optimality and
thus they are not suitable for general systems with differential constraints. Perez et
al. [110] proposed the use of the RRT* algorithm combined with LQR methods to
solve path planning problems with challenging dynamics. Similarly, Tedrake et al.
[126] proposed the LQR-Trees for feedback motion planning. However, these methods
perform well only close to the linearization area. As a result the optimality properties
39
of the RRT* do not hold for the LQR-RRT based algorithms. More recently, Dobson
and Bekris
[331
proposed an algorithm that relaxes asymptotic optimality to near-
optimality, to speed-up computing the solution. However, they still require a steering
function.
One of the main contributions of this thesis is to perform an analysis on samplingbased motion planning algorithms and shed some light on its challenges. As we show
in Chapter (4), asymptotically optimal planning can be achieved by directly sampling
the control space. Furthermore, by modifying existing planners, we proposed a new
family of asymptotically optimal planners. As the number of iterations increases, the
control input these algorithms sample approaches the optimal control input.
2.4
Surface Reconstruction
The 3D model reconstruction problem can be considered a special category of the Simultaneous Localization and Mapping (SLAM) problem [128]. SLAM was originally
introduced by Smith et al. [123] and solved using EKF approaches and feature based
maps [1241. Dissanayake et al. [321 prove that the solution to the SLAM problem is
possible and propose another solution to the EKF-SLAM problem. Thrun, Montemerlo et al. [129] [90] approach the problem from a probabilistic point of view, which
was the foundation of the modern approaches that followed: Batch smoothing least
squares optimization methods [31], its incremental equivalent [65] and incorporating
loop closures [26] [27].
2.4.1
3D Model Reconstruction using Ground Robots
The 3D model reconstruction problem has attracted substantial research interest
over the last 10 years. Although 3D model reconstruction by marine robots has
not been studied extensively due to its difficulty, comparable processes have been
researched using ground robots. Several robotic platforms were used and different
mapping algorithms were proposed. Two different types of sensors (visual sensors
and laser sensors) have been used, and each type has strengths and weaknesses.
40
Visual sensors are less expensive, but they do not provide data in R3 . However, by
using machine learning techniques or vehicle motion or stereo vision, this method can
obtain 3D data, making reconstruction feasible [91] [62]. Currently, vision-based 3D
model reconstruction can be mainly accomplished in indoor environments where the
distances are small and the brightness is limited.
In the early stages of large-scale outdoor mapping research, 2D laser scanners
were used to gather 3D point data. Howard et al. [57] gathered 3D data by mounting a 2D laser scanner on a segway. They fused GPS and INS measurements to get
estimates of vehicle trajectory and reconstructed 3D point cloud representations of
outdoor environments. Thrun et al. [130] reconstructed 3D point cloud maps of indoor environments using two laser scanners (2D). The first laser scanner was mounted
horizontally and was used, combined with odometry measurements, to localize the
vehicle. The second laser scanner was mounted vertically to scan the environment.
Because the vertical scanner is not able to scan horizontal surfaces Zhao and Shibasaki
[136] used 2D laser scanners mounted vertically and shifted 45 degrees to be able to
capture horizontal surfaces. In their work, they used GPS and an expensive INS to
localize the vehicle; estimates of vehicle trajectory and the laser scanner data were
used to reconstruct 3D maps of outdoor environments.
In later research, vehicles were capable of directly gathering 3D point clouds by
coupling a 2D laser scanner with a low cost nodding actuator or by using a 2D spinning
laser scanner. Harrison and Newman [50] developed a system able to gather 3D point
clouds by coupling a 2D laser scanner with a low cost nodding actuator. They used
odometry measurements and feature extraction algorithms to localize the robot and
finally to reconstruct point cloud maps of outdoor environments. A similar approach
was taken by [25]. Recently, Bosse and Zot [17] used a 2D spinning laser scanner to
reconstruct 3D point cloud maps of outdoor environments. They used scan matching
algorithms such as the ICP without using any other navigation sensor. A similar
approach was taken by Holenstein et al. [52] in reconstructing watertight surfaces of
caves using 3D laser data.
A similar approach was taken by Nuchter et al. [95].They used a ground robot
41
equipped with dead-reckoning sensors and a rotating 2D laser scanner to reconstruct
occupancy grid based maps of outdoor environments. The algorithm proposed was
based on the ICP algorithm and dead reckoning measurements obtained onboard that
were used to initialize the ICP algorithm. In addition efficient data structures such
as KD-trees, were used to accelerate the ICP algorithm. In another work, Nuchter
et al. [96] proposed an algorithm that does not use any navigation sensor. This new
approach is based on the existence of skyline features. The skyline features were
extracted from panoramic 3D scans and encoded as strings enabling the use of string
matching algorithms for merging the scans. Initial results of the proposed method in
the old city center of Bremen are presented. They also use the ICP algorithm for fine
registration.
Significant progress on 3D model reconstruction was achieved by Newman and his
group [92]. Newman and his group utilized a combination of stereo vision and laser
data to first localize the vehicle and then reconstruct 3D maps of the environment.
They employed the Sliding Window Filter developed by Sibley [119] (that marginalizes
poses that are further away as opposed to full pose batch optimization methods) to
approximate the locally optimal trajectory. The proposed system also identifies and
utilizes loop closures; their loop-closure system, FAB-MAP is described in [26] [27].
After estimating a good trajectory, they use the laser scanner data obtained and the
estimated trajectory to produce a 3D point cloud map of the environment. They
managed to illustrate good localization results using stereo cameras and loop closures
for missions over 142km [1201.
The majority of the work presented in the previous paragraphs reconstruct point
cloud maps. Konolige et al. [73], instead of using point cloud representation of
the environment, proposed the use of occupancy grid based maps. They developed a
ground vehicle equipped with stereo cameras, GPS and wheel encoders. Using feature
extraction methods they computed visual odometry. The visual odometry, the GPS
and dead reckoning were used to localize the vehicle and reconstruct occupancy grid
maps of outdoor environments. In an extension of their work [72] they incorporated
loop closures and formulated the problem as a Bayes graph estimation problem which
42
was solved using non-linear least squares techniques which computes the trajectory
that minimizes the least squares error.
In recent research, Tong et al. [131] designed a vehicle and algorithms for mapping planetary work site environments. The vehicle used was a rover equipped with
a 2D laser scanner mounted on a pan-tilt mechanism to utilize 3D data capture,
stereo camera to provide visual odometry measurements and IMU. Advanced feature
extraction algorithms were used to extract features. Extracted features and IMU
measurements were used to localize the vehicle using advanced full pose non-linear
least squares SLAM techniques.
2.4.2
3D Model Reconstruction using Marine Vehicles
3D model reconstruction by marine robots deals mostly with underwater surfaces
and reconstruction of bathymetric maps. In the early years of bathymetric mapping,
Singh et al. [121] presented results taken from deep seas around Italy. Their approach
utilized a Long Base-Line (LBL) system that localized the underwater vehicle and a
set of sonar sensors (side-scan sonar and an actuated pencil profiling sonar ) to scan
the seabed. At the same time, Bennett and Leonard [111 reconstructed bathymetric
maps of the Charles River using the AUV Odyssey II with use dead-reckoning sensors
on board and a single altimeter sonar.
In more recent years bathymetric research has been focussing on using SLAM
algorithms to localize the vehicle.
Ruiz et al.
[127] used a side-scan sonar and
onboard sensors to reconstruct bathymetric maps. They manually extracted features
out of the sonar data and they used SLAM techniques to localize the vehicle and get
better estimates of bathymetric maps. A similar approach was taken by Shkurti et
al. [118]. They designed a new vehicle called The AQUA Underwater Robot, which
is equipped with IMU and cameras. By extracting features on the seabed and using
IMU measurements they are able to reconstruct the bathymetric map of small areas.
Their approach utilizes an EKF SLAM approach. Roman et al. [112] reconstructed
bathymetric maps by fusing navigation measurements and relative poses given by the
ICP algorithm.
43
Johnson et al. [63], using an underwater vehicle, presented results on large scale 3D
model reconstruction and visualization of bathymetric maps from Australian coastal
waters. The proposed vehicle was equipped with stereo cameras, sonars, a DVL,
accelerometers, depth sensors, GPS (it surfaces to acquire GPS fixes) and was also
using Ultra-short Base Line (USBL). Using stereo cameras they extracted features
that are used to compute visual odometry. The visual odometry was combined with
readings from other sensors on board to estimate vehicle trajectory and then the
vehicle trajectory was used to reconstruct the map of the seafloor using reading from
cameras. They also used meshing algorithms to reconstruct mesh representations of
the seafloor.
In a different direction of the surface reconstruction problem using marine vehicles,
Fairfield et al. [40] and [39] used a hovering underwater vehicle to reconstruct surfaces
of caves and tunnels. Their method consisted of a Rao-Blackwellized particle filter
with a 3D evidence grid map representation. In addition, they used occupancy based
grid representation of the environment. More recently, significant progress has been
achieved on surface reconstruction for ship hull inspection by Hover et al. [56].
The closest work to that presented in this work (Chapter (3)) is Leedekerken et
al. [83]. In Leedekerken et al. [83], a set of 2D laser scanners is combined with a
high-accuracy localization unit that combines GPS-IMU and DVL. The use of highaccuracy localization units enable Leedekerken et al. [83] to present high-accuracy
surface reconstruction results from non-moving structures. In contrast, our work
uses a powerful LiDAR Velodyne laser scanner and assumes a GPS-denied environment without using any other localization sensors such as IMU or DVL. In addition,
Leedekerken et al. [83] use a forward-looking sonar that mostly provides bathymetry
data rather than data on the submerged part of the structure, whereas we scan the
below-water part of the marine structure of interest with a side-looking sonar.
Another key difference between our approach and other surface reconstruction approaches (including [83]) is that our approach can reconstruct 3D models of moving
structures such as floating platforms and boats. Results, presented later in this thesis, indicate that the proposed algorithm can reconstruct 3D models of slowly moving
44
structures. Slowly moving structures are commonly found in the offshore oil industry. Reconstructing models of large 3D moving structures is not easily done using
known SLAM techniques that first compute vehicle trajectory and then project laser
scanners or cameras data using vehicle trajectory. In the case of moving structures
this cannot be done because vehicle trajectory and laser data obtained from different
views are not sufficient to describe the structure. In this work, we are not interested
in vehicle trajectory, instead we compute an equivalent trajectory (set of transformation matrices) that minimizes registration error between different scans. In the
case of stationary structures, the equivalent trajectory obtained should be close to
the actual one.
45
46
Chapter 3
Surface Reconstruction Using
Marine Vehicles
Over the last ten years, significant scientific effort has been dedicated to the problem
of 3D surface reconstruction for complex structures. However, the critical area of
marine structures remains insufficiently studied. The research presented here focuses
on the problem of 3D surface reconstruction in the marine environment. This Chapter
summarizes our hardware, software, and experimental work on surface reconstruction
over the last five years. We propose the use of off-the-shelf sensors and a robotic
platform to scan marine structures both above and below the waterline, and we
develop a method and software system that uses the Ball Pivoting Algorithm (BPA)
and the Poisson reconstruction algorithm to reconstruct 3D surface models of marine
structures from the scanned data. We have tested our hardware and software systems
extensively in the field in the Singapore coastal zone, including operating in rough
waters, where water currents are around 1-2m/s. We present results on construction
of various 3D models of marine structures, including stationary jetties and slowly
moving structures such as floating platforms and moving boats. Furthermore, the
proposed surface reconstruction algorithm makes no use of any navigation sensor
such as GPS, DVL or INS.
47
3.1
Introduction
As we illustrated in the introduction of this thesis, the inspection problem can be decomposed into the inspection planning problem and the surface reconstruction problem. In the next three chapters we focus on the path and inspection planning problems
and we develop algorithms that compute optimal inspection trajectories, whereas here
we focus on the surface reconstruction problem. In addition, our inspection planing
work does not depend on the eventual inspection application. In this chapter, we
focus on marine structure surface reconstruction problem.
As a starting point we assume that we have data such as laser scanner data and
sonar data that covers all details of a given structure and we would like to put together
this data to reconstruct 3D models (e.g. CAD models and point cloud representations)
of the structure of interest. We will start this chapter by describing a novel vehicle (a
robotic kayak) we assembled to gather data from marine structures (Section (3.2)).
Section (3.3) explains our surface reconstruction algorithms.
In Section (3.4) we
present our experimental results, and we close summarizing our work in Section (3.5).
Performing the experiments presented in this chapter, we realized how difficult is to
design agent paths to make sure that all parts of a given structure would be eventually
visible during execution time, and this is part of our motivation for the work presented
in Chapter (4), Chapter (5) and Chapter (6).
3.2
Robotic Platform for Marine Field Experiments
Our goal is to generate 3D models for marine structures. Given the complexity of
the marine environment, we would like to use a small but powerful vehicle with high
maneuverability that will be able to access hidden places of the structure without
crashing into the structure (due to water currents). For this purpose, we use a SCOUT
Autonomous Surface Vehicle (ASV) - a kayak with a 3m length, 0.5m width, and
90kg mass [28]. The ASV is equipped with a 245N thruster for propulsion, as well
as a steering servo (Fig. (3-1 (a))).
48
No localization sensors such as GPS, INS, or DVL are used in this work, but the
vehicle is nevertheless equipped with a GPS (Garmin GPS-18) and a compass (Ocean
Server OS5000) so that, in the future, it will be able to expand our current work in
all possible directions.
To facilitate data capturing and autonomous control capability, the main compartment of the ASV is equipped with a Main Vehicle Computer (MVC). The MVC
consists of a pair of single-board computers connected through an Ethernet cable.
Each single-board computer is equipped with 1GB RAM. In addition, one of the
single-board computers is equipped with a 120GB hard drive to enable a large data
capturing capability. The ASV can be controlled remotely using a remote control or
autonomously using the well-known autonomy operating system MOOS [93],[10].
(a)
(b)
(c)
Figure 3-1: (a) The ASV with Velodyne LiDAR mounted on top of it. Two pontoons
are attached to the ASV to improve its stability. (b) The Velodyne LiDAR and
the mounting platform for placing the LiDAR in an inverted configuration on the
ASV. (c) The BlueView MB2250 micro-bathymetry sonar is mounted in a sideways
configuration.
49
Registration algorithms frequently fail because the two point clouds to be combined have no common features. Given that our goal is to perform surface reconstruction without using any navigation sensors, we use a laser scanner with a wide
field of view that allows significant overlap between subsequent scans. In addition,
we use a laser scanner that completes a scanning action much faster than the vehicle
and structure motion, so that we do not need to incorporate vehicle and structure
motion within a single scan. This makes the procedure simpler and reduces the computational cost of the algorithm. One sensor that meets the desired specifications is
the Velodyne HDL-64E S2 shown in Fig. (3-1(b)). The Velodyne HDL-64E S2 is a
3D LiDAR (Light Detection and Ranging) which completes each scanning action in
0.1 second (scanning frequency =10 Hz).
The Velodyne LiDAR was initially developed for the 2007 Urban DARPA Grand
Challenge
[841.
Its original configuration was supposed to be mounted on car roofs to
perform scanning actions in which a full 360-degree horizontal picture with vertical
arc of 26.80* (from 2* to -24.80) is captured. The Velodyne is mounted on the kayak
in an inverse configuration to maximize the scanning surface. To reduce the amplitude
of the rolling motion caused by surface waves, we installed stabilizing pontoons (see
Fig. (3-1(a))).
To scan the below-water part of the marine structure of interest, we use a 3D
Micro-bathymetry sonar (BlueView MB2250 micro-bathymetry sonar). The MB225045 sonar uses 256 beams with one degree beam width in elevation.
Since we are
interested in mapping marine structures, instead of mounting the sonar in a forwardlooking configuration such as [83], we mount it sideways on the vehicle (Fig.(3-1(c))).
3.3
Surface Reconstruction Algorithms
The goal of this Chapter is to illustrate that a 3D model of marine slow-moving
structures in a sea environment can be constructed through the use of a single LiDAR
without using any other scanning or navigation sensor. Since this work is an early one
to be done on surface reconstruction of marine structures that are partially submerged
50
in GPS-denied environment, we wish to simplify our testing procedures; thus, we do
not use advanced techniques like feature extraction and loop closures. In addition,
as explained in Chapter (1), advanced SLAM techniques that optimally localize the
agent and reconstruct maps, in their current state, are not easily used to reconstruct
the shape of moving structures.
Based on the vehicle design described in the previous section, we propose algorithms to construct the 3D model of partially submerged marine structures. We
propose three different algorithms; the first one (Algorithm(1)) is used to reconstruct
point cloud representations of the above-water part of marine structures; the second
one, (Algorithm(2)) is used to reconstruct mesh models of the above-water part of
marine structures; and the third algorithm (Algorithm(3)) is used to reconstruct 3D
models (point cloud representations, mesh representations and occupancy grid based
maps) of both parts (the above- and below-waterline parts) of partially submerged
marine structures.
We use scan matching techniques to construct the 3D model for above-water part.
We then use the transformations, computed by the scan matching algorithm for the
above-water part, to construct the 3D model from a sequence of 2D sonar data from
the below-water part. We then combine the 3D model of above- and below-water
parts to construct a complete 3D model of the partially submerged marine structure.
We construct two types of maps: a low quality and a high quality map. The low
quality map can be constructed on-line and would be useful for navigation purposes.
The high quality map is constructed off-line and can be used for inspection purposes.
3.3.1
Registration Algorithm
To scan a marine structure, the vehicle is driven around the structure of interest,
gathering 3D laser data. The data is logged and saved in the ASV's computer in
data structures called point clouds. Each point cloud represents a set of points in
R3
gathered by a complete 3600 rotation of the LiDAR. Since the scanning action is
performed much faster than the vehicle's speed, all points within a point cloud are
expressed within a common orthogonal reference frame that is aligned to the center
51
of the LiDAR at the starting point of the scanning cycle.
Given 2 point clouds M in RMX3 and Mj in RDX3 that include common features,
we need to compute the transformation iT that transforms each point mk of M to
&
each point d, of M,. This problem was originally proposed and solved by Besl
McKay in 1992 using the ICP algorithm [14], by minimizing the following metric:
M
E('Rj,,b,) =
D
EZ
(wk,11mk- (iRjdl +4 bj)11 2)
(3.1)
k=1 1=1
Here,
Wk,l
is a binary variable as follows: wk=1 if mk is the closest point to d, within
a close limit and is equal to zero otherwise. 'Rj and 'bj are the rotation matrix and
the translation vector defined in [951. The minimization is done using non-linear
optimization tools such as the Levenberg-Marquardt algorithm [85].
3.3.2
Above-Water Surface Reconstruction Algorithm
The vehicle gathers point clouds (M1 , M2 ,...M,- 1 , M) with frequency of 10 Hz. We
sequentially merge these 3D point clouds, each in their respective coordinate systems,
into a single coordinate system. The transformations between sequential point clouds
(0 T1 ,1 T2 -,- T._1,"- Tn) is given by the TCP algorithm described in the above section. The first point cloud in the sequence is transformed into the coordinate system
of the second point cloud. The union of the first two point clouds is then transformed
into the coordinate system of the third point cloud, and so on. This process continues
until we transform all the point clouds into the coordinate system of the last point
cloud in the sequence.
The Velodyne LiDAR generates around 8 MB of data per second (250,000 points
per scanning cycle and 10 scanning cycles per second).
The computational cost
and memory requirements to deal with this amount of data are huge, making ICP
impossible to run on-line. The time for &single merging process in the worst-case
is O(NlogN) where N is the number of points in the current map. This worst-case
complexity is dominated by searching for corresponding points. For online-mapping,
we speed up the search process by using the ASV maximum speed to bound the
52
maximum possible displacement d, and hence limit our search space. Furthermore,
we fix a maximum number of possible iterations to ensure termination within the
required time.
In addition, we reduce these computational demands in two other ways. First,
instead of using the raw data as gathered, we use data from scanning actions performed every At seconds. Second, we perform spatial sub-sampling on each point
cloud by discretizing the bounding box of the point cloud into a regular grid with
user-specified resolution; thus, all the points inside a single grid cell are represented
by a single point. By limiting cells to a given size (resolution), we both reduce the
amount of data to a reasonable quantity and also reduce the errors (assuming zero
mean noise).
To get the on-line map, we simplify the data as described above using a large
simplification cell size and large At, as shown in Fig. (3-2) and Algorithm (1). This
on-line map is a low-resolution map that can be used for navigation.
Laser
Scanner
Simplification
Algorithm
Registration
Algorithm
Merge
Simplified Data
Figure 3-2: The online map: we use big simplification cell size.
Algorithm 1 Construct3DModel(P, s, t) Construct a 3D model from a sequence
of point clouds P. The input s and t are the user-specified spatial and temporal
resolution, respectively.
1: MergedData = SpatialSubSampling(P[1], s).
2: for i = t to JP| step t do
3:
P' = SpatialSubSampling(P[i], s).
4:
Let TO be the identity transformation matrix.
5:
T = ICP(MergedData,P', TO).
6:
MergedData = Transform (MergedData, T).
7:
MergedData = MergedData U P'.
8: end for
9: Return MergedData.
To get a higher quality map, we want to use as much data as we can handle. To
do so, we generate an occupancy grid-based map under a probabilistic framework
53
such as OctoMap [1331. Because the LiDAR generates a huge amount of data, we
still use simplified data (albeit less simplified than above), rather than raw data, in
order to get the transformation matrices. We then use the transformation matrices
to merge the raw data, resulting in a single, dense 3D point cloud. An occupancy
grid-based map is then generated using OctoMap. The resulting grid-based map is
used to get the mesh of the structure using the ball-pivoting algorithm [13]. This
process is shown in Fig. (3-3) and in Algorithm (2). This high-resolution map must
be generated off-line, and can be used for inspection or for further analysis (depending
on the application).
Laser Scanner
Raw Data
Save Raw Data
Simplification
Algorithm
Registration
Algorithm
Transformation
Matrices
octoMap
Merg Raw
Meshing
Figure 3-3: The off-line map: Less simplified data are used to get the vehicle's trajectory (a set of transformation matrices) and then the vehicles' trajectory is used to
project raw data resulting in a dense point cloud.
Algorithm 2 Construct3DModel(P, s, t) Construct a 3D high quality models (mesh
representation, dense point clouds, occupancy grid) from a sequence of point clouds P.
The input s and t are the user-specified spatial and temporal resolution, respectively.
1: MergedData = SpatialSubSampling(P[1], s).
2: for i = t to IPI step t do
3:
P' = SpatialSubSampling(P[i], s).
4:
Let TO be the identity transformation matrix.
5:
T = ICP(MergedData,P', TO).
6:
DenseMergedData=Transform (P[i], T).
7:
DenseMergedData = DenseMergedDataU P[i].
8:
MergedData = Transform (MergedData, T).
9:
MergedData = MergedData P'.
10: end for
11: OccupancyGrid=Octomap(DenseMergedData)
U
12: MeshModel=BallPivoting(OccupancyGrid).
13: Return MergedData, OccupancyGrid, MeshModel.
54
Two parameters drastically affect the computational cost of our method: At and
cell size. A small At results in big computational costs, leaving time intervals between
scans that are too small to solve the problem in real-time. On the other hand, given
that we are not using other localization sensors, a large At may result in the failure
of the ICP algorithm, since the algorithm cannot merge point clouds gathered from
sequential locations that are not sufficiently close to each other (i.e., get stuck in a
local minimum).
In Fig.
(3-4), we can see the trajectories that the ICP algorithm produces for
different values of At (C1, C 2 ... C).
We observe that trajectories corresponding to
different values of At form a sequence with decreasing differences as At goes to zero
(i.e., the sequence trajectories have the Cauchy property and thus there exists a limit).
Therefore, for this particular dataset, the benefit of reducing At below 1 second is not
worth the computational cost for either the online mapping or the offline mapping.
sec
2.5 sec
-2sec
+ 1.5 sec
0 1sec
-3
25 -
--
20 -
15
E
10.
5.
0
-20
A
to
-15
-10
-5
0
5
10
Xm
Figure 3-4: T rajectories generated during the first 100 seconds of one of the experiments: Trajectories corresponding to different At form a sequence with decreasing
difference.
Regardless of cell size, as long as it is small enough to capture geometrical features
55
that are important to the ICP algorithm, it does not affect the localization. For
the off-line map, simplification cell size generally does not matter, as long as the
localization works properly, since we are using vehicle's trajectory to project dense
raw data'. However, for the on-line map, the cell size is bounded by the accuracy we
want to have in the map.
3.3.3
Combined Map
In order to create a complete 3D model of the entire partially submerged marine
structure, we use the 3D Micro-bathymetry sonar described in the previous section
to get the below-water part of the marine structure, and then we combine this data
with the model of the above-water part of the structure. The vehicle's trajectory
generated by our above-water mapping algorithm is used to register the 2-D sonar
data into the global 3D map.
The vehicle's sonar generates 2-D data in the polar coordinate system (r, 0, I),
where r and 0 are the ranges and the angles of the returns and I is the intensity
(see Fig. (3-6)). Since we want to project 2-D sonar data into the Cartesian global
coordinate frame generated by the above-water surface reconstruction algorithm, we
transform the 2-D sonar data to an equivalent local Cartesian frame using the equations below:
ax, = r cos 4
(3.2)
= r sin k
(3.3)
8z,
', = 0
(3.4)
The sonar data is then transformed into the current LiDAR coordinate system
using Equation (3.5). This allows the sonar data to be treated and propagated as
'In the offline map, the voxel size and the probability threshold given in the OctoMap algorithm
are important and reflect the resolution we want to capture.
56
Figure 3-5: The coordinate systems of LiDAR and sonar. 'b, and 'R, denote the
translation vector and the rotation matrix that transform the sonar data to the Velodyne coordinate system
57
equivalent to Velodyne data as described in section (3.3.2)
(3.5)
"x, = [T][X]
where [ 8TJ] is the transformation matrix from the sonar coordinate system to the
Velodyne system, as shown in Fig. (3-5) and 'X, = [X",
8
z,
y,]T.
At the present
time, the registration between sonar and LiDAR data is done by manually measuring
the transformation from sonar frame to the LiDAR frame. A registration algorithm
to perform the registration between LiDAR and sonar data can be easily implemented
in practice. The proposed algorithm for surface reconstruction of the combined model
is shown in Fig. (3-7) and Algorithm (3)
ZS
U
Figure 3-6: Sonar coordinate system: Sonar gives returns up to a range of 10 m with
an angle of 45 degrees.
Sonar data is noisy, so to clean up the data, we extract objects from the raw sonar
data using cluster filtering methods. The main concern is to separate the object from
the noise. For this purpose, we use simple background removal on the 2D intensity
map. Initially, we capture a sonar reading when no objects are within the sonar's
range. The 2-D intensity map Fig. (3-8(b)) of this scan becomes the "background"
intensity map.
For robustness, we do not compare the object intensity map and
58
A sequence of 3FMerge
point clouds
A 3b-point
cloud
1A 3D pomnt
'cloud of the
Narmne
structure
57":
transformation
matrix
A sequence of 20
A 3D-point
cloud
OW*intensity maps
A 3D triang ular mesh mo el of
the marine structure of interest
(using octrees and BPA)
Figure 3-7: Surface reconstruction of both parts of marine structures, above and below waterline. The ICP algorithm is used to find the transformation matrices between
different poses (using the laser scanner data). Then we use the transformation matrices to register the laser scanner data under the same coordinate frame and register
the unregistered 2D sonar data as 3D data to the same coordinate frame. Then we
use a probabilistic framework such as the occupancy grid based maps to clean up the
point clouds, and then we use known surface reconstruction algorithms (ball pivoting algorithm and the Poisson reconstruction algorithm) to reconstruct a 3D surface
model of the marine structure.
59
Algorithm 3 Construct3DModel(P, S, s, t,b8 ,,R 8 ,) Construct a 3D models (of the
above- and below-water parts of marine structures) from a sequence of point clouds P.
The input s and t are the user-specified spatial and temporal resolution, respectively.
1: MergedData = SpatialSubSampling(P[1], s).
2: for i = t to 1P1 step t do
3:
P' = SpatialSubSampling(P[i], s).
4:
5:
6:
Let TO be the identity transformation matrix.
T = ICP(MergedData,P', TO).
MergedData = Transform (MergedData, T).
MergedData = MergedData U P'.
{St,,}=FindSonarLogs(t,t - 1)
SonarMergedData/=SonarRegistration ({Stn,}, T,b',,R,).
SonarMergedData=SonarMergedData U SonarMergedData
7:
8:
9:
10:
11: end for
12:
13:
14:
15:
16:
OccupancyGrid=Octomap(DenseMergedData)
OccubancyGridSonar=Octomap(SonarMergedData)
SonarAndVelodyneGrid=OccupancyGrid U OccupancyGridSonar
MeshModel=BallPivoting(SonarAndVelodyneGrid).
Return MeshModel, SonarAndVelodyneGrid.
background intensity map for each pixel. Instead, we use the background map to find
a good threshold to determine if an intensity at a particular pixel can be considered
as object or just noise. This is done by dividing the background intensity map into 6
clusters, Fig. (3-8(b)), based on the background intensity map and then use the most
frequent intensity in each region as the threshold. Given an object intensity map,
we divide the map into 6 regions as in the background intensity map, and consider
a point to be part of an object whenever its intensity is higher than the threshold
for the region. Fig. (3-8(a)) compares the raw sonar data to the data that has been
cleaned using clustering filtering methods.
3.4
Experimental Results
To evaluate our algorithms, we performed a set of experiments in the Singapore
area between January 2009 and August 2010. Results presented in this section were
initially presented in our ISOPE 2011 and IROS 2011 conference papers [76, 1081
and were published to Journal of Field Robotics [1071. Results given here are post60
7M
IS
2ws
in
to
(a) In the left figure we can see a frame of raw noisy sonar data, in
the figure in the right we can see the filtered sonar data using a filter
based on clustering.
(b) An empty sonar frame divided
into 6 clusters.
Figure 3-8: Filtering the sonar data.
61
processing results, since none of our surface reconstruction algorithms were running
in real-time. The experiments were done under the umbrella of the CENSAM project
[1091.
Our goal was to test our system in rough water environments. However, before
testing our system in rough water, we performed preliminary experiments in calm
water to ensure the system is ready for rough water testing. To test the system in
rough water, first we need to decide the location and the time of testing. We would
like to find marine structures, such as jetties, that would be as close as possible to
open waters. Therefore we chose a jetty in a small island (of size less than a square
kilometer). To decide the time and the dates of the experiments we look at the tide
and water current predictions to ensure that the environment is challenging enough
but not too extreme that could possibly be too risky (water currents with speeds
greater than 4 m/s are considered, for our case, dangerous environments). In order
to go to the operational area we had to use a ship and a few auxiliary boats. We
ran the experiments onboard a boat (e.g. Pandan Reservoir experiment, or the boat
reconstruction experiment) or from a workstation at the marine structure of interest
(e.g Selat Pauh experiment).
The first experiment was performed in a calm water environment, in Pandan
Reservoir, Singapore and we reconstructed point clouds and alpha shape representations of the above part of a jetty (see Fig. (3-9(a))). The vehicle was also equipped
with a sonar micro-bathymetry sensor but for technical reasons we did not manage
to gather reliable sonar data to reconstruct the below-water part of the structure.
To give an illustration of the difficulty in merging the scanned data, Fig (3-9(c))
shows the resulting 3D point clouds scanned by the LiDAR, plotted in one coordinate
system. Fig. (3-9(d)) shows the resulting point clouds representation of the structure
when the coordinate systems are transformed to the first coordinate system based on
the GPS and compass information alone. Fig. (3-9(b)) and Fig. (3-9(e)) show the
results of our 3D model reconstruction algorithm on the above data set.
The second experiment was performed in rough sea environment in Selat Pauh in
the Singapore Straits (Fig. (3-10(a))). The water currents at Selat Pauh are around
62
(a)
(c)
(b)
(d)
(e)
Figure 3-9: Reconstruction of a jetty in Pandan Reservoir, Singapore. (a) The target
jetty. (b) Side view of the jetty model constructed by our algorithm. (c) Top view
of multiple frames of the 3D LiDAR data before processing. (d) Top view of the
constructed 3D model based on GPS and compass information. (e) Top view of the
constructed 3D model generated by our algorithm.
2m/s.
In addition, Selat Pauh is a busy strait with a significant amount of ship
traffic, causing high frequency water wakes that significantly disturb small marine
vehicles.
In that particular experiment we reconstruct point clouds representation
for a slowly moving boat, illustrated in Fig. (3-10(b)). Although the boat is moving
slowly, the water currents and wakes cause the boat to drift and heave significantly.
As an illustration of the effect of water currents and wakes on the scanned data, Fig.
(3-10(c)) and Fig. (3-10(f)) show the 3D point clouds scanned by the LiDAR over
a 2-seconds period, plotted in the same coordinate system. Fig. (3-10(d)) and Fig.
(3-10(g)) show the data when the coordinate systems are transformed to the first
coordinate system based on GPS and compass information alone. Fig. (3-10(e)) and
Fig. (3-10(h)) show the results of applying our 3D model reconstruction algorithm
to the above data set.
In our last experiment we present results from two missions performed with a jetty
63
('),()
(e
C>C
(0(g
Figure 3-10: Reconstruction of a slowly moving boat in rough sea water environment,
in Selat Pauh, Singapore. (a) Our operating area. (b) The target boat. (c) Side
view of multiple frames of the 3D LiDAR data before processing. (d) Side view of
the constructed 3D model based on GPS and compass information. (e) Side view of
the constructed 3D model generated by our method. (f) Top view of multiple frames
of the 3D LiDAR data before processing. (g) Top view of the constructed 3D model
based on GPS and compass information. (h) Top view of the constructed 3D model
generated by our method.
located at Pulau Hantu (a small island a few kilometers away from Singapore). We
deployed our vehicle from a ship near Pulau Hantu and drove it about the jetty to
gather data. We present two missions. The first mission lasted 3 min and gathered
data for the above-water part of the jetty. In this mission, we drove the vehicle a
distance of about 200 m making sure that the vehicle approached the structure from
different views to recover all the hidden parts of the structure. The second mission
lasted 1 min and gathered data from the above- and below-water parts of the floating
64
platform that was located in front of the jetty (see Fig. (3-16(a)).
We present three different maps of the jetty and the floating platform. The first
map is a low-quality point cloud-based map that could be generated online and can be
used for navigational purposes (Fig. (3-11(b),3-11(c)) ). The second map is a higher
quality mesh-based map ( Fig. (3-12,3-13,3-14,3-15)).
The third one combines both
the above- and the below-water parts of the marine structure (Fig. (3-16(b),3-16(c))).
In Fig. (3-11(b),3-11(c)) we can see the low-resolution point cloud-based maps of
the jetty for different cell sizes. We can verify that the one that was generated with
30 cm cell size can be generated real-time. For both cases At = 1 second.
In Fig. (3-12,3-13) we present different views of the high-quality mesh-based maps
of the jetty (the mesh was reconstructed using the ball pivoting surface reconstruction
algorithm). The voxel size used in the occupancy grid generation was 8cm and At =1
second. Here we present two different high quality maps; the first one (Fig. (3-12))
was generated using a high probability threshold for occupied cells, and the second
one (Fig. (3-13)) was generated using a low probability threshold for occupied cells
resulting in a dense map.
In Fig. (3-14) we can see the mesh-based high quality map for the above-water
part of the jetty, using a low probability threshold for occupied cells and the Poisson
surface reconstruction algorithm. From our results we can see that the Poisson surface
reconstruction algorithm produces better results than the results the ball pivoting algorithm gives. In addition the computation time of the Poisson surface reconstruction
algorithm is on the order of minutes for a point cloud that includes about 1 million
points. On the other hand the ball pivoting algorithm took several hours to run. In
Fig. (3-15) we can see the Poisson surface reconstruction results focused on certain
parts of the structure.
The upper part, shows the pillars of the structure, we can
see that the algorithm can capture the pillars' geometry pretty well. The lower left
part, shows a surface of a human body; during the experiments we have people sitting
and possibly moving on the jetty, thus the inside part of the structure presents some
anomalies. In the lower right part of Fig. (3-15) we can see two large buoys that are
used to avoid direct collision of boats on the jetty.
65
(a) The marine structure of interest.
(b) Low-resolution map, cell size= 30cm, the mission lasts 3mins, is generated in
3mins (3GHz CPU).
(c) Low-resolution map, cell size= 18cm, the mission lasts
8mins (3GHz CPU).
3mins, is generated in
Figure 3-11: Low resolution "real-time" model.
66
Fig.
(3-16) shows results from both parts of a marine structure.
Specifically,
Fig. (3-16(b)) and Fig. (3-16(c)) show the point based map for both the above- and
below-water parts for the floating platform.
Fig. (3-16(d)) is a zoomed-in view of
the combined mesh-based map. We notice that the buoy that supports the floating
platform is flattened due to regular contact with boats. In all cases, the mesh is generated using meshlab (a tool developed with the support of the 3D-CoForm project)
[132].
Figure 3-12: The mesh-based high quality map for the above-water part of the jetty,
using a high probability threshold for occupied cells and the ball pivoting surface
reconstruction algorithm. Cell-size=8cm.
Figure 3-13: The mesh-based high quality map for the above-water part of the jetty,
using a low probability threshold for occupied cells and the ball pivoting surface
reconstruction algorithm. Cell-size=8cm.
67
Figure 3-14: The mesh-based high quality map for the above-water part of the jetty,
using a low probability threshold for occupied cells and the Poisson surface reconstruction algorithm. Cell-size=8cm.
68
Figure 3-15: Zoom in of the mesh-based high quality map for the above-water part
of the jetty, using a low probability threshold for occupied cells and the Poisson
reconstruction algorithm. Upper part, shows the pillars of the structure. Below left
part, shows a surface of a human body (during the experiments we have people sitting
and possibly moving on the jetty). Below right, shows two large buoys that are used
to avoid direct collision of boats on the jetty. Cell-size=8cm.
69
(a) Picture of the front part of the structure
(b) Above- and below-water parts of marine structure, point cloud-based map. In red color below
water part, in blue color above water part.
(c) Above- and below-water parts of marine structure, point cloud-based map. In red color below
water part, in blue color above water part.
(d) Above- and below-water
Small details.
mesh-based map:
Figure 3-16: Combined map: a) Picture of the front part of the structure. b) Point
cloud-based map, front view. c) Point cloud-based map, side view. d) Above- and
below-water mesh-based map.
70
Results Validation
In this section we validate our experimental results. Probably, the best way to evaluate our results is to use a mesh comparison method, such as the one developed by
Roy et al. [113] that compares 3D models and produces statistics on mesh similarity. Unfortunately, we have no access to 3D models of the jetties we reconstructed,
however we infer our results quality in the following ways:
" We compute the quality of all ICP registrations using as a metric the ICP
"goodness" as defined in [16]. For the cases we studied in this work, the mean
"goodness" for all ICP registrations is 96.5% with standard deviation about
2%. At the same time the ICP took on average 277 iterations to converge.
Another way to capture a numerical quality of the accuracy of the registration
is described by Douillard et al. [34].
" To show consistent reconstruction from both parts of the marine structure of
interest we present an occupancy grid based map of the above- and belowwater parts of the structure and the probability of each cell to be occupied.
The color indicates the probability of the cells to be occupied, Fig. (3-17). Low
probability gives blue colors and high probability gives red colors. From this
figure we can clearly see the waterline and three areas: The above-water part
of the structure, the below-water part of the structure and the interface area
between the LiDAR and the sonar data.
The probability of above-water part of the structure cells to be occupied is close
to uniform and less dense than the ones from the below water part of the structure. Generally, laser scanners produce denser point clouds than sonars, however
this is not the case here because to avoid huge data sets we simplify/regularize
our raw data using very small simplification cell-size (e.g few millimeters to
few centimeters). Still we can see that the above-water part of the structure is
reconstructed consistently.
The underwater part of the structure is also reconstructed consistently and the
probability of the cells of the underwater part of the structure to be occupied is
71
high (the cells with the highest probability are located in the far left side of the
structure where the mission started from and the vehicle probably was sitting
there accumulating sonar data from the same position for a few seconds). In the
interface area between the LiDAR and sonar data the probability of a cell to be
occupied, as expected, is lower than the below-water part of the structure but
is high enough to achieve surface reconstruction in the interface area. In the far
right area of the underwater part of the structure, we do not get enough sonar
returns. This is probably due to the fact that in this particular location the
vehicle started turning towards the left and thus the side looking sonar (with a
very narrow beam) instead of pointing next to the kayak was pointing towards to
previously scanned areas (e.g. in the left side of the platform, behind the kayak)
and, we then stopped the mission. This problem can be solved using a better
sonar sensor such as the Didson sensor (used by Hover et al. [56].) or designing
trajectories -that take into account vehicle dynamics- to be informative enough
to reconstruct structures e.g. [103] and/or next chapters.
e We measure characteristic lengths of the jetty using google maps and compare
them to the ones we get from the reconstructed jetty. To reduce the effect of
constant errors such as conversion from "Velodyne units" to metric units and
errors due to the direction the aerial picture was taken from, instead of comparing the actual lengths we reconstruct dimensionless numbers that characterize
the structure, Fig. (3-18). In our case we have L1/W1 = 4.9, which is close to
those obtained using the google maps, L2 /W 2
=
4.8 (Error 2%). In addition,
L1/D1 = 3.5, which is also close to L 2/D 2 = 3.4 (Error 2.8%), the ones obtained
using the google maps. Where L 1 , W, D, are the characteristic lengths of the
reconstructed jetty and L 2, W2 , D2 are the characteristic lengths of the jetty as
measured using Google maps.
72
Waeline
Bew
Waterline
........
..
g .. . . ........
Figure 3-17: Upper part: A point cloud representation of the above- and below-water
parts of the structure. The color indicates the density of the points. Lower part: An
occupancy grid based map of the above- and below-water parts of the structure. The
color represents the probability of the cells to be occupied. Low probability gives blue
colors and high probability gives red colors.
Figure 3-18: Comparison between our results and pictures of the actual jetty taken
from google maps.
73
3.5
Summary
In this chapter, we present a hardware and software system to reconstruct 3D surface
models of marine structures that are partially submerged. In particular, the work
described in this chapter achieved the following results.
" Using off-the-shelf sensors and a commercial robotic kayak [281, we assembled
a novel surface vehicle that is capable of using a powerful 3D laser scanner
(Velodyne) and a side-looking sonar to scan marine structures both above and
below the waterline.
" Using Velodyne and sonar data, without using any other navigation sensor such
as GPS, DVL or INS, we propose a method to reconstruct 3D models of partially
submerged slowly moving marine structures.
" We present various surface reconstruction experimental results including results
from sea environments with water currents around 1m/s-2m/s. More specifically, we present three experimental results of mapping the above-water part of
marine structures and one experimental result of mapping both the above- and
below-water parts of marine structures. To the best of our knowledge, these
are new results on 3D surface reconstruction experiments from rough waters
and slowly moving structures. Experiments presented here are as realistic as
possible. We present results in rough waters with moving structures (floating
platforms and boats) under tidal effects (1-3 m) and water disturbances arising
from big ships moving in the busy Singapore waters.
The results show that our robotic system for 3D model construction of marine
structures is reliable to operate in a rough sea water environments. The resulting
scanned data indicates that the LiDAR's mounting platform does not pose significant
degradation in the quality of the scanned data. Furthermore, because of the high
scanning frequency of the Velodyne LiDAR and sufficient overlap between point clouds
generated by different scanning cycles the simple merging algorithm we propose is
sufficient to construct a rough 3D model of marine structures.
74
We also show results indicating that the proposed algorithm can reconstruct 3D
models of slowly moving structures. Slowly moving structures are commonly found in
the offshore oil industry (e.g. Fig. (3-19)). Reconstructing models of large 3D moving
structures is not easily done using standard SLAM techniques that first compute vehicle trajectory and then project laser scanners or cameras data using vehicle trajectory.
In the case of moving structures this cannot be done because vehicle trajectory and
laser data obtained from different views are not sufficient to describe the structure.
In contrast to the SLAM problem, we are not interested in vehicle trajectory, instead
we compute an equivalent trajectory (set of transformation matrices) that minimizes
the registration error between different scans. In the case of non-moving structures,
the equivalent trajectory obtained should be close to the actual one.
We caution that extremely strong currents excite vehicle roll and pitch motions,
causing some of the LiDAR scans to be empty (taken with the LiDAR looking at the
sky). 2 If we try to merge one of these LiDAR scans, the ICP algorithm is likely to
fail. To avoid this problem, before we apply the surface reconstruction algorithm we
filter out LiDAR scans with extremely low amount of data.
Figure 3-19: Different types of oil platforms, the majority of the platforms are floating
and slowly moving. The picture is from the Office of Ocean Exploration and Research.
Executing the experiments we present in this chapter, we realize how difficult is
to compute trajectories that guarantee 100% structure coverage. In the rest of this
thesis we are focusing on the path planning and inspection planning problem.
2
In our case we try to minimize this effect by installing pontoons on the kayak.
75
76
Chapter 4
Asymptotically Optimal Planning
for Systems with Differential
Constraints
Over the last 20 years significant effort has been dedicated to the development of
sampling-based motion planning algorithms such as Rapidly-exploring Random Trees
(RRT) and its asymptotically optimal version (e.g. RRT*). However, asymptotic optimality for RRT* only holds for linear and fully actuated systems or for a small
number of non-linear systems (e.g. Dubin's car) for which a steering function is available. The purpose of this chapter is to show that asymptotically optimal motion
planning for agents with differential constraints can be achieved without the use of a
steering function. A novel analysis on asymptotic optimality of a family of algorithms
that directly sample the control space is presented. This analysis only requires Lipschitz continuity on the differential constraints and the cost function. Furthermore,
building upon existing planners, we propose a new asymptotically optimal family of
planners. As the number of iterations increases, the sequence of control inputs these
algorithms sample, approaches the optimal control trajectory, with probability one.
Simulation results are promising. Theoretical results from this chapter will be used
to solve the inspection planning problem in the next chapter.
77
4.1
Problem Definition
Suppose the set of all possible robot states is S and the set of all possible control
inputs is U. Then the equation of motion is given by:
4(t) = f (7(t),
(t), t)
(4.1)
Where for each time t, 7(t) E S is a state, 4(t) is the time derivative of the state and
and 0(t) E U is a control input. Assuming that S and U are manifolds of dimensions
n and m where m < n, using appropriate charts we can treat S as a subset of R"
and U as a subset of R". Furthermore we assume that f is a Lipschitz-continuous
function with respect to state and control with maximum Lipschitz constant to be
less than Lp E R+.
Supposed Sfie C S is the collision free subset of the state space. Then we define
path to be a time parametrized mapping from time to the obstacle free state space
e.g. 7 : [0, TI --+ Sf. We also define trajectory or control function to be a time
parametrized mapping from time to the control space e.g. 0 : [0, TI -+ U. From
Lipschitz-continuity it can be shown that for each trajectory (control function) there
is a unique path that represents the solution to equation of motion [1221.
Let Sgo.
c S be
the goal region. We would like to compute the control function
(trajectory) and the resulting path that drives the agent from the initial state -y(t = 0)
to the goal region and minimizes a given cost function. More formally, let D(O) be the
resulting cost for control function 0 and D to be the set all admissible trajectories. An
admissible trajectory is a trajectory that induces an obstacle free state-space path
(through Equation (4.1)) and stops within the goal region. Among all admissible
trajectories we would like to compute the one that minimizes the cost function, e.g
0* = arg min D()
78
(4.2)
4.2
Planners that Sample the Control Space
In this section we describe the proposed family of algorithms. Since we are interested in systems with challenging dynamics, all of the proposed algorithms sample
the control space directly. Such algorithms are not new[61][87], but the question of
whether such planners can converge to the optimal solution is still largely open. To
answer this question, we first present two minor modifications of existing planners as
detailed in Section 4.2.1.
4.2.1
Algorithm Description
Each of the proposed algorithms constructs a tree T = {M, E} ( M is the set of nodes
of the tree and E is the set of edges of the tree), where the root is the initial state
qo. Each node q E M in the tree corresponds to a collision-free state while each edge
qq' E E corresponds to a control input u E U that drives the robot from q to q' in a
given time, without colliding with any of the obstacles while satisfying the equation
of motion, Equation (4.1). Each node q E M is annotated with the cost of reaching
q from qo. The algorithms are presented in Algorithm 9 - Algorithm 12.
To construct the tree, a node is chosen for expansion and then a control input
is sampled uniformly at random. We present two different methods for selecting
which node to expand. The first method chooses the node for expansion uniformly
at random (Algorithm 11), the second method chooses the node for expansion in an
RRT style, Algorithm 12, (with a Voronoi bias, similar to Sparse-RRT[87).
Once a node to be expanded is selected, the planner samples a control input
uniformly at random, choose a time-interval At on how long the sampled control
input will be applied to the system, and use forward propagation to compute the
resulting state. As we will see in Section 4.3, the integration time At has to be
chosen wisely. In order to guarantee asymptotic optimality, the integration time has to
either be sampled uniformly from zero to a maximum integration time or to decrease
slowly and approach to zero as the number of iterations approaches to infinity. When
constant integration time is used the integration time has to be sufficiently small; In
the results section we present results for both cases (constant integration time and
79
uniformly sampled integration time).
To improve computational efficiency, we can prune nodes that have cost larger
than the cost of other nodes in their neighborhood (see Algorithm 8). Every time we
add a node, we search its neighborhood to find other nodes within a range R(t). R(t)
monotonically shrinks as the number of iterations increases and starts from a given
number e.g. Ro. If there is a node within a ball of radius R(t) with cost less than the
cost of the new node, then we do not add the new node otherwise we add it. On the
other hand, given that we add the new node, we remove all the other nodes that have
cost greater than the cost of the new node. We also make sure not to delete nodes
that are part of the best trajectory found so far.
There are many different variants of the above algorithms that may substantially
improve computational time. However, in this chapter, we focus on answering the
open question of whether a planner that samples the control space directly can converge to the optimal solution. To this end, we will analyze the simplest version of
this class of planners, i.e., the uniform variant (Algorithm 11). The idea is if we can
show that asymptotic convergence holds even for this simplest algorithm, then it is
more likely there will be a more efficient strategy to sample the control space such
that asymptotic optimality holds, and therefore spending more effort finding a better
strategy to directly sample the control space would be a worth while pursuit.
In addition to the analysis, we also present performance comparison between the
three different sampling strategies mentioned above, on various motion planning problems involving robots with complex dynamics.
4.2.2
Complexity
The pruning is the most expensive part of the proposed framework.
To compute
the neighbors within a range, the complexity is 0(log(N)) (where N is the number
of nodes in the tree). To do the cost comparison of each one of the neighbors, the
complexity is O(N) (N, is the number of neighbors, because of the pruning part
N, << N). To remove nodes from the tree, the complexity is O(N)) if we choose to
free the allocated memory or O(N) if we do not. Therefore the complexity of the
proposed algorithm is 0(log(N)) per iteration.
80
Algorithm 4 Planner(MethodTobeUsed)
1: Initialize:
Set
Initial configuration, Set Configuration Space, Obstacle Data Structure,
,
BestCost=+oo,
while (PlanningTime=TRUE) do
R(t)=Shrink(Ro,t)
if Simple-Uniform then
[Child, Parent, TrajFromParent, u}=ExpandTreeUniform(T);
end if
if Weighted then
[Child, Parent, TrajFromParent, u]=ExpandTreeWeighted(T);
end if
if ExpandTreeRRT then
[Child, Parent, TrajlromParent, uJ=ExpandTreeRRT'(T);
end if
if (CollisionFree(TrajFromParent) A PossiblyOptimal(Child) then
if (Prune(T,Child,R(t))=True) then
NodeCost=Cost(Parent)+Cost(TrajFromParent)
if (NodeCost<BestCost) then
NodeList=NodeList U Child
EdgeList=EdgeList U ParentChild
BestCost=NodeCost
BestNode=Child
end if
end if
end if
end while
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
Algorithm 5 ExpandTreeUniform(T)
1: Sample a node: q' <- rand()
2: Sample control input: ui +-rand()
3: Propagate: [qn,,, TraFrornParent]<
4: return qn,0 , q1, TraFromParent,ul
At g(q', u/)dt
Algorithm 6 ExpandTreeWeighted(T)
1: Compute the number of nodes in the neighborhood: Dn(q), Vq E M
2: Probability to choose a node: PR(q) oc 1/(Dn(q)), Vq E M
3: Sample a node: q' 4- rand(Pn(q))
4: Sample control input: ul <- rand()
5: Propagate: [qn,., TraFromParent| fe g(q', ui)dt
6: return qew, q1, TraFromParent,ui
Algorithm 7 ExpandTreeRRT'(T)
1:
2:
3:
4:
Sample a node at random: q' E Cr.ee
Find the nearest: q .. = FindNearest(T)
Sample control input: u/4- rand()
Propagate: [qn,., TraFromParentJ - At g(qn. u/)dt
5: return qneu, q.., TraFromParent,u
81
Algorithm 8 Prune(T, Child,R(t))
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
[Neighbors]=NeighborsWithinRange(Child,R(t))
for (i = 0;i <sizeof(Neighbors);i + +) do
CurrentNeighbor=Neighbors(i)
if CurrentNeighbor-+cost <Child-+cost then
return False
else
ToBeRemoved(CurrentNeighbor)
end if
end for
Remove(ToBeRemoved)
return True
4.3
Analysis
In this section we will provide a novel analysis on the optimality of planners that
directly sample the control space. As we show here, asymptotically optimal planning
for systems with differential constraints (e.g. non-holonomic systems) can be achieved
without the use of a steering function.
We start by showing that under certain
conditions, as the number of samples goes to infinity, the probability a planner, that
directly samples the control space, samples a sequence of controls that are "near" to
the optimal control function goes to one. Afterwards, we show that under certain
conditions, two nearby sequences of controls will -induce state space paths that have
similar costs. The optimality proof described in this section is proven for the case in
which we choose a node to expand uniformly at random and without doing pruning
that removes nodes from the tree (since our analysis considers only valid iterations,
e.g. the ones that resulted in nodes into the tree, it is applicable for the case we
do prunning that does not remove nodes from the tree). We believe that similar
properties hold for the other (RRT). In the case we have non-linear systems, as we
see from the results section, the convergence for an RRT-style algorithm is slower than
the uniform one. Given we know the properties of the underlining sampling strategy
(e.g. choose a node to expand and pruning characteristic), immediate results from
the analysis below could allow researchers to evaluate any sampling strategy.
82
4.3.1
Optimality
First, let's define the notion of "nearby" sequence of control inputs more formally as
Definition 1. Let u = (Uo, ul, ... , u1) and u' = (u',i,.
., u) be two sequences of
control inputs. Then, u and u' are E-close whenever maxiE[o,max(k,)
IHui
- U'l1
< E,
where ui = 0 for i > k and un' = 0 for i > 1.
Let q*(t) : [0, T] -+
reach the goal state.
U denote the optimal trajectory, where T, is the time to
Suppose the optimal trajectory is approximated with a step
function, which is represented as a sequence of control inputs u* = (U, u*,...U*)
where n
=
j, u' =q4*((i- 1) . At) for i E [1, n], and each control input in the
sequence is applied for At time. We can then state our assumptions as,
" The optimal trajectory 4*(t) exists within the control authority and the geometry we have, in addition the optimal trajectory is twice differentiable.
This
assumption inay or may not imply controllability (depending on the location of
the goal region).
" Equation (4.1) (the equation of motion) is Lipschitz continuous on both state
and control arguments.
" The cost function is Lipschitz continuous.
We also consider the standard 6-clearance assumption [21]. This assumption is
not essential for the convergence of the family of planners that sample the control
space to the optimal trajectory (e.g. 6 is arbitrarily small). If 6 is large then we
will be able to get admissible trajectories faster than the case 6 is small, however the
convergence to the optimal trajectory does not explicitly depend on 6.
Before going into the details of our analysis we would like to outline our proof.
Let 0* : [0, T] -+ U be the optimal trajectory and u* : [0, T] -+ U to be a piecewise
approximation of the optimal trajectory. 1 In addition, we consider the trajectory
u : [0, Tg] --+ U to be a trajectory returned by a planner that samples directly the
'for this analysis we are using piecewise constant functions (e.g. step functions) with constant
time intervals At; similar analysis can be derived for different functions and different choices of At.
83
control space. It is important to say that u is of the same form as u* (e.g. both of
them are piecewise constant functions). In addition, the norm we use later in this
chapter is the Li-Norm. The above trajectories are illustrated in the Figure (4-1).
We would like to prove that by sampling directly the control space the probability
U
-
u*t
U*t^
u(t)-
Figure 4-1: We can see the optimal control input (<D*(t)), a representation of the
optimal control input (u* (t)) and a control input that is close to the representation of
the optimal control input (u(t) e.g. a control an algorithm that samples the control
space returns).
to get a trajectory u that is c-close to any approximation of the optimal trajectory
approaches to 1 as the number of samples increases.
Before proving the asymptotic optimality property for the proposed family of
planners, we first need to relate the 6-clearance property (which is defined in the
state space) to properties on the control space such as the c-close property and the
discretization interval At. To this end, we relate the distance between two trajectories
(e.g. the optimal one and the one the proposed algorithm returns) with the distance
between their induced state space paths.
Any sampling based algorithm chooses
discrete control inputs (trajectory), however the optimal control input (trajectory) is
continuous. In our analysis we would like to take into account both distance due to
approximation errors and due to the c-close property.
Lemma 1. Suppose 0(t) : [0, T
--+ U and 0'(t) : [0, Tg] -+ U are two trajectoriesthat
84
start at time 0 and end at the same time T. Let -yo(t) and -y4 (t) be the state space
paths induced by each trajectory, based on the equation of motion, Equation (4.1).
If at each t E [0,T,, f is Lipschitz continuous in both state and control arguments,
then
( - 7y(t)I
74t)
Eu(eLpt - 1)
maximum Lipschitz constant and Eu
Eu(eP9 - 1), Vt E [0, T], where, L is the
=
maxtE[o,TJ 114'(t) -
#(t)II
Proof. We consider two control trajectories with the same time duration applied to
the equations of motion with the same initial conditions, and we would like to study
the distance of the resulting paths in the state space at each time. Direct application
of Lipschitz continuity on the equations of motion gives:
11AY1(t) -
4v(0)| =||f (70((t),O), 0 - A701t#(t),
)01
<; LPIyg(t) - yV(t)|I + LIk#(t) - #1(t)||
In the above equation
(4.3)
11.11 indicates the L, Norm. Because the equation of motion
is Lipschitz continuous, it is important to note that for every trajectory there is a
unique path at each time [122].
Let Eu = maxtE[o,T] |1'(t) - #(t)I and let Es(t) =
the L 1 norm we have
Ih'y(t)
- -yy(t)II. Then, for
Es(t) !5 |Ir(t) - -y(t)I. Direct substitution to Equation (4.3)
yields the differential inequality below that describes the evolution of the error on the
state space:
Es(t)
LEs(t) + LEU
(4.4)
Solving the above differential equation and applying the boundary condition we get:
Es(t)
Eu(eJO - 1) ! Eu(eLPT9 - 1),Vt E [0, TJ
(4.5)
0
Given the maximum distance of two trajectories in the control space (E-close
trajectories), the above Lemma shows a bound on the distance of the induced paths
on the state space.
85
Lemma 2. Let 0(t) : [0, T,] -+ U be a continuous-time trajectory with maximum
absolute value of the slope plus the remainder (the Peano form of the remainder) in
all dimensions of 4(t) equal to a.2 Suppose [0, Tg is discretized into uniform interval
At and u(t) : [0,TgJ
-+
U is a step function approximation of 4(t) where u(t) =
4( [kj At). If 74(t) is the state-spacepath induced by 0(t) and -yu(t) is the state-space
path induced by u(t), then Ih'4(t) - 7-(t)I maAt(eLpt - 1).
Proof. Using Taylor expansion series for 0(t) (in all dimensions of the control space)
we get:
II4(t)
-
u(t)Il
ZaAt = maAt,Vt E [0,T,]
(4.6)
Using results from Lemma (1) we get:
|1-y.(t) - -yu(t)II 5 maAt(eLPt - 1),Vt E [0,Tg
(4.7)
Notice that the results in the above lemma hold for both the case we use constant
integration time and the case we uniformly sample the integration time from an
interval (e.g. At = [0, r]). In the case, that the integration time is sampled uniformly
form an interval e.g. [0, r], r > 0, then At in the above equation is consider such
that 4(t) is approximated by piecewise constant functions each of them applied for
at most time At ( to cover a trajectory with time duration T it takes -2 expected
number of milestones).
Applying the above two lemmata to the optimal trajectory, to its representation
(approximation) and to a trajectory that is c-close to the representation of the optimal
trajectory, we can define the appropriate At for a given 6 and e to be:
II7+*.- 'ya|
(maAt + e)(eLU
-
1)
6
(4.8)
2we do not consider pathological cases where the derivative can be infinity at a few singular points
e.g. the tangent function at zero. From the Taylor theorem using the Peano form of the remainder
we have that f(t) = f(to) + (f'(to) + hi(t))At, hi(t) goes to zero faster than At
86
Using the above requirement for At, we can prove the following convergence theorem.
Theorem 1. Let u* denote the optimal sequence of control inputs when the time
domain is discretized uniformly into At intervals. Then, for any c > 0 and At that
satisfies Equation (4.8), as the number of samples goes to infinity, the probability the
proposed planer samples at least one sequence of control input u that has the same
number of elements as u* and is E-close to u*, goes to one.
Proof. Each path from the root to a node of the tree T encodes a particular sequence
of control input ( e.g jF, the edges of the tree are labeled with control input). The
probability the proposed algorithm selects a particular sequence to be extended is the
.
same as the probability it selects a node of T to be expanded, which is
1
Let pi be the probability the proposed algorithm samples a control input within
e distance from ul. Because the new planner samples control inputs uniformly at
random (from the control space) then, for all i E [0, Iu* I, p = p
>
V01(B)
Where
Vol(.) is the volume function. 3
Let Pk be the probability that from j valid samples (e.g. for up to j iterations),
the proposed algorithm generates at least one control sequence u that is c-close to at
least the first kth subsequence of u*, i.e, Jul = k and dist(uj, u?) <; c for i E [1, k]. As
we show in Appendix (A), this probability can be written as:
P,k
P_1,A -
(1
-
~1'k )P_1,kj or
(4.9)
)
Po-rk-9
P,A
Pl,k -+- (Pj_1,-1 - Pj.1,k)
(4.10)
The above equation holds for all j > kVk E [1,n]. For the case j = k,Vk E [1,nj we
have Pkk > L.
In addition for the base case (k = 1) we have:
Pj,1 > P-1,1 + -(4 - Pj-1,1),IVi > 1
(4.11)
Furthermore, induction on j and k would show that and Pik is monotonically increasing with respect to both j for a given k for
3
In this chapter we are using the Li-norm,
j > k and k > 1, of course with an
in the case the L2-norm is used we have, p =
87
VD
upper bound of 1. In addition, for any finite iteration P.1,k is smaller than Pp_1,A_
(one is subset of the other). Our goal is to show that Poo,k = Poo,k-1 = Poo,k-2 =
=
P00 ,1 = 1.
At first we will prove that Po,, 1 = 1. Let's focus on the inequality in Equation
(4.11) and rewrite it to the following:
,Vj > 1
P, 1 - P-1,1 ;> (1 - Pj- 1,1 )
(4.12)
We can then calculate the following summation
(Pi,1 - Pj_1,1)
>
j=2
j=2
P,,1 ~ P1,1
(L -
! j=2
E
311
-j)j1,
Taking p to the limit at oo gives us
im((P,,1
- P1, ) >1
(1 - A 2
)
(4.13)
-
JAA
j=2
Since limZ-oo
k,=+1 4 = oo, A 2 > 1.
However, since A 2 is an upper bound of a
probability value, A 2 < 1. Therefore A 2 must be 1, and is the least upper bound
of P_,1,1 (Equation (4.14) does not allow A2 < 1).
Since P_,1 is monotonically
increasing in j, it will eventually approach to A 2 = 1, thus Po,,
1
= 1. In the rest of
the proof we will show that Poo,k = Pm,k-l,Vk and thus Po,k = 1 for all k.
88
Let's now focus on the inequality in Equation (4.10) and rewrite it to the following:
Pj,k
P-1,k
-
> (Pj-1,k-1
- Pj-1,k) *,
.7
Vj > k
(4.15)
1,k
-
Pj-,k)
-l,k-
+
-
Pj-1,k)
-
We can then calculate the following summation
j=k+1
i=k+1
PIA,k -
Pk,k
E
j=k+1
P-1,kt-1~ P-1,k)
Taking p to the limit at oo gives us
lim (P,k - Pk,k)
IA--+0
lim.
IA-*00
E
(4.16)
((P-l,k-1 - Pj-1,k) -
=k+l
Now, we can set a lower bound (P- 1,k- 1 - P 1,k) > A', and rewrite Equation (4.16)
as
lim (PS,k - Pk,k)
A'1 lim
(4.17)
-
Using similar arguments we used for the base case (k = 1), we get A' = 0. Due to
monotonicity properties and because Xj-,1 ,t is a subset of Xj-1,k.1 (see Appendix A)
thus P.1,k <
P j -1,k-1
for all finite j > k then POO,k
=
POOk-1
=
... =
POO,1
1.
=
In the case we sample At e.g At = [0, r], identical analysis holds. As we show in
Appendix (A) we just have to replace p with p-,
Vp, r, At
>
0.
T
In the above analysis we use the following: X_1,k is a subset of X_1,k_1 thus
f_,k)
$
Pi-1,k < Pj-1,k-1 for all finite j > k. This is true because P(X-1,kfX-1
0, for all finite j (each of the nodes in the tree can be chosen for expansion with
probability ').
Now, the question is how far away the cost of u is from the optimal cost.
89
Lemma 3. Let 4*(t) : [0, Tg
U be the optimal trajectory and u* be the sequence of
--
control inputs that approximate 0*(t) with time intervals with time duration at most4
At, and u : [0, T] -+ U to be a trajectory that is c-close to 0*(t), where At is given
by Equation (4.8). Suppose -yt*(t) (t), u*(t) (t) and 'yu(t) (t) are the state space paths
induced by 0* (t), u* (t), and u(t) respectively. Then, jD(-y,. (t), t) - D(7y(t), t)II <
LDE(eLPt -
1) < LDE(eL.9 - 1), Vt E [0, T], where E = (e + maAt) and LD is the
maximum Lipschitz constant for the cost function.
Proof. We consider 3 trajectories and the resulting 3 paths, the first trajectory is
the optimal trajectory, the second trajectory is an approximation of the optimal
trajectory and the last one is a trajectory that is c-close to the approximation of
the optimal trajectory (that can be potentially sampled by planners that explore the
control space).
Using Lipschitz continuity on the cost function we get:
11 D(-yo.(t),7 t) - D(-yu(t),7 t)| 1!5 L (|170.( ) - -t W(1)1) (4.18)
Where
LD
is the maximum Lipschitz constant. From Lemma (1) and Lemma (2) we
have:
I|'7o.(t)
- -yu(t)|I
E(eLp t
-
1)
E(eLPTO - 1),Vt E [0,Tg1
(4.19)
Using Equation (4.18) and Equation (4.19) we get:
J|D(74 .(t), t) - D( 7-(t), t)I
LDE(e'
-
1)
LDE(eT
-
1),Vt E [0, Tg]
(4.20)
0
Theorem 2. The proposedfamily of algorithms, that sample directly the control space,
4
it assumes both constant integration time and sampled integration time.
90
return a trajectory that induces a path with cost that asymptotically approaches the
optimal cost. Let 0* : [0, Tg] -+ U be the optimal trajectory and u* be the sequence of
control inputs that approximates 0* with time intervals At, and ui : [0, Tg -+ U to be
a trajectory that is c-close to u* returned by the algorithm until iterationj, where At
is given by Equation (4.8). Suppose -y., -.. and -(, are the state space paths induced
by 0*, u*, and ui respectively. If D(7y,) is the cost of the path induced by trajectory
ui after sampling j samples in the control space then:
lim [P( lim ID( 7?) - D(y*)| 5 cd)} = 1
.- +.o
Where
ed
'O+
E R+
Proof. From Lemma (3) and for any At, f > 0 we get Ed
E = (E
LDE(eLPT9 - 1), where
+ maAt), then there are choices of c = E* and At = At* such that
Ed
is
arbitrarily small:
f* < LD(W* + maAt)eLPT9 - 1)
Where f* E R+ is arbitrarily small, e.g. we can always find c*, At* such that E*
(4.21)
< Ed.
From Theorem (1) we know that for any At, f > 0 the probability of sampling a
trajectory that is arbitrarily close to the optimal trajectory (for any At, c) approaches
to 1 as the number of samples increases.
0
It is important to see that in order to get convergence to the optimal cost we have
to sample At, otherwise (e.g. in the case a constant integration time is used) we can
only get convergence to approximate optimal trajectories (depending on At).
4.3.2
The Lyapunov Approach
Similar results to Theorem (1) can be obtained using the Lyapunov approach. We
consider the system in Equation (4.10) and Equation (4.11), which is in the discrete
domain and describes the probability to get at-least one trajectory with k milestones.
91
We multiply both sides of the above system times -1 and add 1 in both sides and we
get:
1 - P,1
1-Pj,k
We define
QAp
1
P-1,1
-
-
3
< 1 -P-1,A--
3
(1
P-1,1)Vj >1
-
Pj-(,P-1 i-Pj-,k),Vj>k,k>2
(4.22)
(4.23)
= 1 - P,k,Vk. In terms of Q,A the above System is written as follows:
Qj,1
-1,1 - !j-1,1,vj
Qj,Ag
Q _1,k -
3
(4.24)
> 1
- Qj- 1,_ 1),Vj > k, k > 2
(Q-,1,k
(4.25)
We transform this system to its equivalent in the continuous time domain and we
get:
Q(t),k
Where
1 =
-t(),1)Vt
Q(t),1
<
-(Q(t),k
6t
Q(t),k-1),Vt
-
tk = 6tk
(4.26)
E R+ is a time scaling constant that takes the iteration i to time domain t,
e.g. t = 6ti. The initial conditions of the above system are given as follows:
Q(t=t 1 ),1
=
(1
Q(t=t 2 ),2
=
(1 -
Q(t=tA,),k
=(
-
P)
p 2 /2)
Setting the "velocities" in the System in Equation (4.26) equal to zero we can compute
the equilibrium point (Qe,, ,Vk) as follows:
Qeq,k = Q-q,k-1 = ... Qeq, = 0
92
In order to show that the System in Equation (4.26) reaches to the equilibrium
point (e.g. Qeq,,
=
Qeq,k-i = ... Qeq,
= 0) we will use the Lyapunov approach. We
consider the following Lyapunov function:
k
(Q(t),,)
V(Q(t),1, Q(t),2---, Q(t),k) =
2
(4.27)
=1
Taking the time derivative of the above Lyapunov function we get:
k
(Qp(t),1 I (*2---, Q(t),k) = 2
(4.28)
(Q(t),j) (*),j)
Using the System in Equation (4.26), we get
,7
Q(t),k)
Z(Q(t),1, Q(t),2 ...
:5
t
Q(t),1)2 +-
(4.29)
(Q(t)',)(Q(t),i ~_Q(t),i-1)
It is important to see that the Lyapunov function is equal to zero at the equilibrium point and its derivative is always negative (except at the equilibrium point).
Therefore, the system in Equation (4.26) reaches its equilibrium value (e.g.
Q,,
=
Qeq,k-1 = ... Qeq,i = 0).
4.3.3
Convergence Rate
In this subsection we perform an analysis of the convergence rate for algorithms that
sample the control space directly (using uniform sampling and without pruning that
removes nodes from the tree). In the previous subsection we have shown that the
system in Equation (4.26) approaches its Equilibrium point (e.g. Qeq,, =
Qeq,k-I =
... Qq,, = 0) and therefore P,A; = 1,Vk. The question arises here is how fast the
system in question approaches to its equilibrium point.
93
We re-write the system in Equation (4.26) in the following form:
Q(t),k
+
Q(t),k-1,Vt >
PQ(t),k
tk
= Jtk
Q(t),k-2,Vt >.tk_1 = 6t(k - 1)
Q(t),k-1 - PQ(t),k-1
Q(t),2+
Q(t),2
PQ(t),l,Vt > t 2 = 6t(2)
+
Q(t),1
0,vt > tl = 5t
Q(t),1
At first we will solve for Q(t),1. Separating variables (t and Q(t),1) and integrating
both parts we get:
In (Q(t),1) ! in (t-) + In (C)
(4.30)
where C is the integration constant. To compute C we use the initial conditions
e.g. for t = ti, Q(t),, = (1 - p). Using the initial condition and after some algebraic
manipulation we get:
(4.31)
Q(t),1 5 (t~'P)(1 - p)ei
Similar results we get for the discrete system, see Appendix (B).
Now we will compute Q(t),2. To do so we multiply both sides of the differential
= ef- OP= and we get:
equation in question with p(t)
1
+ EQ(t),2t' 5 tpQ(t),1 P
For the left hand side of the above equation we get: Q(t), 2t" + PQ(t), 2t
(4.32)
=
dt
,2
)
10(t),2tp
Using results from the previous milestone we get:
d((tp)Q(t),2 ) < P(i - p)e,
-t
dt
94
(4.33)
Separating variables and using the initial conditions (e.g. Q(t2 ),2 = (1 Q(t),2 5 (tP)[ai ln(t) + a 2 ]
we get:
(4.34)
where a, = p(1 - p)tI, a 2 = (1 - ')tp - ai In(t 2 ).
Using similar techniques we can show that:
Q(t),3 5 (t~P)[b1 (ln(t)) 2 + b 2 ln(t) + b31
where b 1 = ',
b 2 = a 2p, b3 =(1-
(4.35)
bl(ln(t 3)) 2 - b2 ln(th).
)t-
For Q(t),4 we get:
Q(t),4 5 (t~P)[ci(ln(t))3 + c2 (ln(t))2 + c3 In(t) + C4]
(4.36)
wr
2, C3
= =pb3,
,C2 =c4= (1 - )t - c(ln(t4)) 3 -c 2(n4 2-c 3 ln(t4).
Now we can clearly see a pattern for the solution to the system in Equation (4.26).
For the general case we will use induction. We assume that:
Q(t),n-1 5 (t-P)[d(Il(t))n- 2
+ d2 (ln(t))n-3...dn- 2 ln(t)
+ dn-1
(4.37)
where di,Vi = [1,n - 1] are constants.
Using the n-th equation from the system in Equation (4.26) we have:
Q(t),n +
Q(t),n <
(t-P)[d(1n(t))n-2 + d2 (ln(t))n-3...dn- 2 In(t) + dn1
(4.38)
Multiplying both sides with tV we get:
O(t),nt'+
Q(),nt'
tP(t~P)[di(lin(t))n"2 + d2 (ln(t))n-3...dn-2 ln(t) + dn-1]
<
=
t
[d (ln(t))n- 2 + d2 (ln(t))n~ 3 ... dn- 2 Iln(t) + dn-1]
95
(4.39)
Integrating both parts we get
[d 1 (ln(t))n-2 + d2 (bn(t))n-3...dn-21n(t) + dn-]idt + C
Q(t),nt'
(4.40)
where C is an integration constant. Using integration tables we know that:
(441)
(ln(t))ndt _(1n(t))n+1
Using the above mathematical formula, we get:
Q(t),n 5 t-p[e1(n(t))n-1 + e2 (ln(t))"- 2 + e3 (ln(t))n- 3 ... en_ 1 (ln(t)) + en]
(4.42)
,Vi E [1, n - 1] and en is the constant C computed using
where the constants ei =
the initial conditions (e.g. Q(tn) = (1 - e)).
It is important to notice that leading term in Equation (4.42) is given by: e1 (ln(t))n-1twhere el = t 1
').
(n-1)!
Using the "L'Hopital's Rule" n - 1 times [30] we see that
it goes to zero (as expected) for any finite n. Another form of Equation (4.42) is as
follows:
Q(t), 5 t-I[e'
(ln(t))n-1
(n - 1)!
+e
(1n(t))n-2
2
(n - 2)!
(1n(t))n-3
+e3 (
(n
-
---
3)!
'- 1
(ln(t))
+en]
e
(4.43)
where e' are constants with respect to time, however they depend on n e.g.
e'
approaches to zero for large n.
The above inequality gives a bound on the probability "not to get at least one
trajectory with at least n milestones that is E-close to an approximation of the optimal
trajectory (with n milestones and sufficiently small At such that n =
[]J
)" for any
positive p, which is proportional to E. In the case, that the integration time is sampled
uniformly form an interval e.g. [0, r], r > 0, then the probability not to get at least
one trajectory that is E-close to any approximation of the optimal trajectory, with
n milestones (each of them resulted in after applying control input for at most At),
is given by a similar inequality (change p to pg).
96
However to cover an optimal
trajectory with time duration T it takes -Mexpected number of milestones.
4.3.4
Different Sampling Strategies
Both the optimality proof and the convergence analysis hold for planners that sample
directly the control space by choosing a node to expand uniformly at random, applying
control input at random and without using pruning (that removes nodes from the
tree). One can use artificial intelligence methods to guide sampling and thus improve
the convergence rate. For example, let the probability to choose the "correct" node for
expansion to be fi(1) > R (where fi : R+ -+ R+, a > 1), then to get a bound on the
convergence rate for a such algorithm we just replace p to p*a in Equation (4.43). One
way to do this is to use advance pruning techniques or artificial intelligence methods,
however when we use advance pruning techniques it is very difficult to compute the
convergence rate.
In addition, we see in the previous section the convergence rate depends directly
on p. We recall that p is the probability to choose a control input that is within a
ball of radius E centered at the optimal one. In the case we use uniform sampling
of the control space p is given by the ratio of the volume of that ball divided by
the volume of the control space. Therefore, if we use artificial intelligence methods to
guide the control sampling process then we can directly increase p and therefore effect
the convergence of the proposed approach. Studying and analyzing different sampling
strategies is not within the scope of this research; we recall that the goal of this work
is to show that sampling in the control space directly can achieve asymptotically
optimal planning.
4.4
4.4.1
Simulation Results
Case Study: Pendulum on a Cart
This section presents performance comparison between the simplest sampling strategy
we have analyzed in the previous section with more sophisticated strategies, i.e.,
97
uniform with pruning and expandTreeRRT with pruning, which is a simplified version
of Sparse-RRT[87]. For this comparison, we use the problem of "Pendulum on a Cart"
with obstacles (see Figure (4-2)).
F
x
*p.
Figure 4-2: Pendulum on a cart
This is a highly-non-linear 2d order under-actuated system with infinite number of equilibrium points both stable and unstable. The system's state space is 4dimensional, X E R 3 x
I, with X = [x, v, 0, Q]T, where x indicates the displacement
of the cart ( with mass M), v is the velocity of the cart (e.g. v = .), 0 indicates the
angle of the pendulum (with mass m, inertia I and length L), and Q is the angular
velocity of the pendulum (e.g. Q = 0 ).
Using the Euler-Lagrange equations we
can derive the equations of motion, the state-space form of the equation of motion is
given:
(t)
=
2
2
2
(I + mL )(F(t) + mLQ (t) sin(G(t))) + (mL) cOs(O(t)) sin(O(t))g
(t)
0(t)
v(t)
(M + m)(I + mL 2 ) - (mL) 2 cos 2 (O(t))
=
=
Q(t)
2
(-mLcos(O(t)))(F + mLQ (t)sin(O(t))) + (M + m)(-mgLsin(a(t)))
(M + m)(I + mL 2 ) - (mL) 2 cos 2 (O(t))
(4.44)
For this problem we would like to minimize control effort and time to the goal, thus
98
we define as a cost function the following:
D(F) = jTY(aF2 + at) dt.
where:
at = I7,
(4.45)
af = 1000"', M = 10kgm = 5kg,I = 10kgm 2 ,L = 2.5m
(see Figure (4-2)), g = 9.86T, the input to the system is a force F acting on the
cart in the x direction, for this case F is uniformly sampled e.g. F = [0, 300]N.
The initial conditions are all zero, the workspace is such that: x = [0, 60]m, and
includes obstacles as shown in Figure (4-3). The goal region is located in the right
side (48m < x < 52m) in the upright position ((180 - 10)0 < 0 < (180 + 10)0) with
-3.14rad/s < Q < 3.14rad/s and (-4m/s < v < 4m/s).
Figure 4-3: Pendulum on a cart: A typical close-to-optimal trajectory.
We implemented the algorithms in C++ in a Dell computer that runs Linux on
18 Intel Xeon(R) x86 - 64 processors at 2.9 GHz and 64GB RAM (we did not use
all the processors). Each one of the results presented here represents average results
over 21 runs. Figure (4-4) shows the cost of the trajectories generated by simpleuniform (the algorithm we used for analysis), simple-uniform with pruning, and expandTreeRRT with pruning. As expected, the expandTreeRRT method computes an
admissible trajectory faster than the uniform approaches. What is interesting is the
expand T reeRRT with pruning converges to a further lower cost (2 x 108) much slower
than simple-uniform with and without pruning. This happens because the optimal
solution occupies only a small region of the state space. To reach a near optimal
5
the goal here is not to stabilize the system in the upright position. After a method like the
one described here brings the system close to the upright position, one can use closed loop control
methods to stabilize it there.
99
solution fast, one needs to sample more densely near the region of optimal solution.
RRT's expansion strategy which tries to cover the entire state space well will actually
"under sample" state space region that are near optimal solution and "over sample" other regions that do not contribute in generating the optimal trajectory. Note
that this does not mean that the expansion strategy in simple-uniform is a suitable
one, rather the simulation results indicate that finding sampling strategy that biases
sampling towards regions near optimal trajectory would be a more fruitful avenue.
In addition to comparing sampling strategies, we also try to understand the effect
of different ways of setting time discretization. Figure (4-5) shows the statistics for
the case of constant integration time At = is and in the case we sample integration
time from [0, 3s}. In both cases the results were taken using simple-uniform with
pruning. When we use constant integration time (which is sufficiently small) we get
results similar to the ones we get when we sample the integration time, however the
results for constant integration time, for small number of iterations are slightly better
than the ones we get by sampling the integration time.
In the Figure (4-6) we can see the states evolution as a function of time for the
close-to-optimal path presented in Figure (4-3). For this case, the control input is
given in Figure (4-7). Notice that for this case the integration time is not constant.
4.4.2
Additional Simulation Results
We consider 3 cases. In the first case we consider a holonomic point robot (first order
integrator) moving in a 2-D configuration space without obstacles. The cost function
for this case is the distance traveled. This problem is very simple and therefore we
a priory know its optimal cost (65.7m). In Figure (4-8) we can see the tree and the
trajectory the algorithm returns after at certain numbers of iterations. In Figure (4-9)
we can see the average cost and the standard deviation for 50 runs of the algorithm. In
this case the results were taken using the "RRT" way to choose a node for expansion,
uniform sampling of the control space and pruning.
In the second case we consider a point robot moving with a minimum turning
radius (e.g. Dubin's car). In this case we also minimize distance traveled. In Figure
100
Carat
10
-x
Men
Mean+ a
-Mean- u
--
6
5
a4
3
2
0
1
2
3
6
5
4
x Id,
fterations
Const A,
- 108
Mean
-
6
Mean+ o
Mean -a
-
5.5
5
4.5
4-
0
o3.5-
0
2.5
0
1
2
3
5
4
Iteraions
6
X10
Const At
108
-
-
8
Mean
Mean+ a
Mean - o
76
0
4
3.
21
0
1
2
3
Iterations
4
5
6
x 10
Figure 4-4: Cart and pole simulation results. Upper: Simple-uniform without pruning. Middle: Simple-uniform with pruning. Bottom: ExpandTreeRRT with pruning.
The iterations shown are the valid iterations (do not count collisions), the actual
iterations are approximate 2 times more.
101
Const A,
_ 10
CIO'
~~.-....
San
n
A,
Mean
Men+
Mean
-Mean
5.5
Mean-
6
-Mean
+a
-a
5
Ag
5
4.5
i
40
3.5
3
3
25
0
,
1
*
2
3
Iterations
4
5
1.5 ..
6
x id'
0
1
2
ke
.
3
oraionS
.
4
.
2
6
5
x 10
Figure 4-5: Pendulum on a cart, statistics (results obtained by choosing a node to
expand uniformly and using pruning). Left hand side: the statistics for the case
constant integration time At = Is is used. Right hand side: the case the integration
time is uniformly sampled from [0, 3s] is used. The iterations shown are the valid
iterations (do not count collisions), the actual iterations are approximate 2 times
more.
(4-10) we can see two trajectories the algorithm returns at 2 different numbers of
iterations and in Figure (4-11) the average cost and the standard deviation for 50
runs of the algorithm. In this case, the results were taken using the "RRT" way to
choose a node for expansion, uniform sampling of the control space and pruning.
In the last case, we study an aerial vehicle that moves with a minimum turning
radius and constant elevation per unit time. In this case we use the PQP collision
library for the collision checking part and the "weighted" way to choose a node for
expansion without pruning. Preliminary results can be found in Figure (4-12).
4.5
Summary and Discussion
Currently, researchers in the optimal path planning community have devoted significant amount of time to solve the problem of optimal path planning for systems with
differential constraints using methods that directly explore the configuration space.
These methods sample the configuration space and rely on steering functions to connect the configuration space with the control space. However, for general non-linear
102
Pendulum: x
Pendulum: v
An.
1g.
50
10
40
x
-
20[
go 5
.I
-
*
I
E 30
.-''
I
a
0
10
0
2
4
6
lime
8
10
0
12
2
6
Time s
4
s
8
10
12
10
12
Pendulum: 9
Pendulum: 0
1
0
0
-5
0
U
0
c -2
-10
.. %I .'
'*
-3
-15
-4
I
2
4
6
ime s
8
10
12
2
4
6
Time s
Figure 4-6: Close-to-optimal path as a function of time.
103
8
Pendulum: Control Input
150
100
50Z
0
-50
-100
-150
0
2
4
6
8
10
12
Time s
Figure 4-7: Close-to-optimal trajectory, in this case the integration time is uniformly
sampled from [0, 3]sec.
systems, a steering function is not always available. In addition, rigorously speaking,
even if in the case of systems with linear and fully-controllable dynamics, asymptotically optimal path planning using steering functions cannot be achieved because
most of the steering functions (if there exists one) presents asymptotic behaviors
which means they cannot achieve exact connection on the explored samples. We believe that a better way to approach the optimal path-planning problem for systems
with differential constraints is to use methods that directly sample the control space.
In this chapter we have illustrated that asymptotically optimal motion-planning
for agents with differential constraints can be achieved without the use of a steering
function. Going back to first principles, we present a novel analysis on asymptotic
optimality of a family of algorithms that directly sample the control space. Furthermore, by modifying existing planners, we proposed a new family of asymptotically
optimal planners. As the number of iterations increases, the control input these algorithms sample approaches the optimal control input. An important part of these
algorithms is the choice of the "integration time" (At). In order to guarantee asymptotic optimality and completeness, we need to either sample it or to decrease. From
our experience, sampling the integration time presents better results. By sampling
104
Z/
'A ap"5-
71
I?
XG,
'0"
74or
/A
v
A'anAre
SO
ot'a
r-v
iterations, Cost= 66.93
10000 iterations, Cost =74.77
100000
1000000 iterations, Cost=66.3
10000000 iterations, Cost=65.9
Figure 4-8: Holonomic agent in a space without obstacles: We can see the optimal
tree and the optimal trajectory at certain numbers of iterations. The green area in
the upper left part of each figure indicates the goal region.
105
Holonomic Vehicle
110
-Optimal
Cost
-Mean
-Mean+
o
Mean - o
100-
900
0
0 80-
70
Rfl
.1
I
103
p
10
10
i0
Iterations
Figure 4-9: Holonomic agent in a space without obstacles: the statistics for 50 runs
in logarithmic scale.
(a) Cost 103 m, 1000000 iterations
(b) Cost 96.96 m, 5000000 iterations
Figure 4-10: The second case: two trajectories the algorithm returns for certain
number of iterations and the statistics.
106
Dubins Car
145-
-
140 --
Mean+o
-Mean
-a
Mean
-
135
130E 125- 1200
0 115110105100950
0.5
1
1.5
2
2.5
Iterations
x 106
Figure 4-11: The second case: the statistics.
the integration time, we can achieve both long and short actions and we rely on the
pruning part to prune short actions during first iterations of the algorithm and prune
long actions during the last iterations of the algorithm (fine actions give finer results).
107
v~w
,IN
Figure 4-12: Dubin's airplane: Different views of a close-to-optimal trajectory (preliminary results).
108
Chapter 5
Asymptotically Optimal Inspection
Planning for Systems with
Differential Constraints
In this chapter we investigate the inspection planning problem. Building on Chapter
(4), in this chapter we propose a new inspection planning algorithm, called Random
Inspection Tree Algorithm (RITA). Given a perfect model of a structure, sensor specifications, robot dynamics, and the initial configuration of a robot, RITA computes
the optimal inspection trajectory that observes the entire surface of the structure.
RITA computes both observation points and the trajectory to visit the observation
points simultaneously. It uses sampling-based techniques to find admissible trajectories with decreasing cost. Building on the optimality proof of the previous chapter,
we show that RITA is an asymptotically optimal inspection planner thus, as the number of iterations increases, the control input the algorithm samples approaches to the
optimal control input. To illustrate our theory and algorithms, we present a set of
simulation results.
109
5.1
Problem Definition
In the previous chapter we worked on the path/motion planning problem for systems
with differential constraints. For the above formulation (of the path/motion planning problem in the previous chapter) special care is taken to model and analyze the
path/motion planning problem in the control space, thus we search for a trajectory,
as opposed to a path in the configuration or state space, that optimizes an objective function with respect to some variables in the presence of constraints on those
variables. The above constraints can be either hard constraints or soft constraints.
The definition of the path/motion planning problem introduced in the previous
chapter is general and as such it can be used to address both simple motion planning
problems and challenging task planning problems, therefore, the problem definition
of the inspection planning problem is similar to the problem definition of the simple
path/motion planning problem, however the definition of the admissible trajectory
is different in each problem. Here we will formally define the inspection planning
problem repeating certain parts from the previous chapter for completeness.
Let us first discuss the structures to be inspected and the workspace. The workspace
contains the structure to be inspected and obstacles. Obstacles will not be inspected,
however they effect the complexity of the problem by changing the properties of the
inspection problem and the free space as defined below. We model each structure
and obstacle as a simple polygon for 2D workspaces and as simple polyhedra for 3D
workspaces.
To inspect the structures, we use a robot with first-order differential constraints
equipped with a visibility sensor. The proposed approach can be used for both discrete
and continuous sensing actions. Suppose the set of all possible robot states is S and
the set of all possible control inputs is U. Then the equation of motion is given by:
(t) = f(07(t),(t),t)
(5.1)
Where for each time t, -y(t) E S is a state, 4(t) is the time derivative of the state
and and 0(t) E U is a control input (also called trajectory). Assuming that S and U
110
are manifolds of dimensions n and m where m < n, using appropriate charts we can
treat S as a subset of Rn and U as a subset of R'. Furthermore we assume that the
f is a Lipschitz-continuous function with respect to state and control with maximum
Lipschitz constant to be less than L E R+.
Supposed Sf.e c S is the collision free subset of the state space. Then we define
path to be a time parametrized mapping from time to the obstacle free state space
induced by control through Equation (5.1) e.g. 7 : [0, TJ -+ Sf... We also define
trajectory or control function to be a time parametrized mapping from time to the
control space e.g. q : [0, T] -+ U. From Lipschitz-continuity it can be shown that for
each trajectory (control function) there is a unique path that represents the solution
to equation of motion [122].
Suppose STR is the union of all points that lie on the boundary of the structures
in the workspace. We would like to generate the control function (trajectory) 0* and
the resulting path 7* from a given initial state ( y(t = 0)), such that when the robot
follows 7*, it can see (covers) all points in STR from at least one point in y* and
minimizes a given cost function. More formally, let D(4) be the resulting cost for
control function 0 and <D to be the set all admissible trajectories. In this chapter we
define an admissible trajectory to be a trajectory that induces an obstacle free statespace path (through Equation (5.1)) that starts from a given initial state ( -Y(t = 0))
and covers STR, in the sense that UtE o[, VIS(7(O)) = STR (where VIS(.) is the
visibility model). Among all admissible trajectories we would like to compute the one
that minimizes the cost function, e.g
q* = arg min D()
(5.2)
and the resulting path 7*. The proposed framework is flexible enough to use any
cost function that is Lipschitz continuous and spans any subset of the control space,
visibility space, and state space. In this chapter we use as a cost function the distance
travelled.
111
5.2
Proposed Algorithms
Contrary to many inspection planning methods, RITA does not separate the inspection planning problem into the art gallery and the TSP problems. Instead, it
approximates the optimal inspection trajectory 0* and the resulting path -Y* (or -YO.)
by addressing the control and visibility aspects of the in inspection planning problem
simultaneously. RITA approximates the optimal trajectory (if one exists) incrementally. It uses sampling-based motion planning to search the control space U to first
find an admissible trajectory, and then improve it subsequently by continuing to
search for shorter admissible trajectories.
RITA constructs a tree T = {M, E}, where the root of the tree is the initial state
(or configuration) qO. Each node q E M in the tree corresponds to a collision-free
state while each edge qq' E E corresponds to a control input u E U that drives the
robot from q to q' in a given time, without colliding with any of the structures and
obstacles while satisfying the equation of motion, Equation (5.1). Each node q E M
is annotated with the cost of reaching q from qO, and with a set of visible points, i.e.,
the set of points STR' C STR that the robot sees if it moves according to the path
from qo to q in T. The proposed framework is flexible enough to use both continuous
and discrete (e.g on the nodes or in few points within the nodes) scanning actions;
results in this work are taken using discrete scanning actions. A path from qO to a
node q in T is an admissible trajectory whenever q is annotated with STR as its set
of visible points.
To construct the tree, any sampling-based motion planning algorithm [21] can
be used. In this work, we use a similar planner to the one in
[59],
as it samples
the control space directly, and hence does not require inverse-dynamic computation,
which may be difficult to compute for systems with complex dynamics. We developed
2 different versions of RITA, the first is the weighted-RITA that uses as weights the
inverse density of the nodes to bias the search in less explored areas. The second
one is the uniform-RITA that uses uniform sampling. We also proposed (but do
not implement) the use of a planner that chooses a node to expand in an RRT style
112
(RRT'-RITA) and applies control input at random (see ExpandTreeRRT'()). In order
to guarantee asymptotic optimality (as we see in the previous chapter) the integration
time At has to be either sampled or slowly decrease -and approach to zero as the
number of iterations approach to infinity- or to be sufficiently small.
Algorithm 9 Path Planning for inspection (MethodToBeUsed)
1: Initialize: Set Initial configuration, Set Structure and Obstacle data structure, BestCost=+oo
2: while (PlanningTime=TRUE) do
3:
if Biased Mode then
4:
[Child, Parent, TrajFromParent, u]=ExpandTreeWeighted(T);
5:
else
6:
[Child, Parent, TrajFromParent, u]=ExpandTreeUniform(T);
7:
else
8:
[Child, Parent, TrajFromParent, u]=ExpandTreeRRT'(T);
9:
end if
10:
if (CollisionFree(TrajFromParent) A PossiblyOptimal(Child) then
11:
NodeCost=Cost(Parent)+Cost(TrajFromParent)
12:
if (NodeCost<BestCost) then
13:
NodeList=NodeList U Child
14:
EdgeList=EdgeList U ParentChild
15:
NewVisibility=Visib(Child)
16:
Child.GlobalVisib=Parent.GlobalVisibuNewVisibility
17:
Child.Unseen=S \ Child.GlobalVisib
18:
if (Child.Unseen=0) then
19:
BestCost=NodeCost
20:
BestNode=Child
21:
end if
22:
end if
23:
end if
24: end while
Algorithm 10 ExpandTreeWeighted(T)
1:
2:
3:
4:
5:
Compute the number of nodes in the neighborhood: D.(q), Vq E M
Probability to choose a node: P.(q) oc 1/(D.(q)), Vq E M
Sample a node: q' +- rand(PR(q))
Sample control input: u +-rand()
Propagate: [q,.,, TraFromParent]+- f g(q',ul)dt
6: return qn, q1, TraFromParent,u
Algorithm 11 ExpandTreeUniform(T)
1:
2:
3:
4:
Sample a node: q' +- rand()
Sample control input: u/ +- rand()
Propagate: [qi,,., TraFromParent]+-ff g(q', u)dt
return qnew, q1, TraFromParent,u/
113
Algorithm 12 ExpandTreeRRT'(T)
1: Sample a node at random: q' E Ciree
2: Find the nearest: q.r = FindNearest(T)
3: Sample control input: u +- rand()
4: Propagate: [q,,,TraFromParent - ft g(q..r, 2)dt
5: return qn,., q..r, TraFromParent,W
The tree T is constructed incrementally (Algorithm 9). At each iteration of the
algorithm, we try to expand the tree using the "ExpandTreeWeighted(" (or "ExpandTreeUniform(" ), which expands the tree by randomly choosing a node to expand and a control input to apply. In the case the "ExpandTreeWeighted(" is used,
the node to expand q' is chosen with probability proportional to the inverse of the
number of nodes within its neighborhood (a small ball with q' as its center, Fig. (51)). ExpandTreeWeighted() (or ExpandTreeUniform() ) function takes as its input
the current Tree and returns a candidate new node qne., the new node's parent q',
the node's trajectory from q' to qe,, and the control input used. If q,. and the
trajectory from q' to qn,, are obstacle free, then we compute the cost to reach qn,.
through q'. The cost for the best admissible solution found up to the current time is
recorded and used to reject node candidates if they result in trajectories longer than
the best one found up to that point. If the new node qe,, is not rejected, then RITA
inserts qs,. as a child of q' in T, annotates the edge qneq with u, and computes the
set of points STR' C STR that is visible when the robot moves from q' to qne. (in
the case we use continuous scanning actions) or it computes the visibility of qn, (in
the case we use discrete scanning actions, for this work we used discrete scanning
actions).
To speed up the search for the optimal trajectory, RITA rejects nodes that could
not be part of the optimal trajectory (PossiblyOptimal function in line-10 of Algorithm 9). RITA excludes controls that drive the search to areas in which there is
no probability that parts of the optimal trajectory will be present. To do so, RITA
defines the Minkowski sum of the convex hull of the structure of interest and the ob-
stacles within the workspace with a sphere radius (R,8 n,
+ Rturnng), where Rm,,
denotes the maximum range of the sensor and Rta,-ing is the minimum turning radius
114
1/8
n2n
n.
2/8
n
2/8
2/8
Figure 5-1: For the biased-RITA, each node to be expanded is chosen with probability
proportional to the inverse of the density of the nodes within its neighborhood, here
the first 3 nodes would be sampled with probability 2/8 and the last 2 with probability
1/8.
of the agent (derived from dynamics).
The optimal trajectory must lie within the
Minkowski sum defined above. Therefore, if a control u drives the agent to node q,
whose projection in the workspace is located outside of the Minkowski sum described
above, then q is rejected. In addition, we use the idea of macro actions [94], each
time we choose a node for expansion we apply the same control input for a predefined
number of times thus, we add more than one node per iteration. In this way we get
both actions that cover small and large distances in the configuration space.
5.3
Analysis
In this section we will show that uniform-RITA asymptotically approaches the optimal inspection trajectory. Our analysis uses results from the previous chapter. We
also perform an analysis on the completeness of the proposed framework that takes
into account properties of the visibility space and properties of the free space this
completeness analysis. In addition, in Papadopoulos et al. [102] we show the relation
of the proposed framework to RRG algorithms.
115
5.3.1
Optimality of RITA
Here we show that uniform-RITA samples trajectories that are sufficiently close to
any approximation of the optimal inspection trajectory and thus sufficiently close to
the optimal one (asymptotic optimality). Results from this section are based on the
results proven on the previous chapter. We use similar notations and assumptions
from Chapter (4) and we will repeat them here for completeness. Apart from the
assumptions introduced in the previous chapter, here we introduce an additional
assumption that takes into account the visibility model used and the geometry of the
state space.
Let 4* : [0, Tg] -+ U denote the optimal inspection trajectory and -to. : [0, TgJ
-+
Shee to be the induced optimal path. Suppose the optimal inspection trajectory
is approximated with a set of step functions1 , which is represented as a sequence of
control inputs u* = (u4, u*, . .. , u*), where n =
[TJ,
u = 4((i-1) -At) for i E [1, n],
and each control input in the sequence is applied for At time and yu : [0, T,] - S is
the induced approximate optimal path. We also consider the trajectory the proposed
inspection algorithms return, u : [0, T] -+ U, to be an c-close to u* trajectory, that
is of the same form (e.g. piecewise constant) and of the same time duration of u* and
-Yu: [0, T,] -+ S to be the induced path. We can then state our assumptions as,
" The optimal trajectory 0* (t) is differentiable twice.
" We also assume the standard 6-clearance assumption [671. This assumption
is not essential for the convergence of the family of planners that sample the
control space to the optimal trajectory (e.g. 6 is arbitrarily small). If 6 is large
then we will be able to get admissible trajectories faster than the case 6 is small,
however from our analysis does not follow that the convergence to the optimal
trajectory depends on 6.
" Equation (5.1) (the equation of motion) is Lipschitz continuous on both state
and control arguments.
'the proposed framework is flexible to use any base function, e.g. linear, cosine functions etc.
116
*
The cost function is Lipschitz continuous.
* The optimal trajectory 0* induces an optimal path -yo., -y
: [0, T,] -+ Svi",
that is A-elastic, where A E R+. Supposed BA(70.) = UtET 9IBA(7+*(t)),
where BA(7. (t)) is the ball of radius A centered at -yo. (t). Then, -y. is called
A-elastic when any path yo : [0, T] -* Sfir
that lies entirely in BA (-y-) n Sir,
covers the entire STR.
Similarly to the previous chapter, where we related the 6-clearance (which is a
property of the state space or configuration space) to the control space, here we would
like to relate the 6-clearance and the A-elasticity to the control space. Let,
A= min(6, A) E R+
(5.3)
Then, similarly to Equation (4.8), we get
I17*
- -yu| 5 (miAt + E)(e4lTg - 1) < A
Theorem 3. Let trajectories *,u*, u and the induced trajectories
(5.4)
-y,' be as
defined above, then for any At > 0 and any E > 0 respecting Equation (5.4), uniformRITA returns a trajectory with cost that is sufficiently close to any approximation of
the optimal trajectory and thus sufficiently close to the optimal trajectory.
Proof. The proof follows from Theorem (2).
5.3.2
E
Completeness of RITA
The probabilistic completeness property is a weaker property compare to the optimality property, for example given an algorithm convergences to the optimal inspection
trajectory (in probability) then it is also a probabilistic complete algorithm. Because
uniform-RITA is proven to be an optimal algorithm then it is also a probabilistic
complete algorithm. We recall that the A-elasticity property encapsulates joint properties of the visibility model used and the geometry of the free space, however it is
117
not easy to exactly understand the effect of each factor separately. We have already
established the probabilistic completeness of unifrom-RITA, however to get a better
understanding of the effect of the visibility model and the free space we proposed
another analysis on the probabilistic completeness. We simplify our analysis and we
introduce a simplified version of RITA, called ideal-RITA. Ideal-RITA is the same as
RITA, but it replaces the sampling strategy in Algorithm 10 with Ideal-Expand, as
described in Algorithm 13. The notation RI(q) is the set of configurations reachable
in I units of time from q,
[59. The completeness analysis below is different than
the one in Englot and Hover [38] since each of them refer to a different algorithm.
In addition, our analysis uses properties of the "reachable space" and thus takes into
account differential constraints.
Algorithm 13 Ideal-Expand(T = (M, E))
1: Sample a milestone qe,
uniformly at random from R(M).
2: Sample a milestone q1 that can reach ml.
3: Compute a control input: u/ to move from q/ to qe.
4: return q, TraFromParent, ul
Probabilistic completeness in this case means that if an admissible trajectory, i.e.,
one that covers all points in STR, exists, then given enough time, ideal-RITA will
find it with high probability. This notion of completeness is slightly different from
that which is commonly used in motion planning, i.e. covering the entire Cf,... or
Sfee. To show the probabilistic completeness of ideal-RITA, we first need to define
the following property of the inspection planning problem.
Definition 2. Let STR be the set of all points on the boundary of the structure to
be inspected.
We define a v-partitioning of STR as the set S =
{S1, S2,...
,
Sn,
where n E N+ and for each i E [1, n], Si C STR and there exists a non-empty and
connected set A(Si) C Sp,, whose volume is Vol(A(Si)) ;> v -Vol(SSe) and each state
(or configuration) in A(S) can see all points in S. An inspection planningproblem is
(k, v)-inspectionProblemwhenever the smallest v-partitioningof STR has k elements.
Figure (5-2) illustrates the above definition. Intuitively, the problem is easy to
118
S~S
S
Sk
S2
/
A1
'
I'
I
A2
Ak
Figure 5-2: A cartoon illustration of v-partitioning.
solve when k is small and v is large. To analyze the probabilistic completeness of
RITA, we need Sfee to be (a, /3)-expansive [59]. The (a, /3)-expansive property for
robots with differential constraints have been defined by Hsu et al.
[59]; we rewrite
this definition here for completeness.
Definition 3. Let a, O E (0,1] be constants, q E Sf,
1
R(q) C Sf,
be the set of
states (or configurations) reachable from q, and Rj(q) C R(q) be the set of states (or
configurations) reachable from q within 1 time-steps. Then, for any q E Sfr,, R(q)
is (a, /3)-expansive if for every connected subset C' c R(q), Vol(/3-lookout(C')) >
a - Vol(C'), where 3-lookout(C') = {q E C'
I Vol(RI(q)\C')
> 3 - Vol(R(C')\C')}.
The set Sfe is (a, /3)-expansive if for every q E S5.e, R(q) is (a, ,3)-expansive.
Using the above definitions and the results by Hsu et al.
[59]2, we can compute
a lower bound on the number of sampled milestones, such that the structure STR is
covered with high probability. More precisely, we show that:
Theorem 4. Given a (k, v)-inspectionProblem and an (a, /)-expansive collision-free
space as defined in [59], a sequence M of n milestones generated by the ideal-RITA
2
David Hsu et al. [59] defined important properties in the configuration space as opposed to our
work that we define similar properties in the state space.
119
completely covers the structure STR with probability at least 1 - 2kf (13, v)e~7MU."
where f (#, v) = In(2/v) and r = Ln/kj.
Proof. Let L be the event that STR C VIS(M) and let L' be the event that M
contains at least one configuration that lies in each A(Sj) for i E [1, k]. Based on the
definition of (k, v)-inspectionProblem, it is clear that L' C L and hence P(L)
P(L').
Suppose M is divided into k subsequences where each subsequence contains r
consecutive milestones, and L' is the event that the ith subsequence contains a configuration in A(S). Then,
P(L' n L's n ... n V'g)
P(VL)
=
1 - P(L1 U L'2 U ... U L')
> 1 -
P(Log
(5.5)
t=1
Using the results in [59], P(L) <; 2f(#8, v)e-~-u7 and we can rewrite Equation (5.5)
as P(L')
1- 2kf(#8, v)e~fU_7. Since P(L)
P(L'), we get the results we want.
[
Notice from Theorem (4) that with the same number of milestones, ideal-RITA
can cover STR with higher probability when we have smaller k and larger v, which
fits our intuition.
5.4
Results
We implemented uniform-RITA and weighted-RITA in C++ and run a set of simulations (the code is not optimized). Here we present a set of simulation results. At
first we present simulations results from 2D workspaces and then we present simulation results from 3D workspaces. We also provide a set of simulation results using a
TSP-art-gallery approach.
120
5.4.1
2D Workspace Results
We consider a 2D workspace and the following robot's dynamics.
x = u, sin 0
(5.6)
= U, cos 0
The configuration space (or the state space) is 3D and the control space is 2D. The
sensor model is such that it observes everything within a maximum radius R,.g that
is not occluded by the structure. The visibility for a given trajectory is computed
by projecting the footprint of the sensor on the workspace and using linear algebra
methods to identify parts that are occluded by the structure and the obstacles in the
workspace.
We consider 2 different case studies in both case studies the integration time At is
uniformly sampled from [0, 2]seconds. In the first case, the range of the sensor is 20
units, u, is the speed of the agent with values between 0.5 and 3 units per second, and
Q is the angular velocity with values between -0.25 and 0.25 rad/s. These parameters
result in a minimum turning radius of 2 units. The initial configuration for the first
case study is qO
=
[0,0, 0 ]T.
The second case is more challenging with a small visibility range and several
narrow passages. Because the maximum range of the sensor is small compared to the
environment, the agent has to navigate inside several of the narrow passages. The
range of the sensor is 15 units, u, is the speed of the agent with values between 0.5
and 4 units per second, and Q is the angular velocity with values between -0.25 and
0.25 rad/s. These parameters result in a minimum turning radius of 2 units. The
initial configuration for the second case study is qo = [45,0,7r/21T. For both case
studies we also consider additional obstacles that are located close to the structure
of interest.
We also present mode-carol runs for each on of the cases to get running time
and performance statistics. The computer used to generate the results runs on a
121
3GHz processor and Ubuntu 10.04 as the Operating System. It is important to note
that our 2D implementation is not optimized and thus running times given below
only represent and characterize our implementations.
On the other hand our 3D
implementation is better than the 2D implementation and the running times given
for the 3D results could be used to understand the actual running time for RITA.
First Case Study
On average, weighed-RITA finds the first admissible solution within 0.24 seconds
with standard deviation 0.34 seconds.
It generates a solution close to the optimal
(below 65 units) after 11 minutes, with standard deviation 4.6 minutes.
Fig. (5-
3, lower) shows the average cost for monte-carlo runs as a function of the actual
running time. Similarly, uniform-RITA finds the first admissible solution within 0.23
seconds with standard deviation 0.24 seconds. It generates a solution close to the
optimal (below 65 units) after 10 minutes, with standard deviation 12 minutes. Fig.
(5-3, upper) shows the average cost for monte-carlo runs as a function of the actual
running time. Fig. (5-4) shows the generated admissible trajectory across different
time of one of the simulation runs. In this particular run, the algorithm finds the first
admissible trajectory (cost 120.6 units) after running for 0.7 seconds. It is important
to note that the first admissible solution found does not belong to the same homotopy
class as the optimal trajectory. We keep running the algorithm, and at 71.5 seconds
it switches to a different homotopy class and finds another admissible trajectory
with cost 114.9 units. The algorithm improves the current solution within the same
homotopy class (at time 117.3 seconds and cost 100.85 units). At time 171.4 seconds
it finds another admissible trajectory with cost 92.5 units. After 400.3 seconds, it
finds a better trajectory (this particular one of cost 68.7 units), which belongs to
the same homotopy class as the optimal trajectory, and eventually this trajectory
converges to a near-optimal one (63.8 units after 951.1 seconds).
Fig. (5-5, above left) shows a comparison between the trajectory that the algorithm converges to ( here we show another run; at this particular run RITA converges
to a trajectory with cost 64.82 units after 74.73 seconds ) and the optimal holonomic
122
Weighted-RITA: LOG-LOG Mean cost
coil
Mean cost
Weighted-RITA: Won
150
21
120
C)
cost
215
130
0
LOG-LOG Mean
22
140
V
Weighted-RITA:
225
I
110
2.05
0
0
100
2
1.951
80
1.91
70
1.85
0
100
200
300
500
Time Sec
400
&0) 700
I1
1 0
800
0.5
1
1.5
2
25
3
Time Sec
Uniform-RITA: LOG-LOG Mean cost
Uniform: Mean cost value
150.
1.92
140
1.9
130
120
1.88
g 110
0
0 1.86
0
o
10090-
1.84
80.
1.82
70B60.
0
100
200
300
0.5
400
Time Sec
1
1.5
Time Sec
2
2.5
Figure 5-3: First case study: we can see the average cost as a function of the running
time in linear scale (left part) and in log-log scale (right part). In the upper part of
the figure we can see results generated using Weighted-RITA, and in the lower part
of the figure we can see results generated using Uniform-RITA.
123
4
40-
40-
K.
35-
30
25-
2520-
/
20
/
30E
-
5
0
0
10
t
20
30
4
0
o3
13
seconds
=0.7
30
40
t = 71.5 seconds
45
40-
40
3530
25
(
25-
20
13
10:i
0
E N
1C
10
30
30
40
0
3
10
20
20
40
50
t = 171.4x seconds
t = 117.3' seconds
40
40
N
35-U
20
10 -
10-
-
25
20.
/
30
25-
10
-
=
t = 400.3 seconds
10
20
30scd
t = 951.1 seconds
Figure 5-4: First case study, one run: The algorithm finds the first admissible trajectory (cost 120.6 units) after 0.7 seconds. The first admissible solution found does
not belong to the same homotopy class as the optimal trajectory. We keep running
the algorithm, and at 71.5 seconds it switches to a different homotopy class and
finds another admissible trajectory with cost 114.9 units. The algorithm improves
the current solution within the same homotopy class (at time 117.3 seconds and cost
100.85 units). At time 171.4 seconds it finds another admissible trajectory with cost
92.5 units. After 400.3 seconds, it finds a better trajectory (this particular one of
cost 68.7 units), which belongs to the same homotopy class as the optimal trajectory,
and eventually this trajectory converges to a near-optimal one (63.8 units after 951.1
seconds).
124
trajectory (59 units). The results indicate that the length of the trajectory RITA
converges to, is close to the lower bound (optimal holonomic trajectory).
The proposed algorithm can also handle additional obstacles in the workspace.
In Fig. (5-5, upper right and below) we show the results RITA gives if we include
obstacles close to the structure of interest. In this particular run, the cost decreases
from 124.4 to 73.3 units for 0.07 seconds to ihour running time.
Second Case Study
On average, weighted-RITA finds the first admissible solution within 12.87 seconds
(with standard deviation 25.4 seconds) and gets a solution close to the optimal (below
185 units) after 44 minutes respectively (with standard deviation 85 minutes). As
Figure (5-6) shows, by running it longer, the length of the generated admissible
trajectory becomes shorter. Fig. (5-6, upper) shows the average cost for Monte-Carlo
runs as a function of the actual running time. Similarly, on average, uniform-RITA
finds the first admissible solution within 375 seconds (with standard deviation 3.68
seconds) and gets a solution close to the optimal (below 185 units) after 30 minutes
respectively (with standard deviation 50 minutes). Fig. (5-6, lower) shows the average
cost for Monte-Carlo runs a function of the actual running time.
Fig. (5-7,a-d) shows the generated admissible trajectory across different time
of one of the simulation runs. In this run, the algorithm finds the first admissible
solution after running for 43.5 seconds (cost 219 units). It improves the current
solution within the same homotopy class, as shown in Fig. (5-7,b) (113.7 seconds for
cost 200 units). Then the algorithm explores few other homotopy classes and finally
it converges to a trajectory with cost close to the optimal one (177.6 units in 51
minutes). For this case study, due to symmetry, there are different homotopy classes
of trajectories with cost close to the optimal one (or the same one). In Fig. (5-7,e-f)
we can see 2 different runs that resulted in 2 different trajectories with cost close
to the optimal one (177.26 units in 67 seconds and 177.66 units in 100 minutes). 3
3
we stopped all runs within 2 hours, if we keep running the algorithm for more time it switches
back and forth between homotopy classes, continuing to improve the cost.
125
40
40
35
30
-
30
25
-
20
2015
10
-1005-
-10
0
10
x
20
30
40
50
30
40
50
40-
30
30
20U-
N20 --
-
40-
-
x
U
10
10
0-
0.
-10
0
10
20
30
40
50
-10
x
-10
0
10
20
x
Figure 5-5: First case study: Above part) Comparison between the trajectory that
the algorithm converges to and the optimal holonomic trajectory. We see that the
optimal holonomic trajectory is close to the one the algorithm generates. Upper right
and below part) The effect of additional obstacles (gray color) close to the structure
of interest.
126
Weighted-RITA: Mean cost
Weighted-RITA: LOG-LOG Mean cost
260
P 44.
250
2.42
2.4
-
240
2.38
230..
o 2200
[
0
210-
2.36
2.34
2.32
2001
2.3
2.28
0
1000
2000
3000
lime Sec
4000
5000
0
0.5
Uniform-RITA: Mean cost
1
1.5
2
2.5
Time Sec
3
3.5
Uniform-RITA: Mean cost
2-.
230
220[
2100
0
200190.
235-
K
U
0
0
2.3F
2.25L
180
0
1000
2000
3000
4000
5000
0.5
Time Sec
1
1.5
2
2.5
Time Sec
3
3.5
4
Figure 5-6: First case study: we can see the average cost as a function of the running
time in linear scale (left part) and in log-log scale (right part). In the upper part of
the figure we can see results generated using Weighted-RITA, and in the lower part
of the figure we can see results generated using Uniform-RITA.
127
These runs show that the algorithm indeed explores all homotopy classes that need
to be explored, until reaching a trajectory sufficiently close to the optimal one. Fig.
(5-7,g-h) shows some of the results RITA gives if we include obstacles close to the
structure of interest. In this particular run, the cost decreases from 213.9 to 191.5
units for 65 seconds to 2 hours running time.
5.4.2
TSP-Art-gallery
We also implemented an approach based on TSP-art-gallery, similar to the approach
presented to
[36], on C++ (again the code is not optimized). At first we sample and
resample observation points until finding a small set of observation points that covers
the structure, then we use the Concorde TSP solver to compute the visiting order and
then we use an EST planner to compute closed to optimal trajectories that connect
the observation points 4.
As described in the introduction, the separation of the inspection problem to the
TSP and art-gallery problem is difficult to apply when the robot is non-holonomic
and operates in cluttered environments, as the observation points generated by the
art gallery problem may not be reachable from other observation points in the set.
In summary, the problems the TSP-art-gallery presents are as follows:
" In the case there are K observation points, in order to compute trajectories that
visit all observation points we have to explore the whole configuration space K
times.
" The configuration the observation points would be visited is not known a-priori
and in the case we have non-holonomic vehicles it is crucial.
" TSP solvers failed to compute the optimal visiting order because they do not
take into account obstacles.
" The art-gallery solvers could probably place several observation points close to
obstacles, this is likely to result in agent with non-holonomic dynamics to get
'we run the EST algorithm sufficiently large number of iterations such that each of the trajectories
that connects the observation points will be improved several times.
128
40-
30
10
I
407
10-
20.10
10
-10.
-0-20
0
20
4
X
120
W
0
30
40
2C-1
x
(a) t = 43.5 seconds
(b) t
=
113.7 seconds
(c) t
=
~76.8 seconds
70
4010
10
10
40
30
--
E0-
20
10
10
-10
0-
24
-20
0
-20
(d)
0
51
40u
-2C
0
t
20
-
0
$0
X
(e) t = 67.1 seconds
51 minutes
70
10
50
50
40-
107
= 100 minutes
40-
U.
U
> 30
(f) t
N 30
1007
-10
-20
0
20
40
EQ
X
(h) t = 2 hours
(g) t = 39 minutes
Figure 5-7: Second case study, (a-d) the algorithm finds the first admissible solution
after running for 43.5 seconds (cost 219 units). It improves the current solution within
the same homotopy class (113.7 seconds for cost 200 units). Then the algorithm
explores few other homotopy classes and finally it converges to a trajectory with cost
close to the optimal one (177.6 units in 51 min). (e-f) Different homotopy classes of
trajectories with cost close to the optimal one. (i-j) Results with additional obstacles
(obstacles in gray color).
129
trapped as a result, the algorithm fails to compute an inspection trajectory or
it takes long time to compute one.
On the other hand approaches based on TSP and art-gallery methods require less
memory compared to the one we propose here and this is very important for complex
structures.
We run a set of simulation results for the second case described in the previous
subsection. The average time of the runs depends on: the number of iterations (or
number of improvements) the planner that connects 2 sequential observation points
takes, and in the number of iterations the algorithm takes to compute a small number
of observation points (see
[361).
In Fig. (5-8) we can see some of the results the TSP-
art-gallery approach gives, in our case the average running time is about 10 mins to
get an inspection trajectory with average cost of 265 units (Fig.
(5-8, lower left))
. The cost of the best trajectory we got (out of 40 runs) is about 212 units (Fig.
(5-8, upper left)).
In addition, the algorithm fails to return a feasible inspection
trajectory 1 out of 6 times because some of the observation points are chosen to be
close to obstacles and the agent due to its dynamics can not escape (Fig. (5-8, lower
right)). Fig. (5-8, lower left) shows one run that the TSP solver failed to compute
a good visiting order because the obstacles were not taken into account during the
computation.
5.4.3
3D Workspaces
Academic Applications and Algorithm Evaluation
We also implemented uniform-RITA and weighted-RITA for general 3D workspaces
in C++. To handle general 3D structures we use the PQP collision library [781. The
visibility checking was also done using the PQP library. We consider the following
130
Figure 5-8: TSP-art-gallery approach: upper left shows the best inspection trajectory
out of 40 runs (cost 212 units), upper right: shows a typical trajectory (cost 265
units), lower right: the TSP solver failed to compute a good visiting order because
the obstacles were not considered for the TSP problem, lower right: It fails to find
a feasible trajectory because one of the observation points was chosen to be close to
the obstacles.
131
robot's dynamics.
u, sin 6
(5.7)
u.cos6
ZAz
The configuration space now is 4D ([X, y, 6, zIT) and the control space is 3D ([u,, D, Az]).
Thus the agent moves with a minimum turning radius in planes parallel to the blue
plane (see Fig. (5-10)) and constant elevation per unit time towards the Z direction
(perpendicular to the blue plane). Where u, takes values from [0.6,21, Q takes values from [-M, Qrad/s and Az takes values form [-2,2lunits/s. These parameters
result in a minimum turning radius (in the blue plane) of 2 units.
The sensor model is such that it observes everything within a maximum radius
Rr.,ng (sphere) that is not occluded by the structure. In all the missions the range of
the sensor is 8 units. The visibility for a given trajectory is computed by projecting
the footprint of the sensor on the workspace and using the PQP library to identify
parts that are occluded by the structure and the obstacles in the workspace.
We consider 3 case studies, in the first case study we compute close to optimal
trajectories to inspect a link (ring) of a huge chain (that is cominonly found in the
offshore oil industry, see Fig. (5-9)), in the second case study we compute close to
optimal trajectories to inspect a knot (that is also commonly found in the oil industry,
see Fig. (5-9)). In the 3rd case study we compute close to optimal trajectories to
inspect a maze with spheres (10000 faces). In all cases the size of the workspace is
20 x 20 x 20 units. In the Table (5.1) and Table (5.2) we can see the statistics for the
Weighed-RITA and Uniform-RITA respectively. In Fig. (5-10) we can see top views
and isometric views of typical runs for each one of the missions. In the top we can
see a resulting close-to-optimal 5 trajectory for the "ring" mission, in the middle we
5
The exact optimal cost is very difficult to compute. However, for the "Ring" case, because we
know the maximum perimeter of the torus and the cost function is just the distance traveled we can
try to estimate a-priori a cost value that is close to the optimal for the case we have a holonomic
132
part can see a resulting close-to-optimal trajectory for the "knot" mission and in the
lower part we can see a resulting close-to-optimal trajectory for the "maze" mission.
In all runs, the agent's initial configuration is at the intersection of the green red and
blue planes ([-10, -10, -10, 45deg]T).
P
r267m
Pudido spar
ENffTowr
Figure 5-9: The size of marine structures and their mooring lines is huge.
Industrial Applications
We also tested RITA on industrial applications. We consider the same equations of
motion as before, however we reduce the range of the sensor to 6 units. We consider
2 case studies, the first one is a simplified model of the lower part of a real oil rig and
the second one is a torpedo shaped vehicle (e.g. a type of a submarine). The size of
the mesh is about 10000 faces for the oil rig, and about 35000 faces for the torpedo
shaped vehicle. The results can be found in Fig. (5-11).
vehicle e.g. about 70 units. In addition, as we can see from the 2D case studies the algorithm returns
trajectories that are close to optimal.
133
Table 5.1: Weighted-RITA: Average running times and iterations to get the first
feasible inspection trajectory and close-to-optimal inspection trajectories
Mean cost of the 1st feasible solution
STD of the mean cost of the 1st feasible solution
Mean cost of a close-to-optimal solution
STD of the cost of a close-to-optimal solution
Mean time to the 1st feasible solution (sec)
STD of the time to the 1st feasible solution (sec)
Mean time to close-to-optimal solution (sec)
STD. time to close-to-optimal solution (sec)
Ring
104.41
10.18
81.11
2.87
29.7
20.6
870
701
Mean iterations to 1st feasible solution
3.92104
STD of the iterations to 1st feasible solution
Mean iterations to close-to-optimal solution
STD of the iterations to close-to-optimal solution
2.77 10
1.4 106
1(r
Knot
117.55
18.14
91.3
3.14
323
204
2678
1620
3.12 105
2.17 105
2.1 10'
1.3 106
Maze
139.23
12.07
107.67
2.73
904
570
4303
1168
1.6 105
9 10"
6.3 105
2.25 105
Table 5.2: Uniform-RITA: Average running times and iterations to get the first
feasible inspection trajectory and close-to-optimal inspection trajectories
Mean cost of the 1st feasible solution
STD of the mean cost of the 1st feasible solution
Mean cost of a close-to-optimal solution
STD of the cost of a close-to-optimal solution
Mean time to the 1st feasible solution (sec)
STD of the time to the 1st feasible solution (sec)
Mean time to close-to-optimal solution (sec)
STD time to close-to-optimal solution (sec)
Ring
108.82
14.61
80.14
3.2
32.52
16.71
887.22
400
Mean iterations to 1st feasible solution
4 104
STD of the iterations to 1st feasible solution
Mean iterations to close-to-optimal solution
STD of the iterations to close-to-optimal solution
2.21 10
1.47 106
6.05 101
134
Knot
123.41
14.92
88.27
3.1
316
199
3476
1349
2.21 105
1.37 105
2.66 106
1.05 10
Maze
154.6
17.53
105.9
4.2
70
33
5507
684
12364
6261
1.95 10
1.01 105
I4
b
a
I"
K
'K
'K
d
c
.
.
I.'
S
\~/
/
/
*
/
0**
e
f
Figure 5-10: Top and isometric views of typical runs for each one of the missions. a-b)
close-to-optimal trajectory for the "ring" mission, c-d) close-to-optimal trajectory for
the "knot" mission and in e-f) close-to-optimal trajectory for the "maze" mission.
135
.9
-/
fix
a
b
C
d
It
e
f
Figure 5-11: Several views of typical runs for each one of the missions. a-b) close-tooptimal trajectory for the "oil rig" mission, c) a 3D CAD model of a real oil rig, d-f)
close-to-optimal trajectory for the "Submarine" mission.
136
5.4.4
Implementation
To increase the speed of the proposed algorithm we use the idea of macro actions
[941, each time we choose a node for expansion we apply the same control input for
a predefined number of times thus, we add more than one node per iteration. In this
way we get both actions that cover small and large distances in the configuration
space. For instance, for the 2D cases we reapply the same control input 5 and 8 times
respectively and for the 3D cases ("ring", "knot" "maze") we apply the same control
input 8 times. To minimize the memory usage, even if we take observation actions
at each of the intermediate points ("sub-nodes") we do not keep their visibility in
memory. Instead, the visibility of the "sub-nodes" is curried out in the last node in
the sequence (e.g. the intermediate nodes serve as a trajectory from the parent node).
5.5
Summary and Discussion
This Chapter proposes a new inspection planning algorithm, called Random Inspection Tree Algorithm (RITA). Given a perfect model of a structure, sensor specifications, robot's dynamics, and an initial configuration of a robot, RITA computes the
optimal inspection trajectory that observes all points on the structure. This chapter proposes a new way of approaching the inspection planning problem. Instead of
separating the inspection problem to the art-gallery and TSP we proposed the used
of sampling-based techniques to find admissible trajectories with decreasing cost. As
the number of iterations (of the algorithm) increases the control input the algorithm
samples approaches to the optimal control input.
Despite of the promising results there is room for improvement. In this Chapter
we have developed the framework to solve the inspection problem using samplingbased motion planners. From our experience with sampling-based motion planners
we can see that different sampling strategies (RRT, RRG, adaptive etc.) can be used
to accelerate RITA. It would also be interesting to see how RITA behaves with
different cost functions (e.g. penalize the number of observation actions). We would
also like to extend RITA to take into account uncertainty.
137
138
Chapter 6
Robustness of Approximate
Optimal Trajectories
In the previous two chapters we have developed algorithms for optimal planning under
perfect knowledge over vehicle dynamics, environment map and sensing. However,
real world comes with uncertainty. In this chapter, we would like to study robustness
properties of optimal inspection trajectories with respect to noise in the control inputs
and disturbance forces from the environment. For example, we would like to compute
the level of control noise before a trajectory during execution time becomes nonadmissible (e.g collides with the structure, or looses coverage properties).
Using Lipschitz continuity and assuming that the trajectories returned by the
proposed algorithms are elastic and have some clearancel, we derive theoretical deterministic bounds on the maximum noise levels (on the control inputs) we can handle.
Unfortunately, the deterministic bounds are not useful enough and therefore we have
to rely on and derive probabilistic bounds, for example we compute the probability
of failing as a function of the noise properties. In the last section of this chapter,
we introduce a framework based on Monte Carlo methods to evaluate trajectories in
the presence of both noise in the control inputs and disturbance forces that effect the
states directly.
1The elasticity property
was formally define in Chapter (5), while the clearance property for this
chapter is defined formally later in this chapter.
139
6.1
Introduction
We consider again the three trajectories and the three induced paths described in the
previous chapter. Let #*(t) : [0, Tg] -+ U denote the optimal inspection trajectory and
-o. : [0, Tg] -+ Si,, to be the induced optimal path. Suppose the optimal inspection
trajectory is approximated with a set of step functions 2 , which is represented as a
sequence of control inputs u* = (U,,
u,..., u*), where n =
J,
*
= 4* ((i - 1) -At)
for i E [1, n], and each control input in the sequence is applied for At time and
-U : [0, TJ -+ S is the induced approximate optimal path. We also consider the
trajectory the proposed inspection algorithms return, u : [0, T] -+ U, to be an eclose to u* trajectory that is of the same form (e.g. piecewise constant) and of the
same time duration of u* and -yu : [0, T] -+ S to be the induced path.
In addition, we consider the trajectory during execution time , ii*, to be a set of
step functions similar to u*, e.g. i* = (uI + nu,1 , u
+
,2,...,u*
+ n,), where n,,
is the control noise. Let nu,i E U, Vi E [1, n] to be a vector of random variables for
which each entry is distributed according the following probability density function:
P(.), P: R -- [0, 1].
We also assume that u* has 61-clearance and A 1-elasticity such that:
A, = min(Ai, 6)
(6.1)
It is important to notice that in general the clearance assumption is a property of the
state space. In this chapter (as opposed to the previous chapter) we use a different
definition. A trajectory u* : [0, T,] -+ U has 6-clearance if it induces a path in the
state space (e.g. y* : [0, TgJ -+ S) such that for any t E [0, TgJ there exists a ball of
radius 6 centered at yu. that is obstacle free e.g. BA(7.u (t)) C Svee, Vt E [0, Tg].
Given P(.) as a uniform probability density function with bounded support e.g.
-.-Lfor
P(X =)
2
=
2C
0
r
-C( X
elsewhere
(
(6.2)
the proposed framework is flexible to use any base function, e.g. linear, cosine functions etc.
140
we would like to study the robustness to control noise of the approximate optimal
trajectory u*, ie. we would like to:
* compute the level of the control noise such that the trajectory during execution
time, u*, is still an admissible trajectory (e.g. does not collide with the structure
and covers the entire structure).
* compute the level of control noise such that the probability the trajectory during execution time remains an admissible trajectory is greater than a given
threshold.
6.2
Deterministic Theoretical Bounds
To compute the level of the control noise such that the trajectory during execution
time,
12*,
is still an admissible trajectory (e.g. does not collide with the structure and
covers the entire structure) we use the Lipschitz continuity property of the equations
of motion and results from Chapter (4). Direct application of the Equation (4.5)
from Chapter (4) for the approximate optimal trajectory, u*, the trajectory during
execution time,
*, and their induced paths (e.g. 7. ,7'.) gives:
Eu(eP' - 1)
Es(t)
Eu(eLPT9 - 1),Vt E [0,Tg]
(6.3)
We recall that Es(t) is the Li Norm of the difference between -yu. and 7f. (e.g.
Es(t) =
I'y|..(t)
- ya.(t)JJVt E [0, Tg) induced by trajectories u* and 12* such that
EU = maxtE[o,T9, |u*(t)
-
1*(t)I1, Vt E
[0, Tg.
Because the approximate optimal trajectory is assumed to have the properties of
61-clearance and Ar-elasticity, thus the trajectory during execution time (U^*) is still
141
an admissible trajectory if the following holds:
Es(t)
E
L(ept
-1)
Eu(eLp
-
1)
Eu(e pT
1 ,vt E
Eu(epT -1)
A1
EU
A1
A-
Where Eu = maxtE[o, 9 IIu*(t)
-
-
1) !
A 1 ,Vt
E [0,7T1
[OTg}
,Vt
E [0, T]
(6.4)
i*(t)j1,Vt E [0,Tg], since both u* and ^* are rep-
resented by a sequence of step functions each of them applied for a constant time
duration At, then, EU = maxVEh1,l(I|n,iJ). For all i n,,,i is a vector with independent random variables distributed according to P(.). In the above expression,
denotes the L1-Norm, therefore, Eu = 2mC. We can now relate
A,
}1.11
with the noise
support C:
C
6.3
52m(e A,
pT
-
(6.5)
1)
Probabilistic Theoretical Bounds
The deterministic bound, as derived in the above section ( see Equations (6.5)), takes
into account the worst case scenario on both the response of the dynamical system
in question (exponential divergence) and the noise level. Therefore the deterministic
bound results in control noise level that is extremely small and as such probably not
useful for certain cases. On the other hand, if we are interested in computing the
level of control noise such that the probability of success 3 to be greater than a given
threshold, then we can get more useful/realistic bounds.
In the context of this chapter and based on the above assumptions, we define
the event "A" to be the event a trajectory during execution time, e.g. 12*, to be an
admissible trajectory. Then, the probability for the event "A" to happen is equal to:
Pr(A)
3
Pr(Es(t= T) 5
A)
e.g. a trajectory during execution time (e.g. U*) to still remain an admissible trajectory.
142
(6.6)
Using results from Equation (6.4), we get,
Pr(Es(t = T,)
A,)
Pr(EU(eLPT - 1)
Pr(A)
Pr(Eu 5
1)
=
A1
(6.7)
1)-P9
)
Pr(A)
Equation (6.7) gives the probability for the trajectory during execution time (and its
induced path) to be an admissible trajectory (path) in terms of:
* the probability density function ( more specifically, in terms of the Cumulative
Density Function (CDF) ) of EU = maxviE[1,n](I|nU,iIJ), which depends on the
control noise and the dimension of the control space U.
* the clearance and the elasticity properties of the approximate optimal trajecto-
A1
.
ries
* the dynamics of the agent (through the Lipschitz constant L,).
* the time duration of the trajectories, e.g. T,.
_-). Since Eu = maxVE[o,nl1](Jn.,iJ)
We would like to compute Pr(Eu
and for all i, n, ,1 is a vector with independent random variables distributed according
to P(.) then,
Pr(Eu
eu) = Pr(Z ln'i,II < eu,
i=1
Pr(Eu
e)
=
Iln11f2,u| !5 e , E |lni'3,ul 5 e-... E In'Iul|
i=1
M=1
eu)
1=1
Pr(Z IInuII 5 e)Pr(Z Ilni2,uI| < eu)...Pr(Z |hnn,uII 5 eu)
i=1
i=1
i=1
m
Pr(EU 5 eu) =
[Pr(Z lln,,uI !5 eu)]n
i=1
Were 7 ,, is the ith entry to the n, vector. Because ||n i,U1j represents the absolute
value of n'Iu (a random variable with probability density function in Equation (6.2)),
then ||nii,,| is a random variable with probability density function given by the
143
(6.8)
equation below:
A
(
,for 0<xz<
C
0 elsewhere
P(X = X) =
Then, Pr(E'n, [lnii,uIl
(6.9)
e,) can be computed using nth convolution of P(X = x)
or the Irwin-Hall Distribution [491.
Substituting e =
Pr(A) ;> Pr(Eu
we get:
(e[Pr(pj-
)'
E
ni=I
=m,,1
_)]n (eLvTe
(6.10)
1
In the presence of noise on the control inputs, the above equation, Equation (6.10),
represents a lower bound on the probability the trajectory during execution time to
be an admissible trajectory. It is important to notice the dependence of the above
bound on the dimension m of the control space.
6.4
Monte-Carlo Framework for General Trajectory Evaluation
In the previous subsections we derived theoretical bounds that characterize the robustness of the approximate optimal trajectory in the presence of control noise. For
the general case, theoretical bounds are not easy to be derived, thus we propose the
use of the Monte-Carlo framework to evaluate the robustness of a given trajectory
in the general case where we have control noise, state noise or/and additional forces
acting on the agent etc.
In Figure (6-1), we can see the average probability of getting an admissible trajectory for several runs of the algorithm as a function of the noise levels considered
in the experiments. In each run the resulting approximate optimal trajectory was
evaluated using the Monte-Carlo framework, each Monte-Carlo run generated and
evaluated 200 trajectories and paths. The probability of getting an admissible trajec144
tory for each Monte-Carlo is computed by the ratio of the admissible paths divided
by the number of all runs. To get the average probability of getting an admissible
trajectory we simply get the average over all runs. For each of the Monte-Carlo runs,
the generated trajectory was taken by corrupting the approximate optimal control
input and the state with random noise taken from the underlining probability density
functions.
In this particular case we show results from the inspection planning missions
(the "ring" and the "knot" experiments). We assume that the control noise in each
dimension of the control inputs (forward speed, rotational velocity, and elevation per
unit time) is taken from a uniform probability density function given in Equation (6.2)
where C is given as a certain percentage of the approximate optimal control input at
each time step. In addition, the state noise is also take from a uniform probability
density function with C equal to a certain percentage of 20 (the maximum distance
in the workspace) for the x, y, z states and of 360deg. for the 0 state. For example, in
the case we have 0.5% noise in the control inputs (as defined above) and 0.5% noise in
the states (as defined above), then the probability of getting an admissible trajectory
is equal to 92% for the "ring" mission and equal to 81% for the "knot" mission.
6.5
Summary and Discussion
The proposed algorithms described in the previous sections do not take into account
uncertainty or disturbance forces acting on the agent during execution time.
In
this chapter, we show that under certain assumptions (e.g. elasticity and clearance
assumptions on the computed trajectory), the resulting trajectories are robust to noise
in the control inputs and states. In this chapter we derived theoretical deterministic
bounds on the level of the control noise such that the resulting trajectory/path during
execution time to be an admissible trajectory/path. Because the deterministic bounds
are not useful enough, we derived probabilistic bounds on the level of the control noise
such that the probability of getting an admissible trajectory during execution time
to be less than a threshold. In addition, we proposed the use of the Monte-Carlo
145
Robustness
100
90 ---------- ..---..
.....------- --------------.-- ----
- -
Ring
K n ot
--
LL
0
.
---..
....
.
..
.........
.......-----.
----------
3 0 -..................................-
10
0
0.005
0.01
0.015
0.02
0.03
0.025
Noise Level
0.035
0.04
0.045
0.05
Figure 6-1: Effect of additive control and state noise
framework to evaluate evaluate the robustness of a given trajectory in the general
case where we have control noise, state noise or/and additional forces acting on the
agent etc.
This chapter concludes our work on path/motion/inspection planning. In the next
Chapter we will close with conclusions and future work.
146
Chapter 7
Conclusions and Future Work
7.1
Conclusions
Motivated by inspection applications for marine structures this thesis develops algorithms to enable their autonomous inspection. Two essential parts of the inspection
problem are (1) path planning and (2) surface reconstruction. This thesis contributes
to both parts of the autonomous (robotic) structural inspection problem and to the
optimal path, inspection and task planning problem. Regarding the path planning
part of problem, we compute trajectories that are informative enough to gather data
(such as sonar and LiDAR data) from all different views of a given structure that
guarantee that all hidden parts of it would be visible.- In the surface reconstruction part of the problem, we assembled data gathered from all different views of the
structure to reconstruct a 3D CAD model of the external surfaces of the structure.
7.1.1
Surface Reconstruction
An essential part of the inspection problem is the surface reconstruction part.
In
the surface reconstruction part of the problem we assembled data gathered from all
different views of the structure to reconstruct a 3D CAD model of the structure of
interest.
As a starting point we assume that we have data such as laser scanner
data and sonar data that covers all details of a given structure and we would like
147
to assemble this data to reconstruct 3D models (e.g. CAD models and point cloud
representations) of the structure of interest.
Chapter (3) focuses on the problem of 3D surface reconstruction in the marine
environment. This chapter summarizes our hardware, software, and experimental
techniques on surface reconstruction over the last few years. We use off-the-shelf
sensors and a standard robotic platform to scan marine structures both above and
below the waterline, and we developed a method and software system that uses the
Ball Pivoting Algorithm (BPA) and the Poisson reconstruction algorithm to reconstruct 3D surface models of marine structures from the scanned data. We have tested
our hardware and software systems extensively in field experiments in the Singapore
coastal zone, including operating in rough waters, where water currents are around
1-2m/s. We present results on construction of various 3D models of marine structures, including slowly moving structures such as floating platforms, moving boats
and stationary jetties. Furthermore, the proposed surface reconstruction algorithm
makes no use of any navigation sensor such as GPS, DVL or INS.
Executing the experiments we present in Chapter (3), we realize how difficult is
to compute 1) admissible trajectories that guarantee 100% structure coverage and 2)
close-to-optimal trajectories that guarantee 100% structure coverage. In the second
part of this thesis we focused on the path-planning part of the problem.
7.1.2
Path Planning
Although many inspection planning algorithms, have been proposed most of them fail
to compute optimal or close-to-optimal inspection trajectories that take into account
vehicle dynamics for high-dimensional systems. These approaches decompose the
problem to the TSP and the art-gallery problem and solve each problem separately.
Because the computation of the observation points and the visiting order cannot be
computed independently, even for holonomic systems, by separating the inspection
planning problem to the TSP and the art-gallery problem optimal or close to optimal
inspection planning may be difficult to achieve. Instead, we model the problem as
an optimization planning problem and we propose an approach that achieves optimal
148
inspection planning based on tools and ideas developed by the path planning community and novel ideas we present in this thesis. We formulate the inspection planning
problem in a general way. In fact, we frame it as a general planning problem or as
a task planning problem. A general way to define a planning problem is as follows.
Given a dynamical system, the obstacle-free space (e.g. a subset of the state space
that is obstacle-free) and task specifications, we define an admissible path to be any
obstacle-free path that represents a solution to the system dynamics and does not
violate the task specifications.
Then an optimal task planning problem computes
among all admissible paths the one that minimizes a cost function.
The simplest
category of task planning problems is the path planning problem; for the simple path
planning problem an admissible path is an obstacle-free path that starts from a given
state, reaches the goal region and represents a solution to the system dynamics. It is
clear that, the set of all path planning problems is a subset of the set of all inspection
planning problems and the set of all the inspection planning problems is a subset of
the set of all the task planning problems (see Figure (7-1)).
Task
Planning
Inspection
Planning
Path
Planning
Figure 7-1: The set of all path planning problems is a subset of the set of all inspection
planning problems and the set of all the inspection planning problems is a subset of
the set of all the task planning problems.
At first, we studied the simple path planning problem (Chapter (4)). Because we
are interested in high-dimensional systems we took a sample-based approach. Over
149
the last 20 years significant effort has been dedicated to the development of samplingbased motion planning algorithms such as Rapidly-exploring Random Trees (RRT)
and its asymptotically optimal version (e.g. RRT*). However, asymptotic optimality
for RRT* only holds for linear and fully actuated systems or for a small number
of non-linear systems (e.g. Dubin's car) for which a steering function is available.
These approaches sample the configuration space and rely on steering functions to
connect the configuration space with the control space. However, for general non-
linear systems, a steering function is not always available and as a result the optimality
guarantees of the RRT* algorithm do not hold for general non-linear systems. Instead
of sampling the state space, in this thesis we propose a method that samples the
control space directly.
Instead of trying to cover the state space, we are devoting
our efforts to sample a control input (a trajectory) that is arbitrarily close to the
optimal one, and as a result we alleviate the necessity of using a steering function.
We present a novel analysis on asymptotic optimality of a family of algorithms that
directly sample the control space. This analysis only requires Lipschitz continuity on
the differential constraints and the cost function. This analysis shows, for the first
time, that optimal path planning (and inspection planning, and general task planning)
can be achieved by sampling the control space directly. We also provide simulation
results for several cases including the well-studied pendulum-on-a-cart problem in the
presence of obstacles.
In the way we define each one of the path planning problems (simple path plan-
ning, inspection planning and task planning) their definition is identical. The only
difference is on the definition of admissible paths. Because the analysis in Chapter
(4) is done on the control space, we do not really need to worry about the details
of the induced paths in the state space (e.g. to reach the goal region or to visit
several goals in a given order or not). Therefore, as far as all the assumptions are
valid, the analysis in Chapter (4) holds for all the planning problems. Based on this
observation, in Chapter (5) we developed a new inspection planning algorithm, called
Random Inspection Tree Algorithm (RITA). Given a perfect model of a structure,
sensor specifications, robot's dynamics, and an initial configuration of a robot, RITA
150
computes the optimal inspection trajectory that observes all points on the structure.
Instead of separating the inspection problem to the art-gallery and TSP we propose
the use of sampling-based techniques to find admissible trajectories with decreasing
cost. As the number of iterations (of the algorithm) increases, the control input the algorithm samples approaches to the optimal control input. To alleviate the difficulty of
dealing with challenging dynamical systems RITA samples directly the control space.
We also provide a rich set of simulation results motivated by inspection problems for
marine structures.
Our contributions in Chapter (4) and Chapter (5) do not consider uncertainty and
disturbance forces acting on the vehicle during execution time. However, real world
comes with uncertainty and disturbance forces acting on the agent. In Chapter (6),
we studied the robustness of the close-to-optimal trajectories (trajectories returned by
the proposed algorithms in Chapter (4) and Chapter (5)). Using Lipschitz continuity,
and under certain other assumptions we showed that, the resulting trajectories are
robust to noise in the control inputs and states. We derived theoretical deterministic
bounds on the level of the control noise such that the resulting trajectory/path during
execution time to be an admissible trajectory/path. Because the deterministic bounds
are not useful enough, we derived probabilistic bounds on the level of the control noise
such that the probability of getting an admissible trajectory during execution time
to be less than a threshold.
In addition, we proposed the use of the Monte-Carlo
framework to evaluate the robustness of a given trajectory in the general case where
we have control noise, state noise or/and additional forces acting on the agent etc.
7.2
Contributions
The contributions of this thesis are as follows:
* We provide a novel analysis that demonstrates that the optimal path planning
problem for general non-linear dynamical systems can be achieved using random
sampling in the control space. This analysis does not require a steering function.
151
"
We perform a novel analysis on the convergence rate of algorithms that sample
directly the control space. In addition, we provide a closed form solution for
the behavior of a such algorithm.
" Building upon existing planners, we propose a new asymptotically optimal family of planners. As the number of iterations increases, the sequence of control
inputs these algorithms sample, approaches the optimal control trajectory, with
probability one. Simulation results are provided.
" Building on results from our previous contributions on the path planning problem, we propose the first asymptotically optimal sampling-based inspection
planning algorithm, called Random Inspection Tree Algorithm (RITA). RITA
computes both observation points the trajectory to visit the observation points
simultaneously. It uses sampling-based techniques (in the control space) to find
admissible trajectories with decreasing cost.
" We present a set of simulation results from 2D and 3D workspaces and agents
with differential constraints.
" Using off-the-shelf sensors and a commercial robotic kayak [28], we assembled
a novel autonomous surface vehicle that is capable of using a powerful 3D laser
scanner (Velodyne) and a side-looking sonar to scan marine structures both
above and below the waterline.
" Using Velodyne and sonar data, without using any other navigation sensor such
as GPS, DVL or INS, we propose a method to reconstruct 3D models of partially
submerged slowly moving marine structures.
" We present various surface reconstruction experimental results including results
from sea environments with water currents around 1-2m/s. More specifically,
we present three experimental results of mapping the above-waterline part of
marine structures and one experimental result of mapping both the above- and
below-waterline parts of marine structures. To the best of our knowledge, these
are the first results on 3D surface reconstruction field experiments in rough seas
152
and slowly moving structures. Experiments presented here are as realistic as
possible. We present results in rough waters with moving structures (floating
platforms and boats) under tidal effects (1-3 m) and water disturbances arising
from big ships moving in the busy Singapore coastal zone.
7.3
Future Work
There are several directions for future work and further improvement. There are
directions for future work for each one of the problems we address in this thesis (e.g.
path planning, inspection planning and surface reconstruction) and for the inspection
problem as a whole. Probably the first priority for future work is to implement the
inspection planning algorithm on a real vehicle in the ocean. In order to be able to
do this though, our inspection planning algorithms need extension to be able to able
to cope with uncertainty.
7.3.1
Path Planning
Regarding our path planning contributions, despite the promising results, there is
room for improvement. We do not argue that we have definitively solved the general optimal path planning problem for non-linear systems. Instead, we are shedding
some light on it and its challenges. As we see from the results section, the number
of iterations needed to get a trajectory with close-to-optimal cost is large. One can
argue that non-holonomic problems are significantly more complex than linear problems. However, we believe that the number of iterations can significantly be reduced
if instead of using uniform sampling of the control space, we use similar methods researchers have been using to guide the sampling in the configuration space to sample
the control space.
The proposed framework (planning in the control space) can be used to solve
optimal control problems e.g. Model Predictive Control (MCP) problems. We can
generate the proposed optimal controller off-line and use it in real-time. Given that
we can achieve faster convergence rates than the ones we have now (e.g. using clever
153
sampling), then this framework (planning in the control space) can be used to design
optimal controllers in real-time.
7.3.2
Inspection Planning
A long term goal is to develop a system capable of autonomously inspecting marine
structures in the presence of uncertainty. As described in this thesis, we have already
developed an inspection planning algorithm that computes close-to-optimal inspection trajectories and a framework that reconstructs 3D CAD models of the structure
of interest from scanned data. The proposed surface reconstruction framework can
handle uncertainty (e.g. noisy point clouds), however the inspection planning algorithm in its current form can only handle small uncertainty. To be able to accomplish
autonomous inspection of marine structures in realistic environments in the presence
of large uncertainty, we have to develop an efficient inspection planning algorithm
that would be able to handle uncertainty in the environment map, in the observation
actions, and in the control actions (disturbance forces e.g. water currents).
To tackle the inspection planning problem under uncertainty, the use of the Partially Observable Markov Decision Process (POMDP) framework [64] ( a mathematical principled way for making decisions under uncertainty) is suggested.
This
problem is extremely challenging because it involves decision making in the "belief
space" which is continuous and high-dimensional. Similarly to this thesis, to treat the
above challenges the use of sample-based representations of the "belief space"
[75]
is
suggested. The goal is to compute an optimal "Policy" (a mapping from the "belief
space" to the control space, controller) that minimizes a given "Reward" function
that is chosen such that it penalizes distance travelled and at the same time rewards
small variance on the obtained 3D model of the structure. A key difference of the
inspection planning problem to standard POMDP problems is that in the standard
case the reward function depends directly on the states as opposed to the inspection
planning problem that depends on the "belief space", therefore standard POMDP
solvers have to be modified to be used for the inspection problem.
In addition, in order to be able to cope with larger 3-D structures (such as the
154
ones in [38]) we have to increase both the convergence rate of RITA and its memory
management. Similarly, to the simple path planning algorithm proposed in this thesis,
we would like to find an efficient way to guide sampling in the control space to improve
convergence.
7.3.3
Surface Reconstruction
Despite the above promising results, there is still plenty of room for improvement.
In this work, we have assumed a GPS-denied environment, without using any other
navigation sensors such as DVL or INS. Of course whenever possible, we are interested in using GPS and other navigation sensors. Furthermore, we would like to use
advanced SLAM techniques such as feature extraction and loop closures to bound
localization accuracy, which crucially affects the quality of the map. However, as
indicated above, special treatment should be considered in the case of moving structures such as floating platforms [86, 58]. In addition, more research needs to be done
on integrating data from above and below the waterline parts of a marine structure.
More specifically, we would like to use registration algorithms to better align the two
parts. Another possible direction of future work will be the combination of the ICP
solver with randomization methods to find the global minimum in Equation (3.1).
As the number of sample points increases, ICP solvers combined with randomization methods are expected to converge to the globally optimal transformation matrix
(given that the samples are chosen correctly), however this needs to be demonstrated
in future research.
155
156
Appendix A
Probability to Get at Least one
Trajectory that is c-close to Any
Approximation of the Optimal
Trajectory
Constant integration Time
Let the event Xi,A be the following event: " From j valid samples (up to
j iterations),
an algorithm, that samples the control space, generates at least one control sequence u
that is E-close to at least the first k"h subsequence of u*, i.e,
lul
= k and dist(ui,u4) < E
for i E [0, k]. Then using the total probability theorem and condition on the events
X,-1,k and X1.1,k (up to the previous iteration the underlining algorithm (does not)
return(s) at least one trajectory with the desired characteristics) is given by1 :
Pr(Xi,,) = Pr(XkIXj._,k)Pr(X .,)
1
+ Pr(X,kIXj-1,k)(1 - Pr(X-1,)) =*
Pr(Xj,,) = Pr(X_ 1,k) + Pr(Xj,kIXj_1,k)(1 - Pr(X_l,k))
'For simplicity, in the main part of this paper we use Pi,k instead of Pr(Xi,k).
157
(A.1)
To compute the term Pr(XkIXj1,k) in the above equation, we use the total probability theorem condition on the events X_1,-1 and Xj-1,k-1.
+
Pr(Xj,k1X1 .i,k) = Pr (Xj,kix,.1, flgjlkl)Pr(.-l,k-lXj.-l,k)
Pr (Xj,k IX,-1,k
X_1, k1)Pr(X-1,k-1 IX.-1,k)
(A.2)
Because X_1,k is a subset of Xp.1,k-1, then Pr(X,kIX,.1,k flX,. 1,k- 1 ) = Pr(Xj,kl5I1,k-1)
0. In addition, by the definition of p for the second term in the above equation we
get Pr(X,jkIX1,k
n XJI,k-1)
> 4.
Where 1 is the probability to sample any node
for expansion (in the case we use uniform sampling). Similar analysis holds for the
case we do not use uniform sampling. Using the above we get:
Pr(Xj,kjfg_1,k)
Pr(X,k)
P Pr(Xj_1,k_1j75-1,k)
Pr(Xj-l,k) + Pr(X_1,._1I._1,k)(1
-
Pr(X-,1,k)) (A.3)
Using the Bayes' theorem (alternatively Bayes' law or Bayes' rule), we get
,-1
(A.4)
=Pr(Xj-1,kIXj_1,k_1)Pr(Xj-1,-1)
Pr(X-1,)
Using Equation (A.3) and Equation (A.4) we get:
Pr(Xi,,)
Pr(X,k)
Pr(X.1,k) + jPr(Xj-l,kXj-l,kl)Pr(X_1,k_1)
=
Pr(Xp_1,k) + e(1 - Pr(XjlIXj.l,k_1))Pr(X-1,k-1)
(A.5)
Using, the definition of conditional probabilities and because Xj-1,k is a subset of
Xj-1,k_1
Pr(Xj_1,klXj-1,k-1) = Pr(Xj_1,knOXj_1,k_1) -Pr(Xj-1,k)
Pr(Xl,kf
1)
Pr(X 1_1 ,k_1)
158
(A.6)
=
Substituting Equation (A.6) in Equation (A.5) we get:
p
Pr(x-k
Pr(Xj,,) >
Pr(X_,ik) +-(1
j
Pr(Xj,k)
Pr(Xl,k) + E(Pr(X_,k-_) - P(X
-
1,k)
60(X
Pr(Xj-1,k-1)
:
)Pr(Xi,_,k-_)
1
,k))
(A.7)
(A.8)
The above Equation holds for all j > k, /k E [1, n]. For the case j = k, Vk E [1, n]
we have Pk,k
*. In addition for the base case we have:
Pr(Xj,1) ! Pr(Xj-1,) +
~ ' )Pr(Xj-1,o)
Pr(Xy_1 ,O)
Pr(X_,1,) + ((1 - Pr(X_1,k))
I
-
_(
j
Pr(Xj,,)
(A.9)
Sampling Integration Time
Identical derivation follows for the case we sample integration time. For example we
define event Xi,k be the following event: " From j valid samples (up to j iterations),
an algorithm, that samples the control space and time, generates at least one control
sequence u that is E-close to at least the first k"h subsequence of u*, i.e,
lul = k and
dist(ui, u*) <e for i E [0, k] each of them applied for time at most At = [0, r]. Then
the probability Pr(Xj,A Ixi -,,kn X
1,k-1i,) >
-. Thus for the case we sample At:
Pr(Xj,k)
p At
Pr(Xj,,k) ----- (Pr(X 1 ,,k
Pr(Xj,k)
Pr(Xj-1,k) - -- (Pr(X_1 ,k_1) - Pr(Xj_1,k))
where p' = p-A
159
1
) - Pr(X-1,k))
160
Appendix B
Convergence Rate for the First
Milestone for the Discrete System
Here we will perform an analysis on the discrete system for the convergence rate
for the first milestone (e.g. k = 1). Again, we consider uniform sampling without
pruning. To simplify our notations, instead of using inequalities we will use equalities
and work with bounds. We consider another system as follows:
pi,1
=
-1,1 + (1 - Pj- 1, 1 ) ,Vj > k, k = 1
(B.1)
Thus, l',1 < P, 1 , we define Q, = 1 - P3,1 thus
Qj = Q,-1(1-
i
),Vj>k, k=1
(B.2)
Q,
goes to 0. Let a, be the
It is easy to see that as the number of iterations increase
convergence rate then we have:
Qj = j-"]
Q,-1
=
(j
161
- 1)-"
(B.3)
(B.4)
Substituting this equation on the above equations we get:
log (3. *1
3
log (1 --
log (1 -)
3
p
)
=
log (1 10og(1-)
3
)
-
log ( .
(B.5)
Using Taylor series [77] we have log(1 - x) ~ -x, x < 1 E R thus the convergence
rate for large j is equal to p. More formally, using "L'Hopital's Rule" [30] we get:
-
(1
lim log
l--og0 (1-
=
n
j-0oo.
=p
(B.6)
p
The above results agree with our analysis for the continuous equivalent system.
162
Bibliography
[11 TheOceanCorporation, http://www.oceancorp.com/index.html.
[2] E. U. Acar and H. Choset. Sensor-based Coverage of Unknown Environments:
Incremental Construction of Morse Decompositions. Intl. Journal of Robotics
Research, 21(4):345-366, 2002.
[3] Pierre Alliez, David Cohen-Steiner, Yiying Tong, and Mathieu Desbrun.
Voronoi-based Variational Reconstruction of Unoriented Point Sets.
In Pro-
ceedings of the ACM InternationalConference Proceeding Series, pages 39-48,
2007.
[4] N. M. Amato, 0. B. Bayazit, L. K. Dale, C. Jones, and D. Vallejo. OBPRM:
An Obstacle-based PRM for 3D Workspaces. In Proceedings of the Workshop
on Algorithmic Foundationsof Robotics, pages 155-168, 1998.
[5] Nina Amenta and Marshall Bern. Surface Reconstruction by Voronoi Filtering.
Discrete and ComputationalGeometry, 22:481-504, 1998.
[6] P. N. Atkar, A. Greenfield, D. C. Conner, H. Choset, and A. A. Rizzi. Uniform
Coverage of Automotive Surface Patches. Intl. Journal of Robotics Research,
24:883-898, 2005.
[7] P.N. Atkar, H. Choset, and A.A. Rizzi.
Towards Optimal Coverage of 2-
dimensional Surfaces Embedded in R3 : Choice of Start Curve. In Proceedings of
the IEEE/RSJIntl. Conf. on Intelligent Robots and Systems (IROS), volume 4,
pages 3581-3587, 2003.
163
[8] P.N. Atkar, H. Choset, A.A. Rizzi, and E.U. Acar. Exact Cellular Decomposition of Closed Orientable Surfaces Embedded in R3 . In Proceedings of the IEEE
Intl. Conf. on Robotics and Automation (ICRA), pages 699-704, 2001.
[9] Jerome Barraquand, Bruno Langlois, and J-C. Latombe. Numerical Potential
Field Techniques for Robot Path Planning. IEEE Transactions on Systems,
Man and Cybernetics, 22(2):224-241, 1992.
[10] M.R. Benjamin, J.J. Leonard, H. Schmidt, and P. Newman. An Overview
of MOOS-IvP and a Brief Users Guide to the IvP Helm Autonomy Software.
Technical Report CSAIL-2009-028, MIT, June 2009.
[11] Andrew A. Bennett and John J. Leonard. A Behavior-based Approach to Adaptive Feature Detection and Following with Autonomous Underwater Vehicles.
IEEE Journal of Oceanic Engineering, 29(2):213-226, 2000.
[12] Matt Berger, Josh Levine, Luis Gustavo Nonato, Gabriel Taubin, and Claudio T. Silva. An End-to-End Framework for Evaluating Surface Reconstruction.
Technical report, University of Utah, 2011.
[13] Fausto Bernardini, Joshua Mittleman, Holly Rushmeier, Claudio Silva, and
Gabriel Taubin. The Ball-Pivoting Algorithm for Surface Reconstruction. IEEE
Transactions on Visualization and Computer Graphics, 5:349-359, 1999.
[14] Paul J. Besl and Neil D. McKay. A Method for Registration of 3-D Shapes.
IEEE Trans. Pattern Anal. Mach. Intell., 14(2):239-256, 1992.
[15] Jack Beven. Tropical Cyclone Report Hurricane Dennis. Technical report,
National Hurricane Center, 2005.
[16] J.L. Blanco-Claraco. Contributionsto Localization, Mapping and Navigation in
Mobile Robotics. PhD thesis, University of Malaga, Malaga, Spain, November
2009.
164
[17] Michael Bosse and Robert Zlot. Continuous 3D Scan-matching with a Spinning 2D Laser. In Proceedings of the IEEE Intl. Conference on Robotics and
Automation (ICRA), pages 4312-4319, 2009.
[18] John F. Canny. The Complexity of Robot Motion Planning. MIT Press, Cambridge, MA, USA, 1988.
[19] Peng Cheng, J. Keller, and V. Kumar. Time-optimal UAV Trajectory Planning
for 3D Urban Structure Coverage. In Proceedings of the IEEE/RSJ Intl. Conf.
on Intelligent Robots and Systems (IROS), pages 2750-2757, 2008.
[20] H. Choset. Coverage of Known Spaces: The Boustrophedon Cellular Decomposition. Autonomous Robots, 9:247-253, 2000.
[21] H. Choset, K. M. Lynch, S. Hutchinson, G. A. Kantor, W. Burgard, L. E.
Kavraki, and S. Thrun. Principles of Robot Motion: Theory, Algorithms, and
Implementations. The MIT Press, 2005.
[22] H. Choset and P. Pignon. Coverage Path Planning: The Boustrophedon Cellular
Decomposition. In Proceedings of the International Conference on Field and
Service Robotics, 1997.
[23] Howie Choset. Coverage for Robotics - A Survey of Recent Results. Annals of
Mathematics and Artificial Intelligence, 31:113-126, 2001.
[24] Howie Choset and Philippe Pignon. Coverage Path Planning: The Boustrophedon Decomposition. In International Conference on Field and Service Robotics,
1997.
[25] D. Cole and P. Newman. Using Laser Range Data for 3D SLAM in Outdoor
Environments. In Proceedings of the IEEE Intl. Conference on Robotics and
Automation (ICRA), pages 1556-1563, 2006.
[26] M. Cummins and P. Newman. Probabilistic Appearance Based Navigation and
Loop Closing. In Proceedings of the IEEE Intl. Conference on Robotics and
Automation (ICRA), pages 2042-2048, 2007.
165
[27] M. Cummins and P. Newman. Highly Scalable Appearance-Only SLAM - FABMAP 2.0. In Proceedings of the Robotics: Science and Systems (RSS), Seattle,
USA, 2009.
[28] J. Curcio, J. Leonard, and A. Patrikalakis. SCOUT - A Low Cost Autonomous
Surface Platform for Research in Cooperative Autonomy. In Proceedings of the
IEEE/MTS OCEANS Conference and Exhibition, Washington DC, 2005.
[29] T. Danner and L. E. Kavraki. Randomized Planning for Short Inspection Paths.
In Proceedings of the IEEE Intl. Conf. on Robotics and Automation (ICRA),
pages 971-976, San Fransisco, CA, 2000.
[301 Guillaume de L' Hopital. Analyse des Infiniment Petits pour l'Intelligence des
Lignes Courbes. 1696.
[31] F. Dellaert and M. Kaess. Square Root SAM: Simultaneous Localization and
Mapping via Square Root Information Smoothing.
IntL. Journal of Robotics
Research, 25(12):1181-1203, 2006.
[32] M. W. M. Gamini Dissanayake, Paul Newman, Steven Clark, Hugh F. Durrantwhyte, and M. Csorba. A solution to the Simultaneous Localization and Map
Building (SLAM) Problem. IEEE Transactionson Robotics and Automation,
17:229-241, 2001.
[33] A. Dobson and K. E. Bekris. Sparse Roadmap Spanners for Asymptotically
Near-Optimal Motion Planning. Intl. Journal of Robotics Research, 33(1):18-
47, 2014.
[34] Bertrand Douillard, Navid Nourani-Vatani, Matthew Johnson-Roberson, Stefan
Williams, Chris Roman, 2D Pizarro, Ian Vaughn, and Gabrielle Inglis. FFTbased Terrain Segmentation for Underwater Mapping. In Proceedings of the
Robotics: Science and Systems (RSS), Sydney, Australia, 2012.
[35] Chinwe Ekenna, Sam Ade Jacobs, Shawna Thomas, and Nancy M. Amato.
Adaptive Neighbor Connection for PRMs: A Natural Fif for Heterogeneous
166
Environments and Parallelism. In Proceedings of the IEEE/RSJ Intl. Conf. on
Intelligent Robots and Systems (IROS), 2013.
[36] B. Englot and F. Hover. Inspection Planning for Sensor Coverage of 3-D Marine
Structures. In Proceedings of the IEEE/RSJ Intl. Conf. on Intelligent Robots
and Systems (IROS), 2010.
[37] B. Englot and F.S. Hover. Sampling-Based Coverage Path Planning for Inspection of Complex Structures. In Proceedings of the InternationalConference on
Automated Planning and Scheduling, pages 29-37, Sao Paulo, Brazil, 2012.
[38] B. Englot and F.S. Hover. Sampling-Based Sweep Planning to Exploit Local
Planarity in the Inspection of Complex 3D Structures. In Proceedings of the
IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), Vilamoura,
Portugal, 2012.
[39] N. Fairfield and D. Wettergreen. Active Localization on the Ocean Floor with
Multibeam Sonar. In Proceedings of the IEEE/MTS OCEANS Conference and
Exhibition, pages 1-10, 2008.
[40] Nathaniel Fairfield, George A. Kantor, and David Wettergreen.
Real-Time
SLAM with Octree Evidence Grids for Exploration in Underwater Tunnels.
Journal of Field Robotics, 24(1-2):3-21, 2007.
[41] Maurice F. Fallon, Georgios Papadopoulos, and John J. Leonard. Cooperative
AUV Navigation using a Single Surface Craft. In Field and Service Robotics
(FSR), 2009.
[42] Maurice F. Fallon, Georgios Papadopoulos, and John J. Leonard. A Measurement Distribution Framework for Cooperative Navigation using multiple AUVs.
In IEEE Intl. Conf. on Robotics and Automation (ICRA), pages 4803-4808,
May 2010.
[43] Maurice F. Fallon, Georgios Papadopoulos, and John J. Leonard. Cooperative AUV Navigation using a Single Surface Craft. In Karl Iagnemma An167
drew Howard and Alonzo Kelly, editors, Springer Tracts in Advanced Robotics
Volume 62: Field and Service Robotics, Results of the 7th International Conference, pages 331-340. Springer-Verlag, 2010.
[44] Maurice F. Fallon, Georgios Papadopoulos, John J. Leonard, and Nicholas M.
Patrikalakis. Cooperative AUV Navigation using a Single Maneuvering Surface
Craft. Intl. J. of Robotics Research, 29(12):1461-1474, October 2010.
[45] Y. Gabriely and E. Rimon. Spiral-STC: An On-line Coverage Algorithm of
Grid Environments by a Mobile Robot. In Proceedings of the IEEE Intl. Conf.
on Robotics and Automation (ICRA), pages 954-960, 2002.
[46] Enric Galceran and Marc Carreras. A Survey on Coverage Path Planning for
Robotics. Robotics and Autonomous Systems, 61(12):1258-1276, 2013.
[47] H. Gonzalez-Baios and J.-C. Latombe. Planning Robot Motions for RangeImage Acquisition and Automatic 3D Model Construction. In Proceedings of
the AAAI Fall Symposium Series, Integrated Planning for Autonomous Agent
Architectures, pages 23-25, Orlando, FL, 1998.
[48] H. Gonzalez-Bafios and J.-C. Latombe. A Randomized Art Gallery Algorithm
for Sensor Placement. In Proceedings of the 17th Annual ACM Symposium on
Computational Geometry, pages 232-240, 2001.
[49] Philip Hall. The Distribution of Means for Samples of Size N Drawn from a
Population in Which the Variate Takes Values Between 0 and 1, All Such Values
Being Equally Probable. Biometrika, 19(3-4):240-245, 1927.
[50] Alastair Harrison and Paul Newman. High Quality 3D Laser Ranging Under General Vehicle Motion. In Proceedings of the IEEE Intl. Conference on
Robotics and Automation (ICRA), 2008.
[51] Susan Hert, Sanjay Tiwari, and Vladimir Lumelsky. A Terrain-Covering Algorithm for an AUV. Autonomous Robots, 3:91-119, 1996.
168
[52] Claude Holenstein, Robert Zlot, and Michael Bosse. Watertight Surface Reconstruction of Caves from 3D Laser Data. In Proceedings of the IEEE/RSJ Intl.
Conference on Intelligent Robots and Systems (IROS), pages 3830 -3837, 2011.
[53] G.A. Hollinger, B. Englot, F. Hover, U. Mitra, and G.S. Sukhatme. UncertaintyDriven View Planning for Underwater Inspection. In Proceedings of the IEEE
Intl. Conf. on Robotics and Automation (ICRA), pages 4884-4891, St. Paul,
MN, 2012.
[541 John Hopcroft, Deborah Joseph, and Sue Whitesides. Movement problems for
2-Dimensional Linkages. SIAM Journal on Computing, 13(3):610-629, 1984.
[551
Hugues Hoppe, Tony DeRose, Tom Duchamp, John McDonald, and Werner
Stuetzle. Mesh Optimization. In Proceedings of the 20th annual conference on
Computer graphics and interactive techniques, SIGGRAPH '93, pages 19-26,
New York, USA, 1993. ACM.
[56] Franz S. Hover, Ryan Eustice, Ayoung Kim, Brendan Englot, Hordur Johannsson, Michael Kaess, and John J. Leonard. Advanced Perception, Navigation
and Planning for Autonomous In-water Ship Hull Inspection. Intl. Journal of
Robotics Research, 31(12):1445-1464, 2012.
[57] A. Howard, D. F. Wolf, and G. S. Sukhatme. Towards 3D Mapping in Large
Urban Environments.
In Proceedings of the IEEE/RSJ Intl. Conference on
Intelligent Robots and Systems (IROS), pages 419-424, 2004.
[58] Chen-Han Hsiao and Chieh-Chih Wang. Achieving Undelayed Initialization
in Monocular SLAM with Generalized Objects Using Velocity Estimate-based
Classification. In Proceedings of the IEEE Intl. Conference on Robotics and
Automation (ICRA), pages 4060-4066, 2011.
[59] D. Hsu, R. Kindel, J.C. Latombe, and S. Rock. Randomized Kinodynamic
Motion Planning with Moving Obstacles. Intl. Journal of Robotics Research,
21(3):233-255, 2002.
169
[60] D. Hsu, J.C. Latombe, and H. Kurniawati. On the Probabilistic Foundations
of Probabilistic Roadmap Planning. In Proc. Int. Symp. on Robotics Research,
2005.
[61] David Hsu, Jean-Claude Latombe, and Rajeev Motwani. Path Planning in
Expansive Configuration Spaces. International Journal of Computational Geometry and Applications, 3(20-25):2719-2726, 1997.
[62] Shahram Izadi, David Kim, Otmar Hilliges, David Molyneaux, Richard Newcombe, Pushmeet Kohli, Jamie Shotton, Steve Hodges, Dustin Freeman, Andrew Davison, and Andrew Fitzgibbon. KinectFusion: Real-time 3D Reconstruction and Interaction Using a Moving Depth Camera. In Proceedings of
the 24th Annual ACM Symposium on User Interface Software and Technology,
pages 559-568. ACM, 2011.
[63] Matthew Johnson-Roberson, Oscar Pizarro, Stefan B. Williams, and Ian Mahon. Generation and Visualization of Large-scale Three-dimensional Reconstructions from Underwater Robotic Surveys.
Journal of Field Robotics,
27(1):21-51, 2010.
[64] Leslie Pack Kaelbling, Michael L. Littman, and Anthony R. Cassandra. Planning and Acting in Partially Observable Stochastic Domains. Artificial Intelligence, 101:99-134, 1998.
[65] M. Kaess, A. Ranganathan, and F. Dellaert. iSAM: Incremental Smoothing
and Mapping. IEEE Trans. Robotics, 24(6):1365-1378, Dec 2008.
[66] S. Karaman and E. Frazzoli. Optimal Kinodynamic Motion Planning Using
Incremental Sampling-based Methods. In Proceedings of the 49th IEEE Conference on Decision and Control, 2010.
[67] S. Karaman and E. Frazzoli. Sampling-based Algorithms for Optimal Motion
Planning. Intl. Journalof Robotics Research, 30(7):846-894, 2011.
170
[68] Lydia E. Kavraki, M. N. Kolountzakis, and Jean-Claude Latombe. Analysis of Probabilistic Roadmaps for Path Planning. In Proceedings of the IEEE
Intl. Conf. on Robotics and Automation (ICRA), pages 3020-3026, Minneapolis,
MN, 1996.
[69] Lydia E. Kavraki, P. Svestka, Jean-Claude Latombe, and M. Overmars. Probabilistic Roadmaps for Path Planning in High Dimensional Configuration Spaces.
IEEE Transactions on Robotics and Automation, 12(4):566-580, 1996.
[70] Michael Kazhdan, Matthew Bolitho, and Hugues Hoppe. Poisson Surface Reconstruction. In Proceedings of the Fourth EurographicsSymposium on Geometry Processing, 2006.
[71] 0. Khatib. Real-time Obstacle Avoidance for Manipulators and Mobile Robots.
Intl. Journal of Robotics Research, 5(1):90-98, 1986.
[721 K. Konolige and M. Agrawal. FrameSLAM: From Bundle Adjustment to RealTime Visual Mapping. IEEE 'rans. Robotics, 24(5):1066-1077, 2008.
[731
Kurt Konolige, Motilal Agrawal, Robert C. Bolles, Cregg Cowan, Martin Fischler, and Brian Gerkey. Outdoor Mapping and Navigation Using Stereo Vision.
In Proceedings of the InternationalSymposium on ExperimentalRobotics, 2006.
[74] H. Kurniawati and D. Hsu. Workspace-based Connectivity Oracle: An Adaptive
Sampling Strategy for PRM Planning. In Algorithmic Foundations of Robotics
VII. Springer-Verlag, 2006.
[751 H. Kurniawati, D. Hsu, and W. S. Lee. SARSOP: Efficient Point-based POMDP
Planning by Approtimating Optimally Reachable Belief Spaces. In Proceedings
of Robotics: Science and Systems (RSS), 2008.
[76] H. Kurniawati, J.C. Schulmeister, T. Bandyopadhyay, G. Papadopoulos, F.S.
Hover, and N. M. Patrikalakis. Infrastructure for 3D Model Reconstruction of
Marine Structures. In Proceedings of the Int. Offshore and Polar Engineering
171
Conference, InternationalSociety of Offshore and Polar Engineers (ISOPE),
2011.
[77] Joseph Louis Lagrange.
Theorie des fonctions analytiques: contenant les
principes du calcul diffirentiel, di4gagds de toute consideration d'infiniment petits, d'ivanouissans, de limites et de fluxions, et riduits & l'analyse alg6brique
des quantitis finies. Mme. Ve. Courcier, 1813.
[781 Eric Larsen, Stefan Gottschalk, Ming C. Lin, and Dinesh Manocha. Fast Proximity Queries with Swept Sphere Volumes. Technical report, University of North
Carolina, 1999.
[79] Jean-Claude Latombe. Robot Motion Planning. Kluwer Academic Publishers,
Norwell, MA, USA, 1991.
[80] Steven M. Lavalle. Rapidly-Exploring Random Trees: A New Tool for Path
Planning. Technical report, Iowa State University, 1998.
[81] Steven Michael LaValle. Planning Algorithms. Cambridge University Press,
New York, 2006.
[82] Tae-Seok Lee, Jeong-Sik Choi, Jeong-Hee Lee, and Beom-Hee Lee. 3D Terrain
Covering and Map Building Algorithm for an AUV.
In Proceedings of the
IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), pages 44204425, Piscataway, NJ, USA, 2009.
[83] J. C. Leedekerken, M. F. Fallon, and J. J. Leonard. Mapping Complex Marine Environments with Autonomous Surface Craft. In Proceedings of the Intl.
Symposium on Experimental Robotics (ISER), Delhi, India, December 2010.
[84] Leonard, How, Teller, Berger, Campbell, Fiore, Fletcher, Frazzoli, Huang,
Karaman, Koch, Kuwata, Moore, Olson, Peters, J Teo, R Truax, Walter, Barrett, Epstein, Maheloni, Moyer, Jones, Buckley, Antone, Galejs, and Krishnamurthy. A perception driven autonomous urban vehicle. Journal of Field
Robotics, 25(10), 2008.
172
[85] Kenneth Levenberg. A Method for the Solution of Certain Non-Linear Problems
in Least Squares. Quarterly of Applied Mathematics, 2:164-168, 1944.
[861 Kuen Han Lin and Chieh Chih Wang. Stereo-based Simultaneous Localization,
Mapping and Moving Object Tracking. In Proceedings of the IEEE/RSJ Intl.
Conference on Intelligent Robots and Systems (IROS), pages 3975-3980, 2010.
[87] Z. Littlefield, Y. Li, and K. E. Bekris. Efficient Sampling-based Motion Planning
with Asymptotic Near-Optimality Guarantees for Systems with Dynamics. In
Proceedings of the IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems
(IROS), 2013.
[88] T. Lozano-Perez. Spatial Planning: A Configuration Space Approach. IEEE
Transactions on Computers, C-32(2):108-120, 1983.
[89] Vladimir J. Lumelsky and Alexander A. Stepanov. Path-planning Strategies for
a Point Mobile Automaton Moving Amidst Unknown Obstacles of Arbitrary
Ahape. Algorithmica, 2:403-430, 1987.
[90] M. Montemerlo, S. Thrun, D. Roller, and B. Wegbreit. FastSLAM 2.0: An
improved particle filtering algorithm for simultaneous localization and mapping that provably converges. In Proceedings of the 18th International Joint
Conference on Artificial Intelligence, pages 1151-1156, 2003.
[91] R.A. Newcombe and A.J. Davison. Live Dense Reconstruction with a Single
Moving Camera. In Proceedings of the IEEE Int. Conference Computer Vision
and Pattern Recognition, pages 1498 -1505, 2010.
[92] P. Newman, G. Sibley, M. Smith, M. Cummins, A. Harrison, C. Mei, I. Posner,
R. Shade, D. Schroter, L. Murphy, W. Churchill, D. Cole, and I. Reid. Navigating, Recognizing and Describing Urban Spaces With Vision and Laser. Intl.
Journalof Robotics Research, 28(11-12):1406-1433, 2009.
[93] P.M. Newman. MOOS - A Mission Oriented Operating Suite. Technical Report
OE2003-07, Department of Ocean Engineering, MIT, 2003.
173
[94] M. A. H. Newton, J. Levine, and M. Fox. Genetically Evolved Macro-actions in
Al Planning Problems. In Proceedingsof the 24th Workshop of the UK Planning
and Scheduling Special Interest Group (PlanSIG 2005), pages 163-172, 2005.
[951 A. Niichter, K. Lingemann, J. Hertzberg, and H. Surmann.
6D SLAM-3D
Mapping Outdoor Environments. Journal of Field Robotics, 24(8-9):699-722,
2007.
[96 Andreas Nuchter, Stanislav Gutev, Dorit Borrmann, and Jan Elseberg. Skylinebased Registration of 3D Laser Scans. Geo-spatialInformation Science, 14:8590, 2011.
[97] Colm Odunlaing and Chee K. Yap. A Retraction Method for Planning the
Motion of a Disc. Journal of Algorithms, 6(1):104 -111, 1985.
[98] Timo Oksanen and Arto Visala. Coverage Path Planning Algorithms for Agricultural Field Machines. Journal of Field Robotics, 26(8):651-668, 2009.
[991 Joseph O'Rourke. Art Gallery Theorems and Algorithms. The International
Series of Monographs on Computer Science. Oxford University Press, New York,
NY, 1987.
[100] Christos H. Papadimitriou. Computational Complexity. Addison Wesley, 1994.
[101] G. Papadopoulos, H. Kurniawati, and N. M. Patrikalakis. Analysis of Asymptotically Optimal Sampling-based Motion Planning Algorithms for Lipschitz
Continuous Dynamical Systems. arXiv.org, 12 May 2014.
[102] G. Papadopoulos, H. Kurniawati, and N. M. Patrikalakis. Asymptotically Optimal Inspection Planning using Systems with Differential Constraints. In Proceedings of the IEEE Intl. Conf. on Robotics and Automation (ICRA), pages
4126-4133, May 2013.
[103] G. Papadopoulos, H. Kurniawati, and N. M. Patrikalakis. Asymptotically Optimal Inspection Planning using Systems with Differential Constraints. In Pro174
ceedings of the IEEE Intl. Conference on Robotics and Automation (ICRA),
pages 4126-4133, May 2013.
[104] G. Papadopoulos, H. Kurniawati, and N. M. Patrikalakis.
Asymptotically
Optimal Inspection Planning using Systems with Differential Constraints for
Complex 3D Structures. IEEE 7rans. Robotics, 2014. to be submitted by june
30th.
[105] G. Papadopoulos, H. Kurniawati, and N. M. Patrikalakis. Analysis of Asymptotically Optimal Sampling-based Motion Planning Algorithms for Lipschitz
Continuous Dynamical Systems. Intl. Journal of Robotics Research, 2014. submitted.
[106] Georgios Papadopoulos, Maurice F. Fallon, John J. Leonard, and Nicholas M.
Patrikalakis.
Cooperative Localization of Marine Vehicles using Nonlinear
State Estimation. In IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems
(IROS), Taipei, Taiwan, 2010.
[107] Georgios Papadopoulos, Hanna Kurniawati, Ahmed Shafeeq Bin Mohd Shariff,
Liang Jie Wong, and Nicholas M. Patrikalakis. Experiments on surface reconstruction for partially submerged marine structures. Journal of Field Robotics,
31(2):225-244, 2014.
[108] Georgios Papadopoulos, Hanna Kurniawatit, Ahmed Shafeeq Bin Mohd Shariff,
Liang Jie Wong, and Nicholas M. Patrikalakis. 3D-surface Reconstruction for
Partially Submerged Marine Structures using an Autonomous Surface Vehicle.
In Proceedings of the IEEE/RSJ Intl. Conference on Intelligent Robots and
Systems (IROS), pages 3551-3557, 2011.
[109] Nicholas M. Patrikalakis, Gabriel Weymouth, Hanna Kurniawati, Pablo Valdivia y Alvarado, Tawfiq Taher, Rubaina Khan, Joshua C. Leighton, and Georgios Papadopoulos. Modeling and Inspection Applications of a Coastal Distributed Autonomous Sensor Network. In ASME OMAE InternationalConference on Ocean, Offshore and Arctic Enginnering, Rio de Janeiro, 2012.
175
[110] A. Perez, R. Platt, G.D. Konidaris, L.P. Kaelbling, and T. Lozano-P6rez. LQRRRT*: Optimal Sampling-Based Motion Planning with Automatically Derived
Extension Heuristics. In Proceedings of the IEEE International Conference on
Robotics and Automation, pages 2537-2542, May 2012.
[111] J.H. Reif. Complexity of the Mover's Problem and Generalizations. In 1979,
20th Annual Symposium on Foundations of Computer Science, pages 421-427,
1979.
[112] C. Roman and H. Singh. A Self-Consistent Bathymetric Mapping Algorithm.
Journal of Field Robotics, 24(1):23-50, 2007.
[113] Michael Roy, Sebti Foufou, and Frdric Truchetet. Mesh Comparison Using
Attribute Deviation Metric. Journal of Image and Graphics, 4:1-14, 2004.
[114] Mitul Saha, Gildardo Sanchez-Ante, Tim Roughgarden, and Jean claude
Latombe. Planning Multi-Goal Tours for Robot Arms. In Proceedings of the
IEEE Intl. Conf. on Robotics and Automation (ICRA), 2003.
[115] Ketan Savla, Emilio Frazzoli, and Francesco Bullo.
Traveling Salesperson
Problems for the Dubins Vehicle. IEEE Transactions on Automatic Control,
53(6):1378-1391, 2008.
[116] Sebastian Scherer, Joern Rehder, Supreeth Achar, Hugh Cover, Andrew Chambers, Stephen Nuske, and Sanjiv Singh. River Mapping from a Flying Robot:
State Estimation, River Detection, and Obstacle Mapping. Autonomous Robots,
33(1-2):189-214, 2012.
[117] Jacob T. Schwartz and Micha Sharir. On the Piano Movers Problem. II. General
Techniques for Computing Topological Properties of Real Algebraic Manifolds.
Advances in Applied Mathematics, 4(3):298 - 351, 1983.
[118] Florian Shkurti, loannis Rekleitis, and Gregory Dudek. Feature Tracking Evaluation for Pose Estimation in Underwater Environments. In Proceedings of
176
the 2011 IEEE Canadian Conference on Computer and Robot Vision, pages
160-167, May 2011.
[119] G. Sibley. Sliding Window Filters for SLAM. Technical report, University of
Southern California, Center for Robotics and Embedded Systems, 2006.
[120] G. Sibley, C. Mei, I. Reid, and P. Newman. Vast-scale Outdoor Navigation Using Adaptive Relative Bundle Adjustment. Intl. Journal of Robotics Research,
29(8):958-980, 2010.
[121] Hanumant Singh, Louis Whitcomb, Dana Yoerger, and Oscar Pizarro. Microbathymetric Mapping from Underwater Vehicles in the Deep Ocean. Computer
Vision and Image Understanding, 79(1):143 - 161, 2000.
[122] Jean-Jacques E. Slotine and Weiping Li.
Applied Nonlinear Control. NJ:
Prentice-Hall, Englewood Cliffs, 1991.
[123] R. Smith and P. Cheeseman. On the Representation and Estimation of Spatial
Uncertainty. Intl. Journal of Robotics Research, 5(4):56-68, 1986.
[124] R. Smith, M. Self, and P. Cheeseman. Estimating Uncertain Spatial Relationships in Robotics. Autonomous robot vehicles, Springer-Verlag, New York, USA,
1990.
[125] Z. Sun, D. Hsu, T. Jiang, H. Kurniawati, and J. Reif. Narrow Passage Sampling
for Probabilistic Roadmap Planners. IEEE 'hans. Robotics, 21(6):1105-1115,
2005.
[126] Russ Tedrake, Ian R. Manchester, Mark Tobenkin, and John W. Roberts. LQRTrees: Feedback Motion Planning via Sums of Squares Verification. Intl. Journal of Robotics Research, 29:1038-1052, 2010.
[127] I. Tena Ruiz, S. de Raucourt, Y. Petillot, and D.M. Lane. Concurrent Mapping
and Localization Using Sidescan Sonar. IEEE Journal of Oceanic Engineering,
29(2):442 - 456, 2004.
177
[128] S. Thrun, W. Burgard, and D. Fox. ProbabilisticRobotics. The MIT Press,
Cambridge, MA, 2005.
[129] S. Thrun, M. Montemerlo, D. Koller, B. Wegbreit, J. Nieto, and E. Nebot.
FastSLAM: An Efficient Solution to the Simultaneous Localization and Mapping Problem with Unknown Data Association. Journal of Machine Learning
Research, 4(3):380-407, 2004.
[1301 Sebastian Thrun, Wolfram Burgard, and Dieter Fox. A Real-time Algorithm for
Mobile Robot Mapping with Applications to Multi-robot and 3D Mapping. In
Proceedingsof the IEEE Intl. Conference on Robotics and Automation (ICRA),
pages 321-328, 2000.
[131] Chi Hay Tong and Timothy D. Barfoot. Three-Dimensional SLAM for Mapping
Planetary Work Site Environments.
Journal of Field Robotics, 29:381-412,
2012.
[132] Visual
Computing
Lab
ISTI
-
CNR.
Meshlab,
July
2010.
http://meshlab.sourceforge.net/.
[133] K. M. Wurm, A. Hornung, M. Bennewitz, C. Stachniss, and W. Burgard. OctoMap: A Probabilistic, Flexible, and Compact 3D Map Representation for
Robotic Systems. In Proc. of the ICRA 2010 Workshop on Best Practicein 3D
Perception and Modeling for Mobile Manipulation, Anchorage, AK, USA, May
2010.
[134] A. Yershova and S.M. LaValle. Improving Motion-Planning Algorithms by Efficient Nearest-Neighbor Searching. IEEE Trans. Robotics, 23(1):151-157, 2007.
[135] A. Zelinsky, R.A. Jarvis, J. C. Byrne, and S. Yuta. Planning Paths of Complete
Coverage of an Unstructured Environment by a Mobile Robot. In Proceedings
of International Conference on Advanced Robotics, pages 533-538, 1993.
[136] Huijing Zhao and Ryosuke Shibasaki. Reconstructing Textured CAD Model
of Urban Environment Using Vehicle-Borne Laser Range Scanners and Line
178
Cameras. In Proceedings of the Second International Workshop on Computer
Vision Systems, pages 284-297, London, UK, 2001. Springer-Verlag.
179