CN114387462A - 一种基于双目相机的动态环境感知方法 - Google Patents

一种基于双目相机的动态环境感知方法 Download PDF

Info

Publication number
CN114387462A
CN114387462A CN202111652247.8A CN202111652247A CN114387462A CN 114387462 A CN114387462 A CN 114387462A CN 202111652247 A CN202111652247 A CN 202111652247A CN 114387462 A CN114387462 A CN 114387462A
Authority
CN
China
Prior art keywords
obstacle
cluster
point
time
binocular camera
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202111652247.8A
Other languages
English (en)
Inventor
夏焕明
成慧
曾祥伟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Sun Yat Sen University
Original Assignee
Sun Yat Sen University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Sun Yat Sen University filed Critical Sun Yat Sen University
Priority to CN202111652247.8A priority Critical patent/CN114387462A/zh
Publication of CN114387462A publication Critical patent/CN114387462A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G06F18/232—Non-hierarchical techniques
    • G06F18/2321—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions
    • G06F18/23213—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions with fixed number of clusters, e.g. K-means clustering

Landscapes

  • Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Artificial Intelligence (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • General Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Probability & Statistics with Applications (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种基于双目相机的动态环境感知方法，包括获取由双目相机拍摄得到的多帧深度图像，对各帧深度图像进行处理，获得多帧稀疏点云，对各帧稀疏点云分别进行聚类，获得多个聚类簇，对不同帧的稀疏点云中表示相同障碍物的多个聚类簇进行关联，以及识别关联后的聚类簇对应的障碍物属于动态障碍物或者静态障碍物等步骤。本发明能在障碍物间距离较近等复杂环境下快速地、较好地实现对环境中各种形状的障碍物进行检测、动静态属性划分和跟踪的动态环境感知，能够在障碍物出现遮挡并且相机自身运动的复杂情况下对障碍物的动静属性进行较鲁棒的判断。应用本发明的无人机具有负载小、功耗低、轻便易携等优点。本发明广泛应用于图像处理技术领域。

Description

一种基于双目相机的动态环境感知方法
技术领域
本发明涉及图像处理技术领域，尤其是一种基于双目相机的动态环境感知方法。
背景技术
无人机在飞行过程中可能会遇到障碍物，为了顺利完成飞行任务，需要感知障碍物并进行避让等动作。在真实的飞行环境中，无人机面对的往往是未知的、复杂的动态障碍情景，其中的障碍物既可能是动态障碍物也可能是静态障碍物，由于无人机对动态障碍物和静态障碍物的应对措施不同，因此要求无人机能够准确感知障碍物是动态障碍物还是静态障碍物。目前比较先进的障碍物感知技术包括使用高精度激光雷达生成点云数据的障碍物感知技术，以及使用来自图像的视觉信息对障碍物进行感知的技术。使用高精度激光雷达生成点云数据的障碍物感知技术依赖激光雷达采集点云数据，而激光雷达的体积和质量较大，不便于无人机携带飞行，因此限制了其在无人机中的应用。目前的使用来自图像的视觉信息对障碍物进行感知的技术中，普遍存在使用条件苛刻、适应性差等缺点，例如使用Frame-Difference的方法感知动态障碍物的技术在无人机平稳悬停的情况下才能应用，使用神经网络感知障碍物的技术只在预定义类型的障碍物感知上表现较好，但是对于一般的未知类型的障碍物则效果不佳。
发明内容
针对目前的障碍物感知技术使用条件苛刻、适应性差、不适宜用于无人机等至少一个技术问题，本发明的目的在于提供一种基于双目相机的动态环境感知方法，包括：
获取由双目相机拍摄得到的多帧深度图像；
对各帧所述深度图像进行处理，获得多帧稀疏点云；其中，对一帧所述深度图像的处理结果为得到相应的一帧稀疏点云；
对各帧所述稀疏点云分别进行聚类，获得多个聚类簇；其中，对一帧所述稀疏点云的聚类结果为得到相应的若干个个聚类簇，一帧所述稀疏点云中的不同聚类簇分别表示不同的障碍物；
对不同帧的所述稀疏点云中表示相同障碍物的多个所述聚类簇进行关联；
识别关联后的所述聚类簇对应的障碍物属于动态障碍物或者静态障碍物。
进一步地，所述基于双目相机的动态环境感知方法还包括：
当所述聚类簇对应的障碍物属于静态障碍物，根据所述聚类簇更新占用图上的障碍物信息；
当所述聚类簇对应的障碍物属于动态障碍物，将所述聚类簇建模成椭球，使用卡尔曼滤波器对建模后的所述聚类簇进行跟踪。
进一步地，所述基于双目相机的动态环境感知方法还包括：
当在连续的超过阈值长度的时间段内未跟踪到建模后的所述聚类簇，结束对所述聚类簇的跟踪，删除所述聚类簇对应的数据。
进一步地，所述对各帧所述深度图像进行处理，获得多帧稀疏点云，包括：
获取所述双目相机的外参矩阵T和内参矩阵K；
获取所述深度图像的像素坐标Puv；
通过公式Pw＝T-1K-1Puv确定原始点云；其中，Pw为所述原始点云的世界坐标；
对所述原始点云进行裁剪，获得稠密点云；
使用体素过滤法对所述稠密点云进行过滤，获得所述稀疏点云。
进一步地，所述对各帧所述稀疏点云分别进行聚类，获得多个聚类簇，包括：
A1.获取样本点集D＝{x1,x2,……,xm}，其中xm表示所述稀疏点云中的第m个点，设置邻域距离阈值ε和连通阈值δ，初始化核心对象集合
Figure BDA0003445056120000021
初始化聚类簇数k＝0，初始化未访问样本集合F＝D，初始化簇划分
Figure BDA0003445056120000022
A2.对于j＝1,2,……,m，按下面步骤A2a-A2b找出所有的核心对象：
A2a.找到样本xj的ε邻域内的子样本点集Nε(xj)；其中，xj为样本点集D中的样本点；
A2b.如果所述子样本点集Nε(xj)的样本个数满足|Nε(xj)|≥MinPts，则计算点Q＝Nε(xj)∪xj的连通分支数n；如果n<δ，通过公式Ω＝Ω∪{xj}将样本xj加入核心对象集合Ω中；
A3.如果核心对象集合
Figure BDA0003445056120000024
则结束对步骤A1-A6的执行，否则执行步骤A4；
A4.在核心对象集合Ω中，随机选一个核心对象o，初始化当前簇核心对象列表Ωcur＝{o}，初始化类别序号k＝k+1，初始化当前簇样本集合Ck＝{o}，更新未访问样本集合F-F-{o}；
A5.如果当前簇核心对象队列
Figure BDA0003445056120000023
则当前聚类簇Ck生成完毕，更新簇划分C＝{C1,C2,……,Ck}，更新核心对象集合Ω＝Ω-Ck，返回执行步骤A3，否则更新核心对象集合Ω＝Ω-Ck；
A6.在当前簇核心对象队列Ωcur中取出一个核心对象o‘’，通过邻域距离阈值ε找出ε-邻域子样本点集Nε(o‘’)，令Δ＝Nε(o‘’)∩F，更新当前簇样本集合Ck＝Ck∪Δ，更新未访问样本集合F＝F-Δ，更新Ωcur＝Ωcur∪(Δ∩Ω)-o‘’，返回执行步骤A5。
进一步地，所述对不同帧的所述稀疏点云中表示相同障碍物的多个所述聚类簇进行关联，包括：
B1.获取对m个聚类簇
Figure BDA0003445056120000031
其中t表示Ct中的各个聚类簇由采集时间为t的所述稀疏点云聚类得到，其中采集时间为t的所述稀疏点云由拍摄时间为t的所述深度图像处理得到，通过卡尔曼滤波器预测Ct中所有障碍物在时间t下的位置
Figure BDA0003445056120000032
设置距离阈值ε，初始化关联集合F＝Kt；
B2.计算Ct中每个聚类簇
Figure BDA0003445056120000033
的质心
Figure BDA0003445056120000034
得到所有聚类簇当前帧下的质心
Figure BDA0003445056120000035
B3.初始化待关联集合Ω＝Dt；
B4.对于每个
Figure BDA0003445056120000036
按以下步骤B4a-B4c执行：
B4a.找到
Figure BDA0003445056120000037
在F中的最近邻kj；
B4b.找到kj在Ω中的最近邻dj；
B4c.如果
Figure BDA0003445056120000038
即
Figure BDA0003445056120000039
与kj互为最近邻，通过公式
Figure BDA00034450561200000310
把
Figure BDA00034450561200000311
关联到障碍物j；通过公式
Figure BDA00034450561200000312
把
Figure BDA00034450561200000313
移出待关联集合Ω；通过公式F＝F-{kj}把kj移出关联集合F；
B5.如果
Figure BDA00034450561200000314
或者
Figure BDA00034450561200000315
结束对步骤B1-B6的执行，反之，对于每个
Figure BDA00034450561200000316
执行以下步骤B5a-B5b：
B5a.找到
Figure BDA00034450561200000317
在F中的最近邻kj，计算
Figure BDA00034450561200000318
与kj之间的距离δ；
B5b.如果δ<ε，通过公式
Figure BDA00034450561200000319
把
Figure BDA00034450561200000320
关联到障碍物j，通过公式
Figure BDA00034450561200000321
把
Figure BDA00034450561200000322
移出待关联集合Ω，通过公式F＝F-{kj}把kj移出关联集合F；
B6.如果
Figure BDA00034450561200000323
结束对步骤B1-B6的执行，反之，对于每个
Figure BDA00034450561200000324
认为是新出现的障碍物，建立障碍物跟踪历史，所述障碍物跟踪历史表示为
Figure BDA00034450561200000325
其中
Figure BDA00034450561200000326
Figure BDA00034450561200000327
Δt表示时间间隔，n表示障碍物的标号。
进一步地，所述识别各所述聚类簇对应的障碍物属于动态障碍物或者静态障碍物，包括：
C1.获取拍摄时间为t-Δt的深度图像Dt-Δt和拍摄时间为t的深度图像Dt，获取采集时间为t-Δt的稠密点云
Figure BDA0003445056120000041
其中所述采集时间为t-Δt的稠密点云
Figure BDA0003445056120000042
由拍摄时间为t-Δt的所述深度图像处理得到，获取所述双目相机所在无人机在t-Δt时刻的位姿Ot-Δt和在t时刻的位姿Ot，获取
Figure BDA0003445056120000043
其中
Figure BDA0003445056120000044
p表示世界坐标系下的点，l表示点的标号，获取判别参数(β，βmin，Vmin)；
C2.遍历Ct中的任意聚类簇
Figure BDA0003445056120000045
其中I＝1,2，……，m，如果聚类簇
Figure BDA0003445056120000046
对应新识别出的障碍物，将聚类簇
Figure BDA0003445056120000047
对应的障碍物识别为属于未知障碍物，所述未知障碍物为既不确定属于动态障碍物也不确定属于静态障碍物的障碍物；
C3.初始化变量vote＝0，dyn＝0，static＝0；
C4.对聚类簇
Figure BDA0003445056120000048
中的每个点p执行以下投票过程：
C4a.根据位姿Ot-Δt判断点p是否在无人机的t-Δt时刻的视场角内，判断点p在t-Δt时刻是否被遮挡；当点p不在无人机的t-Δt时刻的视场角内，且点p在t-Δt时刻被遮挡，根据公式vote＝vote+1对变量vote进行操作，反之执行步骤C4b；
C4b.在稠密点云
Figure BDA0003445056120000049
中查找点p的最近邻nn；当
Figure BDA00034450561200000410
其中kength(p,nn)为点p与最近邻nn之间的距离，Δt表示时间间隔，根据公式dyn＝dyn+1对变量dyn进行操作，反之执行步骤C4c；
C4c.根据公式static＝static+1对变量static进行操作；
C5.当
Figure BDA00034450561200000411
将聚类簇
Figure BDA00034450561200000412
对应的障碍物识别为属于动态障碍物，反之将聚类簇
Figure BDA00034450561200000413
对应的障碍物识别为属于静态障碍物；其中
Figure BDA00034450561200000414
表示聚类簇
Figure BDA00034450561200000415
的质心，
Figure BDA00034450561200000416
表示
Figure BDA00034450561200000417
与Ot之间的距离，sensingrange表示所述双目相机的最大测量距离。
进一步地，所述判断点p在t-Δt时刻是否被遮挡，包括：
D1.获取ct的质心cent、ct-Δt的质心cent-Δt，以及与时间Δt相关的判断参数β(Δt)；
D2.根据Ot把cent投影到时刻t的像素坐标系，获得qt；
D3.根据Ot-Δt把cent-Δt投影到时刻t-Δt的像素坐标系，获得qt-Δt；
D4.计算qt周围第一范围内的平均深度roundAvg(Dt[qt])；
D5.计算qt-Δt周围第一范围内的平均深度roundAvg(Dt-Δt[qt-Δt])；
D6.如果roundAvg(Dt[qt])-roundAvg(Dt-Δt[qt-Δt])>β(Δt)，判断点p在t-Δt时刻发生了自遮挡，否则判断点p在t-Δt时刻没有发生自遮挡；
D7.根据Ot-Δt把点p投影到t-Δt时刻的相机坐标系
Figure BDA0003445056120000051
获得t-Δt时刻双目相机所在位置到点p的深度
Figure BDA0003445056120000052
其中|.|z表示z方向的坐标值；
D8.根据Ot-Δt把点p投影到t-Δt时刻的像素坐标系qt-Δt；
D9.计算qt-Δt周围第二范围内的平均深度roundAvg(Dt-Δt[qt-Δt])；
D10.如果
Figure BDA0003445056120000053
判断点p在t-Δt时刻被其他点遮挡，否则判断点p在t-Δt时刻没有被其他点遮挡；
D11.如果步骤D6中判断点p所在的物体在t-Δt时刻没有发生自遮挡，并且步骤D10中判断点p在t-Δt时刻被其他点遮挡，则判断点p在t-Δt时刻被遮挡。
进一步地，所述识别各所述聚类簇对应的障碍物属于动态障碍物或者静态障碍物，包括：
E1.获取ct的质心cent、ct-Δt的质心cent-Δt、速度阈值参数Vmin和补偿运动阈值参数β；
E2.判断聚类簇ct对应的障碍物是否需要进行双目相机的自运动补偿，如果需要，跳转执行步骤E3，否则，计算聚类簇ct对应的障碍物的近似速度v＝Length(cent,ent-Δt)/Δt，如果v>Vmin，将聚类簇ct对应的障碍物识别为属于动态障碍物，反之，将聚类簇ct对应的障碍物识别为属于静态障碍物；
E3.计算双目相机运动相对聚类簇ct对应的障碍物质心变化的补偿向量Vc，计算聚类簇ct对应的障碍物质心实际的移动向量Va；如果difference(Vc,Va)<β，其中difference()表示两向量之差的模，则判断物体为静态障碍物，结束对步骤E1-E4的执行，反之，跳转执行步骤E4；
E4.计算聚类簇ct对应的障碍物的速度v＝Length(cent,cent-Δt)/Δt，如果v>Vmin，将聚类簇ct对应的障碍物识别为属于动态障碍物，反之，将聚类簇ct对应的障碍物识别为属于静态障碍物。
进一步地，所述识别各所述聚类簇对应的障碍物属于动态障碍物或者静态障碍物，包括：
F1.获取时间尺度th、衰减因子α和障碍物跟踪历史
Figure BDA0003445056120000054
其中Δt表示时间间隔，k表示序号；
F2.初始化变量Weight＝1,DynWeight＝0,StaticWeight＝0,unknownWeight＝0；
F3.设置变量I，I从0到th遍历，执行以下步骤F3a-F3d：
F3a.如果聚类簇ct-i*Δt对应的障碍物属于动态障碍物，根据公式DynWeight＝DynWeight+Weight对变量DynWeight进行操作；
F3b.如果聚类簇ct-i*Δt对应的障碍物属于静态障碍物，根据公式StaticWeight＝StaticWeight+Weight对变量StaticWeight进行操作；
F3c.如果聚类簇ct-i*Δt对应的障碍物属于未知障碍物，根据公式unknownWeight＝unknownWeight+Weight对变量unknownWeight进行操作；
F3d.根据公式Weight＝Weight*α对变量Weight进行操作；
F4.如果DynWeight>StaticWeight且DynWeight>unknownWeight，将聚类簇ct对应的障碍物识别为属于动态障碍物，反之执行步骤F5；
F5.如果StaticWeight>DynWeight and StaticWeight>unknownWeight，将聚类簇ct对应的障碍物识别为属于静态障碍物，反之执行步骤F6；
F6.将聚类簇ct对应的障碍物识别为属于未知障碍物。
本发明的有益效果是：实施例中的基于双目相机的动态环境感知方法，通过对双目相机采集到的深度图像进行处理，能在障碍物间距离较近、障碍物出现遮挡、相机自身在运动等复杂环境下快速地、较好地实现对环境中各种形状的障碍物进行检测、动静态属性划分和跟踪的动态环境感知，能够在障碍物出现遮挡并且相机自身运动的复杂情况下对障碍物的动静属性进行较鲁棒的判断。实施例中的基于双目相机的动态环境感知方法仅需双目相机即可采集图像数据进行处理，无需配备激光雷达等外设，应用实施例中的基于双目相机的动态环境感知方法的无人机具有负载小、价格便宜、体积小、功耗低、轻便易携等优点，因此实施例中的基于双目相机的动态环境感知方法是适合应用于无人机的方法。
附图说明
图1为实施例中基于双目相机的动态环境感知方法的流程图；
图2为实施例中基于双目相机的动态环境感知方法的算法框架图；
图3为实施例中需要进行双目相机自运动补偿的第一基本情况示意图；
图4为实施例中需要进行双目相机自运动补偿的第二基本情况示意图；
图5和图6为实施例中仿真运行基于双目相机的动态环境感知方法所获得的仿真感知效果示意图；
图7为实施例中实机运行基于双目相机的动态环境感知方法所获得的实机感知效果示意图。
具体实施方式
本实施例中，可由挂载了双目相机的无人机来执行基于双目相机的动态环境感知方法，具体地可由无人机中的CPU执行基于双目相机的动态环境感知方法中的各步骤。
参照图1，基于双目相机的动态环境感知方法包括以下步骤：
S1.获取由双目相机拍摄得到的多帧深度图像；
S2.对各帧深度图像进行处理，获得多帧稀疏点云；其中，对一帧深度图像的处理结果为得到相应的一帧稀疏点云；
S3.对各帧稀疏点云分别进行聚类，获得多个聚类簇；其中，对一帧稀疏点云的聚类结果为得到相应的若干个个聚类簇，一帧稀疏点云中的不同聚类簇分别表示不同的障碍物；
S4.对不同帧的稀疏点云中表示相同障碍物的多个聚类簇进行关联；
S5.识别关联后的聚类簇对应的障碍物属于动态障碍物或者静态障碍物；
S6.当聚类簇对应的障碍物属于静态障碍物，根据聚类簇更新占用图上的障碍物信息；当聚类簇对应的障碍物属于动态障碍物，将聚类簇建模成椭球，使用卡尔曼滤波器对建模后的聚类簇进行跟踪；当在连续的超过阈值长度的时间段内未跟踪到建模后的聚类簇，结束对聚类簇的跟踪，删除聚类簇对应的数据。
其中，步骤S1和S2属于点云生成步骤，步骤S3属于点云聚类步骤，步骤S4属于障碍物关联步骤，步骤S5属于障碍物属性划分步骤，步骤S6属于动态更新环境信息步骤。步骤S1-S6的算法框架如图2所示。
步骤S1中，由无人机上的双目相机拍摄得到多帧深度图像，相邻两帧深度图像的拍摄时间可以是相等的，即相邻两帧深度图像的拍摄时间之间的时间差可以是固定的时间间隔Δt。本实施例中，以拍摄时间t为基准，将拍摄时间为t的那一帧深度图像称为当前帧深度图像，相应地步骤S2中对当前帧深度图像处理得到的一帧稀疏点云为当前帧稀疏点云；那么，拍摄时间为t-Δt的那一帧深度图像称为前一帧深度图像，相应地步骤S2中对当前帧深度图像处理得到的一帧稀疏点云为前一帧稀疏点云。
步骤S2中对各帧深度图像进行处理，获得多帧稀疏点云。具体地，对每一帧深度图像进行处理，都得到相应的一帧稀疏点云，因此多帧深度图像以及对它们处理得到的多帧稀疏点云之间是一一对应的关系。
在执行步骤S2，也就是对各帧深度图像进行处理，获得多帧稀疏点云这一步骤时，具体可以执行以下步骤：
S201.获取双目相机的外参矩阵T和内参矩阵K；
S202.获取深度图像的像素坐标Puv；
S203.通过公式Pw＝T-1K-1Puv确定原始点云；其中，Pw为原始点云的世界坐标；
S204.对原始点云进行裁剪，获得稠密点云；
S205.使用体素过滤法对稠密点云进行过滤，获得稀疏点云。
其中，步骤S201-S205是针对一帧深度图像处理得到相应的一帧稀疏点云，当有多帧深度图像，分别针对各帧深度图像执行步骤S201-S205即可。
步骤S203中，T-1表示外参矩阵T的逆矩阵，K-1表示内参矩阵K的逆矩阵。通过公式Pw＝T-1K-1Puv，可以计算得到世界坐标系下的原始点云的坐标Pw，本实施例中可以用世界坐标Pw来表示原始点云。
步骤S204中，裁剪掉原始点云Pw中可信度较低(例如深度小于Dmin或者大于Dmax，Dmin和Dmax均为预设阈值)的点，以及裁剪掉地面点(高度小于Hmin的点)和天花板点(高度大于Hmax的点)，没被裁剪掉的点(深度大于Dmin且小于Dmax，高度大于Hmin且小于Hmax)组成稠密点云。
步骤S205中，使用体素过滤法对稠密点云进行过滤，可以获得比稠密点云更稀疏的点云，即稀疏点云。后续S3-S6等步骤中对稀疏点云而不是稠密点云进行处理，可以减少需要处理的数据量，从而提高处理速度。
在执行步骤S3，也就是对各帧稀疏点云分别进行聚类，获得多个聚类簇这一步骤时，具体执行以下步骤A1-A6：
A1.获取样本点集D＝{x1,x2,……,xm}，其中xm表示稀疏点云中的第m个点，设置邻域距离阈值ε和连通阈值δ，初始化核心对象集合
Figure BDA0003445056120000081
初始化聚类簇数k＝0，初始化未访问样本集合F＝D，初始化簇划分
Figure BDA0003445056120000082
A2.对于j＝1,2,……,m，按下面步骤A2a-A2b找出所有的核心对象：
A2a.找到样本xj的ε邻域内的子样本点集Nε(xj)；其中，xj为样本点集D中的样本点；
A2b.如果子样本点集Nε(xj)的样本个数满足|Nε(xj)|≥MinPts，则计算点Q＝Nε(xj)∪xj的连通分支数n；如果n<δ，通过公式Ω＝Ω∪{xj}将样本xj加入核心对象集合Ω中；
A3.如果核心对象集合
Figure BDA0003445056120000083
则结束对步骤A1-A6的执行，否则执行步骤A4；
A4.在核心对象集合Ω中，随机选一个核心对象o，初始化当前簇核心对象列表Ωcur＝{o}，初始化类别序号k＝k+1，初始化当前簇样本集合Ck＝{o}，更新未访问样本集合F-F-{o}；
A5.如果当前簇核心对象队列
Figure BDA0003445056120000084
则当前聚类簇Ck生成完毕，更新簇划分C＝{C1,C2,……,Ck}，更新核心对象集合Ω＝Ω-Ck，返回执行步骤A3，否则更新核心对象集合Ω＝Ω-Ck；
A6.在当前簇核心对象队列Ωcur中取出一个核心对象o‘’，通过邻域距离阈值ε找出ε-邻域子样本点集Nε(o‘’)，令Δ＝Nε(o‘’)∩F，更新当前簇样本集合Ck＝Ck∪Δ，更新未访问样本集合F＝F-Δ，更新Ωcur＝Ωcur∪(Δ∩Ω)-o‘’，返回执行步骤A5。
其中，步骤A1-A6是针对一帧稀疏点云进行聚类获得聚类簇，当有多帧稀疏点云，分别针对各帧稀疏点云执行步骤A1-A6即可。
执行步骤A1-A6的结果是将一帧稀疏点云中的各个点聚类成n个聚类簇(对于不同帧的稀疏点云，n的具体数值不同)。同一帧稀疏点云聚类所得的多个聚类簇中，每个聚类簇分别表示双目相机拍摄到的对应一帧深度图像中的一个障碍物。因此，每个聚类簇反映的是其所在那一帧稀疏点云的障碍物的信息，而不同帧的稀疏点云聚类所得的聚类簇，既有可能表示同一障碍物，也有可能表示不同障碍物。步骤A1-A6对距离双目相机较近的障碍物也有较好的聚类效果。
在执行步骤S4，也就是对不同帧的稀疏点云中表示相同障碍物的多个聚类簇进行关联这一步骤时，具体执行以下步骤B1-B6：
B1.获取对m个聚类簇
Figure BDA0003445056120000091
其中t表示Ct中的各个聚类簇由采集时间为t的稀疏点云聚类得到，其中采集时间为t的稀疏点云由拍摄时间为t的深度图像处理得到，通过卡尔曼滤波器预测Ct中所有障碍物在时间t下的位置
Figure BDA0003445056120000092
设置距离阈值ε，初始化关联集合F＝Kt；
B2.计算Ct中每个聚类簇
Figure BDA0003445056120000093
的质心
Figure BDA0003445056120000094
得到所有聚类簇当前帧下的质心
Figure BDA0003445056120000095
B3.初始化待关联集合Ω＝Dt；
B4.对于每个
Figure BDA0003445056120000096
按以下步骤B4a-B4c执行：
B4a.找到
Figure BDA0003445056120000097
在F中的最近邻kj；
B4b.找到kj在Ω中的最近邻dj；
B4c.如果
Figure BDA0003445056120000098
即
Figure BDA0003445056120000099
与kj互为最近邻，通过公式
Figure BDA00034450561200000910
把
Figure BDA00034450561200000911
关联到障碍物j；通过公式
Figure BDA00034450561200000912
把
Figure BDA00034450561200000913
移出待关联集合Ω；通过公式F＝F-{kj}把kj移出关联集合F；
B5.如果
Figure BDA00034450561200000914
或者
Figure BDA00034450561200000915
结束对步骤B1-B6的执行，反之，对于每个
Figure BDA00034450561200000916
执行以下步骤B5a-B5b：
B5a.找到
Figure BDA0003445056120000101
在F中的最近邻kj，计算
Figure BDA0003445056120000102
与kj之间的距离δ；
B5b.如果δ<ε，通过公式
Figure BDA0003445056120000103
把
Figure BDA0003445056120000104
关联到障碍物j，通过公式
Figure BDA0003445056120000105
把
Figure BDA0003445056120000106
移出待关联集合Ω，通过公式F＝F-{kj}把kj移出关联集合F；
B6.如果
Figure BDA0003445056120000107
结束对步骤B1-B6的执行，反之，对于每个
Figure BDA0003445056120000108
认为是新出现的障碍物，建立障碍物跟踪历史，障碍物跟踪历史表示为
Figure BDA0003445056120000109
其中
Figure BDA00034450561200001010
Δt表示时间间隔，具体地可以是双目相机采集两帧深度图像之间的时间间隔，n表示障碍物的标号。
其中，步骤B1-B6是针对采集时间为t的稀疏点云(即当前帧稀疏点云)，其中当前帧稀疏点云由拍摄时间为t的深度图像(即当前帧深度图像)处理得到。
步骤B1-B6是一种基于欧氏距离的聚类簇关联算法，通过执行步骤B1-B6，可以将当前帧稀疏点云中的每个聚类簇，与当前帧稀疏点云之前的各帧稀疏点云中的聚类簇关联起来。如果不同帧稀疏点云中的聚类簇被关联起来，那么表示这些聚类簇所表示的障碍物是同一个障碍物。从另一个角度理解，对于当前帧稀疏点云中的一个聚类簇，其本身表示了当前帧深度图像中的一个障碍物的位置等的信息，在将该聚类簇与之前各帧稀疏点云中的相应聚类簇关联起来后，之前各帧稀疏点云中的相应聚类簇分别表示同一障碍物在之前各帧深度图像中的位置等的信息，相当于赋予了当前帧稀疏点云中的这个聚类簇的历史信息。
本实施例中，在识别各聚类簇对应的障碍物属于动态障碍物或者静态障碍物时，具体可以使用两种不同的判别算法，第一种是基于点投票的深度自适应判别算法，第二种是基于物体质心的相机自运动补偿判别算法，最后，为了提高判别的鲁棒性，需要保证判别班别结果的历史一致性，因此使用基于历史一致性的越近帧权重越大的判别算法来最终确定障碍物是动态障碍物还是静态障碍物。
当应用基于点投票的深度自适应判别算法，在执行步骤S5，也就是识别各聚类簇对应的障碍物属于动态障碍物或者静态障碍物这一步骤时，具体执行以下步骤C1-C5：
C1.获取拍摄时间为t-Δt的深度图像Dt-Δt和拍摄时间为t的深度图像Dt，获取采集时间为t-Δt的稠密点云
Figure BDA00034450561200001011
其中采集时间为t-Δt的稠密点云
Figure BDA00034450561200001012
由拍摄时间为t-Δt的深度图像处理得到，获取双目相机所在无人机在t-Δt时刻的位姿Ot-Δt和在t时刻的位姿Ot，获取
Figure BDA00034450561200001013
其中
Figure BDA00034450561200001014
p表示世界坐标系下的点，l表示点的标号，获取判别参数(β，βmin，Vmin)；
C2.遍历Ct中的任意聚类簇
Figure BDA00034450561200001015
其中I＝1,2，……，m，如果聚类簇
Figure BDA00034450561200001016
对应新识别出的障碍物，将聚类簇
Figure BDA0003445056120000111
对应的障碍物识别为属于未知障碍物，未知障碍物为既不确定属于动态障碍物也不确定属于静态障碍物的障碍物；
C3.初始化变量vote＝0，dyn＝0，static＝0；
C4.对聚类簇
Figure BDA0003445056120000112
中的每个点p执行以下投票过程：
C4a.根据位姿Ot-Δt判断点p是否在无人机的t-Δt时刻的视场角内，判断点p在t-Δt时刻是否被遮挡；当点p不在无人机的t-Δt时刻的视场角内，且点p在t-Δt时刻被遮挡，根据公式vote＝vote+1对变量vote进行操作，反之执行步骤C4b；
C4b.在稠密点云
Figure BDA0003445056120000113
中查找点p的最近邻nn；当
Figure BDA0003445056120000114
其中length(p,nn)为点p与最近邻nn之间的距离，Δt表示时间间隔，根据公式dyn＝dyn+1对变量dyn进行操作，反之执行步骤C4c；
C4c.根据公式static＝static+1对变量static进行操作；
C5.当
Figure BDA0003445056120000115
将聚类簇
Figure BDA0003445056120000116
对应的障碍物识别为属于动态障碍物，反之将聚类簇
Figure BDA0003445056120000117
对应的障碍物识别为属于静态障碍物；其中
Figure BDA0003445056120000118
表示聚类簇
Figure BDA0003445056120000119
的质心，
Figure BDA00034450561200001110
表示
Figure BDA00034450561200001111
与Ot之间的距离，sensingrange表示双目相机的最大测量距离。
步骤C1-C5所执行的基于点投票的深度自适应判别算法的原理是：让组成障碍物的所有点在前一帧点云中寻找它的最近邻，从而计算这个点的移动速度，如果这个点的移动速度大于设定的阈值，则这个点投票认为它所在的障碍物是动态障碍物，否则投票认为是静态障碍物。并且只有那些出现在前一帧FOV内且没有被其他物体遮挡的点可以参与投票(被自己遮挡也可以投票)。当投票为动态障碍物的点的比例超过设定的阈值，则认为这个障碍物是动态障碍物，否则为静态障碍物。这个投票阈值会根据物体离相机的距离(深度)自适应调整，因为离相机越远，投影到世界坐标系下的点云会越少，受噪声干扰的可能性越大，且深度越大相机的噪声也越大，这时候投票的阈值需要相应增大。
步骤C1-C5所执行的基于点投票的深度自适应判别算法，可以用以下伪代码表示：
输入：上一帧没有进行过滤的稠密点云
Figure BDA00034450561200001112
无人机系统上一帧的位姿Ot-Δt和当前帧的位姿Ot；当前帧所有聚类障碍物
Figure BDA00034450561200001113
其中
Figure BDA00034450561200001114
(p表示世界坐标系下的点)，判别参数(β，βmin，Vmin)；上一帧深度图Dt-Δt和当前帧深度图Dt。
输出：当前帧所有障碍物的属性(动态/静态/UNKNOWN)。
算法步骤：
Figure BDA0003445056120000121
步骤C4a中的“点p在t-Δt时刻被遮挡”，具体是指点p所在的物体在t-Δt时刻没有发生自遮挡，并且点p在t-Δt时刻被其他其他的点遮挡。也就是说同时发生“点p在t-Δt时刻没有发生自遮挡”以及“判断点p在t-Δt时刻被其他物体的点遮挡”时，可以认为发生了步骤C4a中的“点p在t-Δt时刻被遮挡”。
基于上述原理，在执行步骤C4a中判断点p在t-Δt时刻是否被遮挡的步骤时，具体可以执行以下步骤：
D1.获取ct的质心cent、ct-Δt的质心cent-Δt，以及与时间Δt相关的判断参数β(Δt)；
D2.根据Ot把cent投影到时刻t的像素坐标系，获得qt；
D3.根据Ot-Δt把cent-Δt投影到时刻t-Δt的像素坐标系，获得qt-Δt；
D4.计算qt周围第一范围内的平均深度roundAvg(Dt[qt])；
D5.计算qt-Δt周围第一范围内的平均深度roundAvg(Dt-Δt[qt-Δt])；
D6.如果roundAvg(Dt[qt])-roundAvg(Dt-Δt[qt-Δt])>β(Δt)，判断点p在t-Δt时刻发生了自遮挡，否则判断点p在t-Δt时刻没有发生自遮挡。
当物体的某些点在前一帧被其他物体遮挡时，在前一帧的点云中无法找到它的位置，因此无法计算这个点的移动速度，也就无法通过这个点来判断物体的动静属性，因此这些点不能参与投票。判断点是否被遮挡的原理主要是：如果在上一帧中存在比p点离相机更近的点(上一帧相同位置存在深度更小的点)，那么p点可能被遮挡了。基于上述原理，可以执行以下步骤D7-D10：
D7.根据Ot-Δt把点p投影到t-Δt时刻的相机坐标系
Figure BDA0003445056120000131
获得t-Δt时刻双目相机所在位置到点p的深度
Figure BDA0003445056120000132
其中|.|z表示z方向的坐标值；
D8.根据Ot-Δt把点p投影到t-Δt时刻的像素坐标系qt-Δt；
D9.计算qt-Δt周围第二范围内的平均深度roundAvg(Dt-Δt[qt-Δt])；
D10.如果
Figure BDA0003445056120000133
判断点p在t-Δt时刻被其他点遮挡，否则判断点p在t-Δt时刻没有被其他点遮挡；
D11如果步骤D6中判断点p所在的物体在t-Δt时刻没有发生自遮挡，并且步骤D10中判断点p在t-Δt时刻被其他物体的点遮挡，则判断点p在t-Δt时刻被遮挡。
上述步骤D1-D11中，步骤D1-D6可以判断点p在t-Δt时刻是否发生自遮挡，步骤D7-D10可以判断点p在t-Δt时刻是否被其他点遮挡，步骤D11综合步骤D1-D6的判断结果以及步骤D7-D10的判断结果，判断点p在t-Δt时刻是否被遮挡。
当应用基于物体质心的相机自运动补偿判别算法，在执行步骤S5，也就是识别各聚类簇对应的障碍物属于动态障碍物或者静态障碍物这一步骤时，具体执行以下步骤E1-E4：
E1.获取ct的质心cent、ct-Δt的质心cent-Δt、速度阈值参数Vmin和补偿运动阈值参数β；
E2.判断聚类簇ct对应的障碍物是否需要进行双目相机的自运动补偿，如果需要，跳转执行步骤E3，否则，计算聚类簇ct对应的障碍物的近似速度v＝Length(cent,cent-Δt)/Δt，如果v>Vmin，将聚类簇ct对应的障碍物识别为属于动态障碍物，反之，将聚类簇ct对应的障碍物识别为属于静态障碍物；
E3.计算双目相机运动相对聚类簇ct对应的障碍物质心变化的补偿向量Vc，计算聚类簇ct对应的障碍物质心实际的移动向量Va；如果difference(Vc,Va)<β，其中difference()表示两向量之差的模，则判断物体为静态障碍物，结束对步骤E1-E4的执行，反之，跳转执行步骤E4；
E4.计算聚类簇ct对应的障碍物的速度v＝Length(cent,cent-Δt)/Δt，如果v>Vmin，将聚类簇ct对应的障碍物识别为属于动态障碍物，反之，将聚类簇ct对应的障碍物识别为属于静态障碍物。
步骤E1-E4所执行的基于物体质心的相机自运动补偿判别算法的原理是：基于同一障碍物在前后两帧中的质心变化，可以计算物体的移动速度，如果速度大于阈值，则认为物体是运动的，否则认为物体是静止的。同时，考虑相机自身运动导致物体的相对运动所产生的影响，将相机的运动补偿到物体质心的变化，从而更好地判断物体的动静属性。当物体一直处于相机的FOV内时，能够比较准确算出物体前后两帧的质心位置，相机的自运动不会对其产生明显干扰，这时候不需要进行相机自运动的补偿。但当物体前一帧或者当前帧处于相机FOV的边界上时，就会出现由于相机的自运动导致的物体的某一部分出现/离开相机的FOV，从而导致物体质心的偏移，这时候需要对物体质心的偏移进行补偿。
图3和图4是需要进行双目相机自运动补偿的两种基本情况，其他情况均可从这两种情况中进行拓展。其中一种需要补偿的基本情况如图3所示，当物体处于FOV边界时如果双目相机进行平移运动，会使物体某一部分出现/离开FOV，从而使计算物体质心时发生偏移。这时候需要对物体的质心偏移进行补偿，相应的补偿向量v3可近似计算：v3＝v2-v4，其中v1是双目相机的平移向量，v2是v1在物体表面的投影向量，v4是当前帧下物体边界到FOV边界的向量。另一种需要补偿的基本情况如图4所示，当物体处于FOV边界上而相机进行旋转运动时，同样会使物体某一部分出现/离开FOV，从而使计算物体质心时发生偏移。这时候也需要对物体的质心偏移进行补偿。相应的补偿向量可近似计算：v5＝rot*vc-vc。其中，rot为相机的旋转矩阵，vc为物体在前一帧的质心。因此，相机的运动对物体质心的变化的补偿向量Vc可表示为Vc＝v3+v5。只有物体的质心运动向量与Vc存在明显的差别并且物体运动的速度大于设定的阈值时，它才会被认为是动态障碍物。
当应用基于历史一致性的越近帧权重越大的判别算法，在执行步骤S5，也就是识别各聚类簇对应的障碍物属于动态障碍物或者静态障碍物这一步骤时，具体执行以下步骤F1-F6：
F1.获取时间尺度th、衰减因子α和障碍物跟踪历史
Figure BDA0003445056120000141
其中Δt表示时间间隔，k表示序号；
F2.初始化变量Weight＝1,DynWeight＝0,StaticWeight＝0,unknownWeight＝0；
F3.设置变量I，I从0到th遍历，执行以下步骤F3a-F3d：
F3a.如果聚类簇ct-i*Δt对应的障碍物属于动态障碍物，根据公式DynWeight＝DynWeight+Weight对变量DynWeight进行操作；
F3b.如果聚类簇ct-i*Δt对应的障碍物属于静态障碍物，根据公式StaticWeight＝StaticWeight+Weight对变量StaticWeight进行操作；
F3c.如果聚类簇ct-i*Δt对应的障碍物属于未知障碍物，根据公式unknownWeight＝unknownWeight+Weight对变量unknownWeight进行操作；
F3d.根据公式Weight＝Weight*α对变量Weight进行操作；
F4.如果DynWeight>StaticWeight且DynWeight>unknownWeight，将聚类簇ct对应的障碍物识别为属于动态障碍物，反之执行步骤F5；
F5.如果StaticWeight>DynWeight and StaticWeight>unknownWeight，将聚类簇ct对应的障碍物识别为属于静态障碍物，反之执行步骤F6；
F6.将聚类簇ct对应的障碍物识别为属于未知障碍物。
步骤F1-F6所执行的基于历史一致性的越近帧权重越大的判别算法，可以用以下伪代码表示：
Figure BDA0003445056120000151
步骤F1-F6所执行的基于历史一致性的越近帧权重越大的判别算法的原理是：根据最近几帧障碍物的判定结果进行投票最终确定障碍物的动静属性，且距离现在越近的帧拥有越高的投票权值，而距离现在越远的帧对当前障碍物的动静属性的识别影响越小，因此对当前障碍物的动静属性的判别具有较高的鲁棒性。
在步骤S6中，当步骤S5识别出聚类簇对应的障碍物属于静态障碍物，根据聚类簇更新占用图上的障碍物信息；当步骤S5识别出聚类簇对应的障碍物属于动态障碍物，将聚类簇建模成椭球，使用卡尔曼滤波器对建模后的聚类簇进行跟踪，如果在连续的超过阈值长度的时间段内未跟踪到建模后的聚类簇，结束对聚类簇的跟踪，删除聚类簇对应的数据。
通过执行步骤S6，可以实现无人机对障碍物等环境信息的动态更新，实现动态环境感知。
使用计算机在仿真环境下运行步骤S1-S6，所获得的仿真感知效果如图5和图6所示。由搭建双目相机的无人机运行步骤S1-S6，所获得的实机感知效果图如图7所示。
通过执行步骤S1-S6，无人机可以在仅使用双目相机的情况下，通过自身CPU或者云服务器对双目相机采集到的深度图像进行处理，能在障碍物间距离较近、障碍物出现遮挡、相机自身在运动等复杂环境下快速地、较好地实现对环境中各种形状的障碍物进行检测、动静态属性划分和跟踪的动态环境感知。具体地，在聚类阶段即步骤S3中，用过连通的密度聚类算法对稀疏点云进行聚类，较好地解决了传统DBSCAN算法存在的当物体距离较近时不同的物体容易被聚成一类的问题；在判定障碍物的动静态属性阶段即步骤S5中，通过基于物体质心的相机自运动补偿判别法和基于点投票的深度自适应判别法这两种算法，并使用了基于历史一致性的最近帧权重越大的动静障碍物判别算法进行最终判定的方案，能够在障碍物出现遮挡并且相机自身运动的复杂情况下对障碍物的动静属性进行较鲁棒的判断。
由于无人机仅使用双目相机，以及自身的CPU即可执行实施例中的动态环境感知方法，无需配备激光雷达或者专用计算设备等外设，因此无人机具有负载小、价格便宜、体积小、功耗低、轻便易携等优点。
可以通过编写执行本实施例中的基于双目相机的动态环境感知方法的计算机程序，将该计算机程序写入至计算机装置或者存储介质中，当计算机程序被读取出来运行时，执行本实施例中的基于双目相机的动态环境感知方法，从而实现与实施例中的基于双目相机的动态环境感知方法相同的技术效果。
需要说明的是，如无特殊说明，当某一特征被称为“固定”、“连接”在另一个特征，它可以直接固定、连接在另一个特征上，也可以间接地固定、连接在另一个特征上。此外，本公开中所使用的上、下、左、右等描述仅仅是相对于附图中本公开各组成部分的相互位置关系来说的。在本公开中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。此外，除非另有定义，本实施例所使用的所有的技术和科学术语与本技术领域的技术人员通常理解的含义相同。本实施例说明书中所使用的术语只是为了描述具体的实施例，而不是为了限制本发明。本实施例所使用的术语“和/或”包括一个或多个相关的所列项目的任意的组合。
应当理解，尽管在本公开可能采用术语第一、第二、第三等来描述各种元件，但这些元件不应限于这些术语。这些术语仅用来将同一类型的元件彼此区分开。例如，在不脱离本公开范围的情况下，第一元件也可以被称为第二元件，类似地，第二元件也可以被称为第一元件。本实施例所提供的任何以及所有实例或示例性语言(“例如”、“如”等)的使用仅意图更好地说明本发明的实施例，并且除非另外要求，否则不会对本发明的范围施加限制。
应当认识到，本发明的实施例可以由计算机硬件、硬件和软件的组合、或者通过存储在非暂时性计算机可读存储器中的计算机指令来实现或实施。所述方法可以使用标准编程技术-包括配置有计算机程序的非暂时性计算机可读存储介质在计算机程序中实现，其中如此配置的存储介质使得计算机以特定和预定义的方式操作——根据在具体实施例中描述的方法和附图。每个程序可以以高级过程或面向对象的编程语言来实现以与计算机系统通信。然而，若需要，该程序可以以汇编或机器语言实现。在任何情况下，该语言可以是编译或解释的语言。此外，为此目的该程序能够在编程的专用集成电路上运行。
此外，可按任何合适的顺序来执行本实施例描述的过程的操作，除非本实施例另外指示或以其他方式明显地与上下文矛盾。本实施例描述的过程(或变型和/或其组合)可在配置有可执行指令的一个或多个计算机系统的控制下执行，并且可作为共同地在一个或多个处理器上执行的代码(例如，可执行指令、一个或多个计算机程序或一个或多个应用)、由硬件或其组合来实现。所述计算机程序包括可由一个或多个处理器执行的多个指令。
进一步，所述方法可以在可操作地连接至合适的任何类型的计算平台中实现，包括但不限于个人电脑、迷你计算机、主框架、工作站、网络或分布式计算环境、单独的或集成的计算机平台、或者与带电粒子工具或其它成像装置通信等等。本发明的各方面可以以存储在非暂时性存储介质或设备上的机器可读代码来实现，无论是可移动的还是集成至计算平台，如硬盘、光学读取和/或写入存储介质、RAM、ROM等，使得其可由可编程计算机读取，当存储介质或设备由计算机读取时可用于配置和操作计算机以执行在此所描述的过程。此外，机器可读代码，或其部分可以通过有线或无线网络传输。当此类媒体包括结合微处理器或其他数据处理器实现上文所述步骤的指令或程序时，本实施例所述的发明包括这些和其他不同类型的非暂时性计算机可读存储介质。当根据本发明所述的方法和技术编程时，本发明还包括计算机本身。
计算机程序能够应用于输入数据以执行本实施例所述的功能，从而转换输入数据以生成存储至非易失性存储器的输出数据。输出信息还可以应用于一个或多个输出设备如显示器。在本发明优选的实施例中，转换的数据表示物理和有形的对象，包括显示器上产生的物理和有形对象的特定视觉描绘。
以上所述，只是本发明的较佳实施例而已，本发明并不局限于上述实施方式，只要其以相同的手段达到本发明的技术效果，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。在本发明的保护范围内其技术方案和/或实施方式可以有各种不同的修改和变化。

Claims (10)

1.一种基于双目相机的动态环境感知方法，其特征在于，所述基于双目相机的动态环境感知方法包括：
获取由双目相机拍摄得到的多帧深度图像；
对各帧所述深度图像进行处理，获得多帧稀疏点云；其中，对一帧所述深度图像的处理结果为得到相应的一帧稀疏点云；
对各帧所述稀疏点云分别进行聚类，获得多个聚类簇；其中，对一帧所述稀疏点云的聚类结果为得到相应的若干个个聚类簇，一帧所述稀疏点云中的不同聚类簇分别表示不同的障碍物；
对不同帧的所述稀疏点云中表示相同障碍物的多个所述聚类簇进行关联；
识别关联后的所述聚类簇对应的障碍物属于动态障碍物或者静态障碍物。
2.根据权利要求1所述的基于双目相机的动态环境感知方法，其特征在于，所述基于双目相机的动态环境感知方法还包括：
当所述聚类簇对应的障碍物属于静态障碍物，根据所述聚类簇更新占用图上的障碍物信息；
当所述聚类簇对应的障碍物属于动态障碍物，将所述聚类簇建模成椭球，使用卡尔曼滤波器对建模后的所述聚类簇进行跟踪。
3.根据权利要求2所述的基于双目相机的动态环境感知方法，其特征在于，所述基于双目相机的动态环境感知方法还包括：
当在连续的超过阈值长度的时间段内未跟踪到建模后的所述聚类簇，结束对所述聚类簇的跟踪，删除所述聚类簇对应的数据。
4.根据权利要求1-3任一项所述的基于双目相机的动态环境感知方法，其特征在于，所述对各帧所述深度图像进行处理，获得多帧稀疏点云，包括：
获取所述双目相机的外参矩阵T和内参矩阵K；
获取所述深度图像的像素坐标Puv；
通过公式Pw＝T-1K-1Puv确定原始点云；其中，Pw为所述原始点云的世界坐标；
对所述原始点云进行裁剪，获得稠密点云；
使用体素过滤法对所述稠密点云进行过滤，获得所述稀疏点云。
5.根据权利要求4所述的基于双目相机的动态环境感知方法，其特征在于，所述对各帧所述稀疏点云分别进行聚类，获得多个聚类簇，包括：
A1.获取样本点集D＝{x1,x2,……,xm}，其中xm表示所述稀疏点云中的第m个点，设置邻域距离阈值ε和连通阈值δ，初始化核心对象集合
Figure FDA0003445056110000021
初始化聚类簇数k＝0，初始化未访问样本集合F＝D，初始化簇划分
Figure FDA0003445056110000022
A2.对于j＝1,2,……,m，按下面步骤A2a-A2b找出所有的核心对象：
A2a.找到样本xj的ε邻域内的子样本点集Nε(xj)；其中，xj为样本点集D中的样本点；
A2b.如果所述子样本点集Nε(xj)的样本个数满足|Nε(xj)|≥MinPts，则计算点Q＝Nε(xj)∪xj的连通分支数n；如果n<δ，通过公式Ω＝Ω∪{xj}将样本xj加入核心对象集合Ω中；
A3.如果核心对象集合
Figure FDA0003445056110000023
则结束对步骤A1-A6的执行，否则执行步骤A4；
A4.在核心对象集合Ω中，随机选一个核心对象o，初始化当前簇核心对象列表Ωcur＝{o}，初始化类别序号k＝k+1，初始化当前簇样本集合Ck＝{o}，更新未访问样本集合F-F-{o}；
A5.如果当前簇核心对象队列
Figure FDA0003445056110000024
则当前聚类簇Ck生成完毕，更新簇划分C＝{C1,C2,……,Ck}，更新核心对象集合Ω＝Ω-Ck，返回执行步骤A3，否则更新核心对象集合Ω＝Ω-Ck；
A6.在当前簇核心对象队列Ωcur中取出一个核心对象o‘’，通过邻域距离阈值ε找出ε-邻域子样本点集Nε(o‘’)，令Δ＝Nε(o‘’)∩F，更新当前簇样本集合Ck＝Ck∪Δ，更新未访问样本集合F＝F-Δ，更新Ωcur＝Ωcur∪(Δ∩Ω)-o‘’，返回执行步骤A5。
6.根据权利要求4所述的基于双目相机的动态环境感知方法，其特征在于，所述对不同帧的所述稀疏点云中表示相同障碍物的多个所述聚类簇进行关联，包括：
B1.获取对m个聚类簇
Figure FDA0003445056110000025
其中t表示Ct中的各个聚类簇由采集时间为t的所述稀疏点云聚类得到，其中采集时间为t的所述稀疏点云由拍摄时间为t的所述深度图像处理得到，通过卡尔曼滤波器预测Ct中所有障碍物在时间t下的位置
Figure FDA0003445056110000026
设置距离阈值ε，初始化关联集合F＝Kt；
B2.计算Ct中每个聚类簇
Figure FDA0003445056110000027
的质心
Figure FDA0003445056110000028
得到所有聚类簇当前帧下的质心
Figure FDA0003445056110000029
B3.初始化待关联集合Ω＝Dt；
B4.对于每个
Figure FDA00034450561100000210
按以下步骤B4a-B4c执行：
B4a.找到
Figure FDA0003445056110000031
在F中的最近邻kj；
B4b.找到kj在Ω中的最近邻dj；
B4c.如果
Figure FDA0003445056110000032
即
Figure FDA0003445056110000033
与kj互为最近邻，通过公式
Figure FDA0003445056110000034
把
Figure FDA0003445056110000035
关联到障碍物j；通过公式
Figure FDA0003445056110000036
把
Figure FDA0003445056110000037
移出待关联集合Ω；通过公式F＝F-{kj}把kj移出关联集合F；
B5.如果
Figure FDA0003445056110000038
或者
Figure FDA0003445056110000039
结束对步骤B1-B6的执行，反之，对于每个
Figure FDA00034450561100000310
执行以下步骤B5a-B5b：
B5a.找到
Figure FDA00034450561100000311
在F中的最近邻kj，计算
Figure FDA00034450561100000312
与kj之间的距离δ；
B5b.如果δ<ε，通过公式
Figure FDA00034450561100000313
把
Figure FDA00034450561100000314
关联到障碍物j，通过公式
Figure FDA00034450561100000315
把
Figure FDA00034450561100000316
移出待关联集合Ω，通过公式F＝F-{kj}把kj移出关联集合F；
B6.如果
Figure FDA00034450561100000317
结束对步骤B1-B6的执行，反之，对于每个
Figure FDA00034450561100000318
认为是新出现的障碍物，建立障碍物跟踪历史，所述障碍物跟踪历史表示为
Figure FDA00034450561100000319
其中
Figure FDA00034450561100000320
Figure FDA00034450561100000321
Δt表示时间间隔，n表示障碍物的标号。
7.根据权利要求6所述的基于双目相机的动态环境感知方法，其特征在于，所述识别各所述聚类簇对应的障碍物属于动态障碍物或者静态障碍物，包括：
C1.获取拍摄时间为t-Δt的深度图像Dt-Δt和拍摄时间为t的深度图像Dt，获取采集时间为t-Δt的稠密点云
Figure FDA00034450561100000322
其中所述采集时间为t-Δt的稠密点云
Figure FDA00034450561100000323
由拍摄时间为t-Δt的所述深度图像处理得到，获取所述双目相机所在无人机在t-Δt时刻的位姿Ot-Δt和在t时刻的位姿Ot，获取
Figure FDA00034450561100000324
其中
Figure FDA00034450561100000325
p表示世界坐标系下的点，l表示点的标号，获取判别参数(β，βmin，Vmin)；
C2.遍历Ct中的任意聚类簇
Figure FDA00034450561100000326
其中I＝1,2，……，m，如果聚类簇
Figure FDA00034450561100000327
对应新识别出的障碍物，将聚类簇
Figure FDA00034450561100000328
对应的障碍物识别为属于未知障碍物，所述未知障碍物为既不确定属于动态障碍物也不确定属于静态障碍物的障碍物；
C3.初始化变量vote＝0，dyn＝0，static＝0；
C4.对聚类簇
Figure FDA00034450561100000329
中的每个点p执行以下投票过程：
C4a.根据位姿Ot-Δt判断点p是否在无人机的t-Δt时刻的视场角内，判断点p在t-Δt时刻是否被遮挡；当点p不在无人机的t-Δt时刻的视场角内，且点p在t-Δt时刻被遮挡，根据公式vote＝vote+1对变量vote进行操作，反之执行步骤C4b；
C4b.在稠密点云
Figure FDA0003445056110000041
中查找点p的最近邻nn；当
Figure FDA0003445056110000042
其中length(p,nn)为点p与最近邻nn之间的距离，Δt表示时间间隔，根据公式dyn＝dyn+1对变量dyn进行操作，反之执行步骤C4c；
C4c.根据公式static＝static+1对变量static进行操作；
C5.当
Figure FDA0003445056110000043
将聚类簇
Figure FDA0003445056110000044
对应的障碍物识别为属于动态障碍物，反之将聚类簇
Figure FDA0003445056110000045
对应的障碍物识别为属于静态障碍物；其中
Figure FDA0003445056110000046
表示聚类簇
Figure FDA0003445056110000047
的质心，
Figure FDA0003445056110000048
表示
Figure FDA0003445056110000049
与Ot之间的距离，sensingrange表示所述双目相机的最大测量距离。
8.根据权利要求7所述的基于双目相机的动态环境感知方法，其特征在于，所述判断点p在t-Δt时刻是否被遮挡，包括：
D1.获取ct的质心cent、ct-Δt的质心cent-Δt，以及与时间Δt相关的判断参数β(Δt)；
D2.根据Ot把cent投影到时刻t的像素坐标系，获得qt；
D3.根据Ot-Δt把cent-Δt投影到时刻t-Δt的像素坐标系，获得qt-Δt；
D4.计算qt周围第一范围内的平均深度roundAvg(Dt[qt])；
D5.计算qt-Δt周围第一范围内的平均深度roundAvg(Dt-Δt[qt-Δt])；
D6.如果roundAvg(Dt[qt])-roundAvg(Dt-Δt[qt-Δt])>β(Δt)，判断点p在t-Δt时刻发生了自遮挡，否则判断点p在t-Δt时刻没有发生自遮挡；
D7.根据Ot-Δt把点p投影到t-Δt时刻的相机坐标系
Figure FDA00034450561100000410
获得t-Δt时刻双目相机所在位置到点p的深度
Figure FDA00034450561100000411
其中|.|z表示z方向的坐标值；
D8.根据Ot-Δt把点p投影到t-Δt时刻的像素坐标系qt-Δt；
D9.计算qt-Δt周围第二范围内的平均深度roundAvg(Dt-Δt[qt-Δt])；
D10.如果
Figure FDA00034450561100000412
判断点p在t-Δt时刻被其他点遮挡，否则判断点p在t-Δt时刻没有被其他点遮挡；
D11.如果步骤D6中判断点p所在的物体在t-Δt时刻没有发生自遮挡，并且步骤D10中判断点p在t-Δt时刻被其他物体的点遮挡，则判断点p在t-Δt时刻被遮挡。
9.根据权利要求6所述的基于双目相机的动态环境感知方法，其特征在于，所述识别各所述聚类簇对应的障碍物属于动态障碍物或者静态障碍物，包括：
E1.获取ct的质心cent、ct-Δt的质心cent-Δt、速度阈值参数Vmin和补偿运动阈值参数β；
E2.判断聚类簇ct对应的障碍物是否需要进行双目相机的自运动补偿，如果需要，跳转执行步骤E3，否则，计算聚类簇ct对应的障碍物的近似速度v＝Length(cent,cent-Δt)/Δt，如果v>Vmin，将聚类簇ct对应的障碍物识别为属于动态障碍物，反之，将聚类簇ct对应的障碍物识别为属于静态障碍物；
E3.计算双目相机运动相对聚类簇ct对应的障碍物质心变化的补偿向量Vc，计算聚类簇ct对应的障碍物质心实际的移动向量Va；如果difference(Vc,Va)<β，其中difference()表示两向量之差的模，则判断物体为静态障碍物，结束对步骤E1-E4的执行，反之，跳转执行步骤E4；
E4.计算聚类簇ct对应的障碍物的速度v＝Length(cent,cent-Δt)/Δt，如果v>Vmin，将聚类簇ct对应的障碍物识别为属于动态障碍物，反之，将聚类簇ct对应的障碍物识别为属于静态障碍物。
10.根据权利要求6所述的基于双目相机的动态环境感知方法，其特征在于，所述识别各所述聚类簇对应的障碍物属于动态障碍物或者静态障碍物，包括：
F1.获取时间尺度th、衰减因子α和障碍物跟踪历史
Figure FDA0003445056110000051
其中Δt表示时间间隔，k表示序号；
F2.初始化变量Weight＝1,DynWeight＝0,StaticWeight＝0,unknownWeight＝0；
F3.设置变量I，I从0到th遍历，执行以下步骤F3a-F3d：
F3a.如果聚类簇ct-i*Δt对应的障碍物属于动态障碍物，根据公式DynWeight＝DynWeight+Weight对变量DynWeight进行操作；
F3b.如果聚类簇ct-i*Δt对应的障碍物属于静态障碍物，根据公式StaticWeight＝StaticWeight+Weight对变量StaticWeight进行操作；
F3c.如果聚类簇ct-i*Δt对应的障碍物属于未知障碍物，根据公式unknownWeight＝unknownWeight+Weight对变量unknownWeight进行操作；
F3d.根据公式Weight＝Weight*α对变量Weight进行操作；
F4.如果DynWeight>StaticWeight且DynWeight>unknownWeight，将聚类簇ct对应的障碍物识别为属于动态障碍物，反之执行步骤F5；
F5.如果StaticWeight>DynWeight and StaticWeight>unknownWeight，将聚类簇ct对应的障碍物识别为属于静态障碍物，反之执行步骤F6；
F6.将聚类簇ct对应的障碍物识别为属于未知障碍物。
CN202111652247.8A 2021-12-30 2021-12-30 一种基于双目相机的动态环境感知方法 Pending CN114387462A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111652247.8A CN114387462A (zh) 2021-12-30 2021-12-30 一种基于双目相机的动态环境感知方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111652247.8A CN114387462A (zh) 2021-12-30 2021-12-30 一种基于双目相机的动态环境感知方法

Publications (1)

Publication Number Publication Date
CN114387462A true CN114387462A (zh) 2022-04-22

Family

ID=81199033

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111652247.8A Pending CN114387462A (zh) 2021-12-30 2021-12-30 一种基于双目相机的动态环境感知方法

Country Status (1)

Country Link
CN (1) CN114387462A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116912403A (zh) * 2023-07-03 2023-10-20 上海鱼微阿科技有限公司 一种xr设备及xr设备的障碍物信息感知方法
CN116912403B (zh) * 2023-07-03 2024-05-10 玩出梦想(上海)科技有限公司 一种xr设备及xr设备的障碍物信息感知方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116912403A (zh) * 2023-07-03 2023-10-20 上海鱼微阿科技有限公司 一种xr设备及xr设备的障碍物信息感知方法
CN116912403B (zh) * 2023-07-03 2024-05-10 玩出梦想(上海)科技有限公司 一种xr设备及xr设备的障碍物信息感知方法

Similar Documents

Publication Publication Date Title
US11915502B2 (en) Systems and methods for depth map sampling
JP7345504B2 (ja) Ｌｉｄａｒデータと画像データの関連付け
CN108230361B (zh) 用无人机探测器和追踪器融合来增强目标追踪方法及系统
EP3583380B1 (en) Shape estimating apparatus
CN111665842B (zh) 一种基于语义信息融合的室内slam建图方法及系统
CN108898676B (zh) 一种虚实物体之间碰撞及遮挡检测方法及系统
US9111172B2 (en) Information processing device, information processing method, and program
US20210264188A1 (en) Image prediction system
KR20200027087A (ko) 로봇 및 그의 제어 방법
KR20200143228A (ko) 3차원 가상 공간 모델을 이용한 사용자 포즈 추정 방법 및 장치
CN105204511B (zh) 一种物体自主移动的决策方法
CN111624875A (zh) 视觉伺服控制方法、装置和无人设备
KR20190041840A (ko) 비동기 방식의 목표물 분류에 기반한 모바일 로봇 제어
CN114387462A (zh) 一种基于双目相机的动态环境感知方法
CN115855086A (zh) 基于自旋转的室内场景自主重建方法、系统及介质
WO2021131990A1 (ja) 情報処理装置、情報処理方法、およびプログラム
CN111862218B (zh) 计算机设备定位方法、装置、计算机设备和存储介质
EP3770809A1 (en) Method and apparatus for estimating user&#39;s pose by using three-dimensional virtual space model
CN113168532A (zh) 目标检测方法、装置、无人机及计算机可读存储介质
Kanai et al. Improvement of 3D Monte Carlo localization using a depth camera and terrestrial laser scanner
CN114613002B (zh) 基于光线投影原理的运动视角下动态物体检测方法及系统
CN114353779B (zh) 一种采用点云投影快速更新机器人局部代价地图的方法
CN116295507B (zh) 一种基于深度学习的激光惯性里程计优化方法、系统
Nitti et al. Application of dynamic distributional clauses for multi-hypothesis initialization in model-based object tracking
CN117689682A (zh) 基于深度点云和里程计的运动物体检测方法、装置和存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination