林木果球采收機(jī)械臂動(dòng)力學(xué)參數(shù)辨識(shí)及補(bǔ)償

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

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

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

中圖分類號(hào)：S776;TP241.3 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：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

基金項(xiàng)目：國(guó)家自然科學(xué)基金項(xiàng)目（61821005）;國(guó)家重點(diǎn)研發(fā)計(jì)劃項(xiàng)目（2018YFE0205802）。

第一作者簡(jiǎn)介：趙月，碩士研究生。研究方向?yàn)闄C(jī)械臂動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)。E-mail： zhaoyue_0329@nefu.edu.cn

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

0 引言

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

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

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

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

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

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

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

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

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

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

式中：M（q）∈Rn×n和n分別為正定對(duì)稱慣性矩陣和關(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ū)動(dòng)力矩和摩擦力矩；J（q）T為機(jī)械臂的雅可比矩陣；fext為作用在機(jī)械臂末端的外力項(xiàng)矢量。

1.1.2 線性化模型

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

式中，Ω為常矩陣，這里通過測(cè)量噪聲計(jì)算該矩陣。

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

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

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

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

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

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

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

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

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

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

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

3 基于3次迭代辨識(shí)的動(dòng)力學(xué)模型辨識(shí)及補(bǔ)償方法

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

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

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

使機(jī)器人沿著特定的激勵(lì)軌跡運(yùn)動(dòng)，以一定的采樣頻率對(duì)運(yùn)動(dòng)過程中機(jī)器人各個(gè)關(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)大于待辨識(shí)動(dòng)力學(xué)參數(shù)的數(shù)目。這樣即可構(gòu)造回歸矩陣Hm、力矩向量Γm

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

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

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

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

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

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

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

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

3.2 摩擦力辨識(shí)

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

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

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

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

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

3.3 基于柔性誤差補(bǔ)償不確定分量

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

式中，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的個(gè)數(shù);Zf=Zs，f，Zξ，f，Zs，f∈［q，q·］=u，Zξ，f∈f（u）。Z的分布由有限高斯分布的GMM表示，概率密度表示為

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

其中第l個(gè)高斯成分定義為

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

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

4 結(jié)果與分析

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

4.1 激勵(lì)軌跡求解

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

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

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

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

4.2 試驗(yàn)及數(shù)據(jù)

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

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

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

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

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

5 結(jié)論

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

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

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

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

【參 考 文 獻(xiàn)

[1]孔慶華，陸懷民，王守忠.RBT14型林木球果采集機(jī)的設(shè)計(jì)[J].東北林業(yè)大學(xué)學(xué)報(bào)，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ù)載條件下的碰撞檢測(cè)算法[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]孫昌國(guó)，馬香峰，譚吉林.機(jī)器人操作器慣性參數(shù)的計(jì)算[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ī)器人動(dòng)力學(xué)參數(shù)辨識(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ù)辨識(shí)[J].華南理工大學(xué)學(xué)報(bào)（自然科學(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ù)辨識(shí)[J].自動(dòng)化與儀表，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ī)器人模型辨識(shí)方法與人機(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ī)器人平滑運(yùn)動(dòng)軌跡規(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)化激勵(lì)軌跡的工業(yè)機(jī)器人動(dòng)力學(xué)參數(shù)辨識(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.