Proceedings of the 3rd RSI International Conference on Robotics and Mechatronics October 7-9, 2015, Tehran, Iran
Three Dimensional Fuzzy Carrot-Chasing Path Following Algorithm For Fixed-Wing Vehicles Seyed Amir Hossein Tabatabaei1, Aghil Yousefi-koma 2*, Moosa Ayati3, Seyed Saeid Mohtasebi4 1,2,4
Center of Advanced Systems and Technologies (CAST), School of Mechanical Engineering, College of Engineering, University of Tehran, Tehran, Iran *
[email protected]
3
Abstract — over the past several years, Micro Aerial Vehicles
LOS[2], carrot-chasing [3] and vector field [4, 5] methods are some common geometric algorithms. On the other hand, there are some different control techniques used for designing guidance law for MAVs. Miao et al [6] described an adaptive control method for a 3D nonlinear path following algorithm for a fixedǦwing MAV with wind disturbances in a complicated terrain. Backstepping control technique was used by Sabarao et al [7] for designing a guidance system for a MAV. Also, some other control techniques such as linear quadratic regulator (LQR) [8], sliding mode control [9], model predictive control [10], backstepping control [11, 12], and gain scheduling theory [13] have been developed for path following problems for MAVs. In this paper, first, by using a new geometric method, a 2D carrot-chasing path following algorithm is modified to a 3D algorithm and then, to improve the performance, a new parametric 3D path following algorithm is presented using a combination of 3D carrot-chasing algorithm, fuzzy logic, and genetic algorithm. This algorithm is called 3D fuzzy carrotchasing algorithm. For comparison, 3D carrot-chasing algorithm, 3D vector field method, and proposed 3D fuzzy carrot-chasing algorithms are implemented on a simulated nonlinear six DoF MAV. Simulations are done to investigate the effects of wind external disturbance on the algorithms.
(MAVs) have received considerable attentions due to many useful applications in military and civil missions. Applications such as search and rescue, mapping, and surveillance require the MAVs to autonomously follow a predefined three dimensional path. Because of unavailability of the pilot during a mission, it is necessary to use autopilot systems. In this paper a parametric path following algorithm is designed and then implemented on a simulated nonlinear six-DoF MAV. For designing the path following algorithm, first, twodimensional (2D) carrot-chasing algorithm is modified to a 3D algorithm. Then a 3D path following algorithm is presented using a combination of the carrot-chasing geometric algorithm, fuzzy logic and genetic algorithm optimization method. It is concluded that adding a fuzzy system to the carrot-chasing algorithm, improves its performance significantly. Also, for comparison, two other 3D carrot-chasing algorithm and 3D vector field method are designed and simulated. Finally, by considering a control system for the simulated MAV, a comparison among the three path following algorithms is done. Wind as an external disturbance is applied to the MAV. Simulation results show significant superiority of the proposed 3D fuzzy carrot-chasing algorithm over both the carrotchasing and vector field methods. Keywords- Micro Aerial Vehicles, Three-dimensional path following, fuzzy carrot-chasing algorithm, vector field algorithm, genetic algorithm
I.
INTRODUCTION In order to have an autonomous predefined mission by a Micro Aerial Vehicle (MAV), a combination of: navigation, guidance and control sub-systems are required. Overall system is generally called an autopilot system.
II.
THREE DIMENSIONAL VECTOR FIELD ALGORITHM
The main idea of this algorithm is that the vector fields show the direction of flow. If the MAV follows the VF direction, then it will follow the path. In this algorithm, the space is divided into two parts by defining a virtual cylinder around the path. The cylinder radius is known as transition distance τ. Then the vectors are created inside and outside of the virtual cylinder such that the external vectors are perpendicular to the virtual cylinder and the internal ones tend to the path smoothly. To implement this algorithm, some MAV flight states are needed such as location ሺǡǡሻǡ
ǡ߰ߛߛ
Ʌ
ሺሻȽǣ
The task of a navigation system is calculation of MAV attitude and location for the delivery to guidance and control systems. On the other hand, a guidance system calculates desired velocity, height and heading angle using MAV horizontal location ሺǡሻ, waypoints location ሺǡǡሻ or some other flight states like vehicle velocity and heading angle. These desired values are delivered to the control system. Guidance system is based on trajectory tracking or path following. In trajectory tracking, the path is time parameterized, though time is not considered in path following. This article deals with a three dimensional path following algorithm for designing a guidance system for fixed-wing MAVs. In the area of path following algorithms, there are different geometric or control based methods. Pure pursuit [1],
978-1-4673-7234-3/15/$31.00 ©2015 IEEE
School of Mechanical Engineering, College of Engineering, University of Tehran Tehran, Iran
784
J ɅǦȽ
Pseudo code of this algorithm was described in [5]. To flight in a 3D space, the algorithm outputs are desired heading angle ߰ and flight path angle ߛ.
It should be noted that the only variable parameter in this algorithm is delta( δ ) and must be optimized; because on one side , small value of delta leads in oscillating convergence to the desired path and on the other side, big value of delta leads in slow convergence [3]. So choosing an optimized value of delta is necessary to have a good convergence.
III. THREE DIMENSIONAL CARROT-CHASING ALGORITHM Another geometric algorithm which is used to follow a predefined desired path is called carrot-chasing. The main idea of this algorithm is imagination of a virtual target on the desired path as a carrot so that the MAV follows it. Fig. 1 shows the 2D carrot-chasing idea.
IV. THREE ALGORITHM
Inputs: ܹଵ ሺݔଵ ǡ ݕଵ ǡ ݖଵ ሻǡ ܹଶ ሺݔଶ ǡ ݕଶ ǡ ݖଶ ሻǡ ܲሺݔǡ ݕሻ
2.
ሾሺݔଵ െ ݔሻ ሺݕଵ െ ݕሻ
3.
ሾሺݔଶ െ ݔଵ
ሻଶ
4.
ܽʹ݊ܽݐሺݕଶ െ ݕଵ ǡ ݔଶ െ ݔଵ ሻ ՜ ߠ
5.
ܽʹ݊ܽݐሺ ݕെ ݕଵ ǡ ݔെ ݔଵ ሻ ՜ ߠ௨
6.
ߠ௨ െ ߠ ՜ ߚ
7.
ܴ௨ ܿݏሺߚሻ ՜ ܴ x
8.
ሺݕଶ െ ݕଵ
՜ ܴ௨
ሻଶ ሿǤହ
՜݈
1.
Absolute value of lateral distance ȁ݀ȁ
2.
ȁ݀ሶ ȁ Time rate of the absolute value of lateral distance തതതത
3.
Sign of ܴሶ
So, as shown in Fig. 3, 7 different values of delta can be chosen based on the MAV flight situation. Different situations and corresponded values of delta are defined in table1. ݓ
ࢊ
ݓାଵ
ܴ݂ܫ Ͳ
ݖଵ ՜ ݄ௗ ǡ ݄ௗ ՜ ݄ x
9.
ଶ ሿǤହ
CARROT -CHASING
According to Fig. 2, to cover all situations that the MAV takes to the desired path, the three parameters are required:
Pseudo code of this algorithm is as follows: ଶ
FUZZY
As it was mentioned before, the value of delta can affect convergence response, significantly. According to flight variable situations of a MAV in terms of lateral distance d and its time rate, the variable values of delta can be chosen in order to have the best response. For this purpose, suitable values of delta are calculated using fuzzy logic.
To develop the algorithm into a 3D space, a locationdependent instantaneous desired height hd should be defined. The intersection of a line perpendicular to the path passing through the first and the last waypoints, passing through the current position, specify the desired instantaneous height hd. In fact, since the MAV longitudinal flight is independent of the lateral-directional flight, following the desired height is done separately. 1.
DIMENSIONAL
݈݁݁ݏ
Fig. 2. Required parameters to describe the MAV situation
ݖଵ ሺݖଶ െ ݖଵ ሻ ܴ כȀ݈ ՜ ݄ௗ ǡ ݄ௗ ՜ ݄ x
ࡾ
ݓ
݂݁݊݀݅
ݓାଵ ࢾ
10. ሾሺܴ ߜሻܿݏሺߠሻ ݔଵ ǡ ሺܴ ߜሻ݊݅ݏሺߠሻ ݕଵ ሻሿ ՜ ሺݔ௧ ǡ ݕ௧ ሻǡ ݏൌ ሺݔ௧ ǡ ݕ௧ ሻ
ࢾ
࣎൘
11. ܽʹ݊ܽݐሺݕ௧ െ ݕǡ ݔ௧ െ ݔሻ ՜ ߰ௗ ǡ ߰ௗ ՜ ߰
ࢾ Small distance
ࢾ
where, the parameters are shown in Fig. 1. In this algorithm, commanded height and heading angle are determined from 8 or 9 and 11 stages, respectively. As shown above, the input of this algorithm is only horizontal location of the vehicle P(x,y).
ࢾ ࣎ ࢾ
ࢾ ݕ
ߜ
ܹାଵ ࢾ ൌ
ݍ
ݔ
݀ ܴ ܴ௨
ܹ
ݏ
ߠ
ߖௗ
Middle distance
ܸ ߖ ࢾ ൌ
ߠ௨
Big distance
Fig. 1. A two dimensional carrot-chasing algorithm
Fig. 3. Different values of delta in terms of the MAV situation
785
TABLE I.
DEFINITION OF DEFFERENT FLIGHT SITUATIONS
TABLE III.
δ
ȁ݀ȁ
തതതത ȁ݀ሶȁ
Sgn(ܴሶ )
δ0=0
ȁ݀ȁ ߬
-
-
߬ ൏ ȁ݀ȁ ൏ ߬ ͵ ߬ ൏ ȁ݀ȁ ൏ ߬ ͵ ߬ ൏ ȁ݀ȁ ൏ ߬ ͵ ߬ ȁ݀ȁ ͵ ߬ ȁ݀ȁ ͵ ߬ ȁ݀ȁ ͵ ߬ ȁ݀ȁ ͵
ξʹ തതതതሶ ܸത ܸത ȁ݀ȁ ʹ ξʹ തതതതሶ ൏ ξʹ ܸത െ ܸത ൏ ȁ݀ȁ ʹ ʹ ξʹ ȁ݀ȁሶ െ ܸത െܸത തതതത ʹ ξʹ തതതതሶ ൏ ξʹ ܸത െ ܸത ൏ ȁ݀ȁ ʹ ʹ ξʹ തതതതሶ ܸത ܸത ȁ݀ȁ ʹ ξʹ ȁ݀ȁሶ െ ܸത െܸത തതതത ʹ ξʹ തതതതሶ ൏ ξʹ ܸത െ ܸത ൏ ȁ݀ȁ ʹ ʹ
-
δ0=0 δ1 δ2 δ3 δ4 δ5 δ6
δ1(m)
δ2(m)
δ3(m)
δ4(m)
δ5(m)
δ6(m)
3.79
11.8
1.86
2.45
11.1
5.45
In the FIS, Mamdani inference engine, “minimum” and “maximum” operators are used as t-norm and s-norm, respectively. Also using the defined intervals in table 1 for creating uniform triangular membership functions of inputs and corresponded optimal values of delta as the center of output triangular membership functions, the fuzzy system is created.
-
Fig. 4 and Fig. 5 show the MAV horizontal planes of motion without and with applying wind external disturbance, respectively, such that the superiority of the fuzzy carrotchasing is clear.
Neg. -
300
Pos.
250
Now, the best values of delta can be achieved using genetic algorithm optimization method. Cost function of this optimization is the horizontal area between the flight path and the desired path during the maneuver after running some appropriate maneuvers which cover the entire defined situations. To have smooth switching between the calculated δs, fuzzy inference system (FIS) can be used so that the values of δ are used as FIS membership function centers. V.
CALCULATED OPTIMAL VALUES OF DELTA
Y(m)
200
150
100
Start/End Des. Path VF CC Fuzzy CC
NUMERICAL RESULTS 50
In this paper, the three path following algorithm are implemented on a simulated nonlinear six-DoF MAV with the same control system using MATLAB Simulink. In this simulation, MAV mission is to start from a certain point, passing through some straight paths and then returning to the its initial point at constant velocity of 15 m/s. Waypoints are given in table 2. The results are shown with and without applying wind external disturbance such that the significant superiority of the fuzzy carrot-chasing approach on both the carrot-chasing and vector field methods is clear.
0 0
100
150 X(m)
200
250
300
100 90 80
In the simulations, the considered transition distance τ in all three approaches is equal to 50 m. In the case of carrot-chasing, using the genetic algorithm and mentioned cost function, the obtained optimal value of delta is 9.3 m. Also, the calculated optimal values of δ in the fuzzy carrot chasing algorithm are given in table 3. TABLE II.
50
70
Y(m)
60 50 40 30
CONSIDERED WAYPOINTS FOR THE MAV MISSION
20
Waypoints
Longitude, X(m)
Latitude, Y(m)
Height(m)
Start
300
0
100
1
0
0
100
2
0
200
120
3
150
300
140
4
300
200
130
End
300
0
100
10 0 0
20
40
60
80
100
X(m)
Fig. 4. MAV horizontal plane of motion without applying wind disturbance
786
TABLE IV.
300
ERROR AND SENSITIVITY TO THE WIND DISTURBANCE
Algorithm
Error without applying wind (m2)
Error with applying wind (m2)
250
Y(m)
200
150
100
Start/End Des. Path VF CC Fuzzy CC
50
Percentage of sensitivity (%)
Vector field
2035
3806
87
Carrot-chasing
3032
3262
7.6
Fuzzy carrotchasing
1504
2020
34
2
1
0 50
100
150 X(m)
200
250
300
0
Error(m)
0
100 90
-1
-2
80 70
Y(m)
VF CC Fuzzy CC
-3
60 50
-4
40
0
10
20
30
30 40 Time(s)
50
60
Fig. 6. Height error during the maneuver without applying wind
20 10 0 -10
3
0
20
40 X(m)
60
80
100
2 1
Fig. 5. MAV horizontal plane of motion with applying wind disturbance
0
Error(m)
Also, horizontal error and sensitivity to the wind external disturbance are given in table 4. As mentioned before, the cost function (error function) of this optimization is the horizontal area between the flight path and desired path during the maneuver.
-1 -2 -3
In the case of third dimension, height, Fig. 6 and Fig. 7 show the height error during the maneuver in all the three approaches without and with applying wind external disturbances, respectively. Also height mean square error (MSE) in each algorithm and corresponded sensitivity to the wind disturbance are given in table 5.
-4
VF CC Fuzzy CC
-5 -6
0
10
20
30 40 Time(s)
50
60
Fig. 7. Height error during the maneuver with applying wind
787
TABLE V.
HEIGHT ERROR AND SENSITIVITY TO THE WIND DISTURBANCE
Algorithm
Error without applying wind (m2)
Error with applying wind (m2)
[2]
Percentage of sensitivity (%)
Vector field
0.3595
3.544
886
Carrot-chasing
0.1756
0.2523
43.7
Fuzzy carrotchasing
0.1412
0.2078
47.2
[3]
[4] [5]
VI. CONCLUSION In this paper, first, 2D carrot-chasing algorithm is modified to a 3D algorithm. Then, by utilizing fuzzy logic a parametric 3D fuzzy carrot-chasing path following algorithm is presented. In addition, the algorithm is optimized using genetic algorithm and implemented in a simulated nonlinear six DoF MAV. For comparison, 3D vector field path following algorithm is implemented.
[6]
[7]
[8]
The simulation results show the superiority of the 3D fuzzy carrot-chasing algorithm on both the carrot-chasing and vector field approaches in horizontal and vertical planes of motion. Also, the results demonstrate the high sensitivity of the vector field algorithm to wind external disturbance. Vector field algorithm is highly sensitive to wind external disturbance or measurement errors. Thus these inputs decrease the efficiency of vector field algorithm. On the other hand, the carrot-chasing and the fuzzy carrot-chasing algorithms have acceptable performance in presence of external disturbance.
[9]
[10] [11]
REFERENCES
[12]
[1]
[13]
G. Conte, S. Duranti, and T. Merz, "Dynamic 3D path following for an autonomous helicopter," 2004.
788
S. H. Jalali Naini, "Optimal Line-of-Sight guidance law for moving targets," presented at the 14th International Conference of Iranian Aerospace Society, Tehran, Iran, 2015. P. Sujit, S. Saripalli, and J. Borges Sousa, "Unmanned Aerial Vehicle Path Following: A Survey and Analysis of Algorithms for Fixed-Wing Unmanned Aerial Vehicless," Control Systems, IEEE, vol. 34, pp. 42-59, 2014. D. R. Nelson, D. B. Barber, T. W. McLain, and R. W. Beard, "Vector field path following for miniature air vehicles," Robotics, IEEE Transactions on, vol. 23, pp. 519-529, 2007. V. Meenakshisundaram, V. Gundappa, and B. S. Kanth, "Vector field guidance for path following of MAVs in three dimensions for variable altitude maneuvers," International Journal of Micro Air Vehicles, vol. 2, pp. 255-265, 2010. C.-X. Miao and J.-C. Fang, "An Adaptive Three-Dimensional Nonlinear Path Following Method for a Fix-Wing Micro Aerial Vehicle," International Journal of Advanced Robotic Systems, vol. 9, 2012. K. Subbarao and M. Ahmed, "Nonlinear Guidance and Control Laws for Three-Dimensional Target Tracking Applied to Unmanned Aerial Vehicles," Journal of Aerospace Engineering, vol. 27, pp. 604-610, 2012. S. Lee, A. Cho, and C. Kee, "Integrated waypoint path generation and following of an unmanned aerial vehicle," Aircraft Engineering and Aerospace Technology, vol. 82, pp. 296-304, 2010. A. J. Healey and D. Lienard, "Multivariable sliding mode control for autonomous diving and steering of unmanned underwater vehicles," Oceanic Engineering, IEEE Journal of, vol. 18, pp. 327339, 1993. Z. Li, J. Sun, and S. Oh, "Handling roll constraints for path following of marine surface vessels using coordinated rudder and propulsion control," American Control Conference (ACC), 2010. V. K. Chitrakaran, D. M. Dawson, H. Kannan, and M. Feemster, "Vision assisted autonomous path following for unmanned aerial vehicles," in Proc. of 45th IEEE Conference on Decision and Control, pp. 63-68, 2006. Z. Li, J. Sun, and S. Oh, "Design, analysis and experimental validation of a robust nonlinear path following controller for marine surface vessels," Automatica, vol. 45, pp. 1649-1658, 2009. R. Cunha, C. Silvestre, and A. Pascoal, "A path following controller for model-scale helicopters," in Proceedings of European Control Conference ECC 2003, 2003.
