Arduino UNO in Action
In this project article, Raul builds a robotic car that navigates to a series of GPS waypoints. Using the Arduino UNO for a controller, the design is aimed at robotics beginners that want to step things up a notch. In the article, Raul discusses the math, programming and electronics hardware choices that went into this project design.
In this article I lay out a basic differential drive robotic car for waypoint autonomous navigation using the Global Positioning System (GPS). The robotic car receives a list of GPS coordinates, and navigates to waypoints in their given order. To understand how it works, I will discuss concepts about GPS, a simple approach to implement autonomous navigation using GPS, the hardware required for the task, how to calculate navigation vectors using the “Haversine Formula” and the “Forward Azimuth Formula” and a simple implementation of a moving average filter for filtering the GPS coordinate readings. I also discuss a simple approach to navigation control by minimizing the robotic car’s distance and heading error with respect to the goal.
This project is aimed at beginners with basic robotic car experience—that is, line followers, ultrasonic obstacle avoiders and others who now want to try something a little more complex—or anyone who is interested in the subject.
Figure 1 shows the main components of the system. The GPS receiver helps to calculate the distance from the robotic car to the goal. With the aid of a digital compass, the GPS also helps to determine in which direction the goal is located. Those two parameters—distance and direction—give us the navigation vector required to control the robotic car toward the goal. I used a four-wheel differential drive configuration for the car, which behaves almost the same as a two-wheel differential drive. The code provided with the project should work well with both configurations.
GPS Robotic Car block diagram
To calculate the distance to the goal, I used the Haversine Formula, which gives great-circle distances between two points on a sphere from their longitudes and latitudes. The Forward Azimuth Formula was used to calculate the direction or heading. This formula is for the initial bearing which, if followed in a straight line along a great-circle arc, will take you from the start point to the end point. Both parameters can be calculated using the following known data: The goal’s GPS coordinate, the robotic car’s coordinate obtained from the GPS receiver and the car’s heading with respect to North obtained from the digital compass.
The robotic car constantly recalculates the navigation vector and uses the obtained distance and heading to control the motors to approach the goal. I also put a buzzer in the robotic car to give audible feedback when the robotic car reaches the waypoints.
— ADVERTISMENT—
—Advertise Here—
HARDWARE
As shown in Figure 1, I used an Arduino UNO board as the main controller. I chose Arduino because it’s incredibly intuitive for beginners, and it has an enormous constellation of libraries. The libraries make it easy to pull off reasonably advanced projects, without excessive details about the hardware and software drivers for sensors and actuators.
The GPS receiver I chose for the task is the HiLetgo GY-GPS6MV2 module, based on the U-blox NEO-6M chip. The digital compass is the GY-271 module, based on the Honeywell HMC5883L chip. Both are low-cost and ubiquitous with readily available Arduino libraries. The U-blox NEO-6M has a UART serial communication interface, and the HMC5883L works with the I2C serial protocol. To avoid interference, the compass should be placed at least 15 cm above the rest of the electronics.
The DC motors are driven using the very popular L298N module, based on the STMicroelectronics L298N dual, full-bridge driver. It can drive two DC motors with a max current of 2 A per channel. It can also drive two DC motors in each channel if the max current specification is not surpassed—which is what I’m doing with the four-wheel drive chassis I used for my prototype. The chassis has a 30 cm × 20 cm aluminum platform, four generic 12 V DC 85 rpm motors and wheels that are 13 cm in diameter. But almost any generic two-wheel or four-wheel drive chassis can be used.
For supplying power to the robotic car, I used an 11.1 V, 2,200 mA-hour (LiPo) Lithium-Polymer battery with a discharge rate of 25C. For my type of chassis, a battery half that size should also work fine. Figure 2 shows the circuit diagram for this project, and Figure 3 shows the finished car.
Circuit diagram for the Robotic Car project
Completed GPS Robotic Car
GLOBAL POSITIONING SYSTEM
The Global Positioning System (GPS) is a global navigation satellite system owned by the United States government. It provides geolocation and time information to any GPS receiver on the surface of the Earth, whenever it has unobstructed line of sight to at least four GPS satellites—the more the better [1]. GPS receivers typically can provide latitude and longitude coordinates with an accuracy of about 2.5 m to 5 m under ideal conditions, such as good sky visibility and lots of visible satellites. My robotic car is programmed with one or more waypoints given by latitude and longitude coordinates, and the car’s GPS receiver gives its actual position in the same type of coordinates.
Figure 4 is a flow diagram of the algorithm working in the robotic car. I think most of it is self-explanatory, so I’ll focus on the navigation vector calculation and the navigation control, which are arguably the most complex part of it. As I said earlier, I’m using the Haversine Formula to calculate the distance to the goal, and the Forward Azimuth Formula to calculate the goal’s orientation with respect to the magnetic North. If the distance to the goal is greater than a given distance tolerance error—which I set to 1 m—the algorithm will proceed to move the car forward. Moreover, if the heading angle of the robotic car with respect to the current goal is outside a given heading tolerance error—which I set to ±5 degrees—the algorithm will also rotate the robotic car until the nose points toward the goal. The algorithm will drive the robot until the current goal is reached, and then repeat the same steps if another waypoint is available. If not, the car will stop.
Flow diagram of the algorithm working in the Robotic Car
The car must constantly repeat this cycle of calculating the navigation vector and controlling its motion, because with every movement, additional distance and heading errors are introduced. Moreover, both the GPS receiver and the digital compass have error tolerances of their own, which will also add to the main error magnitudes—not to mention also actuator error tolerances and external disturbances.
NAVIGATION VECTOR CALCULATION
Figure 5 graphically shows how the navigation vector—distance and heading—is defined. These two components are computed from three known parameters: the goal’s coordinates, the robotic car’s coordinates and the car’s heading with respect to magnetic North. Distance and heading are then the errors we will try to minimize with our control algorithm. The distance error can be reduced by giving the car a linear velocity, and the angle error by giving a corresponding angular velocity. The “Haversine Formula” allows us to calculate the distance error “d,” and the “Forward Azimuth Formula” allows us to calculate the azimuth (waypoint_angle), with which we can calculate the heading error “α”.
Navigation vector calculation
The Haversine Formula is a general formula in spherical trigonometry used to calculate the distance between two points on the surface of the Earth or any sphere. To calculate the distance between two points on the surface of the Earth, we must have the coordinates—latitude and longitude—of the two points. Equations (1) and (2) give us, respectively, the general Haversine Formula and the Haversine trigonometric identity [2]. However, Equations (3), (4) and (5) are the ones I implemented in code. They are just a breakdown of Equations (1) and (2) that express the calculations more conveniently [3].
— ADVERTISMENT—
—Advertise Here—
Where:
The azimuth is defined as a “horizontal” angle measured clockwise from a North base line or meridian to a direction line, defined by two points over the surface of the Earth. If we take the true North as the reference, we get the “true azimuth.” And if we take the magnetic North, we get the corresponding “magnetic azimuth.”
Therefore, the Forward Azimuth—or just Azimuth—Formula enables us to calculate the angle between these two lines: the line between the robotic car’s position and the North and the line between the robotic car’s position and the goal (Figure 5). Equation (6) shows this formula [3]. To calculate the azimuth, we need just the coordinates of the goal and the robotic car.
Where:
Once we have the azimuth or the waypoint angle, we must then calculate the heading error, which is just the difference between the azimuth and the robotic car’s heading with respect to the North, obtained from the digital compass:
heading_error = waypoint_angle – robot_car_heading.
NAVIGATION CONTROL
The control algorithm for the navigation is simple. First, we try to reduce the heading error by turning the robotic car to the right if the angle error is positive, and to the left if the error is negative. Because it’s difficult to reduce the error exactly to zero and maintain it at zero, I’m defining a tolerance range of ±5 degrees. So, it will be fine, in principle, if we reduce the error to any magnitude within that range. At the same time, we try to reduce the distance error by commanding the robotic car forward in the approximate direction to the goal. But by doing that, normally the angle error tends to increase in magnitude positively or negatively. This occurs because of tolerance errors not only in the sensors (GPS receiver and digital compass) but also in the actuators, and also because of outside disturbances. With that in mind, we must permanently iterate over the calculation of a new navigation vector and the minimization of both errors until we finally arrive at the goal.
If the heading error’s absolute value is between 5 and 45 degrees—25% of 180 degrees—the car will turn slowly right or left depending on the sign of the error (as stated earlier). If the error is between 45 and 180 degrees, the car will turn faster to minimize the error more rapidly. I chose the 45-degree limit empirically. It worked well in the tests, but can be changed in the Arduino code.
This control strategy, to my understanding, is some kind of “stepped proportional controller,” because it divides the error space in five steps: -180 to -45, -45 to -5, -5 to +5, +5 to +45, +45 to +180. The algorithm uses a proportional correction control signal “hardcoded” in the velocities of five corresponding maneuvers the robotic car has available, to correct the error and reach to the goal. Those maneuvers are: turn right fast, turn right slowly, forward, turn left slowly and turn left fast.
GPS AND COMPASS PRECISION
Precision and accuracy are two concepts not always correctly understood even by technically minded people, myself included. In the engineering world, both concepts are generally associated with taking measurements using sensors, and are sometimes wrongly used interchangeably. In simple terms, accuracy is how close a measured value is to the actual true value, and precision refers to the repeatability of the measurement—in other words, how close two or more measurements are to each other. Figure 6 shows the difference between accuracy and precision. To more easily understand the difference, we can use the following illustration: If when throwing a basketball, you always hit the left side of the backboard and almost never get a basket, you are precise, but not accurate. The U-blox NEO-6M’s datasheet gives a horizontal position accuracy of 2.5 m, CEP (circular error probability) 50%—which means that only 50% of all measurements taken in a given period of time will have that 2.5-m accuracy. Moreover, this is under ideal conditions. Under most ordinary conditions, the accuracy (CEP 50%) can be reduced to 10 m, 20 m or more.
Difference between accuracy and precision
So, the accuracy with GPS navigation in general isn’t that high. In our case, the robotic car will reach the waypoints, more or less with that same level of accuracy. This doesn’t seem really impressive at first, but seeing the robotic car automatically navigate to all waypoints—within an error range—is still great fun! Generally speaking, we get a more reliable and repeatable behavior from the robotic car in wide-open spaces, with few buildings and other obstacles around. In such spaces, I have seen the car reach the goal in a radius between about 1 m to 3 m.
As you may already know, there are several ways to improve navigation accuracy. For example, you could use a much more sophisticated and expensive GPS, such as a differential GPS, or another type of sensor, such as odometry or an Inertial Measurement Unit (IMU) could be added to the car. In more sophisticated applications, a camera or a LIDAR sensor could be used also.
Regarding the digital compass, the accuracy isn’t much of a problem. The Honeywell HMC5883L’s datasheet states an accuracy of 1-2 degrees. That’s more than enough for reaching the goal, so the lower GPS receiver’s accuracy will always be the dominant factor. However, digital compasses are subject to magnetic interference by electronic circuits, magnetic materials and nearby metallic objects, including metal structures, metal fences and metal meshes.
MOVING AVERAGE FILTER
To somehow mitigate the abundant noise present in GPS data—that is, noise from the inherent low accuracy and precision of GPS receivers—I implemented a moving average filter, which is technically a Finite Impulse Response (FIR) low-pass filter. A moving average filter is, in principle, just an average calculator that takes “N” readings In0 to InN-1 and provides a filtered output Out0, as an average of the N inputs. The next iteration, it takes the inputs In1 to InN and provides a filtered output Out1 and so on. Equations (7) and (8) below provide a mathematical description of this filter [4].
In code, I implemented the filter as two arrays of N floats, one for storing latitudes and the other for storing longitudes. The two arrays work as First-In-First-Out (FIFO) buffers. In other words, every time a new reading has to be stored, the oldest reading is first popped out at the buffer’s tail if the buffer is already full, then all remaining values are moved toward the tail. Finally, the new value is pushed in at the buffer’s head. So, every time a new GPS reading is available, both latitude and longitude values are stored in the arrays, and then filtered latitude and longitude values can be obtained. These, in turn are used to calculate a new navigation vector.
The Arduino code available for the project prints out serial data of the original and filtered latitude readings by default. So, for instance, you can open the Serial Plotter in the Arduino IDE to see graphically the effect of the moving average filter over latitude data. In the Print_Data() function, you can comment/uncomment other types of data to be sent to the Serial Plotter. For this to work, you must have the robotic car’s Arduino board connected to your computer. Figure 7 shows a screen capture of the Serial Plotter with the moving average filter applied to latitude. The blue curve (Figure 7) shows the unfiltered latitude measurements and the red one shows the measurements after being passed through the low pass filter. The filter smooths the measurements by reducing the high frequency components (the up/down little spikes in the blue curve) generally associated with noise, but it also adds some delay to the signal (the filtered red curve is shown “ahead” of the blue one; that means it is effectively delayed in time).
— ADVERTISMENT—
—Advertise Here—
Screen capture of the Serial Plotter with the moving average filter applied to latitude data
CODE IMPLEMENTATION
The waypoints are stored in code statically in an array of type struct t_waypoint. This struct type stores latitude and longitude values for a given waypoint. At the beginning, the first waypoint in the array is set as the current goal. Once it is reached, the second waypoint is set as the current goal, and so on, until the last one. There’s a function Get_Waypoint_With_Index(int index) that returns a waypoint from the array for a given index.
In the loop() function, which is the “main” function in Arduino, the compass sensor value is read first with the function Get_Compass_Heading() to obtain the robotic car’s heading. Then, the Query_Gps() function is run to see if there are available data from the GPS receiver. If that’s the case, the Gps_Dump(gps) function is called to get the robotic car’s current latitude and longitude coordinates. Next, the function Store_Gps_Reading() is run to save those coordinates in the filter buffers. Then, the function Compute_Filtered_Gps() is called to obtain the filtered latitude and longitude values. Once we have these filtered values, the Compute_Navigation_Vector() function is run to obtain the distance and heading parameters for the navigation vector.
Next, the Control_Navigation() function is called to move the car toward the goal. This same function verifies if the current goal has been reached. If so, a waypoint_index variable that points to the current goal will be incremented to set the next waypoint as the new goal. If all waypoints have been reached, the robotic car will stop.
In code, the digital compass readings are being compensated for magnetic declination. This refers to the angle on the horizontal plane between magnetic North—the direction of the North returned by the digital compass corresponding to the direction of the Earth’s magnetic field lines—and true North, which is the direction along a meridian toward the geographic North Pole. This angle called “declination” varies not only with position on the Earth’s surface, but also changes over time. There are declination maps online, where you can find the corresponding value for your location and change it in the code. Alternately, you can ignore it altogether and set it to zero. In that case the algorithm should still work, though perhaps a little less efficiently. The buzzer beeps twice every time an intermediate waypoint is reached, and three times when the last point has been reached.
GPS always works better in open spaces, so it is preferable to test the robotic car in a wide-open area, such as a park, a football field, an open parking lot or even a basketball court. Make sure there aren’t too many nearby trees, buildings and other structures that could interfere with the GPS reception. Metallic structures, such as chain-link fences, can also interfere to some degree with the digital compass.
I used Google Maps to define the waypoint coordinates for my test routes. You can mark some waypoints in Google Maps and then copy their coordinates into the waypoints array in code, and you’re good to go. Don’t forget to change the waypoints in code to your own, or you’ll have your robotic car trying to reach waypoints in a park in my hometown!
CONCLUSIONS
Figure 8 shows the robotic car in action. I tried a couple of different maps in different places, and the robot car always completed the routes, reaching all waypoints. But I noticed that it wanders a bit when going between some waypoints, sometimes describing a parabola. When the car arrives at the area in the vicinity of the goal, it circles the area a little before reaching the goal. This was particularly noticeable when metal structures were nearby.
The GPS Robotic Car in action. It was tested using a couple of different maps in different places, and the robot car always completed the routes, reaching all waypoints.
The moving average filter for the GPS readings really made a difference. I ran the filter with 15 points—an average of 15 readings—and compared the performance without the filter. With the filter on, I noticed the robotic car wandered somewhat less than without the filter, reaching the waypoint a little faster in general.
The “stepped proportional” control strategy performed well. But sometimes the robotic car tended to oscillate when trying to correct for the heading error, or when trying to follow straight paths. This happened most often when the error was in the vicinity of an error step limit—that is, when passing from one error range to the other. Remember, the error space is divided into five error steps.
The following are some improvements I might add. First, I would like to implement a full PID controller for navigation. I think it could make navigation smoother and more effective. Second, I’d add encoders to the wheels for odometry, and then implement a Kalman filter to fuse the odometry with the GPS readings. Third, I’d write a Python application to receive telemetry data in my computer via a wireless transceiver and visualize waypoints, sensor readings, errors, filtering, trajectories and the like. And finally, it would be exciting to build a GPS robotic boat for autonomous navigation over water, beginning with some of the hardware and code from this project.
For detailed article references and additional resources go to:
www.circuitcellar.com/article-materials
References [1] through [4] as marked in the article can be found there
RESOURCES
Arduino | www.arduino.cc
HiLetgo | www.hiletgo.com
Honeywell | www.honeywell.com
STMicroelectronics | www.st.com
U-blox | www.u-blox.com
PUBLISHED IN CIRCUIT CELLAR MAGAZINE • JUNE 2019 #347 – Get a PDF of the Issue
Sponsor this Article
Raul Alvarez Torrico has a B.E. in electronics engineering and is the founder of TecBolivia, a company offering services in physical computing and educational robotics in Bolivia. In his spare time, he likes to experiment with wireless sensor networks, robotics and artificial intelligence. He also publishes articles and video tutorials about embedded systems and programming in his native language (Spanish), at his company’s web site www.TecBolivia.com. You may contact him at raul@tecbolivia.com
