機器人運動中六維力傳感器的重力補償研究

王志軍，韓靜如，李占賢，劉立偉

（華北理工大學 機械工程學院，河北 唐山 063009）

1 引言

六維力傳感器能夠檢測大小和方向不斷變化的三維力和三維力矩信息，一般安裝在工業機器人末端，協助機器人完成力/位置控制、輪廓跟蹤、軸孔配合等一些精細復雜的操作，在機器人中具有廣泛應用[1-3]。但是，六維力傳感器在隨機器人運動的過程中，由于其位姿不斷發生變化，同時受安裝在傳感器上的操作工具重力的影響，傳感器的零位值也會隨之不斷變化，這就導致傳感器測量上產生一定偏差，降低了其測量精度，進而影響機器人的操作。因此，有必要對機器人運動中的六維力傳感器進行重力補償。

關于不同情況下的重力補償，國內一些學者也分別進行過理論算法推導或相關實驗研究等，并取得了一定的研究成果。文獻[4]提出了基于最小二乘法的參數識別方法，可用于對機器人臂的重力補償；文獻[5]采用矢量分解的方法推導出一種機器人的重力補償算法；文獻[6]利用拉格朗日方程的方法對機構進行重力補償的理論計算，實現了對力覺交互設備的重力補償；文獻[7]通過對機器人建立動力學方程，分析得出重力補償可使控制器克服重力，從而使機器人到達期望位置。以上這些研究從不同出發點和落腳點，根據各自不同情況分析或推導了機械臂重力補償的算法、驗證及控制等問題，證明了重力補償在機器人操作中的重要和必要性，對重力補償研究具有一定的學習和借鑒意義。

針對機器人姿態變化導致末端工具重力影響六維力傳感器零位值的問題，在結合前人研究的基礎上，提出了一種簡單有效的重力補償算法，利用機器人各連桿變換矩陣和不同坐標系的力坐標變換，消除六維力傳感器測量中工具重力的影響，能夠實時補償傳感器的零位值，使傳感器在機器人運動過程中也能檢測出末端實際所受外力的大小，從而使機器人能夠完成更加精密的操作任務。

2 重力補償算法推導與分析

工業機器人在一些主動柔順作業中，機械臂的腕關節上會通過一個六維力傳感器來連接末端執行工具，完成各項操作任務[8]。傳感器能夠測得末端工具所受的外力/力矩，進而反饋給控制系統控制機器人實現柔順運動。在這種情況下，為了消除工具自身重力對傳感器示數造成的影響，得到準確的外力反饋信息，我們需要以機器人本體、六維力傳感器、末端工具為研究平臺，推導和研究相應的補償算法對傳感器測量結果進行重力補償。

2.1 機器人各連桿的變換矩陣

以某公司的TAWERS-1800機器人為研究對象來推導重力補償算法，該機器人共有六個關節，每個關節均為轉動關節。

由D-H法建立基坐標系｛0｝及各關節坐標系｛i｝，如圖1所示，同時定義各連桿的參數與變量，并列出連桿參數表，如表1所示。

圖1 機器人的連桿坐標系Fig.1 Connecting Rod Coordinate System of the Robot

表1 機器人的連桿參數Tab.1 Connecting Rob Parameters of the Robot

連桿變換矩陣i-1iT的通式為：

根據表1的各連桿參數，帶入上述通式（1），可得到各連桿的位姿變換矩陣將以上各連桿變換矩陣相乘，可得到機器人腕部坐標系相對于基坐標的位姿變換矩陣：

2.2 重力補償算法的推導

定義傳感器坐標系｛S｝及工具坐標系｛T｝與空間基坐標系｛0｝的位姿關系，如圖2所示。

圖2 機器人傳感器及末端工具坐標系Fig.2 Sensor and Tool Coordinate System of the Robot

傳感器坐標系｛S｝相對機器人腕部坐標系｛6｝沿z6正方向平移距離h，工具坐標系｛T｝與傳感器坐標系｛S｝姿態相同，坐標原點 OT到坐標系｛S｝的 x 軸，y 軸，z軸投影分別為 px，py，pz，則傳感器及末端工具與相鄰坐標系的變換矩陣分別為：

由以上各位姿變換矩陣，最終可以得到機器人末端工具相對空間基坐標系｛0｝的變換矩陣為：

設機器人末端工具重力的大小為G，方向沿空間基坐標系｛0｝的z0軸負方向，重心在工具坐標系｛T｝的坐標原點，則在基坐標系｛0｝下，重力的三維力分量矩陣恒為：0f=［0 0 G］T。

機器人運動過程中，隨著末端工具的位置和姿態不斷改變，其重力在工具坐標系｛T｝下的三維分量會發生變化，相應地，傳感器的零位值也會隨之變化。因此，我們需要推導出末端工具重力在傳感器坐標系｛S｝下的三維力和力矩值，即在測量機器人所受外力之前，對傳感器零位位置的重力補償值。

由式（4）可知工具坐標系相對于基坐標系的旋轉矩陣，則重力在工具坐標系｛T｝下的三維力分量為：

因為重心在工具坐標系｛T｝的坐標原點，所以重力在｛T｝坐標系三軸方向的力矩為0。因此，重力在｛T｝坐標系下的六維力/力矩為：

根據文獻[9]中力和力矩矢量在不同坐標系中的變換關系，可得重力在傳感器｛S｝與工具｛T｝坐標系中的關系式為：

由此可得重力在傳感器坐標系｛S｝下的六維力/力矩，即最終所需要的重力補償值為：

六維力傳感器測得的力和力矩值，是工具自身重力與其所受外力共同作用的結果。通過式（9）中求得的操作工具重力隨機器人位姿變化在六維力傳感器上的力/力矩值，即需要進行的重力補償值，對傳感器測得示數進行補償，能夠消除由機器人姿態變化引起的末端工具重力影響，提高六維力傳感器的測量精度。

當機器人處于某一位姿下，末端工具受到外力時，如果六維力傳感器上力與力矩分量的讀數分別用Sfc、Smc表示，那么經過重力補償，最終實際機器人末端所受外力/力矩信息f、m為：

由式（10）可以看到，通過重力補償，我們可以準確得到機器人末端執行工具所受的外力/力矩，將末端工具實際所受外力反饋到控制系統，就能控制機器人按照操作人員的意圖實現主動柔順操作。

2.3 重力補償算法的數值算例

根據上文推導的重力補償算法式（9），下面結合具體算例，運用MATLAB軟件進行重力補償理論推導的數值計算與分析。由TAWERS-1800機器人的結構尺寸，可知各連桿變換所需參數 a1，a2，a3，d5，a5，d6的值；設機器人末端工具重心相對傳感器坐標系的位置為SPTO=［115-50 55］T，重力大小為100N，沿基坐標系｛0｝的Z0軸負方向；在機器人各關節工作角度范圍內，取θ1∈［0，-π/6］rad，θ2∈［-π/2，-2π/3］rad，θ3∈［0，-π/6］rad，θ4∈［0，-π/6］rad，θ5∈［π/2，π/3］rad，θ6∈［0，-π/6］rad 各關節轉動角速度為3rad/s。將以上參數帶入式（9），運用MATLAB編程計算并生成重力在傳感器坐標系x，y，z三個方向的力和力矩補償的理論計算值，如圖3所示。

圖3 重力補償的計算值Fig.3 The Calculated Value of Gravity Compensation

從圖中可以看出，隨著機器人的運動，工具重力在六維力傳感器上的力和力矩隨機器人的姿態不斷變化，通過重力補償結果能夠對六維力傳感器的零位值進行實時補償，從而消除了由機器人姿態變化引起的末端工具重力影響，提高了六維力傳感器的測量精度。

3 重力補償仿真分析

Adams具有強大的運動學和動力學仿真功能，使用該軟件可以產生復雜機械系統的虛擬樣機，真實地仿真其運動過程，進行相關分析[10]。運用Adams軟件進行重力補償相關仿真，目的是希望通過對機器人的運動仿真，測得在機器人運動中傳感器所在坐標系的六維力/力矩值，與我們推導出的重力補償算法的理論計算值進行比較，驗證推導的重力補償算法是否正確。

首先將簡化的TAWERS-1800機器人三維模型導入Adams中，如圖4所示，然后定義各零部件的材料、質量等相關屬性，使仿真模型盡可能與TAWERS-1800機器人實際物理屬性相近，最后定義模型的各種約束及驅動，進行運動仿真。

圖4 機器人仿真模型Fig.4 Robot Simulation Model

按照2.3節所描述的運動，定義機器人各關節旋轉驅動函數為：Funcition（time）=3.0d*time，設置仿真時間為 t=10s，仿真步數step=500進行仿真，并測量傳感器坐標系三個方向的力和力矩，生成六維力傳感器的力與力矩測量曲線，如圖5所示。

圖5 重力補償的測量值Fig.5 The Measured Value of the Gravity Compensation

通過與圖3對比可知，六維力傳感器的力和力矩仿真測量曲線與重力補償的理論計算值基本一致，從而驗證了推導重力補償算法的正確性。

4 結論

通過對機器人、六維力傳感器及其末端執行工具的位姿變換進行分析，推導得到一種簡單有效的六維力傳感器重力補償算法，并運用MATLAB、Adams軟件對機器人運動中六維力傳感器的重力補償分別進行理論計算與仿真分析，得到了末端執行工具重力隨機器人位姿變化，實時作用在六維力傳感器上的力/力矩值曲線，通過比較可以看出重力補償算法的理論計算與仿真測量的結果曲線基本一致，最終驗證了該重力補償算法的正確性和可行性。該重力補償算法能夠消除由機器人姿態變化引起的末端工具重力影響，提高六維力傳感器的測量精度，實時獲得機器人運動中實際作用在機末端執行機構的外力/力矩信息，進而反饋給控制系統以實現機器人的主動柔順運動。