尹 劍，高教波，張琬琳，陳 紅

（西安應(yīng)用光學(xué)研究所，陜西 西安710065）

概述

某三軸可旋轉(zhuǎn)光電偵查載體平臺(tái)在工作時(shí)，發(fā)射一束指向光束，該光束可以在載體平臺(tái)平面上進(jìn)行全周轉(zhuǎn)動(dòng)，旋轉(zhuǎn)的角度可以測(cè)得。光電平臺(tái)在進(jìn)行后續(xù)定位計(jì)算及圖像處理時(shí)，需要知道該光速的姿態(tài)角，即該光束的坐標(biāo)在地理坐標(biāo)系下的方位軸轉(zhuǎn)過角度和俯仰軸旋轉(zhuǎn)的角度。

這個(gè)問題可以用坐標(biāo)系加以描述，如圖1所示，地理坐標(biāo)系（g系）采用東、北、天坐標(biāo)系標(biāo)記為E、N、D；載體坐標(biāo)系（b系）標(biāo)記為X、Y、Z，其中，Y沿機(jī)體縱向軸向前，X沿機(jī)體橫軸指向右，Z與X、Y構(gòu)成右手定則沿機(jī)體垂直向上；光束坐標(biāo)系（u系）標(biāo)記為X1、Y1、Z，其中光束坐標(biāo)系的Z軸與載體坐標(biāo)系的Z軸為同一個(gè)軸，且光束坐標(biāo)系只圍繞Z軸在載體X、Y軸平面的旋轉(zhuǎn)。如圖1所示，α角即為光束坐標(biāo)系繞Z軸轉(zhuǎn)過的角度。

圖1 坐標(biāo)示意圖Fig.1 Coordinates system

圖2對(duì)實(shí)體平臺(tái)進(jìn)行坐標(biāo)系抽象，其中ψ、θ、γ分別為載體平臺(tái)坐標(biāo)系航向角、俯仰角和橫滾角，也就是載體坐標(biāo)系相對(duì)于地理坐標(biāo)系3個(gè)軸旋轉(zhuǎn)的角度；Ψ1、θ1就是光束的坐標(biāo)系相對(duì)于地理坐標(biāo)系的兩軸旋轉(zhuǎn)角度，分別為航向角、俯仰角即光束的姿態(tài)角。

圖2 旋轉(zhuǎn)角度示意Fig.2 Diagram of circumrotation angle

1 采用四元數(shù)法算法

1.1 算法描述

四元數(shù)是一個(gè)由4個(gè)元構(gòu)成的數(shù)，其形式為

在剛體定點(diǎn)轉(zhuǎn)動(dòng)理論中，根據(jù)歐拉定理，動(dòng)坐標(biāo)系相對(duì)參考坐標(biāo)系的方位，等效于動(dòng)坐標(biāo)系繞某一個(gè)等效轉(zhuǎn)軸轉(zhuǎn)動(dòng)一個(gè)角度θ。如果用u表示等效轉(zhuǎn)軸方向的單位向量，則動(dòng)坐標(biāo)系的方位完全由u和θ兩個(gè)參數(shù)確定。用u和θ可構(gòu)造一個(gè)四元數(shù)如轉(zhuǎn)動(dòng)后的矢量用r′表示，則以四元數(shù)描述的r′和r的坐標(biāo)變換關(guān)系為，其中

根據(jù)這個(gè)性質(zhì)，可用四元數(shù)表示地理坐標(biāo)系到光束坐標(biāo)系的變換關(guān)系：

設(shè)載體平臺(tái)四元數(shù)為

根據(jù)四元數(shù)性質(zhì)，由于光束繞Z軸旋轉(zhuǎn)角度為α，可知旋轉(zhuǎn)四元數(shù)為Q2＝（p0，p1，p2，p3）T，式中：p0cos（α／2）；p1＝0；p2＝0；p3＝－sin（α／2）。

于是，旋轉(zhuǎn)后的四元數(shù)為

Q3＝（r0，r1，r2，r3）T

其中Q3＝Q1?Q2，展開成矩陣形式為

則Q3就是光束的四元數(shù)，利用（1）式，根據(jù)這個(gè)四元數(shù)求得光束坐標(biāo)姿態(tài)矩陣，然后利用這個(gè)姿態(tài)矩陣進(jìn)行姿態(tài)角轉(zhuǎn)換如下，

就可以求出光束坐標(biāo)系的姿態(tài)值。其中，（1）式和（2）式見文獻(xiàn)［1］。

1.2 算法仿真

利用四元數(shù)算法，模擬平臺(tái)水平和側(cè)傾90°兩種情況時(shí)光束坐標(biāo)繞Z軸從0°到360°進(jìn)行旋轉(zhuǎn)，計(jì)算結(jié)果與預(yù)期值一致。

當(dāng)光電平臺(tái)水平時(shí)，光束坐標(biāo)Z軸從0°到360°進(jìn)行旋轉(zhuǎn)，四元數(shù)算法求得光束的姿態(tài)值，如圖3所示。

當(dāng)光電平臺(tái)橫滾90°時(shí)，光束坐標(biāo)Z軸從0°到360°進(jìn)行旋轉(zhuǎn)，用四元數(shù)算法求得光束的姿態(tài)值，如圖4所示。

圖3 平臺(tái)水平時(shí)四元數(shù)算法光束姿態(tài)仿真Fig.3 Quaternion algorithm simulation for horizontal platform

圖4 平臺(tái)側(cè)傾90度時(shí)四元數(shù)算法光束姿態(tài)仿真Fig.4 Quaternion algorithm simulation for vertical platform

2 采用坐標(biāo)變換算法

2.1 算法描述

根據(jù)上述該問題的坐標(biāo)變換描述，地理坐標(biāo)系（g系）繞負(fù)D轉(zhuǎn)ψ角，繞E軸轉(zhuǎn)θ角，再繞N軸轉(zhuǎn)γ角，得到光電平臺(tái)機(jī)體坐標(biāo)系（b系），利用坐標(biāo)變換算法，其姿態(tài)矩陣為

在平臺(tái)坐標(biāo)系（b系）的基礎(chǔ)上，光束坐標(biāo)系繞Z軸繼續(xù)旋轉(zhuǎn)角度為α，得到光束坐標(biāo)系（u系），變換算法如下：

其中，

則有：

根據(jù)（2）式就可以得到光束姿態(tài)值如下：

2.2 算法仿真

利用坐標(biāo)變換算法，模擬平臺(tái)水平和側(cè)傾90°兩種情況時(shí)光束坐標(biāo)繞Z軸從0°到360°進(jìn)行旋轉(zhuǎn)，當(dāng)光電平臺(tái)水平時(shí)，光束坐標(biāo)Z軸從0°到360°進(jìn)行旋轉(zhuǎn)，用坐標(biāo)變換算法求得光束的姿態(tài)值，如圖5所示。

圖5 平臺(tái)水平時(shí)坐標(biāo)變換算法光束姿態(tài)仿真Fig.5 Coordinate transformation algorithm simulation for horizontal platform

圖6 平臺(tái)側(cè)傾90°時(shí)坐標(biāo)變換算法光束姿態(tài)仿真Fig.6 Coordinate transformation algorithm simulation for vertical platform

當(dāng)光電平臺(tái)橫滾90°時(shí)，光束坐標(biāo)Z軸從0°到360°進(jìn)行旋轉(zhuǎn)，用坐標(biāo)變換算法求得光束的姿態(tài)值，如圖6所示。

3 結(jié)論

針對(duì)某三軸可旋轉(zhuǎn)光電偵查平臺(tái)上光束旋轉(zhuǎn)的姿態(tài)計(jì)算問題，分別采用了四元數(shù)算法和坐標(biāo)變換算法進(jìn)行了推算。經(jīng)過仿真驗(yàn)證，這兩種算法都能夠得到正確結(jié)果。相比較而言，坐標(biāo)變換算法形式簡(jiǎn)單，便于理解和實(shí)現(xiàn)。但是，坐標(biāo)變換算計(jì)算過程中大量使用三角函數(shù)計(jì)算，而三角函數(shù)計(jì)算在工程實(shí)現(xiàn)時(shí)采用級(jí)數(shù)展開的方式，展開的級(jí)數(shù)的多少因計(jì)算機(jī)及處理器的不同而有差異，因此計(jì)算量大，實(shí)時(shí)性差；而四元數(shù)算法使用的三角函數(shù)計(jì)算很少，從而更易于工程應(yīng)用。

［1］ Qing Yongyuan.Stapdown inertial navigation［M］.Xi'an：Northwestern Polytechnial University Press，2006.秦永元.慣性導(dǎo)航［M］.西安：西北工業(yè)大學(xué)出版社，2006.

［2］ Yang Peigeng，Gong Zhibing.Photoelectricity inertial technology ［M］.Beijing：Ordnance Industry Press.1999：195－199.楊培根，龔智炳.光電慣性技術(shù)［M］.北京：兵器工業(yè)出版社，1999：195－199.

［3］ Deng Zhenglong.Inertial technology［M］.Harbin：Harbin Institute of Technology Press， 2006：140－180.鄧正隆.慣性技術(shù)［M］.哈爾濱：哈爾濱工業(yè)大學(xué)出版社，2006：140－180.

［4］ Hu Xiaoping.Self navigation theory and application［M］.Changsha：National University of Defense Technology Press，2002.胡小平.自主導(dǎo)航理論與應(yīng)用［M］.長(zhǎng)沙：國防科技大學(xué)出版社，2002.

［5］ Jin Yimin，Tao Zhong，Sun Zhijia.Mathematical simulation and shafting errors of shafting movement ［J］.Journal of Applied Optics，2003，24（S）：37－46.金毅民，陶忠，孫治家.軸系運(yùn)動(dòng)的數(shù)學(xué)仿真與軸系誤差 ［J］.應(yīng)用光學(xué)，2003，24（S）：37－49.

［6］ Zhang Ronghui，Jia Hongguang，Chen Tao，et al.Attitude solution for strapdown inertial navigation system based on quaternion algorithm ［J］.Optics and Precision Engineering，2008，16（10）：1964－1970.張榮輝，賈宏光，陳濤，等.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算［J］.光學(xué)精密工程，2008，16（10）：1964－1970.

［7］ Li Xiaoguang，Wang Zhaonan，Wang Zhi，et al.Research on location compute method of object［J］.Optics and Precision Engineering，2006，14（6）：1076－1081.李曉光，王兆楠，王智，等.一種目標(biāo)位置解算方法［J］.光學(xué)精密工程，2006，14（6）：1076－1081.

［8］ Zhang Wanlin，Yin Jian，Guo Shuanyun.Temperature compensation for Q－MEMS gyro［J］.Journal of Applied Optics，2010，31（4）：549－552.張琬琳，尹劍，郭栓運(yùn)，Q－MEMS陀螺零偏補(bǔ)償技術(shù)研究［J］.應(yīng)用光學(xué)，2010，31（4）：549－552.

［9］ Liu Junfeng.Three dimensional rotation represented by quaternion［J］.College Physics，2004，23（4）：39－43.劉俊峰.三維轉(zhuǎn)動(dòng)的四元數(shù)表述［J］.大學(xué)物理，2004，23（4）：39－43.

［10］Liu Haiying，Wang Huinan，Liu Xinwen.Attitude measurementising quaternion based on UKF［J］.Journal of Nanjing University of Aeronautics ＆ Astronautics，2006，38（1）：37－42.劉海穎，王惠南，劉新文.基于UKF的四元數(shù)載體姿態(tài)確定［J］.南京航空航天大學(xué)學(xué)報(bào)，2006，38（1）：37－42.

［11］Yang Yanjuan.Study on the crucial technology of SINS［D］.Harbin：Harbin Engineering University，2001.楊艷娟.捷聯(lián)慣性導(dǎo)航系統(tǒng)關(guān)鍵技術(shù)研究［D］.哈爾濱：哈爾濱工程大學(xué)，2001.