Any Optimization algorithms which are capable of solving various multimodal functions can be implemented to find out the inverse kinematic solutions. The fitness function is given by the Eq. (46) fitness function F(x). Each individual represents a joint variable solution of the inverse kinematic problem for adopted population based metaheuristic algorithm. All individuals moving in D-dimensional search space and sharing the information to find out best fitness

made on global searching point, computational cost and quality of the result.

3.1. Optimization approach to solve inverse kinematics

5 (35)

,

,

,

(34)

(36)

(37)

(38)

The current position of the manipulator is described by (39):

$$P\_{\mathfrak{c}} = \begin{bmatrix} X\_{\mathfrak{c}}, & Y\_{\mathfrak{c}}, & Z\_{\mathfrak{c}} \end{bmatrix} \tag{39}$$

Desired position of end effector can be denoted by (40):

$$P\_d = \begin{bmatrix} X\_d, & \mathcal{Y}\_d, & Z\_d \end{bmatrix} \tag{40}$$

Current position of end effector will be compared with the desired position Pd. General equation for the fitness function is given in Eq. (41) that is based on the distance norm of homogeneous Euclidian distance between the current positions to the desired position of end effector Pd evaluated by number of iterations.

$$P\_{\min} = \left\| P\_d - P\_c(i) \right\|^2 \tag{41}$$

Current position Pc can be evaluated from Eqs. (34) through (38). Now putting the value of Pc on Eq. (42)

$$P\_{\min} = \sqrt{\left(X\_d - X\_c(\mathbf{i})\right)^2 + \left(Y\_d - Y\_c(\mathbf{i})\right)^2 + \left(Z\_d - Z\_c(\mathbf{i})\right)^2} \tag{42}$$

#### 3.1.2. Joint angle error

Corresponding joint error can be given by the difference between current set of joint variables to the final required angles.

$$
\theta\_{\mathcal{C}} = (\theta\_{\mathcal{C}1}, \theta\_{\mathcal{C}2}, \theta\_{\mathcal{C}3}, \theta\_{\mathcal{C}4} \,\theta\_{\mathcal{C}5}) \tag{43}
$$

$$
\Theta\_d = (\theta\_{d1}, \theta\_{d2}, \theta\_{d3}, \theta\_{d4} \,\theta\_{d5}) \tag{44}
$$

Therefore using square norm the objective function can be given as

$$\Theta\_{\rm min} = \left\| \Theta\_d - \Theta\_c \right\|^2 \tag{45}$$

Subjected to joint limits

θ1 ∈½ � θ1min; θ1max θ2 ∈½ � θ2min; θ2max θ3 ∈½ � θ3min; θ3max θ4 ∈½ � θ4min; θ4max θ5 ∈½ � θ5min; θ5max

Now overall error minimization can be given by using Eqs. (42) and (45),

$$\begin{split} F(\mathbf{x})\_{\text{min}} &= \lambda \left[ \sqrt{\left( \mathbf{X}\_d - \mathbf{X}\_c(\mathbf{i}) \right)^2 + \left( \mathbf{Y}\_d - \mathbf{Y}\_c(\mathbf{i}) \right)^2 + \left( \mathbf{Z}\_d - \mathbf{Z}\_c(\mathbf{i}) \right)^2} \right] \\ &+ \left[ \left\| \boldsymbol{\Theta}\_d - \boldsymbol{\Theta}\_c \right\|^2 \right] \end{split} \tag{46}$$

This work does not use special tuning of various parameters of GA and TLBO algorithm. In future research the sensitivity analysis can be performed to achieve better results. From Table 3, TLBO generated solutions for the position 4 is better as compared to GA in account of fitness function evaluation. There are different distance based norms, one of them is Euclidean distance

Positions TLBO joint angles Function value

Positions GA Joint angles Function value θ1 θ2 θ3 θ4 θ5

P1(76.09, 54.36, 61.94) 86.598 72.165 72.165 40.894 30.459 0 P2(89.69, 192.55, 90.87) 83.874 69.895 69.895 39.607 30.76 0 P3(4.24, 94.08, 97.55) 84.512 70.427 70.427 39.909 30.686 0 P4(29.10,154.02, 31.52) 85.566 71.305 71.305 40.406 30.53 0 P5(184.33, 43.21, 8.27) 87.818 73.181 73.181 41.469 30.364 0

P1(76.09, 54.36, 61.94) 60.619 49.504 58.384 62.281 27.903 0 P2(89.69, 192.55, 90.87) 88.293 34.091 14.439 15.241 51.738 0 P3(4.24, 94.08, 97.55) 55.004 49.274 63.942 47.842 33.633 0 P4(29.10,154.02, 31.52) 72.594 22.689 68.297 85.886 27.044 0.0137 P5(184.33, 43.21, 8.27) 25.669 70.588 31.341 66.807 52.884 0

θ1 θ2 θ3 θ4 θ5

θ1 θ2 θ3 θ4 θ5 X YZ 1 112.5641 47.3165 8.2447 65.8373 39.8977 186.6903 183.0670 14.7039 2 153.1316 21.9812 126.9031 57.2629 30.4168 92.6981 32.1423 157.3316 3 66.1779 143.2985 14.6124 73.6231 41.5228 131.5420 22.3866 32.2155 4 57.6085 119.6396 104.5818 71.1946 33.8225 10.7684 111.7435 77.4862 5 31.4308 2.9242 71.5757 63.0358 39.8749 64.7966 172.0372 151.5714 6 124.3702 116.7337 102.4999 53.1482 22.4807 111.8590 59.6708 60.8590 7 89.1765 13.1827 101.6747 80.3340 29.4704 76.9533 96.2813 121.3505 8 5.6698 30.9685 57.2308 29.3079 29.6421 174.3873 107.6283 143.1839 9 131.5857 108.7086 92.8278 5.6664 36.4826 104.6410 109.7511 40.5523 10 32.8579 102.3539 138.8770 26.4141 33.7466 146.4984 48.7416 54.4041 11 134.1878 70.4224 26.3511 82.2471 44.0566 188.7864 15.4108 53.9823

Optimization Approach for Inverse Kinematic Solution http://dx.doi.org/10.5772/intechopen.71409 225

SN Position of joints determined through quaternion algebra

Table 2. Desired joint variables determined through quaternion algebra.

Table 3. TLBO results for joint variable and function value.

where λ is proportional weight factor for the minimization of the problem and calculation of the entire joint angles base on constraint can be achieved using fitness function (46). The performance of considered algorithm is checked with the parameters: a1 = 60 mm, a2 = 145 mm, d1 = 150 mm, d2 = 125 mm, d3 = 130 mm. Upper and lower limit of five joint angles are: θ1 = [0, 180]; θ2 = [0, 150]; θ3 = [0, 150], θ4 = [0, 85] and θ5 = [15, 45].

#### 4. Results and discussions

TLBO and GA has been used to compute the inverse kinematics of 5-R manipulator and comparison of obtained results has been made on the basis of quality and performance. Table 1 gives the five random position of end effector and respective inverse kinematics solutions. Current work is performed in MATLAB R2013a. The data sets are obtained from Eq. (34) through (38). The data sets are generated using quaternion vector based inverse kinematics equations as given in Table 2. These generated data sets are used to compare the IK solution through adopted GA and TLBO. In Table 3, comparative evaluations of fitness function and obtained joint variables through TLBO and GA is presented.

Table 1. Five different positions and joint variables.

Table 2. Desired joint variables determined through quaternion algebra.

θ1 ∈½ � θ1min; θ1max

θ2 ∈½ � θ2min; θ2max

θ3 ∈½ � θ3min; θ3max

θ4 ∈½ � θ4min; θ4max

θ5 ∈½ � θ5min; θ5max

where λ is proportional weight factor for the minimization of the problem and calculation of the entire joint angles base on constraint can be achieved using fitness function (46). The performance of considered algorithm is checked with the parameters: a1 = 60 mm, a2 = 145 mm, d1 = 150 mm, d2 = 125 mm, d3 = 130 mm. Upper and lower limit of five joint angles are: θ1 = [0, 180]; θ2 = [0, 150];

TLBO and GA has been used to compute the inverse kinematics of 5-R manipulator and comparison of obtained results has been made on the basis of quality and performance. Table 1 gives the five random position of end effector and respective inverse kinematics solutions. Current work is performed in MATLAB R2013a. The data sets are obtained from Eq. (34) through (38). The data sets are generated using quaternion vector based inverse kinematics equations as given in Table 2. These generated data sets are used to compare the IK solution through adopted GA and TLBO. In Table 3, comparative evaluations of fitness function and

P1(�76.09, 54.36, �61.94) 84.559 77.518 101.74 30.616 38.697 P2(89.69, 192.55, 90.87) 84.791 97.25 130.44 50.771 36.428 P3(�4.24, 94.08, 97.55) 18.384 78.688 35.234 77.708 34.889 P4(29.10,154.02, �31.52) 104.43 115.47 124.11 7.3372 33.774 P5(�184.33, �43.21, 8.27) 39.177 107.13 97.052 65.672 15.374

θ1 θ2 θ3 θ4 θ5

ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi Xd � Xc ð Þ ð Þi 2 þ Yd � Yc ð Þ ð Þi 2 þ Zd � Zc ð Þ ð Þi 2 � � q

(46)

Now overall error minimization can be given by using Eqs. (42) and (45),

þ k k θd � θc 2 h i

obtained joint variables through TLBO and GA is presented.

F xð Þmin ¼ λ

224 Kinematics

θ3 = [0, 150], θ4 = [0, 85] and θ5 = [15, 45].

Positions Joint angles

Table 1. Five different positions and joint variables.

4. Results and discussions

This work does not use special tuning of various parameters of GA and TLBO algorithm. In future research the sensitivity analysis can be performed to achieve better results. From Table 3, TLBO generated solutions for the position 4 is better as compared to GA in account of fitness function evaluation. There are different distance based norms, one of them is Euclidean distance

Table 3. TLBO results for joint variable and function value.

norm and which is used here for minimum distance between the end effector positions. If the distance between two points reached to 0 or less than 0.001 than the evolutions of fitness function can be reached best or global minimum value. It is clear that the obtained fitness value is less than the defined distance norm so adopting these algorithms are fruitful and qualitative.

Figures 3–7 signify the best fitness function value and analogous joint variables for position 1. These figures show efficiency of adopted algorithms for IK solution of 5-R manipulator. The convergence of objective function evaluation lies to zero error for GA and TLBO algorithms while for position 4, GA yields 0.013 error. It means that GA is less performing as compared to TLBO. From Figures 8–12, the results obtained through GA shows in terms of convergence and histogram graph and the obtained joint angles are in radian which is later converted into degree and given in Table 3. The GA results are obtained through MATLAB toolbox and that shows the zero convergence in single run. Figures 8–12, it can be seen that the generated solutions for joint angles are multiple for single position and similarly there are multiple fitness function evaluations. The best fitness function achieved here using the termination criteria and the corresponding joint variables has taken for comparison.

The proposed work is performed in dual core system with 4 GB RAM computer. It has been observe that the convergence of the solution for GA is taking less computation time as compared to TLBO and quaternion algebra. Corresponding joint angles trajectory using 4th order cubic spline is presented in Figure 13. Using inverse kinematic solution joint variables are used

Figure 3. Joint variables and fitness function value for position P1.

Figure 7. Joint variables and fitness function value for position P5.

Figure 8. Joint variables and fitness function value for position P1.

Figure 5. Joint variables and fitness function value for position P3.

Optimization Approach for Inverse Kinematic Solution http://dx.doi.org/10.5772/intechopen.71409 227

Figure 6. Joint variables and fitness function value for position P4.

Figure 4. Joint variables and fitness function value for position P2.

Figure 5. Joint variables and fitness function value for position P3.

norm and which is used here for minimum distance between the end effector positions. If the distance between two points reached to 0 or less than 0.001 than the evolutions of fitness function can be reached best or global minimum value. It is clear that the obtained fitness value is less

Figures 3–7 signify the best fitness function value and analogous joint variables for position 1. These figures show efficiency of adopted algorithms for IK solution of 5-R manipulator. The convergence of objective function evaluation lies to zero error for GA and TLBO algorithms while for position 4, GA yields 0.013 error. It means that GA is less performing as compared to TLBO. From Figures 8–12, the results obtained through GA shows in terms of convergence and histogram graph and the obtained joint angles are in radian which is later converted into degree and given in Table 3. The GA results are obtained through MATLAB toolbox and that shows the zero convergence in single run. Figures 8–12, it can be seen that the generated solutions for joint angles are multiple for single position and similarly there are multiple fitness function evaluations. The best fitness function achieved here using the termination criteria and

The proposed work is performed in dual core system with 4 GB RAM computer. It has been observe that the convergence of the solution for GA is taking less computation time as compared to TLBO and quaternion algebra. Corresponding joint angles trajectory using 4th order cubic spline is presented in Figure 13. Using inverse kinematic solution joint variables are used

than the defined distance norm so adopting these algorithms are fruitful and qualitative.

the corresponding joint variables has taken for comparison.

226 Kinematics

Figure 3. Joint variables and fitness function value for position P1.

Figure 4. Joint variables and fitness function value for position P2.

Figure 6. Joint variables and fitness function value for position P4.

Figure 7. Joint variables and fitness function value for position P5.

Figure 8. Joint variables and fitness function value for position P1.

Figure 9. Joint variables and fitness function value for position P2.

Figure 11. Joint variables and fitness function value for position P4.

to calculate the joint space trajectory for TLBO and GA as presented in Figures 14 and 15. Final time has been taken tf = 6 second for trajectory generation but to complete this trajectory overall computational time is 5.674 seconds. The computation time for TLBO is 15.671 seconds which is more than the GA i.e. 7.932 seconds. Therefore, on the basis of computational cost GA is

Optimization Approach for Inverse Kinematic Solution http://dx.doi.org/10.5772/intechopen.71409 229

performing better than TLBO while quaternion algebra taken least time (Table 4).

Figure 13. Trajectory of joint angle for quaternion.

Figure 14. Trajectory of joint angle for TLBO.

Figure 12. Joint variables and fitness function value for position P5.

Figure 13. Trajectory of joint angle for quaternion.

Figure 14. Trajectory of joint angle for TLBO.

Figure 12. Joint variables and fitness function value for position P5.

Figure 11. Joint variables and fitness function value for position P4.

Figure 9. Joint variables and fitness function value for position P2.

228 Kinematics

Figure 10. Joint variables and fitness function value for position P3.

to calculate the joint space trajectory for TLBO and GA as presented in Figures 14 and 15. Final time has been taken tf = 6 second for trajectory generation but to complete this trajectory overall computational time is 5.674 seconds. The computation time for TLBO is 15.671 seconds which is more than the GA i.e. 7.932 seconds. Therefore, on the basis of computational cost GA is performing better than TLBO while quaternion algebra taken least time (Table 4).

quaternion algebra. Table 3 gives the comparative results of adopted algorithm and proposed quaternion solutions of 5-R manipulator. This work considered forward and inverse kinematic equations for preparing the objective function for TLBO and GA. These adopted algorithms has shown the potential of getting faster convergence and yielding global optimum solution for the stated problem. In future the tuning of various parameters of GA and TLBO can be considered so as to avoid trapping in local minimum point. Even the hybridization of these

Optimization Approach for Inverse Kinematic Solution http://dx.doi.org/10.5772/intechopen.71409 231

[1] Ahuactzin J, Gupta K. Completeness Results for a Point-to-Point Inverse Kinematics

[2] Albert F, Koh S, Tiong S, Chen C. Inverse Kinematic Solution in Handling 3R Manipula-

[3] Ayyıldız M, Çetinkaya K. Comparison of four different heuristic optimization algorithms for the inverse kinematics solution of a real 4-DOF serial robot manipulator. Neural

[4] Chapelle F, Bidaud P. A Closed Form for Inverse Kinematics Approximation of General

[5] Chen C, Her M, Hung Y, Karkoub M. Approximating a robot inverse kinematics solution using fuzzy logic tuned by genetic algorithms. The International Journal of Advanced

[6] Dulęba I, Opałka M. A comparison of Jacobian-based methods of inverse kinematics for serial robot manipulators. International Journal of Applied Mathematics and Computer

[7] Eiben A, Smith J. Introduction to Evolutionary Computing. New York: Springer; 2003

[8] Funda J, Taylor R, Paul R. On homogeneous transforms, quaternions, and computational efficiency. IEEE Transactions on Robotics and Automation. 1990;6:382-388. DOI: 10.1109/

Computing and Applications. 2015. DOI: 10.1007/s00521-015-1898-8

Manufacturing Technology. 2002;20:375-380. DOI: 10.1007/s001700200166

6R Manipulators Using Genetic Programming. 2001;4:3364-3369

algorithms may be proposed and adopt for the IK problems.

\* and Bibhuti Bhusan Biswal2

\*Address all correspondence to<EMAIL_ADDRESS>
Algorithm. Detroit, MI: IEEE. Vol. 2. p. 1526-1531

tor via Real-Time Genetic Algorithm. IEEE. p. 1-6

Science. 2013. DOI: 10.2478/amcs-2013-0028

1 Raghu Engineering College, Visakhapatnam, India 2 National Institute of Technology, Meghalaya, India

Author details

Panchanand Jha1

References

70.56658

Figure 15. Trajectory of joint angle for GA.

Table 4. Computational time for inverse kinematic evaluations.

#### 5. Conclusions

In this paper, the work discourses the problem associated to the optimization of positional and angular error of end effector using TLBO and GA for 5R robot manipulator. Metaheuristic algorithms like PSO, GA, ABC, etc. have been used in various field of industrial robotics but the most critical issue is to solve inverse kinematic problem for any configuration of robot manipulators. Most of the optimization approach are being used for numerical solution but it has been observed that the numerical solutions does not yield solution when the manipulator is in ill-conditioned besides this it has also been observed that classical optimization methods converge in local minima. Therefore in this work global optimization method like TLBO and GA is adopted and after analyzing the results it can be concluded that adopted optimization algorithms convergence rate is higher and complexity does not increase with the manipulator configuration. Although many researchers are tried to obtain global solution but the computations cost are more in the problem henceforth overcoming the problem of computational cost with quaternion objective function.

The adopted algorithms are very much appropriate for constrained and unconstrained problems. To estimate the effectiveness of considered algorithms, comparison has been made with quaternion algebra. Table 3 gives the comparative results of adopted algorithm and proposed quaternion solutions of 5-R manipulator. This work considered forward and inverse kinematic equations for preparing the objective function for TLBO and GA. These adopted algorithms has shown the potential of getting faster convergence and yielding global optimum solution for the stated problem. In future the tuning of various parameters of GA and TLBO can be considered so as to avoid trapping in local minimum point. Even the hybridization of these algorithms may be proposed and adopt for the IK problems.

### Author details

Panchanand Jha1 \* and Bibhuti Bhusan Biswal2

#### References

5. Conclusions

230 Kinematics

Figure 15. Trajectory of joint angle for GA.

Table 4. Computational time for inverse kinematic evaluations.

In this paper, the work discourses the problem associated to the optimization of positional and angular error of end effector using TLBO and GA for 5R robot manipulator. Metaheuristic algorithms like PSO, GA, ABC, etc. have been used in various field of industrial robotics but the most critical issue is to solve inverse kinematic problem for any configuration of robot manipulators. Most of the optimization approach are being used for numerical solution but it has been observed that the numerical solutions does not yield solution when the manipulator is in ill-conditioned besides this it has also been observed that classical optimization methods converge in local minima. Therefore in this work global optimization method like TLBO and GA is adopted and after analyzing the results it can be concluded that adopted optimization algorithms convergence rate is higher and complexity does not increase with the manipulator configuration. Although many researchers are tried to obtain global solution but the computations cost are more in the problem henceforth overcoming the problem of computational cost with quaternion objective function.

SN Method Computational time

1 TLBO 15.671 s 2 GA 7.932 s 3 Quaternion 0.993 s

The adopted algorithms are very much appropriate for constrained and unconstrained problems. To estimate the effectiveness of considered algorithms, comparison has been made with

[9] Galicki M. Control-based solution to inverse kinematics for mobile manipulators using penalty functions. Journal of Intelligent and Robotic Systems. 2005;42:213-238. DOI: 10.1007/s10846-004-7196-9

[23] Parker J, Khoogar A, Goldberg D 1989, Inverse kinematics of redundant robots using genetic algorithms. Proceedings of IEEE International Conference on Robotics and Automation [24] Pham D, Castellani M, Fahmy A. Learning the Inverse Kinematics of a Robot Manipula-

Optimization Approach for Inverse Kinematic Solution http://dx.doi.org/10.5772/intechopen.71409 233

[25] Piazzi A, Visioli A. A Global Optimization Approach to Trajectory Planning for Industrial

[26] Rajpar A, Zhang W, Jia D. Object Manipulation of Humanoid Robot Based on Combined

[27] Rao R, Savsani V, Vakharia D. Teaching–learning-based optimization: A novel method for constrained mechanical design optimization problems. Computer-Aided Design.

[28] Rocke D, Michalewicz Z. Genetic algorithms + data structures = evolution programs. Journal of the American Statistical Association. 2000;95:347. DOI: 10.2307/2669583 [29] Sun L, Lee R, Lu W, Luk L. Modelling and simulation of the intervertebral movements of the lumbar spine using an inverse kinematic algorithm. Medical & Biological Engineering

[30] Tabandeh S, Clark C, Melek W 2006, A Genetic Algorithm Approach to solve for Multiple Solutions of Inverse Kinematics using Adaptive Niching and Clustering. In: IEEE, pp.

[31] Vrongistinos K, Wang Y, Hwang Y. Quaternion smoothing on three-dimensional kinematics data. Medicine & Science in Sports & Exercise. 2001;33:S84. DOI: 10.1097/00005768-200105001-

[32] Wang L, Chen C. A combined optimization method for solving the inverse kinematics problems of mechanical manipulators. IEEE Transactions on Robotics and Automation.

[33] Wolpert D, Macready W. No free lunch theorems for optimization. IEEE Transactions on

[34] Xu D, Acosta Calderon C, Gan J. An analysis of the inverse kinematics for a 5-DOF manipulator. International Journal of Automation and Computing. 2005;2:114-124. DOI:

[35] Zoric N, Lazarevic M, Simonovic A. Multi-body kinematics and dynamics in terms of quaternions: Langrange formulation in covariant form – Rodriguez approach. FME

[36] Zu D. Efficient inverse kinematic solution for redundant manipulators. JME. 2005;41:71.

Evolutionary Computation. 1997;1:67-82. DOI: 10.1109/4235.585893

tor using the Bees Algorithm. Daejeon: IEEE; 2008. p. 493-498

Optimization Approach. Harbin: IEEE; 2007. p. 1148-1153

& Computing. 2004;42:740-746. DOI: 10.1007/bf02345206

Robots. Grenoble: IEEE. 1997;3:1553-1559

2011;43:303-315. DOI: 10.1016/j.cad.2010.12.015

1991;7:489-499. DOI: 10.1109/70.86079

10.1007/s11633-005-0114-1

Transactions. 2010;38:19-28

DOI: 10.3901/jme.2005.06.071

1815–1822

00480

[23] Parker J, Khoogar A, Goldberg D 1989, Inverse kinematics of redundant robots using genetic algorithms. Proceedings of IEEE International Conference on Robotics and Automation

[9] Galicki M. Control-based solution to inverse kinematics for mobile manipulators using penalty functions. Journal of Intelligent and Robotic Systems. 2005;42:213-238. DOI:

[10] Gan D, Liao Q, Wei S. Dual quaternion-based inverse kinematics of the general spatial 7R mechanism. Proceedings of the Institution of Mechanical Engineers, Part C: Journal of Mechanical Engineering Science. 2008;222:1593-1598. DOI: 10.1243/09544062jmes1082 [11] Geradin M, Cardona A. Kinematics and dynamics of rigid and flexible mechanisms using finite elements and quaternion algebra. Computational Mechanics. 1988;4:115-135. DOI:

[12] He G, Hongming G, Zhang G, Wu L. Using Adaptive Genetic Algorithm to the Placement

[13] Huang H, Xu S, Hsu H. Hybrid Taguchi DNA swarm intelligence for optimal inverse kinematics redundancy resolution of six-DOF humanoid robot arms. Mathematical Prob-

[14] Husty M, Pfurner M, Schröcker H. A new and efficient algorithm for the inverse kinematics of a general serial 6R manipulator. Mechanism and Machine Theory. 2007;42:66-

[15] Kalra P, Mahapatra P, Aggarwal D. On the Solution of Multimodal Robot Inverse Kinematic Functions using Real-coded Genetic Algorithms. IEEE. 2003;2:1840-1845

[16] Kennedy J, Eberhart R, Shi Y. Swarm Intelligence. San Francisco: Morgan Kaufmann

[17] Khatami S, Sassani F. Isotropic Robotic Design Optimization of Manipulators Using a

[18] Kim S, Kim J. Optimal Trajectory Planning of a Redundant Manipulator Using Evolu-

[19] Korein B. Techniques for generating the goal-directed motion of articulated structures. IEEE Computer Graphics and Applications. 1982;2:71-81. DOI: 10.1109/mcg.1982.1674498

[20] Kucuk S, Bingul Z. Inverse kinematics solutions for industrial robot manipulators with offset wrists. Applied Mathematical Modelling. 2014;38:1983-1999. DOI: 10.1016/j.apm.2013.10.014

[21] Liu S, Zhu S. An optimized real time algorithm for the inverse kinematics of general 6R robots. In: Control and Automation, 2007. ICCA 2007. IEEE International Conference on.

[22] Nearchou A. Solving the inverse kinematics problem of redundant robots operating in complex environments via a modified genetic algorithm. Mechanism and Machine

of Serial Robot Manipulator. Islamabad: IEEE; 2006. p. 1-6

lems in Engineering. 2014;2014:1-9. DOI: 10.1155/2014/358269

Genetic Algorithm Method. Vancouver: IEEE; 2002. p. 562-567

Theory. 1998;33:273-292. DOI: 10.1016/s0094-114x(97)00034-7

tionary Programming. Nagoya: IEEE; 1996. p. 738-743

IEEE, Guangzhou. 2007. pp. 2080–2084

81. DOI: 10.1016/j.mechmachtheory.2006.02.001

10.1007/s10846-004-7196-9

232 Kinematics

10.1007/bf00282414

Publishers; 2001

**Chapter 12**

Provisional chapter

**A Random Multi-Trajectory Generation Method for**

A Random Multi-Trajectory Generation Method for

**Application in Path Planning Algorithm)**

Online Emergency Threat Management

Liang Yang, Yuqing He, Jizhong Xiao, Bing Li and

Additional information is available at the end of the chapter

Additional information is available at the end of the chapter

Liang Yang, Yuqing He, Jizhong Xiao, Bing Li

http://dx.doi.org/10.5772/intechopen.71410

time and mission performance.

Zhaoming Liu

and Zhaoming Liu

Abstract

1. Introduction

**Online Emergency Threat Management (Analysis and**

DOI: 10.5772/intechopen.71410

(Analysis and Application in Path Planning Algorithm)

This paper presents a novel randomized path planning algorithm, which is a goal and homology biased sampling based algorithm called Multiple Guiding Attraction based Random Tree, and robots can use it to tackle pop-up and moving threats under kinodynamic constraints. Our proposed method considers the kinematics and dynamics constraints, using obstacle information to perform informed sampling and redistribution around collision region toward valid routing. We pioneeringly propose a multiple path planning method using 'Extending Forbidden' algorithm, rather than using variant cost principles for online threat management. The threat management method performs online path switching between the planned multiple paths, which is proved with better time performance than conventional approaches. The proposed method has advantage in exploration in obstacle crowded environment, where narrow corridor fails using the general sampling based exploration methods. We perform detailed comparative experiments with peer approaches in cluttered environment, and point out the advantages in

Keywords: multiple path planning, online emergency threat management, path switching, goal biased probability, sampling based algorithm

Robot path planning have been witnessed a great achievement these years with the various application of robots [1, 2]. Problems such as path planning, motion planning, and online moving obstacle management have been widely studied toward the goal of performing autonomy. Unmanned Aerial Vehicles (UAVs), an easy access robot platform, has been increasingly

> © The Author(s). Licensee InTech. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and eproduction in any medium, provided the original work is properly cited.

© 2017 The Author(s). Licensee InTech. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

Provisional chapter

#### **A Random Multi-Trajectory Generation Method for Online Emergency Threat Management (Analysis and Application in Path Planning Algorithm)** A Random Multi-Trajectory Generation Method for Online Emergency Threat Management

DOI: 10.5772/intechopen.71410

(Analysis and Application in Path Planning Algorithm)

Liang Yang, Yuqing He, Jizhong Xiao, Bing Li and Zhaoming Liu Liang Yang, Yuqing He, Jizhong Xiao, Bing Li

Additional information is available at the end of the chapter and Zhaoming Liu

http://dx.doi.org/10.5772/intechopen.71410 Additional information is available at the end of the chapter

#### Abstract

This paper presents a novel randomized path planning algorithm, which is a goal and homology biased sampling based algorithm called Multiple Guiding Attraction based Random Tree, and robots can use it to tackle pop-up and moving threats under kinodynamic constraints. Our proposed method considers the kinematics and dynamics constraints, using obstacle information to perform informed sampling and redistribution around collision region toward valid routing. We pioneeringly propose a multiple path planning method using 'Extending Forbidden' algorithm, rather than using variant cost principles for online threat management. The threat management method performs online path switching between the planned multiple paths, which is proved with better time performance than conventional approaches. The proposed method has advantage in exploration in obstacle crowded environment, where narrow corridor fails using the general sampling based exploration methods. We perform detailed comparative experiments with peer approaches in cluttered environment, and point out the advantages in time and mission performance.

Keywords: multiple path planning, online emergency threat management, path switching, goal biased probability, sampling based algorithm

#### 1. Introduction

Robot path planning have been witnessed a great achievement these years with the various application of robots [1, 2]. Problems such as path planning, motion planning, and online moving obstacle management have been widely studied toward the goal of performing autonomy. Unmanned Aerial Vehicles (UAVs), an easy access robot platform, has been increasingly

© 2017 The Author(s). Licensee InTech. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

© The Author(s). Licensee InTech. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and eproduction in any medium, provided the original work is properly cited.

applied in research and commercial areas in recent years. UAV autonomy denotes the ability of tackling with obstacle (or called no-fly zone) avoidance and trajectory planning online from a starting position to a destination while satisfying the kinematic constraints [3].

biased is called Multi-GART (MGART). We consider the environment to be known as a priori to us, and the UAVs are with the ability to understand the clearance region. Experiments and comparative simulations are illustrated to provide the effective evaluation of the proposed

A Random Multi-Trajectory Generation Method for Online Emergency Threat Management…

http://dx.doi.org/10.5772/intechopen.71410

237

For path planner, the purpose is to find a feasible path pf (cost minimum or complete) from the initial position to the goal position in the workspace W∈ R{n}, n denotes the dimension of space

The cenergy, ctime, cthreat denote energy cost, time consumption, and threat respectively. These costs are not fixed, since the energy cost can be path length, and time consumption can change according to the velocity limitation. For cost constrained path planner, the goal is to find the asymptotic optimal rather than the completeness solution. Then, more than one path can be found during the process, and the paths can be homotopic or belong to different homotopic

It is illustrated in Figure 1. Given a continuous map H : I � I ! Γ or H : h(s, t) = ht(s), Γ denotes the topological space and I = [0, 1] is the unit interval. The obstacle regions are labeled with Ro1, xinitial ¼ hð Þ 0; t denotes start point, xinter ¼ hð Þ 1; t denotes the goal position, xinter denotes an inter node for obstacle avoidance. For the continuous deformation, given h(s, 0) =π0, h(s, 1) =π1, the path can be continuously mapping through π0 to π1 with t∈[0, 1]. For any path deformed between, they are homotopic with π0, π1 if and only if they stay in the closed loop π0∐ � π1,

Definition 1—Homotopic Paths: It denotes the equivalence class of all paths under continuous mapping H : h(s, t) = ht(s), which locates in the closed loop formed h(s, 0)∐ � h(s, 1). Any

cenergy þ ctime þ cthreatdt (1)

the robots locate. A general cost function can be represented as:

where the closed loop cannot collide with any obstacle region.

Figure 1. Homotopic and homologous classes and paths.

J ¼ ð

methods.

2. Preliminary materials

2.1. Homology and homotopy

classes (or called homology).

For robot path planning, emergency threat management (ETM) is one of the hardest challenges that needs to be solved, where a sudden threat may burst into view or dynamic obstacles are detected on line, especially when UAV is following the desired path. Under such conditions, UAV should consider the following attributes:

Development with open robot platform [7] and field implementation [8] has witnessed the promising performance of Sampling Based (SB) methods. SB algorithms (SBA) have the advantages for planning in high dimensional space, and it is with the ability to deal with multiple classes of path or motion planning problem in both static and dynamic environment [9]. Rapidly-exploring random trees (RRTs) are single query methods which obtain Voronoi biased property and only generate homotopy paths simultaneously [12]. Although it proposes to solve the multiple degrees of freedom (DOF) operating problems in known static environments [10, 11], SBA shows great performance of dealing with any kind of path or motion planning problem in complex environments for unmanned ground robots or aerial robots.

In this paper, we introduce two biased-sampling methods, which are obstacle biased and Homologous Classes (HC) biased to perform path planning respectively. For obstacle biased path method, we have discussed in [13] with UAV demonstration. For HC classed biased approach, it aims at solving the ET problem by generating alternative paths for online dynamically switching. HC introduces an online dynamic reconfiguration approach to ensure singularity between paths, which tries to generate more paths with different obstacle reference. Thus, it can perform alternative switching online when confronted with ET. The obstacle biased planning method is called Guiding Attraction based Random Tree (GART) and HC biased is called Multi-GART (MGART). We consider the environment to be known as a priori to us, and the UAVs are with the ability to understand the clearance region. Experiments and comparative simulations are illustrated to provide the effective evaluation of the proposed methods.

#### 2. Preliminary materials

applied in research and commercial areas in recent years. UAV autonomy denotes the ability of tackling with obstacle (or called no-fly zone) avoidance and trajectory planning online from a

For robot path planning, emergency threat management (ETM) is one of the hardest challenges that needs to be solved, where a sudden threat may burst into view or dynamic obstacles are detected on line, especially when UAV is following the desired path. Under such conditions,

1. Time efficiency: The most important requirement for ETM algorithm is time efficiency. For general ETM, the configuration is periodically updated, such as heuristic algorithm A\* [4], which it is computationally intensive if the map is represented with high resolution. In

2. Kinematic feasibility: Kinematic feasibility denotes that the output of the planner meets the kinematic constraints of the robot as well as the environment. The constraints include: (a) Path smoothness: The planner is required to output kinematic smooth path, sometimes even kinodynamically feasible as well. Thus, the path should meet the state of art tracking constraints, and enables low tracking error for UAV; (b) Minimum cost of switching: The strategy of handling the threat, especially ET, is to find the cost minimum path by generating a new path or multiple paths besides the initial one. The cost for choosing the best path should take the dynamic constraints, energy consumption and time performance into consideration. 3. Specific requirements: UAVs have already been applied to many areas, such as inspection, photography, and monitoring. They have to meet some specific requirements according to environments and system constraints. For example, best pose based illumination of tunnel inspection for crack and spalling [5], and stable tracking with obstacle avoidance as UAV photography [6] which should be able to keep stable capturing even during the flying. Development with open robot platform [7] and field implementation [8] has witnessed the promising performance of Sampling Based (SB) methods. SB algorithms (SBA) have the advantages for planning in high dimensional space, and it is with the ability to deal with multiple classes of path or motion planning problem in both static and dynamic environment [9]. Rapidly-exploring random trees (RRTs) are single query methods which obtain Voronoi biased property and only generate homotopy paths simultaneously [12]. Although it proposes to solve the multiple degrees of freedom (DOF) operating problems in known static environments [10, 11], SBA shows great performance of dealing with any kind of path or motion planning problem in complex environments for unmanned ground robots or aerial robots.

In this paper, we introduce two biased-sampling methods, which are obstacle biased and Homologous Classes (HC) biased to perform path planning respectively. For obstacle biased path method, we have discussed in [13] with UAV demonstration. For HC classed biased approach, it aims at solving the ET problem by generating alternative paths for online dynamically switching. HC introduces an online dynamic reconfiguration approach to ensure singularity between paths, which tries to generate more paths with different obstacle reference. Thus, it can perform alternative switching online when confronted with ET. The obstacle biased planning method is called Guiding Attraction based Random Tree (GART) and HC

starting position to a destination while satisfying the kinematic constraints [3].

order to guarantee safety, ETM requires real-time performance.

UAV should consider the following attributes:

236 Kinematics

#### 2.1. Homology and homotopy

For path planner, the purpose is to find a feasible path pf (cost minimum or complete) from the initial position to the goal position in the workspace W∈ R{n}, n denotes the dimension of space the robots locate. A general cost function can be represented as:

$$J = \int c\_{energy} + c\_{time} + c\_{threat}dt\tag{1}$$

The cenergy, ctime, cthreat denote energy cost, time consumption, and threat respectively. These costs are not fixed, since the energy cost can be path length, and time consumption can change according to the velocity limitation. For cost constrained path planner, the goal is to find the asymptotic optimal rather than the completeness solution. Then, more than one path can be found during the process, and the paths can be homotopic or belong to different homotopic classes (or called homology).

It is illustrated in Figure 1. Given a continuous map H : I � I ! Γ or H : h(s, t) = ht(s), Γ denotes the topological space and I = [0, 1] is the unit interval. The obstacle regions are labeled with Ro1, xinitial ¼ hð Þ 0; t denotes start point, xinter ¼ hð Þ 1; t denotes the goal position, xinter denotes an inter node for obstacle avoidance. For the continuous deformation, given h(s, 0) =π0, h(s, 1) =π1, the path can be continuously mapping through π0 to π1 with t∈[0, 1]. For any path deformed between, they are homotopic with π0, π1 if and only if they stay in the closed loop π0∐ � π1, where the closed loop cannot collide with any obstacle region.

Definition 1—Homotopic Paths: It denotes the equivalence class of all paths under continuous mapping H : h(s, t) = ht(s), which locates in the closed loop formed h(s, 0)∐ � h(s, 1). Any

Figure 1. Homotopic and homologous classes and paths.

path in the set can be continuously deformed into any other without colliding with any obstacle in the space. For all paths in the set, they are with the same fixed end points.

We can conclude that π2 and π3 belong to the same homotopic class. However, we can find path π4, which shares the same start and ending node, cannot be continuously deformed to π3 due to the isolation of the obstacle. It means (π3 ∪ � π4) ∩Ro3 6¼ Θ. In such case, we call π3 and π4 are homologous, and they belong to different homotopic classes.

Definition 2—Homologous Paths: Paths, which follows the same continuous mapping H : h (s, t) = ht(s), cannot form a closed loop by h(s, 0) ∐ � h(s, 1). The homologous paths belong to different homotopic classes.

#### 2.2. Problem statement

Path planning follows a common procedure to perform trial and error process under empirical constraint to achieve completeness. The problem of path planning does not only solve a problem for exploration optimization, but also try to model the environment with a best descriptor as discussed in [13]. Let us take a look again with the problem of path planning which can be represented as:

$$\mathbf{H} = \{ \mathbf{h}(\mathbf{s}) | \mathbf{x}\_{\text{initial}} = h(\mathbf{0}), \mathbf{x}\_{\text{initial}} = h(\mathbf{1}), \mathbf{s} \in [0, 1] \}\tag{2}$$

3. Rapidly exploring random tree path planner

connection under minimum Euclidean metric,

Where gi is an element of all valid states set GT.

3.1. RRT connection with kinematic constraints

For RRT planner, given a system with state x\_

In this section, we try to describe the underlying research of rapidly-exploring random tree (RRT [12], upon which we propose a novel state of art approach to facilitate the active exploration in cluttered environments). SBAs are incremental search algorithms which perform random sampling with collision checking for extension, and they were first proposed to solve high dimension planning problem. They have the merits of considering the control and kinematics constraints together, and can incrementally construct a tree from the start node (or initial state) to the goal node (or goal state) with continuously sampling and expansion.

A Random Multi-Trajectory Generation Method for Online Emergency Threat Management…

It is shown Figure 2, the whole tree graph by exploration is represented as GT, the black solid dot denotes the valid state within step accessibility under kinematics constraints, and the black solid lines connect each parent state with child state for extension. Every step, a new sample gsample will be generated randomly. It should be cleared to all that the initial random sampling does not mean a fixed connection, that is, the random sampling can be a direction for extending. Then, the random sample gsample tries to find the nearest state in the tree for

gi ∈ GT

x ; x\_

y xx; uy � �

θð Þ θ; uθ

E gi

; gsample � � (5)

http://dx.doi.org/10.5772/intechopen.71410

239

(6)

y ; θ\_ � �, and a general form of system model:

gparent ¼ sup min

X\_ x ¼ f x ð Þ xx; ux

8 ><

>:

x\_ y ¼ f

θ\_ ¼ f

It can extend with simply random sampling with control inputs [ux, uy, uθ]. The random sample has to follow the kinematics constraints. Given the robot system, the differential

Figure 2. RRTs propagate by applying the minimal cost criterion to connect the newly sampled guard to the previous tree.

The path h(s) (homologous) should stay in obstacle free region Rfree, that is, h(s) ∈ Rfree. Usually, the path is piecewise continuously, and it can also be smoothed to obtain first order continuous thus to ensure kinematics continuous [14]. Besides the exploration to achieve completeness (in Eq. (1)), the obstacle modeling method is also important and affect the planning results.

To solve this problem, this paper proposed a multi-path online switching approach, that is, the path planner can find alternative homologous-paths. Then, this paper designs an online fast switching strategy. For multiple path planner, it aims at finding as many paths as possible,

$$\mathbf{H}\_{\text{alter}} = \cup\_{i=1} h^i(\mathbf{x}(t), \boldsymbol{\mu}(t)) \tag{3}$$

Halter denotes the set of all the alternative paths hi (x(t), u(t)), x(t) denotes the state, and u(t) denotes the control. However, the mission planner cannot use all the planed paths for online switching, it should find the reasonable paths without redundancy. We propose the follow rule,

$$\mathbf{H}\_{\text{reason}} = \left\{ h^i \middle| h\_i \underset{\neq}{H} \ h\_{\text{'}} \,\forall \text{'} \neq \text{j} \right\} \tag{4}$$

Hreason denotes the paths set where any two paths are not homotopic to each other, H6¼ denotes non-homotopy. Now, we have the paths which keep distinguishable from each other with different obstacles sequence surrounding.

#### 3. Rapidly exploring random tree path planner

path in the set can be continuously deformed into any other without colliding with any

We can conclude that π2 and π3 belong to the same homotopic class. However, we can find path π4, which shares the same start and ending node, cannot be continuously deformed to π3 due to the isolation of the obstacle. It means (π3 ∪ � π4) ∩Ro3 6¼ Θ. In such case, we call π3 and

Definition 2—Homologous Paths: Paths, which follows the same continuous mapping H : h (s, t) = ht(s), cannot form a closed loop by h(s, 0) ∐ � h(s, 1). The homologous paths belong to

Path planning follows a common procedure to perform trial and error process under empirical constraint to achieve completeness. The problem of path planning does not only solve a problem for exploration optimization, but also try to model the environment with a best descriptor as discussed in [13]. Let us take a look again with the problem of path planning

The path h(s) (homologous) should stay in obstacle free region Rfree, that is, h(s) ∈ Rfree. Usually, the path is piecewise continuously, and it can also be smoothed to obtain first order continuous thus to ensure kinematics continuous [14]. Besides the exploration to achieve completeness (in Eq. (1)), the obstacle modeling method is also important and affect the

To solve this problem, this paper proposed a multi-path online switching approach, that is, the path planner can find alternative homologous-paths. Then, this paper designs an online fast switching strategy. For multiple path planner, it aims at finding as many paths as

denotes the control. However, the mission planner cannot use all the planed paths for online switching, it should find the reasonable paths without redundancy. We propose the follow

hi H

non-homotopy. Now, we have the paths which keep distinguishable from each other with

Halter ¼ ∪i¼1hi

Hreason ¼ hi

Hreason denotes the paths set where any two paths are not homotopic to each other, H

Halter denotes the set of all the alternative paths hi

different obstacles sequence surrounding.

H ¼ f g h sð Þjxinitial ¼ hð Þ0 , xinitial ¼ hð Þ1 , s∈ ½ � 0; 1 (2)

ð Þ x tð Þ; u tð Þ (3)

6¼ hj, ∇i 6¼ j g (4)

6¼ denotes

(x(t), u(t)), x(t) denotes the state, and u(t)

obstacle in the space. For all paths in the set, they are with the same fixed end points.

π4 are homologous, and they belong to different homotopic classes.

different homotopic classes.

which can be represented as:

planning results.

possible,

rule,

2.2. Problem statement

238 Kinematics

In this section, we try to describe the underlying research of rapidly-exploring random tree (RRT [12], upon which we propose a novel state of art approach to facilitate the active exploration in cluttered environments). SBAs are incremental search algorithms which perform random sampling with collision checking for extension, and they were first proposed to solve high dimension planning problem. They have the merits of considering the control and kinematics constraints together, and can incrementally construct a tree from the start node (or initial state) to the goal node (or goal state) with continuously sampling and expansion.

It is shown Figure 2, the whole tree graph by exploration is represented as GT, the black solid dot denotes the valid state within step accessibility under kinematics constraints, and the black solid lines connect each parent state with child state for extension. Every step, a new sample gsample will be generated randomly. It should be cleared to all that the initial random sampling does not mean a fixed connection, that is, the random sampling can be a direction for extending. Then, the random sample gsample tries to find the nearest state in the tree for connection under minimum Euclidean metric,

$$\mathbf{g}\_{\text{parent}} = \sup\_{\mathcal{g}\_i \in G\_T} \min\_{\mathcal{G}\_T} \mathbb{E}\left(\mathcal{g}\_i, \mathcal{g}\_{sample}\right) \tag{5}$$

Where gi is an element of all valid states set GT.

#### 3.1. RRT connection with kinematic constraints

For RRT planner, given a system with state x\_ x ; x\_ y ; θ\_ � �, and a general form of system model:

$$\begin{cases} \dot{\mathbf{X}}\_{\mathbf{x}} = f^{\mathbf{x}}(\mathbf{x}\_{\mathbf{x}}, \boldsymbol{u}\_{\mathbf{x}}) \\ \dot{\mathbf{x}}\_{\mathbf{y}} = f^{\mathbf{y}}(\mathbf{x}\_{\mathbf{x}}, \boldsymbol{u}\_{\mathbf{y}}) \\ \dot{\boldsymbol{\Theta}} = f^{\boldsymbol{\Theta}}(\boldsymbol{\Theta}, \boldsymbol{u}\_{\boldsymbol{\Theta}}) \end{cases} \tag{6}$$

It can extend with simply random sampling with control inputs [ux, uy, uθ]. The random sample has to follow the kinematics constraints. Given the robot system, the differential

Figure 2. RRTs propagate by applying the minimal cost criterion to connect the newly sampled guard to the previous tree.

constraints can be represented as a set of implicit equations as g xð Þ¼ ; x\_ 0, and it can be further represented as:

$$
\dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}, \mathbf{u}) \tag{7}
$$

same for heuristic informed RRT [16]. Here, unlike the dominant region of a point, RRT branch can be also treated as a distinct individual with its own Voronoi region for acquiring the

A Random Multi-Trajectory Generation Method for Online Emergency Threat Management…

http://dx.doi.org/10.5772/intechopen.71410

241

Figure 3. Results of incremental exploration of RRT and hRRT [16] after 200 and 1000 iterations, respectively.

In this Section, we propose approaches to solve two main problems, which are handling cluttered environment and online ET processing, using obstacle-biased method and homologybiased method. Collision detection during the incremental exploration is time consuming, and it follows a routine procedure to guarantee safety. It should be noted that the step validation of

SBAs mostly deploy the general idea of generating random samples for incremental exploration, and the sample locating in obstacle region will be discarded since it is time consuming and no benefits for increasing the performance of exploring. We firstly deployed a simple idea

This paper introduces an obstacle biased algorithm, using obstacle information to help gener-

locates in obstacle region. To use the obstacle information, this paper proposes an active

1 performs a collision checking, and find the nearest nodes o

xa � xs

xb. We define that the further the obstacle to the sample, the more attraction it can support, that is, the attraction is proportional to the distance between obstacle and the sample using L2-

1Þ þ o

xb � xs

1Þ (11)

1, xs 1 tries

2

1 leads toward the obstacle region, xs

each new sampling state provides the directional information of obstacle distribution.

which was proved to have much higher time performance then RRT and RRT\* in [17].

ating more samples for connection. It is shown in Figure 4, the newly sampled states xs

norm L2. The sample then re-allocation by add a obstacle biased attraction as:

1 þ k o

Where k is a constant to adjust the shifting percentage of the attraction vector.

4. Obstacle and homology biased path planner

4.1. Obstacle biased path planner under kinematic constraints

to connect to the nearest state in the tree. However, xs

For outer attraction, new sample xs

xa, o

exploring method, that is, inner propulsion and outer attraction.

xs∗ 1 ¼ xs

extending resource.

Here, x denotes the state, and u∈ U denotes the valid control in allowable controls set. Given the parent state gparent(t), the time step follows a Δt limits. Then, the control inputs vary with u = {u(t0 )| t ≤ t 0 ≤ t +Δt}. To compute x(t +Δt), we can follow a typical procedure as [12]. It should be noted that the planner should extend toward the newly sampled gsample. The planner first computes the possible region of reachability from current state x(t):

$$\mathbf{x}(\mathbf{t} + \Delta \mathbf{t}) \in [\mathbf{x}(\mathbf{t}) + \mathbf{f}(\mathbf{x}(\mathbf{t}), \mathbf{u}(\mathbf{t}) - \Delta \mathbf{t} \cdot \mathbf{e}), \mathbf{x}(\mathbf{t}) + \mathbf{f}(\mathbf{x}(\mathbf{t}), \mathbf{u}(\mathbf{t}) + \Delta \mathbf{t} \cdot \mathbf{e})] \tag{8}$$

where e is the maximum first order factor of control input. RRT now picks a new state along the direction from parent to new sample, that is, gnew ∈[x(t) + f(x(t), u(t) � Δt ∙ e), x(t) + f(x(t), u (t) +Δt ∙ e)] and gnew = gparent + δ(gsample � gparent) with δ∈[0, 1].

#### 3.2. Voronoi biased incremental search

Before discussing the Voronoi biased property of the SBAs, let first introduce some basic notation. Given a set of points S = {si| i = 1, 2, …, n} in a n-dimension space Χ. For any two distinct points sp and sq in set S, the dominant region (Voronoi region) of sp over sq is defined as the region where any inside point should be closer to sp over sq, that is,

$$\mathcal{R}\_{\mathfrak{s}\_p} = \left\{ \chi \in \left| \left| \mathbf{s}\_p - \chi \right|^L < \left| \mathbf{s}\_q - \chi \right|^L \right\} \right. \tag{9}$$

Where χ is the dominant region corresponding to sp, ||L denotes the Lebesgue measurement. In a normal case, any point si has its own dominant region with,

$$\mathcal{R}\_{\mathbf{s}} = \left\{ \chi \in \left| |\mathbf{s}\_i - \chi|^L < \left| \mathbf{s}\_j - \chi \right|^L \right.\right. \text{ for all } \mathbf{i} \neq \mathbf{j} \right\} \tag{10}$$

Normally, random sampling of RRT follows a Monte-Carlo Method [15] to perform an uniformly sampling in a n-dimensional space under Lebesgue measurement. We can look back at the beginning of Section 3, the new sampled node tries to connect to the nearest node under Euclidean metric. We can now analyze the problem in another perspective that given gparent and gs, they connect to the same origin go. Then, a new sample gsample is generated randomly following a Monte-Carlo process. In order to explore and reach the goal, gsample tries to connect to the tree following the metric defined in Eq. (5). It means that gparent and gs can be connected for expansion under minimum distance principle, then gsample has to be assigned to the dominant region which subjects to a closer point (the Voronoi region). Under this principle, gparent and gs can acquire new resource for extension with the ability to keep distinct region and extending their branches.

A typical Voronoi-biased exploration using sampling can be seen in Figure 3, where each branch keeps distinct with each other to form a star network like structure and it behaves the A Random Multi-Trajectory Generation Method for Online Emergency Threat Management… http://dx.doi.org/10.5772/intechopen.71410 241

Figure 3. Results of incremental exploration of RRT and hRRT [16] after 200 and 1000 iterations, respectively.

same for heuristic informed RRT [16]. Here, unlike the dominant region of a point, RRT branch can be also treated as a distinct individual with its own Voronoi region for acquiring the extending resource.

#### 4. Obstacle and homology biased path planner

constraints can be represented as a set of implicit equations as g xð Þ¼ ; x\_ 0, and it can be further

Here, x denotes the state, and u∈ U denotes the valid control in allowable controls set. Given the parent state gparent(t), the time step follows a Δt limits. Then, the control inputs vary with

be noted that the planner should extend toward the newly sampled gsample. The planner first

where e is the maximum first order factor of control input. RRT now picks a new state along the direction from parent to new sample, that is, gnew ∈[x(t) + f(x(t), u(t) � Δt ∙ e), x(t) + f(x(t), u

Before discussing the Voronoi biased property of the SBAs, let first introduce some basic notation. Given a set of points S = {si| i = 1, 2, …, n} in a n-dimension space Χ. For any two distinct points sp and sq in set S, the dominant region (Voronoi region) of sp over sq is defined

Where χ is the dominant region corresponding to sp, ||L denotes the Lebesgue measurement.

L < sj � χ � � � � L

Normally, random sampling of RRT follows a Monte-Carlo Method [15] to perform an uniformly sampling in a n-dimensional space under Lebesgue measurement. We can look back at the beginning of Section 3, the new sampled node tries to connect to the nearest node under Euclidean metric. We can now analyze the problem in another perspective that given gparent and gs, they connect to the same origin go. Then, a new sample gsample is generated randomly following a Monte-Carlo process. In order to explore and reach the goal, gsample tries to connect to the tree following the metric defined in Eq. (5). It means that gparent and gs can be connected for expansion under minimum distance principle, then gsample has to be assigned to the dominant region which subjects to a closer point (the Voronoi region). Under this principle, gparent and gs can acquire new resource for extension with the ability to keep distinct region

A typical Voronoi-biased exploration using sampling can be seen in Figure 3, where each branch keeps distinct with each other to form a star network like structure and it behaves the

n o

computes the possible region of reachability from current state x(t):

(t) +Δt ∙ e)] and gnew = gparent + δ(gsample � gparent) with δ∈[0, 1].

as the region where any inside point should be closer to sp over sq, that is,

� � � � �sp � χ � � L

R sp ¼ χ∈

� � �jsi � χj

In a normal case, any point si has its own dominant region with,

Rsi ¼ χ∈

3.2. Voronoi biased incremental search

and extending their branches.

≤ t +Δt}. To compute x(t +Δt), we can follow a typical procedure as [12]. It should

x tð Þ þ Δt ∈½ � x tð Þþ fxt ð Þ ð Þ; u tð Þ� Δt∙e ; x tð Þþ fxt ð Þ ð Þ; u tð Þþ Δt∙e (8)

< sq � χ � � � �

, for all i 6¼ j

(9)

(10)

L n o

x\_ ¼ f xð Þ ; u (7)

represented as:

)| t ≤ t 0

u = {u(t0

240 Kinematics

In this Section, we propose approaches to solve two main problems, which are handling cluttered environment and online ET processing, using obstacle-biased method and homologybiased method. Collision detection during the incremental exploration is time consuming, and it follows a routine procedure to guarantee safety. It should be noted that the step validation of each new sampling state provides the directional information of obstacle distribution.

#### 4.1. Obstacle biased path planner under kinematic constraints

SBAs mostly deploy the general idea of generating random samples for incremental exploration, and the sample locating in obstacle region will be discarded since it is time consuming and no benefits for increasing the performance of exploring. We firstly deployed a simple idea which was proved to have much higher time performance then RRT and RRT\* in [17].

This paper introduces an obstacle biased algorithm, using obstacle information to help generating more samples for connection. It is shown in Figure 4, the newly sampled states xs 1, xs 1 tries to connect to the nearest state in the tree. However, xs 1 leads toward the obstacle region, xs 2 locates in obstacle region. To use the obstacle information, this paper proposes an active exploring method, that is, inner propulsion and outer attraction.

For outer attraction, new sample xs 1 performs a collision checking, and find the nearest nodes o xa, o xb. We define that the further the obstacle to the sample, the more attraction it can support, that is, the attraction is proportional to the distance between obstacle and the sample using L2 norm L2. The sample then re-allocation by add a obstacle biased attraction as:

$$\mathbf{x}\_1^{s\*} = \mathbf{x}\_1^s + \mathbf{k} \left[ \left( ^o \mathbf{x}\_a - \mathbf{x}\_1^s \right) + \left( ^o \mathbf{x}\_b - \mathbf{x}\_1^s \right) \right] \tag{11}$$

Where k is a constant to adjust the shifting percentage of the attraction vector.

Figure 4. Obstacle biased SBA uses the obstacle location as input, with inner propulsion and outer attraction, to generate more samples for exploration. xs 1, xs 2 are new samples, the black region denotes obstacle region.

The inner sample in collision with the obstacle is regarded to provide guiding information for the algorithm. This paper tries to find two more states g x1, g x2 within kinematic reachable region (discussed with Eq. (8)), it tries to find out the first two safe state with two directions which are out of obstacle region in the kinematic reachable region (the light blue fan-shaped region). Then, the two newly generated samples g x1, g x2 follows principle Eq. (11) to redistribute to the final position, and connect to the tree.

4.2.1. Extending-forbidden algorithm

probability to the nearby branches.

D ¼ sup bT ð Þi

exploration probability can be represented as:

� � � �

The path planner performs exploration following the Monte-Carlo approach. Given a configuration space C in a topological space (the rectangle region as illustrated in Figure 5), we denote the Lebesgue Measurement of the configuration space as L∗ ∣C∣. Then we can get the Lebesgue measurement of each branch bT(i) of the tree using the same metric. Authors in [18] proved that

Figure 5. The extending-forbidden algorithm (EFA) tries to find all the states along the goal reached branch at each goal reaching checking step, such as branch B2. Then, EFA sets the flag of the states to be inactive, switching the extending

> j j V bð Þ Tð Þi L∗j j C

Where ψ(bt(i)) denotes the number of samples m, 1 ≤ m ≤ n, that lies in the sub-branch bT(i), n is the number of all the sampling, d is the dimension of the configuration space. D denotes the difference between the ration of sampling probability and ration of space Lebesgue measurement, which follows the knowledge that Monte-Carlo method performs a uniform sampling. It means the sampling probability approaches the ratio of Lebesgue measurements, that is, the

PbTð Þi ¼ L∗j j V bð Þ Tð Þi

However, the probability of exploring in the configuration space does not benefit the extending bias toward the goal. Let us still take a look at Figure 5(a), the branch B2 dominant

� � �

A Random Multi-Trajectory Generation Method for Online Emergency Threat Management…

http://dx.doi.org/10.5772/intechopen.71410

243

� ¼ o log nð Þ n � �1

!

d

L∗j j C (13)

(12)

the dispersion of n random nodes sampled uniformly and independently in V(bT(i)) is,

n � L∗

ψ bt ð Þ ð Þi : n

By using the two proposed approaches, we can generate more useful samples for extending, especially, the samples generate around the edge of the obstacles with the ability to perform more active exploration in cluttered environments. Besides, the outer attraction redistributes the samples toward the narrow corridor between the obstacle, which thus increases the probability of finding safe path through such obstacle crowed region.

#### 4.2. Homology biased

We assume any path ht(s) generated using SBA is consisted by a set of nodes htð Þ¼ s fht jht s0 ð Þ;s0 ∈ ½ �g 0; 1 , as it is illustrated in Figure 5(a) that exploring tree is consist of the red nodes. Each red node is regarded as distinct with other nodes in the tree, with a distinct dominant region, i.e. Voronoi region. Thus, a path ht(s), which is consisted a set of states from the initial state to the goal, can be isolated with each other with a distinct region V(bT) combined by all Voronoi regions of the states.

The region dominant property differs the path with each other, where a SB tree with multiple paths (the path here may not connect to the goal, but they keep distinct with each other from the initial position) can be described by a set of branches BT = {bT(1), bT(2), …, bT(n)}. For each branch, it consists of a list of states which connect with each other to form tree structure. In the tree, the end state relies on the parent state for extending as well as trajectory tracking.

A Random Multi-Trajectory Generation Method for Online Emergency Threat Management… http://dx.doi.org/10.5772/intechopen.71410 243

Figure 5. The extending-forbidden algorithm (EFA) tries to find all the states along the goal reached branch at each goal reaching checking step, such as branch B2. Then, EFA sets the flag of the states to be inactive, switching the extending probability to the nearby branches.

#### 4.2.1. Extending-forbidden algorithm

The inner sample in collision with the obstacle is regarded to provide guiding information for

Figure 4. Obstacle biased SBA uses the obstacle location as input, with inner propulsion and outer attraction, to generate

2 are new samples, the black region denotes obstacle region.

region (discussed with Eq. (8)), it tries to find out the first two safe state with two directions which are out of obstacle region in the kinematic reachable region (the light blue fan-shaped

By using the two proposed approaches, we can generate more useful samples for extending, especially, the samples generate around the edge of the obstacles with the ability to perform more active exploration in cluttered environments. Besides, the outer attraction redistributes the samples toward the narrow corridor between the obstacle, which thus increases the prob-

We assume any path ht(s) generated using SBA is consisted by a set of nodes htð Þ¼ s fht jht s0 ð Þ;s0 ∈ ½ �g 0; 1 , as it is illustrated in Figure 5(a) that exploring tree is consist of the red nodes. Each red node is regarded as distinct with other nodes in the tree, with a distinct dominant region, i.e. Voronoi region. Thus, a path ht(s), which is consisted a set of states from the initial state to the goal, can be isolated with each other with a distinct region V(bT) combined by all

The region dominant property differs the path with each other, where a SB tree with multiple paths (the path here may not connect to the goal, but they keep distinct with each other from the initial position) can be described by a set of branches BT = {bT(1), bT(2), …, bT(n)}. For each branch, it consists of a list of states which connect with each other to form tree structure. In the tree, the end state relies on the parent state for extending as well as

x1, g x1, g

x2 within kinematic reachable

x2 follows principle Eq. (11) to redistrib-

the algorithm. This paper tries to find two more states g

ability of finding safe path through such obstacle crowed region.

region). Then, the two newly generated samples g

1, xs

ute to the final position, and connect to the tree.

4.2. Homology biased

more samples for exploration. xs

242 Kinematics

Voronoi regions of the states.

trajectory tracking.

The path planner performs exploration following the Monte-Carlo approach. Given a configuration space C in a topological space (the rectangle region as illustrated in Figure 5), we denote the Lebesgue Measurement of the configuration space as L∗ ∣C∣. Then we can get the Lebesgue measurement of each branch bT(i) of the tree using the same metric. Authors in [18] proved that the dispersion of n random nodes sampled uniformly and independently in V(bT(i)) is,

$$\mathbf{D} = \sup\_{b \uparrow(\boldsymbol{i})} \left| \frac{\psi(b\_l(\boldsymbol{i}):\boldsymbol{n})}{\boldsymbol{n}} - \frac{\mathbf{L}^\* |V(b\_T(\boldsymbol{i}))|}{\mathbf{L} \* |\mathbf{C}|} \right| = \mathbf{o} \left( \left( \frac{\log \left( \mathbf{n} \right)}{\mathbf{n}} \right)^{\frac{1}{2}} \right) \tag{12}$$

Where ψ(bt(i)) denotes the number of samples m, 1 ≤ m ≤ n, that lies in the sub-branch bT(i), n is the number of all the sampling, d is the dimension of the configuration space. D denotes the difference between the ration of sampling probability and ration of space Lebesgue measurement, which follows the knowledge that Monte-Carlo method performs a uniform sampling. It means the sampling probability approaches the ratio of Lebesgue measurements, that is, the exploration probability can be represented as:

$$\mathbf{P\_{b\_{T}(i)}} = \frac{\mathbf{L^\*} |V(b\_T(i))|}{\mathbf{L^\*} |\mathbf{C}|} \tag{13}$$

However, the probability of exploring in the configuration space does not benefit the extending bias toward the goal. Let us still take a look at Figure 5(a), the branch B2 dominant the near-goal region, and other region are not able to extend toward the goal as the samples will not connect to the branches if it locates in the near-goal region. To solve this problem, this paper proposes an Extending-Forbidden Algorithm (EFA), it shifts the source for extending to other branches by forbidding the goal reached path.

Definition 3—Goal-biased Probability: Given a configuration space C, the exploring tree T and all its branches which are main branches BT and its corresponding sub-branches. The goalbiased event denotes a branch can exploring toward the goal. If a goal region can be represented as Gr, and Voronoi(Gr2BT(i)) is the region that belongs to goal region and the Voronoi region of branch BT(i). Then, the nominal goal biased probability of branch BT(i) toward Gr is:

$$\mathbf{P}\_{\mathbf{G}}^{\*}\big(\mathbf{B}\_{\mathbf{T}}(i)\big) = \frac{L^{\*}\big|\operatorname{vorroni}(\mathbf{G}\_{\mathbf{r}}\mathbf{2B\_{T}}(i))\big|}{L^{\*}\big|\mathbf{C}\big|}\tag{14}$$

configuration space. After the tree stops extending at a certain iteration, we can have the results as illustrated in Figure 5(a). Since goal region is in Voronoi region of branch B2, then we know that we have the goal biased probability as PG(B(2)) = 1. One branch B2 reached the goal region which is represented with dotted back rectangle, EFA searches the SSBs and LSBs based on Definition 4, and it labels states and executing forbidden. Then, we have a resource shifted Voronoi graph as illustrated in Figure 5(b), where we can see that branch B1 and B3 obtains the Voronoi region which belongs to branch B2. The two branches also obtain the rectangle goal region, that is, their goal biased probabilities are bigger than zero, PG(B(1)) > 0, PG(B(3)) > 0.

A Random Multi-Trajectory Generation Method for Online Emergency Threat Management…

http://dx.doi.org/10.5772/intechopen.71410

245

Since EFA can shift the goal biased by extending resource to other branches, while not all paths

1. The increasing of goal biased probability ensures the generation of a feasible path toward the goal, but not all branches with goal biased probability can reach the goal at the same time. Only one can reach the goal because of Voronoi dominant probability, thus the

2. The efficiency of generating multiple paths mainly depends on the environment adaptability of random exploring algorithms. For random exploring algorithm, their merits of

The proposed MGART is able to perform extending-forbidden toward multiple paths, as the random exploring property guarantees completeness and diversity. However, the quality of explored paths cannot be guaranteed, particularly a large number of homotopic paths are generated. This paper proposes an approach to generate the reasonable path, and we analysis under the hypothesis that the environment is highly cluttered and it is not practical to set

Definition 5—Reasonable Alternative Paths: Consider two homotopic paths h(π1) and h(π2) in a configuration space C. The surrounding obstacle information along each path are ℶð Þ hð Þ π1 and ℶð Þ hð Þ π2 . The reasonable alternative path exists if and only the surrounding information

Given the sensing range of a robot as Υ, and the obstacles set O = {o1, o2, …., on}. For path h(π1), it consists of a set of discrete states X(π1) = {x(π1)1, x(π1)2, …, x(π1)n}. In this paper, we assume

Delaunay Triangulation [19] connection (DTc) using the obstacle centers, the initial state, and the goal state. For DTc, it can generate a network like structure, and each two states have at most one connection. It is illustrated in Figure 8(a) that the green edges are the valid connections, with labels to distinct with each other. For any path, if the path intersects with an edge, the edge information should be added to the information factor, such as the solid red path intersects with edge L7, then L7∈ ℶð Þ hð Þ π1 . The edge labeling method can guarantee the uniqueness toward homology, while we note that homotopic paths can also be used to perform emergency threat management. It is represented in Figure 7(b), the solid red path hs and the

i , then we can build a

generating multiple branches enable the generation of multiple paths.

threshold for path planner to choose the best homological paths.

of the two paths are not the same, such that, ℶð Þ hð Þ π1 6¼ ℶð Þ hð Þ π2 .

that any obstacle οi can be described with a circle or ellipse centered at oc

can obtain such resource. The following truths are hold:

general SBA cannot find multiple paths.

4.2.2. Reasonable alternative reference chosen

And the real goal biased probability is normalized value of all branches, that is,

$$P\_{\mathcal{G}}(\mathcal{B}\_{\mathcal{T}}(i)) = \frac{P\_{\mathcal{G}}^{\*}(\mathcal{B}\_{\mathcal{T}}(i))}{\sum\_{j=1}^{n} P\_{\mathcal{G}}^{\*}(\mathcal{B}\_{T}(j))} \tag{15}$$

Definition 4—Long Sub-branch (LSB) and Short Sub-branch (SSB): Given a tree T and all its Voronoi distinct main branches BT. Then, we can define a length threshold δB. For all end vertices in each main branch, we calculate the length lsb from end to the goal reach reached path (any state which firstly reached). If the length lsb > δB, then we call it Long Sub-branch (LSB). If lsb ≤ δB, we call the sub-branch as Short Sub-branch (SSB).

It should be noted that the threshold is very empirical in Definition 4, and it is decided based on the configuration space and the kinematic constraints. In Figure 6, we set it as 15 meters, then we have SSB1, SSB2, SSB3 as SSB, and LSB1 as LSB. The reason why we have this definition is that we cannot shift all the extending resource to the neighbor branches, and we have the hypothesis that the SSB must has lower probability of finding a new path even given the resource for extending. For example, the main Voronoi dominant branches BT keep distinct with each other, and each main branch has the probability PbT(i) for exploration in the

Figure 6. An intuitive example of SSB and LSB in an exploring tree, which are generated using threshold principle as defined in Definition 4.

configuration space. After the tree stops extending at a certain iteration, we can have the results as illustrated in Figure 5(a). Since goal region is in Voronoi region of branch B2, then we know that we have the goal biased probability as PG(B(2)) = 1. One branch B2 reached the goal region which is represented with dotted back rectangle, EFA searches the SSBs and LSBs based on Definition 4, and it labels states and executing forbidden. Then, we have a resource shifted Voronoi graph as illustrated in Figure 5(b), where we can see that branch B1 and B3 obtains the Voronoi region which belongs to branch B2. The two branches also obtain the rectangle goal region, that is, their goal biased probabilities are bigger than zero, PG(B(1)) > 0, PG(B(3)) > 0.

Since EFA can shift the goal biased by extending resource to other branches, while not all paths can obtain such resource. The following truths are hold:

#### 4.2.2. Reasonable alternative reference chosen

the near-goal region, and other region are not able to extend toward the goal as the samples will not connect to the branches if it locates in the near-goal region. To solve this problem, this paper proposes an Extending-Forbidden Algorithm (EFA), it shifts the source for extending to

Definition 3—Goal-biased Probability: Given a configuration space C, the exploring tree T and all its branches which are main branches BT and its corresponding sub-branches. The goalbiased event denotes a branch can exploring toward the goal. If a goal region can be represented as Gr, and Voronoi(Gr2BT(i)) is the region that belongs to goal region and the Voronoi region of

> Gð Þ¼ BTð Þi L∗∣voronoið Þ Gr2BTð Þi ∣ L∗

> > Pn j¼1 P∗

Definition 4—Long Sub-branch (LSB) and Short Sub-branch (SSB): Given a tree T and all its Voronoi distinct main branches BT. Then, we can define a length threshold δB. For all end vertices in each main branch, we calculate the length lsb from end to the goal reach reached path (any state which firstly reached). If the length lsb > δB, then we call it Long Sub-branch

It should be noted that the threshold is very empirical in Definition 4, and it is decided based on the configuration space and the kinematic constraints. In Figure 6, we set it as 15 meters, then we have SSB1, SSB2, SSB3 as SSB, and LSB1 as LSB. The reason why we have this definition is that we cannot shift all the extending resource to the neighbor branches, and we have the hypothesis that the SSB must has lower probability of finding a new path even given the resource for extending. For example, the main Voronoi dominant branches BT keep distinct with each other, and each main branch has the probability PbT(i) for exploration in the

Figure 6. An intuitive example of SSB and LSB in an exploring tree, which are generated using threshold principle as

Gð Þ BTð Þi

∣C∣ (14)

Gð Þ BTð Þj (15)

branch BT(i). Then, the nominal goal biased probability of branch BT(i) toward Gr is:

And the real goal biased probability is normalized value of all branches, that is,

PGð Þ¼ BTð Þi P∗

other branches by forbidding the goal reached path.

244 Kinematics

P∗

(LSB). If lsb ≤ δB, we call the sub-branch as Short Sub-branch (SSB).

defined in Definition 4.

The proposed MGART is able to perform extending-forbidden toward multiple paths, as the random exploring property guarantees completeness and diversity. However, the quality of explored paths cannot be guaranteed, particularly a large number of homotopic paths are generated. This paper proposes an approach to generate the reasonable path, and we analysis under the hypothesis that the environment is highly cluttered and it is not practical to set threshold for path planner to choose the best homological paths.

Definition 5—Reasonable Alternative Paths: Consider two homotopic paths h(π1) and h(π2) in a configuration space C. The surrounding obstacle information along each path are ℶð Þ hð Þ π1 and ℶð Þ hð Þ π2 . The reasonable alternative path exists if and only the surrounding information of the two paths are not the same, such that, ℶð Þ hð Þ π1 6¼ ℶð Þ hð Þ π2 .

Given the sensing range of a robot as Υ, and the obstacles set O = {o1, o2, …., on}. For path h(π1), it consists of a set of discrete states X(π1) = {x(π1)1, x(π1)2, …, x(π1)n}. In this paper, we assume that any obstacle οi can be described with a circle or ellipse centered at oc i , then we can build a Delaunay Triangulation [19] connection (DTc) using the obstacle centers, the initial state, and the goal state. For DTc, it can generate a network like structure, and each two states have at most one connection. It is illustrated in Figure 8(a) that the green edges are the valid connections, with labels to distinct with each other. For any path, if the path intersects with an edge, the edge information should be added to the information factor, such as the solid red path intersects with edge L7, then L7∈ ℶð Þ hð Þ π1 . The edge labeling method can guarantee the uniqueness toward homology, while we note that homotopic paths can also be used to perform emergency threat management. It is represented in Figure 7(b), the solid red path hs and the dotted red path hD are homotopic to each other. Given the sensing range Υ, we have the sensing envelop which are dotted purple lines hL 1 , hR 1 for hD and solid black lines hL 2 , hR 2 for hs, indicating the maximum detection range for emergency threat. Then, we have the of g1 ⊂ ℶð Þ hD and of g 1; o2; o3; o4 ⊂ℶð Þ hS , thus we have hs and hD both regarded as reasonable alternative paths for online threat management.

The informative approach discussed thus can help to label each path in a configuration space, such as the results listed in Table 1 of paths in Figure 7(a). Then according to Definition 5, we can find the label of each path. For any several paths which have the same label, we choose the shortest path and use as the candidate for online fast switching.

#### 4.3. Emergency threat management

The reasonable alternative path set HRAP provides a network with cluttered environment adaptivity. The concept of visibility was discussed in [20], where the cycle information is used to enable fast deformation for motion planning. Visibility is defined as:

Table 1. The path information parameter is consist of two parts, which are edge information using DTc and obstacle information using sensing envelop, respectively.

V :

can only in obstacle free region.

paper applies the heuristic:

x∗ ¼ argmin i, j

8 >><

>>:

½ �� 0; 1 ½ �! 0; 1 f g 0; 1

A Random Multi-Trajectory Generation Method for Online Emergency Threat Management…

CFE xUAV; HRAP ið Þ xRAP jð Þ � � � � þ CTC HRAP ið Þ xRAP jð Þ � � � � (17)

(16)

247

http://dx.doi.org/10.5772/intechopen.71410

V xt; xt ð Þ¼0 1, if Llink xt; xt ð Þ0 ∈ Cfree ⊂ C V xt; xt ð Þ¼0 0, if Llink xt; xt ð Þ0 ∈ Cobs ⊂ C

Where x() denotes a state of a path, Llink is the connection of two states, Cfree is the free space and Cobs is the obstacle region. A visual illustration is provided in Figure 8(a), where visibility

It is noted that the visibility in this paper means a possible connection to switch from one path to another for emergency threat management. For switching with visibility, given all the reasonable alternative paths HRAP, the algorithm performs exploration for visibility state at each UAV state xUAV among HRAP. The algorithm then outputs the visible guards (states) xRAP as illustrated in Figure 8(b). To avoid the pop-up threat (or dynamic threat), UAV must select one entry guard from the visible guard set to reconnect to another pre-planned path to the goal. To validate the best connection, that is, the entry point and the entry connection, this

Using a simple cost based metric, where CFE is the forward energy cost which is the distance and the turning cost from UAV position xUAV to entry state xRAP(j) and the path from entry state to the goal HRAP(i)(xRAP(j)). The turning cost is the integration of heading angle difference at each state, which denotes the smoothness of the planned path. CTC is the threat cost, the integration of inverse distance between state and obstacles. Using such approach, the algorithm can find five visible states xRAP = {xRAP(1), …, xRAP(5)} as illustrated in Figure 8(b). Then,

Figure 8. For online switching, UAV should follow the visible-node selection algorithm (a) to explore the possible

switching route, then it switches to the cost minimum path for ETM (b).

it tries to find the best entry state using the minimum cost principle (Eq. (17)).

Figure 7. An illustration of surrounding information used to find reasonable alternative path for online emergency threat management. The information parameter consists of edge information and obstacle surrounding information within sensing envelop.

$$\mathcal{V}: \begin{cases} \begin{aligned} [0,1] \times [0,1] \to \{0,1\} \end{aligned} \end{cases} $$

$$\begin{aligned} V: \begin{cases} V(\mathbf{x}\_{l}, \mathbf{x}\_{l'}) = & 1, \text{if } \mathcal{L}\_{\text{link}}(\mathbf{x}\_{l}, \mathbf{x}\_{l'}) \in \mathsf{C}\_{\text{free}} \subset \mathsf{C} \\\ V(\mathbf{x}\_{l}, \mathbf{x}\_{l'}) = & 0, \text{if } \mathcal{L}\_{\text{link}}(\mathbf{x}\_{l}, \mathbf{x}\_{l'}) \in \mathsf{C}\_{\text{obs}} \subset \mathsf{C} \end{aligned} \end{cases} \end{aligned} \tag{16}$$

Where x() denotes a state of a path, Llink is the connection of two states, Cfree is the free space and Cobs is the obstacle region. A visual illustration is provided in Figure 8(a), where visibility can only in obstacle free region.

It is noted that the visibility in this paper means a possible connection to switch from one path to another for emergency threat management. For switching with visibility, given all the reasonable alternative paths HRAP, the algorithm performs exploration for visibility state at each UAV state xUAV among HRAP. The algorithm then outputs the visible guards (states) xRAP as illustrated in Figure 8(b). To avoid the pop-up threat (or dynamic threat), UAV must select one entry guard from the visible guard set to reconnect to another pre-planned path to the goal. To validate the best connection, that is, the entry point and the entry connection, this paper applies the heuristic:

$$\mathbf{x}^\* = \underset{i, \dot{\imath}}{\text{argmin }} \mathbb{C}\_{FE} \{ \mathbf{x}\_{\text{LAV}}, H\_{\text{RAP}(i)} \{ \mathbf{x}\_{\text{RAP}(j)} \} \} + \mathbb{C}\_{TC} \{ H\_{\text{RAP}(i)} \{ \mathbf{x}\_{\text{RAP}(j)} \} \} \tag{17}$$

Using a simple cost based metric, where CFE is the forward energy cost which is the distance and the turning cost from UAV position xUAV to entry state xRAP(j) and the path from entry state to the goal HRAP(i)(xRAP(j)). The turning cost is the integration of heading angle difference at each state, which denotes the smoothness of the planned path. CTC is the threat cost, the integration of inverse distance between state and obstacles. Using such approach, the algorithm can find five visible states xRAP = {xRAP(1), …, xRAP(5)} as illustrated in Figure 8(b). Then, it tries to find the best entry state using the minimum cost principle (Eq. (17)).

dotted red path hD are homotopic to each other. Given the sensing range Υ, we have the

indicating the maximum detection range for emergency threat. Then, we have the of g1 ⊂ ℶð Þ hD and of g 1; o2; o3; o4 ⊂ℶð Þ hS , thus we have hs and hD both regarded as reasonable alternative

The informative approach discussed thus can help to label each path in a configuration space, such as the results listed in Table 1 of paths in Figure 7(a). Then according to Definition 5, we can find the label of each path. For any several paths which have the same label, we choose the

The reasonable alternative path set HRAP provides a network with cluttered environment adaptivity. The concept of visibility was discussed in [20], where the cycle information is used

Dotted red L2.L3.L6.L7.L8.L19.L31 O5, O2 Solid red L2.L3.L6.L7.L8.L19.L31 O5, O2, O3, O4 Dotted blue L2.L3.L6.L14.L16.L17.L18.L31 O5, O2, O3, O4 Solid blue L2.L3.L6.L14.L16.L20.L23.L24 O5, O2, O3 Dotted pink L2.L10.L11.L22.L27.L29.L30 O6, O7, O8 Solid pink L2.L10.L11.L22.L27.L29.L30 O6, O9, O10, O3

Table 1. The path information parameter is consist of two parts, which are edge information using DTc and obstacle

Figure 7. An illustration of surrounding information used to find reasonable alternative path for online emergency threat management. The information parameter consists of edge information and obstacle surrounding information within

1 , hR

1 for hD and solid black lines hL

Edge information Obstacle

2 , hR

information

2 for hs,

sensing envelop which are dotted purple lines hL

shortest path and use as the candidate for online fast switching.

Path Path surrounding information

to enable fast deformation for motion planning. Visibility is defined as:

paths for online threat management.

246 Kinematics

4.3. Emergency threat management

information using sensing envelop, respectively.

sensing envelop.

Figure 8. For online switching, UAV should follow the visible-node selection algorithm (a) to explore the possible switching route, then it switches to the cost minimum path for ETM (b).

We further consider a situation that there may have no visible states at current location. The paper proposes to use a long-term memory approach to handle this problem, that is, the travel path should be stored in memory, such as the orange edges and states illustrated in Figure 8(b). In the meanwhile, the method stores the visible state along the traveled path. Then, the UAV has to fly back to find a cost minimum path toward the goal if it confronts with pop-threat and has no visible states.

#### 5. Experiment and discussion

In this section, we highlight the performance of the obstacle biased and homology biased path planner with the ability of emergency threat management (avoiding pop-up and dynamic threat online). In the section, we will discuss the following points: (1) How the threshold of EFA affects the performance of MGART. (2) The time performance and reliability of reasonable alternative chosen algorithm. (3) The online emergency threat management performance. The algorithm is implemented using MatLab 2016b on a laptop computer with a 2.6 GHz Intel Core I5 processor.

#### 5.1. Comparative simulation of multiple path exploration

We design three different scenarios, which are non-obstacle scenario, rounded obstacle crowed scenario, and irregular polygons crowed scenario, to perform comparative simulations. All the scenarios are 2D with 100∗ 65 m2 space, and obstacles randomly generated.

For scenario 1, it is a non-obstacle environment, and we set the variable threshold as a set with value {3, 5, 7, 8, 11,13,15,18,20,25,28} for representation. As we know the length of EFA threshold affects the goal biased probability, which directly decide the area of the newly obtained Voronoi region of the neighbor branches, we design a set of comparative experiments to study the effects between EF length and RAPs. An intuitive result of the relationship between planned paths and EF length after 10,000 iterations are provided in Figure 9(a)–(d). MGART can find 37 paths after 10,000 iterations if EF length is set as 3 step-length, and the number decreases to 22 if the EF length is set as 28. The reason is that the longer the EF length, the further the neighbor branches can obtain the goal biased resource. Thus, the neighbor branches need more steps to exploring toward the goal, that is, less paths will be achieved with better homology performance. As we can see that the paths in Figure 9(d) have a better homology performance than Figure 9(a). The same EF length variation experiment is also deployed in scenario 2, and results are shown in Figure 9(e)–(h). For RAPs, it is the same with the results in scene 1 that RAPs decrease with the increasing of the EF length. However, as the increasing of EF length enables more branches to explore toward the goal as well as increasing the homologous paths (see in Figure 9(e)–(h)), the number of the RAPs increasing with the increasing of the EF length. The statistic relation between the RAPs and the EF length is illustrated in Figure 10, which further proves the conclusion.

The EFA can be used to any SBAs by shifting the goal biased resource to achieve multiple RAPs for online switching. This paper compares the performance between MGART and MRRT\* in three scenarios with 10,000 iterations. We compare the efficiency of generating a path, RAPs, average time for finding a path, and average time for any RAP are compared in Table 2. GART has a better performance in both path exploration and RAP generation, such that MGART can find at least 3 times of the number of paths toward the goal than MRRT\*. Because GART introduces the environmental information to speed up the exploring process, the results prove

Figure 10. Relation between EF backward length and APs and RAPs with two scenarios. Here the solid diamond line

denotes the relation of scene 1, and the triangle lines denotes the results of scene 2.

Figure 9. Illustration of alternative paths generated by MGART vary with representative backward EF length. (a)–(d)

A Random Multi-Trajectory Generation Method for Online Emergency Threat Management…

http://dx.doi.org/10.5772/intechopen.71410

249

denotes the results in non-obstacles scenario, (e)–(h) denotes the results in obstacle crowed.

A Random Multi-Trajectory Generation Method for Online Emergency Threat Management… http://dx.doi.org/10.5772/intechopen.71410 249

We further consider a situation that there may have no visible states at current location. The paper proposes to use a long-term memory approach to handle this problem, that is, the travel path should be stored in memory, such as the orange edges and states illustrated in Figure 8(b). In the meanwhile, the method stores the visible state along the traveled path. Then, the UAV has to fly back to find a cost minimum path toward the goal if it confronts with pop-threat and has

In this section, we highlight the performance of the obstacle biased and homology biased path planner with the ability of emergency threat management (avoiding pop-up and dynamic threat online). In the section, we will discuss the following points: (1) How the threshold of EFA affects the performance of MGART. (2) The time performance and reliability of reasonable alternative chosen algorithm. (3) The online emergency threat management performance. The algorithm is implemented using MatLab 2016b on a laptop computer with a 2.6 GHz Intel

We design three different scenarios, which are non-obstacle scenario, rounded obstacle crowed scenario, and irregular polygons crowed scenario, to perform comparative simulations. All the

For scenario 1, it is a non-obstacle environment, and we set the variable threshold as a set with value {3, 5, 7, 8, 11,13,15,18,20,25,28} for representation. As we know the length of EFA threshold affects the goal biased probability, which directly decide the area of the newly obtained Voronoi region of the neighbor branches, we design a set of comparative experiments to study the effects between EF length and RAPs. An intuitive result of the relationship between planned paths and EF length after 10,000 iterations are provided in Figure 9(a)–(d). MGART can find 37 paths after 10,000 iterations if EF length is set as 3 step-length, and the number decreases to 22 if the EF length is set as 28. The reason is that the longer the EF length, the further the neighbor branches can obtain the goal biased resource. Thus, the neighbor branches need more steps to exploring toward the goal, that is, less paths will be achieved with better homology performance. As we can see that the paths in Figure 9(d) have a better homology performance than Figure 9(a). The same EF length variation experiment is also deployed in scenario 2, and results are shown in Figure 9(e)–(h). For RAPs, it is the same with the results in scene 1 that RAPs decrease with the increasing of the EF length. However, as the increasing of EF length enables more branches to explore toward the goal as well as increasing the homologous paths (see in Figure 9(e)–(h)), the number of the RAPs increasing with the increasing of the EF length. The statistic relation between the RAPs and the EF length is illustrated in Figure 10, which further proves the conclusion.

The EFA can be used to any SBAs by shifting the goal biased resource to achieve multiple RAPs for online switching. This paper compares the performance between MGART and MRRT\* in three scenarios with 10,000 iterations. We compare the efficiency of generating a path, RAPs,

65 m2 space, and obstacles randomly generated.

no visible states.

248 Kinematics

Core I5 processor.

scenarios are 2D with 100∗

5. Experiment and discussion

5.1. Comparative simulation of multiple path exploration

Figure 9. Illustration of alternative paths generated by MGART vary with representative backward EF length. (a)–(d) denotes the results in non-obstacles scenario, (e)–(h) denotes the results in obstacle crowed.

Figure 10. Relation between EF backward length and APs and RAPs with two scenarios. Here the solid diamond line denotes the relation of scene 1, and the triangle lines denotes the results of scene 2.

average time for finding a path, and average time for any RAP are compared in Table 2. GART has a better performance in both path exploration and RAP generation, such that MGART can find at least 3 times of the number of paths toward the goal than MRRT\*. Because GART introduces the environmental information to speed up the exploring process, the results prove that MGART is more efficient in finding RAPs, which is almost 100% faster than MRRT\*. For time performance, we can see that MGART also outperforms MRRT\* with at least 3 times advantage.

the complexity of the configuration space. For our tested with area 100∗

5.3. Experiments of emergency threat management

switching from current position to safe path.

Scenario 2 Scenario 3

threats, the black and red circles are threats. (d) Tests of avoiding three moving threats.

Table 3. Time performance of proposed method in two scenarios.

environments, and we also compared the time performance.

pursing of our cases is 0.139 s.

acquiring the information for labeling 0.078 s (see in Table 3). The average time needs for RAP

A Random Multi-Trajectory Generation Method for Online Emergency Threat Management…

MGART can be used for 3D and 2D pop-up threat management, and the 3D environments can be easily segmented by DT. We evaluate the performance of our method in both 2D and 3D

For 2D environments, we implement three tests with different number of dynamic threats. The RAP chosen algorithm works when robot realizes that the path will collide with the pop-up threat, that is, robot at position xUAV detects the moving threat (see in Figure 12(a)). The simulation setting is illustrated in Table 4, where the robot speed is 10 m/s and the moving threat can be detected within 10 m detection range. Thus, the robot has less than 1 s to re-plan a path and executing to avoid the obstacle. RAP chosen algorithm first evaluates all its neighbor RAPs (the green lines) around the robot, and chooses the cost minimal and collision free path based on principle Eq. (17) (the dotted green path in Figure 12(b)). It is noted that Figure 12(b)–(d) are results of using MGART to avoid one, two, and three moving threats, respectively. The black parts along the navigation path denote the position where threat is detected by robot. We also execute test in 3D environment (see in Figure 13) with pup-up and moving threats. The on-line switching is supposed to be used for aerial robots in 3D, thus Dublin's Curves is used when

For all the experiments, we study the time efficiency of each switching to escape from current dangerous situation. For one moving threat avoiding (see in Figure 12(b)), the time needed to switch to other RAP is 0.0507 s, and the whole navigation duration is 13.14 s with 10 m/s

Time for labeling (s) Time for RAPs (s) Time for labeling (s) Time for RAPs (s)

Figure 12. Tests of on-line switching to avoid dynamic threats using MGART in 2D scenarios. (a) Robot detects moving threat at position xUAV, then it evaluates all its visible neighbor RAPs (the green lines) to choose the switching path. (b) Complete navigation of avoiding one moving threat, the red path is the navigation path. (c) Test of avoiding two moving

0.0721 0.146 0.0842 0.132

60, the average time for

251

http://dx.doi.org/10.5772/intechopen.71410

Besides comparison of the time performance of finding online switching paths (that is RAPs), we also pay attention to the quality of the path generated. The average lengths and standard deviation of the length of all paths in each scene are illustrated in Figure 11. The average length of the paths that generated by MGART and MRRT\* are illustrated in Figure 11(a), we can see that MGART has a strong convergence performance than MRRT\*. The standard deviation of the lengths is shown in Figure 11(b), results demonstrate that MGART is more likely to find paths with smaller fluctuation as well as smaller cost.

#### 5.2. Performance of reasonable alternative path chosen

We also test the path labeling algorithm, that is, the surrounding information pursuing using DTc and sensing envelop, which is used to obtain the reasonable alternative paths under Definition 5. It is should be noted that the under the definition, any two paths do not have the same information parameter, which enables fast switching when facing pop-up threat. As the path label method guarantees the unique labeling of all the paths, only the paths which stretch in a parallel way and within the same sensing envelop have the same labels.

The results of simulation after 10,000 iterations in scenario 2 and 3 are provided in Table 3. For each single path, the time needed for labeling the path mainly depends on the area, dimension,

Figure 11. Comparison of (a) average length and (b) standard deviation of the APs generated by MGART and M-RRT\* in three scenarios.

the complexity of the configuration space. For our tested with area 100∗ 60, the average time for acquiring the information for labeling 0.078 s (see in Table 3). The average time needs for RAP pursing of our cases is 0.139 s.

#### 5.3. Experiments of emergency threat management

that MGART is more efficient in finding RAPs, which is almost 100% faster than MRRT\*. For time performance, we can see that MGART also outperforms MRRT\* with at least 3 times advantage. Besides comparison of the time performance of finding online switching paths (that is RAPs), we also pay attention to the quality of the path generated. The average lengths and standard deviation of the length of all paths in each scene are illustrated in Figure 11. The average length of the paths that generated by MGART and MRRT\* are illustrated in Figure 11(a), we can see that MGART has a strong convergence performance than MRRT\*. The standard deviation of the lengths is shown in Figure 11(b), results demonstrate that MGART is more likely to find

We also test the path labeling algorithm, that is, the surrounding information pursuing using DTc and sensing envelop, which is used to obtain the reasonable alternative paths under Definition 5. It is should be noted that the under the definition, any two paths do not have the same information parameter, which enables fast switching when facing pop-up threat. As the path label method guarantees the unique labeling of all the paths, only the paths which stretch

The results of simulation after 10,000 iterations in scenario 2 and 3 are provided in Table 3. For each single path, the time needed for labeling the path mainly depends on the area, dimension,

> RAPs after 10,000 iterations

MMRRT\* 12 / 7.825 /

MRRT\* 11 5 7.169 15.772

MRRT\* 11 5 6.789 14.935

Average time for a feasible path

Average time for a RAP

in a parallel way and within the same sensing envelop have the same labels.

10,000 iterations

Scenario 1 MGART 48 / 2.081 /

Scenario 2 MGART 43 9 2.093 8.213

Scenario 3 MGART 34 10 2.257 7.674

Table 2. Detailed comparison of planning efficiency between MH-GART and MH-RRT\* in all three scenarios.

Figure 11. Comparison of (a) average length and (b) standard deviation of the APs generated by MGART and M-RRT\* in

paths with smaller fluctuation as well as smaller cost.

250 Kinematics

5.2. Performance of reasonable alternative path chosen

Scenario Algorithm Paths after

three scenarios.

MGART can be used for 3D and 2D pop-up threat management, and the 3D environments can be easily segmented by DT. We evaluate the performance of our method in both 2D and 3D environments, and we also compared the time performance.

For 2D environments, we implement three tests with different number of dynamic threats. The RAP chosen algorithm works when robot realizes that the path will collide with the pop-up threat, that is, robot at position xUAV detects the moving threat (see in Figure 12(a)). The simulation setting is illustrated in Table 4, where the robot speed is 10 m/s and the moving threat can be detected within 10 m detection range. Thus, the robot has less than 1 s to re-plan a path and executing to avoid the obstacle. RAP chosen algorithm first evaluates all its neighbor RAPs (the green lines) around the robot, and chooses the cost minimal and collision free path based on principle Eq. (17) (the dotted green path in Figure 12(b)). It is noted that Figure 12(b)–(d) are results of using MGART to avoid one, two, and three moving threats, respectively. The black parts along the navigation path denote the position where threat is detected by robot. We also execute test in 3D environment (see in Figure 13) with pup-up and moving threats. The on-line switching is supposed to be used for aerial robots in 3D, thus Dublin's Curves is used when switching from current position to safe path.

For all the experiments, we study the time efficiency of each switching to escape from current dangerous situation. For one moving threat avoiding (see in Figure 12(b)), the time needed to switch to other RAP is 0.0507 s, and the whole navigation duration is 13.14 s with 10 m/s

Table 3. Time performance of proposed method in two scenarios.

Figure 12. Tests of on-line switching to avoid dynamic threats using MGART in 2D scenarios. (a) Robot detects moving threat at position xUAV, then it evaluates all its visible neighbor RAPs (the green lines) to choose the switching path. (b) Complete navigation of avoiding one moving threat, the red path is the navigation path. (c) Test of avoiding two moving threats, the black and red circles are threats. (d) Tests of avoiding three moving threats.

GART and the ability of exploring in cluttered environments, and it guarantees asymptotically optimal and completeness. It is also shown that the algorithm can generate multiple paths without using variant cost principles, but only relying on the EFA threshold, thus it enables

A Random Multi-Trajectory Generation Method for Online Emergency Threat Management…

http://dx.doi.org/10.5772/intechopen.71410

253

In the future, we would like to research on online visual positioning and environment perception topic, which is lack of discussion in this paper. We would like to enable cognitive sensing

selection for online dynamical switching.

This work was supported by NSFC under grant No. 61528303.

2 The City College/City University of New York, New York, USA

Journal of Control Science and Engineering. 2016;5(2016):1-22

space. Applied Soft Computing. 2013;13(1):709-720

gent Systems (CYBER); 2016. pp. 273-278

3 University of Chinese Academy of Sciences, Beijing, China

\*Address all correspondence to<EMAIL_ADDRESS>
Liang Yang1,2,3, Yuqing He1,3, Jizhong Xiao1,2,3\*, Bing Li2 and Zhaoming Liu1,3

1 State Key Laboratory of Robotics, Chinese Academy of Sciences, Shenyang, China

[1] Goerzen C, Kong Z, Mettler B. A survey of motion planning algorithms from the perspective of autonomous UAV guidance. Journal of Intelligent and Robotic Systems. 2010;

[2] Yang L, Qi J, Song D, Xiao J, Han J, Xia Y. Survey of robot 3D path planning algorithms.

[3] Chen H, Chang K, Agate CS. UAV path planning with tangent-plus-Lyapunov vector field guidance and obstacle avoidance. IEEE Transactions on Aerospace and Electronic

[4] Davoodi M, Panahi F, Mohades A, Hashemi SN. Multi-objective path planning in discrete

[5] Rathbun D, Kragelund S, Pongpunwattana A, Capozzi B. An evolution based path planning algorithm for autonomous motion of a UAV through uncertain environments. In: Proceeding of IEEE Digital Avionics Systems Conference. Vol. 2; 2002. pp. 8D2-1-8D2-12 [6] Valenti RG, Jian Y-D, Ni K, Xiao J. An autonomous flyer photographer. In: Proc. of 2016 IEEE International Conference on Cyber Technology in Automation, Control, and Intelli-

and autonomous for robots.

Acknowledgements

Author details

References

57(1–4):65

Systems. 2013;49(2):840-856

Figure 13. The experiment of using MGAT for 3D emergency threat management case, where pop-up threat and moving threat are exist in the environment.

Table 4. Setting of simulation for ETM.

speed. For two dynamic threats avoiding case (see in Figure 12(c)), the whole navigation time is 13.32 s, and the time spend to avoid the second threat is 0.0912 s. In scene 3, we designed a long duration for threat (see in Figure 12(d)). The two cyan threats disable the blue-path, thus robot has to switch for more times while tracking the dark path. The average time is no more than 0.15 s which can be decreased when implemented in robot's platform with C++ implementation, and the whole navigation time is 13.5 s.

#### 6. Conclusion

The main contribution of this paper is that an online EMT planner is proposed, where pop-up threat and moving obstacle happen during tracking the pre-planned path. We propose a new multiple path planning approach called MGART, which is improved based on GART, by introducing an 'Extending Forbidden' algorithm to shift the goal biased probability to neighbor branches around goal reached branch. The algorithm is shown to inherit the merits of GART and the ability of exploring in cluttered environments, and it guarantees asymptotically optimal and completeness. It is also shown that the algorithm can generate multiple paths without using variant cost principles, but only relying on the EFA threshold, thus it enables selection for online dynamical switching.

In the future, we would like to research on online visual positioning and environment perception topic, which is lack of discussion in this paper. We would like to enable cognitive sensing and autonomous for robots.

### Acknowledgements

This work was supported by NSFC under grant No. 61528303.

### Author details

Liang Yang1,2,3, Yuqing He1,3, Jizhong Xiao1,2,3\*, Bing Li2 and Zhaoming Liu1,3

\*Address all correspondence to<EMAIL_ADDRESS>

### References

speed. For two dynamic threats avoiding case (see in Figure 12(c)), the whole navigation time is 13.32 s, and the time spend to avoid the second threat is 0.0912 s. In scene 3, we designed a long duration for threat (see in Figure 12(d)). The two cyan threats disable the blue-path, thus robot has to switch for more times while tracking the dark path. The average time is no more than 0.15 s which can be decreased when implemented in robot's platform with C++ imple-

Figure 13. The experiment of using MGAT for 3D emergency threat management case, where pop-up threat and moving

Robot speed 10 m/s Detection range 10 m Speed of cyan obstacle (in Figure 12(b)–(c)) 1.4 m/s Speed of red obstacles (in Figure 12(c)–(d)) 0.8 m/s Speed of cyan obstacle (in Figure 12(d)) 0.8 m/s Online switching range 20 m

The main contribution of this paper is that an online EMT planner is proposed, where pop-up threat and moving obstacle happen during tracking the pre-planned path. We propose a new multiple path planning approach called MGART, which is improved based on GART, by introducing an 'Extending Forbidden' algorithm to shift the goal biased probability to neighbor branches around goal reached branch. The algorithm is shown to inherit the merits of

mentation, and the whole navigation time is 13.5 s.

6. Conclusion

threat are exist in the environment.

252 Kinematics

Table 4. Setting of simulation for ETM.

[7] Coleman D, Sucan I, Chitta S, Correll N. Reducing the barrier to entry of complex robotic

[8] Yang K, Gan SK, Sukkarieh S. A Gaussian process-based RRT planner for the exploration of an unknown and cluttered environment with a UAV. Advanced Robotics. 2013;27(6):

[9] Yoshida E, Kanehiro F. Reactive robot motion using path replanning and deformation. In: IEEE International Conference on Robotics and Automation (ICRA); 2011. pp. 5456-5462

[10] Lindemann SR, LaValle SM. Incrementally reducing dispersion by increasing Voronoi bias in RRTs. In: IEEE International Conference on Robotics and Automation. Vol. 4;

[11] Kavraki L, Latombe J-C. Randomized preprocessing of configuration for fast path planning. In: Proc. of IEEE International Conference on Robotics and Automation; 1994 May

[12] LaValle SM, Kuffner JJ Jr. Randomized kinodynamic planning. The International Journal

[13] Yang L, Xiao J, Qi J, Yang L, Wang L, Han J. GART: An environment-guided path planner for robots in crowded environments under kinodynamic constraints. International Jour-

[14] Yang L, Song D, Xiao J, Han J, Yang L, Cao Y. Generation of dynamically feasible and collision free trajectory by applying six-order Bezier curve and local optimal reshaping. In: Proc. of IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS);

[15] Karaman S, Frazzoli E. Sampling-based algorithms for optimal motion planning. The

[16] Urmson C, Simmons R. Approaches for heuristically biasing RRT growth. In: IEEE/RSJ International Conference on Intelligent Robots and Systems. Vol. 2; 2003, October. pp.

[17] Pan J, Manocha D. Fast probabilistic collision checking for sampling-based motion planning using locality-sensitive hashing. The International Journal of Robotics Research.

[18] Yang L, Qi J, Jiang Z, Song D, Han J, Xiao J. Guiding attraction based random tree path planning under uncertainty: Dedicate for UAV. In: IEEE International Conference on

[19] Levcopoulos C, Krznaric D. Quasi-greedy triangulations approximating the minimum

[20] Jaillet L, Siméon T. Path deformation roadmaps: Compact graphs with useful cycles for motion planning. The International Journal of Robotics Research. 2008;27(11–12):1175-1188

software: A moveit! case study. arXiv preprint. 2014;1404(3785):1-14

431-443

254 Kinematics

2004. pp. 3251-3257

8. pp. 2138-2145

2015 Sep 28. pp. 643-648

2016;35(12):1477-1496

1178-1183

of Robotics Research. 2001;20(5):378-400

nal of Advanced Robotic Systems. 2016;13(6):1-18

International Journal of Robotics Research. 2011;30(7):846-894

Mechatronics and Automation (ICMA); 2014 Aug 3. pp. 1182-1187

weight triangulation. Journal of Algorithms. 1998;27(2):303-338