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

摘要

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

主权项

一种三维壁面可抓取位置判别算法，其特征在于：包括如下步骤：步骤1，定义最小可用角度θmin：假设θb代表尖钩的圆心轨迹法向与壁面法向之间的夹角，当θb≥θmin时，尖钩能够完成钩附；其中，最小可用角度θmin的计算公式如下：θmin＝θload+arc cot(μ)                (3.1)式中，θload为尖钩的载荷角，μ为尖钩与壁面的摩擦系数；步骤2，获取初始点云数据：采用粗糙表面测量仪器提取粗糙壁面的表面特征，对壁面轮廓进行测量，采集粗糙壁面特征点数据，通过系统采集整理，获得初始的三维点云数据M21[xset,yset,zset]；其中，xset为壁面轮廓上点的x坐标值集合，yset为与之对应的y坐标值集合，zset为与之对应的z坐标值集合，x与y坐标表示粗糙壁面表平面，z坐标表示粗糙壁面凹凸起伏的变化；步骤3，获取插值后的点云样本：假设机器人用于抓取的尖钩半径为r，根据尖钩半径r对步骤2获取的初始点云数据进行插值操作，得到插值后的点云样本M21[xset,yset,zset]；插值后，应使得连续两个点云之间的间距小于2r；步骤4，寻找可接触三角形的点集合：采用下述公式(3.4)与公式(3.5)对步骤3得到的插值后的点云样本M21[xset,yset,zset]采取遍历的方式，求得所有可接触三角形的三个点组合，并放入可接触三角形的点集合M22[xcombine,ycombine,zcombine]中；公式(3.4)为：(xA-xC)2+(yA-yC)2+(zA-zC)2≤4r2(xA-xB)2+(yA-yB)2+(zA-zB)2≤4r2(xB-xC)2+(yB-yC)2+(zB-zC)2≤4r2---(3.4)]]>式中，点A、点B和点C为其中一个可接触三角形ΔABC的三个点，其中，点A的三维坐标为(xA,yA,zA),点B的三维坐标为(xB,yB,zB),点C的三维坐标为(xC,yC,zC)；公式(3.5)为：0<p(p-AB)(p-AC)(p-BC)≤33r2/4---(3.5)]]>其中，p＝(AB+AC+BC)/2式中，AB、AC和BC分别为可接触三角形ΔABC的三条边长；步骤5，确定所有的接触三角形：将点集合M22[xcombine,ycombine,zcombine]中的每一个组合均按照如下方法进行判断；步骤51，求解尖钩球心坐标：以点集合M22[xcombine,ycombine,zcombine]中当前组合的三个点坐标，也即点A、点B和点C的坐标以及尖钩半径r，求解对应的尖钩球心坐标(x,y,z)；求解时，采用Matlab中的solve函数，具体求解公式为：syms x,y,z；[x,y,z]＝solve(r2‑(x‑xA)2‑(y‑yA)2‑(z‑zA)2,r2‑(x‑xB)2‑(y‑yB)2‑(z‑zB)2,r2‑(x‑xC)2‑(y‑yC)2‑(z‑zC)2)；步骤52，尖钩球心坐标的确定：对步骤51求得的结果进行判断：若结果为实数且只有一个解时，则该实数解即为确定的尖钩球心坐标，进行步骤53；若结果为实数且有两个解时，取z轴坐标值较大的实数解，作为确定的尖钩球心坐标，进行步骤53；否则，跳转至步骤51，以点集合M22[xcombine,ycombine,zcombine]中下一个组合进行尖钩球心坐标的求解，直至求得的结果为实数；步骤53，接触三角形的确定：以步骤52确定的尖钩球心坐标(x,y,z)先对步骤4寻找的点集合M22[xcombine,ycombine,zcombine]中的当前组合的三个点A、B、C按如下公式求取距离并判断；(x-xA)2+(y-yA)2+(z-zA)2=r(x-xB)2+(y-yB)2+(z-zB)2=r(x-xC)2+(y-yC)2+(z-zC)2=r(x-xi)2+(y-yi)2+(z-zi)2≥r---(3.6)]]>式中，点坐标(xi,yi,zi)为当前组合ΔABC内的任一点i；当三个点A、B、C以及点坐标为(xi,yi,zi)的点i与尖钩球心之间的间距均满足公式(3.6)的要求时，则判定当前组合ΔABC为接触三角形，并将当前三个点A、B、C以及对应的尖钩球心均放入M23矩阵中；否则，对点集合M22[xcombine,ycombine,zcombine]中的下一个组合按照公式(3.6)进行求距与判断，直至完成点集合M22[xcombine,ycombine,zcombine]中的所有组合的求距与判断，将所有判断为接触三角形的三个点均放入M23矩阵中；步骤6，计算接触三角形法向量：将M23矩阵中的其中一个组成接触三角形的三个点A、B、C根据公式(3.7)求得接触三角形ΔABC的法向量结合尖钩球心所在位置，使法向量始终朝向尖钩球心；n→=((xB,yB,zB)-(xA,yA,zA))×((xC,yC,zC)-(xA,yA,zA))||(xB,yB,zB)-(xA,yA,zA))×((xC,yC,zC)-(xA,yA,zA)||---(3.7)]]>步骤7，计算接触角度θ：步骤6计算的法向量与尖钩移动反方向之间所形成的夹角，即为接触角度θ；步骤8，抓取条件判定：若π/2‑θ>θmin，则判断为满足抓取条件，满足抓取条件的三个点均为可抓取点，并将该组点放入M24矩阵；步骤9，获得所有可抓取点：对M23矩阵中下一组合中的三个点进行步骤6至步骤8的判断，直到M23矩阵中所有组合判断结束，得到所有可抓取点。