空間多體系統(tǒng)軌道姿態(tài)及機械臂一體化控制

魏春嶺，袁泉，張軍，王夢菲

（1.北京控制工程研究所，北京100190； 2.空間智能控制技術(shù)國家級重點實驗室，北京100190）

空間機器人是未來在軌服務(wù)任務(wù)中的實施主體，隨著空間開發(fā)程度的提高，各國均發(fā)射了空間機器人進行空間操作任務(wù)演示，如日本的ETSVII、美國的“軌道快車”、XSS-10、XSS-11等［1-4］。

空間機器人是典型的空間多體系統(tǒng)，一般采用衛(wèi)星本體基座與用于目標(biāo)操作的機械臂組成，這種典型的空間多體系統(tǒng)自由度多、動力學(xué)復(fù)雜，給其控制帶來了挑戰(zhàn)。

目前，已經(jīng)有一些文獻進行了空間多體系統(tǒng)的控制研究。文獻［5-6］針對目標(biāo)捕獲時如何減小機械臂對基座的干擾進行了機械臂控制器的設(shè)計；文獻［7］在不進行軌道控制但同時對基座和機械臂姿態(tài)進行控制的情況下，設(shè)計了關(guān)節(jié)空間內(nèi)的自適應(yīng)控制器；文獻［8-9］在既不進行基座軌道控制，也不進行基座姿態(tài)控制時，針對外干擾的不確定性，通過將工作空間內(nèi)的狀態(tài)轉(zhuǎn)換到關(guān)節(jié)空間內(nèi)，設(shè)計了工作空間內(nèi)的自適應(yīng)控制器；文獻［10］在基座軌道停控但姿態(tài)受控時，針對機械臂在工作空間的操作控制，進行了基座姿態(tài)與機械臂的控制器設(shè)計；文獻［11］針對基座完全停控與基座進行姿態(tài)控制進行了比較研究；文獻［12］在只進行基座姿態(tài)控制時，通過廣義Jacobi矩陣規(guī)劃機械手的運動，可實現(xiàn)對基座的近似零干擾，該方法后來在ETS-VII上進行了試驗。

從上述文獻可以看到，空間機器人的控制器設(shè)計思路主要分為2種：一種是基座完全停控，即軌道和姿態(tài)都不施加控制，機器人只進行機械臂的關(guān)節(jié)控制，該方法相當(dāng)于完全喪失基座的控制能力，機械臂末端可以達到的空間位置姿態(tài)非常有限；另一種是基座姿態(tài)施加控制，但軌道不進行控制，同時機械臂各關(guān)節(jié)施加控制，該方法利用了基座的3個姿態(tài)轉(zhuǎn)動自由度，部分?jǐn)U展了空間機器人的工作空間，但還是喪失了基座軌道控制后所能達到的工作空間。上述2種情況都只利用了機械臂和（或）基座的轉(zhuǎn)動自由度，會導(dǎo)致機械臂可達空間有限，對于復(fù)雜的操作任務(wù)，常常還會導(dǎo)致機械臂構(gòu)型奇異，無法完成操作任務(wù)。

本文突破傳統(tǒng)方法中只對機械臂和（或）基座姿態(tài)進行控制的思路，在通用的空間多體系統(tǒng)的動力學(xué)模型基礎(chǔ)上，對基座的軌道、姿態(tài)及機械臂的各個自由度同時進行協(xié)同控制，即利用系統(tǒng)的所有自由度來實現(xiàn)空間操作，相比以往機械臂對目標(biāo)操作時基座停控的情況，理論上可以達到空間中的任意位置，徹底消除機械臂操作時的構(gòu)型奇異問題，極大地增加了空間機器人的操作范圍和工作空間，增強了機器人在軌實現(xiàn)各種復(fù)雜任務(wù)的能力。

1 動力學(xué)建模

空間機器人由基座及機械臂兩大部分組成，如圖1所示。

圖1 空間機器人結(jié)構(gòu)示意圖Fig.1 Configuration illustration of space robot

為了對圖1所示系統(tǒng)進行描述，定義如下坐標(biāo)系：慣性坐標(biāo)系oexeyeze，固定在慣性空間中；第i（0≤i≤n）體的本體坐標(biāo)系oixiyizi，i=0對應(yīng)基座，o0固定在基座的質(zhì)心，i＞0時對應(yīng)各節(jié)機械臂，oi固定在第i體與i-1體的鉸接處。該多體系統(tǒng)的動力學(xué)方程可表示為

在上述動力學(xué)方程的建立過程中，選取了基座的質(zhì)心位置、基座的姿態(tài)及機械臂的各關(guān)節(jié)角度作為動力學(xué)方程的狀態(tài)變量，與傳統(tǒng)文獻不進行基座的軌道控制相比，多出了基座質(zhì)心的平動動力學(xué)方程。由于空間機器人在進行空間任務(wù)操作時，在基座本體上常常安裝了全局相機，以對周邊環(huán)境進行觀察，通過本文基座質(zhì)心，可以方便地計算出固定在基座上的相機的位置與姿態(tài)，從而對相機的觀察模型進行數(shù)值建模與仿真。而傳統(tǒng)文獻由于不考慮基座的軌道運動，常常將系統(tǒng)的平動點取在整個系統(tǒng)的質(zhì)心處，但由于空間多體系統(tǒng)的變構(gòu)型特點，系統(tǒng)的質(zhì)心沒有固定的位置，因此也無法利用系統(tǒng)質(zhì)心對基座上的全局相機等進行建模。

2 控制律設(shè)計

為了對空間機器人進行控制，選取需要控制的狀態(tài)量為qE=［RT0θT0RTEθTE］T，注意qE與動力學(xué)方程中q的區(qū)別，R0、θ0、RE、θE分別為基座位置、姿態(tài)及機械臂末端位置、末端姿態(tài)。

為了描述基座的姿態(tài)運動，采用如下形式定義基座的姿態(tài)運動：

式中：θ0=［φ0θ0ψ0］T為基座的三軸歐拉角。

采用312轉(zhuǎn)序時，D（θ0）可表示為

為了建立qE與q的關(guān)系，需要將機械臂末端的位置和姿態(tài)表示成機械臂各關(guān)節(jié)的函數(shù)。

第i體質(zhì)心的速度可表示為

式中：Aek為從okxkykzk到oexeyeze的坐標(biāo)轉(zhuǎn)換矩陣；Ski為第i體相對第k體的靜矩，即

其中：Aki為從oixiyizi到okxkykzk的坐標(biāo)轉(zhuǎn)換矩陣；rj，j+1為從oj到oj+1的矢量坐標(biāo)（見圖1）；rci為第i體質(zhì)心在oixiyizi中的位置。

機械臂末端的速度可表示為

機械臂末端的角速度可表示為

機械臂末端姿態(tài)的運動學(xué)也采用312歐拉角來描述，則

式中：J為Jacobi矩陣。式（9）即建立了控制狀態(tài)量qE與動力學(xué)方程中的狀態(tài)量q之間的關(guān)系。

式（9）中，當(dāng)J不是滿秩矩陣時，稱為“動力學(xué)奇異”，出現(xiàn)動力學(xué)奇異時，空間機器人無法將機械臂末端在慣性空間中進行任意位置、姿態(tài)的跟蹤。由于本文對空間機器人的基座位置、姿態(tài)及機械臂的各關(guān)節(jié)同時進行控制，最大程度上利用了空間機器人的所有自由度，因此可以將這種“動力學(xué)奇異”現(xiàn)象降低到最少，同時從物理意義上，由于基座的位置可控，因此可以將機械臂的末端定位到任意位置和姿態(tài)，即使遇到了這種“動力學(xué)奇異”現(xiàn)象，也可以在通過所謂的“零運動”，即在不改變末端位姿的情況下，基座和機械臂進行協(xié)調(diào)運動，將基座與機械臂的構(gòu)型進行重構(gòu)，脫離出奇異構(gòu)型。

“動力學(xué)奇異”與系統(tǒng)的當(dāng)前狀態(tài)相關(guān)，從理論上，永遠無法完全消除這種現(xiàn)象，所以本文對逆矩陣J-1采用J的Moore-Penrose逆J+來代替，J滿秩時二者相等，J奇異時，J+則可有效避免高幅值的關(guān)節(jié)角速度指令。從后文仿真來看，采用本文的控制方法，未遭遇到奇異構(gòu)型。

下面針對控制狀態(tài)量qE進行系統(tǒng)跟蹤控制器設(shè)計。

選取如下形式的Lyapunov函數(shù)：

對式（12）求導(dǎo)，選取如下控制律：

式中：K1為控制增益。

由式（13）可解得

因此系統(tǒng)是漸近穩(wěn)定的。

在式（12）基礎(chǔ)上進一步取如下形式的Lyapunov函數(shù)：

由式（11）、式（13）與式（16）可得如下等式：

將式（19）代入式（18）可得

取

式中：K3為控制增益。

則

即采用式（22）中的控制輸入時，系統(tǒng)是漸近穩(wěn)定的。

將式（14）和式（16）代入式（22）可得最后的控制輸入為

式（25）即為整個系統(tǒng)的控制輸入，包括基座位置控制力、姿態(tài)控制力矩與機械臂各關(guān)節(jié)的控制力矩。

3 軌道控制脈寬調(diào)制

圖2 偽速率調(diào)制器框圖Fig.2 Block diagram of pseudo-rate modulator

式（25）給出的是連續(xù)控制指令，而對于軌道控制來講，采用的是噴氣發(fā)動機；基座的姿態(tài)控制可以采用噴氣發(fā)動機，也可以采用角動量交換執(zhí)行機構(gòu)，如控制力矩陀螺；機械臂的關(guān)節(jié)控制采用伺服電機。本文中只利用噴氣進行基座的軌道控制，基座的姿態(tài)控制采用能輸出連接指令的控制力矩陀螺。因此，利用發(fā)動機的固定推力器來實現(xiàn)軌道的控制，需要將上述連續(xù)指令調(diào)制成離散脈寬。現(xiàn)有多種指令調(diào)制方法，而本文采用常用的偽速率調(diào)制方法進行脈寬調(diào)制，其形式如圖2所示。將三軸軌道控制指令F1、F2、F3輸入到偽速 率調(diào)制器，hA、hE、KM、TM均為偽速率調(diào)制器的設(shè)計參數(shù)，F(xiàn)max（針對三軸分 別為Fmax1、Fmax2、Fmax3）為該軸發(fā)動機能提供的噴氣力。通過偽速率調(diào)制器的調(diào)節(jié)，即可將連續(xù)指令調(diào)制成離散脈寬，并在系統(tǒng)姿態(tài)控制性能上近似等價。偽速率調(diào)制器不是本文的重點，具體設(shè)計可參見文獻［15］。

4 數(shù)學(xué)仿真

表1 系統(tǒng)動力學(xué)與控制仿真參數(shù)Table 1 Parameters of system dynamics and control simulation

為了對控制律的性能進行驗證，以某空間機器人為對象進行建模與仿真，假設(shè)空間機器人的機械臂具有6個關(guān)節(jié)自由度。式（25）中的控制增 益K1、k2、K3分 別 取2.5×10-5I12×12、0.7、0.25I12×12。空間機器人基座及各節(jié)機械臂的質(zhì)量特性參數(shù)及尺寸見表1，表中Jci為第i體相對其質(zhì)心的轉(zhuǎn)動慣量，設(shè)空間機器人初始時機械臂處于收攏狀態(tài)，即各關(guān)節(jié)轉(zhuǎn)角為0°，Ai-1，i_ini為從第i體到第i-1體的初始坐標(biāo)轉(zhuǎn)換矩陣。

控制目的是：使基座位置控制到［0.5 0.6 -0.3］m，基 座 姿 態(tài) 機 動 到［-12 10 0］°；機械臂末端在oexeyeze內(nèi)位置達到［0.6 -0.2 0.8］m，在oexeyeze內(nèi)姿態(tài)達到［15 -30 60］°。

圖3 軌道變化（基座質(zhì)心o0 位置）Fig.3 Orbit variation（position of mass center o0 of base of robot）

圖4 軌道控制發(fā)動機噴氣脈寬Fig.4 Jet impulse width of orbit control engine

圖5 軌道控制發(fā)動機噴氣脈寬換算出的推力Fig.5 Thrust calculated from orbit control engine jet impulse width

圖6 基座三軸姿態(tài)Fig.6 Triaxial attitude of base of robot

圖7 基座三軸控制力矩Fig.7 Triaxial control torque of base of robot

圖8 機械臂末端在慣性空間中的位置Fig.8 Position of arm tip of manipulator in inertial space

仿真結(jié)果如圖3～圖11所示。圖3為軌道控制曲線，即基座質(zhì)心的位置曲線，從圖中可以看到，約60 s后，軌道位置到達期望的位置。圖4為軌道控制發(fā)動機噴氣脈寬。圖5為由脈寬換算成的軌控推力，實現(xiàn)了由噴氣脈寬對發(fā)動機連續(xù)指令的調(diào)制。圖6為基座的三軸姿態(tài)，約70 s后，星體機動到期望的目標(biāo)姿態(tài)。基座的姿態(tài)控制力矩如圖7所示，在動態(tài)過程中其最大峰值約為60 N·m，該力矩可采用控制力矩陀螺來實現(xiàn)，約20 s后趨于穩(wěn)定。圖8為機械臂末端的位置，約5 0 s后逐漸趨于指令值。圖9為機械臂末端在慣性空間中的姿態(tài)，也趨于期望姿態(tài)。圖10為機械臂各關(guān)節(jié)角曲線。圖11為機械臂6只關(guān)節(jié)的控制力矩，其幅值也在可以接受的范圍內(nèi)。

圖9 機械臂末端在慣性空間中的姿態(tài)Fig.9 Attitude of arm tip ofmanipulator in inertial space

圖10 機械臂關(guān)節(jié)角Fig.10 Joint angles of manipulator

圖11 機械臂關(guān)節(jié)控制力矩Fig.11 Control torque of joints of manipulator

傳統(tǒng)的方法對基座的軌道和（或）姿態(tài)不施加主動控制，基座的軌道、姿態(tài)將只能隨機械臂關(guān)節(jié)的運動而被動變化，在本文中，通過對基座的軌道和姿態(tài)同時進行控制，將基座的位置、姿態(tài)控制到期望狀態(tài)，就能有效利用基座的平動和轉(zhuǎn)動自由度，與機械臂各關(guān)節(jié)協(xié)同，實現(xiàn)機械臂末端的精確控制。

仿真顯示，采用本文設(shè)計的一體化控制器，能同時實現(xiàn)空間機器人基座的軌道、姿態(tài)和機械臂位置、姿態(tài)的精確控制，并使系統(tǒng)具有較好的動態(tài)特性。

5 結(jié) 論

空間多體系統(tǒng)在未來新型空間任務(wù)中具有重要應(yīng)用價值，值得大家廣泛關(guān)注與研究，本文所做的工作和得到的結(jié)論如下：

1）針對空間多體系統(tǒng)的軌道、姿態(tài)及操作機構(gòu)進行了建模，并針對解析的系統(tǒng)模型，設(shè)計了基座軌道、姿態(tài)及操作機構(gòu)的一體化控制器。相對于基座的軌道或姿態(tài)不施加控制的傳統(tǒng)方法，本文控制器充分利用了空間多體系統(tǒng)的所有自由度，能實現(xiàn)所有自由度的主動控制，從而擴展了空間多體系統(tǒng)的工作空間。

2）設(shè)計的控制器能使空間機器人同時進行大范圍的軌道轉(zhuǎn)移、姿態(tài)機動，同時操作機構(gòu)可進行末端快速精準(zhǔn)操作，滿足快速、同步、協(xié)調(diào)的高性能要求，通過完整的空間多體系統(tǒng)動力學(xué)與控制仿真建模，驗證了本文控制器設(shè)計方法的有效性。