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

魏春嶺，袁泉，張軍，王夢(mèng)菲

（1.北京控制工程研究所，北京100190； 2.空間智能控制技術(shù)國(guó)家級(jí)重點(diǎn)實(shí)驗(yàn)室，北京100190）

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

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

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

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

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

1 動(dòng)力學(xué)建模

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

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

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

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

2 控制律設(shè)計(jì)

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

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

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

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

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

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

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

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

機(jī)械臂末端的速度可表示為

機(jī)械臂末端的角速度可表示為

機(jī)械臂末端姿態(tài)的運(yùn)動(dòng)學(xué)也采用312歐拉角來(lái)描述，則

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

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

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

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

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

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

式中：K1為控制增益。

由式（13）可解得

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

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

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

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

取

式中：K3為控制增益。

則

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

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

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

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

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

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

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

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

為了對(duì)控制律的性能進(jìn)行驗(yàn)證，以某空間機(jī)器人為對(duì)象進(jìn)行建模與仿真，假設(shè)空間機(jī)器人的機(jī)械臂具有6個(gè)關(guān)節(jié)自由度。式（25）中的控制增 益K1、k2、K3分 別 取2.5×10-5I12×12、0.7、0.25I12×12。空間機(jī)器人基座及各節(jié)機(jī)械臂的質(zhì)量特性參數(shù)及尺寸見(jiàn)表1，表中Jci為第i體相對(duì)其質(zhì)心的轉(zhuǎn)動(dòng)慣量，設(shè)空間機(jī)器人初始時(shí)機(jī)械臂處于收攏狀態(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) 機(jī) 動(dòng) 到［-12 10 0］°；機(jī)械臂末端在oexeyeze內(nèi)位置達(dá)到［0.6 -0.2 0.8］m，在oexeyeze內(nèi)姿態(tài)達(dá)到［15 -30 60］°。

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

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

圖5 軌道控制發(fā)動(dòng)機(jī)噴氣脈寬換算出的推力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 機(jī)械臂末端在慣性空間中的位置Fig.8 Position of arm tip of manipulator in inertial space

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

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

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

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

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

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

5 結(jié) 論

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

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

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