基于力矩前饋的機(jī)器人直線度算法

李彥華， 李會(huì)英， 劉仁芬

（1.河北軌道運(yùn)輸職業(yè)技術(shù)學(xué)院，石家莊050051；2.石家莊鐵道大學(xué)四方學(xué)院，石家莊050043）

0 引 言

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

目前，對(duì)機(jī)器人的精度高、速度快、執(zhí)行安全性有更高的要求，引入動(dòng)力學(xué)的控制是一條有效的途徑［5-6］。本文利用指數(shù)積（POE）法建立了各關(guān)節(jié)連桿的相對(duì)轉(zhuǎn)換關(guān)系，開(kāi)發(fā)了一套動(dòng)力學(xué)前饋控制系統(tǒng)（電流力控制、動(dòng)力學(xué)前饋）。最后提出了機(jī)器人直線度的概念，可以反映機(jī)器人運(yùn)動(dòng)的平穩(wěn)性、軌跡一致性和軌跡跟蹤性等參數(shù)的好壞。

1 機(jī)器人控制系統(tǒng)架構(gòu)

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

本文研究的傳統(tǒng)機(jī)器人在運(yùn)動(dòng)過(guò)程中實(shí)現(xiàn)可編程，并且實(shí)現(xiàn)可變的預(yù)定步進(jìn)姿勢(shì)和力感測(cè)，如圖1所示。

1.1.1 機(jī)器人正運(yùn)動(dòng)學(xué)

手臂運(yùn)動(dòng)學(xué)正解（串聯(lián)開(kāi)鏈機(jī)器人的正向運(yùn)動(dòng)學(xué)公式）：對(duì)于n個(gè)轉(zhuǎn)動(dòng)／移動(dòng)關(guān)節(jié)的串聯(lián)機(jī)器人，設(shè)base0為基坐標(biāo)系，Tooln＋1為工具坐標(biāo)系，則應(yīng)用POE計(jì)算機(jī)器人的正向運(yùn)動(dòng)學(xué)模型僅需3步完成：

圖1 柔性下肢康復(fù)機(jī)器人（mm）

（1）機(jī)器人末端初始位姿gst（0）

（2）關(guān)節(jié)對(duì)應(yīng)的運(yùn)動(dòng)旋量坐標(biāo)ξ＾i

（3）代入POE公式，得：

由于：

轉(zhuǎn)換為T矩陣的表達(dá)形式為：

1.1.2 機(jī)器人反運(yùn)動(dòng)學(xué)

與正向運(yùn)動(dòng)學(xué)模型的輸入、輸出正好相反，逆向運(yùn)動(dòng)學(xué)由給定的機(jī)器人末端位姿解計(jì)算對(duì)應(yīng)的關(guān)節(jié)運(yùn)動(dòng)量［7-9］。注意：正向運(yùn)動(dòng)學(xué)的解唯一，而逆向運(yùn)動(dòng)學(xué)可能有多解、唯一解或者無(wú)解等不同情況。

給定機(jī)器人運(yùn)動(dòng)學(xué)正解映射gst，一個(gè)期望位姿gd，逆運(yùn)動(dòng)學(xué)即是解算如下方程：

運(yùn)動(dòng)學(xué)逆解可以分為兩種思路：

（1）解析解（位置級(jí)解法）。①幾何法直接解算：對(duì)于簡(jiǎn)單的平面運(yùn)動(dòng)模型，一般直接根據(jù)幾何關(guān)系求解。②P-K子問(wèn)題：將運(yùn)動(dòng)學(xué)逆問(wèn)題轉(zhuǎn)化成三類P-K子問(wèn)題，而后通過(guò)求解子問(wèn)題得到逆解。

（2）數(shù)值解（速度級(jí)解法）。數(shù)值解即合理選擇數(shù)值算法，對(duì)gst（θ）＝gd進(jìn)行求解，例如牛頓迭代法。

P-K子問(wèn)題法的基本技巧是：將POE運(yùn)動(dòng)學(xué)模型應(yīng)用于某些特殊點(diǎn)，比如兩個(gè)或多個(gè)軸的交點(diǎn)，可以消除這些軸關(guān)節(jié)的耦合，從而消掉其對(duì)應(yīng)的變化矩陣eξ＾θ。

子問(wèn)題1 繞1個(gè)軸的旋轉(zhuǎn)，求θ

子問(wèn)題2 繞2個(gè)有序軸的旋轉(zhuǎn)，求θ1和θ2

子問(wèn)題3 旋轉(zhuǎn)至給定距離，求θ

1.2 Elmo驅(qū)動(dòng)器控制結(jié)構(gòu)

動(dòng)力學(xué)控制有較多，如計(jì)算力矩法、Lagrange、牛頓歐拉法等。但各大機(jī)器人公司對(duì)完整的動(dòng)力學(xué)控制方案都是保密的。圖2是Elmo公司驅(qū)動(dòng)器的控制結(jié)構(gòu)，可以看到Elmo驅(qū)動(dòng)器提供了力矩前饋的控制接口和控制結(jié)構(gòu)，可以將計(jì)算的理論值下發(fā)至驅(qū)動(dòng)器［14-15］。

圖2 Elmo驅(qū)動(dòng)器的控制結(jié)構(gòu)

2 機(jī)器人動(dòng)力學(xué)前饋

控制系統(tǒng)結(jié)構(gòu)：綜合運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)、伺服控制結(jié)構(gòu)。動(dòng)力學(xué)簡(jiǎn)化：簡(jiǎn)化機(jī)器人動(dòng)力學(xué)計(jì)算。動(dòng)力學(xué)參數(shù)標(biāo)定：慣性參數(shù)、連桿參數(shù)等標(biāo)定。

2.1 動(dòng)力學(xué)前饋

針對(duì)機(jī)器人本體建立動(dòng)力學(xué)模型時(shí)，采用牛頓-歐拉遞推方程。受限于機(jī)器人控制系統(tǒng)的實(shí)時(shí)性要求，如果采用拉格朗日方程會(huì)增加運(yùn)算量［10-13］。

牛頓-歐拉內(nèi)推，從基座到末端計(jì)算速度和加速度：

牛頓-歐拉外推，從末端到基座計(jì)算慣性力和關(guān)節(jié)力矩：

其中各個(gè)關(guān)節(jié)實(shí)際的受力可以根據(jù)關(guān)節(jié)電流計(jì)算而來(lái)。通過(guò)重力、摩擦力可以分析得出各個(gè)關(guān)節(jié)受力情況，最終通過(guò)伺服力矩輸出到機(jī)器人本體，以此實(shí)現(xiàn)對(duì)機(jī)器人的直接控制，如圖3所示。還需要考慮手臂基座的傾斜，重力加速度在各軸的分量需要做相應(yīng)的調(diào)整。

圖3 零力控制原理框圖

2.2 摩擦力采集

相較于采用力矩傳感器方法，無(wú)傳感器零力控制的方式省去了在每個(gè)關(guān)節(jié)增加傳感器的成本，避免因?yàn)榧友b傳感器而帶來(lái)的柔性改變，基礎(chǔ)動(dòng)力學(xué)模型相對(duì)簡(jiǎn)化［16］。但無(wú)傳感器模式電流信號(hào)噪聲較大，電流信號(hào)響應(yīng)頻率高，容易混雜高頻噪聲，該問(wèn)題需要謹(jǐn)慎考慮。

在理想的動(dòng)力學(xué)模型中，摩擦力近似為零，在機(jī)器人運(yùn)動(dòng)過(guò)程中，可以實(shí)時(shí)計(jì)算得出當(dāng)前驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)所需要的力矩，同時(shí)通過(guò)伺服電流來(lái)獲取機(jī)器人當(dāng)前的實(shí)際輸出力矩，而兩者的差值即為當(dāng)前的實(shí)際摩擦力。

式中：Tactual為實(shí)際采集的力矩；Ttheory為動(dòng)力學(xué)模型所輸出的當(dāng)前理論力矩。

通過(guò)濾波后的驅(qū)動(dòng)器信息數(shù)據(jù)可以直接獲得效果較好的摩擦力數(shù)據(jù)，摩擦力模型可以通過(guò)大量數(shù)據(jù)的擬合來(lái)完成。

目前機(jī)器人關(guān)節(jié)摩擦力的基礎(chǔ)模型主要有3個(gè)選擇：①庫(kù)倫模型＋黏性摩擦模型；②Stribeck模型；③n次多項(xiàng)式模型。采用模型①分析摩擦力時(shí)，因速度而來(lái)的變化不夠敏感，導(dǎo)致最終推動(dòng)關(guān)節(jié)時(shí)易產(chǎn)生過(guò)補(bǔ)償，造成機(jī)器人不易停止甚至在牽引速度較快時(shí)發(fā)生飛車；模型②的特征是在高速段的擬合較為準(zhǔn)確，在低速段卻會(huì)有一定程度的失真；模型③的特征是在低速段的擬合較為準(zhǔn)確，而高速段會(huì)遠(yuǎn)離實(shí)際數(shù)據(jù)。

以機(jī)器人的關(guān)節(jié)1為例，驅(qū)動(dòng)機(jī)器人第1個(gè)軸在不同速度下做往復(fù)運(yùn)動(dòng)，采集到摩擦力的2004組數(shù)據(jù)，如圖4所示。

圖4 機(jī)器人關(guān)節(jié)摩擦力

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

常規(guī)的低頻電流數(shù)據(jù)可得到較為完整的保留，信號(hào)的大部分高頻成分被濾波器濾除。

2.3 摩擦力矩?cái)M合方法

在曲線擬合中普遍采用最小二乘法，需要提前確定離散點(diǎn)分布情況的階次，即使是相同的離散點(diǎn)，所擬合的多項(xiàng)式階次不同也會(huì)造成曲線有很大差異［9-10］。擬合函數(shù)超過(guò)3次時(shí)所擬合曲線的誤差就會(huì)很大，并出現(xiàn)病態(tài)問(wèn)題。所以最小二乘法應(yīng)用范圍還是很有限的，離散點(diǎn)規(guī)律過(guò)于復(fù)雜，最小二乘法擬合出來(lái)的曲線誤差會(huì)非常大。雖然采用切比雪夫算法擬合曲線差值達(dá)不到要求，需重新計(jì)算，使得計(jì)算繁煩，但是借助于牛頓差值，使得擬合曲線各階差值在所規(guī)定的范圍內(nèi)，這樣的指標(biāo)使得擬合曲線誤差很小，所得擬合曲線準(zhǔn)確。

根據(jù)對(duì)機(jī)器人的摩擦力矩Tf的測(cè)試，可以看出摩擦力矩的大小與機(jī)器人的重力矩、加速轉(zhuǎn)矩和轉(zhuǎn)動(dòng)角速度的大小相關(guān)。為了簡(jiǎn)化線性化擬合，將摩擦力矩?cái)M合公式確定如下：

式中：Tf為摩擦力矩；Tm為重力矩；φ為重心與旋轉(zhuǎn)關(guān)節(jié)中心連線與水平線方向的夾角；為轉(zhuǎn)動(dòng)角速度；為加速轉(zhuǎn)矩；a、b、c分別為重力矩、角速度、加速轉(zhuǎn)矩的影響因數(shù)。

（1）重力矩影響因數(shù)a的確定。a的確定可以通過(guò)mglcos φ求解。在機(jī)器人執(zhí)行程序前后這兩個(gè)時(shí)間段中，沒(méi)有受到角速度和角加速度的影響。因此，可以選擇這兩段時(shí)間對(duì)a進(jìn)行擬合。mg為機(jī)器人繞關(guān)節(jié)旋轉(zhuǎn)部分的總質(zhì)量；l為機(jī)器人繞關(guān)節(jié)旋轉(zhuǎn)部分的重心到旋轉(zhuǎn)關(guān)節(jié)的距離；φ為重心與旋轉(zhuǎn)關(guān)節(jié)中心連線與水平線方向的夾角。

（2）角速度影響因數(shù)b的確定。對(duì)測(cè)量軟件得到的數(shù)據(jù)分析計(jì)算后得到Tf，將Tf中的重力矩影響去除，這時(shí)Tf只有角速度和加速轉(zhuǎn)矩的影響。由于機(jī)器人在程序控制下運(yùn)行，存在平穩(wěn)運(yùn)行時(shí)間段。選取角速度平穩(wěn)運(yùn)行時(shí)對(duì)b進(jìn)行擬合。

（3）加速轉(zhuǎn)矩影響因數(shù)c的確定。在重力矩和角速度影響因數(shù)確定之后，通過(guò)簡(jiǎn)單地運(yùn)算使Tf中只剩下加速轉(zhuǎn)矩的影響。機(jī)器人處在加速和減速階段的c進(jìn)行擬合。

2.4 前饋控制

圖5為前饋控制模型。圖中：WM（s）為前饋控制器；WO（s）為過(guò)程控制通道傳遞函數(shù)；Wf（s）為過(guò)程擾動(dòng)通道傳遞函數(shù)。

圖5 前饋控制模型

不變性原理決定的動(dòng)態(tài)前饋控制器是由被過(guò)程擾動(dòng)通道與控制通道特性之比決定的，即：

因?qū)崿F(xiàn)了完全補(bǔ)償，達(dá)到了被控量不受擾動(dòng)影響的控制效果。前饋模型的結(jié)構(gòu)有時(shí)比較復(fù)雜，甚至難以實(shí)現(xiàn)（見(jiàn)圖6）。

圖6 驗(yàn)證前饋接口

3 實(shí)驗(yàn)結(jié)果及分析

求出了機(jī)器人動(dòng)力學(xué)前饋模型，對(duì)模型參數(shù)采用辨識(shí)方法實(shí)現(xiàn)了動(dòng)力學(xué)參數(shù)獲取，將力矩作為前饋值發(fā)送至驅(qū)動(dòng)器。機(jī)器人軌跡精度和穩(wěn)定性好壞依靠千分表進(jìn)行測(cè)試。本文提出了機(jī)器人直線度的概念，即機(jī)器人走直線的精度。從實(shí)驗(yàn)可以看出，采用力矩前饋控制法可有效地改善機(jī)器人的性能。

3.1 采集位置曲線

利用激光跟蹤儀對(duì)機(jī)器人在不同速度下進(jìn)行直線行走過(guò)程中的軌跡進(jìn)行測(cè)試，如圖7所示。

圖7 機(jī)器人在不同速度下直線行走過(guò)程中的軌跡

3.2 直線度分析

從圖7采集到的帶動(dòng)力學(xué)前饋和不帶動(dòng)力學(xué)前饋的直線對(duì)比，如表1所示。

表1 直線度對(duì)比

機(jī)器人直線度是反映機(jī)器人震蕩強(qiáng)弱的一個(gè)重要指標(biāo)，依靠動(dòng)力學(xué)前饋可以很好地提高機(jī)器人的直線度。

4 結(jié) 語(yǔ)

針對(duì)傳統(tǒng)機(jī)器人的結(jié)構(gòu)參數(shù)特點(diǎn)，運(yùn)用指數(shù)積（POE）運(yùn)動(dòng)控制求解了運(yùn)動(dòng)學(xué)問(wèn)題，然后構(gòu)建了動(dòng)力學(xué)方程，同時(shí)提出了一種摩擦力矩?cái)M合的方法來(lái)構(gòu)建控制算法模型。采用力矩前饋控制（動(dòng)力學(xué)前饋補(bǔ)償）法，有效地改善了機(jī)器人的性能。提出了機(jī)器人直線度的概念，可以反映機(jī)器人運(yùn)動(dòng)的平穩(wěn)性、軌跡一致性和軌跡跟蹤性等參數(shù)的好壞。將動(dòng)力學(xué)應(yīng)用到控制器中，機(jī)器人控制器在運(yùn)動(dòng)周期內(nèi)，按照運(yùn)動(dòng)學(xué)位置、速度、加速度計(jì)算動(dòng)力學(xué)結(jié)果，將其按前饋方式加到電流環(huán)的計(jì)算結(jié)果，輸出給驅(qū)動(dòng)器。但是由于控制器周期在4 ms，驅(qū)動(dòng)器周期500 μm級(jí)別，所以算法上可以隨著控制器周期的縮短，達(dá)到更好的效果。