This paper considers a variation on the Dubins path problem and proposes an improved waypoint navigation (WN) algorithm called Dubins waypoint navigation (DWN). Based on the Dubins path problem, an algorithm is developed that is updated in real-time with a horizon of three waypoints. The purpose of DWN is to overcome a problem that we find in existing WN for small-class fixed-wing unmanned aerial vehicles (UAV) of not accurately reaching waypoints. This problem results at times in high overshoot and, in the presence of wind disturbances, it can cause a vehicle to miss the waypoint and swirl around it. To prevent this, the DWN creates “new waypoints” that are in the background, called turning points. Examples illustrate the improvement of the performance of WN achieved using the DWN algorithm in terms of the targeting of waypoints while reducing fuel and time.
The user community of unmanned aerial vehicle (UAV) systems has been growing significantly; the commercial market net worth reached a reported $8.3 billion in 2018 [
The shortest path between two waypoints with a limited turning radius is called a Dubins path [
This paper considers a variation on the Dubins path problem and proposes an improved navigation algorithm called Dubins waypoint navigation (DWN). The proposed DWN algorithm considers a current vehicle state (position and velocity) along with the next two prescribed waypoints to plan the vehicle’s path. Compared with traditional WN, one more waypoint is involved in the determination of the path, enabling consideration of the manner in which the vehicle should circle around its previous waypoint. The objective is similar to the Dubins path problem in that a shortest path and a minimum fuel path between waypoints consists of straight-line segments and arcs. The major difference is that in DWN the heading at the waypoints are not specified and we consider three points instead of two. Also, for simplicity, the paper only considers the case in which the vehicle’s turning radius is constant.
The interest here lies in an algorithm that is updated in real-time with a horizon of three waypoints. The proposed DWN algorithm employs a current vehicle position and heading for the first waypoint, uses a prescribed vehicle position for the second waypoint, not specifying its heading, and uses a third waypoint to determine how the vehicle should circle around the second waypoint.
The purpose of DWN is to overcome a problem that we find in existing WN for small-class fixed-wing vehicles of not accurately reaching waypoints. This problem results from the present use of a waypoint radius as a threshold that determines when to move on to a next waypoint. Presently, a vehicle approaches a first waypoint and switches to navigating to a second waypoint when the vehicle is within a waypoint radius around the first waypoint, thereby not accurately reaching the first waypoint. This leads the operator to desire a smaller waypoint radius. However, the smaller waypoint radius causes several problems. First, it causes high overshoot. Secondly, in the presence of wind disturbances, it can cause a vehicle to miss the waypoint and swirl around it.
When correcting this problem, it became important to preserve the simplicity of prescribing waypoints without adding operator complexity. Therefore, the DWN does not have to change anything in the foreground, including the operator’s specified waypoints. Instead, the DWN creates “new waypoints” that can be in the background, called turning points, as explained in the method section.
Section 2 describes the DWN algorithm. Section3 compares performances of WN and DWN with and without unknown wind disturbances, and the paper finishes in Section 4 with a summary and conclusions.
This section develops the DWN. The DWN eliminates waypoint circles and, instead, employs the new concept of turning points along with the new criterion for updating turning points based on whether or not a turning point is reachable. See
Consider the illustrative WN problem shown in
The vehicle starts at W0 and heads to W1. At this point, the DWN considers waypoints W0, W1, and W2. The other points are beyond the horizon.
As shown, T1 denotes the turning point associated with W1. It lies on the line tangent to a turning circle (not to be confused with the waypoint circle in classical WN) that has a turning radius R. The turning radius, specified by the user, is the desirable radius of the turns around waypoints. An optimization problem minimizes the fuel from W0 around and touching W1 to W2. The orientation of the turning circle is free to rotate about W1 so the optimization problem is a minimization problem of fuel expressed in terms of the turning circle’s orientation. The fuel consumed during a turn is greater than the fuel consumed while flying in a straight line over the same distance. Even though the minimization is over the distance extending to W2, the DWN updates the vehicle’s path once it reaches point T1. Thus, the vehicle does not follow the section of the path from T1 to W2 (In
1The x axis, because it is along the line through C and Wn, can cause β to exceed 90˚.
As shown, α is the heading angle (between –180˚ and 180˚) and β is the turn angle (β is between 0˚ and 90˚)1. We assume that there are no sharp turns between specified waypoints (the angle between any two lines connecting three consecutive waypoints is never greater than 90˚). At point C, the vehicle turns
counter-clockwise (CCW), or clockwise (CW). Likewise, it turns around Wn CCW or CW. In total, there are four cases: CCW-CCW, CCW-CW, CW-CCW, and CW-CW.
The interest lies in finding closed-form expressions for the paths as functions of A, B, α and β by curve fitting. Toward this end, note that the minimum fuel paths are continuous with respect to these parameters for each case, individually. Discontinuities occur when transitioning from one case to another. Therefore, the curve-fitting needs to be performed for each case and the case that consumes the smallest amount of fuel yields the true minimum (See
F u = w a L a + w s L s (1)
where La is the total length of the two arcs (around the first and second turning circles), Ls is the length of the two straight segments (after the first and second turning circles), and wa and ws are corresponding weights (in units of fuel per length). Letting wa = ws = 1 (in which the weights have units of 1), yields the shortest path problem:
L = L a + L s (2)
In the results section, we will show that the shortest path and minimum fuel solutions are nearly indistinguishable under a broad range of conditions, allowing the shortest path solution to approximate the minimum fuel solution. This is important because the shortest path solution is independent of the vehicle, increasing ease of implementation and the versatility of DWN.
Referring again to
The coordinates x1, y1, x2, y2, x3, and y3 for each of the four cases are given in
By appropriately manipulating the geometric relationships, the path length (the objective function) becomes a function of the orientation angle θ of the turning circle, and the solution of this 3-point optimization problem becomes a function of θ. This part of the iteration can be solved off-line and curve-fit to determine Tn. There are two ways to fit Tn. One way is to directly fit Tn to A, B, α, and β. The second way is to fit θ to A, B, α, and β followed by determining Tn from θ. The second way of curve fitting the data will inherently lead to a more accurate curve fit than the first way because the relationship between Tn and θ is rather complicated and its analytical relationship is available, too [
| x 1 | y 1 | x 2 | y 2 | |
|---|---|---|---|---|
| CW-CW | − A + sin α + cos γ | − cos α + sin γ | cos θ + cos γ | sin θ + sin γ |
| γ = arctan sin θ + cos α cos θ + A − sin α + π 2 | ||||
| CW-CCW | − A + sin α + cos γ | − cos α + sin γ | cos θ + cos γ | sin θ + sin γ |
| γ = arctan sin θ + cos α cos θ + A − sin α + arccos 2 ( sin θ + cos α ) 2 + ( cos θ + A − sin α ) 2 | ||||
| CCW-CW | − A − sin α + cos γ | cos α + sin γ | cos θ − cos γ | sin θ + sin γ |
| γ = arctan sin θ − cos α cos θ + A + sin α − arccos 2 ( sin θ − cos α ) 2 + ( cos θ + A + sin α ) 2 | ||||
| CW-CCW | − A − sin α + cos γ | cos α + sin γ | cos θ − cos γ | sin θ − sin γ |
| γ = arctan sin θ − cos α cos θ + A + sin α − π 2 | ||||
| x 3 | y 3 | |
|---|---|---|
| CW-CW & CCW-CW | cos θ + cos δ | sin θ + sin δ |
| δ = arctan B cos β − cos θ B sin β − sin θ + arccos 1 ( B cos β − cos θ ) 2 + ( B sin β − sin θ ) 2 | ||
| CW-CCW & CCW-CCW | cos θ + cos δ | sin θ + sin δ |
| δ = arctan B cos β − cos θ B sin β − sin θ − arccos 1 ( B cos β − cos θ ) 2 + ( B sin β − sin θ ) 2 | ||
As described earlier, the DWN guides a vehicle to a waypoint until it becomes unreachable, at which point it updates the turning point.
Note that the purpose of “enclosing” the CCW and CW with the line segment was to prevent the possibility of point C passing Tn undetected, which would otherwise be possible because the detection is not truly performed continuously but only at discrete instances of time.
Comparing the reachability zone used in DWN with the waypoint circle used in WN, first notice that the reachability zone, in the absence of disturbances, allows the vehicle to accurately reach a first waypoint, as opposed to beginning its turn to a second waypoint before reaching the first waypoint. Secondly, in WN, the operator can easily be fooled into the natural desire to select a waypoint radius that is smaller than the vehicle’s turning radius toward the goal of more closely reaching a waypoint. However, when doing so in the presence of a wind disturbance, this can result in missing the waypoint, finding it difficult to reach, and causing the vehicle to swirl around the waypoint. In the DWN, no waypoint radius is considered.
We formulated the 3-point horizon optimization problem to keep the computational effort minimal for real-time implementation. To further reduce computational effort and for robustness, we parameterized the solution, effectively reducing it to a look-up table. Toward this end, we determined the orientation angle θ of a turning circle as a function of the four parameters A, B, α, and β. We parameterized the orientation angle by smoothly fitting the data to the four parameters separately for each case (CCW-CCW, CCW-CW, CW-CCW, and
CW-CW). Toward distinguishing between the different cases, we needed to parameterize the boundaries of each of the cases in terms of the four parameters. In particular, α and β transition from case to case due to changes in A and B. For example, consider the graph of α versus β shown in
As shown, there are three boundaries and three interior regions. (It turns out that the CW-CW case never produces an optimal orientation angle.) We express the boundary of the turn angle β as a function of the heading angle α in which its coefficients are expressed as a function of the distances A and Bas follows:
β = { a 0 + a 1 δ + a 2 δ 2 α ≥ − a 1 / 2 a 2 4 a 0 a 2 − a 1 2 4 a 2 α ≤ − a 1 / 2 a 2 2 (3)
a i = b 0 i + b 1 i A + b 2 i A 2 + b 3 i B + b 4 i B 2 + b 5 i A B ( i = 1 , 2 , 3 ) (4)
We determined the coefficients b0i through b6i (i = 1, 2, 3) separately for each of the cases. The real-time procedure of updating the turning points is as follows:
1) Calculate A, B, α, and β at an instant of time from a current state of the vehicle.
2) Calculate the coefficients associated with the boundaries between the interior regions (See [
3) Determine the interior region in which the vehicle lies.
4) Calculate the orientation angle θ of the next turning circle (See [
5) Calculate the next turning point Tn (expressed in terms of θ analytically).
Let us continue with the 6-waypoint example and compare WN and DWN. In WN, navigation performance depends on the minimum turning radius and the waypoint radius. In DWN, navigation performance depends on minimum turning radius alone. The feedback control algorithms are the same for WN and DWN. However, the resulting overshoots differ. An illustrative feedback control algorithm described below accounts for turning toward an endpoint limited by the vehicle’s minimum turning radius R and serves as a basis for the comparison between WN and DWN.
Consider
ψ i = { ψ max ϕ T i + ϕ R i > ψ max − ψ max ϕ T i + ϕ R i < − ψ max ϕ T i + ϕ R i | ϕ T i + ϕ R i | ≤ ψ max ϕ R i = | g s i + h s ˙ i | ϕ T i | ϕ T i | ψ max = v Δ t R (5)
Above, ϕTi is a target angle and ϕRi is a reference angle (all of the angles are positive counter-clockwise), g and h are control gains, and ϕ T / | ϕ T | determines whether ϕR is counter-clockwise or clockwise.
The parameters used in the results, both with and without wind disturbances, are as follows: The minimum turning radius was R = 0.7, the time step was 0.02, the vehicle speed was v = 0.8, and the control gains were g = 0.1 and h = 5.25. With these parameters the turning radius was 0.7.
Let us compare the minimum fuel and the shortest path solutions, the latter determined when wa = ws = 1. In practice, the distance between waypoints is at least four times larger than the turning radius R. Therefore, we will only consider distances A and B that are more than or equal to 4 times greater than R. When the distances A and B are exactly 4 times greater than R, the vehicle is flying in a straight line the least amount of the time and, for this case, we expect the difference between the minimum fuel solution and the shortest path solution to be the greatest.
acceptable in the vast majority of small-class UAV problems. This finding is significant because the shortest path solutions apply to any vehicle whereas the minimum fuel solutions need to consider vehicle parameters. In the results below, we employ the shortest path solutions.
First consider DWN (solid lines). In each case, the vehicle passes through waypoints exactly and follows the minimum fuel paths—by design. Next, consider WN. In the first case, the vehicles passes close to the waypoints but with significant overshoot. In the other two cases, we see as the waypoint radius increases that the vehicle passes farther from the waypoints. In the third case, the vehicle undershoots the desired path. Note that in the WN and DWN cases given above, the feedback control had very little effect on the response because there was no unknown wind disturbance.
In the left figure, the waypoint radius was set to be small, intending to achieve higher accuracy. However, the wind exceeds the capability of the feedback controller and the vehicle just misses the second waypoint sending it into a swirl. DWN also misses the turning point paired with the second waypoint but moves on to complete its journey. It can do this because it recognizes the waypoint to be unreachable. In the middle and the right figures, the waypoint radii are larger and WN manages to navigate the vehicles. It is worth noticing that errors exist
for both WN and DWN. The DWN errors result from using the turning points for navigation instead of waypoints. It is clear that the DWN errors are smaller than the WN errors due to its features already discussed, hence one still recognizes that DWN improves the performance of the WN navigation.
WN suffers from an existing trade-off between minimum turning radius and waypoint radius that prevents vehicles from reaching waypoints and closely following desired paths. This paper developed an algorithm, called Dubins waypoint navigation (DWN) that remedies this problem by not using waypoint circles. Instead, we introduced a reachability zone and turning points. The DWN significantly reduces undershoot, overshoot, and swirl. Furthermore, the algorithm remedies the problem in a way that can be hidden from the operator to avoid confusion. The paper showed, by establishing a horizon that includes two future waypoints, how to improve the performance of WN in terms of the targeting of waypoints while reducing fuel and time. Pertaining to real-time implementation, we also reduced the computational effort, essentially eliminating it, by parameterizing the shortest path solution. We also compared WN and DWN tracking performance (in the absence of an unknown disturbance) and regulation performance (in the presence of an unknown disturbance). The comparisons are illustrative of the improvements in path following (tracking and regulation) obtained by DWN.
The authors gratefully recognize the Namibia Wildlife Aerial Observatory (WAO) for its support of this work.
The authors declare no conflicts of interest regarding the publication of this paper.
Silverberg, L.M. and Xu, D.H. (2019) Dubins Waypoint Navigation of Small-Class Unmanned Aerial Vehicles. Open Journal of Optimization, 8, 59-72. https://doi.org/10.4236/ojop.2019.82006
A = distance between current position and the following waypoint
a = coefficient for curve fitting β with respect to δ
B = distance between the following two successive waypoints
b = coefficient for curve fitting a with respect to A and B
C = current position
Fu = total fuel cost
g = proportional term for PID controller
h = derivative term for PID controller
La = total length of the arcs
Ls = total length of the straight segments
R = turning radius of the vehicle
T = target point
v = speed of the vehicle
W = waypoint
wa = fuel cost per unit length travelled on an arc
ws = fuel cost per unit length travelled on a straight line
α = angle of heading in local system
β = angle between the following two successive waypoints in local system
γ = angular position of the first tangential point in local system
δ = angular position of the last tangential point in local system
θ = angular position of the center of the turning circle
ϕi = angle that denotes the heading in global frame at the th time instance
ϕR = angle of turning resulted from the feedback controller
ϕT = angle between heading and the target
ψ = angle of turn in one time step