[發(fā)明專利]三維壁面可抓取位置判別算法

有效

• 申請?zhí)枺?/td>: 201711068221.2
• 申請日: 2017-11-03
• 公開（公告）號: CN107908841B
• 公開（公告）日: 2020-10-20
• 發(fā)明（設(shè)計）人: 徐豐羽;孟凡昌;王貝;蔣國平
• 申請（專利權(quán)）人: 南京郵電大學(xué)
• 主分類號: G06F30/20
• 分類號: G06F30/20;G06F30/10;G06T7/00;G06T3/40
• 代理公司: 南京經(jīng)緯專利商標(biāo)代理有限公司 32200
• 代理人: 石艷紅

1.一種三維壁面可抓取位置判別算法，其特征在于：包括如下步驟：

步驟1，定義最小可用角度θ_min：假設(shè)θ_b代表尖鉤的圓心軌跡法向與壁面法向之間的夾角，當(dāng)θ_b≥θ_min時，尖鉤能夠完成鉤附；其中，最小可用角度θ_min的計算公式如下：

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

式中，θ_load為尖鉤的載荷角，μ為尖鉤與壁面的摩擦系數(shù)；

步驟2，獲取初始點(diǎn)云數(shù)據(jù)：采用粗糙表面測量儀器提取粗糙壁面的表面特征，對壁面輪廓進(jìn)行測量，采集粗糙壁面特征點(diǎn)數(shù)據(jù)，通過系統(tǒng)采集整理，獲得初始的三維點(diǎn)云數(shù)據(jù)M₂₁[x_set,y_set,z_set]；其中，x_set為壁面輪廓上點(diǎn)的x坐標(biāo)值集合，y_set為與之對應(yīng)的y坐標(biāo)值集合，z_set為與之對應(yīng)的z坐標(biāo)值集合，x與y坐標(biāo)表示粗糙壁面表平面，z坐標(biāo)表示粗糙壁面凹凸起伏的變化；

步驟3，獲取插值后的點(diǎn)云樣本：假設(shè)機(jī)器人用于抓取的尖鉤半徑為r，根據(jù)尖鉤半徑r對步驟2獲取的初始點(diǎn)云數(shù)據(jù)進(jìn)行插值操作，得到插值后的點(diǎn)云樣本M₂₁[x_set,y_set,z_set]；插值后，應(yīng)使得連續(xù)兩個點(diǎn)云之間的間距小于2r；

步驟4，尋找可接觸三角形的點(diǎn)集合：采用下述公式(3.4)與公式(3.5)對步驟3得到的插值后的點(diǎn)云樣本M₂₁[x_set,y_set,z_set]采取遍歷的方式，求得所有可接觸三角形的三個點(diǎn)組合，并放入可接觸三角形的點(diǎn)集合M₂₂[x_combine,y_combine,z_combine]中；

公式(3.4)為：

式中，點(diǎn)A、點(diǎn)B和點(diǎn)C為其中一個可接觸三角形△ABC的三個點(diǎn)，其中，點(diǎn)A的三維坐標(biāo)為(x_A,y_A,z_A),點(diǎn)B的三維坐標(biāo)為(x_B,y_B,z_B),點(diǎn)C的三維坐標(biāo)為(x_C,y_C,z_C)；

公式(3.5)為：

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

式中，AB、AC和BC分別為可接觸三角形△ABC的三條邊長；

步驟5，確定所有的接觸三角形：將點(diǎn)集合M₂₂[x_combine,y_combine,z_combine]中的每一個組合均按照如下方法進(jìn)行判斷；

步驟51，求解尖鉤球心坐標(biāo)：以點(diǎn)集合M₂₂[x_combine,y_combine,z_combine]中當(dāng)前組合的三個點(diǎn)坐標(biāo)，也即點(diǎn)A、點(diǎn)B和點(diǎn)C的坐標(biāo)以及尖鉤半徑r，求解對應(yīng)的尖鉤球心坐標(biāo)(x,y,z)；求解時，采用Matlab中的solve函數(shù)，具體求解公式為：

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，尖鉤球心坐標(biāo)的確定：對步驟51求得的結(jié)果進(jìn)行判斷：若結(jié)果為實數(shù)且只有一個解時，則該實數(shù)解即為確定的尖鉤球心坐標(biāo)，進(jìn)行步驟53；若結(jié)果為實數(shù)且有兩個解時，取z軸坐標(biāo)值較大的實數(shù)解，作為確定的尖鉤球心坐標(biāo)，進(jìn)行步驟53；否則，跳轉(zhuǎn)至步驟51，以點(diǎn)集合M₂₂[x_combine,y_combine,z_combine]中下一個組合進(jìn)行尖鉤球心坐標(biāo)的求解，直至求得的結(jié)果為實數(shù)；

步驟53，接觸三角形的確定：以步驟52確定的尖鉤球心坐標(biāo)(x,y,z)先對步驟4尋找的點(diǎn)集合M₂₂[x_combine,y_combine,z_combine]中的當(dāng)前組合的三個點(diǎn)A、B、C按如下公式求取距離并判斷；

式中，點(diǎn)坐標(biāo)(x_i,y_i,z_i)為當(dāng)前組合△ABC內(nèi)的任一點(diǎn)i；

當(dāng)三個點(diǎn)A、B、C以及點(diǎn)坐標(biāo)為(x_i,y_i,z_i)的點(diǎn)i與尖鉤球心之間的間距均滿足公式(3.6)的要求時，則判定當(dāng)前組合△ABC為接觸三角形，并將當(dāng)前三個點(diǎn)A、B、C以及對應(yīng)的尖鉤球心均放入M₂₃矩陣中；否則，對點(diǎn)集合M₂₂[x_combine,y_combine,z_combine]中的下一個組合按照公式(3.6)進(jìn)行求距與判斷，直至完成點(diǎn)集合M₂₂[x_combine,y_combine,z_combine]中的所有組合的求距與判斷，將所有判斷為接觸三角形的三個點(diǎn)均放入M₂₃矩陣中；

步驟6，計算接觸三角形法向量：將M₂₃矩陣中的其中一個組成接觸三角形的三個點(diǎn)A、B、C根據(jù)公式(3.7)求得接觸三角形△ABC的法向量結(jié)合尖鉤球心所在位置，使法向量始終朝向尖鉤球心；

步驟7，計算接觸角度θ：步驟6計算的法向量與尖鉤移動反方向之間所形成的夾角，即為接觸角度θ；

步驟8，抓取條件判定：若π/2-θθ_min，則判斷為滿足抓取條件，滿足抓取條件的三個點(diǎn)均為可抓取點(diǎn)，并將該可抓取點(diǎn)放入M₂₄矩陣；

步驟9，獲得所有可抓取點(diǎn)：對M₂₃矩陣中下一組合中的三個點(diǎn)進(jìn)行步驟6至步驟8的判斷，直到M₂₃矩陣中所有組合判斷結(jié)束，得到所有可抓取點(diǎn)。

2.根據(jù)權(quán)利要求1所述的三維壁面可抓取位置判別算法，其特征在于：步驟3中，根據(jù)尖鉤半徑r對初始點(diǎn)云數(shù)據(jù)進(jìn)行插值的具體操作方法為：從步驟2測量獲得的初始點(diǎn)云樣本數(shù)據(jù)M₂₁[x_set,y_set,z_set]中取連續(xù)兩點(diǎn)(x_i,y_i,z_i),(x_i+1,y_i+1,z_i+1)，i＝1,2…n，若成立，則表示相鄰兩點(diǎn)滿足間距要求；若不成立，在相鄰兩點(diǎn)間插入一個數(shù)據(jù)點(diǎ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)繼續(xù)進(jìn)行上述操作，直到滿足然后對下一組點(diǎn)重復(fù)這一操作，得到插值后的最終的點(diǎn)云樣本M₂₁[x_set,y_set,z_set]。