本發(fā)明屬于翻滾目標(biāo)的姿態(tài)估計(jì)及參數(shù)辨識(shí)領(lǐng)域，涉及航天領(lǐng)域的空間在軌服務(wù)技術(shù)，具體涉及一種任意形狀翻滾衛(wèi)星的非接觸式慣量系數(shù)辨識(shí)方法。

背景技術(shù)：

航天器在軌服務(wù)的對(duì)象多種多樣，其中動(dòng)力失效的衛(wèi)星或空間碎片占據(jù)很大比例。由于失去了動(dòng)力控制，失效衛(wèi)星在初始角速度及干擾力矩的作用下一般會(huì)陷入繞慣性主軸自由慢翻滾狀態(tài)，若要對(duì)其進(jìn)行近距離交會(huì)、逼近乃至抓捕，必須對(duì)其姿態(tài)變化規(guī)律進(jìn)行預(yù)判，然后選擇合適的接近角度靠近，以避免碰撞而導(dǎo)致裝置損壞。

為了對(duì)翻滾目標(biāo)未來(lái)較長(zhǎng)時(shí)間的運(yùn)動(dòng)狀態(tài)進(jìn)行精確預(yù)測(cè)，以制定更合理的抓捕時(shí)機(jī)和抓捕路徑，必須在抓捕之前使用非接觸式的方法對(duì)翻滾目標(biāo)的慣性參數(shù)進(jìn)行準(zhǔn)確的辨識(shí)。這對(duì)于節(jié)省抓捕所耗費(fèi)的時(shí)間和燃料、提高抓捕成功率是十分必要的。

對(duì)于大部分失效衛(wèi)星來(lái)說(shuō)由于燃料消耗或結(jié)構(gòu)的損壞等可能會(huì)使轉(zhuǎn)動(dòng)慣量發(fā)生變化。在現(xiàn)有技術(shù)下利用立體視覺(jué)設(shè)備或激光測(cè)距儀可以在不接觸的情況下對(duì)未知衛(wèi)星的姿態(tài)信息進(jìn)行離散測(cè)量，但是，在非接觸的情況下對(duì)衛(wèi)星的慣性參數(shù)進(jìn)行精確辨識(shí)極其困難。目前最常用的參數(shù)辨識(shí)方法是使用卡爾曼濾波器將其轉(zhuǎn)動(dòng)慣量作為狀態(tài)變量之一進(jìn)行遞推擬合，但精度很低，而且需要給定足夠精確的角速度測(cè)量初值才能保證算法收斂。有文獻(xiàn)提出使用常值參數(shù)設(shè)計(jì)濾波器進(jìn)行辨識(shí)，但僅適用于翻滾目標(biāo)為對(duì)稱(chēng)剛體的特殊情況。實(shí)際應(yīng)用中多在抓捕后使用接觸的方法對(duì)翻滾衛(wèi)星的轉(zhuǎn)動(dòng)慣量進(jìn)行測(cè)量，這樣雖然簡(jiǎn)化了任務(wù)過(guò)程，但會(huì)增加額外的能量消耗。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于提供一種任意形狀翻滾衛(wèi)星的非接觸式慣量系數(shù)辨識(shí)方法，以克服上述現(xiàn)有技術(shù)存在的缺陷，本發(fā)明直接推算出翻滾衛(wèi)星的慣性參數(shù)，即主轉(zhuǎn)動(dòng)慣量之間的比值。相比于以往的常值估計(jì)法，本方法不限于軸對(duì)稱(chēng)目標(biāo)的參數(shù)估計(jì)，對(duì)任意形狀的翻滾目標(biāo)的主轉(zhuǎn)動(dòng)慣量之間的比值均可進(jìn)行辨識(shí)。

為達(dá)到上述目的，本發(fā)明采用如下技術(shù)方案：

一種任意形狀翻滾衛(wèi)星的非接觸式慣量系數(shù)辨識(shí)方法，包括如下步驟：

1)通過(guò)近似方法建立翻滾目標(biāo)四元數(shù)λ的運(yùn)動(dòng)方程的近似解；

2)通過(guò)近似解提取常值參數(shù)作為系統(tǒng)的狀態(tài)量x；

3)構(gòu)造擴(kuò)展卡爾曼濾波器；

4)通過(guò)卡爾曼濾波器得到系統(tǒng)的狀態(tài)量的估計(jì)值，并采用估計(jì)值估計(jì)翻滾衛(wèi)星的慣性參數(shù)。

進(jìn)一步地，步驟1)具體為：

1-1)建立翻滾目標(biāo)的姿態(tài)四元數(shù)的運(yùn)動(dòng)學(xué)模型：

其中，λ₀,λ₁,λ₂,λ₃為翻滾目標(biāo)的本體主軸坐標(biāo)系相對(duì)于慣性坐標(biāo)系的姿態(tài)四元數(shù)四個(gè)分量，ω_x,ω_y,ω_z為目標(biāo)的角速度在本體坐標(biāo)系中的投影；

1-2)忽略高頻小量，得到翻滾目標(biāo)的角速度近似解：

其中，ω_xm,ω_ym,ω_zm分別為本體主軸坐標(biāo)系三個(gè)坐標(biāo)軸方向上的角速度分量的最大值，t為時(shí)間，為待辨識(shí)的常值參數(shù)之一，ε_x,ε_y,ε_z均為該坐標(biāo)軸方向上的高階無(wú)窮小量；

1-3)使用含有三個(gè)特征頻率的正弦函數(shù)構(gòu)造描述姿態(tài)運(yùn)動(dòng)的四元數(shù)微分方程的近似解，四元數(shù)λ＝[λ₀ λ₁ λ₂ λ₃]^T的解的具體形式為：

λ＝A_4×6ξ (3)

其中A_4×6為4行6列的常數(shù)矩陣，且

其中，θ也是待辨識(shí)的常值參數(shù)；

1-4)將系統(tǒng)的微分方程轉(zhuǎn)化為代數(shù)方程，將式(2)與式(3)同時(shí)代入式(1)，使用三角函數(shù)運(yùn)算規(guī)則合并相同頻率項(xiàng)，由三角函數(shù)的正則性可知，若等式(1)成立，須令各頻率之前的系數(shù)均為零，則得到一個(gè)由56個(gè)算術(shù)方程組成的方程組：

其中a為矩陣A_4×6按行展開(kāi)成向量后生成的行向量，為由θ以及ω_xm，ω_ym，ω_zm決定的常系數(shù)矩陣，其各項(xiàng)的具體形式由式(3)推導(dǎo)得到，下標(biāo)表示矩陣維數(shù)，考慮到系統(tǒng)的初值λ(0)，將上式擴(kuò)展為：

其中，其中常數(shù)矩陣D′的值由D′a＝λ(0)推得：

式(4)是約束數(shù)大于變量數(shù)的超定方程組，使用最小二乘準(zhǔn)則求得a，及θ的最小二乘解，得到四元數(shù)λ關(guān)于時(shí)間t的近似解，即姿態(tài)四元數(shù)隨時(shí)間變化的近似方程，也就是翻滾目標(biāo)的姿態(tài)運(yùn)動(dòng)的近似方程。

進(jìn)一步地，步驟2)具體為：

實(shí)際觀(guān)測(cè)量η為觀(guān)測(cè)坐標(biāo)系相對(duì)于慣性系的四元數(shù)，它與λ之間的關(guān)系為：

η＝λομ

其中“ο”表示四元數(shù)乘法運(yùn)算；μ為觀(guān)測(cè)坐標(biāo)系與本體主軸坐標(biāo)系之間的姿態(tài)四元數(shù)；

由于四元數(shù)乘法不涉及高次項(xiàng)運(yùn)算，且μ為常數(shù)，故η各項(xiàng)均為λ各項(xiàng)的線(xiàn)性組合，由式(3)可知，觀(guān)測(cè)量η同樣具有周期性質(zhì)，即有：

η＝B_4×6ξ (5)

其中B為滿(mǎn)足Aξ＝Bξομ的常數(shù)矩陣，將矩陣B按行來(lái)展開(kāi)得到行向量b_24×1，并考慮到待辨識(shí)的常值參數(shù)及θ，共同組成了系統(tǒng)的常值的狀態(tài)量：

其中，b,θ，均為待辨識(shí)的常值參數(shù)。

進(jìn)一步地，步驟3)具體為：

系統(tǒng)的觀(guān)測(cè)方程為

h(x)＝η

將h相對(duì)于狀態(tài)量x求偏導(dǎo)數(shù)得到觀(guān)測(cè)敏感度矩陣：

由于系統(tǒng)的狀態(tài)量均為常值參數(shù)，故狀態(tài)轉(zhuǎn)移矩陣Φ為：

Φ＝Ι_14×14 (7)

其中，I_14×14為14×14的單位矩陣；

由干擾力矩引起的狀態(tài)量的誤差認(rèn)為是高斯分布的白噪聲，則系統(tǒng)過(guò)程噪聲的方差矩陣Q_k定義為：

Q_k＝σ²Ι_14×14 (8)

其中，σ²為過(guò)程噪聲的分布方差；

將式(6)-(8)代入卡爾曼濾波器的一般方程，即得到非對(duì)稱(chēng)翻滾目標(biāo)參數(shù)辨識(shí)的步驟，構(gòu)造出由待辨識(shí)參數(shù)作為狀態(tài)量構(gòu)成的廣義卡爾曼濾波器。

進(jìn)一步地，步驟4)具體為：

通過(guò)卡爾曼濾波器得到狀態(tài)量x的估計(jì)值后，將估計(jì)值代入式(4)并求最小二乘解，即得到矩陣D以及角速度參數(shù)ω_xm，ω_ym及ω_zm的估計(jì)值，并進(jìn)一步求解得到慣量參數(shù)的估計(jì)值：：

其中，p_x,p_y,p_z為待辨識(shí)的轉(zhuǎn)動(dòng)慣量比值參數(shù)，K為角速度的橢圓函數(shù)所對(duì)應(yīng)的第一類(lèi)完全橢圓積分系數(shù)。

與現(xiàn)有技術(shù)相比，本發(fā)明具有以下有益的技術(shù)效果：

(1)該方法使用三個(gè)正弦函數(shù)近似姿態(tài)四元數(shù)的運(yùn)動(dòng)規(guī)律，不局限于特定的解的形式，可以證明該近似方法在所有運(yùn)動(dòng)狀態(tài)下均能取得足夠高的精度，因此該方法對(duì)目標(biāo)形狀沒(méi)有限制，適用于任意非對(duì)稱(chēng)形狀的翻滾目標(biāo)的參數(shù)辨識(shí)。(2)該方法用常量參數(shù)代替變量參數(shù)作為系統(tǒng)的狀態(tài)參量，使得標(biāo)稱(chēng)狀態(tài)下?tīng)顟B(tài)參量相對(duì)于時(shí)間的偏導(dǎo)數(shù)為零，當(dāng)觀(guān)測(cè)的時(shí)間間隔較大時(shí)，可以顯著減小使用數(shù)值積分計(jì)算的預(yù)測(cè)值的誤差，從而提高慣量參數(shù)的估計(jì)精度。(3)由于方程具有線(xiàn)性形式，在特征頻率θ和的初始值給定較為精確的情況下，對(duì)其他初值的精確度沒(méi)有任何要求，避免了由于初值精度太低而導(dǎo)致的濾波發(fā)散現(xiàn)象，提高了慣量參數(shù)估計(jì)的成功率。使用快速傅里葉變換的數(shù)值方法得到θ和的初值，能夠保證其精度符合要求。

附圖說(shuō)明

圖1為構(gòu)造四元數(shù)動(dòng)力學(xué)方程近似解的流程圖；

圖2為含有噪聲的觀(guān)測(cè)數(shù)據(jù)的實(shí)例圖；

圖3為使用快速傅里葉變化將姿態(tài)四元數(shù)變化到頻域的實(shí)例圖；

圖4為辨識(shí)得到的系統(tǒng)運(yùn)動(dòng)狀態(tài)的估計(jì)值與預(yù)測(cè)值隨時(shí)間變化的軌跡的實(shí)例圖，其中(a)為觀(guān)測(cè)坐標(biāo)系對(duì)應(yīng)的四元數(shù)分量η₀的軌跡圖，(b)為觀(guān)測(cè)坐標(biāo)系對(duì)應(yīng)的四元數(shù)分量η₁的軌跡圖，(c)為觀(guān)測(cè)坐標(biāo)系對(duì)應(yīng)的四元數(shù)分量η₂的軌跡圖，(d)為觀(guān)測(cè)坐標(biāo)系對(duì)應(yīng)的四元數(shù)分量η₃的軌跡圖，(e)為角速度分量ω_x的軌跡圖，(f)為角速度分量ω_y的軌跡圖，(g)為角速度分量ω_z的軌跡圖；

圖5為慣性參數(shù)估計(jì)值的相對(duì)誤差的收斂過(guò)程的實(shí)例圖。

具體實(shí)施方式

下面對(duì)本發(fā)明作進(jìn)一步詳細(xì)描述：

本發(fā)明是在非接觸的情況對(duì)翻滾狀態(tài)的任意形狀的空間碎片或失效衛(wèi)星的慣性參數(shù)進(jìn)行精確的測(cè)算。其主要原理在于：首先選取三個(gè)主頻率構(gòu)造含有待定參數(shù)的三角函數(shù)，作為翻滾目標(biāo)姿態(tài)運(yùn)動(dòng)方程的近似解，將這組解代入翻滾目標(biāo)的四元數(shù)姿態(tài)動(dòng)力學(xué)方程中，即可將原本的微分方程轉(zhuǎn)化為代數(shù)方程，對(duì)這組代數(shù)方程求解可得以上待定參數(shù)的具體值。使用這些待定參數(shù)可以代替系統(tǒng)變量表征系統(tǒng)的狀態(tài)，即翻滾衛(wèi)星的姿態(tài)四元數(shù)被表示為關(guān)于這些常值參數(shù)和時(shí)間的函數(shù)。翻滾衛(wèi)星的姿態(tài)四元數(shù)可以通過(guò)現(xiàn)有的技術(shù)手段進(jìn)行測(cè)量，本方法通過(guò)線(xiàn)性最小方差估計(jì)，對(duì)待定參數(shù)的值進(jìn)行實(shí)時(shí)的估計(jì)，且隨著觀(guān)測(cè)量的增加，估計(jì)精度越來(lái)越高。利用這些待定參數(shù)的值，本方法直接推算出翻滾衛(wèi)星的慣性參數(shù)，即主轉(zhuǎn)動(dòng)慣量之間的比值。相比于以往的常值估計(jì)法，本方法不限于軸對(duì)稱(chēng)目標(biāo)的參數(shù)估計(jì)，對(duì)任意形狀的翻滾目標(biāo)的主轉(zhuǎn)動(dòng)慣量之間的比值均可進(jìn)行辨識(shí)。

本發(fā)明的方法具體包括以下步驟：

步驟一：通過(guò)近似方法建立翻滾目標(biāo)四元數(shù)運(yùn)動(dòng)方程的近似解，如圖(1)所示。具體流程為：首先建立翻滾目標(biāo)的姿態(tài)四元數(shù)的運(yùn)動(dòng)學(xué)模型

其中，λ₀,λ₁,λ₂,λ₃為翻滾目標(biāo)的主軸坐標(biāo)系相對(duì)于慣性系的姿態(tài)四元數(shù)四個(gè)分量，ω_x,ω_y,ω_z為目標(biāo)的角速度在本體主軸坐標(biāo)系中的投影；

忽略高頻小量，寫(xiě)出翻滾目標(biāo)的角速度近似解

其中ω_xm,ω_ym,ω_zm分別為本體坐標(biāo)系三個(gè)坐標(biāo)軸方向上的角速度分量的最大值，t為時(shí)間，為待辨識(shí)的常值參數(shù)之一，ε_x,ε_y,ε_z均為該坐標(biāo)軸方向上的高階無(wú)窮小量，ε為高頻項(xiàng)，其對(duì)于角速度的長(zhǎng)期影響為零。

通過(guò)頻域分析可以發(fā)現(xiàn)四元數(shù)隨時(shí)間變化的函數(shù)在頻域中有三個(gè)明顯的尖峰，因此使用含有三個(gè)特征頻率的正弦函數(shù)構(gòu)造描述姿態(tài)運(yùn)動(dòng)的四元數(shù)微分方程的近似解。四元數(shù)λ的解的具體形式為

λ＝A_4×6ξ(3)

其中A_4×6為4行6列的常數(shù)矩陣，且

其中θ與也為待識(shí)別的常值參數(shù)，且的值僅由角速度特性決定。

將系統(tǒng)的微分方程轉(zhuǎn)化為代數(shù)方程。將式(2)與式(3)同時(shí)代入式(1)，使用三角函數(shù)運(yùn)算規(guī)則合并相同頻率項(xiàng)，由三角函數(shù)的正則性可知，若等式(1)成立，須令各頻率之前的系數(shù)均為零，則得到一個(gè)由56個(gè)算術(shù)方程組成的方程組

其中a為矩陣A_4×6按行展開(kāi)成向量后生成的行向量，為由θ以及ω_xm，ω_ym，ω_zm決定的常系數(shù)矩陣，其各項(xiàng)的具體形式由式(3)推導(dǎo)可得，下標(biāo)表示矩陣維數(shù)?？紤]到系統(tǒng)的初值λ(0)，上式可擴(kuò)展為

其中，其中常數(shù)矩陣D′的值由D′a＝λ(0)推得：

式(4)是約束數(shù)大于變量數(shù)的超定方程組，可使用最小二乘準(zhǔn)則求得a，及θ的最小二乘解，得到四元數(shù)λ關(guān)于時(shí)間t的近似解，即姿態(tài)四元數(shù)隨時(shí)間變化的近似方程，也就是翻滾目標(biāo)的姿態(tài)運(yùn)動(dòng)的近似方程。事實(shí)上，由于自由翻滾目標(biāo)的角速度滿(mǎn)足Jacobi橢圓函數(shù)積分的形式，高頻項(xiàng)系數(shù)很小，因此以此方法確定的近似解具有較高的精度。

步驟二：提取常值參數(shù)作為系統(tǒng)的狀態(tài)量?？紤]到實(shí)際觀(guān)測(cè)量η為觀(guān)測(cè)坐標(biāo)系相對(duì)于慣性系的四元數(shù)，它與λ之間的關(guān)系

η＝λομ

其中“ο”表示四元數(shù)乘法運(yùn)算，μ為觀(guān)測(cè)坐標(biāo)系與本體主軸坐標(biāo)系之間的姿態(tài)四元數(shù)。由于四元數(shù)乘法不涉及高次項(xiàng)運(yùn)算，且μ為常數(shù)，故η各項(xiàng)可均為λ各項(xiàng)的線(xiàn)性組合。由式(3)的形式可知，觀(guān)測(cè)量η同樣具有周期性質(zhì)，即有

η＝B_4×6ξ (5)

其中B為滿(mǎn)足Aξ＝Bξομ的常數(shù)矩陣。將矩陣B按行來(lái)展開(kāi)得到行向量b_24×1，并考慮到待辨識(shí)的常值參數(shù)及θ，共同組成了系統(tǒng)的常值的狀態(tài)量x

b,θ，均為待辨識(shí)的常值參數(shù)；

步驟三：構(gòu)造擴(kuò)展卡爾曼濾波器。

系統(tǒng)的觀(guān)測(cè)方程為

h(x)＝η

將h相對(duì)于狀態(tài)量x求偏導(dǎo)數(shù)得到觀(guān)測(cè)敏感度矩陣

由于系統(tǒng)的狀態(tài)量均為常值參數(shù)，故狀態(tài)轉(zhuǎn)移矩陣Φ為單位矩陣

Φ＝Ι_14×14 (7)

其中，I_14×14為14×14的單位矩陣；

由干擾力矩引起的狀態(tài)量的誤差認(rèn)為是高斯分布的白噪聲，則系統(tǒng)過(guò)程噪聲的方差矩陣Q_k定義為

Q_k＝σ²Ι_14×14 (8)

其中，σ²為過(guò)程噪聲的分布方差；

將式(6)-(8)代入卡爾曼濾波器的一般方程，即得到非對(duì)稱(chēng)翻滾目標(biāo)參數(shù)辨識(shí)的步驟。與對(duì)稱(chēng)目標(biāo)的情況類(lèi)似，狀態(tài)量中b向量的各項(xiàng)滿(mǎn)足線(xiàn)性形式，不需要給定精確的初值。而θ與涉及三角函數(shù)運(yùn)算，因此需要使用離散傅里葉變換確定特征角頻率θ和的初值。

步驟四：通過(guò)卡爾曼濾波器得到狀態(tài)量x的估計(jì)值后，將估計(jì)值代入式(4)并求最小二乘解，即可得到矩陣D以及角速度參數(shù)ω_xm，ω_ym及ω_zm的估計(jì)值。進(jìn)一步，由下式計(jì)算轉(zhuǎn)動(dòng)慣量參數(shù)的估計(jì)值

p_x,p_y,p_z為待辨識(shí)的轉(zhuǎn)動(dòng)慣量比值參數(shù)，K為角速度的橢圓函數(shù)所對(duì)應(yīng)的第一類(lèi)完全橢圓積分系數(shù)。

為了更好地說(shuō)明本發(fā)明的目的和優(yōu)點(diǎn)，下面結(jié)合附圖和實(shí)例對(duì)本發(fā)明內(nèi)容做進(jìn)一步說(shuō)明：

設(shè)翻滾目標(biāo)在空間中作自由漂浮運(yùn)動(dòng)，使用立體視覺(jué)設(shè)備或激光測(cè)距儀可以測(cè)得姿態(tài)四元數(shù)四個(gè)變量隨時(shí)間變化的函數(shù)，如圖2所示。由于干擾力矩和觀(guān)測(cè)誤差的影響，測(cè)量結(jié)果是受噪聲污染的。應(yīng)用本方法，可以利用這些觀(guān)測(cè)噪聲實(shí)時(shí)地估計(jì)出該翻滾衛(wèi)星的慣性參數(shù)p_x，p_y及p_z，具體包括以下步驟：

步驟一：先使用快速傅里葉變換算法將λ₀(t)的部分?jǐn)?shù)據(jù)變化到頻域，如圖3所示。頻域中有三個(gè)尖峰，將中間尖峰的橫坐標(biāo)的值賦給θ，尖峰橫坐標(biāo)之間的間隔賦給作為初值。x中其他參數(shù)的初值被賦為0。

步驟二：以x為狀態(tài)參數(shù)，以姿態(tài)四元數(shù)λ的實(shí)時(shí)觀(guān)測(cè)量為輸入，構(gòu)建卡爾曼濾波器，逐步估計(jì)出狀態(tài)參數(shù)更精確的值。如圖4所示，由常值參數(shù)的估計(jì)值計(jì)算并預(yù)測(cè)得到的姿態(tài)四元數(shù)及角速度的誤差均小于直接觀(guān)測(cè)值的誤差。

步驟三：利用x各參數(shù)的值計(jì)算翻滾衛(wèi)星慣量參數(shù)p_x，p_y及p_z的值，其估計(jì)值隨時(shí)間變化曲線(xiàn)如圖5所示，可見(jiàn)慣性參數(shù)的估計(jì)值隨著觀(guān)測(cè)量的增加而趨近真實(shí)值。

本實(shí)例中采用的系統(tǒng)參數(shù)的值如表1所示：

表1.實(shí)例采用的系統(tǒng)參數(shù)值