Analysis of autonomous flight algorithms for an unmanned aerial vehicle

(1)

SLAM APFA

G G

Master Thesis Report

Analysis of autonomous flight algorithms for an unmanned aerial vehicle

Mattias Sjöberg

May 7, 2018

Spring 2018

Master Thesis, 30 ECTS

(2)

(3)

Abstract

Unmanned Aerial Vehicles (UAV) have been heavily studied in the past decade, where autonomous flights have been a popular subject. More complex applications have led to higher requirements on the autonomous flight algorithms and the absence of performance data complicates the selection of what algorithm to use for various applications. Therefore, this thesis focused in analyzing the performance difference between two methods, Simultaneous Localization And Mapping (SLAM) and Artificial Potential Field Approach (APFA), which are planning and reactive algorithms, respectively.

Fundamental dynamics were applied, Feedback Linear Controllers

(FBLC)s for stabilization and an odometry position model combined with an inverse dynamics technique that linearizes the non-linear odometry model.

The SLAM approach was set up in four steps: landmark extraction which uses a point distance based method for segment separation, combined with a Split-And-Merge algorithm for extracting linear landmarks, data association that validates the landmarks, Extended Kalman Filter (EKF) that uses the landmarks together with the odometry model for estimating the position of the UAV, and a modified TangentBug as the reactive algorithm. The APFA was constructed of two functions, an attractive and a repulsive function.

The two methods were implemented on the robotics simulation platform Virtual Robot Experimentation Platform (V-REP), where a quadcopter was used as the model for the UAV. All theory was implemented onto the quadcopter model and embedded scripts were used for communication within V- REP, mainly through internal Application Programming Interface (API) - functions. Furthermore, a script was written that randomly generates three different types of simulation environments.

The implementation of both methods was analyzed in reaching an arbitrary goal position in terms of: the most successful, the most time efficient and the safest navigation path. Another thing analyzed was the time- and space-complexity of both implemented methods.

The results stated that the implemented APFA and the SLAM approach had approximately equal success rate, SLAM had the safest navigation, was the most time efficient, and had the highest time- and space-complexity for a worst case scenario. One of the conclusions were that improvements could be done in the implementations.

Future work includes adding a proper damping method, improving the flaws in the implemented methods as well as to use V-REP as a Robot Op- erating System (ROS)-node for creating a Software In The Loop (SITL)- simulation, in order to achieve more realistic simulations.

Keywords: UAV, SLAM, Split-And-Merge, APFA, V-REP, Inverse dynamics, FBLC, EKF.

(4)

Acknowledgements

I would like to express my gratitude to both my supervisors, Saqib Sarker and Anna Filipsson for being there as a helping hand, still leaving a lot of room for my own innovation and creativity. Also, I would like to thank all the people in Cybercom Group AB that are working at the Kista office for creating such a great working environment.

Mattias Sjöberg, Umeå, May 2018

(5)

List of Figures

2.1 Quadcopter dynamics . . . 4

2.2 Position update system . . . 7

2.3 Proportional-Integral-Derivative controller . . . 9

3.1 Simultaneous Localization And Mapping process . . . 13

3.2 Artificial Potential Field Approach process . . . 23

4.1 Quadcopter model . . . 27

4.2 3D-Sensor beam pattern . . . 27

4.3 Quadcopter geometry . . . 28

4.4 Simulation environments . . . 31

5.1 Customized user interface . . . 33

5.2 Repulsive test . . . 33

6.1 Velocity versus velocity rate, without inverse dynamics . . . . 35

6.2 Velocity versus velocity rate, with inverse dynamics . . . 36

6.3 Robustness test translational acceleration . . . 38

6.4 Robustness test rotational acceleration . . . 38

6.5 Extended Kalman Filter convergence . . . 39

6.6 Standard deviation for the APFA straight-line-test . . . 40

6.7 Quadcopter trajectory in the repulsive test . . . 40

6.8 Average time for different simulation environments . . . 42

6.9 Average time for all simulation environments. . . 42

6.10 Average collision rates for different simulation environments . 43 6.11 Average collision rates for all simulation environments. . . 43

6.12 Average minimum rates for different simulation environments . 44 6.13 Average minimum rates for all simulation environments. . . . 44

(8)

List of Tables

2.1 Range sensor properties . . . 11

3.1 Modifications of the TangentBug algorithm . . . 21

4.1 Computer specifics . . . 26

4.2 Sensor properties . . . 28

4.3 Quadcopter geometric values . . . 28

4.4 GPS properties . . . 29

6.1 Feedback Linear Controllers parameter values . . . 37

6.2 Model stability limitations . . . 37

6.3 Tuning parameters for the APFA . . . 41

6.4 Average performance result of both methods . . . 45

6.5 Time- and space-complexity . . . 45

(9)

List of Algorithms

1 Split-And-Merge . . . 16 2 Modified TangentBug . . . 20 3 Memoization . . . 22

(10)

List of Acronyms

AFA Autonomous Flight Algorithms APF Artificial Potential Field

APFA Artificial Potential Field Approach API Application Programming Interface DA Data Association

EKF Extended Kalman Filter FBLC Feedback Linear Controller FOV Field Of View

FOVH Field Of View Horizontally FOVV Field Of View Vertically GCS Ground Control Station GPS Global Positioning System

IEKF Iterative Extended Kalman Filter KF Kalman Filter

LE Landmark Extraction LMS Least Mean Squares LTG Local Tangent Graph

MILP Mixed Integer Linear Programming MP Mission Planner

MSE Mean Squared Error

ODCA Obstacle Detection Collision Avoidance OMPL Open Motion Planning Library

OS Operating System

PDBM Point-Distance-Base-Method PD Proportional-Derivative

PID Proportional-Integral-Derivative PRM Probabilistic Roadmap

QGC QGroundControl

RMSE Root Mean Square Error ROS Robot Operating System SAM Split-And-Merge

SLAM Simultaneous Localization And Mapping SITL Software In The Loop

TB TangentBug

UAV Unmanned Aerial Vehicle UI User Interface

UKF Unscented Kalman Filter

V-REP Virtual Robot Experimentation Platform

(11)

Chapter 1 Introduction

1.1 Background

Unmanned Aerial Vehicles (UAV) have been heavily studied in the past decade.

One of several motivations for this is that the hardware has improved a lot, giving rise to varieties of applications e.g. delivering mail, providing internet in disaster- and hard-to-reach-areas, filming, and much more[1]. Studies have been made in many fields where autonomous flights have been a recurring subject. UAVs are now required to navigate in complex terrains, unknown environments and at distances where manual control is impossible. A well known solution for handling the long distances is to integrate cameras onto the UAV and connect it to a ground station that will manage the communication. Usu- ally one can set up flight paths in know Ground Control Stations (GCS) like Mission Planner¹ (MP) or QGroundControl² (QGC). However, the question how to direct the UAV in complex and unknown environments still remains.

Planning a route for an UAV can be done by using a flight controller with integrated software and an external Global Positioning System (GPS), together with a GCS. A general setup of the UAV uses a GPS and integrated sensors[2]

such as accelerometer, magnetometer, gyroscope, and barometer when direct- ing itself to the nearest way point. The absence of range sensors makes the UAV unaware of obstacles that can occur during its flight e.g. buildings, trees, and other obstacles, which normally would have been avoided in a manual flight by the human interception. The unknown obstacles can be avoided by integrating different sensors and uploading an Autonomous Flight Algorithm (AFA) onto the UAV, where the main process is often referred to as Obstacle Detection and Collision Avoidance system (ODCA). All the integrated sensors can then, in real time, send the obtained data to the ODCA system that will utilize it to complete an autonomous flight.

1Mission Planner is a free open source software available for Windows.

2QGroundControl is a free open source software available for Windows, OS X, Linux, iOS and Android.

(12)

1.2 Purpose

Numerous variations of AFAs for UAVs exist, where some algorithms are more widely used then others. However, it is not easy being a novice in the field and to know what is the appropriate approach to use for different applications. In the research papers studied about AFAs most of the advantages and disadvantages have been clearly stated, but without much reference and analysis. A few stated properties were that the SLAM approach is more complex than the APFA and the APFA have no global convergence guarantee[3][4][5][6].

Learning what approach to use takes time, not least finding an efficient and working solution. That is why this thesis is directed towards individuals interested in which approach of two of the most frequently used AFAs that are the most successful in reaching an arbitrary position, have the best time- and space-complexity, have the safest navigated flight, and the highest time efficiency for various simulation environments.

Multiple studies were made using various AFAs, where the AFAs from the prior studies to this thesis can be divided into two main groups:

• Planning and Reaction - Commonly based on the concept of Simul- taneous Localization And Mapping (SLAM)[7][8][9][10][11], with various range sensors and reaction algorithms[5][12].

• Pure Reaction - Recently more commonly based on steering the UAV to a positively charged goal position and avoiding the negatively charge obstacles using an Artificial Potential Field Approach (APFA)[13][14]

[15][3][16], which concept originates from the Artificial Potential Field method (APF)[17].

Other known planning concepts are:

• Geometric difference - Frequently applied for applications involving multiple UAVs in motion, where the method in general calculates relative velocity vectors of the UAVs and if an encounter is found in the path, trajectory corrections will be made[18][19][20][21].

• Optimization techniques - Often constructed around a discrete grid- based shortest path algorithm[16][22].

• Mixed integer linear programming (MILP) - Dissembles the prob- lem into a mathematical problem containing a set of linear constraints and then uses a MILP-tool to generate a best path outcome[16][22].

In this thesis, the analysis will be made on the SLAM approach and the APFA, due to their popularity and the large contrast in construction as well as their characteristics. A desired result of the analysis would include a major performance difference between the approaches, which then can be investigated in every aspect. This investigation could then be used to optimize the implementation of both approaches for achieving a more realistic result.

(13)

1.3 Objective

The main objective of this thesis is to analyze: Which of the AFAs, SLAM or APFA, have the safest navigated flight, the best time- and space-complexity, the highest time efficiency, and are most successful in reaching an arbitrary position for various simulation environments. However, the performance analysis is not the only objective, but also some knowledge how to implement both approaches. A summary of the expected steps in the implementation is defined accordingly:

1. Set up a 3D robotic simulation platform.

2. Create an UAV model and apply proper dynamics.

3. Integrate mandatory sensors for the approaches onto the model.

4. Construct and apply both approaches in the simulation platform.

5. Create a simulation environment, simulate and analyze the performance difference between the two approaches.

Earlier, in the subsection advantages and disadvantages for both the AFAs were stated. The results expected are that the SLAM approach is more computational heavy, which induces implementation problems due to its high complexity, compared to a pure reaction algorithm[5][4]. It is also expected to have a safer navigated flight due to its robustness. Another thing to expect is the APFA having a lower success rate, since it is a pure reaction algorithm that has no global convergence guarantee and can easily get stuck at a local minimum[3]. However, the APFA is also expected to be most time efficient due of its low time- and space-complexity.

1.4 Outline

This thesis consists of 8 chapters. In chapter 2: dynamics, stabilization techniques, and control laws are explained. Chapter 3 contains details and theory about the APFA and the SLAM approach. Chapter 4 describes how the whole simulation environment was constructed from the robotic platform to model, sensors, and communication. Chapter 5 hold information about how the analysis was performed. Chapter 6 presents the obtained result and chapter 7-8 contains discussion, conclusion and possibilities for future development.

(14)

Chapter 2 Theory

In this chapter, fundamental theory used in the thesis is presented. It touches areas such as quadcopter dynamics, stabilization and control strategies.

2.1 Quadcopter dynamics

In a simulations environment there are limitations of real world phenomena.

Still one should model the UAV in an as realistic way as possible to achieve credible results. For simplicity the UAV model will be a quadcopter, seen as a rigid body with the dynamic structure seen in Figure 2.1(a), where two rotors rotate counter-clockwise and the other two clockwise. The reason for this construction is to cancel out unwanted effects that occur due to Newton’s third law. The two coordinate systems are: a fixed inertial frame seen in Figure 2.1(b) and the rotational body frame fixed at the centre of mass of the quadcopter.

(a)

(b)

Figure 2.1: (a) Dynamics of a quadcopter with corresponding rotational reference frame. (b) Inertial reference frame.

(15)

The dynamical structure of Figure 2.1(a) is used for the proceeding derivations Eq.(2.1) → Eq.(2.10), which is an adaption of an existing model[23].

However, the only dynamical model sought is in terms of position (x, y, z) and all the moments acting on the quadcopter. Let (x, y, z) be the-translational- and the-rotational-coordinates (ϕ, θ, ψ) in the inertial reference frame, defined in IR³, where (ϕ, θ, ψ) are the Euler angles of the quadcopter, relative to the inertial frame. By neglecting rotor dynamics the generalized coordinate vector q for the quadcopter can be written, q = (x, y, z, ϕ, θ, ψ). For a rigid body the kinetic-T and potential-U -energies can be expressed,

U = mgz, (2.1a)

T = T_rot+ T_tra, (2.1b)

where m is the mass of the quadcopter, g is the gravitational acceleration, T_tra and T_rot is the rotational- and translational-kinetic-energies, respectively.

The quadcopter dynamics can be described by Lagrange’s equations for the external forces F_tot,

− ∂L

∂q + d dt

∂L

∂ ˙q = F_tot, (2.2)

where the Lagrangian is defined as the difference between the kinetic and the potential energy, L = T − U. Ftot(F_i, τ ) is created from the torque τ = (τ_ϕ, τ_θ, τ_ψ) in the directions of the Euler angles and the total thrust F_i in the inertial frame. F_i can be derived from the relation,

F_i = RF_r, (2.3)

where R is the rotation matrix from the body frame to the inertial frame and can be expressed,

R =



 c_ψc_θ c_ψs_θs_ϕ− sψc_ϕ c_ψs_θc_ϕ+ s_ψs_ϕ sψcθ sψsθsϕ+ cψcϕ sψsθcϕ− cψsϕ

−sθ c_θs_ϕ c_θc_ϕ



 , (2.4)

where c_i = cos(i), s_i = sin(i), i ϵ (θ, ϕ, ψ) and F_r is the total thrust in the rotational frame,

Fr =



 0 0 u



 , (2.5)

that is depending on the control input u defined,

u =





 xy_rate xyrate

z_rate ψ_rate





 , (2.6a)

xy_rate =

∑4 k=1

f_k, (2.6b)

(16)

where ψ_rate, z_rate are control inputs for the quadcopter to turn in respectively ψ-direction and z-direction and f_k is the thrust of rotor k, proportional to the angular speed ωkof rotor k. τ represents all moments acting on the quadcopter in the rotational frame,

τ =



 L(f₄− f2) L(f₃− f1) L(f₂+ f₁+ f₄+ f₃)



 , (2.7)

where L is the distance from the rotors to the quadcopters centre of mass.

Neglecting aerodynamical effects the final dynamical model can be written,

m



 x¨

¨ y

¨ z



 +



 0

0 mg



 = F_i, (2.8)

which can be expressed differently using the fact that the rotation matrix is orthogonal[24] R⁻¹ =R^T combined with Eq.(2.3),



 x¨

¨ y

¨ z



 =



 0

0

−g



 + 1 m







cos ψ sin ψ sin θ

cos θ 0





 u^T. (2.9)

2.1.1 Equilibrium conditions

The quadcopter requires four conditions to be fulfilled in order to have a state equilibrium (force balance, parallel force directions, zero moments and balance between the angular velocities), also referred to as hovering[25]:

∑4 k=1

fk =−mg, (2.10a)

f_k||g, k ∈ [1, 4], (2.10b)

∑4 k=1

L× fk = 0, (2.10c)

(ω₁+ ω₃)− (ω2+ ω₄) = 0, (2.10d) which is the state of the quadcopter if the total control input u in Eq.(2.6a) equals zero, assuming the system is set up with a stabilization technique.

2.1.2 Damping and Constraint

Damping is often introduced into a dynamical systems, especially if the control input u is discontinuous. Without a damping term and at large transition steps in the control input, the quadcopter would be stuttering forward. An easy

(17)

way to implement a damping system is by using a ramp function, which can be constructed with the dependency of the control input in Eq.(2.6b),

Dr = 1− 1

1 + δ_ramp, (2.11a)

δ_ramp,k+1 =

{ δ_ramp,k+ C_xy^C

rate xy_rate > 0

δ_ramp,k− C_xy^C_rate xy_rate < 0 (2.11b) where C is a constant tuned with respect to extreme values of xy_rate and k is the time step.

It is common that the range sensors on an UAV have higher Field Of View Horizontally (FOVH) than Field Of View Vertically (FOVV), which can lead to unwanted collisions for a xy_rate ≪ zrate. This problem can be dealt with by adding a constraint on the z_rate,

z_rate ≤ cconxy_rate, (2.12) where c_con is a constant defined by the geometry of the quadcopter.

2.2 Control Design

2.2.1 Navigation system

In order for the AFAs to properly navigate the quadcopter a position update system was created which connects the methods to the dynamical model. The position update system implemented in this thesis is shown in Figure 2.2.

Figure 2.2: Position update system implemented on the quadcopter.

The position system update was constructed such that it takes two con- trol vectors u_EA containing two Euler angles and the control vector u from Eq.(2.6a), where r in the first and second step stands for rate. The control vectors are then sent to a method called inverse dynamics[23], which calculates a new xy_rso that the odometry position model gives a linear output. Then the updated pose is used with the odometry data from the previous time iteration, together with Feedback Linear Controllers (FBLC)s to navigate[26] and stabilize the quadcopter around the new equilibrium position. Two FBLCs are used, Proportional-Integral-Derivative (PID) for the thrust F_i and Proportional- Derivative (PD) for the (ψ, θ, ϕ) angels, because the PID controller have a tendency to oscillate for highly oscillatory processes[27][28], which makes it

(18)

unfit for stabilization of the Euler angles. The data is then sent to the rotor function which applies the appropriate torque and thrust corresponding to Eq.(2.7) and Eq.(2.5). More details about the model, the inverse dynamics method, and the FBLCs are described in the forthcoming subsections.

2.2.2 Model

One of the key points in an ODCA system is to know the position were the UAV is located. Receiving an approximate position when the UAV is at rest is a trivial task, however not so trivial when the UAV is in motion. To obtain its new position and state a motion model for the UAV needed to be derived. The motion model selected is a odometry model, equal for both approaches. It was selected because in general its more accurate then velocity models, especially if the velocity model are using some type of PID controller[29]. This model is a bit unsuitable for a general SLAM approach because it only works after the UAV have been moving and the SLAM approach is using motion planning.

However, this does not apply for algorithms with a filter function[29], which the SLAM approach in this thesis have. The model is derived by solving Eq.(2.9), assuming the that control expression is time invariant, adding the ramp function Eq.(2.11a), which gives the recursion model for position and orientation around the z-axis for each time iteration k,





 xk+1

y_k+1 z_k+1 ψk+1





 =





 xk

y_k z_k ψk





 +







xk−xk−1

∆tk

yk−yk−1

∆tk

zk−zk−1

∆tk

0





+







Drcψ+∆ψ

D_rs_ψ+∆ψs_θ+∆θ c_θ+∆θ

1





 u^T, (2.13)

where ∆ψ, ∆θ represents change in the angles (θ, ψ) and the time step ∆t for each time iteration k, ci+∆i = cos(i + ∆i) and si+∆i = sin(i + ∆i), where i ϵ [ψ, θ].

2.2.3 Dynamic inversion

Dynamic inversion[23] is a method used to linearize the non-linear odometry model defined in Eq.(2.13), which can be derived by knowing that the velocity in the internal frame v_k,i for each time iteration k,

vk,i =

√

˙x²_k+ ˙y_k²+ ˙z²_k, (2.14) where the velocities are,

˙x_k= xk− xk−1

∆t_k , (2.15a)

˙

y_k= y_k− yk−1

∆t_k , (2.15b)

˙z_k= z_k− zk−1

∆t_k . (2.15c)

(19)

By knowing the linear dependency of the UAV following a straight line one can construct an expression for the velocity rate xy_r from Eq.(2.6a) by using Eq.(2.14),

xy_r = Bv_k,i, (2.16)

where B is the linear dependency constant. By combining Eq.(2.16), Eq.(2.13) and solving for xyr in u^Tone gets the following inverse dynamics relation,

xy_wr = xy_r∆t_k− ˙xk− ˙yk− ˙zk− zrc_θ+∆θ

D_rc_ψ+∆ψ+ D_rs_ψ+∆ψs_θ+∆θ , (2.17) where xy_wr is the wanted velocity rate such that Eq.(2.13) gives a linear output. Some simulations platforms have discontinuous angle intervals for which Eq.(2.17) can be approximated without using direct angle orientations,

xy_wr = xy_r∆t_k− ˙xk− ˙yk− ˙zk

1 + E| ˙ψψr| , (2.18)

where E is a tuning constant.

2.2.4 Feedback Linear Controllers

For a quadcopter to navigate, maintain its position and orientation it requires a controller that can stabilize the quadcopter at its equilibrium state Eq.(2.10).

The main process of a controller is to reduce the residuals between a wanted x_w and measured value x_m. Two widely used FBLCs are PD and PID. PD and PID controllers main advantage is that they are easy to implement[30].

Figure 2.3 visualizes a general PID scheme.

Figure 2.3: A general control scheme for a PID controller.

The residual of a PD and PID controller is calculated by[25],

e = xw− xm, (2.19)

which then is set in the control input signal for the PD controller[25], u_{P D}(t) = K_Pe + K_Dde

dt, (2.20)

(20)

and for the PID controller[25],

u_{P ID}(t) = K_Pe + K_I

∫ t 0

e(τ )dτ + K_Dde

dt, (2.21)

where K_P, K_I and K_D are controller constants in IR and τ is the time step between each measurement. K_P is the proportional gain, K_I is the integral gain and K_D is the derivative gain. The unknown constants can either be tuned by analyzing experimental data or by using the Ziegler-Nichols method, as well as the Cohen-Coon method. It can also be tuned algorithmic and with software tools[31].

The total stabilization of the quadcopter was achieved by using both PD- and PID controllers, where the control law is designed¹, modified for thrust,





 f₁ f₂ f₃ f₄





 = F^Tⁱ







1− α + β + γ 1− α − β − γ 1 + α− β + γ 1 + α + β− γ





 , (2.22)

where the control input α, β and γ is the θ, ϕ and ψ correction respectively. Ex- pressions for the parameters for each time iteration k are defined from Eq.(2.22) , using Eq.(2.15c), Eq.(2.19), Eq.(2.20) and Eq.(2.21),

F_k+1,z = F_k,z+K_P,Fe_k,z+K_I,F

∑k i=1

e_k,z+K_D,F(e_k,z−ek−1,z)+K_C,F e_k,z

∆t_k, (2.23a) α_k+1 = K_P,αe_k,α+ K_D,α(e_k,α− ek−1,α) + K_P,yy_k+ K_D,y(e_k,y− ek−1,y), (2.23b) β_k+1 = K_P,βe_k,β+ K_D,β(e_k,β− ek−1,β) + K_P,xx_k+ K_D,x(e_k,x− ek−1,x), (2.23c) γ_k+1 = K_P,γe_k,γ+ K_D,γ(e_k,γ− ek−1,γ), (2.23d) where K_C,F is a tuning constant, K_i,j, e_k,j are the controller constants and errors, respectively for i ϵ [P, I, D], j ϵ [F, α, β, γ] at the time iteration k.

2.3 Range sensors

Range sensors are used to measure the distance between the quadcopter and some arbitrary point e.g. at an obstacles surface. The choice of range sensors depends mainly on the application. An arbitrary 3D-range sensor often get the following set s_scan from a sensor scan,

s_scan ={pi(r, θ, ψ), i = 1, 2, ...}, (2.24) where p_iis the detected point, r is the radius to the object, (θ, ψ) are the angles to the object in an egocentric reference frame. Moreover, some commonly used sensors[32] with listed properties are seen in Table 2.1.

1Design found within the code of the Quadricopter.ttm model in the 3D robotic software platform: Virtual Robotic Experimental Platform (V-REP).

(21)

Table 2.1: Range sensor properties for commonly used sensors.

Sensor Advantages Disadvantages

Laser

Precise and efficient, exists models with single beam and with large Field Of View (FOV)

Expensive, some models have the tendency of disturbing external hardware, (e.g. GPS) [33], low performance in water and on glass

Radar

Works in most environments, there exists models with long range and good accuracy[34]

Expensive, large and heavy in general

Sonar

Works well under water, generally the cheapest ones on the market

Low precision, temperature dependent, often a lot of noise in signal

Stereo-

vision Extracts a lot of information

Bad performance in highly intense illuminated areas and in darkness

(22)

Chapter 3 Methods

This chapter contains a general system process, derivation and detailed information about the two methods.

3.1 Simultaneous Localization And Mapping

Simultaneous Localization And Mapping (SLAM) is a technique used in autonomous flight planning and can in general be categorized in three groups:

”grid-based”, ”topological based”, and ”feature-based”[35]. The grid-based ap- proach usually divide a map into a grid of elements containing a probability of being occupied. This feature becomes memory- and computational-heavy for increasing objects and map size. A topological-based approach generally creates a graph network of nodes, which gives an approximate description of the environment. The graph network can then be used with a shortest path algorithm such as Dijkstra’s algorithm¹ to find the shortest path, combined with defined actions at each node. The main disadvantage of this approach is that it is not easy to implement for large and complex environments. The concept that stands out from prior studies is the feature-based SLAM approach which identifies features in the environment to build a map which then can be used to estimate its position. This feature consists of four steps:

• Landmark extraction

• Data association

• Extended Kalman Filter

• Reactive algorithm

Figure 3.1 describes a general feature-based SLAM process, which is an adap- tion from [36].

1Dijkstra’s algorithm is an algorithm for finding the shortest path between two graph vertices in a graph.

(23)

Figure 3.1: A general overview of a feature-based SLAM process.

A SLAM process starts by receiving data from a range sensor, which is then sent to the landmark extraction (LE) algorithm. Then the LE algorithm extracts landmarks from the environment, which can be corners, lines and other features[37]. These landmarks are then sent to the data association method which associate the landmark to its closets landmark from its neigh- borhood and checks if it has been seen before[36]. The new and re-observed landmarks are then sent to the EKF which uses them together with the odometry data for a more accurate position estimation. The estimated position is then sent to the reactive algorithm for position update. The construction of these four steps varies a lot depending on the application, where the forthcoming subsections describes a possible way to construct them.

(24)

3.1.1 Landmark Extraction

The Landmark extraction algorithm selected for this thesis is a Slit-And- Merge (SAM) algorithm with the usage of a Point-Distance-Based-Method (PDBM)[38] working as an initial cluster. It was selected due to its high fre- quency updates and accuracy[39]. This setup is a modification of the SAM algorithm with an agglomerative hierarchical clustering algorithm described in[39]. However, the data received from a range sensor is often given in spher- ical coordinates (r, θ, ψ). Therefore, the data is converted to Cartesian co- ordinates before being sent to the LE algorithm, through the equations[40],

x = rsin(ψ)cos(θ), (3.1a)

y = rsin(ψ)sin(θ), (3.1b)

z = rcos(ψ), (3.1c)

where r is the radius from the range sensor to the scanned point p(x, y, z). All scanned points p are then divided into linear segments by the PDBM by the condition: d > d_th[38],

d =

√

r²_i + r²_i+1− 2rir_i+1cos(∆α), (3.2a) d_th = C₀+ C₁min(r_i, r_i+1), (3.2b) where d_th is a threshold condition, d is the Euclidean distance between two scanned points, i indicates the index of each consecutively scanned point, ∆α is range sensors angular resolution, C₀ is a constant and C₁ is an adaptive parameter depending on Eq.(3.2a),

C₁ = d

r_i. (3.3)

The calculated linear segments are then used as an input to the SAM algorithm. These linear segments are then divided into smaller segments if they are not significantly linear, which is tested by a Least Mean Squares (LMS) method. For a set of points in each segment s(x, y, z) the least mean square fit problem is defined,

min f (s), (3.4)

where f (s) is a function of residuals between each point in s and the line z = a₁+ a₂x + a₃y. Eq.(3.4) can be solved using the normal equations,

A^TAx_sol = A^Tb, (3.5a)

A =







1 x₁ y₁ 1 x₂ y₂ ... ... ...

1 x_i y_i





, b =





 z₁ z₂ ...

z_i





, (3.5b)

where x_sol is the solution vector containing the calculated constants (a₁, a₂, a₃) and i is the number of points in the segment s_iϵ s. A general implementation

(25)

of the EKF uses landmarks N defined as points[36]. Therefore, the linear segments s_i can be converted to a point landmark by selecting a fixed point F_p on the map and selecting the point p in si that is orthogonal to the the fixed point by,

N_i = pϵ s_i ⊥ Fp. (3.6)

Next step in the the SAM algorithm is to check if two or more landmarks can be merged into one landmark. This is done by checking the collinear- ity between two abreast landmarks by using the Cayley–Menger determi- nant, where the Cayley–Menger matrix[41] is defined for a finite metric space (X = M, d), M ϵ (x, y, z),

CM(X, d) =

[ 0 e

e^T D ]

, (3.7)

where e is an vector of ones and D = d(M_i, M_j)². The determinant of the CM-matrix Eq.(3.7) can be used to determine the collinearity of at least three distinct points with the following expression[42],

det(CM ) =







0 d(AB)² d(AC)² 1 d(AB)² 0 d(BC)² 1 d(AC)² d(BC)² 0 1

1 1 1 0





 = 0. (3.8)

The pseudo code for the complete SAM algorithm is described in Algo- rithm 1.

(26)

Algorithm 1: Split-And-Merge

Input : A vector of linear segments s

Output: A vector L ϵ IR^N containing N landmarks Create an empty sub-list Slist;

Initialize an empty list of landmarks L;

for i← 1 to number of segments in s do

if (number of points in segment si ≥ 5) then S_list← s;

while (S_list is not empty) do a1, a2, a3 ← LMS(si);

for j ← 1 to number of points in si do res← calculateResiduals(si, a, b, c);

end

res_max ← max(res);

if (res_threshold< res_max) then Slist← si(resmax);

sort S_list in ascending order;

if (number of points in segment s_i ≤ 2) then remove si from Slist;

end else

N ← extractP ointF romLine(si);

L← N;

remove s_i from S_list; end

end end end

for k ← to number of landmarks N in L do

CM_det ← calculateCMDeterminant(Nk, N_k+1);

if (CM_det < C_tolerance) then

N_k← mergeLandmarks(Nk, N_k+1);

remove N_k+1 from L;

L← Nk; end

end

(27)

3.1.2 Data Association

The Data Association (DA) algorithm can be thought of as a protocol that neglect bad landmarks, with characteristic properties such as[36],

• Not re-observable each time step

• Does not stand out and might not be observed again

• Getting associated with different landmark

Using bad landmarks might lead to an even worse estimated position update in the EKF than the odometry data alone and can induce devastating results.

The following concept about data association is an approximation based on[36].

The first step in the DA algorithm is to initialize an empty landmark database which stores new and re-observed landmarks. In order to remove landmarks that does not stand out, only landmarks re-observed n times are used for a position update. These landmarks are paired to their nearest neighbour by a nearest neighbour approach. Thus, for these pairs to be accepted as the same landmark the following condition must hold,

v ≤ λ∆α, (3.9)

where ∆α is the range sensor angular resolution, λ is a constant defined as half the amount of the minimum points required in a linear segment s_iϵ S and v is a parameter expressed,

v = zL− hL, (3.10a)

hL=

√

(Nxp− x)²+ (Nyp− y)²+ (Nzp− z)², (3.10b) z_L=

√

(N_xd− x)²+ (N_yd− y)²+ (N_zd− z)², (3.10c) where h_L is the expected range between the landmark N_p(N_xp, N_yp, N_zp) and the quadcopter using the odometry data and z_L is the expected range be- tween the landmark N_d(N_xd, N_yd, N_zd) and the quadcopter using the updated landmark position from the landmark database.

3.1.3 Extended Kalman Filter

The usage of filtering functions is crucial to the feature-based SLAM approach.

Popular filters for estimating a state with a non-linear transition function is the Extended Kalman Filter (EKF)[43], Unscented Kalman Filter (UKF)[44] and Iterative Kalman Filter (IEKF)[45]. The reason that the transition function is still being treated as non-linear is because the inverse dynamics method is only successful as long as the inner dynamics is stable[23], which is not guaranteed in this case. However, it will reduce strong non-linearities which makes it redundant to use a IEKF that increase performance in significantly non-linear systems, compared to the EKF[45]. In [44] a head and hand experiment was

(28)

made for orientation, which they represented by quaternions²[40]. Here they analyzed the differences between a EKF and UKF and the result gave roughly the same accuracy, implying that the EKF is the better choice when working in a virtual environment, due to e.g. simplicity and speed. Therefore, the EKF was selected to be the filter for the SLAM approach. The proceeding derivations Eq.(3.11) → Eq.(3.16) and reasoning are from[43].

The extended Kalman filter estimates a time-discrete state vector x(x, y, z) ϵ IRⁿ by a stochastic non-linear function,

xk= f (xk−1, uk−1, wk−1), (3.11a) together with a measurement z ϵ IR^m,

z_k= h(x_k, v_k), (3.11b)

where f and h are non-linear functions, w_k and v_k represent Gaussian white noise, u_k is the control function and k is the discrete time index. The noise terms in Eq.(3.11a) and Eq.(3.11b) are in general unknown so one can approximate Eq.(3.11a) and Eq.(3.11b) by neglecting the noise,

x˜_k = f (ˆx_k₋₁, u_k₋₁, 0), (3.12a)

˜zk = h(˜xk, 0), (3.12b) where ˆx_k is the a posteriori state estimate. Eq.(3.12a) and Eq.(3.12b) still represents a non-linear process and one can linearize the estimates by the approximations,

x_k ≈ ˜xk+ A_k(x_k₋₁− ˆxk−1) + W_kw_k₋₁, (3.13a) zk ≈ ˜zk+ Hk(xk− ˜xk) + Vkvk, (3.13b) where A(f (x))_k, W(f (w))_k, H(h(x))_k and V(h(v))_k are Jacobian matrices consisting of partial derivatives. Together with Eq.(3.12a), Eq.(3.12b), the Kalman gain K_k and another a posteriori state estimation relation[43],

xˆ_k= ˜x_k+ K_k(z_k− ˜zk), (3.14) the prediction update can be formulated,

xˆ⁻_k = f (ˆx_k₋₁, u_k₋₁, 0), (3.15a) P⁻_k = AkPk−1A_k^T + WkQ_k₋₁W^T_k, (3.15b) where ˆx⁻_k is the a priori state estimate, Q_k is the process noise covariance matrix, P_k is the error covariance and P⁻_k is the a priori error covariance.

2A quaternion can be written q = q0+ q₁i + q₂j + q₃k, where q_[0,3]ϵ IR and i, j, k fulfills the Hamilton’s rules, {i²= j²= k²=−1, ij = k, ji = −k, jk = i, kj = −i, ki = j, ik = −j}.

(29)

The corresponding measurement update are,

K_k = P⁻_kH^T_k(H_kP⁻_kH^T_k + V_kR_kV^T_k)⁻¹, (3.16a) xˆ_k= ˆx⁻_k + K_k(z_k− h(ˆx⁻k, 0)), (3.16b) P_k = (I− KkH_k)P⁻_k, (3.16c) where R_k is the measurement noise covariance matrix. After the estimated state update ˆx_k have been calculated from odometry data all landmarks N are then corrected even further by the data achieved from the data association step. This is done by a simplification taken from [36], by not taking into account the covariance update for the landmarks that are corresponding to the quadcopter. By using Eq.(3.16b) and Eq.(3.10a) the state is updated for each landmark N ϵ L as,

xˆ_k,L+1 = ˆx_k,L+ c_n(z_L− hL), (3.17) where c_n = 1 − 1/n, and n is the number of times the landmark has been observed.

3.1.4 TangentBug

When it comes to reaction algorithms there are a whole sea to choose from. In [12] some commonly used algorithms are mentioned such as Bug 1, vector field histogram approach, Bubble and band technique, Bug 2, and TangentBug.

Preferable properties of the reaction algorithm are low computational power and the possibility to find a global minimum solution. From the algorithms mentioned above, Bug 1 and Bug 2 are simple to implement, but might be a bit too simple when it comes to optimization properties. The vector field histogram approach, which models the quadcopter to move in arcs or straight-line does not fit our model. The Bubble and band technique acquire a lot of global information for a fully developed path plan which also makes it unsuitable.

Therefore, the TangentBug (TB) was selected, which is an improvement of the Bug 2 algorithm with the property to possibly find a global minimum solution by using small amounts of global information [5].

The TB algorithm are using different functions such as, atan2[46] defined,

atan2(y, x)≡











arctan(^y_x) , x > 0

arctan(^y_x) + π , x < 0, y ≥ 0 arctan(^y_x)− π , x < 0, y < 0 +^π₂ , x = 0, y > 0

−^π₂ , x = 0, y < 0 undefined , x = 0, y = 0

(3.18)

and the Euclidean distance d ϵ IR³[47], d =√

(x₂− x1)² + (y₂− y1)²+ (z₂− z1)². (3.19)

(30)

The TB algorithm is a 2D algorithm and it is applied as a 2D algorithm due to the thrust rate constraint, which is discussed in subsection 2.1.2, Eq.(2.12). Instead the SLAM method is 3D reactive, in such a way that it is constantly moving towards the goal height, with a thrust rate depending on the Euler angle ψ. The main feature of the TB algorithm is an algorithm known as Local Tangent Graph (LTG)[5] that locally scans its surroundings at every time step k to determine if there is a obstacle free path to the goal.

If no obstacle is found it goes straight to the goal, otherwise it uses a normal Bug2 algorithm[12] to surpass the obstacle. This feature is one of many modifications of the original TB algorithm. The pseudo code for the modified TB algorithm can be found in Algorithm 2.

Algorithm 2: Modified TangentBug

Input : Goal position and current position

Output: A Boolean variable for success in reaching the goal if (not at goal) then

S(θ, ψ, d) ← LaserSensor3D;

θ_g ← atan2(goalPosition,currentPosition);

d_min ← min(d);

if (obstacle detected) and (d_min<threshold) then pathToGoal ← LTG(S, θg);

if (no pathToGoal) then Bug2(S);

else

quadcopter ← u(θg, ψ_g, θ_r, ψ_r) ; end

else

quadcopter ← u(θ, ψ, θr, ψ_r) ; end

else

quadcopter ← u(0, 0, 0, 0) ; at goal ←true ;

end

There are a few major differences between the modified TB algorithm described in Algorithm 2 compared to the one defined in [5]. A List of modifications done to the original TB algorithm can be found in Table 3.1.

(31)

Table 3.1: A point-list of modifications done to the original TangentBug algorithm.

Original TB:

• 1) Detection state false:

Moves towards the goal position along the locally optimal direction on the current LTGs sub-graph

• 2) Obstacle found: If local minimum found

• 3) Turn direction: Not specified

• 4) Obstacle follow: Move along the boundary using the LTG, while continuously recording d_min

• 5) Stop condition: If goal position reached or a loop around the obstacle is completed

Modified TB:

• 1) Detection state false:

Moves towards the globally optimal direction using the atan2 function

• 2) Obstacle found: If the obstacle is within a threshold

• 3) Turn direction:

Depending on angle to goal and the angle to the obstacle relative the quadcopter

• 4) Obstacle follow: Move along boundary using Bug2 with TB conditions from the LTG, while continuously recording d_min

• 5) Stop condition: If goal position reached

Point five for the modified TB algorithm in Table 3.1 does not contain any stop condition for unreachable goal positions. The reason for this is that both methods will be using a type of memoization³[48] algorithm that will keep track of the quadcopter so that it will stop if it encounters a local minimum.

In order for the memoization algorithm to find a local minimum it requires some conditions on what should count as a local minimum. In our case a local minimum is found if,

• The quadcopter is located in an earlier encountered grid cell having approximately the same direction and that it is not the last new cell it was located in

• The quadcopter is stationary in a cell grid for an arbitrary defined amount of time

3Memoization is an optimization technique that is often used to speed up computer programs by e.g. storing complex data to avoid unnecessary re-calculations.

(32)

These conditions are inserted in the memoization algorithm, see Algorithm 3.

Algorithm 3: Memoization

Input : Current position P_c and current yaw angle A_c Output: Boolean variable defining minimum state Initializing positionMap;

Initializing directionMap;

Initializing time variable t;

positionMap← index corresponding to Pc; minFound←false;

if (positionMap(P_c) ̸=empty and Pc ̸= Plast) then if (t≥ tth) then

minFound←true;

else

t← newT ime();

if (directionMap(A_c)≈directionMap(Alast)) then minFound←true;

end end else

positionMap← index(Pc);

directionMap← index(Ac);

t ← 0;

end

3.2 Artificial Potential Field Approach

Pure reactive algorithms are very useful in applications that does not handle complex computations and heavy memory usage. For this thesis, an Artificial Potential Field Approach (APFA) was selected as the second approach, due to similarities to the SLAM approach, mainly the property that it strives to find a global minimum from the usage of mostly local information. The APFA was built from two functions, an attractive potential function that pulls the quadcopter towards the goal position and a repulsive potential function that pushes the quadcopter away from all detected obstacles, within its local surroundings.

As mentioned earlier, the APFA originates from[17]. It was constructed by[13], where the main difference between the two approaches is that the steering does not come from translational movements but from angular acceleration, which makes it more smooth. This characteristic can be very practical for simpler stabilization systems, because it puts less load on the control-model algorithm. The progression of this subsection will go more into detail how this approach is constructed. A picture of an APFA-process can be seen in

Figure 3.2.

(33)

Figure 3.2: An overview of the constructed APFA-Process.

The constructed APFA is a hybrid between [13] and [14], where the foun- dation is taken from [13] with properties such as a second order turn rate and [14], that took [13]s concept from 2D→3D. Therefore, the attractive potential function is expressed,

A3D(θ, ψ) = kg

[ θg

ψ_g ]

(e^−c¹^d^g + c2), (3.20) and the repulsive potential function,

R_3D(O(θ, ψ)) = k_o [

s₁(θ_o)s₂(t₁(1−^|ψ_t₂^o^|)) s₁(ψ_o)s₂(t₁(1− ^|ψ_t₂^o^|))

]

(e^−c³^d^o)

[ e^−c⁴^|θ^o^| e^−c⁴^|ψ^o^|

]

, (3.21)

where (k_g, k_o) are constants set as the magnitude of attraction and repulsion, respectively, (θ_g, ψ_g, θ_o, ψ_o ) are angles to goal position and angles to obsta- cles, respectively, (d_g, d_o) are distance to goal and obstacle, respectively and

Analysis of autonomous flight algorithms for an unmanned aerial vehicle

Master Thesis Report

Analysis of autonomous flight algorithms for an unmanned aerial vehicle

Mattias Sjöberg

Abstract

Acknowledgements

Contents

List of Figures

List of Tables

List of Algorithms

List of Acronyms

Chapter 1 Introduction

1.1 Background

1.2 Purpose

1.3 Objective

1.4 Outline

Chapter 2 Theory

2.1 Quadcopter dynamics

2.1.1 Equilibrium conditions

2.1.2 Damping and Constraint

2.2 Control Design

2.2.1 Navigation system

2.2.2 Model

2.2.3 Dynamic inversion

2.2.4 Feedback Linear Controllers

2.3 Range sensors

Chapter 3 Methods

3.1 Simultaneous Localization And Mapping

3.1.1 Landmark Extraction

3.1.2 Data Association

3.1.3 Extended Kalman Filter

3.1.4 TangentBug

3.2 Artificial Potential Field Approach