[发明专利]三维壁面可抓取位置判别算法

权利要求书

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]。