雙軸孔裝配系統(tǒng)及其控制方法與流程

本發(fā)明涉及大型多軸孔部件裝配領(lǐng)域，尤其涉及一種雙軸孔裝配系統(tǒng)及其控制方法。

背景技術(shù)：

在產(chǎn)品生產(chǎn)制造過(guò)程中，裝配作業(yè)是非常重要的環(huán)節(jié)，該環(huán)節(jié)將直接影響最終產(chǎn)品質(zhì)量。據(jù)統(tǒng)計(jì)，在機(jī)械電子類產(chǎn)品的整個(gè)制造工作中，裝配工作量占到20％～70％，裝配費(fèi)用也占到了總成本的1/3～1/2。目前工業(yè)中的裝配環(huán)節(jié)很多還是由裝配工人完成，但是人工裝配存在很多問(wèn)題，例如效率低，成本高，工人操作要求高，而且還容易發(fā)生安全事故。尤其對(duì)于大型工件的裝配，工件太重，人工裝配很不方便。在這樣的背景下，能夠進(jìn)行自動(dòng)裝配的機(jī)器人便顯得尤為重要。相比于人工裝配，機(jī)器人適用的范圍更廣，尤其適用于重型工件等特殊環(huán)境下的裝配。

通過(guò)對(duì)已公開(kāi)的文獻(xiàn)、專利和工業(yè)產(chǎn)品調(diào)研發(fā)現(xiàn)，機(jī)器人裝配主要可以采用兩種方式，一種是基于視覺(jué)伺服的裝配方式，視覺(jué)伺服是通過(guò)采集圖像并進(jìn)行對(duì)比，從而判斷此時(shí)工件的位姿，并將判斷結(jié)果反饋給機(jī)械臂進(jìn)行調(diào)整的一種方法。然而視覺(jué)伺服控制機(jī)械臂進(jìn)行自動(dòng)裝配的方法還存在以下不足：(1)視覺(jué)伺服控制無(wú)法精確地控制裝配工件的接觸力大小，有可能發(fā)生嚴(yán)重的磕碰以致?lián)p傷工件；(2)存在局部遮擋和特征點(diǎn)不明顯時(shí)，造成位姿判斷出錯(cuò)或者無(wú)法判斷的問(wèn)題。

另外一種裝配方式是基于力覺(jué)的裝配，也成為機(jī)器人的力控制。力控制可以分為被動(dòng)力控制和主動(dòng)力控制。被動(dòng)力控制是設(shè)計(jì)柔順的末端關(guān)節(jié)，幫助工件在位置不完全準(zhǔn)確的情況下完成裝配。主動(dòng)力控制則是利用力傳感器對(duì)機(jī)器人末端受力進(jìn)行實(shí)時(shí)的測(cè)量，通過(guò)比較參考力與真實(shí)力，判斷當(dāng)前的接觸情況，控制機(jī)械臂運(yùn)用以減小接觸力，從而可以更好地完成裝配。然而 現(xiàn)有的力控制裝配方法還有如下幾個(gè)問(wèn)題：(1)被動(dòng)力控制方法柔順程度有限，且針對(duì)不同的工件需要設(shè)計(jì)不同的柔順機(jī)械裝置，適用程度有限。且因?yàn)槿嵝匝b置自由空間太大，不方便精確地控制；(2)現(xiàn)有的主動(dòng)力控制裝配方法主要適用于單軸孔裝配，很少適用于多軸孔裝配，而且沒(méi)有發(fā)現(xiàn)適用于柔性多軸孔裝配的方法。對(duì)于大型工件，例如飛機(jī)，汽車等工件，其體積大質(zhì)量高，本身會(huì)因?yàn)橹亓Φ茸饔迷斐勺冃?，從而不能完全視為剛性體，且這些大型工件的裝配主要為多軸孔裝配，因此需要對(duì)柔性多軸孔的裝配方法進(jìn)行研究。

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

鑒于背景技術(shù)中存在的問(wèn)題，本發(fā)明的目的在于提供一種雙軸孔裝配系統(tǒng)及其控制方法，其適用范圍廣，裝配精度高，穩(wěn)定性好。

為了實(shí)現(xiàn)上述目的，在第一方面，本發(fā)明提供了一種雙軸孔裝配系統(tǒng)，其用于將雙軸工件裝配于雙孔工件中，包括：基座；機(jī)械臂，固定于基座且與雙軸工件固定連接；激光跟蹤測(cè)量?jī)x，測(cè)量雙軸工件、雙孔工件以及機(jī)械臂末端的位姿；傳感器，實(shí)時(shí)感測(cè)雙軸孔裝配過(guò)程中雙軸工件的各軸與雙孔工件的相應(yīng)孔之間的接觸力及接觸力矩；以及上位機(jī)控制系統(tǒng)，電連接于機(jī)械臂，且通信連接于傳感器并接收傳感器傳輸?shù)慕佑|力和接觸力矩的數(shù)據(jù)以實(shí)時(shí)操縱機(jī)械臂運(yùn)動(dòng)。

為了實(shí)現(xiàn)上述目的，在第二方面，本發(fā)明提供了一種雙軸孔裝配系統(tǒng)的控制方法，其用于控制本發(fā)明第一方面所述的雙軸孔裝配系統(tǒng)，包括步驟S1、S2、S3、S4、S5以及S6。

S1，確定雙軸工件在機(jī)械臂末端中心坐標(biāo)系的轉(zhuǎn)換矩陣T_t^p、雙孔工件在機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣以及雙軸工件在機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣包括步驟：S11，利用激光跟蹤測(cè)量?jī)x，在激光跟蹤測(cè)量?jī)x坐標(biāo)系下，測(cè)出雙軸工件的雙軸底面圓心的三維坐標(biāo)分別為(X_plm，Y_plm，Z_plm)、(X_prm，Y_prm，Z_prm)，雙軸底面的法向量為(X_pnm，Y_pnm，Z_pnm)，雙孔工件(H)的雙孔頂面圓心的三維坐標(biāo)分別為(X_hlm，Y_hlm，Z_hlm)、(X_hrm，Y_hrm，Z_hrm)，雙孔頂面的法向量為(X_hnm，Y_hnm，Z_hnm)，機(jī)械臂末端的中心的三維坐標(biāo)為(X_tm，Y_tm，Z_tm)；S12，建立雙軸坐標(biāo)系和雙孔坐標(biāo)系，并計(jì)算 出激光跟蹤測(cè)量?jī)x坐標(biāo)系到雙軸坐標(biāo)系的轉(zhuǎn)換矩陣和激光跟蹤測(cè)量?jī)x坐標(biāo)系到雙孔坐標(biāo)系的轉(zhuǎn)換矩陣S13，機(jī)械臂自動(dòng)讀取機(jī)械臂末端的中心在機(jī)械臂基座坐標(biāo)系下的三維坐標(biāo)(X_tw，Y_tw，Z_tw)和歐拉角(EX，EY，EZ)，并計(jì)算出機(jī)械臂末端中心坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣和激光跟蹤測(cè)量?jī)x坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣S14，根據(jù)步驟S12求得的和S13求得的和分別得到和的表達(dá)式，即：

S2，上位機(jī)控制系統(tǒng)初始化，檢查所有傳感器是否正常工作以及傳感器與上位機(jī)控制系統(tǒng)之間通信是否正常，并對(duì)傳感器進(jìn)行回零標(biāo)定。

S3，通過(guò)上位機(jī)控制系統(tǒng)操縱機(jī)械臂以實(shí)時(shí)調(diào)整軸孔的相對(duì)位置，包括步驟S31、S32和S33。

S31，調(diào)整機(jī)械臂以使雙軸坐標(biāo)系和雙孔坐標(biāo)系初步對(duì)齊，此時(shí)與中除了對(duì)應(yīng)原點(diǎn)的Z坐標(biāo)值不相等之外其它值均相等，對(duì)齊前機(jī)械臂末端中心坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣為雙軸工件坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣令經(jīng)調(diào)整對(duì)齊之后的機(jī)械臂末端中心坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣為經(jīng)調(diào)整對(duì)齊之后的雙軸工件坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣為

S32，根據(jù)S31中的初始值，計(jì)算出調(diào)整之后的設(shè)調(diào)整對(duì)齊前后雙軸坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣的變化矩陣為dT，機(jī)械臂末端中心坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣的變化矩陣為dT2，計(jì)算過(guò)程為：

S33，根據(jù)S32中得到的解算出調(diào)整之后機(jī)械臂末端的中心在機(jī)械臂基座坐標(biāo)系中的三維坐標(biāo)(X_tw2，Y_tw2，Z_tw2)和歐拉角(EX2，EY2，EZ2)，解算公式為：

S4，上位機(jī)控制系統(tǒng)將機(jī)械臂末端的中心調(diào)整到步驟S33中得到的三維坐標(biāo)為(X_tw2，Y_tw2，Z_tw2)、歐拉角為(EX2，EY2，EZ2)的位姿，完成軸孔對(duì)齊。

S5，上位機(jī)控制系統(tǒng)采用增量型P控制方法并基于雙軸工件的位置反饋和雙軸坐標(biāo)系原點(diǎn)的Z坐標(biāo)值與雙孔坐標(biāo)系原點(diǎn)的Z坐標(biāo)值的差值大小操縱機(jī)械臂運(yùn)動(dòng)，以使雙軸工件豎直下降、逐漸與雙孔工件接近、直至接觸。

S6，上位機(jī)控制系統(tǒng)采用阻抗控制方法并基于傳感器實(shí)時(shí)感測(cè)到的雙軸孔裝配過(guò)程中雙軸工件的各軸與雙孔工件的相應(yīng)孔之間的接觸力以及接觸力矩操縱機(jī)械臂運(yùn)動(dòng)，以使雙軸工件繼續(xù)下降直至與雙孔工件完全配合。

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

在根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)中，上位機(jī)控制系統(tǒng)基于機(jī)械臂內(nèi)部的自標(biāo)定程序、激光跟蹤測(cè)量?jī)x測(cè)量出的雙軸工件、雙孔工件以及機(jī)械臂末端的位姿完成雙軸工件和雙孔工件的初步對(duì)準(zhǔn)，并進(jìn)一步基于與傳感器的通信合作以實(shí)時(shí)操縱機(jī)械臂運(yùn)動(dòng)，進(jìn)而完成雙軸工件和雙孔工件的裝配，其裝配精度高，穩(wěn)定性好，適用范圍廣。

附圖說(shuō)明

圖1是根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)的整體裝配圖；

圖2至圖5是根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)的控制方法控制雙軸孔裝配系統(tǒng)的過(guò)程示意圖，其中圖2是雙軸工件P安裝于雙孔工件H前的位置示意 圖，圖3是雙軸工件P與雙孔工件H安裝時(shí)且雙軸工件P與雙孔工件H對(duì)齊時(shí)的位置示意圖，圖4是雙軸工件P下移并與雙孔工件H接觸時(shí)的位置示意圖，圖5是雙軸工件P與雙孔工件H完全裝配時(shí)的示意圖；

圖6是根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)的控制方法中的各坐標(biāo)系的轉(zhuǎn)換關(guān)系示意圖；

圖7是根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)的控制方法中的增量型P控制方法的控制流程圖；

圖8是根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)的控制方法中的阻抗控制方法的控制流程圖。

其中，附圖標(biāo)記說(shuō)明如下：

W基座 P1軸

T機(jī)械臂 P2連接板

M激光跟蹤測(cè)量?jī)x H雙孔工件

S傳感器 H1孔

C上位機(jī)控制系統(tǒng) H2底板

P雙軸工件

具體實(shí)施方式

下面參照附圖來(lái)詳細(xì)說(shuō)明根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)及其控制方法。

首先說(shuō)明本發(fā)明第一方面的雙軸孔裝配系統(tǒng)。

參照?qǐng)D1，根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)用于將雙軸工件P裝配于雙孔工件H中，包括：基座W；機(jī)械臂T(內(nèi)設(shè)有自標(biāo)定程序)，固定于基座W且與雙軸工件P固定連接；激光跟蹤測(cè)量?jī)xM，測(cè)量雙軸工件P、雙孔工件H以及機(jī)械臂末端T1的位姿；傳感器S，實(shí)時(shí)感測(cè)雙軸孔裝配過(guò)程中雙軸工件P的各軸P1與雙孔工件H的相應(yīng)孔H1之間的接觸力及接觸力矩；以及上位機(jī)控制系統(tǒng)C，電連接于機(jī)械臂T，且通信連接于傳感器S并接收傳感器S傳輸?shù)慕佑|力和接觸力矩的數(shù)據(jù)以實(shí)時(shí)操縱機(jī)械臂T運(yùn)動(dòng)。

在根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)中，上位機(jī)控制系統(tǒng)C基于機(jī)械臂T內(nèi)部的自標(biāo)定程序、激光跟蹤測(cè)量?jī)xM測(cè)量出的雙軸工件P、雙孔工件H以及 機(jī)械臂末端T1的位姿完成雙軸工件P和雙孔工件H的初步對(duì)準(zhǔn)，并進(jìn)一步基于與傳感器S的通信合作以實(shí)時(shí)操縱機(jī)械臂T運(yùn)動(dòng)，進(jìn)而完成雙軸工件P和雙孔工件H的裝配，其裝配精度高，穩(wěn)定性好，適用范圍廣。

在根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)中，雙軸工件P包括兩個(gè)軸P1和將兩個(gè)軸P1連接為一體的連接板P2。雙軸工件P可為剛性工件或柔性工件。

在根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)中，雙孔工件H包括兩個(gè)孔H1和用于設(shè)置兩個(gè)孔H1的底板H2。雙孔工件H可為剛性工件或柔性工件。

在這里補(bǔ)充說(shuō)明的是，機(jī)械臂末端T1的位姿在本發(fā)明中具體指的是機(jī)械臂末端T1的中心的位置坐標(biāo)。雙孔工件H的位置應(yīng)處于機(jī)械臂末端T1的運(yùn)動(dòng)范圍之內(nèi)。

根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)，在一實(shí)施例中，傳感器S可為力傳感器。

其次說(shuō)明根據(jù)本發(fā)明第二方面的雙軸孔裝配系統(tǒng)的控制方法。

參照?qǐng)D2至圖8，雙軸孔裝配系統(tǒng)的控制方法用于控制本發(fā)明第一方面所述的雙軸孔裝配系統(tǒng)，包括步驟S1、S2、S3、S4、S5以及S6。

S1，參照?qǐng)D6，確定雙軸工件P在機(jī)械臂末端中心坐標(biāo)系的轉(zhuǎn)換矩陣T_t^p、雙孔工件H在機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣以及雙軸工件P在機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣包括步驟：S11，利用激光跟蹤測(cè)量?jī)xM，在激光跟蹤測(cè)量?jī)x坐標(biāo)系下，測(cè)出雙軸工件P中的兩個(gè)軸P1的底面圓心的三維坐標(biāo)分別為(X_plm，Y_plm，Z_plm)、(X_prm，Y_prm，Z_prm)，雙軸底面的法向量為(X_pnm，Y_pnm，Z_pnm)，雙孔工件H的雙孔頂面圓心的三維坐標(biāo)分別為(X_hlm，Y_hlm，Z_hlm)、(X_hrm，Y_hrm，Z_hrm)，雙孔頂面的法向量為(X_hnm，Y_hnm，Z_hnm)，機(jī)械臂末端T1的中心的三維坐標(biāo)為(X_tm，Y_tm，Z_tm)；S12，建立雙軸坐標(biāo)系和雙孔坐標(biāo)系，并計(jì)算出激光跟蹤測(cè)量?jī)x坐標(biāo)系到雙軸坐標(biāo)系的轉(zhuǎn)換矩陣 和激光跟蹤測(cè)量?jī)x坐標(biāo)系到雙孔坐標(biāo)系的轉(zhuǎn)換矩陣S13，機(jī)械臂T自動(dòng)讀取機(jī)械臂末端T1的中心在機(jī)械臂基座坐標(biāo)系下的三維坐標(biāo)(X_tw，Y_tw，Z_tw)和歐拉角(EX，EY，EZ)，并計(jì)算出機(jī)械臂末端中心坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣和激光跟蹤測(cè)量?jī)x坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的 轉(zhuǎn)換矩陣S14，根據(jù)步驟S12求得的和S13求得的和分別得到T_t^p、和的表達(dá)式，即：

其中，為的逆矩陣，為的逆矩陣。

在這里補(bǔ)充說(shuō)明的是，“機(jī)械臂T自動(dòng)讀取(基于內(nèi)部設(shè)置的自標(biāo)定程序)”屬于機(jī)械臂T自帶功能，屬于公知常識(shí)。

S2，上位機(jī)控制系統(tǒng)C初始化，檢查所有傳感器S是否正常工作以及傳感器S與上位機(jī)控制系統(tǒng)C之間通信是否正常，并對(duì)傳感器S進(jìn)行回零標(biāo)定。

S3，通過(guò)上位機(jī)控制系統(tǒng)C操縱機(jī)械臂T以實(shí)時(shí)調(diào)整軸孔的相對(duì)位置，包括步驟S31、S32和S33。

S31，調(diào)整機(jī)械臂T以使雙軸坐標(biāo)系和雙孔坐標(biāo)系初步對(duì)齊(即雙軸坐標(biāo)系的原點(diǎn)和雙孔坐標(biāo)系的原點(diǎn)的連線垂直于雙孔頂面)，此時(shí)可以機(jī)械臂基座坐標(biāo)系為參考，則中除了雙軸坐標(biāo)系的原點(diǎn)的Z坐標(biāo)值與中的雙孔坐標(biāo)系的原點(diǎn)的Z坐標(biāo)值不相等之外，與中的其它值均相等。對(duì)齊前機(jī)械臂末端中心坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣為雙軸工件P坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣令經(jīng)調(diào)整對(duì)齊之后的機(jī)械臂末端中心坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣為經(jīng)調(diào)整對(duì)齊之后的雙軸工件P坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣為

S32，根據(jù)S31中的初始值，計(jì)算出調(diào)整之后的設(shè)調(diào)整對(duì)齊前后雙軸坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣的變化矩陣為dT，機(jī)械臂末端中心坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣的變化矩陣為dT2，計(jì)算過(guò)程為：

其中，為T_t^p的逆矩陣，為的逆矩陣。

在這里補(bǔ)充說(shuō)明的是，為了防止雙軸工件P和雙孔工件H發(fā)生磕碰，雙軸坐標(biāo)系和雙孔坐標(biāo)系在初步對(duì)齊操作中始終不會(huì)下移，即其中，表示中的第三行第四列的值(即對(duì)齊調(diào)整之前雙軸坐標(biāo)系的原點(diǎn)的Z坐標(biāo)值)，表示中的第三行第四列的值(即對(duì)齊調(diào)整之后雙軸坐標(biāo)系的原點(diǎn)的Z坐標(biāo)值)。

S33，根據(jù)S32中得到的解算出調(diào)整之后機(jī)械臂末端T1的中心在機(jī)械臂基座坐標(biāo)系中的三維坐標(biāo)(X_tw2，Y_tw2，Z_tw2)和歐拉角(EX2，EY2，EZ2)，解算公式為：

S4，上位機(jī)控制系統(tǒng)C將機(jī)械臂末端T1的中心調(diào)整到步驟S33中得到的三維坐標(biāo)為(X_tw2，Y_tw2，Z_tw2)、歐拉角為(EX2，EY2，EZ2)的位姿，完成軸孔對(duì)齊(如圖3所示)。

S5，參照?qǐng)D3、圖4和圖7，上位機(jī)控制系統(tǒng)C采用增量型P控制方法并基于雙軸工件P的位置反饋和雙軸坐標(biāo)系原點(diǎn)的Z坐標(biāo)值與雙孔坐標(biāo)系原點(diǎn)的Z坐標(biāo)值的差值大小操縱機(jī)械臂T運(yùn)動(dòng)，以使雙軸工件P豎直下降、逐漸與雙孔工件H接近、直至接觸(如圖4所示)，此時(shí)雙軸坐標(biāo)系的原點(diǎn)在機(jī)械臂基座坐標(biāo)系下的Z坐標(biāo)值為Z₀(預(yù)設(shè)為100mm)。

S6，參照?qǐng)D4、圖5和圖8，上位機(jī)控制系統(tǒng)C采用阻抗控制方法并基于傳感器S實(shí)時(shí)感測(cè)到的雙軸孔裝配過(guò)程中雙軸工件P的各軸P1與雙孔工件H的相應(yīng)孔H1之間的接觸力以及接觸力矩操縱機(jī)械臂T運(yùn)動(dòng)，以使雙軸工件P繼續(xù)下降直至與雙孔工件H完全配合。

根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)的控制方法，在步驟S12中，可以以雙軸底面圓心連線的中心為原點(diǎn)、連線為Y_p軸、雙軸工件(P)的連接板(P2) 的法向量為Z_p軸、由Y_p叉乘Z_p軸得到X_p軸建立雙軸坐標(biāo)系(如圖1所示)，并用激光跟蹤測(cè)量?jī)xM測(cè)出雙軸坐標(biāo)系的原點(diǎn)在激光跟蹤測(cè)量?jī)x坐標(biāo)系下的三維坐標(biāo)O_p，則激光跟蹤測(cè)量?jī)x坐標(biāo)系與雙軸坐標(biāo)系的轉(zhuǎn)換關(guān)系如下：

可以以雙孔頂面圓心連線的中心為原點(diǎn)、連線為Y_h軸、雙孔頂面所在平面的法向量為Z_h軸、由Y_h叉乘Z_h軸得到X_h軸建立雙孔坐標(biāo)系(如圖1所示)，并用激光跟蹤測(cè)量?jī)xM測(cè)出雙孔坐標(biāo)系的原點(diǎn)在激光跟蹤測(cè)量?jī)x坐標(biāo)系下的三維坐標(biāo)O_h，則激光跟蹤測(cè)量?jī)x坐標(biāo)系與雙孔坐標(biāo)系的轉(zhuǎn)換關(guān)系如下：

在步驟S13中，機(jī)械臂末端中心坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣 的計(jì)算公式為：

激光跟蹤測(cè)量?jī)x坐標(biāo)系到機(jī)械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣的計(jì)算公式為：

即，

根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)的控制方法，在步驟S5中，上位機(jī)控制系統(tǒng)C中設(shè)置有豎直方向的接觸力閾值，當(dāng)雙軸工件P與雙孔工件H接觸時(shí)的接觸力到達(dá)該閾值后，上位機(jī)控制系統(tǒng)C控制雙軸工件P停止下降。通常上位機(jī)控制系統(tǒng)C中設(shè)置的接觸力的閾值可為1N～3N，但不僅限如此，該接觸力的閾值可根據(jù)雙軸工件P與雙孔工件H的不同程度的柔性(或剛性)進(jìn)行適當(dāng)調(diào)整。

根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)的控制方法，在步驟S5中，參照?qǐng)D7，增量型P控制方法的算法為：

P1，計(jì)算第k次循環(huán)中機(jī)械臂基座坐標(biāo)系到雙軸坐標(biāo)系的轉(zhuǎn)換矩陣以及此時(shí)雙軸坐標(biāo)系原點(diǎn)的Z坐標(biāo)值與雙孔坐標(biāo)系原點(diǎn)的Z坐標(biāo)值的差值，即：

其中，表示雙孔坐標(biāo)系的原點(diǎn)的Z坐標(biāo)值，表示雙軸坐標(biāo)系的原點(diǎn)的Z坐標(biāo)值；

P2，第k次循環(huán)中雙軸坐標(biāo)系原點(diǎn)的下移量為dZ_k，由于增量型P控制為比例控制，則dZ_k＝K_pe_zk，計(jì)算第k+1次循環(huán)中機(jī)械臂末端T1的中心的三維坐標(biāo)(X_tw(k+1)，Y_tw(k+1)，Z_tw(k+1))，即：

(X_tw(k+1),Y_tw(k+1),Z_tw(k+1))＝(X_twk,Y_twk,Z_twk+dZ_k)

其中K_p表示比例因子，且為一常數(shù)。

在這里補(bǔ)充說(shuō)明的是，增量型P控制方法的控制過(guò)程為：將第k次循環(huán)測(cè)量得到的Z(k)值與Z₀值(上述已設(shè)定Z₀＝100mm)進(jìn)行比較，當(dāng)Z(k)值大于Z₀時(shí)，說(shuō)明此時(shí)軸P1還未下移到指定位置，因此位置誤差e_zk為負(fù)值，此時(shí)再乘以比例系數(shù)K_p得到的位置增量dZ_k也為負(fù)值，軸P1便會(huì)繼續(xù)下降(反 之說(shuō)明下移過(guò)度，位置增量便為正，軸P1會(huì)上移)并接著進(jìn)行第k+1次循環(huán)。其中，k取整數(shù)。

在一實(shí)施例中，K_p＝0.9。

根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)的控制方法，在增量型P控制循環(huán)過(guò)程中，當(dāng)雙軸坐標(biāo)系原點(diǎn)的Z坐標(biāo)值與雙孔坐標(biāo)系原點(diǎn)的Z坐標(biāo)值的差值(即dZ_k＝Z_tw(k+1)-Z_twk)小于0.001mm時(shí)，循環(huán)停止。

根據(jù)本發(fā)明的雙軸孔裝配系統(tǒng)的控制方法，在步驟S6中，參照?qǐng)D8，阻抗控制方法的算法為：

K1，設(shè)置相關(guān)參數(shù)(參數(shù)可以隨操作者的具體實(shí)驗(yàn)進(jìn)行調(diào)整)，K_p＝0.02，K_d＝0.002，K_v＝5，[F_x0,F_y0,F_z0]＝[5,10,20](N)，[M_x0,M_y0,M_z0]＝[0,0,0]((N·m)，Z_C＝-5mm，其中K_p為阻抗控制中的比例因子，K_d為阻抗控制中的微分參數(shù)，K_v為阻阻抗控制中的阻尼參數(shù)，[F_x0,F_y0,F_z0]為接觸力參考值，[M_x0,M_y0,M_z0]為接觸力矩參考值，Z_C為機(jī)械臂末端T1的中心的Z坐標(biāo)總下移量；

K2，在第k次循環(huán)中，由力傳感器S采集到的接觸力和接觸力矩分別為[F_xk,F_yk,F_zk]、[M_xk,M_yk,M_zk]，即：

F_k＝[F_xk,F_yk,F_zk,M_xk,M_yk,M_zk]

dF_k＝[F_x0-F_xk,F_y0-F_yk,F_z0-F_zk,M_x0-M_xk,M_y0-M_yk,M_z0-M_zk]

其中，F(xiàn)_k為第k次循環(huán)中接觸力和接觸力矩構(gòu)成的六維力，dF_k為接觸力與接觸力參考值的差以及接觸力矩與接觸力矩參考值的差構(gòu)成的六維力；

K3，計(jì)算第k次循環(huán)中需要調(diào)整的位姿，即機(jī)械臂末端中心坐標(biāo)系的原點(diǎn)平移量(dX_k,dY_k,dZ_k)和各坐標(biāo)軸的轉(zhuǎn)動(dòng)量，計(jì)算公式為：

其中，dX_k為第k次循環(huán)中機(jī)械臂末端中心坐標(biāo)系的原點(diǎn)在X方向的平移量、dY_k為第k次循環(huán)中機(jī)械臂末端中心坐標(biāo)系的原點(diǎn)在Y方向的平移量、dZ_k為第k次循環(huán)中機(jī)械臂末端中心坐標(biāo)系的原點(diǎn)在Z方向的平移量、dθ_xk為第k次循環(huán)中機(jī)械臂末端中心坐標(biāo)系繞X坐標(biāo)軸的轉(zhuǎn)動(dòng)量、dθ_yk為第k次循環(huán)中機(jī)械臂末端中心坐標(biāo)系繞Y坐標(biāo)軸的轉(zhuǎn)動(dòng)量、dθ_zk為第k次循環(huán)中機(jī)械臂末端中心坐標(biāo)系繞Z坐標(biāo)軸的轉(zhuǎn)動(dòng)量，dX_k-1為第k-1次循環(huán)中機(jī)械臂末端中心坐標(biāo)系的原點(diǎn)在X方向的平移量、dY_k-1為第k-1次循環(huán)中機(jī)械臂末端中心坐標(biāo)系的原點(diǎn)在Y方向的平移量，dZ_k-1為第k-1次循環(huán)中機(jī)械臂末端中心坐標(biāo)系的原點(diǎn)在Z方向的平移量，dF_k為第k次循環(huán)中接觸力與接觸力參考值的差以及接觸力矩與接觸力矩參考值的差構(gòu)成的六維力，dF_k-1為第k-1次循環(huán)中接觸力與接觸力參考值的差以及接觸力矩與接觸力矩參考值的差構(gòu)成的六維力，dF_k-2為第k-2次循環(huán)中接觸力與接觸力參考值的差以及接觸力矩與接觸力矩參考值的差構(gòu)成的六維力，V_Zk為第k次循環(huán)中雙軸工件P的下移速度。dF_k(1)為dF_k表達(dá)式中的第一個(gè)值，dF_k(2)為dF_k表達(dá)式中的第二個(gè)值，dF_k(3)、dF_k(4)、dF_k(5)、dF_k(6)依次類推。同理，dF_k-1(1)為dF_k-1表達(dá)式中的第一個(gè)值，dF_k-2(1)為dF_k-2表達(dá)式中的第一個(gè)值，依次類推。

K4，根據(jù)K3中得到的dX_k、dY_k、dZ_k、dθ_xk、dθ_yk、dθ_zk，計(jì)算機(jī)械臂末端中心坐標(biāo)系的原點(diǎn)需要調(diào)整的變換矩陣dT_kpos、以及各坐標(biāo)軸需要調(diào)整的變換矩陣dT_kx、dT_ky、dT_kz，計(jì)算公式為：

K5，根據(jù)K4中得到的dT_kx、dT_ky、dT_kz，計(jì)算機(jī)械臂末端中心坐標(biāo)系需要調(diào)整的總變換矩陣dT_k，以及變換之后的機(jī)械臂基座坐標(biāo)系到機(jī)械臂末端中心坐標(biāo)系的轉(zhuǎn)換矩陣計(jì)算公式為：

dT_k＝dT_kx dT_ky dT_kz dT_kpos

K6，根據(jù)K2-K5中給出的計(jì)算式，分別計(jì)算每次循環(huán)調(diào)整后的機(jī)械臂末端T1的中心的位姿，直到機(jī)械臂末端T1的中心的Z坐標(biāo)總下移量接近設(shè)定的Z_C值(此步驟中Z_C＝-5mm)。

在這里補(bǔ)充說(shuō)明的是，圖8中的Z_k(T1)為在第K次循環(huán)時(shí)機(jī)械臂T自動(dòng)讀取出的機(jī)械臂末端T1的中心的Z坐標(biāo)值，dZ_k(T1)為第k次循環(huán)中機(jī)械臂末端T1的中心的下移量。其中，在阻抗控制中，因?yàn)榻佑|力方向?yàn)樨?fù)，所以當(dāng)?shù)趉次循環(huán)中測(cè)量得到的接觸力F_k的絕對(duì)值小于設(shè)定的參考力F₀的絕對(duì)值時(shí)，此時(shí)計(jì)算出來(lái)的接觸力誤差dF_k為負(fù)值，接觸力誤差dF_k乘上比例因子K_p后得到的Z坐標(biāo)增量dZ_k(T1)也為負(fù)值，說(shuō)明此時(shí)沒(méi)有卡阻，雙軸工件P可以繼續(xù)下降(反之得到的Z坐標(biāo)增量dZ_k(T1)為正值，說(shuō)明此時(shí)軸P1被卡住，需要抽出一點(diǎn)，軸P1便上移)并接著進(jìn)行第k+1次循環(huán)，直到機(jī)械臂 末端T1的中心的Z坐標(biāo)總下移量接近設(shè)定的Z_C值，循環(huán)停止。其中，k取整數(shù)。

K7，改變K1中的相關(guān)參數(shù)，即接觸力參考值為[F_x,F_y,F_z]＝[0,0,50](N)，接觸力矩參考值為[M_x,M_y,M_z]＝[0,0,0](N·m)，Z坐標(biāo)總下移量Z_C＝-100mm，繼續(xù)按照K2-K5中給出的計(jì)算式計(jì)算出每次循環(huán)調(diào)整后的機(jī)械臂末端T1的中心的位姿，直到機(jī)械臂末端T1的中心的Z坐標(biāo)總下移量接近設(shè)定的Z_C值(此步驟中Z_C＝-100mm)。