機器人參數辨識和計算力矩法軌跡控制研究*

賴 含，韓奉林，胡金飛

（1.中南大學機電工程學院，長沙 410012；2.浙江大學機械工程學院，杭州 310027）

0 引言

隨著對機器人運動速度、載荷、軌跡控制精度和穩定性等性能的要求越來越高，單純基于運動學的機器人軌跡控制方法已不能滿足要求，必須在機器人軌跡控制中考慮機器人的動力學特性。

在眾多的機器人動態軌跡控制算法中，計算力矩法采用前饋與反饋相結合的控制策略，具有較好的穩定性[1]。目前，針對計算力矩法軌跡控制的研究，一方面為方法論的完善和創新，如劉德滿等[2]將計算力矩法和自適應控制相結合；王東署等[3]把神經網絡用于計算力矩法，進一步提升實際工業過程的控制精度。另一方面為計算力矩法的工業應用，如梁捷等[4]將計算力矩法用于空間機械臂運動控制，發現該種方法可以節省資源，同時控制精度很高；Sharkawy等[5]展開了兩自由度機器人的計算力矩法控制研究；龔捷等[6]研究了基于計算力矩法的裝載機工作裝置軌跡控制，并針對三維模型進行了控制仿真。

計算力矩法是一種基于動力學模型的控制方法，在機械臂的數學模型及相關參數確定以后，可以實現精確控制。實施計算力矩法控制的前提是需要知道獲得機器人慣性參數及關節摩擦狀態，但由于制造、裝配誤差、附屬零件等多種原因，上述參數難以直接通過幾何設計信息準確測算，往往必須通過在線辨識獲取。周軍等[7]提出了一種柔性關節模塊機器人動力學模型參數辨識方法，蘇二虎等[8]進行了考慮電機慣量的機器人動力學參數辨識研究。

為便于計算力矩法的進一步推廣應用，本文系統地提出了包括動力學參數辨識、動態軌跡控制在內的完整實施方法，建立了參數辨識和動態軌跡控制的理論模型，并以平面三自由度機器人為例，開展了數值仿真和實驗研究。

1 三自由度機器人的運動學和動力學建模

1.1 運動學建模

圖1 三自由度機器人模型

平面三自由度機器人在工業中應用廣泛，且結構相對簡單，是研究動態軌跡控制算法的理想對象。考慮如圖1 所示的平面三自由度機器人，運用Danevit-Hartenberg 法[9]建立如圖1 所示坐標系，采用圖中標識的符號，其中bi（i=1,2,3）指各桿質心據桿前端坐標系沿連桿的距離，lx指偏距，可以得到如表1所示的機器人D-H參數。

表1 三自由度機器人D-H參數

若末端執行器在笛卡爾空間中的位姿由向量(x,y,φ)表示，則(x,y,φ)與θ1、θ2、θ3的關系：

式中：si=sinθi；ci=cosθi；sij=sin(θi+θj)；cij=cos(θi+θj)；sijk=sin(θi+θj+θk)；cijk=cos(θi+θj+θk)。

下面確定x˙、y˙、φ˙與θ˙1、θ˙2、θ˙3的關系：

式中：雅可比矩陣為：

1.2 動力學建模

如圖1 所示，令i=1,2,3，設各桿質量為mi，各桿繞質心坐標系（與前一關節坐標系平行但原點位于質心處的坐標系）Z 軸的轉動慣量為Ji。考慮實際情況，引入各桿傳動裝置的等效轉動慣量Ii，各關節所受動摩擦力fvi，靜摩擦力fci。再利用拉格朗日法建立機器人動力學方程，根據文獻[10]，動力學方程可線性參數化：

式中： D(θ)為慣性矩陣；C(θ,) 為科里奧利和離心力矩陣； g(θ)為重力項矩陣；f=fvi+fci為關節摩擦力； β 為最小參數集向量，對于本文中的三自由度機器人，動力學參數可整合為15個最小參數集，如表2所示；Y 為最小參數集的系數矩陣，該矩陣的各項元素均為關節變量的函數。

表2 三自由度機器人動力學最小參數集

矩陣D(θ)、C(θ,θ˙)、g(θ)均可表示為最小參數集的線性組合，其中D(θ)為對稱矩陣。各矩陣每項元素如下所示：

2 激勵軌跡規劃和動力學參數辨識

2.1 激勵軌跡規劃

前文所述動力學模型有未知動力學參數，所以應先辨識出機器人的動力學參數。為降低測量噪聲的影響，使辨識出的數據更加準確，需先優化激勵軌跡。文獻[11]表明，每個關節的激勵軌跡可為N個諧波正弦和余弦函數的有限和。

式中：ωf是傅里葉級數的基頻，周期Tf=2π/ωf，所有關節基頻相同，以保證整個機器人激勵的周期性。每個關節的激勵軌跡包含2N+1個參數，正、余弦函數的幅值以及位置軌跡的偏移量ci。

根據式（4），對于第n個樣本點，動力學方程可寫為：

若共有m個樣本點，動力學方程可寫為矩陣形式：

圖2 激勵軌跡位移圖

圖3 激勵軌跡速度圖

圖4 激勵軌跡加速度圖

2.2 參數辨識實驗

圖5 實驗中采用的柯馬Racer3機器人

柯馬Racer3 機器人當關節1、4、6 鎖定于零位，關節2、3、5 運動時與圖1 所示的三自由度機器人同構，因此后續將以此機器人為平臺進行實驗，如圖5所示。柯馬Racer3 機器人采用了貝加萊系統，編程環境為Automation Studio，支持IEC 61131-3語言和C 語言。首先，以0.8 ms為周期對激勵軌跡進行采樣，將30 s 內的采樣數據導入實驗系統，在機器人各關節回零后進行不基于動力學的PID 控制實驗。實驗過程中，由電樞電流可實時計算出關節的實際力矩，由關節編碼器可得出實時位置數據。實驗結束后，在Automation Studio的function block中讀取各樣本點的力矩和位置數據，并進行濾波處理，將濾波后的位置數據進行一階差分、二階差分得到速度和加速度值。由式（7）可得：

將濾波后的力矩、位置、速度、加速度數據代入式（9），參數辨識結果如表3所示。

表3 參數辨識結果

利用辨識出的參數值，結合實際軌跡數據，根據動力學公式計算出理論力矩，和實際力矩的對比如圖6～8 所示，兩力矩值吻合程度較高。由于低速情況下，摩擦力的影響更為顯著，故力矩誤差在每次關節速度為0時會出現峰值。所以將實際力矩中速度低于0.007 rad/s的樣本點剔除，計算剩余樣本點的力矩誤差的均方根值，依次為1.9243 N·m、1.5922 N·m、0.5999 N·m，較小的均方根值說明了數據的穩定性，也說明了各采樣點誤差集中分布在零附近，證明了參數辨識的準確性。

圖6 關節1力矩誤差

圖7 關節2力矩誤差

圖8 關節3力矩誤差

3 計算力矩法控制理論

計算力矩法是對機器人的非線性化模型進行補償，從而實現線性化控制。機器人的動力學方程如式（4）所示。

通常為實現關節間的解耦，Kd和Kp選擇為正定對角矩陣[12]。聯立式（4）、（10）、（11）可得：

由于Kd、 Kp為正定矩陣，則是全局漸近穩定的平衡點，即最終收斂于

計算力矩法的完整控制輸入如下：

控制框圖如圖9所示。

圖9 線性化逆動力學控制框圖

4 軌跡跟蹤仿真和實驗

4.1 計算力矩法控制仿真

在Simulink 環境中，利用表3 的參數辨識結果，按照圖9建立三自由度機器人的運動學、動力學及控制系統仿真模型。

為更好地模擬實際機械臂運行情況，機器人主體程序中考慮一定程度的白噪聲。設置仿真時間為30 s，采樣時間為0.001 s，經反復地調試，得到較好的控制器參數Kd和Kp分別為diag（6，6，5.15）、diag（250，250，195.48），三自由度機器人各關節的仿真結果如圖10～12所示。

圖10 關節1的動態軌跡跟蹤仿真結果

圖11 關節2的動態軌跡跟蹤仿真結果

圖12 關節3的動態軌跡跟蹤仿真結果

4.2 計算力矩法控制實驗

上述仿真結果表明，基于動力學的計算力矩法能取得較好的控制效果。采取反復調試所得的控制器參數Kd和Kp，展開以下計算力矩法控制實驗。

類似辨識實驗，仍利用柯馬Racer3 機器人同平面的3 個關節，以激勵軌跡為控制輸入，進行計算力矩法控制實驗。實驗結果濾波處理后，各關節的位置跟蹤結果如圖13 所示。對比輸入軌跡和實際軌跡，跟蹤誤差如圖14所示。由于作為控制輸入的激勵軌跡加速度較大，實驗過程中會引發相對更大的振動，導致一定程度的軌跡跟蹤誤差，各關節的最大跟蹤誤差依次為0.0227 rad、0.0119 rad、0.0064 rad。關節1相較于關節2、3整體速度更小，受摩擦力的影響更大，而且根據表3的辨識結果，關節1所受摩擦力是最大的，所以關節1的跟蹤誤差相對更大。但總體而言，各關節誤差在0附近的小范圍內波動，穩定可控，說明計算力矩法實現了較高精度的軌跡跟蹤控制。

圖13 各關節的位置跟蹤結果

圖14 各關節的位置跟蹤誤差

5 結束語

本文建立了三自由機器人的運動學、動力學模型，并借助柯馬Racer3 機器人2、3、5 關節進行了動力學參數辨識實驗，再對基于動力學模型的計算力矩法展開了軌跡跟蹤仿真和控制實驗，得出以下結論。

（1）以諧波正弦和余弦函數的有限和作為激勵軌跡，并對其系數優化以減小噪聲的影響，再進行不基于動力學的PID辨識實驗，能夠實現機器人動力學參數的準確辨識。

（2）在準確的參數辨識基礎上，計算力矩法控制實驗的結果表明，該種方法能夠實現機器人較高精度的軌跡控制，具有很好的應用前景。