1 Introduction
The unmanned systems have been more and more popular in recent years, including unmanned aerial vehicles supervisor , unmanned ground vehicles, autonomous ships, etc. However, the accompanying result is the various concerns, for example, the concern on environmental pollution and privacy. Among all concerns, the concern on their safety performance, namely the concern of the potential casualty caused by the unmanned system, is the first factor to consider. Evidently, the potential collision between the unmanned system and other obstacles, including both moving and static objects, accounts for a great proportion of the safety concerns.
As a matter of fact, the path planning and collision avoidance is a natural topic when it comes to the unmanned system, which has been studied for decades. RapidlyExploring Random Tree (RRT) RRT was a samplingbased singlequery path planning method, which operated by expanding a tree rooted at the start towards the new random sample from the closest node in the tree until it approached the goal. The velocity obstacle (VO) method VO and reciprocal velocity obstacle (RVO) method RVO ensured that the path was collision free given that the relative velocity was outside the VO, where RVO considered further the reaction of the obstacle. The A* algorithm A* formed an optimal path by iterative selection of the node with the less cost in the open list, where the cost was the sum of both the cost from the start to the node and the estimated cost from the node to the goal. The artificial potential field (APF) algorithm APF generated a collisionfree path by supposing that the agent moved under both attractive force by the goal and repulsive force by the obstacle.
Early researches on path planning archived satisfactory effects on deterministic cases. While the world is actually full of uncertainties. As for the unmanned system, the uncertainty may have three sources uncertainty : (1)The modeling error. The mathematical model is merely an abstract of the real world, which thus could not be totally exact; (2)The observation error. The real state of the unmanned system is unknown and could only be estimated by sensors, in which process the noise is led in; (3)The disturbances on the unmanned system. For example, the acceleration caused by wind.
In the presence of uncertainty, the unmanned system may deviate from its predetermined route and collide with the obstacle. That is why the uncertainty was incorporated in many of the recent researches. The Chance Constrained RRT (CCRRT) algorithm CCRRT was an improvement of the RRT algorithm, which ensured the probabilistic feasibility of the planned path by bounding the collision probability (CP) at each time step within a certain range. In LQGMP
, a number of candidate paths were first generated by the RRT algorithm, then the best one among them was selected with respect to the chosen planning objective based on the apriori probability distribution of the system state. The simulation showed that the quality of the candidate paths differed starkly with the uncertainty. The Probabilistic RVO (PRVO) method
PRVO improved the RVO method to assure that the CP is less than a certain threshold utilizing the Bayesian decomposition. In BUAVC , the probabilistic collision avoidance was guaranteed, namely the interrobot CP was below a userspecified threshold, by constraining each robot’s motion within a certain separated area called buffered uncertaintyaware Voronoi cell (BUAVC). PrSBC guaranteed the probabilistic safety if a certain constraint was imposed on the input. PU managed to improve the robustness of the model predictive control collision avoidance method for the autonomous ground vehicle with respect to the parametric uncertainties.These researches did take the uncertainty into consideration, while the CP, as the key measurement of safety, was not detailed. They at most gave a sufficient condition that the CP was within a certain range. For better evaluation of the safety under uncertainty, the CP should be estimated precisely. In this paper, the CP is defined mathematically as the probability that the unmanned system collides with an obstacle or another unmanned system in a certain time interval to come.
To estimate the CP, the Monte Carlo (MC) method MCbook could be utilized, which is to estimate the CP with the collision frequency, after a number of independent trials. The MC method is relatively accurate, while it generally takes a lot of time to accomplish the estimation. Therefore, a fast estimation method is needed. In condition , a collision estimation method was proposed based on conditional probability. The whole route was discretized to a number of sampling points, and a conditional probability that the collision did not occur at the current sampling point given that the collision did not occur for all previous sampling points was estimated. The CP was estimated by multiplying all the conditional probabilities. However, the CP estimation may tend to 1 with the increment of sampling points in some cases. Moreover, the method was suitable mainly for the 2dimensional collision. In PCE
, the CP was estimated based on polynomial chaos expansion. Its main idea was to approximate the solution of the system model with polynomial chaos expansion, based on which the MC method was applied to estimate the CP. However, this method applied mainly to random variables instead of random processes. In
NASA , the CP was estimated with an integral within a certain area in the case of free flight. However, the assumption that the relative velocity and error ellipse was constant made it not precise enough. Moreover, it applied mainly to the case of collision with only one obstacle. In MDTC BBN , the shipship CP was estimated with the product of the potential encounter numbers and the probability that both ships failed to evade the collision. Whereas, it was rather an overall CP than that of a specific case. For example, in SouthKorea , the CP was estimated for a scenario of maritime spent nuclear fuel transportation. The ship knew little about the predetermined route on departure: whether it would encounter another ship, where the potential encounters would occur, and which type another ship would be. The CP was estimated based on experience and historical data collected from the Automatic Identification System (AIS). Therefore, it could not estimate the CP for a specific case precisely.In this paper, we come up with a FiniteDimensional Monte Carlo (FDMC) method, which attempts to reduce the estimation time of the MC method. The MC method consumes a lot of time, mainly due to three disadvantages. First, in the MC method, a number of propagations are needed in the state generation, and dense sampling points are required to ensure precision. However, collision generally occurs on few sampling points. While, we have no idea neither on which sampling points the collide may occur, nor how could the state of a certain sampling point be generated directly without a lot of timeconsuming propagations. Second, the MC method propagates the whole state, which may include the velocity and attitude, etc. While it is merely the position that should be taken into consideration when it comes to the collision problem. Third, the MC method selects the sampling points in an equitime manner. However, there should be more sampling points where the velocity is greater and less where the velocity is smaller. Four improvements are led in in this paper to eliminate these disadvantages. First, the state is proved to be a normal process, which makes it possible to be generated directly. Second, the importance sampling filters out those sampling points that are prone to collision. These two improvements eliminate the first disadvantage – the sampling points with a great CP could now be filtered out and generated directly. In this procedure, the states are generated directly with samples of a finitedimensional distribution (FDD) instead of state propagation in the MC method. Third, the dimension reduction generates directly the position, which eliminates the second disadvantage. Last, the probabilistic equidistant sampling determines the density of the sampling points proportional to the velocity, eliminating the third disadvantage. With all these improvements, the CP estimation time has been reduced greatly, which is verified by the simulation.
The remainder of this paper is organized as follow. Some preliminaries are given in section II. The problem is formulated in section III. The FDMC method is presented in section IV. The simulation follows in section V, and the paper ends with the conclusion in section VI.
2 Preliminaries
For better clarification of our method, first, some relevant definitions and theorems are led in.
Definition 1(Probability Space): The probability space is defined as the triplet , where is the sample space, is a nonempty subset of the power set of , and the probability maps each element (defined as an event) to a real number , with ,
(1) 
Definition 2
(Normal Distribution): An
dimensional random vector
is said to obey the Normal Distribution(denoted as), if it has the probability density function(pdf)
(2) 
where is the expectation vector, and is the covariance matrix, which is positive definite. The
is defined for the standard normal distribution such that .Definition 3(Normal Process): A random process is said to be a Normal Process if any FDD of is a normal distribution, i.e.,
is normally distributed.
Definition 4(Brownian Motion): The normal process is said to be Standard Brownian Motion, if 1); 2)the increment is standard and stationary , i.e., ; 3)the increments are independent, i.e., , the increments and are independent.
Theorem 1: Suppose that is the solution to the equation
(3) 
where is a constant vector, and are constant matrices of proper size, then is a normal process with the expectation and covariance
(4) 
The proof is given in the appendix.
Theorem 2: Suppose that is the solution to
(5) 
with initial state normally distributed
(6) 
Here, are constants, and is the 1D standard Brownian motion. If the corresponding characteristic equation
(7) 
has two unequal real roots , then is a normal process with the expectation
and covariance
where and
Proof:
The solution to (5) is
and the calculation of the expectation and covariance is similar to that of Theorem 1, which is thus omitted here.
Definition 5
(ChiSquare Distribution): Suppose that
random variables are independently and identically distributed , thenis a chisquare distribution with degree of freedom (DOF)
, denoted as . The quantile is defined such that .Proposition 1: Suppose that with dimension , then is chisquared distributed with DOF
(8) 
Proof:
Notice that the positive definite covariance matrix could be decomposed as , let , then with . The conclusion is drawn directly by the definition of the chisquare distribution.
Definition 6: The ellipsoid
(9) 
is called the error ellipsoid of the dimensional normal distribution with confidence . It implies that lies in with probability
(10) 
as is shown in Fig. 1.
Definition 7(Population): A population is a set of individuals with each individual attached to a certain fitness value , where the fitness function is defined as . The cumulative frequency is defined as
(11) 
Theorem 3(Roulette Wheel Selection): With the aforementioned population , let the selection be in the case that , where
is uniformly distributed, then the probability that
is selected is proportional to its fitness(12) 
with .
The conclusion is evident and is omitted here.
Remark 1: In the situation where are continuous quantities, and the repeated selections are not expected, the selection could be modified to
(13) 
3 Modeling and Problem Formulation
3.1 CP Modelling
Consider the linear dynamic model with uncertainty of the unmanned system ,
(14) 
where with the position of the barycenter and the velocity, and there exist transform matrices such that ; is the system matrix, is a constant vector, is the uncertainty in the form of the standard Brownian Motion, which is independent with the initial state , and is the matrix which represents the properties of the uncertainty.
In the case where the channels are independent, (14) could be divided into three parts, where the dynamic model of the channel could be rewritten as
(15) 
with initial state normally distributed
(16) 
Here, are control parameters, is constant, is the 1D standard Brownian motion, and represents the property of the uncertainty. The channels are similar and omitted here.
Suppose that there exist obstacles , which, together with the unmanned system , are all considered to be 3D rigid bodies in the form of a sphere
where and are the barycenter and radius of , and are the geometric center and radius of the obstacle , and is the Euclidean norm of a vector.
To formulate the CP, first the probability space should be defined, with the sample space defined as
(17) 
namely the set of all the paths that are subject to the dynamic model (14). The collision event on is defined
(18) 
as the event that the positions of the unmanned system and the obstacle intersect. The CP could then be defined.
The exact value of the CP is not available; as a result, it should be estimated. The MC estimation could be utilized in this procedure.
3.2 CP Estimation Based on the MC Method
The main idea of the MC method is to propagate the dynamic model (14) to calculate the states at different stages and judge whether the collision occurs. After a number of independent trials, the CP is estimated with the collision frequency.
In this procedure, there are three kinds of probabilities.

Firstly, the real CP . It is the probability defined on a continuous path, which has been defined in the previous subsection.

Secondly, the discrete CP . The continuous path should be discretized to a number of sampling points that are dense enough to represent the original path. Similar with , the collision event in the discrete form could be defined as
(19) where is the th sampling point out of , and is the sampling step. The discrete CP is utilized to approximate the true CP , provided that the sampling points are dense enough.

Finally, the discrete collision frequency Fr with independent trials. The discrete CP is generally difficult to calculate directly, therefore, the MC method estimates it with the corresponding frequency Fr, defined as
(20) where
are a set of independent and identically distributed samples from the Bernoulli distribution
, which takes value 1 if occurs and 0 otherwise. In each sample, the state is propagated from to through the discrete form of (14)(21)
With the following Theorem 4, the CP could be estimated with .
Theorem 4: converges towards with probability 1
(22) 
with approximate confidence that the estimation error
(23) 
The proof is given in the appendix.
The procedure is summarised in Fig. 3. As we can see, the flow includes the inner loop and the outer loop, where the inner loop generates the states along the path and judge whether the collision occurs in this single trial. In other words, it serves to generate the sample . While the outer loop counts the total times of collision and further estimates the CP with the collision frequency for the independent trials. It is evident that the estimation time is proportional to both the number of independent trials and the number of sampling points, therefore, the time complexity of the MC method is
(24) 
3.3 Problem Formulation
With the MC method, the CP could be estimated by the collision frequency. However, as is known to us, the MC method generally consumes a lot of time. Hence, our problem is formulated as:
CP Estimation Problem: For the unmanned system subject to the dynamic model (14) and the given time interval , the CP could be estimated as by the timeconsuming MC method according to Theorem 4. Then how could an alternative method be proposed, which is equivalent with the MC method to the estimation time reduced greatly?
4 CP Estimation Based on the FDMC Method
In this section, the FDMC method is presented in three aspects. Subsection 4.1 clarifies the theory of the FDMC method, subsection 4.2 supplies details and improvements, while subsection 4.3 ends this section with the summary and discussion of the FDMC method.
4.1 Basic Theory
Theorem 4 indicates that, the number of independent trials and the number of sampling points in one trial must be great enough to ensure the precision in the MC method, which is the cause of the long estimation time. The idea of the FDMC method is to ensure the precision with a much smaller amount of sampling points to reduce the estimation time.
As is shown in Fig. 3, the MC method is implemented through the state propagation. While it has an obvious disadvantage. In practice, the collision may only occur within few states that are near the obstacle. However, to obtain these states, a number of state propagations are necessary, which is evidently a timeconsuming procedure. Here, an alternative method is introduced to replace the state propagation of the MC method, which considers the direct generation of states that are near the obstacle. This procedure is done by Theorem 1 or Theorem 2.
Theorem 1 indicates that the solution to the dynamic model (14) is a normal process. Therefore, any FDD of is normally distributed, namely,
(25) 
with
(26) 
In the case where channels are independent, it is more convenient to reduce the dimension by calculating the FDD of the position directly through Theorem 2.
Therefore, it is possible to select sampling points out of that are near the obstacles and generate them with a normal distribution instead of the state propagation, reducing a great deal of estimation time, as is shown in Fig. 4. Only these states are to be considered in the CP estimation.
Similarly, there are also two kinds of probabilities.

Firstly, the discrete CP , where the event is defined as
(27) 
Secondly, the discrete collision frequency Fr with independent trials, defined as
(28) where are a set of independently and identically distributed samples from the Bernoulli distribution
, which takes value 1 if occurs and 0 otherwise. In each sample, the states are generated directly through the normal distribution .
Theorem 5: It is assumed that with a small constant, then we have approximate confidence that the estimation error
(29) 
The proof is similar with that of Theorem 4, which is thus omitted here.
Remark 2: In Theorem 5, an assumption is made that the distance between and is small enough. As a matter of fact, on the one hand, occurs occurs; on the other hand, given that occurs, it is almost guaranteed that occurs, as are the nearest sampling points from the obstacles. In other words, the sampling points are representative. Therefore, is close to , and the assumption is reasonable.
The procedure is summarised in Fig. 5. It is shown that the inner loop is simplified by replacing times the state propagation with a sample of the FDD, while the independent trials in the outer loop are unchanged. In other words, compared with Theorem 4 of the MC method, FDMC method only requires that is great enough, namely it reduces the time complexity of the MC method to , which of course reduces a great deal of the estimation time. In the meanwhile, there is little loss of the precision, as is a small constant.
Only the improvement on the number of the sampling points are made in this paper, and the improvement on the number of the independent trials are to be studied in future researches.
4.2 Details and improvements
The basic theory of the FDMC method is presented in the previous subsection, whereas, there remain two questions to answer: 1) how is the term ”near the obstacle” defined, namely how could the sampling points be determined? 2)in which form are the sampling points organized? This subsection completes the FDMC method by answering these two questions.
4.2.1 Importance sampling
The importance sampling answers the first question by defining mathematically the term ”near the obstacle” as the points where the CP is high enough to consider. It is a filtering procedure which filters out the sampling points where the CP is high enough and ignores the others, which is carried out by Theorem 6.
Theorem 6: Recall that the position of
is normally distributed at the moment
with radius , and the center of the th obstacle is , with radius . is the error ellipsoid of with confidence . Define the augmented obstacle sphere , which includes the radii of both and . Then a sufficient condition that the CP between and is less than is that does not intersect with , namely(30) 
Proof:
Notice that the collision occurs if and only if the barycenter of lies in , namely , then
(31) 
with pdf of .
With Theorem 6, the nearest sampling points from the obstacles could be determined. There remains the last question: in which form are the sampling points organized?
4.2.2 Equidistant Sampling in Probability
In the MC method, the equitime sampling is usually applied. While it has two disadvantages. First, when the velocity of the unmanned system is rather high, it is highly likely that the discrete collision event would omit collision moment between sampling points; second, when the velocity is rather low, the sampling points are rather dense, and the estimation time is wasted. Therefore, the second question is answered with the equidistant sampling. Notice that the solution to (14) is given by Theorem 1, then the probabilistic equidistant sampling is to determine a set of sampling points such that their expected positions are equidistant
(32) 
where are a set of equidistant positions. In the case where channels are independent, the dimension could be reduced by replacing (32) with the much simpler 1D equation
(33) 
While, sometimes, it may be not easy to solve equation (32) or (33), thus an alternative method is given.
Recall that the sampling points in the MC method are selected in an equitime manner
(34) 
By Theorem 1, the expected states could be calculated, based on which the expected velocities could be obtained. Then could be obtained by independent rounds of the roulette wheel selection by Theorem 3, where the individuals are the equitime sampling points with the expected velocity as the fitness.
4.3 Summary and Discussion
Up to now, the CP Estimation Problem has been solved, the flow of which has been summarised in Table 1.
Step 1 



Step 2 


Step 3 


Step 4 

Compared with the MC method, the FDMC method could reduce a great deal of the estimation time due to the three reasons

The importance sampling, which could eliminate most of the lowcollisionprobability sampling points;

The dimension reduction, which could eliminate other parts of the state except the position.

The probabilistic equidistant sampling, which could reduce the amount of sampling points of low velocity;
5 Simulation
The quadcopter is controlled to fly in the negative direction of the axis, starting from . Along the path, there exist three other quadcopters , as is shown in Fig. 7.
In this case, it is that is in our control, and our target is to estimate the probability that collides with for some time to come. Therefore, the quadcopters are deemed the obstacles, where is hovering at the origin, and are moving along straight lines. All these quadcopters are moving within the same horizontal plane. Some other parameters are shown in Table 2.
Quadcopter radii  

Starting point of  
Starting point of  
Velocity of  1ms 
Time interval  
Trial times 
The simulation in MATLAB R2014a is carried out automatically, whereas, for better clarification, the flow is analyzed step by step. First, by Step 1, a set of probabilistic equidistant sampling points are selected and compared with the equitime ones. Fig. 8 shows how the expected position varies over time in both sampling manners.
It could be inferred that the quadcopter decelerates over time. It is not advisable to sample with the same density in and , the velocity of which is evidently much smaller than the former one. As a matter of fact, sampling points in is merely a waste of the estimation time, which is avoided thanks to the probabilistic equidistant sampling.
Second, the importance sampling of Step 2 is carried out, as is shown in Fig. 7. It is indicated that the collision may only occur on few moments: probable collision with on and on . As for the rest, they are not to be considered in Step 3 and 4. There is no doubt that the importance sampling would reduce the estimation time significantly.
After the calculation of Step 3 and 4, the CP estimation result is shown in Table 3.
CP estimation  estimation time  

FDMC  0.0049970.000138  95.792s 
MC  0.0054610.000144  20471.655s 
It is shown that the FDMC method reduces 99.53% the estimation time compared with the MC method with little loss of precision, which has verified the effectiveness of the FDMC method in the case of CP estimation with multiple static or moving obstacles.
6 Conclusion
With the development of the unmanned system, there have been more and more concerns on their safety performance, among which the collision problem is drawing more and more attention. In this paper, we come up with an FDMC method, which is much faster than the MC method in the CP estimation. First, those moments with high CP are filtered out by the importance sampling, and the states are generated directly by a sample of the normal process, without the timeconsuming propagations. Second, the dimension reduction makes it possible to generate merely the position, with the rest of the state ignored. Third, the probabilistic equidistant sampling reduces the number of sampling points with low CP. With these improvements, the estimation time has been reduced greatly. The simulation proves the effectiveness of the proposed method, which reduces over 99% the estimation time.
Actually, there remain a lot of aspects to improve. The model studied in this paper is the linear timeinvariant system, which should be extended to the systems that are more general, namely linear timevariant systems and nonlinear systems. The determination of the parameter of the uncertainty is not yet mentioned in this paper, and the method is to be modified to be datadriven. The method proposed in this paper, though improved a lot compared with the MC method, remains an MC method in nature. It could be more analytical, if the CP could be calculated by an integral of the pdf. In that case, the CP could be estimated absolutely in real time. In all, there remains a lot of work to do.
References
 (1) Quan Q. Introduction to multicopter design and control. Singapore: Springer Singapore, 2017.
 (2) Kuffner J J, LaValle S M. RRTconnect: An efficient approach to singlequery path planning//Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065). IEEE, 2000, 2: 9951001.
 (3) Fiorini P, Shiller Z. Motion planning in dynamic environments using velocity obstacles. The International Journal of Robotics Research, 1998, 17(7): 760772.
 (4) Van den Berg J, Lin M, Manocha D. Reciprocal velocity obstacles for realtime multiagent navigation//2008 IEEE International Conference on Robotics and Automation. IEEE, 2008: 19281935.

(5)
Hart P E, Nilsson N J, Raphael B. A formal basis for the heuristic determination of minimum cost paths. IEEE transactions on Systems Science and Cybernetics, 1968, 4(2): 100107.
 (6) Khatib O. RealTime Obstacle Avoidance for Manipulators and Mobile Robots. The International Journal of Robotics Research, 1986, 5(1): 90.
 (7) Blackmore L, Li H, Williams B. A probabilistic approach to optimal robust path planning with obstacles//American Control Conference, 2006. IEEE, 2006: 7pp.
 (8) Luders B, Kothari M, How J. Chance constrained RRT for probabilistic robustness to environmental uncertainty//AIAA guidance, navigation, and control conference. 2010: 8160.
 (9) Van Den Berg J, Abbeel P, Goldberg K. LQGMP: Optimized path planning for robots with motion uncertainty and imperfect state information. The International Journal of Robotics Research, 2011, 30(7): 895913.
 (10) Gopalakrishnan B, Singh A K, Kaushik M, et al. Prvo: Probabilistic reciprocal velocity obstacle for multi robot navigation under uncertainty//2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017: 10891096.
 (11) Zhu H, AlonsoMora J. Buavc: Buffered uncertaintyaware voronoi cells for probabilistic multirobot collision avoidance//2019 International Symposium on MultiRobot and MultiAgent Systems (MRS). IEEE, 2019: 162168.
 (12) Luo W, Kapoor A. MultiRobot Collision Avoidance under Uncertainty with Probabilistic Safety Barrier Certificates. arXiv preprint arXiv:1912.09957, 2019.
 (13) Liu J, Jayakumar P, Stein J L, et al. A doubleworstcase formulation for improving the robustness of an MPCbased obstacle avoidance algorithm to parametric uncertainty//2017 American Control Conference (ACC). IEEE, 2017: 55625567.
 (14) Kroese D P, Chan J C C. Monte Carlo sampling//Statistical Modeling and Computation. Springer, New York, NY, 2014.
 (15) Patil S, Van Den Berg J, Alterovitz R. Estimating probability of collision for safe motion planning under Gaussian motion and sensing uncertainty//2012 IEEE International Conference on Robotics and Automation. IEEE, 2012: 32383244.
 (16) Jones B A, Doostan A. Satellite collision probability estimation using polynomial chaos expansions. Advances in Space Research, 2013, 52(11): 18601875.
 (17) Paielli R A, Erzberger H. Conflict probability estimation for free flight. Journal of Guidance, Control, and Dynamics, 1997, 20(3): 588596.
 (18) Montewka J, Hinz T, Kujala P, et al. Probability modelling of vessel collisions. Reliability Engineering & System Safety, 2010, 95(5): 573589.
 (19) Christian R, Kang H G. Probabilistic risk assessment on maritime spent nuclear fuel transportation (Part II: Ship collision probability). Reliability Engineering & System Safety, 2017, 164: 136149.
 (20) Goerlandt F, Kujala P. Traffic simulation based ship collision probability modeling. Reliability Engineering & System Safety, 2011, 96(1): 91107.
 (21) Hnninen M, Kujala P. Influences of variables on ship collision probability in a Bayesian belief network model. Reliability Engineering & System Safety, 2012, 102: 2740.
Comments
There are no comments yet.