楔形稳定平台的等效机械臂模型

朱元平，汤心溢，袁修鹏（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 – ），男，硕士研究生，研究方向为楔形稳定平台。