趙月 劉亞秋 徐妍 劉勛 房立金 張華良

摘 要：由于林木果球采收機(jī)械臂的工作場景復(fù)雜，對機(jī)械臂控制精準(zhǔn)度的要求越來越高，研究機(jī)械臂動力學(xué)模型對其控制精度的影響非常重要。為提升機(jī)械臂的控制精度，提出一種在優(yōu)化后的激勵軌跡下基于最小二乘法和高斯混合模型（GMM）的3次迭代整體參數(shù)辨識方法。該方法以6自由度機(jī)械臂構(gòu)型為例，通過建立動力學(xué)模型及QR（正交三角）分解得到最小參數(shù)集；通過軌跡優(yōu)化算法得到激勵軌跡的優(yōu)化參數(shù)，進(jìn)而得到優(yōu)化的激勵軌跡；得到軌跡后，依次對關(guān)節(jié)力矩采用迭代加權(quán)最小二乘法進(jìn)行理論辨識，構(gòu)建區(qū)分關(guān)節(jié)高、低速的非線性模型對機(jī)械臂非線性摩擦力進(jìn)行擬合，用GMM算法來補償無法精確建模的不確定力矩分量。在COMAN R5機(jī)械臂上進(jìn)行試驗測試，結(jié)果表明，所提出的軌跡參數(shù)優(yōu)化方法將條件數(shù)從329減少到193，力矩殘差的平均均方根從9.53降低到6.14，從而證明激勵軌跡和辨識方案的可行性和有效性。

關(guān)鍵詞：林木果球采收機(jī)械臂；三次迭代動力學(xué)參數(shù)辨識；激勵軌跡；非線性摩擦力模型；GMM補償算法

中圖分類號：S776;TP241.3 文獻(xiàn)標(biāo)識碼：A 文章編號：1006-8023（2023）03-0150-11

Abstract：Due to the complexity of the working scene of the forest fruit ball harvesting manipulator， the accuracy of the manipulator control is increasingly required. Considering the influence of the dynamic model of the manipulator on its control accuracy， in order to improve the control accuracy of the manipulator， a three-iterative global parameter identification method based on the least square method and GMM under the optimized excitation trajectory is proposed. This method takes the 6 - DOF manipulator configuration as an example， and obtains the minimum parameter set by establishing the dynamic model and QR decomposition. The optimization parameters of the incentive trajectory are obtained through the trajectory optimization algorithm， and then the optimized incentive trajectory is obtained. After the trajectory is obtained， the joint torque is identified by iterative weighted least square method， and the nonlinear friction force of the manipulator is fitted by constructing a nonlinear model that distinguishes high and low speed. The GMM algorithm is used to compensate the uncertain torque component that cannot be accurately modeled. Experimental tests are carried out on the COMAN R5 manipulator. Results show that the proposed trajectory parameter optimization method reduces the number of conditions from 329 to 193， and the average root mean square of torque residuals from 9.53 to 6.14， which proves the feasibility and effectiveness of the excitation trajectory and identification scheme.

Keywords：Forest ball harvesting manipulator; dynamic parameter identification of three-iterations; excitation trajectory; nonlinear friction model; GMM algorithm

基金項目：國家自然科學(xué)基金項目（61821005）;國家重點研發(fā)計劃項目（2018YFE0205802）。

第一作者簡介：趙月，碩士研究生。研究方向為機(jī)械臂動力學(xué)和運動學(xué)。E-mail： zhaoyue_0329@nefu.edu.cn

*通信作者：劉亞秋，博士，教授。研究方向為機(jī)械臂動力學(xué)和運動學(xué)、移動機(jī)器人。E-mail： yaqiuLiu@nefu.edu.cn

0 引言

林木果球采收過程需要大量的勞動力，傳統(tǒng)人工方式存在效率低和準(zhǔn)確率低等問題，同時近幾年新冠疫情導(dǎo)致了勞動力缺失，嚴(yán)重影響林業(yè)采收產(chǎn)業(yè)發(fā)展。面向林木果球采收的自動化設(shè)備或大型工業(yè)機(jī)器人設(shè)備[1]研發(fā)及維護(hù)成本較高、通用性差、缺乏協(xié)作功能，因此在當(dāng)前研究中采用協(xié)作操作臂進(jìn)行取代。

建立精確的動力學(xué)模型對于多自由度協(xié)作機(jī)械臂的精確控制至關(guān)重要，如Xiao 等[2]提出的機(jī)器人示教方法、李智靖等[3]提出的碰撞檢測和Fu等[4]實現(xiàn)的準(zhǔn)確對機(jī)械臂末端的控制，這些都是基于機(jī)器人動力學(xué)參數(shù)實現(xiàn)的。對于動態(tài)參數(shù)識別的研究目前有很多，如孫昌國等[5]利用物理試驗來測量各連桿的動力學(xué)參數(shù)、Armstrong等[6]通過CAD法對PUMA560動力學(xué)參數(shù)的測量、通過整體識別法[7-9]對動力學(xué)參數(shù)進(jìn)行辨識。目前廣泛應(yīng)用的是整體辨識方法，其大致可以分為確定動力學(xué)模型（包括摩擦力）、設(shè)計最優(yōu)激勵軌跡和確定整體辨識算法。

在動力學(xué)模型中，由于多數(shù)機(jī)器人依靠有摩擦的齒輪傳動，所以摩擦模型對整體辨識至關(guān)重要。目前在線性辨識方法中摩擦力通常建模為庫侖摩擦力和線性黏性摩擦力，如Gautier[10]使用庫侖摩擦力和線性黏性摩擦力擬合摩擦。然而，一些研究表明，黏性摩擦力和關(guān)節(jié)速度之間存在非線性關(guān)系[11-12]。因此本研究提出一種改進(jìn)摩擦模型，在區(qū)分關(guān)節(jié)低速和高速的基礎(chǔ)上，將庫侖摩擦和黏性摩擦的非線性模型結(jié)合起來，并在整體辨識中迭代確定低高速的閾值。

在整體辨識中，使用合適的激勵軌跡[13-14]會加快辨識算法的收斂速度，提高抗噪聲能力。本研究選用5階傅里葉級數(shù)作為激勵軌跡模型，回歸矩陣的條件數(shù)作為優(yōu)化準(zhǔn)則。研究表明[15]，對回歸矩陣和其子矩陣進(jìn)行優(yōu)化，實現(xiàn)對回歸矩陣內(nèi)部結(jié)構(gòu)的約束。Bonnet等[16]研究表明，迭代優(yōu)化能夠充分激勵機(jī)器人的動態(tài)特性。因此，本研究提出一種迭代最小二乘法來同時優(yōu)化回歸矩陣和子回歸矩陣，并使用變異系數(shù)法來確定子回歸矩陣的權(quán)重。

就整體辨識算法而言，目前已有的算法十分豐富，如使用神經(jīng)網(wǎng)絡(luò)辨識參數(shù)[17]、混沌粒子群優(yōu)化算法辨識[18]、基于李理群論實現(xiàn)參數(shù)辨識[19]和最小二乘法[8-9]等。最小二乘法是一種穩(wěn)健且被廣泛使用的方法。例如，Gautier 等[20]提出了一種非線性最小二乘優(yōu)化算法來估計二自由度機(jī)器人的SCARA（Selective Compliance Assembly Robot Am，選擇順應(yīng)性裝配機(jī)器手臂）、韓勇[21]提出了基于迭代的加權(quán)最小二乘法。本研究基于迭代加權(quán)最小二乘法，對連桿的慣性和關(guān)節(jié)摩擦模型進(jìn)行估計。但在整體參數(shù)辨識過程中，一些不確定的分量無法通過建模來辨識，使得擬合效果始終達(dá)不到預(yù)期效果，本研究針對這部分通過擬合剩余力矩來補償，鑒于這部分的困難和非線性等性質(zhì)，運用高斯混合（GMM）統(tǒng)計模型[22]實現(xiàn)擬合。

綜上，本研究提出一種基于3次迭代的協(xié)作機(jī)械臂動力學(xué)模型辨識及補償算法。辨識算法使用區(qū)分低速庫侖摩擦和黏性摩擦的非線性摩擦模型，結(jié)合加權(quán)迭代最小二乘法和擬合非線性殘差部分的GMM算法，完成協(xié)作機(jī)械臂的參數(shù)辨識及補償。為使結(jié)果更加準(zhǔn)確，使機(jī)械臂在優(yōu)化后的激勵軌跡下運動并且使激勵軌跡充分激發(fā)動力學(xué)特性，提出一種基于分塊回歸矩陣的迭代最小二乘的激勵軌跡參數(shù)優(yōu)化算法。該算法利用迭代最小二乘法優(yōu)化回歸矩陣，使用變異系數(shù)法確定子回歸矩陣的權(quán)重，從而得到最優(yōu)激勵軌跡。進(jìn)而提高辨識結(jié)果的準(zhǔn)確性和魯棒性。

1 建立機(jī)器人動力學(xué)模型

1.1 建立線性動力學(xué)模型

1.1.1 牛頓-歐拉動力學(xué)模型

基于牛頓-歐拉方程，若機(jī)械臂各關(guān)節(jié)位置、速度和加速度已知，并給定機(jī)器人運動學(xué)參數(shù)和各連桿質(zhì)量分布特征，那么引起剛體產(chǎn)生上述運動的理論力矩值可通過牛頓-歐拉迭代法[23]求得。機(jī)械臂關(guān)節(jié)力矩公式為

式中：M（q）∈Rn×n和n分別為正定對稱慣性矩陣和關(guān)節(jié)數(shù)；C（q，q·）∈Rn×n和G（q）∈Rn×1分別為科里奧利離心力矩陣和重力力矩矢量；q，q·，q¨分別為 n×1的關(guān)節(jié)角位移、角速度和角加速度矢量；τ∈Rn×1和τf∈Rn×1分別為關(guān)節(jié)的驅(qū)動力矩和摩擦力矩；J（q）T為機(jī)械臂的雅可比矩陣；fext為作用在機(jī)械臂末端的外力項矢量。

1.1.2 線性化模型

由于外力項與機(jī)械臂動力學(xué)參數(shù)本身無關(guān)，所以本研究不考慮外力項，式（1）改為

從式（2）中可以看到摩擦力在動力學(xué)模型中有著至關(guān)重要的影響，目前廣泛使用的摩擦模型是庫侖黏滯摩擦模型[24] ，該模型的描述為

式中，kc和kv分別為庫侖摩擦系數(shù)和黏性摩擦系數(shù)。

但這種單一的線性模型不能準(zhǔn)確表示真實的摩擦現(xiàn)象，為此本研究使用下述的摩擦力模型，該模型在設(shè)計時考慮擬合低速時的負(fù)斜率現(xiàn)象并與庫侖黏性模型相結(jié)合

式中，λ是關(guān)節(jié)速度的閾值。

公式中的黏性摩擦模型kv（q·）改為

式中，kv3、kv2和kv1是實系數(shù)。

在本研究提出的模型中，庫侖摩擦在正方向和負(fù)方向都有表現(xiàn)。當(dāng)機(jī)器人關(guān)節(jié)向不同方向運動時，重力以與重力相反的方向施加到關(guān)節(jié)上。因此，式（4）中使用2個庫侖摩擦系數(shù)方向相反的kc。另一方面，準(zhǔn)確定義靜摩擦和低速摩擦是一個巨大的挑戰(zhàn)，與式（3）相比，式（4）中設(shè)置合適的閾值λ，使得關(guān)節(jié)的低速運動和高速運動更加平滑。此外，在計算中考慮關(guān)節(jié)的速度平方和速度立方，以保證摩擦模型的準(zhǔn)確性。

式（2）中機(jī)械臂動力學(xué)模型是非線性的，但參數(shù)辨識算法一般要求模型為線性的。因此需要對式（2）進(jìn)行線性化，轉(zhuǎn)換為下面的線性形式[25]

式中：τ為機(jī)械臂各關(guān)節(jié)力矩組成的向量；H（q，q·，q¨）為關(guān)于q，q·，q¨的非線性函數(shù)矩陣，即動力學(xué)參數(shù)回歸矩陣，其中包括連桿動力學(xué)部分回歸矩陣Hlink和摩擦力部分回歸矩陣Hf；δ為動力學(xué)的標(biāo)準(zhǔn)參數(shù)集向量，其中包括連桿標(biāo)準(zhǔn)動力學(xué)參數(shù)向量δlink和摩擦力動力學(xué)參數(shù)向量δf。

由于式（6）中的連桿動力學(xué)回歸矩陣存在整列為零和某些列線性相關(guān)的問題，需要重新整理待辨識的機(jī)械臂動力學(xué)參數(shù)，得到基本連桿動力學(xué)參數(shù)。當(dāng)前廣泛使用的方法是QR（正交三角）分解[26]，QR分解將矩陣Hlink和機(jī)械臂標(biāo)準(zhǔn)動力學(xué)參數(shù)向量δlink都分成2部分，動力學(xué)方程改為

式中：Hlink，b為矩陣Hlink中nb個線性無關(guān)列組成的子矩陣；Hlink，d為Ηlink中剩余nd個列組成的子矩陣；δlink，b為基本連桿動力學(xué)參數(shù)組成的列向量；δlink，d為連桿動力學(xué)模型中不起作用參數(shù)組成的列向量;τlink為由連桿動力學(xué)（不含摩擦力）產(chǎn)生的力矩。

綜上，將線性化后的參數(shù)集進(jìn)一步簡化，得到僅包含基本動力學(xué)參數(shù)的動力學(xué)方程

因此，機(jī)械臂的動力學(xué)模型為

式中：H=Hlink，bHf（Hf為摩擦力部分對應(yīng)的回歸矩陣）；π=δTlink，bδTfT（其參數(shù)數(shù)目p=nb+6n）。

1.2 建立非線性動力學(xué)模型

在伺服系統(tǒng)中，諧波環(huán)節(jié)和力矩傳遞存在誤差，摩擦力建模存在誤差，加速度測量波動較大以及濾波產(chǎn)生誤差，這部分慣量產(chǎn)生的力矩?zé)o法精確建模，本研究將其融進(jìn)誤差，即非線性動力學(xué)部分。這里基于傳統(tǒng)動力學(xué)模型得到模型力矩與實際力矩之差，對這部分進(jìn)行分析，對動力學(xué)模型的線性部分和非線性部分進(jìn)行統(tǒng)一建模

式中：τlink為不包含摩擦力部分的連桿動力學(xué)力矩；τm為關(guān)節(jié)電流驅(qū)動力矩，是真實情況下控制關(guān)節(jié)力矩，可由電流和換算系數(shù)直接獲得；τf為摩擦力矩；τerr為無法建模力矩包含一部分摩擦力和上述諧波誤差等不精確分量力矩的合力矩。

2 基于迭代的激勵軌跡優(yōu)化算法

本研究使用基于有限項傅里葉級數(shù)的激勵軌跡模型為設(shè)計目標(biāo)，提出了基于分塊回歸矩陣的迭代最小二乘的參數(shù)優(yōu)化算法，充分激發(fā)機(jī)器人動力學(xué)特性并使得回歸矩陣的條件數(shù)達(dá)到理想數(shù)據(jù)。

2.1 基于有限傅里葉級數(shù)的軌跡模型

激勵軌跡的確定為無限維泛函問題，很難得到解析解，因此對激勵軌跡參數(shù)化，從而將無限維泛函問題轉(zhuǎn)變?yōu)橛邢蘧S的優(yōu)化問題。本研究通過有限項的傅里葉級數(shù)對激勵軌跡參數(shù)化，有限項傅里葉級數(shù)表示激勵軌跡如下

式中：qi（t）、q·i（t）、q¨i（t）分別為關(guān)節(jié)i在t時刻下的關(guān)節(jié)位置、速度和加速度；N為傅里葉級數(shù)的階數(shù)；ωf為頻率；ci為關(guān)節(jié)i位置的常數(shù)項，al，i，bl，i為關(guān)節(jié)i的正余弦函數(shù)的幅值，因此al，i、bl，i、ci為待優(yōu)化參數(shù)；則每個關(guān)節(jié)的待辨識參數(shù)為2N+1個。

2.2 基于分塊觀測矩陣條件數(shù)的優(yōu)化目標(biāo)

針對激勵軌跡參數(shù)的優(yōu)化問題，已存在多種優(yōu)化指標(biāo)，如條件數(shù)法、行列式法和協(xié)方差矩陣法等，本研究選取矩陣條件數(shù)作為衡量激勵軌跡優(yōu)劣的指標(biāo)。對于任意給定矩陣A，其條件數(shù)的定義為

式中，σmax（A）和σmin（A）分別表示矩陣A奇異值中的最大值和最小值。

選取條件數(shù)作為優(yōu)化指標(biāo)的意義為：條件數(shù)值越小，機(jī)械臂在其允許的工作空間內(nèi)以較高速度和加速度盡可能地遍歷整個機(jī)器人工作空間，從而采集對參數(shù)辨識較為有用的信息。

由于激勵軌跡每個關(guān)節(jié)獨立，摩擦力的觀測矩陣存在很多0，容易造成條件數(shù)增大，所以本研究的激勵軌跡中所用的觀測矩陣不包含摩擦力部分。

對于加權(quán)最小二乘法，優(yōu)化目標(biāo)轉(zhuǎn)換為加權(quán)回歸矩陣H*，本研究用參考文獻(xiàn)[21]中的研究方法

式中，Ω為常矩陣，這里通過測量噪聲計算該矩陣。

同時，僅優(yōu)化H*的條件數(shù)難以達(dá)到想要的效果，還需要對H*子矩陣進(jìn)行優(yōu)化約束H*的內(nèi)部結(jié)構(gòu)。這里主要研究重力相關(guān)列組成的回歸矩陣H*1和重力無關(guān)列組成的回歸矩陣H*2。其中H*1和H*2的權(quán)重由變異系數(shù)法確定，H*的權(quán)重為H*1和H*2權(quán)重之和。用變異系數(shù)法求出基本連桿動力學(xué)回歸矩陣中每列所對應(yīng)的權(quán)重，對于本研究所使用的6自由度機(jī)械臂，該矩陣包含的列數(shù)為36，因此基本連桿動力學(xué)回歸矩陣中每列對應(yīng)的權(quán)重Wc=（wc1，wc2，…，wc36）。

式中：std（·）為標(biāo)準(zhǔn)差函數(shù)；mean（·）為平均值函數(shù)；H*（：，i）為回歸矩陣中第i個參數(shù)對應(yīng)的矢量。

變異系數(shù)法根據(jù)統(tǒng)計學(xué)方法計算出系統(tǒng)各指標(biāo)變化程度，該方法中變化差異較大的指標(biāo)權(quán)重較大，變化差異較小的指標(biāo)權(quán)重較小。

將重力相關(guān)列和重力無關(guān)列權(quán)重分別相加，得到H*1和H*2的權(quán)重系數(shù)wh1和wh2。

綜上所述，本研究的優(yōu)化目標(biāo)為

式中：α為待辨識參數(shù)向量；H*為歸一化回歸矩陣；H*1為H*子矩陣，對應(yīng)重力相關(guān)列組成的回歸矩陣；H*2為H*子矩陣，對應(yīng)重力無關(guān)列組成的回歸矩陣。

2.3 基于迭代方法的激勵軌跡參數(shù)優(yōu)化

使用迭代方法對激勵軌跡參數(shù)進(jìn)行優(yōu)化，讓當(dāng)前優(yōu)化的激勵軌跡與前面優(yōu)化的激勵軌跡形成互補，使得激勵軌跡的組合充分激發(fā)機(jī)器人動力學(xué)特性。

迭代優(yōu)化過程中，H*具有以下結(jié)構(gòu)：在第1次優(yōu)化時，H*形狀為 6 m×nb（m為采樣點數(shù)，nb為基本動力學(xué)參數(shù)（不含摩擦力參數(shù)）的數(shù)目）；在第2次優(yōu)化時，H*的形狀為12 m×nb，其中前面部分為第1次優(yōu)化的結(jié)果，后面部分為本次迭代要優(yōu)化的部分；在第k次優(yōu)化時，H*的形狀為6×k×m×nb，其中最后6 m行為當(dāng)前步驟要優(yōu)化的部分。在每次優(yōu)化過程中，α的起始點都隨機(jī)選取，若經(jīng)過當(dāng)前步驟的優(yōu)化，目標(biāo)函數(shù)降低，則記錄當(dāng)前結(jié)果；否則重新隨機(jī)選擇起始點繼續(xù)優(yōu)化；當(dāng)經(jīng)過15次嘗試后，目標(biāo)函數(shù)未降低，則認(rèn)為達(dá)到全局最優(yōu)并停止優(yōu)化。

最后激勵軌跡的優(yōu)化公式為

式中：qi，max、q·i，max、q¨i，max分別代表各關(guān)節(jié)可運行的最大角度、最大角速度以及最大角加速度，保證了機(jī)械臂在可承受的速度和加速度下安全運行不發(fā)生碰撞。后3個等式保證機(jī)械臂的平穩(wěn)啟停，其中t0和tf分別表示單個激勵軌跡周期內(nèi)的起始和終止時刻。

3 基于3次迭代辨識的動力學(xué)模型辨識及補償方法

本研究提出3次迭代辨識的動力學(xué)模型辨識方法，第1層先對連桿動力學(xué)τlink進(jìn)行理論辨識；第2層對摩擦力τf進(jìn)行辨識；第3層基于柔性誤差來補償不確定分量τerr。

3.1 辨識基本連桿動力學(xué)參數(shù)

該部分是辨識方法的內(nèi)層，根據(jù)各關(guān)節(jié)力矩噪聲大小對回歸矩陣進(jìn)行加權(quán)，將多維線性回歸問題轉(zhuǎn)化為單維線性回歸問題。

使機(jī)器人沿著特定的激勵軌跡運動，以一定的采樣頻率對運動過程中機(jī)器人各個關(guān)節(jié)的關(guān)節(jié)位置q、關(guān)節(jié)角速度q·、關(guān)節(jié)角加速度q¨和關(guān)節(jié)輸出力矩τ進(jìn)行采樣，且保證采樣數(shù)目m遠(yuǎn)遠(yuǎn)大于待辨識動力學(xué)參數(shù)的數(shù)目。這樣即可構(gòu)造回歸矩陣Hm、力矩向量Γm

式中，π^為動力學(xué)參數(shù)近似值；Hm∈Rmn×p，Γm∈Rmn×1。

使用最小二乘法解式（19）的線性回歸方程組，回歸系數(shù)π的計算公式如下

為使方差達(dá)到最優(yōu)，使用加權(quán)最小二乘法（WLS）。首先，計算殘差，即預(yù)測力矩和實測力矩之差

式中：R∈Rmn是殘差，其次

式中：Σjj為殘差方差矩陣Σ對角線j元素；Ej為E的第j行；var（·）為方差函數(shù)；E∈Rn×m是殘差的變形。加權(quán)最小二乘法計算回歸系數(shù)

式中：Σm∈Rmn×mn為分塊對角矩陣，其對角線上有m個Σ。在傳統(tǒng)辨識方法中，假設(shè)關(guān)節(jié)力矩的噪聲是相互獨立的，這里去掉相互獨立條件，則可以計算非對角協(xié)方差矩陣Ω∈Rn×n為

計算回歸系數(shù)的加權(quán)最小二乘法改為[20]

式中，Ωm∈Rmn×mn為塊對角矩陣，其對角線上有m個Ω，為提高辨識模型的精度，將迭代加權(quán)最小二乘（IRLS）方法引入用來識別基本動力學(xué)模型參數(shù)。

3.2 摩擦力辨識

對于這部分，在估計出摩擦力大小后，可尋找更準(zhǔn)確的模型來擬合摩擦力。本研究從測量力矩中減去與摩擦不相關(guān)的力矩（即由連桿參數(shù)計算的剛體動力學(xué)的力矩）來估計摩擦力[21]，即

在式（26）估計的摩擦力基礎(chǔ)上，本研究外層選擇的非線性摩擦力模型如下。

式中：庫侖摩擦部分包括正反方向2個正反方向相反的參數(shù)kc;黏滯摩擦部分包括kv3、kv2和kv13個參數(shù)。因此每個關(guān)節(jié)待辨識的參數(shù)為5個，這里通過閾值λ將摩擦力辨識分為2個部分。

摩擦力辨識首先通過低速運動部分的數(shù)據(jù)，辨識低速運動部分的摩擦力參數(shù)。并且為了準(zhǔn)確辨識參數(shù)，得到合適的λ。本研究使用修改的Lawson和Hanson的NNLS非線性最小二乘法確定λ，算法如下

式中：Festi為通過測量力矩和內(nèi)層辨識確定的第i個關(guān)節(jié)的摩擦力矩；τfi為通過摩擦模型估計的第i個關(guān)節(jié)的摩擦力矩。在確定λ之后再通過高速運動部分的數(shù)據(jù)，辨識高速運動部分的摩擦力參數(shù)。

3.3 基于柔性誤差補償不確定分量

針對τerr這部分無法精確建模的力矩分量，本研究通過GMM算法進(jìn)行非線性逼近，誤差力矩估計為

式中，u為輸入向量。

假設(shè)GMM中包含高斯成分的數(shù)量為Gl，定義為O=el，μl，ΣlGll=1，其中e1，e2，…，eGl是高斯成分的混合系數(shù)，并且有約束el>0和∑Gll=1el=1;μ1，μ2，…，μGl為高斯成分的均值向量;Σ1，Σ2，…，ΣGl為高斯成分的協(xié)方差矩陣。

采用GMM算法擬合數(shù)據(jù)的過程：首先，創(chuàng)建數(shù)據(jù)集為Z={Z1，Z2，…，Zf}（f=1，2，…，m），式中，m為數(shù)據(jù)集Z的個數(shù);Zf=Zs，f，Zξ，f，Zs，f∈［q，q·］=u，Zξ，f∈f（u）。Z的分布由有限高斯分布的GMM表示，概率密度表示為

式中：Ol為第l個高斯成分的相關(guān)變量。

其中第l個高斯成分定義為

GMM通過期望最大化（EM）算法得到GMM參數(shù)，再使用高斯混合回歸（GMR）復(fù)合期望函數(shù)f（·），在給定數(shù)據(jù)u的情況下，f（u）的條件概率也滿足高斯分布，即

3次迭代辨識算法的流程圖如圖3所示。

4 結(jié)果與分析

本研究試驗均在中科深谷的COMAN R5 6自由度機(jī)械臂上完成，代碼程序均在聯(lián)想電腦的PyCharm上開發(fā)完成，試驗的機(jī)械臂實物如圖4所示，模擬機(jī)械臂果球采收如圖5所示，機(jī)械臂修改后的D-H參數(shù)見表1。

4.1 激勵軌跡求解

對于激勵軌跡的求解，根據(jù)變異系數(shù)法求出每個參數(shù)權(quán)重的均值為

確定慣性子回歸矩陣和重力子回歸矩陣的權(quán)重比為

其中，round（·）為四舍五入取整算法?；貧w矩陣H*前的系數(shù)為22。因此優(yōu)化激勵軌跡的目標(biāo)改為

此過程中，本研究軌跡單位為弧度，基礎(chǔ)頻率為ωf=2πff，激勵頻率為ff=0.1 Hz。同時，為客觀展現(xiàn)本研究方法與傳統(tǒng)最小二乘法、普通迭代最小二乘法相比的優(yōu)越性，在表2中列出了不同方法的條件數(shù)。

4.2 試驗及數(shù)據(jù)

通過試驗評估3次迭代辨識的準(zhǔn)確性。操作如下：1）采集機(jī)械臂在激勵軌跡下運動的關(guān)節(jié)角度、關(guān)節(jié)力矩值（電機(jī)電流，單位mA），采樣頻率為fs=1 kHz；2）使用五階巴特沃斯濾波器對關(guān)節(jié)角度和關(guān)節(jié)力矩進(jìn)行濾波，從而降低噪聲對數(shù)據(jù)的影響；3）對關(guān)節(jié)角度進(jìn)行中心差分運算，得到關(guān)節(jié)角速度和關(guān)節(jié)角加速度；4）分別使用不同的參數(shù)辨識算法，辨識動力學(xué)參數(shù)，并使用辨識出的參數(shù)進(jìn)行力矩擬合，驗證結(jié)果。

關(guān)于摩擦部分，通過速度-摩擦力矩電流擬合證明摩擦模型的有效性，使用本研究提出的摩擦模型擬合摩擦力矩效果如圖6所示。

為驗證辨識算法的準(zhǔn)確性和有效性，分別將傳統(tǒng)最小二乘法、迭代最小二乘法和本研究方法的力矩擬合情況展示在圖7—圖9中。從圖7—圖9中可以發(fā)現(xiàn)，本研究提出的算法計算的殘差相對最小，參數(shù)模型精度最高。為客觀展現(xiàn)本研究方法的優(yōu)越性，列出使用不同識別方法的驗證噪聲殘差的均方根，其中最小二乘法表示標(biāo)準(zhǔn)最小二乘法；噪聲代表測量電流和濾波電流之間的殘差均方根。

為更好地展現(xiàn)加入GMM的效果，將加入GMM前和加入GMM后的測量電流和濾波電流之間殘差進(jìn)行對比，如圖10所示。

上述結(jié)果表明，與傳統(tǒng)的最小二乘法相比，本研究提出的3次迭代辨識方案有顯著的改進(jìn)，并且在幾個關(guān)節(jié)中殘差的力矩均方根值降低了30%；參數(shù)辨識精度由6個關(guān)節(jié)的平均力矩殘差均方根衡量，其中使用最小二乘法的力矩殘差平均均方根為9.53、迭代最小二乘法的力矩殘差平均均方根為9.32，本研究方法力矩殘差平均均方根為6.14。證明了本研究方法相較于傳統(tǒng)方法的優(yōu)越性。

5 結(jié)論

針對林木果球采收協(xié)作機(jī)械臂動力學(xué)模型參數(shù)精準(zhǔn)辨識問題，充分考慮機(jī)械臂多連桿動力學(xué)辨識誤差、非線性摩擦力和不確定噪聲力矩影響，提出一種協(xié)作機(jī)械臂動力學(xué)參數(shù)模型精準(zhǔn)辨識及補償方法，即1次軌跡優(yōu)化和3次迭代辨識過程，通過激勵軌跡優(yōu)化，收集采樣機(jī)械臂在激勵軌跡下運動的關(guān)節(jié)角度/關(guān)節(jié)力矩值，3次迭代辨識及柔性補償處理，獲得了相對傳統(tǒng)方法的突出優(yōu)化效果。

首先設(shè)計了能充分激發(fā)機(jī)械臂動力學(xué)特性的激勵軌跡。通過對比條件數(shù)為329的最小二乘法和條件數(shù)為275的迭代最小二乘法，條件數(shù)為193的本研究方法有明顯優(yōu)勢。

在對協(xié)作機(jī)械臂的動力學(xué)參數(shù)辨識提出了以下2方面改進(jìn)，一是使用構(gòu)建區(qū)分高、低速的非線性摩擦力模型擬合機(jī)械臂動摩擦力效應(yīng)；二是將GMM算法引入動力學(xué)參數(shù)辨識中，補償不可線性擬合的不確定力矩殘差部分。

通過COMAN R5六軸機(jī)械臂實機(jī)測試結(jié)果表明，該方法與傳統(tǒng)最小二乘法相比，使得辨識結(jié)果的殘差均方根降低了30%，模型參數(shù)辨識精度提升效果非常顯著。

【參 考 文 獻(xiàn)

[1]孔慶華，陸懷民，王守忠.RBT14型林木球果采集機(jī)的設(shè)計[J].東北林業(yè)大學(xué)學(xué)報，1997（6）：61-63。

KONG Q H， LU H M， WANG S Z. Design of RBT14-cone collecting machine[J]. Journal of Northeast Forestry University， 1997， 25（6）： 60-62.

[2]XIAO J L， ZENG F， ZHANG Q L， et al. Research on the forcefree control of cooperative robots based on dynamic parameters identification[J]. Industrial Robo， 2019， 46（4）： 499-509.

[3]李智靖，葉錦華，吳海彬.機(jī)器人帶未知負(fù)載條件下的碰撞檢測算法[J].機(jī)器人，2020，42（1）：29-38。

LI Z J， YE J H， WU H B. Collision detection algorithm for robots with unknown payload[J]. Robot， 2020， 42（1）： 29-38.

[4]FU Z T， SPYRAKOS-PAPASTAVRIDIS E， LIN Y H， et al. Analytical expressions of serial manipulator jacobians and their high-order derivatives based on lie theory[C]//2020 IEEE International Conference on Robotics and Automation （ICRA）. May 31 - August 31， 2020， Paris， France. IEEE， 2020： 7095-7100.

[5]孫昌國，馬香峰，譚吉林.機(jī)器人操作器慣性參數(shù)的計算[J].機(jī)器人，1990，12（2）：19-24。

SUN C G， MA X F， TAN J L. Calculation of inertial parameter of robot manipulator[J]. Robot， 1990， 12（2）： 19-24.

[6]ARMSTRONG B， KHATIB O， BURDICK J. The explicit dynamic model and inertial parameters of the PUMA 560 arm[C]//Proceedings of 1986 IEEE International Conference on Robotics and Automation. April 7-10， 1986， San Francisco， CA， USA. IEEE， 2003： 510-518.

[7]WU J， WANG J S， YOU Z. An overview of dynamic parameter identification of robots[J]. Robotics and Computer-Integrated Manufacturing， 2010， 26（5）： 414-419.

[8]KINSHEEL A. Robust least square estimation of the CRS A465 robot arm's dynamic model parameters[J]. Journal of Mechanical Engineering Research， 2012， 4（3）：89.

[9]周軍，余躍慶.考慮關(guān)節(jié)柔性的模塊機(jī)器人動力學(xué)參數(shù)辨識[J].機(jī)器人，2011，33（4）：440-448。

ZHOU J， YU Y Q. Dynamic parameter identification of modular robot with flexible joints[J]. Robot， 2011， 33（4）： 440-448.

[10]GAUTIER M. Dynamic identification of robots with power model[C]//Proceedings of International Conference on Robotics and Automation. April 25-25， 1997， Albuquerque， NM， USA. IEEE， 2002： 1922-1927.

[11]WOLF S， ISKANDAR M. Extending a dynamic friction model with nonlinear viscous and thermal dependency for a motor and harmonic drive gear[C]//2018 IEEE International Conference on Robotics and Automation （ICRA）. May 21-25， 2018， Brisbane， QLD， Australia. IEEE， 2018： 783-790.

[12]ISKANDAR M， WOLF S. Dynamic friction model with thermal and load dependency： modeling， compensation， and external force estimation[C]//2019 International Conference on Robotics and Automation （ICRA）. May 20-24， 2019， Montreal， QC， Canada. IEEE， 2019： 7367-7373.

[13]GAUTIER M， KHALIL W. Exciting trajectories for the identification of base inertial parameters of robots[C]//[1991] Proceedings of the 30th IEEE Conference on Decision and Control. December 11-13， 1991， Brighton， UK. IEEE， 2002： 494-499.

[14]PRESSE C， GAUTIER M. New criteria of exciting trajectories for robot identification[C]//[1993] Proceedings IEEE International Conference on Robotics and Automation. May 2-6， 1993， Atlanta， GA， USA. IEEE， 2002： 907-912.

[15]丁力，吳洪濤，姚裕，等.基于WLS-ABC算法的工業(yè)機(jī)器人參數(shù)辨識[J].華南理工大學(xué)學(xué)報（自然科學(xué)版），2016，44（5）：90-95。

DING L， WU H T， YAO Y， et al. Parameters identification of industrial robots based on WLS-ABC algorithm[J]. Journal of South China University of Technology （Natural Science Edition）， 2016， 44（5）： 90-95.

[16]BONNET V， FRAISSE P， CROSNIER A， et al. Optimal exciting dance for identifying inertial parameters of an anthropomorphic structure[J]. IEEE Transactions on Robotics， 2016， 32（4）： 823-836.

[17]CHEN E W， LIU Z S， GAN F J. Application of ANN in identification of inertial parameters of end-effector of robot[C]//2006 IEEE International Conference on Information Acquisition. August 20-23， 2006， Veihai， China. IEEE， 2007： 972-977.

[18]豐非，扈宏杰.基于混沌PSO的SCARA機(jī)器人參數(shù)辨識[J].自動化與儀表，2017，32（12）：14-18。

FENG F， HU H J. Parameter identification of SCARA robot based on PSO algorithm of chaos[J]. Automation & Instrumentation， 2017， 32（12）： 14-18.

[19]FU Z T， PAN J B， SPYRAKOS-PAPASTAVRIDIS E， et al. A lie-theory-based dynamic parameter identification methodology for serial manipulators[J]. IEEE/ASME Transactions on Mechatronics， 2021， 26（5）： 2688-2699.

[20]GAUTIER M， JANOT A， VANDANJON P O. A new closed-loop output error method for parameter identification of robot dynamics[J]. IEEE Transactions on Control Systems Technology， 2013， 21（2）： 428-444.

[21]韓勇.協(xié)作機(jī)器人模型辨識方法與人機(jī)交互控制技術(shù)研究[D].上海：上海交通大學(xué)，2020。

HAN Y. Model identification and human-robot interaction control of robots[D]. Shanghai： Shanghai Jiao Tong University， 2020.

[22]BLUNDELL R， BOND S. GMM Estimation with persistent panel data： an application to production functions[J]. Econometric Reviews， 2000， 19（3）： 321-340.

[23]NIKU S B. An introduction to robotics analysis， systems， applications[M]. Upper Saddle River， NJ： Prentice Hall， 2001

[24]BAGLIONI S， CIANETTI F， BRACCESI C， et al. Multibody modelling of N DOF robot arm assigned to milling manufacturing. Dynamic analysis and position errors evaluation[J]. Journal of Mechanical Science and Technology， 2016， 30（1）： 405-420.

[25]羅欣.機(jī)器人平滑運動軌跡規(guī)劃及控制方法的研究[D].廣州：華南理工大學(xué)，2017。

LUO X. Smooth motion trajectory planning and control method research for robot[D]. Guangzhou： South China University of Technology， 2017.

[26]蘇二虎.基于優(yōu)化激勵軌跡的工業(yè)機(jī)器人動力學(xué)參數(shù)辨識[D].蕪湖：安徽工程大學(xué)，2019。

SU E H. Dynamic parameter identification of industrial robot based on the optimal exciting trajectory[D]. Wuhu： Anhui Polytechnic University， 2019.