4.2 Aerial Collision Avoidance Case Study
4.2.5 The General Approach
An overview of the controller is given in Figure 4.8. The inputs to the system are: 1. The current state of the MAV St= (x, y, z, ψ, ˙x, ˙y, ˙z, ˙ψ), this takes the form of the
current position and velocity for each of the four controllable degrees of freedom. Roll and Pitch are coupled to the linear movement in the x and y axes as discussed in Section 2.6.1 and therefore not directly controllable.
2. The current goal trajectory G = (g1, . . . , gn) which is a sequence goal states where
gi= (xg, yg, zg, ψg).
3. The current state of the other MAVs AS = (as1, . . . , asm) where each state takes
the form: asi = (id, x, y, z, ˙x, ˙y, ˙z, r) where id is the unique agent identifier and r
At each moment in time the ComputeControl procedure is used to calculate the next control command to send to the MAV based on the current state estimate, the current goal and the current state of any other MAVs. This procedure is described in Algorithm 6.
Algorithm 6 ComputeControl(St, gt, ASt, Th, r)
Input: The current MAV state St, current goal trajectory point gt, current agent
states ASt, the time horizon Th and the MAV radius r.
Output: The control command to send to the MAV spacc.
1: epos ← gt− St
2: spvel← CombinedPID(epos)
3: spsafevel ← ComputeSafeVel(spvel, St, ASt, Th, r)
4: evel← St− spsafevel
5: spacc← CombinedPID(evel)
6: return spacc
Algorithm 7 PID(et, Kp, Ki, Kd, maxI, sf)
Input: The current error et the proportional Kp integral Ki and derivative Kd
gains, maximum integral maxI, derivative filter smoothing factor sf
Output: The output set point O
1: P ← Kpet 2: I ← Ki( t X i=0 ei∆t) 3: if I > maxI then 4: I ← maxI 5: end if 6: D ← et− et−1 ∆t 7: F D ← (1 − sf)F Dt−1+ sfD 8: sp ← P + I + F D 9: return sp
The PID controller used is a modified version of the classical PID controller described in Section 2.6.2, see Algorithm 7. To improve performance in practical applications two modifications are made to the way in which the integral and derivative terms are calculated. These are both widely used in control theory literature. A common issue when the integral term in a classical PID controller is referred to as integral wind-up. The integral term is based on the accumulated error and is therefore susceptible to run-away situations. For example a MAV attempting to reach a goal height may be temporarily stopped by an obstacle. In such a case the error between the goal position and the current position remains high despite the efforts of the PID controller. This causes the integral term to accumulate a large amount of error over a short time. If the obstacle is suddenly removed the accumulated error in the integral term will cause the MAV to shoot up at maximum velocity well past its original goal. A common solution to this problem is to restrict the maximum accumulated error, the ensures the integral
term can still account for disturbances, but prevents run-away situations caused by unobservable control failures.
The derivative term can also be the source of problems if noise in the feedback signal is sufficiently high. The derivative term accounts for the rate of change of the error. However noise in the feedback signal results in oscillations in the derivative, effectively amplifying the noise. In the context of MAVs this has the effect of inducing oscillations as the derivative term is continuously reacting to changes that do not occur. Typical sources of noise on board a MAV are the motors and propellers which create high frequency noise. Therefore a common practise is to include a low pass filter on the derivative term to filter out this high frequency noise.
A PID controller is used for each controllable degree of freedom for a MAV as de- scribed in Section 2.6.3. These controllers are arranged in a cascaded PID scheme described in Section 2.6.4 where the Combined PID controller (see Algorithm 8) is used for both position and velocity control in the cascaded structure shown in Figure 4.8. Algorithm 8 CombinedPID(ex, ey, ez, eψ)
Input: The current roll, pitch, yaw and thrust error (ex, ey, ez, eψ) Output: The roll, pitch, yaw, thrust set points (spx, spy, spz, spψ)
1: spx ← PIDx(ex, Kx p, Kix, Kdx, maxxI, sf) 2: spy = PIDy(ey, Kpy, Kiy, Kdy, maxyI, sf) 3: spz = PIDz(ez, Kpz, Kiz, Kdz, maxzI, sf) 4: spψ = PIDψ(eψ, Kpψ, Kiψ, Kdψ, maxψI, sf) 5: return (spx, spy, spz, spψ)
Algorithm 9 ComputeSafeVel(spvel, St, ASt, Th, r)
Input: The current velocity set point spvel, the current MAV state St, the current
state of all other agents ASt, the time horizon Th and the MAV radius r.
Output: A collision free velocity set point vsaf e.
1: HRVOall ← ∅
2: for all as ∈ AStdo
3: aspos← aspos+ (asvel∆t).
4: HRVOall ← HRVOall∪ HRVOas
5: end for
6: vsaf e← arg min v /∈HRVOall
k St− spvelk2
7: return vsaf e
After calculating the desired velocity set point, using the position PID controllers this is passed to the Compute Safe Velocity procedure in order to compute the closest collision free velocity to the desired velocity set point. This procedure is described in Algorithm 9. The first step of this procedure is to compute the current position of the other MAVs. This is done by taking the last position and velocity message (AS) received from each agent and updating the position to the current moment in time using a linear velocity model. This helps account for any delay in communication. Then an HRVO
is computed for each agent and combined into a single HRVO for all the other MAVs. From this the closest collision free velocity can be computed. We use the HRVO library developed by Snape et al. [105] to compute the 2D and 3D velocity obstacles.
Figure 4.9: MAV trajectory plot for a collision avoidance experiment with 4 MAVs, the start location for each MAV is marked with a dot.