基于光线投影原理的运动视角下动态物体检测方法及系统

技术特征：
1.基于光线投影原理的运动视角下动态物体检测方法，其特征在于，包括以下步骤：步骤1：采用上一时刻的深度图像d
t-1
结合位姿信息t
t,t-1
进行投影变换，得到本时刻的深度投影预测图d
′
t
，将d
′
t
与本时刻的深度图像观测d
t
相减求绝对值，得到深度投影误差图步骤2：中包含了物体的运动信息，还包含着噪声信号；结合实验中噪声的特点，对依次进行二值化、腐蚀膨胀、高斯滤波，得到动态目标对应的二维区域，将动态物体对应彩色点云进行输出；步骤3：对动态物体进行建模，使用卡尔曼滤波器对动态物体中心进行滤波操作，预测到动态物体的速度信息，将动态物体中心坐标x以及动态物体的速度进行输出。2.如权利要求1所述的基于光线投影原理的运动视角下动态物体检测方法，其特征在于，所述步骤1还包括：接收到来自视觉传感器发送的三种消息，包括位姿信息、深度图像和彩色图像；三种消息的时间不同步，对三种消息在时间上进行匹配；匹配成功后，构建新的消息结构m
t
，m
t
包括结构时刻t、位姿变换矩阵t
t
、深度图像d
t
、彩色图像f
t
；将消息结构m
t
＝(t,t
t
,d
t
,f
t
)储存在更新队列。3.如权利要求2所述的基于光线投影原理的运动视角下动态物体检测方法，其特征在于，所述对三种消息在时间上进行匹配，具体为：为三种消息构建各自的匹配队列，分别为位姿匹配队列、深度图像匹配队列以及彩色图像匹配队列；将新接收到的三种消息存入各自的匹配队列中，每当新的深度图像到来都在三种匹配队列中进行一轮搜索，以深度图像匹配队列中深度图像的时间为参考时间，分别与彩色图像匹配队列和位置信息匹配队列中进行匹配；若匹配队列中各时间差均小于阈值时认为匹配成功，构造新的消息结构m
t
；在匹配成功的匹配队列中删除匹配成功的消息防止重复匹配，并且每次匹配过程结束后，根据时间区间判断出没有匹配结果的信息并删除，防止重复查询。4.如权利要求2所述的基于光线投影原理的运动视角下动态物体检测方法，其特征在于，所述步骤1，具体包括以下步骤：取更新队列相邻的两消息取出本时刻深度图像d
t
和上一时刻深度图像d
t-1
，由t
t-1
和t
t-1
计算得到两帧之间相对位姿变换t
t,t-1
：t
t,t-1
＝t
t-1
t
t-1
其中p为三维空间一点，在t-1时刻p被深度图d
t-1
观测到，对应的像素为p
t-1
＝[u
t-1 v
t-1
]
t
，u
t-1
为t-1时刻在深度图像中的像素坐标，v
t-1
为t-1时刻在深度图像中的像素坐标，t-1时刻点的深度为z
t-1
，在t
t-1
对应的位姿坐标系下空间点p对应空间坐标p
t-1
＝[x
t-1 y
t-1 z
t-1
]
t
；在t时刻p被深度图d
t
观测到，对应的像素为p
t
＝[u
t v
t
]
t
，u
t
为t时刻在深度图像中的像素坐标，v
t
为t时刻在深度图像中的像素坐标，点的深度为z
t
，在t
t
对应的坐标系下空间点p对应空间坐标p
t
＝[x
t y
t z
t
]
t
；空间点坐标和像素坐标之间有如下关系，
其中u、v分别是像素横坐标与像素纵坐标，u的取值包括u
t
、u
t-1
，v的取值包括v
t-1
、v
t
，x、y、z分别是空间三个轴向坐标，x的取值包括x
t-1
、x
t
，y的取值包括y
t-1
、y
t
，z的取值包括z
t-1
、z
t
，下标t代表其在t时刻的取值，下标t-1代表在t-1时刻的取值；f
x
、f
y
、c
x
和c
y
为相机内参，f
x
、f
y
分别为相机图像二维坐标系两个方向上的相机焦距、c
x
、c
y
分别为相机图像二维坐标系两个方向上的相机偏移量；k是相机的内参矩阵：因此，在两个时刻分别有：因此，在两个时刻分别有：并且同一空间点在两个坐标系下的空间坐标有以下关系：联立以上式子求解，即使用t-1时刻深度图的每一个像素以及其深度测量值，结合相机内参矩阵k与t
t,t-1
去预测该空间点在t时刻对应的像素坐标p
′
t
＝[u
′
t v
′
t
]
t
以及点的深度预测值z
′
t
；其中p
′
t
、u
′
t
、v
′
t
、z
′
t
增加了上标表示对p
t
、u
t
、v
t
、z
t
的预测值；对深度图像中每一个点进行上述操作就可以得到一张完整的t时刻的深度投影预测图d
′
t
；对每一点上t时刻的深度预测和t时刻的深度观测做差求绝对值，就得到了深度投影误差图至此得到了一张t时刻的深度投影预测图d
′
t
以及深度投影误差图5.如权利要求4所述的基于光线投影原理的运动视角下动态物体检测方法，其特征在于，所述步骤2中，对进行二值化，具体包括以下步骤：通过物体运动速度阈值velocity_thesh、程序处理频率f结合深度系数depthscale确定二值化阈值threshold：根据计算得到的深度阈值对深度误差图中每一个像素进行二值化操作，对应的公式如下：
是第j行的第i列对应的深度误差值，g(j,i)是二值化后误差图中第j行的第i列对应的值。6.如权利要求5所述的基于光线投影原理的运动视角下动态物体检测方法，其特征在于，所述步骤2中，依次进行二值化、腐蚀膨胀、高斯滤波，得到动态目标对应的二维区域；其中高斯滤波使用标准差σ，大小为(2k 1)
×
(2k 1)的卷积核，将图像进行高斯滤波，根据高斯公式确定卷积核中第j行i列的权重h(j,i)如下：7.如权利要求4～6任一所述的基于光线投影原理的运动视角下动态物体检测方法，其特征在于，所述步骤3：对动态物体进行建模，使用卡尔曼滤波器对动态物体中心进行滤波操作，预测到动态物体的速度信息，将动态物体中心坐标x以及动态物体的速度进行输出，具体为：使用x
k
＝[x
k y
k z
k
]
t
表示动态物体空间位置中心，表示动态物体的速度，δt表示程序观测的时间间隔；由于缺乏动态物体的加速度信息，假设物体具有恒定的运动速度：序观测的时间间隔；由于缺乏动态物体的加速度信息，假设物体具有恒定的运动速度：δt＝t
k-t
k-1
以为状态变量构造线性的运动学模型：模型的运动方程为：模型的观测方程为：a
k
与h的具体取值如下：h＝[i
3x3 0
3x3
]上式中a
k
为状态矩阵；v
k
为运动过程中受到的噪声；h为观测矩阵；w
k
为观测过程中受到的噪声；z
k
为观测值；i
3x3
为3
×
3的单位矩阵；0
3x3
为3
×
3的零矩阵；其中受到随机噪声影响，更新过程和观测过程误差呈高斯分布：v
k
～n(0,q),w
k
～n(0,r)，其中q与r分别为运动过程与观测过程中噪声的协方差矩阵；使用此模型按照通用的流程构造卡尔曼滤波器；采用所构造的卡尔曼滤波器对动态物体的中心进行滤波，得到滤波后的中心坐标以及对应的速度预测。
8.基于管线投影原理的运动视角下动态物体检测系统，其特征在于，构造执行如权利要求1～7所述的基于光线投影原理的运动视角下动态物体检测方法的软件程序，包括深度投影模块、动态物体检测模块以及卡尔曼滤波模块；所述深度投影模块，采用上一时刻的深度图像d
t-1
结合位姿信息t
t,t-1
进行投影变换，得到本时刻的深度投影预测图d
′
t
，将d
′
t
与本时刻的深度图像观测d
t
相减求绝对值，得到深度投影误差图所述动态物体检测模块，中包含了物体的运动信息，还包含着噪声信号；结合实验中噪声的特点，对依次进行二值化、腐蚀膨胀、高斯滤波，得到动态目标对应的二维区域，将动态物体对应彩色点云进行输出；所述卡尔曼滤波模块，对动态物体进行建模，使用卡尔曼滤波器对动态物体中心进行滤波操作，预测到动态物体的速度信息，将动态物体中心坐标x以及动态物体的速度进行输出；所述软件程序运行在机器人操作系统ros平台，形成动态物体检测系统；在软件程序中选择图像下采样，同时更新对应的内参矩阵的参数；选定缩放系数为α，相应的内参系数变化为：f
′
x
＝αf
x
；f
′
y
＝αf
y
；c
′
x
＝αc
x
；c
′
y
＝αc
y
。9.如权利要求8所述的基于管线投影原理的运动视角下动态物体检测软件，其特征在于，在cpu版本程序上采用多线程技术将所述软件程序进行改写，其中n
t
是线程数，n
c
是cpu核数；n
t
＝n
c
1。10.如权利要求8所述的基于管线投影原理的运动视角下动态物体检测软件，其特征在于，在gpu版本程序上采用cuda编程将所述软件程序在gpu上进行部署。

技术总结
本发明公开了基于光线投影原理的运动视角下动态物体检测方法及系统，以相机位姿、彩色图像和深度图像作为基本输入，使用以光线投影原理为基础的动态物体检测算法对画面中的动态物体进行检测。程序接收到以上消息后使用消息队列对其进行匹配，使用上一时刻的深度图像结合位姿信息进行投影变换，得到本时刻的深度图像预测。然后将本时刻的深度图像预测与本时刻的深度图像观测进行对比，进行相应操作后得到动态物体对应区域。最后将动态物体的中心坐标、运动速度、彩色点云信息进行发布输出。本发明解决了大部分环境感知框架以静态环境为前提，无法感知物体运动信息的问题。无法感知物体运动信息的问题。无法感知物体运动信息的问题。


技术研发人员：方浩 王奥博 胡展铭 魏韶谆
受保护的技术使用者：北京理工大学
技术研发日：2022.02.22
技术公布日：2022/6/10