P545

1
P545 Embedded Real-Time Systems

• Obstacle Avoidance
• by
• Yasir Ibrahim

2
Execution per clock tick

• One loop on the following
• Heading control
• Path following
• Laser reading
• Avoid obstacles
• Adjust speed
• Check corridor boundaries

3
Heading

• PID
• Continuous control
• Cart turns when the error in the feed back is not
  ZERO.
• Actually it is not even checking against zero, it
  continuously feeds back inputs and issues turn
  commands.

4
waypoint2
waypoint3
waypoint1
5
waypoint2
waypoint3
waypoint1
6
waypoint2
waypoint3
Supposed heading
Actual heading (compass)
waypoint1
7
waypoint2
waypoint3
Supposed heading
Actual heading (compass)
error
waypoint1
8
waypoint2
waypoint3
Supposed heading
Actual heading (compass)
Errorsupposed heading actual
heading Turn_command_inverseperror
P1/(smallest_turn_radiussuitable_angle) P1/(2.0
90.0)
error
waypoint1
9
waypoint2
waypoint3
Supposed heading
Actual heading (compass)
Errorsupposed heading actual
heading Turn_command_inverseperror
P1/(smallest_turn_radiussuitable_angle) P1/(2.0
90.0)
error
waypoint1
10
GPS follower

• When to pick the next waypoint.
• Two cases
• 1) The distance to the next waypoint is getting
  shorter. (till it is less than LBO)
• 2) The distance from the previous waypoint is
  getting longer. (till it is larger than distance
  between the two waypoints)
• The heading is working continuously.

11
waypoint2
waypoint3
waypoint1
12
waypoint2
waypoint3
length
waypoint1
13
waypoint2
waypoint3
LBO
length
waypoint1
14
waypoint2
waypoint3
LBO
r
length
r lt LBO Or d gt length
d
waypoint1
15
waypoint2
waypoint3
LBO
r
length
r lt LBO Or d gt length
d
waypoint1
16
waypoint2
waypoint3
r
LBO
d
length
r lt LBO Or d gt length
waypoint1
17
waypoint2
waypoint3
d
length
r lt LBO Or d gt length
waypoint1
18
waypoint2
waypoint3
waypoint1
19
waypoint2
waypoint3
waypoint1
20
Obstacle Avoidance

• Reading the laser device.
• Filtering the list (taking only what is in the
  carts path).
• Averaging the list of obstacles
• Adjusting the output obstacles LBO.
• Keeping history of obstacles, specially the
  previous one.

21
waypoint2
waypoint3
waypoint1
22
waypoint2
waypoint3
waypoint1
23
a
Obstacle GPS
c
ab(sin(alpha))
b
Outside when a gt c/2 O_lbo
alpha
GPS
24
Obstacle GPS
a
c
ab(sin(alpha))
b
Inside when altc/2 O_lbo
alpha
GPS
25
Obstacle GPS
a
c
ab(sin(alpha))
b
Inside when altc/2 O_lbo
alpha
GPS
26
a
Obstacle GPS
c
ab(sin(alpha))
b
Inside when altc/2 O_lbo
alpha
GPS
27
a
Obstacle1
Obstacle3
c
Obstacle2
Obstacle4
b
1 clock tick
28
a
Obstacle1
c
ab(sin(alpha))
b
Inside when altc/2 O_lbo
alpha
GPS
29
Obstacle2
a
c
ab(sin(alpha))
b
Inside when altc/2 O_lbo
alpha
GPS
30
Obstacle3
a
c
ab(sin(alpha))
b
Inside when altc/2 O_lbo
alpha
GPS
31
Obstacle4
a
c
ab(sin(alpha))
b
Outside when agtc/2 O_lbo
alpha
GPS
32
Obstacle1
Obstacle3
Obstacle2
c
1 clock tick
33
Obstacle1
Obstacle3
Obstacle2
c
1 clock tick
34
Obstacle1
Obstacle3
Obstacle2
c
AO0
AO?(Oi AO )/i
ALBO(LBOk-1dLBOk)/2
ddistance(AO,Oj)
1 clock tick
35
c
Obstacle 1
1 clock tick
36
waypoint2
waypoint3
waypoint1
37
waypoint2
waypoint3
Next heading of corridor
Current heading of corridor
waypoint1
38
waypoint2
waypoint3
Next heading of corridor
Current heading of corridor

• Next turn is left
• Optimal avoidance from the right of obstacles

waypoint1
39
waypoint2
waypoint3
Does not fit
waypoint1
40
waypoint2
waypoint3
Fits
waypoint1
41
waypoint2
waypoint3
Fits
Synthesized waypoint
waypoint1
42
waypoint2
waypoint3
Becomes my next waypoint
waypoint1
43
waypoint2
waypoint3
waypoint4
waypoint1
44
waypoint2
waypoint3
waypoint4
waypoint1
45
waypoint2
waypoint3
waypoint4
waypoint1
46
waypoint2
waypoint3
waypoint4
waypoint1
47
waypoint2
waypoint3
waypoint4
waypoint1
48
waypoint2
waypoint3
waypoint4
waypoint1
49
waypoint2
waypoint3
waypoint4
waypoint1
50
waypoint2
waypoint3
waypoint4
waypoint1
51
More obstacles

• Other configurations

52
waypoint2
waypoint3
waypoint4
waypoint1
53
waypoint2
waypoint3
waypoint4
waypoint1
54
waypoint2
waypoint3
waypoint4
waypoint1
55
waypoint2
waypoint3
waypoint4
waypoint1
56
waypoint2
waypoint3
waypoint4
waypoint1
57
waypoint2
waypoint3
waypoint4
waypoint1
58
waypoint2
waypoint3
waypoint4
waypoint1
59
waypoint2
waypoint3
waypoint1
60
waypoint2
waypoint3
waypoint4
waypoint1
61
waypoint2
waypoint3
waypoint4
waypoint1
62
waypoint2
waypoint3
waypoint4
waypoint1
63
waypoint2
waypoint3
waypoint4
waypoint1
64
waypoint2
waypoint3
waypoint4
waypoint1
65
waypoint2
waypoint3
waypoint4
waypoint1
66
waypoint2
waypoint3
Does not fit
waypoint4
waypoint1
67
waypoint2
waypoint3
Fits
waypoint4
waypoint1
68
waypoint2
waypoint3
Fits
waypoint5
waypoint4
Synthesized waypoint
waypoint1
69
waypoint2
waypoint3
Becomes my next waypoint
waypoint5
waypoint4
waypoint1
70
waypoint2
waypoint3
Becomes my next waypoint
waypoint5
waypoint4
If overlaps with my current waypoint, replace.
waypoint1
71
waypoint2
waypoint3
Becomes my next waypoint
waypoint5
waypoint4
If overlaps with my current waypoint, replace.
waypoint1
72
waypoint2
waypoint3
Becomes my next waypoint
waypoint5
waypoint4
If overlaps with my current waypoint, replace.
waypoint1
73
waypoint2
waypoint3
Becomes my next waypoint
waypoint5
waypoint4
If overlaps with my current waypoint, replace.
waypoint1
74
waypoint2
waypoint3
waypoint6
Becomes my next waypoint
waypoint5
waypoint1
75
waypoint2
waypoint3
waypoint6
Becomes my next waypoint
waypoint5
waypoint1
76
waypoint2
waypoint3
waypoint6
Becomes my next waypoint
waypoint5
waypoint1
77
waypoint2
waypoint3
STOP
STOP
waypoint6
Becomes my next waypoint
waypoint5
waypoint1
78
waypoint2
waypoint3
waypoint4
waypoint1
79
waypoint2
waypoint3
waypoint4
waypoint1
80
waypoint2
waypoint3
fits
waypoint4
waypoint1
81
waypoint2
waypoint3
waypoint5
Synthesize waypoint
waypoint4
waypoint1
82
waypoint2
waypoint3
waypoint5
Synthesize waypoint
waypoint4
Average waypoint4 and waypoint5
waypoint1
83
waypoint2
waypoint3
waypoint5
Synthesize waypoint
waypoint4
Average waypoint4 and waypoint5
waypoint1
84
waypoint2
waypoint3
waypoint5
Synthesize waypoint
waypoint4
Average waypoint4 and waypoint5
waypoint1
85
waypoint2
waypoint3
waypoint5
Synthesize waypoint
waypoint4
Average waypoint4 and waypoint5
waypoint1
86
waypoint2
waypoint3
waypoint5
Synthesize waypoint
waypoint4
Average waypoint4 and waypoint5
waypoint1
87
waypoint2
waypoint3
waypoint5
Synthesize waypoint
waypoint4
Average waypoint4 and waypoint5
waypoint1
88
Staying inside corridor

• Synthesized collision bubble.
• How far it is synthesized depends on carts
  speed.
• It can be used to control speed (accelerate,
  decelerate, and stop) and help in heading control
  (pushing bigger error to the heading control to
  force on turning if the cart is going out the
  corridor).

89
waypoint2
waypoint3
waypoint1
90
?
91
It Is a synthesized point. Same as if it were a
synthesized waypoint Or an obstacle.
?
92
It Is a synthesized point. Same as if it were a
synthesized waypoint Or an obstacle.
?
Speed
93
It Is a synthesized point. Same as if it were a
synthesized waypoint Or an obstacle.
?
Speed
(D)istance from GPS to the carts font
94
It Is a synthesized point. Same as if it were a
synthesized waypoint Or an obstacle.
How far Speed/2.0D
Speed
(D)istance from GPS to the carts font
95
It Is a synthesized point. Same as if it were a
synthesized waypoint Or an obstacle.
How far Speed/2.0D
Speed 3.0 m/s
(D)istance from GPS to the carts font
1.6764 m
96
It Is a synthesized point. Same as if it were a
synthesized waypoint Or an obstacle.
How far Speed/4.0D How far 3.0/4.01.6764
2.4264 m From the GPS. Or How far 2.4264
1.6764 0.75m From the front of the cart.
Speed 3.0 m/s
(D)istance from GPS to the carts font
1.6764 m
97
waypoint2
waypoint3
0.5
2
waypoint1
98
waypoint2
waypoint3
0.75
3
waypoint1
99
waypoint2
waypoint3
1.0
4
The cart is going out
waypoint1
100
waypoint2
waypoint3
1.0
4
The cart is going out
Signal to slow down
waypoint1
101
waypoint2
waypoint3
0.75
3
Slowed down
waypoint1
102
waypoint2
waypoint3
Slowed down
waypoint1
103
waypoint2
waypoint3
Slowed down
waypoint1
104

• Also,
• Collision bubble can be used to know if
  the cart is going out from the side of corridor.

105
waypoint2
waypoint3
waypoint1
106
waypoint2
waypoint3
waypoint1
107
Bubble LBO
a
ab(sin(alpha))
b
Outside when a Bubble LBO gt LBO
alpha
LBO
108
Bubble LBO
a
ab(sin(alpha))
b
Outside when a Bubble LBO gt LBO
alpha
LBO
109
Bubble LBO
a
b
ab(sin(alpha))
Inside when a Bubble LBOlt LBO
alpha
LBO
110
Path planning

• At each cycle, an algorithm iterates through all
  the waypoints and obstacles together.
• The algorithm adds two more waypoints at each
  existing waypoint. One before and one after the
  waypoint.
• The algorithm synthesizes the best waypoint
  around obstacles.

111

• Previous Labs

112
(No Transcript)
113
Safety
114

• try
• adjust_speed()
• if (signal_stop False)
• read_laser()
• follow_path()
• if(panicTrue)
• self.which_side_of_obstacle()
  self.synthesize_waypoint()
• self.put_synthesized_waypoint()
• self.panicFalse
• catch

115

• Default value for speed. Always storing the
  previous value.
• If one of the following sensors are providing
  unacceptable values (same value for along time or
  empty values), exception is thrown
• GPS, Compass, or Laser.

116

• TheEND