基于力矩前饋的機器人直線度算法

李彥華， 李會英， 劉仁芬

（1.河北軌道運輸職業技術學院，石家莊050051；2.石家莊鐵道大學四方學院，石家莊050043）

0 引 言

工業機器人通常都是多關節串聯式機器人，結構懸臂較長，轉動慣量較大，具有典型的殘余振動特性。機器人關節或機械臂耦合會帶來非線性力的變化，一個關節的振動還可以傳遞給另外的關節。對機器人的振動進行抑制控制，不僅可以提高工業機器人和機械系統精度和性能，還可以簡化機械結構。采用力矩前饋抑制技術后的驅動器，定位時間大幅減少，同時路徑跟蹤變得更加精準和平穩。汪克峰等［1］提出了一種用于表面永磁同步電動機平滑控制的負載轉矩前饋補償控制策略，建立了卡爾曼濾波器，利用機械運動學方程實現了對轉速和負載轉矩的精確估計，將負載轉矩擾動和電動機參數波動轉化為負載轉矩的變化。Bing等提出了一種永磁同步機轉矩前饋控制技術，利用參數非線性，隨直流和正交電流變化的特點，提出了一種新的最優工作面，該工作平面結合了每安培最大轉矩（MTPA）曲線、電流極限圓和每伏特最大轉矩（MTPV）曲線、電壓限制和在非線性參數變化下的轉矩計算。徐淑正等［3］針對電液負載系統壓力前饋控制，提出了一種不同于傳統補償方法的舵壓前饋補償方法。姚誠偉等［4］利用加速度前饋對三軸慣性穩定平臺進行不平衡轉矩補償，為了減小不平衡轉矩的影響，提出了一種更好的實用解決方案，并提供給萬向驅動，Matlab仿真結果表明，該方法提高了ISP的精度。

目前，對機器人的精度高、速度快、執行安全性有更高的要求，引入動力學的控制是一條有效的途徑［5-6］。本文利用指數積（POE）法建立了各關節連桿的相對轉換關系，開發了一套動力學前饋控制系統（電流力控制、動力學前饋）。最后提出了機器人直線度的概念，可以反映機器人運動的平穩性、軌跡一致性和軌跡跟蹤性等參數的好壞。

1 機器人控制系統架構

1.1 POE 運動學

本文研究的傳統機器人在運動過程中實現可編程，并且實現可變的預定步進姿勢和力感測，如圖1所示。

1.1.1 機器人正運動學

手臂運動學正解（串聯開鏈機器人的正向運動學公式）：對于n個轉動／移動關節的串聯機器人，設base0為基坐標系，Tooln＋1為工具坐標系，則應用POE計算機器人的正向運動學模型僅需3步完成：

圖1 柔性下肢康復機器人（mm）

（1）機器人末端初始位姿gst（0）

（2）關節對應的運動旋量坐標ξ＾i

（3）代入POE公式，得：

由于：

轉換為T矩陣的表達形式為：

1.1.2 機器人反運動學

與正向運動學模型的輸入、輸出正好相反，逆向運動學由給定的機器人末端位姿解計算對應的關節運動量［7-9］。注意：正向運動學的解唯一，而逆向運動學可能有多解、唯一解或者無解等不同情況。

給定機器人運動學正解映射gst，一個期望位姿gd，逆運動學即是解算如下方程：

運動學逆解可以分為兩種思路：

（1）解析解（位置級解法）。①幾何法直接解算：對于簡單的平面運動模型，一般直接根據幾何關系求解。②P-K子問題：將運動學逆問題轉化成三類P-K子問題，而后通過求解子問題得到逆解。

（2）數值解（速度級解法）。數值解即合理選擇數值算法，對gst（θ）＝gd進行求解，例如牛頓迭代法。

P-K子問題法的基本技巧是：將POE運動學模型應用于某些特殊點，比如兩個或多個軸的交點，可以消除這些軸關節的耦合，從而消掉其對應的變化矩陣eξ＾θ。

子問題1 繞1個軸的旋轉，求θ

子問題2 繞2個有序軸的旋轉，求θ1和θ2

子問題3 旋轉至給定距離，求θ

1.2 Elmo驅動器控制結構

動力學控制有較多，如計算力矩法、Lagrange、牛頓歐拉法等。但各大機器人公司對完整的動力學控制方案都是保密的。圖2是Elmo公司驅動器的控制結構，可以看到Elmo驅動器提供了力矩前饋的控制接口和控制結構，可以將計算的理論值下發至驅動器［14-15］。

圖2 Elmo驅動器的控制結構

2 機器人動力學前饋

控制系統結構：綜合運動學、動力學、伺服控制結構。動力學簡化：簡化機器人動力學計算。動力學參數標定：慣性參數、連桿參數等標定。

2.1 動力學前饋

針對機器人本體建立動力學模型時，采用牛頓-歐拉遞推方程。受限于機器人控制系統的實時性要求，如果采用拉格朗日方程會增加運算量［10-13］。

牛頓-歐拉內推，從基座到末端計算速度和加速度：

牛頓-歐拉外推，從末端到基座計算慣性力和關節力矩：

其中各個關節實際的受力可以根據關節電流計算而來。通過重力、摩擦力可以分析得出各個關節受力情況，最終通過伺服力矩輸出到機器人本體，以此實現對機器人的直接控制，如圖3所示。還需要考慮手臂基座的傾斜，重力加速度在各軸的分量需要做相應的調整。

圖3 零力控制原理框圖

2.2 摩擦力采集

相較于采用力矩傳感器方法，無傳感器零力控制的方式省去了在每個關節增加傳感器的成本，避免因為加裝傳感器而帶來的柔性改變，基礎動力學模型相對簡化［16］。但無傳感器模式電流信號噪聲較大，電流信號響應頻率高，容易混雜高頻噪聲，該問題需要謹慎考慮。

在理想的動力學模型中，摩擦力近似為零，在機器人運動過程中，可以實時計算得出當前驅動機器人運動所需要的力矩，同時通過伺服電流來獲取機器人當前的實際輸出力矩，而兩者的差值即為當前的實際摩擦力。

式中：Tactual為實際采集的力矩；Ttheory為動力學模型所輸出的當前理論力矩。

通過濾波后的驅動器信息數據可以直接獲得效果較好的摩擦力數據，摩擦力模型可以通過大量數據的擬合來完成。

目前機器人關節摩擦力的基礎模型主要有3個選擇：①庫倫模型＋黏性摩擦模型；②Stribeck模型；③n次多項式模型。采用模型①分析摩擦力時，因速度而來的變化不夠敏感，導致最終推動關節時易產生過補償，造成機器人不易停止甚至在牽引速度較快時發生飛車；模型②的特征是在高速段的擬合較為準確，在低速段卻會有一定程度的失真；模型③的特征是在低速段的擬合較為準確，而高速段會遠離實際數據。

以機器人的關節1為例，驅動機器人第1個軸在不同速度下做往復運動，采集到摩擦力的2004組數據，如圖4所示。

圖4 機器人關節摩擦力

從圖4可見，所采集的摩擦力存在周期性的尖峰，通過分析其周期所對應的關節速度可知：①通過關節電流采集力矩信號伴隨有高頻噪聲；②摩擦力與關節角速度相關。為得到有效的力矩信號，在控制系統中加入二階巴特沃斯低通濾波器。

常規的低頻電流數據可得到較為完整的保留，信號的大部分高頻成分被濾波器濾除。

2.3 摩擦力矩擬合方法

在曲線擬合中普遍采用最小二乘法，需要提前確定離散點分布情況的階次，即使是相同的離散點，所擬合的多項式階次不同也會造成曲線有很大差異［9-10］。擬合函數超過3次時所擬合曲線的誤差就會很大，并出現病態問題。所以最小二乘法應用范圍還是很有限的，離散點規律過于復雜，最小二乘法擬合出來的曲線誤差會非常大。雖然采用切比雪夫算法擬合曲線差值達不到要求，需重新計算，使得計算繁煩，但是借助于牛頓差值，使得擬合曲線各階差值在所規定的范圍內，這樣的指標使得擬合曲線誤差很小，所得擬合曲線準確。

根據對機器人的摩擦力矩Tf的測試，可以看出摩擦力矩的大小與機器人的重力矩、加速轉矩和轉動角速度的大小相關。為了簡化線性化擬合，將摩擦力矩擬合公式確定如下：

式中：Tf為摩擦力矩；Tm為重力矩；φ為重心與旋轉關節中心連線與水平線方向的夾角；為轉動角速度；為加速轉矩；a、b、c分別為重力矩、角速度、加速轉矩的影響因數。

（1）重力矩影響因數a的確定。a的確定可以通過mglcos φ求解。在機器人執行程序前后這兩個時間段中，沒有受到角速度和角加速度的影響。因此，可以選擇這兩段時間對a進行擬合。mg為機器人繞關節旋轉部分的總質量；l為機器人繞關節旋轉部分的重心到旋轉關節的距離；φ為重心與旋轉關節中心連線與水平線方向的夾角。

（2）角速度影響因數b的確定。對測量軟件得到的數據分析計算后得到Tf，將Tf中的重力矩影響去除，這時Tf只有角速度和加速轉矩的影響。由于機器人在程序控制下運行，存在平穩運行時間段。選取角速度平穩運行時對b進行擬合。

（3）加速轉矩影響因數c的確定。在重力矩和角速度影響因數確定之后，通過簡單地運算使Tf中只剩下加速轉矩的影響。機器人處在加速和減速階段的c進行擬合。

2.4 前饋控制

圖5為前饋控制模型。圖中：WM（s）為前饋控制器；WO（s）為過程控制通道傳遞函數；Wf（s）為過程擾動通道傳遞函數。

圖5 前饋控制模型

不變性原理決定的動態前饋控制器是由被過程擾動通道與控制通道特性之比決定的，即：

因實現了完全補償，達到了被控量不受擾動影響的控制效果。前饋模型的結構有時比較復雜，甚至難以實現（見圖6）。

圖6 驗證前饋接口

3 實驗結果及分析

求出了機器人動力學前饋模型，對模型參數采用辨識方法實現了動力學參數獲取，將力矩作為前饋值發送至驅動器。機器人軌跡精度和穩定性好壞依靠千分表進行測試。本文提出了機器人直線度的概念，即機器人走直線的精度。從實驗可以看出，采用力矩前饋控制法可有效地改善機器人的性能。

3.1 采集位置曲線

利用激光跟蹤儀對機器人在不同速度下進行直線行走過程中的軌跡進行測試，如圖7所示。

圖7 機器人在不同速度下直線行走過程中的軌跡

3.2 直線度分析

從圖7采集到的帶動力學前饋和不帶動力學前饋的直線對比，如表1所示。

表1 直線度對比

機器人直線度是反映機器人震蕩強弱的一個重要指標，依靠動力學前饋可以很好地提高機器人的直線度。

4 結 語

針對傳統機器人的結構參數特點，運用指數積（POE）運動控制求解了運動學問題，然后構建了動力學方程，同時提出了一種摩擦力矩擬合的方法來構建控制算法模型。采用力矩前饋控制（動力學前饋補償）法，有效地改善了機器人的性能。提出了機器人直線度的概念，可以反映機器人運動的平穩性、軌跡一致性和軌跡跟蹤性等參數的好壞。將動力學應用到控制器中，機器人控制器在運動周期內，按照運動學位置、速度、加速度計算動力學結果，將其按前饋方式加到電流環的計算結果，輸出給驅動器。但是由于控制器周期在4 ms，驅動器周期500 μm級別，所以算法上可以隨著控制器周期的縮短，達到更好的效果。