五自由度旋轉(zhuǎn)鏈?zhǔn)酱a垛機(jī)械臂的運(yùn)動控制方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及機(jī)器人的運(yùn)動控制領(lǐng)域，尤其設(shè)及一種五自由度旋轉(zhuǎn)鏈?zhǔn)酱a煤機(jī)械臂 的運(yùn)動控制方法。
【背景技術(shù)】
[0002] 隨著碼煤機(jī)械臂在工業(yè)領(lǐng)域中的廣泛應(yīng)用，針對碼煤機(jī)械臂控制系統(tǒng)的研究便具 有重要意義，其中運(yùn)動反解問題是控制碼煤機(jī)械臂運(yùn)動的先決條件。通過反解運(yùn)算，可將 機(jī)械臂空間位姿轉(zhuǎn)換為各轉(zhuǎn)動關(guān)節(jié)變量，進(jìn)而通過控制轉(zhuǎn)動關(guān)節(jié)的轉(zhuǎn)動角度實(shí)現(xiàn)機(jī)構(gòu)的運(yùn) 動，完成給定的運(yùn)動軌跡。對于全旋轉(zhuǎn)的鏈?zhǔn)綑C(jī)械臂，可用單個(gè)連桿變換矩陣41可(巧_)左乘 末端的齊次變換矩陣F，依次得到各臂的轉(zhuǎn)動角度值，但如此會出現(xiàn)許多角度的禪合，對計(jì) 算帶來不便。

【發(fā)明內(nèi)容】

[0003] 本發(fā)明提供了一種五自由度鏈?zhǔn)酱a煤機(jī)械臂的運(yùn)動控制方法，W克服反解過程中 角度禪合給計(jì)算帶來的困擾。
[0004] 本發(fā)明的五自由度旋轉(zhuǎn)鏈?zhǔn)酱a煤機(jī)械臂的運(yùn)動控制方法，包括如下步驟：
[0005] 1)獲取機(jī)械臂末端運(yùn)動軌跡曲線，在該運(yùn)動軌跡曲線上，連續(xù)選取包含起始點(diǎn)和 終止點(diǎn)的n個(gè)坐標(biāo)點(diǎn)，記為Pi (Xi, y;，Zi)，其中i = 1，…，n ;
[0006] 2)對選取的每一坐標(biāo)點(diǎn)Pi(Xi,y;，Zi)進(jìn)行運(yùn)動學(xué)反解計(jì)算，得到該坐標(biāo)點(diǎn)對應(yīng)的 各驅(qū)動關(guān)節(jié)處的轉(zhuǎn)動角度信息；從而獲得運(yùn)動軌跡曲線與各臂轉(zhuǎn)動角度之間的對應(yīng)關(guān)系；
[0007] 3)將上述對應(yīng)關(guān)系下發(fā)至運(yùn)動控制卡，通過伺服驅(qū)動系統(tǒng)驅(qū)動各臂轉(zhuǎn)動，通過傳 感器檢測并反饋轉(zhuǎn)動信號，各臂聯(lián)動完成給定的運(yùn)動軌跡。
[0008] 對于五自由度旋轉(zhuǎn)鏈?zhǔn)綑C(jī)械臂，采用D-H方法描述該機(jī)械臂，對所有的關(guān)節(jié)建立 各自的坐標(biāo)系，如圖1所示。
[0009] 其中，臂1可繞Z軸轉(zhuǎn)動，其轉(zhuǎn)動角度記為0 1，所在坐標(biāo)系記為{1};
[0010] 臂2長度為12,與臂1間距離為d2,可相對臂1轉(zhuǎn)動，其轉(zhuǎn)動角度記為0 2,所在坐 標(biāo)系記為{2};
[0011] 臂3長度為13,與臂2間距離為d3,可相對臂2轉(zhuǎn)動，其轉(zhuǎn)動角度記為0 3,所在坐 標(biāo)系記為{3};
[0012] 臂4長度為14,與臂3間距離為d4,可相對臂3轉(zhuǎn)動，其轉(zhuǎn)動角度記為0 4,所在坐 標(biāo)系記為{4};
[0013] 末端執(zhí)行器5可相對臂4繞Z軸轉(zhuǎn)動，其轉(zhuǎn)動角度記為0y所在坐標(biāo)系記為巧};
[0014] 末端n、0、a分別為法向向量、方位向量和接近向量，用于描述末端執(zhí)行器5的姿 態(tài)。
[0015] 根據(jù)圖1得到的碼煤機(jī)械臂的連桿參數(shù)如表1所示。
[0016] 表1碼煤機(jī)械臂的連桿參數(shù)
[0017]
[001*^~碼煤機(jī)械臂中連桿間變換的通式為：I ' '
[0020]
[00川由通式（1)可知連桿變換"It依賴于四個(gè)參數(shù)aki，aki，dk和0k，其中3k康示 關(guān)節(jié)偏移，ak1表示關(guān)節(jié)扭角，dk表示臂k與k-1間的距離，0k表示臂k的旋轉(zhuǎn)角度。對 于轉(zhuǎn)動關(guān)節(jié)k而言/I了是0k的函數(shù)。
[0022] 將表1中的連桿參數(shù)代入通式（1)可得各連桿的變換矩陣：
[0026] 步驟2)中所述的對坐標(biāo)點(diǎn)Pi(Xi，yi，Zi)進(jìn)行運(yùn)動學(xué)反解計(jì)算，具體包括如下步 驟：
[0027] 1)將選取的坐標(biāo)點(diǎn)Pi(Xi，yi，Zi)由末端執(zhí)行器5所在的坐標(biāo)系間移動 到臂4所在的坐標(biāo)系{4}中，得到運(yùn)動軌跡曲線上的點(diǎn)在坐標(biāo)系{4}中相應(yīng)的坐標(biāo) (Xi+l4sin0 1，Yi-IaCos01，Zi);其中14為臂4的長度，0 1為臂1的轉(zhuǎn)動角度；
[0028] 2)不考慮機(jī)械臂末端執(zhí)行器轉(zhuǎn)動角度時(shí)，坐標(biāo)系{4}處的齊次變換矩陣可表示 為：
[0029]
[0030] 其中，n'、〇'、a'分別為法向向量、方位向量和接近向量，用于描述臂4的姿態(tài)， [00引]由背游)-呼'=江(爲(wèi)朽T(巧) 34了(錢,)，;其中)為連桿k相對連桿k-1的變換矩陣， 得：
[0032]
[003引其中，12、13分別為臂2和臂3的長度，02、03分別為臂2、臂3的轉(zhuǎn)動角度，cU、d3、d4分別為臂2與1間的距離、臂3與2間的距離、臂4與3間的距離，
[0034] 由（3)式可得，
[0035]
，其中 tana=yl/Xl，求解得到坐標(biāo)點(diǎn)Pl(Xl，yl，Zl)相應(yīng)的臂l的轉(zhuǎn)動角度0l;リ由步驟リ中 似、（4)式可得，
[0036]
求解得到 坐標(biāo)點(diǎn)Pi(Xi，yi，Zi)相應(yīng)的臂3的轉(zhuǎn)動角度0 3;
[0037] 4)由步驟。中（4)式可得，
[0040] 求解得到坐標(biāo)點(diǎn)Pi(Xi，yi，Zi)相應(yīng)的臂2的轉(zhuǎn)動角度0 2;
[00川W根據(jù)白4=白3_白2,得到坐標(biāo)點(diǎn)Pi(Xi，yi，Zi)相應(yīng)的臂4的轉(zhuǎn)動角度白4;
[0042] 6)考慮機(jī)械臂末端執(zhí)行器的轉(zhuǎn)動角度時(shí)，碼煤機(jī)械臂末端執(zhí)行器位姿的齊次變換 矩陣可表示為：
[0043]
[0044] 其中n、0、a分別為法向向量、方位向量和接近向量，用于描述末端執(zhí)行器5的姿 態(tài)；
[0045] 根據(jù)與連桿變換方程聯(lián)立，碼煤機(jī)械臂的運(yùn)動學(xué)方程可表示為 F=4T(0義T辟巧T(巧巧T的HT(巧)，
[0046] 得到：
[0048] 求解得到坐標(biāo)點(diǎn)Pi (Xi，yi，Zi)相應(yīng)的臂5的轉(zhuǎn)動角度0 5。
[0049] 本發(fā)明方法的有益效果：對于全旋轉(zhuǎn)的鏈?zhǔn)綑C(jī)械臂，可用單個(gè)連桿變換矩陣 了(巧）左乘末端的齊次變換矩陣F，依次得到各臂的轉(zhuǎn)動角度值，但如此會出現(xiàn)許多角度 的禪合，對計(jì)算帶來不便。本發(fā)明的方法針對五自由度鏈?zhǔn)酱a煤機(jī)械臂的工作特征，即臂4 始終處于水平工作狀態(tài)，通過坐標(biāo)系的移動，使得反解的過程變得簡單。
【附圖說明】
[0050] 圖1是五自由度鏈?zhǔn)綑C(jī)械臂的D-H模型；
[0051] 圖2是本發(fā)明的方法的流程。
[005引圖3是兩種具體的運(yùn)動軌跡曲線，其中a)為向低處放置物品，b)為向高處放置物 品。
[0053] 圖4是對運(yùn)動軌跡進(jìn)行運(yùn)動學(xué)反解的流程。
【具體實(shí)施方式】
[0054] 下面結(jié)合附圖對本發(fā)明作進(jìn)一步說明。
[0055] 本發(fā)明的對五自由度旋轉(zhuǎn)鏈?zhǔn)綑C(jī)械臂的運(yùn)動控制方法，其流程如圖2所示，碼煤 機(jī)械臂的運(yùn)動控制流程包括獲取曲線段運(yùn)動軌跡坐標(biāo)、反解得到各臂轉(zhuǎn)動角度、下發(fā)轉(zhuǎn)動 角度信號至運(yùn)動控制卡、伺服驅(qū)動系統(tǒng)、傳感器檢測、各臂聯(lián)動完成運(yùn)動軌跡等。