楔形穩(wěn)定平臺的等效機(jī)械臂模型

朱元平，湯心溢，袁修鵬（1. 上海科技大學(xué) 信息學(xué)院，上海，200030；2. 中國科學(xué)院上海技術(shù)物理研究所，上海，200080；3. 中國科學(xué)院紅外探測與成像技術(shù)重點實驗室，上海，200080；4. 上海市現(xiàn)場物證重點實驗室，上海，200080）

楔形穩(wěn)定平臺的等效機(jī)械臂模型

朱元平1，2，3，4，湯心溢1，2，3，4，袁修鵬2，3，4
（1. 上海科技大學(xué) 信息學(xué)院，上海，200030；2. 中國科學(xué)院上海技術(shù)物理研究所，上海，200080；3. 中國科學(xué)院紅外探測與成像技術(shù)重點實驗室，上海，200080；4. 上海市現(xiàn)場物證重點實驗室，上海，200080）

為使運動載體上的觀測儀器相對于大地保持靜止，需要使用穩(wěn)定平臺對運動載體的擾動進(jìn)行補(bǔ)償。楔形穩(wěn)定平臺利用多個堆疊楔塊之間的相對轉(zhuǎn)動，改變頂部平面的傾角以抵消船搖擾動。它具有成本低、精度高、負(fù)載轉(zhuǎn)矩小以及重量輕、能耗低等優(yōu)點。然而楔形穩(wěn)定平臺獨特的結(jié)構(gòu)使得其分析較為困難。因此本文提出一種將三維空間內(nèi)楔塊的轉(zhuǎn)動近似等效成二維空間內(nèi)機(jī)械臂運動的簡化變換，并以此分析比較了雙層、改進(jìn)型雙層、多層等楔形穩(wěn)定平臺的結(jié)構(gòu)，得出改進(jìn)型雙層結(jié)構(gòu)是最佳實踐方案。最后利用旋轉(zhuǎn)矩陣進(jìn)行坐標(biāo)系變換，計算得到改進(jìn)型雙層結(jié)構(gòu)楔形穩(wěn)定平臺的控制函數(shù)。

楔形穩(wěn)定平臺；船搖；等效機(jī)械臂；層級結(jié)構(gòu)；

0 引 言

穩(wěn)定平臺通常用于隔離艦船、車輛等運動載體的擾動，使其裝載的觀測儀器相對于大地坐標(biāo)系保持靜止。楔形穩(wěn)定平臺是一種新型穩(wěn)定平臺。它最先由荷蘭信號公司設(shè)計［1］，現(xiàn)已應(yīng)用于艦載“天狼星”遠(yuǎn)程紅外搜索與跟蹤系統(tǒng)［2］。與陀螺儀式穩(wěn)定平臺不同，楔形穩(wěn)定平臺主要由若干自由轉(zhuǎn)動的楔塊組成。楔塊的轉(zhuǎn)動能改變頂部平面的傾角，若傾角時時與載體搖擺的方向相反，就能抵消擾動。

1 等效機(jī)械臂模型推導(dǎo)

楔塊的轉(zhuǎn)動與頂部平面傾角的變化關(guān)系并不直觀。為了使分析更為形象，在此先引入等效機(jī)械臂的概念。圖 2 中，ns，nv，n1和 n2分別為海平面、艦船和 2 個楔塊上平面的單位法向量。假定 nv靜止，則艦船搖動等效于 ns相對于 nv擺動。再假設(shè)楔塊夾角均為β，則 n1與 nv和 n2與 n1的夾角也是 β。那么當(dāng)楔塊 1旋轉(zhuǎn)時，n1以固定夾角 β 圍繞 nv旋轉(zhuǎn)。同理當(dāng)楔塊 2旋轉(zhuǎn)時，n2以固定夾角 β 圍繞 n1旋轉(zhuǎn)。將上述單位向量平移使它們的起點重合，從而終點位于同一單位球面（見圖 3）。于是三維空間內(nèi)楔塊的旋轉(zhuǎn)可以等效為二維球面空間內(nèi)圓心做相同角度的旋轉(zhuǎn)。

圖 1 “天狼星”系統(tǒng)的楔形穩(wěn)定平臺［3］Fig. 1 The wedge-platform of Sirius IRST

圖 2 海平面、艦船和楔形穩(wěn)定平臺的單位法向量示意圖Fig. 2 Schematic diagram of unit normal vectors of sea level，vessel and wedge-platform

圖 3 海平面、艦船和楔形穩(wěn)定平臺的單位法向量球面圖Fig. 3 Sphere map of unit normal vectors of sea level， vessel and wedge-platform

因為船搖的角度和楔塊的楔角都比較小，所以點Ns，N1，N2均僅在以 Nv為中心的小范圍內(nèi)運動。在不影響定性分析準(zhǔn)確性的前提下，不妨用從北極方向看去的平面投影（簡稱“平面圖”）代替球面圖，同時將球面上的弧近似為平面上的線段。平面圖中 y 軸沿艦船縱軸方向指向船首，x 軸沿橫軸方向指向右舷。Ns在 Nv周圍一定區(qū)域內(nèi)做隨機(jī)運動，線段 ON1圍繞O（Nv）旋轉(zhuǎn)，N1N2圍繞 N1旋轉(zhuǎn)，轉(zhuǎn)角大小等于楔塊轉(zhuǎn)動的角度。根據(jù) 2 個線段的運動形態(tài)，可以將 ON1和 N1N2等效為一個二維自由度機(jī)械臂的上臂和前臂，O 和 N1等效為機(jī)械臂肩關(guān)節(jié)和肘關(guān)節(jié)，N2等效為機(jī) 械手。用等效機(jī)械臂模型來描述楔形穩(wěn)定平臺的工作原理，就是控制機(jī)械臂 ON1N2讓機(jī)械手N2實時跟蹤目標(biāo)點 Ns的運動。

圖 4 平面圖和等效機(jī)械臂Fig. 4 Plane graph and equivalent manipulator

2 楔角和層級結(jié)構(gòu)分析

在設(shè)計楔形穩(wěn)定平臺的機(jī)械結(jié)構(gòu)時，必須考慮楔角和層級結(jié)構(gòu)。楔角主要影響平定平臺的補(bǔ)償范圍，而層級結(jié)構(gòu)則對平臺的補(bǔ)償效果有決定性影響。

2.1 楔塊的楔角

平面圖中等效機(jī)械臂的臂長與楔角大小成正比，同時臂長決定了活動范圍。較大的楔角雖然能增加活動范圍，但也會使整體重心不穩(wěn)。對于雙層結(jié)構(gòu)楔形穩(wěn)定平臺而言如果 2 個楔角不相等（即臂長不相等），機(jī)械手將無法觸及肩關(guān)節(jié)及其附近的盲點區(qū)域。因此雙層結(jié)構(gòu)的 2 個楔角必須相等。多層結(jié)構(gòu)雖然沒有這個問題，但在設(shè)計時也應(yīng)當(dāng)考慮到楔角變化對控制效果的影響，合理設(shè)計每層楔塊的楔角大小。

2.2 雙層結(jié)構(gòu)

圖 5 楔角不相等時雙層結(jié)構(gòu)楔形穩(wěn)定平臺的盲點區(qū)域Fig. 5 Blind zone of 2-layer structure when wedge angles are not equal

圖 6 雙層結(jié)構(gòu)和“轉(zhuǎn)角階躍”示意圖Fig. 6 Schematic diagram of 2-layer structure and rotation angle step

雙層結(jié)構(gòu)是指由一個水平底座和 2 個可旋轉(zhuǎn)的楔塊組成的楔形穩(wěn)定平臺的設(shè)計結(jié)構(gòu)。平面圖中雙層結(jié)構(gòu)等效為上臂與前臂等長的機(jī)械臂，且肩關(guān)節(jié)與原點重合。雖然這種結(jié)構(gòu)最為簡單，但是它有一個明顯缺陷——當(dāng)跟蹤軌跡過肩關(guān)節(jié)（原點）時，上臂（楔塊1）會發(fā)生“轉(zhuǎn)角階躍”（轉(zhuǎn)角變化不連續(xù)）。為說明這個情況，不妨假設(shè)目標(biāo)點的運行軌跡為 P1經(jīng)過 O 到 P2的一段折線（見圖 6）。機(jī)械臂可以在 P1O 上方運行或者下方運行，兩者沒有本質(zhì)區(qū)別。假定在上方運行，當(dāng)目標(biāo)點沿著 P1O 經(jīng)過原點時，機(jī)械臂與 y 軸重合。然而當(dāng)目標(biāo)點轉(zhuǎn)向沿著 O P2運行時，上臂必須突然轉(zhuǎn)動 90° 到與 y 軸垂直的位置才能使機(jī)械手繼續(xù)沿著 O P2運動。但實際上伺服系統(tǒng)的階躍響應(yīng)需要較長時間，因此“轉(zhuǎn)角階躍”無法精確實現(xiàn)的。這就會導(dǎo)致雙層結(jié)構(gòu)楔形穩(wěn)定平臺的跟蹤精度大大下降。

2.3 改進(jìn)型雙層結(jié)構(gòu)

之所以發(fā)生“轉(zhuǎn)角階躍”是因為機(jī)械手要經(jīng)過與原點重合的肩關(guān)節(jié)。若將肩關(guān)節(jié)與原點分開，就可以避免這個問題。為此可以將底座改成相同楔角的楔塊（稱為楔塊 0）。從平面圖上看楔塊 0 的作用是將肩關(guān)節(jié)從原點移到了跟蹤區(qū)域外的 N0。可以證明在肩關(guān)節(jié)和肘關(guān)節(jié)連續(xù)轉(zhuǎn)動的情況下，機(jī)械手能夠跟蹤區(qū)域內(nèi)的任意軌跡。并且針對每一個區(qū)域內(nèi)的點，上臂和前臂的轉(zhuǎn)角 q1，q2（0°≤q1≤180°，0°≤q2≤180°）有且只有唯一解。

圖 7 改進(jìn)雙層結(jié)構(gòu)的示意圖和平面圖Fig. 7 Schematic diagram and plane graph of advanced 2-layer structure

2.4 多層結(jié)構(gòu)

如果有 3 層或 3 層以上可旋轉(zhuǎn)的楔塊，則稱之為多層結(jié)構(gòu)。平面圖中多層結(jié)構(gòu)等效為 3 節(jié)或 3 節(jié)以上機(jī)械臂。雖然多層結(jié)構(gòu)提供了更高的控制靈活性，但是鑒于雙層結(jié)構(gòu)已經(jīng)能夠達(dá)到二維活動空間的自由度要求，增設(shè)的楔塊不僅提高了成本，而且多層結(jié)構(gòu)中楔塊轉(zhuǎn)角的解不唯一，徒增控制復(fù)雜度。

圖 8 多層結(jié)構(gòu)的示意圖和平面圖Fig. 8 Schematic diagram and plane graph of multi-layer structure

3. 楔形塊轉(zhuǎn)角的控制函數(shù)

通過第 2 節(jié)對不同的楔形穩(wěn)定平臺的結(jié)構(gòu)分析比較不難發(fā)現(xiàn)，改進(jìn)型雙層結(jié)構(gòu)平衡了可用性和經(jīng)濟(jì)性，符合大多數(shù)情況下對穩(wěn)臺的需求。接下來介紹改進(jìn)型雙層結(jié)構(gòu)楔形穩(wěn)定平臺的控制函數(shù)及求解方法。

3.1 平臺系統(tǒng)中坐標(biāo)系的定義

因為楔形穩(wěn)定平臺主要用于隔離艦船偏航、橫滾和縱搖 3 種旋轉(zhuǎn)擾動的影響，所以忽略各坐標(biāo)系原點的相對位置而僅考慮各坐標(biāo)系間旋轉(zhuǎn)關(guān)系。首先對初始狀態(tài)下各坐標(biāo)系的相對位置做設(shè)定。

自下而上依次為大地坐標(biāo)系：zs指向天空與海平面垂直，ys指向正北，xs指向正東；艦船載體坐標(biāo)系：zv軸指向艦船載體正上方與裝載平面垂直，yv軸指向艦首，xv軸指向右舷，初始狀態(tài)下艦船坐標(biāo)系與大地坐標(biāo)系重合；楔塊 0 坐標(biāo)系：z0垂直于楔塊 0 上平面，y0軸指向楔角方向，x0軸與 z0和 y0成右手螺旋關(guān)系，初始狀態(tài)下 x0指向右舷；楔塊 1 坐標(biāo)系：z1垂直于楔塊 1 上平面，y1軸指向楔角方向，x1軸與 z1和y1成右手螺旋關(guān)系，初始狀態(tài)下 x1指向左舷；楔塊 2坐標(biāo)系：z2垂直于楔塊 2 上平面，y2軸指向楔角方向，x2軸與 z2和 y2成右手螺旋關(guān)系，初始狀態(tài)下 x2指向右舷；圓盤坐標(biāo)系：zp垂直于圓盤，yp，xp與 zp成右手螺旋關(guān)系，初始狀態(tài)下圓盤坐標(biāo)系與楔塊 2 坐標(biāo)系重合。

圖 9 各坐標(biāo)系相對關(guān)系示意圖Fig. 9 Schematic diagram of relative coordinate systems

3.2 旋轉(zhuǎn)矩陣和坐標(biāo)系轉(zhuǎn)換

旋轉(zhuǎn)的正方向和轉(zhuǎn)角根據(jù)初始狀態(tài)坐標(biāo)系以右手螺旋法則確定。按照我國慣用的慣性元件檢測順序，從大地坐標(biāo)系向艦船坐標(biāo)系的轉(zhuǎn)換應(yīng)按照偏航-橫滾-縱搖的順序進(jìn)行［4］。用 H、R、P 分別表示偏航角、橫滾角和縱搖角，那么

偏航旋轉(zhuǎn)矩陣：

橫滾旋轉(zhuǎn)矩陣：

縱搖旋轉(zhuǎn)矩陣：

船搖擾動等效矩陣：

楔塊對坐標(biāo)系旋轉(zhuǎn)變化的影響包括轉(zhuǎn)角旋轉(zhuǎn)和楔角旋轉(zhuǎn) 2 個部分。分別用 β 表示楔角，q1、q2和 q3表示楔塊 1、楔塊 2 和圓盤的轉(zhuǎn)角，那么

楔角旋轉(zhuǎn)矩陣：

楔塊 1 轉(zhuǎn)角旋轉(zhuǎn)矩陣：

楔塊 2 轉(zhuǎn)角旋轉(zhuǎn)矩陣：

頂圓盤轉(zhuǎn)角旋轉(zhuǎn)矩陣：

補(bǔ)償?shù)刃D(zhuǎn)矩陣：

于是從大地坐標(biāo)系到圓盤坐標(biāo)系的完整變換過程可以用這些旋轉(zhuǎn)矩陣的乘積來表示：

3.3 楔塊的控制函數(shù)

楔形穩(wěn)定平臺可以準(zhǔn)確補(bǔ)償偏航、橫滾和縱搖 3個方向的任何擾動，也就是說對于大地坐標(biāo)系的任意被觀測向量 a 在經(jīng)過擾動和補(bǔ)償之后，在圓盤坐標(biāo)系上應(yīng)當(dāng)保持不變［5］。用向量和旋轉(zhuǎn)矩陣則表示為：

即

式中 q1，q2為楔塊 1 與楔塊 2 的轉(zhuǎn)角。由于楔塊 1 與楔塊 2 的作用僅僅是保持被穩(wěn)定對象在垂直方向上的穩(wěn)定，不妨取為方便計算可以將式（12）一部分旋轉(zhuǎn)矩陣移動到等式左邊，并代入 a'：

由上式可以算得：

因為 q1與 q2的取值范圍都是 0°～180°（見圖 7），

所以應(yīng)當(dāng)通過 cos（q1） 和 cos（q2） 計算q1和 q2。

式中 q3為圓盤的轉(zhuǎn)角，用于補(bǔ)償水平方向上偏航擾動和楔塊 1 和楔塊 2 轉(zhuǎn)動時附加的影響，因此可以取由于 q3的取值范圍是 0°～360°，所以要綜合考慮 cos（q3） 和 sin（q3） 才能計算 q3。調(diào)整式（12）并代入 a"

計算可得：

4 結(jié) 語

楔塊的轉(zhuǎn)角變化比平臺的傾角高一個數(shù)量級，因此它的控制精度高。又因為楔塊基本是在水平面轉(zhuǎn)動，所以伺服電機(jī)的負(fù)載轉(zhuǎn)矩小，也因此質(zhì)量輕、能耗低。加之它的結(jié)構(gòu)簡單，相對于陀螺儀式穩(wěn)定平臺它的制造成本也更低。總之，楔形穩(wěn)定平臺為穩(wěn)定平臺的設(shè)計提供了一種有益的新思路。

［1］MAAS A J. Wedge platform stabilization［J］. Revue Technique-Thomson-CSF， 1994， 26（3）: 553–569.

［2］孫成祿. 甩不掉的"尾巴"——艦載"天狼星"遠(yuǎn)程紅外搜索與跟蹤系統(tǒng)［J］. 現(xiàn)代兵器， 2004（6）: 28–29.

［3］郭隆華. 加拿大海軍驗收首套天狼星紅外搜索與跟蹤系統(tǒng)［EB/OL］. （2008–5-20）. http://news.xinhuanet.com/ mil/2008–05/20/content_8213388.html.

［4］張志遠(yuǎn)， 羅國富. 艦船姿態(tài)坐標(biāo)變換及穩(wěn)定補(bǔ)償分析［J］. 艦船科學(xué)技術(shù)， 2009， 31（4）: 34–40. ZHANG Zhi-yuan， LUO Guo-fu. Coordinate transformation of warship pose and analysis of stabilization compensation［J］. Ship Science and Technology， 2009， 31（4）: 34–40.

［5］張坤石. 潛艇光電裝備技術(shù)［M］. 哈爾濱: 哈爾濱工程大學(xué)出版社， 2007: 429–437.

The equivalent manipulator model of wedge-platform

ZHU Yuan-ping1，2，3，4， TANG Xin-yi1，2，3，4， YUAN Xiu-peng2，3，4
（1. School of Information Science and Technology， Shanghai Technology University， Shanghai， 200030， China；2. Shanghai Institute of Technical Physics of the Chinese Academy of Sciences， Shanghai， 200080， China；3. Key Laboratory of Infrared System Detection and Imaging Technology， Chinese Academy of Sciences， Shanghai， 200080， China；4. Shanghai Key Laboratory of Crime Scene Evidence， Shanghai， 200080， China）

A stabilized platform is needed to compensate the disturbance of a moving carrier to keep the observation instrument relatively static to the ground. The wedge-platform changes the lean of its top plane to offset the vessel sway by rotating several wedges systematically， which has advantages as low produce cost， high accuracy， small load torque， low mass and low energy consumption. However it is confusing to analyze the wedge-platform for its unique structure. Therefore a simplifying transformation is created in this article which transforms the three-dimensional rotations of wedges to the twodimensional movement of an approximately equivalent manipulator. Then 2-layer， advanced 2-layer and multi-layer structures of wedge-platform are analyzed and compared， which leads to the conclusion that the advanced 2-layer structure is the most appropriate practice solution. Finally， a set of control functions for the advanced 2-layer wedge-platform is calculated through coordinate systems transformations by rotation matrices.

wedge-platform；sway；equivalent manipulator；layer；

U666.12+1

A

1672 – 7619（2016）04 – 0051 – 05

10.3404/j.issn.1672 – 7619.2016.04.011

2015 – 09 – 06；

2015 – 10 – 08

國家十二五國防預(yù)研資助項目（41101050501）；上海市現(xiàn)場物證重點實驗室資金資助項目（2011xcwzk04）

朱元平（1990 – ），男，碩士研究生，研究方向為楔形穩(wěn)定平臺。