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