CN104899869A - 基于rgb-d相机和姿态传感器的平面和障碍检测方法 - Google Patents

Abstract

本发明公开了一种基于RGB-D相机和姿态传感器的平面和障碍检测方法，该方法基于RGB-D相机和姿态探测器实现了分辨率更高、刷新频率更高、精度更高的环境三维信息识别；能够实现地面通路识别，地面坑洼和各种障碍的判断预警，上下楼梯场景的识别，更接近人的视觉对环境的感知；有姿态传感器的配合，整个系统更加灵活、携带方便，相机不需要与使用者或地面等任何参照物保持固定的相对位置。视障人士出行时，可以将相机放置在身体的任何可以拍摄三维环境的位置，可以选择自己与RGB-D相机兼容的任何便携式计算机、平板实现同样功能。

Description

基于RGB-D相机和姿态传感器的平面和障碍检测方法

技术领域

本发明属于三维环境识别、盲人辅助技术领域，涉及一种基于RGB-D相机和姿态传感器的平面和障碍检测方法。

背景技术

视觉信息是人类识别周围环境的最重要信息来源，人类获得的信息中80％左右是从视觉系统输入的。对于缺失视觉感知的盲人，手杖、导盲犬等传统工具无法为他们提供充分的协助。上世纪中期以来，众多研究人员致力于研发电子盲人辅助技术。电子式盲人辅助技术，主要功能是为视障人士出行提供通路提示、危险预警。

根据探测三维环境的手段不同，传统的电子盲人辅助技术主要分为有超声波和激光测距辅助、双目立体视觉辅助。基于传统超声波或激光的主动测距技术缺点显而易见，易受干扰、分辨率低、探测范围小、输出信息有限。这类系统实际上仅是一个具有报警功能的距离探测装置，它与使用者交互的内容十分有限，交互形式也较为单一，实际使用中容易出现误判、漏判的情况。基于双目立体视觉技术的盲人辅助设备虽然克服了以上缺点，但是也有一定的不足。首先，得到三维深度信息的特征点提取、匹配算法较为复杂，而且视场中得到的有效点较少、精度有限；其次，它对于处理模块的计算性能和存储要求较高，较难集成化。

近来RGB-D相机的流行，为环境三维信息的获取提供了新的思路。它具有主动测距和双目视觉两种探测方法的优势，获取的三维深度信息(D图像)分辨率高、视场大、刷新频率高；结合彩色图像(RGB图像)，可以获得丰富的环境信息，拓展多种功能。RGB-D相机普遍采用TOF或结构光等技术对整个视场进行主动测距，不需要经过双目视觉中的匹配等步骤，即可直接获得深度信息。这一方面简化了后期处理的计算过程，为实现集成化的导盲系统创造了便利；另一方面，获取的深度信息误差较小，能够更好地实现三维环境的识别。

发明内容

本发明的目的在于针对现有技术的不足，提供一种基于RGB-D相机和姿态传感器的平面和障碍检测方法，以辅助盲人出行为目标，该方法通过实时获取RGB-D相机和姿态传感器两种数据源的数据，检测现实三维环境中的各种平面和物体的存在，进而实现通路指示、障碍预警功能。

本发明的目的是通过以下技术方案来实现的：硬件方面，本方法需要RGB-D相机、姿态传感模块与便携式计算机支持。RGB-D相机的作用是拍摄得到可见光波段的彩色图像(RGB图像)和红外测距得到的深度图像(D图像)。姿态传感模块与RGB-D相机固定连接，用来探测RGB-D相机的实时姿态。便携式计算机可以是RGB-D相机支持的笔记本电脑、平板电脑等设备。从RGB-D相机和姿态模块中读取的数据，经过平面和障碍检测处理，输出识别结果。本发明方法具体包括以下步骤：

(1)环境三维信息的获取，具体包括以下子步骤：

(1.1)对RGB-D相机进行初始化，获得彩色相机和深度相机的内部参数，所述内部参数包括水平视场角和竖直视场角，使用轮询方法同时从RGB-D相机获得RGB图像和D图像；D图像像素的灰度即该像素对应的物体在三维环境中距离相机的水平深度值。

(1.2)将RGB图像与D图像进行视场匹配，具体为：使用时域修复方法，填补D图像空洞造成的映射后彩色图像不完整，对RGB图像与D图像的原始映射关系β＝R’_t(α)进行逐帧修复得到β＝R_t(α)，具体规则如下：

当R’_t(α)不可用时，进行R_t(α)＝R_t-1(α)处理，否则进行R_t(α)＝γR’_t(α)+(1-γ)R_t(α)处理；

其中0<γ<1，α为RGB图像的像素集合，β为D图像的像素集合，t为当前帧，t-1为当前帧的前一帧；

得到了完整映射关系后，依据该完整映射关系获得与D图像视场相匹配的RGB图像；

(1.3)对D图像进行滤波预处理，具体为：采用时空域联合四邻域滤波填补D图像空洞、去除孤立点，对D图像的像素β_(i,j)进行修复，规则如下：

当β_(i,j)＝0时，执行以下公式：

β_(i,j)＝Average(β_(i-1,j),β_(i,j-1),β_(i+1,j),β_(i,j+1))  (1)

如果公式(1)中β_(i,j)的结果为零，执行β_(i,j)|_t＝β_(i,j)|_t-1；

当β_(i,j)为孤立点时，执行β_(i,j)＝0；

其中Average()表示对参数中非零值求均值，对像素零值的修复以空域滤波优先，当空域滤波无效时采用时域滤波；

(1.4)通过深度相机的内部参数以及D图像获得以相机为原点的相机坐标系下的三维位置信息Xc、Yc，具体为：为了得出空间三维物体到RGB-D相机的二维图像映射关系，首先关注相机坐标系和图像坐标系的关系。对于D图像中的某个像素(u,v)，其像素值即为Zc，由u、v、Zc计算Xc、Yc值，计算公式如下：

$\frac{\frac{\mathrm{Xc}}{\mathrm{Zc}}}{\tan \left(W_1\right)}=\frac{2u-\mathrm{Width}}{\mathrm{Width}}$

$\frac{\frac{\mathrm{Yc}}{\mathrm{Zc}}}{\tan \left(W_2\right)}=\frac{\mathrm{Height}-2v}{\mathrm{Height}}$

其中，(u,v)表示以像素为单位、以图像左上角为原点的图像二维坐标，u是水平方向，v是竖直方向；(Xc,Yc,Zc)表示以相机为原点的相机三维坐标系，Zc是相机的光轴方向，Xc是相机成像面的水平方向，Yc是相机成像面的竖直方向，采集到的二维图像分辨率为Width×Height，水平半视场角W₁，垂直半视场角W₂；

(1.5)通过深度相机的内部参数以及D图像获得以相机为原点的世界坐标系下的三维位置信息(Xw，Yw，Zw)，读取与RGB-D相机固定连接的姿态传感器的姿态角数值，设读数为绕传感器X轴旋转a’，绕传感器Z轴旋转b’，绕传感器Y轴旋转c’，该X、Y、Z三轴的定义与相机坐标的X、Y、Z三轴定义一致；令绕Y轴旋转角的起始轴为光轴(Z轴正向)而非姿态角传感器定义的北方向，所以不考虑c’的数值，那么(Xc,Yc,Zc)绕X轴旋转a＝-a’，绕Z轴旋转b＝-b’，绕Y轴旋转c＝0将得到(Xw,Yw,Zw)；当传感器的角度解算顺序为Z-Y-X时，按照X-Y-Z的顺序将(Xc,Yc,Zc)与旋转矩阵相乘，即得到(Xw,Yw,Zw)，具体公式如下：

$\left[\begin{array}{c}\mathrm{Xw}\\ \mathrm{Yw}\\ \mathrm{Zw}\end{array}\right]=\left[\begin{array}{ccc}\cos b& -\sin b& 0\\ \sin b& \cos b& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos c& 0& \sin c\\ 0& 1& 0\\ -\sin c& 0& \cos c\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos a& -\sin a\\ 0& \sin a& \cos a\end{array}\right]\left[\begin{array}{c}\mathrm{Xc}\\ \mathrm{Yc}\\ \mathrm{Zc}\end{array}\right]$

对D图像中所有像素点都按本步骤的方法，求出在世界坐标系下的三维坐标值(Xw,Yw,Zw)；

(2)环境信息的检测：选取步骤1.5得到的世界三维坐标中的高度量Yw绘制成Y图像，深度量Zw绘制成Z图像，宽度量Xw绘制成X图像；三个图像的灰度值即表示该像素在三维空间中对应的物体在三个坐标轴(Xw,Yw,Zw)上投影的数值；具体包括以下子步骤：

(2.1)水平地面的检测，具体包括以下子步骤：

(2.1.1)生长水平面的预处理，具体为：对Y图像进行水平和竖直方向的边缘提取，并对边缘处理结果进行二值化处理，阈值选择以灰度直方图为依据，选择峰值对应灰度值小的一端的谷值作为二值化的阈值；

对Y图像进行区域分割的预处理，确定水平线(Yw＝0)在视场的位置，并依据水平线位置将视场划分为上下两部分进行区域分割；基于一般使用者佩戴位置，对水平线下方区域按照行间距h个像素的密度进行间隔行扫描，其中0<h<Height/8；在行扫描中，遇到二值化处理得到的边缘像素则在行的同样位置设置分段断点，一次行扫描结束得到断点集合S_i，记录断点的位置和个数；所有行扫描结束后，选取任意相邻两行i行和i+1行中S_i与S_i+1点数更多的断点集合，在两行之间依据断点位置划分出矩形区域；

(2.1.2)水平面生长，在每个划分的区域选择随机点作为种子点，开始一次生长，具体为：

(2.1.2.1)种子点Gs满足以下两个条件则开始本次生长：①Gi不是步骤2.1.1提取到的二值化边缘点，②Gi在本次和其他种子点的生长中未被划分到任何区域，否则，退出本次生长；

(2.1.2.2)设置两个上下边缘计数器m、n，设置一个平面点数计数器Xn，并清零；令G＝Gs，标记其为已被访问，开始生长；

(2.1.2.3)点G高度为hs，确定阈值δ实现四邻域区域生长，其中0<δ<0.6hs；访问G像素点的四邻域像素Gi，该像素的高度为hi，考察如下四个条件是否满足：①Gi不是步骤2.1.1提取到的二值化边缘点，②Gi在本次和其他种子点的生长中未被划分到任何区域，③Gi在本次生长中未被访问过，④|hi-hs|<δ；当四个条件均满足时，将Gi标记为与G属于同一区域，Xn增一；当四个条件不全满足时，将Gi标记为与Gs不属于同一区域；当满足①②③而不满足④时，计数器m增一，计数器n增一；当满足③而不满足①②④时，计数器m不变，计数器n增一；

(2.1.2.4)处理步骤2.1.2.3得到的与G属于同一区域的像素点所有Gi，令G＝Gi，标记其为已被访问，重复步骤2.1.2.3；

(2.1.2.5)对步骤2.1.1得到的所有种子点重复步骤2.1.2.1-2.1.2.4，Y图像中产生若干平面，以种子点高度值作为平面特征；

(2.1.3)对步骤2.1.2产生的平面进行合并和排除，具体为：提取平面特征，对分割出的相邻平面特征进行比较，如果二者偏差在与平面高度有关的阈值δ₁之内，即合并为一个平面，其中δ<δ₁<3δ；

对合并后的平面的点数Xn进行统计，对Xn小于阈值λ的平面进行排除，

λ＝k*ave{S(i)}δ

其中，ave{S(i)}是与步骤2.1.1中断点集合元素间距平均值，k是比例系数，1<k<5；

考虑平面边缘形成原因，步骤2.1.2的生长过程中，因超出阈值而停止生长而形成的上下边缘点的数目是m，上下边缘所有点数是n，对于E＝m/n，当E大于阈值e(e<0.3)时，认为该平面是非水平面，应予排除；

(2.1.4)水平地面的检出，具体为：在步骤2.1.3合并和排除后的平面中较大的高度绝对值对应的平面拥有较大的优先级，将其中高度绝对值最大的平面标记为水平地面，并按照优先级由大到小的顺序逐个标记其余各个水平面；

(2.2)障碍物的检测，具体包括以下子步骤：

(2.2.1)对彩色图像进行滤波预处理：对RGB图像进行平滑滤波，减少后续边缘提取产生的噪声，在RGB图像和Z图像中去除在步骤2.14中已经检出的平面，避免重复检测；

(2.2.2)边缘提取和处理：对RGB图像和Z图像提取边缘，并进行图像二值化；阈值选择以灰度直方图为依据，选择峰值对应灰度值小的一端的谷值作为二值化的阈值；

对图像进行区域分割的预处理，对整个图像按照行间距h的密度进行间隔行扫描，其中0<h<Height/8；在行扫描中，遇到二值化处理得到的边缘的则设置分段断点，一次行扫描结束得到断点集合S_i；行扫描全部结束后，相邻两行断点集合S_i与S_i+1以点数更多的为准，在行之间划分出矩形区域；

(2.2.3)区域生长：对Z图像和RGB图像同时进行生长，种子点的选择采用与步骤2.1.2的方法相同，生长规则与步骤2.1.3类似；但是，障碍检测与平面检测不同，同一物体在深度值上并不一定存在严格一致性；故生长时，在停止条件上采用宽松的δ’阈值控制，其中δ<δ’<4δ；

(2.2.4)生长结束后，在Z图像中提取区域如下特征：深度平均值g、区域点数Xn、区域中心位置o、区域矩形边框角点位置di_(1-4)；在RGB图像中提取区域如下特征：灰度平均值g’、区域点数Xn’、区域中心位置o’、区域矩形边框角点位置di’_(1-4)；根据点数Xn和Xn’的多少，按照步骤2.1.3的排除法则，进行小区域舍弃；

(2.2.5)对Z图像和RGB图像共同的检出区域像进行匹配：对步骤2.2.4处理后得到的RGB图像中区域的区域中心位置o与Z图像中区域的区域中心位置o作差，差值为Δo；RGB图像中区域的边框角点位置di_(1-4)与Z图像中区域的边框角点位置di’_(1-4)作差，差值为Δdi_(1-4)；当Δo满足-δo<Δo<δo，而且Δdi_(1-4)满足-δdi_(1-4)<Δdi_(1-4)<δdi_(1-4)时，认定是Z图像和RGB图像共同的检出区域，其中0<δo<(0.3Height+0.3Weight)/2，0<δdi_(1-4)<(0.3Height+0.3Weight)/2；

共同的检出区域认定为一个物体，是视场中确实存在的障碍物，而不是背景中的纹理或者Z图像的噪声；

(2.2.6)对障碍物进行框选：对步骤2.2.5找到物体按照RGB图和D图中的共同检出区域，按照二者中区域点数Xn与Xn’小的为准，确定障碍物的深度平均值g、灰度平均值g’、区域中心位置o、区域矩形边框角点位置di_(1-4)和长L和高H；其中，长L＝(L1+L2)/2，L1为边框左上角点像素与右上角点像素在X图像中数值的差值，L2为边框左下角点像素与右下角点像素在X图像中数值的差值；高H＝(H1+H2)/2，H1为边框左上角点像素与左下角点像素在Y图像中数值的差值，H2为边框右上角点像素与右下角点像素在Y图像中数值的差值；

(2.3)通路检测和危险预警：步骤2.1和步骤2.2分别检测了地面和障碍，对步骤2.1.4和步骤2.2.6检测到的每帧图像信息，进行进一步识别解读，形成可供使用者理解的声音或文字信号；由于图像信息可达到每秒多帧，而文字或声音输出不需要这样快的速率，所以在使用时可灵活设置每隔P帧执行步骤2.3.1-2.3.5，在主流配置计算机中10<P<100；具体包括以下子步骤：

(2.3.1)根据步骤1.5确定的世界坐标系的坐标值，将步骤2.2.6框选出的图像分为左上、左下、右上、右下、正上和正下六个区域，具体为：人体的宽度设为w，将视场中世界坐标水平线(Yw＝0)以上的部分定义为上部，下方定义为下部；X图像中间部分，即满足-w-p<Xw<w+q的像素集合，称为中部，其中0<|p|,|q|<3w；中部两侧分别为左部和右部；由于使用者携带RGB-D相机的位置是可能不断变化的，前述六个方位并非大小相同，也并非同时存在；

(2.3.2)对障碍物的处理：将步骤2.2.6框选出的障碍物按照深度平均值g和在图像中的位置的不同进行优先级排序，位于中间区域的优先级高，两侧的优先级低，g较小的优先级高，g较大的优先级低，位置优先级总体高于深度平均值优先级，按照优先级顺序提示使用者对应方位的障碍物情况；对障碍物依照灰度平均值g’分为红橙黄绿青蓝紫黑白等不同颜色类别；

(2.3.3)对地面通路的处理，按照步骤2.1.4检测出的水平地面，对左下、右下、中下三个分区内的平面检出情况进行分析，具体为：与图像视场下沿连接的平面或距离最近的平面标记为使用者现在所在水平面，确认所在平面为通路；某个分区像素点个数为A，分区中水平面通路像素个数为B，当时，认为该方向可以通行，否则不能通行；其中，0＜δv＜1；

(2.3.4)典型场景检测：本方法除对通路和障碍能进行提示之外，还可以对台阶或楼梯场景进行提示。对于步骤2.1.4检测得到的平面图像和步骤2.2.6检测得到的障碍物图像，如果两图像相对应的同一位置存在多个相互交错的检出物时，则认为存在楼梯，具体方法是：步骤2.2.6框选出的所有障碍物检测结果在图像中的间距D如果小于阈值d(0<d<0.5)，则认为他们同属一个集合，将该集合中的元素找全后，框选集合位置并统计元素个数num；在步骤2.1.4水平面检测结果中，在集合框选的同样位置如果出现个数为num-2与num+2之间的水平面，则认为该区域的确为台阶或楼梯；

(2.3.5)检测信息的输出：步骤2.2.2得到的障碍信息、步骤2.2.3得到的通路信息和步骤2.2.4得到的场景信息，这三类信息按照障碍-通路-场景的顺序输出；在障碍和通路信息中，按照2.3.2和2.3.3确定的优先级确定信息提示先后。

本发明与一般的基于超声波和双目视觉的盲人辅助系统相比，优势在于：

1.高效准确。与其他探测手段相比，基于RGB-D相机和姿态探测器实现了分辨率更高、刷新频率更高、精度更高的环境三维信息识别。能够实现地面通路识别，地面坑洼和各种障碍的判断预警，上下楼梯场景的识别，更接近人的视觉对环境的感知。

2.便携易用。有姿态传感器的配合，整个系统更加灵活、携带方便，相机不需要与使用者或地面等任何参照物保持固定的相对位置。使用者可以在自己认为合适的位置放置相机和处理模块，只要将相机和姿态模块与处理模块相连接，即可实现功能。

3.成本较低。硬件系统的各个部分可以实现通用化和自主组合。视障人士出行时，可以将相机放置在身体的任何可以拍摄三维环境的位置，可以选择自己与RGB-D相机兼容的任何便携式计算机、平板实现同样功能。

附图说明

图1是本发明方法流程图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细说明。

如图1所示，本发明一种基于RGB-D相机和姿态传感器的平面和障碍检测方法，包括以下步骤：

(1)环境三维信息的获取，具体包括以下子步骤：

(1.1)对RGB-D相机进行初始化，获得彩色相机和深度相机的内部参数，所述内部参数包括水平视场角和竖直视场角，使用轮询方法同时从RGB-D相机获得RGB图像和D图像；D图像像素的灰度即该像素对应的物体在三维环境中距离相机的水平深度值。

(1.2)将RGB图像与D图像进行视场匹配，具体为：由RGB-D相机内部函数得到的RGB图像的像素集合α和D图像的像素集合β的视场映射关系R_t是变化的，对于不同拍摄场景会有不同的两种图像映射关系。某一帧的映射关系R_t无法直接用于匹配，因为映射R_t不能覆盖D图像的像素集合β，匹配后会在彩色图像中留下较多空洞。本发明使用时域修复方法，填补D图像空洞造成的映射后彩色图像不完整，对RGB图像与D图像的原始映射关系β＝R’_t(α)进行逐帧修复得到β＝R_t(α)，具体规则如下：

当R’_t(α)不可用时，进行R_t(α)＝R_t-1(α)处理，否则进行R_t(α)＝γR’_t(α)+(1-γ)R_t(α)处理；

其中0<γ<1，α为RGB图像的像素集合，β为D图像的像素集合，t为当前帧，t-1为当前帧的前一帧；

得到了完整映射关系后，依据该完整映射关系获得与D图像视场相匹配的RGB图像；

(1.3)对D图像进行滤波预处理，具体为：采用时空域联合四邻域滤波填补D图像空洞、去除孤立点，对D图像的像素β_(i,j)进行修复，规则如下：

当β_(i,j)＝0时，执行以下公式：

β_(i,j)＝Average(β_(i-1,j),β_(i,j-1),β_(i+1,j),β_(i,j+1))  (1)

如果公式(1)中β_(i,j)的结果为零，执行β_(i,j)|_t＝β_(i,j)|_t-1；

当β_(i,j)为孤立点时，执行β_(i,j)＝0；

其中Average()表示对参数中非零值求均值，对像素零值的修复以空域滤波优先，当空域滤波无效时采用时域滤波；

(1.4)通过深度相机的内部参数以及D图像获得以相机为原点的相机坐标系下的三维位置信息Xc、Yc，具体为：为了得出空间三维物体到RGB-D相机的二维图像映射关系，首先关注相机坐标系和图像坐标系的关系。对于D图像中的某个像素(u,v)，其像素值即为Zc，由u、v、Zc计算Xc、Yc值，计算公式如下：

$\frac{\frac{\mathrm{Xc}}{\mathrm{Zc}}}{\tan \left(W_1\right)}=\frac{2u-\mathrm{Width}}{\mathrm{Width}}$

$\frac{\frac{\mathrm{Yc}}{\mathrm{Zc}}}{\tan \left(W_2\right)}=\frac{\mathrm{Height}-2v}{\mathrm{Height}}$

其中，(u,v)表示以像素为单位、以图像左上角为原点的图像二维坐标，u是水平方向，v是竖直方向；(Xc,Yc,Zc)表示以相机为原点的相机三维坐标系，Zc是相机的光轴方向，Xc是相机成像面的水平方向，Yc是相机成像面的竖直方向，采集到的二维图像分辨率为Width×Height，水平半视场角W₁，垂直半视场角W₂；

某些特定种类的RGB-D相机具有直接获得Xc、Yc数值的功能，不需执行本步骤。

(1.5)通过深度相机的内部参数以及D图像获得以相机为原点的世界坐标系下的三维位置信息(Xw，Yw，Zw)，读取与RGB-D相机固定连接的姿态传感器的姿态角数值，设读数为绕传感器X轴旋转a’，绕传感器Z轴旋转b’，绕传感器Y轴旋转c’，该X、Y、Z三轴的定义与相机坐标的X、Y、Z三轴定义一致；令绕Y轴旋转角的起始轴为光轴(Z轴正向)而非姿态角传感器定义的北方向，所以不考虑c’的数值，那么(Xc,Yc,Zc)绕X轴旋转a＝-a’，绕Z轴旋转b＝-b’，绕Y轴旋转c＝0将得到(Xw,Yw,Zw)；当传感器的角度解算顺序为Z-Y-X时，按照X-Y-Z的顺序将(Xc,Yc,Zc)与旋转矩阵相乘，即得到(Xw,Yw,Zw)，具体公式如下：

$\left[\begin{array}{c}\mathrm{Xw}\\ \mathrm{Yw}\\ \mathrm{Zw}\end{array}\right]=\left[\begin{array}{ccc}\cos b& -\sin b& 0\\ \sin b& \cos b& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos c& 0& \sin c\\ 0& 1& 0\\ -\sin c& 0& \cos c\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos a& -\sin a\\ 0& \sin a& \cos a\end{array}\right]\left[\begin{array}{c}\mathrm{Xc}\\ \mathrm{Yc}\\ \mathrm{Zc}\end{array}\right]$

对D图像中所有像素点都按本步骤的方法，求出在世界坐标系下的三维坐标值(Xw,Yw,Zw)；

(2)环境信息的检测

选取步骤1.5得到的世界三维坐标中的高度量Yw绘制成Y图像，深度量Zw绘制成Z图像，宽度量Xw绘制成X图像；三个图像的灰度值即表示该像素在三维空间中对应的物体在三个坐标轴(Xw,Yw,Zw)上投影的数值；

(2.1)水平地面的检测

(2.1.1)生长水平面的预处理，具体为：对Y图像进行水平和竖直方向的边缘提取，并对边缘处理结果进行二值化处理，阈值选择以灰度直方图为依据，选择峰值对应灰度值小的一端的谷值作为二值化的阈值；

对Y图像进行区域分割的预处理，确定水平线(Yw＝0)在视场的位置，并依据水平线位置将视场划分为上下两部分进行区域分割；基于一般使用者佩戴位置，对水平线下方区域按照行间距h个像素的密度进行间隔行扫描，其中0<h<Height/8；在行扫描中，遇到二值化处理得到的边缘像素则在行的同样位置设置分段断点，一次行扫描结束得到断点集合S_i，记录断点的位置和个数；所有行扫描结束后，选取任意相邻两行i行和i+1行中S_i与S_i+1点数更多的断点集合，在两行之间依据断点位置划分出矩形区域；

(2.1.2)水平面生长，在每个划分的区域选择随机点作为种子点，开始一次生长，具体为：

(2.1.2.1)种子点Gs满足以下两个条件则开始本次生长：①Gi不是步骤2.1.1提取到的二值化边缘点，②Gi在本次和其他种子点的生长中未被划分到任何区域，否则，退出本次生长；

(2.1.2.2)设置两个上下边缘计数器m、n，设置一个平面点数计数器Xn，并清零；令G＝Gs，标记其为已被访问，开始生长；

(2.1.2.3)点G高度为hs，确定阈值δ实现四邻域区域生长，其中0<δ<0.6hs；访问G像素点的四邻域像素Gi，该像素的高度为hi，考察如下四个条件是否满足：①Gi不是步骤2.1.1提取到的二值化边缘点，②Gi在本次和其他种子点的生长中未被划分到任何区域，③Gi在本次生长中未被访问过，④|hi-hs|<δ；当四个条件均满足时，将Gi标记为与G属于同一区域，Xn增一；当四个条件不全满足时，将Gi标记为与Gs不属于同一区域；当满足①②③而不满足④时，计数器m增一，计数器n增一；当满足③而不满足①②④时，计数器m不变，计数器n增一；

(2.1.2.4)处理步骤2.1.2.3得到的与G属于同一区域的像素点所有Gi，令G＝Gi，标记其为已被访问，重复步骤2.1.2.3；

(2.1.2.5)对步骤2.1.1得到的所有种子点重复步骤2.1.2.1-2.1.2.4，Y图像中产生若干平面，以种子点高度值作为平面特征；

(2.1.3)对步骤2.1.2产生的平面进行合并和排除，具体为：提取平面特征，对分割出的相邻平面特征进行比较，如果二者偏差在与平面高度有关的阈值δ₁之内，即合并为一个平面，其中δ<δ₁<3δ；

对合并后的平面的点数Xn进行统计，对Xn小于阈值λ的平面进行排除，

λ＝k*ave{S(i)}δ

其中，ave{S(i)}是与步骤2.1.1中断点集合元素间距平均值，k是比例系数，1<k<5；

考虑平面边缘形成原因，步骤2.1.2的生长过程中，因超出阈值而停止生长而形成的上下边缘点的数目是m，上下边缘所有点数是n，对于E＝m/n，当E大于阈值e(e<0.3)时，认为该平面是非水平面，应予排除；

(2.1.4)水平地面的检出，具体为：在步骤2.1.3合并和排除后的平面中较大的高度绝对值对应的平面拥有较大的优先级，将其中高度绝对值最大的平面标记为水平地面，并按照优先级由大到小的顺序逐个标记其余各个水平面；

(2.2)障碍物的检测

(2.2.1)对彩色图像进行滤波预处理：对RGB图像进行平滑滤波，减少后续边缘提取产生的噪声，在RGB图像和Z图像中去除在步骤2.14中已经检出的平面，避免重复检测；

(2.2.2)边缘提取和处理：对RGB图像和Z图像提取边缘，并进行图像二值化；阈值选择以灰度直方图为依据，选择峰值对应灰度值小的一端的谷值作为二值化的阈值；

对图像进行区域分割的预处理，对整个图像按照行间距h的密度进行间隔行扫描，其中0<h<Height/8；在行扫描中，遇到二值化处理得到的边缘的则设置分段断点，一次行扫描结束得到断点集合S_i；行扫描全部结束后，相邻两行断点集合S_i与S_i+1以点数更多的为准，在行之间划分出矩形区域；

(2.2.3)区域生长：对Z图像和RGB图像同时进行生长，种子点的选择采用与步骤2.1.2的方法相同，生长规则与步骤2.1.3类似；但是，障碍检测与平面检测不同，同一物体在深度值上并不一定存在严格一致性；故生长时，在停止条件上采用宽松的δ’阈值控制，其中δ<δ’<4δ；

(2.2.4)生长结束后，在Z图像中提取区域如下特征：深度平均值g、区域点数Xn、区域中心位置o、区域矩形边框角点位置di_(1-4)；在RGB图像中提取区域如下特征：灰度平均值g’、区域点数Xn’、区域中心位置o’、区域矩形边框角点位置di’_(1-4)；根据点数Xn和Xn’的多少，按照步骤2.1.3的排除法则，进行小区域舍弃；

(2.2.5)对Z图像和RGB图像共同的检出区域像进行匹配：对步骤2.2.4处理后得到的RGB图像中区域的区域中心位置o与Z图像中区域的区域中心位置o作差，差值为Δo；RGB图像中区域的边框角点位置di_(1-4)与Z图像中区域的边框角点位置di’_(1-4)作差，差值为Δdi_(1-4)；当Δo满足-δo<Δo<δo，而且Δdi_(1-4)满足-δdi_(1-4)<Δdi_(1-4)<δdi_(1-4)时，认定是Z图像和RGB图像共同的检出区域，其中0<δo<(0.3Height+0.3Weight)/2，0<δdi_(1-4)<(0.3Height+0.3Weight)/2；

共同的检出区域认定为一个物体，是视场中确实存在的障碍物，而不是背景中的纹理或者Z图像的噪声；

(2.2.6)对障碍物进行框选：对步骤2.2.5找到物体按照RGB图和D图中的共同检出区域，按照二者中区域点数Xn与Xn’小的为准，确定障碍物的深度平均值g、灰度平均值g’、区域中心位置o、区域矩形边框角点位置di_(1-4)和长L和高H；其中，长L＝(L1+L2)/2，L1为边框左上角点像素与右上角点像素在X图像中数值的差值，L2为边框左下角点像素与右下角点像素在X图像中数值的差值；高H＝(H1+H2)/2，H1为边框左上角点像素与左下角点像素在Y图像中数值的差值，H2为边框右上角点像素与右下角点像素在Y图像中数值的差值；

(2.3)通路检测和危险预警

步骤2.1和步骤2.2分别检测了地面和障碍，对步骤2.1.4和步骤2.2.6检测到的每帧图像信息，进行进一步识别解读，形成可供使用者理解的声音或文字信号。由于图像信息可达到每秒多帧，而文字或声音输出不需要这样快的速率，所以在使用时可灵活设置每隔P帧执行步骤2.3.1-2.3.5，在主流配置计算机中10<P<100。

(2.3.1)根据步骤1.5确定的世界坐标系的坐标值，将步骤2.2.6框选出的图像分为左上、左下、右上、右下、正上和正下六个区域，具体为：人体的宽度设为w，将视场中世界坐标水平线(Yw＝0)以上的部分定义为上部，下方定义为下部；X图像中间部分，即满足-w-p<Xw<w+q的像素集合，称为中部，其中0<|p|,|q|<3w；中部两侧分别为左部和右部；由于使用者携带RGB-D相机的位置是可能不断变化的，前述六个方位并非大小相同，也并非同时存在；

(2.3.2)对障碍物的处理：将步骤2.2.6框选出的障碍物按照深度平均值g和在图像中的位置的不同进行优先级排序，位于中间区域的优先级高，两侧的优先级低，g较小的优先级高，g较大的优先级低，位置优先级总体高于深度平均值优先级，按照优先级顺序提示使用者对应方位的障碍物情况；对障碍物依照灰度平均值g’分为红橙黄绿青蓝紫黑白等不同颜色类别；

(2.3.3)对地面通路的处理，按照步骤2.1.4检测出的水平地面，对左下、右下、中下三个分区内的平面检出情况进行分析，具体为：与图像视场下沿连接的平面或距离最近的平面标记为使用者现在所在水平面，确认所在平面为通路；某个分区像素点个数为A，分区中水平面通路像素个数为B，当时，认为该方向可以通行，否则不能通行；其中，0＜δv＜1；

(2.3.4)典型场景检测：本方法除对通路和障碍能进行提示之外，还可以对台阶或楼梯场景进行提示。对于步骤2.1.4检测得到的平面图像和步骤2.2.6检测得到的障碍物图像，如果两图像相对应的同一位置存在多个相互交错的检出物时，则认为存在楼梯，具体方法是：步骤2.2.6框选出的所有障碍物检测结果在图像中的间距D如果小于阈值d(0<d<0.5)，则认为他们同属一个集合，将该集合中的元素找全后，框选集合位置并统计元素个数num；在步骤2.1.4水平面检测结果中，在集合框选的同样位置如果出现个数为num-2与num+2之间的水平面，则认为该区域的确为台阶或楼梯；

(2.3.5)检测信息的输出：步骤2.2.2得到的障碍信息、步骤2.2.3得到的通路信息和步骤2.2.4得到的场景信息，这三类信息按照障碍-通路-场景的顺序输出；在障碍和通路信息中，按照2.3.2和2.3.3确定的优先级确定信息提示先后；例如在某一特定的环境下的输出信息为：正下方1.56m处有长1m、高1m的白色障碍物，右方有通路可以通行，左方有台阶。

Claims (1)

1.一种基于RGB-D相机和姿态传感器的平面和障碍检测方法，其特征在于，包括以下步骤：

(1)环境三维信息的获取，具体包括以下子步骤：

(1.1)对RGB-D相机进行初始化，获得彩色相机和深度相机的内部参数，所述内部参数包括水平视场角和竖直视场角，使用轮询方法同时从RGB-D相机获得RGB图像和D图像；D图像像素的灰度即该像素对应的物体在三维环境中距离相机的水平深度值。

(1.2)将RGB图像与D图像进行视场匹配，具体为：使用时域修复方法，填补D图像空洞造成的映射后彩色图像不完整，对RGB图像与D图像的原始映射关系β＝R’_t(α)进行逐帧修复得到β＝R_t(α)，具体规则如下：

当R’_t(α)不可用时，进行R_t(α)＝R_t-1(α)处理，否则进行R_t(α)＝γR’_t(α)+(1-γ)R_t(α)处理；

其中0<γ<1，α为RGB图像的像素集合，β为D图像的像素集合，t为当前帧，t-1为当前帧的前一帧；

得到了完整映射关系后，依据该完整映射关系获得与D图像视场相匹配的RGB图像；

(1.3)对D图像进行滤波预处理，具体为：采用时空域联合四邻域滤波填补D图像空洞、去除孤立点，对D图像的像素β_(i,j)进行修复，规则如下：

当β_(i,j)＝0时，执行以下公式：

β_(i,j)＝Average(β_(i-1,j),β_(i,j-1),β_(i+1,j),β_(i,j+1))       (1)

如果公式(1)中β_(i,j)的结果为零，执行β_(i,j)|_t＝β_(i,j)|_t-1；

当β_(i,j)为孤立点时，执行β_(i,j)＝0；

其中Average()表示对参数中非零值求均值，对像素零值的修复以空域滤波优先，当空域滤波无效时采用时域滤波；

(1.4)通过深度相机的内部参数以及D图像获得以相机为原点的相机坐标系下的三维位置信息Xc、Yc，具体为：为了得出空间三维物体到RGB-D相机的二维图像映射关系，首先关注相机坐标系和图像坐标系的关系。对于D图像中的某个像素(u,v)，其像素值即为Zc，由u、v、Zc计算Xc、Yc值，计算公式如下：

$\frac{\frac{\mathrm{Xc}}{\mathrm{Zc}}}{\tan \left(W_1\right)}=\frac{2u-\mathrm{Width}}{\mathrm{Width}}$

$\frac{\frac{\mathrm{Yc}}{\mathrm{Zc}}}{\tan \left(W_2\right)}=\frac{\mathrm{Height}-2v}{\mathrm{Height}}$

其中，(u,v)表示以像素为单位、以图像左上角为原点的图像二维坐标，u是水平方向，v是竖直方向；(Xc,Yc,Zc)表示以相机为原点的相机三维坐标系，Zc是相机的光轴方向，Xc是相机成像面的水平方向，Yc是相机成像面的竖直方向，采集到的二维图像分辨率为Width×Height，水平半视场角W₁，垂直半视场角W₂；

(1.5)通过深度相机的内部参数以及D图像获得以相机为原点的世界坐标系下的三维位置信息(Xw，Yw，Zw)，读取与RGB-D相机固定连接的姿态传感器的姿态角数值，设读数为绕传感器X轴旋转a’，绕传感器Z轴旋转b’，绕传感器Y轴旋转c’，该X、Y、Z三轴的定义与相机坐标的X、Y、Z三轴定义一致；令绕Y轴旋转角的起始轴为光轴(Z轴正向)而非姿态角传感器定义的北方向，所以不考虑c’的数值，那么(Xc,Yc,Zc)绕X轴旋转a＝-a’，绕Z轴旋转b＝-b’，绕Y轴旋转c＝0将得到(Xw,Yw,Zw)；当传感器的角度解算顺序为Z-Y-X时，按照X-Y-Z的顺序将(Xc,Yc,Zc)与旋转矩阵相乘，即得到(Xw,Yw,Zw)，具体公式如下：

$\left[\begin{array}{c}\mathrm{Xw}\\ \mathrm{Yw}\\ \mathrm{Zw}\end{array}\right]=\left[\begin{array}{ccc}\cos b& -\sin b& 0\\ \sin b& \cos b& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos c& 0& \sin c\\ 0& 1& 0\\ -\sin c& 0& \cos c\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos a& -\sin a\\ 0& \sin a& \cos a\end{array}\right]\left[\begin{array}{c}\mathrm{Xc}\\ \mathrm{Yc}\\ \mathrm{Zc}\end{array}\right]$

对D图像中所有像素点都按本步骤的方法，求出在世界坐标系下的三维坐标值(Xw,Yw,Zw)；

(2)环境信息的检测：选取步骤1.5得到的世界三维坐标中的高度量Yw绘制成Y图像，深度量Zw绘制成Z图像，宽度量Xw绘制成X图像；三个图像的灰度值即表示该像素在三维空间中对应的物体在三个坐标轴(Xw,Yw,Zw)上投影的数值；具体包括以下子步骤：

(2.1)水平地面的检测，具体包括以下子步骤：

(2.1.1)生长水平面的预处理，具体为：对Y图像进行水平和竖直方向的边缘提取，并对边缘处理结果进行二值化处理，阈值选择以灰度直方图为依据，选择峰值对应灰度值小的一端的谷值作为二值化的阈值；

对Y图像进行区域分割的预处理，确定水平线(Yw＝0)在视场的位置，并依据水平线位置将视场划分为上下两部分进行区域分割；基于一般使用者佩戴位置，对水平线下方区域按照行间距h个像素的密度进行间隔行扫描，其中0<h<Height/8；在行扫描中，遇到二值化处理得到的边缘像素则在行的同样位置设置分段断点，一次行扫描结束得到断点集合S_i，记录断点的位置和个数；所有行扫描结束后，选取任意相邻两行i行和i+1行中S_i与S_i+1点数更多的断点集合，在两行之间依据断点位置划分出矩形区域；

(2.1.2)水平面生长，在每个划分的区域选择随机点作为种子点，开始一次生长，具体为：

(2.1.2.1)种子点Gs满足以下两个条件则开始本次生长：①Gi不是步骤2.1.1提取到的二值化边缘点，②Gi在本次和其他种子点的生长中未被划分到任何区域，否则，退出本次生长；

(2.1.2.2)设置两个上下边缘计数器m、n，设置一个平面点数计数器Xn，并清零；令G＝Gs，标记其为已被访问，开始生长；

(2.1.2.3)点G高度为hs，确定阈值δ实现四邻域区域生长，其中0<δ<0.6hs；访问G像素点的四邻域像素Gi，该像素的高度为hi，考察如下四个条件是否满足：①Gi不是步骤2.1.1提取到的二值化边缘点，②Gi在本次和其他种子点的生长中未被划分到任何区域，③Gi在本次生长中未被访问过，④|hi-hs|<δ；当四个条件均满足时，将Gi标记为与G属于同一区域，Xn增一；当四个条件不全满足时，将Gi标记为与Gs不属于同一区域；当满足①②③而不满足④时，计数器m增一，计数器n增一；当满足③而不满足①②④时，计数器m不变，计数器n增一；

(2.1.2.4)处理步骤2.1.2.3得到的与G属于同一区域的像素点所有Gi，令G＝Gi，标记其为已被访问，重复步骤2.1.2.3；

(2.1.2.5)对步骤2.1.1得到的所有种子点重复步骤2.1.2.1-2.1.2.4，Y图像中产生若干平面，以种子点高度值作为平面特征；

(2.1.3)对步骤2.1.2产生的平面进行合并和排除，具体为：提取平面特征，对分割出的相邻平面特征进行比较，如果二者偏差在与平面高度有关的阈值δ₁之内，即合并为一个平面，其中δ<δ₁<3δ；

对合并后的平面的点数Xn进行统计，对Xn小于阈值λ的平面进行排除，

λ＝k*ave{S(i)}δ

其中，ave{S(i)}是与步骤2.1.1中断点集合元素间距平均值，k是比例系数，1<k<5；

考虑平面边缘形成原因，步骤2.1.2的生长过程中，因超出阈值而停止生长而形成的上下边缘点的数目是m，上下边缘所有点数是n，对于E＝m/n，当E大于阈值e(e<0.3)时，认为该平面是非水平面，应予排除；

(2.1.4)水平地面的检出，具体为：在步骤2.1.3合并和排除后的平面中较大的高度绝对值对应的平面拥有较大的优先级，将其中高度绝对值最大的平面标记为水平地面，并按照优先级由大到小的顺序逐个标记其余各个水平面；

(2.2)障碍物的检测，具体包括以下子步骤：

(2.2.1)对彩色图像进行滤波预处理：对RGB图像进行平滑滤波，减少后续边缘提取产生的噪声，在RGB图像和Z图像中去除在步骤2.14中已经检出的平面，避免重复检测；

(2.2.2)边缘提取和处理：对RGB图像和Z图像提取边缘，并进行图像二值化；阈值选择以灰度直方图为依据，选择峰值对应灰度值小的一端的谷值作为二值化的阈值；

对图像进行区域分割的预处理，对整个图像按照行间距h的密度进行间隔行扫描，其中0<h<Height/8；在行扫描中，遇到二值化处理得到的边缘的则设置分段断点，一次行扫描结束得到断点集合S_i；行扫描全部结束后，相邻两行断点集合S_i与S_i+1以点数更多的为准，在行之间划分出矩形区域；

(2.2.3)区域生长：对Z图像和RGB图像同时进行生长，种子点的选择采用与步骤2.1.2的方法相同，生长规则与步骤2.1.3类似；但是，障碍检测与平面检测不同，同一物体在深度值上并不一定存在严格一致性；故生长时，在停止条件上采用宽松的δ’阈值控制，其中δ<δ’<4δ；

(2.2.4)生长结束后，在Z图像中提取区域如下特征：深度平均值g、区域点数Xn、区域中心位置o、区域矩形边框角点位置di_(1-4)；在RGB图像中提取区域如下特征：灰度平均值g’、区域点数Xn’、区域中心位置o’、区域矩形边框角点位置di’_(1-4)；根据点数Xn和Xn’的多少，按照步骤2.1.3的排除法则，进行小区域舍弃；

(2.2.5)对Z图像和RGB图像共同的检出区域像进行匹配：对步骤2.2.4处理后得到的RGB图像中区域的区域中心位置o与Z图像中区域的区域中心位置o作差，差值为Δo；RGB图像中区域的边框角点位置di_(1-4)与Z图像中区域的边框角点位置di’_(1-4)作差，差值为Δdi_(1-4)；当Δo满足-δo<Δo<δo，而且Δdi_(1-4)满足-δdi_(1-4)<Δdi_(1-4)<δdi_(1-4)时，认定是Z图像和RGB图像共同的检出区域，其中0<δo<(0.3Height+0.3Weight)/2，0<δdi_(1-4)<(0.3Height+0.3Weight)/2；

共同的检出区域认定为一个物体，是视场中确实存在的障碍物，而不是背景中的纹理或者Z图像的噪声；

(2.2.6)对障碍物进行框选：对步骤2.2.5找到物体按照RGB图和D图中的共同检出区域，按照二者中区域点数Xn与Xn’小的为准，确定障碍物的深度平均值g、灰度平均值g’、区域中心位置o、区域矩形边框角点位置di_(1-4)和长L和高H；其中，长L＝(L1+L2)/2，L1为边框左上角点像素与右上角点像素在X图像中数值的差值，L2为边框左下角点像素与右下角点像素在X图像中数值的差值；高H＝(H1+H2)/2，H1为边框左上角点像素与左下角点像素在Y图像中数值的差值，H2为边框右上角点像素与右下角点像素在Y图像中数值的差值；

(2.3)通路检测和危险预警：步骤2.1和步骤2.2分别检测了地面和障碍，对步骤2.1.4和步骤2.2.6检测到的每帧图像信息，进行进一步识别解读，形成可供使用者理解的声音或文字信号；由于图像信息可达到每秒多帧，而文字或声音输出不需要这样快的速率，所以在使用时可灵活设置每隔P帧执行步骤2.3.1-2.3.5，在主流配置计算机中10<P<100；具体包括以下子步骤：

(2.3.1)根据步骤1.5确定的世界坐标系的坐标值，将步骤2.2.6框选出的图像分为左上、左下、右上、右下、正上和正下六个区域，具体为：人体的宽度设为w，将视场中世界坐标水平线(Yw＝0)以上的部分定义为上部，下方定义为下部；X图像中间部分，即满足-w-p<Xw<w+q的像素集合，称为中部，其中0<|p|,|q|<3w；中部两侧分别为左部和右部；由于使用者携带RGB-D相机的位置是可能不断变化的，前述六个方位并非大小相同，也并非同时存在；

(2.3.2)对障碍物的处理：将步骤2.2.6框选出的障碍物按照深度平均值g和在图像中的位置的不同进行优先级排序，位于中间区域的优先级高，两侧的优先级低，g较小的优先级高，g较大的优先级低，位置优先级总体高于深度平均值优先级，按照优先级顺序提示使用者对应方位的障碍物情况；对障碍物依照灰度平均值g’分为红橙黄绿青蓝紫黑白等不同颜色类别；

(2.3.3)对地面通路的处理，按照步骤2.1.4检测出的水平地面，对左下、右下、中下三个分区内的平面检出情况进行分析，具体为：与图像视场下沿连接的平面或距离最近的平面标记为使用者现在所在水平面，确认所在平面为通路；某个分区像素点个数为A，分区中水平面通路像素个数为B，当时，认为该方向可以通行，否则不能通行；其中，0＜δv＜1；

(2.3.4)典型场景检测：本方法除对通路和障碍能进行提示之外，还可以对台阶或楼梯场景进行提示。对于步骤2.1.4检测得到的平面图像和步骤2.2.6检测得到的障碍物图像，如果两图像相对应的同一位置存在多个相互交错的检出物时，则认为存在楼梯，具体方法是：步骤2.2.6框选出的所有障碍物检测结果在图像中的间距D如果小于阈值d(0<d<0.5)，则认为他们同属一个集合，将该集合中的元素找全后，框选集合位置并统计元素个数num；在步骤2.1.4水平面检测结果中，在集合框选的同样位置如果出现个数为num-2与num+2之间的水平面，则认为该区域的确为台阶或楼梯；

(2.3.5)检测信息的输出：步骤2.2.2得到的障碍信息、步骤2.2.3得到的通路信息和步骤2.2.4得到的场景信息，这三类信息按照障碍-通路-场景的顺序输出；在障碍和通路信息中，按照2.3.2和2.3.3确定的优先级确定信息提示先后。