本發(fā)明屬于機(jī)器人控制領(lǐng)域，具體是一種基于接觸力觀測(cè)器的機(jī)器人柔順控制方法，無(wú)需多維力傳感器，采用模型估計(jì)的方法估計(jì)接觸力作為柔順控制的力反饋，實(shí)現(xiàn)機(jī)器人的柔順控制。

背景技術(shù)：

機(jī)器人柔順控制在切削、磨光、裝配作業(yè)中應(yīng)用廣泛，也應(yīng)用于行走機(jī)器人的行走過(guò)程中。順性控制分為主動(dòng)柔順控制和被動(dòng)柔順控制兩類。機(jī)器人憑借一些輔助柔順機(jī)構(gòu)，使其在與環(huán)境接觸時(shí)能夠?qū)ν獠孔饔昧Ξa(chǎn)生自然順應(yīng)，稱為被動(dòng)柔順控制；被動(dòng)柔順機(jī)構(gòu)即利用一些可以使機(jī)器人在與環(huán)境作用時(shí)，能夠吸收或存儲(chǔ)能量的機(jī)構(gòu)器件如彈簧、阻尼等而構(gòu)成的機(jī)構(gòu)。機(jī)器人采用被動(dòng)柔順控制在作業(yè)中存在一些問題：（1）無(wú)法根除機(jī)器人高剛度與高柔性間的矛盾。（2）被動(dòng)柔順裝置的專用性強(qiáng)，適應(yīng)能力差，適用范圍受到限制。（3）無(wú)法使機(jī)器人本身產(chǎn)生對(duì)力的反應(yīng)動(dòng)作，成功率較低等。機(jī)器人利用力的反饋信息采用一定的控制策略去主動(dòng)控制作用力，稱為主動(dòng)柔順控制。主動(dòng)柔順控制也稱為力控制。機(jī)器人主動(dòng)柔順控制實(shí)現(xiàn)克服了被動(dòng)柔順控制的不足。因此，機(jī)器人的主動(dòng)柔順控制研究成果具有十分廣闊的應(yīng)用前景。

目前實(shí)現(xiàn)主動(dòng)柔順控制的方法主要有兩類。一為阻抗控制，另一類是力和位置的混合控制。阻抗控制不是直接控制期望的力和位置，而是通過(guò)控制力和位置之間的動(dòng)態(tài)關(guān)系來(lái)實(shí)現(xiàn)柔順功能。這樣的動(dòng)態(tài)關(guān)系類似于電路中阻抗的概念，因此稱為阻抗控制。阻抗控制又可分為兩類，一類是位置型阻抗控制，原理是對(duì)電機(jī)的控制采用位置控制，將力的差值通過(guò)阻抗模型轉(zhuǎn)化為位置誤差，修正目標(biāo)位置。該方法由于內(nèi)環(huán)是位置控制，所以具有較強(qiáng)的魯棒性，且由于位置控制比較成熟，所以該方法應(yīng)用廣泛。另一類是基于力的阻抗控制，該方法的直接控制電機(jī)的驅(qū)動(dòng)力矩，抗擾動(dòng)能力較弱。力/位混合控制是指根據(jù)作業(yè)需要在任務(wù)空間中對(duì)不同方向上分別進(jìn)行力和位置控制，通過(guò)雅克比矩陣映射到關(guān)節(jié)空間，相加后合并為統(tǒng)一的關(guān)節(jié)力矩，從而實(shí)現(xiàn)任務(wù)空間中有約束方向上進(jìn)行力控制，無(wú)約束方向上實(shí)現(xiàn)力控制。其他主動(dòng)柔順控制方法多為這兩種方法的變形和改進(jìn)。

無(wú)論是阻抗控制還是力/位混合控制，都需要機(jī)器人末端與外界環(huán)境接觸的力的信息作為反饋。目前多采用在機(jī)器人手腕處或者腳踝處安裝多維力傳感器的方法實(shí)現(xiàn)力的測(cè)量，再反饋給控制。但是多維力傳感器往往價(jià)格昂貴，而且極易損壞，對(duì)于部分機(jī)器人還存在無(wú)法安裝力傳感器的情況。本發(fā)明將采用一種基于動(dòng)力學(xué)模型的方法估計(jì)機(jī)器人與外界的接觸力，然后采用阻抗控制實(shí)現(xiàn)機(jī)器人的柔順控制。因此該方法無(wú)需力傳感器，具有重要的應(yīng)用價(jià)值。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明提供的一種基于接觸力觀測(cè)器的機(jī)器人柔順控制方法，該方法主要由位置控制器、阻抗控制器和基于動(dòng)力學(xué)模型的環(huán)境接觸力觀測(cè)器組成，其特征在于采用接觸力傳感器代替多維力傳感器得到接觸力的反饋，采用位置型阻抗控制器實(shí)現(xiàn)柔順控制，該方法包括下述步驟：

步驟1：預(yù)先建立機(jī)器人運(yùn)動(dòng)學(xué)模型、動(dòng)力學(xué)模型和主動(dòng)關(guān)節(jié)摩擦力模型；

步驟2：根據(jù)任務(wù)需要規(guī)劃?rùn)C(jī)器人末端運(yùn)動(dòng)軌跡，通過(guò)逆運(yùn)動(dòng)學(xué)計(jì)算得到關(guān)節(jié)運(yùn)動(dòng)角度；

步驟3：通過(guò)位置控制器（如PID控制器等）控制機(jī)器人的運(yùn)動(dòng)；

步驟4：機(jī)器人運(yùn)動(dòng)過(guò)程中實(shí)時(shí)采集電機(jī)電流值和電機(jī)轉(zhuǎn)動(dòng)角速度；

步驟5：將角速度信息輸入狀態(tài)觀測(cè)器，得到估計(jì)的角位移、角速度和角加速度信息；

步驟6：將電流信息和步驟3中得到的角位移、角速度和角加速度信息輸入到擾動(dòng)觀測(cè)器中，得到關(guān)節(jié)輸出力矩；

步驟7：將步驟3中得到的角位移、角速度和角加速度信息輸入到機(jī)器人動(dòng)力學(xué)模型中，得到由動(dòng)力學(xué)模型計(jì)算而來(lái)的關(guān)節(jié)驅(qū)動(dòng)力矩；

步驟8：用步驟4中得到的關(guān)節(jié)輸出力矩減掉步驟5中得到的由動(dòng)力學(xué)模型計(jì)算得到的關(guān)節(jié)驅(qū)動(dòng)力矩，得到末端接觸力占關(guān)節(jié)驅(qū)動(dòng)力的部分；

步驟9：根據(jù)步驟8中得到的接觸力的大小判斷機(jī)器人有沒有跟外界環(huán)境發(fā)生接觸；如果有接觸，則進(jìn)行阻抗控制，如果沒有發(fā)生接觸，則阻抗控制不起作用；

步驟10：經(jīng)過(guò)步驟9中的判斷，在機(jī)器人跟外界發(fā)生接觸情況下，將步驟8中得到的接觸力占關(guān)節(jié)驅(qū)動(dòng)力的部分乘以力雅克比矩陣即可估計(jì)出接觸力在三維空間中X/Y/Z三個(gè)方向上的大?。?/p>

步驟11：分別對(duì)X/Y/Z三個(gè)方向上的接觸力與期望作用力作差，將差值輸入阻抗控制器，輸出該方向上的位移修正量；

步驟12：將步驟11中的阻抗控制器輸出結(jié)果與期望位移作差作為新的軌跡，實(shí)現(xiàn)柔順控制。

如上所述，本發(fā)明提出一種基于接觸力觀測(cè)器的機(jī)器人柔順控制方法，具有以下有益效果：

僅依據(jù)關(guān)節(jié)驅(qū)動(dòng)電機(jī)的電流和電機(jī)編碼器測(cè)量得到的角速度信息，運(yùn)用接觸力觀測(cè)器即可估計(jì)出環(huán)境接觸力，作為力反饋信息完成阻抗控制，從而代替多維力傳感器，節(jié)約設(shè)備開發(fā)成本，降低因力傳感器損壞造成的損失。

附圖說(shuō)明

圖1是基于接觸力觀測(cè)器的機(jī)器人柔順控制方法控制框圖。

圖2是3自由度并聯(lián)機(jī)器人機(jī)構(gòu)簡(jiǎn)圖。

圖3是3自由度串聯(lián)機(jī)器人機(jī)構(gòu)簡(jiǎn)圖。

圖4是3自由度并聯(lián)機(jī)器人柔順控制實(shí)驗(yàn)示意圖。

圖5是柔順控制力控制實(shí)驗(yàn)結(jié)果。

圖6是柔順控制位移實(shí)驗(yàn)結(jié)果。

具體實(shí)施方式

以下結(jié)合實(shí)施例及附圖對(duì)本發(fā)明作進(jìn)一步詳細(xì)的說(shuō)明，但本發(fā)明實(shí)施方式不限于此。該具體實(shí)施例選取三自由度并聯(lián)機(jī)器人（如圖2所示）作為作用對(duì)象，但實(shí)施對(duì)象不限于此，多自由度串聯(lián)機(jī)器人（如圖3所示）同樣適用。

以下將詳細(xì)敘述本發(fā)明的一種基于接觸力觀測(cè)器的機(jī)器人柔順控制方法的原理及實(shí)施方式，使本領(lǐng)域技術(shù)性人員不需要?jiǎng)?chuàng)造性勞動(dòng)即可理解本發(fā)明一種基于接觸力觀測(cè)器的機(jī)器人柔順控制方法。

如圖1所示，本發(fā)明提供一種基于接觸力觀測(cè)器的機(jī)器人柔順控制方法具體包括以下步驟：

步驟1：圖2中機(jī)器人具有一條UP支鏈，兩條UPS支鏈，U代表萬(wàn)向節(jié)，P代表移動(dòng)副，S代表球副。其中P為主動(dòng)關(guān)節(jié)，由伺服電機(jī)帶動(dòng)滾珠絲杠副實(shí)現(xiàn)驅(qū)動(dòng)。建立機(jī)器人運(yùn)動(dòng)學(xué)模型、動(dòng)力學(xué)模型和關(guān)節(jié)摩擦模型，并對(duì)未知參數(shù)進(jìn)行參數(shù)辨識(shí)，得到精確的機(jī)器人動(dòng)力學(xué)模型。

步驟2：根據(jù)任務(wù)需求，規(guī)劃?rùn)C(jī)器人的末端運(yùn)動(dòng)軌跡 ，通過(guò)逆運(yùn)動(dòng)學(xué)即可求得關(guān)節(jié)轉(zhuǎn)角。

步驟3：對(duì)每一個(gè)關(guān)節(jié)都采用單獨(dú)的位置控制器控制關(guān)節(jié)運(yùn)動(dòng)，此處以PID控制器為例加以說(shuō)明，PID控制器的控制率為

(1)

式中，為比例系數(shù)，為積分時(shí)間常數(shù)，為微分時(shí)間常數(shù)。對(duì)于關(guān)節(jié)i而言，采樣周期k對(duì)應(yīng)的位置誤差由期望關(guān)節(jié)轉(zhuǎn)角和編碼器測(cè)得轉(zhuǎn)角作差得到，

(2)

PID控制器的參數(shù)、、可以采用Z—N法整定。

步驟4：機(jī)器人運(yùn)動(dòng)過(guò)程中，伺服電機(jī)可以實(shí)時(shí)將電機(jī)電流信息和編碼器得到的角位移信息上傳至控制器，該角位移信息伴有噪聲。

步驟5：動(dòng)力學(xué)模型中需要用到關(guān)節(jié)的角度、角速度和角加速度，而電機(jī)編碼器只能得到角位移，因此需要狀態(tài)觀測(cè)器估計(jì)角速度和角加速度，而且編碼器直接輸出的角位移是帶有噪聲的信號(hào)，狀態(tài)觀測(cè)器也可以濾除噪聲。此處以卡爾曼濾波器作為狀態(tài)觀測(cè)器進(jìn)行說(shuō)明?？柭鼮V波器是一種最優(yōu)狀態(tài)觀測(cè)器，由預(yù)測(cè)和修正兩個(gè)步驟組成。

設(shè)狀態(tài)方程和觀測(cè)方程為

(3)

(4)

式中為第k個(gè)采樣周期的狀態(tài)向量，為第k-1個(gè)采樣周期的狀態(tài)向量，和分別為狀態(tài)矩陣和觀測(cè)矩陣，和分別為過(guò)程噪聲和測(cè)量噪聲，它們之間是彼此獨(dú)立的，而且不同k之間也是獨(dú)立的。

設(shè)關(guān)節(jié)位置信號(hào)是由噪聲驅(qū)動(dòng)的ARMA模型，表示為如下形式

(5)

式中為頻域符號(hào)?？梢宰C明在采樣頻率非常小的情況下，不會(huì)影響卡爾曼濾波的特性，這樣可以得到一個(gè)噪聲驅(qū)動(dòng)的全積分模型

(6)

令(6)式中，，，這樣可將(3)和(4)式化為狀態(tài)空間形式，

(7)

(8)

有了(7)式和(8)式，就可以應(yīng)用卡爾曼濾波對(duì)信號(hào)進(jìn)行估計(jì)。卡爾曼濾波的具體過(guò)程為四步。

第一步，基于系統(tǒng)的上一狀態(tài)而預(yù)測(cè)出現(xiàn)在狀態(tài)：

(9)

第二步，更新協(xié)方差：

(10)

式中是對(duì)應(yīng)的協(xié)方差，是系統(tǒng)過(guò)程的協(xié)方差。

有了現(xiàn)在狀態(tài)的預(yù)測(cè)結(jié)果，然后我們?cè)偈占F(xiàn)在狀態(tài)的測(cè)量值。結(jié)合預(yù)測(cè)值和測(cè)量值，我們可以得到現(xiàn)在狀態(tài)x(k)的最優(yōu)化估算值x(k|k)。第三步就是求狀態(tài)最優(yōu)估計(jì)值：

(11)

其中為卡爾曼增益，可由下式求得

(12)

式中R為測(cè)量噪聲協(xié)方差。至此，已經(jīng)得到了狀態(tài)向量的最優(yōu)估計(jì)值，即，也就是角位移、角速度和角加速度的最優(yōu)估計(jì)值。為了使迭代能夠繼續(xù)進(jìn)行需要更新對(duì)應(yīng)的協(xié)方差，

(13)

式中I為單位矩陣。當(dāng)進(jìn)入下一個(gè)采樣周期后，又可以依次按照這四步求解下一個(gè)周期的最優(yōu)估計(jì)狀態(tài)。

對(duì)于本發(fā)明中涉及的機(jī)器人，需要分別估計(jì)一條腿中三個(gè)關(guān)節(jié)的角位移、角速度和角加速度狀態(tài)。設(shè)，則通過(guò)以上卡爾曼濾波器，即可得到狀態(tài)的最優(yōu)估計(jì)值、和。

步驟6：采用擾動(dòng)觀測(cè)器計(jì)算電機(jī)輸出的有效力矩。

由于電機(jī)不是直接驅(qū)動(dòng)連桿的，往往是通過(guò)減速器或者其他運(yùn)動(dòng)副與連桿相連，而這些中間環(huán)節(jié)往往具有較大的慣量和摩擦，需要通過(guò)擾動(dòng)觀測(cè)器求解關(guān)節(jié)力矩。

對(duì)于關(guān)節(jié)i具有以下平衡方程：

(14)

式中為電機(jī)力矩系數(shù)，為電機(jī)電流，為折算到電機(jī)軸上的關(guān)節(jié)轉(zhuǎn)動(dòng)慣量，是外界所有擾動(dòng)之和。定義如下，

(15)

式中即為需要求取的關(guān)節(jié)扭矩，由步驟1中建立的摩擦模型求得。值得注意的是此處將其他擾動(dòng)全部合并到摩擦力中，因此摩擦模型一般不能用簡(jiǎn)單的庫(kù)侖摩擦加粘性摩擦來(lái)描述，需要根據(jù)實(shí)際情況合理建立新的摩擦模型。聯(lián)立(14)和(15)式即可得到關(guān)節(jié)力矩為

(16)

將步驟5中得到的代入(16)式，即可得到關(guān)節(jié)有效輸出力矩。

步驟7：依據(jù)機(jī)器人腿部機(jī)構(gòu)動(dòng)力學(xué)模型，計(jì)算驅(qū)動(dòng)腿部運(yùn)動(dòng)所需要的關(guān)節(jié)驅(qū)動(dòng)力。步驟1中建立的腿部機(jī)構(gòu)的動(dòng)力學(xué)模型為

(17)

其中M為3×3的慣性矩陣，為科氏力和向心力項(xiàng)，G為重力項(xiàng)，為除驅(qū)動(dòng)關(guān)節(jié)摩擦外的其他關(guān)節(jié)摩擦。將狀態(tài)觀測(cè)器得到的狀態(tài)估計(jì)值，和代入(15)式中，即可得到。

將步驟5中得到的、和代入(17)式，即可得到關(guān)節(jié)動(dòng)力學(xué)驅(qū)動(dòng)力矩。

步驟8：步驟6和步驟7中得到的力矩作差即可得到外力作用占關(guān)節(jié)驅(qū)動(dòng)力的部分。

關(guān)節(jié)驅(qū)動(dòng)力包含兩部分，一部分是無(wú)外力作用時(shí)驅(qū)動(dòng)機(jī)構(gòu)運(yùn)動(dòng)所需驅(qū)動(dòng)力，另一部分是外力作用對(duì)關(guān)節(jié)驅(qū)動(dòng)力的影響，即有下式

(18)

則

(19)

式(19)得到的關(guān)節(jié)空間中的力矩，因此需要步驟9進(jìn)一步將該力矩轉(zhuǎn)化為外力。

步驟9：根據(jù)步驟8中得到的結(jié)果，判斷機(jī)器人有沒有跟外界環(huán)境發(fā)生接觸。由于模型估計(jì)存在一定誤差，所以設(shè)定一個(gè)閾值范圍判斷，如果滿足

(20)

則說(shuō)明機(jī)器人跟外界環(huán)境發(fā)生了接觸。如果發(fā)生接觸則使阻抗控制起作用，如果沒有發(fā)生接觸，阻抗控制不起作用。

步驟10：將步驟8中得到的外力引起的關(guān)節(jié)驅(qū)動(dòng)力矩映射為足底接觸力。

關(guān)節(jié)空間和任務(wù)空間的映射采用雅克比矩陣完成，即有

(21)

上式中為機(jī)器人腳在笛卡爾坐標(biāo)系中三個(gè)方向的運(yùn)動(dòng)速度，為腿部機(jī)構(gòu)雅克比矩陣。則由虛功原理可得到

(22)

將步驟8的結(jié)果代入(22)式即可得

(23)

步驟11：如果步驟9的判斷結(jié)果為真，則規(guī)定三個(gè)方向上的期望接觸力，再分別對(duì)X/Y/Z三個(gè)方向上的接觸力與期望作用力作差，將差值輸入阻抗控制器，輸出該方向上的位移修正量，如下式所示

(24)

步驟12：采用阻抗控制，將力的差值轉(zhuǎn)化為位置量，轉(zhuǎn)換關(guān)系為

(25)

其中， ，。寫成傳遞函數(shù)形式為

(26)

式(26)求得的位移量將作為參考軌跡的修正量，從而改變參考軌跡，使特定方向上的接觸力控制到期望的力的大小，實(shí)現(xiàn)柔順控制。

圖4為采用本發(fā)明所提供的柔順控制方法進(jìn)行的驗(yàn)證性試驗(yàn)。將本發(fā)明所提控制方法應(yīng)用到圖2所示的3自由度并聯(lián)機(jī)器人上。機(jī)器人末端跟蹤一空間軌跡，只有y方向和z方向有運(yùn)動(dòng)。增加一障礙物，障礙物會(huì)阻礙機(jī)器人的運(yùn)動(dòng)。令z方向上的期望接觸力為300N。圖5和圖6顯示了力的控制效果和位移變化情況。圖5顯示當(dāng)機(jī)器人接觸到障礙物后z方向上的力可以快速控制到300N；圖6顯示，在遇到障礙物之前，機(jī)器人末端軌跡很好的跟蹤了期望軌跡，遇到障礙物后為實(shí)現(xiàn)力的控制改變了期望軌跡。因此實(shí)驗(yàn)結(jié)果表明本發(fā)明所提一種基于接觸力觀測(cè)器的機(jī)器人柔順控制方法是可行有效的。