CN108805906A - 一种基于深度图的运动障碍物检测及定位方法 - Google Patents

Abstract

一种基于深度图的运动障碍物检测及定位方法，涉及一种基于双目视觉深度图像的运动障碍物检测与定位方法。本发明为了解决现有基于视觉的障碍物检测与定位方法存在的计算量大、实时性较差、难以区分图像特征较为相似的障碍物与周围环境的问题。本发明通过平行配置的双目摄像机采集深度图，并对深度图像进行去噪、二值化等处理获得候选障碍物在图像中的轮廓，然后将候选障碍物所在位置转换到局部地理坐标系中，结合空间位置关系去除特定位置干扰，并使用卡尔曼滤波的方式对障碍物的运动状态进行估计，提到对于障碍物的定位准确度。本发明适用于基于双目视觉深度图像的运动障碍物检测与定位。

Description

一种基于深度图的运动障碍物检测及定位方法

技术领域

本发明涉及数字图像处理与信息融合领域，具体涉及双目视觉中的深度图处理方法。

背景技术

无人机是一种无人驾驶的，通过遥控或者机载控制设备进行飞行的飞机。无人机由于不受载人因素制约，通常体积较小、机动性较强、造价也更为低廉，因此近年来逐渐成为了研究的热点。根据机身结构的不同，无人机可以分为旋翼式无人机与固定翼式无人机。V.Kumar等人曾论证在小负载的情况下，正交多旋翼飞行器的机动性要好于其他类型的飞行器。

四旋翼飞行器作为多旋翼飞行器的代表，受到了越来越多的关注。同时传感器技术、计算机技术以及数据处理技术的快速发展，使得各类传感器与处理器呈现出低功耗、小型化的趋势，这为无人机自主飞行并执行特定任务提供了可能。现阶段，无人机已经在搜索救援、农业植保以及自动放牧等领域表现出很大的发展潜力。然而，由于无人机感知外界环境的相关技术仍然不成熟，这极大地限制了无人机在这些领域的进一步发展。

运动物体检测技术是实现无人机与目标机器人交互同时自动避障的关键技术与前提条件。目前，无人机感知运动物体主要是通过雷达和计算机视觉进行。现有的小型雷达已经可以在小型无人机上使用。但是由于需要满足线检测的条件，所以传感器覆盖的视野十分受限。同时由于无人机的体积与负载的限制，很难设置增大其视野范围的机械结构。而随着图像技术的不断方法，基于视觉的障碍物信息检测也不断成熟和丰富，因此采用视觉传感器的方法感知周边环境，检测运动物体已经成为一种趋势。视觉传感器避障的配置方案可以分为单目相机目和多目相机。单目相机目和多目相机各有优劣，单目相机硬件配置简单，计算量较小，但是由于获取的信息相对较少，尤其是难以获得图像深度信息，因此难以对运动的物体进行准确定位；多目相机硬件配置较为复杂，计算量也较大，但是与单目相机相比能够较好地解决图像深度的获取问题，较为适合对运动物体的检测，但是对于图像信息的处理也相对复杂。

障碍物的检测方法关键在于如何更好地描述障碍物特征。针对这个问题，研究者们做了大量的研究，也提出了众多人工设计的特征描述方法。此类特征主要是基于设计者本身对于障碍物特征的理解，往往具有易于理解、便于调整改进的优点，因此也被广泛应用于各类障碍物检测算法中。早在1998年，C.P.Papageorgiou就提出了Haar特征，之后的2002年，R.Lienhart与J.Maydt对Haar特征库进行扩展，加入了对角特征。1999年，D.G.Lowe提出了基于梯度特征的SIFT(Scale Invariant Feature Transform)方法，并于2004年对该方法进行了进一步的完善，该方法具有较好的尺度不变性与旋转不变性，即改变图像的拍摄角度以及周围环境亮度不会影响特征点的提取效果。2005年，N.Dalal等提出了针对静态图像物体检测的HOG方法。该方法通过统计图像局部区域的梯度方向直方图来构成待检测物体的特征。由于HOG方法的处理对象是图像中的局部连通区域，所以它对图像中物体的几何形变与光学形变都能有较好的适应性。2008年，P.Felzenszwalb对HOG方法进行了一些优化和改进，提出了DPM，(Deformable Parts Model)方法，该系统将DPM与Latent SVM结合，取得了极好的检测效果。

此外，随着GPU并行计算能力的不断加强，利用模型和网络逐渐拟合出图像特征的方法逐渐被应用起来。1998年，Y.LeCun等人就提出了LeNet-5网络。2012年，A.Krizhevsky等人在LeNet-5网络的基础上进行了深入和改进，提出了AlexNet网络。该网络增加了网络的层数以及各层之间的卷积核数，从而大幅增加了网络的可训练参数，提高了网络的检测能力。2015年产生了VGG网络与GoogLeNet网络[18]。VGG网络在AlexNet网络的基础上继续复杂化，值得一提的是，VGG网络首次提出了采用多个小卷积核来替代一个大卷积核的做法。在该网络中，全部的卷积核大小均为3×3，但是通过使用多次使用来模拟更大的卷积核，如5×5、7×7等。而这种做法也被后来的网络广泛采用。GoogLeNet的代表性贡献则是提出了Inception模块，在该模块中不单单是多个卷积核的组合，而且加入了瓶颈层以减少运算量，从而实现在保证检测效果的情况，消耗更少的计算量。2016年，何恺明等人提出了ResNet网络。此外，针对图像处理领域，又有基于区域提名的R-CNN网络、SPP-Net网络、FastR-CNN网络、Faster R-CNN网络以及无需区域提名的的YOLO网络与SSD网络。此类网络的检测效果往往比传统的图像特征描述方法更好，但是需要很大的计算量。

综上所述，现有的图像处理算法虽然往往能实现较好的效果，但是针对某些特定情况却会存在一些问题。常规的障碍物检测往往集中于对于视野中被识别目标特征的识别，但是在实际环境中往往存在一些特征比较相似，或者特征难以描述的情况。而利用模型和网络逐渐拟合出图像特征的检测方法，诸如深度学习网络Faster R-CNN、YOLO等由于所需的计算量较大，因此难以在机载平台上在线实时运行，实用性较差。

发明内容

本发明为了解决现有的障碍物检测方法所存在的难以识别某些特征与环境相似的障碍物以及计算量较大、难以快速高效运行的问题。

一种基于深度图的运动障碍物检测及定位方法，包括以下步骤：

步骤一、将八个焦距相等、内部参数一致的摄像头分为四组，每组两个，即构成四组双目摄像头。将四组双目摄像头分别安装到四旋翼飞行器的前、后、左、右四个方向上。将每组双目摄像头按照平行配置方式进行配置，并建立平型配置双目摄像头的成像模型；

步骤二、摄像头同时采集视频图像，获取每组双目摄像头所形成的深度图。并以灰度图的形式显示出来；

步骤三、对获取到的深度图进行二值化处理，得到黑白二值图像，然后进行形态学处理，并提取图像中的轮廓；

步骤四、去除图像中长宽比以及像素数量不满足要求的轮廓，计算出每个轮廓对应的候选障碍物在相机坐标系下的位置，并计算剩下轮廓之间的极角，将极角小于一定值的两个轮廓合并为一个轮廓，得到候选障碍物在图像坐标系中的位置；

步骤五、利用四旋翼飞行器惯性测量单元的相关信息，求取每个轮廓对应的候选障碍物在局部地理坐标系下的位置；

步骤六、去除在局部地理坐标系下高度在特定区间内候选障碍物，得到最终检测到的障碍物；

步骤七、利用卡尔曼滤波方法对障碍物的运动状态进行估计，最终获得障碍物的运动轨迹。

进一步地，步骤一所述的将每组双目摄像头按照平行配置方式进行配置的过程包括以下步骤：

将两个焦距相等、内部参数一致的摄像头C1与C2以光轴互相平行的方式固定。由于光轴与图像平面垂直，使得两个摄像机的图像坐标系的y轴互相平行，x轴互相重合。可以认为将一个摄像机沿其x轴平移一段距离后便与另一个摄像机完全重合。

进一步地，步骤一所述的建立平行配置双目摄像头的成像模型的过程包括以下步骤：

首先确定平行配置双目摄像头C1与C2的两个坐标系仅在x轴上相差的平移距离b，即为基线长度。接下来假设C1的坐标系为O_lX_lY_lZ_l，C2的坐标系为O_rX_rY_rZ_r，在上述摄像机配置下，空间点P(X_c,Y_c,Z_c)在C1坐标系下为(X_l,Y_l,Z_l)，在C2坐标系下为(X_l-b,Y_r,Z_r)。同时假设P点在左摄像头图像平面和右摄像头图像平面上的图像像素坐标为(u₁,v₁)，(u₂,v₂)。根据平行配置方法的特点可得到v₁＝v₂，令视差d＝u₁-u₂。则根据几何关系可以求得：

其中

其中(u₀,v₀)为主点在左图像上的坐标，u'₀为主点在右图像上的横坐标，f为两个摄像头的焦距。

在双目相机平行配置的条件下，u₀＝u'₀，矩阵Q右下角元素值为0。进而得到点P在三维空间的坐标为：

进一步地，步骤二的具体过程包括以下步骤：

步骤二一、以左摄像头图像平面上的一点为中心选定一区域(窗口)，由该区域内的像素灰度分布来表征该像素。在右摄像头图像平面上沿水平线在搜索范围内取出窗口大小相同的像素邻域。

步骤二二、利用绝对误差和算法(SAD算法)依次与待匹配的窗口进行匹配代价计算，找到搜索范围内SAD值最小的窗口，作为与待匹配窗口最佳匹配的像素块。SAD算法的具体计算公式如下所示：

其中，(i,j)为窗口的左上角坐标。M与N为窗口的长度与宽度。S(i+s-1,j+t-1)，T(s,t)分别为右摄像机图像与左摄像机图像的像素灰度值之和。选取D_SAD(i,j,d)值最小的窗口作为匹配窗口。d值为所求的视差。视差图可以通过这种方法得到。在得到视差图后，我们根据得到的平行配置双目摄像头的成像模型求得任意点的三维深度，生成原始深度图。

步骤二三、对原始深度图进行预处理，将其转换为8位单通道灰度图以方便显示；原始深度图每个像素点存储的数据范围是0至32767，以毫米为单位；根据检测范围将灰度值的单位转化为设定值α，单位是厘米；根据上述需求对深度图的数据进行压缩，压缩公式如下所示：

其中，d₁₆为原始深度图每个像素的数据，d₈为预处理之后的深度图数据。然后将得到的数据进行四舍五入，以满足8位单通道灰度图的显示要求。

进一步地，步骤三的具体过程包括以下步骤：

步骤三一、观察深度图可以发现深度图中的噪点集中的灰度值区间，确定二值化处理的图像阈值，得到黑白二值图像。

步骤三二、利用数学形态学处理方法中的腐蚀和膨胀算子对二值图像进行操作，去除噪声点和填补目标区域中的小孔。膨胀算子的具体计算公式如下所示：

g(u,v)＝erode[f(u,v),B]＝min{f(u+du,v+dv)-B(du,dv)|(du,dv)∈D_B}

腐蚀算子的具体计算公式如下所示：

g(u,v)＝dilate[f(u,v),B]＝min{f(u-du,v-dv)+B(du,dv)|(du,dv)∈D_B}

其中，g(u,v)为腐蚀后的灰度图像，f(u,v)为原灰度图像，B为结构元素。腐蚀运算是由结构元素确定的邻域块中选取图像值与结构元素值的差的最小值。膨胀运算是由结构元素确定的邻域块中选取图像值与结构元素值的和的最大值。

步骤三三、利用二值图像的黑白边界得到多个图像轮廓。

进一步地，步骤四所述的计算出每个轮廓对应的候选障碍物在相机坐标系下位置的过程包括以下步骤：

利用公式

求得各个轮廓在相机坐标系下的坐标值。

进一步地，步骤四所述的获取计算轮廓之间的极角并将极角小于一定值的两个轮廓合并为一个轮廓的过程包括以下步骤：

求取各个轮廓在摄像头C1极坐标系下的角度极角，当二个轮廓之间的极角小于一定值时将这两个轮廓合并。计算极角的具体公式如下所示：

该步骤主要用于解决障碍物表面反射的问题，即一个障碍物在深度图像中显示成为两个连通区域的情况。

进一步地，步骤五的具体过程包括以下步骤：

步骤五一、通过标定的方法标定算法标定得到的标定模板与摄像机之间的坐标转换矩阵为R_cca，标定模板与机体之间的坐标转换矩阵R_bca可以通过标定模板的摆放位置确定。为了方便计算，通常令模板的坐标轴分别与机体坐标系的坐标轴平行放置。可以求得相机坐标系到机体坐标系的转换矩阵R_bc，具体的计算公式为：

R_bc＝R_bca·(R_cca)^-1

步骤五二、通过无人机的惯性测量单元得到无人机的姿态信息，此信息是以四元数q₀,q₁,q₂,q₃的形式返回。需要将其转化为旋转矩阵。转换关系式如下所示。

步骤五三、利用公式

求得局部地理坐标系与地理坐标系的之间的旋转矩阵R_mg，其中θ为局部地理坐标系的X_m轴与地理坐标系X_g轴之间的夹角。接下来利用公式R_mb＝R_mg·R_gb求得局部地理坐标系与机体坐标系之间的转换矩阵R_mb。最后利用公式R_mc＝R_mb·R_bc求得相机坐标系到局部地理坐标系的转换矩阵R_mc。

步骤五四、利用公式

可以求得每个轮廓对应的候选障碍物在局部地理坐标系下的位置。

进一步地，步骤七的具体过程包括以下步骤：

步骤七一、设障碍物机器人的状态为X_o＝[x_o y_o v_ox v_oy]^T，其中x_o和y_o代表障碍物机器人在局部地理坐标系的X_m轴方向和Y_m轴方向上的位置坐标，v_ox和v_oy代表障碍物机器人在局部地理坐标系的x轴方向和y轴方向上的速度分量。则有如下公式：

X_e＝X_o-X_p＝[Z_e1 Z_e2 Z_e3 Z_e4]^T

其中X_e为障碍物机器人与飞行器之间的状态变量差值，包括相对位置以及相对速度。则可以得到状态方程为：

其中a_ox和a_oy分别为障碍物机器人在局部地理坐标系的X_m轴方向和Y_m轴方向上的加速度分量；a_px和a_py分别为四旋翼飞行器在局部地理坐标系的X_m轴方向和Y_m轴方向上的加速度分量，可以通过飞行器机载IMU模块获得。

步骤七二、通过以上对于目标机器人运动状况的描述，我们可以得到障碍物机器人与四旋翼飞行器之间的运动过程模型：

其中T_b是两次获取信息之间的时间间隔，ξ_e1、ξ_e2、ξ_e3和ξ_e4为系统过程噪声。

步骤七三、由于状态变量中的障碍物与飞行器的相对速度信息无法测量得到，测量信息只有相对位置，因此测量模型可以写为：

其中Y_e(k)为测量模型输出，η_e是测量过程的噪声。

步骤七四、利用卡尔曼公式迭代得到障碍物的运动轨迹。

进一步地，步骤四所述的机体坐标系的具体表述为：

机体坐标系O_bX_bY_bZ_b的原点O_b为无人机的质心，X_b轴位于无人机参考平面内且平行于机身轴线并指向无人机前方，Y_b轴垂直于无人机参考面并指向无人机右方，Z_b轴在参考面内垂直于X_bO_bY_b平面，指向无人机下方。

进一步地，步骤五所述的局部地理坐标系、地理坐标系和局部地理坐标系的具体表述为：

局部地理坐标系O_mX_mY_mZ_m以无人机的质心为原点，坐标轴分别于场地边缘平行，Z_m轴指向正下方。

地理坐标系O_gX_gY_gZ_g以无人机起飞点作为坐标原点O_g，X_g轴指向正北方，Y_g轴指向正东方，Z_g轴指向正下方。

局部地理坐标系O_mX_mY_mZ_m以无人机的质心为原点，坐标轴分别于规定场地边缘平行。Z_m轴指向正下方。

本发明具有以下有益效果：

本发明是一种基于深度图并结合空间位置关系去除特定位置干扰的运动障碍物检测方法。该方法针对无人机自主飞行情景下进行运动障碍物检测与定位的实际需求进行设计，实用性较强。本发明解决了现有基于视觉的障碍物检测与定位方法存在的计算量大、实时性较差、难以区分图像特征较为相似的障碍物与周围环境的问题。

本发明通过平行配置的双目摄像机采集深度图，并对深度图像进行去噪、二值化等处理获得候选障碍物在图像中的轮廓，然后将候选障碍物所在位置转换到局部地理坐标系中，结合空间位置关系去除特定位置干扰，并使用卡尔曼滤波的方式对障碍物的运动状态进行估计，提到对于障碍物的定位准确度。本发明适用于基于双目视觉深度图像的运动障碍物检测与定位。

本发明所需的硬件配置简单，安装方便，体积轻，重量小，具有较高的便携性和可拓展性。

本发明充分考虑了四旋翼飞行器机载平台计算能力有限的问题，提出的方法具有计算量较小，实时性高，对于处理器的性能要求较低等优点。

本发明解决了现有基于视觉的障碍物检测与定位方法存在的计算量大、实时性较差、难以区分图像特征较为相似的障碍物与周围环境的问题。

本发明在具有主频为2.6Ghz的X86架构的处理器以及2G内存的计算平台上运行，处理器使用率为8％，内存占用率为14％，处理频率大于20hz，满足实时性要求。同时对于障碍物的检测精度小于10cm。

附图说明

图1是具体实施方式一的方法流程图；

图2是双目摄像头平行配置方式示意图；

图3是实施例中障碍物与实验环境图；

图4是障碍物位置解算过程示意图；

图5是实施例中障碍物原始深度图像；

图6是实施例中经过实施方式处理后的障碍物图像；

图7是实施例中障碍物运动轨迹对比图；

图8是实施例中障碍物定位误差图。

具体实施方式

具体实施方式一：结合图1说明本实施方式，本实施方式所述的一种基于深度图的运动障碍物检测及定位方法，包括以下步骤：

步骤一、将八个焦距相等、内部参数一致的摄像头分为四组，每组两个，即构成四组双目摄像头。将四组双目摄像头分别安装到四旋翼飞行器的前、后、左、右四个方向上。将每组双目摄像头按照平行配置方式进行配置，并建立平型配置双目摄像头的成像模型；图2为双目摄像头平行配置方式示意图。

步骤二、摄像头同时采集视频图像，获取每组双目摄像头所形成的深度图。并以灰度图的形式显示出来；

步骤三、对获取到的深度图进行二值化处理，得到黑白二值图像，然后进行形态学处理，并提取图像中的轮廓；

步骤四、去除图像中长宽比以及像素数量不满足要求的轮廓，计算出每个轮廓对应的候选障碍物在相机坐标系下的位置，并计算剩下轮廓之间的极角，将极角小于一定值的两个轮廓合并为一个轮廓，得到候选障碍物在图像坐标系中的位置；

步骤五、利用四旋翼飞行器惯性测量单元的相关信息，求取每个轮廓对应的候选障碍物在局部地理坐标系下的位置；

步骤六、去除在局部地理坐标系下高度在特定区间内候选障碍物，得到最终检测到的障碍物；

步骤七、利用卡尔曼滤波方法对障碍物的运动状态进行估计，最终获得障碍物的运动轨迹。

该方法采用白色细长(10cm左右)高度不等的柱状运动体作为障碍物。处理场景如图3所示。由于需要为飞行器的飞行提供定位信息，场景内布置有白色方形网格线。且地面网格线与障碍物的特征十分相似，同时室内环境中的灯管及其他物体由于与障碍物具有相同的特征，也容易造成误检测。本发明采用此环境进行实施方式示例。

具体实施方式二：结合图2说明本实施方式，

本实施方式步骤一所述的将每组双目摄像头按照平行配置方式进行配置的过程包括以下步骤：

将两个焦距相等、内部参数一致的摄像头C1与C2以光轴互相平行的方式固定。由于光轴与图像平面垂直，使得两个摄像机的图像坐标系的y轴互相平行，x轴互相重合。可以认为将一个摄像机沿其x轴平移一段距离后便与另一个摄像机完全重合。

其他步骤和参数与具体实施方式一相同。

具体实施方式三：

本实施方式步骤一所述的建立平行配置双目摄像头的成像模型的过程包括以下步骤：

首先确定平行配置双目摄像头C1与C2的两个坐标系仅在x轴上相差的平移距离b，即为基线长度。接下来假设C1的坐标系为O_lX_lY_lZ_l，C2的坐标系为O_rX_rY_rZ_r，在上述摄像机配置下，空间点P(X_c,Y_c,Z_c)在C1坐标系下为(X_l,Y_l,Z_l)，在C2坐标系下为(X_l-b,Y_r,Z_r)。同时假设P点在左摄像头图像平面和右摄像头图像平面上的图像像素坐标为(u₁,v₁)，(u₂,v₂)。根据平行配置方法的特点可得到v₁＝v₂，令视差d＝u₁-u₂。则根据几何关系可以求得：

其中

其中(u₀,v₀)为主点在左图像上的坐标，u'₀为主点在右图像上的横坐标，f为两个摄像头的焦距。

在双目相机平行配置的条件下，u₀＝u'₀，矩阵Q右下角元素值为0。进而得到点P在三维空间的坐标为：

其他步骤和参数与具体实施方式一或二相同。

具体实施方式四：

本实施方式步骤二的具体过程包括以下步骤：

步骤二一、以左摄像头图像平面上的一点为中心选定一区域(窗口)，由该区域内的像素灰度分布来表征该像素。在右摄像头图像平面上沿水平线在搜索范围内取出窗口大小相同的像素邻域。

步骤二二、利用绝对误差和算法(SAD算法)依次与待匹配的窗口进行匹配代价计算，找到搜索范围内SAD值最小的窗口，作为与待匹配窗口最佳匹配的像素块。SAD算法的具体计算公式如下所示：

其中，(i,j)为窗口的左上角坐标。M与N为窗口的长度与宽度。S(i+s-1,j+t-1)，T(s,t)分别为右摄像机图像与左摄像机图像的像素灰度值之和。选取D_SAD(i,j,d)值最小的窗口作为匹配窗口。d值为所求的视差。视差图可以通过这种方法得到。在得到视差图后，我们根据得到的平行配置双目摄像头的成像模型求得任意点的三维深度，生成原始深度图。

步骤二三、对原始深度图进行预处理，将其转换为8位单通道灰度图以方便显示。原始深度图每个像素点存储的数据范围是0至32767，以毫米为单位。考虑到实际需要的检测范围是0至10米，因此需要将灰度值的单位转化为5cm。根据上述需求对深度图的数据进行压缩，压缩公式如下所示：

其中，d₁₆为原始深度图每个像素的数据，d₈为预处理之后的深度图数据。然后将得到的数据进行四舍五入，以满足8位单通道灰度图的显示要求。

其他步骤和参数与具体实施方式一至三之一相同

具体实施方式五：

本实施方式步骤三的具体过程包括以下步骤：

步骤三一、观察深度图可以发现深度图中的噪点集中的灰度值区间，确定二值化处理的图像阈值，得到黑白二值图像。在实验中发现深度图中的噪点多集中在灰度值为0至30的范围内，因此通常以灰度值30作为阈值进行图像分割获得二值化图像。

步骤三二、利用数学形态学处理方法中的腐蚀和膨胀算子对二值图像进行操作，去除噪声点和填补目标区域中的小孔。膨胀算子的具体计算公式如下所示：

g(u,v)＝erode[f(u,v),B]＝min{f(u+du,v+dv)-B(du,dv)|(du,dv)∈D_B}

腐蚀算子的具体计算公式如下所示：

g(u,v)＝dilate[f(u,v),B]＝min{f(u-du,v-dv)+B(du,dv)|(du,dv)∈D_B}

其中，g(u,v)为腐蚀后的灰度图像，f(u,v)为原灰度图像，B为结构元素。腐蚀运算是由结构元素确定的邻域块中选取图像值与结构元素值的差的最小值。膨胀运算是由结构元素确定的邻域块中选取图像值与结构元素值的和的最大值。

步骤三三、利用二值图像的黑白边界得到多个图像轮廓。

其他步骤和参数与具体实施方式一至四之一相同。

具体实施方式六：

本具体实施方式步骤四所述的计算出每个轮廓对应的候选障碍物在相机坐标系下位置的过程包括以下步骤：

利用公式

求得各个轮廓在相机坐标系下的坐标值。

其他步骤和参数与具体实施方式一至五之一相同。

具体实施方式七：

本具体实施方式步骤四所述的获取计算轮廓之间的极角并将极角小于一定值的两个轮廓合并为一个轮廓的过程包括以下步骤：

求取各个轮廓在摄像头C1极坐标系下的角度极角，当二个轮廓之间的极角小于一定值时将这两个轮廓合并。计算极角的具体公式如下所示：

该步骤主要用于解决障碍物表面反射的问题，即一个障碍物在深度图像中显示成为两个连通区域的情况。

其他步骤和参数与具体实施方式一至六之一相同。

具体实施方式八：结合图4说明本实施方式，

本具体实施方式步骤五的具体过程包括以下步骤：

步骤五一、通过标定的方法标定算法标定得到的标定模板与摄像机之间的坐标转换矩阵为R_cca，标定模板与机体之间的坐标转换矩阵R_bca可以通过标定模板的摆放位置确定。为了方便计算，通常令模板的坐标轴分别与机体坐标系的坐标轴平行放置。如对于安装在飞行器前方的双目摄像头，可以令标定模板坐标系中的x轴，y轴，z轴分别与机体坐标系的z轴，y轴，x轴重合。可以求得相机坐标系到机体坐标系的转换矩阵R_bc，具体的计算公式为：

R_bc＝R_bca·(R_cca)^-1

步骤五二、通过无人机的惯性测量单元得到无人机的姿态信息，此信息是以四元数q₀,q₁,q₂,q₃的形式返回。需要将其转化为旋转矩阵。转换关系式如下所示。

步骤五三、利用公式

求得局部地理坐标系与地理坐标系的之间的旋转矩阵R_mg，其中θ为局部地理坐标系的X_m轴与地理坐标系X_g轴之间的夹角。接下来利用公式R_mb＝R_mg·R_gb求得局部地理坐标系与机体坐标系之间的转换矩阵R_mb。最后利用公式R_mc＝R_mb·R_bc求得相机坐标系到局部地理坐标系的转换矩阵R_mc。

步骤五四、利用公式

求得每个轮廓对应的候选障碍物在局部地理坐标系下的位置。

其他步骤和参数与具体实施方式一至七之一相同。

具体实施方式九：

本具体实施方式步骤七的具体过程包括以下步骤：

步骤七一、设障碍物机器人的状态为X_o＝[x_o y_o v_ox v_oy]^T，其中x_o和y_o代表障碍物机器人在局部地理坐标系的X_m轴方向和Y_m轴方向上的位置坐标，v_ox和v_oy代表障碍物机器人在局部地理坐标系的x轴方向和y轴方向上的速度分量。则有如下公式：

X_e＝X_o-X_p＝[Z_e1 Z_e2 Z_e3 Z_e4]^T

其中X_e为障碍物机器人与飞行器之间的状态变量差值，包括相对位置以及相对速度。则可以得到状态方程为：

其中a_ox和a_oy分别为障碍物机器人在局部地理坐标系的X_m轴方向和Y_m轴方向上的加速度分量；a_px和a_py分别为四旋翼飞行器在局部地理坐标系的X_m轴方向和Y_m轴方向上的加速度分量，可以通过飞行器机载IMU模块获得。

步骤七二、通过以上对于目标机器人运动状况的描述，我们可以得到障碍物机器人与四旋翼飞行器之间的运动过程模型：

其中T_b是两次获取信息之间的时间间隔，ξ_e1、ξ_e2、ξ_e3和ξ_e4为系统过程噪声。

步骤七三、由于状态变量中的障碍物与飞行器的相对速度信息无法测量得到，测量信息只有相对位置，因此测量模型可以写为：

其中Y_e(k)为测量模型输出，η_e是测量过程的噪声。

步骤七四、利用卡尔曼公式迭代得到障碍物的运动轨迹。

其他步骤和参数与具体实施方式一至八之一相同。

具体实施方式十：

本具体实施方式步骤四所述的机体坐标系具体表述为：

机体坐标系O_bX_bY_bZ_b的原点O_b为无人机的质心，X_b轴位于无人机参考平面内且平行于机身轴线并指向无人机前方，Y_b轴垂直于无人机参考面并指向无人机右方，Z_b轴在参考面内垂直于X_bO_bY_b平面，指向无人机下方。

其他步骤和参数与具体实施方式一至九之一相同。

具体实施方式十一：

本具体实施方式步骤五所述的局部地理坐标系、地理坐标系和局部地理坐标系具体表述为：

局部地理坐标系O_mX_mY_mZ_m以无人机的质心为原点，坐标轴分别于场地边缘平行，Z_m轴指向正下方。

地理坐标系O_gX_gY_gZ_g以无人机起飞点作为坐标原点O_g，X_g轴指向正北方，Y_g轴指向正东方，Z_g轴指向正下方。

局部地理坐标系O_mX_mY_mZ_m以无人机的质心为原点，坐标轴分别于规定场地边缘平行。Z_m轴指向正下方。

其他步骤和参数与具体实施方式一至十之一相同。

按照本实施方式进行障碍物检测的效果如图5和图6所示，图5为未经过的处理的原始深度图，图6为使用该检测方法之后获得的障碍物图像。通过图5和图6的对比能够明显看出本实施方式可以较好地去除干扰，获得比较准确的障碍物图像。

按照本实施方式进行障碍物检测的效果如图7和图8所示，图7为使用本实施方式测得的障碍物运动轨迹和通过动作捕捉系统测得的障碍物的运动轨迹对比图。其中实线为使用本实施方式测得的障碍物机器人运动轨迹，虚线为通过动作捕捉系统测得的障碍物的运动轨迹。图8为本实施方式测得的障碍物机器人运动轨迹相对于动作捕捉系统的定位误差图。从图8可以看出，误差最大值小于9cm，说明本实施方式对于障碍物的定位效果较好。

Claims (10)

1.一种基于深度图的运动障碍物检测及定位方法，其特征在于，包括以下步骤：

步骤一、将八个焦距相等、内部参数一致的摄像头分为四组，每组两个，即构成四组双目摄像头；将四组双目摄像头分别安装到四旋翼飞行器的前、后、左、右四个方向上；将每组双目摄像头按照平行配置方式进行配置，并建立平型配置双目摄像头的成像模型；

步骤二、摄像头同时采集视频图像，获取每组双目摄像头所形成的深度图；并以灰度图的形式显示出来；

步骤三、对获取到的深度图进行二值化处理，得到黑白二值图像，然后进行形态学处理，并提取图像中的轮廓；

步骤四、去除图像中长宽比以及像素数量不满足要求的轮廓，计算出每个轮廓对应的候选障碍物在相机坐标系下的位置，并计算剩下轮廓之间的极角，将极角小于预设值的两个轮廓合并为一个轮廓，得到候选障碍物在图像坐标系中的位置；

步骤五、利用四旋翼飞行器惯性测量单元的相关信息，求取每个轮廓对应的候选障碍物在局部地理坐标系下的位置；

步骤六、去除在局部地理坐标系下高度在特定区间内候选障碍物，得到最终检测到的障碍物；

步骤七、利用卡尔曼滤波方法对障碍物的运动状态进行估计，最终获得障碍物的运动轨迹。

2.根据权利要求1所述的一种基于深度图的运动障碍物检测及定位方法，其特征在于，步骤一所述的将每组双目摄像头按照平行配置方式进行配置的过程包括以下步骤：

将两个焦距相等、内部参数一致的摄像头C1与C2以光轴互相平行的方式固定；由于光轴与图像平面垂直，使得两个摄像机的图像坐标系的y轴互相平行，x轴互相重合；将一个摄像机沿其x轴平移一段距离后便与另一个摄像机完全重合。

3.根据权利要求2所述的一种基于深度图的运动障碍物检测及定位方法，其特征在于，步骤一所述的建立平行配置双目摄像头的成像模型的过程包括以下步骤：

首先确定平行配置双目摄像头C1与C2的两个坐标系仅在x轴上相差的平移距离b，即为基线长度；设定C1的坐标系为O_lX_lY_lZ_l，C2的坐标系为O_rX_rY_rZ_r，在上述摄像机配置下，空间点P(X_c,Y_c,Z_c)在C1坐标系下为(X_l,Y_l,Z_l)，在C2坐标系下为(X_l-b,Y_r,Z_r)；同时设P点在左摄像头C1的图像平面和右摄像头C2的图像平面上的图像像素坐标为(u₁,v₁)，(u₂,v₂)；根据平行配置方法的特点可得到v₁＝v₂，令视差d＝u₁-u₂，则根据几何关系可求得：

其中Q为中间矩阵；W为系数；

其中(u₀,v₀)为主点在左摄像头图像上的横、纵坐标，u'₀为主点在右摄像头图像上的横坐标，f为两个摄像头的焦距(双目摄像头C1与C2的焦距)；

在双目相机平行配置的条件下，u₀＝u'₀，矩阵Q右下角元素值为0；进而得到点P在三维空间的坐标为：

4.根据权利要求3所述的一种基于深度图的运动障碍物检测及定位方法，其特征在于，步骤二的具体过程包括以下步骤：

步骤二一、以左摄像头图像平面上的一点为中心选定一区域(窗口)，由该区域内的像素灰度分布来表征该像素；在右摄像头图像平面上沿水平线在搜索范围内取出窗口大小相同的像素邻域；

步骤二二、利用绝对误差和算法(SAD算法)依次与待匹配的窗口进行匹配代价计算，找到搜索范围内SAD值最小的窗口，作为与待匹配窗口最佳匹配的像素块；SAD算法的具体计算公式如下所示：

其中，(i,j)为窗口的左上角坐标；M与N为窗口的长度与宽度；S(i+s-1,j+t-1)，T(s,t)分别为右摄像机图像与左摄像机图像的像素灰度值之和；选取D_SAD(i,j,d)值最小的窗口作为匹配窗口；d值为所求的视差；视差图利用SAD算法得到，在得到视差图后，根据得到的平行配置双目摄像头的成像模型求得任意点的三维深度，生成原始深度图；

步骤二三、对原始深度图进行预处理，将其转换为8位单通道灰度图以方便显示；原始深度图每个像素点存储的数据范围是0至32767，以毫米为单位；根据检测范围将灰度值的单位转化为设定值α，单位是厘米；根据上述需求对深度图的数据进行压缩，压缩公式如下所示：

其中，d₁₆为原始深度图每个像素的数据，d₈为预处理之后的深度图数据；然后将得到的数据进行四舍五入，以满足8位单通道灰度图的显示要求。

5.根据权利要求4所述的一种基于深度图的运动障碍物检测及定位方法，其特征在于，步骤三的具体过程包括以下步骤：

步骤三一、观察深度图可以发现深度图中的噪点集中的灰度值区间，确定二值化处理的图像阈值，得到黑白二值图像；

步骤三二、利用数学形态学处理方法中的腐蚀和膨胀算子对二值图像进行操作，去除噪声点和填补目标区域中的小孔；膨胀算子的具体计算公式如下所示：

g(u,v)＝erode[f(u,v),B]＝min{f(u+du,v+dv)-B(du,dv)|(du,dv)∈D_B}

腐蚀算子的具体计算公式如下所示：

g(u,v)＝dilate[f(u,v),B]＝min{f(u-du,v-dv)+B(du,dv)|(du,dv)∈D_B}

其中，g(u,v)为腐蚀后的灰度图像，f(u,v)为原灰度图像，B为结构元素；腐蚀运算是由结构元素确定的邻域块中选取图像值与结构元素值的差的最小值；膨胀运算是由结构元素确定的邻域块中选取图像值与结构元素值的和的最大值；

步骤三三、利用二值图像的黑白边界得到多个图像轮廓。

6.根据权利要求5所述的一种基于深度图的运动障碍物检测及定位方法，其特征在于，步骤四所述的计算出每个轮廓对应的候选障碍物在相机坐标系下位置的过程包括以下步骤：

利用公式

求得各个轮廓在相机坐标系下的坐标值。

7.根据权利要求6所述的一种基于深度图的运动障碍物检测及定位方法，其特征在于，步骤四所述的获取计算轮廓之间的极角并将极角小于一定值的两个轮廓合并为一个轮廓的过程包括以下步骤：

求取各个轮廓在摄像头C1极坐标系下的角度极角，当二个轮廓之间的极角小于一定值时将这两个轮廓合并；计算极角θ的具体公式如下所示：

X_l1Z_l1为一个轮廓在摄像头C1坐标系下的坐标值，X_l2Z_l2为另一个轮廓在摄像头C1坐标系下的坐标值。

8.根据权利要求7所述的一种基于深度图的运动障碍物检测及定位方法，其特征在于，步骤五的具体过程包括以下步骤：

步骤五一、通过标定的方法标定算法标定得到的标定模板与摄像机之间的坐标转换矩阵为R_cca，标定模板与机体之间的坐标转换矩阵R_bca可以通过标定模板的摆放位置确定；令模板的坐标轴分别与机体坐标系的坐标轴平行放置，求得相机坐标系到机体坐标系的转换矩阵R_bc，具体的计算公式为：

R_bc＝R_bca·(R_cca)^-1

步骤五二、通过无人机的惯性测量单元得到无人机的姿态信息，此信息是以四元数q₀,q₁,q₂,q₃的形式返回；需要将其转化为旋转矩阵，转换关系式如下所示：

步骤五三、利用公式

求得局部地理坐标系与地理坐标系的之间的旋转矩阵R_mg，其中θ为局部地理坐标系的X_m轴与地理坐标系X_g轴之间的夹角；接下来利用公式R_mb＝R_mg·R_gb求得局部地理坐标系与机体坐标系之间的转换矩阵R_mb；最后利用公式R_mc＝R_mb·R_bc求得相机坐标系到局部地理坐标系的转换矩阵R_mc；

步骤五四、利用公式

求得每个轮廓对应的候选障碍物在局部地理坐标系下的位置；

地理坐标系O_gX_gY_gZ_g以无人机起飞点作为坐标原点O_g，X_g轴指向正北方，Y_g轴指向正东方，Z_g轴指向正下方；

局部地理坐标系O_mX_mY_mZ_m以无人机的质心为原点，坐标轴分别于规定场地边缘平行；Z_m轴指向正下方。

9.根据权利要求8所述的一种基于深度图的运动障碍物检测及定位方法，其特征在于，步骤七的具体过程包括以下步骤：

步骤七一、设障碍物机器人的状态为X_o＝[x_o y_o v_ox v_oy]^T，其中x_o和y_o代表障碍物机器人在局部地理坐标系的X_m轴方向和Y_m轴方向上的位置坐标，v_ox和v_oy代表障碍物机器人在局部地理坐标系的x轴方向和y轴方向上的速度分量，则有如下公式：

X_e＝X_o-X_p＝[Z_e1 Z_e2 Z_e3 Z_e4]^T

其中X_e为障碍物机器人与飞行器之间的状态变量差值，包括相对位置以及相对速度；则可以得到状态方程为：

其中a_ox和a_oy分别为障碍物机器人在局部地理坐标系的X_m轴方向和Y_m轴方向上的加速度分量；a_px和a_py分别为四旋翼飞行器在局部地理坐标系的X_m轴方向和Y_m轴方向上的加速度分量，可以通过飞行器机载IMU模块获得；

步骤七二、通过以上对于目标机器人运动状况的描述，得到障碍物机器人与四旋翼飞行器之间的运动过程模型：

其中T_b是两次获取信息之间的时间间隔，ξ_e1、ξ_e2、ξ_e3和ξ_e4为系统过程噪声；

步骤七三、由于状态变量中的障碍物与飞行器的相对速度信息无法测量得到，测量信息只有相对位置，因此测量模型可以写为：

其中Y_e(k)为测量模型输出，η_e是测量过程的噪声；

步骤七四、利用卡尔曼公式迭代得到障碍物的运动轨迹。

10.根据权利要求9所述的一种基于深度图的运动障碍物检测及定位方法，其特征在于，步骤四所述的机体坐标系具体表述为：

机体坐标系O_bX_bY_bZ_b的原点O_b为无人机的质心，X_b轴位于无人机参考平面内且平行于机身轴线并指向无人机前方，Y_b轴垂直于无人机参考面并指向无人机右方，Z_b轴在参考面内垂直于X_bO_bY_b平面，指向无人机下方。