張科備，王大軼，王有懿

(1.北京控制工程研究所，北京 100190；2. 北京空間飛行器總體設(shè)計部，北京 100190)

UKF位姿估計的超靜平臺耦合模型參數(shù)辨識

張科備1，王大軼2，王有懿1

(1.北京控制工程研究所，北京 100190；2. 北京空間飛行器總體設(shè)計部，北京 100190)

針對載荷無陀螺時辨識超靜平臺耦合動力學(xué)參數(shù)存在位姿確定問題，設(shè)計了一種基于Schur分解以及無跡卡爾曼濾波(UKF)的位姿確定及參數(shù)辨識方法。首先，建立加速度計和姿態(tài)敏感器組成的測量系統(tǒng)狀態(tài)模型和觀測模型，并給出測量系統(tǒng)的可觀性分析。然后，給出基于UKF的載荷位姿確定方法；在UKF中引入姿態(tài)修正信息，從而提高載荷角速度估計精度，實現(xiàn)載荷廣義位移、廣義速度、廣義加速度的準(zhǔn)確估計。通過Schur分解實現(xiàn)超靜平臺動力學(xué)模型解耦及辨識模型中動力學(xué)參數(shù)顯式表達。以濾波器估計載荷位姿信息為依據(jù)，采用最小二乘法辨識動力學(xué)參數(shù)。仿真結(jié)果表明UKF能夠準(zhǔn)確估計載荷角速度以及超靜平臺支桿剛度系數(shù)，辨識誤差優(yōu)于百分之一。

Stewart超靜平臺；位姿確定；無跡卡爾曼濾波(UKF) ；參數(shù)辨識

0 引 言

空間觀測等航天任務(wù)要求載荷實現(xiàn)毫角秒級的指向精度和極高的姿態(tài)穩(wěn)定度[1]。而精密跟瞄Stewart平臺具有高性能隔振、高精度指向和高穩(wěn)定控制等優(yōu)點，使得Stewart超靜平臺成為實現(xiàn)這類航天任務(wù)必不可少的一環(huán)[2-3]。Stewart超靜平臺準(zhǔn)確的動力學(xué)參數(shù)是實現(xiàn)載荷高精度指向控制的可靠保障。同時，Stewart平臺屬于多通道耦合系統(tǒng)，準(zhǔn)確的運動學(xué)參數(shù)有利于通道解耦以及控制器設(shè)計。然而，結(jié)構(gòu)形變、安裝偏差等因素造就了系統(tǒng)動力學(xué)參數(shù)實際值與理論值相差較大，這直接影響了Stewart超靜平臺控制性能。因此，辨識Stewart超靜平臺耦合模型動力學(xué)參數(shù)是實現(xiàn)其高精度指向控制必要前提。

獲取超靜平臺載荷位姿信息是辨識動力學(xué)參數(shù)首要解決的問題。文獻[4-5]提出采用計算機視覺圖像處理算法獲取Stewart超靜平臺的位移和姿態(tài)信息。這種信息獲取方法僅適用于Stewart平臺位移和姿態(tài)估計精度要求較低的場合。而應(yīng)用于天文觀測等航天任務(wù)的精密跟瞄Stewart平臺，其要求運動范圍小精度高，此時采用視覺測量獲取載荷位姿具有以下局限性：1)很難獲得攝像機坐標(biāo)系與平臺基坐標(biāo)系之間的廣義轉(zhuǎn)移矩陣，2)無法精確實現(xiàn)攝像機坐標(biāo)系位姿向基坐標(biāo)系位姿轉(zhuǎn)化[4]。文獻[6]提出一種基于加速度測量的兩剛體間相對姿態(tài)信息估計方法。該方法在僅有加速度可觀測時，采用非線性Kalman濾波實現(xiàn)剛體間相對角速度和角加速度的估計。由于缺乏姿態(tài)測量信息，該方法無法對估計的角速度偏差進行修正，造成角速度及積分獲得的姿態(tài)估計誤差較大。文獻[7]在研究火星進入過程中平動測量受限時，采用擴展Kalman濾波(EKF)估計加速度計零偏以及載荷的平動位移、速度信息。該算法需要精確獲得航天器姿態(tài)和角速度信息以及迭代估計過程中的各個零偏之間協(xié)方差信息。

設(shè)計解耦方法是Stewart超靜平臺參數(shù)辨識另一個需要解決的問題。Stewart超靜平臺屬于多輸入多輸出(MIMO)系統(tǒng)，平臺各通道之間耦合特性復(fù)雜，不利于超靜平臺控制器設(shè)計。通過模型解耦實現(xiàn)多輸入多輸出系統(tǒng)轉(zhuǎn)變?yōu)閱屋斎雴屋敵?SISO)系統(tǒng)，將極大的簡化控制器的設(shè)計。文獻[8-10]針對Stewart平臺高耦合動力學(xué)模型，提出了一種基于關(guān)節(jié)空間質(zhì)量-慣量解耦法，實現(xiàn)了Stewart平臺MIMO模型到SISO模型的轉(zhuǎn)換。同時采用最小方差約束和對稱正定法辨識解耦模型動力學(xué)參數(shù)，從而提高Stewart平臺載荷控制性能。文獻[11-12]采用矩陣分解方法將Stewart超靜平臺由高度耦合的復(fù)雜多輸入多輸出系統(tǒng)變?yōu)槎鄠€相對簡單的單輸入單輸出系統(tǒng)，從而簡化了控制器的設(shè)計。

本文針對辨識Stewart超靜平臺耦合動力學(xué)參數(shù)需要解決的兩個問題，設(shè)計了1)基于UKF的載荷位姿估計方法，實現(xiàn)了Stewart超靜平臺載荷廣義位移、廣義速度、廣義加速度信息準(zhǔn)確估計；2)基于Schur分解的動力學(xué)模型解耦方法，實現(xiàn)辨識模型中超靜平臺動力學(xué)參數(shù)顯式表達。并將該方法應(yīng)用到Stewart超靜平臺的動力學(xué)參數(shù)辨識中，仿真結(jié)果表明該方法正確可行。

1 問題的提出

1.1 辨識模型描述

針對Stewart超靜平臺動力學(xué)建模問題，國內(nèi)外學(xué)者采用牛頓-歐拉法[13]、Kane[14]等方法給出了考慮不同因素的動力學(xué)模型。這些動力學(xué)模型都可采用多輸入多輸出的二階系統(tǒng)進行描述。而工程實際中主要關(guān)心影響Stewart平臺頻率特性的剛度、阻尼等動力學(xué)參數(shù)。因此本文只考慮Stewart平臺剛度和阻尼等因素進行動力學(xué)建模，系統(tǒng)描述為：

(1)

式中：M=diag(mp,mp,mp,Ix,Iy,Iz,)，mp為載荷的質(zhì)量；Ix，Iy，Iz為載荷三軸主慣量；Ms為Stewart平臺支桿質(zhì)量參數(shù)陣，為對稱正定矩陣。Jp為Stewart支桿對載荷質(zhì)心作用力的雅克比矩陣；f為Stewart平臺六個支桿驅(qū)動力；xp=[r,θ]為載荷的廣義位移，r為載荷平動位移，θ為載荷姿態(tài)。fu為Stewart平臺載荷受到來自星體平臺干擾力；Cpp和Kpp分別為系統(tǒng)合成阻尼和剛度陣，定義為：

(2)

式中，C0=diag(c1,…,c6)，K0=diag(k1,…,k6)，ci、ki(i=1,2,…,6)分別為Stewart平臺第i個支桿的阻尼和剛度系數(shù)。

(3)

式中，Jb為Stewart支桿對星體平臺質(zhì)心作用力的雅克比矩陣；xb為星體平臺廣義位移；Fd為載荷受到廣義外作用力；Fc包含所有的科氏項；G包含所有的重力項。

對式(1)中的Stewart平臺動力學(xué)參數(shù)辨識，需要解決兩個問題。第一：載荷位姿信息確定方法。準(zhǔn)確辨識式(1)的動力學(xué)參數(shù)，需要獲取載荷廣義位移、廣義速度、廣義加速度信息。加速度計能夠測量系統(tǒng)的加速度，同時能夠反映載荷的角速度信息。而且與陀螺相比，加速度計體積小、測量精度高，更適合Stewart平臺這種小范圍運動系統(tǒng)的測量。因此，有必要研究基于加速度測量的載荷位姿估計方法。第二：Stewart平臺耦合模型解耦方法。由式(1)可知，Stewart平臺載荷的位姿信息由六個支桿剛度和阻尼共同作用產(chǎn)生。因此，辨識動力學(xué)參數(shù)需要設(shè)計解耦方法，將耦合MIMO模型轉(zhuǎn)變?yōu)镾ISO模型進行辨識。

1.2 測量系統(tǒng)描述

Stewart超靜平臺測量系統(tǒng)包括三個三軸加速度計A1、A2、A3和姿態(tài)敏感器。加速度計用于測量Stewart超靜平臺載荷平動信息。姿態(tài)敏感器測量Stewart超靜平臺載荷姿態(tài)。如圖1所示，三個加速度計呈立體幾何安裝，三個加速度計測點在Stewart下平面坐標(biāo)系Ob-XYZ(原點為下平面中心點Ob)的矢量距為ri(i=1,2,3)。

考慮Stewart平臺載荷面安裝i個三軸加速度計，則第i個三軸加速度計測量模型為：

(4)

式中，hi為第i個加速度計的三軸測量輸出，Tib為Stewart平臺載荷坐標(biāo)系到第i個加速度計本體坐標(biāo)系的方向余弦陣，α=[αx,αy,αz]T為載荷的x,y,z方向的平動加速度，ω=[ωx,ωy,ωz]T為載荷三軸角速度；=[x，y，z]T為載荷三軸角加速度；ηi=[ηix，ηiy，ηiz]T為第i個加速度計的測量噪聲，為均值為0，方差σu的高斯白噪聲；ω×為ω的反對稱陣，定義為：

(5)

2 系統(tǒng)測量模型

2.1 觀測系統(tǒng)建模

(6)

式中，I3×3為3×3的單位陣。

取三個加速度計測點矢量距分別為r1=[1,0,1],r2=[0,1,1],r3=[1,1,0]時，且方向余弦陣Ti=I3，則由式(4)知，由加速度計組成的測量系統(tǒng)的觀測方程為：

(7)

考慮加速度計安裝偏差，以及采用加速度計估計載荷角速度存在偏差等因素，需要在采用UKF估計載荷位姿信息基礎(chǔ)上，采用序貫估計方法，引入姿態(tài)修正UKF估計載荷角速度偏差，提高載荷角速度收斂速度和估計精度。載荷角速度等效偏差參數(shù)估計及載荷角速度修正方法如下：

載荷運動學(xué)模型為：

(8)

(9)

δx=A2δx+B2u2

(10)

當(dāng)采樣時間為ΔT時，對測量系統(tǒng)式(6)、式(7) 的連續(xù)模型進行離散化，從而方便濾波器設(shè)計。式(6)的測量系統(tǒng)離散化狀態(tài)方程為：

x(k+1)=Φ1x(k)+Γ1u1

(11)

式(7)測量系統(tǒng)離散觀測方程為：

h(x(k-1))=[h1(x(k-1)) …h(huán)9(x(k-1))]T

(12)

式(10)誤差狀態(tài)方程離散形式為：

δx=Φ2δx+Γ2u2

(13)

測量系統(tǒng)離散模型的狀態(tài)轉(zhuǎn)移陣和輸入陣定義為：

2.2 系統(tǒng)可觀性分析

由系統(tǒng)可觀性定義可知，如果系統(tǒng)可觀測，則系統(tǒng)的狀態(tài)能夠通過有限觀測量和輸入量唯一確定。對于線性系統(tǒng)來說，系統(tǒng)可觀為全局可觀，系統(tǒng)中每個狀態(tài)量都可以唯一確定。而離散系統(tǒng)式(11)中狀態(tài)方程為線性模型，觀測方程為非線性模型。對于非線性模型，需要構(gòu)建可觀性空間。通過可觀性空間的秩準(zhǔn)則進行可觀性分析。采用李導(dǎo)數(shù)的方法構(gòu)建系統(tǒng)的可觀性空間為[6][15]：

(14)

通過可觀性空間O對狀態(tài)x求導(dǎo)，構(gòu)建可觀性矩陣為：

(15)

通過判斷可觀性矩陣OM的秩分析系統(tǒng)的可觀性。記：

(16)

式(16)中的子矩陣為：

對式(16)中OM1的每列分析可知，對于任意狀態(tài)量x(x≠0)，矩陣OM1每一列是線性無關(guān)矢量，即rank(OM1)=9；

記

(17)

(18)

由式(16)、式(17)和式(18)可知，對于任意狀態(tài)量x(x≠0)，構(gòu)成觀測矩陣OM的每一列為線性無關(guān)矢量，即rank(OM)=9。表明采用三個加速度計和姿態(tài)敏感器組成的測量系統(tǒng)能夠?qū)崿F(xiàn)Stewart超靜平臺載荷的平動和轉(zhuǎn)動信息全觀測。

2.3 基于UKF載荷位姿估計方法

無跡Kalman濾波摒棄了對非線性函數(shù)線性化的做法，采用無跡變換(UT)處理系統(tǒng)均值和協(xié)方差的非線性傳遞問題，沒有忽略高階項的誤差，因此，UKF與EKF相比，估計精度更高。針對式(11)和式(13)的載荷位姿確定系統(tǒng)，文中采用UKF估計Stewart超靜平臺載荷平動和轉(zhuǎn)動信息。如圖2所示，采用序貫估計方法實現(xiàn)載荷的廣義位移、廣義速度和廣義加速度準(zhǔn)確估計。其中，第1-3步為標(biāo)準(zhǔn)的UKF估計過程。圖2右側(cè)中第4步采用姿態(tài)信息修正UKF估計載荷角速度偏差參數(shù)，進一步提高載荷姿態(tài)和角速度估計精度。

(19)

式中，qm為姿態(tài)敏感器測量的載荷姿態(tài)。Δqv為誤差四元素Δq的矢量部分。誤差系統(tǒng)的觀測陣H2=[I3×303×3]。

3 動力學(xué)參數(shù)辨識

Stewart超靜平臺控制器帶寬設(shè)計與自身剛度、阻尼等系數(shù)息息相關(guān)。Stewart超靜平臺支桿剛度、阻尼系數(shù)等受到熱變形、外接線纜等的附加剛度等因素影響，使得Stewart超靜平臺支桿剛度、阻尼系數(shù)理論值與實際值差別較大。因此，辨識Stewart超靜平臺剛度、阻尼系數(shù)等動力學(xué)參數(shù)，有利于提高Stewart超靜平臺控制器性能。

為準(zhǔn)確辨識動力學(xué)參數(shù)，需要獲取載荷廣義位移、速度、加速度信息。通過上節(jié)的UKF實現(xiàn)Stewart載荷平動加速度α，載荷姿態(tài)θ、角速度ω、角加速度的估計。通過對平臺加速度α的積分，可得到平動速度v以及平動位移r。則，式(1)中的模型中廣義加速度、廣義速度、廣義位移都可由UKF估計得到。Stewart超靜平臺動力學(xué)模型(1)中的剛度、阻尼等動力學(xué)參數(shù)與載荷位姿存在相互耦合，需要對動力學(xué)模型(1)進行解耦。令則動力學(xué)模型(1)改寫為：

(20)

(21)

由上式可知，Mp為對稱矩陣。采用Schur分解方法對Mp分解，則有：

Mp=V·u·VT

(22)

其中，V為單位正交陣，且滿足VVT=VTV=I3。U為對角陣。由式(2)可知，剛度和阻尼矩陣K0，C0為對角陣。則式(20)中的矩陣Mp，K0，C0經(jīng)過Schur分解可以轉(zhuǎn)換為相互獨立的對角陣。其中，VTMpV=u，VTK0V=K0，VTC0V=C0。

定義解耦模型中新的輸入輸出如下：

(23)

將上式(23)代入式(20)，則原系統(tǒng)的耦合動力學(xué)模型可轉(zhuǎn)換為：

(24)

顯然，上式中u=diag(u1，…，u6)，K0，C0都為對角陣。即采用Schur分解進行模型解耦，原動力學(xué)模型式(1)轉(zhuǎn)換為SISO時不變系統(tǒng)。從而實現(xiàn)辨識模型中動力學(xué)參數(shù)顯示表達。

定義辨識模型中狀態(tài)量為Θ=[u1,…,u6,c1,…,c6，k1,…,k6]，將上述SISO模型式(24)改寫為：

(25)

通過Schur方法解耦，式(25)實現(xiàn)了辨識Stewart平臺剛度、阻尼等動力學(xué)參數(shù)的顯式表達，采用遞推最小二乘法，進行動力學(xué)參數(shù)辨識。

4 仿真校驗

為了驗證基于UKF的Stewart平臺的姿態(tài)、角速度、角加速度估計的正確性，進行采用加速度計結(jié)合姿態(tài)敏感器測量信息的載荷位姿估計數(shù)學(xué)仿真。仿真參數(shù)見表1。在0-10 s進行Stewart超靜平臺載荷穩(wěn)態(tài)控制。在10 s以后進行Stewart超靜平臺載荷沿x軸的姿態(tài)小角度機動和x方向的平動位移機動。在載荷同時具有平動和轉(zhuǎn)動機動模式下，以載荷平動加速度、姿態(tài)角速度等估計誤差為指標(biāo)，校驗UKF估計的正確性。

表1 仿真參數(shù)Table 1 Simulation parameters

采用上述UKF估計器能夠?qū)崿F(xiàn)Stewart超靜平臺載荷平動信息(包括加速度、速度、位移)和姿態(tài)信息(包括姿態(tài)角加速度、角速度、角位移)準(zhǔn)確估計。采用最小二乘法進行Stewart平臺動力學(xué)參數(shù)辨識。其中，Stewart平臺支桿剛度系數(shù)直接關(guān)系著平臺的結(jié)構(gòu)頻率，因此，辨識參數(shù)中主要以剛度系數(shù)為主。圖7給出了Stewart平臺6個支桿剛度辨識誤差百分比仿真結(jié)果。由圖可知，六個支桿的剛度辨識誤差小于0.01。

5 結(jié) 論

針對辨識Stewart超靜平臺耦合動力學(xué)參數(shù)需要獲取載荷廣義位移、速度、加速度信息的問題，文中提出了一種基于加速度計結(jié)合姿態(tài)敏感器估計載荷位姿的方法。在建立測量系統(tǒng)狀態(tài)模型和觀測模型的基礎(chǔ)上，采用UKF估計載荷平動加速度、角加速度、角速度、姿態(tài)信息。其載荷角速度估計誤差優(yōu)于0.005(°/s)。以UKF估計的載荷位姿信息為依據(jù)，采用最小二乘辨識超靜平臺耦合模型動力學(xué)參數(shù)，其六個支桿的剛度系數(shù)辨識誤差小于0.01。

[1] Bronowicki A J. Vibration isolator for large space telescopes [J]. Journal of Spacecraft and Rockets, 2006, 43(01): 45-43.

[2] 徐高楠, 黃海, 李偉鵬, 等. 空間撓性結(jié)構(gòu)的Stewart超靜平臺主動基座振動控制[J]. 宇航學(xué)報, 2015, 36(4): 439-445. [Xu Gao-nan, Huang Hai, Li Wei-peng, et al. Active vibration control of space flexible structure using Stewart platform as active base [J]. Journal of Astronautics, 2015, 36(4): 439-455.]

[3] 王萍萍, 劉磊. 柔性航天器高精度隔振與定向研究[J]. 宇航學(xué)報, 2012, 23(09): 1195-1202. [Wang Ping-ping, Liu Lei. Research on high accuracy pointing of flexible spacecraft with Stewart platform [J]. Journal of Astronautics, 2012, 23(09): 1195-1202.]

[4] 馬瑾, 張國峰, 戴樹嶺, 等. 相對視覺位姿的Stewart超靜平臺運動學(xué)參數(shù)標(biāo)定[J]. 系統(tǒng)仿真學(xué)報, 2016, 28(9): 2267-2274. [Ma Jin, Zhang Guo-feng, Dai Shuling, et al. Kinematic calibration of Stewart platform based on relative visual pose [J]. Journal of System Simulation, 2016, 28(9): 2267-2274.]

[5] Chevailer A, Copot C, Ionescu C M. Automatic calibration with robust control of a six DOF mechatronic system [J]. Mechatronics, 2016, 35:102-108.

[6] Schopp P, Graf H, Maurer M, et al. Observing relative motion with three accelerometer triads [J]. IEEE Transactions on Instrumentation and Measurement, 2014, 63(12): 3137-3151.

[7] Yu Z S,Crassidis J C. Accelerometer bias calibration using attitude and angular velocity information [J]. Journal of Guidance, Control, and Dynamic, 2016, 39(04): 741-753.

[8] Chen Y,McInroy, J E. Decoupled control of flexure-jointed hexapods using estimated joint-space mass-inertia matrix [J]. IEEE Transaction on Control Systems Technology, 2004, 12(3): 413-421.

[9] McInroy, J E. Modeling and design of flexure jointed Stewart platform for control purposes [J]. IEEE/ASME Transaction on mechatronics, 2002, 7(1): 95-99.

[10] Mcinroy J E, Hamann J C. Design and control of flexure jointed hexapods [J]. IEEE Transactions on robotics and automation, 2000, 16(4): 372-381.

[11] 王有懿, 湯亮, 何英姿. 一種超靜平動主動指向容錯控制方法[J]. 宇航學(xué)報, 2016, 37(12): 1457-1465. [Wang You-yi, Tang Liang, He Yin-zhi . Fault-tolerant control method for active pointing of ultra-quiet platform [J]. Journal of Astronautics, 2016, 37(12), 1457-1465.]

[12] 王曉雷, 楊慶俊, 鄭鋼鐵. 八作動器隔振平臺的通道耦合分析及解耦控制[J]. 宇航學(xué)報, 2007, 28(04): 245-249. [Wang Xiao-lei, Yang Qin-jun, Zheng Gang-tie. Decoupling analysis and decoupling control of vibration isolation platform with eight actuators [J]. Journal of Astronautics, 2007, 28(04), 245-249.]

[13] 劉磊, 王萍萍, 孔憲仁, 等. Stewart 平臺動力學(xué)建模及魯棒主動隔振控制[J]. 宇航學(xué)報, 2011, 32(06): 1231-1238. [Liu Lei, Wang Ping-ping, Kong Xian-ren. Dynamics modeling and robust active isolation control of Stewart platform [J]. Journal of Astronautics, 2011, 32(06): 1231-1238.]

[14] Yang J F,Xu Z B, Wu Q W, et al. Dynamics modeling and control of a 6-dof micro-vibration simulator. Mechanism and Machine Theory, 2016, 104: 350-369.

[15] Ning X L, Wang F, Fang J C. Implicit UKF and its observability analysis of satellite stellar refraction navigation [J]. Aerospace Science and Technology, 2016, 54: 49-58

ParameterIdentificationforUltraquietPlatformCouplingModelBasedonUKF

ZHANG Ke-bei1, WANG Da-yi2, WANG You-yi1

(1. Beijing Institute of Control Engineering, Beijing 100190, China; 2. Beijing Institute of Spacecraft Engineering, Beijing 100190, China)

A method based on Schur decomposition and unscented Kalman filter (UKF) is designed to identify the parameters of the Stewart coupling model. The state model, measurement model and observability are given. The UKF is adopted to determine the payload position and attitude. The attitude measurements are used in the UKF to correct the angular velocity error so that the estimation accuracy and convergence speed of the angular velocity can be improved. The payload generalized displacement, generalized velocity and generalized acceleration should be obtained. The parameter decoupling method is designed and the least squares method is used to identify the dynamics parameters. Numerical simulations results indicate that the angular velocity is estimated accurately by the UKF and the stiffness identification errors of the Stewart six struts are less than 0.01.

Stewart ultraquiet platform; Position and attitude determination; Unscented Kalman filter (UKF); Parameter identification

2017- 06- 15;

2017- 09- 25

國家杰出青年科學(xué)基金(61525301)；國家自然科學(xué)基金重大項目課題(61690215)

TB535

A

1000-1328(2017)12- 1289- 08

10.3873/j.issn.1000- 1328.2017.12.005

張科備(1985-)，男，博士生，主要從事航天器姿態(tài)高精高穩(wěn)控制。

通信地址：北京市中關(guān)村南三街北京控制工程研究所(100190)

E-mail:zhangkb.2008@163.com