Autonomous vehicle navigation is a very popular research area in the vision and control field. Based on Prof. Dickmanns' philosophy, we implement a navigation algorithm on thc small robot. The robot can rely on its eyes (the camera mounted on thc top of the robot) and control its wheels to walk through the sub-basement hallways of Caltech Moore Lab building. The speed we achieve is robot's mechanical maximum speed 0.5 m/s

Feng, Xiaolin

Liu, Wenhai

English

Caltech Authors - Main

CDS 
TECHNICAL, MEMORANDUM NO. CIT-CDS 97-009 
June, 1997 
"Robot Autonomous NavigationT9 
Xiaoliin Feng and Wenhai kirr 
Control and Dynamical Systems 
California Institute of Technology 
Pasadena, California 9 1 125 
Robot Autonomous Navigation 
Xiaolin Feng 
Wenhai Liu 
California In s t i t~~ te  of Technology 
Pasadena, CA 91125 
June 2nd, 1997 
CONTENTS 1 
Contends 
1 Introduction 1 
2 Robot Hardware System 1 
. . . . . . . . . . . . . . . . . . . . . . . . . . .  2.1 Motor System 1 
. . . . . . . . . . . . . . . . . . . . . . .  2.2 Sensory Information 2 
. . . . . . . . . . . . . . . . . .  2.2.1 Visual Camera Sensor 2 
. . . . . . . . . . . . . . . . . . . . . .  2.2.2 Bumper Sensor 2 
. . . . . . . . . . . . . . . . . . . . . .  2.2.3 Robot's Status 2 
. . . . . . . . . . . . . . . .  2.3 On-board Computational System 3 
3 Geometric Analysis 3 
4 Image Analysis 5 
. . . . . . . . . . . . . . . . . . . . . . . . . .  4.1 Edge Detection 5 
. . . . . . . . . . . . . . . . . . . . .  4.2 Focal Length Calibration 6 
5 Autonomous Navigation 6 
. . . . . . . . . . . . . . . . . . . . . . . . . . . .  5.1 Initialization 8 
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .  5.2 Navigation 9 
5.2 1 Ya,v igation Diagram Flow . . . . . . . . . . . . . . . .  9 
5.2.2 Control Strategy . . . . . . . . . . . . . . . . . . . . .  9 
. . . . . . . . . . . . . . . .  5.2.3 Kalman Filter Estimation 11 
. . . . . . . . . . . . . . . . . . . . . .  5.3 Turning At The Corner 12 
6 Experiment Performance 12 
7 Conclusion 13 
8 Acknowledge 13 
EE248: Sensory Information Processing Lab 
1 Introduction 1 
1 Introduction 
Autonomous vehicle navigation is a very popular 
research area in the vision and control field. Based on 
Prof. Dickmanns' philosophy, we implement a navi- 
gation algorithm on thc small robot. The robot can 
rcly on its eyes ( the camera mounted on thc top of 
the robot ) and control its wheels t o  walk through the 
sub-basement hallways of Caltech WIoore Lab building. 
The spced we achieve is robot's mechanical maximum 
speed 0.5 m/s. 
2 Robot Hardware System 
Figure 1: iyomad 200 
system 
\.;lie use the robot Nomad 200 by NOMADIC TECH as our nax~igation vchiclc. 
It is composed by three major parts: motor, sensory and computational 
system. 
2.1. Motor System 
The robot's mobile parts include three components: three wheels, one turret 
and one pan-tilt unit. All the motors are driven by 5 12-volts DC batteries. 
Wheel System (2-DOF) The wheels are driven by two axes: translational 
and rotational. The three wheels are rnechanically coupled to reduce 
slip in translation and always remain parallel to  each other a t  any time 
while rotJating. 
Turret (1-DOF) The turret) only rotates independently to the wheels. By 
this way, the robot can keep constant heading in navigation. 
Pan-Tilt Unit (2-DOF) Pan-tilt unit is mounted on the top of turret and 
carries the camera. It has two motion axes, panting left and right and 
tilting up and down. 
EE248: Sensory Information Processing Lab 
2.2 Sensory Information 2 
All the  rotational motion are relative to a static base frame which provides 
the initial direction for these axes before the robot gets started walking. 
2.2 Sensory Information. 
The robot has two sensory input: visual from a camera and tactile from a 
bumper sensor. Also, it can tell us its own status a t  any time. 
2.2.1 Visual Camera Sensor 
Visual sensor is the most important part in the project. It  provides useful 
cnvironrnent information to the robot. Based on this information , the robot 
can decidc whcre to go for next step. The camera mounted on the top of pan- 
tilt unit is a Mitsubislli CCU-300 color camera. It  provides a few zooming 
and focusing function, also a limited auto-gain control. Camcra's output is 
digitized by DT3133 digital image acquisition board. The digitized irnagcl is 
black and whitc with 256 gray level per pixel. 
2.2.2 Bumper Sensor 
There is a total of 20 individual bumper sensors on the robot that  are ar- 
ranged in two rings of ten sensors each. one upper ring and one lower ring. 
These burripcrs can effectively feel the contact from the outside erlvironrnent 
and thus protect the robot. 
2.2.3 Robot's Status 
Information about the current status of the robot, its configuration and read- 
ing of the sensors can be obtained from a global STATE vector. The status 
is getting updated by a command routine. This provides useful information 
as a feedback for us to control the robot. 
EE248: Sensory Information Processing Lab 
2.3 On-board Computational System 3 
2.3 On-board Computational System 
Robot's computational system consists of an on-board computer and a wire- 
less communication setup. The main features are: 
=a One 120 MHz Pentium on a PC1 backplane bus. 
e Linux operating system 
e i\/licro-controllers for motor control 
a A speech synthesizer 
a A wireless communication port for remote access to  the robot. 
Geometric Analysis 
We first a,nalyze the system from the geometric point of view. The robot 
cruises through the hallways by detecting the two stripes mounted as the 
intersecting line between the hallway walls and the floor. The stripes are 
parallel to  each other. -Also assume that  the camera is parallel to  the floor, 
there is no tilting angle. 'This can be archived by initiaiizing the pan-tiit 
unit. 
Let the origin of the ~ ~ o r l d  frame be on the hallway's centerline with x-axis 
perpendicular to  the stripe edges and y parallel to  the hallway direction. 
Camera fi-arne is creatjed a t  the camera's image center. Assume the cam- 
era is locating H higher than the floor and d ,  f i r  away from the centerline. 
There is an angle offset 1/; between the directions of camera's optical axis and 
hallway. (Geometry figure is shown in Figure 2. 
Assume a point on the stripe edge has coordinate in the world frame: 
P, = (X,, Y,; O ) ,  according to the geometric transformation between world 
IEE248: Sensory Information Processing Lab 
3 Geometric Analysis 4 
Left-s~de strlpe edge 
- -- 
Right-side stripe edge 
Figure 2: Geometric frame for the robot system 
and camera frame 
and the pin-hole projection p, = p (1) P (2) (f &. f*) ,  its pixel coordinate on the 
image plane rclative to the image center is: 
Let thc vanishing point be the infinity intersection of two stripes on the 
image plane. Its pixel coordinate is easily obtained as: p,,, = [- f x tan,($), 01. 
Therefore, the camera's angle offset $ can be calculated frorn detected van- 
ishing point position. If given the edge pixel coordinate p, = (x,, ye) on 
camera image and X coordinate X ,  from known hallway width, we can also 
derive the  carnera's distance offset by: 
EE248: Sensory Information Processing Lab 
4 Image Analysis 
Figure 3: Typical image to show edge detection algorithm 
4 Image Analysis 
4.1 Edge Detection 
In each frame, the computer needs to  detect the stripe edges to  obtain the 
angle offset. Our edge detection algorithm consists of searching in 20 small 
windows. The windows are opened along the predicted edge positions which 
were given by Kalman filter. In each window we find a most likely edge and 
finally linear-fit these data to get the whole stripes. 
As shown in figure 3, the four yellow stars are points predicted for the 
position of edges and blue windows are opened to search small edges. To 
EE248: Sensory Information Processing Lab 
4.2 Focal Length Calibration 6 
avoid ending up with the edges of the floor pattern. cve choose the most 
left or right possible edge in each window according to  the corresponding 
side of wall. Also the edges wc found should have enough gradient on both 
horizontal and vertical directions. This is because our camera doesn't tilt, 
so as long as it can see the stripes, the stripes must be neither horizontal nor 
vertical. By setting this threshold, the possible door edge can be discarded. 
The red circle points are what we found as edge points in each window. The 
position of the two edge intersection tells the location of vanishing point. 
4.2 Focal Length Calibration 
From the geometric analysis, we see the focal length is a critical variable 
we nwd to  know. In the project, we develop a simple algorithm for fo- 
cal length calibration. For each frame, the vanishing point (x,,, g,,) is a t  
(- f x tan($), 0). The robot can precisely control the rotation of the camera 
by controlling either turret or pan-tilt unit. IWc then take two imagc.s I1 
arid 12, wllerc I2 is obtained by rotating A$ with respect to  11. Fos both 
images, wc3 have x,,,~ = - f x tan(Gl) and z,,2 = - f x tan(& + A v ) .  Since 
xLZI1. xCV2, A+ are known by edge detection and robot's rotation. the focal 
length f is calculated by solving the equation: 
Figure 4 shows hocv much the vanishi~g point changes by rotating image 
(a) -5 degrees to  get image (b). 
5 Autonomous Navigation 
The robot walks through the hallways in the building. On each hallcvay, the 
algorithm consists of three phases: Initialization, IVavigation, Turning a t  the 
end of hallway. 
EE248: Sensory Information Processing Lab 
5 Autonomous Navigation 7 
(b) 
Figure 4: Vanishing point is changing with respective to  camera's rotation 
EE248: Sensory Information Processing Lab 
5.1 Initialization 8 
(a) top view of robot orientation (b)view#l, no edge found 
(c)view#2, one edge found (d)view#3, facing to  the hallway direction 
Figure 5: Initialization 
5.1 Initialization 
The robot is located at  an arbitrary position in the hallway. After hardware 
initialization, robot searches 10 width windows for possible left or right stripe 
edges. Turret is rotated 30 degree for each view until onc or two cdges are 
located. Extracting the angle diRerence bctween camera optical axis arid the 
edges, robot rotates the carncra to the direction of edge. After fine tuning of 
camera angle and extracting the left and right stripe edges, robot is in the 
initialized state, that camera and steering wheel faces to the hallway direction 
within il degree and distances to left and right walls are calculated. 
EE248: Sensory Information Processing Lab 
5.2 Navigation 9 
5.2 Navigation 
5.2.1 Navigation Diagram Flow 
The initialization provides the first guess of robot's starting position. Then 
for each next step of navigation. the robot can predict its possiblc position by 
integrating the dynamic model. According to this prediction, we apply next 
step's control input. Tlie reason we put control input here instead of waiting 
until we obtain the next step's true position is to  avoid time delay of frarne 
analysis. Following is to grab next frame. By detecting edges and analyzing 
the data,  we can get the measurement of robot's position. Since we also 
have our prediction, but both prediction and measurement may hax~e noise, 
Kalrnan filter combines two of them to  give us best estimation of robot's 
position in this step. The diagram Aow of this procedure is shown in figure 
6: 
5.2.2 Control Strategy 
Our controllablc state is 6, the distance of the robot away from the cen- 
terline, and 8, the wheel angle offset from thc centerlinc direction. (Notice 
that B is obtained hv $, the camera angle offset, and the angle difference 
of camera and wheel which can be read from robot status vector). Robot's 
Systcm Model: 
Where: u: control input which is the angle velocity of wheel. V: wheel 
translation velocity. In order to  enforce the robot walk along tlie centerline 
from arbitrary initial position, we design the PD controller: 
The closed loop system function is: 
EE248: Sensory Information Processing Lab 
5.2 Navigation 10 
Diagram Flow of Navigation 
Figure 6: Flow diagram of navigation control 
EE248: Sensory Information Processing Lab 
5.2 Navigation I1 
Robot's Pas~t~on a d D>rectlon Sim~iaiion Resuit Robots Posl~on and D.recl~o? Experimeota Result 
8 0 1 ~ ~ ' ~  - , -  
I 
w 
-02 0 5 I 1 ' l 1  10 15 20 25 30 35 40 65 50 025*f-" 0 10 20 30 40 50 60 70 80 90 100 
Time (secj Time (secj 
Figure 7: Cornparison between simulation (a) and experiment (b) results 
e Stability Analysis: The system is stable if and only if K P  and KD are 
positive. 
e Perfornriancc Analysis: To get fast and best performance, we choose 
K P  and KD such that the system's damping c:oefficient is 0.707. 
The simulation and experimental results of the robot status under such 
control strategy are given in figure 7. The initial condition is (d,, 0) = 
(61.4984, -0.1029). 
5.2.3 Kalman Filter Estimation 
Given the robot's dynamic model, and each step's status measurement from 
image analysis, we can use Kalman filter to get best estimation of the robot's 
true position. System model is given as (5), for numerical computation pur- 
EE248: Sensory Information Processing Lab 
5.3 Turninn At The Corner 12  
pose, assume u is constant in each step, we derive its discrete-time model: 
Discrete Kalman filter is easily applied t#o this model. We liere skip the 
filter's forrriula which can be found in [l]. 
5.3 mrning At The Corner 
When robot finally loses the stripe edges of the wall, we suppose that the 
robot has come to  the end of the hallway within 5 meters (which is the case 
in all our cxperimcnts). Robot will search for the kiorizontal strip(. edge of 
tlie facing wall within 2 to 10 meters in front of the robot. Once the front 
wall edge is found, the distance is calculated and robot moves straight ahead 
until 1.5 meters to  the wall. Then turning is finished by rotate robot to  the 
right by 90 degree. Robot is ready for navigate the new hallwav. 
For simplicity. the robot moves blindly to  the front wall after extracting 
the distance to  the wall. This is done with the assumption that  robot is 
f'dcing accurately to  the wall and the distance is smaller than 10 meters. 
For our case, this simple algorithm works fine and safe. Since the control 
algorithm has the robot direction error smaller than 5 degree. Even robot 
walks blindly, i t  would not hit the wall as far as the distance is smaller than 
10 meters. If the robot stops more than 10 meters away from the front wall, 
after turning 90 degree to  right, the initialization algorithm will have the 
robot find the hallway direction and walk backward. 
6 Experiment Performance 
Above algorithm is implemented successfully on the robot. It can navigates 
around the sub-basenlent of Moore Lab with robot's maximum speed 0.5in/s 
from arbitrary position. The robot can automatically locating the direction 
of hallway, walking along the center line and turning to leftlright a t  the end 
EE248: Sensory Information Processing Lab 
7 Conclusion 13 
of hallway. The movie of the robot navigating and the vision of robot are 
available. 
7 Conclusion 
Kalman filter and control algorithm are implemented onto a robot. The robot 
has been successfully navigating around the -known environrnent (hallway 
with known structure and width in our case). 
For future works, it is straightforward to  control robot locate a certain 
target upon navigation. Also it will be interesting to  have the robot to  learn 
and map an unknown or pseudo-unknown environment. For example, ask 
the robot to  determine the width, length and structure of the hallway. 
8 Acknowledge 
We mould like to thank Drs.Psaltis and Perona for their instruction and 
advises. Special thanks go to Greg, Mario,Bob and David, without their 
coritribution and technical emergent service, the robot will never walk safclly 
around the hallway. 
References 
[I]  Arthur Gclb, Applied Optima! Estimation, 1974 
[2] E.D.Dickmanns, An Integrated Spatio-Temporal Approach to  Auto- 
matic Visual Guidance of Autonomous Vehicles, IEEE transactions on, 
systems, Vol 20, No 6,1990 
[3] Greg Steckman and George Ouyang, Robot 3avigation; CNS 248 Re- 
port, 1996 
EE248: Sensory Information Processing Lab 
