CN107908841B - 三维壁面可抓取位置判别算法 - Google Patents

Abstract

本本发明公开了一种三维壁面可抓取位置判别算法，包括步骤为：定义最小可用角度；获取初始点云数据；获取插值后的点云样本；寻找可接触三角形的点集合；确定所有的接触三角形：求解尖钩球心坐标、尖钩球心坐标的确定、接触三角形的确定；计算接触三角形法向量；计算接触角度；抓取条件判定；获得所有可抓取点。本算法可以判断并找出较大范围粗糙壁面内的可抓取点的位置，为爬壁机器人实现抓取真实粗糙壁面提供了实用性算法，是对二维壁面轮廓上的抓附研究成果的改进，提高了机器人抓取粗糙壁面合适位置的的准确度，从而提高了机器人对粗糙壁面抓附的稳定性，可以有效克服高空风载和壁面振动对机器人爬壁性能的部分影响。

Description

三维壁面可抓取位置判别算法

技术领域

本发明涉及一种爬壁机器人应用领域，特别是一种三维壁面可抓取位置判别算法。

背景技术

以理想化模型对尖钩与壁面的接触进行理论分析，虽然能直观、准确的得到尖钩与壁面的接触角度，进而判断是否可以稳定抓取，但在实际的工程应用中，钩爪抓取式爬壁机器人面对的工作环境通常是凹凸不平的壁面，难以用统一的形状进行描述。二维、三维壁面的轮廓信息均可以通过粗糙表面测量仪进行提取，分别以轮廓曲线、点云的方式呈现，国外学者在二维壁面轮廓信息采集的基础上，通过实验模拟的方式对尖钩截面在二维壁面轮廓上的抓附进行了相关研究。

上述尖钩截面在二维壁面轮廓上的抓附分析，以尖钩截面为圆形，二维壁面轮廓中以线段代替相邻两点进行分析。然而，在实际中，尖钩末端为球面，尖钩与壁面为面接触。因此，本发明旨在他人研究的基础上，通过对尖钩截面与二维壁面轮廓上点的接触分析，提出一种基于三维壁面点云数据的可抓取位置判别算法。

发明内容

本发明要解决的技术问题是针对上述现有技术的不足，而提供一种三维壁面可抓取位置判别算法，该三维壁面可抓取位置判别算法能够判断并找出较大范围粗糙壁面内的可抓取点的位置，为爬壁机器人实现真实粗糙壁面的抓附提供了一种实用性算法，是对二维壁面轮廓上的抓附研究成果的改进，提高了机器人抓取粗糙壁面合适位置的的准确度，从而提高了机器人对粗糙壁面抓附的稳定性，可以有效克服高空风载和壁面振动对机器人爬壁性能的部分影响。

为解决上述技术问题，本发明采用的技术方案是：

一种三维壁面可抓取位置判别算法，包括如下步骤。

步骤1，定义最小可用角度θ_min：假设θ_b代表尖钩的圆心轨迹法向与壁面法向之间的夹角，当θ_b≥θ_min时，尖钩能够完成钩附；其中，最小可用角度θ_min的计算公式如下：

θ_min＝θ_load+arccot(μ) (3.1)

式中，θ_load为尖钩的载荷角，μ为尖钩与壁面的摩擦系数。

步骤2，获取初始点云数据：采用粗糙表面测量仪器提取粗糙壁面的表面特征，对壁面轮廓进行测量，采集粗糙壁面特征点数据，通过系统采集整理，获得初始的三维点云数据M₂₁[x_set,y_set,z_set]；其中，x_set为壁面轮廓上点的x坐标值集合，y_set为与之对应的y坐标值集合，z_set为与之对应的z坐标值集合，x与y坐标表示粗糙壁面表平面，z坐标表示粗糙壁面凹凸起伏的变化。

步骤3，获取插值后的点云样本：假设机器人用于抓取的尖钩半径为r，根据尖钩半径r对步骤2获取的初始点云数据进行插值操作，得到插值后的点云样本M₂₁[x_set,y_set,z_set]；插值后，应使得连续两个点云之间的间距小于2r。

步骤4，寻找可接触三角形的点集合：采用下述公式(3.4)与公式(3.5)对步骤3得到的插值后的点云样本M₂₁[x_set,y_set,z_set]采取遍历的方式，求得所有可接触三角形的三个点组合，并放入可接触三角形的点集合M₂₂[x_combine,y_combine,z_combine]中。

公式(3.4)为：

式中，点A、点B和点C为其中一个可接触三角形ΔABC的三个点，其中，点A的三维坐标为(x_A,y_A,z_A),点B的三维坐标为(x_B,y_B,z_B),点C的三维坐标为(x_C,y_C,z_C)。

公式(3.5)为：

其中，p＝(AB+AC+BC)/2

式中，AB、AC和BC分别为可接触三角形ΔABC的三条边长。

步骤5，确定所有的接触三角形：将点集合M₂₂[x_combine,y_combine,z_combine]中的每一个组合均按照如下方法进行判断。

步骤51，求解尖钩球心坐标：以点集合M₂₂[x_combine,y_combine,z_combine]中当前组合的三个点坐标，也即点A、点B和点C的坐标以及尖钩半径r，求解对应的尖钩球心坐标(x,y,z)；求解时，采用Matlab中的solve函数，具体求解公式为：

syms x,y,z；

[x,y,z]＝solve(r²-(x-x_A)²-(y-y_A)²-(z-z_A)²,r²-(x-x_B)²-(y-y_B)²-(z-z_B)²,

r²-(x-x_C)²-(y-y_C)²-(z-z_C)²)

步骤52，尖钩球心坐标的确定：对步骤51求得的结果进行判断：若结果为实数且只有一个解时，则该实数解即为确定的尖钩球心坐标，进行步骤53；若结果为实数且有两个解时，取z轴坐标值较大的实数解，作为确定的尖钩球心坐标，进行步骤53；否则，跳转至步骤51，以点集合M₂₂[x_combine,y_combine,z_combine]中下一个组合进行尖钩球心坐标的求解，直至求得的结果为实数。

步骤53，接触三角形的确定：以步骤52确定的尖钩球心坐标(x,y,z)先对步骤4寻找的点集合M₂₂[x_combine,y_combine,z_combine]中的当前组合的三个点A、B、C按如下公式求取距离并判断；

式中，点坐标(x_i,y_i,z_i)为当前组合ΔABC内的任一点i。

当三个点A、B、C以及点坐标为(x_i,y_i,z_i)的点i与尖钩球心之间的间距均满足公式(3.6)的要求时，则判定当前组合ΔABC为接触三角形，并将当前三个点A、B、C以及对应的尖钩球心均放入M₂₃矩阵中；否则，对点集合M₂₂[x_combine,y_combine,z_combine]中的下一个组合按照公式(3.6)进行求距与判断，直至完成点集合M₂₂[x_combine,y_combine,z_combine]中的所有组合的求距与判断，将所有判断为接触三角形的三个点均放入M₂₃矩阵中。

步骤6，计算接触三角形法向量：将M₂₃矩阵中的其中一个组成接触三角形的三个点A、B、C根据公式(3.7)求得接触三角形ΔABC的法向量

结合尖钩球心所在位置，使法向量

始终朝向尖钩球心。

步骤7，计算接触角度θ：步骤6计算的法向量

与尖钩移动反方向之间所形成的夹角，即为接触角度θ。

步骤8，抓取条件判定：若π/2-θ>θ_min，则判断为满足抓取条件，满足抓取条件的三个点均为可抓取点，并将该组点放入M₂₄矩阵。

步骤9，获得所有可抓取点：对M₂₃矩阵中下一组合中的三个点进行步骤6至步骤8的判断，直到M₂₃矩阵中所有组合判断结束，得到所有可抓取点。

步骤3中，根据尖钩半径r对初始点云数据进行插值的具体操作方法为：从步骤2测量获得的初始点云样本数据M₂₁[x_set,y_set,z_set]中取连续两点(x_i,y_i,z_i),(x_i+1,y_i+1,z_i+1)(i＝1,2…n)，若

成立，则表示相邻两点满足间距要求；若不成立，在相邻两点间插入一个数据点((x_i+x_i+1)/2,(y_i+y_i+1)/2,(z_i+z_i+1)/2)成为新的(x_i+1,y_i+1,z_i+1)，对新的(x_i+1,y_i+1,z_i+1)与(x_i,y_i,z_i)继续进行上述操作，直到满足

然后对下一组点重复这一操作，得到插值后的最终的点云样本M₂₁[x_set,y_set,z_set]。

本发明具有如下有益效果：本发明的三维壁面可抓取位置判别算法能够判断并找出较大范围粗糙壁面内的可抓取点的位置，为爬壁机器人实现真实粗糙壁面的抓附提供了一种实用性算法，是对二维壁面轮廓上的抓附研究成果的改进，提高了机器人抓取粗糙壁面合适位置的的准确度，从而提高了机器人对粗糙壁面抓附的稳定性，可以有效克服高空风载和壁面振动对机器人爬壁性能的部分影响。

附图说明

图1显示了本发明球形微凸体点云的接触模型。

图2显示了尖钩末端与三角形接触模型。

图3显示了壁面锥形凸起。

图4显示了尖钩末端与壁面锥形凸起接触模型。

图5显示了空间点与球体接触模型。

图6显示了三角形对应的尖钩球心位置。

图7显示了采用三维壁面可抓取位置判别算法后的仿真结果1。

图8显示了采用三维壁面可抓取位置判别算法后的仿真结果2。

图9显示了采用三维壁面可抓取位置判别算法后的仿真结果3。

具体实施方式

下面结合附图和具体较佳实施方式对本发明作进一步详细的说明。

一种三维壁面可抓取位置判别算法，包括如下步骤。

步骤1，定义最小可用角度θ_min：假设θ_b代表尖钩的圆心轨迹法向与壁面法向之间的夹角，当θ_b≥θ_min时，尖钩能够完成钩附；其中，最小可用角度θ_min的计算公式如下：

θ_min＝θ_load+arccot(μ) (3.1)

式中，θ_load为尖钩的载荷角，μ为尖钩与壁面的摩擦系数。

步骤2，获取初始点云数据：采用例如三维激光扫描仪等类似的粗糙表面测量仪器提取粗糙壁面的表面特征，对壁面轮廓进行测量，采集粗糙壁面特征点数据，通过系统采集整理，获得初始的三维点云数据M₂₁[x_set,y_set,z_set]，其中，坐标原点为三维激光扫描仪等自选默认原点；x_set为壁面轮廓上点的x坐标值集合，y_set为与之对应的y坐标值集合，z_set为与之对应的z坐标值集合，x与y坐标表示粗糙壁面表平面，z坐标表示粗糙壁面凹凸起伏的变化；

上述获取的初始点云数据中的每个点都带有x、y、z的坐标信息，组成一个空间曲面。当尖钩在该空间曲面上滑动时，并不是所有的位置都能接触：当曲面有较深的凹沟或一些细小的夹缝时，尖钩无法到达，在以三维点云数据进行描述时，表现为有些点不可接触。由于空间曲面的局部曲率不同，尖钩的接触角度也会发生变化，在三维点云数据中，壁面的局部曲率包含在形成这片区域的点的坐标信息中。壁面的局部曲率，我们可以通过将壁面分割来进行求解，因此，本发明提出一种点云的三角形组合策略。

如图1所示，当尖钩末端与点云描述的球形壁面微凸体接触时，将尖钩与球形微凸体的接触等效为尖钩与球体表面点云组成三角形的接触。

如图2所示，当球形微凸体表面一块区域内只有三个点A、B、C时，将A、B、C组成的三角形视为该块区域的表面形貌，尖钩末端在该块区域与球形微凸体接触时，视为与ΔABC的接触，ΔABC法向量与竖直方向的夹角即为尖钩末端与球形微凸体的接触角度。

如图3所示，若ΔABC区域的上侧有一点D，由于空间内三个不共线的点确定一个平面，此时，可将原ΔABC区域分割为一个三棱锥凸起，尖钩末端在原ΔABC区域内滑动时，变为与面a、b、c的接触。

在图4所示的接触模型中，尖钩末端与球形微凸体的接触角度为ΔBCD法向量与竖直方向的夹角。

通过上述分析可知，将初始点云数据进行三角形组合，利用已知点云数据的坐标信息，反应壁面轮廓的局部形貌，可以有效的判断尖钩末端与粗糙壁面的接触角度。但对于点云数据，我们只能获得单个点的坐标信息，无法读取相邻点的数据，因此必须根据尖钩末端半径的尺寸，结合点的坐标信息采取遍历的方式求解所有的可接触三角形组合，进而求解出尖钩与每个三角形的接触角度，然后结合最小可用角度θ_min进行是否可抓取的判断。

步骤3，获取插值后的点云样本：假设机器人用于抓取的尖钩半径为r，根据尖钩半径r对步骤2获取的初始点云数据进行插值操作，得到插值后的点云样本M₂₁[x_set,y_set,z_set]；插值后，应使得连续两个点云之间的间距小于2r。

步骤4，寻找可接触三角形的点集合：采用下述公式(3.4)与公式(3.5)对步骤3得到的插值后的点云样本M₂₁[x_set,y_set,z_set]采取遍历的方式，求得所有可接触三角形的三个点组合，并放入可接触三角形的点集合M₂₂[x_combine,y_combine,z_combine]中。

可接触三角形的判断：与尖钩截面和二维壁面轮廓接触不同的是，三维空间中，尖钩末端为半球体，代表三维壁面轮廓信息的为空间点云数据，因此，我们对可接触条件进行下列简化：若三个点可同时与尖钩末端半球体接触，则这三个点组成的三角形为可接触三角形。

我们以球体模型来对尖钩末端半球体与空间点云的接触进行分析。对于如图5所示的球体，在表面随机取七个点A、B、C、D、E、F、G。假设任意三点均不共线，则可以连成21条线段，所组成的三维空间接触多边形结构，可分解为35个三角形，同时可以发现，对于多个空间点与球体的所有接触结构，都可以分解为若干个三角形。以接触点A、B、C形成的ΔABC为例，分析对于指定半径的球体，可与其接触的三角形三个顶点需要满足三个约束条件。

约束条件1：ΔABC的三条边AB、AC、BC必须满足任意一条边的长度不大于球的直径2r，即：

式中，点A、点B和点C为其中一个可接触三角形ΔABC的三个点，其中，点A的三维坐标为(x_A,y_A,z_A),点B的三维坐标为(x_B,y_B,z_B),点C的三维坐标为(x_C,y_C,z_C)。

约束条件2：ΔABC的面积要小于等于球的内接三角形最大面积，对于半径为r的球体，可知当内接三角形所在平面经过球心时取得最大值，假设内接三角形的各边对应圆心角为α_o、β_o、γ₀，其中：α_o>0,β_o>0,γ_o>0，α_o+β_o+γ_o＝2π。则各个角对应的三角形面积之和S＝r²(sinα_o+sinβ_o+sinγ_o)/2，当α_o＝β_o＝γ_o＝2π/3时取得极值，此时

同时根据海伦公式，三角形面积

其中p＝(AB+AC+BC)/2，则可以建立起ΔABC三条边与球的半径r的关系式：

满足上述约束条件1和约束条件2后，对于较深的凹沟或一些细小的夹缝内的点可能无法接触，因此还有约束条件3，将在步骤5中进行阐述。

步骤5，确定所有的接触三角形。

将点集合M₂₂[x_combine,y_combine,z_combine]中的每一个组合均按照如下方法进行判断。

步骤51，求解尖钩球心坐标：以点集合M₂₂[x_combine,y_combine,z_combine]中当前组合的三个点坐标，也即点A、点B和点C的坐标以及尖钩半径r，求解对应的尖钩球心坐标(x,y,z)；求解时，采用Matlab中的solve函数，具体求解公式为：

syms x,y,z；

[x,y,z]＝solve(r²-(x-x_A)²-(y-y_A)²-(z-z_A)²,r²-(x-x_B)²-(y-y_B)²-(z-z_B)²,

r²-(x-x_C)²-(y-y_C)²-(z-z_C)²)

步骤52，尖钩球心坐标的确定：对步骤51求得的结果进行判断：若结果为实数且只有一个解时，则该实数解即为确定的尖钩球心坐标，进行步骤53；若结果为实数且有两个解时，取z轴坐标值较大的实数解，作为确定的尖钩球心坐标，进行步骤53；否则，跳转至步骤51，以点集合M₂₂[x_combine,y_combine,z_combine]中下一个组合进行尖钩球心坐标的求解，直至求得的结果为实数。

步骤53，接触三角形的确定：以步骤52确定的尖钩球心坐标(x,y,z)先对步骤4寻找的点集合M₂₂[x_combine,y_combine,z_combine]中的当前组合的三个点A、B、C按如下公式求取距离并判断；

式中，点坐标(x_i,y_i,z_i)为当前组合ΔABC内的任一点i。

当三个点A、B、C以及点坐标为(x_i,y_i,z_i)的点i与尖钩球心之间的间距均满足公式(3.6)的要求时，则判定当前组合ΔABC为接触三角形，并将当前三个点A、B、C以及对应的尖钩球心均放入M₂₃矩阵中；否则，对点集合M₂₂[x_combine,y_combine,z_combine]中的下一个组合按照公式(3.6)进行求距与判断，直至完成点集合M₂₂[x_combine,y_combine,z_combine]中的所有组合的求距与判断，将所有判断为接触三角形的三个点均放入M₂₃矩阵中。

因此，可接触三角形的三个顶点需满足的第三个约束条件，也即约束条件3为：接触点A、B、C与尖钩球心的距离均为尖钩半径，且尖钩球体内部不包含其它空间点：即对于空间内任一点(x_i,y_i,z_i)与尖钩球心的距离不小于尖钩半径r。

步骤6，计算接触三角形法向量：将M₂₃矩阵中的其中一个组成接触三角形的三个点A、B、C根据公式(3.7)求得接触三角形ΔABC的法向量

结合尖钩球心所在位置，使法向量

始终朝向尖钩球心。

由公式(3.7)求得的法向量朝向可能会出现不一致的情况，为了使法向量始终朝向尖钩球心，选取ΔABC上任一点，以顶点A为例，与尖钩球心O得到向量

若向量

与ΔABC法向量

夹角小于90°，则

否则

以此法向量与尖钩移动反方向求得夹角(本文中假设尖钩沿着y轴负方向滑动)，即为尖钩末端与该三角形的接触角度。

步骤7，计算接触角度θ：步骤6计算的法向量

与尖钩移动反方向之间所形成的夹角，即为接触角度θ。

步骤8，抓取条件判定：若π/2-θ>θ_min，则判断为满足抓取条件，满足抓取条件的三个点均为可抓取点，并将该组点放入M₂₄矩阵。

步骤9，获得所有可抓取点：对M₂₃矩阵中下一组合中的三个点进行步骤6至步骤8的判断，直到M₂₃矩阵中所有组合判断结束，得到所有可抓取点。

仿真实验

为了直观的查看可抓取点的位置和统计可抓取点数量，本发明采用MATLAB进行算法的仿真，CPU硬件平台为：Intel(R)Core(TM)i5-2450MCPU,2.5GHZ，4GB内存，抓取算法编程环境为MATLAB R2014a。

初始的点云数据，我们通过在空间随机生成数据点，然后应用Matlab散乱点插值函数griddata中的cubic方法进行插值得到。理论上插值后的数据点间距越小越好，但间距太小又会导致计算量太大的问题，这里参照末端尖钩设计尺寸(r>10μm)，选择插值间距为4μm，最终得到如图7、图8和图9所示的曲面。由于壁面轮廓以点云的方式呈现，数据点的z坐标变化代表了壁面高低的变化，但点云数据的x与y坐标并没有指定明确的信息，故在仿真过程中，我们假定尖钩的滑动方向均是沿着y轴负方向滑动。

由于算法的分析仅基于几何关系，因此只讨论尖钩尺寸r与θ_min对可抓取点数目的影响。在仿真结果的分析中，我们以可抓取点的数量作为衡量的标准而不是可抓取面积或者组成的可抓取三角形数目。这是因为相邻可抓取点组成的三角形可能会发生部分区域重合的现象，导致面积统计的不准确。另外，对于同时可抓取的n(n≥3)个点，能组成的可抓取三角形数目为：n(n-1)(n-2)/6，当某一小块区域内同时可抓取点的数目n较大时，会使统计结果无意义。

图7中组成壁面轮廓的点云共计1176个，图中黑色星号符号是可抓取点，曲面为点云数据拟合的壁面形貌，用不同的颜色区域表示壁面的高度。由仿真图可以看出，当尖钩末端沿着y轴负方向在壁面上滑动时，并不是所有的区域都能完成有效抓取：凸峰的下侧和凸峰上侧部分区域无法满足抓取角度的条件；对于壁面的凹坑底部，虽然有些区域满足角度的抓附条件，但凹坑无法容纳尖钩的尺寸，即不满足接触条件。通过对比仿真图a、b可知，当r值增大时，壁面的可抓取点数量发生明显的减小。对比两次仿真结果的M₂₄矩阵，r＝10μm、θ_min＝40°时，可抓取点的数量为104个；r＝20μm、θ_min＝40°时，可抓取点的数量为46个，这是因为对于给定壁面，当尖钩尺寸增大时，两个凸峰之间的区域以及凹坑部分无法容纳尖钩的尺寸，从而导致可接触区域减少，可抓取位置也发生相应的减少。对比图7中的a、c可发现，当θ_min增大时，可抓取点的数量明显减少，分析两次仿真结果的M₂₄矩阵发现，θ_min＝40°时可抓取点数目为104个，θ_min＝50°时可抓取点数目为73个。同时观察图7中的三个仿真结果，发现在y＝120μm处的边界区域存在多个不满足条件的可抓取点，这是由于在仿真过程中，我们假定尖钩的移动方向为沿着y轴负方向滑动，y＝120μm对应起始位置，此处约束条件3会部分失效。

图8中组成壁面轮廓的点云共计1436个。选取r＝10μm、θ_min＝40°与r＝20μm、θ_min＝40°以及r＝10μm、θ_min＝50°进行对比。图中黑色星号符号是可抓取点，曲面为点云数据拟合的壁面形貌，用不同的颜色区域表示壁面的高度。查看三次仿真结果的M₂₄矩阵发现：当r＝10μm、θ_min＝40°时，可抓取点数量为272个；r＝20μm、θ_min＝40°时可抓点数目减少为114个；r＝10μm、θ_min＝50°时可抓取点数目为191个。

图9中组成壁面轮廓的点云共计1176个。同样选取r＝10μm、θ_min＝40°与r＝20μm、θ_min＝40°以及r＝10μm、θ_min＝50°进行对比。图中黑色星号符号是可抓取点，曲面为点云数据拟合的壁面形貌，用不同的颜色区域表示壁面的高度。查看三次仿真结果的M₂₄矩阵发现：当r＝10μm、θ_min＝40°时，可抓取点数量为67个；r＝20μm、θ_min＝40°时可抓点数目减少为46个；r＝10μm、θ_min＝50°时可抓取点数目为56个。

经过上述仿真实验，验证了本发明三维壁面可抓取位置判别算法的可行性，通过总结三次仿真结果可以得出下列结论：对于点云描述的粗糙壁面，尖钩半径r越小，对应的可抓取点数目越多，最小可用角度θ_min越小，可抓取点数目越多，这要求我们在进行爬壁机器人设计时，需要合理的设计机器人本体尺寸，并且在材料强度允许的情况下，尽可能减小尖钩的尺寸。

以上详细描述了本发明的优选实施方式，但是，本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换，这些等同变换均属于本发明的保护范围。

Claims (2)

1.一种三维壁面可抓取位置判别算法，其特征在于：包括如下步骤：

步骤1，定义最小可用角度θ_min：假设θ_b代表尖钩的圆心轨迹法向与壁面法向之间的夹角，当θ_b≥θ_min时，尖钩能够完成钩附；其中，最小可用角度θ_min的计算公式如下：

θ_min＝θ_load+arccot(μ) (3.1)

式中，θ_load为尖钩的载荷角，μ为尖钩与壁面的摩擦系数；

步骤2，获取初始点云数据：采用粗糙表面测量仪器提取粗糙壁面的表面特征，对壁面轮廓进行测量，采集粗糙壁面特征点数据，通过系统采集整理，获得初始的三维点云数据M₂₁[x_set,y_set,z_set]；其中，x_set为壁面轮廓上点的x坐标值集合，y_set为与之对应的y坐标值集合，z_set为与之对应的z坐标值集合，x与y坐标表示粗糙壁面表平面，z坐标表示粗糙壁面凹凸起伏的变化；

步骤3，获取插值后的点云样本：假设机器人用于抓取的尖钩半径为r，根据尖钩半径r对步骤2获取的初始点云数据进行插值操作，得到插值后的点云样本M₂₁[x_set,y_set,z_set]；插值后，应使得连续两个点云之间的间距小于2r；

步骤4，寻找可接触三角形的点集合：采用下述公式(3.4)与公式(3.5)对步骤3得到的插值后的点云样本M₂₁[x_set,y_set,z_set]采取遍历的方式，求得所有可接触三角形的三个点组合，并放入可接触三角形的点集合M₂₂[x_combine,y_combine,z_combine]中；

公式(3.4)为：

式中，点A、点B和点C为其中一个可接触三角形△ABC的三个点，其中，点A的三维坐标为(x_A,y_A,z_A),点B的三维坐标为(x_B,y_B,z_B),点C的三维坐标为(x_C,y_C,z_C)；

公式(3.5)为：

其中，p＝(AB+AC+BC)/2

式中，AB、AC和BC分别为可接触三角形△ABC的三条边长；

步骤5，确定所有的接触三角形：将点集合M₂₂[x_combine,y_combine,z_combine]中的每一个组合均按照如下方法进行判断；

步骤51，求解尖钩球心坐标：以点集合M₂₂[x_combine,y_combine,z_combine]中当前组合的三个点坐标，也即点A、点B和点C的坐标以及尖钩半径r，求解对应的尖钩球心坐标(x,y,z)；求解时，采用Matlab中的solve函数，具体求解公式为：

syms x,y,z；

[x,y,z]＝solve(r²-(x-x_A)²-(y-y_A)²-(z-z_A)²,r²-(x-x_B)²-(y-y_B)²-(z-z_B)²,r²-(x-x_C)²-(y-y_C)²-(z-z_C)²)；

步骤52，尖钩球心坐标的确定：对步骤51求得的结果进行判断：若结果为实数且只有一个解时，则该实数解即为确定的尖钩球心坐标，进行步骤53；若结果为实数且有两个解时，取z轴坐标值较大的实数解，作为确定的尖钩球心坐标，进行步骤53；否则，跳转至步骤51，以点集合M₂₂[x_combine,y_combine,z_combine]中下一个组合进行尖钩球心坐标的求解，直至求得的结果为实数；

步骤53，接触三角形的确定：以步骤52确定的尖钩球心坐标(x,y,z)先对步骤4寻找的点集合M₂₂[x_combine,y_combine,z_combine]中的当前组合的三个点A、B、C按如下公式求取距离并判断；

式中，点坐标(x_i,y_i,z_i)为当前组合△ABC内的任一点i；

当三个点A、B、C以及点坐标为(x_i,y_i,z_i)的点i与尖钩球心之间的间距均满足公式(3.6)的要求时，则判定当前组合△ABC为接触三角形，并将当前三个点A、B、C以及对应的尖钩球心均放入M₂₃矩阵中；否则，对点集合M₂₂[x_combine,y_combine,z_combine]中的下一个组合按照公式(3.6)进行求距与判断，直至完成点集合M₂₂[x_combine,y_combine,z_combine]中的所有组合的求距与判断，将所有判断为接触三角形的三个点均放入M₂₃矩阵中；

步骤6，计算接触三角形法向量：将M₂₃矩阵中的其中一个组成接触三角形的三个点A、B、C根据公式(3.7)求得接触三角形△ABC的法向量

结合尖钩球心所在位置，使法向量

始终朝向尖钩球心；

步骤7，计算接触角度θ：步骤6计算的法向量

与尖钩移动反方向之间所形成的夹角，即为接触角度θ；

步骤8，抓取条件判定：若π/2-θ>θ_min，则判断为满足抓取条件，满足抓取条件的三个点均为可抓取点，并将该可抓取点放入M₂₄矩阵；

步骤9，获得所有可抓取点：对M₂₃矩阵中下一组合中的三个点进行步骤6至步骤8的判断，直到M₂₃矩阵中所有组合判断结束，得到所有可抓取点。

2.根据权利要求1所述的三维壁面可抓取位置判别算法，其特征在于：步骤3中，根据尖钩半径r对初始点云数据进行插值的具体操作方法为：从步骤2测量获得的初始点云样本数据M₂₁[x_set,y_set,z_set]中取连续两点(x_i,y_i,z_i),(x_i+1,y_i+1,z_i+1)，i＝1,2…n，若

成立，则表示相邻两点满足间距要求；若不成立，在相邻两点间插入一个数据点((x_i+x_i+1)/2,(y_i+y_i+1)/2,(z_i+z_i+1)/2)成为新的(x_i+1,y_i+1,z_i+1)，对新的(x_i+1,y_i+1,z_i+1)与(x_i,y_i,z_i)继续进行上述操作，直到满足

然后对下一组点重复这一操作，得到插值后的最终的点云样本M₂₁[x_set,y_set,z_set]。

Images