楔形穩定平臺的等效機械臂模型

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

楔形穩定平臺的等效機械臂模型

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

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

楔形穩定平臺；船搖；等效機械臂；層級結構；

0 引 言

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

1 等效機械臂模型推導

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

圖 1 “天狼星”系統的楔形穩定平臺［3］Fig. 1 The wedge-platform of Sirius IRST

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

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

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

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

2 楔角和層級結構分析

在設計楔形穩定平臺的機械結構時，必須考慮楔角和層級結構。楔角主要影響平定平臺的補償范圍，而層級結構則對平臺的補償效果有決定性影響。

2.1 楔塊的楔角

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

2.2 雙層結構

圖 5 楔角不相等時雙層結構楔形穩定平臺的盲點區域Fig. 5 Blind zone of 2-layer structure when wedge angles are not equal

圖 6 雙層結構和“轉角階躍”示意圖Fig. 6 Schematic diagram of 2-layer structure and rotation angle step

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

2.3 改進型雙層結構

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

圖 7 改進雙層結構的示意圖和平面圖Fig. 7 Schematic diagram and plane graph of advanced 2-layer structure

2.4 多層結構

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

圖 8 多層結構的示意圖和平面圖Fig. 8 Schematic diagram and plane graph of multi-layer structure

3. 楔形塊轉角的控制函數

通過第 2 節對不同的楔形穩定平臺的結構分析比較不難發現，改進型雙層結構平衡了可用性和經濟性，符合大多數情況下對穩臺的需求。接下來介紹改進型雙層結構楔形穩定平臺的控制函數及求解方法。

3.1 平臺系統中坐標系的定義

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

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

圖 9 各坐標系相對關系示意圖Fig. 9 Schematic diagram of relative coordinate systems

3.2 旋轉矩陣和坐標系轉換

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

偏航旋轉矩陣：

橫滾旋轉矩陣：

縱搖旋轉矩陣：

船搖擾動等效矩陣：

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

楔角旋轉矩陣：

楔塊 1 轉角旋轉矩陣：

楔塊 2 轉角旋轉矩陣：

頂圓盤轉角旋轉矩陣：

補償等效旋轉矩陣：

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

3.3 楔塊的控制函數

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

即

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

由上式可以算得：

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

所以應當通過 cos（q1） 和 cos（q2） 計算q1和 q2。

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

計算可得：

4 結 語

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

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

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

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

［4］張志遠， 羅國富. 艦船姿態坐標變換及穩定補償分析［J］. 艦船科學技術， 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］張坤石. 潛艇光電裝備技術［M］. 哈爾濱: 哈爾濱工程大學出版社， 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

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

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