機械臂腕力傳感器負載端重力補償算法與仿真

劉巖 武俊峰 夏科睿 馬常友

摘 要：針對機械臂運動時負載端姿態(tài)變化影響腕力傳感器測量結果問題，提出一種腕力傳感器負載端重力補償算法。通過分析機械臂負載端不同姿態(tài)下的重力變化形式，采用實驗方法測定負載端重力與重心參數(shù)。建立了腕力傳感器重力補償系統(tǒng)模型，結合機械臂運動學方程，推導出腕力傳感器負載端重力補償算法和機械臂末端作用力解算方法，并應用算例進行MATLAB與ADAMS聯(lián)合仿真。仿真結果表明，本補償算法能有效地消除負載端姿態(tài)變化影響，通過腕力傳感器準確地測量機械臂所受的外力信息，驗證了機械臂腕力傳感器負載端重力補償算法的正確性。

關鍵詞：機械臂；腕力傳感器；重力補償；仿真

DOI：10.15938/j.jhust.2018.02.014

中圖分類號： TP241.3

文獻標志碼： A

文章編號： 1007-2683（2018）02-0078-08

Abstract：Aiming at the problem that the measuring results of wrist force sensor can be influenced by load end′s attitude change when mechanical arm moves， this paper proposes a load gravity compensation algorithm for wrist force sensor.According to gravity variation of the load end in different mechanical arm posture， the relationship between the gravity of load end and the position of the center of gravity can be measured by experiment. We build a wrist force sensor gravity compensation model， and combining manipulator kinematics equation， we deduct a load gravity compensation algorithm for wrist force sensor and a calculation method for force acting on the end of mechanical arm. Specific examples have been tested by adopting the ADAMS and MATLAB joint simulation method. The results show that this compensation algorithm can effectively eliminate the influence of the attitude change of load end， the external force information of mechanical arm can be measured by wrist force sensor.And the results verify the correctness of the load gravity compensation algorithm for wrist force sensor.

Keywords：mechanical arm； the wrist sensor； gravity compensation； simulation

0 引 言

如今機器人廣泛地應用到工業(yè)、軍事、航天等自動化領域。為使機器人更好的順應接觸的作業(yè)環(huán)境，一般會在機器人上安裝力傳感器，通過采集的力信息作為反饋條件，結合柔順控制算法完成作業(yè)。如工業(yè)機器人完成精密的裝配、磨光、切削等任務時，用以修正機械臂末端的運動軌跡，使其達到理想效果[1-3]；再如操作員為了通過施加較小的力，直觀調(diào)整機械臂末端的姿態(tài)，實現(xiàn)機械臂的隨動[4-5]；還可以分析機核電站救災機器人械臂作業(yè)時各關節(jié)受力情況，優(yōu)化控制策略[6]。所以當機器人與外界環(huán)境交互時，不僅依靠力傳感器的反饋信息實現(xiàn)機器人高精度的柔順作業(yè)，同時還確保機器人本體的安全性。

目前，國內(nèi)外學者對力傳感器的性能指標做了大量的研究工作，包括力傳感器的靜態(tài)標定和動態(tài)標定，測量結果的準確性和可靠性都有一定的提高和改善[7-9]。但是當機械臂與六維力傳感器配合使用時，安裝的轉接機構會對六維力傳感器讀數(shù)產(chǎn)生影響。如果機械臂的負載端重力較小，可以不考慮負載端重力對六維力傳感器力和力矩讀數(shù)的影響[5]。當機械臂負載端重力較大時，對力傳感器讀數(shù)產(chǎn)生的影響一般不可忽略。負載端對傳感器的受力敏感程度也會因外力的作用位置不同分為3個作用區(qū)域[10]。在控制精密要求不高時，根據(jù)機械臂所受外界力的作用區(qū)域，忽略力或力矩的影響[4][11]。浙江大學吳文祥針對摩擦引起機器人低速時運動性能惡化、作業(yè)精度差的問題，采用自適應魯棒控制算法補償機械臂關節(jié)摩擦[12]。東南大學陳建元早些年利用增加姿態(tài)敏感單元的方法獲取手爪空間姿態(tài)，在腕力傳感器上增加三維直線加速度計，再通過信息處理來消除影響[13]。該方法適用于一般早期工業(yè)機械臂，如今不需要借助外界傳感器獲取機械臂末端姿態(tài)，此外對于機構精密的機械臂、內(nèi)部線路集成化，安裝配合也不利于實現(xiàn)。

本文以帶有腕力六維力傳感器的六自由機械臂模型為研究對象，在空間中多次測定不同姿態(tài)下六維力傳感器數(shù)據(jù)，采用最小二乘法擬合數(shù)據(jù)，獲得負載端的重力與重心參數(shù)。通過建立腕力傳感器重力補償系統(tǒng)模型，結合機械臂運動學方程，推導出腕力傳感器負載端重力補償算法，修正讀數(shù)，進而得出機械臂末端作用力的解算方法，準確獲取機械臂末端接觸力，最后通過仿真驗證機械臂腕力傳感器負載端重力補償算法的正確性。

1 傳感器負載端重力補償方法與測量

1.1 傳感器負載端重力大小與重心位置測量方法

對傳感器負載端重力進行補償，需要明確腕力傳感器負載端重力大小和重心位置。一般將六維力傳感器安裝在機械臂末端的腕部，再通過轉接裝置與執(zhí)行器相連，轉接機構的復雜性使得傳感器負載端重力與重心位置難以確定。

2 仿真實驗與分析

仿真實驗包括兩方面內(nèi)容，首先根據(jù)機械臂運動學方程與軌跡規(guī)劃算法，規(guī)劃出機械臂末端運動曲線；然后應用具體算例，驗證六維力傳感器負載端重力補償方法和機械臂末端作用力解算方法的正確性，為后續(xù)物理樣機的程序開發(fā)打下基礎。

2.1 仿真平臺的搭建

多體系統(tǒng)動力學分析軟件Adams具有強大的運動學和動力學仿真及運算功能，但是在Adams軟件中很難精準建立較為復雜的三維模型，所以憑借SolidWorks軟件精準、快速的建模功能，事先在SolidWorks軟件中設計并建立六自由度機械臂與移動平臺的三維實體模型[14-15]。將三維模型保存為x_t文件形式，導入Adams軟件。六自由度機械臂的三維實體模型，如圖4所示。

模型導入到Adams軟件后，需要對每個零件的質(zhì)量、材料等參數(shù)進行設置，與真實機械臂參數(shù)保持一致。機械臂底座中心點所在的坐標系為基坐標系，用固定副將機械臂底座固定在大地上。機械臂六個關節(jié)均為旋轉關節(jié)采用轉動副進行連接，其它機構均采用固定副連接，這樣機械臂每兩個零件之間都定義好了約束關系，確保機械臂運動方式正確。其中六維力傳感器安裝在機械臂腕部位置，末端裝有兩指夾持器。虛擬樣機中的移動機械臂模型，如圖5所示。

為了實現(xiàn)對機械臂運動控制，首先采用D-H模型表示機械臂六個關節(jié)坐標系，利用關節(jié)間坐標系的變換，求取機械臂正逆運動學方程[16-17]。并完成機械臂末端運動軌跡的規(guī)劃，以及關節(jié)空間規(guī)劃，實現(xiàn)了機械臂末端在笛卡爾空間的直線、圓弧等運動[18-20]。應用Matlab軟件的強大的計算功能，建立M文件，設計并編寫控制程序算法，包括邏輯控制程序、機械臂運動學、軌跡規(guī)劃及六維力傳感器負載端重力補償?shù)人惴╗21-22]。

2.2 機械臂運動軌跡規(guī)劃與仿真

機械臂末端是否變姿態(tài)運動對負載端重力補償起決定性作用，要驗證補償算法的正確性，需對機械臂規(guī)劃一段變姿態(tài)的運動軌跡。本文在機械臂常作業(yè)的工作空間內(nèi)，規(guī)劃一段由直線和空間圓弧組成的變姿態(tài)的運動軌跡，并對運動軌跡進行位置和姿態(tài)插補算法。

首先，將該軌跡分兩段分別進行的直線和圓弧位置插補運算：第一段軌跡由點P1到P2，進行直線位置插補運算；第二段軌跡由點P2、P3、P4組成，進行空間圓弧位置插補運算。在笛卡爾空間下各點的空間坐標參見表1。

運用Matlab軟件完成機械臂末端的上述軌跡規(guī)劃，在兩段軌跡插入足夠數(shù)量的點數(shù)，確保機械臂末端軌跡仿真運動的平滑。應用機械臂運動學逆解方程，在笛卡爾空間下求取運動軌跡中每一點坐標對應的六組關節(jié)角，確定6個關節(jié)隨時間運動的角度，將角度擬合成曲線如圖6所示，機械臂末端從點P1運動到P4，運動時間為40s，將6個關節(jié)隨時間運動的關節(jié)角的函數(shù)錄入到在Adams軟件中，應用SPLINE函數(shù)作為機械臂各關節(jié)運動變化的規(guī)律曲線。

在軟件中獲取仿真中機械臂末端在基坐標系{O}下的運動軌跡的空間坐標值，與規(guī)劃的軌跡進行比對，軌跡各軸誤差小于0.01mm，可忽略不計。仿真軌跡的曲線如圖7所示。

2.3 傳感器負載端重力補償仿真實驗與分析

仿真需要明確負載端重力大小和重心位置，可利用Adams軟件對負載端的質(zhì)量及質(zhì)心進行設定。在實際應用中，可參照1.1節(jié)介紹的方法進行計算。機械臂的負載端由末端執(zhí)行器和轉接件兩部分組成，末端執(zhí)行器質(zhì)量m1=0.94kg，轉接件質(zhì)量m2=0.20kg，負載端總質(zhì)量即兩者質(zhì)量總和約為m=1.14kg。負載端在坐標系{S}下的重心坐標為（10，10，32），單位mm。利用2.2節(jié)規(guī)劃的直線和圓弧運動軌跡，令機械臂做空載運動，六維力傳感器的數(shù)據(jù)如圖8所示。

由圖8可見，負載端重力對六維力傳感器讀數(shù)的影響，為了消除這種影響，需要在機械臂規(guī)劃的軌跡運動過程中根據(jù)機械臂正運動學方程，確定機械臂末端每一時刻的姿態(tài)矩陣OSR。并將已知量負載端總質(zhì)量、重心位置代入式（13），即可求解出運動過程中每一時刻傳感器負載端重力補償數(shù)值，與六維力傳感器實際值作差，實時標定六維力傳感器的零位值，標定的誤差曲線如圖9所示。

仿真結果表明，當機械臂空載運動時，傳感器負載端重力補償算法解算出的結果與負載端對傳感器讀數(shù)的影響結果僅存在很小的誤差，力誤差小于0.01N，力矩誤差小于0.02N·m。其誤差主要來源于仿真時機械臂運動軌跡插補精度，這個數(shù)據(jù)是仿真時，機械臂未抓取重物時任意方向的最大誤差，補償誤差在允許范圍內(nèi)，所以傳感器負載端重力補償算法可有效地消除負載端對機械臂空載運動的影響。

2.4 機械臂末端作用力解算仿真實驗與分析

機械臂末端作用力解算仿真實驗，令機械臂末端夾持器抓取已知質(zhì)量的重物，對采用負載端重力補償算法前后兩組仿真實驗數(shù)據(jù)進行對比分析，驗證是否可以直接獲取機械臂抓取重物的重量大小。仿真算例中，機械臂夾持器抓取2kg的重物，其重力約為19.6N，在Adams仿真平臺中，移動機械臂抓取重物示意圖，如圖10所示。

機械臂末端運動同樣采用2.2節(jié)所規(guī)劃的軌跡，圖11（a）、（b）是在0～20s的時間內(nèi)，機械臂抓取重物運動一段變姿態(tài)的直線軌跡；圖11（c）、（d）是在20～40s時間內(nèi)，機械臂抓取重物運動一段變姿態(tài)的圓弧軌跡。分別驗證負載端重力補償算法前后兩組對比實驗，待兩組仿真實驗運動完成后，在Adams軟件中獲取機械臂腕部六維力傳感器力與力矩的測量值。

采用力傳感器負載端重力補償算法前后，六維力傳感器的力和力矩數(shù)據(jù)曲線分別如圖12（a）、圖12（b）所示。在機械臂末端抓取重物變姿態(tài)的運動過程中，當沒有進行負載端重力補償時，由于六維力傳感器負載端重物的干擾，不能準確的讀出機械臂末端所受的外界力信息。

根據(jù)式（17）可以計算出機械臂末端作用力，為了直觀的觀察解算的的測力結果，將機械臂傳感器的測試力信息轉換到基坐標系{O}下，如圖13所示。由圖像可知，在基坐標系{O}下，Z軸方向的讀數(shù)FZ=-19.6N，即機械臂抓取物體的重量，準確地獲得了外界對機械臂末端的作用力。

3 結 論

本文對基于六自由度機械臂腕部六維力傳感器的零位標定進行了研究，得出如下結論：

1）對機械臂負載端姿態(tài)變化對六維力傳感器零位值的影響進行分析，采用實驗方法可以得到機械臂負載端重力大小與重心位置。

2）建立了腕力傳感器重力補償系統(tǒng)模型，結合機械臂運動學，推導出腕力傳感器負載端重力補償算法和機械臂末端作用力解算方法。

3）搭建了移動機械臂運動學仿真平臺，對采用機械臂負載端重力補償算法前后進行仿真對比實驗，驗證了該算法的正確性。

本方法解決了機械臂負載端重力過大對腕部六維力傳感器讀數(shù)的影響。機械臂在角加速度較低的變姿態(tài)運動時，忽略運動時慣性力，消除負載端重力對力傳感器的影響在原理上可行，并經(jīng)過仿真驗證了該方法的有效，可以普遍應用到基于力反饋的重型工業(yè)機器人及其他類似的作業(yè)環(huán)境中。

參 考 文 獻：

[1]DIETRICH F， BUCHHOLZ D， WOBBE F， et al. On Contact Models for Assembly Tasks： Experimental Investigation Beyond the Peg-in-hole Problem on the Example of Force-torque Maps[C]// Intelligent Robots and Systems （IROS）， 2010 IEEE/RSJ International Conference on. IEEE， 2010： 2313-2318.

[2]繆新， 田威. 機器人打磨系統(tǒng)控制技術研究[J]. 中國空間科學技術， 2014（11）：8-14.

[3]LIANG W， SONG Y， LV H， et al. A Novel Control Method for Robotic Belt Grinding Based on SVM and PSO Algorithm[C]// International Conference on Intelligent Computation Technology and Automation（ICICTA）， IEEE， 2010（1）：258-261.

[4]林君健. 基于力傳感器的工業(yè)機器人主動柔順裝配系統(tǒng)研究[D]. 廣州：華南理工大學，2013.

[5]BISCHOFF R， KURTH J， SCHREIBER G， et al. The KUKA-DLR Lightweight Robot Arm-a New Reference Platform for Robotics Research and Manufacturing[C]//Robotics （ISR）， 2010 41st international Symposium on and 2010 6th German Conference on Robotics （ROBOTIK）. VDE， 2010： 1-8.

[6]黎佳駿. 面向核電救災的機械臂開門擰閥門力學測試平臺研究[D]. 哈爾濱：哈爾濱工業(yè)大學，2014.

[7]XU K J， LI C， ZHU Z N. Dynamic Modeling and Compensation of Robot Six-axis Wrist Force/torque Sensor[J]. Instrumentation and Measurement. IEEE Transactions on， 2007，56（5）：2094-2100.

[8]姜力， 劉宏， 蔡鶴皋. 多維力/力矩傳感器靜態(tài)解耦的研究[J]. 儀器儀表學報， 2004，25（3）：284-287.

[9]HOWARD C Q， HANSEN C H. Six-axis Vibrational Power Transducer for Active Vibration Isolation[C]//Proceedings of acoustics 2005： Annual conference of the Australian Acoustical Society， 2005：4-6.

[10]王志軍. 雙層預緊式六維力傳感器基礎理論與應用研究[D]. 秦皇島：燕山大學， 2012.

[11]干方建. 基于多維力傳感器的機器人動態(tài)特性若干問題的研究[D]. 合肥：合肥工業(yè)大學， 2003.

[12]吳文祥， 朱世強， 王宣銀， 等. 基于摩擦模糊建模與補償?shù)臋C器人低速控制[J]. 電機與控制學報， 2013， 17（8）： 46-53.

[13]陳建元. 一種可消除機器人手爪姿態(tài)變化對腕力傳感器影響的方法探討[J]. 機器人，1992，14（6）：29-33.

[14]馬如奇， 郝雙暉， 鄭偉峰. 基于MATLAB與ADAMS的機械臂聯(lián)合仿真研究[J]. 機械設計與制造， 2010， 4（4）： 93-95.

[15]宗曉萍， 李月月. 基于ADAMS和MATLAB 的機械臂控制仿真[J]. 微計算機信息， 2009 （35）： 29-30.

[16]秦利， 劉福才， 金振林， 等. 仿人機器人混聯(lián)靈巧手臂設計及運動學逆解研究[J]. 電機與控制學報， 2014， 18（12）： 102-107.

[17]王帥， 梅濤， 趙江海. 新型六自由度機械臂的運動學分析與仿真驗證[J]. 計算機工程與設計， 2014， 35（9）：3213-3218.

[18]卓揚娃， 白曉燦， 陳永明. 機器人的三種規(guī)則曲線插補算法[J]. 裝備制造技術， 2009 （11）： 27-29.

[19]COSTANTINESCU D， CROFT E A. Smooth and Time-optimal Trajectory Planning for Industrial Manipulators Along Specified Paths[J]. Journal of Robotic Systems， 2000， 17（5）： 233-249.

[20]劉松國. 六自由度串聯(lián)機器人運動優(yōu)化與軌跡跟蹤控制研究 [D]. 杭州：浙江大學， 2009.

[21]孫銘. 基于MATLAB和ADAMS的機械臂的軌跡規(guī)劃與協(xié)調(diào)控制[D]. 北京：北京化工大學， 2015.

[22]許家忠， 王溫強， 謝文博等. 故障機械臂模型重構后的軌跡規(guī)劃與實驗分析[J]. 哈爾濱理工大學學報， 2016， 21（2）： 1-7.

（編輯：溫澤宇）