CN114387462B 一种基于双目相机的动态环境感知方法 （中山大学）

本发明公开了一种基于双目相机的动态环的聚类簇对应的障碍物属于动态障碍物或者静机自身运动的复杂情况下对障碍物的动静属性2对不同帧的所述稀疏点云中表示相同障碍物的多个所述聚类簇识别关联后的所述聚类簇对应的障碍物属于动态障碍物间为t的所述稀疏点云聚类得到，其中采集时间为t的所述稀疏点云由拍摄时间为t的所述B4c.如果即与kj互为最近邻，通过公式把关联到障碍物骤B5a-B5b：3物，建立障碍物跟踪历史，所述障碍物跟踪历史表示为其中域距离阈值ε和连通阈值δ,初始化核心对象集合初始化聚类簇数k＝0，初始化未访问样本集合F＝D，初始化簇划分C=;A2b.如果所述子样本点集Nε(xj)的样本个数满足|Nε(xj)|≥MinPts，则计算点Q＝Nε(xj)∪xj的连通分支数n；如果n<δ,通过公式Ω=Ω∪{xj}将样本xA3.如果核心对象集合Q=0,则结束对步骤A1-A6的执4刻的位姿Ot，获取其中p表示世界坐minC4.对聚类簇cf中的每个点p执行以下投票过程：C4b.在稠密点云中查找点p的最近邻nn；当其中length(p,nC4c.根据公式static＝static+1对变量C5.当将聚类簇cf对应的障碍物识别为属示聚类簇cf的质心，length(o,centroid(cf))表示centroid(cf)与Ot之间的距离，D5.计算qt-Δt周围第一范围内的平均深度roundAvg(Dt-Δt[qt-Δt])；位置到点p的深度ir-se:其中|.|z表示z方向的坐标值；D9.计算qt-Δt周围第二范围内的平均深度roundAvg(Dt-Δt[qt-Δt])；D10.如果⃞-srle-roundAvg(D:-selar-sel)>F(At),判断点p在t-Δt时刻被其他点D11.如果步骤D6中判断点p所在的物体在t-Δt时刻没有发生自遮挡，并且步骤D10中5F2.初始化变量Weight＝1,DynWeight＝0,StaticWeight＝0,unknownWeiF3a.如果聚类簇ct-i*Δt对应的障碍物属于动态障碍物，根据公式DynWeight=DynWeight+Weight对变量DynWF3b.如果聚类簇ct-i*Δt对应的障碍物属于静态障碍物，根据公式StaticWeight=StaticWeight+Weight对变量SF3c.如果聚类簇ct-i*Δt对应的障碍物属于未知障碍物，根据公式unknownWeight=unknownWeight+Weight对变量unknownWF4.如果DynWeight>StaticWeight且DynWeight>unF5.如果StaticWeight>DynWeightandStaticWeight>unknownWeight，将聚类簇ct对F6.将聚类簇ct对应的障碍物识别为属于67置邻域距离阈值ε和连通阈值δ,初始化核心对象集合Q=0,初始化聚类簇数k＝0，初始化Nε[0026]A4.在核心对象集合Ω中，随机选一个核心对象o，初始化当前簇核心对象列表[0030]B1.获取对m个聚类簇其中t表示Ct中的各个聚类簇由采集时间为t的所述稀疏点云聚类得到，其中采集时间为t的所述稀疏点云由拍摄时间为t的所述深度图像处理得到，通过卡尔曼滤波器预测Ct中所有障碍物在时间t下的位置8障碍物，建立障碍物跟踪历史，所述障碍物跟踪历史表示为其中[0041]进一步地，所述识别各所述聚类簇对应的障碍物属于动态障碍物或者静态障碍[0042]C1.获取拍摄时间为t-Δt的深度图像Dt-Δt和拍摄时间为t的深度图像Dt，获取采集时间为t-Δt的稠密点云其中所述采集时间为t-Δt的稠密点云由拍摄时间障碍物，将聚类簇ct对应的障碍物识别为属于[0045]C4.对聚类簇ct中的每个点[0046]C4a.根据位姿Ot-Δt时刻是否被遮挡；当点p不在无人机的t-Δt时刻的视场角内，且点p在t-Δt时刻被遮[0049]C5.当将聚类簇cf对应的障碍物识别9centroid(cf)表示聚类簇cf的质心，length(o,centroid(cf))表示centroid(cf)与Ot之间的[0055]D5.计算qt-Δt周围第一范围内的平均深度roundAvg(Dt-Δt[qt-Δt])；[0057]D7.根据Ot-Δt把点p投影到t-Δt时刻的相机坐标系jt-st,获得t-Δt时所在位置到点p的深度其中|.|z表示z方向的[0059]D9.计算qt-Δt周围第二范围内的平均深度roundAvg(Dt-Δt[qt-Δt])；[0060]D10.如果⃞r-srle-roundAvg(D:-s:lqr-s:l)>B(At),判断点p在t-Δt时刻被其[0061]D11.如果步骤D6中判断点p所在的物体在t-Δt时刻没有发生自遮挡，并且步骤[0062]进一步地，所述识别各所述聚类簇对应的障碍物属于动态障碍物或者静态障碍类簇ct对应的障碍物质心实际的移动向量Va；如果difference(Vc,Va)<β,其中difference[0067]进一步地，所述识别各所述聚类簇对应的障碍物属于动态障碍物或者静态障碍[0071]F3a.如果聚类簇ct-i*Δt对应的障碍物属于动态障碍物，根据公式DynWeight=DynWeight+Weight对变量DynW[0072]F3b.如果聚类簇ct-i*Δt对应的障碍物属于静态障碍物，根据公式StaticWeight=StaticWeight+Weight对变量SunknownWeight+Weight对变量unknownW[0075]F4.如果DynWei[0083]图5和图6为实施例中仿真运行基于双目相机的动态环境感知方法所获得的仿真[0084]图7为实施例中实机运行基于双目相机的动态环境感知方法所获得的实机感知效[0092]S6.当聚类簇对应的障碍物属于静态建模后的聚类簇进行跟踪；当在连续的超过阈值长度的时间段内未跟踪到建模后的聚类深度图像，相应地步骤S2中对当前帧深度图像处理得到的一帧稀疏点云为当前帧稀疏点前帧深度图像处理得到的一帧稀疏点云为前一帧稀[0104]步骤S204中，裁剪掉原始点云Pw中可信度较低(例如深度小于Dmin或者大于Dmax，Dmin和Dmax均为预设阈值)的点，以及裁剪掉地面点(高度小于Hmin的点)和天花板点(高度大域距离阈值ε和连通阈值δ,初始化核心对象集合初始化聚类簇数k＝0，初始化未访(xj)∪xj的连通分支数n；如果n<δ,通过公式Ω=Ω∪{xj}将样本x[0111]A3.如果核心对象集合Q=,则结束对步骤A1-A6的执行，否则执行步骤A4；[0112]A4.在核心对象集合Ω中，随机选一个核心对象o，初始化当前簇核心对象列表[0116]执行步骤A1-A6的结果是将一帧稀疏点云中的各个点聚类成n个聚类簇(对于不同帧稀疏点云由拍摄时间为t的深度图像(即当前帧深度帧深度图像中的位置等的信息，相当于赋予了当前帧稀疏点云中的这个聚类簇的历史信[0131]本实施例中，在识别各聚类簇对应的障碍物属于动态障[0133]C1.获取拍摄时间为t-Δt的深度图像Dt-Δt和拍摄时间为t的深度图像Dt，获取采集时间为t-Δt的稠密点云其中采集时间为t-Δt的稠密点云由拍摄时间为t-Δt的深度图像处理得到，获取双目相机所在无人机在t-Δt时刻的位姿Ot-Δt和在t时刻的位姿Ot，获取其中p表示世界坐标系[0137]C4a.根据位姿Ot-Δt时刻是否被遮挡；当点p不在无人机的t-Δt时刻的视场角内，且点p在t-Δt时刻被遮表示聚类簇的质心，length(o,centroid(ct))表示centroid(cf)与Ot之间的距离，[0141]步骤C1-C5所执行的基于点投票的深度自适应判别算法的原理是：让组成障碍物是静态障碍物。并且只有那些出现在前一帧FOV内且没有被其他物体遮挡的点可以参与投个障碍物是动态障碍物，否则为静态障碍物。这个投票阈值会根据物体离相机的距离(深[0153]D5.计算qt-Δt周围第一范围内的平均深度roundAvg(Dt-Δt[qt-Δt])；[0156]D7.根据Ot-Δt把点p投影到t-Δt时刻的相机坐标系jt-st,获得t-Δt所在位置到点p的深度其中|.|z表示z方向的[0158]D9.计算qt-Δt周围第二范围内的平均深度roundAvg(Dt-Δt[qt-Δt])；[0159]D10.如果⃞r-srle-roundAvg(D:-slqr-sl)>B(At),判断点p在t-Δt时刻被其[0161]上述步骤D1-D11中，步骤D1-D6可以判断点p在t-Δt时刻是否发生自遮挡，步骤聚类簇对应的障碍物属于动态障碍物或者静态障碍物这一步骤时，具体执行以下步骤E1-类簇ct对应的障碍物质心实际的移动向量Va；如果difference(Vc,Va)<β,其中difference[0167]步骤E1-E4所执行的基于物体质心的相机自运动补偿判别算法的原理是：基于同[0171]F2.初始化变量Weight＝1,Dy[0173]F3a.如果聚类簇ct-i*Δt对应的障碍物属于动态障碍物，根据公式DynWeight=DynWeight+Weight对变量DynW[0174]F3b.如果聚类簇ct-i*Δt对应的障碍物属于静态障碍物，根据公式StaticWeight=StaticWeight+Weight对变量SunknownWeight+Weight对变量unknownW[0177]F4.如果DynWei[0180]步骤F1-F6所执行的基于历史一致性的越近帧权重越大的判别算法，可以用以下[0182]步骤F1-F6所执行的基于历史一致性的越近帧权重越大的判别算法的原理是：根[0185]使用计算机在仿真环境下运行步骤S1-S6，所获得的仿真感知效果如图5和图6所够在障碍物出现遮挡并且相机自身运动的复杂情况下对障碍物的动静属性进行较鲁棒的[0187]由于无人机仅使用双目相机，以及自身的CPU即可执行实施例中的动态环境感知[0188]可以通过编写执行本实施例中的基于双目相机的动态环境感知方法的计算机程程技术-包括配置有计算机程序的非暂时性计算机可读存