機(jī)械臂腕力傳感器負(fù)載端重力補(bǔ)償算法與仿真

劉巖 武俊峰 夏科睿 馬常友

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

關(guān)鍵詞：機(jī)械臂；腕力傳感器；重力補(bǔ)償；仿真

DOI：10.15938/j.jhust.2018.02.014

中圖分類號： TP241.3

文獻(xiàn)標(biāo)志碼： 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 引 言

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

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

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

1 傳感器負(fù)載端重力補(bǔ)償方法與測量

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

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

2 仿真實(shí)驗(yàn)與分析

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

2.1 仿真平臺的搭建

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

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

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

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

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

首先，將該軌跡分兩段分別進(jìn)行的直線和圓弧位置插補(bǔ)運(yùn)算：第一段軌跡由點(diǎn)P1到P2，進(jìn)行直線位置插補(bǔ)運(yùn)算；第二段軌跡由點(diǎn)P2、P3、P4組成，進(jìn)行空間圓弧位置插補(bǔ)運(yùn)算。在笛卡爾空間下各點(diǎn)的空間坐標(biāo)參見表1。

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

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

2.3 傳感器負(fù)載端重力補(bǔ)償仿真實(shí)驗(yàn)與分析

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

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

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

2.4 機(jī)械臂末端作用力解算仿真實(shí)驗(yàn)與分析

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

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

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

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

3 結(jié) 論

本文對基于六自由度機(jī)械臂腕部六維力傳感器的零位標(biāo)定進(jìn)行了研究，得出如下結(jié)論：

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

2）建立了腕力傳感器重力補(bǔ)償系統(tǒng)模型，結(jié)合機(jī)械臂運(yùn)動學(xué)，推導(dǎo)出腕力傳感器負(fù)載端重力補(bǔ)償算法和機(jī)械臂末端作用力解算方法。

3）搭建了移動機(jī)械臂運(yùn)動學(xué)仿真平臺，對采用機(jī)械臂負(fù)載端重力補(bǔ)償算法前后進(jìn)行仿真對比實(shí)驗(yàn)，驗(yàn)證了該算法的正確性。

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

參 考 文 獻(xià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]繆新， 田威. 機(jī)器人打磨系統(tǒng)控制技術(shù)研究[J]. 中國空間科學(xué)技術(shù)， 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è)機(jī)器人主動柔順裝配系統(tǒng)研究[D]. 廣州：華南理工大學(xué)，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]黎佳駿. 面向核電救災(zāi)的機(jī)械臂開門擰閥門力學(xué)測試平臺研究[D]. 哈爾濱：哈爾濱工業(yè)大學(xué)，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]. 儀器儀表學(xué)報(bào)， 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]王志軍. 雙層預(yù)緊式六維力傳感器基礎(chǔ)理論與應(yīng)用研究[D]. 秦皇島：燕山大學(xué)， 2012.

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

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

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

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

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

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

[17]王帥， 梅濤， 趙江海. 新型六自由度機(jī)械臂的運(yùn)動學(xué)分析與仿真驗(yàn)證[J]. 計(jì)算機(jī)工程與設(shè)計(jì)， 2014， 35（9）：3213-3218.

[18]卓揚(yáng)娃， 白曉燦， 陳永明. 機(jī)器人的三種規(guī)則曲線插補(bǔ)算法[J]. 裝備制造技術(shù)， 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)機(jī)器人運(yùn)動優(yōu)化與軌跡跟蹤控制研究 [D]. 杭州：浙江大學(xué)， 2009.

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

[22]許家忠， 王溫強(qiáng)， 謝文博等. 故障機(jī)械臂模型重構(gòu)后的軌跡規(guī)劃與實(shí)驗(yàn)分析[J]. 哈爾濱理工大學(xué)學(xué)報(bào)， 2016， 21（2）： 1-7.

（編輯：溫澤宇）