本發(fā)明涉及一種被機(jī)械手夾取的物體的姿態(tài)變化檢測(cè)裝置及方法，涉及機(jī)器人操控領(lǐng)域。

背景技術(shù)：

機(jī)器人在非結(jié)構(gòu)環(huán)境中實(shí)現(xiàn)對(duì)物體的穩(wěn)定、靈活?yuàn)A取，是機(jī)器人技術(shù)發(fā)展中最具挑戰(zhàn)的研究領(lǐng)域之一。在實(shí)際夾取操作中，被夾取物體不可避免會(huì)出現(xiàn)傾斜、滑移、旋轉(zhuǎn)等姿態(tài)變化等情況，如沿x、y、z軸的平移和繞三軸的轉(zhuǎn)角變化等。如果能對(duì)被夾取物體的姿態(tài)變化進(jìn)行實(shí)時(shí)定量檢測(cè)并反饋給機(jī)械手，機(jī)械手控制器會(huì)根據(jù)這些實(shí)時(shí)數(shù)據(jù)做出相應(yīng)的操作，則可大大提高夾取的穩(wěn)定性和操作的靈活性。

目前，對(duì)被夾取物體的姿態(tài)變化檢測(cè)方法主要有兩類(lèi)：一是采用視覺(jué)方法，即通過(guò)在機(jī)械手外部布置相機(jī)來(lái)監(jiān)測(cè)被夾取物體的姿態(tài)變化，這種方法缺點(diǎn)是相機(jī)光路易被工件或機(jī)械手遮擋，影響適用范圍，另外，檢測(cè)效果對(duì)環(huán)境照明也比較敏感；二是采用觸覺(jué)傳感器方法，即通過(guò)機(jī)械手指與被抓物體的觸覺(jué)傳感信息來(lái)判斷被夾取物體的姿態(tài)變化，這種方法盡管可以克服視覺(jué)方法的缺點(diǎn)，其缺點(diǎn)是只能定性的檢測(cè)，只能檢測(cè)被夾取物體姿態(tài)變化是平移、旋轉(zhuǎn)還是翻滾，無(wú)法對(duì)姿態(tài)變化進(jìn)行定量測(cè)量。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是為了克服現(xiàn)有技術(shù)的不足，提出一種被機(jī)械手夾取的物體的姿態(tài)變化檢測(cè)裝置及方法，本方法通過(guò)嵌入到機(jī)械手指內(nèi)的三個(gè)滑覺(jué)傳感器來(lái)實(shí)現(xiàn)，其原理是首先采用滑覺(jué)傳感器檢測(cè)姿態(tài)變化前后物體表面滑動(dòng)位移量大小及方向，然后通過(guò)幾何空間變換得到被夾取物體沿x、y、z方向的平移距離和繞x、y、z軸的轉(zhuǎn)角變化，從而實(shí)現(xiàn)物體姿態(tài)變化的定量檢測(cè)。檢測(cè)方法可以進(jìn)一步用于被夾取物體連續(xù)軌跡監(jiān)測(cè)和跟蹤。

為了實(shí)現(xiàn)上述目的，本發(fā)明采用如下技術(shù)方案：

一種被機(jī)械手夾取的物體的姿態(tài)變化檢測(cè)裝置，包括：機(jī)械手夾取系統(tǒng)，所述機(jī)械手夾取系統(tǒng)包括機(jī)械手和兩個(gè)機(jī)械手指；所述機(jī)械手為橫桿，兩個(gè)機(jī)械手指均為與橫桿垂直的豎桿，兩個(gè)機(jī)械手指分別安裝在橫桿的兩端；所述兩個(gè)機(jī)械手指用于夾持物體；

其中一個(gè)機(jī)械手指上內(nèi)嵌第一滑覺(jué)傳感器sensor1和第二滑覺(jué)傳感器sensor2，另外一個(gè)機(jī)械手指上內(nèi)嵌第三滑覺(jué)傳感器sensor3；三個(gè)滑覺(jué)傳感器均為全方向性滑覺(jué)傳感器，每個(gè)滑覺(jué)傳感器均用于檢測(cè)滑動(dòng)位移量和滑動(dòng)方向；

每個(gè)滑覺(jué)傳感器均通過(guò)對(duì)應(yīng)的a/d轉(zhuǎn)換器與存儲(chǔ)器連接，所述存儲(chǔ)器與dsp連接，所述dsp通過(guò)usb接口與控制計(jì)算機(jī)連接。

一種被機(jī)械手夾取的物體的姿態(tài)變化檢測(cè)方法，包括如下步驟：

步驟(1)：建立機(jī)械手坐標(biāo)系o-xyz；建立被夾取物體坐標(biāo)系o-xyz；將三個(gè)滑覺(jué)傳感器分別安裝到機(jī)械手指中；

步驟(2)：機(jī)械手上的機(jī)械手指夾取物體，每個(gè)滑覺(jué)傳感器采集被夾取物體的坐標(biāo)值；

在當(dāng)前狀態(tài)state(n-1)下，第i個(gè)滑覺(jué)傳感器輸出的坐標(biāo)值為：其中，i＝1,2...,3，對(duì)應(yīng)于sensor1,sensor2,sensor3三個(gè)滑覺(jué)傳感器；

在被夾取物體在機(jī)械手指上產(chǎn)生滑動(dòng)之后的狀態(tài)state(n)下，第i個(gè)滑覺(jué)傳感器輸出的坐標(biāo)值為：

步驟(3)：將sensor1,sensor2,sensor3三個(gè)滑覺(jué)傳感器檢測(cè)輸出的坐標(biāo)值表示為點(diǎn)集p＝{p1,p2,p3}，則

對(duì)應(yīng)于state(n-1)狀態(tài)，sensor1,sensor2,sensor3三個(gè)滑覺(jué)傳感器檢測(cè)輸出的點(diǎn)集p^state(n-1)表示為：

其中，表示state(n-1)狀態(tài)下第一滑覺(jué)傳感器sensor1檢測(cè)輸出的坐標(biāo)值；

表示state(n-1)狀態(tài)下第二滑覺(jué)傳感器sensor2檢測(cè)輸出的坐標(biāo)值；

表示state(n-1)狀態(tài)下第三滑覺(jué)傳感器sensor3檢測(cè)輸出的坐標(biāo)值；

對(duì)應(yīng)于state(n)狀態(tài)，sensor1,sensor2,sensor3三個(gè)傳感器檢測(cè)輸出的點(diǎn)集p^state(n)表示為：

其中，表示state(n)狀態(tài)下第一滑覺(jué)傳感器sensor1檢測(cè)輸出的坐標(biāo)值；

表示state(n)狀態(tài)下第二滑覺(jué)傳感器sensor2檢測(cè)輸出的坐標(biāo)值；

表示state(n)狀態(tài)下第三滑覺(jué)傳感器sensor3檢測(cè)輸出的坐標(biāo)值；

則，兩個(gè)點(diǎn)集p^state(n-1)與p^state(n)之間的關(guān)系式表示為：

p^state(n)＝r·p^state(n-1)+t，(1)；

其中，是兩個(gè)點(diǎn)集之間的旋轉(zhuǎn)矩陣；t＝[t11t12t13]^t是兩個(gè)點(diǎn)集之間的平移矩陣；旋轉(zhuǎn)矩陣包含著被測(cè)物體由狀態(tài)state(n-1)到state(n)時(shí)的旋轉(zhuǎn)信息；平移矩陣包含著被測(cè)物體由狀態(tài)state(n-1)到state(n)時(shí)的平移信息；

步驟(4)：基于交叉協(xié)方差矩陣的奇異值分解方法求解旋轉(zhuǎn)矩陣r和平移矩陣t，分別由旋轉(zhuǎn)矩陣r和平移矩陣t計(jì)算得到被夾取物體在x、y、z軸方向的直線(xiàn)位移δx、δy、δz及繞三個(gè)坐標(biāo)軸的轉(zhuǎn)角θx、θy、θz；從而實(shí)現(xiàn)物體的姿態(tài)變化的檢測(cè)；

δx＝t11，(2)；

δy＝t12，(3)；

δz＝t13，(4)；

θx＝atan2(r32,r33)，(5)；

θz＝atan2(r21,r11)，(7)；

其中，atan2()是反正切函數(shù)。

所述步驟(4)基于交叉協(xié)方差矩陣的奇異值分解方法求解旋轉(zhuǎn)矩陣r和平移矩陣t的步驟為：

步驟(401)：

定義旋轉(zhuǎn)向量：其中，q0＞0，且，

定義平移向量：

由和構(gòu)建完整的狀態(tài)向量：

步驟(402)：構(gòu)建目標(biāo)方程通過(guò)迭代，使趨向于0，

步驟(403)：計(jì)算點(diǎn)集p^state(n-1)的質(zhì)心計(jì)算點(diǎn)集p^state(n)的質(zhì)心

則，點(diǎn)集p^state(n-1)和p^state(n)的交叉協(xié)方差矩陣∑p表示為：

用交叉協(xié)方差矩陣∑p計(jì)算最佳旋轉(zhuǎn)矩陣，并計(jì)算最佳平移向量，步驟為：

構(gòu)建反對(duì)稱(chēng)矩陣并用反對(duì)稱(chēng)矩陣的循環(huán)分量構(gòu)成列向量：δ＝[a23a31a12]^t，用列向量δ＝[a23a31a12]^t進(jìn)一步構(gòu)造4*4對(duì)稱(chēng)矩陣q(∑p)：

其中，i3是3*3單位矩陣，對(duì)應(yīng)于矩陣q(∑p)最大特征值的特征向量為旋轉(zhuǎn)向量利用旋轉(zhuǎn)向量的分量得到旋轉(zhuǎn)矩陣r，

計(jì)算平移向量

計(jì)算平移矩陣t：

步驟(5)：檢測(cè)被夾取物體的多姿態(tài)連續(xù)變化情況，利用式(1)，使n＝1，2，……，得到被夾取物體的連續(xù)軌跡變化，實(shí)現(xiàn)對(duì)被夾取物體的監(jiān)測(cè)和跟蹤。

優(yōu)選的，所述步驟(1)中，建立機(jī)械手坐標(biāo)系o-xyz的步驟為：

以與豎桿的豎直方向中心線(xiàn)平行的方向?yàn)閥軸，以與橫桿和y軸均垂直的方向?yàn)閤軸，以與x軸和y軸均垂直的方向?yàn)閦軸；

優(yōu)選的，所述步驟(1)中，建立被夾取物體坐標(biāo)系o-xyz的步驟為：

以被夾持物體的縱切面上中心軸線(xiàn)為x軸，以被夾持物體的橫切面上與z軸平行的線(xiàn)z軸，以被夾取物體的縱切面上與x軸和z軸均垂直的線(xiàn)為y軸；

優(yōu)選的，所述步驟(1)中，將三個(gè)滑覺(jué)傳感器分別安裝到機(jī)械手指中的位置：

在y軸方向上，三個(gè)滑覺(jué)傳感器組成的三角形應(yīng)與o-xz面平行；

第三滑覺(jué)傳感器sensor3在x軸方向上位于機(jī)械手指的中心；

第一滑覺(jué)傳感器sensor1和第二滑覺(jué)傳感器sensor2在x方向上關(guān)于機(jī)械手指豎直中心線(xiàn)對(duì)稱(chēng)分布。

本發(fā)明的有益效果：

1.本發(fā)明可以定量地檢測(cè)被夾取物體的姿態(tài)變化量，包括平移距離、旋轉(zhuǎn)角度等，利用這些檢測(cè)量，可以容易地實(shí)現(xiàn)對(duì)被夾取物體的連續(xù)軌跡監(jiān)測(cè)和跟蹤。

2.本發(fā)明通過(guò)嵌入在機(jī)械手指內(nèi)部的滑覺(jué)傳感器來(lái)實(shí)現(xiàn)，結(jié)構(gòu)簡(jiǎn)單，不受外部環(huán)境影響，抗干擾能力強(qiáng)。

附圖說(shuō)明

圖1為本發(fā)明檢測(cè)系統(tǒng)的組成；

圖2(a)為滑覺(jué)傳感器sensor1的檢測(cè)原理示意圖；

圖2(b)為滑覺(jué)傳感器sensor2的檢測(cè)原理示意圖；

圖2(c)為滑覺(jué)傳感器sensor3的檢測(cè)原理示意圖；

圖3為三個(gè)滑覺(jué)傳感器與機(jī)械手控制器的電氣連接關(guān)系圖；

其中，1、機(jī)械手，2、機(jī)械手指，3、滑覺(jué)傳感器，4、被夾取物體。

具體實(shí)施方式

下面結(jié)合附圖與實(shí)施例對(duì)本發(fā)明作進(jìn)一步說(shuō)明。

如圖1所示，本發(fā)明設(shè)計(jì)的檢測(cè)系統(tǒng)由機(jī)械手1、機(jī)械手指2、三個(gè)滑覺(jué)傳感器3以及被夾取物體4組成。其中，機(jī)械手1和機(jī)械手指2組成機(jī)械手夾取系統(tǒng)；滑覺(jué)傳感器3為三個(gè)全方向性滑覺(jué)傳感器，該傳感器既能檢測(cè)滑動(dòng)位移量大小，又能檢測(cè)滑動(dòng)方向。如圖2(a)、圖2(b)和圖2(c)所示是滑覺(jué)傳感器sensor1,sensor2,sensor3的檢測(cè)原理示意圖，其中state(n-1)表示當(dāng)前狀態(tài)下三個(gè)傳感器分別檢測(cè)到的坐標(biāo)；state(n)表示當(dāng)被夾取物體和機(jī)械手指表面發(fā)生滑動(dòng)后，各個(gè)傳感器檢測(cè)到的坐標(biāo)變化情況。本發(fā)明中，三個(gè)滑覺(jué)傳感器(如圖1)安裝如下原則布置：

①在y軸方向上，三個(gè)傳感器組成的三角形應(yīng)與o-xz面平行；

②第三滑覺(jué)傳感器sensor3在x軸方向上位于機(jī)械手指的中心；

③第一滑覺(jué)傳感器sensor1和第二滑覺(jué)傳感器sensor2在x方向上關(guān)于機(jī)械手指中心線(xiàn)對(duì)稱(chēng)分布。

如圖1所示，本發(fā)明中用被夾取物體在x、y、z軸方向的直線(xiàn)位移δx、δy、δz，以及繞三個(gè)坐標(biāo)軸的轉(zhuǎn)角θx、θy、θz來(lái)描述被夾取物體由狀態(tài)state(n-1)(圖1中的被夾取物體4)到狀態(tài)state(n)(圖1中虛線(xiàn)所示)的姿態(tài)變化，本發(fā)明的主要任務(wù)是要實(shí)現(xiàn)這幾個(gè)參數(shù)的檢測(cè)。

為了實(shí)現(xiàn)上述各參數(shù)的檢測(cè)，本發(fā)明的實(shí)現(xiàn)原理如下：

①如圖2(a)-圖2(c)所示，假設(shè)在狀態(tài)state(n-1)每個(gè)滑覺(jué)傳感器檢測(cè)輸出的當(dāng)前坐標(biāo)值分別為:當(dāng)被夾取物體和機(jī)械手指產(chǎn)生滑動(dòng)后(對(duì)應(yīng)于狀態(tài)state(n))，各滑覺(jué)傳感器檢測(cè)輸出的當(dāng)前坐標(biāo)值表示為:其中，i＝1,2...,m，此處，m＝3，對(duì)應(yīng)于sensor1,sensor2,sensor3三個(gè)滑覺(jué)傳感器。

②將三個(gè)傳感器檢測(cè)輸出的當(dāng)前坐標(biāo)值表示為點(diǎn)集p＝{p0,p1,p2}，則對(duì)應(yīng)于state(n-1)和state(n)兩個(gè)狀態(tài)，三個(gè)傳感器檢測(cè)輸出的點(diǎn)集分別表示為：

及

則，兩個(gè)點(diǎn)集之間的關(guān)系式表示為：

p^state(n)＝r·p^state(n-1)+t，(1)；

其中，矩陣和t＝[t11t12t13]^t分別是兩個(gè)點(diǎn)集之間的旋轉(zhuǎn)矩陣和平移矩陣。這兩個(gè)矩陣包含著被測(cè)物體由狀態(tài)state(n-1)到state(n)時(shí)的旋轉(zhuǎn)信息和平移信息。

③分別由矩陣t和r計(jì)算得到描述物體姿態(tài)變化的各個(gè)參數(shù)δx、δy、δz及θx、θy、θz。具體為：

δx＝t11，(2)；

δy＝t12，(3)；

δz＝t13，(4)；

θx＝atan2(r32,r33)，(5)；

θz＝atan2(r21,r11)，(7)；

其中，atan2()是反正切函數(shù)。

由上可見(jiàn)，為了求解描述姿態(tài)變化的各個(gè)參數(shù)，需要根據(jù)點(diǎn)集p^state(n-1)和p^state(n)求解旋轉(zhuǎn)矩陣r和平移矩陣t，本發(fā)明中，基于交叉協(xié)方差矩陣的奇異值分解(svd)方法，被用于這兩個(gè)矩陣的計(jì)算。具體步驟為：

其求解步驟如下：

①定義旋轉(zhuǎn)向量：其中，q0＞0，且，

定義平移向量：由和構(gòu)建完整的狀態(tài)向量：

②構(gòu)建目標(biāo)方程通過(guò)迭代，使其趨向于0，

③計(jì)算點(diǎn)集p^state(n-1)的質(zhì)心，

計(jì)算點(diǎn)集p^state(n)的質(zhì)心，

式中，m＝3。

則，點(diǎn)集p^state(n-1)和p^state(n)的交叉協(xié)方差矩陣表示為：

(4)用交叉協(xié)方差矩陣∑p計(jì)算最佳旋轉(zhuǎn)矩陣，并計(jì)算最佳平移向量，步驟為：

構(gòu)建反對(duì)稱(chēng)矩陣并用其循環(huán)分量構(gòu)成列向量：δ＝[a23a31a12]^t,用此列向量進(jìn)一步構(gòu)造4*4對(duì)稱(chēng)矩陣：

其中，i3是3*3單位矩陣，對(duì)應(yīng)于矩陣q(∑p)最大特征值的特征向量即為旋轉(zhuǎn)向量將其分量帶入下式就可以得到旋轉(zhuǎn)矩陣r，

進(jìn)一步計(jì)算平移向量，

則，平移矩陣通過(guò)下式計(jì)算；

按照以上步驟分別計(jì)算得到平移矩陣t和旋轉(zhuǎn)矩陣r，利用式(2)-(7)分別計(jì)算得到被夾取物體在x、y、z軸方向的直線(xiàn)位移δx、δy、δz，以及繞三個(gè)坐標(biāo)軸的轉(zhuǎn)角θx、θy、θz，從而實(shí)現(xiàn)物體的姿態(tài)變化的檢測(cè)。

上述步驟給出了被夾取物體兩個(gè)相鄰狀態(tài)，從state(n-1)到state(n)姿態(tài)變化的檢測(cè)方法，如需要檢測(cè)被夾取物體的多姿態(tài)連續(xù)變化情況，可利用式(1)，使n＝1,2...,...，即可以得到被夾取物體的連續(xù)軌跡變化，實(shí)現(xiàn)對(duì)被夾取物體的監(jiān)測(cè)和跟蹤。

三個(gè)滑覺(jué)傳感器sensor1,sensor2,sensor3與機(jī)械手控制器的電氣連接關(guān)系如圖3所示。三個(gè)滑覺(jué)傳感器輸出的電信號(hào)分別經(jīng)過(guò)a/d轉(zhuǎn)換器變?yōu)閿?shù)字信號(hào)之后存儲(chǔ)在存儲(chǔ)器中，dps處理器從存儲(chǔ)器中分別讀取三個(gè)滑覺(jué)傳感器的輸入信號(hào)并生成當(dāng)前坐標(biāo)值。當(dāng)前坐標(biāo)值的生成方法如下：

①以機(jī)械手初次夾取物體時(shí)三個(gè)傳感器的輸入信號(hào)對(duì)應(yīng)的坐標(biāo)值為初始坐標(biāo)值，分別記為其中，c是滑覺(jué)傳感器sensor1和sensor2安裝中心之間沿x方向的距離；d是兩個(gè)機(jī)械手指(豎桿)沿z方向的距離。并將該初始坐標(biāo)值存儲(chǔ)于存儲(chǔ)器中；

②以初始坐標(biāo)值為基礎(chǔ)，根據(jù)全方向性滑覺(jué)傳感器輸出的滑動(dòng)位移量大小和滑動(dòng)方向，通過(guò)坐標(biāo)計(jì)算得到狀態(tài)state(n)下的三個(gè)傳感器的當(dāng)前坐標(biāo)值。

當(dāng)前坐標(biāo)值經(jīng)usb通訊模塊發(fā)送到控制計(jì)算機(jī)，再由控制計(jì)算機(jī)通過(guò)計(jì)算完成被夾取物體的姿態(tài)變化檢測(cè)。

上述雖然結(jié)合附圖對(duì)本發(fā)明的具體實(shí)施方式進(jìn)行了描述，但并非對(duì)本發(fā)明保護(hù)范圍的限制，所屬領(lǐng)域技術(shù)人員應(yīng)該明白，在本發(fā)明的技術(shù)方案的基礎(chǔ)上，本領(lǐng)域技術(shù)人員不需要付出創(chuàng)造性勞動(dòng)即可做出的各種修改或變形仍在本發(fā)明的保護(hù)范圍以?xún)?nèi)。