破拆機器人機械臂自主運動控制系統軟件設計*

何 鋒，章小建，趙江海

(1．常州先進制造技術研究所 機器人系統實驗室， 江蘇 常州 213164;2.中國科學院合肥物質科學研究院先進制造技術研究所 機器人系統實驗室，江蘇 常州 213164)

破拆機器人機械臂自主運動控制系統軟件設計*

何 鋒1，2，章小建1，2，趙江海1，2

(1．常州先進制造技術研究所 機器人系統實驗室， 江蘇 常州 213164;2.中國科學院合肥物質科學研究院先進制造技術研究所 機器人系統實驗室，江蘇 常州 213164)

針對現有破拆機器人手工定位時間長、定位不精確的問題，研發了一個基于激光定位的破拆機器人機械臂自主運動的控制系統軟件。設計了運動學模塊、閉環反饋調節模塊、液壓裝置控制模塊、手眼標定模塊和運動控制模塊。實際使用效果表明，該軟件系統能完成控制系統設計指標，提高工作效率。

破拆機器人；運動學；自主運動

0 引言

目前，施工現場作業的破拆機器人采用手動方式進行作業[1]。由于破拆機器人機械臂運動過程產生的慣性力大，操作人員手工操控需要反復調整才能運動到大致位置，因此，研發一種破拆機器人機械臂自主運動控制系統，設計該控制系統的上位軟件，來實現機械臂目標導引的自主運動控制[2]功能是十分有必要的。

本文設計了控制系統軟件的各個模塊，給出了各模塊的設計方法，最后，通過實驗證明了控制系統軟件能完成破拆機器人機械臂目標導引[3]的自主控制功能。

1 破拆機器人總體硬件架構

圖1 破拆機器人整機實物圖

本文實驗平臺在驚天液壓股份公司的 GTRC-15 型破拆機器人基礎上開展功能部件研究，機器人整機實物圖見圖1。由圖1可看出，機器人由機械臂、移動回轉平臺、行走履帶和作業支架組成。機械臂主要由大臂、二臂、三臂和末端執行器構成。回轉平臺由回轉馬達驅動且安裝有角度編碼器以獲得當前回轉平臺偏置角度，機械臂的各個關節由液壓缸驅動，安裝了角度編碼器傳感器，可檢測當前關節角的角度值。行走履帶使用液壓馬達驅動，可以驅使機器人進行前進、后退、旋轉等動作。機器人的通信使用 CAN 總線模式[4]。控制系統主要包括機械臂各關節的電液比例控制系統、無線視頻采集與傳輸系統、激光定位平臺控制系統及機器人主控PC。電液比例控制系統主要是通過控制比例閥閥口開度大小來控制液壓缸體內液體的流速，而流速與液壓缸體的運動速度及關節角的即時速度成近似線性關系。無線視頻采集與傳輸系統是用于操作員遠離作業現場環境下，通過遠程無線視頻來觀察定位作業點，進而自動驅動激光定位平臺找到作業目標點，使液壓機械臂自動運動到指定目標位置。

2 破拆機器人軟件設計

破拆機器人的控制系統軟件選用的平臺是 Windows XP 系統，程序運行框架是用 VC6 的 MFC 類庫開發。在控制系統中設計了運動學計算模塊、閉環反饋調節模塊、液壓裝置控制模塊、運動控制模塊、手眼標定模塊、無線視頻采集與處理模塊等。控制系統軟件界面如圖2。

圖2 破拆機器人控制系統軟件界面

破拆機器人激光定位下機械臂自主運動整體控制系統工作原理為：操作員通過遠程視頻監控系統觀察作業現場，并根據現場環境確定所要破碎的目標點，然后操作激光定位云臺進行俯仰和旋轉運動，直至激光點落在定位的目標點位置上。此時控制系統根據激光定位平臺在兩個方向上旋轉的角度和激光測距儀當前的讀數，運用前面給出的激光云臺坐標系求解公式很容易計算出作業目標點在激光定位平臺坐標系下的空間坐標，該坐標通過剛性轉置可轉換為機械臂坐標系下的作業目標的空間坐標值。通過運動學逆解[5]或在線軌跡規劃方法[6]將空間運動量轉為關節運動輸出量，進而驅動液壓執行元件使機械臂實現多軸聯動動作，快速準確到達指定破拆目標。

自主破拆的算法步驟為：(1)根據標定裝置測出的作業目標距離，經過旋轉及偏置后得到視覺云臺坐標系下三維坐標。激光測距傳感器安裝在云臺坐標系下的Y軸原點，且激光器發射方向與Y軸正向一致，若當前激光測距傳感器讀數是dm，則作業目標在第五坐標系下的坐標表示為 [0,d,0]。(2)通過基坐標系與末端執行器坐標系之間的齊次轉換矩陣計算獲取激光測距系統下的目標物體坐標。(3)通過運動學逆解將第一坐標系下目標點坐標及設定的末端執行器位姿轉換為液壓機械臂五個關節角的角度值。(4)通過關節角與液壓缸長度的映射關系得到液壓缸運動的目標長度。(5)使用 PID 算法控制液壓元件的速度信號，使其快速準確地到達目標點，完成破拆定位。

3 運動學模塊設計

運動學模塊主要包括關節角求解部分、正運動學求解部分和逆運動學求解部分[7]。其中關節角求解部分主要完成液壓缸的變化量與關節角變化量的線性映射關系的建立。正運動學求解部分完成各關節角的當前值與末端破碎錘的空間坐標和姿態的對應關系建立。逆運動學求解部分完成在指定目標位置和指定末端破碎錘姿態的情況下求出滿足能量最優條件下[8]的各關節角的目標值。通過運動學模塊設計，可以在線獲得當前機械臂的姿態和末端執行器的位置，也能根據指定目標點和末端破碎錘姿態求得各關節角的目標值[9]，并將目標值轉化為液壓缸運動的長度。

4 閉環反饋調節模塊設計

閉環反饋調節模塊主要是用于對各液壓缸的位置運動誤差進行調節。使用PID控制方法進行閉環調節，PID控制參數通過經驗法進行設計，獲取合適的各液壓缸的PID控制參數。通過PID閉環反饋調節后，每個液壓缸關節的位置運動誤差在0.01 mm以內。PID控制模型圖見圖3。

圖3 PID控制結構圖

5 液壓裝置控制模塊設計

液壓裝置控制模塊通過CAN總線與各關節的電液比例控制系統進行通信，控制各個液壓缸的閥門開度、運動方向和運動速度。通過液壓裝置控制模塊設計，可以控制機械臂各關節的運動。

6 手眼標定模塊設計

控制系統軟件使用激光定位平臺獲取視覺的坐標系點，為將視覺坐標系和機械臂坐標系進行整合，需要進行手眼標定模塊設計，手眼標定模塊使用最小二乘算法來計算兩坐標系之間的齊次變換矩陣。通過齊次變換矩陣可以將視覺坐標系的目標坐標和目標姿態轉化為機械臂坐標系下的坐標，進一步通過運動學模塊求出各液壓缸的變化量，通過液壓裝置控制模塊和閉環反饋調節模塊運動到指定的目標位置[10]。

7 運動控制模塊設計

運動控制模塊主要完成機械臂的多軸運動控制，在軟件中使用了三次樣條曲線插值的方法，將逆解求出的各關節角目標值、各關節編碼器反饋的各關節角當前值和約定的整體運動時間作為插值輸入參量代入，求得每個定時器周期下各關節角的位置、速度和加速度值。利用液壓缸的運動特性，進行機械臂多軸運動控制。

8 結論

使用本文設計的控制系統軟件，通過激光定位裝置獲取6組目標點，取末端破碎錘與地面垂直的姿態為最終姿態，通過實驗數據表1可發現定位誤差在5 cm內，定位時間為10 s，且可調。通過實驗可證明該控制系統軟件滿足了破拆機器人機械臂運動自動化的目標。

表1 控制系統軟件實驗數據

[1] 劉慶運. 破拆救援機器人關鍵技術[J]. 現代制造工程, 2009,30(7):149-153.

[2] 楊政, 尚建忠，王彪，等. 液壓驅動機械臂的軌跡規劃[J].

機械研究與應用,2009,25(4):47-51.

[3] 周孟然,吳永祥.基于紅外激光定位技術的礦井提升機位置跟蹤系統的研究[J].煤炭學報,2002,27(6):658-660.

[4] 胡敏,梁聰慧，鄭慶華，等.多關節液壓臂架實時控制軟件的設計與實現[J].中國工程機械學報,2013,11(3):243-247.

[5] 蔡自興. 機器人學[M]. 北京：清華大學出版社, 2000.

[6] CRAIG J. Introduction to robotics:mechanics and control (third edition)[M].Pearson Education,Inc.,2005.

[7] 陸震. 冗余自由度機器人原理及應用[M]. 北京：機械工業出版社,2006.

[8] XIANG J, ZHONG C, WEI W. A varied weights method for the kinematic control of redundant manipulators with multiple constraints[J]. IEEE Transactions on Robotics, 2012,28(2):330-340.

[9] CARRILER W F, KHOSLA P K， KROGH B H. Path planning for mobile manipulators for multiple task exceution[J]. IEEE Transactions on Robotics and Automation, 1991,7(3):403-408.

[10] FINZEL R, HELDUSER S, JANG D S. Electro-hydraulic control systems for mobile machinery with low energy consumption[C].Proceeings of the Seventh International Conference on Fluid Power Transmission and Control, 2009:214-219.

Software design of demolish robot manipulator autonomous motion control system

He Feng1，2，Zhang Xiaojian1，2，Zhao Jianghai1，2

(1.Robotic Lab, Institute of Advanced Manufacturing Technology, Changzhou 213164, China;2.Robotic Lab, Institute of Advanced Manufacturing Technology, Hefei Institutes of Physical Science, Chinese Academy of Sciences, Changzhou 213164，China)

Aiming at the problems that the existing demolish robot has long manual positioning time and inaccurate positioning, the control system software of the demolish manipulator autonomous motion which based on laser-positioning is given. The paper has designed the kinematic model, close-loop model, hydraulic device controller model, hand-eye calibration model and motion controller model. The application results show that the software system can complete the design indexes of the control system and improve the work efficiency.

demolish robot; kinematic; autonomous motion

國家科技支撐計劃(2015BAK06B02)；江蘇省產學研合作計劃(BY2016040-02)

TP273

A

10.19358/j.issn.1674- 7720.2017.09.030

何鋒，章小建，趙江海.破拆機器人機械臂自主運動控制系統軟件設計[J].微型機與應用，2017,36(9)：103-105.

2016-12-14)

何鋒(1977-)，男，碩士，工程師，主要研究方向：機器人。