# Energy-Optimal 3D Path Planning for MAV with Motion Uncertainty

Authors: Yamin Li; Bowen Sun; Ping Xia; Yang Yang
Journal: Complexity
 (2021)

---

## Abstract

Practical applications of microaerial vehicle face significant challenges including imprecise localization, limited on-board energy, and motion uncertainty. This paper focuses on the latter two issues. The core of proposed energy-optimal path planning algorithm is an energy consumption model deriving from real measurements of a specific quadrotor and utilizing a 2D Gaussian distribution function to simulate the uncertainty of random drift. Based on these two models, we formulate the optimal path traversing the 3D map with minimum energy consumption using a heuristic ant colony optimization. Multiple sets of contrast experiments demonstrate the effectiveness and efficiency of the proposed algorithm.


---

## Body

## 1. Introduction

Microaerial vehicle (MAV) has shown great application potentials in both military and civilian fields such as security surveillance, aerial photography, and medical escort [1–4]. While the flight endurance of MAV is limited relating to its minisize and possible on-board fuel/battery that can be carried. And the MAV may unpredictably deviate from the planned path due to the random drift or aerodynamic interferences. Therefore, the practical applications of MAV face significant challenges including imprecise localization, motion uncertainty, and limited energy. In this paper, we aim to plan an energy-optimal path for MAV under mission with motion uncertainty.The research of efficient energy path planning for the MAV has received increasing attention. In the studies of traveling salesman problem (TSP), effective routes are chosen to reduce the total path length [5]. Yet, this method does not take into account the orientations of the MAV. Moreover, minimum path length does not always correlate to minimum energy consumption. Franco et al. proposed an energy-aware path planning algorithm based on a real energy measurement of unmanned aerial vehicle (UAV) in different velocities and operating conditions [6]. However, the energy consumption for different flight motions is assumed to be constant, which is unrealistic. A different approach has been taken by Al-Sabban et al. [7], who developed an energy-efficient path planning algorithm by using the available environmental wind energy within the medium where the UAV is operating to extend the flight endurance.Most of the current path planning methods, such as the above algorithms, only focus on the path planning problems in 2-dimensional (2D) plane and unrealistically assume that the MAV can accurately move to the target point without any motion error according to the control commands [8]. The orientation of the MAV after each move is not considered either. Different from these methods, this paper proposes an energy-optimal 3-dimensional (3D) path planning algorithm that minimizes the energy consumption while also considering the motion uncertainty.The contributions of this paper are as follows. (1) An energy consumption model is established deriving from real measurements of a specific MAV, characterizing the relationship between energy consumption and particular flying motions. (2) Instead of correcting motion deviations continuously, we exploit the trend of movement drift, and a 2D Gaussian distribution function is employed to simulate the random drift and position bias. (3) A heuristic procedure fused with the Gaussian distribution is proposed to search the energy-optimal path traversing a 3D map using a modified ant colony optimization (ACO) algorithm.


## 2. System Model

As shown in Figure1, this paper considers a MAV flying in indoor testbed deployed with 3D visual sensor network (VSN) composed of multiple RGB-D sensors. To completely cover the test space, a number of RGB-D sensors (C1, C2,...), which are Microsoft Kinect sensors [9], are installed. The testbed with obstacles is abstracted as a meshed cuboid model, which is the planning space. Evenly divide the space into grids and the complete path can be stored as a list of coordinates p (x, y, z). To avoid the obstacles, only grids above the obstacles are searchable.Figure 1
Indoor testbed configuration and the two coordinate systems.For autonomous flight, the visual feedback concept [10] is employed and two coordinate systems are defined: the inertial frame OEXEYEZE and the body frame ObXbYbZb attached at the center of the quadrotor. The center of mass and the body frame origin are assumed to coincide. The rotation matrix RbE from the body frame to the inertial frame is denoted using Euler angles yaw ψ, pitch θ, and roll φ:(1)RbE=cosθcosψsinφsinθcosψ−cosφsinψcosφsinθsinψ+sinφsinψcosθsinψsinφsinθsinψ+cosφcosψcosφsinθsinψ−sinφcosψ−sinθsinφcosθcosφcosθ.The core problem we address in this paper is to plan an optimal pathUopt from the arbitrary starting point ps to the arbitrary ending point pe that minimizes the energy consumption.


## 3. Energy Consumption Estimation

### 3.1. Energy Consumption Analysis

The energy consumption sources of a MAV mainly include the powertrain, communications, sensors, and control circuits. Table1 shows a power consumption measurement result of Crazyflie Nano Quadcopter from Bitcraze [11], which is a 19 g mini MAV used in our current system. As can be seen from Table 1, the motor drive consumes more than 85% of its total energy, which is absolutely the main energy consumer.Table 1
The power consumptions of different modules of Crazyflie.
MCU (%)Sensors (%)Wireless communication (%)Motor drive (%)Others (%)9.480.461.9887.021.06According to Franco et al. [6], the MAV has a most energy-efficient speed v0 defined as the speed that minimizes the energy required to cover a given straight path of length. When the MAV is flying straight in a constant speed v, the thrust T is equal to the sum of weight G (payload involved) and the air friction Fdrag which approximates a constant, as illustrated in Figure 2. The power consumed by the DC motors PMv under a specific speed condition is also a constant.Figure 2
Force analysis of the MAV flying at speedv.Assume that the total weightG and the power consumed by other airborne modules P0 remain unchanged. We have(2)Tcosθ=G,(3)Tsinθ=Fdrag=12Cdθv2S,(4)PMv=Tvcosπ2−θ,where Cdθ=C11−cos3θ+C21−sin3θ is the drag coefficient, ρ is the air density, and S is the area of the propellers. The energy consumed in the straight flight to cover a distance d at speed v can be computed as(5)Ev=∫0d/vPMv+P0dt=PMv+P0dv.Substituting equations (2)–(4) into equation (5), a function of Ev varies with v is obtained. Then, the most energy-efficient speed v0 can be found by computing the partial derivative of Ev with respect to v The MAV is controlled to fly at v0 between each grid.


## 3.1. Energy Consumption Analysis

The energy consumption sources of a MAV mainly include the powertrain, communications, sensors, and control circuits. Table1 shows a power consumption measurement result of Crazyflie Nano Quadcopter from Bitcraze [11], which is a 19 g mini MAV used in our current system. As can be seen from Table 1, the motor drive consumes more than 85% of its total energy, which is absolutely the main energy consumer.Table 1
The power consumptions of different modules of Crazyflie.
MCU (%)Sensors (%)Wireless communication (%)Motor drive (%)Others (%)9.480.461.9887.021.06According to Franco et al. [6], the MAV has a most energy-efficient speed v0 defined as the speed that minimizes the energy required to cover a given straight path of length. When the MAV is flying straight in a constant speed v, the thrust T is equal to the sum of weight G (payload involved) and the air friction Fdrag which approximates a constant, as illustrated in Figure 2. The power consumed by the DC motors PMv under a specific speed condition is also a constant.Figure 2
Force analysis of the MAV flying at speedv.Assume that the total weightG and the power consumed by other airborne modules P0 remain unchanged. We have(2)Tcosθ=G,(3)Tsinθ=Fdrag=12Cdθv2S,(4)PMv=Tvcosπ2−θ,where Cdθ=C11−cos3θ+C21−sin3θ is the drag coefficient, ρ is the air density, and S is the area of the propellers. The energy consumed in the straight flight to cover a distance d at speed v can be computed as(5)Ev=∫0d/vPMv+P0dt=PMv+P0dv.Substituting equations (2)–(4) into equation (5), a function of Ev varies with v is obtained. Then, the most energy-efficient speed v0 can be found by computing the partial derivative of Ev with respect to v The MAV is controlled to fly at v0 between each grid.


## 4. Energy Consumption Model

The flight speed and movements of the MAV determine the energy consumption. Therefore, we characterize the relationship between the energy consumption of a candidate pathU and flight motions of the MAV as an energy consumption model to evaluate U. The energy consumption of U is determined by the following three factors.(1)
Path length: the energy consumption is proportional to the path length. So, the first energy consumption factors1 is defined as(6)s1U=dprojps,pe,wheredprojps,pe is the length of the projection of U on the OEXEYE plane from ps to pe, as illustrated in Figure 3.(2)
Climbing and descending rate: as shown in Figure3, the climbing and descending rate is defined as the change of the altitude of the MAV on Ze-dimension over time. The energy consumption increases as the rate increases. Therefore, the second energy consumption factor s2 is denoted as(7)s2U=maxΔzΔt=maxzpi+1−zpitpi,pi+1.(3)
Turning angle: a sharp turning is undesirable as turnings often associated with decelerations [12], which are both time and energy consuming. The turning angle is defined as the angle formed by any three consecutive points on the projection of U on the OEXEYE plane, as illustrated in Figure 3. The third energy consumption factor s3 is expressed as(8)s3U=180°−min∠pi−1′pi′pi+1′.Figure 3
An illustration of (U) and the three desirability factors.The energy consumption modelfeng(U) is defined as a weighted sum of the three aforementioned factors and the optimal path Uopt should have the energy consumption model being minimized. Such that(9)Uopt=argminfengU=argminω1s1+ω2s2+ω3s3ω1+ω2+ω3.where ω1, ω2, and ω3 are the energy consumption tuning parameters. The energy consumption model will assist in achieving path with energy optimization.


## 5. Path Planning with Motion Uncertainty

Instead of correcting the motion deviations constantly in the control phase, we deal with the problem of the motion uncertainty of a flying MAV in advance, that is, in the process of path planning. Different from a traditional ACO algorithm [13] whose artificial ants are only allowed to select the vertexes of the grids as their next hop, in the proposed path planning procedure, all the artificial ants are given “drift” characteristics which allow them to select any point on the circumference of a drift circle with a radius of R centered at the searchable vertex of the grid as their next hop. This method is developed to simulate the motion uncertainty of the flying MAV and also give the ants extraflexibility in making routing decisions.As illustrated in Figure4, the main direction of the path planning is set along the XE-axis. In each iteration, q artificial ants with drift characteristics are released at the starting point ps. Except for the ending point pe, these q ants are free to choose any point on the drift circles. The coordinates of current location pi of the ant are (xi, yi, zi) and there are nine searchable vertexes whose coordinates are (xi+1, yc, zc) on the searchable plane ∏i+1 paralleling to the OEYEZE-plane. Here, xi+1 is determined by the grid size. The drift circles are on the searchable plane ∏i+1 centered at (xi+1, yc, zc). The probability of choosing point pi+1(xi+1, yi+1, zi+1) on the drift circles is(10)Ppi+1=fhxi+1,yi+1,zi+1fτxi+1,yi+1,zi+1∑fhxi+1,yi+1,zi+1fτxi+1,yi+1,zi+1.Figure 4
The searchable plane and drift circle.The next point of pathU is chosen using the Roulette method according to the probabilities of all the searchable points on the drift circle.Here,fhxi+1,yi+1,zi+1 is the heuristic function defined as(11)fhxi+1,yi+1,zi+1=S×λ1D+λ2Q+λ3Z+λ4T,where S = {0, 1} is the safety factor indicating whether the next point is reachable, D is the distance between pi+1 and pi, Q is the distance between pi+1 and pe, Z is the altitude difference in ZE-axis between pi+1 and pi, T is the turning angle from pi to pi+1, and λ1, …, λ4 are the coefficients.fτxi+1,yi+1,zi+1 is the distribution of the pheromone concentration on the circumferences of the drift circles. Once the ant reaches pe, the complete path is evaluated by the energy consumption model feng(U) and the pheromone will be distributed on the drift circles that have been visited, which is in the form of 2D Gaussian distribution function given by(12)fτxi+1,y,z=Aexp−y−yc22σy2+z−zc22σz2,where σy = σz are the variances along YE-axis and ZE-axis, respectively, since only symmetric 2D Gaussian functions are considered in the proposed method. A is the amplitude of its pheromone concentration whose initial value is inversely proportional to its energy consumption feng(U). By the end of this iteration, A will be updated as follows to reinforce the pheromone concentration of the shorter path:(13)An+1=1−εAn+εKfengUn,where n is the number of iterations, K is a coefficient, and 0 < ε < 1 is the update factor.When all theq ants reach pe, on each searchable vertex, there might be k(0 ≤ k ≤ q) 2D Gaussian distribution functions. These k Gaussian functions will superpose and form a joint distribution function on each searchable plane ∏i+1 and update the pheromone concentration on the circumferences of the drift circles. In the following iterations, the ants will select their moving direction based on the pheromone residue on the drift circles.


## 6. Simulations and Results

### 6.1. Simulation Settings

To evaluate the performance of the proposed method, we compare the proposed energy-optimal path planning method with the traditional ACO-based path planning method. Using only path lengths1(U) as the evaluation factor, the traditional ACO-based path planning algorithm neither employs the energy consumption model feng(U) nor has the drift characteristic. The purposed energy-optimal path planning method is evaluated in two cases which have the difference of whether the drift characteristic is fused.The experiments are applied in the testbed introduced in the section system model and performed in the MATLAB R2012a. The size of the planning space is 6m × 6 m × 3 m, and it is evenly divided into 20 × 20 × 10 grids. The Crazyflie starts from point (0, 2, 4) and ends at point (20, 6, 6) at a speed of 0.5 m/s which is close to the most energy-efferent speed in the current configuration. Parameters used in the simulations are listed in Table2. It is worth noting that the tuning parameters ω1, ω2, and ω3 of the energy consumption model, the coefficients of the proposed energy-optimal path planning algorithm such as λ1, λ2, λ3, and λ4 in equation (11), the variances σy and σz in equation (12), and even the number of the artificial ants can significantly impact the simulation results. Therefore, the parameters’ list in Table 2 are reasonably determined by experimental analyses based on uniform design [14], which is used to convert the problem of parameter establishment into the experimental design of multifactor and multilevel and reduces the work load of experiment greatly of simulation. The number of the iteration of each algorithm is 100, and in each iteration, 10 artificial ants are released into the 3D map.Table 2
Parameters used in the simulation.
ParametersTraditional ACOProposed energy-optimal methodWithout drift characteristicWith drift characteristicGrid size20 × 20 × 1020 × 20 × 1020 × 20 × 10Turning parameters (ω1, ω2, ω3)4, 0, 04, 2, 24, 2, 2Coefficients (λ1, λ2, λ3, λ4)50, 50, 0, 050, 50, 30, 1050, 50, 30, 10Radius (R)N/AN/A0.5Variances (σy, σz)N/AN/A0.05, 0.05Coefficients (K)100100100Update factor (ε)0.20.20.2


### 6.2. Performance Analysis

Each algorithm is run for 40 times individually. Figure5 displays an example of two paths planned by the traditional ACO-based path planning algorithm which is displayed in red line and the proposed energy-optimal path planning method without drift characteristic which is displayed in blue line. As can be observed visually, both the two path have successfully avoided the obstacles in the 3D map. However, the blue path planned by the proposed method is smoother with less climbings, descendings, and sharp turns than the red one planned by the traditional ACO algorithm.Figure 5
The planned paths of the two methods.The average results of the repeated simulations are presented in Table3. As can be seen, the proposed energy-optimal path planning method shows greatly improved performance over the traditional ACO algorithm in all three evaluation factors, especially the one without simulating the drift characteristic. It outperforms the traditional ACO algorithm by reducing the path length and climbing and descending rate by more than 6.4% and 39.1%, and increasing the minimum turning angle by more than 14.3%.Table 3
Simulation results of the two methods.
EvaluationsTraditional ACOProposed energy-optimal methodWith drift characteristicWithout drift characteristicAveraged path length/m10.309.649.94Averaged climbing and descending rate/m/s0.230.140.16Averaged minimum turning angle41.46°47.39°43.94°Such improvements are achieved by employing the energy consumption modelfeng(U) during the optimization process. The one with drift characteristic also has better performance over the traditional ACO algorithm. However, compared with the one without drift characteristic, the planned path slightly degrades, which is reasonable and unavoidable when the ants are given the ability to randomly deviate from the planned path.


### 6.3. Grid Size Experiments

In addition, we verify the performances of the proposed algorithm when the planning space is divided into 20 × 20 × 10 grids or 40 × 40 × 20 grids. As displayed in Table4, the proposed path planning algorithm in finer division space shows improved performance in terms of all three evaluation factors, which means better accuracy and lower power consumption. However, the computing time is many times longer than the former, which means worse real-time performance. Therefore, appropriate grid size should be set to weigh these two factors for practical application.Table 4
Performances of the proposed algorithm in different grid sizes.
Grid size20 × 20 × 1040 × 40 × 20Averaged path length (m)9.6429.186Averaged climbing and descending rate (m/s)0.1380.117Averaged minimum turning angle47.39°48.62°Computing time (s)3.80231.122


## 6.1. Simulation Settings

To evaluate the performance of the proposed method, we compare the proposed energy-optimal path planning method with the traditional ACO-based path planning method. Using only path lengths1(U) as the evaluation factor, the traditional ACO-based path planning algorithm neither employs the energy consumption model feng(U) nor has the drift characteristic. The purposed energy-optimal path planning method is evaluated in two cases which have the difference of whether the drift characteristic is fused.The experiments are applied in the testbed introduced in the section system model and performed in the MATLAB R2012a. The size of the planning space is 6m × 6 m × 3 m, and it is evenly divided into 20 × 20 × 10 grids. The Crazyflie starts from point (0, 2, 4) and ends at point (20, 6, 6) at a speed of 0.5 m/s which is close to the most energy-efferent speed in the current configuration. Parameters used in the simulations are listed in Table2. It is worth noting that the tuning parameters ω1, ω2, and ω3 of the energy consumption model, the coefficients of the proposed energy-optimal path planning algorithm such as λ1, λ2, λ3, and λ4 in equation (11), the variances σy and σz in equation (12), and even the number of the artificial ants can significantly impact the simulation results. Therefore, the parameters’ list in Table 2 are reasonably determined by experimental analyses based on uniform design [14], which is used to convert the problem of parameter establishment into the experimental design of multifactor and multilevel and reduces the work load of experiment greatly of simulation. The number of the iteration of each algorithm is 100, and in each iteration, 10 artificial ants are released into the 3D map.Table 2
Parameters used in the simulation.
ParametersTraditional ACOProposed energy-optimal methodWithout drift characteristicWith drift characteristicGrid size20 × 20 × 1020 × 20 × 1020 × 20 × 10Turning parameters (ω1, ω2, ω3)4, 0, 04, 2, 24, 2, 2Coefficients (λ1, λ2, λ3, λ4)50, 50, 0, 050, 50, 30, 1050, 50, 30, 10Radius (R)N/AN/A0.5Variances (σy, σz)N/AN/A0.05, 0.05Coefficients (K)100100100Update factor (ε)0.20.20.2


## 6.2. Performance Analysis

Each algorithm is run for 40 times individually. Figure5 displays an example of two paths planned by the traditional ACO-based path planning algorithm which is displayed in red line and the proposed energy-optimal path planning method without drift characteristic which is displayed in blue line. As can be observed visually, both the two path have successfully avoided the obstacles in the 3D map. However, the blue path planned by the proposed method is smoother with less climbings, descendings, and sharp turns than the red one planned by the traditional ACO algorithm.Figure 5
The planned paths of the two methods.The average results of the repeated simulations are presented in Table3. As can be seen, the proposed energy-optimal path planning method shows greatly improved performance over the traditional ACO algorithm in all three evaluation factors, especially the one without simulating the drift characteristic. It outperforms the traditional ACO algorithm by reducing the path length and climbing and descending rate by more than 6.4% and 39.1%, and increasing the minimum turning angle by more than 14.3%.Table 3
Simulation results of the two methods.
EvaluationsTraditional ACOProposed energy-optimal methodWith drift characteristicWithout drift characteristicAveraged path length/m10.309.649.94Averaged climbing and descending rate/m/s0.230.140.16Averaged minimum turning angle41.46°47.39°43.94°Such improvements are achieved by employing the energy consumption modelfeng(U) during the optimization process. The one with drift characteristic also has better performance over the traditional ACO algorithm. However, compared with the one without drift characteristic, the planned path slightly degrades, which is reasonable and unavoidable when the ants are given the ability to randomly deviate from the planned path.


## 6.3. Grid Size Experiments

In addition, we verify the performances of the proposed algorithm when the planning space is divided into 20 × 20 × 10 grids or 40 × 40 × 20 grids. As displayed in Table4, the proposed path planning algorithm in finer division space shows improved performance in terms of all three evaluation factors, which means better accuracy and lower power consumption. However, the computing time is many times longer than the former, which means worse real-time performance. Therefore, appropriate grid size should be set to weigh these two factors for practical application.Table 4
Performances of the proposed algorithm in different grid sizes.
Grid size20 × 20 × 1040 × 40 × 20Averaged path length (m)9.6429.186Averaged climbing and descending rate (m/s)0.1380.117Averaged minimum turning angle47.39°48.62°Computing time (s)3.80231.122


## 7. Conclusions

This paper proposes an energy-optimal 3D path planning algorithm for indoor flying MAV which deals with the problem of motion uncertainty in the process of path planning and relaxes the limitation of making routing decisions. Numerical experiments results demonstrate its effectiveness and optimal performances. Our algorithm reduces the path length and climbing and descending rate by more than 6.4% and 39.1%, increasing the minimum turning angle by more than 14.3%. For future work, extension of this method to plan missions for multiple MAVs is considered.



---

*Source: 9994680-2021-10-18.xml*