基于干擾力矩補(bǔ)償?shù)目臻g飛行器姿態(tài)控制方法

鄧博煒，田 源，王 悅，金 文

（北京航天長(zhǎng)征飛行器研究所，北京，100076）

0 引 言

空間飛行器在飛行過(guò)程中需要開(kāi)啟軌控發(fā)動(dòng)機(jī)進(jìn)行機(jī)動(dòng)，其在提供所需過(guò)載的同時(shí)，由于安裝誤差等非理想化因素，也會(huì)產(chǎn)生附加的干擾力矩。其中影響較大的是飛行器質(zhì)心漂移和軌控發(fā)動(dòng)機(jī)推力偏心、推力偏移引起的干擾力矩，并且這幾個(gè)誤差項(xiàng)在地面試驗(yàn)中難以進(jìn)行測(cè)量。若其引起的干擾力矩過(guò)大，可能導(dǎo)致姿態(tài)控制精度無(wú)法滿足要求，不利于導(dǎo)引頭跟蹤目標(biāo)。

在已經(jīng)進(jìn)行過(guò)的空間飛行器姿態(tài)控制研究中，多是將質(zhì)心漂移、軌控推力偏心和偏移引起的干擾力矩與其他外部干擾力矩一起考慮，視作一個(gè)總的有界干擾，并進(jìn)一步設(shè)計(jì)各種魯棒控制律。但將其視為有界干擾，難免存在控制保守的問(wèn)題。

有學(xué)者針對(duì)軌控干擾力矩的估計(jì)進(jìn)行了一些相關(guān)研究。梁彤等考慮到軌控干擾力矩在地面試驗(yàn)中難以測(cè)量，提出了一種在飛行過(guò)程中進(jìn)行的測(cè)試方法，在空間飛行器角速度較小時(shí)，令軌控發(fā)動(dòng)機(jī)輪流開(kāi)機(jī)，利用開(kāi)機(jī)時(shí)刻和關(guān)機(jī)待慣組穩(wěn)定后所測(cè)角速度來(lái)計(jì)算軌控干擾力矩，但其基于軌控發(fā)動(dòng)機(jī)開(kāi)機(jī)時(shí)引起的干擾力矩為常值的假設(shè)。張朋等分別基于擴(kuò)展卡爾曼濾波（Extended Kalman Filter,EKF）和無(wú)跡卡爾曼濾波（Unscented Kalman Filter，UKF），推導(dǎo)了飛行器推力偏心和質(zhì)心漂移的濾波模型并進(jìn)行數(shù)學(xué)仿真，但只考慮了一個(gè)軌控發(fā)動(dòng)機(jī)工作的情況，且采用飛行器的速度作為量測(cè)變量，通常認(rèn)為飛行器速度是由衛(wèi)星導(dǎo)航系統(tǒng)獲得，因此該方法不適用于無(wú)衛(wèi)星導(dǎo)航的情況。Tahk 等利用空間飛行器地面懸浮度試驗(yàn)的數(shù)據(jù)，選取角速度和視加速度作為量測(cè)變量，基于擴(kuò)展卡爾曼濾波，對(duì)質(zhì)心漂移和軌控推力偏心進(jìn)行了估計(jì)，但同樣只考慮了僅安裝一個(gè)軌控發(fā)動(dòng)機(jī)的情形。

本文考慮質(zhì)心漂移、軌控推力偏心和推力偏移這幾個(gè)引起干擾力矩的主要誤差源，建立了相應(yīng)的空間飛行器姿態(tài)動(dòng)力學(xué)方程，選取慣組測(cè)得的角速度作為量測(cè)變量。通過(guò)可觀測(cè)性秩條件，對(duì)該系統(tǒng)中狀態(tài)變量的可觀測(cè)性進(jìn)行了分析。在可觀測(cè)性分析的基礎(chǔ)上，基于擴(kuò)展卡爾曼濾波對(duì)空間飛行器軌控發(fā)動(dòng)機(jī)引起的干擾力矩進(jìn)行在線估計(jì)，并進(jìn)一步在姿態(tài)控制中對(duì)其做出補(bǔ)償。通過(guò)數(shù)學(xué)仿真，驗(yàn)證干擾力矩估計(jì)的可行性及其補(bǔ)償對(duì)姿態(tài)控制精度的改善。

1 空間飛行器的動(dòng)力學(xué)建模

1.1 空間飛行器的姿態(tài)動(dòng)力學(xué)

本文針對(duì)的研究對(duì)象是一種典型布局的空間飛行器，4 個(gè)軌控發(fā)動(dòng)機(jī)正交安裝在飛行器標(biāo)稱質(zhì)心所在的橫截面上，6 個(gè)姿控發(fā)動(dòng)機(jī)安裝在飛行器的尾部，發(fā)動(dòng)機(jī)布局及其編號(hào)如圖1 所示。在現(xiàn)實(shí)中，由于結(jié)構(gòu)、安裝誤差等因素的影響，飛行器的實(shí)際質(zhì)心和設(shè)計(jì)的標(biāo)稱質(zhì)心未必能夠重合，并且隨著燃料的消耗，實(shí)際質(zhì)心的位置也會(huì)發(fā)生變化，稱為質(zhì)心漂移；發(fā)動(dòng)機(jī)推力的實(shí)際方向未必與理想的推力作用線重合，稱為推力偏心，此外，發(fā)動(dòng)機(jī)推力的實(shí)際作用點(diǎn)位置與理想作用點(diǎn)也會(huì)存在一定的偏差，稱為推力偏移。質(zhì)心漂移、軌控推力偏心和推力偏移引起的干擾力矩是影響姿態(tài)控制精度的主要因素。

圖1 發(fā)動(dòng)機(jī)布局及編號(hào)Fig.1 Configuration and Serial Number of Thrusters

本文對(duì)空間飛行器進(jìn)行建模時(shí)，認(rèn)為其姿控發(fā)動(dòng)機(jī)的推力偏心和推力偏移較小，故在姿態(tài)動(dòng)力學(xué)模型中對(duì)其引起的干擾力矩忽略不計(jì)?？紤]質(zhì)心漂移、軌控發(fā)動(dòng)機(jī)推力偏心和推力偏移時(shí)的姿態(tài)動(dòng)力學(xué)模型如下：

式中J ,J ,J為飛行器沿載體系各軸的轉(zhuǎn)動(dòng)慣量；ω , ,為載體系中的飛行器角速度；M ,M ,為姿控發(fā)動(dòng)機(jī)和軌控發(fā)動(dòng)機(jī)的合力矩在載體系各軸的分量；,,為外部隨機(jī)干擾力矩在載體系中的分量。

作用在空間飛行器上的合外力（除重力）為

式中F ,F ,F為飛行器所受合外力（除重力）在載體系各軸的分量；,,,為1#至4#軌控發(fā)動(dòng)機(jī)的推力大小；,,,,,為1#至6#姿控發(fā)動(dòng)機(jī)的推力大??； ,分別為1#軌控發(fā)動(dòng)機(jī)真實(shí)推力方向與平面和平面的夾角；,分別為2#軌控發(fā)動(dòng)機(jī)真實(shí)推力方向與平面和平面的夾角；,分別為3#軌控發(fā)動(dòng)機(jī)真實(shí)推力方向與平面和平面的夾角；,分別為4#軌控發(fā)動(dòng)機(jī)真實(shí)推力方向與平面和平面的夾角，即推力偏心。由這些推力偏心引起的軌控推力在體坐標(biāo)軸上的分量為正時(shí)，相應(yīng)的推力偏心值為正，反之為負(fù)。

進(jìn)而空間飛行器受到的姿控發(fā)動(dòng)機(jī)和軌控發(fā)動(dòng)機(jī)的合力矩可以由下式計(jì)算：

式中為空間飛行器的半徑； L為載體系原點(diǎn)（理想質(zhì)心）到姿控發(fā)動(dòng)機(jī)安裝平面的距離；,,為飛行器真實(shí)質(zhì)心在載體系中的坐標(biāo)，即質(zhì)心漂移；Δ, Δ,Δ為#（=1,2,3,4）軌控發(fā)動(dòng)機(jī)真實(shí)作用點(diǎn)位置相對(duì)于理想作用點(diǎn)的偏差在載體系中的分量，即推力偏移。

1.2 濾波模型的建立

對(duì)式（3）進(jìn)行近似處理，得到便于濾波的形式，在濾波時(shí)實(shí)際采用式（4）所示的近似合力矩：

式中 status為（#=1,2,3,4）軌控發(fā)動(dòng)機(jī)狀態(tài)，st atus=1表示#軌控發(fā)動(dòng)機(jī)為開(kāi)機(jī)狀態(tài)，st atus= 0表示#軌控發(fā)動(dòng)機(jī)為關(guān)機(jī)狀態(tài)；F，F為合外力分量，可以由慣組測(cè)得的視加速度乘以空間飛行器質(zhì)量得到。

由于質(zhì)心漂移、軌控推力偏心等通常都是1 0數(shù)量級(jí)或更小的量，通過(guò)對(duì)式（3）和式（4）中各力和力矩的數(shù)量級(jí)進(jìn)行分析可知，當(dāng)軌控發(fā)動(dòng)機(jī)等效推力遠(yuǎn)大于姿控發(fā)動(dòng)機(jī)標(biāo)稱推力時(shí)，用式（4）所示的近似合力矩來(lái)代替式（3）所示的合力矩，能夠達(dá)到很高的精度。

定義各軌控發(fā)動(dòng)機(jī)的等效力臂為

用式（9）至式（16）定義的等效力臂對(duì)近似合力矩式（4）中的相應(yīng)變量進(jìn)行替換，則式（4）可以改寫成：

選取慣組測(cè)得的飛行器角速度作為量測(cè)變量，選取飛行器角速度和各軌控發(fā)動(dòng)機(jī)的等效力臂作為待估計(jì)的狀態(tài)變量，可以建立估算軌控干擾力矩的狀態(tài)方程和量測(cè)方程。

狀態(tài)方程如下：

狀態(tài)方程式（18）可以寫成如下形式：

量測(cè)方程式（20）可以寫成如下形式：

2 可觀測(cè)性分析

在設(shè)計(jì)卡爾曼濾波器之前，通常先進(jìn)行系統(tǒng)的可觀測(cè)性分析。由于系統(tǒng)的可觀測(cè)性與控制輸入無(wú)關(guān)，并忽略狀態(tài)噪聲和量測(cè)噪聲，則由式（19）和式（21）組成的系統(tǒng)可以寫成：

記軌控干擾力矩：

新能源汽車的普及和發(fā)展離不開(kāi)政府的技術(shù)和資金支持，國(guó)家財(cái)政需要充分發(fā)揮自身的積極作用，全面推動(dòng)我國(guó)新能源汽車的快速進(jìn)步。政府支持新能源汽車的發(fā)展和應(yīng)用應(yīng)體現(xiàn)在技術(shù)和財(cái)力方面，上述條件都能促進(jìn)新能源汽車的發(fā)展。另外，國(guó)家還應(yīng)對(duì)新能源汽車實(shí)施強(qiáng)有力的財(cái)政補(bǔ)貼和稅收優(yōu)惠政策，同時(shí)還要高度關(guān)注汽車產(chǎn)業(yè)體系的建設(shè)與發(fā)展。

顯然式（24）表示的系統(tǒng)是一個(gè)非線性系統(tǒng)，其可觀測(cè)矩陣為

如果對(duì)于定義區(qū)間內(nèi)的任意，有()滿秩，則稱由式（24）表示的系統(tǒng)滿足可觀測(cè)性秩條件，進(jìn)而該系統(tǒng)局部弱可觀。

新?tīng)顟B(tài)向量的維數(shù)=6 ，對(duì)于式（24）表示的系統(tǒng)，有：

3 擴(kuò)展卡爾曼濾波

顯然由狀態(tài)方程式（19）和量測(cè)方程式（21）組成的系統(tǒng)是一個(gè)非線性系統(tǒng)，為得到較高精度的軌控發(fā)動(dòng)機(jī)等效力臂估計(jì)值，同時(shí)兼顧計(jì)算的快速性要求，本文選取基于濾波值線性化的方式，即擴(kuò)展卡爾曼濾波?；贓KF 的濾波算法如下。

b）量測(cè)更新。

式中K為時(shí)刻的濾波增益矩陣；H為時(shí)刻的量測(cè)矩陣；R為量測(cè)噪聲的方差陣；Z為時(shí)刻的量測(cè)向量。

對(duì)式（17）所示的近似合力矩進(jìn)行分析。顯然，當(dāng)某個(gè)軌控發(fā)動(dòng)機(jī)處于關(guān)閉狀態(tài)時(shí)，其對(duì)應(yīng)的等效力臂不可觀；姿控發(fā)動(dòng)機(jī)的實(shí)際推力無(wú)法實(shí)時(shí)測(cè)量，只能用其標(biāo)稱推力作為估計(jì)值，當(dāng)姿控發(fā)動(dòng)機(jī)推力處于建立或撤消階段時(shí)，該估計(jì)的誤差較大；由于軌控發(fā)動(dòng)機(jī)等效力臂與姿控發(fā)動(dòng)機(jī)力臂相比是小量，而姿控發(fā)動(dòng)機(jī)推力與標(biāo)稱值存在偏差，當(dāng)軌控發(fā)動(dòng)機(jī)的等效推力較小時(shí)，其等效力臂估計(jì)受姿控推力偏差影響較大。綜合以上因素考慮，本文僅在當(dāng)影響某一軌控等效力臂估計(jì)的所有姿控發(fā)動(dòng)機(jī)推力均處于穩(wěn)定或關(guān)閉狀態(tài)、軌控等效推力遠(yuǎn)大于姿控標(biāo)稱推力時(shí)，才對(duì)其進(jìn)行濾波，否則關(guān)閉其濾波。而對(duì)載體角速度的濾波始終進(jìn)行。關(guān)閉對(duì)某個(gè)狀態(tài)變量濾波的具體方法是：在每一步濾波中，將誤差協(xié)方差陣 P中代表該狀態(tài)變量與其余狀態(tài)變量估計(jì)誤差協(xié)方差的非對(duì)角元素置為0。令代表該狀態(tài)變量估計(jì)誤差方差的對(duì)角元素保持不變，該狀態(tài)變量的估計(jì)值也保持不變，直到重新開(kāi)始對(duì)該狀態(tài)變量濾波為止。

4 仿真分析

4.1 仿真條件

b）假設(shè)軌控發(fā)動(dòng)機(jī)在給出開(kāi)機(jī)指令10 ms 內(nèi)推力進(jìn)入穩(wěn)態(tài)，其穩(wěn)態(tài)值與標(biāo)稱推力存在10%的常值偏差，在穩(wěn)態(tài)值基礎(chǔ)上還存在幅值為5%的推力波動(dòng)，在給出關(guān)機(jī)指令10 ms 內(nèi)推力減少到0；姿控發(fā)動(dòng)機(jī)在給出開(kāi)機(jī)指令5 ms 內(nèi)推力進(jìn)入穩(wěn)態(tài)，其穩(wěn)態(tài)值與標(biāo)稱推力存在5%的常值偏差，在穩(wěn)態(tài)值基礎(chǔ)上還存在幅值為1%的推力波動(dòng)，在給出關(guān)機(jī)指令5 ms 內(nèi)推力減少到0。

c）在動(dòng)力學(xué)模型中飛行器角速度的狀態(tài)噪聲方差為10rad/s，飛行器角速度的量測(cè)噪聲方差為10rad/s。

d）制導(dǎo)律采用比例導(dǎo)引，姿態(tài)控制采用PID 控制，連續(xù)變化的制導(dǎo)和姿態(tài)控制指令通過(guò)PWPF 調(diào)節(jié)器離散化為常推力發(fā)動(dòng)機(jī)的開(kāi)關(guān)指令；

e）取姿態(tài)控制指令的計(jì)算周期為2 ms，姿控發(fā)動(dòng)機(jī)開(kāi)關(guān)指令的更新周期為10 ms，軌控發(fā)動(dòng)機(jī)開(kāi)關(guān)指令的更新周期為30 ms，擴(kuò)展卡爾曼濾波的計(jì)算周期為1 ms；

f）待估狀態(tài)變量的濾波初值均取為0，狀態(tài)誤差協(xié)方差陣的初值取為= 10?，狀態(tài)噪聲的方差陣取為Q= 10?，量測(cè)噪聲的方差陣取為R= 10?；

h）認(rèn)為姿控發(fā)動(dòng)機(jī)的推力偏心較小，從而將其忽略，傳感器誤差只考慮量測(cè)噪聲。

4.2 結(jié)果分析

當(dāng)影響滾轉(zhuǎn)通道的兩個(gè)軌控發(fā)動(dòng)機(jī)同時(shí)開(kāi)機(jī)時(shí)，其等效力臂的估計(jì)存在耦合，而每個(gè)通道的軌控干擾力矩始終可觀，此外對(duì)于姿態(tài)控制，主要就是關(guān)心軌控干擾力矩，因此在本小節(jié)中僅給出由等效力臂濾波值得到的軌控干擾力矩的估計(jì)結(jié)果和有/無(wú)軌控干擾力矩補(bǔ)償時(shí)的姿態(tài)控制結(jié)果對(duì)比。

圖2給出了滾轉(zhuǎn)、偏航、俯仰通道的軌控干擾力矩估計(jì)值與真實(shí)值的對(duì)比。在考慮轉(zhuǎn)動(dòng)慣量變化和姿控發(fā)動(dòng)機(jī)穩(wěn)態(tài)推力偏差的情況下，雖然軌控干擾力矩的估計(jì)值與真實(shí)值存在一定偏差，但其估計(jì)精度還處于可以接受的范圍，能夠在較大程度上反映出真實(shí)軌控干擾力矩的大小和變化。特別是從圖2a 中17.07~ 17.11 s 的曲線可以看出，當(dāng)滾轉(zhuǎn)通道同時(shí)存在2 個(gè)軌控發(fā)動(dòng)機(jī)開(kāi)機(jī)時(shí)，其總的軌控干擾力矩仍具有較好的估計(jì)效果，這也驗(yàn)證了本文第2 節(jié)中可觀測(cè)性分析的正確性。

圖2 軌控發(fā)動(dòng)機(jī)干擾力矩的估計(jì)Fig.2 Estimation of Disturbance Torques of Divert Thrusters

分析式（18）所示的濾波狀態(tài)方程，轉(zhuǎn)動(dòng)慣量的實(shí)際變化規(guī)律未知，姿控發(fā)動(dòng)機(jī)的穩(wěn)態(tài)推力也無(wú)法實(shí)時(shí)測(cè)量，只能采用其標(biāo)稱推力作為估計(jì)值。濾波模型和動(dòng)力學(xué)模型中轉(zhuǎn)動(dòng)慣量和姿控穩(wěn)態(tài)推力的差異導(dǎo)致了濾波結(jié)果的誤差。

圖3為有/無(wú)軌控干擾力矩補(bǔ)償時(shí)俯仰通道的姿態(tài)控制效果對(duì)比。

圖3 有/無(wú)軌控干擾力矩補(bǔ)償?shù)母┭鼋荈ig.3 Pitch Angle with/without Compensation of Disturbance Torque Caused by Divert Thrusters

由圖3 可知，在22 s 后，補(bǔ)償?shù)男Ч诛@著，若不對(duì)軌控干擾力矩進(jìn)行補(bǔ)償，則俯仰角偏差迅速增大，最大偏差可以達(dá)到0.81°，而對(duì)其進(jìn)行補(bǔ)償則可以獲得很高的俯仰控制精度。在5~22 s 的時(shí)間段內(nèi)，有軌控干擾力矩補(bǔ)償?shù)母┭隹刂凭纫矁?yōu)于無(wú)補(bǔ)償情況。圖4 為有/無(wú)軌控干擾力矩補(bǔ)償時(shí)偏航通道的姿態(tài)控制效果對(duì)比。

圖4 有/無(wú)軌控干擾力矩補(bǔ)償?shù)钠浇荈ig.4 Yaw Angle with/without Compensation of Disturbance Torque Caused by Divert Thrusters

有軌控干擾力矩補(bǔ)償時(shí)最大偏航角偏差為0.54°，無(wú)軌控干擾力矩補(bǔ)償時(shí)最大偏航角偏差為0.57°，2 種情況下的偏航角控制精度無(wú)顯著區(qū)別。這是由于根據(jù)制導(dǎo)指令，安裝在飛行器載體系軸方向的軌控發(fā)動(dòng)機(jī)開(kāi)機(jī)次數(shù)較少，相應(yīng)地其引起的干擾力矩對(duì)偏航通道控制的影響較小，因此對(duì)其進(jìn)行補(bǔ)償?shù)男Ч膊皇呛苊黠@。

圖5為有/無(wú)軌控干擾力矩補(bǔ)償時(shí)滾轉(zhuǎn)通道的姿態(tài)控制效果對(duì)比。與俯仰通道的情況類似，在22 s 以后，不進(jìn)行軌控干擾力矩補(bǔ)償?shù)那闆r下，滾轉(zhuǎn)角偏差逐漸增大，最大偏差達(dá)到1.24°；而對(duì)軌控干擾力矩進(jìn)行補(bǔ)償后，滾轉(zhuǎn)角偏差保持在0.43°以內(nèi)，干擾力矩補(bǔ)償?shù)男Ч@著。

圖5 有/無(wú)軌控干擾力矩補(bǔ)償?shù)臐L轉(zhuǎn)角Fig.5 Roll Angle with/without Compensation of Disturbance Torque Caused by Divert Thrusters

5 結(jié) 論

本文基于EKF 對(duì)狀態(tài)變量進(jìn)行了估計(jì)，數(shù)學(xué)仿真結(jié)果表明，即使濾波模型中使用的轉(zhuǎn)動(dòng)慣量和姿控發(fā)動(dòng)機(jī)穩(wěn)態(tài)推力與動(dòng)力學(xué)模型存在一定偏差，也能對(duì)軌控干擾力矩實(shí)現(xiàn)較好的估計(jì)。當(dāng)滾轉(zhuǎn)通道存在2 個(gè)軌控發(fā)動(dòng)機(jī)同時(shí)開(kāi)機(jī)時(shí)，其總的軌控干擾力矩仍具有較好的估計(jì)效果，這也驗(yàn)證了前述可觀測(cè)性分析的正確性。對(duì)軌控干擾力矩進(jìn)行補(bǔ)償能夠改善姿態(tài)控制效果，軌控發(fā)動(dòng)機(jī)開(kāi)機(jī)越頻繁，補(bǔ)償效果越顯著。軌控干擾力矩的估計(jì)精度主要是受轉(zhuǎn)動(dòng)慣量和姿控發(fā)動(dòng)機(jī)穩(wěn)態(tài)推力這2 個(gè)誤差源影響，姿控發(fā)動(dòng)機(jī)穩(wěn)態(tài)推力偏差可以通過(guò)發(fā)動(dòng)機(jī)設(shè)計(jì)和工藝水平的提升來(lái)減小，而轉(zhuǎn)動(dòng)慣量的實(shí)際變化規(guī)律無(wú)法獲得，但根據(jù)數(shù)學(xué)仿真的結(jié)果，若轉(zhuǎn)動(dòng)慣量在其初值附近較小的范圍內(nèi)變化，軌控干擾力矩的估計(jì)就能夠達(dá)到較高的精度。