- Matthew Gilli, Electrical Engineering B.S.
- Yu-Chia Hsu, Mechanical Engineering M.S.
- Jason Mayeda, Mechanical Engineering B.S.
- Roy Sun, Mechanical Engineering B.S.
GPS and Autonomous Vehicles
Global Positioning System (GPS) is formed by a network of satellites that provides geolocation data (time stamps, coordinates) to compatible GPS receivers. GPS data is commonly used along with a suite of other sensors in autonomous systems for navigation and control. The data provides coordinates in a global coordinate frame (latitude, longitude) that can be used to perform simply point-to-point navigation tasks. For more information about GPS fundamentals, see GPS.gov..
Due to complications with previous plug-and-play GPS modules for DonkeyCar autonomous vehicles, we sought to build our own DonkeyCar-compatible GPS module from scratch. The main flow of our project is as follows:
- Initialize a list of waypoints.
- Recieve GPS data from GP-20U7 reciever.
- Determine current position and bearing of the car with respect to the waypoint.
- Calculate throttle and steering commands to direct the car to the waypoint.
- Repeat steps 2-4 until the waypoint is reached.
- Once the car reaches a waypoint, drive in circle for X amount of time.
- Repeat steps 2-6 until all waypoints in the list have been visited.
This project was implemented with two main processes: planning and GPS interface.
- Polls for GPS data through the Pi serial port.
- Parses GPS strings for relevant coordinate and time data.
- Inputs: Bit/s
- Outputs: Car location: GPS coordinates in latitude and longitude.
- Implements control algorithms to calculate actuator commands for the car.
- Keeps track of additional stop conditions.
- Manages waypoint list.
- Inputs: Car location: GPS coordinates in latitude and longitude
- Outputs: Throttle and steering commands
To implement our navigation tasks, we used the inexpensive GP-20U7 GPS receiver. Here you can find more information on the receiver including cost, sample projects, and a data sheet.
An embedded system is a system with dedicated function within larger system.
The gps system was embedded into car system
In order for gps to communicate with car they must share a common communication protocol. In this case, serial communication.
Serial interfaces stream their data, one single bit at a time. These interfaces can operate on as little as one wire, usually never more than four. A serial bus consists of just two wires - one for sending data and another for receiving. As such, serial devices should have two serial pins: the receiver, RX, and the transmitter, TX.
It's important to note that those RX and TX labels are with respect to the device itself. So the RX from one device should go to the TX of the other, and vice-versa.
Some serial busses might get away with just a single connection between a sending and receiving device. In our case, the RPI does not need to relay any data back to the GPS. All that is needed is a single wire from the GP's TX to the pi's RX An example of this is shown in wiring diagram below.
To connect the GPS unit to the Pi, follow these pinout instructions:
GP-20U7 --> Pi VCC --> 3V GND --> GND TX --> RX RX --> None
Our GPS analog to the Donkey Car manage.py. Add parts to the DK vehicle and calls Vehicle.py.
DonkeyCar, time, threading
Threaded Donkey Car part that reads GPS data from the Pi serial port. Waits for identifier in strings to parse relevant GPS data (latitude, longitude coordinates).
numpy, serial, pynmea2
Donkey Car part that takes GPS coordinates of current and previous time step to calculate distance and bearing to goal. Uses distance and bearing data to calculate the throttle and steering commands, respectively. Manages list of waypoints and stop conditions.
Our distance equations were based on the haversine derivations on this page.
A Python implementation of the haversine equation can be found in our planner.py part.
- Calculate the bearing θi of the vector made by the current location and the goal point, with respect to North.
- Calculate the bearing ψi of the vector made by the previous time step location and the current location, with respect to North.
Our bearing calculation method is outlined at this page.
def calc_bearing(self, pointA, pointB): # extract lat and long coordinates lat1 = pointA lon1 = pointA lat2 = pointB lon2 = pointB diffLon = lon2 - lon1 x = sin(diffLon) * cos(lat2) y = cos(lat1) * sin(lat2) - (sin(lat1)*cos(lat2)*cos(diffLon)) initialBeading = arctan2(x, y) # bearing (deg) compassBearingRad = (initialBearing + 2*pi) % (2*pi)
GPS Interfacing: gps.py
gps.py is a threaded donkey car part. This allows us to run our gps polling method in parallel to the Donkey Car Vehicle.py loop.
For more information on multi-threading in Python, see the official documentation here.
A great introduction to Donkey Car parts from the developers can be found here and here. It is HIGHLY RECOMMENDED that you watch these videos first to save time later. Adding threaded functionality to Donkey Car parts is a very simple process.
(1) First, define a Donkey Car part (python class) with the following methods:
def __init__(self, ...): # initialize any attributes for the class
def run(self, ...): # main function that will be called in each loop of Vehicle.py
def shutdown(self, ...): # shutdown function when Vehicle.py exits loop
(2) To define a process as threaded, add the following two methods to the class:
def update(self, ...): while self.on: # call methods that you would like to run in the thread
def run_threaded(self, ...): return output1, output2
Instead of calling "run", Vehicle.py will call "update" while the flag self.on is "on".
(3) When adding the part to the Vehicle.py use the following syntax:
V.add(gps, outputs=, inputs=, threaded=True)
def poll(self): gpsdataByte = self.gpsObj.readline() # read data from serial port gpddata = gpsdataByte.decode('utf-8') # convert from byte to string if 'GPGGA' in gpsdata: self.prevLocation = self.currLocation # update previous location self.currLocation = self.GPStoRad(gpsdata) # GPStoRad converts parses GPS string to radians else: pass return self.currLocation, self.prevLocation
Poll is the main method called in the GPS class. It monitors the serial port, and converts the data from byte to string to float.
Planning Implementation: planner.py
The planner consists of two controllers: one for steering and one for throttle. At the moment, they are simply proportional controllers that take in their respective error signals and output a control signal. Future tuning of these controllers or implementation of a different controller altogether is suggested to improve the car performance.
When the Planner loops through waypoints, the car will drive in a circle for X amount of time before continuing on to the next point.
Here are the methods used in the planner class.
def run(self): # loop through waypoints, check for stop conditions, and call controllers return steering_cmd, throttle_cmd
def update_distance(self): # implement haversine formula outlined in the distance section above return self.distance
def calc_bearing(self): # calculate the bearing error using the process outlined in the bearing section above return self.bearing
def steering_controller(self, currLocation, prevLocation) # implement control algorithm based on bearing return steering_cmd
def throttle_controller(self, currLocation): # implement control algorithm based on distance to goal return throttle_cmd
def drive_in_circle(self): # send constant throttle and steering to drive in a circle once waypoint is reached return throttle_cmd, steering_cmd
Results of a typical two point waypoint navigation. The starting point is shown in black. The first waypoint, "Warren Mall", is shown in red. The threshold goal circle surrounding the waypoint is shown as the dashed circle. The graph shows that the car did trigger the "drive in circle" method once the goal threshold was reached. The second waypoint, Geisel Library, is not shown.
You can see the car LIVE in action here: https://www.youtube.com/watch?v=O6DhMk9fdZ4
- Implement a better control algorithm for improved steering performance.
- Timing Issues
- Default loop frequency for Vehicle.py is 20Hz, consider changing refresh rate.
- Threaded GPS part updates GPS location slowly, consider different baud rates.
- Heading calculation issues
- Because the bearing with respect to goal is calculated between the current and previous time step, small perturbations in the car trajectory (due to poorly tuned controllers, high speed, or difference in frequencies of main loop and threaded loop), can cause large deviations from the straight-line trajectory from start to goal.
- Adjusting these parameters will improve performance.
- Create an APP to set waypoints and destination and plan a route.