CN109635398A - 一种基于有限维分布的碰撞概率实时评估方法 - Google Patents

Abstract

本发明涉及一种基于有限维分布的碰撞概率实时评估方法，包括如下步骤：步骤1：建立待评估系统控制模型以及碰撞模型。步骤2：根据待预测时间区间，生成等时间间隔取样点；步骤3：重复进行轮盘赌，生成依概率等距离分布的取样点；步骤4：基于待评估系统的控制模型，求解质心位置的有限维分布；步骤5：对质心位置有限维分布的协方差矩阵进行Cholesky分解，得到其具体生成方式。步骤6：预测障碍物刚体位置并求解经过滤的有限维分布；步骤7：重构系数矩阵得到经过滤的有限维分布的具体生成方式；步骤8：对步骤7得到的有限维分布进行若干次独立生成，以离散点的碰撞频率估计真实概率。之后的下一个评估决策时刻，算法返回步骤6继续进行计算。

Description

一种基于有限维分布的碰撞概率实时评估方法

技术领域

本发明涉及一种基于有限维分布的碰撞概率实时评估方法，该发明属于安全评估领域。

背景技术

随着微小型无人飞行器的发展，低空域中的无人飞行器安全评估越来越重要，而多旋翼则是近些年来微小型无人机的一个典型代表。关于多旋翼空中安全性能的担忧，已经成为多旋翼在人口密集区域各种应用进一步发展的一个瓶颈。而关于多旋翼安全性能的诸多担忧之中，其可能的空中碰撞问题，包括多旋翼与高楼大树等静止障碍物碰撞，以及与空域中其他无人飞行器等运动物体的碰撞，显然占据了重要地位。因此多旋翼的防碰撞问题尤为重要。目前航空领域应用几何计算法、最优路径法、人工势场法等对多旋翼进行路径规划，为多旋翼进行路径规划以避开障碍物，可是在实际飞行中，建模误差、估计误差以及外界扰动等不确定性可能使多旋翼的实际轨迹偏离预定轨迹，撞上障碍物。因此，多旋翼在有不确定性情况下的空中碰撞概率评估具有重要研究价值。

本文以多旋翼为例引出碰撞概率评估，而实际上，碰撞问题以及随之而来的不确定情况下的碰撞概率评估，广泛存在于在无人控制系统中，如无人车、无人船等，而不仅仅局限于多旋翼和无人机。对各种民用无人系统而言，安全性能无疑是其发展壮大的一个硬性条件，尤其是在人口密集区域，这种刚性需求体现得更加强烈。安全评估，以及作为其重要组成部分的碰撞概率评估的重要意义不言而喻。

碰撞概率评估的一种自然的方法是应用蒙特卡罗方法进行多次独立计算，以碰撞频率估计碰撞概率。蒙特卡罗方法思想简单，评估准确，但是计算量太大，难以满足实时性能。

本发明提出了一种基于有限维分布的碰撞概率实时评估方法，以多旋翼的空中碰撞为例，实现碰撞概率的在线评估，评估得到的碰撞概率可以进一步应用于无人系统的安全决策等。

发明内容

本发明给出了一种基于有限维分布的碰撞概率实时评估方法。该方法是对单纯蒙特卡罗方法的改进。首先将待评估系统的动力学模型进行预处理，得到该系统状态的有限维分布，并基于该有限维分布进行独立蒙特卡罗仿真，以碰撞频率近似碰撞概率实时估计待评估系统的碰撞概率。

本发明提出了一种基于有限维分布的碰撞概率实时评估方法，具体步骤如下：

步骤1：建立待评估系统控制模型以及碰撞模型

首先为待评估系统M设计控制器，建立含不确定性的模型

其中x(t)为待评估系统M在t时刻的状态向量，d为微分符号，B(t)为建模为标准布朗运动的噪声，为经反馈的系统状态矩阵，c为经反馈的常向量，Σ为表征噪声统计特性的矩阵。这里，状态向量x(t)至少包括待评估系统M在t时刻的质心位置p(t)以及质心速度v(t)，即存在状态向量x(t)到质心位置p(t)以及质心速度v(t)的线性映射

其中T_p,T_v为对应的线性变换矩阵。此外，初始状态x(0)

服从期望μ₀和方差Σ₀的正态分布且与噪声B(t)独立。

其次建立待评估系统M的碰撞模型。考虑二维平面上的碰撞，将待评估系统M和障碍物均建模为二维平面上的平面刚体。如将待评估系统M建模为以质心为圆心的平面圆形刚体，则待评估系统M的刚体位置以平面点集描述

M(t)＝{d|||d-p(t)||₂≤r} (4)

其中d为平面点集元素，r为待评估系统M的半径，p(t)为待评估系统M的质心位置，||·||₂为向量的2-范数。假设存在k_O个圆形障碍物O_j,j＝1,2,…,k_O，则障碍物O_j在t时刻的刚体位置为平面点集

O_j(t)＝{d|||d-c_j(t)₂≤R_j} (5)

其中o_j(t)和R_j分别为障碍物O_j的圆心和半径。定义碰撞事件为

即为事件：在时间区间[0,T]上存在时刻t，使得障碍物O_j与待评估系统M刚体位置相交。我们要评估的即为概率Pr(C_T)。

步骤2：根据待预测时间区间，生成等时间间隔采样点；

我们的待评估时间区间为[0,T]，在其中均匀取N_uni个时刻

t_k,uni＝kτ,k＝1,2,…,N_uni (7)

其中

为足够小的时间间隔，如τ≤0.1s。

步骤3：重复进行轮盘赌，生成依概率等距离分布的取样点

待评估系统M在不同采样时刻点的质心速度是不同的。在质心速度较大的采样时刻点，应减小采样时刻点的间隔以避免漏掉中间的碰撞时刻；反之，在质心速度较小的采样时刻点，则应增大采样时刻点的间隔以避免浪费计算量。因此步骤2中的采样方法并不是最佳方案。如果能等距离取采样点是一种比较好的方案，但是在有不确定性的情况下，我们只能依概率等距离采样。

首先根据待评估系统M的控制模型(1)求解状态向量x(t)的期望

其中t_k,uni即为步骤2中求解的等时间间隔采样点。由式(2)中速度v(t)与状态向量x(t)的关系v(t)＝T_vx(t)，我们可以求解出t_k,uni时刻的质心期望速度

以质心期望速度大小对采样时刻点进行重新生成。为此，进行N次(N由待评估系统M的预计运动距离确定，不必与由预计运动时间确定的N_uni相等)轮盘赌。每次独立轮盘赌，生成一个[0,1]区间上的均匀分布随机数rand，如果

CF_l-1≤rand＜CF_l (10)

其中

为累计频率，即本次轮盘赌转到了采样时刻点t_l,uni对应的扇形，那么本次轮盘赌取采样时刻点t_l,uni，如图1所示。这样，采样点t_l,uni在一次轮盘赌中被选取的概率

正比于t_k,uni时刻的质心期望速度而且显然

为避免采样时刻点重复，根据随机数rand在区间[CF_l-1,CF_l)上的位置选取上的均匀采样时刻点。

由此确定的t_l即是最终经速度修正后的采样时刻点。这样选取的N个采样时刻点物理意义鲜明，即依概率等距离取样。

步骤4：基于待评估系统的控制模型，求解质心位置的有限维分布

基于待评估系统M的控制模型(1)，可以求解状态向量x(t)的期望向量和协方差矩阵

其中t,s∈[0,T]为给定时间区间[0,T]内的两个时刻，x(t),x(s)分别为对应时刻的状态向量。由式(2)中质心位置p(t)与状态向量x(t)的关系p(t)＝T_px(t)，可以进一步得到质心位置p(t)的期望向量和协方差矩阵

其中p(s)为s时刻待评估系统M的质心位置。由此可以得到质心位置的一个有限维分布其中

分别为质心位置有限维分布P的期望向量和协方差矩阵，t₁,t₂,…t_N为步骤3中依概率等距离取样的采样时刻点，p(t₁),p(t₂),…p(t_N)分别为对应时刻待评估系统M的质心位置。

注意区分，小写的p为待评估系统M的质心位置，且常常注明时刻p(t)，大写的P为待评估系统质心位置p的有限维分布，两者并不相同。

步骤5：对质心位置有限维分布的协方差矩阵进行Cholesky分解，得到其具体生成方式

将有限维分布P的协方差矩阵进行Cholesky分解，即求解矩阵L使得

Σ_P＝LL^T (18)

求解结束后，对有限维分布P按以下步骤生成

(1)独立生成2N个标准正态随机数Y_k,k＝1,2,…,2N，其中N为步骤3中的采样时刻数；

(2)令中间变量Y＝[Y₁ Y₂ … Y_2N]^T，

则Y有期望向量和协方差矩阵μ_Y＝0_2N×1,Σ_Y＝I_2N，这里I_2N为2N阶的单位矩阵；

(3)P＝LY+μ_P。

按步骤(1)-(3)生成的有限维分布P，期望为μ_P，协方差为LΣ_YL^T＝Σ_P，与原分布一致。

步骤6：预测障碍物位置并求解经过滤的有限维分布

待评估系统M可能会运行很长一段路径，其中只有很短的一部分接近障碍物可能发生碰撞，而剩下的大部分时间远离障碍物，根本没有碰撞可能性，这就需要我们事先经过一步过滤，滤掉几乎没可能碰撞的采样时刻点，只留下碰撞概率超过一定阈值的采样时刻点。

为进行步骤6的阐述，首先引入一些概念。

在t_k时刻，简记质心位置p(t_k)为

其中分别为p_k的期望和协方差矩阵。

可以证明服从二自由度的卡方分布

称椭圆

为待评估系统质心位置p_k对应于α的误差椭圆，其中为二自由度卡方分布α对应的分位点，即满足概率且可以证明

记半长轴长为a_α，称和共焦点共长轴，且半长轴长为a_α+r+R_j的椭圆为增广误差椭圆。

称以障碍物圆心o_j为圆心，以待评估系统与障碍物半径之和aα+r+R_j为半径的圆为增广障碍圆O_a,j。显然，待评估系统M在t时刻与障碍物发生碰撞等价于

p(t)∈O_a,j (21)

可以证明，待评估系统在t_k时刻碰撞概率小于α的一个充分条件是障碍物圆心o_j在增广误差椭圆外，如图2所示。由此，

遍历(17)中求得的Cov[p(t_k),p(t_k)]],k＝1,2,…,N，判断是否满足障碍物圆心o_j在增广误差椭圆外，如满足，则舍掉。

如是，我们得到N′个采样时刻点t′_k,k＝1,2,…,N′。由此将质心位置有限维分布过滤为其中

分别为质心位置有限维分布P′的期望向量和协方差矩阵。

待评估系统在运动过程中，往往只会在短暂瞬间靠近障碍物，所以一般有N′＜＜N，如图3所示。

步骤7：重构系数矩阵得到经过滤的有限维分布的具体生成方式

经步骤6，全有限维分布P被简化为经过滤的有限维分布P′。过滤完成之后，应当重新组织系数矩阵L。方法很简单，为避免复杂情况下符号的滥用，这里仅以一个简单例子说明重构L至L′的方法。

假设原有限维分布P＝[P₁ P₂ … P₈]^T通过P＝LY生成，其中

中间变量Y＝[Y₁ Y₂ … Y₈]^T的各个分量为独立标准正态随机数。过滤后仅剩P′＝[P₃ P₄]^T两个采样时刻点。令

Y′＝[Y₁ Y₂ Y₃]^T，则P′＝L′Y′显然与原有限维分布P中的分量P′＝[P₃ P₄]^T具有同样的分布。

步骤8：对步骤7得到的有限维分布进行若干次独立生成，以离散点的碰撞频率估计真实概率

本发明中的方法实质上还是对于蒙特卡罗方法的改进，所以最终还是要进行蒙特卡罗方法的若干次独立生成。步骤6中得到了待评估采样时刻质心位置的有限维分布步骤7中得到了该有限维分布的具体生成方式，现对该分布进行S次独立生成

P′_i＝[p_i(t′₁) p_i(t′₂) … p_i(t′_N)]^T (25)

其中p_i(t′_k)为第i次独立路径生成时，待评估系统在t′_k时刻的质心位置，i＝1,2,…,S,k＝1,2,…,N′。定义一个指示第i次独立路径生成是否发生碰撞的随机变量C_i，满足

即若第i次独立路径生成存在碰撞时刻则C_i＝1，无碰撞则C_i＝0。以碰撞频率近似碰撞概率

其中C_T为步骤1中定义的碰撞事件。

如图6所示，步骤1-5是离线计算部分，而步骤6-8是在线计算部分。当步骤8计算完后，下一个评估决策时刻又返回到步骤6，进行下一次循环。

优点及功效：本发明给出一种基于有限维分布的碰撞概率实时评估方法。该方法的优点是：解决了碰撞概率的实时评估问题，可以实现碰撞概率的在线评估并将其作为进一步安全决策的依据。

本文算法相对于单纯蒙特卡罗方法的优势有

(1)将待评估系统的固有属性——控制模型放到离线计算中，避免重复计算，大大简化了运算量；

(2)基于解析方法而非数值计算，所以解析方法固有的高于数值计算的精度，且精度不受取样间隔的影响；

(3)可以只考虑与碰撞有关的状态，而不考虑与碰撞无关的状态(只对位置进行生成)；

(4)可以依概率等距离取样，避免了待评估系统速度低下时对计算时间的浪费(即轮盘赌依概率等距离确定采样时刻点)；

(5)可以不经过前面的计算直接计算最易碰撞部分，节省大量运算(采样时刻点过滤)。

附图说明

图1：轮盘赌示意图。

图2：采样时刻点过滤方法示意图。

图3：采样时刻点过滤结果示意图。

图4：任务仿真场景示意图。

图5：依概率等距离分布的采样时刻点示意图。

图6：本发明流程框图。

图中符号说明

图1：t_k,uni为步骤2中的等时间间隔采样点，θ_k为t_k,uni对应轮盘赌扇形圆心角，其正比于t_k,uni时刻的期望速度大小

图2：图中所有概念都来源于步骤6，其中O_a,j为增广障碍圆，其圆心为o_j，半径为待评估系统半径r和障碍物半径R_j之和；α为单采样时刻点碰撞概率阈值，为待评估系统质心位置的误差椭圆，a_α和b_α分别为其半长轴长和半短轴长；为增广误差椭圆，与共焦点共长轴，其半长轴长和半短轴长分别为a′和b′

图3：N为步骤3中依概率等距离分布的采样时刻点数量，N′为步骤6中经过滤后的采样时刻点数量。

图4：r为待评估多旋翼半径，r_s为静止多旋翼半径，r_d为运动多旋翼，后两个多旋翼不为我们所控制，因此都视为障碍物。

具体实施方式

本发明提供了一种基于有限维分布的碰撞概率实时评估方法。以研究多旋翼在一个空域中避开其他悬停或者飞行多旋翼直线飞行为例，对本发明的具体实施方式进行进一步说明。下面介绍根据本发明中提到的方法对多旋翼空中碰撞概率进行评估的流程。

(1)方法具体实施步骤

步骤一：建立多旋翼控制模型以及碰撞模型

为建立形如式(1)的控制模型，首先建立地面坐标系O_gx_gy_gz_g。在地面上任选一点O_g作为坐标系原点，O_gz_g轴垂直于地面指向地心，O_gx_g轴在水平面内任取，O_gy_g轴也在水平面内且满足右手定则。此处，待评估系统M即为我们控制的多旋翼，以下记为多旋翼M。考虑其定高飞行，即在二维平面上的飞行。多旋翼M在t时刻的质心位置为

其中x_g(t)和y_g(t)分别为多旋翼t时刻质心位置沿O_gx_g轴、O_gy_g的坐标。多旋翼M在t时刻的质心速度v(t)为质心位置p(t)的变化速率

其中和分别为x_g(t)和y_g(t)的变化速率，定义为

其中d为微分符号。

基于以上描述，由牛顿第二定律建立多旋翼的控制模型如下

dx(t)＝Ax(t)dt+F[u(t)dt+GdB(t)] (31)

其中

为状态向量，右上角的符号T表示矩阵转置，如

矩阵

分别是多旋翼M的状态矩阵和输入矩阵，m为多旋翼M的质量，控制输入u(t)＝[F_x(t) F_y(t)]^T分别为多旋翼M在地面坐标系O_gx_gy_gz_g下沿O_gx_g轴和O_gy_g轴受力大小，dB(t)是建模为二维标准布朗运动微分的噪声，G是表征噪声统计特性的对角矩阵，状态初值x(0)

服从期望μ₀和方差Σ₀的正态分布且与噪声B(t)独立。

多旋翼M的控制目标是x_d。为此，设计PD控制器。控制误差为

x_e(t)＝x(t)-x_d (36)

增益矩阵为

其中k_p,k_d分别为比例系数和微分时间常数。设计PD控制器为

u(t)＝Kx_e(t)＝Kx(t)-Kx_d (38)

上式代回(31)得

dx(t)＝(A+FK)x(t)dt-FKx_ddt+FGdB(t) (39)

令c＝-FKx_d,Σ＝FG，此处三量为经反馈代换后的新参数，我们得到形如式(1)的多旋翼M控制模型

我们控制多旋翼M使其从初始位置(10m,2m)直线飞到(-10m,2m)，即状态初值x(0)的期望μ₀＝[10m 2m]^T，控制目标x_d＝[-10m 2m]^T。起飞时检测到航路附近还有两个多旋翼，其中一个半径为r_s＝1m的多旋翼于原点(0,0)附近悬停，另一个半径为r_d＝0.3m的多旋翼质心位置在(4m,5m)在正在以大小为4m·s^-1方向沿O_gy_g轴负方向的速度飞行，如图4所示。当然，相对于我们控制的多旋翼而言，它们都被视为障碍物，静止障碍物和运动障碍物。其他仿真参数如

表1所示。

表1仿真参数

注意区分，本发明中的m为国际单位制中物理量“质量”的符号，m为国际单位制中长度单位“米”的符号，M则为待评估系统的代号，具体到这里，待评估系统为多旋翼，则作为多旋翼的代号，三者并不相同；s为国际单位制中时间单位“秒”的符号，S为独立蒙特卡洛仿真的次数，两者并不相同。

步骤二：根据待预测时间区间，生成等时间间隔采样点

我们的待评估时间区间为[0,100s]，在其中均匀取N_uni＝1000个时刻

t_k,uni＝kτ,k＝1,2_,…,N_uni (41)

其中τ＝0.1s。

步骤三：重复进行轮盘赌，生成依概率等距离分布的取样点

首先，根据多旋翼M的控制模型(40)求解状态向量x(t)的期望

其次，式(2)中质心速度v(t)与状态向量x(t)的关系为v(t)＝T_vx(t)，由质心速度v(t)的定义(29)以及状态向量x(t)的定义(32)，这里有

由此计算期望速度大小并进行N次轮盘赌生成依概率等距离分布的取样点。多旋翼的预期飞行距离只有20m，因此这里选择N＝200个依概率等距离采样时刻点，即依概率每0.1m选取一个采样时刻点，这已经具有较高精度。如图5所示。

步骤四：基于多旋翼的控制模型，求解质心位置的有限维分布

式(2)中质心位置p(t)与状态向量x(t)的关系为p(t)＝T_px(t)，由质心p(t)的定义(28)以及状态向量x(t)的定义(32)，这里有

根据状态向量(15)计算质心位置(16)的有限维分布计算结果期望向量μ_P为一个400维(200个采样时刻点，位置为2维)向量，协方差矩阵Σ_P为一个400×400维的正定矩阵。

步骤五：对质心位置有限维分布的协方差矩阵进行Cholesky分解，得到其具体生成方式

将有限维分布P的协方差矩阵Σ_P进行Cholesky分解，即求解矩阵L使得

Σ_P＝LL^T (45)

求得的L为与Σ_P同维度，即400×400维的下三角矩阵。

求解结束后，对有限维分布P按以下步骤生成

(1)独立生成400个标准正态随机数Y_k,k＝1,2,…,400；

(2)令中间变量Y＝[Y₁ Y₂ … Y₄₀₀]^T，

则Y有期望向量和协方差矩阵μ_Y＝0_400×1,Σ_Y＝I₄₀₀，这里I₄₀₀为400阶的单位矩阵；

(3)P＝LY+μ_P。

按以上步骤生成的有限维分布P，期望为μ_P，协方差为LΣ_YL^T＝Σ_P与原分布一致。

步骤六：预测障碍物位置并求解经过滤的有限维分布

如果采取单阈值过滤，那么如果该阈值设置过小，容易使经过滤后的待评估采样时刻点数N′过高，导致计算量的浪费；而如果阈值设置过大，又可能使太多采样时刻点被过滤掉导致N′过低甚至为0。为避免这种由阈值设置不合理导致的不理想结果，可以采用多阈值过滤。

我们采取三个碰撞概率阈值0.05,0.01,0.001对采样时刻点进行过滤。当阈值为0.05时，没有任何一个取样点的单点碰撞概率达到阈值；算法自动跳转到第二个阈值0.01。有22个点达到单点碰撞概率阈值0.01。

步骤七：重构系数矩阵得到经过滤的有限维分布的具体生成方式

经步骤6，400维的全有限维分布P被简化为经过滤的44维(22个采样时刻点，位置为2维)有限维分布P′。过滤完成之后，应当重新组织系数矩阵L至L′。步骤如前所述，得到P′的生成方式P′＝L′Y′。

步骤八：对步骤七得到的有限维分布进行若干次独立生成，以离散点的碰撞频率估计真实概率

对P′进行S＝1000次独立生成，并以碰撞频率估计碰撞概率。评估结果碰撞概率约为0.003，在线部分耗时约0.15s。仿真验证了发明的快速性。

(2)与单纯蒙特卡罗方法的比较

本发明的方法实质上是对蒙特卡罗方法在时间和精度上的改进，因此与单纯蒙特卡罗方法进行比较，以验证本发明的准确性以及快速性。

仿真的一些结果如表2所示。改变静止障碍物半径r_s和运动障碍物半径r_d，得到不同情况下的仿真结果。注意到单纯的蒙特卡罗方法作为一种纯数值计算方法，其精度与采样间隔息息相关，采样间隔越小精度越高，因此这里对两种采样间隔(τ＝0.1s,0.01s)进行了仿真。

结果显示，本发明比采样间隔为τ＝0.1s的蒙特卡罗方法精度要高，同时耗时还减少了99.69％；和采样间隔τ＝0.01s的蒙特卡罗方法精度相仿，但耗时却降低了99.97％。以上分析验证了发明的可行性。

表2碰撞概率仿真结果

Claims (6)

1.一种基于有限维分布的碰撞概率实时评估方法，其特征在于：具体步骤如下：

步骤1：建立待评估系统控制模型以及碰撞模型

首先，为待评估系统M设计控制器，建立含不确定性的模型

其中，x(t)为待评估系统M在t时刻的状态向量，d为微分符号，B(t)为建模为标准布朗运动的噪声，为经反馈的系统状态矩阵，c为经反馈的常向量，Σ为表征噪声统计特性的矩阵；这里，状态向量x(t)至少包括待评估系统M在t时刻的质心位置p(t)以及质心速度v(t)，即存在状态向量x(t)到质心位置p(t)以及质心速度v(t)的线性映射

其中，T_p,T_v为对应的线性变换矩阵；此外，初始状态x(0)

服从期望μ₀和方差Σ₀的正态分布且与噪声B(t)独立；

其次，建立待评估系统M的碰撞模型；将待评估系统M建模为以质心为圆心的平面圆形刚体，则待评估系统M的刚体位置以平面点集描述

M(t)＝{d|||d-p(t)||₂≤r} (4)

其中，d为平面点集元素，r为待评估系统M的半径，p(t)为待评估系统M的质心位置，||·||₂为向量的2-范数；假设存在k_O个圆形障碍物O_j,j＝1,2,…,k_O，则障碍物O_j在t时刻的刚体位置为平面点集

O_j(t)＝{d|||d-c_j(t)||₂≤R_j} (5)

其中，o_j(t)和R_j分别为障碍物O_j的圆心和半径；定义碰撞事件为

在时间区间[0,T]上存在时刻t，使得障碍物O_j与待评估系统M刚体位置相交；要评估的即为概率Pr(C_T)；

步骤2：根据待预测时间区间，生成等时间间隔采样点；

待评估时间区间为[0,T]，在其中均匀取N_uni个时刻

t_k,uni＝kτ,k＝1,2,…,N_uni (7)

其中，

步骤3：重复进行轮盘赌，生成依概率等距离分布的取样点

首先，根据待评估系统M的控制模型(1)求解状态向量x(t)的期望

其中，t_k,uni即为步骤2中求解的等时间间隔采样点；由式(2)中质心速度v(t)与状态向量x(t)的关系v(t)＝T_vx(t)，

求解出t_k,uni时刻的质心期望速度

以质心期望速度大小对采样时刻点进行重新生成；为此，进行N次轮盘赌；每次独立轮盘赌，生成一个[0,1]区间上的均匀分布随机数rand，如果

CF_l-1≤rand＜CF_l (10)

其中，

为累计频率，即本次轮盘赌转到了采样时刻点t_l,uni对应的扇形，那么本次轮盘赌取采样时刻点t_l,uni，采样点t_l,uni在一次轮盘赌中被选取的概率

正比于t_k,uni时刻的质心期望速度而且显然

为避免采样时刻点重复，根据随机数rand在区间[CF_l-1,CF_l)上的位置选取上的均匀采样时刻点；

由此确定的t_l即是最终经速度修正后的采样时刻点；依概率等距离取样；

步骤4：基于待评估系统的控制模型，求解质心位置的有限维分布

基于待评估系统M的模型公式(1)，求解状态向量x(t)的期望向量和协方差矩阵

其中，t,s∈[0,T]为给定时间区间[0,T]内的两个时刻，x(t),x(s)分别为对应时刻的状态向量；由式(2)中质心位置p(t)与状态向量x(t)的关系p(t)＝T_px(t)，进一步得到质心位置p(t)的期望向量和协方差矩阵

其中，p(s)为s时刻待评估系统M的质心位置；由此得到质心位置的一个有限维分布其中

分别为质心位置有限维分布P的期望向量和协方差矩阵，t₁,t₂,…t_N为步骤3中依概率等距离取样的采样时刻点，p(t₁),p(t₂),…p(t_N)分别为对应时刻待评估系统M的质心位置；

步骤5：对质心位置有限维分布的协方差矩阵进行Cholesky分解，得到其具体生成方式

将有限维分布P的协方差矩阵进行Cholesky分解，即求解矩阵L使得

Σ_P＝LL^T (18)

步骤6：预测障碍物位置并求解经过滤的有限维分布

为进行步骤6的阐述，首先引入一些概念；

在t_k时刻，简记质心位置p(t_k)为

其中分别为p_k的期望和协方差矩阵；

证明服从二自由度的卡方分布

称椭圆

为待评估系统质心位置p_k对应于α的误差椭圆，其中为二自由度卡方分布α对应的分位点，即满足概率且证明

记半长轴长为a_α，称和共焦点共长轴，且半长轴长为a_α+r+R_j的椭圆为增广误差椭圆；

称以障碍物圆心o_j为圆心，以待评估系统与障碍物半径之和a_α+r+R_j为半径的圆为增广障碍圆O_a,j；显然，待评估系统M在t时刻与障碍物发生碰撞等价于

p(t)∈O_a,j (21)

待评估系统在t_k时刻碰撞概率小于α的一个充分条件是障碍物圆心o_j在增广误差椭圆外，

遍历(17)中求得的判断是否满足障碍物圆心o_j在增广误差椭圆外，如满足，则舍掉；如是，得到N′个采样时刻点t′_k,k＝1,2,…,N′；由此将质心位置有限维分布过滤为其中

分别为质心位置有限维分布P′的期望向量和协方差矩阵；

步骤7：重构系数矩阵得到经过滤的有限维分布的具体生成方式

经步骤6，全有限维分布P被简化为经过滤的有限维分布P′；过滤完成之后，应当重新组织系数矩阵L；假设原有限维分布P＝[P₁ P₂…P₈]^T通过P＝LY生成，其中

中间变量Y＝[Y₁ Y₂…Y₈]^T的各个分量为独立标准正态随机数；过滤后仅剩P′＝[P₃ P₄]^T两个采样时刻点；令

Y′＝[Y₁ Y₂ Y₃]^T，则P′＝L′Y′显然与原有限维分布P中的分量P′＝[P₃ P₄]^T具有同样的分布；

步骤8：对步骤7得到的有限维分布进行若干次独立生成，以离散点的碰撞频率估计真实概率

步骤6中得到了待评估采样时刻质心位置的有限维分布步骤7中得到了该有限维分布的具体生成方式，现对该分布进行S次独立生成P_i′＝[p_i(t′₁)p_i(t′₂)…p_i(t′_N′)]^T (25)

其中，p_i(t′_k)为第i次独立路径生成时，待评估系统在t′_k时刻的质心位置，i＝1,2,…,S,k＝1,2,…,N′；定义一个指示第i次独立路径生成是否发生碰撞的随机变量C_i，满足

即若第i次独立路径生成存在碰撞时刻则C_i＝1，无碰撞则C_i＝0；以碰撞频率近似碰撞概率

其中，C_T为步骤1中定义的碰撞事件；

步骤1-5是离线计算部分，而步骤6-8是在线计算部分；当步骤8计算完后，下一个评估决策时刻又返回到步骤6，进行下一次循环。

2.根据权利要求1所述的一种基于有限维分布的碰撞概率实时评估方法，其特征在于：τ≤0.1s。

3.根据权利要求1所述的一种基于有限维分布的碰撞概率实时评估方法，其特征在于：待评估系统在运动过程中，只会在短暂瞬间靠近障碍物，所以有N′<<N。

4.根据权利要求1或3所述的一种基于有限维分布的碰撞概率实时评估方法，其特征在于：N由待评估系统M的预计运动距离确定，不必与由预计运动时间确定的N_uni相等。

5.根据权利要求1所述的一种基于有限维分布的碰撞概率实时评估方法，其特征在于：考虑二维平面上的碰撞，将待评估系统M和障碍物均建模为二维平面上的平面刚体。

6.根据权利要求1所述的一种基于有限维分布的碰撞概率实时评估方法，其特征在于：对有限维分布P按以下步骤生成：

5.1独立生成2N个标准正态随机数Y_k,k＝1,2,…,2N，其中，N为步骤3中的采样时刻数；

5.2令中间变量Y＝[Y₁ Y₂…Y_2N]^T，则Y有期望向量和协方差矩阵μ_Y＝0_2N×1,Σ_Y＝I_2N，这里I_2N为2N阶的单位矩阵；

5.3P＝LY+μ_P；

按以上步骤5.1-5.3生成的有限维分布P，期望为μ_P，协方差为LΣ_YL^T＝Σ_P，与原分布一致。