光電穩(wěn)瞄系統(tǒng)的瞄準(zhǔn)線姿態(tài)補(bǔ)償算法

任元斌，吳玉敬，韓 瑞，王 璞，劉小強(qiáng)

(西安應(yīng)用光學(xué)研究所, 西安 710065)

瞄準(zhǔn)線穩(wěn)定與跟蹤系統(tǒng)(光電穩(wěn)瞄系統(tǒng)[1])是多種光電成像系統(tǒng)的核心裝備，可以完成對(duì)地觀測(cè)，圖像信息收集，測(cè)繪等任務(wù)。它通常固連在其所在的載體上(飛機(jī)，艦船等)，能抵抗外部擾動(dòng)并保持系統(tǒng)內(nèi)部成像傳感器慣性穩(wěn)定。

光電穩(wěn)瞄系統(tǒng)的穩(wěn)定和瞄準(zhǔn)功能實(shí)現(xiàn)的核心在于瞄準(zhǔn)線的慣性穩(wěn)定和準(zhǔn)確指向。目前光電穩(wěn)瞄系統(tǒng)是通過在系統(tǒng)的內(nèi)框架安裝陀螺儀，用陀螺儀敏感陀螺軸上的角速度，并以此作為反饋實(shí)現(xiàn)慣性穩(wěn)定。大多數(shù)光電穩(wěn)瞄系統(tǒng)采用雙軸陀螺，分別敏感其方位和俯仰方向的角速度信息。但當(dāng)光電穩(wěn)瞄系統(tǒng)所搭載的載體發(fā)生三軸角運(yùn)動(dòng)和線運(yùn)動(dòng)時(shí)，其雙軸陀螺將無法完全獲取全部的運(yùn)動(dòng)信息，因此無法保證瞄準(zhǔn)線的準(zhǔn)確指向。此時(shí)就需要結(jié)合光電穩(wěn)瞄系統(tǒng)的載體慣導(dǎo)信息，去計(jì)算光電穩(wěn)瞄系統(tǒng)的正確指向。

之前的瞄準(zhǔn)線補(bǔ)償算法，大多是對(duì)地理系下東向和北向軸的載體線性運(yùn)動(dòng)速度進(jìn)行補(bǔ)償，并未將載體發(fā)生的三軸角運(yùn)動(dòng)和線運(yùn)動(dòng)信息全部包含進(jìn)去。故適用范圍有限，在載體頻繁做三軸角運(yùn)動(dòng)時(shí)和所在高度劇烈變化時(shí)誤差較大。且傳統(tǒng)的通過慣導(dǎo)數(shù)據(jù)計(jì)算瞄準(zhǔn)線指向的方法是歐拉旋轉(zhuǎn)法，該方法運(yùn)算復(fù)雜，且存在奇異點(diǎn)，適用性不強(qiáng)。

本研究從剛體一般螺旋運(yùn)動(dòng)理論出發(fā)，結(jié)合光電穩(wěn)瞄系統(tǒng)所在載體的慣導(dǎo)數(shù)據(jù)，使用四元代數(shù)描述了光電穩(wěn)瞄系統(tǒng)隨載體的運(yùn)動(dòng)。為使瞄準(zhǔn)線方向不受載體運(yùn)動(dòng)影響，求解了一次一般螺旋運(yùn)動(dòng)后光電穩(wěn)瞄系統(tǒng)所需調(diào)轉(zhuǎn)的三軸姿態(tài)角度，免去了使用歐拉旋轉(zhuǎn)帶來的復(fù)雜矩陣運(yùn)算，易于工程實(shí)現(xiàn)。為三軸光電穩(wěn)瞄系統(tǒng)提供了新的理論計(jì)算視角，拓寬了瞄準(zhǔn)線姿態(tài)補(bǔ)償方法的適用范圍。

1 基礎(chǔ)理論

四元數(shù)[2-3]可看成是對(duì)復(fù)數(shù)的擴(kuò)展，一個(gè)四元數(shù)q的定義如下：

q=a+bi+cj+dk

(1)

其中i2=j2=k2=-1，若a2+b2+c2+d2=1，q稱為單位四元數(shù)。

記Qu為全體單位四元數(shù)組成的集合，Qu與酉群SU2同構(gòu)，且是SO3群的雙覆蓋。SO3群是空間中所有姿態(tài)旋轉(zhuǎn)變換組成的集合。

對(duì)偶四元數(shù)[4]定義為

(2)

其中：p和q都是四元數(shù)，p稱為實(shí)部，q為對(duì)偶部；ε為對(duì)偶算子，ε2=0且ε≠0。

所有單位對(duì)偶四元數(shù)組成的集合DQu是剛體姿態(tài)變換群SE3的雙覆蓋。SE3群是空間中所有姿態(tài)旋轉(zhuǎn)和平移變換組成的集合。

(3)

sinθ=2(q0q2-q1q3)

(4)

(5)

四維球面線性插值[6]是在兩個(gè)單位四元數(shù)q1和q2之間的弧線函數(shù)，插值公式為

(6)

2 問題描述

站心坐標(biāo)系Ot-XtYtZt：是與大地表面固連的地理坐標(biāo)系，以靶目標(biāo)觀測(cè)點(diǎn)處為原點(diǎn)Ot。Xt，Yt，Zt分別指向東向，北向，天向。

載體導(dǎo)航坐標(biāo)系Ob-XbYbZb：是與光電穩(wěn)瞄系統(tǒng)所搭載載體固連的地理坐標(biāo)系，以載體質(zhì)心為原點(diǎn)Ob。Xb，Yb，Zb分別指向東向，北向，天向。通常載體導(dǎo)航系的質(zhì)心和光電穩(wěn)瞄系統(tǒng)的質(zhì)心不在同一位置，此處為簡(jiǎn)化處理，假定載體和光電穩(wěn)瞄系統(tǒng)為同一質(zhì)心。

瞄準(zhǔn)線坐標(biāo)系Ol-XlYlZl：是與光電穩(wěn)瞄系統(tǒng)質(zhì)心固連的坐標(biāo)系，以光電穩(wěn)瞄系統(tǒng)質(zhì)心為原點(diǎn)Ol，Xl軸為以質(zhì)心為出發(fā)點(diǎn)，與瞄準(zhǔn)線重合的射線，與光電穩(wěn)瞄系統(tǒng)的橫滾軸重合，Yl，Zl分別為光電穩(wěn)瞄系統(tǒng)的俯仰和偏航軸。

不失一般性假設(shè)瞄準(zhǔn)線坐標(biāo)系初始時(shí)刻和載體導(dǎo)航坐標(biāo)系Ob-XbYbZb相互重合。隨后經(jīng)歷一次旋轉(zhuǎn)，得到初始時(shí)刻的瞄準(zhǔn)線坐標(biāo)系Ol-XlYlZl，如圖1所示，此時(shí)瞄準(zhǔn)線指向站心坐標(biāo)系的原點(diǎn)Ot。

圖1 瞄準(zhǔn)線變化示意圖

3 問題分析和求解

(7)

由式(8)可以解得旋轉(zhuǎn)過程的姿態(tài)四元數(shù)

(8)

寫成矩陣形式為

(9)

其中ωx，ωy，ωz為慣導(dǎo)所測(cè)量的載體坐標(biāo)系三軸的角速度。對(duì)此過程q進(jìn)行歸一化為單位四元數(shù)，即得到qr。

(10)

(11)

(12)

(13)

(14)

導(dǎo)航坐標(biāo)系下此三點(diǎn)所成平面的方程為

Ax+By+Cz+D=0

(15)

各系數(shù)根據(jù)行列式求得：

記F=Ax+By+Cz+D

該平面通過原點(diǎn)處的法向量為

(16)

將以上法向量單位化為單位法向量：

(17)

構(gòu)造Grassmenn行列式

(18)

(19)

其中各個(gè)系數(shù)均為Grassmenn行列式的子式，

(20)

根據(jù)plüker坐標(biāo)空間直線所成角度公式

(21)

(22)

該數(shù)用q2表示

(23)

其中

(24)

根據(jù)四元數(shù)到歐拉角的轉(zhuǎn)化公式將qi轉(zhuǎn)化，得到在載體導(dǎo)航系下要使瞄準(zhǔn)線指向原目標(biāo)所需調(diào)轉(zhuǎn)到的三軸姿態(tài)角為[7-13]：

(25)

θt= arcsin(42(qt 0qt 2-qt 1qt 3))

(26)

(27)

若涉及多次一般螺旋運(yùn)動(dòng)可使用上述步驟重復(fù)累計(jì)計(jì)算。

欲尋找q0到qt的最短路徑，可用單位四維球面線性插值[14-15]，初始時(shí)刻q0到qt的四維球面線性插值為

(28)

其中t∈(0,1)，μt為q0與qt所成夾角。

4 計(jì)算實(shí)例

圖2 機(jī)載光電穩(wěn)瞄系統(tǒng)瞄準(zhǔn)線變化示意圖

根據(jù)上節(jié)理論， 計(jì)算得到平移帶來的旋轉(zhuǎn)量，平臺(tái)需要以下單位四元數(shù)表示的旋轉(zhuǎn)量來補(bǔ)償

經(jīng)計(jì)算在載體導(dǎo)航坐標(biāo)系下的穩(wěn)瞄平臺(tái)所需的姿態(tài)調(diào)轉(zhuǎn)到的四元數(shù)表示為

qt=q2°q0=(0.560 1 -0.140 0 0.700 1 -0.420 1)

下面使用Matlab仿真驗(yàn)證上述實(shí)例計(jì)算過程。在(2，1，2)位置時(shí)刻時(shí)，瞄準(zhǔn)線的單位矢量是由單位矢量(1，0，0)(用純虛四元數(shù)表示為(0，1，0，0))經(jīng)過由q0表示的一次旋轉(zhuǎn)得到，旋轉(zhuǎn)后的瞄準(zhǔn)線向量為

Matlab計(jì)算結(jié)果用純虛四元數(shù)表示為：

在(1，2，2)位置時(shí)刻時(shí)，瞄準(zhǔn)線的單位矢量是單位矢量(1，0，0)經(jīng)過由q2°q0表示的姿態(tài)旋轉(zhuǎn)得到，旋轉(zhuǎn)后的瞄準(zhǔn)線向量為

Matlab計(jì)算結(jié)果用純虛四元數(shù)表示為

根據(jù)式(25)、式(26)、式(27)計(jì)算得載體導(dǎo)航坐標(biāo)系下的光電穩(wěn)瞄系統(tǒng)最終所需調(diào)轉(zhuǎn)到三軸姿態(tài)角表示為

ψt=219.5°,θt=41.81°,φt=92.59°

圖3是四維球面線性插值給出的從起始時(shí)刻到末端時(shí)刻的最優(yōu)調(diào)轉(zhuǎn)路徑。圖3 (a)為航向方向姿態(tài)角度的調(diào)轉(zhuǎn)路徑規(guī)劃圖，圖3(b)為俯仰方向姿態(tài)角度的調(diào)轉(zhuǎn)路徑規(guī)劃圖，圖3(c)為橫滾方向姿態(tài)角度的調(diào)轉(zhuǎn)路徑規(guī)劃圖。圖3中橫軸表示采樣步長，縱軸表示姿態(tài)調(diào)轉(zhuǎn)時(shí)三軸姿態(tài)應(yīng)在采樣點(diǎn)同時(shí)調(diào)轉(zhuǎn)到達(dá)所規(guī)劃的角度。

圖3 機(jī)載光電穩(wěn)瞄系統(tǒng)三軸姿態(tài)角度規(guī)劃示意圖

5 結(jié)論

從四元代數(shù)理論角度出發(fā)，求解了光電穩(wěn)瞄系統(tǒng)在隨載體做一般運(yùn)動(dòng)時(shí)的瞄準(zhǔn)線指向問題，提出了一種全方向的瞄準(zhǔn)線補(bǔ)償方法，拓寬了瞄準(zhǔn)線補(bǔ)償方法的適用范圍，并以機(jī)載光電穩(wěn)瞄系統(tǒng)為例，對(duì)該方法進(jìn)行了驗(yàn)證。該方法可應(yīng)用于飛機(jī)，艦船等姿態(tài)穩(wěn)定系統(tǒng)上，僅需結(jié)合載體慣導(dǎo)信息，實(shí)現(xiàn)光電穩(wěn)瞄系統(tǒng)的視軸指向解算。該方法采用四元代數(shù)運(yùn)算，免去了復(fù)雜的矩陣變換，便于實(shí)現(xiàn)程序代碼并將代碼運(yùn)行在嵌入計(jì)算機(jī)內(nèi)重復(fù)迭代計(jì)算。經(jīng)過仿真計(jì)算驗(yàn)證，可知在經(jīng)歷載體運(yùn)動(dòng)后，應(yīng)用本研究的方法能正確計(jì)算系統(tǒng)指向目標(biāo)所需到達(dá)的姿態(tài)角度，通過姿態(tài)調(diào)轉(zhuǎn)達(dá)到消除瞄準(zhǔn)線偏移的目的。

[1] 紀(jì)明.多環(huán)架光電穩(wěn)定系統(tǒng)及分析[J].應(yīng)用光學(xué),1994,15(3):60-64.

[2] CHOU J C K.Quaternion Kinematic and Dynamic Differential Equations [J].IEEE Transactions on Robotics and Automation,1992,8(1):53-64.

[3] GUI Haichao,GEORGE VUKOVICH.Dual-Quaternion-based adaptive motion tracking of spacecraft with reduced control effort[J].Nonlinear Dyn 2016,83:597-614.

[4] WANG Xiangke,YU Changbin.Unit dual Quaternion-based feedback linearization tracking problem for attitude and position dynamics[J].Systems&Control Letters,2013,62:225-233.

[5] WEN J,KREUTZ-DELGADO K.The attitude control problem [J].IEEE Trans.Autom.Control,1991,36(10):1148-1162.

[6] AHN Jin-su,CHUNG Won-jee,JUNG Chang-doo.Realization of orientation interpolation of 6-axis articulated robot using quaternion [J].J.Cent.South Univ.,2012,19(3):3407-3414.

[7] KONSTANTINOS D.Hand-Eye Calibration Using Dual Quaternions [J].The International Journal of Robotics Research,1999,18(3):286-298.

[8] MILOS IEFRANT,VIJAY KUMAR.Interpolation schemes for rigid body motions [J].Computer-Aided Design,1998,30(3):179-189.

[9] WEN Qiuqiu,XIA Qunli,SU Weixia.Bank-to-Turn Guidance Law with Terminal Impact-Angle Constraint [J].Journal of Aerospace Engineering,2016,29(3):04015067.

[10] YANG X L,WU H T,LI Y,et al.A dual quaternion solution to the forward kinematics of a class of six-DOF parallel robots with full or reductant actuation[J].Mechanism and Machine Theory,2017,107:27-36.

[11] QUINN J A.A Complex Quaternion Model for Hyperbolic 3-Space[M].arXiv preprint arXiv:1701.06709,2017.

[12] ERHARD M,HORN G,DIEHL M.A quaternion-based model for optimal control of an airborne wind energy system[J].ZAMM-Journal of Applied Mathematics and Mechanics/Zeitschrift für Angewandte Mathematik und Mechanik,2017,97(1):7-24.

[13] DJAMEL K,ABDELLAH M,BENALLEGUE A.Attitude Optimal Backstepping Controller Based Quaternion for a UAV[J].Mathematical Problems in Engineering,2016(4):1-11.

[14] NEX F,REMONDINO F.UAV for 3D mapping applications:a review[J].Applied Geomatics,2014,6(1):1-15.

[15] 劉小強(qiáng),邢軍智,任元斌,等.基于電流環(huán)的改進(jìn)ADRC在微型光電穩(wěn)瞄系統(tǒng)中的應(yīng)用[J].光電工程,2016,43(7):28-33.