Development and Experimental Validation of a Sense and Avoid System for a Mini-UAV

1. Introduction

During the recent years, Unmanned Aircraft Systems (UAS) and their operations have registered a significant increase in number, sophistication and technical complexity. UAS are no longer a military exclusive reality, but several studies are currently evaluating their possible employment in a broad variety of civil applications. These range from monitoring and surveillance tasks such as border and maritime patrol mission, search and rescue operations, forest fire detection, road traffic surveillance, critical infrastructure protection to commercial purposes such as aerial imagery, crop assessment in agriculture and communication relays [1,2,3]. This rapid expansion is motivated by the unmatched versatility in terms of application scenarios that these systems offer, both military and civil, and by the numerous advantages connected with the employment of a vehicle that doesn’t require a pilot onboard. However, the lack of the presence of a human pilot onboard, also poses a series of safety related concerns and technical challenges mainly connected with the risk of a mid-air collision with another aircraft, that may hinder the successful integration of unmanned aircraft in civil non-segregated airspace [4]. For instance, in Italy, UAV operation is restricted within Visual Line of Sight (VLOS) by the Italian regulatory authority (ENAC) and a specific request must be filed and approved every time a Beyond Visual Line of Sight (BVLOS) operation is to be conducted [5]. Moreover, in the past years, Aeronautical agencies and authorities were involved in the definition of proper regulations to allow UAVs for a safe access to flight, culminating with FAA issuing mandatory installation, onboard UAVs, of dedicated systems for autonomous obstacle detection and avoidance, as a requirement for non-segregated flight operation [6]. Such systems are required to meet or exceed human capabilities in terms of avoiding potential collision threats, a concept that several authors [7,8] refer as “equivalent level of safety” that the UAS should possess for the safe integration in the global airspace. Consequently, a huge effort by worldwide research institutions has been put into place to devise proper Sense and Avoid (SAA) systems with the overall goal of mitigating the risk of mid-air collision between UAVs and other aircraft. The limitations connected with UAS own size and weight, the so-called SWaP (Seize, Weight and Power constraints), severely impact the implementation of autonomous collision avoidance solutions often preventing the adoption of more consolidated technologies such as TCAS [9] or ADB-S. Such constraints often impose to solve real-time, complex, detection and tracking problems utilizing the limited computational resources available onboard. In addition, the fundamental requirement of being able to autonomously detect and avoid non-cooperative intruders which are generally not equipped with transponders and do not take active part in the conflict resolution, force the designer to develop innovative solution leveraging a multitude of sensors with the aim of rendering UAS capable of solving the impending conflict on its own. From a taxonomical standpoint, SAA systems are usually categorized according to the sensor type they rely on, which can be active or passive. For the former case, radar exploiting Frequency Modulated Continuous Wave (FMCW) technology is becoming more and more popular for SAA purposes because of the economical, low Size, weight and power solution they offer. Besides, radar sensors appear interesting because they provide positioning information (in terms of range, range rate and angular measurement) practically under any weather or illumination condition. Several works have investigated the use of radar sensors for SAA applications: in [10] a SAA system comprising the combination of cooperative sensors (TCAS and ADB-S) and a radar is tested in an extensive flight trials campaign; the authors in [11] describe an algorithm for safe navigation of small-sized UAVs within an airspace leveraging secondary Surveillance Radar (SSR); other examples of UAV detection methods based on radar are described in [12,13]. Radar is not the only active sensor which is being investigated for SAA applications. An example of another active sensor, the LIDAR, currently studied for SAA applications is provided in [14]. Passive sensors and specifically optical sensor, on the other hand, generally offer a higher accuracy in terms of target angular position reconstruction, a higher refresh rate and a lower power absorption and weight; their main drawbacks lies in the fact that their operation is severely limited in low lighting conditions (i.e. night) and direct target position reconstruction is not possible because a single camera sensor can’t measure distance directly. In [15] the problem of estimating target distance with only a camera sensor is overcome by means of a depth estimation model based on a lightweight encoder-decoder architecture trained on a large database of airborne object. The deep learning model is trained to translate the input image from the monocular camera to a depth mask, classifying every pixel into depth values. In [16] the authors leverage an implementation of the You Only Look Once (YOLO) object detector to detect cooperative micro UAV within the FOV of a camera. Sensor data coming from the cooperative navigation system are exploited to reduce the search window in a strip of the image which is computed starting from the approximate relative position of the two UAVs and attitude information of the UAV where the camera is mounted. Other examples of different strategies employed to tackle the task of intruder detection relying solely on camera sensor can be found in [17,18]. Other authors have instead favoured an approach exploiting the fusion of passive and active sensors. This choice is motivated by the fact that sensor fusion strategies can leverage the complementary advantages of different sensor sources. For example, in [19] a radar/visual fusion approach designed to fuse the radar and visual tracks is discussed for a non-cooperative Sense and Avoid system for low altitude flying UAVs. In [20] an integrated Sense and Avoid system developed by the Italian Aerospace Research Centre (CIRA) and the university of Naples and deployed onboard a customized TECNAM P-92, based on data fusion between EO and radar sensor is thoroughly discussed. In the work, particular focus is placed on integration details, fusion logics, algorithms and flight results relevant to the achievable situational awareness. A radar/visual multisensory architecture was chosen also for the implementation of one of the activities of the recently concluded Italian project named TERSA. The project aimed at the design and development of several innovative technologies for a lightweight UAV including the development of a collision avoidance system for unmanned aerial vehicles leveraging data fusion between optical and radar sensor. The project involved the University of Pisa (UNIPI) and a consortium of industrial partners led by the Italian UAV manufacturing company Sky Eye Systems (SES), and it led to the development of a fully autonomous SAA system to be installed onboard a new generation of lightweight UAV manufactured by SES in Italy. This work presents an overview of the system that was developed during the project, from the identification and definition of the technical requirements to the actual flight test conducted to validate the system. Specifically, the present article is organized as follows: the reminder of this first Chapter describes the high level requirements of the system; Chapter II briefly recalls the sense and avoid system architecture developed during the project; Chapter III details the principal algorithms designed for the implementation of the system, with particular focus to the computer vision object detection and tracking algorithms and the data fusion strategy adopted; Chapter IV describes the numerical simulations conducted to validate the system providing an overview of the integrated HIL simulation environment that was developed at the Fly-By-Wire laboratories of the Civil and Industrial Engineering Department of the University of Pisa; Chapter V describes the flight test campaign conducted in cooperation with Sky Eye System (SES), and ECHOES radar technologies, the partner who manufactured the radar, aimed at the validation of the integrated system. Finally, Chapter VI presents the conclusion of the research work and provides an outlook of the future developments.

1.1. Reference Aircraft

The Sense and Avoid system, object of the present work is designed to be embarked and operated onboard the reference aircraft manufactured by SES (Figure 1), whose main geometrical and inertial characteristics are reported in Table 1.

1.2. System High Level Requirements

UAV autonomous anti-collision systems are still at a developing stage and research studies are still being carried out to identify system requirements and sensing solutions. As a consequence, at present, specific requirements to be met by the system designer don’t exist; however, a collection of universally accepted specifications, guidelines and recommendations formulated by ICAO [21,22], FAA [8], ASTM [23], EUROCONTROL [24], RTCA [25] and other researcher [26,27] has been used to define the system requirement for the present application. A SAA system is usually composed of two main modules. The sense module, from a functional standpoint must acquire information from the environment to contribute building awareness of what is happening in the vicinity of the UAV and to detect potential hazards. The task of the avoid module, on the other hand, is to guarantee both a separation-provision and a collision-avoidance function. By separation provision is intended the capability to separate the UAVs from other aircraft whenever Air Traffic Control (ATC) is not providing separation. The Pilot in Command (PIC) should be warned by the system following a loss of separation so that PIC may manoeuvre as required. System should immediately prompt the PIC as soon as separation is restored and should seek for approval to return on initial trajectory. This function should separate the UAV from other aircraft by a minimum distance of 0.5 NM in the horizontal plane and 500 ft in the vertical plane. Collision-avoidance function instead should operate independently from ATC and should either provide the PIC with valuable cues about an impending collision, waiting for him to employ corrective actions or should autonomously resolve imminent collision executing avoidance manoeuvres. Several requirements can be pointed out both for sense and avoid modules.

1.2.1. Sense Requirements

Principal requirements are summarized below:

• Detection range, $d_{min}.$ It is connected to the time-to-collision by means of approaching speed. It must be sufficiently large to allow for an avoidance manoeuvre complying with aircraft dynamic, structural and aerodynamical constraints that guarantees a minimum separation of 500 ft at the closest point of approach to the obstacle (“miss distance”) [23]. For the characterization of this requirement, the procedure described in [27] has been adopted by considering the most critical condition of a frontal encounter. Since the evasive manoeuvre will be conducted by means of a corrected turn on the horizontal plane, $d_{min}$ essentially depends on four factors: UAV speed, intruder aircraft speed, latency between detection and evasive command execution and especially the minimum turn radius. As well known, minimum turn radius (in stationary manoeuvre at constant altitude and speed) for a conventional fixed wing aircraft depends on three constraints: aerodynamic, structural and propulsive. As an example, Figure 2 (a) shows the minimum attainable turn radius at sea level for the TERSA aircraft according to each constraint where realistic bounds have been assumed for maximum lift coefficient $C{l}_{max}=1.45$, maximum vertical load factor $n_z^{max}=1.15$, and available thurst $T_d^{max}=20N$ (actual exact values provided by the manufacturer have been omitted due to NDA reasons). A lower threshold of $R_{min}=100 m$ is enforced by the FCS as a stall protection mechanism. As it can be noted, in the range of operative flight speeds ($20<V<28 m/s$), structural constraint is the only effective bound but in turn is capped by the stall protection limit. Figure 2 (b) shows the required minimum detection range for various intruder speed and UAV minimum turn radius, according to the procedure described in [27] (conservative estimate with a delay factor of $\mathsf{\Delta }t=5 s$ between detection and evasive command execution). A detection range of $d_{min}=700 m$ appears satisfactory for the effectiveness of a collision avoidance maneuver with an intruder flying at a comparable cruise speed as the UAV.
• Field of View: A widely accepted guideline for manned aircraft manufacturers is to build cockpits as to ensure a visibility of ±110° (azimuthal) and ±15° (in elevation) [23]. Usually, the same requirement is extended to Unmanned aircraft. However, due to the prototypical nature of this system a FOV of ±60° has been identified as the design requirement. As a matter of fact, preliminary sensitivity analysis conducted on a wide range of collision encounter scenarios, considering an intruder cruise speed comparable with the UAV cruise speed, have shown that, given an assigned detection range $d_{min}$, FOV anlges greater than 55° don’t substantially affect the detectability of the intruder aircraft.
• Refresh rates: For a precise tracking of the intruder aircraft, computer vision algorithms should run at a sufficiently high frequency (> 10 FPS). Given the limited computational power available on board, it is thus essential to optimize object detection and tracking algorithms and to select the best compromise between image resolution and performance. Data fusion algorithms are based on a standard implementation of an extended Kalman filter and thanks to optimized linear algebra libraries do not pose a significant computation burden on the VPU processor. Likewise, collision avoidance routines, that exploit simple geometric laws, do not constitute a bottleneck for performance; the computation of the setpoint on the desired ground track angle to carry out the maneuver, essentially boils down to the extraction of the roots of a 4^th order polynomial which again is performed effectively on the VPU processor by means of optimized linear algebra libraries (OpenBLAS/cuBLAS). The bottleneck is thus represented by the object detector pipeline.
• Optical sensor resolution: the choice of the optimal compromise is the result of a trade-off between computational efficiency and capability of detecting small targets at a great distance. The performance of the visual tracker chosen for the implementation of the system is essentially independent of the resolution because it acts on a finite set of discrete anchor points. On the contrary, the object detection pipeline is strongly affected by the size of the image as detailed in chapter 3. At the same time, a higher resolution is beneficial for two major reasons:

• The attainable maximum angular resolution can be expressed as:

  $$\mathsf{\Delta }\psi ={\tan }^{-1}\left({\displaystyle \frac{{\mathrm{S}}_{\mathrm{w}}\mathsf{\Delta }h}{Hf}}\right)={\tan }^{-1}\left({\displaystyle \frac{1}{{\rho }_{H}f}}\right)$$

  (1)

  where ${\rho }_H$ is the horizontal pixel density of the sensor and $f$ the focal length.
• Johnson criteria states that the maximum distance $R_{det}$ at which an object of physical characteristic size of $l$ can be detected by an optical sensor, denoting with $q$ the minimum number of pixel the object needs to occupy for a successful detection (Johnson’s factor usually assumed = 2) and with $m$ the physical size of the pixel, is given by [27]:

  $$R_{det}={\displaystyle \frac{lf}{qm}}$$

  (2)

For a given sensor’s size, the higher the resolution, the lower the single pixel physical size. For these two reasons, it is thus clear that a higher resolution would be beneficial. For the development of the system, considering the specifics of the Vision Processing Unit chosen (see Chapter 2) and the typical dimensions of the UAVs belonging to the same class as the TERSA aircraft, a resolution of $640\times 360$ was chosen.

1.2.2. Avoid Requirements

Given the prototypical nature of the system, some simplified assumptions have been made during the definition of the requirements for the collision avoidance module that can be summarized as follows:

• One single intruder aircraft present at a given time in the conflict scenario belonging to the same UAV class with comparable cruise speed and altitude.
• Such aircraft maintains its original linear stationary flight course, wing levelled, constant speed. Several combinations of relative initial positions and relative speeds are evaluated.
• Evasive maneuver is conducted by means of a correct turn on the horizontal plane, conducted at constant speed and altitude.

The reason for a climbing evasive maneuver has not been considered is briefly justified in the following. A study has been conducted evaluating the maximum flight path angle attainable by the reference aircraft in a steep (stationary) climb maneuver utilizing aero mechanical dataset provided by courtesy of SES. Specifically, as well known, the maximum attainable flight path angle ${\gamma }_{max}$ is given by:

$${\gamma }_{max}=\max \left({\displaystyle \frac{T_d-D}{W}}\right)$$

(3)

where $T_d=f(V,RPM,h)$ is the available thrust, $D=f(\alpha ,\dot{\alpha ,}\beta , V,h,\dots )$ is the aerodynamic drag and $W$ the aircraft weight. Considering the equilibrium in the vertical plane:

$$\left\{\begin{array}{c}L-W=0\\ M_y=0\end{array}\right.$$

(4)

And applying the principle of superimposition of effects (PSE), former equations can be expressed by means of Eq. (5) where the $\mathsf{\Delta }$ terms represent variation w.r.t to the reference condition.

$$\left\{\begin{array}{c}\left(C{l}_{ref}+\mathsf{\Delta }C{l}_{{\delta }_e}\cdot \mathsf{\Delta }{\delta }_e+\mathsf{\Delta }C{l}_V\cdot \mathsf{\Delta }\mathrm{V}+\mathsf{\Delta }C{l}_{\alpha }\cdot \mathsf{\Delta }\alpha \right){\displaystyle \frac{1}{2}}\rho {\mathrm{V}}^{2}S-W=0\\ \left(C{m}_{ref}+\mathsf{\Delta }C{m}_{{\delta }_e}\cdot \mathsf{\Delta }{\delta }_e+\mathsf{\Delta }C{m}_V\cdot \mathsf{\Delta }\mathrm{V}+\mathsf{\Delta }C{m}_{\alpha }\cdot \mathsf{\Delta }\alpha \right)=0\end{array}\right.$$

(5)

Once the trim problem is solved for AOA and elevator deflection ensuring equilibrium, ($\alpha , {\delta }_e$ respectively), drag force is computed by means of Eq. (6)

$$D=\left(C_{Dref}+\mathsf{\Delta }{C}_{D{\delta }_e}\cdot \mathsf{\Delta }{\delta }_e+\mathsf{\Delta }{C}_{DV}\cdot \mathsf{\Delta }\mathrm{V}+\mathsf{\Delta }{C}_{D\alpha }\cdot \mathsf{\Delta }\alpha \right){\displaystyle \frac{1}{2}}\mathsf{\rho }{\mathrm{V}}^2\mathrm{S}$$

(6)

Available thrust as a function of flight speed, engine RPM and altitude is obtained by means of engine maps provided by courtesy of SES. Figure 3 shows the attainable ${\gamma }_{max}$ as a function of RPM and flight speed at a given altitude.

The exact value of ${\gamma }_{max}$ for a given altitude is found by solving the maximization of the expression (3) while enforcing the respect of trim equilibrium described by (5). For instance, at an altitude of $h=1000 m,$ ${\gamma }_{max}=$9.57° corresponding to a True air speed of TAS = 18.8 $m/s$ and an engine setting of 7950 RPM. Considering a detection range of $d_m=700 m$, this would result in a $\mathsf{\Delta }h=\tan \left({\gamma }_{max}\right)\cdot d_m\approx 118 m$, insufficient to ensure a minimum separation provision of $R_{sb}=500 ft$ as recommended in the guidelines. It should be noted that the actual minimum separation attainable with a climbing manoeuvre alone would be even smaller than the afore mentioned estimate due to engine and aircraft dynamics which prevent the onset of an immediate maximum flight path angle and unavoidable latencies between actual detection and evasive command implementation.

Granted that a climbing manoeuvre alone wouldn’t provide necessary separation to avoid the obstacle and given the prototypical nature of the application, it is decided to carry out the evasive action by means of a coordinated turn on the horizontal plane.

The requirements of the avoid modules are thus hereby summarized:

• Once a threat has been positively identified by the sense module, the avoid function has to perform an avoidance manoeuvre in a completely autonomous fashion which guarantees minimum separation provision at all times between the UAV and the intruder aircraft. The Autonomous Collision Avoidance (ACA) strategy employed must be effective for non-cooperative intruders.
• Such manoeuvre must be conducted on the horizontal plane by means of a coordinate turn that takes into account the aerodynamic and structural limits of the UAV.
• After the conflict has been deemed resolved by the Conflict Detection and Resolution (CDR) algorithms, the avoid module must bring the UAV on his original trajectory.
• It is desirable that the avoidance manoeuvre produces the minimum deviation from the original planned trajectory of the UAV.

2. System Architecture

The Typical integration of a SAA system within the closed loop control system of an UAV is shown in Figure 4. As can be noted, the underlying algorithms take as input the UAV position and velocity states provided by the Inertial Navigation System (INS) and the velocity and position states of the intruder aircraft and yield as output, in the most general case, the demanded speed, flight path angle and ground track angle which in turn are fed as input to the autopilot system to avoid the collision threat.

For this specific application, the only output of the SAA system is the demanded heading angle used as setpoint for the coordinated turn on the horizontal plane. For the actual implementation of the system, a multisensory architecture leveraging data fusion between radar and EO sensor has been chosen. As it has been shown in several previous works [20,27,28] this allows to compensate for the shortcomings of each sensor type, reduce noise and increase the overall solution accuracy. In particular, data fusion between radar and EO sensors leads to an increase in system robustness with respect to false alarms and missed detections during the object detection stage, coupled with a higher accuracy in the tracking stage. A diagram portraying the high-level schematics of the developed architecture is shown in Figure 5. The working principle of the system can be summarized as follows: both the EO (camera) sensor and the radar scan the environment ahead of the UAV in search of a potential threat. Each new frame captured by the camera is compared against a background model of the scene leveraging an object detection strategy based on Single Gaussian Model (SGM), where suitable motion compensation algorithms are employed to compensate for the ego motion of the camera.

A Ransac approach is used to derive the homography matrix describing the transformation between current frame and background model, starting from point correspondences between the two scenes. Background model is then warped according to this transformation as to co-register it to the current frame and minimize the noise due to camera ego-motion. Connected component analysis (CAA) is then used to group various blobs in the binary mask into a single object of interest. Statistical criteria are then applied to validate only those tracks that exhibit a certain regularity in terms of position within the field of view and dimensions across the various frames; more details on the specific algorithms involved are provided in the subsequent chapters. A validated track is then followed across various frames leveraging an implementation of the Kanade Lucas Tomasi (KLT) feature tracker [29]. From the knowledge of camera parameters (focal length, resolution and sensor size) track centroid position in terms of pixel within the scene is then transformed into relative elevation and azimuth angles, as shown in Figure 6 by means of Eq. (7). Here, $\psi , \lambda$ are the azimuth and elevation angles respectively in a reference system fixed with the camera, $S_w, { S}_h$ are the sensor width and height respectively, $V, H$ the vertical and horizontal resolution of the scene, $f$ the focal length and $\mathsf{\Delta }v, \mathsf{\Delta }h$ the vertical and horizontal position of the intruder aircraft within the scene (expressed in pixel).

$$\begin{gathered} \psi ={\tan }^{-1}\left({\displaystyle \frac{\frac{S_w}{2}\cdot \frac{\mathsf{\Delta }\mathrm{h}}{H/2}}{f}}\right)={\tan }^{-1}\left({\displaystyle \frac{S_w\mathsf{\Delta }h}{Hf}}\right) \\ \lambda ={\tan }^{-1}\left({\displaystyle \frac{\frac{S_h}{2}\cdot \frac{\mathsf{\Delta }v}{V/2}}{\sqrt{f^2+{\left(\frac{S_w\mathsf{\Delta }\mathrm{h}}{H}\right)}^2}}}\right)={\tan }^{-1}\left({\displaystyle \frac{S_h\mathsf{\Delta }v}{V\sqrt{f^2+{\left(\frac{S_w\mathsf{\Delta }\mathrm{h}}{H}\right)}^2}}}\right) \end{gathered}$$

(7)

The knowledge of the target distance provided by the radar allows to close the tracking problem. Specifically, distance measure, elevation and azimuth angles together with UAV position, velocity and attitude states are fed as input to an Extended Kalman Filter (EKF) which carries out data fusion and outputs intruder aircraft velocity and position states expressed in an inertial reference system. These states are then fed as input to collision avoidance routines which compute an evasive manoeuvre when an impending collision is predicted. A new setpoint on the heading angle is computed at every iteration step in a such a way that the trajectory of the UAV will be tangent to a sphere centred around the intruder aircraft in the point of closest approach. More details about the designed algorithms are provided in chapter 3.

System Components

The main components identified for the realization of the system are:

• A range doppler radar in Ku-band (13.325 GHz) with forward-looking configuration. Radar emits a Linear Frequency Modulated Continuous Wave (FMCW) with a maximum transmitted band of 150 MHz. Azimuth estimation is obtained by means of monopulse amplitude technique.
• Electro-optical sensor with at least 30 FPS, HD ready resolution. Focal length must be chosen to ensure a sufficiently high FOV.
• Vision processing Unit. An embedded platform with low power absorption, easily interfaceable with camera and radar (through ethernet) must be chosen, ideally optimized for computer vision tasks. Both Sense and avoid algorithms run on the embedded platform
• An Inertial Measurement Unit (IMU) must be employed to retrieve attitude angles which are necessary to transform intruder aircraft position and velocity states into the inertial frame where the conflict is resolved.

3. Algorithms Design

The present section details the implementation of principal algorithms underlying the sense and avoid system.

3.1. Sense Module

As explained above, for the implementation of the system, a multisensory architecture based on data fusion between electro-optical (EO) and radar sensor was chosen. Digital Image processing algorithms thus play a crucial role for the correct functioning of the SAA system. The three main tasks such algorithms are called to perform can be summarized as:

4.

Segmentation between background and objects of interest within the FOV with the aim of identifying the presence of an intruder aircraft, Object Detection (OB).

5.

Carry out noise suppression due to camera ego-motion, Ego Motion Compensation (EMO)

6.

Once the presence of an object has been confirmed, track its motion efficiently across frames after its identification.

The requirement of achieving a sufficiently high frame rate on the VPU chosen, to allow for an effective target tracking while at the same time maintaining a sufficiently high precision to distinguish small targets at a great distance from the optical sensor, guided the entire designing process.

Figure 7 shows a high-level schematic of the adopted computer vision pipeline underlying the sense module. Some of the key algorithms constituting the building blocks of the pipeline (such has Harris Corner detector and Kanade Lucas Tomasi feature tracker) appear at several stages of the processing chain; they will be briefly explained in the following.

3.1.1. Harris Corner Detector

The Harris Corner Detector [30] is one of the most well-known, robust and efficient algorithms for extracting singular points within a digital image. Singular points are points of considerable importance as they tend to be invariant to translation, rotation and changes in illumination; their stability makes them ideal for, among other, image tracking or stabilization applications. The name corner derives from the fact that they are usually located at the intersection of at least two edges within the image, where an edge is defined as an area characterized by a sudden, sharp change in light intensity. Consider a digital image of size $s=W\times H$ and a small window of size $s_w=w\times w$; then a shift of the window in a certain direction will either produce negligible change in mean intensity, if the window lies above a flat region, a substantial change in mean intensity value when shifting in two orthogonal direction if the window lies over a corner and lastly if the window lies over an edge, the shift will produce a substantial change while shifting along a direction but a negligible one if shifting along the orthogonal direction. Mathematically, this can be expressed in terms of the Sum of Square Differences (SSD) of the intensity values of all the pixels within the window before and after the translation as:

$$E\left(u,v\right)=\sum _{\left(x,y\right)\in W}w\left(x,y\right)[\mathit{I}\left(x+u,y+v\right)-\mathit{I}\left(x,v\right){]}^2$$

(8)

where $w(x,y)$ is a weight matrix, $\mathit{I}\left(x,y\right)$ denotes the intensity value of the pixel located at position $(x,y)$ and $\mathit{I}(x+u,y+v)$ denotes the intensity value of the same pixel after a small shift $(u,v)$ of the window in the direction $(x,y)$.

By means of Taylor expansion, Eq. (8) can be rearranged as:

$$E\left(u,v\right)\simeq [u v] \sum _{\left(x,y\right)\in W}w\left(x,y\right)\left[\begin{array}{cc}{\mathit{I}}_x^2& {\mathit{I}}_x{\mathit{I}}_y\\ {\mathit{I}}_x{\mathit{I}}_y& {\mathit{I}}_y^2\end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]=\left[\begin{array}{cc}u& v\end{array}\right] \mathit{M} \left[\begin{array}{c}u\\ v\end{array}\right]$$

(9)

where the matrix $\mathit{M}={\sum }_{\left(x,y\right)\in W}w\left(x,y\right)\left[\begin{array}{cc}{\mathit{I}}_x^2& {\mathit{I}}_x{\mathit{I}}_y\\ {\mathit{I}}_x{\mathit{I}}_y& {\mathit{I}}_y^2\end{array}\right]$ is generally referred as the structural tensor with ${\mathit{I}}_{\mathit{x}}, {\mathit{I}}_{\mathit{y}}$ representing (spatial) horizontal and vertical image gradient respectively. Eigenvectors of M matrix denote the directions along which the mean square deviation takes on maximum and minimum values. When both the associated eigenvalues are sufficiently high (in a relative sense with respect to the others), ${\lambda }_1, {\lambda }_2\gg 0$ the patch is above a singular value (corner point). A suitable Harris response, defined as:

$$R={\mathsf{\lambda }}_1{\mathsf{\lambda }}_2-\mathrm{k}{\left({\mathsf{\lambda }}_1+{\mathsf{\lambda }}_2\right)}^2=\det \left(\mathit{M}\right)-k\cdot tr{\left(\mathit{M}\right)}^2$$

(10)

Is used in place of the actual eigenvalues computation, where $k\in \left[\mathrm{0.004,0.006}\right]$The chosen implementation is articulated in the following steps:

• Gray scale conversion of original image
• Spatial gradient calculation by means of linear spatial convolution with kernels $k=[-1 0 1]$ and its transpose $k^T={\left[-1 0 1\right]}^T$
• Structural tensor calculation for each pixel $p(x,y)$ in a window $\mathit{W}\left(x,y\right)$ centered around the pixel.
• Harris response calculation, by computing trace and determinant of M matrix.
• Non maxima suppression (to eliminate duplicate points) and thresholding: in a nutshell, only those points whose response is higher then 1% of the maximum R are selected as valid points.
• Biquadratic fitting to refine corner position and obtain a subpixel location.

3.1.2. Kanade Lucas Tomasi Feature Tracker

The KLT feature tracker is a well-known, efficient feature tracking algorithm which exhibits some robustness w.r.t the change in illumination condition, camera motion, and change of appearance of the tracked object. In its pyramidal implementation [31], it is capable of tracking large motions while preserving a small integration window. The algorithm finds correspondences of points from a primary image $\mathit{I}(x,y)$ and a secondary image $\mathit{J}(x,y)$ by solving a non-linear optimization problem. It attempts to minimize the residual function $ϵ$ defined as:

$$ϵ\left(\overline{d}\right)=ϵ\left(d_x,d_y\right)=\sum _{x=p_x-w_x}^{p_x+w_x}\sum _{y=p_y-w_y}^{p_y+w_y}{\left(\mathit{I}\left(x,y\right)-\mathit{J}\left(x+d_x,y+d_y\right)\right)}^2$$

(11)

where $\overline{\mathit{d}}=\left(d_x,d_y\right)$ is the displacement vector to be determined, $\mathit{p}={\left[p_x,p_y\right]}^T$ is the current corner point under evaluation and $w=(w_x, w_y)$ is an integration window centered on it. The objective of the algorithm is to find the position of a corresponding window centered on the pixel $\mathit{v}=[p_x+d_x, p_y+d_y]$ on the secondary image, such that the sum of square differences among the intensity values is minimized. The vector $\mathit{d}={\left[d_x,d_y\right]}^T$ is called optical flow and is the unknown to be determined. Applying Taylor expansion to Eq. (11) and setting its derivative with respect to displacement to zero (at the optimum there should be no change in intensities values) yields:

$${\displaystyle \frac{\partial ϵ\left(\overline{d}\right)}{\partial \overline{d}}}\approx -2\sum _{x=p_x-w_x}^{p_x+w_x}\sum _{y=p_y-w_y}^{p_y+w_y}\left(I\left(x,y\right)-J\left(x,y\right)-\left[{\displaystyle \frac{\partial J}{\partial x}} {\displaystyle \frac{\partial J}{\partial y}}\right]\overline{d}\right)\cdot \left[{\displaystyle \frac{\partial J}{\partial x}} {\displaystyle \frac{\partial J}{\partial y}}\right]=0$$

(12)

where the quantity $\delta I\left(x,y\right)\doteq I\left(x,y\right)-J(x,y)$ can be interpreted as the time derivative of the intensity values of the image in the point ${\left[x,y\right]}^T$, whereas the matrix $\left[{\displaystyle \frac{\partial J}{\partial x}} {\displaystyle \frac{\partial J}{\partial y}}\right]$ is simply the spatial gradient of the intensity values of the secondary image (current frame). For a purely translational displacement, it can be proved that intensity gradients of the primary and secondary images are interchangeable i.e.

$$\nabla \mathrm{J}\left(x,y\right)=\left[\begin{array}{c}{J}_x\\ J_y\end{array}\right]\doteq {\left[\begin{array}{cc}{\displaystyle \frac{\delta J}{\delta x}}& {\displaystyle \frac{\delta J}{\delta y}}\end{array}\right]}^T={\left[\begin{array}{cc}{\displaystyle \frac{\delta I}{\delta x}}& {\displaystyle \frac{\delta I}{\delta y}}\end{array}\right]}^T=\left[\begin{array}{c}{I}_x\\ I_y\end{array}\right]=\nabla \mathrm{I}\left(x,y\right)$$

(13)

This fact is exploited in the implementation leading to great computational advantage because for each image, for each pyramidal level, gradients are computed only once. Gradients are computed by means of the central difference operator Eq. (14)-(15)

$$I_x\left(x,y\right)={\displaystyle \frac{\partial I\left(x,y\right)}{\partial x}}={\displaystyle \frac{I\left(x+1,y\right)-I(x-1,y)}{2}}$$

(14)

$$I_y\left(x,y\right)={\displaystyle \frac{\partial I\left(x,y\right)}{\partial y}}={\displaystyle \frac{I\left(x,y+1\right)-I(x,y-1)}{2}}$$

(15)

Plugging equations (14)-(15) in (12) yields Eq (16).

$${\displaystyle \frac{1}{2}}{\left[{\displaystyle \frac{\partial ϵ\left(\overline{d}\right)}{\partial \overline{d}}}\right]}^T\approx \sum _{x=p_x-w_x}^{p_x+w_x}\sum _{y=p_y-w_y}^{p_y+w_y}\left(\left[\begin{array}{cc}{I}_x^2& I_x{I}_y\\ I_x{I}_y& I_y^2\end{array}\right]\overline{d}-\left[\begin{array}{c}\delta I I_x\\ \delta I I_y\end{array}\right]\right)=0$$

(16)

Now, defining the so-called image mismatch vector $\overline{b}$ as:

$$\overline{b}\doteq \sum _{x=p_x-w_x}^{p_x+w_x}\sum _{y=p_y-w_y}^{p_x+w_y}\left[\begin{array}{c}\delta I I_x\\ \delta I I_y\end{array}\right]$$

(17)

And the structural tensor $G$ as:

$$G\doteq \sum _{x=p_x-w_x}^{p_x+w_x}\sum _{y=p_y-w_y}^{p_y+w_y}\left[\begin{array}{cc}{I}_x^2& I_x{I}_y\\ I_x{I}_y& I_y^2\end{array}\right]$$

(18)

Eq. (16) can be rewritten as:

$${\displaystyle \frac{1}{2}}{\left[{\displaystyle \frac{\partial \mathsf{ϵ}\left(\overline{d}\right)}{\partial \overline{d}}}\right]}^T\approx G\overline{d}-\overline{b}=0$$

(19)

Which yields the optical flow solution:

$${\overline{\mathrm{d}}}_{opt}=G^{-1}\overline{\mathrm{b}}$$

(20)

Notice that in chosen implementation, the matrix G is constant for each pyramidal level, thus the computation of image gradients (an expensive operation that involves spatial convolution) is performed only once per pyramidal level; the only quantity updated at each iteration is the image mismatch vector $\overline{b}$ which is a function of the difference between the reference window centered around the current pixel in the primary image (which is constant) and window in the secondary image which is a warping of the original window. Notice however that the interpolation required to compute the warped window is not performed on the entire image but only on the window, thus the smaller the window the faster the algorithm. Since the maximum displacement that can be tracked is equal to half the window size, the pyramidal implementation is a clever approach to track higher displacement while preserving a small window and thus computational efficiency. Essentially, each pyramid level is a down sampled version of the original image with a subsample factor of 2. When the algorithm converges at a the $l^{th}$ level, the displacement is propagated at the $l_{-1}^{th}$ level and used as a new starting point. A visual representation of the pyramidal expansion is shown in Figure 8. An example of matched points correspondences for an aeronautical sequence is shown in Figure 9. Finally, a high-level scheme of the implemented algorithm is provided in Figure 10.

3.1.3. Object Detection

Several approaches exist in literature to address the problem of detecting an object of interest within the FOV. These algorithms range from simple colour thresholding, to edge detection coupled with proper thresholding and morphological operations [32], segmentation based on motion estimation through optical flow [6], background subtraction methods, of whom a comprehensive review is presented in [33] or template/block matching. Recent approaches based on machine learning/deep learning such as region proposal convolutional neural networks (R-CNN) have proved to be extremely powerful for the purposes of object detection, but they come at the expense of a higher computational cost. Unified Real time object detection, of which YOLO [34] constitutes the most notorious example, aims at reducing computational cost while retaining the flexibility of R-CNN based methods. For the present application however, also in light of the expected limited object size within the FOV, due to the distance and small size, a more traditional computer vision object detection technique has been employed. A background subtraction scheme based on single gaussian model has been chosen. In addition, in order to reduce noise arising from the ego-motion of the camera, a proper motion compensation strategy, inspired by the works in [35,36,37] is employed; more details about the ego motion compensation are provided in the next section. To address the problem of the dynamic background (due to camera motion new portion of the background enter the FOV while others are no longer visible), two main approaches are commonly found in literature:

• The construction of a panoramic background model by co-registering and stitching consecutive frames of the scene, the so-called panorama stitching.
• The update of a background model which preserves its size in terms of pixels. For this approach it is necessary to warp the background model on the current frame according to a matrix describing the transformation of the scene. The overlapping regions can then be updated while the new regions must be considered as background at the first iteration.

The first approach is very expensive in terms of memory requirements, mainly because it requires constant storage of the new images captured by the camera as it moves; it also requires precise 'stitching' between the images, resulting in a high computational cost. For the system implementation thus, the second approach, inspired by the works [35,36,37], was chosen as it allows for an efficient and effective 'object detection' system, both in terms of memory required and computational cost. The main steps of the developed object detection algorithm are summarized below:

• Grayscale conversion of each new frame and preprocessing by spatial convolution with a Gaussian filter (low pass) to reduce aliasing effects and noise.
• A number of initial frames are stored in a buffer $S(x,y,i)$ and used to build the single gaussian background model. Here $x,y$ denote pixel position while $i$ is the index of the single image in the buffer. In particular, mean intensity value $\mu$ and standard deviation $\sigma$ are computed as:

  $$\mu \left(x,y\right)={\displaystyle \frac{1}{n}}\sum _{i=1}^{n}S\left(x,y,i\right) \sigma \left(x,y\right)=\sqrt{{\displaystyle \frac{1}{n-1}}\sum _{i=1}^n{\left(S\left(x,y,i\right)-\mu \left(x,y\right)\right)}^2}$$

  (21)
• Once the background model is ready, at each iteration, once a new frame becomes available, the model is coregistered on the current frame following a procedure similar to the one detailed in [35]. The portion of the background which lies outside the new frame is discarded while the new portion of the scene which is not present in the model is flagged as background with maximum learning rate $\beta =1$. This is done by computing the homography matrix $H$ which describes the perspective transformation between model and current frame using corner points as anchor points and applying this matrix on the background model (warping).
• For each pixel $X_c\left(x,y\right)$ in the current frame, a corresponding pixel in the background model $X_B(x^{\text{'}},y^{\text{'}})$ is computed as

  $$X_b=H\cdot X_c$$

  (22)
• The comparison of the intensity value of a certain pixel in the current frame with the neighborhood of the corresponding pixel in the background model allows to determine whether or not said pixel belongs to the foreground. In analytical form this is expressed by Eq. (23)(24) and (25).

  $$D_{X_c,X_i}={\displaystyle \frac{1}{{\sigma }^2\left(X_i\right)}}{\left(I\left(X_c\right)-\mu \left(X_i\right)\right)}^2$$

  (23)

  $${\widehat{X}}_b=argmi{n}_{X_i}\left(D_{X_c,X_i}\right) \mathrm{where} X_i\in N\left(X_b\right)$$

  (24)

  $$\left(X_c\right)=\left\{\begin{array}{c}Background \mathrm{i}\mathrm{f} D_{X_C,{\widehat{X}}_b}<\theta ;\\ Foreground otherwise\end{array}\right.$$

  (25)

  where $\theta$ is a threshold chosen arbitrarily (design parameter).
• Once background and foreground section of the frame have been identified (segmentation process), the model is updated according to Eq. (26), (27)

  $${\displaystyle \genfrac{}{}{0pt}{}{{\mu }_b=\left(1-\alpha \right)\cdot {\mu }_b+\alpha \cdot I_b}{{\sigma }_b=\left(1-\alpha \right)\cdot {\sigma }_b+\alpha \cdot |I_b-{\mu }_b|}} 0\le \alpha \le 1$$

  (26)

  $${\displaystyle \genfrac{}{}{0pt}{}{{\mu }_f=\left(1-\beta \right)\cdot {\mu }_f+\beta \cdot I_f}{{\sigma }_f=\left(1-\beta \right)\cdot {\sigma }_f+\beta \cdot |I_f-{\mu }_f|}} con 0\le \beta \le 1$$

  (27)

In the previous equations, the subscripts _b,f indicate that the corresponding mean value $\mu$or standard deviation $\sigma$ must be considered on the portion of the scene labelled as background or foreground respectively. The terms $\alpha$, $\beta$ are the learning rates of the model. To better understand the effect of these parameters, let’s consider some limit cases. The case for $\alpha =\beta =0$ represent a static background model which is never updated. When a new object of interest appears in the scene it will be permanently detected in the associated binary mask. This is desirable but the associated problem lies in the fact that if there’s an actual change in the background scene due to camera motion or dynamic lighting conditions or others, such differences in the scene would be flagged as false positives (ghosting) and the model would be unable to adapt to the dynamic change in the scene. The limit case for $\alpha =\beta =1$ is equivalent to having no background pattern at all. With these settings, in fact, each frame is instantaneously compared only with the previous one, resulting in what in technical terms is defined as inter-frame background subtraction. In this case, there are no ghosting problems, but there are at least two important limitations. Since there is no background pattern in fact, the object is only highlighted if it has a relative motion w.r.t to the previous frame and for objects with a uniform pattern, only the edges are highlighted, drastically reducing the effectiveness of detection. The foreground learning rate parameters $\alpha$ is related to the speed at which regions of the model labelled as foreground are updated with new information coming from the current frame. A low value means that an object of interest will exhibit its presence in the binary mask for longer times (which is desirable) but at same time excessively low values would determine unwanted streaks with longer decay time associated with object of interest motion within the FOV. Conversely a higher value is beneficial for reducing streaks in the wake of object motions but could entail shorter times of effective detection in terms of binary mask for an object of interest. Similar considerations can be made for the effects of the background learning rate parameter $\beta .$

7.

Connected Component Analysis (CAA) is finally applied to condense several close blobs into the same track reading. In the chosen implementation, a connectivity analysis is essentially performed to filter out the various clusters of scattered pixels with little detection significance and due essentially to noise, in order to select only a trace with dimensions compatible with the intended purposes.

Referring to Figure 7, the output of the object detection algorithm is fed as input to the track validation stage. Specifically bounding box size and its centroid coordinates are evaluated in the track validation algorithm for statistical coherence before being confirmed as a valid track and passed on to the Extended Kalman filter for data fusion with radar reading.

3.2. Avoid Module

The decision-making algorithm for pair wise non-cooperative aircraft collision avoidance chosen for the developed system is adopted from the results of the work [38] and it is based on the concept of minimum separation distance:

$${\overrightarrow{d}}_{AB}={\displaystyle \frac{\overrightarrow{r} \cdot {\overrightarrow{V}}_{AB} }{{‖{\overrightarrow{V}}_{AB}‖}^2}}{\overrightarrow{V}}_{AB}-\overrightarrow{r}$$

(28)

where ${\overrightarrow{d}}_{AB}$ is the minimum distance that will be registered between the two aircraft at future time, $\overrightarrow{r}$ is the relative distance vector and ${\overrightarrow{V}}_{AB}$ is the relative speed vector, and it’s valid only for linear trajectories at constant speed. In this framework, the intruder aircraft is modelled as a safety bubble of radius R with constant speed ${\overrightarrow{V}}_B$ whereas the UAV is a point mass with a 3DOF dynamic, and a constant speed ${\overrightarrow{V}}_A$. With these assumptions, the collision avoidance problem can be formulated as a two-stage problem:

• “Conflict detection problem”: at every processing step, the feasibility of a potential conflict in the future is investigated by determining whether or not, loss of minimum separation will occur at a future time. In other words, it is determined whether the UAV will cross the safety bubble centred around the intruder aircraft. If the response is affirmative the next problem is solved.
• “Conflict resolution problem”: based on the concept of collision cone, during conflict resolution stage, the system computes a heading setpoint that will bring the trajectory of the UAV to be tangent to the safety bubble around the intruder aircraft.

It can be proved [39], that two objects are bound for a collision if Eq. (29)(52) holds:

$$‖{\overrightarrow{d}}_{AB}‖<R and \dot{r}<0$$

(29)

The algorithm thus, at each time step, starting from the position and velocity states of the intruder aircraft, given by the EKF and the UAV states provided by the INS, computes the relative distance and speed to update the predicted minimum separation distance vector ${\overrightarrow{d}}_{AB}$. If the norm of the latter is lower than the safety bubble radius, then a collision is predicted to occur, and an evasive manoeuvre is computed to avoid the obstacle. Conflict resolution is based on the concept of collision cone [38]:

$$C_{AB}\left(R\right)=\left\{{\overrightarrow{V}}_A\left(V_A,{\gamma }_A,{\chi }_A\right)\in {\mathcal{R}}^3|V_A>0, {\gamma }_A\in ]-{\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}}], {\chi }_A\in [-\pi ,\pi ], \dot{r}<0, {‖{\overrightarrow{d}}_{AB}\left({\overrightarrow{V}}_A\right)‖}^2<R^2\right\}$$

(30)

i.e. all the triplets of airspeed $V_A^d$, flight path angle ${\gamma }_A^d$, and ground track angle ${\chi }_a^d$ resulting in a speed vector inside the collision cone defined be Eq. (30) will result in a collision. The set of solutions is composed of all the combinations of speed vectors that lie outside such collision cone and at the same time comply with UAV aero mechanical constraints:

$$\mathsf{\Sigma }=\left\{\left(V_A^d,{\gamma }_A^d,{\chi }_a^d\right)\in {\mathcal{R}}^3\left|V_A^d\in \left[V_{Am},V_{AM}\right], {\gamma }_A^d\in \left[{\gamma }_{am},{\gamma }_{AM}\right], {\chi }_A^d\in \right]-\pi ,\pi ],r\ge 0 or {‖{\overrightarrow{d}}_{AB}\left(V_A^d,{\gamma }_A^d,{\chi }_A^d\right)‖}^2\ge R^2\right\}$$

(31)

Each solution provided by Eq. (31) is clearly a viable one. However, the tangential solution is chosen, as suggested in the original paper because it is the one the minimize the deviation from the nominal trajectory. This can be obtained from Eq. (32)

$${‖{\overrightarrow{d}}_{AB}‖}^2={‖\overrightarrow{r}‖}^2-{\displaystyle \frac{{\left(\overrightarrow{r}\cdot {\overrightarrow{V}}_{AB}\right)}^2}{{‖{\overrightarrow{V}}_{AB}‖}^2}} \equiv R^2$$

(32)

After some manipulation, Eq. (32) yields:

$$\begin{array}{ll}(r_x{V}_A\cos {\gamma }_A\cos {\chi }_A& {+r_y{V}_A\cos {\gamma }_A\sin {\chi }_A-r_z{V}_A\sin {\gamma }_A)}^2\\ & -\left(2\overrightarrow{r}\cdot {\overrightarrow{V}}_B\right){\displaystyle \frac{r_x\cos {\gamma }_A\cos {\chi }_A+r_y\cos {\gamma }_A\sin {\chi }_A-r_z\sin {\gamma }_A}{V_A}}+{\left({\displaystyle \frac{\overrightarrow{r}\cdot {\overrightarrow{V}}_B}{V_A}}\right)}^2\\ & ={\displaystyle \frac{{‖\overrightarrow{r}‖}^2-R^2}{V_A^2}}[V_A^2+V_B^2-2V_A(V_{BX}\cos {\gamma }_A\cos {\chi }_A\\ & -V_{BY}\cos {\gamma }_A\sin {\chi }_A-V_{BZ}\sin {\gamma }_A\left)\right]\end{array}$$

(33)

This can be interpreted as searching for all the triplets of: UAV airspeed $V_A$, flight path angle ${\gamma }_A$ and ground track ${\chi }_A$ that correspond to a tangential solution for the particular conflict scenario defined by the triplets of R, $\overrightarrow{r}$, ${\overrightarrow{V}}_B$. Evidently there are ${\infty }^2$ solutions corresponding to all the tangent vectors to the safety bubble in the 3D space. As specified in chapter 1.2.2, for the present application, an evasive maneuver based on a coordinated turn on the horizontal plane is considered. Thus, the only unknown of Eq. (33), is the UAV ground track angle ${\chi }_A$ while, $V_A$ and ${\gamma }_A$ can be considered constant. The previous equation is thus rearranged yielding:

$$a_1{\cos }^2{\chi }_A+a_2{\sin }^2{\chi }_A+a_3\cos {\chi }_A\sin {\chi }_A+a_4\cos {\chi }_A+a_5\sin {\chi }_A=b$$

(34)

With: $a_1=r_x^2{\cos }^2{\gamma }_A$, $a_2=r_y^2{\cos }^2{\gamma }_A,$ $a_3=2r_x{r}_y{\cos }^2{\gamma }_A,$

$a_4=2{\displaystyle \frac{{‖\overrightarrow{r}‖}^2-R^2}{V_A^2}}{V}_A{V}_{BX}\cos {\gamma }_A-2r_x{r}_z\cos {\gamma }_A\sin {\gamma }_A-2{\displaystyle \frac{\overrightarrow{r}\cdot {\overrightarrow{V}}_B}{V_A}}{r}_x\cos {\gamma }_A$

$a_5=2{\displaystyle \frac{{‖\overrightarrow{r}‖}^2-R^2}{V_A^2}}{V}_A{V}_{BY}\cos {\gamma }_A-2r_y{r}_z\cos {\gamma }_A\sin {\gamma }_A-2{\displaystyle \frac{\overrightarrow{r}\cdot {\overrightarrow{V}}_B}{V_A}}{r}_y\cos {\gamma }_A$

$b={\displaystyle \frac{{‖\overrightarrow{r}‖}^2-R^2}{V_A^2}}\left(V_A^2+V_B^2\right)+2{\displaystyle \frac{{‖\overrightarrow{r}‖}^2-R^2}{V_A^2}}{V}_A{V}_{BZ}\sin {\gamma }_A-r_z^2\sin {\gamma }_A-2{\displaystyle \frac{\overrightarrow{r}\cdot {\overrightarrow{V}}_B}{V_A}}{r}_Z\sin {\gamma }_A-{\left({\displaystyle \frac{\overrightarrow{r}\cdot {\overrightarrow{V}}_B}{V_A}}\right)}^2$

This 4^th order polynomial yields four solutions corresponding to two tangent solutions in the forward collision cone and two tangent solutions in the backward collision cone as explained in [38]. By filtering out the solutions pertaining to the backward collision cone, two viable ground track angles are left and can be chosen to perform the maneuver. The decision-making algorithm developed for this work always selects the angle which allows the UAV to pass behind the approaching intruder aircraft rather than in front (i.e. turning right for a starboard approaching encounter and turning left for a port approaching encounter); for a front collision encounter a right turn is always chosen. A Finite State Machine (FSM), schematized in Figure 11, handles the transition between the state of navigation and avoidance. A further differentiation is made between a state where the target is within the FOV where both its velocity and position states are constantly corrected with measurements coming from the sensors and a state when the target is outside the FOV and where its states are updated solely by means of the dynamic model. If the intruder aircraft exits the FOV (f.e. due to the avoidance maneuver which turns the nose of the UAV) the maneuver is conducted on the basis of the projected states until the rate of change of the relative distance between UAV and projected target position ( $\dot{r}$) becomes positive again. If on the other hand, the intruder aircraft comes back within the FOV, the ongoing maneuver is updated and corrected on the basis of new measurements. Once the conflict scenario is solved, FST brings the control back to the navigation module which sets the original trajectory waypoints as setpoints for the autopilot.

3.3. Data Fusion

Fusion between optical and radar data is accomplished by means of an implementation of the Extended Kalman Filter (EKF). The use of Kalmand Filters (and its variations) for the purposes of collision avoidance is well known in literature: some examples are [19] where the filter is used to fuse radar and visual tracks with a Track-to-Track Fusion (T2TF) approach, taking into account uncertainties associated with each sensor tracking processes; in [40] the performance of an Unscented Kalman Filter (UKF) based tracking system is investigated for different SAA flight scenarios; in [41] the problem of cooperative collision avoidance in a formation of flight of multiple UAVs is tackled by means of an EKF together with a model predictive control (MPC) strategy. The mathematical formulation of the data fusion strategy adopted for the system implementation is briefly summarized below, further details can be found in [42]:

The state vector is composed of position and velocities states of the intruder aircraft expressed in a fixed North East Down (NED) cartesian reference frame which is supposed inertial by neglecting Earth’s rotation and revolution motions Eq. (35).

$${\mathit{x}}_{\mathit{N}\mathit{E}\mathit{D}}=\left[x_N,{\dot{x}}_N,y_N,{\dot{y}}_N,z_N,{\dot{z}}_N\right]$$

(35)

Measurements are obtained in a reference system fixed with camera by combining azimuth and elevation readings given by Eq. (7) with a distance measure provided by the radar as expressed by Eq. (36)

$$\begin{gathered} x_{cam}=R\cos \lambda \cos \psi \\ {y}_{cam}=R\cos \lambda \sin \psi \\ {z}_{cam}=R\sin \lambda \end{gathered}$$

(36)

It’s useful to express the measurement vector $m=[R,\lambda , \psi ]$ in terms of position states expressed in camera reference frame as in Eq (37)

$$\begin{gathered} R=\sqrt{x_{cam}^2+y_{cam}^2+z_{cam}^2} \\ \lambda ={\sin }^{-1}\left({\displaystyle \frac{z_{cam}}{R}}\right) \\ \psi ={\tan }^{-1}\left({\displaystyle \frac{y_{cam}}{x_{cam} }}\right) \end{gathered}$$

(37)

The intruder centre position can be converted from camera axes to UAV body axes by means of a suitable transformation matrix (describing the camera pose):

$${\left[x_b,y_b,z_b\right]}^T={\mathit{T}}_{BC}{\left[x_{cam},y_{cam},z_{cam}\right]}^T$$

(38)

Then, the relationship between state vector and measurement is obtained combining Eq. (37) and (38) with the transformation matrix between body and NED axes expressed in Eq. (39).

$$\begin{gathered} \left[\begin{array}{c}{x}_B\\ y_B\\ z_B\end{array}\right]=\left[\begin{array}{ccc}\cos \theta \cos \psi & \cos \theta \sin \psi & -\sin \theta \\ \sin \varphi \sin \theta \cos \psi -\cos \varphi \sin \psi & \sin \varphi \sin \theta \sin \psi +\cos \varphi \cos \psi & \sin \varphi \cos \theta \\ \cos \varphi \sin \theta \cos \psi +\sin \varphi \sin \psi & \cos \varphi \sin \theta \sin \psi -\sin \varphi \cos \psi & \cos \varphi \cos \theta \end{array}\right]\cdot \\ \cdot \left[\begin{array}{c}{x}_N-x_{UAV}\\ y_N-y_{UAV}\\ z_N-z_{UAV} \end{array}\right] \end{gathered}$$

(39)

where $\psi , \theta , \varphi$ are the UAV’s yaw, pitch and roll angles respectively. The relationship between measurement model and state vector components is clearly nonlinear, hence the need to employ an extended Kalman filter. Within collision avoidance literature, it is common practice to assume stationary motion for the intruder aircraft (in an inertial reference frame) therefore, it is assumed that the speed of the intruder is constant within the small integration step such as the position state update can be described by:

$${\mathit{x}}^{\mathit{i}+1}={\mathit{x}}^i+\mathit{v}\cdot \mathsf{\Delta }t$$

(40)

where $\mathit{v}$ is the speed vector and $\mathsf{\Delta }t$ the integration step. State transition matrix can thus be expressed as:

$${\widehat{\mathit{x}}}_k=\left[\begin{array}{c}{\widehat{x}}_k\\ {\widehat{\dot{x}}}_k\\ {\widehat{y}}_k\\ {\widehat{\dot{y}}}_k\\ {\widehat{z}}_k\\ {\widehat{\dot{z}}}_k\end{array}\right]=\left[\begin{array}{cccccc}1& \mathsf{\Delta }t& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& \mathsf{\Delta }t& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& \mathsf{\Delta }t\\ 0& 0& 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}\begin{array}{c}{\widehat{x}}_{k-1}\\ {\widehat{\dot{x}}}_{k-1}\\ {\widehat{y}}_{k-1}\\ {\widehat{\dot{y}}}_{k-1}\\ {\widehat{z}}_{k-1}\\ {\widehat{\dot{z}}}_{k-1}\end{array}\end{array}\right]={\mathit{F}}_k{\widehat{\mathit{x}}}_{k-1}$$

(41)

The Jacobian of the measurement model is computed as follows:

$${\mathit{H}}_k={\left.{\displaystyle \frac{\partial h}{\partial {\mathit{x}}_{\mathit{N}\mathit{E}\mathit{D}}}}\right|}_{{\widehat{\mathit{x}}}_{k|k-1}}=\left[\begin{array}{cccccc}{K}_R{r}_X& 0& K_R{r}_Y& 0& K_R{r}_z& 0\\ K_{\lambda }{\lambda }_x& 0& K_{\lambda }{\lambda }_y& 0& K_{\lambda }{\lambda }_z& 0\\ K_{\psi }{\psi }_x& 0& K_{\psi }{\psi }_y& 0& K_{\psi }{\psi }_z& 0\end{array}\right]$$

(42)

where:

$$\begin{gathered} K_R={\displaystyle \frac{1}{2\sqrt{\left(x_B^2+y_B^2+z_B^2\right)}}} \\ {r}_X=2x_B{R}_{NB}\left(\mathrm{1,1}\right)+2y_B{R}_{NB}\left(\mathrm{2,1}\right)+2z_B{R}_{NB}\left(\mathrm{3,1}\right) \\ {r}_Y=2x_B{R}_{NB}\left(\mathrm{1,2}\right)+2y_B{R}_{NB}\left(\mathrm{2,2}\right)+2z_B{R}_{NB}\left(\mathrm{3,2}\right) \\ {r}_Z=2x_B{R}_{NB}\left(\mathrm{1,3}\right)+2y_B{R}_{NB}\left(\mathrm{2,3}\right)+2z_B{R}_{NB}\left(\mathrm{3,3}\right) \\ {K}_{\lambda }=-{\displaystyle \frac{1}{\sqrt{1-{\left(\frac{z_B}{R}\right)}^2}}} \\ {\lambda }_x={\displaystyle \frac{R_{NB}\left(\mathrm{3,1}\right)R-\frac{z_B}{2\sqrt{R}}\left(2x_B{R}_{NB}\left(\mathrm{1,1}\right)+2y_B{R}_{NB}\left(\mathrm{2,1}\right)+2z_B{R}_{NB}\left(\mathrm{3,1}\right)\right)}{R^2}} \\ {\lambda }_y={\displaystyle \frac{R_{NB}\left(\mathrm{3,2}\right)R-\frac{z_B}{2\sqrt{R}}\left(2x_B{R}_{NB}\left(\mathrm{1,2}\right)+2y_B{R}_{NB}\left(\mathrm{2,2}\right)+2z_B{R}_{NB}\left(\mathrm{3,2}\right)\right)}{R^2}} \\ {\lambda }_z={\displaystyle \frac{R_{NB}\left(\mathrm{3,3}\right)R-\frac{z_B}{2\sqrt{R}}\left(2x_B{R}_{NB}\left(\mathrm{1,3}\right)+2y_B{R}_{NB}\left(\mathrm{2,3}\right)+2z_B{R}_{NB}\left(\mathrm{3,3}\right)\right)}{R^2}} \\ {K}_{\psi }={\displaystyle \frac{1}{1+{\left(\frac{y_B}{x_B}\right)}^2}} \\ {\psi }_x={\displaystyle \frac{R_{NB}\left(\mathrm{2,1}\right)x_B-R_{NB}\left(\mathrm{1,1}\right)y_B}{x_B^2}} \\ {\psi }_y={\displaystyle \frac{R_{NB}\left(\mathrm{2,2}\right)x_B-R_{NB}\left(\mathrm{1,2}\right)y_B}{x_B^2}} \\ {\psi }_z={\displaystyle \frac{R_{NB}\left(\mathrm{2,3}\right)x_B-R_{NB}\left(\mathrm{1,3}\right)y_B}{x_B^2}} \end{gathered}$$

(43)

The terms $R_{NB}(m, n)$ are the corresponding elements of the rotation matrix (39) which maps NED coordinates into body-fixed coordinates. Standard Kalman formulation follows. When measurements are not available (due to the lower refresh rate of the radar sensor w.r.t optical data) or when, following the evasive maneuver intruder aircraft exit the FOV, states are clearly updated only by means of the model update; in other words, no correction is carried out when measurements are not available. Both the initial intruder position and velocity states are initialized following a batch linear least square regression method for higher robustness against the presence of noise. In a nutshell, once a track is validated and confirmed, $n$ number of measurements are stored before initializing the filter. These are converted in position states expressed in the NED reference frame by means of the inverse of equation (39). Then, matrices $\mathit{A}\in {\mathbb{R}}^{nx2}$, $\mathit{B}\in {\mathbb{R}}^{nx1}$ and $\mathit{x}\in {\mathbb{R}}^{2xn}$ are defined as:

$$\mathit{A}=\left[\begin{array}{cc}1& t_1-t_0\\ 1& t_2-t_1\\ ⋮& ⋮\\ 1& t_n-t_{n-1}\end{array}\right] \mathit{B}=\left[\begin{array}{c}{p}_1\\ p_2\\ ⋮\\ p_n\end{array}\right] \mathit{x}=\left[\begin{array}{ccc}{x}_0& \dots & x_0\\ v_0& \dots & v_0\end{array}\right]$$

(44)

where $p_1, \dots , p_n$ are clearly the position states and $\mathit{x}$ is the vector of the unknown initial position and velocity states such that $\mathit{A}\mathit{x}=\mathit{B}$. The model clearly assumes constant speed for the intruder aircraft. The overdetermined system is solved by means of the well-known linear least square close formulation as:

$$\mathit{x}=(x_0,v_0)={\left({\mathit{A}}^T\mathit{A}\right)}^{-1}{\mathit{A}}^T\mathit{B}$$

(45)

The procedure is repeated for each component of 3D motion. Then the EKF is initialized with ${\mathit{v}}_0^{\mathit{E}\mathit{K}\mathit{F}}=[v_{0x},v_{0y},v_{0z}]$ and ${\mathit{x}}_0^{\mathit{E}\mathit{K}\mathit{F}}={\mathit{x}}_0+{\mathit{v}}_0^{\mathit{E}\mathit{K}\mathit{F}}\cdot t_n$Once the filter is initialized, the norm of the innovation vector is used to determine the convergence of the states. The $i^{th}$ component of the innovation vector at time $t$ is defined as:

$$er{r}^i={\displaystyle \frac{1}{w}}\sum _{k=t-w}^t{y}_k^i-{\displaystyle \frac{1}{w}}\sum _{k=t-w}^t{m}_k^i$$

(46)

where $m_k^i$ is the measurement computed at time $t=k$ whereas $y_k^i$ is the reconstructed measurement from the predicted states and $w$is the length of the moving average used to populate each component of the innovation vector. At each simulation step, the norm of innovation vector $\left|err\right|$ is compared against a threshold and when it is below it, the filter is considered converged.

In chapter 4, results of the EKF in terms of reconstruction of position and velocity states from noisy measurements are shown for a starboard encounter collision scenario.

3.4. Radar Algorithms

The Radar Doppler used in the SAA and its processing algorithms were designed and manufactured by ECHOES radar technologies, one of the industrial partners of the project. The two main algorithms developed are briefly summarized below.

3.4.1. Detection

Figure 12 shows a high-level scheme of the signal processing architecture for the radar sensor.

The architecture has been implemented in multithread C code on the ARM processor of the motherboard of the Radar and it leverages optimized mathematical libraries such as FFTW for Fourier Transform computation and BLAS for linear algebra calculations. For each receiving channel, range profiles are fed as input to a Doppler FFT block. This block implements a floating-point Fast Fourier Transform (FFT) with a user programmable number of points. The output of this block is represented by the range-Doppler maps representing the scene captured by the radar in a bidimensional domain range-doppler speed Figure 13 (a). The detection module is tasked with the population of a list of detected tracks for each frame. It employs an implementation of a Constant False Alarm Rate (CFAR) detector algorithm. This adaptive algorithm is aimed at the detection of targets surrounded by background noise (clutter) with a tolerable false alarm probability. As shown in Figure 13(b), this technique compares the Cell Under Test (CUT) with an appropriate threshold which is function of the false alarm probability and of the leading and lagging cells. The cells immediately adjacent to the CUT (Guard cells) are ignored to avoid corrupting this estimate with power from the CUT itself. In particular, the developed system implements the cell-averaging CFAR (CA-CFAR) shown in Figure 13 (c). The CUT is compared with a threshold given by:

$$\alpha =P_{FA}^{-{\displaystyle \frac{1}{N_{ref}}}}-1$$

(47)

where $P_{FA}$ is the desired false alarm probability and $N_{ref}$ is the number of reference cells. If the CUT is above the threshold, then a track is identified, otherwise no targets are detected. Starting from the two-dimensional range-doppler map, CFAR is applied along the two directions and the resulting two masks are combined with a logical and operation as shown in Figure 14 (a). The result is a binary mask similar to the output of the computer vision object detector algorithm, showing the detected tracks (Figure 14 (b)).

Morphological operations (dilations) are then applied to the binary mask in order to fill in possible gaps between pixels of the same object occupying several adjacent cells. Finally, a unique label is assigned to each region of connected pixel in the current frame and a series of properties is computed for each track such as range, doppler speed, power, signal to noise ratio, detection area in pixels and bounding box.

3.4.2. Monopulse

Monopulse is a technique for the measurement of the Direction of Arrival (DOA) of a signal by comparing the contributions received simultaneously by two or more antennas. The developed SAA radar implements a phase monopulse technique. The working principle is briefly summarized below. The idea is to exploit the difference in phase of the same signal received by different antennas as shown in Figure 15.

By considering a radar consisting of two receiving antennas with a spatial separation $d<\lambda /2$ having their beams pointing in the same direction and applying the parallel beam approximation, the distance between each antenna and the target can be expressed as:

$$\begin{gathered} R_1\approx R+{\displaystyle \frac{d}{2}}\sin \left(\theta \right) \\ {R}_2\approx R-{\displaystyle \frac{d}{2}}\sin \left(\theta \right) \end{gathered}$$

(48)

Thus, the difference between the two distances is $\mathsf{\Delta }R=R_1-R_2=d\sin \left(\theta \right)$ where $d$ is the physical spatial separation between the two antennas and $\theta$ is the DOA to be estimated. The distance $\mathsf{\Delta }R$ is related to the wavelength $\lambda$ and to the phase difference $\varphi$ by the well-known Eq. (49).

$$\varphi ={\displaystyle \frac{2\pi }{\lambda }} \mathsf{\Delta }R={\displaystyle \frac{2\pi d}{\lambda }}\sin \left(\theta \right)$$

(49)

By inverting the previous equation, the formula for the DOA estimation is obtained:

$$\theta ={\sin }^{-1}\left({\displaystyle \frac{\varphi }{2\pi }}\cdot {\displaystyle \frac{\lambda }{d}}\right)$$

(50)

Since phase is periodical, there is a maximum value of $\theta$ in modulus that can be estimated without ambiguity and it is given by:

$$\left|\theta \right|\le {\sin }^{-1}\left({\displaystyle \frac{\lambda }{2d}}\right)$$

(51)

In the present application, $\varphi$ is obtained by computing the interferogram between the range-Doppler maps of the two receiving channels as described by Eq. (52)

$$\gamma ={\displaystyle \frac{1}{\mathrm{N}}}\sum _{n=1}^N{\displaystyle \frac{X_n{Y}_n^{*}}{\sqrt{{\left|X_n\right|}^2{ \left|Y_n\right|}^2}}}$$

(52)

Where $N$ is the number of pixels within each bounding box whereas $X_n$ and $Y_n$ are the complex numbers populating the two range-Dopplers maps. Each region of pixel which has been detected and labelled is thus also accompanied by the value of the estimated $\theta$. At this stage of the development of the system however, this estimation has not yet been incorporated into the EKF for the refinement of track estimates or used as a hint for delimiting the search region of the computer vision-based object detector.

4. Simulation Campaign

The effectiveness of the system and the real time interaction between sense and avoid modules have been tested by means of a complex, hardware-in-the-loop simulation environment that was implemented at the Flight-By-Wire laboratories of the aerospace section of the Civil and Industrial Engineering Department of the University of Pisa. A realistic, completely non-linear 6 DOF flight dynamic model of the aircraft was developed in Simulink leveraging aero-mechanical data and Flight Control Laws provided by courtesy of SES. The model features an automatic trim finding routine which exploits a gradient descent strategy and automatically computes engine setting and control surface deflection to ensure equilibrium for each flight condition. Initially, for the purposes of testing avoidance algorithms alone, the position of intruder aircraft was considered known in advance. The effectiveness of the avoidance manoeuvre was evaluated according to several metrics that are summarized in Figure 16 (a). Among these, are the minimum registered distance between UAV and intruder, the duration of the anti-collision evasive manoeuvre, the normal deviation w.r.t the original trajectory at the end of the manoeuvre and the maximum normal deviation registered.

A campaign of simulations was conducted for a combination of different speeds and initial starting positions of the intruder aircraft; the latter were taken on a circumference centred on the initial position of the UAV as shown in Figure 16 (b). The heading of the intruder aircraft was pre-computed for each combination of starting position and speed module as to guarantee a perfect collision supposing, linear, constant speed trajectory for both aircraft with no evasive action carried out by the UAV. Figure 17 shows some results of the simulations: plot (a) shows the 2D trajectory of the UAV, plot (b) shows aileron deflection command which is always well below the saturation limit of ${\delta }_a^{max}={10}^{°}$ prescribed by the manufacturer. Figure 18 (a) shows that the minimum distance registered is always above the threshold of the safety bubble; clearly the value of minimum distance decreases as the intruder speed increases; plot (b) shows the maximum normal distance to the original planned trajectory. It’s interesting to note that as the intruder speed increases, the coordinated turn required for a successful evasive manoeuvre UAV becomes steeper. Subsequently, simulations were repeated with a higher fidelity model including the Extended Kalman Filter, where camera intrinsic and extrinsic matrix were used, together with precomputed intruder trajectory, to reconstruct, for each simulation step, the apparent position of the intruder within the FOV in terms of relative elevation and azimuth angles. Zero mean, Gaussian noise was then artificially injected both in relative distance and angle readings to simulate for real sensor noise. Figure 19 shows the reconstruction of position and velocity states by the EKF plotted against ground truth and noisy measurements. The lag between the moment the intruder is visible, and the moment where states begin to be reconstructed (blue line in Figure 19) is due to the fact that a certain number of initial readings are used to estimate the initial velocity states by means of a linear regression as explained in chapter 3.3. Even when the initial reconstruction of velocity states is not accurate due to biased initial measurements, the filter converges rapidly to the true velocity states value. In the simulation it is chosen a threshold on the norm of the innovation factor of $t{h}_{err}={10}^{-2}$.

The first dashed vertical line indicates the moment the intruder is detectable (within radar range), the second dashed line indicates the moment where the EKF has reached convergence and its output is fed into avoidance algorithms.

After the validation of the avoidance algorithms in the presence of sensor noise, the complete HIL simulation system, whose high-level scheme is shown in Figure 20, was used to test the simultaneous interaction between sense and avoid algorithms. Essentially, the 6 DOF flight mechanic model, drives a real time realistic rendering of the scene, leveraging the open-source flight simulator Flight Gear v 2020.3 (FG). A python program was then developed to efficiently acquire screenshot of the monitor where the FG instance was running and a Gstreamer pipeline was built to send the video stream from the host PC to the VPU via UDP protocol in real time. In order not to saturate the network adapter of the Nvidia Jetson Nano, the video stream was compressed according to the H264 (MPEG-4 AVC) standard. The two pipelines used for sending and receiving video data are schematized in Figure 21. The intruder aircraft was inserted within the FG visual rendering by creating a tailored xml file defining the scenario and its waypoints according to its precomputed trajectory. The intruder aircraft was rendered with a mockup of the Cessna 172p SkyHawk single-engine aircraft. Some glue code in the scripting language nasal interfacing FG properties tree with the underlying C++ simulation engine code was required to ensure proper synchronization between the various components of the simulation framework. Additional code was required to create a camera view in front of the aircraft as to simulate the SAA camera sensor positioned on the nose of the UAV. An example of such view is shown in Figure 22. Sense algorithms were re-implemented on the Jetson Nano by means of Python code leveraging optimized OpenCV library when possible, to speed up the execution rate. Likewise, the avoidance decision-making algorithms, including the Extended Kalman filter and the finite state machine, was deployed on the jetson nano as well. The intruder range was sent to the jetson nano alongside the video stream on the same UDP interface. The VPU was thus processing the video stream in real time, performing both object detection and tracking and merging the distance information to computing the evasive maneuver. The setpoints on the ground track angle outputted by the avoidance algorithms by the VPU were then fed back to the Simulink model via another UDP interface and were used to update the simulation in real time, so that at each simulation step the position of the UAV could reflect the results of the SAA system calculations.

All the simulations conducted have shown a full compliance with aero mechanical constraints of the aircraft while at the same time respecting the safety boundaries; the minimum separation distance is never violated for every initial position of the intruder aircraft according to the scheme of Figure 16 (b) and for a range of intruder’s flight speed equal to $V_{intr}\in \left[22, 52\right] m/s$, greatly exceeding the cruise speed of other UAV belonging to the same class as imposed by system requirements. Figure 23 illustrates some results in terms of UAV flight parameters throughout the evasive manoeuvre, obtained with the HIL complete simulation framework, for a conflict scenario with intruder aircraft approaching from starboard side and flying at a constant speed of $V_{intr}=50 m/s$. For the same simulation, Figure 24 shows the comparison between the actual elevation and azimuth as they would appear on the sensors based on the ground truth position of the intruder aircraft and the ones reconstructed by the SAA system, compensated for the transmission and processing delay of the signal between VPU and host pc. Figure 25 shows some screenshots from the Flight Gear rendering relative to the same collision scenario with the intruder aircraft approaching from the starboard side, highlighted with a red bounding box. Both the intruder aircraft and the UAV were rendered with a Cessna 172p mockup. Notice however that the simulations were conducted with the viewpoint shown in Figure 22, with the camera pointing forward so that the UAV actual rendering was meaningless; in Figure 25 UAV rendering has been included for the sake of clarity in showing the evolution of the evasive manoeuvre on the horizontal plane.

; $V_{intr}=50 m/s$. 3D trajectory (a), Euler angles (b), Speed components in NED reference frame (c), load factors (d).

5. Flight Tests

A preliminary flight test campaign aimed at the validation of the effectiveness of the system in detecting an intruder aircraft was conducted at the Lucca-Tassignano airport in conjunction with SES and Echoes. Due to flight safety restrictions, it was impractical to operate two distinct UAV at the same time in the same airspace in light of the fact that the sense and avoid system hasn’t received yet airworthiness certification; it was therefore decided to position the SAA system fixed on the ground and compare track readings of the detected aircraft with actual telemetry data recorded onboard during the flight. The prototype of the SAA system that was used during the test is shown in Figure 26.

As highlighted by the picture, the system is composed of several components, briefly listed below:

• Color camera Arducam 1/4” 8MP IMX219, 30 FPS. Recording resolution was set at 1280 x 720. Due to the small footprint size of the target in terms of pixels it was chosen a lens with a horizontal FOV of around $FO{V}_h={50}^{\circ }$.
• Sense and Avoid doppler radar, developed by Echoes.
• Vision Processing Unit. Nvidia Jetson Nano was chosen thanks to its small size and weight and low power absorption.
• Receiving and transmitting antennas are housed inside a fiberglass mockup of the nose of the aircraft to better simulate operating conditions of the system. Fig shows the position of the antennas within the nose. Camera sensor was mounted on the nose mockup in such a way to ensure the alignment of the camera Line of Sight with radar beam.
• Lipo batteries provide power to the radar at a tension of 22V, while a voltage regulator is employed to provide power to VPU and camera at a lower voltage.
• Two different workstations were used for real time monitoring of both radar and camera sensors.

Figure 27. (a) Antennas placement within the nose mockup; (b) camera sensor chosen for the system implementation; (c) Jetson nano Vision Processing Unit.

Figure 27. (a) Antennas placement within the nose mockup; (b) camera sensor chosen for the system implementation; (c) Jetson nano Vision Processing Unit.

During the flight test, each sensor was recording on a local storage the result of its own processing; moreover, a synchronization of radar processor and VPU with the system time of the telemetry onboard the aircraft was implemented to guarantee temporal alignment of data acquisition. Specifically, data acquired by each sensor during the test is summarized in Table 2.

Figure 28 shows the position of the Sense and Avoid system during the flight test with respect to the trajectory of the UAV. Tue blue line indicates the direction of the geographic north, the green line is parallel to the SAA LOS and the launch site of the drone is marked with a red rectangle. The drone was launched in the air leveraging its pneumatic catapult platform. It was decided to position the radar with the highway behind (visible in the lower right corner of the image) in order to reduce the ground clutter as much as possible. Radar sensor was designed for operation in the air where ground targets are not so close, thus during flight test some adjustments to the gains were made in order to compensate for the presence of cars and other highly reflective objects nearby. As noticeable in Figure 28, after take-off, the UAV entered an helicoidal climbing trajectory and then it remained in a Loiter circuit (approximately rectangular in shape) at a constant altitude of $h\approx 50 m$.

Telemetry data was post processed and transformed into a cartesian fixed Baseline Reference Frame (BRF), with its origin centred in the SAA system position and the x axis pointing north, $y$ axis towards right and $z$ chosen as to comply with the right-hand rule. The position of the UAV was reconstructed by the system in a cartesian Reference Frame fixed with the SAA system with $x$ axis oriented along the LOS. Attitude angles of the system as recorded by the IMU installed on the radar were used to transform position state recorded by the system in the BRF in order to compare them with telemetry. Figure 29 show one example of such reconstruction of the target position states, expressed in the BRF. Due to the presence of ground clutter, in certain section of the flights radar distance wasn’t available but optical algorithms still correctly performed detection and tracking. In absence of distance readings it was still possible to reconstruct heading and azimuth angles in camera reference frame. Figure 30 shows the comparison of said angles with ground truth (obtained by projecting the real UAV position logged by telemetry into the focus plane by means of camera intrinsic and extrinsic matrices) for two distinct segments of flight.

position state, (b) $y$ position state, (c) $z$ position state.

6. Conclusions

This paper has detailed the design and development of a sense and avoid system for a mini-UAV. The system was designed to comply with the strict SWaP constraints of the aircraft platform manufactured by SES. Particular focus was employed in the development of efficient processing algorithms with a sufficiently high refresh rate and ego motion compensation both for optical and radar sensor to ensure a robust detection and tracking capability. The developed, hardware in the loop, integrated simulation environment has allowed the validation of the sense and avoid algorithms for a multitude of different collision scenarios leveraging a realistic, real-time, dynamic representation of the scene. Finally, the preliminary flight test campaign has produced promising results for the reconstruction of the position states of a representative target aircraft despite its small size and ground clutter noise. Several aspects still need to be investigated and will constitute the core of an important future research effort. First of all, the interaction between the FCS and avoidance algorithms has not been tested because the system was static during flight tests and during simulations, attitude and position states of the UAV were considered known in advance. Attitude angles are essential for the correct reconstruction of target position in an inertial reference frame. Therefore, complete flight tests with two flying aircraft will be conducted in the future where the SAA system will be deployed onboard one of the UAVs. The interaction between FCS and SAA system will be validated during said flight campaign and the test will provide precious insights about the effectiveness of motion compensation capabilities in rejecting noise caused by the ego-motion of the platform and engine vibration. Future research will also investigate more advanced collision avoidance strategies, leveraging 3D manoeuvres with optimality constraints (fuel/time minimization). Another essential aspect to be investigated is the simultaneous presence of multiple intruder aircraft within the FOV, where possibly each of them pose a collision threat. Finally, the benefits of employing an array of camera sensors properly oriented to enlarge the overall available FOV will be investigated together with the utilisation of the coarse azimuth reading provided by the radar with the aim of increasing system capabilities and robustness w.r.t to noise.