李籽興，陳 鋼，盧 瑤

(北京郵電大學(xué)自動(dòng)化學(xué)院，北京100876)

1 引言

月球是人類深空探測(cè)的首選目標(biāo)，月球探測(cè)的多個(gè)領(lǐng)域都需要月面機(jī)器人[1]。其中，為勘測(cè)月面礦物和巖石分布情況，需要基于巡視器采樣進(jìn)行月面探測(cè)。采樣過程中，機(jī)械臂末端與樣本會(huì)發(fā)生接觸碰撞，接觸力過大會(huì)影響采樣任務(wù)的執(zhí)行甚至損壞機(jī)械臂。因此需要設(shè)計(jì)采樣策略以保證機(jī)械臂采樣任務(wù)的安全性。

本文借鑒陳萌等[2-3]土壤切削力學(xué)建模方法以及機(jī)械臂控制方法，在建立的機(jī)械臂動(dòng)力學(xué)模型和末端執(zhí)行器-月壤接觸力學(xué)模型的基礎(chǔ)上，根據(jù)末端接觸力與入土角和采樣速度之間的函數(shù)關(guān)系，選定使采樣時(shí)末端接觸力最小的采樣方案，并結(jié)合力/位混合控制方法設(shè)計(jì)了末端接觸力的控制方法，以保證采樣過程中機(jī)械臂的有效控制以及安全性。

2 動(dòng)力學(xué)模型

月面巡視器車載機(jī)械臂6自由度模型連桿坐標(biāo)系如圖1所示。

圖1 6自由度機(jī)械臂DH坐標(biāo)系Fig.1 DH coordinate system of a 6-DOF manipulator

根據(jù)機(jī)器人學(xué)[4]各連桿質(zhì)心所受慣性力IFk及慣性力矩INk如式(1)：

其中，k表示第k個(gè)連桿，R表示旋轉(zhuǎn)矩陣，m表示質(zhì)量，ω表示關(guān)節(jié)角速度，I表示慣性矩陣。各連桿所受外力Ifk和外力矩Ink如式(2)：

fe及ne為末端操作力及操作力矩，各關(guān)節(jié)力矩 τ =[τ1，τ2，…，τn]T∈ R Rn×1，τk=InTk·I z k。

3 采樣策略

3.1 采樣器與月壤作用力學(xué)模型

本文采用如圖2所示采樣挖掘器。挖取機(jī)構(gòu)板寬為w、頂板長(zhǎng)度為a、底板寬度為b、圓弧板長(zhǎng)度為R、側(cè)邊厚度和底邊厚度為d1。 月面土壤特性如表 1～2所示[5]。

圖2 挖掘器結(jié)構(gòu)Fig.2 Excavator structure

表1 月壤物理力學(xué)性質(zhì)Table 1 Mechanical properties of lunar soil

表2 月壤剪切特性Table 2 Shear characteristics of lunar soil

采樣接觸力主要與采樣過程狀態(tài)相關(guān)。機(jī)械臂采樣可分為插入、切削、旋轉(zhuǎn)抬起3部分。插入過程實(shí)際可轉(zhuǎn)換為切削深度隨鏟斗運(yùn)動(dòng)變化的切削過程，旋轉(zhuǎn)抬起過程與切削過程的主要區(qū)別在于切削為平移，速度方向水平，而旋轉(zhuǎn)抬起為定軸轉(zhuǎn)動(dòng)。可將總的采樣過程簡(jiǎn)化為以切削深度、切削速度等為變量的切削過程。采樣方案主要是調(diào)整末端采樣姿態(tài)、操作速度、采樣方向?qū)δ┒私佑|力的影響，使接觸力最小，因此首先要研究采樣接觸力與采樣過程中各變量的關(guān)系。采樣器受力如圖3所示。

圖3 末端采樣器采樣過程受力示意圖Fig.3 Diagram of force on the sampler during sampling

可認(rèn)為末端執(zhí)行器在整個(gè)采樣過程均為勻速運(yùn)動(dòng)(平移和轉(zhuǎn)動(dòng))，末端加速度和角加速度為0，因此忽略慣性力的作用。月壤樣本對(duì)采樣器的接觸力主要由式(3)～(13)所示6個(gè)部分組成：

1)月壤剪切阻力：

其中，Kh為挖掘阻力系數(shù)，ρ為土壤密度，ω為鏟斗切削角速度，h為入土深度。

2)月壤粘著阻力：

其中，c為月壤粘著力。

3)已挖掘月壤對(duì)采樣器產(chǎn)生的阻力：

其中，F(xiàn)D1如式(6)所示，為鏟斗內(nèi)月壤重力產(chǎn)生的阻力：

式中，AN為鏟斗內(nèi)側(cè)壁面積，g為月面重力加速度，KC為松散系數(shù)，φ為月壤內(nèi)摩擦系數(shù)。FD2如式(7)所示，為鏟斗外月壤與鏟斗尖部產(chǎn)生的摩擦阻力：

式中，ξ為月壤與鏟斗間的摩擦系數(shù)，R為切削半徑。FD3如式(8)所示，為剪斷土體的月壤重力產(chǎn)生的阻力：

式中， Wd1=Ad1b=(r21-r20)bφ/4，Wd2=r21b cosβsinβ，Wd3=0.00873r2γh。 其中，α 為鏟斗與采樣面傾角(入土角)，β為剪斷直線段與采樣面夾角。FD4如式(9)所示，為被剪斷土體與未挖掘土體的摩擦阻力：

4)月壤對(duì)鏟斗內(nèi)側(cè)面的摩擦阻力：

式中，KS為側(cè)壓系數(shù)。

5)月壤對(duì)鏟斗外側(cè)面的摩擦阻力：

式中，AW為鏟斗外側(cè)壁面積。

6)與速度相關(guān)的挖掘阻力：

在挖掘過程中，單位時(shí)間內(nèi)的待挖掘土壤對(duì)執(zhí)行器會(huì)產(chǎn)生與其挖掘速率相關(guān)的挖掘阻力。待挖掘土壤微元體進(jìn)行運(yùn)動(dòng)學(xué)分析如圖4所示。

圖4 月壤微元體運(yùn)動(dòng)示意圖Fig.4 Schematic diagram of soil micro-element

在單位時(shí)間d t內(nèi)鏟斗內(nèi)會(huì)產(chǎn)生新的月壤微元體，其質(zhì)量為 d m=ρwVed t d z。

由動(dòng)量定理可得d t時(shí)間段內(nèi)深度在z處的鏟斗微元受到的挖掘阻力為式(12)：

積分得到鏟斗所受總接觸力為式(13)：

3.2 采樣方案設(shè)計(jì)

月壤可根據(jù)其密實(shí)度大致分為松散、中密、緊密3種。本文分別對(duì)3種不同狀態(tài)月壤下的采樣方案進(jìn)行設(shè)計(jì)。不同松散度的月壤性質(zhì)如表3所示[5]。

表3 不同松散度月壤力學(xué)特性Table 3 Mechanical properties of different compactnesslunar soil

根據(jù)接觸阻力力學(xué)模型式(13)，代入不同緊密程度土壤下入土角數(shù)據(jù)，得到不同月壤性質(zhì)下接觸阻力隨采樣入土角的變化趨勢(shì)如圖5所示。

由圖5可知，接觸阻力隨著入土角的增大，呈現(xiàn)先增后減的趨勢(shì)，故存在最優(yōu)的入土角，使得接觸阻力最小化。松散土壤與中密土壤阻力曲線最優(yōu)入土角分布在[50°，70°]之間，緊密土壤下末端采樣最優(yōu)入土角分布在[40°，60°]之間。

圖5 接觸阻力與入土角關(guān)系圖Fig.5 Relationship between contact force and angle

為更明顯地體現(xiàn)入土角與接觸阻力及月壤性質(zhì)的關(guān)系，繪制在不同月壤下、不同入土角下，接觸阻力隨挖掘深度變化如圖6所示?？梢婋S著采樣深度的增加，接觸阻力隨之增大，在同樣的采樣深度時(shí)，在最優(yōu)角度范圍外的入土角產(chǎn)生的接觸阻力大于范圍內(nèi)入土角產(chǎn)生的接觸阻力。

圖6 接觸阻力隨采樣深度變化圖Fig.6 Relationship between contact force and sampling depth

除了入土角之外，末端接觸阻力還與末端執(zhí)行器速率相關(guān)。由式(13)可知，在其他變量維持常量的情況下，接觸阻力隨挖掘速率的增大成恒定遞增的關(guān)系，但隨著挖掘速率的增大，挖掘阻力增大，挖掘效率提高，隨著挖掘速率的減小，挖掘阻力雖然減小，但挖掘效率隨之降低。任務(wù)需求在保證挖掘效率的前提下，盡可能減小接觸阻力。需要綜合兩點(diǎn)互相沖突的任務(wù)目標(biāo)來選取挖掘速率。

可認(rèn)為挖掘過程末端速度恒定，則接觸阻力即為挖掘阻力，挖掘阻力與挖掘速率呈式(15)所示正相關(guān)關(guān)系：

由挖掘時(shí)間衡量挖掘效率，其與挖掘速率呈式(16)所示負(fù)相關(guān)關(guān)系：

選取權(quán)重比A1、A2，結(jié)合兩個(gè)任務(wù)目標(biāo)，確定總目標(biāo)函數(shù)如式(17)，且確保兩分目標(biāo)不會(huì)超出各自極限。

其中 v∈[5，30](mm/s)。 取A1=0.9、A2=0.1，得到總目標(biāo)值隨挖掘速率變化如圖7所示。

圖7 優(yōu)化目標(biāo)值與挖掘速率關(guān)系圖Fig.7 Relationship between optimized goal and speed

由圖可知，在采樣速率分布在[5，10]mm/s之間時(shí)，目標(biāo)函數(shù)值存在最優(yōu)值。初步設(shè)計(jì)采樣方案如表4所示。

對(duì)不同密實(shí)度下的月壤，利用Matlab軟件分析入土角和挖掘速率影響，結(jié)果如圖8～10所示。

圖8 松散狀態(tài)月壤初步方案結(jié)果Fig.8 Results of preliminary scheme for loose lunar soil

圖9 中密狀態(tài)月壤初步方案結(jié)果Fig.9 Results of preliminary scheme for medium loose lunar soil

從而設(shè)計(jì)3種狀態(tài)月壤下的最佳采樣方案如表5。

圖10 中密狀態(tài)月壤初步方案結(jié)果Fig.10 Results of preliminary scheme for compact lunar soil

表5 最佳采樣方案Table 5 Optimal sampling scheme

4 接觸力控制策略

4.1 機(jī)械臂力/位混合控制方法

在機(jī)械臂操作空間中分別引入位置控制律和力控制律如式(18)：

將控制律由操作空間向關(guān)節(jié)空間轉(zhuǎn)化可得關(guān)節(jié)空間位置控制和力控制律如式(19)：

根據(jù)環(huán)境的約束情況建立約束坐標(biāo)系{C}，針對(duì)末端任務(wù)將任務(wù)空間分為位控子空間和力控子空間，分別在位控子空間和力控子空間實(shí)現(xiàn)對(duì)機(jī)械臂的控制，在位控子空間和力控子空間分別設(shè)定控制律如式(20)：

其中，S-、S為操作臂力/位混合控制對(duì)角選擇矩陣，有S-=I-S。

圖11 機(jī)械臂力位混合控制系統(tǒng)框圖Fig.11 Block diagram of manipulator compliant control system

4.2 仿真驗(yàn)證

接觸力控制主要發(fā)生在機(jī)械臂末端與樣本接觸過程中，由機(jī)械臂采樣任務(wù)規(guī)劃模型可知，機(jī)械臂在采樣過程中，末端以恒定姿態(tài)沿特定入土角做直線規(guī)劃，其接觸力可簡(jiǎn)化為與末端運(yùn)動(dòng)方向相反的力。設(shè)采樣深度depth=0.05 m，機(jī)械臂系統(tǒng)控制周期為step=0.05 s，任務(wù)總時(shí)間total=10 s。

設(shè)機(jī)械臂初始關(guān)節(jié)角序列為qin=[-1.326，3.247，1.622，2.052， -1.025， -0.310]rad， 此時(shí)，機(jī)械臂末端在直線規(guī)劃的起始處。設(shè)位置控制增益和力控制增益分別為Kp=diag[0.1，0.1，0， 0， 0.1， 0.1]， Kd=diag[15， 15， 0， 15， 15，15]，Kpf=diag[0， 0， -0.9， 0， 0， 0]， Kd=diag[0，0，0.02，0，0 0]。 采用上一節(jié)研究得到的機(jī)械臂力位混合控制算法，控制結(jié)果如圖12所示。

由上述結(jié)果可知，機(jī)械臂力位混合控制算法造成的沿約束曲面法線方向的接觸力誤差很小，在2×10-2N以內(nèi)，在實(shí)際操作過程中，該誤差可忽略不計(jì)。因此，采用本節(jié)研究得到的力位混合控制算法，可保證機(jī)械臂在執(zhí)行采樣任務(wù)時(shí)，其末端接觸力始終維持在期望范圍內(nèi)，不會(huì)因?yàn)槟┒私佑|力的急劇增大導(dǎo)致采樣任務(wù)失敗或機(jī)械臂系統(tǒng)發(fā)生損壞。

圖12 接觸力控制仿真結(jié)果Fig.12 Simulation results of contact force control

5 結(jié)論

本文設(shè)計(jì)的采樣方案能夠快速對(duì)末端采樣器接觸力進(jìn)行控制，使末端-月壤接觸力最小。