There is a simple way to implement waypoint navigation. The algorithm was introduced by Lawrence, Frew, and Pisano back in 2008. You can view the paper here: http://dx.doi.org/10.2514/1.34896.
I have flight tested this algorithm multiple times on a low-resource autopilot embarked on a fixed-wing UAV (see figure below). In this blog post, I want to share my approach to implementing this algorithm. I cannot claim any academic novelty. However, I hope to make this simple and effective algorithm more accessible to other researchers/hobbyists.
In the remainder of this post, I will provide a high-level view of the algorithm. I will also show some flight test results. If the reader wants a more mathematical treatment of the implementation, the reader is referred to the attached PDF.
The algorithm is simple because it uses a single guidance vector field. Vector fields, in general, assign a vector to each point in space. Guidance vector fields, in particular, assign a desired velocity vector to each point in a 2D plane. The vector fields are designed such that they guide the vehicle to a particular path. In this case, the vector field brings the vehicle into a loiter circle. The following figure shows the vector field. The vector field also generates a desired acceleration vector. The acceleration vector is needed for accurate path following.
We feed the guidance commands from the guidance vector field into the lateral control system. However, the input to the lateral control system is a bank angle (see figure below). The block labeled “AC” represents the aircraft dynamics and the block labeled “Ail” represents the dynamics of the aileron actuator.
Thus, it is necessary to convert the desired velocity and the desired acceleration into a desired bank angle. We use a two-step conversion.
First, we convert the guidance commands into a lateral acceleration command. The lateral acceleration command has two terms. The first term drives the angular misalignment between the vehicle velocity and the desired velocity to zero. The second term incorporates the desired acceleration vector; the second term functions as a feed-forward acceleration term needed for accurate path tracking.
Second, we convert the lateral acceleration command to a desired bank angle. The relationship between lateral acceleration and bank angle is illustrated in the figure below. By banking, the lift force attains a lateral component, which produces the desired lateral acceleration.
Having described basic loiter circle tracking, we are ready to move on to waypoint navigation. The waypoint navigation routine is actually loitering in disguise: the routine positions loiter circles so that the vehicle smoothly intersects each waypoint in succession. The positioning algorithm is shown in the figure below. The positions of the previous waypoint, current waypoint, and next waypoint are denoted by A, B, and C, respectively. The center point lies along the bisector of angle ABC. The loiter radius sets the distance between the center point and the current waypoint. Having determined the loiter center, the next step is to determine the sign of the circulation constant. If C is to the left of the line AB, then the circulation is counter-clockwise (<0). If C is to the right the line AB, then the circulation is clockwise (>0).
A nice feature of the positioning routine is that it can work with only two waypoints. The waypoints are stored in a circularly linked list. Hence, the "next" waypoint and the "previous" waypoint can point to the same waypoint.
Next, we describe the algorithm that governs how the aircraft switches from one waypoint to the next.
The positions of the current waypoint and next waypoint are denoted by A and B, respectively. Let LA denote the loiter circle that brings the aircraft into A. Suppose the aircraft has just "hit" the current waypoint. The navigation routine sets the current waypoint to B and computes the parameters of LB, the loiter circle that brings the aircraft into B.
Now, the straightforward approach is to immediately switch from using LA to LB. However, this approach will change the guidance vector in an abrupt manner. To achieve a smooth transition between waypoints, the switch from LA to LB occurs when the velocity vectors from both loiter circles are pointing in roughly the same direction. A smooth transition protects the low-level control system from large changes in the input command.
The figure below illustrates the switching algorithm. In the top plot, the aircraft is about to hit A. In the second plot, the aircraft has hit A and has set up the loiter circle of B. The aircraft, however, continues tracking the loiter circle of A. In the third plot, the guidance vectors from both loiter circles are aligned. The aircraft begins tracking the loiter circle of B. In the fourth plot, the aircraft is en route to B.
I implemented the guidance system described herein using fixed-point arithmetic on the AMP autopilot, which is made in-house by our research group (see https://doi.org/10.2514/1.I010445). The microprocessor belongs to the DSPIC33 family of microprocessors made by Microchip.
Flight test results are shown in the figures below (map data ©2020 Google). The test took place at the Flying Gators RC Airport in Archer, FL. The first plot shows the flight path of the delta wing UAV performing waypoint navigation with four waypoints. The waypoints are positioned to create a figure-eight trajectory. The second plot shows the flight path of the delta wing UAV performing waypoint navigation with two waypoints. You can view a synthetic video of the flight test here: https://youtu.be/otRW2_80G0U.The video is reconstructed from downlinked telemetry data. You can view an actual video of a portion of the flight test here: https://youtu.be/jQyc3_tk7MA.
In the first plot, we note that there is a clear asymmetry in the flight pattern. This asymmetry was due to the aircraft being out of trim. When I examined the roll data after the flight test, I found that the aircraft was better at tracking negative roll commands as opposed to positive roll commands. Hence, the asymmetry has to do with the control system, not the guidance system.
In conclusion, this blog post provides an overview of a simple and effective waypoint navigation scheme. For a more mathematical treatment, the reader is referred to the attached PDF. I have also attached a MATLAB code that simulates loiter circle tracking. The vehicle dynamics are represented using a matrix state-space model.
Comments
There were two reasons we wanted our own autopilot. First, we wanted an all-in-one board that had a GPS receiver, barometer, 9-axis IMU, temperature/humidity sensor, and a telemetry radio. It was important to have a single, compact board because the board was designed to control a micro aerial vehicle. Second, we were worried that commercial hardware would become unavailable/discontinued after a few years. For these reasons, my professor thought it worth the investment to develop our own board.
There are definite disadvantages to using your own hardware. Soldering the boards is tedious. Also, people outside your group cannot use your code without porting it to another system.
Knowing what I know now, I probably would have preferred using a commercial autopilot. The UDB5, for example, is very similar to our home-brew autopilot. Eventually, I want to port my autopilot code to that system.