Professional Documents
Culture Documents
1
V
0
R
total max
0.8
ω
2.5
0.4
2
0.2
V0
1.5 0
−0.2 1 2
−1 −0.8 −0.6 −0.4 −0.2 0 0.2 10 10 10
Relative Turn Rate ω / ω
max
Fig. 1: Cornering paths allowed by the constraints.
Rmin is the designed minimum radius of curvature. The Fig. 2: Variation of execution time with turn rate for
dashed path is allowable within the constraints, but will the maneuver shown in Fig. 1.
always take longer than the solid path.
aircraft remains at maximum speed and turns at the
change of heading angle is limited by the maximum prescribed maximum turning rate ωmax , therefore fol-
bank angle of the aircraft. Writing these constraints lowing the prescribed minimum radius Rmin . It is also
exactly results in nonlinear expressions, which cannot feasible to follow the dashed path, decelerating first,
be handled in a MILP framework. Therefore, the air- then applying the maximum force to achieve a smaller
craft is approximated here as a point mass, moving radius of curvature, before accelerating back to maxi-
with limited speed and acted on by a force of limited mum speed and rejoining the solid path. Although it
magnitude. The optimization seeks the minimum time is allowed in the linear model, this trajectory is not
solution, so it is favorable to remain at, or near, the possible for a real aircraft. Fig. 2 shows the variation
maximum speed throughout the entire maneuver. in total maneuver time with the radius of turn used.
This was found analytically by calculating the time for
The turning rate constraint is effected by a force mag- the turn itself, the necessary deceleration and acceler-
nitude limit. Consider a mass m traveling with speed ation times, and the adjoining segments of maximum
V subject to a force of magnitude f . The instantaneous speed travel. It is clear from the figure that the fastest
turning rate ω will be greatest if the force is perpen- turning maneuver is achieved by remaining at maxi-
dicular to the velocity. It is therefore limited by mum speed and obeying the nominal maximum turn-
f ing rate ωmax . Using higher turning rates leads to a
ω≤ (1) slower overall maneuver, due to the additional deceler-
mV
ation required. This result matches intuition, because
Furthermore, if the magnitude of the force is limited to the solid path in Fig. 1 is shorter in length than any
fmax and the speed is a constant V0 , the rate is limited others and has a higher average speed.
throughout the problem by
When avoidance constraints are added, some arrange-
fmax ments of obstacles could cause the model to favor the
ωmax = (2)
mV0 tighter turn. Therefore, it is necessary to post-analyze
each trajectory to ensure it is flyable by real-world dy-
In this implementation of the problem, there is only namics. If not, the problem can be rerun with a lower
an upper bound on the speed. The inclusion of this force limit until an acceptable solution is found.
magnitude limit as linear constraints is shown in Sec-
tion 2.2. It is feasible for the speed to fall below V0 ,
2.2 Dynamics Constraints in the Problem
allowing tighter turns than the bound in Eqn. 2. How-
To analyze the types of problems discussed in the In-
ever, for the minimum time solution, it is favorable to
troduction, assume that there are N aircraft, each ap-
remain at maximum speed and obey the specified turn
proximated as a point mass moving in 2-D. The posi-
rate limit.
tion of aircraft p at time step i is given by (xip , yip )
For example, consider the situation shown in Fig. 1, and its velocity by (vxip , vyip ), forming the elements of
in which an aircraft must turn through 90o from one the state vector sip . Each aircraft is assumed to be
trajectory on to another. Two of the possible paths acted upon by control forces (fxip , fyip ) in the X- and
are shown in this figure. Following the solid line, the Y-directions respectively, forming the force vector fip .
The discretized dynamics of the overall system, applied
to all N vehicles up to T time steps, can be written in 1.5
∀p ∈ [1 . . . N ] ∀i ∈ [0 . . . T ]
1
(3)
s(i+1)p = Ap sip + Bp fip 0.5
max
v /v
0
y
s0p = sIp (4)
−0.5
where sIp is the initial state of vehicle p.
As discussed previously, both the velocity of the air- −1
craft and the force acting upon the aircraft are subject
to magnitude constraints. The exact representation of
−1.5
these constraints would be nonlinear, but they can be −1.5 −1 −0.5 0 0.5 1 1.5
v /v
approximated by linear inequalities. The true magni- x max
is used to solve the problem [13]. A series of scripts 8 Turn rate 15 o/s
o
in Matlab and AMPL allow the entire path-planning Turn rate 12 /s
Y
4 Examples
4
this paper. The dynamics were discretized with a time Fig. 4: The designed trajectories for an aircraft moving
step of two seconds. The initial and final states are through obstacles. Paths for two different turn rate
shown in Table 4.1. settings are shown.
The real vehicle has a maximum speed vmax =
0.225m/s and turn rate ωmax = 15o /s. Using Eqn. 2,
this corresponds to a force limit fmax = 0.294N. A find a different solution. Reducing the force limit in the
force penalty weighting = 0.001 was used. The cir- model by 20%, corresponding to a nominal turn rate
cles in Fig. 4 mark the trajectory design using these limit of 12o /s, gives the result shown by the triangles in
values. It avoids all the obstacles and reaches its tar- Fig. 4. Since the available force has been reduced, the
get after 60 seconds or 30 time steps. The solid line in tight turns in the first maneuver would require greater
Fig. 5 shows the speed of the vehicle during the ma- deceleration. This makes the new path more favorable.
neuver. As expected, it remains close to the maximum The solid line in Fig. 6 shows the turning rate during
throughout. The turn rate, shown by the dash-dot line the redesigned maneuver, which now remains below the
in Fig. 6, is either near its maximum or zero. The 15o /s limit.
trajectory is approximately a series of straight lines The optimization was solved in 49 seconds with the first
joined by the tightest possible turns, all at constant settings and 65 seconds with the reduced force. To il-
speed. This satisfies the necessary conditions derived lustrate the importance of the force penalty, the same
in [1], which found that the extremal path consists of problems using the cost function in Eqn. 11, without
segments of minimum radius turns joined by straight the force penalty, were solved in 142 and 277 seconds re-
lines. Note that the trajectory cuts the corners of the spectively. In a further experiment to test the problem-
obstacles, since collision avoidance is enforced only at dependency of solution times, 783 randomly-generated,
the discrete time points marked. The obstacle regions feasible problems, each involving similar obstacles and
in the optimization must be slightly larger that the real operating region as in this example, were solved using
obstacles to allow for this margin. the cost function with force penalty. The average solu-
Fig. 6 shows that the turn rate exceeds the 15o /s limit tion time was 11 seconds, with a maximum time of 68
during some of the tighter turns around obstacle cor- seconds.
ners. This possibility was discussed in Section 2.1. In
this case, the infringement of the turn rate limit is 4.2 Three Aircraft
small, but if the resulting trajectory is unflyable by the Having verified the model in the previous section, this
real aircraft, post-processing would indicate the need to example applies it to a collision avoidance problem in-
volving multiple aircraft. Three aircraft, similar to
those used in the previous example, are required to tra-
verse different diameters of a circle, as shown in Fig. 7.
Table 1: Initial and final states for the aircraft
Their starting points are marked by squares and desti-
maneuvers.
nations by stars. The straight line paths to the desti-
Position Velocity nations, shown dotted in the figure, would clearly lead
X Y X Y to a collision. The designed trajectories, also shown in
Initial 5 5 -0.2 0 Fig. 7, form a ‘roundabout’ maneuver and successfully
Final -5 4 -0.2 5 avoid collision with minimal deviation. The square ex-
clusion region is 1m across. A similar result was shown
0.25 2.5
2
0.2
1.5
o
Turn rate 15 /s 1
0.15
o
Turn rate 12 /s
Speed (m/s
0.5
0.1
0
Y
−0.5
0.05
−1
0 −1.5
0 5 10 15 20 25 30
Time (s)
−2
Fig. 5: Speed of vehicle during maneuvers shown in
Fig 4. The dashed line shows the specified limit.
−2.5
−2.5 −2 −1.5 −1 −0.5 0 0.5 1 1.5 2 2.5
18 X
o
Turn rate 15 /s
o
Turn rate 12 /s
16 Fig. 7: The designed trajectories for the aircraft. The
stars mark the target positions.
14
12
4.3 Four Aircraft
Turn Rate (degree/s
10
This example applies the method shown to a more
realistic-looking problem. It involves four aircraft fly-
8 ing intersecting routes. The straight-line path for each
aircraft is shown dotted in Fig. 8 and the targets are
6
shown by stars. The circles mark the trajectories
4
designed using the collision avoidance constraints in
Equ. 8. As expected, the resulting trajectories consist
2 of nearly straight sections joined by turns. Each air-
craft makes a small deviation from the straight path to
0
0 5 10 15 20 25 30 avoid collision, demonstrating the co-operative nature
Time (s)
of the method.
Fig. 6: Turning rate of vehicle during maneuvers
shown in Fig 4. The dashed line shows the real air- 4.4 Multiple Waypoints
craft limit of 15o /s. This section presents an example of the multiple way-
point formulation, in which the aircraft must visit sev-
eral different points in an unspecified order. In Fig. 9,
the waypoints are marked by stars. The algorithm has
in [1], found by an iterative process involving the nec- chosen the ordering to give the fastest trajectory vis-
essary conditions for optimal avoidance. This paper iting all three points. In Fig. 10, an obstacle blocks
has repeated that result by direct optimization. The the path used in Fig. 9. The algorithm now selects a
trajectories also demonstrate the co-operative nature different ordering to visit the points in minimum time.
of this method. This illustrates the potential of this method for solv-
ing planning problems including aspects of high-level
The heavy dots mark the positions of the aircraft at the
co-ordination.
18th time step. The exclusion regions around these po-
sitions are shown by the dotted boxes. Observe that the
vehicles are separated by exactly the safety distance,
illustrating the efficiency of the formulation and the 5 Conclusion
direct physical significance of the avoidance distance.
This contrasts with penalty methods such as potential It has been shown by theory and example that a con-
functions [12], in which the avoidance weighting is not stant speed, limited turn rate vehicle, such as an air-
so obviously related to the achieved distance and may craft, can be modeled as a point mass with limited
need tuning. speed and subject to limited force in a minimum time
3 10
2 6
1
2
0
0
−2
−4
−1
−6
−2 −8
−10
−10 −5 0 5 10
−3
Fig. 9: Designed trajectory for an aircraft to visit three
waypoints, marked by stars, in any order.
10
−4
−4 −3 −2 −1 0 1 2 3
8
Fig. 8: Designed Trajectories for Four Aircraft on In-
tersecting Routes 6
4
problem. This has been combined with collision avoid-
ance constraints to form a mixed integer linear pro- 2
−10
−10 −5 0 5 10