Optimal path planning in a constant win with a boune turning rate Timothy G. McGee, Stephen Spry an J. Karl Herick Center for Collaborative Control of Unmanne Vehicles, University of California, Berkeley, CA 97 In this paper, we explore the problem of generating the optimal time path from an initial position an orientation to a final position an orientation in the two-imensional plane for an aircraft with a boune turning raius in the presence of a constant win. Following the work of Boissonnat, we show using the Minimum Principle that the optimal path consists of perios of maximum turn rate or straight lines. We emonstrate, however, that unlike the no win case, the optimal path can consist of three arcs where the length of the secon arc is less than π. A metho for generating the optimal path is also presente which iteratively solves the no win case to intercept a moving virtual target. I. Introuction Optimal path planning is an important problem for robotics an unmanne vehicles. In this paper, we explore a metho for fining the shortest path from an initial position an orientation to a final position an an orientation in the two-imensional plane for an aircraft with a boune turning raius in the presence of a constant win. This work was motivate by our group s work with our fleet of small autonomous aircraft. Each moifie Sig Rascal aircraft flies uner the combine control of an off-the-shelf Piccolo avionics package for low level control an an onboar PC computer for higher level tasks. These aircraft fly at a nominal spee of m/s, an win spees of over 5 m/s have been encountere uring flight testing. The problem escribe above was first solve in the case of no win by Dubins using geometric arguments 8 an later by Boissonnat using optimal control methos., The solution of this problem for the no win case has also been use for higher level path planning of mobile robots 9, an to travel aroun obstacles., Similar work has been one investigating the renezvous of a robotic manipulator with moving objects 7 an on path planning an trajectory generation for coorinate renezvous of multiple unmanne aerial vehicles (UAVs). A. Vehicle Kinematic Moel The kinematic moel consiere is shown below. The control input of the aircraft, u, is the turning rate, ψ, which is assume to be boune ψ < ψ max. V a is the constant velocity of the aircraft, an V W is the constant velocity of the win, with V a > V W. x = x y ψ ẋ = V a cos ψ + V W x V a sin ψ + V W y u () These equations of motion can be expresse in a more compact way without loss of generality by efining ψ = in the irection of the win an normalizing: Grauate Stuent, Department of Mechanical Engineering, tmcgee@me.berkeley.eu Postoctoral Researcher, Department of Mechanical Engineering, sspry@me.berkeley.eu Professor, Department of Mechanical Engineering, kherick@me.berkeley.eu of
ẋ = cosψ + β sinψ u where u < ψ max an β < is the ratio of the win spee to the aircraft spee. B. Orientation Angle vs. Velocity Direction () The goal of the optimization problem is to fin the shortest path from x i = (x i, y i, ψ i ) to x f = (x f, y f, ψ f ) in the plane. It shoul be note that in the presence of win, the irection of the velocity vector is not equal to the heaing angle, ψ. The irection of the velocity vector, α, can be efine as: ( ) ( ) ẏ sinψ α = atan = atan () ẋ cosψ + β where the atan function preserves the quarant of the angle by accounting for the sign of the numerator an enominator. Thus, if one wishes to fin the optimal path from an initial position an velocity irection (x i, y i, α i ) to a final position an velocity irection (x f, y f, α f ), each α can be converte to the corresponing ψ by where the value of the acos function is chosen so that ψ > if α >. ψ = acos( βsinα) π + α () C. Expressing the Problem as one with a Moving Virtual Target The original problem of fining an optimal path in the presence of a constant win can be re-expresse as one of fining the optimal path from an initial position an orientation with no win to a final orientation over a moving virtual target. The velocity of the virtual target is equal an opposite to the velocity of the win. The path of the aircraft in this reefine problem is its path with respect to the moving air frame, an will be referre to as the air path. If this path is flown in the presence of win, the path of the aircraft with respect to the groun will be known as the groun path. The new equations of motion for the reefine problem become: x = x y ψ ẋ = f(x, u) = cos ψ sin ψ u (5) an with x i given. We seek to fin the input such that x(t ) = x (T ), where x f βt x (t) = y f () ψ f. II. Minimum Principle Following the work of Boissonnat, insight into the problem can be gaine through the application of classical optimal control theory. Using the notation of Bryson an Ho, 5 we first efine the performance inex, J, which is equal to the intercept time, T : J = T L t = T t = T (7) In orer to enforce the maximum turning raius of the vehicle, the ynamics of Eq. 5 are ajoine to the performance inex with a multiplier λ to form the Hamiltonian, H: H = L + λ T f = + λ cos ψ + λ sin ψ + λ u (8) of
where the ynamics of the multiplier satisfies: λ = H x = sin ψ cos ψ (9) By efining the Hamiltonian an multiplier ynamics in this way, the Minimum Principle requires that on the optimal trajectory, (x, λ ), the optimal control, u must minimize H over the set of all possible inputs, u a, at each instance in time: H(x, u, λ, t) H(x, u, λ, t) u u a, t [, T ] () Since λ an λ are constant as shown in Eq. 9, satisfying the conition in Eq. is equivalent to satisfying: λ u λ u u ψ max () These necessary conitions lea to several properties that the optimal solution must have. Property The optimal path consists of intervals of maximum rate turns an straight lines. Proof: First, we note again that λ an λ are constants an that λ is a continuous function whose time erivative is zero whenever sin ψλ = cos ψλ. Given λ an λ, this equation can be use to efine a characteristic irection ψ c. During any non-zero time interval, λ is constant if an only if travel is in a straight line parallel to the characteristic irection. Now, the optimality conition in Eq. allows only three input values, ictate by the value of λ : on any segment having λ >, u = ψ max ; on any segment having λ <, u = ψ max ; on any segment having λ, we must have travel in a straight line, implying u =. Property All straight segments an changes in turning irection on the optimal path must occur on a single line in the plane. Proof: As shown by Boissonnat, using ẋ = cos ψ an ẏ = sin ψ, the equation for λ becomes λ = ẏλ ẋλ, which integrates to c = λ + xλ + yλ () where c is a constant. For fixe values of c, λ, an λ, each value of λ efines one of a family of lines, all parallel to the characteristic irection, ψ c, in the x, y plane. The line efine by λ = is the line on which all switching an straight-line travel must occur. Property The characteristic line efine in Property ivies the plane into two regions such that when following the optimal path, the aircraft executes a maximum right turn in one of the regions an a maximum left turn in the other region. Proof: Without loss of generality, we write λ an λ as λ = q cos ψ c an λ = q sin ψ c for some constant q. We efine the characteristic unit vector e c an the perpenicular unit vector e p as: [ ] [ ] cos ψ e c = c sin ψ e p = c () sin ψ c cos ψ c We also efine the vehicle velocity vector v an the win velocity vector v w as: [ ] [ ] cos ψ β v = v w = sin ψ () In terms of these vectors, we have [ λ λ ] = qe c (5) of
an λ = qv T e p () Integrating Eq., with the knowlege that λ = on the characteristic line, shows that λ = q c, where c is the signe istance from the current aircraft position to the characteristic line. This shows that λ is always positive on one sie of the characteristic line an negative on the other. Combine with the optimality conitions, this implies that the input value is always minimum on one sie of the line an maximum on the other, corresponing to maximum left an right turns. Property All intersections of the optimal path with the characteristic line will occur at the same angle. Proof: On any optimal path, the time erivative of the Hamiltonian is given by Ḣ = H t + H u u = λ u (7) Since u = on all segments, H is piecewise constant. Since λ u is continuous, H is also continuous an thus a constant. Combining Eqs. 8,, an 5, H can be written in the form H = + qv T e c + λ u (8) Since H is constant, this implies that whenever λ =, v T e c an thus the angle that the aircraft intercepts the characteristic line are also constant. This also implies that any path that meets the characteristic line at a non-zero angle can have no straight segments. III. Strategy for Generating Minimum Paths While the application of the Minimum Principle gives insight into the nature of the solution, it oes not provie a metho for actually solving for the paths. For the no win case of the problem, the generation of caniate paths using maximum turns an straight lines is straightforwar since it involves only calculating tangents to circles. If a constant win is ae, however, the groun path of the aircraft becomes a much more complicate shape, an the generation of paths consisting of maximum rate turns an straight lines is more ifficult. This problem can be aresse by iteratively solving for the optimal paths using the problem formulation with a virtual moving target outline above. Uner the virtual target formulation, the final position of the aircraft is that of a virtual target moving in a straight line. Any point along this line can be expresse by a single variable which is the istance to the initial target position. We can efine two functions T a () an T vt () where T a is the time it takes the aircraft to fly the optimal air path from its initial position an orientation to a final orientation at point, an T vt is the time it takes the virtual target to move to point. The goal of the optimization problem is then to fin the smallest at which T a () = T vt (). This approach is very similar to that use by Croft to calculate renezvous of a manipulator with moving objects. 7 The T a () function can be generate by calculating the length of each of the amissible Dubins path types (RSR,LSL, RSL, LSR, RLR outer, LRL outer ) an letting T a equal the minimum of these lengths ivie by the constant vehicle spee. For these six path types, R enotes a right turn with a minimum raius of curvature, L a left turn with minimum raius of curvature, an S a straight line. The outer subscript inicates that the arc length of the center curve is greater than π. The RLR outer an LRL outer curves only exist when the centers of the outer circles are less than R min apart, an the LSR an RSL only exist when the outer circles have centers greater than R min apart. Examples of the six types of amissible paths are shown in Figure for x i = (,, ) an x f = (.5,.5, π). We next efine a new function which is the ifference of T a () an T vt (). By calculating where =, we fin points where the aircraft will intercept the virtual moving target. ef = T a () T vt () (9) Remark is piecewise continuous, an all iscontinuities occur when the RSL an LSR paths fail to exist. of
5 5 RSR LSL 5 LSR 5 RSL 5 5 RLR outer LRL outer Figure. Example of six amissible Dubins paths. Proof: It was shown by Bui that the length of the optimal path is piecewise continuous in the no-win case, an that iscontinuities occur when the RSL an LSR paths fail to exist. Since the virtual target follows a continuous path, the ifference of T a () an T vt () must also be piecewise continuous. An example of the iscontinuity is shown in Figure. For this example, x i = (,, ) an x f = {(.95,, ), (.5,, )}. Note that a small shift in the final position to the right prouces a iscontinuity in the optimal path length when the LSR curve comes into existence. Remark G() > an lim () <. Proof: T vt () = since the virtual target is alreay at point, an T a () > since the aircraft requires some positive time to fly to any point ifferent from its own starting point. Thus T a () T vt () >. As approaches, the majority of the path of the aircraft is a straight line that is nearly parallel to the path of the virtual target. Since β <, if is mae large enough, the aircraft will always reach point before the virtual target, making <. Given Remark, it is clear that must either equal zero or have a iscontinuity across zero for at least one value of. For given initial an final positions an orientations, the resulting function can thus be 5 of
.5 LSR RSR.5.5.5.5.5.5.5.5 Figure. Discontinuity in optimal path length for small change in final position. classifie as one of two types. Type This first type of function consists of all functions such that = at some =, an > for all <. Examples of plots of Type are shown in Figure. 5 8 5 5.5.5.5.5.5 5....8....8 5 5....8....8 5 7 Figure. Examples of functions of Type. Type This secon category consists of any functions that o not belong to the first. Examples of plots of Type are shown in Figure. Remark For any function of Type, the point represents the first point at which the aircraft can intercept the virtual target at the specifie final orientation. of
8 7 8 5.5.5.5.5 Figure. Examples of functions of Type. Proof: Assume is not the first point at which the aircraft can intercept the virtual target. Thus there is some ˆ < such that G( ˆ) =. However, by the efinition of Type, > for all <. Thus by contraiction is the first point at which the aircraft can intercept the virtual target. Remark The existence of the Type functions for implies that for some initial an final positions an orientations, the aircraft must fly a non minimum istance air path to the optimal interception point on the path of the virtual target. Proof: The function is calculate using the optimal aircraft path to each point. Thus if the function is never equal to zero, the aircraft can never intercept the virtual target by flying an optimal air path. Remark implies that for some bounary conitions, the aircraft must fly a longer air path to the optimal interception point in orer to allow the virtual target to catch up. From the use of the Minimum Principle, we know that in orer to optimally intercept the target, the aircraft can only use straight lines an arcs of minimum turning raius. The first option to o this is to use one of the longer Dubins paths. For example in Figure, the LSR path is the shortest, an the remaining paths shown are the longer Dubins paths. The secon option is to introuce RLR inner an LRL inner curves where the inner subscript refers to the fact that the secon curve has an angle less than π. Examples of these lengthene air paths are shown below in Figure 5. In orer to further analyze the problem with the aition of these extra path types, we now efine a new family of functions: G X () ef = T Xa () T vt () () where X enotes the type of path incluing the six Dubins paths (RSR, RSL, etc.) an the aitional RLR inner an LRL inner paths, an T Xa () enotes the time it takes the aircraft to fly to a specific point on the virtual target path using that specific path type. It is important to note that not all eight of the G X () functions are efine for every value of. Examples of these G X () functions are shown below in Figure....5..5..5...5.8....8....8 5 Figure 5. Examples of lengthene air paths to a given. Remark 5 If there is a function of Type, there will be either a RLR outer, LRL outer, RLR inner path that intercepts the virtual target. LRL inner or 7 of
.5.5.5.5 7 G RLRinner G zero line G RSR G zero line 8 5 8 8 Figure. Examples of G RLRinner () an G RSR () functions. Proof: Consier the case where the jump across zero occurs when the LSR path comes into existence. Before the jump, G LRLouter () > since > an G LRLouter (). At the jump iscontinuity G LRLinner () < because the LRL inner path is equivalent to the LSR path. This occurs because the length of the secon left arc in the LRL inner path is equal to zero, an the straight portion of the LSR path is equal to zero. This leaves two LR paths which are equivalent. As increases, the centers of the two left arcs in the LRL outer an LRL inner curves move further apart. When the two centers are exactly R min apart, the LRL outer an LRL inner curves are equivalent, an the arc length center curve is π. Because G LRLouter () > an G LRLinner () < an they converge to the same value, one of them must cross zero. This convergence is illustrate in Figure 7. LRL outer.5 LRL inner.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5 Figure 7. Convergence of LRL inner an LRL outer paths. Remark The optimal path from x i to x f for a aircraft with a minimum turning raius in the presence of a constant win will be one of the following set of curves {RSR, LSL, RSL, LSR, LRL outer, RLR outer, LRL inner,, RLR inner, SR π S, or SL π S). Proof: It was shown using optimal control methos that the optimal path consists of perios of maximum turning rate an straight lines. It was also shown that for bounary conitions which prouce Type functions, the optimal path is one of the stanar Dubins curves. For Type paths, the iscontinuity occurs when either a RSL or LSR comes into existence, proucing a jump of less than πr min. The optimal 8 of
renezvous point with the virtual target will be foun by either using the shortest path that oesn t cause a iscontinuity, which is one of the longer Dubins paths, or by lengthening the RSL or LSR path that prouce the iscontinuity. The most that a RSL or LSR path can be lengthene without increasing the length of the curve more than πr min is to use a LRL inner or RLR inner path. Finally, Remark 5 guarantees that one of these types of curves will always intercept the virtual target. The SL π S an SR π S paths can be thought of as limiting cases for RSR an LSL where one of the curves approaches zero arc length an the secon curve approaches an arc length of π. These path types are optimal for only a small set of bounary conitions where the initial an final heaing angles must be ientical. An example of this is when x i = (,, ) an x f = ( 5.5,, ) with β = -.9. This correspons to a aircraft with a final position irectly behin the initial position flying into a strong heawin. The optimal path in this case is to fly a full circle an a very short straight section into the win (in the irection of the virtual target path). The straight section of this path can be ivie arbitrarily before an after the full circle. Examples of these paths are shown in Figure 8. Notice that the bounary conitions an path lengths are equivalent. While paths of type SR π S or SL π S are iscusse here for completeness, they can be isregare in practice by always changing ψ i or ψ f by a negligible ranom amount if they are ever equal. This will cause any possible SR π S or SL π S path to become a RSR or LSL path..5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5.5 Figure 8. Example of SR π S paths. IV. Final Solution Strategy Solving for = is not aequate to fin the optimal path for all sets of bounary conitions since it fails for functions of Type. In orer to overcome this, a G X () function can be create for each path type in the extene set of Remark. For each of these functions, the values are calculate where that function is equal to zero. The optimal path is chosen as the curve which prouces the smallest. The values can calculate using a variety of stanar iterative root fining methos such as a bisection algorithm or graient escent algorithm. V. Examples Several examples of optimal paths in the presence of win are shown in Figures 9,,, an. For Figure 9, x i = (,, π/) an x f = (.5,, ). For Figure, x i = (,, π/) an x f = (5,, π). In these first two examples, the optimal renezvous paths are LRL outer an RSR respectively, an represent cases that prouce Type functions. The secon two examples illustrate bounary conitions that prouce Type functions. For Figure, x i = (,, ), x f = ( 5.5,, ), an the renezvous path is a RSR path which is a lengthene air path. For Figure, x i = (,, ), x f = (.,.5, ), an the renezvous path is of type LRL inner. VI. Conclusions an Future Work A stuy of the problem of fining the optimal path from an initial position an orientation to a final position an orientation for an aircraft with a limite turning raius in the presence of a constant win has been presente. It was shown that the set of amissible optimal paths is greater than in the case with no 9 of
.5 groun path air path.5.5.5 win.5.5.5.5 Figure 9. Example of optimal LRL outer path in presence of win β =.5..5 groun path air path.5.5.5 win.5.5.5.5.5.5 5 Figure. Example of optimal LSR path in presence of win β =.5. win. An iterative metho for solving for the paths has also been presente an several examples of optimal paths in the presence of win have been presente. Current work is being one to implement this algorithm on our actual fleet of aircraft to optimally visit a set of points in a constant win. In orer to account for small variability in the win, a turning rate less than the actual maximum turning rate will be use to calculate a near optimal path. This near optimal path can then be use as a trajectory that can be followe in the presence of isturbances. Acknowlegments This work was supporte by the ONR grant #N--C-87, SPO #7- an a Berkeley Fellowship for Mr. McGee. References Pankaj K. Agarwal, Prabhakar Raghavan, an Hisao Tamakai. Motion planning for a steering-constraine robot through moerate obstacles. In Proceeings of the Twenty-Seventh Annual ACM Symposium on the Theory of Computing, 995. Ranal W. Bear, Timothy W. McLain, an Michael Goorich. Coorinate target assignment an intercept for unmanne air vehicles. In Proceeings of the IEEE International Conference on Robotics an Automation,. of
groun path air path win 7 5 Figure. Example of optimal RSR path in presence of win β =.9..5 groun path air path.5.5 win.5.5.5.5 Figure. Example of optimal LRL inner path in presence of win β =.9. Antonio Bicchi, Giuseppe Casalino, an Corrao Santilli. Planning shortest boune-curvature paths for a class of nonholonomic vehicles among obstacles. Journal of Intelligent an Robotic Systems,, 99. Jean-Daniel Boissonnat, Anre Cerezo, an Juliette Leblon. Shortest paths of boune curvature in the plane. In Proceeings of the 99 IEEE Int. Conf. on Robotics an Automation, 99. 5 Arthur Bryson an Yu-Chi Ho. Applie Optimal Control: Optimization, Estimation, an Control. John Wiley an Sons, New York, New York, 975. Xuan-Nam Bui, Jean-Daniel Boissonnat, Philippe Soueres, an Jean-Paul Laumon. Shortest path synthesis for ubins non-holonomic robot. In Proceeings of the 99 IEEE Int. Conf. on Robotics an Automation, 99. 7 Elizabeth Croft, R.G. Fenton, an Beno Benhabib. Optimal renezvous-point selection for robotic interception of moving objects. IEEE Transactions on Systems, Man, an Cybernetics PartB: Cybernetics, 8, 998. 8 L.E. Dubins. On curves of minimal length with a constraint on average curvature, an with prescribe initial an ternminal positions an tangents. American Journal of Mathmatics, 79, 957. 9 Zhijun Tang an Umit Ozguner. On motion planning for multi-target surveillance with limite resources of mobile sensor agents. Accepte to the IEEE Journal of Robotics,. Guang Yang an Vikram Kapila. Optimal path planning for unmanne air vehicles with kinematic an tactical constraints. In Proceeings of the st IEEE Int. Conf. on Decision an Control,. of