未知時(shí)變環(huán)境下機(jī)器人自校正阻抗控制研究

曾獻(xiàn)文，翟敬梅

(華南理工大學(xué)機(jī)械與汽車工程學(xué)院，廣東 廣州 510000)

1 引言

隨著“中國制造2025”制造強(qiáng)國戰(zhàn)略的不斷推進(jìn)，機(jī)器人已經(jīng)在工業(yè)、服務(wù)業(yè)領(lǐng)域獲得越來越廣泛的應(yīng)用，工作任務(wù)也不再僅僅需要位置控制，在面對(duì)諸多作業(yè)場合如人體按摩、皮下注射、柔性體抓取等操作時(shí)，需要進(jìn)行機(jī)器人位置和力的雙重控制如阻抗控制。不同于剛性體，上述作業(yè)對(duì)象具有柔順材料典型的非線性彈性等特征；另一方面，作業(yè)對(duì)象為非結(jié)構(gòu)化環(huán)境，不同作業(yè)對(duì)象往往具有不同的力學(xué)特性，即使同一對(duì)象，不同區(qū)域的力學(xué)特性往往由于生物體征、組織結(jié)構(gòu)等因素而存在變化。上述材料種類及其力學(xué)特性的不確定導(dǎo)致阻抗控制力跟蹤誤差較大甚至失去力跟蹤能力。

相關(guān)研究表明機(jī)器人力、位控制器如阻抗控制器的穩(wěn)定性及精準(zhǔn)性受到環(huán)境力學(xué)特性的直接影響[1]，因此對(duì)環(huán)境接觸動(dòng)力學(xué)的研究能夠有效改善機(jī)器人阻抗控制器力控制性能。相關(guān)工作如李臣[2]在人體按摩機(jī)器人柔順控制中采用純剛度模型描述機(jī)器人與環(huán)境之間的接觸動(dòng)力學(xué)特征。Jones K.C[3]、Koga H[4]基于Kelvin-Voigt模型描述環(huán)境動(dòng)力學(xué)對(duì)面向人體表面工作的機(jī)器人進(jìn)行力和位置雙重控制。上述研究的接觸動(dòng)力學(xué)模型是線性的，適用于剛性材料的線彈性特征，但對(duì)于肌膚之類柔順材料，則無法描述其非線性彈性特征[5]。Hunt–Crossley(HC)模型通過位置和阻尼的耦合同時(shí)引入冪指數(shù)項(xiàng)，同時(shí)表征剛性和柔順材料的力學(xué)特征[6]。文獻(xiàn)[7]基于HC模型建立按摩機(jī)器人與人體接觸時(shí)浸入深度方向上的受力模型，適應(yīng)肌膚柔性體的非線性彈性等生物力學(xué)特征，但是未應(yīng)用于實(shí)際控制系統(tǒng)。

阻抗控制要求建立準(zhǔn)確的機(jī)械手與環(huán)境之間的接觸動(dòng)力學(xué)模型，在未知環(huán)境下由于接觸動(dòng)力學(xué)模型參數(shù)未知，因此阻抗控制器不能實(shí)現(xiàn)穩(wěn)定力位跟蹤控制效果，需要在經(jīng)典控制的基礎(chǔ)上進(jìn)行自適應(yīng)調(diào)整。研究[8]采用自校正控制在未知環(huán)境下進(jìn)行環(huán)境參數(shù)的在線辨識(shí)和控制量的預(yù)測調(diào)整，但是采用的基于遞推最小二乘辨識(shí)算法的自校正控制無法適用于接觸動(dòng)力學(xué)參數(shù)存在時(shí)變的控制對(duì)象。陳雙[9]基于遺忘因子遞推最小二乘法對(duì)汽車座椅非線性模型參數(shù)進(jìn)行在線辨識(shí)克服了傳統(tǒng)遞推最小二乘算法在面向時(shí)變參數(shù)時(shí)的數(shù)據(jù)飽和現(xiàn)象。類似地，基于窗函數(shù)的最小二乘法[10]在傳統(tǒng)最小二乘算法中引入矩形窗，去除舊數(shù)據(jù)而只截取一段長度的新數(shù)據(jù)進(jìn)行模型參數(shù)辨識(shí)。但是上述形式的遞推最小二乘法通常難以兼具收斂性和跟蹤性，無法很好地滿足未知時(shí)變環(huán)境下的參數(shù)實(shí)時(shí)辨識(shí)需求。針對(duì)上述研究現(xiàn)狀及問題，基于HC接觸動(dòng)力學(xué)模對(duì)阻抗控制進(jìn)行位置規(guī)劃，引入自擾動(dòng)遞推最小二乘辨識(shí)算法(SPRLS)至最小方差自校正控制中，實(shí)現(xiàn)對(duì)未知且時(shí)變環(huán)境下的接觸動(dòng)力學(xué)模型參數(shù)的實(shí)時(shí)辨識(shí)和對(duì)阻抗控制參考位置的自適應(yīng)調(diào)整，研究方法可有效跟蹤辨識(shí)材料(包括剛性和柔性)的力學(xué)特性，并對(duì)其進(jìn)行實(shí)時(shí)、準(zhǔn)確的力位跟蹤控制。

2 基于HC非線性模型的阻抗控制系統(tǒng)

基于位置的阻抗控制中位置誤差x-xd與力誤差Fr-F之間的傳遞函數(shù)G(s)：

式中：Md、Bd、Kd—期望阻抗模型的慣性矩陣、阻尼矩陣和剛度矩陣，x—機(jī)器人末端實(shí)際位置，xd—機(jī)器人期望的軌跡，在浸入深度方向上，xd—達(dá)到期望接觸力所需要的機(jī)器人末端運(yùn)動(dòng)位置。F—機(jī)器人末端與環(huán)境的實(shí)際接觸力，F(xiàn)r—期望力。實(shí)際控制時(shí)，xd不能直接得到，通過接觸動(dòng)力學(xué)模型計(jì)算得到期望力對(duì)應(yīng)的位置記為xr，稱為參考位置。

HC接觸動(dòng)力學(xué)模型[6]具有如下形式：

式中：LkHC和bHC—彈性和阻尼系數(shù)，n—由接觸體的局部接觸區(qū)域中的材料和幾何性質(zhì)決定的冪指數(shù)，n的存在使HC模型更好的描述接觸力隨侵入深度的非線性變化，當(dāng)指數(shù)n=1時(shí)則表征了線彈性物體的力學(xué)特性。

基于HC模型進(jìn)行xr的預(yù)測輸入，此時(shí)機(jī)器人末端執(zhí)行器達(dá)到目標(biāo)位置便不再進(jìn)給，進(jìn)給速度x˙=0。此時(shí)的接觸力fHC=Fr，機(jī)器人末端位置x=xr，xe是環(huán)境未受力變形時(shí)的初始位置。代入上式(2)得到：

建立面向柔順和剛性材料的阻抗控制系統(tǒng)框架，如圖1所示。

圖1 基于HC模型的阻抗控制系統(tǒng)Fig.1 Impedance Control System Based on HC Model

3 面向未知環(huán)境的融合SPRLS算法的自校正阻抗控制系統(tǒng)

基于HC模型的阻抗控制算法中，系統(tǒng)輸入xr的確定，需kHC、n、xe等環(huán)境參數(shù)，而實(shí)際控制中這些參數(shù)往往未知；不同作業(yè)區(qū)域的力學(xué)特性存在變化，機(jī)器人在不同區(qū)域間運(yùn)動(dòng)導(dǎo)致接觸動(dòng)力學(xué)參數(shù)存在時(shí)變；且單純阻抗控制還未考慮信號(hào)噪聲、工業(yè)延時(shí)等干擾因素的影響，以上環(huán)境力學(xué)特性參數(shù)的不確定性和外界干擾將會(huì)對(duì)傳統(tǒng)阻抗控制的力跟蹤性能產(chǎn)生不利影響。

為適應(yīng)未知、時(shí)變作業(yè)對(duì)象的動(dòng)力學(xué)特性，提出融合最小方差控制與SPRLS算法的自適應(yīng)阻抗控制系統(tǒng)，通過前向檢測預(yù)測誤差來判斷環(huán)境力學(xué)特性的變化，并據(jù)此設(shè)置自擾動(dòng)項(xiàng)，實(shí)現(xiàn)對(duì)接觸動(dòng)力學(xué)模型參數(shù)的實(shí)時(shí)跟蹤辨識(shí)；同時(shí)，基于辨識(shí)結(jié)果設(shè)計(jì)最小方差自校正調(diào)節(jié)器，實(shí)現(xiàn)對(duì)未知時(shí)變環(huán)境下阻抗控制參考位置的自適應(yīng)調(diào)整，具體結(jié)構(gòu)，如圖2所示。

圖2 融合SPRLS算法的自校正阻抗控制系統(tǒng)結(jié)構(gòu)框架Fig.2 Structure of Self-Tunning Impedance Control System with SPRLS Algorithm

3.1 HC模型的自擾動(dòng)遞推最小二乘參數(shù)辨識(shí)

自擾動(dòng)遞推最小二乘法核心思想是在線辨識(shí)的過程中檢測前向預(yù)測誤差，據(jù)此設(shè)置自擾動(dòng)項(xiàng)，在前向預(yù)測誤差大于閾值時(shí)，介入自擾動(dòng)項(xiàng)增加參數(shù)遞推估計(jì)時(shí)的迭代量。在前向預(yù)測誤差小于閾值時(shí)，自擾動(dòng)項(xiàng)歸零，等效于標(biāo)準(zhǔn)遞推最小二乘算法，不影響收斂性。實(shí)現(xiàn)辨識(shí)算法對(duì)環(huán)境參數(shù)變化的快速響應(yīng)以及參數(shù)辨識(shí)的收斂性，算法流程見圖3模塊1。

普通辨識(shí)算法不適用于對(duì)于HC模型的非線性結(jié)構(gòu)，目前的解決方案有二階辨識(shí)算法[6]和一階辨識(shí)算法[11]，相比于二階辨識(shí)算法，一階辨識(shí)算法將模型轉(zhuǎn)化為線性化形式，其收斂條件更易于滿足，具有更高的魯棒性。故使用一階辨識(shí)算法將HC模型轉(zhuǎn)換為線性形式，并采用自擾動(dòng)遞推最小二乘法對(duì)其進(jìn)行單點(diǎn)參數(shù)辨識(shí)。根據(jù)對(duì)HC模型下參考位置的推導(dǎo)式(3)整理并經(jīng)一階辨識(shí)算法對(duì)等式兩邊取自然對(duì)數(shù)，化為如下線性形式：

上式(4)寫成最小二乘形式：

其中，Yk=ln(F)

按照自擾動(dòng)遞推最小二乘法，算法的遞推方程如下：

式中：β和γ—擾動(dòng)增益和誤差增益；I—單位矩陣，NINT()的表達(dá)式為：

3.2 基于參數(shù)辨識(shí)結(jié)果的最小方差自校正調(diào)節(jié)器設(shè)計(jì)

自校正控制系統(tǒng)在獲取模型參數(shù)辨識(shí)的同時(shí)，最小方差自校正調(diào)節(jié)器按照最小輸出方差為目標(biāo)進(jìn)行輸入調(diào)整。其基本思想：由于機(jī)器人控制存在純延遲d，因此要使輸出方差最小就需要提前d步對(duì)輸出量做預(yù)測并設(shè)計(jì)所需控制。對(duì)式(4)進(jìn)行參數(shù)辨識(shí)后，對(duì)應(yīng)如下形式的時(shí)間平移算子多項(xiàng)式：

其中，u=ln(x-xe)；y=ln(F)-ln(kHC)；

ξ(k)—方差為σ2噪聲，d≥1—控制延時(shí)。

基于k時(shí)刻的輸入和輸出，對(duì)k+d時(shí)刻的輸出的預(yù)測記作，則預(yù)測誤差：。

式中E、F、G式滿足如下Deophantine方程：

B(z-1)為Hurwitz多項(xiàng)式，設(shè)控制目標(biāo)是使輸出y(k+d)跟蹤期望輸出yr(k+d)要滿足如下性能指標(biāo)：

最小方差控制率為：

最小方差自校正調(diào)節(jié)器結(jié)構(gòu)見圖3模塊2。

基于前述自校正控制算法進(jìn)行阻抗控制參考位置自適應(yīng)調(diào)整，根據(jù)對(duì)自校正調(diào)節(jié)器輸入u的最優(yōu)預(yù)測，將u中的機(jī)器人末端位置量x作為阻抗控制參考位置輸入圖3模塊3所示的阻抗控制系統(tǒng)實(shí)現(xiàn)對(duì)阻抗控制的位置修正。具體算法及其相互關(guān)系，如圖3所示。

圖3 融合SPRLS算法的自校正阻抗控制系統(tǒng)Fig.3 Self-Tunning Impedance Control System with SPRLS Algorithm

4 仿真實(shí)驗(yàn)及分析

為模擬環(huán)境力學(xué)特性參數(shù)未知且在不同作業(yè)區(qū)域間存在差異的應(yīng)用場景，設(shè)基于HC模型的環(huán)境動(dòng)力學(xué)參數(shù)未知且時(shí)變，線彈性和非線性彈性兩種環(huán)境并存。設(shè)環(huán)境的真實(shí)動(dòng)力學(xué)特性如下：

第(0～100)個(gè)采樣周期內(nèi)：F=1.9(x-6)；

第(101～200)個(gè)采樣周期內(nèi)：F=2(x-7.5)1.2；

第(201～400)個(gè)采樣周期內(nèi)：F=2.5(x-9)1.4。

第(0～150)個(gè)采樣周期內(nèi)期望力Fr=10N，(151～300)個(gè)采樣周期內(nèi)Fr=5N，(301～400)個(gè)采樣周期內(nèi)Fr=10N。

這里設(shè)xe在6、7.5、9間變化，可經(jīng)視覺設(shè)備獲取。設(shè)機(jī)器人延時(shí)d=4，外界干擾為方差為0.05的白噪聲；阻抗控制的阻抗參數(shù)設(shè)為Md=65Bd=15Kd=1。

仿真結(jié)果如圖4～圖6及表1、表2所示，k為第k個(gè)采樣周期。

圖4 參數(shù)ln(kHC)、n辨識(shí)過程Fig.4 Identification Process of Parameter ln(kHC)、n

圖5 自適應(yīng)阻抗控制系統(tǒng)力跟蹤效果Fig.5 Force Tracking Effect of Adaptive Impedance Control System

圖6 機(jī)器人末端浸入環(huán)境方向的位置量Fig.6 Amount of Robot Tip’s Position Immersed in the Environment.

表1 力跟蹤穩(wěn)態(tài)誤差Tab.1 Steady State Error of Force Tracking

表2 環(huán)境突變時(shí)控制效果Tab.2 Control Effects when the Environment is Changed

在仿真實(shí)驗(yàn)中，環(huán)境模型事先未知，且在多個(gè)時(shí)刻存在參數(shù)的突變，期望力也存在變化，實(shí)驗(yàn)結(jié)果及分析可見：

(1)由表1和圖5中穩(wěn)態(tài)接觸力可見，接觸力穩(wěn)態(tài)均值與期望值吻合，誤差小于2%，力控制效果穩(wěn)定良好。

(2)由圖4可見SPRLS算法對(duì)時(shí)變環(huán)境特性參數(shù)能夠?qū)崿F(xiàn)較好的跟蹤辨識(shí)，且當(dāng)環(huán)境參數(shù)發(fā)生變化時(shí)，由表2和圖6可見接觸力存在短暫小范圍變化后便穩(wěn)定到期望值，對(duì)于期望力的變化也能實(shí)現(xiàn)很好的力跟蹤效果。

(3)由圖6可見在接觸面浸入方向上，機(jī)器人末端始終與環(huán)境有效接觸并根據(jù)力跟蹤需求調(diào)整浸入深度。

為了進(jìn)一步驗(yàn)證所提出的算法合理性，與當(dāng)前多種算法進(jìn)行了對(duì)比仿真試驗(yàn)：基于HC模型分別結(jié)合遞推最小二乘(RLS)、窗函數(shù)遞推最小二乘(BLS)[10]、遺忘因子遞推最小二乘(FFRLS)辨識(shí)方法[9]的最小方差自校正控制(MVSTC)算法。對(duì)比實(shí)驗(yàn)結(jié)果，如圖7、圖8及表3所示。

圖7 對(duì)比試驗(yàn)結(jié)果Fig.7 Results of Comparitive Experiment

圖8 算法力跟蹤性能對(duì)比Fig.8 Comparison of Different Algorithms’Force Tracking Performance

表3 對(duì)比實(shí)驗(yàn)力跟蹤數(shù)據(jù)分析Tab.3 Force Tracking Data Analysis of Comparative Experiments

由圖7、圖8及表3對(duì)比試驗(yàn)結(jié)果可知：

對(duì)于非線性彈性接觸環(huán)境，由于實(shí)際跟蹤控制要基于其線性化后的數(shù)學(xué)對(duì)象，因而所提算法及對(duì)比實(shí)驗(yàn)在環(huán)境力學(xué)特性突變時(shí)力突變均存在一定誤差擴(kuò)大，但是在以下方面有較大區(qū)別：

(1)傳統(tǒng)基于遞推最小二乘辨識(shí)的自校正控制不能適應(yīng)環(huán)境力學(xué)特性的變化，環(huán)境力學(xué)特性出現(xiàn)變化時(shí)會(huì)產(chǎn)生力跟蹤誤差，最大達(dá)期望值的27%，本研究所提算法的穩(wěn)態(tài)接觸力誤差則小于期望值的2%；

(2)期望力發(fā)生變化時(shí)，本研究所提算法能夠很快調(diào)整進(jìn)給量實(shí)現(xiàn)實(shí)時(shí)跟蹤，其他三種算法則有較大延時(shí)。

(3)基于窗函數(shù)遞推最小二乘和遺忘因子遞推最小二乘參數(shù)辨識(shí)的自校正控制，在環(huán)境力學(xué)特性變化時(shí)的跟蹤調(diào)整時(shí)間較大，本研究所提算法調(diào)整時(shí)間減少約70%。

可見環(huán)境參數(shù)存在未知和變化時(shí)，基于HC模型，結(jié)合自擾動(dòng)遞推最小二乘辨識(shí)的最小方差自校正阻抗控制能夠保持柔順控制能力的基礎(chǔ)上，具有較快的反應(yīng)速度和收斂能力，控制效果準(zhǔn)確穩(wěn)定。

5 結(jié)論

針對(duì)當(dāng)前環(huán)境力學(xué)特性未知或時(shí)變下的力位控制問題，提出了基于HC接觸動(dòng)力學(xué)模型的自校正阻抗控制算法，在多個(gè)時(shí)變環(huán)境下進(jìn)行仿真實(shí)驗(yàn)與分析，驗(yàn)證了提出方法的適用性和有效性，該算法具有如下優(yōu)點(diǎn)：

(1)基于HC接觸動(dòng)力學(xué)模型指導(dǎo)機(jī)器人阻抗控制的參考位置規(guī)劃，阻抗控制系統(tǒng)對(duì)柔順材料和剛性材料都具有較好適用性。

(2)在環(huán)境動(dòng)力學(xué)特性未知且存在時(shí)變的情況下，提出的結(jié)合SPRLS算法的自校正阻抗控制系統(tǒng)，能夠快速穩(wěn)定地跟蹤辨識(shí)接觸動(dòng)力學(xué)參數(shù)，并克服機(jī)器人控制延時(shí)，實(shí)時(shí)自適應(yīng)調(diào)整阻抗控制參考位置，穩(wěn)態(tài)力跟蹤誤差小于2%，環(huán)境特性突變時(shí)的控制調(diào)整時(shí)間僅約7個(gè)采樣周期。