一種基于阻抗控制的機(jī)械臂雙軸孔插孔柔順控制方法研究

李歆雨， 張 強(qiáng)， 文 聞， 周曉東， 李曉輝

北京控制工程研究所,北京 100094

0 引 言

在軌加注是指通過某種技術(shù)手段將推進(jìn)劑從服務(wù)航天器轉(zhuǎn)移到目標(biāo)航天器從而使目標(biāo)航天器恢復(fù)正常的推進(jìn)系統(tǒng)功能的服務(wù)技術(shù)，是多國(guó)大力研究發(fā)展的具有低成本延長(zhǎng)航天器使用壽命能力的重要在軌服務(wù)技術(shù).美國(guó)“軌道快車”于2007年實(shí)現(xiàn)先期技術(shù)驗(yàn)證裝置升空，進(jìn)行了自主交會(huì)對(duì)接、服務(wù)星對(duì)目標(biāo)星的捕獲對(duì)接、燃料輸送以及模塊更換等技術(shù)驗(yàn)證，在在軌加注演示實(shí)驗(yàn)中服務(wù)衛(wèi)星向客戶星進(jìn)行了多次燃料往返傳輸，對(duì)其泵傳輸和壓力傳輸兩種方式均進(jìn)行了全面測(cè)試[1-2].美國(guó)2011年啟動(dòng)的RRM在軌試驗(yàn)，對(duì)不具備專門在軌燃料加注接口的低地球軌道非合作衛(wèi)星，采用直接傳輸?shù)姆绞竭M(jìn)行在軌燃料加注[3].我國(guó)天舟一號(hào)貨運(yùn)飛船與天宮二號(hào)空間實(shí)驗(yàn)室于2017年首次成功完成推進(jìn)劑在軌加注實(shí)驗(yàn)[4].通過在軌加注技術(shù)的實(shí)現(xiàn)，可以提高航天器的機(jī)動(dòng)能力，靈活機(jī)動(dòng)增強(qiáng)了目標(biāo)航天器的靈活性，使其能夠完成更多任務(wù)；可以增強(qiáng)航天器的航行距離，變相提高航天器推進(jìn)劑攜帶能力，有助于深空探測(cè)；有助于挽救失效衛(wèi)星，通過對(duì)失效航天器補(bǔ)充燃料可以實(shí)現(xiàn)姿態(tài)控制、軌道機(jī)動(dòng)等，延長(zhǎng)航天器的壽命.同時(shí)加注推進(jìn)器也是對(duì)空間站等大型空間平臺(tái)日常維護(hù)不可或缺的項(xiàng)目.

在軌加注過程中，通常需要補(bǔ)充的航天器燃料由燃燒劑和氧化劑兩種，因此燃料傳輸機(jī)構(gòu)具有兩個(gè)接口，如圖1所示，可同時(shí)接受燃燒劑和氧化劑注入，傳統(tǒng)剛性機(jī)械臂位置控制很難同時(shí)滿足兩個(gè)軸孔插入的空間要求，為解決這一插孔問題需要針對(duì)性設(shè)計(jì)適用于雙軸孔插孔的控制方法.除在軌加注過程之外，雙軸孔插孔的控制還適用于一些為保證對(duì)接的相對(duì)位置姿態(tài)采用多個(gè)定位銷引導(dǎo)對(duì)接的對(duì)接元件.

圖1 對(duì)接與流體傳輸機(jī)構(gòu)Fig.1 Docking and fluid transfer mechanism

插孔過程中在機(jī)械臂由自由空間到與環(huán)境空間接觸的過程中由于尺寸、定位以及運(yùn)動(dòng)控制的誤差以及機(jī)械臂慣性和相應(yīng)頻率等原因可能會(huì)造成機(jī)械臂末端與對(duì)象的剛性碰撞，從而導(dǎo)致空間機(jī)器人和操作對(duì)象的運(yùn)動(dòng)姿態(tài)變化或碰撞破壞.為避免這樣的誤差和破壞對(duì)任務(wù)造成的負(fù)面影響，需要引入對(duì)機(jī)械臂的柔順控制.

柔順控制主要包括阻抗控制、力跟隨控制、力位混合控制等的控制算法實(shí)現(xiàn).力位混合控制將空間氛圍位控切向和力控法向兩個(gè)正交空間，分別進(jìn)行位置控制和力控制，通過在一些自由度上采用力控制，其余自由度采用位置控制，以實(shí)現(xiàn)綜合控制[5-6].這種控制方法原理簡(jiǎn)單，相對(duì)容易實(shí)現(xiàn)，但缺乏機(jī)械臂與環(huán)境之間動(dòng)態(tài)耦合關(guān)系的描述，難以實(shí)現(xiàn)力位控制的配合，在復(fù)雜現(xiàn)實(shí)環(huán)境的應(yīng)用容易造成較大的控制誤差.阻抗控制將對(duì)象位置偏差與接觸作用力之間的關(guān)系稱為目標(biāo)阻抗，通過動(dòng)態(tài)調(diào)節(jié)阻抗來實(shí)現(xiàn)柔順控制[7-9].在實(shí)際應(yīng)用情況下，阻抗控制對(duì)于環(huán)境的位置、剛度和阻尼等參數(shù)提出了很高的要求，即使是很小測(cè)量誤差都將對(duì)力的輸出造成很大影響.對(duì)于雙軸孔插孔的控制問題，由于插孔過程中接觸力的大小和方向?qū)崟r(shí)受環(huán)境和機(jī)械臂多重因素的影響，難以分解到有限的自由度控制，因此選擇側(cè)重于描述機(jī)械臂與環(huán)境柔性交互的阻抗控制方法實(shí)現(xiàn)柔順控制.

本文針對(duì)雙軸孔插孔控制問題，首先分析雙軸孔插孔的插孔過程和接觸狀態(tài)，隨后設(shè)計(jì)一種基于阻抗控制在接觸力反饋的作用下實(shí)現(xiàn)柔順插孔過程的阻抗控制器，最后通過仿真實(shí)驗(yàn)的方法驗(yàn)證該柔順控制方法的實(shí)用意義.本文提出的柔順控制方法滿足在軌加注、在軌裝配等需要同時(shí)實(shí)現(xiàn)雙軸孔配合的空間操作要求，適應(yīng)于空間在軌服務(wù)機(jī)械臂遙操作以及智能化操作技術(shù)的發(fā)展需求.

1 雙軸孔插孔的過程和狀態(tài)分析

在在軌加注過程中，加注主動(dòng)端固定在機(jī)械臂末端，加注口位于目標(biāo)航天器表面，若期望一對(duì)加注口同時(shí)插入加注口中，則控制機(jī)械臂帶動(dòng)油槍靠近目標(biāo)加注口，在二者相接觸后依據(jù)接觸力的反饋信息調(diào)整機(jī)械臂末端姿態(tài)，在阻抗控制下機(jī)械臂末端具有近似于阻尼彈簧的特性，在貼近的過程中逐漸實(shí)現(xiàn)軸孔間的配合.

圖2 雙軸孔插孔過程Fig.2 Double Peg-in-Hole Assembly

通常情況下，雙軸孔的插孔過程包括自由接近、一點(diǎn)接觸、多點(diǎn)接觸、配合插入4個(gè)主要過程[10]，如圖2所示，對(duì)插孔結(jié)果起決定性作用是二者接觸過程的相對(duì)位置姿態(tài).二者接觸過程中，通過感應(yīng)器反饋的作用力和扭矩的值分析二者接觸的相對(duì)位置姿態(tài)，設(shè)計(jì)進(jìn)一步的運(yùn)動(dòng)趨勢(shì)，使所有可能的位置狀態(tài)下軸孔都在較小接觸力的作用下朝盡可能減少誤差的狀態(tài)運(yùn)動(dòng).

與單軸孔的插孔過程相比，實(shí)現(xiàn)雙軸孔插孔面臨更復(fù)雜的控制要求[11]：首先對(duì)于自由接近的初始位姿具有一定要求，只有當(dāng)軸與孔相接觸時(shí)的傾斜角θ、插入過程Z軸的扭轉(zhuǎn)角φ以及軸與孔的相對(duì)位置關(guān)系都限定在一定范圍內(nèi)時(shí)才能實(shí)現(xiàn)雙軸同時(shí)插入，這個(gè)范圍由軸孔的尺寸和裝配精度決定，這樣的要求對(duì)自由接近過程機(jī)械臂的規(guī)劃曲線提出了要求；其次插孔過程中兩軸孔的位置關(guān)系相互約束，限定了插孔過程中可調(diào)整的位姿幅度，對(duì)于單個(gè)軸孔插孔起效的位姿調(diào)整策略在雙軸孔插孔控制中可能受到另一軸孔的約束而無法實(shí)現(xiàn)，要求阻抗控制下機(jī)械臂具有較小的剛性特性，同時(shí)設(shè)計(jì)參考軌跡時(shí)需要考慮進(jìn)行多頻次小幅度的往復(fù)位姿調(diào)整.

圖3 雙軸孔接觸過程的受力Fig.3 The force on the contact process of the double axle hole

2 機(jī)械臂的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型

本文以UR16e機(jī)械臂為操作對(duì)象，在不考慮機(jī)械臂和關(guān)節(jié)的被動(dòng)柔順特性的前提下，將機(jī)械臂連桿視為剛性細(xì)桿，連桿之間以單自由度旋轉(zhuǎn)關(guān)節(jié)連接，機(jī)械臂特性如圖4所示[12-13].

圖4 UR16e機(jī)械臂的結(jié)構(gòu)及參數(shù)Fig.4 Structure and parameters of UR16e robotic arm

根據(jù)UR16e工業(yè)機(jī)器人模型和尺寸，以各關(guān)節(jié)轉(zhuǎn)動(dòng)量為0的初始狀態(tài)建立坐標(biāo)系組，建立如表1所示的機(jī)械臂連桿D-H參數(shù)表.其中扭轉(zhuǎn)角(αi)為第i個(gè)關(guān)節(jié)的坐標(biāo)系Zi軸變換至與第i+1個(gè)關(guān)節(jié)的坐標(biāo)系Zi+1軸方向重合過程中沿第i個(gè)關(guān)節(jié)的軸Xi旋轉(zhuǎn)的角度，連桿長(zhǎng)度(βi)為第i個(gè)關(guān)節(jié)的坐標(biāo)系Zi軸與第i+1個(gè)關(guān)節(jié)的坐標(biāo)系Zi+1軸沿第i個(gè)關(guān)節(jié)的軸Xi方向偏移的距離，關(guān)節(jié)偏距(di)為第i個(gè)關(guān)節(jié)的軸Xi與第i+1個(gè)關(guān)節(jié)的軸Xi+1沿第i+1個(gè)關(guān)節(jié)的坐標(biāo)系Zi+1軸方向偏移的距離，關(guān)節(jié)轉(zhuǎn)角(θi)為第i個(gè)關(guān)節(jié)的軸Xi變換至與第i+1個(gè)關(guān)節(jié)的軸Xi+1方向重合過程中沿第i+1個(gè)關(guān)節(jié)的坐標(biāo)系Zi+1軸旋轉(zhuǎn)的角度.

表1 UR16e機(jī)械臂D-H參數(shù)表Tab.1 UR16e robot arm D-H parameter table

使用拉格朗日動(dòng)力學(xué)方法可以描述機(jī)械臂的動(dòng)力學(xué)動(dòng)態(tài)變化特性[14-15]：

根據(jù)機(jī)械臂拉格朗日動(dòng)力方程可根據(jù)給定的控制力矩可積分求解關(guān)節(jié)角度，在由運(yùn)動(dòng)學(xué)位姿坐標(biāo)變換矩陣描述機(jī)械臂末端運(yùn)動(dòng)軌跡[16]；反之在通過軌跡規(guī)劃設(shè)計(jì)機(jī)械臂運(yùn)動(dòng)的時(shí)間函數(shù)后可通過運(yùn)動(dòng)學(xué)反解求解關(guān)節(jié)變量，進(jìn)而帶入拉格朗日動(dòng)力方程構(gòu)成力矩控制系統(tǒng).

3 機(jī)械臂雙軸孔插孔的阻抗控制器設(shè)計(jì)

阻抗控制算法是解決機(jī)械臂與環(huán)境柔性交互的控制方法，當(dāng)機(jī)械臂與外界環(huán)境產(chǎn)生接觸力時(shí)機(jī)械臂會(huì)產(chǎn)生響應(yīng)，將機(jī)械臂系統(tǒng)看作一個(gè)等效阻抗模型，系統(tǒng)的輸入項(xiàng)與輸出項(xiàng)測(cè)量值的比值即為系統(tǒng)的阻抗[17-18].將末端執(zhí)行器位置變化與力變換之間的關(guān)系視為設(shè)計(jì)阻抗值，即可得到位置變化時(shí)外界環(huán)境作用于機(jī)械臂的力與機(jī)械臂末端位置關(guān)系的函數(shù)：

式中,M、B、K為期望阻抗參數(shù)，其中K為剛度系數(shù)矩陣，B為阻尼系數(shù)矩陣，M為慣性系數(shù)矩陣；Fext為機(jī)械臂受到外界環(huán)境的作用力，F(xiàn)d為機(jī)械臂期望受到外界環(huán)境的作用力；xd為機(jī)械臂期望位置，x為機(jī)械臂實(shí)際位置.

阻抗控制的過程如圖5所示，通過傳感器檢測(cè)機(jī)械臂受到的外力Fext得到與期望的偏差值，代入阻抗控制率將期望力偏差值轉(zhuǎn)化為位置偏差值ex，進(jìn)而得到機(jī)械臂的期望位置.將期望位置作為控制器的控制目標(biāo)，控制機(jī)械臂運(yùn)動(dòng)的結(jié)果經(jīng)傳感器檢測(cè)，實(shí)際的關(guān)節(jié)轉(zhuǎn)角和實(shí)際接觸力反饋分別用來修正目標(biāo)期望和修正力矩,實(shí)現(xiàn)機(jī)械臂位置跟蹤.

圖5 阻抗控制框圖Fig.5 Impedance control block diagram

在該控制系統(tǒng)中，設(shè)計(jì)阻抗控制器的期望阻抗參數(shù)M、D、K，使機(jī)械臂末端執(zhí)行器具有“質(zhì)量—彈簧—阻尼”的等價(jià)動(dòng)力學(xué)性能，當(dāng)機(jī)械臂末端執(zhí)行器接觸到環(huán)境障礙物時(shí)，機(jī)械臂受到環(huán)境力的作用不會(huì)發(fā)生剛性碰撞，減少其可能造成的部件損害和航天器姿態(tài)擾動(dòng)，通過調(diào)整期望阻抗參數(shù)M、D、K可以調(diào)整機(jī)械臂阻抗控制的相應(yīng)速度、穩(wěn)定性和柔順特性[19-20].

4 仿真結(jié)果分析

為驗(yàn)證這一阻抗控制算法解決雙軸孔插孔問題的有效性，采用圖1所示的六自由度串聯(lián)型機(jī)械臂UR16e參數(shù)進(jìn)行插孔的模擬仿真.

通過在Simulink和Vrep中建立聯(lián)合仿真環(huán)境，在Simulink中建立阻抗控制器模型，將控制指令通過通信傳輸至Vrep中建立的UR16e機(jī)械臂模型中，模擬機(jī)械臂的運(yùn)動(dòng)軌跡及與環(huán)境力學(xué)交互特性，將模擬的輸出通過虛擬力傳感器采集傳輸反饋至Simulink的控制器中.

仿真過程中，設(shè)計(jì)機(jī)械臂末端工具在碰撞前保持不動(dòng)，使用阻抗控制算法和傳統(tǒng)剛性機(jī)械臂位置控制多次重復(fù)插孔過程，記錄比較兩種控制算法下插孔過程中的接觸力和關(guān)節(jié)角變化趨勢(shì)，實(shí)現(xiàn)本文提出的阻抗控制算法控制性能的驗(yàn)證.

采用兩種控制算法的雙軸孔插孔仿真結(jié)果如圖6～8所示.兩種控制算法控制下，機(jī)械臂于24 s后實(shí)現(xiàn)機(jī)械臂末端與目標(biāo)問題的接觸.在插孔過程中，采用阻抗控制算法控制下各軸方向接觸力均在正負(fù)30 N的區(qū)間范圍內(nèi)波動(dòng)；傳統(tǒng)剛性機(jī)械臂位置控制算法控制下各軸方向力最大值均超過200 N，Z軸方向最大接觸力可達(dá)500 N以上.采用阻抗控制方案能夠在一定程度上降低碰撞交互瞬間的沖擊力，降低機(jī)械臂和操作對(duì)象的發(fā)生碰撞破壞或運(yùn)動(dòng)姿態(tài)變化的不利影響.

圖6 兩種控制方法下X軸向接觸力變化曲線Fig.6 Change curves of X-axis contact force under two control methods

圖7 兩種控制方法下Y軸向接觸力變化曲線Fig.7 Change curves of Y-axis contact force under two control methods

圖8 兩種控制方法下Z軸向接觸力變化曲線Fig.8 Change curves of Z-axis contact force under two control methods

對(duì)比兩種控制算法下各軸方向接觸力大小關(guān)系可以得出，阻抗控制算法將機(jī)械臂末端X軸向接觸力減小至剛性機(jī)械臂位置控制算法的10%左右，Y軸向接觸力減小至剛性機(jī)械臂位置控制算法的25%左右，Z軸向接觸力減小至剛性機(jī)械臂位置控制算法的30%左右.總體而言阻抗控制算法顯著降低了機(jī)械臂末端與環(huán)境的接觸力，實(shí)現(xiàn)二者之間的柔順交互.

采用兩種控制算法的雙軸孔插孔仿真過程中關(guān)節(jié)角位置變化過程部分曲線如圖9和圖10所示.采用阻抗控制算法控制下關(guān)節(jié)角位置變化更為平滑，幾乎沒有出現(xiàn)傳統(tǒng)剛性機(jī)械臂位置控制算法控制下關(guān)節(jié)角位置突然變化的現(xiàn)象，可以有效減免機(jī)械臂運(yùn)行過程中的沖擊震蕩.

圖9 兩種控制方法下關(guān)節(jié)角位置變化曲線Fig.9 Change curves of joint angle position under two control methods

圖10 關(guān)節(jié)角位置變化曲線放大對(duì)比圖Fig.10 Enlarged comparison of joint angle position curve

通過對(duì)阻抗控制算法和傳統(tǒng)剛性機(jī)械臂位置控制算法的對(duì)比驗(yàn)證，這種基于阻抗柔順控制的機(jī)械臂雙軸孔插孔的柔順控制方法對(duì)UR16e機(jī)械臂末端執(zhí)行器的動(dòng)力學(xué)性能起了良好的優(yōu)化效果，對(duì)插孔過程的控制接觸力產(chǎn)生了至少30%的削弱效果，穩(wěn)定消除了控制過程的位置突變信號(hào)，使機(jī)械臂具有更平穩(wěn)的控制過程和更柔順的環(huán)境交互性能，最終可以實(shí)現(xiàn)良好的雙軸孔插孔控制效果.

5 結(jié) 論

本文提出了一種基于阻抗柔順控制的機(jī)械臂雙軸孔插孔的柔順控制方法，通過設(shè)計(jì)阻抗控制器的期望阻抗參數(shù)M、D、K，使機(jī)械臂末端執(zhí)行器具有“質(zhì)量—彈簧—阻尼”的等價(jià)動(dòng)力學(xué)性能，減少其可能發(fā)生剛性碰撞造成的部件損害和航天器姿態(tài)擾動(dòng).在Simulink和Vrep中聯(lián)合仿真環(huán)境中進(jìn)行了控制器的模擬驗(yàn)證，最終實(shí)現(xiàn)了良好的柔順雙軸孔插孔效果.和傳統(tǒng)控制原理的控制方案相比，本文提出的方法能有效實(shí)現(xiàn)雙軸孔的柔順插孔過程，具有更平穩(wěn)的控制過程以及更微小的控制接觸力，適應(yīng)于在軌加注、在軌裝配等需要同時(shí)實(shí)現(xiàn)雙軸孔配合的空間操作需求.