CN104315998A - 一种基于深度图像和方位角的门开度判断方法 - Google Patents

Abstract

本发明公开了一种基于深度图像和方位角的门开度判断方法。本方法采用非接触式测量，通过对门把手进行陆标标定的前提下，在Kinect获取的RGB图像中对门把手进行定位，同时在深度图像中获取该定位位置右上方的深度图像数据，最后根据该深度数据采用特征值法进行平面拟合，获取到平面方程，由平面方程的法向量与Kinect坐标系的平面的法向量获取到夹角信息，最终得到门的开度信息。采用了非接触形式的测量操作，操作方式简单，计算结果精确，在机器人开门技术中具有较高的研究意义。

Description

一种基于深度图像和方位角的门开度判断方法

技术领域

本发明设计的是一种门开度判断方法，具体涉及的是一种基于深度图像和方位角的门开度判断方法。

背景技术

机器人越来越广泛的应用在人们的生活中，使其成为人类工作、生活的帮手，帮助人们完成各种各样的任务是研究机器人技术的最终目的。机器人的研究热点和难点很多，这些热点和难点通常都与实际中特定的应用要求相结合。对移动机械臂平台开门技术进行研究，是考虑到随着机器人的功能越来越强大，其活动范围也相应的越来越广阔，而机器人也越来越深入人类的工作生活环境中，开门技术的研究将能够进一步拓展机器人的工作空间，从而使得机器人服务于人类的能力得到进一步提高。因此，对当前门的开度进行判断，也就成了开门技术中一个重要的组成部分。

目前机器人开门技术在国内还没有进行较为深入的研究，因此有关门开度判断的方法也较为欠缺。

发明内容

本发明所要解决的技术问题是提供一种基于深度图像和方位角的门开度判断方法，以在非接触式的方式下对门的开度进行判断，并达到准确检测的目的,在机器人开门技术中具有较高的研究意义。

本发明解决其技术问题采用以下的技术方案：

一种基于深度图像和方位角的门开度判断方法，应用于机器人在非接触式的方式下对门的开度进行判断，其特征在于包括如下步骤：

首先对门把手进行陆标标定，即在门把手上做上颜色标记，该颜色标记将有助于在Kinect获取的图像中对门把手进行定位；

将数字罗盘与Kinect安装在云台之上，数字罗盘的安装面应与Kinect的底座处于同一水平面，数字罗盘的轴线应与Kinect坐标系yoz平面相平行；这样是为了保证数字罗盘的方位角度测量即为Kinect的朝向角度；

结合数字罗盘的方位角度测量控制云台进行转动，使Kinect位于正对向门的前提下，即Kinect坐标系的x轴与门闭合时候的状态相平行，该方位角α需要提前根据现场的实际情况进行设定；因Kinect可以俯仰以便于观测，因此Kinect坐标系的y轴，z轴不作要求，其y轴，z轴方向对最终结果不会产生影响；

Kinect接收图像信号，同时获取RGB图像和深度图像，在RGB图像中对其进行RGB颜色空间到HSI颜色空间的转换；从而获取到HSI图像；在HSI图像中进行H,S,I三分量的阈值分割操作，通过实验法得到门把手上陆标的H,S,I三分量的阈值范围，由该阈值范围实时对该陆标进行定位。

在Kinect获取的RGB图像中对门把手上的陆标进行实时定位，同时获取到了其在深度图像中的位置信息，然后选取该位置右上方处(陆标在图像坐标系中的坐标偏移，偏移量为(Δu,Δv))的深度图像数据点集；

由得到的深度数据点集采用特征值法进行平面拟合，得到了当前状态下，门平面的方程表示；

由该平面方程的法向量，Kinect坐标系yoz平面的法向量进行夹角计算，获得夹角，再求取该夹角的余角，即为当前状态下门的开度。

上述技术方案中，由数字罗盘的方位角检测功能引导安装在云台上的Kinect进行旋转，使得Kinect坐标系的x轴与门闭合状态时的平面相平行，所述Kinect坐标系以Kinect为坐标原点，水平方向为x轴，垂直方向为y轴，与xoy平面垂直的方向为z轴。

上述技术方案中，所述实验法为：通过对不同的阈值进行试验，看是否满足目标要求，具体步骤分为三步：

Step1：在H分量上分割，获取门把手显示最佳效果，H分量阈值确定；

Step2：在S分量上分割，获取门把手显示最佳效果，S分量阈值确定；

Step3：在I分量上分割，获取门把手显示最佳效果，I分量阈值确定；

经过这三个步骤将H、S、I三色分量的阈值范围确定下来，以便在后续的定位实验中使用；通过依次改变H、S、I的最大值和最小值，将门把手就从HSI颜色空间中分割出来，并最终确定当前的最佳H、S、I三色分量的阈值范围。

上述技术方案中，对深度图中的陆标进行定位，定位方法是基于RGB图像中定位位置，以深度图中相同位置标记为定位位置。

本发明的方法采用了非接触形式的测量操作，操作方式简单，计算结果精确，具有一定的实用价值,在机器人开门技术中具有较高的研究意义。

附图说明

图1所示为根据本发明的实施例的门开度判断方法的流程图。

图2所示为Kinect坐标系示意图。

图3所示为由Kinect获取的RGB图像信息(a)和深度图像信息(b)。

图4所示为对Kinect获取到的RGB图像(a)进行中值滤波处理的图像，中值滤波处理后为(b)。

图5所示为对Kinect获取到的RGB图像进行色彩平衡处理前(a)后(b)的图像。

图6的(a)、(b)、(c)依次为将RGB图像从RGB颜色空间转换到HSI颜色空间后的H、S、I三分量图像。

图7由(a)到(o)依次为对HSI图像采用实验法进行阈值分割的过程图。

图8所示为基于HSI三分量阈值范围后，分别在RGB图像中对门把手进行定位的结果图，(a)为定位前；(b)为定位后。

图9所示为在RGB图像和深度图像中同时对门把手进行定位的结果图；(a)为RGB图像定位，(b)为深度图像定位。

图10所示为对深度图像进行中值滤波的效果图；(a)为原始深度图像；(b)为中值滤波后。

图11所示为基于门把手定位后选取其右上方深度数据的深度数据点云图(a)和拟合平面图(b)。

图12所示由(a)至(c)依次为当门的实际开度为30°、45°、60°时候的拟合平面图。

具体实施方式

以下将对本发明的实施例给出详细的说明。尽管本发明将结合一些具体实施方式进行阐述和说明，但需要注意的是本发明并不仅仅只局限于这些实施方式。相反，对本发明进行的修改或者等同替换，均应涵盖在本发明的权利要求范围当中。

另外，为了更好的说明本发明，在下文的具体实施方式中给出了众多的具体细节。本领域技术人员将理解，没有这些具体细节，本发明同样可以实施。在另外一些实例中，对于大家熟知的方法、流程、元件和电路未作详细描述，以便于凸显本发明的主旨。

图1为根据本发明的实施例的门开度判断方法的流程图。图1描述了如何对门开度进行判断的方法的流程。

在步骤102中，由数字罗盘的方位角检测功能引导安装在云台上的Kinect进行旋转，使得Kinect坐标系的x轴与门闭合状态时的平面相平行。这样是为了建立门开度判断参考平面，以便于最后对门开度进行判断。

图2为Kinect坐标系示意图，该坐标系以Kinect为坐标原点，水平方向为x轴，垂直方向为y轴，与xoy平面垂直的方向为z轴。

回到图1，流程图步骤进入104。由Kinect采集图像信息，该图像信息包括RGB图像和深度图像。图3示出了由Kinect获取的RGB图像和深度图像。

回到图1，流程图步骤进入106。在步骤106至108中，对原始RGB图像进行图像预处理操作。

在步骤106中，首先对RGB图像进行中值滤波，采用中值滤波的原因是兼顾实时性和滤波效果。中值滤波本质上是一种统计排序滤波器，属于一种常见的非线性滤波方法，其基本原理是，首先选取一个包含有奇数点的滑动窗口，该窗口以像素点p(x,y)为中心点，再将该窗口内部所有像素点的值进行从小到大的排序，最后以排序的中间值作为新的目标像素点p′(x,y)，中值滤波算法的数学描述如下：

设S为滑动窗口内所包含的像素点集合，为S经过排序后的新的集合，则：

$\stackrel{\&OverBar;}{S}=\left\{p_n(x,y)\right|p_n(x,y),p_{n+1}(x,y)\&Element;S,p_n(x,y)<p_{n+1}(x,y),n\&Element;N\}---\left(1\right)$

$p^{\&prime;}(x,y)=p_{\frac{N+1}{2}}(x,y)---\left(2\right)$

中值滤波的优点在于能够在抑制随机噪声的同时不使边缘模糊。图4示出了对原始RGB图像进行中值滤波后的效果图。由图4中可以看出采用中值滤波方法后，在平滑图像的同时，也较好的保留了门把手的边缘信息。

回到图1，流程图步骤进入108。对中值滤波后的图像进行色彩平衡处理。彩色图像数字化后，经常在显示的时候看起来有些不正常。这是由于色通道的不同敏感度、增光因子和偏移量等原因所导致的，也称为三基色不平衡，将其校正的过程就是彩色平衡。彩色平衡的校正算法如下：

Step1：从图像中选取两点颜色为灰色，设为：

F₁＝(R₁,G₁,B₁)  (3)

F₂＝(R₂,G₂,B₂)  (4)

Step2：设以G分量为基准，匹配R和B分量，则：

$\begin{array}{c}{F}_1=(R_1,G_1,B_1)\\ R_2=(R_2,G_2,B_2)\end{array}\&DoubleRightArrow;\begin{array}{c}{F}_1^{*}=(R_1,G_1,B_1)\\ F_2^{*}=(R_2,G_2,B_2)\end{array}---\left(5\right)$

Step3：由

$R_1^{*}=k1*R_1+k2---\left(6\right)$

$R_2^{*}=k1*R_2+k2---\left(7\right)$

可推导出：

$\left\{\begin{array}{c}k1=\frac{R_1^{*}-R_2^{*}}{R_1-R_2}\\ k2=R_1-\frac{R_1{R}_1^{*}-R_1{R}_2^{*}}{R_1-R_2}\end{array}\right.---\left(8\right)$

由

$B_1^{*}=l1+B_1+l2---\left(9\right)$

$B_2^{*}=l1+B_2+l2---\left(10\right)$

可推导出：

$\left\{\begin{array}{c}l1=\frac{B_1^{*}-B_2^{*}}{B_1-B_2}\\ l2=B_1-\frac{B_1{B}_1^{*}-B_1{B}_2^{*}}{B_1-B_2}\end{array}\right.---\left(11\right)$

Step4：由

$\left\{\begin{array}{c}R{(x,y)}^{*}=k1*R(x,y)+k2\\ B{(x,y)}^{*}=l2*B(x,y)+l2\\ G{(x,y)}^{*}=G(x,y)\end{array}\right.---\left(12\right)$

对原图像进行处理后的图像即为彩色平衡后的图像。图5示出了色彩平衡处理前的图像和色彩平衡处理后的图像。由图5可以看出，经过色彩平衡处理后，显示效果更加明亮且接近真实环境。

回到图1，流程图步骤进入110。在步骤110中，将RGB图像由RGB颜色空间转换到HSI颜色空间。HSI颜色空间是由色调(H)、饱和度(S)和强度(I)三个分量来对图像进行描述。其中色调(H)表示人眼所能获取的不同颜色，如红、黄、蓝等；饱和度(S)表示颜色的深浅程度，该值越大，则人眼所感受到的颜色越深；强度(I)表示颜色的明暗程度，该值越大，则人眼所感受到的颜色越明亮。采用HSI颜色空间进行描述，是由于HSI颜色空间是基于人的视觉系统原理来描述多种颜色的，由于人的视觉系统对于亮度的敏感程度远大于对颜色浓淡的敏感程度，为便于进行颜色识别和处理，人的视觉系统经常采用HSI颜色空间，对比与RGB颜色空间，HSI颜色空间更加符合人的视觉特性。计算机图像处理中的大量算法都可以应用在HSI颜色空间中，因为其分量是可以单独处理并且相互独立。因此，在HSI颜色空间中进行图像处理会减少计算量。采用几何推导法作为RGB颜色空间转换到HSI颜色空间的转换方法，转换公式见式(13)：

$\left\{\begin{array}{c}H=\left\{\begin{array}{cc}\mathrm{\&theta;},& G\&GreaterEqual;B\\ 2\mathrm{\&pi;}-\mathrm{\&theta;},& G<B\end{array}\right.,\mathrm{\&theta;}=\arctan \left[\frac{(R-G)+(R-B)}{2\sqrt{{(R-G)}^2+(R-B)(G-B)}}\right]\\ S=1-\frac{3\mathrm{Min}}{R+G+B}\\ I=\frac{R+G+B}{3}\end{array}\right.---\left(13\right)$

其中，Min＝min(R,G,B)。式中的R、G、B分量均在计算前进行归一化处理，归一化区间为[0,1]。图6示出了经过颜色空间转换后的HSI三色分量图。由图中可以看出，转换到

HSI颜色空间后，门把手的图像可以在分量图中较为突出的显示，尤其是在S分量图中，因此，在HSI颜色空间中对H、S、I三色分量进行阈值分割可以将门把手分离出来。

回到图1，流程图步骤进入112。采用实验法确定门把手上陆标的H、S、I三色分量的阈值范围从而对陆标进行图像分割。步骤112采用的方法为实验法，即采用人眼观察，通过对不同的阈值进行试验，看是否满足目标要求，具体步骤分为三步：

Step1：在H分量上分割，获取门把手显示最佳效果，H分量阈值确定；

Step2：在S分量上分割，获取门把手显示最佳效果，S分量阈值确定；

Step3：在I分量上分割，获取门把手显示最佳效果，I分量阈值确定。

经过这三个步骤后，H、S、I三色分量的阈值范围就可以确定下来，以便在后续的定位实验中使用。图7示出了分割过程效果图。由图7可以看出，H、S、I的变会引起图像的显著改变，因此选取H、S、I阈值范围的两个原则是最大化保存门把手图像数据，最大化去除其余背景图像数据。基于这两个原则，通过依次改变H、S、I的最大值和最小值，门把手就可以从HSI颜色空间中分割出来(见图3.11(o))，并最终确定当前的最佳H、S、I三色分量的阈值范围。

回到图1，流程图步骤进入114。在步骤114中，基于步骤112所获取的H、S、I三色分量的阈值范围，可以在实时图像获取中对门把手上的陆标进行定位。图8示出了对门把手上的陆标进行实时定位的效果图。图8中以矩形方框对其进行定位，定位效果准确、快速且不丢失。

回到图1，流程图步骤进入116。在步骤116中，对深度图像进行配准操作，由于深度摄像头与RGB摄像头所处位置不同，导致Kinect的深度图像与RGB图像不相匹配，故需对两者进行配准。考虑到RGB图像是三维空间坐标系中的点在二维平面的投影，因此，只需旋转和平移的仿射变换。深度摄像头与RGB摄像头的坐标变换公式如下：

$\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=R*\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]+T---\left(14\right)$

公式(14)中，(x,y,z)为RGB摄像头坐标系中点的坐标，R为旋转矩阵，(X,Y,Z)为深度摄像头坐标系中点的坐标，T为平移矩阵。RGB图像的点坐标为：

$\left[\begin{array}{c}u\\ v\end{array}\right]=\left[\begin{array}{c}{f}_x*x/z\\ f_y*y/z\end{array}\right]+\left[\begin{array}{c}{c}_x\\ c_y\end{array}\right]---\left(15\right)$

公式(15)中，(u,v)为RGB图像的点坐标，f_x、f_y为RGB图像对应的比例参数，(c_x,c_y)为RGB图像的中心点。

流程图步骤进入118。在步骤118中，对深度图中的陆标进行定位，定位方法是基于RGB图像中定位位置，以深度图中相同位置标记为定位位置。图9示出了同时在RGB图像和深度图像中进行定位的结果图。图9中显示了定位的结果是准确的。

流程图步骤进入120。在步骤120中，采用中值滤波对深度图像进行预处理，从而去除深度图像中的噪声信息。Kinect深度成像原理可知，深度图像的噪声点一般是0值点，即无法探测点。中值滤波方法在步骤106中已经给出，图10示出了对深度图像进行中值滤波的结果图。图10中经过中值滤波，对部分无法探测点进行了滤除。

流程图步骤进入122。在步骤122中，本实例选取了深度图像中门把手陆标定位处右上方，图像坐标系中对角线坐标分别为(350,160)、(440,250)的矩形区域内的深度数据。该深度数据为探测目标在Kinect坐标系中的三维坐标值(x,y,z)。因为本实例中，门把手位于门的左边，因此右上方的深度数据更能够正确表示门平面。

流程图步骤进入124。在步骤124中，采用特征值法对点云数据进行平面拟合。特征值法的算法如下：

设平面方程的表达式为：

ax+by+cz＝d  (16)

公式(16)中，a,b,c为该平面的单位法向量，满足：

a²+b²+c²＝1  (17)

d代表平面所处坐标系到平面的距离，且d≥0。因此，要确定该平面的数学表达式，关键是要得到a,b,c,d这4个参数。

获取到的平面点云数据集合：

P＝{(x,y,z)|(x,y,z)＝(x_i,y_i,z_i),i＝1,2,…,n}  (18)

根据公式(16)表示的平面方程，集合P中任意一个坐标数据点(x_i,y_i,z_i)到该平面的距离d_i为：

d_i＝|ax_i+by_i+cz_i-d|  (19)

为得到最佳的拟合平面，则应满足以下条件：

$\left\{\begin{array}{c}{a}^2+b^2+c^2=1\\ e=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{d}_i^2=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{({\mathrm{ax}}_i+{\mathrm{by}}_i+{\mathrm{cz}}_i-d)}^2\&RightArrow;\min \end{array}\right.---\left(20\right)$

由拉格朗日乘数法求取极值，得到函数：

$f=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{d}_i^2-\mathrm{\&lambda;}(a^2+b^2+c^2-1)---\left(21\right)$

对公式(21)中的d求导，并使得导数为0，则：

$\frac{\&PartialD;f}{\&PartialD;d}=-2\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}({\mathrm{ax}}_i+{\mathrm{by}}_i+{\mathrm{cz}}_i-d)=0---\left(22\right)$

由公式(22)可推导出：

$d=a\frac{\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{x}_i}{n}+b\frac{\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{y}_i}{n}+c\frac{\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{z}_i}{n}---(4-23)$

则公式(23)又可以表示为：

$d_i=|a(x_i-\stackrel{\&OverBar;}{x})+b(y_i-\stackrel{\&OverBar;}{y})+c(z_i-\stackrel{\&OverBar;}{z})|---(4-24)$

公式(24)中：

$\stackrel{\&OverBar;}{x}=\frac{\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{x}_i}{n},\stackrel{\&OverBar;}{y}=\frac{\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{y}_i}{n},\stackrel{\&OverBar;}{z}=\frac{\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{z}_i}{n}$

在公式(4-21)中对a求偏导并令导数为0，则：

$2\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}(\mathrm{a\&Delta;}{x}_i+\mathrm{b\&Delta;}{y}_i+\mathrm{c\&Delta;}{z}_i)\mathrm{\&Delta;}{x}_i-2\mathrm{\&lambda;a}=0---\left(25\right)$

公式(4-25)中：

$\mathrm{\&Delta;}{x}_i=x_i-\stackrel{\&OverBar;}{x},\mathrm{\&Delta;}{y}_i=y_i-\stackrel{\&OverBar;}{y},\mathrm{\&Delta;}{z}_i=z_i-\stackrel{\&OverBar;}{z}---\left(26\right)$

同样在公式(21)中对b,c求偏导并令导数为0，则：

$2\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}(\mathrm{a\&Delta;}{x}_i+\mathrm{b\&Delta;}{y}_i+\mathrm{c\&Delta;}{z}_i)\mathrm{\&Delta;}{y}_i-2\mathrm{\&lambda;b}=0---\left(27\right)$

$2\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}(\mathrm{a\&Delta;}{x}_i+\mathrm{b\&Delta;}{y}_i+\mathrm{c\&Delta;}{z}_i)\mathrm{\&Delta;}{z}_i-2\mathrm{\&lambda;c}=0---\left(28\right)$

将公式(26)，公式(27)，公式(28)以矩阵形式构建特征值方程，则：

$\left[\begin{array}{ccc}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{x}_i\mathrm{\&Delta;}{x}_i& \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{x}_i\mathrm{\&Delta;}{y}_i& \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{x}_i\mathrm{\&Delta;}{z}_i\\ \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{x}_i\mathrm{\&Delta;}{y}_i& \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{y}_i\mathrm{\&Delta;}{y}_i& \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{y}_i\mathrm{\&Delta;}{z}_i\\ \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{x}_i\mathrm{\&Delta;}{z}_i& \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{y}_i\mathrm{\&Delta;}{z}_i& \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{z}_i\mathrm{\&Delta;}{z}_i\end{array}\right]\left[\begin{array}{c}a\\ b\\ c\end{array}\right]=\mathrm{\&lambda;}\left[\begin{array}{c}a\\ b\\ c\end{array}\right]---\left(29\right)$

由公式(29)可知，求解a,b,c转化为求解矩阵特征值和特征向量的问题。

由公式(29)，设：

$A=\left[\begin{array}{ccc}\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{x}_i\mathrm{\&Delta;}{x}_i& \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{x}_i\mathrm{\&Delta;}{y}_i& \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{x}_i\mathrm{\&Delta;}{z}_i\\ \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{x}_i\mathrm{\&Delta;}{y}_i& \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{y}_i\mathrm{\&Delta;}{y}_i& \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{y}_i\mathrm{\&Delta;}{z}_i\\ \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{x}_i\mathrm{\&Delta;}{z}_i& \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{y}_i\mathrm{\&Delta;}{z}_i& \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{z}_i\mathrm{\&Delta;}{z}_i\end{array}\right],x=-{(a,b,c)}^T$

矩阵A为一个3×3的实对称矩阵，由矩阵相关理论可知，对于实对称矩阵，其特征值的求取可由公式(4-30)得到：

$\mathrm{\&lambda;}=\frac{(\mathrm{Ax},x)}{(x,x)},x\&NotEqual;0---\left(30\right)$

将公式(30)展开，注意满足条件a²+b²+c²＝1，即(x,x)＝1，则：

$\mathrm{\&lambda;}=\frac{(\mathrm{Ax},x)}{(x,x)}=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{(\mathrm{a\&Delta;}{x}_i+\mathrm{b\&Delta;}{y}_i+\mathrm{c\&Delta;}{z}_i)}^2=\underset{i}{\overset{n}{\mathrm{\&Sigma;}}}{d}_i^2---\left(31\right)$

由公式(31)可以看出，的最小值就是实对称矩阵A的最小特征值，因此最小特征值所对应的特征向量值即为a,b,c。

实对称矩阵A的特征值可以由公式(32)得到：

|A-λI|＝0  (32)

由于实对称矩阵A为3阶，因此其最多有3个实数特征值λ₁,λ₂,λ₃，设其中最小的实数特征值为λ_min。将λ_min代入相应的齐次线性方程组：

(A-λ_minI)x＝0  (32)

由公式(32)求解出的非零解即为λ_min所对应的特征向量，也就是平面方程公式(16)中的a,b,c。图11示出了门处于闭合状态下的深度数据图和拟合平面图。

回到图1，流程图步骤进入126。在步骤126中，根据拟合平面方程系数可以得到该平面的法向量，由法向量结合平面夹角计算公式就可以得到两平面的夹角。由点云数据拟合得到的平面法线向量为公式(16)中的系数(a,b,c)。再由Kinect坐标系下yoz平面方程表示为：

x＝0  (33)

设公式(33)中所表示平面的法向量为(a,b,c)，则(a,b,c)＝(1,0,0)，根据平面夹角公式可以求出，拟合平面与yoz平面的夹角θ_j可由公式(34)求取：

${\mathrm{\&theta;}}_j=\arccos \frac{A*a+B*b+C*c}{\sqrt{A^2+B^2+C^2}\sqrt{a^2+b^2+c^2}}---\left(34\right)$

流程图进入128。在步骤128中，由公式(34)中求得拟合平面方程与真实坐标系yoz平面的夹角θ_j，门的开度θ_o可由公式(35)求取：

θ_o＝90°-θ_j  (35)

从而最终获取到门的开度θ_o。图12示出了门的实际开度为30°、45°、60°时的拟合平面图，计算结果误差在±3°之间。

本发明的优点在于非接触式的测量，测量结果精度较高，具有一定的实用性，在机器人开门技术中具有较高的研究意义。

上文具体实施方式和附图仅为本发明之常用实施例。显然，在不脱离权利要求书所界定的本发明精神和发明范围的前提下可以有各种增补、修改和替换。本领域技术人员应该理解，本发明在实际应用中可根据具体的环境和工作要求在不背离发明准则的前提下在形式、结构、布局、比例、材料、元素、组件及其它方面有所变化。因此，在此披露之实施例仅用于说明而非限制，本发明之范围由后附权利要求及其合法等同物界定，而不限于此前之描述。

Claims (4)

1.一种基于深度图像和方位角的门开度判断方法，应用于机器人在非接触式的方式下对门的开度进行判断，其特征在于包括如下步骤：

首先对门把手进行陆标标定，即在门把手上做上颜色标记，所述颜色标记将有助于在图像中对门把手进行定位；

将数字罗盘与Kinect安装在云台之上，数字罗盘的安装面应与Kinect的底座处于同一水平面，数字罗盘的轴线应与Kinect坐标系yoz平面相平行；保证数字罗盘的方位角度测量即为Kinect的朝向角度；

结合数字罗盘的方位角度测量控制云台进行转动，使Kinect位于正对向门的前提下，即Kinect坐标系的x轴与门闭合时候的状态相平行，该方位角α需要提前根据现场的实际情况进行设定；因Kinect可以俯仰以便于观测，因此Kinect坐标系的y轴，z轴不作要求，其y轴，z轴方向对最终结果不会产生影响；

Kinect接收图像信号，同时获取RGB图像和深度图像，在RGB图像中对其进行RGB颜色空间到HSI颜色空间的转换；从而获取到HSI图像；在HSI图像中进行H,S,I三分量的阈值分割操作，通过实验法得到门把手上陆标的H,S,I三分量的阈值范围，由该阈值范围实时对该陆标进行定位；

在Kinect获取的RGB图像中对门把手上的陆标进行实时定位，同时获取到陆标在深度图像中的位置信息，然后选取该位置右上方处的深度图像数据点集；

由得到的深度数据点集采用特征值法进行平面拟合，得到了当前状态下，门平面的方程表示；

由该平面方程的法向量，Kinect坐标系yoz平面的法向量进行夹角计算，获得夹角，再求取该夹角的余角，即为当前状态下门的开度。

2.根据权利要求1所述的基于深度图像和方位角的门开度判断方法，其特征在于：由数字罗盘的方位角检测功能引导安装在云台上的Kinect进行旋转，使得Kinect坐标系的x轴与门闭合状态时的平面相平行，所述Kinect坐标系以Kinect为坐标原点，水平方向为x轴，垂直方向为y轴，与xoy平面垂直的方向为z轴。

3.根据权利要求2所述的基于深度图像和方位角的门开度判断方法，其特征在于：所述实验法为：通过对不同的阈值进行试验，看是否满足目标要求，具体步骤分为三步：

Step1：在H分量上分割，获取门把手显示最佳效果，H分量阈值确定；

Step2：在S分量上分割，获取门把手显示最佳效果，S分量阈值确定；

Step3：在I分量上分割，获取门把手显示最佳效果，I分量阈值确定；

经过这三个步骤将H、S、I三色分量的阈值范围确定下来，以便在后续的定位实验中使用；通过依次改变H、S、I的最大值和最小值，将门把手就从HSI颜色空间中分割出来，并最终确定当前的最佳H、S、I三色分量的阈值范围。

4.根据权利要求3所述的基于深度图像和方位角的门开度判断方法，其特征在于：对深度图中的陆标进行定位，定位方法是基于RGB图像中定位位置，以深度图中相同位置标记为定位位置。