[发明专利]一种基于有限维分布的碰撞概率实时评估方法

权利要求书

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-o_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)为

其中Σ_k＝Cov[p_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)中求得的Cov[p(t_k),p(t_k)]],k＝1,2,…,N，判断是否满足障碍物圆心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，进行下一次循环。