CN104156946A - 一种空间绳系机器人抓捕区域内的障碍物检测方法 - Google Patents

Abstract

本发明公开了一种空间绳系机器人抓捕区域内的障碍物检测方法，首先在抓捕区域内求取图像梯度的模值和方向；接着根据梯度模值伪排序结果，得到的连通像素区域作为直线段候选区域；然后用外接矩形描述候选区域，满足判定标准的长轴就是所要求的障碍物的直线段轮廓；最后遍历轮廓上的点，得出其三维坐标和最近的距离。本发明的直线段检测部分采用的是基于图像梯度信息的直线段检测方法，能够在多种复杂场景中快速准确地检测出直线段，利用本发明对抓捕点周围的障碍物进行检测，然后对检测到的直线段边缘上点进行三维重建，并计算与抓捕点的三维距离，找出距离最近的点，并得出其三维坐标和最近的距离。

Description

一种空间绳系机器人抓捕区域内的障碍物检测方法

技术领域

本发明属于图像处理与计算机视觉技术领域，具体涉及一种空间绳系机器人抓捕区域内的障碍物检测方法。

背景技术

空间绳系机器人系统由“空间绳系机器人+空间系绳+空间平台”组成，具有安全、灵活、操作距离远等特点。近年来，随着空间技术的不断发展，空间绳系机器人被广泛地应用于包括在轨维修、在轨加注、辅助变轨、在轨空间站辅助组装、轨道垃圾清理、目标捕获等在轨服务中，已经成为空间操控技术领域的研究热点之一。

在空间绳系机器人对目标实施抓捕的过程中，要先对抓捕点附近进行障碍物检测，得出障碍物到抓捕点的最近距离，把该距离信息返回给控制系统，再由控制系统判断该抓捕点是否能够安全实施抓捕，并控制机械爪张开的大小和角度，从而避开障碍物，最终实现安全可靠的抓捕住目标。由于一般抓捕点都选取在卫星翻板支架等具有一般特征的结构上，而附近的可能出现的障碍物一般是卫星的外部轮廓、伸出的天线、发动机等，这些障碍物一般纹理结构都比较简单，尺寸比较大，多数情况下是只有一部分出现在我们需要检测的抓捕区域的范围之内，而且一般都包含丰富的直线段结构。基于这些特点，本发明先对障碍物进行直线段检测，得出障碍物大概的轮廓结构，然后遍历障碍物轮廓上的点，对其进行三维重建，找出距离抓捕点的三维空间距离最近的障碍物上的点，并得出三维的最近距离和最近障碍物点的三维空间坐标。

传统的用于直线段检测的方法是，首先用Canny边缘检测，接着用Hough变换，提取那些包含的边缘点数超过给定阈值的所有直线，然后再使用长度阈值把这些直线切割为线段。尽管本发明具有良好的抗噪性，对于不完整边缘具有鲁棒性，以及对部分遮盖区域不敏感等优点，但是其计算量大、存储量大、多峰值检测以及断点检测效率低等缺陷严重限制了它的应用范围，而且Hough变换更适合于全局的直线段检测而不是局部直线段检测。

发明内容

本发明主要针对配备有双目相机的空间绳系机器人对抓捕点附近进行障碍物检测的应用，提供了一种适用于空间绳系机器人的空间绳系机器人抓捕区域内的障碍物检测方法，该方法用于检测复杂场景中抓捕点附近障碍物直线段的轮廓，并得到距离抓捕点最近的障碍物点的三维坐标和最近的距离，本发明具有良好的鲁棒性，实时性，计算速度快，检测结果准确等优点。

为实现上述目的，本发明所采用的技术方案包括以下步骤：

1)利用空间绳系机器人上的双目相机对目标进行图像采集；

采集到目标的左图像I_L和右图像I_R；

2)在左图像I_L上，以抓捕点O为中心设置一个矩形抓捕区域R，作为处理的感兴趣区域R；

3)在感兴趣区域R内计算该图像的梯度，并计算梯度模值和直线方向角；

4)根据图像的像素梯度模值的大小，利用伪序法对图像的像素进行排序；

5)按照伪序排列结果，选取种子像素，按照区域增长方法计算直线段候选区域Re；

6)划分出直线段候选区域Re后，采用外接矩形法描述和判定直线段候选区域Re，并且计算外接矩形的短轴和长轴的方向以及端点坐标；

7)采用外接矩形长短轴之比与阈值进行比较的方法，确定出所要检测障碍物的直线段边缘；

8)遍历障碍物直线段边缘上的像素点，对其进行三维重建，并计算与抓捕点O之间的三维距离，得出最近的距离和最近的障碍物点的三维坐标。

所述的步骤3)中，计算梯度模值和直线方向角的具体步骤如下：

3-1)：对于感兴趣区域R中的像素(x,y)，其x，y方向上的梯度g_x(x,y)，g_y(x,y)定义为：

$g_x(x,y)=\frac{I(x+1,y)+I(x+1,y+1)-I(x,y)-I(x,y+1)}{2}$

$g_y(x,y)=\frac{I(x,y+1)+I(x+1,y+1)-I(x,y)-I(x+1,y)}{2}---\left(1\right)$

式中，I(x,y)为像素(x,y)的灰度值；

3-2)：计算梯度的模值G(x,y)，公式如下：

$G(x,y)=\sqrt{({g^2}_x(x,y)+{g^2}_y(x,y))}---\left(3\right)$

直线方向角θ定义为：

$\mathrm{\θ}=\arctan \left(\frac{g_x(x,y)}{-g_y(x,y)}\right).$

所述的步骤4)中，利用伪序法对像素进行排序的具体方法如下：

首先按照梯度模值的最大值和最小值线性生成一个区间，然后将每一个像素按照梯度模值大小放入对应区间内。

对于同一区间内的像素，认为其梯度模值相差很小，以同等的概率选为邻域增长的种子点。

所述的步骤5)中，计算直线段候选区域Re的具体方法如下：

5-1)按照伪序排列结果，根据梯度模值从大到小选取区域增长的种子点，并且要求该点没有被访问过，区域增长从该种子像素开始；

5-2)定义区域的角度为：

${\mathrm{\θ}}_{\mathrm{region}}=\arctan \left(\frac{{\mathrm{\Σ}}_i\sin \left({\mathrm{\θ}}_i\right)}{{\mathrm{\Σ}}_i\cos \left({\mathrm{\θ}}_i\right)}\right)---\left(5\right)$

其中，θ_region表示区域的角度，θ_i表示区域中第i个像素的直线方向角；

5-3)设种子点为(x,y)，则开始时区域角度为种子点的直线方向角，即：θ_region＝θ_(x,y)，令S_x＝cos(θ_region)，S_y＝sin(θ_region)；

5-4)对于每一个区域中的点P，访问其邻域像素首先判断是否被访问过，如果被访问过，则访问下一个邻域像素；如果没有被访问过，则标记被访问过，并且判断像素点的直线方向角与区域方向θ_region之差，如果方向之差不超过阈值2π/n，那么就认为其方向一致，则把该点加入到区域中，并更新区域方向角度，如下：

${\mathrm{\θ}}_{\mathrm{region}}=\arctan \left(\frac{S_y+\sin \left({\mathrm{\θ}}_{\stackrel{\‾}{P}}\right)}{S_x+\cos \left({\mathrm{\θ}}_{\stackrel{\‾}{P}}\right)}\right)---\left(6\right)$

5-5)重复步骤5-4)，直到当没有新的像素增加到该区域中时，该种子像素区域增长过程结束；返回到步骤5-1)，重新从伪排序列中选取下一个种子点，开始下一个区域的增长，直到伪排序列中没有能够满足要求的种子点为止，区域增长算法结束。

在每一个像素被访问之前，先判断其梯度模值大小，如果小于设定的梯度模值阈值，则该像素不能够被访问；阈值选取的越大，提取到的边缘越清晰，候选区域包含的像素个数越少；反之，候选区域包含的像素个数越多，计算量也越大。

所述的步骤6)中，采用外接矩形法描述和判定候选区域Re，并且计算外接矩形的短轴和长轴的方向以及端点坐标的具体方法如下：

6-1)首先计算候选区域Re的形心(c_x,c_y)，公式如下：

$\begin{array}{cc}{c}_x=\frac{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)*x\left(j\right)}{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)}& c_y=\frac{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)*y\left(j\right)}{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)}---\left(7\right)\end{array}$

式中，(c_x,c_y)是候选区域Re的形心坐标，G(j)为候选区域Re中第j个像素的梯度模值，(x(j),y(j))是第j个像素的坐标；

6-2)计算候选区域的长轴和短轴方向，是由下面矩阵的特征值和特征向量计算得到：

$M=\left[\begin{array}{cc}{m}^{\mathrm{xx}}& m^{\mathrm{xy}}\\ m^{\mathrm{xy}}& m^{\mathrm{yy}}\end{array}\right]---\left(8\right)$

矩阵中 $m^{\mathrm{xx}}=\frac{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)*{(x\left(j\right)-c_x)}^2}{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)}$

$m^{\mathrm{yy}}=\frac{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)*{(y\left(j\right)-c_y)}^2}{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)}$

$m^{\mathrm{xy}}=\frac{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)*(x\left(j\right)-c_x)*(y\left(j\right)-c_y)}{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)}$

设矩阵M的特征值分别为a，b(b>a)，对应的特征向量分别为(a₁,a₂)^T和(b₁,b₂)^T；

矩阵的特征值反映的是长轴和短轴两个方向上像素的分布情况，较大的特征值对应候选区域的长轴，其特征向量方向就是候选区域的长轴方向；由特征向量能够计算出候选区域的短轴和长轴方向，分别为：

k₁＝tan(θ₁)＝a₂/a₁，k₂＝tan(θ₂)＝b₂/b₁                (9)

其中k₁，k₂分别为候选区域的短轴和长轴斜率，θ₁，θ₂分别为两个特征值对应的特征向量方向角；

6-3)确定候选区域的外接矩形：以步骤7-2)计算得到的候选区域的短轴和长轴方向作为矩形的两个垂直边的方向，矩形的长和宽取为最小的可以覆盖整个候选区域的值；

6-4)确定过区域形心(c_x,c_y)的长轴直线解析式为：

y-c_y＝k₂(x-c_x)                   (10)

整理为点斜式为：

y＝k₂*x+c_y-k₂*c_x                   (11)

同理，短轴的解析式为：

y＝k₁*x+c_y-k₁*c_x                   (12)

6-5)计算长轴的两个端点p₁和p₂，公式如下：

p₁＝(c_x,c_y)+d₁*(b₁,b₂)               (13)

p₂＝(c_x,c_y)+d₂*(b₁,b₂)               (14)

式中， $d_{\mathrm{1,2}}=\min ,\max \left\{\frac{(y_k-k_2*x_k+c_y-k_2*c_x)}{\sqrt{k_2+1}}\right\}$

同理，计算出短轴的两个端点；

另外，对于长轴斜率为零的情况，两个端点的纵坐标为c_y，横坐标为像素横坐标的极大极小值。

所述的步骤7)中，确定障碍物直线段边缘的具体方法如下：

采用外接矩形的长轴与短轴长度的比值判定候选区域是否对应直线段结构，长短轴之比值大于设定的阈值，则区域中像素主要分布在长轴方向，认为候选区域对应直线段模型，长轴就是所要求的直线段；如果长短轴之比小于设定的阈值，则认为此区域对应的不是直线段。

所述的步骤8)中，得出最近的距离和最近的障碍物点的三维坐标的具体方法如下：

8-1)遍历障碍物直线段边缘上的像素点，设Q_i为其中的一个点，并且Q_i在左相机图像中的坐标为[u_L,v_L]^T，在右相机图像中的坐标为[u_R,v_R]^T，那么Q_i映射到世界坐标系中的三维坐标[X_i,Y_i,Z_i]^T为：

$X_i=\frac{(u_L-c_{u,L})\·b}{d}---\left(15\right)$

$Y_i=\frac{(v_L-c_{v,L})\·b}{d}---\left(16\right)$

$Z_i=\frac{b\·f}{d}---\left(17\right)$

其中b代表立体相机的基线，c_u，L和c_v，L是左相机的主点，f是焦距，d表示视差，d＝|u_L-u_R|；

8-2)设抓捕点O在三维空间中的坐标为[X_O,Y_O,Z_O]^T，则Q_i点到O的三维距离为：

$L_i=\sqrt{{(X_i-X_O)}^2+{(Y_i-Y_O)}^2+{(Z_i-Z_O)}^2}---\left(18\right)$

8-3)遍历障碍物直线段边缘上的所有像素点，找出最短距离L_io和与之对应的障碍物上的最近点Q_io。

与现有技术相比，本发明具有如下有益效果：

本发明的直线段检测部分采用的是基于图像梯度信息的直线段检测方法，本发明的基本思想是拥有一致梯度方向的像素队列很有可能是某物体的边缘直线段。本发明能够在多种复杂场景中快速准确地检测出直线段，利用本发明对抓捕点周围的障碍物进行检测，然后对检测到的直线段边缘上点进行三维重建，并计算与抓捕点的三维距离，找出距离最近的点，并得出其三维坐标和最近的距离。本发明具体拥有以下优点：

1、检测效果好，结果准确，精度较高；

本发明基于像素梯度，利用区域增长方法，能够比较精确的检测出场景中的局部和全局直线段，并且能准确给出直线段的两端点，解决了Hough变换方法的多峰值检测以及断点检测效率低的缺陷。

2、计算量小，计算速度快，能够满足实时性的要求；

本发明中对像素梯度模值的排序利用了伪序法，不仅能够满足精度的要求，而且大大减小了计算量，加快了计算速度。

3、具有良好的鲁棒性，能够应用于多种复杂场景，抗噪声能力比较强；

由于算法中采用的是图像像素间的梯度关系，并且用到了区域增长方法寻找直线段区域，所以在一定程度上对光照变换，噪声影响等都具有鲁棒性，能够适应复杂场景中的应用。

4、能够有效抵抗细小边缘结构的干扰；

在算法中设置合适的阈值，对于梯度模值小于该阈值的像素不进行区域增长，从而能够抵抗细小边缘结构的干扰。

5、本发明对各种复杂场景具有很强的适应性，使用于各种场景图像的处理。

本发明中所用的到参数都是通过大量试验，精心选取的，作为内部参数包装在算法内，所以本发明应用到不同场景图像处理中时，几乎不用调节参数，具有很强的适应性。

附图说明

图1为本发明的算法总流程框图。

具体实施方式

以下结合附图对本发明进行详细的描述。应当指出的是，所描述的实施例仅旨在便于对本发明的理解，而对其不起任何限定作用。

如图1所示，本发明实施例提供的应用于空间绳系机器人的抓捕点附近障碍物的检测方法包括：

步骤一：空间绳系机器人上的双目相机采集到目标的左右图像I_L和I_R。

步骤二：在左图像I_L上，以抓捕点O为中心设置一个矩形抓捕区域R，作为处理的感兴趣区域，在实际中矩形的大小根据实际情况设定，本实施例中设为80*80像素大小，能够满足要求。

步骤三：在感兴趣区域内计算图像的梯度，并计算梯度模值和直线方向角，具体步骤分解如下：

(1)：对于感兴趣区域R中的像素(x,y)，其x，y方向上的梯度g_x(x,y)，g_y(x,y)定义为：

$g_x(x,y)=\frac{I(x+1,y)+I(x+1,y+1)-I(x,y)-I(x,y+1)}{2}$

$g_y(x,y)=\frac{I(x,y+1)+I(x+1,y+1)-I(x,y)-I(x+1,y)}{2}---\left(1\right)$

式中，I(x,y)为像素(x,y)的灰度值。

(2)：计算梯度的模值G(x,y)，公式如下：

$G(x,y)=\sqrt{({g^2}_x(x,y)+{g^2}_y(x,y))}---\left(3\right)$

直线方向角θ定义为：

$\mathrm{\θ}=\arctan \left(\frac{g_x(x,y)}{-g_y(x,y)}\right).$

步骤四：根据像素梯度模值的大小，利用伪序法对像素进行排序：

本发明中所用到的梯度邻域增长是一种全局的遍历算法，因此处理像素的顺序会影响到处理结果，所以通常是从一个具有较大梯度模值的像素先开始梯度邻域增长。这就需要对像素首先按照梯度模值排序。然而由于图像的数据量往往都比较庞大，所以要实现对图像像素快速的排序往往是比较困难的，很难达到实时性的要求。在本发明中，采用一种计算量小的伪序法对像素实现快速排序，可以保证算法的实时性，具体方法如下：

首先按照梯度模值的最大值和最小值线性生成一个区间，然后将每一个像素按照梯度模值大小放入对应区间内。

对于本实施例中灰度级为0～255的图像来说，采用1024分辨率的区间便可以很好的对像素梯度模值进行排序。对于同一区间内的像素，认为其梯度模值相差很小，以同等的概率选为邻域增长的种子点。

步骤五：按照伪序排列结果，选取种子像素，按照区域增长方法计算直线段候选区域Re，具体步骤分解如下：

(1)：按照伪序排列结果，根据梯度模值从大到小选取区域增长的种子点，并且要求该点没有被访问过，区域增长从该种子像素开始；

(2)：定义区域的角度为：

${\mathrm{\θ}}_{\mathrm{region}}=\arctan \left(\frac{{\mathrm{\Σ}}_i\sin \left({\mathrm{\θ}}_i\right)}{{\mathrm{\Σ}}_i\cos \left({\mathrm{\θ}}_i\right)}\right)---\left(5\right)$

其中，θ_region表示区域的角度，θ_i表示区域中第i个像素的直线方向角。

(3)：设种子点为(x,y)，则开始时区域角度为种子点的直线方向角，即：θ_region＝θ_(x,y)，令S_x＝cos(θ_region)，S_y＝sin(θ_region)；

(4)：对于每一个区域中的点P，访问其邻域像素(本实施例中采用的是8邻接像素邻域，即对于像素点p(x,y)，其8邻域像素为p(x-1,y-1)，p(x,y-1)，p(x+1,y-1)，p(x-1,y)，p(x+1,y)，p(x-1,y+1)，p(x,y+1)，p(x+1,y+1))。首先判断是否被访问过，如果被访问过，则访问下一个邻域像素；如果没有被访问过，则标记被访问过，并且判断像素点的直线方向角与区域方向θ_region之差，如果方向之差不超过阈值2π/n，那么就认为其方向一致(在本实施例中n取为16)，则把该点加入到区域中，并更新区域方向角度，如下：

${\mathrm{\θ}}_{\mathrm{region}}=\arctan \left(\frac{S_y+\sin \left({\mathrm{\θ}}_{\stackrel{\‾}{P}}\right)}{S_x+\cos \left({\mathrm{\θ}}_{\stackrel{\‾}{P}}\right)}\right)---\left(6\right)$

(5)：重复步骤(4)，直到当没有新的像素增加到该区域中时，该种子像素区域增长过程结束。返回到步骤(1)，重新从伪排序列中选取下一个种子点，开始下一个区域的增长，直到伪排序列中没有能够满足要求的种子点为止，区域增长算法结束。

需要注意，在每一个像素被访问之前，要先判断其梯度模值大小，如果小于设定的梯度模值阈值，则该像素不能够被访问。阈值选取的越大，提取到的边缘越清晰，候选区域包含的像素个数越少。反之，候选区域包含的像素个数越多，计算量也越大。所以，梯度阈值要根据实际需求选取。在本实施例中，选取一个择中的梯度阈值5/sin(2π/n)，其中n同样取为16。

步骤六：划分出直线段的候选区域Re后，采用外接矩形法描述和判定候选区域Re，并且计算外接矩形的短轴和长轴的方向以及端点坐标，具体步骤分解如下：

(1)：首先计算候选区域Re的形心(c_x,c_y)，公式如下：

$\begin{array}{cc}{c}_x=\frac{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)*x\left(j\right)}{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)}& c_y=\frac{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)*y\left(j\right)}{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)}---\left(7\right)\end{array}$

式中，(c_x,c_y)是候选区域Re的形心坐标，G(j)为候选区域Re中第j个像素的梯度模值，(x(j),y(j))是第j个像素的坐标。

(2)：计算候选区域的长轴和短轴方向，是由下面矩阵的特征值和特征向量计算得到：

$M=\left[\begin{array}{cc}{m}^{\mathrm{xx}}& m^{\mathrm{xy}}\\ m^{\mathrm{xy}}& m^{\mathrm{yy}}\end{array}\right]---\left(8\right)$

矩阵中 $m^{\mathrm{xx}}=\frac{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)*{(x\left(j\right)-c_x)}^2}{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)}$

$m^{\mathrm{yy}}=\frac{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)*{(y\left(j\right)-c_y)}^2}{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)}$

$m^{\mathrm{xy}}=\frac{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)*(x\left(j\right)-c_x)*(y\left(j\right)-c_y)}{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)}$

设矩阵M的特征值分别为a，b(b>a)，对应的特征向量分别为(a₁,a₂)^T和(b₁,b₂)^T。

矩阵的特征值反映的是长轴和短轴两个方向上像素的分布情况，较大的特征值对应候选区域的长轴，其特征向量方向就是候选区域的长轴方向。由特征向量可以计算出候选区域的短轴和长轴方向(斜率)，分别为：

k₁＝tan(θ₁)＝a₂/a₁，k₂＝tan(θ₂)＝b₂/b₁                 (9)

其中k₁，k₂分别为候选区域的短轴和长轴斜率，θ₁，θ₂分别为两个特征值对应的特征向量方向角。

(3)：确定候选区域的外接矩形：以步骤(2)计算得到的候选区域的短轴和长轴方向作为矩形的两个垂直边的方向，矩形的长和宽取为最小的可以覆盖整个候选区域的值。

(4)：确定过区域形心(c_x,c_y)的长轴直线解析式为：

y-c_y＝k₂(x-c_x)                   (10)

整理为点斜式为：

y＝k₂*x+c_y-k₂*c_x                   (11)

同理，短轴的解析式为：

y＝k₁*x+c_y-k₁*c_x                   (12)

(5)：计算长轴的两个端点p₁和p₂，公式如下：

p₁＝(c_x,c_y)+d₁*(b₁,b₂)                   (13)

p₂＝(c_x,c_y)+d₂*(b₁,b₂)                   (14)

式中， $d_{\mathrm{1,2}}=\min ,\max \left\{\frac{(y_k-k_2*x_k+c_y-k_2*c_x)}{\sqrt{k_2+1}}\right\}$

同理，也容易计算出短轴的两个端点。

另外，对于长轴斜率为零的特殊情况，两个端点的纵坐标为c_y，横坐标为像素横坐标的极大极小值。

步骤七：采用外接矩形长短轴之比与阈值进行比较的方法，确定出所要检测的直线段：

本发明采用外接矩形的长轴与短轴长度的比值判定候选区域是否对应直线段结构，长短轴之比值大，则区域中像素主要分布在长轴方向，可以认为候选区域对应直线段模型，长轴就是所要求的直线段。如果长短轴之比小于设定的阈值(在本实施例中设定的阈值为6)，则认为此区域对应的不是直线段。

步骤八：遍历障碍物直线段边缘上的像素点，对其进行三维重建，并计算与抓捕点O之间的三维距离，得出最近的距离和最近的障碍物点的三维坐标，具体步骤如下：

(1)：遍历障碍物直线段边缘上的像素点，设Q_i为其中的一个点，并且Q_i在左相机图像中的坐标为[u_L,v_L]^T，在右相机图像中的坐标为[u_R,v_R]^T，那么Q_i映射到世界坐标系中的三维坐标[X_i,Y_i,Z_i]^T为：

$X_i=\frac{(u_L-c_{u,L})\·b}{d}---\left(15\right)$

$Y_i=\frac{(v_L-c_{v,L})\·b}{d}---\left(16\right)$

$Z_i=\frac{b\·f}{d}---\left(17\right)$

其中b代表立体相机的基线，c_u,L和c_v,L是左相机的主点，f是焦距，d表示视差，d＝|u_L-u_R|。

(2)：设抓捕点O在三维空间中的坐标为[X_O,Y_O,Z_O]^T，则Q_i点到O的三维距离为：

$L_i=\sqrt{{(X_i-X_O)}^2+{(Y_i-Y_O)}^2+{(Z_i-Z_O)}^2}---\left(18\right)$

(3)遍历障碍物直线段边缘上的所有像素点，找出最短距离L_io和与之对应的障碍物上的最近点Q_io。

至此，就能正确的检测出距离抓捕点最近的障碍物距离和障碍物上的最近点了。

以上所述，仅为本发明中的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人在本发明所揭露的技术范围内，可理解想到的变换或替换，都应涵盖在本发明的包含范围之内，因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (9)

1.一种空间绳系机器人抓捕区域内的障碍物检测方法，其特征在于，包括以下步骤：

1)利用空间绳系机器人上的双目相机对目标进行图像采集；

采集到目标的左图像I_L和右图像I_R；

2)在左图像I_L上，以抓捕点O为中心设置一个矩形抓捕区域R，作为处理的感兴趣区域R；

3)在感兴趣区域R内计算该图像的梯度，并计算梯度模值和直线方向角；

4)根据图像的像素梯度模值的大小，利用伪序法对图像的像素进行排序；

5)按照伪序排列结果，选取种子像素，按照区域增长方法计算直线段候选区域Re；

6)划分出直线段候选区域Re后，采用外接矩形法描述和判定直线段候选区域Re，并且计算外接矩形的短轴和长轴的方向以及端点坐标；

7)采用外接矩形长短轴之比与阈值进行比较的方法，确定出所要检测障碍物的直线段边缘；

8)遍历障碍物直线段边缘上的像素点，对其进行三维重建，并计算与抓捕点O之间的三维距离，得出最近的距离和最近的障碍物点的三维坐标。

2.根据权利要求1所述的空间绳系机器人抓捕区域内的障碍物检测方法，其特征在于：所述的步骤3)中，计算梯度模值和直线方向角的具体步骤如下：

3-1)：对于感兴趣区域R中的像素(x,y)，其x，y方向上的梯度g_x(x,y)，g_y(x,y)定义为：

$g_x(x,y)=\frac{I(x+1,y)+I(x+1,y+1)-I(x,y)-I(x,y+1)}{2}$

$g_y(x,y)=\frac{I(x,y+1)+I(x+1,y+1)-I(x,y)-I(x+1,y)}{2}---\left(1\right)$

式中，I(x,y)为像素(x,y)的灰度值；

3-2)：计算梯度的模值G(x,y)，公式如下：

$G(x,y)=\sqrt{({g^2}_x(x,y)+{g^2}_y(x,y))}---\left(3\right)$

直线方向角θ定义为：

$\mathrm{\θ}=\arctan \left(\frac{g_x(x,y)}{-g_y(x,y)}\right).$

3.根据权利要求1所述的空间绳系机器人抓捕区域内的障碍物检测方法，其特征在于：所述的步骤4)中，利用伪序法对像素进行排序的具体方法如下：

首先按照梯度模值的最大值和最小值线性生成一个区间，然后将每一个像素按照梯度模值大小放入对应区间内。

4.根据权利要求3所述的空间绳系机器人抓捕区域内的障碍物检测方法，其特征在于：对于同一区间内的像素，认为其梯度模值相差很小，以同等的概率选为邻域增长的种子点。

5.根据权利要求1所述的空间绳系机器人抓捕区域内的障碍物检测方法，其特征在于：所述的步骤5)中，计算直线段候选区域Re的具体方法如下：

5-1)按照伪序排列结果，根据梯度模值从大到小选取区域增长的种子点，并且要求该点没有被访问过，区域增长从该种子像素开始；

5-2)定义区域的角度为：

${\mathrm{\θ}}_{\mathrm{region}}=\arctan \left(\frac{{\mathrm{\Σ}}_i\sin \left({\mathrm{\θ}}_i\right)}{{\mathrm{\Σ}}_i\cos \left({\mathrm{\θ}}_i\right)}\right)---\left(5\right)$

其中，θ_region表示区域的角度，θ_i表示区域中第i个像素的直线方向角；

5-3)设种子点为(x,y)，则开始时区域角度为种子点的直线方向角，即：θ_region＝θ_(x,y)，令S_x＝cos(θ_region)，S_y＝sin(θ_region)；

5-4)对于每一个区域中的点P，访问其邻域像素首先判断是否被访问过，如果被访问过，则访问下一个邻域像素；如果没有被访问过，则标记被访问过，并且判断像素点的直线方向角与区域方向θ_region之差，如果方向之差不超过阈值2π/n，那么就认为其方向一致，则把该点加入到区域中，并更新区域方向角度，如下：

${\mathrm{\θ}}_{\mathrm{region}}=\arctan \left(\frac{S_y+\sin \left({\mathrm{\θ}}_{\stackrel{\‾}{P}}\right)}{S_x+\cos \left({\mathrm{\θ}}_{\stackrel{\‾}{P}}\right)}\right)---\left(6\right)$

5-5)重复步骤5-4)，直到当没有新的像素增加到该区域中时，该种子像素区域增长过程结束；返回到步骤5-1)，重新从伪排序列中选取下一个种子点，开始下一个区域的增长，直到伪排序列中没有能够满足要求的种子点为止，区域增长算法结束。

6.根据权利要求5所述的空间绳系机器人抓捕区域内的障碍物检测方法，其特征在于：在每一个像素被访问之前，先判断其梯度模值大小，如果小于设定的梯度模值阈值，则该像素不能够被访问；阈值选取的越大，提取到的边缘越清晰，候选区域包含的像素个数越少；反之，候选区域包含的像素个数越多，计算量也越大。

7.根据权利要求1所述的空间绳系机器人抓捕区域内的障碍物检测方法，其特征在于：所述的步骤6)中，采用外接矩形法描述和判定候选区域Re，并且计算外接矩形的短轴和长轴的方向以及端点坐标的具体方法如下：

6-1)首先计算候选区域Re的形心(c_x,c_y)，公式如下：

$\begin{array}{cc}{c}_x=\frac{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)*x\left(j\right)}{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)}& c_y=\frac{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)*y\left(j\right)}{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)}---\left(7\right)\end{array}$

式中，(c_x,c_y)是候选区域Re的形心坐标，G(j)为候选区域Re中第j个像素的梯度模值，(x(j),y(j))是第j个像素的坐标；

6-2)计算候选区域的长轴和短轴方向，是由下面矩阵的特征值和特征向量计算得到：

$M=\left[\begin{array}{cc}{m}^{\mathrm{xx}}& m^{\mathrm{xy}}\\ m^{\mathrm{xy}}& m^{\mathrm{yy}}\end{array}\right]---\left(8\right)$

矩阵中 $m^{\mathrm{xx}}=\frac{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)*{(x\left(j\right)-c_x)}^2}{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)}$

$m^{\mathrm{yy}}=\frac{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)*{(y\left(j\right)-c_y)}^2}{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)}$

$m^{\mathrm{xy}}=\frac{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)*(x\left(j\right)-c_x)*(y\left(j\right)-c_y)}{{\mathrm{\Σ}}_{j\∈\mathrm{Re}}G\left(j\right)}$

设矩阵M的特征值分别为a，b(b>a)，对应的特征向量分别为(a₁,a₂)^T和(b₁,b₂)^T；

矩阵的特征值反映的是长轴和短轴两个方向上像素的分布情况，较大的特征值对应候选区域的长轴，其特征向量方向就是候选区域的长轴方向；由特征向量能够计算出候选区域的短轴和长轴方向，分别为：

k₁＝tan(θ₁)＝a₂/a₁，k₂＝tan(θ₂)＝b₂/b₁              (9)

其中k₁，k₂分别为候选区域的短轴和长轴斜率，θ₁，θ₂分别为两个特征值对应的特征向量方向角；

6-3)确定候选区域的外接矩形：以步骤7-2)计算得到的候选区域的短轴和长轴方向作为矩形的两个垂直边的方向，矩形的长和宽取为最小的可以覆盖整个候选区域的值；

6-4)确定过区域形心(c_x,c_y)的长轴直线解析式为：

y-c_y＝k₂(x-c_x)                  (10)

整理为点斜式为：

y＝k₂*x+c_y-k₂*c_x                   (11)

同理，短轴的解析式为：

y＝k₁*x+c_y-k₁*c_x                   (12)

6-5)计算长轴的两个端点p₁和p₂，公式如下：

p₁＝(c_x,c_y)+d₁*(b₁,b₂)                   (13)

p₂＝(c_x,c_y)+d₂*(b₁,b₂)                   (14)

式中， $d_{\mathrm{1,2}}=\min ,\max \left\{\frac{(y_k-k_2*x_k+c_y-k_2*c_x)}{\sqrt{k_2+1}}\right\}$

同理，计算出短轴的两个端点；

另外，对于长轴斜率为零的情况，两个端点的纵坐标为c_y，横坐标为像素横坐标的极大极小值。

8.根据权利要求1所述的空间绳系机器人抓捕区域内的障碍物检测方法，其特征在于：所述的步骤7)中，确定障碍物直线段边缘的具体方法如下：

采用外接矩形的长轴与短轴长度的比值判定候选区域是否对应直线段结构，长短轴之比值大于设定的阈值，则区域中像素主要分布在长轴方向，认为候选区域对应直线段模型，长轴就是所要求的直线段；如果长短轴之比小于设定的阈值，则认为此区域对应的不是直线段。

9.根据权利要求1所述的空间绳系机器人抓捕区域内的障碍物检测方法，其特征在于：所述的步骤8)中，得出最近的距离和最近的障碍物点的三维坐标的具体方法如下：

8-1)遍历障碍物直线段边缘上的像素点，设Q_i为其中的一个点，并且Q_i在左相机图像中的坐标为[u_L,v_L]^T，在右相机图像中的坐标为[u_R,v_R]^T，那么Q_i映射到世界坐标系中的三维坐标[X_i,Y_i,Z_i]^T为：

$X_i=\frac{(u_L-c_{u,L})\·b}{d}---\left(15\right)$

$Y_i=\frac{(v_L-c_{v,L})\·b}{d}---\left(16\right)$

$Z_i=\frac{b\·f}{d}---\left(17\right)$

其中b代表立体相机的基线，c_u，L和c_v，L是左相机的主点，f是焦距，d表示视差，d＝|u_L-u_R|；

8-2)设抓捕点O在三维空间中的坐标为[X_O,Y_O,Z_O]^T，则Q_i点到O的三维距离为：

$L_i=\sqrt{{(X_i-X_O)}^2+{(Y_i-Y_O)}^2+{(Z_i-Z_O)}^2}---\left(18\right)$

8-3)遍历障碍物直线段边缘上的所有像素点，找出最短距离L_io和与之对应的障碍物上的最近点Q_io。