賴 含，韓奉林，胡金飛

（1.中南大學(xué)機(jī)電工程學(xué)院，長(zhǎng)沙 410012；2.浙江大學(xué)機(jī)械工程學(xué)院，杭州 310027）

0 引言

隨著對(duì)機(jī)器人運(yùn)動(dòng)速度、載荷、軌跡控制精度和穩(wěn)定性等性能的要求越來(lái)越高，單純基于運(yùn)動(dòng)學(xué)的機(jī)器人軌跡控制方法已不能滿足要求，必須在機(jī)器人軌跡控制中考慮機(jī)器人的動(dòng)力學(xué)特性。

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

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

為便于計(jì)算力矩法的進(jìn)一步推廣應(yīng)用，本文系統(tǒng)地提出了包括動(dòng)力學(xué)參數(shù)辨識(shí)、動(dòng)態(tài)軌跡控制在內(nèi)的完整實(shí)施方法，建立了參數(shù)辨識(shí)和動(dòng)態(tài)軌跡控制的理論模型，并以平面三自由度機(jī)器人為例，開(kāi)展了數(shù)值仿真和實(shí)驗(yàn)研究。

1 三自由度機(jī)器人的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)建模

1.1 運(yùn)動(dòng)學(xué)建模

圖1 三自由度機(jī)器人模型

平面三自由度機(jī)器人在工業(yè)中應(yīng)用廣泛，且結(jié)構(gòu)相對(duì)簡(jiǎn)單，是研究動(dòng)態(tài)軌跡控制算法的理想對(duì)象?？紤]如圖1 所示的平面三自由度機(jī)器人，運(yùn)用Danevit-Hartenberg 法[9]建立如圖1 所示坐標(biāo)系，采用圖中標(biāo)識(shí)的符號(hào)，其中bi（i=1,2,3）指各桿質(zhì)心據(jù)桿前端坐標(biāo)系沿連桿的距離，lx指偏距，可以得到如表1所示的機(jī)器人D-H參數(shù)。

表1 三自由度機(jī)器人D-H參數(shù)

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

式中：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的關(guān)系：

式中：雅可比矩陣為：

1.2 動(dòng)力學(xué)建模

如圖1 所示，令i=1,2,3，設(shè)各桿質(zhì)量為mi，各桿繞質(zhì)心坐標(biāo)系（與前一關(guān)節(jié)坐標(biāo)系平行但原點(diǎn)位于質(zhì)心處的坐標(biāo)系）Z 軸的轉(zhuǎn)動(dòng)慣量為Ji?？紤]實(shí)際情況，引入各桿傳動(dòng)裝置的等效轉(zhuǎn)動(dòng)慣量Ii，各關(guān)節(jié)所受動(dòng)摩擦力fvi，靜摩擦力fci。再利用拉格朗日法建立機(jī)器人動(dòng)力學(xué)方程，根據(jù)文獻(xiàn)[10]，動(dòng)力學(xué)方程可線性參數(shù)化：

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

表2 三自由度機(jī)器人動(dòng)力學(xué)最小參數(shù)集

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

2 激勵(lì)軌跡規(guī)劃和動(dòng)力學(xué)參數(shù)辨識(shí)

2.1 激勵(lì)軌跡規(guī)劃

前文所述動(dòng)力學(xué)模型有未知?jiǎng)恿W(xué)參數(shù)，所以應(yīng)先辨識(shí)出機(jī)器人的動(dòng)力學(xué)參數(shù)。為降低測(cè)量噪聲的影響，使辨識(shí)出的數(shù)據(jù)更加準(zhǔn)確，需先優(yōu)化激勵(lì)軌跡。文獻(xiàn)[11]表明，每個(gè)關(guān)節(jié)的激勵(lì)軌跡可為N個(gè)諧波正弦和余弦函數(shù)的有限和。

式中：ωf是傅里葉級(jí)數(shù)的基頻，周期Tf=2π/ωf，所有關(guān)節(jié)基頻相同，以保證整個(gè)機(jī)器人激勵(lì)的周期性。每個(gè)關(guān)節(jié)的激勵(lì)軌跡包含2N+1個(gè)參數(shù)，正、余弦函數(shù)的幅值以及位置軌跡的偏移量ci。

根據(jù)式（4），對(duì)于第n個(gè)樣本點(diǎn)，動(dòng)力學(xué)方程可寫(xiě)為：

若共有m個(gè)樣本點(diǎn)，動(dòng)力學(xué)方程可寫(xiě)為矩陣形式：

圖2 激勵(lì)軌跡位移圖

圖3 激勵(lì)軌跡速度圖

圖4 激勵(lì)軌跡加速度圖

2.2 參數(shù)辨識(shí)實(shí)驗(yàn)

圖5 實(shí)驗(yàn)中采用的柯馬Racer3機(jī)器人

柯馬Racer3 機(jī)器人當(dāng)關(guān)節(jié)1、4、6 鎖定于零位，關(guān)節(jié)2、3、5 運(yùn)動(dòng)時(shí)與圖1 所示的三自由度機(jī)器人同構(gòu)，因此后續(xù)將以此機(jī)器人為平臺(tái)進(jìn)行實(shí)驗(yàn)，如圖5所示?？埋RRacer3 機(jī)器人采用了貝加萊系統(tǒng)，編程環(huán)境為Automation Studio，支持IEC 61131-3語(yǔ)言和C 語(yǔ)言。首先，以0.8 ms為周期對(duì)激勵(lì)軌跡進(jìn)行采樣，將30 s 內(nèi)的采樣數(shù)據(jù)導(dǎo)入實(shí)驗(yàn)系統(tǒng)，在機(jī)器人各關(guān)節(jié)回零后進(jìn)行不基于動(dòng)力學(xué)的PID 控制實(shí)驗(yàn)。實(shí)驗(yàn)過(guò)程中，由電樞電流可實(shí)時(shí)計(jì)算出關(guān)節(jié)的實(shí)際力矩，由關(guān)節(jié)編碼器可得出實(shí)時(shí)位置數(shù)據(jù)。實(shí)驗(yàn)結(jié)束后，在Automation Studio的function block中讀取各樣本點(diǎn)的力矩和位置數(shù)據(jù)，并進(jìn)行濾波處理，將濾波后的位置數(shù)據(jù)進(jìn)行一階差分、二階差分得到速度和加速度值。由式（7）可得：

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

表3 參數(shù)辨識(shí)結(jié)果

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

圖6 關(guān)節(jié)1力矩誤差

圖7 關(guān)節(jié)2力矩誤差

圖8 關(guān)節(jié)3力矩誤差

3 計(jì)算力矩法控制理論

計(jì)算力矩法是對(duì)機(jī)器人的非線性化模型進(jìn)行補(bǔ)償，從而實(shí)現(xiàn)線性化控制。機(jī)器人的動(dòng)力學(xué)方程如式（4）所示。

通常為實(shí)現(xiàn)關(guān)節(jié)間的解耦，Kd和Kp選擇為正定對(duì)角矩陣[12]。聯(lián)立式（4）、（10）、（11）可得：

由于Kd、 Kp為正定矩陣，則是全局漸近穩(wěn)定的平衡點(diǎn)，即最終收斂于

計(jì)算力矩法的完整控制輸入如下：

控制框圖如圖9所示。

圖9 線性化逆動(dòng)力學(xué)控制框圖

4 軌跡跟蹤仿真和實(shí)驗(yàn)

4.1 計(jì)算力矩法控制仿真

在Simulink 環(huán)境中，利用表3 的參數(shù)辨識(shí)結(jié)果，按照?qǐng)D9建立三自由度機(jī)器人的運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)及控制系統(tǒng)仿真模型。

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

圖10 關(guān)節(jié)1的動(dòng)態(tài)軌跡跟蹤仿真結(jié)果

圖11 關(guān)節(jié)2的動(dòng)態(tài)軌跡跟蹤仿真結(jié)果

圖12 關(guān)節(jié)3的動(dòng)態(tài)軌跡跟蹤仿真結(jié)果

4.2 計(jì)算力矩法控制實(shí)驗(yàn)

上述仿真結(jié)果表明，基于動(dòng)力學(xué)的計(jì)算力矩法能取得較好的控制效果。采取反復(fù)調(diào)試所得的控制器參數(shù)Kd和Kp，展開(kāi)以下計(jì)算力矩法控制實(shí)驗(yàn)。

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

圖13 各關(guān)節(jié)的位置跟蹤結(jié)果

圖14 各關(guān)節(jié)的位置跟蹤誤差

5 結(jié)束語(yǔ)

本文建立了三自由機(jī)器人的運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)模型，并借助柯馬Racer3 機(jī)器人2、3、5 關(guān)節(jié)進(jìn)行了動(dòng)力學(xué)參數(shù)辨識(shí)實(shí)驗(yàn)，再對(duì)基于動(dòng)力學(xué)模型的計(jì)算力矩法展開(kāi)了軌跡跟蹤仿真和控制實(shí)驗(yàn)，得出以下結(jié)論。

（1）以諧波正弦和余弦函數(shù)的有限和作為激勵(lì)軌跡，并對(duì)其系數(shù)優(yōu)化以減小噪聲的影響，再進(jìn)行不基于動(dòng)力學(xué)的PID辨識(shí)實(shí)驗(yàn)，能夠?qū)崿F(xiàn)機(jī)器人動(dòng)力學(xué)參數(shù)的準(zhǔn)確辨識(shí)。

（2）在準(zhǔn)確的參數(shù)辨識(shí)基礎(chǔ)上，計(jì)算力矩法控制實(shí)驗(yàn)的結(jié)果表明，該種方法能夠?qū)崿F(xiàn)機(jī)器人較高精度的軌跡控制，具有很好的應(yīng)用前景。