M A S T E R’S T H E S I S
2005:017 CIV
SOFIA SUNDBERG
Swept Areas and Collision Detection with Application
to Autonomous Vehicles
MASTER OF SCIENCE PROGRAMME
Luleå University of Technology
Department of Computer Science and Electrical Engineering
Division of Computer Science and Networking
Swept areas and collision detection with application to autonomous vehicles
Sofia Sundberg
January 24, 2005
Abstract
This thesis presents an algorithm for collision detection for an autonomous ar- ticulated vehicle following pregenerated paths in a mining environment. Two types of vehicles are studied. The tricycle vehicle and the articulated vehicle.
The characteristics of the mine is presented. A way of using these character-
istics is studied. An algorithm for collision detection using the swept area of
the vehicle following a pregenerated path is given. As proof of concept a small
implementation is also given along, with a few examples.
Contents
1 Introduction 3
1.1 Environments . . . . 4
1.2 Vehicles . . . . 4
1.3 Applications . . . . 5
1.4 Contribution . . . . 5
2 Environments 6 2.1 Our environment . . . . 6
3 Autonomous vehicles 8 3.1 The tricycle vehicle . . . . 8
3.2 The articulated vehicle . . . . 9
4 The boundary of a swept area and collision detection 11 4.1 The boundary of a swept area. . . 11
4.1.1 Computing the candidate curves . . . 13
4.2 Collision detection . . . 13
4.2.1 Triangulation . . . 14
4.2.2 Numerical issues . . . 15
4.3 Summary of the Algorithm . . . 16
5 Implementation 17 5.1 Future improvements . . . 18
6 Examples from the implementation 19 6.1 Example of the fixed triangulation of the mine . . . 19
6.2 A curve following example . . . 20
7 Conclusions 24
7.1 Future work . . . 24
Chapter 1
Introduction
There are many applications today where the use of autonomous vehicles are becoming important. Mining operations and different types of factory tasks are such applications. In environments where there is potential danger for the driver it is important to get the driver away from the vehicle. An alternative is then to operate the vehicle from a control room. This may, however, decrease the speed and accuracy in the control of the vehicle compared to when a skilled driver is driving the vehicle manually. This puts the focus towards even more autonomous vehicles. There are different possibilities for the control of autonomous vehicles.
One is for the vehicle to find its way around without knowing anything about the environment ahead except for the goal. This is the approach taken by Juneau et. al. [9]. Another possibility is when the vehicles are running independently along pregenerated paths. If the control system of the vehicle is good enough, the path is properly adjusted for the environment and the capability of the vehicle, the speed and accuracy can be as good as a manual driver or even better as mentioned by Ridley et. al. [10]. It may also be cheaper since the vehicle can operate almost 24/7 without taking a break and several vehicles can be monitored by a single operator instead of a driver in each vehicle.
This is how some of the mining operations at the LKAB mine in Kiruna is done. The mining trucks, called Load Haul Dumps, moving the ore from the blast site to the dump site are following pregenerated paths when moving between the two sites. The Load Haul Dump, LHD vehicle for short, are large articulated vehicles of the type described in Chapter 3. The loading is still done manually.
In general when paths for autonomous vehicles are generated, the issue of collision detection becomes important. For many applications there are no ob- stacles from above. Which allows us to only consider obstacles that the side of the vehicle can collide with. This simplifies the model to two dimensions. For other applications obstacles in all directions have to be considered, and we need a more general three dimensional model.
A collision can have different impact depending on the application. If the vehicle, or the environment, is damaged due to a collision it could become very expensive to repair. Even if there are no material damage, bumping into the obstacles will probably slow the vehicle down, and a collision free path would enable the vehicle to go faster.
Collisions can be detected by looking at the area that the vehicle will cover
when moving around. This is the swept area of the moving vehicle. If the swept area is overlapping any of the obstacles in the environment there is a collision.
When we know the path the vehicle is following beforehand, it can be checked for collisions and changed or discarded before the vehicle is actually using the path. This makes collision detection an important step in generating these paths without collisions.
In this thesis we have the following parts: In Chapter 2, how does the environment impact on the collision detection problem? We look closer at two different types of vehicles in Chapter 3. In Chapter 4 we suggest a collision detection algorithm for pregenerated paths in our specific environment with our specific vehicles. We have also done an implementation of parts of the algorithm. The implementation is presented in Chapter 5 and a few examples from the implementation is displayed in Chapter 6. Finally we have conclusions of our work in Chapter 7.
1.1 Environments
There are many types of environments where we would like to use autonomous vehicles with pregenerated paths. A possible environment would be on a fac- tory floor where an autonomous truck is supplying the different stations with material, or moving ready parts from one station to another. This type of en- vironment is studied by Åström [13]. Another in a metalliferous mine where a mining vehicle is transporting ore through the narrow tunnels [3, 10]. In these types of environments all obstacles are stationary and once the best possible path is computed there will not appear any better paths.
Other environments may not be as static as above. An example could be a forest machine moving timber from the area of felling to the main road. The obstacles could be the trees still standing. When the felling of the trees proceeds a new and better path could occur. We also have the issue of the felling machine which is a moving obstacle. This is a dynamic environment and pregenerated paths that never change may not be the best choice. Ringdahl [11] has studied this type of problem further.
This shows that the environment plays a big role when choosing how to maneuver the vehicle and how to chose paths. Depending on the type of envi- ronment we will also get different types of collision avoidance algorithms.
When we know the type of environment that we are working with we should take advantage of this. For instance in a mining environment with straight and narrow tunnels. This should be taken advantage of in our application and it is further investigated in Chapter 2.
1.2 Vehicles
There are many different vehicles to be considered. Different types of vehicles behave differently and the difficulty of collision detection could vary depending on the type of vehicle.
The simplest vehicle model looked into is the tricycle model vehicle, with a
steering wheel in front and two non steering wheels in the back. This is one of
the vehicles that are studied in this thesis, mainly because it is easy to model and extensively studied, among others by Allison et. al. [2].
Another type of vehicle is the articulated vehicle. This is a vehicle with two bodies connected with a joint. There are two non steering wheels at each body and steering is achieved by changing the angle at the connecting joint.
This type of vehicle is very common in industry applications because of its high maneuverability. It can negotiate corners within a smaller space than a tricycle vehicle of the same size. Altafini [3] has studied these advantages more closely.
The forest machine above and the LHD vehicle in the mine are both of this type. These two vehicle types are studied further in Chapter 3.
1.3 Applications
Applications for using the swept area (or swept volume) in collision detection are many, as mentioned above. Some industrial applications is the LHD-vehicle in the metalliferous mine or the truck on the factory floor.
Another application could be for virtual environments in simulation tools where the behavior of different vehicles are studied or paths for different appli- cations, for instance the industrial applications mentioned above, are generated.
Other types of virtual environments could be in computer games, where the movement of the objects have no limitations in the same sense as a real vehicle would have.
1.4 Contribution
Our application is in generating paths for the LHD vehicle used in the LKAB mine in Kiruna. The environment consists of the narrow mining tunnels con- nected to each other at junctions. The only obstacles are the mining walls. We study the environment and how to use the specific mining environment to our advantage when detecting collisions in the planning phase. We continue with studying the articulated LHD vehicle and how it can be modeled in a simple way by using the tricycle vehicle.
The main focus in this thesis it to detect collisions with the LHD vehicle and the mining walls when the LHD vehicle is following a given path. This is done by computing the swept area of the vehicle following the given path. When the boundary of the swept area is computed collisions are detected by finding intersections between the boundary of the swept area and the walls of the mine.
We give a collision detection algorithm using the swept area of a vehicle,
following a given path through a mine, which is more efficient than the brute
force solution.
Chapter 2
Environments
The real world contains many different environments where autonomous vehicles can be used. Basically there are two types of environments to consider when we are working with collision detection. One is the static environment where the obstacles are stationary. The other is the dynamic environment where the obstacles may change over time or even move around.
For the static environment pregenerated paths can easily be used. These paths can be checked for collisions before taken into use. After the best possible path has been computed there is no need to change the path over time. Since the environment is static there will not appear any new possibilities for a better path over time.
For the dynamic environment it is not very practical to have pregenerated, fixed paths. Since the environment is changing there could appear new possi- bilities for better paths over time. Collision detection can be impossible to do before the vehicle is actually moving around and a collision avoidance algorithm has to be used in real time.
2.1 Our environment
Figure 2.1: A map of a level in a metalliferous mine.
Our environment is a mining environment consisting of narrow tunnels con-
nected at junctions. An example is seen in Figure 2.1. There might be obstacles
in the middle of a tunnel, such as big rocks that have been dropped when trans-
porting the ore. These obstacles are removed over time and we do not need to consider them when generating the paths. The interest in our application is to generate static and collision free paths that will be used many times before generating a new path, see the examples in Chapter 6. This means that the dynamic obstacles as the rocks in the way will not be considered. The only obstacles considered are the walls of the mine. The narrow tunnels are straight and connected at junctions. Another characteristics is that the paths trough the mine will go through the tunnels from end to end without any unnecessary turns in the middle of the tunnel.
The main goal of the paths inside the mine is to generate them such that the vehicle can go as fast as possible. This is achieved by generating very wide curves so that the vehicle will not have to slow down too much when taking the curve. Although such curves yield higher speed they also make the vehicle go close to the walls of the mine. The issue of detecting and avoiding collisions becomes an important task when generating these paths.
An advantage of the mine is the characteristics of the tunnels. The mine is modeled as a polygon M with tunnel segments m i connected at junctions as monotone pieces. Piece m i of a polygon M is monotone to a line s such that when moving a line s ′ orthogonal to s over m i , s ′ will intersect at most two edges in m i at all times.
Most tunnel segments in our specific application will be monotone towards
either the x-axis or the y-axis. This is an important characteristic that should
be taken advantage of further on.
Chapter 3
Autonomous vehicles
When working with models of real autonomous vehicles there are a few things to consider. We have to consider the type of vehicle being modeled, the two types considered here are the tricycle vehicle and the articulated vehicle. The tricycle vehicle is chosen because it is easy to work with and extensively studied. The articulated vehicle is studied because of its use in various industry applications.
The important difference between these types of vehicles is mainly in their kinematic model and not in their shape, since two vehicles can vary very much in shape and still be of the same type.
We also have to consider the control system that is used. Control over an autonomous vehicle is usually issued over one or more reference points on the vehicle. These are set to follow a given path. Most control systems tries to minimize the error in distance between the reference points and the given path [7]. This usually means that there can be no adjustment before there is an error, which leads to the fact that the path followed by the reference points usually differs from the given path. If the error becomes big collision detection should be done on the real path followed and not the given path. If the error is minimal, collision detection can be done on the given path without risking the safety of the vehicle. With a given control system the expected path of the vehicle can be computed with very high accuracy.
When studying the movement of a vehicle we usually want to simplify the description of the movement as much as possible. Any type of continuous move- ment can be described as a translation and a rotation. At any instance in time a movement described like this can be reduced to pure rotation in a point called the instantaneous center of rotation. This point is denoted C t at time t. A special case occurs when the movement is pure translation. Then C t will be at infinity. The position of C t is of big importance when computing the swept area of a moving vehicle and it is used in Chapter 4.
3.1 The tricycle vehicle
The tricycle type vehicle has a steering wheel in front and two non-steering rear
wheels. The body of such a vehicle is rigid and we model it as a simple polygon,
V three . It can be proven that the instantaneous center of rotation, C t , always
lies on the line through the rear axis. In fact, it lies where the line orthogonally
Figure 3.1: An non autonomous tricycle vehicle.
C t
p ref l
Figure 3.2: The tricycle vehicle.
through the center of the steering wheel and the line through the rear axis meet [2]. We use a reference point that is following a given path, p ref , that is at the center of the rear axis. The orientation of the vehicle is such that the axis l is always following the tangent line of the given path through p ref . For a tricycle vehicle to be able to follow a given path we need the curve describing the path to be continuous with a continuous first derivate. This is because the movement of l has to be continuous, and it is following the tangent line of the curve.
3.2 The articulated vehicle
The second vehicle is the articulated vehicle, for instance an LHD vehicle in mining operations. This vehicle has four non-steering wheels, two in the front and two in the back. The front and the rear of the vehicle is joined together by a joint. Steering is achieved by changing the angle, α, of the joint. The body of the vehicle has two rigid parts that are modeled as two simple polygons, one for the front part of the vehicle and one for the rear part. These two are denoted V Afront and V Arear respectively.
The instantaneous center of rotation C t is where the lines through the front
Figure 3.3: An articulated vehicle.
C t
l 1
l 2
α p ref1
p ref2
Figure 3.4: The articulated vehicle.
and rear axis intersect [3, 10].
The control system for this type of vehicle is usually more complex than for the tricycle vehicle and it can use several different reference points. In our model we use two different reference points, p ref1 and p ref2 , one for the front and one for the rear, located at the center of each wheel axis. Even if these are not the points used by the control system they are a good choice for us since they are along the same line as C t . When given the control system we can compute the actual paths for p ref1 and p ref2 respectively. Note that the actual path for p ref1 can differ from the actual path for p ref2 and both these paths can differ from the given path.
The direction of the front and the rear of the vehicle is such that l 1 and l 2
is following the tangent line of real path for p ref1 and p ref2 respectively. When
the real paths of p ref1 and p ref2 are computed, the front and rear of the vehicle
can be modeled as two tricycle vehicles independent of each other. This allows
collision detection for the front and the rear of the vehicle separately.
Chapter 4
The boundary of a swept area and collision detection
When generating paths for a vehicle in an underground mine the task of collision detection is very important. It would be very expensive if the mining vehicle collided with the wall of the mine.
Figure 4.1: Two examples of how a swept area could look like.
The swept area of a vehicle is the area covered by the vehicle following a given path. Figure 4.1 gives two examples of how a swept area could look like.
Given the model M of a mine and the swept area of the vehicle a collision occurs when the swept area goes outside the boundaries of M. By finding the intersection between the boundary of M and the boundary of the swept area a collision can be reported.
4.1 The boundary of a swept area.
We have the following model, the vehicle model is the tricycle model and it is
represented by a simple polygon denoted V and the path is represented by a
continuous curve denoted f(t). The control system for a real vehicle usually
tries to get a reference point on the vehicle to follow a given path. The vehicle
V contains a fixed reference point denoted p ref . Regardless of where along the
curve f(t) the vehicle is located, p ref is always on f(t). Moreover V has a given
direction which is always parallel to the tangent of f(t) at p ref . The swept area
is denoted as SA = S(V, f(t), p ref ) and is defined by all curves the points of V is following when p ref is following f(t).
Let boundary of an area x be denoted by ∂(x), the boundary of the vehicle, V , is denoted by ∂(V ) and finally the boundary of the swept area, SA, is denoted by ∂(SA). The environment, the mine, is modeled as a polygon M. The interior of M is free space and a collision occurs when ∂(M) is intersected by ∂(SA).
We therefore start by finding ∂(SA).
Sambandan et. al. [12] describes the general case of how to find the boundary of the swept area of a moving polygon. We use their results in our specific case. They state that the boundary of the swept area, ∂(SA), is defined by the boundary of V ; a point within V can not possibly lie on ∂(SA) . This means that only the vertices and edges of V has to be considered. When tracing the vertices they will generate curves, and the edges will generate swept areas.
Sambandan et.al. proves that the only vertices of interest are the convex vertices.
When tracking the edges of V a swept area for each edge is generated. For a moving object there is at each instant moment in time a point where the movement of the object can be reduced to pure rotation. This point is called the instantaneous center of rotation, denoted C t . For pure translation C t is located at infinity. When generating the boundary of the swept area of an edge of V there are at most three candidate points that could be on the boundary.
That is the two end points and a third point moving along the edge. The moving point is the orthogonal projection of C t onto the the edge. The only exception to this is when the movement is pure translation and the edge is parallel to the direction of the movement, the entire edge can then be on the boundary of the swept area. Figure 4.2 illustrated this special case, with a line segment following the dashed curve at a fixed distance parallel to the tangent line of the curve.
The solid lines are the candidate curves and the dotted lines are samples of the line segment. At the inflection point on the curve the entire line segment is on the boundary of the swept area. For the entire ∂(SA), the concave vertices are still discarded.
−2 −1 0 1 2 3 4
−10
−8
−6
−4
−2 0 2 4 6 8 10
(m)
(m)
Figure 4.2: The swept area of a line segment following a curve. Here including
the special case of pure translation where an entire edge has to be checked.
This gives a number of candidate points for which we can compute the candidate curves. For an n-sided V there are at most 2n candidate curves for
∂(SA), the curves generated by the fixed points at the convex vertices and the curves generated by the moving points over the edges. We also have the special case of pure translation where the entire edge have to be considered. Further more, at the start and end moments entire V has to be considered. This gives us a number of candidate line segments. The outer boundary of these curves and line segments gives the ∂(SA) and how to find this explicitly is described by Sambandan et. al. [12].
4.1.1 Computing the candidate curves
c i
d α p ref
Figure 4.3: Following a candidate point.
Figure 4.3 shows the relation between the reference point on the vehicle p ref and a candidate point c i . The curve of a point c i on the vehicle V where p ref is following the path
f (t) =
x(t) y(t)
(4.1) is
g i (t) = f (t) + t(f )α(t)d(t) (4.2) where
t(f ) =
dx dy dt dt
1
q
dx dt
2 + dy dt 2
(4.3)
and
α(t) =
cos v(t) sin v(t)
− sin v(t) cosv(t)
(4.4) is the rotation matrix. Both v(t) and d(t) are fixed in t if the point c i is a fixed point on the vehicle. The angle v(t) is the angle between the tangent line through p ref on f(t) and the line through p ref and c i and d(t) is the distance between p ref to c i .
4.2 Collision detection
The environment in which we want to detect collisions is a mining environment.
To detect collisions with the walls of the mine when a vehicle V is following f (t) it is sufficient to detect intersections with ∂(SA) and the walls of the mine,
∂(M ). The brute force method would be to check every edge in ∂(M ) against
∂(SA). This is time consuming work and even unnecessary if the edge in the
wall is far away from ∂(SA). We want a way to check for collisions locally without having to check the entire mine when driving through a small section of the mine.
We make it possible to perform local collision detection by triangulating the mine M. A triangulation of a polygon P is a subdivision of P into triangular pieces such that diagonals are inserted between the vertices in P . By triangu- lating M the mine is divided into smaller portions that can be used to check
∂(SA) against ∂(M ) locally. The collision detection is done as follows. Select a point a on ∂(SA), find the triangle ∆ 0 where a is located. Let a move along
∂(SA) until an intersection with the boundary of ∆ 0 is found. If the triangle edge intersected is an edge in ∂(M) a collision is detected. Otherwise continue moving a along ∂(SA) into ∆ 1 on the other side of the intersected edge. In the same manner we continue to ∆ 2 and up to ∆ n to checking for collisions. When a has been moved all the way along ∂(SA) all collisions with the walls of the mine have been detected and reported.
4.2.1 Triangulation
What kind of triangulation is needed and how is it computed? Any triangulation will work, in fact, for the collision detection method above, but there are some things to consider when choosing the triangulation method.
The first issue is the shape of the triangles in the triangulation. In a trian- gulation with many long and narrow triangles there can be numerical problems in detecting in which triangle a given point is located. The result can be that several triangles hold the given point, or that no triangle will hold the point, even if the point obviously is in the triangulation. Therefore we want to avoid the long and narrow shape of the triangles if possible. This is taken care of in the Delaunay triangulation [6] where all points are connected to their closest neighbors, which maximizes the minimum angle in any triangle. This minimizes the numerical problems that could occur in the long and narrow triangles.
Another way of looking at the problem of finding the intersections in the triangles is to find the order in which the triangles are entered. That is to find the sleeve of ∂(SA) going through the triangulation. A sleeve is the set of triangles ordered in the same order as they are intersected by ∂(SA). If the sleeve is known we know what edges in the triangulation that is intersected and at what edges there are collisions. The characteristics of the mine is such that we have monotone tunnels connected at junctions. We also know that the movement of V is such that it goes through the tunnels from end to end. If the diagonals of the triangulation, that is the triangle edges that are not part of the wall of mine, would cross the tunnel at all times we would know the order in which the triangles should be entered. For each triangle there is one edge where it is entered and one edge where it is exited. The third edge is left for collision detection. In Figure 4.4 there is a piece of the mine triangulated like this. The solid lines are the walls of the mine and the dotted lines are the diagonals of the triangulation.
Another issue is the time complexity it takes to triangulate the mine. There
exists an algorithm by Chazelle [4] for general triangulation of a polygon on
O(n) time where n is the number of vertices. There also exists an Delaunay
triangulation of a polygon on O(n) time by Chin et. al. [5]. However, these
algorithms are difficult to implement, and there are no known implementations
Figure 4.4: A part of the mine triangulated.
today. Instead we take advantage of the actual shape of the mine that we are triangulating. The monotone tunnel segments m i gives us a much simpler O(n) time triangulation that is possible to implement since a monotone polygon can be triangulated in linear time [6]. What about the shape of this triangulation?
The algorithm follows both sides of the polygon and diagonals are inserted whenever possible. This could result in a triangulation where all diagonals crosses over the tunnel, but there is no guarantee for this. This algorithm could be modified so that all diagonals do cross the tunnel in the way we need them to. How to do this needs further investigation.
4.2.2 Numerical issues
For a simple description of ∂(SA) the intersections with the triangles can easily be computed using exact analytical methods, that is when ∂(SA) is composed of line segments and curves composed of polynomials of at most degree four.
Otherwise numerical methods have to be used. In most cases this is not a problem and all collisions will be found. However, there could be problems.
If there are no solution or if the solution is too far away from the start point Newton’s method [8] will not converge. The interval bisection method [8] have problems with finding the initial interval [a, b] where the solution exists. The initial interval could be found by taking small steps along ∂(SA) in the trian- gulation and when the two end points on the interval lay in different triangles there is a solution in that interval. If ∂(SA) exits and reenters the same triangle in one small step, these two intersections will never be found. This could result in problems finding all collisions that occurs.
With the nice triangulation described above we will however have a some-
what better situation. Since the order in which the diagonals are intersected
by ∂(SA) is known Newton’s method should converge when trying to find these
intersections. If it does not the only reason is that the start point of the method
is too far away from the intersection. The solution is to move the start point
for Newton’s method forward along ∂(SA). When we get close enough to the
intersection we will get convergence and find the intersection. There are two
edges of any given triangle that we know we have to intersect and a third one that is part of the mining wall. If this edge is intersected we have a collision. By starting inside the triangle we hope to be close enough to a possible intersection with the wall of the mine and Newton’s method is used to check for a collision.
If the solution does not converge we conclude that there is no collision. This does improve our possibilities to find all collisions but unfortunately there is no guarantee that all collisions are found, and this is a question that needs further investigation.
4.3 Summary of the Algorithm
The algorithm consists of three steps all described above.
1. Preprocessing of the mine. The map of the mine is divided into monotone tunnels that are triangulated. The triangulation is described in section 4.2.1.
2. Finding the swept area of the vehicle following a given path through the mine as described in section 4.1.
3. Finally, the collision detection is done by computing all intersections be-
tween the tunnel walls and the boundary of the swept area as described
in section 4.2.
Chapter 5
Implementation
The implementation is a simplified model implemented in matlab [1]. It gives a proof of concept that the ideas in the algorithm presented earlier in this thesis works.
The first simplification is in the vehicle model. We use the tricycle vehicle.
The only candidate points considered are the fixed ones, which is sufficient for a rectangular vehicle. For a rectangular vehicle there are six fixed points. Four are the corners of the vehicle and the last two are on the side of the vehicle that are orthogonal towards the rear axis of the vehicle. The instantaneous center of rotation C t will always lie on the line through the rear axis. This means that the orthogonal projection of C t on these two sides will be two fixed points on the side of the vehicle. The cases not considered here is pure translation, the start and end moments and also when the rotation of the vehicle is such that C t can be orthogonally projected onto the front and rear sides of the vehicle.
The preprocessing of the mine is done with the Delaunay triangulation func- tion delaunay(x, y), which is build into Matlab. This function takes as argu- ments x that are the x-coordinates of the vertices in the mine and y that are the y-coordinates of the mine. This triangulation takes a set of points and per- form the Delaunay-triangulation regardless of the order of the points. This is a difference from the original algorithm where the outer boundary of the triangu- lation is the actual walls of the mine. The walls of the mine does not even have to be included in this triangulation. For the case of detection collisions this is a problem. It is solved such that the edge between (x i , y i ) and (x i+1 , y i+1 ) is considered to be a part of the wall of the mine. If there is no edge between (x i , y i ) and (x i+1 , y i+1 ) in the triangulation a new point ( x
i+x 2
i+1, y
i+y 2
i+1) is in- serted between (x i , y i ) and (x i+1 , y i+1 ). The triangulation is then recomputed and checked again until all walls of the mine are included as triangle edges in the triangulation. An example of this is shown in Chapter 6.
The collision detection is not performed on the actual swept area, as stated in the algorithm, but on the curves of the candidate points for the swept area.
If none of the candidate points intersects the wall of the mine the path followed by the vehicle is considered to be collision free.
In our implementation the collision detection for a candidate curve is simpli-
fied and performed as follows. The candidate curve is discretisized by sampling
a number of points on the curve. The set of triangles that the curve is going
through, the sleeve, is found by using the triangle search method in matlab,
tsearch(x, y, T RI, a, b) where x and y are the coordinates for the wall of the mine, T RI is the triangulation matrix, and a and b are the coordinates for the sampled curve. This method returns the set of triangles where the sampled points are located as an array of indexes into T RI. Since there are many sam- ple points following each other located in the same triangle the set of triangles is then cleaned up so that the resulting sleeve has one entry for each time a triangle is entered.
When given the sleeve the edges crossed in the triangulation are computed.
If the intersected edge is part of the wall of the mine a collision is reported. If the sampling is to coarse so that a triangle is missed by the sampled points, such that two triangles following each other in the sleeve has no edge in common, this will also be detected and reported. If the triangulation is missed entirely we are sure to have a collision and this is also reported.
The case where a triangle is exited and reentered between two sample points will not be detected, and a solution for this has to be found to be able to guar- antee that all collisions are found. This is also a difference from the algorithm where the explicit intersections with the triangles are computed.
5.1 Future improvements
For future improvements the implementation should be able to handle the spe- cial cases for pure translation and the start end stop moments. The candidate points moving over the edges should also be handled.
Where the step size is obviously to large the program should not report this but recompute with a smaller step size. We would also like to compute the exact point for the intersection when we have a collision.
Even further ahead the actual boundary of the swept area should be com-
puted, instead of only computing the curves for the candidate points and work
with them. The triangulation should also be changed so that it minimizes the
various numerical problems. Preferably such that all diagonals in the triangu-
lation are crossing the mine tunnel.
Chapter 6
Examples from the implementation
We here give a few examples from the matlab implementation showing how it works. The examples are similar to the real application.
6.1 Example of the fixed triangulation of the mine
The left part of figure 6.1 shows the first Delaunay triangulation of the mining example. The dotted line is a part of the wall of the mine that is not included in the triangulation. This has to be fixed. As described earlier a new point on the middle of the line is inserted an the triangulation is recomputed. To the right in figure 6.1 we see the final iteration of the triangulation algorithm. Here the solid lines are the mining walls included in the triangulation and the dotted lines are the rest of the triangulation.
0 2 4 6 8 10 12 14 16 18 20
0 2 4 6 8 10 12 14 16 18
(m)
(m)
0 2 4 6 8 10 12 14 16 18 20
0 2 4 6 8 10 12 14 16 18
(m)
(m)
