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

1.本发明涉及机器人感知和计算机视觉领域技术领域，具体涉及一种基于光线投影原理的运动视角下动态物体检测方法及系统。


背景技术：

2.基于视觉的同时定位与建图(visual simultaneous localization and mapping，vslam)方法是指通过分析相机获取的图像信息流来估计移动机器人位姿的一种方法。移动机器人系统经常需要执行一些复杂的任务，获知自身的位置和姿态对移动机器人而言尤为重要。相机相较于其他类型的传感器具有重量轻、漂移小、价格低等优点，适合一些小型无人平台进行环境感知。vslam生成的可以用于定位的稀疏的点云地图，但是由于它稀疏的性质无法满足导航与路径规划的需求。为了解决这一问题，常在定位与导航规划之间增加独立的建图模块，将无人平台配备深度传感器并使用建图模块结合位姿信息建立稠密的占据栅格地图，供导航与路径规划使用。
3.以上算法模块均建立在场景静止的假设之下，当无人平台进入动态环境后，需要配备动态物体检测模块来对环境中的动态物体进行感知。在动态场景下，无人平台需要感知动态物体的位姿、形状和速度等信息来规划安全的路线。针对动态场景有以下几种类型的解决方案：
4.方案1：文献j.zhang,m.henein,r.mahony,and v.ila,“vdo-slam:a visual dynamic object-aware slam system,”arxiv:2005.11052,2020.该文献以orb-slam为基础，使用深度学习的方法进行动态物体检测。文献使用静态环境上的特征点与动态目标中的特征点构建优化问题，使用后端优化估计自身姿态变化与动态物体姿态变化。该方案有着优秀的检测效果，在自身位置估计的同时可以输出动态物体的位姿和速度。但是由于该方法使用了离线的语义分割图像，需要使用到深度学习网络，这就带来了极大的运算量。其他的一些基于深度学习的方案也对运算量要求极大，尤其在无人机等小型无人平台上无法满足算力要求以及实时性的要求。
5.方案2：文献zhang t,zhang h,li y,et al.flowfusion:dynamic dense rgb-d slam based on optical flow[j].2020.该文献采用了光流法检测动态物体检测，通过迭代来使检测结果更加精确。但是由于光流法具有灰度不变、图像灰度梯度均匀、运动较小较强的假设，对使用场景有着较强的要求。同时由于相机自身运动以及噪声信号等的存在，基于光流法的动态检测算法在实践中发现误差较大，常常出现误检测等现象。
[0006]
方案3：文献dai w,zhang y,zheng y,et al.rgb-d slam with moving object tracking in dynamic environments[j].iet cyber-systems and robotics.该文献采用光线投影原理将传统的slam的地图点进行投影，将地图点分为动态和静态并将地图点之间相关联并进行优化。该方法有着优秀的检测效果，但是由于该方法将动态检测和slam紧耦合在一起，限制了slam方案的选择。
[0007]
综合以上现有技术方案，可以看出目前已有的针对动态物体检测的方案存在检测
1vt-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
。
[0023]
空间点坐标和像素坐标之间有如下关系，
[0024][0025]
其中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
、fy、c
x
和cy为相机内参，f
x
、fy分别为相机图像二维坐标系两个方向上的相机焦距、c
x
、cy分别为相机图像二维坐标系两个方向上的相机偏移量；k是相机的内参矩阵：
[0026][0027]
因此，在两个时刻分别有：
[0028][0029][0030]
并且同一空间点在两个坐标系下的空间坐标有以下关系：
[0031][0032]
联立以上式子求解，即使用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
的预测值。
[0033]
对深度图像中每一个点进行上述操作就可以得到一张完整的t时刻的深度投影预测图d
′
t
；对每一点上t时刻的深度预测和t时刻的深度观测做差求绝对值，就得到了深度投影误差图至此得到了一张t时刻的深度投影预测图d
′
t
以及深度投影误差图
[0034]
进一步地，步骤2中，对进行二值化，具体包括以下步骤：
[0035]
通过物体运动速度阈值velocity_thesh、程序处理频率f结合深度系数depthscale确定二值化阈值threshold：
[0036][0037]
根据计算得到的深度阈值对深度误差图中每一个像素进行二值化操作，对应的公式如下：
[0038][0039]
是第j行的第i列对应的深度误差值，g(j,i)是二值化后误差图中第j行的第i列对应的值。
[0040]
进一步地，步骤2中，依次进行二值化、腐蚀膨胀、高斯滤波，得到动态目标对应的二维区域；其中高斯滤波使用标准差σ，大小为(2k 1)
×
(2k 1)的卷积核，将图像进行高斯滤波，根据高斯公式确定卷积核中第j行i列的权重h(j,i)如下：
[0041][0042]
进一步地，步骤3：对动态物体进行建模，使用卡尔曼滤波器对动态物体中心进行滤波操作，预测到动态物体的速度信息，将动态物体中心坐标x以及动态物体的速度进行输出，具体为：
[0043]
使用xk＝[x
k y
k zk]
t
表示动态物体空间位置中心，表示动态物体的速度，δt表示程序观测的时间间隔；由于缺乏动态物体的加速度信息，假设物体具有恒定的运动速度：
[0044][0045][0046]
δt＝t
k-t
k-1
[0047]
以为状态变量构造线性的运动学模型：
[0048][0049]
模型的运动方程为：
[0050][0051]
模型的观测方程为：
[0052][0053]ak
与h的具体取值如下：
[0054][0055]
h＝[i
3x3 0
3x3
]
[0056]
上式中ak为状态矩阵；vk为运动过程中受到的噪声；h为观测矩阵；wk为观测过程中受到的噪声；zk为观测值；i
3x3
为3
×
3的单位矩阵；0
3x3
为3
×
3的零矩阵。
[0057]
其中受到随机噪声影响，更新过程和观测过程误差呈高斯分布：vk～n(0,q),wk～n(0,r)，其中q与r分别为运动过程与观测过程中噪声的协方差矩阵；使用此模型按照通用的
流程构造卡尔曼滤波器。
[0058]
采用所构造的卡尔曼滤波器对动态物体的中心进行滤波，得到滤波后的中心坐标以及对应的速度预测。
[0059]
本发明的另外一个实施例还提供了基于管线投影原理的运动视角下动态物体检测系统，构造执行上述基于光线投影原理的运动视角下动态物体检测方法的软件程序，包括深度投影模块、动态物体检测模块以及卡尔曼滤波模块。
[0060]
深度投影模块，采用上一时刻的深度图像d
t-1
结合位姿信息t
t,t-1
进行投影变换，得到本时刻的深度投影预测图d
′
t
，将d
′
t
与本时刻的深度图像观测d
t
相减求绝对值，得到深度投影误差图
[0061]
动态物体检测模块，中包含了物体的运动信息，还包含着噪声信号；结合实验中噪声的特点，对依次进行二值化、腐蚀膨胀、高斯滤波，得到动态目标对应的二维区域，将动态物体对应彩色点云进行输出。
[0062]
卡尔曼滤波模块，对动态物体进行建模，使用卡尔曼滤波器对动态物体中心进行滤波操作，预测到动态物体的速度信息，将动态物体中心坐标x以及动态物体的速度x
·
进行输出。
[0063]
软件程序运行在机器人操作系统ros平台，形成动态物体检测系统。
[0064]
在软件程序中选择图像下采样，同时更新对应的内参矩阵的参数；选定缩放系数为α，相应的内参系数变化为：f
′
x
＝αf
x
；f
′y＝αfy；c
′
x
＝αc
x
；c
′y＝αcy。
[0065]
优选地，在cpu版本程序上采用多线程技术将软件程序进行改写，其中n
t
是线程数，nc是cpu核数；n
t
＝nc 1。
[0066]
优选地，在gpu版本程序上采用cuda编程将软件程序在gpu上进行部署。
[0067]
有益效果：
[0068]
本发明提供的基于光线投影原理的运动视角下动态物体检测方法及系统，以相机位姿、彩色图像和深度图像作为基本输入，使用以光线投影原理为基础的动态物体检测算法对画面中的动态物体进行检测。程序接收到以上消息后使用消息队列对其进行匹配，使用上一时刻的深度图像结合位姿信息进行投影变换，得到本时刻的深度图像预测。然后将本时刻的深度图像预测与本时刻的深度图像观测进行对比，进行相应操作后得到动态物体对应区域。最后将动态物体的中心坐标、运动速度、彩色点云信息进行发布输出。本发明解决了大部分环境感知框架以静态环境为前提，无法感知物体运动信息的问题。
[0069]
本发明无需物体模型先验知识，使用几何方法实现动态目标检测。本发明使用视觉图像和位置消息作为输入，可以可靠的感知环境中存在的动态物体的位置、形状、速度等信息，相较于基于深度学习的方法只能利用训练过的语义模型识别特定的动态物体，本发明无需具备对被检测对象先验知识，只关注于物体是否运动本身。因此本发明不仅可以完成动态检测的任务还可以适用于更广阔的任务场景。
[0070]
本发明采用模块化设计，输入信息简单适用性强。相较于一般解决方案中和slam前端紧耦合的方式，本发明采用模块化的设计，无需依赖特定的slam框架，可以在模块搭配上提供更多的选择。本发明输入采用容易获取的位姿信息、深度图像和彩色图像，也极大的方便了模块间的信息交互，降低了不同模块相互搭配的难度。
[0071]
占用运算资源少，方便无人平台实时性以及计算能力的要求。本发明经过算法流程优化以及采用编程技巧，可以使用较少的计算资源完成动态物体检测，同时满足实时性的要求。
附图说明
[0072]
图1—动态物体检测框架图；
[0073]
图2—光线投影原理图；
[0074]
图3—光线投影流程图；
[0075]
图4—卡尔曼滤波效果图。
具体实施方式
[0076]
下面结合附图并举实施例，对本发明进行详细描述。
[0077]
本发明提供了一种基于光线投影原理的运动视角下动态物体检测方法，本发明的核心框架如图1所示。
[0078]
步骤1：程序通过机器人操作系统ros平台，接收到来自视觉传感器发送的位姿信息、深度图像和彩色图像。三种消息的时间不同步，因此对消息在时间上进行匹配。
[0079]
首先将新接收到的消息存入各自的匹配队列中，每当新的深度图像到来都在三种消息队列中进行一轮搜索，以队列中深度图像的时间为参考时间，在彩色图像和位置信息队列中进行匹配。当时间差均小于阈值时认为匹配成功，构造新的消息结构m
t
。根据实验现象发现，需要在匹配成功后在匹配队列中删除对应的消息防止重复匹配，并且每次匹配过程结束需要根据时间区间判断出没有匹配结果的信息并删除，防止重复查询造成运算资源的浪费甚至程序的运行失败。新的消息结构m
t
包括结构时刻t、位姿变换矩阵t
t
、深度图像d
t
、彩色图像f
t
。将消息结构m
t
＝(t,t
t
,d
t
,f
t
)储存在更新队列。其中位姿变换矩阵t
t
即为位姿信息。
[0080]
取更新队列相邻的两消息取出本时刻深度图像d
t
和上一时刻深度图像d
t-1
，由t
t-1
和t
t-1
计算得到两帧之间相对位姿变换t
t,t-1
：
[0081][0082]
该原理如图2所示，其中p为三维空间一点，在t-1时刻p被深度图d
t-1
观测到，对应的像素为p
t-1
＝[u
t-1vt-1
]
t
，u
t-1
为t-1时刻在深度图像中的像素坐标，v
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分别是像素横坐标与像素纵坐标，x、y、z分别是空间三个轴向坐标，下标t代表其在t时刻的取值。
[0083]
空间点坐标和像素坐标之间有如下关系，
[0084][0085]
其中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
、fy、c
x
和cy为相机内参，f
x
、fy分别为相机图像二维坐标系两个方向上的相机焦距、c
x
、cy分别为相机图像二维坐标系两个方向上的相机偏移量；k是相机的内参矩阵：
[0086]
其中f
x
、fy分别为两个方向上的相机焦距、c
x
、cy分别为两个方向上的相机偏移量，写成矩阵形式
[0087][0088]
其中k是相机的内参矩阵：
[0089][0090]
因此，在两个时刻分别有：
[0091][0092][0093]
并且同一空间点在两个坐标系下的空间坐标有以下关系：
[0094][0095]
联立以上式子求解，就可以使用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
的预测。
[0096]
忽略噪声的影响，如图2所示。当场景中没有动态物体时，该点的深度预测值z
′
t
等于深度观测值z
t
，深度预测误差为0。但是当场景中有动态物体时，由于动态物体的出现，该处的深度观测值发生变化，从而深度预测值z
′
t
不等于深度观测值z
t
，深度预测误差不为0。
[0097]
按照如图3所示的过程，对深度图像中每一个点进行上述操作就可以得到一张完整的t时刻的深度投影预测图d
′
t
。对每一点上t时刻的深度预测和t时刻的深度观测做差求绝对值，就得到了深度投影误差图
[0098]
至此得到了一张t时刻的深度投影预测图d
′
t
以及深度投影误差图可以进行之后的操作。
[0099]
步骤二：动态物体检测以及噪声过滤算法
[0100]
中包含了物体的运动信息，因此可以使用对动态物体进行检测。同时由于深度图像的噪声和位姿估计误差，还包含着噪声信号。经过实验观察，这些噪声区域往往具有以下特点：噪声区域面积较小、存在多个噪声区域且彼此之间相互分离、噪声产生的观测误差较大。针对以上特点，本发明采用依次进行二值化、形态学滤波、高斯滤波、寻找连通域、动态物体检测。
[0101]
通过物体运动速度阈值velocity_thesh、程序处理频率f结合深度系数depthscale确定二值化阈值threshold：
[0102][0103]
根据计算得到的深度阈值对深度误差图中每一个像素进行二值化操作，当误差大于阈值时即认为此像素点对应空间点处于运动状态，否则则认为该像素点对应空间点静止。对应的公式如下，是第j行的第i列对应的深度误差值，g(j,i)是二值化后第j行的第i列对应的值。
[0104][0105]
经过二值化后的误差图由于噪声存在，除了检测到运动物体对应区域外还会出现噪声点对应区域。根据噪声面积较小且噪声之间相互分离特点，采用形态学滤波算法，将二值化后的图像先后进行腐蚀膨胀操作，将小面积的噪声消除，同时将大范围的动态物体保留。经过了腐蚀操作后噪声对应的区域被过滤掉，同时算法保留了动态物体的主体部分。经过腐蚀操作的主体部分存在孔洞，进行膨胀操作后算法将检测主体部分进行还原，得到了较为完整的动态物体区域。
[0106]
为了防止中间细小的动态物体在形态学滤波后被分成两个部分，再进行高斯滤波，解决上述问题，同时将物体的边界进行平滑。使用标准差σ，大小为(2k 1)
×
(2k 1)的卷积核，将图像进行高斯滤波。其中的具体参数根据实验设定，根据高斯公式确定卷积核中第j行i列的权重如下：
[0107][0108]
对滤波后的图像进行连通域的查找，使用opencv中的connectedcomponentswithstats函数即可得到原图像中的标记图，每个连通域的外接矩形和面积以及质心的位置。考虑到滤波算法对检测精度带来一定影响，只有检测到的动态物体面积大于画面的百分之一时才认为该处存在动态物体，并将物体信息进行输出，否则不输出物体信息。
[0109]
步骤3：对动态物体进行建模，使用卡尔曼滤波器对动态物体中心进行滤波操作，预测到动态物体的速度信息，将动态物体中心坐标x以及动态物体的速度进行输出。
[0110]
以上步骤可以检测到动态物体带有噪声的三维空间坐标，噪声来源于深度图像噪声、位姿估计噪声、视角范围限制等。因此采用卡尔曼滤波算法对动态物体中心进行滤波以及对动态物体速度进行预测。对动态障碍物建立以下模型：
[0111]
使用xk＝[x
k y
k zk]
t
表示动态物体空间位置中心，表示动态物体的速度，δt表示程序观测的时间间隔。由于缺乏动态物体的加速度信息，假设物体具有恒定的运动速度：
[0112][0113][0114]
δt＝t
k-t
k-1
[0115]
以为状态变量构造线性的运动学模型：
[0116][0117]
模型的运动方程为：
[0118][0119]
模型的观测方程为：
[0120][0121]ak
与h的具体取值如下：
[0122][0123]
h＝[i
3x3 0
3x3
]
[0124]
上式中ak为状态矩阵；vk为运动过程中受到的噪声；h为观测矩阵；wk为观测过程中受到的噪声；zk为观测值；i
3x3
为3
×
3的单位矩阵；0
3x3
为3
×
3的零矩阵。
[0125]
其中受到随机噪声影响，更新过程和观测过程误差呈高斯分布：vk～n(0,q),wk～n(0,r)，其中q与r分别为运动过程与观测过程中噪声的协方差矩阵。使用此模型按照通用的流程构造卡尔曼滤波器，使用到的其他符号均为中间过程量，下标p表示状态估计值，下标m表示最优估计值，k与k-1是离散时刻：
[0126][0127][0128]kk
＝p
p,kht
(hp
p,kht
r)-1
[0129][0130][0131]
按照此过程对动态物体的中心进行滤波，就可以得到滤波后的中心坐标以及对应的速度预测。
[0132]
为了验证本发明提供的检测方法的效果，本发明根据实验中动态物体中心坐标出现的特点，设计了一条带有噪声的中心轨迹。该物体中心沿着空间直线匀速前进，为轨迹增加了高斯噪声来模拟算法成功检测时结果中的误差，为轨迹按照一定概率增加值为零的噪声，以模拟算法在某些时候失效。整个算法的效果如图4所示，左边是未经滤波的轨迹图，右
边是经过卡尔曼滤波的轨迹图，图中的长直线为轨迹真值，可以看到卡尔曼滤波器对物体的轨迹有着较好的滤波效果。
[0133]
本发明的另外一个实施例还提供了一种执行上述基于光线投影原理的运动视角下动态物体检测方法的系统，即构造执行上述基于光线投影原理的运动视角下动态物体检测方法的软件程序，包括深度投影模块、动态物体检测模块以及卡尔曼滤波模块。
[0134]
深度投影模块，采用上一时刻的深度图像d
t-1
结合位姿信息t
t,t-1
进行投影变换，得到本时刻的深度投影预测图d
′
t
，将d
′
t
与本时刻的深度图像观测d
t
相减求绝对值，得到深度投影误差图
[0135]
动态物体检测模块，中包含了物体的运动信息，还包含着噪声信号；结合实验中噪声的特点，对依次进行二值化、腐蚀膨胀、高斯滤波，得到动态目标对应的二维区域，将动态物体对应彩色点云进行输出。
[0136]
卡尔曼滤波模块，对动态物体进行建模，使用卡尔曼滤波器对动态物体中心进行滤波操作，预测到动态物体的速度信息，将动态物体中心坐标x以及动态物体的速度进行输出。
[0137]
软件程序运行在机器人操作系统ros平台，形成动态物体检测系统。
[0138]
在cpu版本程序上采用多线程技术将软件程序进行改写，其中n
t
是线程数，nc是cpu核数；n
t
＝nc 1。
[0139]
在gpu版本程序上采用cuda编程将软件程序在gpu上进行部署。
[0140]
由于以上软件程序本身对无人平台的算力要求较高，本发明采用了一些方法来对该检测方法进行加速。
[0141]
首先在程序中可以选择图像下采样，同时更新对应的内参矩阵的参数。下采样操作可以缩小图像尺寸到可处理的范围内，减少算力消耗。根据实验过程选定缩放系数为α，在本实验条件下选定α＝0.5；相应的内参系数变化为：
[0142]f′
x
＝αf
x f
′y＝αfy[0143]c′
x
＝αc
x c
′y＝αcy[0144]
其次在cpu版本动态物体检测程序中本发明采用了多线程技术，将最为耗时的光线投影操作分为若干个独立的线程，提高了cpu的利用效率。对于线程数的设定，采取经验公式，其中n
t
是线程数，nc是cpu核数。
[0145]nt
＝nc 1
[0146]
最后针对拥有gpu运算资源的平台，本发明的gpu版本的程序可以将算法运行效率进一步提高。使用基于nvidia的cuda进行gpu编程实现。cuda是一种基于c 的gpu编程语言，专门用于nvidia显卡的开发，可以进行高效的多线程并行处理
[0147]
在本发明中，图像尺寸为width
×
height，将图像数据传递到gpu显存当中，gpu接收到数据后，开启总共width
×
height数量的线程进行并行处理，每个线程处理一个像素点的深度数据。
[0148]
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。