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

背景技術：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

附圖說明

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

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

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

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

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

其中，附圖標記說明如下：

W基座 P1軸

T機械臂 P2連接板

M激光跟蹤測量儀 H雙孔工件

S傳感器 H1孔

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

P雙軸工件

具體實施方式

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

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

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

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

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

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

在這里補充說明的是，機械臂末端T1的位姿在本發(fā)明中具體指的是機械臂末端T1的中心的位置坐標。雙孔工件H的位置應處于機械臂末端T1的運動范圍之內(nèi)。

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

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

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

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

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

在這里補充說明的是，“機械臂T自動讀取(基于內(nèi)部設置的自標定程序)”屬于機械臂T自帶功能，屬于公知常識。

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

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

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

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

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

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

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

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

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

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

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

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

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

激光跟蹤測量儀坐標系到機械臂基座坐標系的轉(zhuǎn)換矩陣的計算公式為：

即，

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

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

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

其中，表示雙孔坐標系的原點的Z坐標值，表示雙軸坐標系的原點的Z坐標值；

P2，第k次循環(huán)中雙軸坐標系原點的下移量為dZ_k，由于增量型P控制為比例控制，則dZ_k＝K_pe_zk，計算第k+1次循環(huán)中機械臂末端T1的中心的三維坐標(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ù)。

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

在一實施例中，K_p＝0.9。

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

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

K1，設置相關參數(shù)(參數(shù)可以隨操作者的具體實驗進行調(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為機械臂末端T1的中心的Z坐標總下移量；

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，計算第k次循環(huán)中需要調(diào)整的位姿，即機械臂末端中心坐標系的原點平移量(dX_k,dY_k,dZ_k)和各坐標軸的轉(zhuǎn)動量，計算公式為：

其中，dX_k為第k次循環(huán)中機械臂末端中心坐標系的原點在X方向的平移量、dY_k為第k次循環(huán)中機械臂末端中心坐標系的原點在Y方向的平移量、dZ_k為第k次循環(huán)中機械臂末端中心坐標系的原點在Z方向的平移量、dθ_xk為第k次循環(huán)中機械臂末端中心坐標系繞X坐標軸的轉(zhuǎn)動量、dθ_yk為第k次循環(huán)中機械臂末端中心坐標系繞Y坐標軸的轉(zhuǎn)動量、dθ_zk為第k次循環(huán)中機械臂末端中心坐標系繞Z坐標軸的轉(zhuǎn)動量，dX_k-1為第k-1次循環(huán)中機械臂末端中心坐標系的原點在X方向的平移量、dY_k-1為第k-1次循環(huán)中機械臂末端中心坐標系的原點在Y方向的平移量，dZ_k-1為第k-1次循環(huá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表達式中的第一個值，dF_k(2)為dF_k表達式中的第二個值，dF_k(3)、dF_k(4)、dF_k(5)、dF_k(6)依次類推。同理，dF_k-1(1)為dF_k-1表達式中的第一個值，dF_k-2(1)為dF_k-2表達式中的第一個值，依次類推。

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

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

dT_k＝dT_kx dT_ky dT_kz dT_kpos

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

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

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