欢迎来到利索能及~ 联系电话:18621327849
利索能及
我要发布
收藏
专利号: 2023110549885
申请人: 哈尔滨理工大学
专利类型:发明专利
专利状态:已下证
专利领域: 计算;推算;计数
更新日期:2024-10-29
缴费截止日期: 暂无
联系人

摘要:

权利要求书:

1.一种动态环境下联合多目标跟踪的物体SLAM方法,其特征在于,包括以下步骤:S1、获取输入图像序列,包括RGB图像;

S2、对输入帧的RGB图像进行ORB特征点的提取;

S3、将输入帧的RGB图像输入短时密集连接STDC网络进行语义分割,得到含有运动物体语义信息的运动物体掩膜;

S4、根据STDC网络获得的物体掩膜,将落在运动物体掩膜上的特征点判定为动态特征点,否则,判定为静态特征点;

S5、将输入帧t的RGB图像输入单目3D目标检测SMOKE网络进行三维目标检测,得到该帧中所有目标的目标检测数据 其中nt为检测到的物体的数量,每一个检测结果 为一个8维向量(x,y,z,l,w,h,θ,s),其中x,y,z为物体中心的三维坐标,l,w,h为物体长宽高,θ为朝向角,s为置信度;

S6、对S5所得到的目标检测数据创建对象实例,结合卡尔曼滤波预测之前帧目标在当前帧的位置,使用匈牙利匹配算法进行数据关联以实现目标跟踪,具体步骤如下:S61、将目标轨迹状态建模为10维向量T=(x,y,z,θ,l,w,h,vx,vy,vz),其中vx,vy,vz分别代表目标在3D空间的速度;

S62、在输入帧t中,对前一帧所有目标轨迹状态 进行前向预测:其中,xest,yest,zest为当前帧目标中心的预测坐标,mt‑1为前一帧中的轨迹数,则目标轨迹状态在当前帧被相应的预测为:其中, 为当前帧目标轨迹状态的预测;

S63、计算每一个预测轨迹 与检测结果 间的3D交并比IoU,并以此来构建维数为mt‑1×nt的相似度矩阵;

S64、判断每个3D交并比IoU是否小于阈值IoUmin,若小于,则匹配失败,否则匹配成功,得到数据关联的输出:其中,Tmatch与Dmatch为匹配成功的轨迹与检测的数量,wt为成功匹配的轨迹与检测的数量,Tunmatch与Dunmatch为匹配失败的轨迹与检测的数量;

S65、在Dmatch中根据每个轨迹的相应检测更新Tmatch中每个轨迹的状态,得到最终关联轨迹 其中每个轨迹的更新状态为 此处k∈{1,2,...,wt}, 为 与 的加权平均值,权重由 与 的状态不确定性确定;

S66、对Dunmatch在连续帧中匹配失败的次数进行计数,若大于Fmin帧就创建新的轨迹对象S67、对Tunmatch在连续帧中匹配失败的次数进行计数,若大于Agemax帧,则取消对其的跟踪;

S7、使用S6得到的对象状态数据,结合S4中得到的动态与静态特征点构建图优化,包括以下步骤:S71、通过对S6得到的对象状态数据与S4得到的动态与静态特征点计算三维欧氏距离得到动态对象与静态对象的初始位姿,并将动态对象与静态对象的初始位姿设为静态对象节点与动态对象节点;

S72、将S4中得到的动态与静态特征点设为动态点节点与静态点节点;

S73、根据相机在t‑1帧与t帧的位姿变换T和相机在t‑1帧的位姿确定相机在t帧的初始位姿,并将其设为相机节点;

S74、根据动态对象在t‑1帧的位姿与t‑1帧到t帧的时间戳确定动态对象在t帧的速度,并将其设为速度节点;

S75、以静态点节点与相机节点为顶点构造静态投影边ecp,以相机节点与动态物体节点为顶点构造动态物体测量边eco‑3D,以静态物体节点与静态点节点为顶点构造静态物体测量边eop,以相机节点、动态物体节点与动态点节点为顶点构造三元动态重投影边edp,以前一帧动态物体节点、当前帧动态物体节点与速度节点构造三元速度估计边emo;

S76、对上述构造的图优化问题进行求解,获得当前帧相机、动静态特征点与动静态物体的优化后位姿。