GPS-BASED NAVIGATION OF AN ACKERMAN DRIVE ROBOT
A Project
Presented to the faculty of the Department of Electrical and Electronic Engineering
California State University, Sacramento
Submitted in partial satisfaction of
the requirements for the degree of
MASTER OF SCIENCE
in
Electrical and Electronic Engineering
by
David M. Sweeney
SUMMER
2013
© 2013
David M. Sweeney
ALL RIGHTS RESERVED
ii
GPS-BASED NAVIGATION OF AN ACKERMAN DRIVE ROBOT
A Project
by
David M. Sweeney
Approved by:
__________________________________, Committee Chair
Fethi Belkhouche, Ph.D
__________________________________, Second Reader
B. Preetham Kumar, Ph.D
____________________________
Date
iii
Student: David M. Sweeney
I certify that this student has met the requirements for format contained in the University format
manual, and that this project is suitable for shelving in the Library and credit is to be awarded for
the project.
__________________________, Graduate Coordinator
___________________
B. Preetham Kumar, Ph.D
Date
Department of Electrical and Electronic Engineering
iv
Abstract
of
GPS-BASED NAVIGATION OF AN ACKERMAN DRIVE ROBOT
by
David M. Sweeney
Self-driving vehicles require a sophisticated system of sensors and a real-time controller,
which navigates and drives the vehicle toward an intended goal while actively reacting to changes
in the environment. For this project, the method of Kinematic Control is applied to the path
planning and tracking problems for self-driving car-like robots. A brief overview of navigation
concepts used is presented followed by the definition and application of the Kinematic Control
Law to autonomous mobile robots. A computer model simulation of the Kinematic Control Law
shows the robot trajectory for a differential-drive mobile robot. The Kinematic Control is then
implemented in a microprocessor controller for an autonomous car-like robot to demonstrate the
effectiveness of the Kinematic Control Law in self-driving vehicle applications.
_______________________, Committee Chair
Fethi Belkhouche, Ph.D
_______________________
Date
v
ACKNOWLEDGEMENTS
First, I would like to take a moment to thank Dr. Fethi Belkouche for supervising
this project and providing valuable technical guidance and insight. In addition, I would
like to thank past and present CSUS faculty Dr. Fethi Belkhouche, Dr. Pradeep Setlur,
and Dr. Warren D. Smith for their high expectations in the classroom, which helped to
build the foundation for my successful engineering career. I also would like to thank Dr.
B. Preetham Kumar for his guidance throughout my educational journey at CSUS. Of
course, I must also thank my wife, Kim, and our children, Emily and Brian, for their
tremendous support and sacrifice throughout the course of my education.
vi
TABLE OF CONTENTS
Page
Acknowledgements .................................................................................................................. vi
List of Tables ........................................................................................................................... ix
List of Figures ........................................................................................................................... x
Chapter
1. INTRODUCTION ............................................................................................................... 1
2. NAVIGATION AND PATH PLANNING .......................................................................... 3
2.1 Definition of Navigation Heading ............................................................................... 3
2.2 Definition of Navigation Bearing................................................................................ 4
2.3 The Great Circle Navigation Method .......................................................................... 4
2.4 Types of Path Planning Methods ................................................................................ 5
3. KINEMATIC CONTROL LAW ........................................................................................ 7
3.1 Overview ..................................................................................................................... 7
3.2 Open Loop Kinematic Model of Robot ...................................................................... 8
3.3 Control Law and Block Diagram ............................................................................... 9
3.4 Closed Loop Performance and Stability ................................................................... 12
4. MATLAB SIMULATION OF KINEMATIC CONTROL LAW .................................... 13
4.1 Introduction .............................................................................................................. 13
4.2 Robot Trajectories .................................................................................................... 13
4.3 System State Variable ρ vs. Time ............................................................................. 14
4.4 System State Variable α vs. Time…………………………………………………. 15
4.5 System State Variable β vs. Time…………………………………………………. 16
vii
5. AUTONOMOUS VEHICLE NAVIGATION IMPLEMENTATION ............................. 18
5.1 The Robot Platform ................................................................................................... 18
5.2 Model of Robot Dynamics ........................................................................................ 22
5.3 Kinematic Control Application ................................................................................. 23
5.4 Software Block Diagram ........................................................................................... 25
5.5 State Diagram ............................................................................................................ 26
5.6 Test Data and Results ................................................................................................ 28
6. CONCLUSION .................................................................................................................. 30
Appendix A. MATLAB Simulation Code ............................................................................ 31
Appendix B. Embedded Microprocessor C Code ................................................................. 36
References ............................................................................................................................... 49
viii
LIST OF TABLES
Tables
1.
Page
Navigation Data Log for Test…………………… ... .………………………………. 28
ix
LIST OF FIGURES
Figures
Page
1.
Heading angle Φh in the robot coordinate frame ................... .…………………….3
2.
Bearing angle Φb in the robot coordinate ……….………………………………....4
3.
Definition of the controller variables and the robot and goal coordinate systems…......8
4.
Block diagram of closed loop system……………………………………………….....9
5.
Simulation plot of robot trajectory Y vs. X………………………………………..…14
6.
Simulation plot of state variable ρ vs. Time…………………………………….........15
7.
Simulation plot of state variable α vs. Time………………………………………….16
8.
Simulation plot of state variable β vs. Time………………………………………….17
9.
The autonomous car-like robot designed and built for this project…………………..18
10.
Robot photo with components labeled……………………………………………….19
11.
Autonomous car-like robot hardware interface……………………………………....21
12.
Simplified Ackerman steering geometry……………………………………………..22
13.
Block diagram of primary control loop function navProcess()……………………....25
14.
State diagram of robot controller……………………………………………………..27
15.
Aerial photo of course showing GPS points along the trajectory…………………….29
x
1
Chapter 1
INTRODUCTION
Autonomous self-driving vehicle technologies are being developed for numerous
applications. Agricultural applications include autonomous agricultural crop spraying
vehicles that prevent harmful chemical exposure to the operator, and autonomous turf
management vehicles, which eliminate the need for an operator and allow for
nighttime operation. Military applications include unmanned ground vehicles (UGVs)
which can provide support to ground troops such as performing reconnaissance.
Consumer applications are emerging in the form of self-driving cars, which may
eventually be commonplace on our city streets and highways. All of these robotic
vehicle applications require a controller that employs navigation, path planning,
disturbance rejection, and obstacle avoidance techniques to perform their duties
successfully.
Navigation involves the identification of the robot’s location and orientation on
the surface of the Earth. In addition, the robot must also determine the location and
orientation of the desired goal. A sequence of goal locations or “waypoints” may be
specified so that the robot can follow a prescribed course to its final intended goal.
Latitude and longitude coordinate data from onboard Global Positioning Satellite
(GPS) receivers combined with the compass angle from an electronic compass can be
processed with classical navigation techniques to provide the necessary heading,
bearing, and distance data used for path planning.
2
The process of path planning covers the determination of the path from the
starting point to the goal point. Path planning methods may be classified as open loop
algorithms or closed loop algorithms. The path planning method utilized in this
project is a closed loop method known as Kinematic Control. The merits of closed
loop control will be discussed with a focus on Kinematic Control.
A computer simulation using MATLAB software will be conducted which will
exercise the Kinematic Control law and produce trajectory data for a differentialdrive mobile robot. The simulation covers the trajectories produced for various initial
starting conditions.
In order to demonstrate the effectiveness of the Kinematic Control Law, an
autonomous car-like robot was constructed and tested. The robot is equipped with an
array of sensors to provide location, orientation, and speed data to the microprocessor
controller. The controller then uses the Kinematic Control Law to determine the
appropriate velocity and steering angle for the vehicle to reach the intended target
location and achieve the intended goal orientation. A data logging tool was created in
the microprocessor code to allow logging of the sensor data, system states, and
controller outputs at regular intervals for later review.
3
Chapter 2
NAVIGATION AND PATH PLANNING
2.1 Definition of Navigation Heading
The heading angle of the robot specifies the orientation of the robot as it travels in
a forward direction. The angle Φh in Figure 1 below is the heading angle, which is
defined as the angle between the robot’s forward velocity vector and a vector directed
from the robot to the true north pole. True north is the reference for this angle so if the
robot’s velocity vector is directed toward true north then the heading is Φh = 0 radians.
The convention used for this project is that Φh lies in the range -π <= Φh <= π.
Figure 1: Heading angle Φh in the robot coordinate frame
4
2.2 Definition of Navigation Bearing
The bearing angle is the angle between a vector directed along the line-of-sight
path from the robot to the goal and a vector directed toward true north. True north is the
reference for the heading and bearing angles. The convention used for this project is that
Φb lies in the range -π <= Φb <= π.
Figure 2: Bearing angle Φb in the robot coordinate frame
2.3 The Great Circle Navigation Method
There are a few different navigation methods with varying levels of accuracy. For
short distances, most of the methods will suffice. However, for long distances, the effect
of the Earth’s ellipsoidal shape must be taken into account in order to achieve accurate
5
results when calculating distances across the Earth’s surface. Even though this project
requires calculations of relatively short distances, the use of more accurate methods
allows scalability of the algorithms for future work. The method used for this project is
based on a spherical approximation of the Earth’s shape and is quite accurate even for
long distance calculations. It is called the “Great Circle Path” method of navigation[3].
The formula for calculating a distance between two sets of latitude and longitude points
(𝝋1,λ1) and (𝝋2,λ2) is given by:
𝜟𝝋
𝜟𝝀
𝒂 = 𝒔𝒊𝒏𝟐 ( 𝟐 ) + 𝐜𝐨𝐬 𝝋𝟏 × 𝐜𝐨𝐬 𝝋𝟐 𝒔𝒊𝒏𝟐 ( 𝟐 )
√𝒂
𝒄 = 𝟐 × 𝐭𝐚𝐧−𝟏 (
)
(2)
√𝟏−𝒂
𝑫𝒊𝒔𝒕𝒂𝒏𝒄𝒆 = 𝑹 × 𝒄
(1)
(3)
where R is the Earth’s radius.
The bearing angle between the two points is:
𝑩𝒆𝒂𝒓𝒊𝒏𝒈 𝑨𝒏𝒈𝒍𝒆 = 𝐭𝐚𝐧−𝟏 (𝐜𝐨𝐬 𝝋
𝐬𝐢 𝐧 𝜟𝝋×𝐜𝐨 𝐬 𝝀𝟐
𝟏 ×𝐬𝐢𝐧 𝝀𝟐 −𝐬𝐢𝐧 𝝀𝟏 ×𝐜𝐨𝐬 𝝀𝟐 ×𝐜𝐨𝐬 𝜟𝝋
)
(4)
2.4 Types of Path Planning Methods
Open loop control strategies assume that the path is known in advance. The
required motor velocity and steering angle must be predetermined for each point in time.
If the velocity is not regulated, then there will likely be deviations from the desired
velocity caused by changes in gravitational forces when the incline changes. Position
errors will accumulate and the robot will not reach its intended goal position. The robot
will not automatically adapt or correct the trajectory if dynamic changes of the
6
environment occur [1]. Closed-loop control strategies are far more effective than open
loop strategies because all of the system states such as the position, heading angle, and
bearing angle are accurately measured and controlled dynamically. Robots driven by
closed loop navigation systems are able to reach their goal position more quickly and
with a high degree of accuracy. Also, unlike open loop controllers, closed-loop
controllers will cause the robot to respond to disturbances and modify the path in realtime to achieve the goal position and/or orientation.
7
Chapter 3
KINEMATIC CONTROL LAW
3.1 Overview
Autonomous navigation of a car-like vehicle requires a controller that can direct
the vehicle to respond to changes in the vehicle’s environment to maintain motion along
the desired course to the goal. The Kinematic Control Law is a full state feedback
controller designed to accomplish this task. As with any full state feedback controller, the
controller seeks to drive the states of the system to zero to achieve an equilibrium point.
The state variables are characteristic of the open loop system and must be measureable.
Figure 3, below, shows the definitions of the state variables ρ, α, and β. It also shows the
relationship between the robot’s frame and the goal’s frame.
8
Figure 3: Definition of the controller variables and robot and goal coordinate systems
3.2 Open Loop Kinematic Model of Robot
For a differential-drive robot, the open loop model for the system is given by:
𝒅𝒙
𝒅𝒕
= 𝐯𝐜𝐨𝐬 𝜽
(5)
9
𝐝𝐲
𝐝𝐭
= 𝒗𝐬𝐢𝐧 𝜽
𝒅𝜽
𝒅𝒕
=𝝎
(6)
(7)
3.3 Control Law and Block Diagram
Figure 4: Block diagram of closed loop system
The block diagram shown above in Figure 4 shows the block diagram for the
complete full-state feedback controller known as Kinematic Control. The supporting
equations are explained in this section. With the goal position at the origin, (0,0,0), the
transformation to polar coordinates is made using the definitions:
𝝆 = √𝜟𝒙𝟐 + 𝜟𝒚𝟐
(8)
𝜶 = −𝜽 + 𝐚𝐭𝐚𝐧𝟐(𝜟𝒚, 𝜟𝒙)
(9)
10
𝜷 = −𝜽 − 𝜶
(10)
And the resulting system in polar form is given by:
𝒅𝝆
𝒅𝒕
𝒅𝜶
𝒅𝒕
𝒅𝜷
𝒅𝒙
= −𝒗 𝐜𝐨𝐬 𝜶
=
𝐬𝐢𝐧 𝜶
𝝆
=−
−𝝎
𝐬𝐢𝐧 𝜶
𝝆
(11)
(12)
(13)
The actual control law equations based on the states are given by,
𝒗 = 𝒌𝝆 𝝆
𝝎 = 𝒌𝜶 𝜶 + 𝒌𝜷 𝜷
(14)
(15)
11
The continuous-time closed loop model is given by the equations,
𝒅𝝆(𝒕)
𝒅𝒕
𝒅𝜶(𝒕)
𝒅𝒕
= −𝒌𝝆 𝝆 (𝐭)𝐜𝐨𝐬 𝜶(𝒕)
(16)
= 𝒌𝝆 𝐬𝐢𝐧 𝜶(𝒕) − 𝒌𝜶 𝜶(𝒕) − 𝒌𝜷 𝜷(𝒕)
𝒅𝜷(𝒕)
𝒅𝒕
= −𝒌𝝆 𝐬𝐢𝐧 𝜶(𝒕)
(17)
(18)
By applying the Forward Euler Method to the continuous-time closed-loop equations, the
corresponding discrete-time closed-loop system equations are given by:
Discrete-Time Closed-Loop Equations:
𝝆𝒇𝒊𝒏𝒂𝒍 −𝝆𝒊𝒏𝒊𝒕𝒊𝒂𝒍
𝒕𝒇𝒊𝒏𝒂𝒍 −𝒕𝒊𝒏𝒊𝒕𝒊𝒂𝒍
𝜶𝒇𝒊𝒏𝒂𝒍 −𝜶𝒊𝒏𝒊𝒕𝒊𝒂𝒍
𝒕𝒇𝒊𝒏𝒂𝒍 −𝒕𝒊𝒏𝒊𝒕𝒊𝒂𝒍
= −𝒌𝝆 𝝆𝒊𝒏𝒊𝒕𝒊𝒂𝒍 𝐜𝐨𝐬 𝜶𝒊𝒏𝒊𝒕𝒊𝒂𝒍
(19)
= 𝒌𝝆 𝐬𝐢𝐧 𝜶𝒊𝒏𝒊𝒕𝒊𝒂𝒍 − 𝒌𝜶 𝜶𝒊𝒏𝒊𝒕𝒊𝒂𝒍 − 𝒌𝜷 𝜷𝒊𝒏𝒊𝒕𝒊𝒂𝒍
𝜷𝒇𝒊𝒏𝒂𝒍 −𝜷𝒊𝒏𝒊𝒕𝒊𝒂𝒍
𝒕𝒇𝒊𝒏𝒂𝒍 −𝒕𝒊𝒏𝒊𝒕𝒊𝒂𝒍
= −𝒌𝝆 𝐬𝐢𝐧 𝜶𝒊𝒏𝒊𝒕𝒊𝒂𝒍
(21)
(20)
12
3.4 Closed Loop Performance and Stability
The stability criterion for the closed loop system will now be determined.
First, the closed loop system is linearized about an equilibrium point,
cos(x) = 1 and sin(x) = x,
is described by the follow linear matrix equation,
𝒅𝝆
𝒅𝒕
𝒅𝜶
𝒅𝒕
𝒅𝜷
[ 𝒅𝒕 ]
−𝒌𝝆
=[ 𝟎
𝟎
𝟎
𝟎
𝝆
−(𝒌𝜶 −𝒌𝝆 ) −𝒌𝜷 ] [𝜶]
𝜷
−𝒌𝝆
𝟎
(22)
The state matrix A is isolated and yields:
−𝒌𝝆
A=[ 𝟎
𝟎
𝟎
𝟎
−(𝒌𝜶 −𝒌𝝆 ) −𝒌𝜷 ]
−𝒌𝝆
𝟎
(23)
The characteristic polynomial of the system is given by:
(𝝀 + 𝒌𝝆 )(𝝀𝟐 + 𝝀(𝒌𝜶 − 𝒌𝝆 ) − 𝒌𝝆 𝒌𝜷 ) = 𝟎
(24)
Therefore, all roots will have a negative real component and will be stable if
𝒌𝝆 > 0 ; −𝒌𝜷 > 0 ; (𝒌𝜶 − 𝒌𝝆 ) > 0
(25)
Thus, the controller gains kρ, kα, and kβ should be set in compliance with this criterion in
order to maintain stability.
13
Chapter 4
MATLAB SIMULATION OF KINEMATIC CONTROL LAW
4.1 Introduction
The Kinematic Control Law was simulated with MATLAB software using a
dynamic model for a differential-drive robot. The closed-loop system equations were
entered and the simulation was run until all of the system states were sufficiently
approaching zero, or (ρ,α,β) = (0,0,0). To ensure stability, the stability criterion derived in
Chapter 3.4 are followed to tune the controller to the gains (kρ=8, kα=3,kβ=-1.5).
4.2 Robot Trajectories
The simulated trajectories for the robot are shown below in Figure 4 for initial
conditions of ρ=100 feet, β=π radians, and α is set in increments of π/4 over the range
from –π radians to +π radians. The trajectories all begin as indicated below in the
“START” position and end at the “GOAL” position. For increasing values of α, such as α
= +/-π, the length of the path traveled by the robot increases. Since θ=0, all of the state
variables will approach zero when the robot has reached the goal position and orientation.
All of the trajectories end at the goal with ρ=0 feet, α=0 radians, and β=π radians which is
specifies the expected goal position and orientation.
14
Figure 5: Simulation plot of robot trajectory Y vs. X
4.3 System State Variable ρ vs. Time
The plot shown in Figure 5 shows the state variable ρ which begins initially at t=0
with a value of 100 feet. Then, ρ increases for a short period of time with peak values that
are directly proportional to the starting value of state variable α. Then ρ begins to descend
and will approach zero as the time increases due to the influence of the state-feedback
control law. As discussed in Chapter 3, the effect of ρ approaching zero is that the robot
approaches the goal finally reaching the goal when ρ=0.
15
Figure 6: Simulation plot of state variable ρ vs. Time
4.4 System State Variable α vs. Time
The state variable α, is shown below in Figure 6 and begins at a maximum or
minimum value and, as with all states, it will approach zero as time increases due to the
influence of the state-feedback. Since α is the effective difference between the robot’s
heading and bearing angles, whenever α=0, the robot is heading is in directed along the
direct path to the goal. Thus, neglecting the effect of β, when the robot has reached the
goal, it will be oriented in the direction of the goal.
16
Figure 7: Simulation plot of state variable α vs. Time
4.5 System State Variable β vs. Time
The state variable β is shown below in Figure 8. Depending on the initial value of
α, β may increase to a peak value and then will eventually approach -θ. If θ is zero, then β
will approach zero. The contribution of β to the system opposes the direct tracking of the
bearing contributed by α, and causes the robot to reach its desired orientation at the goal
specified by θ. It is then not surprising that β is a function of α and θ.
17
Figure 8: Simulation plot of state variable β vs. Time
18
Chapter 5
AUTONOMOUS VEHICLE NAVIGATION IMPLEMENTATION
Figure 9: The autonomous car-like robot designed and built for this project
5.1 The Robot Platform
The robot platform chassis employs a car-like design with 4 wheels and a
simplified Ackerman steering geometry where the front wheels are forced into parallel
alignment at all times by an axle. The axle pivots about its center on a vertical steering
rod, which is connected to a steering servomotor through a 2-gear system. The gearing
system creates a gain in torque so the steering servomotor is able to rotate the steering rod
and overcome the large static friction created between the wheels and the road surface.
The steering servomotor is controlled by the microprocessor using a pulse-width
modulation (PWM) signal and is capable of steering angles of -45° <= δ <= 45°.
19
Figure 10: Robot Photo with Components Labeled
The drive motor is a simple brushed DC type and provides power to turn the rear
wheels through a 2-gear system. The gearing system provides a gain in rotational velocity
to the rear axle to increase the linear velocity of the robot. The DC motor is controlled by
the microprocessor using a PWM signal with a duty cycle of 0-100% corresponding with
0% when the motor is stopped to 100% when the motor is at its maximum speed. The
duty cycle controls a H-Bridge circuit which dictates the magnitude and direction of the
motor current. The maximum linear speed of the robot is approximately 4.5 ft/s with fully
charged batteries on a level road-type surface.
20
The robot is equipped with sensors to allow measurement of all state variables.
These sensors include a global positioning system (GPS) receiver, a digital compass
mounted with its reference along the robot’s forward velocity, and an optical incremental
encoder mounted on the motor drive shaft. The GPS and the digital compass are used to
derive the state variables required for the Kinematic Control Law. The incremental
encoder has been calibrated in software to measure the instantaneous linear velocity of
the robot. The linear velocity is required in order to translate the controller output ω into
a steering angle δ.
Ultrasonic sensors are mounted on the front-left and front-right corners of the
vehicle and they are monitored by the microprocessor, which simply stops the robot if it
encounters an obstacle within 2 feet. For this project, the purpose of the sensors is simply
to prevent damage to the robot. They are intended for future work where they may be
used to implement an active obstacle avoidance feature.
21
Figure 11: Autonomous car-like robot hardware interface
The primary controller of the robot is a Microchip dsPIC33EP512MY810 16-bit
DSP microprocessor [6]. The primary controller receives streams of data from all sensors
and executes a software state-machine to control the navigation and path planning tasks.
It also controls all actuators including the steering servo and drive motor. While the robot
is in motion, the microprocessor logs sensor data, the feedback controller states, and the
output actuator levels. The microprocessor also provides an RS-232 serial interface so the
operator can retrieve the logged data to a PC for further examination.
22
5.2 Model of Robot Dynamics
Figure 12: Simplified Ackerman steering geometry
A simple car-like platform was used where the front wheels are connected by an
axle, which maintains the parallel alignment of the left and right wheels at all times.
Upon steering, the center of the axle rotates about a fixed point. This steering geometry,
which is shown above in Figure 12, is known as “skid steering”, parallel steering, or
simplified Ackerman steering. For a simplified Ackerman steering configuration the
steering angle or “Ackerman angle” δ is defined by,
𝑳
𝜹=𝑹
(26)
where L is the wheelbase of the robot and r is the turning radius of the robot[5]. The controller
cannot measure its turning radius directly but by measuring the linear velocity v and using the
relationship,
𝒗 = 𝑹𝝎
(27)
23
The controller can compute the turning radius. The variable v is the robot’s linear velocity,
R is the turning radius, and ω is the rotational velocity. The Kinematic Control Law specifies an
output ω, which must be translated into a steering angle δ to drive the steering servo. Combining
the equations yields the equation
𝜹=
𝑳
𝒗
𝝎
,
( )
(28)
which the controller uses to determine the steering angle δ for a controller output ω.
5.3 Kinematic Control Application
In a state-feedback controller, the state vector or, all state variables must be
measureable. If they are not measurable, then a state observer is used to estimate the state
variables from the input(s) and output(s) of the system. In this case, the state variables are
all measurable using sensors.
The state variables are ρ, α, and β. The state variable ρ is the magnitude of the
displacement vector from the robot to the goal. The state variable α is essentially the
difference between the bearing and the heading. Lastly, the state variable β is the
difference between the angle of the robot’s velocity vector and the x-axis of the goal
coordinate system. The controller seeks to drive these 3 states to zero where the system
reaches an equilibrium point.
The state variable ρ is determined by using the formula for the great-circle
distance between two points on the Earth indicated by their latitude and longitude
components. The goal for the robot is stationary and its position is indicated by a given
set of GPS coordinates. The instantaneous GPS coordinates of the robot are determined
24
using a GPS receiver mounted on the robot. The Haversine formula is used to calculate ρ
from these two sets of GPS coordinates and returns a result measured in feet.
The state variable α is defined as the difference between the heading and the bearing
angles. The heading angle is measurable using a digital compass. The digital compass is
composed of a 3-axis magnetometer and a 3-axis accelerometer and returns a stream of
calibrated heading angle data[7]. The bearing angle is measurable using the instantaneous
GPS coordinates of the robot and the GPS coordinates of the goal position. A formula can
be used to determine the initial bearing, or forward azimuth, given the robot and goal
GPS coordinate pairs. The goal for the robot is specified by its GPS coordinates. The
bearing angle is determined from the present GPS coordinates and the goal’s GPS
coordinates.
The state variable β is determined from the state variable α, which is measured,
and the constant θ, which indicates the goal orientation of the robot.
The command signal to the steering servo is a PWM signal corresponding to a desired
servo angle. Since only the robot’s angular velocity ω is available as a controller output,
the steering angle δ must be determined. Then the steering angle δ is converted to a
pulse-width modulation (PWM) signal to drive the steering servo on the robot. Because
of the limited steering angle of the robot, the steering angle output may be saturated.
To ensure stability, the stability criterion derived in Chapter 3.4 are followed to
tune the controller to the gains (kρ=1.00, kα=2.66, kβ=-0.5). These settings allow for
minimal saturation of the output drive motor and steering servo motor and good
performance.
25
5.4 Software Block Diagram
Figure 13 below shows a block diagram for the navProcess() function. This
function is executed at a frequency of 100Hz based on an accurate hardware timer and is
the effective sampling rate for the control loop.
Figure 13: Block diagram of primary control loop function navProcess()
26
5.5 State Diagram
The state diagram shown below in Figure 14, shows the behavior of the
navigation controller process after a reset of the microprocessor. Initially, the controller is
waiting for a switch press from the user. Then once the switch is pressed, the controller
waits for the GPS data to become valid, also known as a “Good GPS Fix”. The controller
then repeats the process of running the navigation loop and logging trajectory data until
the robot is within 20ft of the target, 90 seconds has elapsed, or the GPS fix is lost.
27
.
Figure 14: State diagram of robot controller
28
5.6 Test Data and Results
The robot was taken to an open area large enough for the robot to travel without
obstacles but with adequate GPS reception. The robot was placed at a starting position
and orientation and the robot was allowed to drive itself toward the goal position and
orientation. While the test was running, the robot collected a set of operating parameters
at a rate of once per every 8 seconds using its custom data logging feature. One
interesting trajectory occurs when the robot must drive itself to a goal position that is
“behind” the robot or equivalently has an initial α ~ +/-180°. As the simulation results in
Chapter 4 have shown, this condition produces a longer path toward the goal. The test
was conducted with approximately this condition and the parameters collected are listed
in Table 1, below:
Time
sec
GPS
Latitude
GPS
Longitude
Heading
°
Bearing
°
ρ
α
β
Motor
V
Duty
Cycle
%
Linear
Velocit
y
ft/s
0
38.793891
-121.207239
-92.3
80.4
227.2
172.7
142.3
108.9
4.418
8
38.793896
-121.207344
-155.3
81.9
256.4
-122.8
77.8
108.9
4.537
16
38.793772
-121.207288
142.9
71.1
251.5
-71.8
26.8
100.0
4.418
24
38.793813
-121.207070
92.7
69.3
188.1
-23.5
-21.5
100.0
4.197
32
38.793820
-121.206832
83.4
59.5
125.7
-23.9
-21.1
100.0
4.418
40
38.793892
-121.206623
48.1
52.4
61.6
4.3
-49.3
79.8
3.513
48
38.793960
-121.206495
51.3
44.4
17.87
-6.9
-38.1
59.3
2.956
Table 1: Navigation Data Log for Test
29
Figure 15: Aerial photo of course showing GPS points along the trajectory (Google Earth)
The aerial photo[4] shown above in Figure 15 shows the course taken by the
robot. There were no obstructions in the area during the test as the photo shown was
taken at a prior time. Table 1, above, shows the distance to the goal, or ρ decreases from
approximately 227ft. to 18ft. where it sufficiently reaches the goal. The difference
between the heading and bearing angles, or α, decreases over the course and approaches
zero which is that action that drives the robot toward the goal. The final orientation at the
goal is dictated by β, and the table shows that it approaches -45 degrees or -θ as the
vehicle approaches the goal.
30
Chapter 6
CONCLUSION
All of the required navigation parameters were defined in the context of an
autonomous car-like robot. Some fundamental types of path planning methods were
investigated and the merits of each were discussed. The Kinematic Control Law was
presented for a differential-drive type mobile robot with open loop equations, closed loop
equations, and the discrete-time equivalent equations using the forward Euler
approximation for use in a digital controller. A MATLAB simulation was then performed
using the Kinematic Control Law applied to a differential-drive robot. The focus of the
project was the design and testing of a car-like robot, which utilizes the Kinematic
Control Law to drive itself from the starting location and orientation to a goal location
and orientation. The critical aspects of the design were presented including the dynamic
model for the robot’s simplified Ackerman steering, the use of sensors to measure the
state variables, and the translation of controller outputs to actuator inputs. Testing of the
robot was conducted to demonstrate that the robot follows a trajectory dictated by the
Kinematic Control Law.
Examples of future work include the addition of an obstacle avoidance algorithm
or the tracking of a moving goal. The robot itself could benefit greatly from a larger,
more powerful drive train and a chassis with a wider steering angle range.
31
APPENDIX A
MATLAB Simulation Code
%**********************************************************************
%
Closed-Loop Kinematic Control of a Differential-Drive Robot
%
Author: David Sweeney
%
Purpose: A MATLAB simulation demonstrating the kinematic
%
control law for starting orientations from -pi to +pi
%
in pi/4 radian (45 degree) increments. The goal
%
orientation and line-of-sight distance to the goal
%
are fixed. Plots are generated for state variables vs
%
time and trajectories in the x-y plane.
%**********************************************************************
%**********************************************************************
T=0.01; %Sampling Interval in Seconds
t=zeros(1,1000);
k=0;
index=0;
%Gains
k1=3.0;
k2=8.0;
k3=-1.5;
for index=-4:1:4
32
%Initialize arrays for system states
rho=zeros(1,1000);
alpha=zeros(1,1000);
beta=zeros(1,1000);
x=zeros(1,1000);
y=zeros(1,1000);
%Initial Conditions all in radians, index=0 is pure pursuit
rho(1)=100;
alpha(1)=index*pi/4; %45 deg
beta(1)=pi; %180 deg
%Convert to cartesian form
x(1)=rho(1)*cos(beta(1));
y(1)=rho(1)*sin(beta(1));
k=0;
for n=2:1:1000
t(n)=(n-1)*T;
%Update state variables
rho(n)=rho(n-1)+k*T*(-k1*rho(n-1)*cos(alpha(n-1)));
alpha(n)=alpha(n-1)+k*T*(k1*sin(alpha(n-1)) -k2*alpha(n-1)k3*beta(n-1));
beta(n)=beta(n-1)+k*T*(-k1*sin(alpha(n-1)));
%Conversion to cartesian form
33
x(n)=rho(n)*cos(beta(n));
y(n)=rho(n)*sin(beta(n));
k=k+1;
%Use some bound to stop simulation as states approach zero but
% never necessarily reach zero.
%
if x(n) <= 0.001
%
break;
%
end
%
if y(n) <= 0.001
%
%
break;
end
if rho(n) <= 0.0001
break;
end
%
if alpha(n) <= 0.001
%
break;
%
end
%
if beta(n) <= 0.001
%
%
break;
end
end
%Convert all angles to degrees for plotting purposes only:
for n=1:1:1000
alpha(n)=alpha(n)*180/pi;
beta(n)=beta(n)*180/pi;
34
end
figure(1)
hold on
title('Y vs X')
xlabel('X Position (feet)')
ylabel('Y Position (feet)')
plot(x,y)
figure(2)
hold on
title('Rho vs Time');
xlabel('Time (seconds)')
ylabel('Distance (feet)')
plot(t,rho,'*')
figure(3)
hold on
title('Alpha vs Time')
xlabel('Time (seconds)')
ylabel('Alpha (degrees)')
plot(t,alpha,'*')
figure(4)
hold on
title('Beta vs Time')
xlabel('Time (seconds)')
35
ylabel('Beta (degrees)')
plot(t,beta,'*')
end
36
APPENDIX B
Embedded Microprocessor C Code
/********************************************
* Filename: navigation.c
* Author: David Sweeney
* Purpose: Autonomous Car-Like Robot Controller Project
* Target Processor: Microchip dsPIC33EP512MU810
**********************************************************
*/
#include "p33EP512MU810.h"
#include <math.h>
#include "navigation.h"
#include "imu.h"
#include "pwm.h"
#include "drive.h"
#include "stdio.h"
#include "system.h"
#include "uart.h"
#include "encoder.h"
#define DEG_TO_RAD (3.14159 / 180.0)
#define RAD_TO_DEG (180.0 / 3.14159)
#define TWO_PI 6.2831
#define PI 3.14159
37
unsigned char navState=0;
double goalGPSCoord[2]; //Latitude, Longitude
//System States
double rho,beta,alpha;
double bearing;
//Controller Gains
double kRho=1.00;
double kAlpha=2.66;
double kBeta=-0.50;
//Controller Outputs
double v,omega;
double delta=0;
double navigationTs = 0.01; //10ms, 100Hz servo rate
double theta=45.0;
double linearVelocity=0;
char navDbg[144];
char navLog[8][144];
unsigned char navLogCnt=0;
unsigned long myQeiCnts=0;
extern unsigned char indexOccurred;
extern IMU_DATA_TYPE imuData;
void navInit()
38
{
//Rho is great-circle distance between robot and goal
rho =
navHaversineFeet(imuData.latitude,imuData.longitude,goalGPSCoord[0],goa
lGPSCoord[1]);
kRho=100.0/rho; //Full forward drive when rho is initially
maximum
}
//Initial bearing or forward azimuth adapted from http://www.movabletype.co.uk/scripts/latlong.html
double navBearing(double lat1,double long1,double lat2,double long2)
{
double x,y,bearing;
double dLon;
//all angles must be in radians for trig functions
lat1 *= DEG_TO_RAD;
lat2 *= DEG_TO_RAD;
long1 *= DEG_TO_RAD;
long2 *= DEG_TO_RAD;
dLon = (long2-long1);
y = sin(dLon) * cos(lat2);
x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon);
bearing=RAD_TO_DEG * atan2(y,x);
39
return bearing;
}
//Calculate Haversine distance in miles from lattitude/longitude in
degrees
double navHaversineMiles(double lat1, double long1, double lat2, double
long2)
{
double dlong = (long2 - long1) * DEG_TO_RAD;
double dlat = (lat2 - lat1) * DEG_TO_RAD;
double a = pow(sin(dlat/2.0), 2) + cos(lat1*DEG_TO_RAD) *
cos(lat2*DEG_TO_RAD) * pow(sin(dlong/2.0), 2);
double c = 2 * atan2(sqrt(a), sqrt(1-a));
double d = 3956.0 * c;
return d;
}
//Calculate Haversine distance in feet from lattitude/longitude in
degrees
double navHaversineFeet(double lat1, double long1, double lat2, double
long2)
{
return 5280.0*navHaversineMiles(lat1,long1,lat2,long2);
}
40
void navSetGoal(double latitude, double longitude)
{
goalGPSCoord[0]=latitude;
goalGPSCoord[1]=longitude;
}
void navStateMachine()
{
//Index Pulse Event Received From Encoder Interface Interrupt
Handler
if(indexOccurred==1)
{
//Calculate new linear velocity based on encoder revolution
linearVelocity = ((7.0/3.0) * 0.7195)/(myQeiCnts * 0.01);
//ft per sec
indexOccurred=0;
myQeiCnts=0;
}
else
{
myQeiCnts++;
}
switch(navState)
{
41
static unsigned short navWaitCnt=0;
static unsigned short navStopWaitCnt=0;
static unsigned short switchState=0;
//STATE 0 IDLE
case 0:
// "GO" Switch Debouncing Routine
//Look for high to low transition
if(PORTAbits.RA7==0)
{
if(switchState==0)
switchState=1; //Went Low
}
else
{
if(switchState==1) //Went low then went high
again 10ms later, valid press
{
//Switch Pressed
navState=1;
navWaitCnt=0;
switchState=0;
}
else
switchState=0; //Reset Flag
42
}
break;
//STATE 1 COLLECT INITIAL GPS DATA and WAIT FOR FIX=1
case 1:
navWaitCnt=0;
if(imuData.gpsFix==1)
{
navState=2;
}
break;
//STATE 2 SET GOAL AND INITIALIZE CONTROLLER
case 2:
//Set Goal GPS Coordinates
navSetGoal(38.560906,-121.422490); //CSUS ENGINEERING
navInit();
navState=3;
navWaitCnt=0;
break;
//STATE 3 NAVIGATION CONTROL LOOP PROCESS
case 3:
if(imuData.gpsFix)
{
navProcess();
43
if(navWaitCnt>=800)
{
navLogStatus();
navWaitCnt=0;
}
else
navWaitCnt++;
if(navStopWaitCnt>9000) //1.5 minutes max
{
navStopWaitCnt=0;
navState=4;
}
else
navStopWaitCnt++;
if(rho<20) //Check if sufficiently close to
goal position
navState=4;
}
else
{
//Lost GPS Fix, Abort
navState=4;
}
break;
//GOAL HAS BEEN REACHED, STOP DRIVE MOTOR
case 4:
44
navStop();
navState=5;
break;
//WAIT FOR USER TO RETRIEVE TRAJECTORY DATA FROM RAM
//BY USING RS-232 SERIAL COMMANDS
case 5:
break;
default:
navState=0;
break;
}
}
//Robot has reached goal position, stop motor
void navStop()
{
driveForward(0);
}
//Save Trajectory Log Entry To RAM
void navLogStatus()
{
if(navLogCnt<7)
sprintf(&navLog[navLogCnt++][0],"**%d,%3.6f,%3.6f,HG:%3.1f,BG:%3.
45
1f\n\rR:%3.1f,A:%3.1f,B:%3.1f\n\rW:%3.1f,Delta:%3.1f,V:%.1f,LV:%.3f",na
vLogCnt,imuData.latitude,imuData.longitude,imuData.heading,bearing,rho,
RAD_TO_DEG*alpha,RAD_TO_DEG*beta,RAD_TO_DEG*omega,delta,v,linearVelocit
y);
}
//Retrieve Trajectory Log Data From RAM
void navLogGet()
{
unsigned char h;
if(navState==5)
{
for(h=0;h<=navLogCnt;h++)
{
sprintf(navDbg,"\r\n%s",&navLog[h][0]);
sendString(UART_2,navDbg);
}
}
}
//Runs periodically in executive loop at rate of navigationTs
void navProcess()
{
//0deg Center, +theta CCW, -theta CW
steerDegrees(delta);
46
driveForward(v); //Drive Forward at Duty Cycle v%, 0<= v <=100
//Robot Frame Definition: +y extends from "driver's" side, +x is
in robot's forward direction
//Bearing: Angle between True North Pole and Goal with vertex at
robot, True North is 0, +90 is east, -90 is west
//Heading: Angle between True North Pole and Robot's Velocity
Vector, from magnetometer data, +90 is east, -90 is west
//Alpha: Angle between robot's current heading and straight line
path to goal.
//Beta: Angle between velocity vector of robot and Xgoal axis
//Theta: Angle between robot's velocity vector and final goal Xaxis
//rho is great-circle distance between robot and goal
rho =
navHaversineFeet(imuData.latitude,imuData.longitude,goalGPSCoord[0],goa
lGPSCoord[1]);
//Bearing Angle, Angle between Robot->True North and Robot->Goal
with vertex at Robot
//North is 0 degrees, East is 90 degrees, South is +/-180 degrees
47
//Heading Angle, Angle between Robot->True North and Robot's
Direction of Forward Travel Vector
//MGH is tilt compensated, declination adjusted, true heading in
degrees
//0deg deg NORTH, +90deg EAST, -90 WEST, 180deg SOUTH,
//Should probably take a moving average of 10 MGH values to avoid
errors
//alpha and beta are always bounded by -pi<alpha<pi and
-
pi<beta<pi
bearing =
navBearing(imuData.latitude,imuData.longitude,goalGPSCoord[0],goalGPSCo
ord[1]);
alpha =
DEG_TO_RAD * (bearing - imuData.heading);
if(alpha>PI)
alpha=alpha-TWO_PI;
else
if(alpha<-PI)
alpha=alpha+TWO_PI;
//alpha and beta are always bounded by -pi<alpha<pi and
pi<beta<pi
beta = -(DEG_TO_RAD*theta + alpha);
-
48
if(beta>PI)
beta=beta-TWO_PI;
else
if(beta<-PI)
beta=beta+TWO_PI;
//Assign new omega based on system states alpha and beta
omega = kAlpha*alpha + kBeta*beta;
//Calculate steering angle delta based on controller output omega
and measured linear velocity
//Angular Velocity = Radius * Omega
//Radius = Linear Velocity / Omega
//Delta=Wheelbase/Radius
delta=RAD_TO_DEG*(WHEELBASE_FT / (linearVelocity/omega));
//Assign new drive servo speed based on new rho
v = kRho * rho;
}
49
REFERENCES
[1]. Roland Siegwart, Illah Reza Nourbakhsh, Davide Scaramuzza, Autonomous Mobile
Robots, 2nd Edition, 2011, The MIT Press, Massachusetts Institute of Technology
Cambridge, Massachusetts 02142.
[2]. National Geophysical Data Center,
http://www.ngdc.noaa.gov/geomag/declination.shtml, 6/28/13.
[3]. Movable Type Scripts: Calculate distance, bearing and more between
Latitude/Longitude points, http://www.movable-type.co.uk/scripts/latlong.html,
6/23/13.
[4]. Google Earth Software, Google Corporation http://www.earth.google.com, 7/21/13.
[5]. Geometry and Linkage - College of Engineering, Purdue University,
http://www.google.com/url?sa=t&rct=j&q=ackerman%20steering%20equation&s
ource=web&cd=9&ved=0CEoQFjAI&url=https%3A%2F%2Fengineering.purdue
.edu%2FABE%2FPeople%2FKrutz%2FLecture%25201&ei=XXQAUv_DJcasig
LfhYC4Aw&usg=AFQjCNFjxtqfkwEcA7l752Hec4eZVh9FBQ&bvm=bv.50310
824,d.cGE, 7/1/13.
[6]. Explorer 16 Development Board User Manual,
http://ww1.microchip.com/downloads/en/DeviceDoc/Explorer%2016%20User%2
0Guide%2051589a.pdf, 2/1/13.
[7]. HomePage – ardu-imu – Arduino based IMU & AHRS – Google Project Hosting,
https://code.google.com/p/ardu-imu/wiki/HomePage, 3/1/13.