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

,


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][2][3][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. erefore, 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. e 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].
e 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. e 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.

System Model
As shown in Figure 1, 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 (C 1 , C 2 ,...), which are Microsoft Kinect sensors [9], are installed. e 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.
For autonomous flight, the visual feedback concept [10] is employed and two coordinate systems are defined: the inertial frame O E X E Y E Z E and the body frame attached at the center of the quadrotor. e center of mass and the body frame origin are assumed to coincide. e rotation matrix R E b from the body frame to the inertial frame is denoted using Euler angles yaw ψ, pitch θ, and roll φ: 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 θ e core problem we address in this paper is to plan an optimal path U opt from the arbitrary starting point p s to the arbitrary ending point p e that minimizes the energy consumption.

Energy Consumption Analysis.
e energy consumption sources of a MAV mainly include the powertrain, communications, sensors, and control circuits. Table 1 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.
According to Franco et al. [6], the MAV has a most energy-efficient speed v 0 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 F drag which approximates a constant, as illustrated in Figure 2. e power consumed by the DC motors P M (v) under a specific speed condition is also a constant.
Assume that the total weight G and the power consumed by other airborne modules P 0 remain unchanged. We have where is the drag coefficient, ρ is the air density, and S is the area of the propellers. e energy consumed in the straight flight to cover a distance d at speed v can be computed as Substituting equations (2)-(4) into equation (5), a function of E v varies with v is obtained. en, the most energy-efficient speed v 0 can be found by computing the partial derivative of E v with respect to v e MAV is controlled to fly at v 0 between each grid.

Energy Consumption Model
e flight speed and movements of the MAV determine the energy consumption. erefore, we characterize the relationship between the energy consumption of a candidate  2 Complexity path U and flight motions of the MAV as an energy consumption model to evaluate U. e 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 factor s 1 is defined as where d proj (p s , p e ) is the length of the projection of U on the O E X E Y E plane from p s to p e , as illustrated in Figure 3. (2) Climbing and descending rate: as shown in Figure 3, the climbing and descending rate is defined as the change of the altitude of the MAV on Z e -dimension over time. e energy consumption increases as the rate increases. erefore, the second energy consumption factor s 2 is denoted as (3) Turning angle: a sharp turning is undesirable as turnings often associated with decelerations [12], which are both time and energy consuming. e 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. e third energy consumption factor s3 is expressed as e energy consumption model f eng (U) is defined as a weighted sum of the three aforementioned factors and the optimal path U opt should have the energy consumption model being minimized. Such that where ω 1 , ω 2 , and ω 3 are the energy consumption tuning parameters. e energy consumption model will assist in achieving path with energy optimization.

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. is 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 Figure 4, the main direction of the path planning is set along the X E -axis. In each iteration, q artificial ants with drift characteristics are released at the starting point p s . Except for the ending point p e , these q ants are free to choose any point on the drift circles. e coordinates of current location p i of the ant are (x i , y i , z i ) and there are nine searchable vertexes whose coordinates are (x i+1 , y c , z c ) on the searchable plane i+1 paralleling to the O E Y E Z E -plane. Here, x i+1 is determined by the grid size. e drift circles are on the searchable plane i+1 centered at (x i+1 , y c , z c ). e  Complexity probability of choosing point p i+1 (x i+1 , y i+1 , z i+1 ) on the drift circles is e next point of path U is chosen using the Roulette method according to the probabilities of all the searchable points on the drift circle.
Here, f h (x i+1 , y i+1 , z i+1 ) is the heuristic function defined as (11) where S � {0, 1} is the safety factor indicating whether the next point is reachable, D is the distance between p i+1 and p i , Q is the distance between p i+1 and p e , Z is the altitude difference in Z E -axis between p i+1 and p i , T is the turning angle from p i to p i+1 , and λ 1 , . . ., λ 4 are the coefficients.
f τ (x i+1 , y i+1 , z i+1 ) is the distribution of the pheromone concentration on the circumferences of the drift circles. Once the ant reaches p e , the complete path is evaluated by the energy consumption model f eng (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 where σ y � σ z are the variances along Y E -axis and Z E -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 f eng (U). By the end of this iteration, A will be updated as follows to reinforce the pheromone concentration of the shorter path: where n is the number of iterations, K is a coefficient, and 0 < ε < 1 is the update factor. When all the q ants reach p e , on each searchable vertex, there might be k(0 ≤ k ≤ q) 2D Gaussian distribution functions. ese 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.

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 length s 1 (U) as the evaluation factor, the traditional ACO-based path planning algorithm neither employs the energy consumption model f eng (U) nor has the drift characteristic. e purposed energyoptimal path planning method is evaluated in two cases which have the difference of whether the drift characteristic is fused. e experiments are applied in the testbed introduced in the section system model and performed in the MATLAB R2012a. e size of the planning space is 6m × 6 m × 3 m, and it is evenly divided into 20 × 20 × 10 grids. e 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 Table 2. 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. erefore, 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. e number of the iteration of each algorithm is 100, and in each iteration, 10 artificial ants are released into the 3D map.

Performance Analysis.
Each algorithm is run for 40 times individually. Figure 5 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. e average results of the repeated simulations are presented in Table 3. As can be seen, the proposed energyoptimal 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%.
Such improvements are achieved by employing the energy consumption model f eng (U) during the optimization process.
e 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.

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.

Complexity
As displayed in Table 4, 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. erefore, appropriate grid size should be set to weigh these two factors for practical application.

Conclusions
is 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.

Data Availability
e data used to support the findings of this study are available from the corresponding author upon request.

Conflicts of Interest
e authors declare that there are no conflicts of interest regarding the publication of this paper.