聶佳偉，辛鵬飛，榮吉利，王 瑞，潘成龍，程修妍

(1. 北京理工大學(xué)宇航學(xué)院，北京 100081；2. 北京空間飛行器總體設(shè)計(jì)部，北京 100094；3. 齊魯工業(yè)大學(xué)(山東省科學(xué)院)數(shù)學(xué)與統(tǒng)計(jì)學(xué)院，濟(jì)南 250353)

0 引 言

月球資源具有巨大的戰(zhàn)略價值，是人類社會長期可持續(xù)發(fā)展的重要支撐。通過深層采樣分析月壤組成和結(jié)構(gòu)，不僅能探查月球資源的分布情況，對將來月球基地選址也發(fā)揮著重要作用。

對于月球采樣，不少學(xué)者已經(jīng)做了很多研究。史曉萌等對月壤與采樣機(jī)具間相互作用中的鉆進(jìn)負(fù)載進(jìn)行了研究；梁磊等研究了小型采樣器在月表低氣壓重力環(huán)境下淺層月壤的鏟挖阻力預(yù)測及參數(shù)優(yōu)選問題；凌云等提出一種由三個剛性臂和一個柔性臂組合、結(jié)構(gòu)新穎的月壤取樣器，并通過動力學(xué)響應(yīng)確定了取樣器的工作極限；盧偉等設(shè)計(jì)了一種重量輕、功效低、收縮體積小的卷簧式可伸縮月壤取樣器，并通過理論分析、仿真及實(shí)驗(yàn)驗(yàn)證了取樣器設(shè)計(jì)的合理性及振動法取樣的有效性。然而，上述這些研究都著重對采樣機(jī)鉆取采樣過程進(jìn)行了研究，未對采樣過程中機(jī)械臂的運(yùn)動控制進(jìn)行研究。在月球深層采樣過程中，主要涉及到機(jī)械臂與復(fù)雜接觸面的接觸碰撞問題，在接觸操作時，如何設(shè)計(jì)控制策略來使機(jī)械臂柔順、安全地完成任務(wù)是該過程中最主要的問題。

針對類似復(fù)雜接觸面機(jī)械臂的控制問題，不少學(xué)者已經(jīng)進(jìn)行過研究。王彬巒從協(xié)作型機(jī)械臂的碰撞檢測響應(yīng)和柔順控制兩個功能切入，提出了一套不依賴外部傳感器的協(xié)作型機(jī)械臂碰撞檢測、碰撞響應(yīng)和柔順控制策略。錢震杰根據(jù)赫茲接觸理論建立了考慮摩擦的接觸碰撞動力學(xué)模型，并對實(shí)際的空間機(jī)械臂模型做了仿真研究。危清清等提出了柔性機(jī)械臂輔助大負(fù)載空間艙對接的阻抗控制方法，并利用Adams-Matlab進(jìn)行了聯(lián)合仿真。莫洋等對空間機(jī)械臂輔助大質(zhì)量艙體對接任務(wù)，提出了采用阻抗控制的方法，并通過Adams與Matlab/Simulink對接觸過程進(jìn)行聯(lián)合仿真，證明了阻抗控制算法能夠有效減小接觸力。在這些文獻(xiàn)中，不足之處在于模型比較簡單，或者運(yùn)動比較簡單，文獻(xiàn)[6]采用的是一個二自由度的機(jī)械臂系統(tǒng)，文獻(xiàn)[8]和文獻(xiàn)[9]中機(jī)械臂的運(yùn)動相對比較簡單，這些簡單的模型和運(yùn)動無法滿足采樣機(jī)械臂的任務(wù)需求，而文獻(xiàn)[7]則沒有涉及到機(jī)械臂的軌跡規(guī)劃和控制問題。賈慶軒等對太空七自由度機(jī)械臂的碰撞問題進(jìn)行了研究，對機(jī)械臂碰前軌跡進(jìn)行了研究，并建立了碰撞動力學(xué)模型對碰撞過程進(jìn)行仿真，但其研究中同樣未涉及到對機(jī)械臂的控制。

由于空間機(jī)械臂輔助深層采樣過程中，機(jī)械臂不可避免地存在與地面及孔洞的接觸碰撞，這對機(jī)械臂控制精度以及采樣任務(wù)完成帶來了較大的挑戰(zhàn)。為了減小深層采樣過程中的系統(tǒng)振動，本文基于四自由度機(jī)械臂，建立了非光滑系統(tǒng)的精確動力學(xué)模型，提出了含碰撞問題的阻抗控制方法，實(shí)現(xiàn)了深層采樣任務(wù)中機(jī)械臂的柔順運(yùn)動。首先，基于局部標(biāo)架的剛體李群SO(3)方法對機(jī)械臂進(jìn)行建模，通過梯形規(guī)劃對機(jī)械臂的軌跡進(jìn)行了規(guī)劃，采用阻抗控制方法對機(jī)械臂進(jìn)行控制，推導(dǎo)了李群SO(3)模型下機(jī)械臂關(guān)節(jié)空間與末端笛卡爾空間之間的雅可比矩陣，并且得到了兩個空間的相互轉(zhuǎn)換關(guān)系。其次，采用了錐互補(bǔ)方法計(jì)算接觸力，相較于罰函數(shù)方法，該方法實(shí)行了非嵌入的原則，可以采用相對較大的時間步長進(jìn)行計(jì)算，并且相較于罰參數(shù)將引起系統(tǒng)高頻振動的缺點(diǎn)，錐互補(bǔ)方法描述的非光滑動力學(xué)能較為精確地處理剛性碰撞問題。最后，通過對控制參數(shù)進(jìn)行調(diào)整，探究了不同控制參數(shù)對機(jī)械臂控制的影響，優(yōu)化得到了合適的控制參數(shù)，從而能控制機(jī)械臂輔助完成深層采樣的任務(wù)。

1 空間機(jī)械臂建模

1.1 機(jī)械臂介紹

空間機(jī)械臂是由多個關(guān)節(jié)和臂桿等機(jī)構(gòu)部件組成，在空間零重力、微重力作用環(huán)境中進(jìn)行工作的多體系統(tǒng)。本文針對一類四自由度機(jī)械臂進(jìn)行了建模，并且忽略了機(jī)械臂的柔性，采用剛體單元對機(jī)械臂進(jìn)行建模。

1.2 機(jī)械臂參數(shù)介紹

機(jī)械臂結(jié)構(gòu)示意圖如圖1所示，～為機(jī)械臂結(jié)構(gòu)長度，其長度見表1。

圖1 機(jī)械臂結(jié)構(gòu)示意圖Fig.1 Schematic diagram of manipulator structure

在該模型中，機(jī)械臂線密度為=2000 kg/m，第一個機(jī)械臂的主轉(zhuǎn)動慣量的三個分量為16.67 kg·m、8.35 kg·m、8.35 kg·m；第二個機(jī)

表1 機(jī)械臂結(jié)構(gòu)長度數(shù)據(jù)Table 1 Structure length data of the manipulator

械臂的三個分量為183.33 kg·m、119.4 kg·m、119.4 kg·m；的三個分量為166.67 kg·m、104.17 kg·m、104.17 kg·m；的三個分量為33.33 kg·m、16.83 kg·m、16.83 kg·m。

1.3 李群SO(3)非光滑廣義-a錐互補(bǔ)方法

本文采用局部標(biāo)架的剛體李群SO(3)方法對機(jī)械臂進(jìn)行建模，并采用非光滑廣義-a方法進(jìn)行求解，其動力學(xué)方程為：

(1)

(2)

=

(3)

(4)

式中：+1表示第+1個時間步的變量，為質(zhì)量矩陣，為轉(zhuǎn)動慣量矩陣，loc表示連體局部坐標(biāo)系下的變量，glo為全局坐標(biāo)系下的變量。需要注意以下幾點(diǎn)：

(5)

該映射將任意三維向量映射為三階反對稱矩陣；

2) 式(1)中，雙邊約束被放到了光滑的運(yùn)動學(xué)方程中，這點(diǎn)對結(jié)果是不會有影響的，傳統(tǒng)的廣義-a方法在求解帶雙邊約束的DAE方程時就具有與求解ODE方程相同的性質(zhì)；

(6)

其中，為接觸點(diǎn)個數(shù)，=[,,,],=1,2,…,，其中,可以通過間隙函數(shù)對廣義坐標(biāo)求導(dǎo)得到，而其他兩項(xiàng)則可以在接觸平面內(nèi)任意選?。?/p>

4) 式(3)為雙邊約束方程，在該機(jī)械臂系統(tǒng)中，為旋轉(zhuǎn)鉸約束，其通過將兩個臂上的接觸點(diǎn)進(jìn)行固定，并令兩個臂上平行于轉(zhuǎn)軸的單位向量，在全局坐標(biāo)系重合實(shí)現(xiàn)：

(7)

(8)

(9)

在該方法中，速度的變化被分為兩部分，一部分是由加速度引起的光滑分量，另一部分是由碰撞等非光滑力引起的非光滑分量。其離散格式如下：

(10)

(11)

(12)

(13)

(14)

+1=+Δ+1

(15)

(16)

式(1)～(4)的求解方法可以參考文獻(xiàn)[14]，不過該論文中需要求解一個線性互補(bǔ)問題(LCP)，而此處需要求解一個錐互補(bǔ)問題(CCP)，因此，需要將其中的LCP求解器，替換為CCP對應(yīng)的求解器。至于求解CCP的方法，則采用文獻(xiàn)[15]中的加速投影梯度下降(APGD)算法進(jìn)行求解。

2 位置型阻抗控制

2.1 機(jī)械臂關(guān)節(jié)空間與末端笛卡爾空間

在機(jī)械臂系統(tǒng)中，存在描述關(guān)節(jié)運(yùn)動的關(guān)節(jié)坐標(biāo)系和描述末端位置和姿態(tài)的末端笛卡爾坐標(biāo)系等，由機(jī)械臂關(guān)節(jié)坐標(biāo)系的坐標(biāo)到機(jī)械臂末端位置和姿態(tài)之間的映射，稱為機(jī)械臂的正向運(yùn)動學(xué)；由機(jī)械臂末端位置和姿態(tài)到機(jī)械臂關(guān)節(jié)坐標(biāo)系的坐標(biāo)之間的映射，稱為逆向運(yùn)動學(xué)。

圖2為機(jī)械臂關(guān)節(jié)坐標(biāo)示意圖，～為關(guān)節(jié)空間的四個關(guān)節(jié)角，從軸正方向看，順時針方向?yàn)榈恼较?；從軸正方向看，逆時針方向?yàn)椤恼较?。①～④為四個機(jī)械臂編號。通過關(guān)節(jié)坐標(biāo)系，可以表示出第四個臂質(zhì)心的位置及姿態(tài)，也就是機(jī)械臂末端笛卡爾空間的位置和姿態(tài)：

(17)

其中，為第四個臂質(zhì)心的坐標(biāo)，～均為機(jī)械臂長度參數(shù)。

圖2 機(jī)械臂四個關(guān)節(jié)角示意圖Fig.2 Schematic diagram of the four joint angles of the manipulator

由于：

(18)

(19)

于是可以表示機(jī)械臂末端的姿態(tài)：

(20)

式(17)和式(20)可以統(tǒng)一表示為：

(21)

式(21)即為關(guān)節(jié)空間向末端笛卡爾空間轉(zhuǎn)換公式。

將式(17)前兩個分量進(jìn)行化簡可以得到以下兩個關(guān)系：

sin+cos=cos+cos(+)

(22)

cos-sin=++

(23)

由式(23)可以得到：

(24)

由式(22)和式(17)第三個分量可以得到如下關(guān)系式：

(25)

cos-(sin+cossin)=

sin

(26)

由式(23)和式(25)可以求得：

(27)

由式(26)可以求得：

(28)

由式(20)可以求得：

(29)

其最終角度需根據(jù)機(jī)械臂的實(shí)際運(yùn)動情況進(jìn)行選取。

2.2 機(jī)械臂笛卡爾空間和關(guān)節(jié)空間的雅可比矩陣

機(jī)械臂笛卡爾空間運(yùn)動速度與關(guān)節(jié)空間運(yùn)動速度之間的變換，稱為雅可比矩陣。雅可比矩陣是關(guān)節(jié)空間速度向笛卡爾空間速度的傳動比：

(30)

其中，=[]。

由式(17)和式(20)可以計(jì)算得到機(jī)械臂的雅可比矩陣：

=1,2,3;=1,2,…,4

(31)

(32)

(33)

(34)

(35)

(36)

(37)

2.3 笛卡爾空間與關(guān)節(jié)空間的靜力變換

機(jī)械臂笛卡爾空間與關(guān)節(jié)空間的靜力變換在文獻(xiàn)[12]中有詳細(xì)介紹，此處簡單介紹一下。機(jī)械臂在關(guān)節(jié)空間中的虛功，可以表示為：

(38)

其中，δ是機(jī)械臂在關(guān)節(jié)空間所做的虛功，=[]是機(jī)械臂關(guān)節(jié)空間的等效靜力矩，δ=[δδδδ]是關(guān)節(jié)空間的虛擬位移。

同樣的，機(jī)械臂在笛卡爾空間中的虛功為：

δ=δ

(39)

其中，δ為笛卡爾空間廣義力的虛擬位移。

機(jī)械臂笛卡爾空間與關(guān)節(jié)空間的虛擬位移之間存在如下關(guān)系：

δ=()δ

(40)

其中，為機(jī)械臂的雅可比矩陣。

考慮到機(jī)械臂在笛卡爾空間與關(guān)節(jié)空間的虛功是等價的，由式(38)～(40)可得：

=()

(41)

2.4 位置型阻抗控制

(42)

圖3 位置型阻抗控制流程圖Fig.3 Flow chart of position-based impedance control

由于在李群的建模方法中，動力學(xué)方程中的外力即為廣義控制力，因此在仿真的過程中，不需要計(jì)算關(guān)節(jié)空間的力或力矩。在實(shí)際的機(jī)械臂系統(tǒng)中，需要根據(jù)下式計(jì)算得到關(guān)節(jié)空間的驅(qū)動力：

=

(43)

然后就能夠驅(qū)動機(jī)械臂進(jìn)行運(yùn)動。

3 空間機(jī)械臂輔助深層采樣控制與仿真

3.1 機(jī)械臂與固定面接觸仿真

對于圖1所示的機(jī)械臂，采用速度梯形規(guī)劃，令四個關(guān)節(jié)角度在6 s內(nèi)從初始位置=[00 -π0]運(yùn)動至=[-π/2 -π/6 -π/6 -π/6]，初末時刻速度和加速度均為0，其軌跡規(guī)劃結(jié)果如圖4所示。

圖4 機(jī)械臂與固定面接觸問題軌跡規(guī)劃Fig.4 Trajectory planning results of the manipulator contact with fixed surface

圖5 機(jī)械臂初始構(gòu)型圖Fig.5 Initial configuration of the manipulator

在機(jī)械臂的仿真控制中，在=-0.2 m處增加了一個接觸面，限制機(jī)械臂方向的運(yùn)動，機(jī)械臂初始構(gòu)型圖及接觸面模型如圖5所示。機(jī)械臂與接觸面的摩擦系數(shù)=0.2，分別使用位置型阻抗控制和位置控制對機(jī)械臂進(jìn)行控制，其中位置型阻抗控制的控制參數(shù)為：

(44)

=4

(45)

=10000

(46)

其中，的前三個分量單位為kg/s，后三個分量單位為kg·m/s；的前三個分量單位為kg/s，后三個分量單位為kg·m/s；的前三個分量單位為kg/s，后三個分量單位為kg·m/s，為了方便，后文中這三個變量單位與此處保持一致，不再強(qiáng)調(diào)；為6階單位矩陣。位置控制的控制參數(shù)為：

(47)

其中，為4階單位矩陣。

機(jī)械臂末端位置和姿態(tài)隨時間變化曲線如圖6所示。相較位置控制，位置型阻抗控制更能夠精確柔順地控制機(jī)械臂沿著預(yù)期軌跡運(yùn)動。

圖6 機(jī)械臂與固定面接觸情況末端位置和姿態(tài)隨時間變化曲線Fig.6 Curves of the end position and attitude of the manipulator contact with fix surface

3.2 控制參數(shù)對機(jī)械臂柔順控制的影響

在空間機(jī)械臂輔助深層采樣的過程中，需要柔順地控制機(jī)械臂沿著預(yù)期軌跡運(yùn)動。為此，通過調(diào)節(jié)圖3中PID控制方法的相關(guān)控制參數(shù)，比較不同控制參數(shù)對機(jī)械臂柔順控制的影響，為機(jī)械臂控制參數(shù)的選取提供依據(jù)。

對于3.1小節(jié)中的算例，首先比較了不同對控制結(jié)果的影響，控制其他參數(shù)不變，分別計(jì)算了=/10和=10的情況，并與的情況進(jìn)行了對比，機(jī)械臂末端位置與姿態(tài)對比結(jié)果如圖7所示，可以發(fā)現(xiàn)，當(dāng)取值較小時，機(jī)械臂運(yùn)動的曲線會難以跟蹤預(yù)期軌跡，而當(dāng)取值較大時，機(jī)械臂在發(fā)生碰撞時會出現(xiàn)較大的碰撞力，并且會有明顯的振動，由此可見，在實(shí)際控制過程中，應(yīng)當(dāng)取合適的值，從而使得機(jī)械臂能夠柔順地跟蹤預(yù)期軌跡進(jìn)行運(yùn)動。

圖7 不同kpp情況機(jī)械臂末端位置和姿態(tài)對比Fig.7 Comparison of the end position and attitude of the manipulator under different kpp conditions

同樣的，可以確定中剩余參數(shù)的大致取值范圍，見表2。

然后，比較了不同對控制結(jié)果的影響，控制其他參數(shù)不變，分別計(jì)算了=/10和=

表2 kpp各參數(shù)取值范圍Table 2 Value range of each parameter in kpp

10的情況，并與的情況進(jìn)行了對比，發(fā)現(xiàn)不同對控制結(jié)果的影響較小。

最后，比較了不同對控制結(jié)果的影響，令其他參數(shù)不變，分別計(jì)算了=/10和=10的情況，在計(jì)算過程中，出現(xiàn)了Newton迭代不收斂的現(xiàn)象，并且反復(fù)進(jìn)行嘗試，最終令=15000，若將的取值較大，則會出現(xiàn)迭代不收斂的情況，將=/10和=15000的情況進(jìn)行了對比，機(jī)械臂末端位置與姿態(tài)對比結(jié)果如圖8所

圖8 不同kvd情況機(jī)械臂末端位置和姿態(tài)對比Fig.8 Comparison of the end position and attitude of the manipulator under different kvd conditions

示，可以發(fā)現(xiàn)，當(dāng)取值較小時，機(jī)械臂在發(fā)生碰撞時會出現(xiàn)較大的碰撞力，并且會有明顯的振動；而取值較大時，則會出現(xiàn)迭代不收斂的情況，因此，選取合適的對于機(jī)械臂的柔順控制也發(fā)揮著重要作用。

進(jìn)一步地，控制和不變，確定了中各元素的大致取值范圍均為7500～16000。在實(shí)際應(yīng)用中，，和應(yīng)該結(jié)合具體問題具體分析，確保能夠控制機(jī)械臂順利完成任務(wù)。

3.3 機(jī)械臂深層采樣仿真

為了實(shí)現(xiàn)對機(jī)械臂深層采樣的情況進(jìn)行仿真，對于圖1所示機(jī)械臂，設(shè)計(jì)了如圖9所示深層孔，孔圓心橫縱坐標(biāo)為(0.075 m，0.1 m)，孔分為上下兩層，上層半徑=0.07 m，深度=0.1 m，下層=0.03 m，深度=0.1 m，接觸平面位于=-0.2 m處，機(jī)械臂與接觸面的摩擦系數(shù)=0.2。針對該深層孔，分別對兩種工況進(jìn)行仿真，一種采樣深度淺，一種采樣深度深。其中，采樣深度淺的工況中，設(shè)定機(jī)械臂的采樣深度比復(fù)雜孔深度淺；而采樣深度深的工況則比復(fù)雜孔深度深，這樣機(jī)械臂就由于孔深度的限制而無法到達(dá)預(yù)期位置，該工況是為了探究機(jī)械臂在與孔底部發(fā)生接觸后是否繼續(xù)向前運(yùn)動。

圖9 復(fù)雜深層孔模型Fig.9 Complex deep hole model

在接觸檢測過程中，首先需要計(jì)算檢測點(diǎn)距離圓心的水平距離以及檢測點(diǎn)前一時刻距離圓心的水平距離 0，然后根據(jù)判斷和 0與半徑的大小以及檢測點(diǎn)與接觸面的相對高度，就可以判斷該接觸點(diǎn)可能會與哪個面進(jìn)行接觸，具體接觸檢測流程如圖10所示。

首先是采樣深度淺的工況，機(jī)械臂初始四個關(guān)節(jié)角為=[0 0 -π 0]，令其末端從初始位置運(yùn)動至(0.075 m，0.1 m，-0.3 m)處，并令末端終止時刻姿態(tài)保持豎直。

機(jī)械臂的軌跡規(guī)劃結(jié)果如圖11所示，機(jī)械臂控制參數(shù)中，的六個參數(shù)依次為：2.55×10, 2.55×10, 4.25×10, 2.55×10, 2.55×10, 2.55×10；=4;=15000。

圖10 圓柱孔接觸面檢測流程圖Fig.10 Cylindrical hole contact surface detection flow chart

圖11 機(jī)械臂深層采樣情況軌跡規(guī)劃Fig.11 Trajectory planning results of the manipulator for deep sampling

機(jī)械臂末端位置和姿態(tài)隨時間變化曲線如圖12所示。從這些圖中可以看出，機(jī)械臂末端能夠沿著目標(biāo)軌跡運(yùn)動至預(yù)期位置，實(shí)現(xiàn)深層采樣的任務(wù)。

然后是采樣深度深的工況，控制復(fù)雜孔的位置和形狀不變，機(jī)械臂控制參數(shù)及與接觸面摩擦系數(shù)不變，改變機(jī)械臂末端的終了位置，令其從初始位置運(yùn)動至(0.075 m，0.1 m，-0.4 m)處，機(jī)械臂末端終止時刻仍保持豎直。

采用同樣的方法對機(jī)械臂進(jìn)行軌跡規(guī)劃，并保持機(jī)械臂控制參數(shù)不變。

機(jī)械臂末端位置和姿態(tài)隨時間變化曲線如圖13所示。根據(jù)末端位置以及構(gòu)型圖可以看出，在=5 s時，機(jī)械臂末端已經(jīng)與復(fù)雜孔底部發(fā)生接觸，

圖12 采樣深度淺情況下機(jī)械臂末端位置和姿態(tài)隨時間變化曲線Fig.12 Changes of the end position and attitude of the manipulator versus time in the case of shallow sampling

圖13 采樣深度深情況下機(jī)械臂末端位置和姿態(tài)隨時間變化曲線Fig.13 Changes of the end position and attitude of the manipulator versus time in the case of deep sampling

無法繼續(xù)向下運(yùn)動，這與預(yù)期結(jié)果相符合，由此表明該阻抗控制能夠?qū)崿F(xiàn)對機(jī)械臂的仿真和控制。

4 結(jié) 論

本文采用剛體李群SO(3)方法建立了機(jī)械臂動力學(xué)模型，分別采用位置控制與位置型阻抗控制方法研究了含碰撞的機(jī)械臂軌跡跟蹤問題。研究表明，位置型阻抗控制能夠精準(zhǔn)對施加在機(jī)械臂末端的力進(jìn)行調(diào)整，使機(jī)械臂能夠柔順地沿著平面繼續(xù)運(yùn)動。通過調(diào)節(jié)PID控制器不同參數(shù)，發(fā)現(xiàn)和對控制結(jié)果的影響較大：取較小值時機(jī)械臂會無法精準(zhǔn)沿著預(yù)期軌跡運(yùn)動，取較大值時機(jī)械臂會產(chǎn)生較大的振動；取較小值時機(jī)械臂同樣會產(chǎn)生較大的振動，取較大值時則會導(dǎo)致仿真的迭代不收斂。進(jìn)一步探究，通過控制變量的方法確定了和的大致取值范圍?；谏鲜鼋Ｅc控制方法，針對采樣深度淺與采樣深度深兩種工況，通過選取合適的控制參數(shù)，使得機(jī)械臂能夠柔順地輔助采樣機(jī)完成采樣任務(wù)。