where αd,n,i and βd,n,iare the desired tilt and pan angles for object n calculated at time step i.
dobj,d,n,i is the desired distance between object n and the UAV at time step i. In practice we
want the UAV to track a circle with radius d and center given by robj, thus we need to define a distance dobj =
q
(x− xobj)2+ (y− yobj)2 between the object and the UAV. By using this distance (radius), the controls and the gimbal’s attitude we can define u = [α, β], ud= [αd, βd],
z = [dobj, ρ] and zd = [dobj,d, ρd] where the time discrete references αd and βd were calculated
earlier in this chapter. The radius dobj,d should be chosen along with the yaw rate bounds in order to assure acceptable tracking of the circle7.
In addition one can include a penalty if the objects to be tracked are not located within the projected camera image. As mentioned in 4.2, if the GFV’s corners are calculated it is quite easy to check whether the objects are within the bounds of the GFV or not. If the result of the test described in chapter 4.2 is negative one can multiply the result with a constant penalty. If we call the result from the test for Pres, which is the sum of all the negative same side tests for the objects of interest, we can write the penalty ρ as
ρ =
(
0 if Pres≥ 0
γ|Pres| if Pres< 0,
(5.17)
where γ is a penalty gain. Equation (5.12) should then be modified to include the penalty ρ and it’s reference ρd= 08, as done in equation (5.14) and (5.16). By using the constraints given by equation (5.9) together with a bound on the yaw rate r given as
rmin≤ rt+k ≤ rmax, (5.18)
one can use mathematical tools such as ACADO to calculate and solve the object tracking problem. The first calculated control move, including UAV way-point and gimbal angles, should be used as control action. This is not always possible since delays in the system may require the use of the second or third control step in each of the MPC’s horizons.
5.6 Tracking multiple objects
To acquire the capability to track multiple objects we need to implement functionality which chooses the objects to be tracked based on the objects’ locations relative the UAV. This functionality was realized by implementing a local state machine. In addition to choose the objects to be tracked at each horizon, the state machine allows the UAV to move from one object to the next and hold a circular pattern centered at each object for a given time. The state machine consists of three states used during normal operation. The first state is the Initialization state, which initializes the state machine. The second state is called Underways, where the UAV is controlled to move towards the nearest object until the distance between the object and the UAV is closer than e.g. 300 meters. At this point the GFV’s center is approximately a couple of hundred meters, depending on the UAV’s altitude, from the
7
By choosing −0.5 ≤ r ≤ 0.5 a possible value of dobj,d could be 60 meters while −0.1 ≤ r ≤ 0.1 could be 300
meters.
8
Chapter 5. Model Predictive Control 41
objective, which is an acceptable distance. However, we want to keep the GFV center as close to the object as possible to ensure that the object stays within the GFV even if the UAV’s attitude were to change.
The third state is called Holding, and keeps the UAV in a holding pattern, often referred to as loitering, within a radius of 300 meters centered at the object’s position. The radius is chosen from experiments as it is the shortest distance the UAV can reliably achieve with the yaw rate bounded by −0.1 ≤ r ≤ 0.1 rad/s. The limited range is due to the gimbal’s maximum tilt angle (64◦) combined with the needed roll angle (≈ 28◦) to conduct the circular motion. These
restrictions does not appear to present any problems as the GFV will be large enough to still keep the objective reliably within the GFV with a seizable margin. It is worth noting that although the object is within the GFV it can not be assumed to be visible. One should also note that there is a considerable stretch in the GFV which causes the camera’s resolution to be unevenly spread throughout. This can cause the objects to not be covered sufficiently and thus be undetectable.
Initialize
Initialize Split GroupSplit Group
Underways
Underways HoldingHolding
Error Error
Figure 5.3: UML state diagram: Object planning.
The transition to the Holding state is dependent on the UAV’s operational characteristics and the distance between the UAV and the object to be tracked. We have assumed an operational
42 5.6. Tracking multiple objects
altitude of 100 meters9 and a maximum turn rate of−0.1 ≤ r ≤ 0.1 rad/s. If these parameters were to change, depending on the operational platform or envelope, the accepted distance between the UAV and the targeted objective might have to be adjusted. One should not be to lenient with these parameters as this could impair the quality of the object tracking system’s performance. By simplifying the problem we can use rmaxto find the minimum time to complete a full circle during the Holding state, which is tC = rmax2π . The circumference of the circle is then ΩC = U tC, where U is the UAV’s velocity, given as
U =qν2
x+ νy2. (5.19)
This gives a minimum turn radius, qmin, which can be calculated by
qmin = ΩC 2π = U tC 2π = U rmax . (5.20)
By using the turn rate, ˙Ψ, a more accurate solution can be acquired which results in the following acceptance radius given by
q = U˙ Ψ+
ǫ ˙
Ψ, (5.21)
where ǫ is a tuning parameter. In this simplified representation the tilt angle αdcan be written as the sum of a contribution from roll, φ, and the angle given by the distance to the target and the altitude (see figure 5.4),
αd= arctan q z +|φ| . (5.22)
Figure 5.4: Simplified representation of αd.
The desired tilt angle, αd, stated in equation (5.22) is only valid if the UAV is in a holding pattern. By decreasing the yaw rate r, the radius q will increase since ˙Ψ is dependent on r.
9
Chapter 5. Model Predictive Control 43
The increased radius, which decreased the roll angle, will in turn increase the contribution from the angle given by q and z (the first part of eq. (5.22)). It is possible to increase the altitude, z, to further reduce αd but this will be at the cost of the image quality.
The amount of time the UAV stays in the holding state is controlled by a timer counting the number of iterations used to provide control action to the Piccolo. The number of iterations is strictly related to time as one iteration spans a pre-determined time horizon. When the number of iterations counted by the timer matches the number set by the operator, the state machine will check whether there are any unvisited objects in the object list. If this is true, the object closest to the UAV is selected as the next object to track. If there are no unvisited objects, the algorithm will choose the object it first visited and start a new visiting loop. Hence, ensuring the UAV’s time is divided equally between the objects in the object list. If there is no other object in the object list, the UAV will continue to hold its pattern around the current object until the operator confirms another object to track, or aborts the object tracking mission.
The algorithm choosing the next object simply compares the UAV’s position with its knowledge of the objects’ positions, where the closest is chosen as the next object to track. By doing so, the path, and thus the object tracking, is more efficient than picking the objects at random, or by order of entry. In addition to the distance between the UAV and the objects, the algorithm also accounts for the UAV’s heading and the required distance caused by the turning radius. This results in a more optimal path where the objects located behind the UAV are suitably penalized. Although this system is designed to be implemented in a small UAV, it is worth mentioning that an efficient path could in other cases lead to considerable savings due to fuel cost as well as reduced emissions.
To get a penalty corresponding to the segment of the turning circle’s circumference, we need to calculate the changes in the UAV’s heading which must be conducted to point the UAV towards the object. We start by calculating the UAV’s heading angle which must be set to steer the UAV towards the object. The changes in the UAV’s heading is given by
δ = arctan xobj− x yobj− y
!
. (5.23)
In order to compare δ to the UAV’s heading, Ψ, δ should be on the format given by
δnorth= 0◦ δnorth−west= 45◦ δnorth−east=−45◦ δeast= 90◦ δwest=−90◦ δsouth−east= 135◦ δsouth−west=−135◦ δsouth=±180◦, (5.24)
which is accomplished with a correction, δ′ = −180 + δ for the south-west quadrant, and