维普资讯 http://www.cqvip.com 第２９卷第７期 ２００８年７月 仪 器仪 表 学 报 Ｖ０１．２９ Ｎｏ．７ Ｃｈｉｎｅｓｅ Ｊｏｕｒｎａｌ ｏｆ Ｓｃｉｅｎｔｉｆｉｃ Ｉｎｓｔｒｕｍｅｎｔ Ｊｕ１．２００８ 研究球 小型便携式船舶姿态测量系统 金鸿章，王帆，綦志刚 １５０００１） （哈尔滨工程大学自动化学院 哈尔滨摘要：针对小型船舶的运动姿态测量及自航模试验等不便安装姿态测量设备的问题，提出一种小型化便携式船舶姿态测量方 法，并设计了相应的系统。通过分析船舶各点的运动状态，采用捷联方式，多组惯性器件直接敏感姿态参数，系统软件实现姿态解 算。为保证系统的可靠性和克服惯性元件特性随环境的变化，研究了一种基于状态空间估计的软件算法。在不同海情下与传统 Ｋａｌｍａｎ滤波算法进行了仿真比较，验证了该算法的有效性。最后给出了该系统的实测结果，初步证明了系统的可行性。 关键词：船舶姿态；姿态测量；小型便携式；自适应Ｋａｌｍａｎ滤波 中图分类号：ＴＰ２１６ 文献标识码：Ａ 国家标准学科分类代码：５１０．８０４０ Ｒｅｓｅａｒｃｈ ｏｎ ｍｉｎｉ－ｔｙｐｅ ａｎｄ ｐｏｒｔａｂｌｅ ｓｈｉｐ ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ ｓｙｓｔｅｍ Ｊｉｎ Ｈｏｎｇｚｈａｎｇ，Ｗａｎｇ Ｆａｎ，Ｑｉ Ｚｈｉｇａｎｇ （Ｃｏｌｌｅｇｅ ｏｆ Ａｕｔｏｍａｔｉｏｎ，Ｈａｒｂｉｎ Ｅｎｇｉｎｅｅｒｉｎｇ Ｕｎｉｖｅｒｓｉｔｙ，Ｈａｒｂｉｎ １５０００１，Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ａ ｋｉｎｄ ｏｆ ｍｉｎｉ—ｔｙｐｅ ａｎｄ ｐｏｒｔａｂｌｅ ｓｈｉｐ ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ ｍｅｔｈｏｄ ａｎｄ ｓｙｓｔｅｍ ｗｅｒｅ ｄｅｖｅｌｏｐｅｄ ｉｎ ｔｈｉｓ ｐａ． ｐｅｒ ｆｏｒ ｓｍａｌｌ ｓｈｉｐ ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ ａｎｄ ｓｅｌｆ－ｐｒｏｐｅｌｌｅｄ ｍｏｄｅｌ ｔｒｉａｌ，ｗｈｅｒｅ ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ ｅｑｕｉｐｍｅｎｔ ｗａｓ ｉｎｃｏｎｖｅｎｉｅｎｔ ｔｏ ｉｎｓｔａｌ１．Ｂｙ ａｎａｌｙｚｉｎｇ ｔｈｅ ｍｏｔｉｏｎ ｓｔａｔｅ ｏｆ ｅｖｅｒｙ ｐｏｉｎｔ ｉｎ ｔｈｅ ｓｈｉｐ，ｔｈｅ ｓｔｒａｐ—ｄｏｗｎ ｍｏｄｅ ｗａｓ ａｐｐｌｉｅｄ ｔｏ ｍｅａｓｕｒｅ ｔｈｅ ａｔｔｉｔｕｄｅ ｐａｒａｍｅｔｅｒｓ ｄｉｒｅｃｔｌｙ ｕｓｉｎｇ ｓｅｖｅｒａｌ ｂａｎｋｓ ｏｆ ｉｎｅｒｔｉａｌ ｃｏｍｐｏｎｅｎｔｓ，ａｎｄ ｓｙｓｔｅｍ ｓｏｆｔｗａｒｅ ｗａｓ ｕｓｅｄ ｔｏ ｒｅａｌｉｚｅ ａｔｔｉｔｕｄｅ ａｌｇｏｒｉｔｈｍ．Ｔｏ ｅｎｓｕｒｅ ｓｙｓｔｅｍ ｒｅｌｉａｂｉｌｉｔｙ ａｎｄ ｏｖｅｒｃｏｍｅ ｔｈｅ ｃｈａｒａｃｔｅｒｉｓｔｉｃ ｃｈａｎｇｉｎｇ ｏｆ ｔｈｅ ｉｎｅｒｔｉａｌ ｃｏｍｐｏ－ ｎｅｎｔｓ ｗｉｔｈ ｗｏｒｋｉｎｇ ｅｎｖｉｒｏｎｍｅｎｔ，ａｎ ａｌｇｏｒｉｔｈｍ ｂａｓｅｄ ｏｎ ｓｔａｔｅ ｓｐａｃｅ ｅｓｔｉｍａｔｉｏｎ ｗａｓ ｐｒｏｐｏｓｅｄ．Ｔｈｅ ｖａｌｉｄｉｔｙ ｏｆ ｔｈｉｓ ａｌ－ ｇｏｒｉｔｈｍ ｗａｓ ｐｒｏｖｅｄ ｂｙ ｃｏｍｐａｒｉｎｇ ｔｈｅ ｓｉｍｕｌａｔｉｏｎ ｒｅｓｕｌｔ ｏｆ ｔｈｉｓ ａｌｇｏｒｉｔｈｍ ｗｉｔｈ ｔｈａｔ ｏｆ ｔｒａｄｉｔｉｏｎａｌ Ｋａｌｍａｎ ｆｉｌｔｅｒｉｎｇ ｉｎ ｄｉｆ－ ｆｅｒｅｎｔ ｓｅａ—ｃｏｎｄｉｔｉｏｎｓ．Ｆｉｎａｌｌｙ，ｔｈｅ ｒｅｓｕｌｔｓ ｆｒｏｍ ｒｅａｌ ｔｅｓｔｓ ｗｅｒｅ ｇｉｖｅｎ，ｗｈｉｃｈ ｐｒｏｖｅｓ ｔｈｅ ｆｅａｓｉｂｉｌｉｔｙ ｏｆ ｔｈｉｓ ｓｙｓｔｅｍ． Ｋｅｙ ｗｏｒｄｓ：ｓｈｉｐ ａｔｔｉｔｕｄｅ；ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ；ｍｉｎｉ—ｔｙｐｅ ａｎｄ ｐｏｒｔａｂｌｅ；ａｄａｐｔｉｖｅ Ｋａｌｍａｎ ｆｉｌｔｅｒｉｎｇ 量轻、携带方便、易装卸等特点，且能提供多种参数满足控 １ 引 言 制和试验要求。初步实测证明本系统是可行的。 小型船舶的运动控制、船模及自航模试验等都需要船 ２船舶姿态测量原理 舶（模）的姿态参数。根据试验的具体要求，除了姿态角信 息外，有时还需要横荡、纵荡和升沉等数据，且经常需要随 ２．１船舶姿态角与受扰运动 时安装和拆卸。目前船舶姿态测量设备大都结构复杂、造 价高、体积质量大，不便于装卸，无法适用于上述场合。基 于以上原因，本文研究了一种小型便携式船用姿态测量系 统。该系统分为软硬件两部分，具有结构紧凑、体积小、质 收稿日期：２００７－０７ Ｒｅｃｅｉｖｅｄ Ｄａｔｅ：２００７－０７ 船舶的航向及水平姿态角实际上是船舶坐标系 Ｏｘｙｚ（简记ｂ系）和当地地理坐标系ＯＥＮＺ（简记ｎ系） 之间的转动关系。当船舶绕地垂线ＯＺ转动，船舶纵轴 ０ 在水平面上的投影与地理子午线ＯＮ之间的夹角称航 ｝基金项目：十五国防预研项目（１０１０６０４０３０１０４）资助 维普资讯 http://www.cqvip.com 第７期 金鸿章等：小型便携式船舶姿态测量系统研究 １５４９ 向角，以 表示，规定北向偏东为正；当船舶绕纵向水平 ２．２船舶任意点的运动分析 轴转动，船舶的横轴和横向水平轴之间的夹角称船舶的 由刚体运动学知．冈０体的一般运动可看作随基点的 横摇角，以 表示，规定左舷抬高为正；当船舶绕横轴转 动，船舶的纵轴和纵向水平轴之间的夹角称为船舶的纵 摇角，以０表示，规定抬头为正。如图１所示。 平动与绕基点转动的合成。基点可选刚体上任意点，但 由于刚体上各点的线速度与加速度一般不同，故平动速 度和加速度与基点的选取有关。而刚体绕基点转动的瞬 时角速度和角加速度与基点选取无关。 可以将船舶在海洋中的运动看作刚体在空间的运动。 船舶在海洋中受到海浪、海流和海风等自然条件的影响， 船舶重心发生平动，船体绕自身三轴转动，因此船舶的一 般运动可看作随重心的平动与绕重心转动的合成。 如图２所示，Ｅ— 图１坐标变换 Ｆｉｇ．１ Ｃｏｏｒｄｉｎａｔｅ ｆｒａｍｅ ｔｒａｎｓｆｏｒｍ 和Ｏ—ｘｙｚ分别为平移坐标系 和船舶坐标系，基点Ｏ为船舶重心， 、Ｙ、 分别为船舶 横摇、纵摇和艏摇轴，单位方向矢量记为ｉ、 、ｋ。 根据欧拉角变换算法，可以得到： 睡 ； ㈩ 图２船舶与坐标位置关系 Ｆｉｇ．２ Ｌｏｃａｔｉｏｎ ｒｅｌａｔｉｏｎｓｈｉｐ ｂｅｔｗｅｅｎ ｓｈｉｐ ａｎｄ ｃｏｏｒｄｉｎａｔｅ ｆｒａｍｅ 船舶不受扰动时两坐标系重合。假设Ｐ为船舶上一固 定点，在船舶坐标系中坐标为（ ，Ｙ， ），位矢为ｒ ，则： ＝ａ０＋ ×ｒ ＋ ×（ × ） Ⅱ ＝ａ 一（ 。 ＋ ‘ｚｓｉｎ ＋ ＺＣＯＳ ）＋（ 。ｙｃｏｓ，ｃｏｓ ０一 ‘ｙｓｉｎ ｃｏｓ ０一 ‘ｙｃｏｓ￣ｂｓｉｎ ０一 ‘ｙｓｉｎ ０一 （２） ・ｙｃｏｓ ）一（ｄｏ・ｙｃｏｓ￣ｂｓｉｎ ０＋ ・ｙｓｉｎ／，ｃｏｓ￣ｂｓｉｎ ０＋ ・ｙｃｏｓ ０＋ ・ｙｓｉｎ ｑｂｃｏｓ ）＋（ ・ ＋扩・ｘｓｉｎ ｑｂ＋ ２ ・ｘｓｉｎ ）＋（ ・ ｃｏｓ ｃｏｓ ０＋ ・ｘｓｉｎ ０—２ ・ ｃｏｓ￣ｂｓｉｎ ０ｃｏｓ ）一（矿・ＺＣＯＳ ￣ｂｓｉｎ ０ｃｏｓ ０一 ・ ＺＣＯＳ￣ｂｓｉｎ２ ０＋ ・ＺＣＯＳ ｑｂｃｏｓ ０一 ・ｚｓｉｎ ０ｃｏｓ ） ｃｏｓ ０）＋（ ＺＣＯＳ￣ｂｓｉｎ ０一 口 ＝ａｙ一（ 。 ｃｏｓ，ｃｏｓ ０一 。ｘｓｉｎ ｃｏｓ ０一 。 ｃｏｓ￣ｂｓｉｎ ０一 。ｘｓｉｎ ０一 ・ｚｓｉｎ￣ｂｓｉｎ ０＋ ・ＺＣＯＳ ｃｏｓ ０＋ ・ＺＣＯＳ ０一 ・ｚｓｉｎ ）一（ ・ＺＣＯＳ ｃｏｓ ０一 ・ｚｓｉｎ ０＋ ・ｚｓｉｎ ｃｏｓ ｃｏｓ ０一 ・ｚｓｉｎ￣ｂｓｉｎ ）＋（ ・ｙｃｏｓ ＋ ・ｙ）一（ ・ ｃｏｓ￣ｂｓｉｎ ０＋ ・ｘｓｉｎ ｃｏｓ￣ｂｓｉｎ ０＋ ・ ｃｏｓ ０＋ ・ｘｓｉｎ（￣ｃｏｓ ） 口 ： 一（ ・ｙｃｏｓ￣ｂｓｉｎ ０一 ・ｙｓｉｎ￣ｂｓｉｎ ０＋ ・ｙｃｏｓ，ｃｏｓ ０＋ ・ｙｃｏｓ ０一 ・ｙｓｉｎ ）＋（ ・ ＋ ・ｘｓｉｎ ＋ ・ ｃｏｓ ）一（ ・ ｃｏｓ ￣ｂｓｉｎ ０ｃｏｓ ０一 ・ ｃｏｓ￣ｂｓｉｎ ０＋ ・ ｃｏｓ ｃｏｓ ０一 ・ｘｓｉｎ ０ｃｏｓ ）＋ （矿・ＺＣＯＳ ￣ｂｓｉｎ ０＋ ・ＺＣＯＳ ０＋２Ｉ硒・ＺＣＯＳ￣ｂｓｉｎ ０ｃｏｓ ）＋（ ・ ＋ ・ｚｓｉｎ ＋２ ・ｚｓｉｎ ）一（ｄｏ・ ｙｃｏｓ ｃｏｓ ０一 ・ｙｓｉｎ ０＋ ・ｙｓｉｎ，ｃｏｓ ｃＯＳ ０一 ．ｙｓｉｎ ｓｉｎ ） （３） 式中： 为Ｐ点的绝对线加速度； 为重心点的绝对线加 速度； 为船舶绕重心的转动角速度； 为船舶绕重心的 转动角加速度。设ａ、 、 、 在船舶坐标系中的坐标 分别为（ ，ｎ，，ｎ：）、（ｎ ，ｎ ，，ｎ ：）、（卢 ， ，卢：）、 （ ， ， ：），将式（２）分解到船舶坐标系，结合式 （１），得到重心点绝对线加速度在船舶坐标上的投影，如 式（３）所示。再经线性变换得： 维普资讯 http://www.cqvip.com １５５０ —．．． ．．．．．．．．．．．．．．．仪Ｌ 器仪表学报 第２ ９卷 Ａ Ａ Ａ 叶 ｆ １， ●●●●，●Ｊ 。ｉ“ＣＯＳ＠ｓｌｎ Ｏｓｌｎ击一ｓｉｎ ｑ，ｃｏｓ击 ｃ。。‘ｌ，口ｃ。ＩＩ —．．．．．．．． ．。．．．．．ＬＣ Ｓ ｓｉｎ Ｏｓｉｎ Ｏｓｉｎ ＋ＣＯＳ ｃｏｓ ＣＯＳ Ｏｓｉｎ西 ｓｉｎｓｉｎＯｃｏｓＯｎ ｏ Ｓ — ● ＣＯＳ。 Ｏ；ｃｏ二ｓ宝主 Ｏｓ。ｉ“ｎ］・Ｌｆ 一西ｃｏ Ｊｓ Ｊｏｊ ］Ｉ ａ（４） 式中： 、 、 分别为船舶的纵荡、横荡和垂荡线加速 度。测量中根据传感器的安装位置和输出值按式（３）、 Ｓ Ｏ ４．２．１在线姿态测量估计算法 惯性测量系统可以测量船舶所需的各种运动参数， 但一些形式的滤波器仍然是必要的 。计算机不仅要提 （４）计算即可获得船舶受扰线运动加速度。再通过一 次、二次积分便可求出相应的运动速度和位移。为便于 计算机实现，可根据海情、船舶条件或具体试验要求对式 供数学平台，完成姿态解算和坐标变换，同时还要具有对 （４）适当化简。 ３测量系统 本测量系统由７只传感器（３只单轴压阻式线加速 度传感器、３只单轴微惯性陀螺、１只全方位角度传感 器）、数据采集板和１台计算机组成。采用捷联方式测 量。配置系统各传感器的安装示意图，如图３所示。 ， 图３传感器配置与敏感轴 Ｆｉｇ．３ Ｓｅｎｓｏｒ ｃｏｎｆｉｇｕｒａｔｉｏｎ ａｎｄ ｔｈｅ ｓｅｎｓｉｔｉｖｅ ａｘｉｓ 数据采集板采用时分多路方式，程序控制数字通道 开关，每０．５ Ｓ对各参数进行一次记录，同时将采集的数 据存储在采集板的Ｆｌａｓｈ内，并可选择将数据发送至计算 机显示存储。系统硬件部分组成如图４所示。 图４硬件部分组成 Ｆｉｇ．４ Ｓｔｒｕｃｔｕｒｅ ｏｆ ｔｈｅ ｈａｒｄｗａｒｅ ４系统软件设计 ４．１数学平台与姿态解算 本系统采用捷联方式代替了平台系统复杂的框架结 构，通过传感器和陀螺仪直接敏感船舶的运动参数，因此 需要计算机提供数学解析平台，根据式（１）、式（４）进行 姿态解算和坐标变换，从而求出真实的姿态参数。由于 篇幅所限，此处不再敖述，详见文献［３］、文献［９］。 姿态参数估计、预测和必要的信号处理能力。软件算法 能够弥补单传感器获得数据的可信度，在系统工作环境 变化或受到较严重的冲击、振荡使系统的某些传感器失 效或损坏时，仍能确保有效的实时测量。因此，系统辅助 软件的设计对整个系统是十分重要的。 多年来，国内外学者研究出了一些有效的算法，并已 在一些实船上使用，但这些软件存在共同的缺陷，即依赖 于船舶各运动量的统计规律，包括谱密度、峰值频率等， 他们的准确性直接关系到估计参数的准确度。所以这种 软件只能针对某一特定的船舶，并需要获得足够多的运 动统计，无法推广。 ４．２．２一种新的软件算法研究 基于以上原因，本文研究了一种基于状态空间估计 的软件算法，出于横摇、纵摇和升沉运动对船舶状态的主 要影响，以下讨论以这三种运动为例。 船舶在海洋环境扰动下的横摇、纵摇和升沉运动可 以表示为长短周期两主导正弦波成分叠加的形式…，周 期分别在６～１０ Ｓ和２～３ Ｓ，如下式： （ｔ） Ａｌ ｓｉｎ（ ｌｔ＋ １）＋Ａ２ｓｉｎ（ ２ｔ＋ ２） （５） 式中： （ｔ）为位移量；｛ 。， ｝为一定范围内的两正弦 波角频率值；｛ 。， ｝、｛ ， ｝未知。显然 （ｔ）满足 如下线性微分方程： ＋（ｏＪ： ２） ２ ２ －０（６） 将上式写成状态空间方程： 未：Ａ ＋云∞ （７） 式中： ＝（ 。， ， ，， ， ） 分别对应船舶运动量的位 移、速度、加速度等；由于实际信号不可能只由两个主导 波表达，其中包含了许多非主导波作用，因此上述模型加 入曰 修正项，Ｂ＝（０，０，１，０，０） ， （ｔ）为一个零均值 白噪声过程，方差为 ，大小由非主导波对每种信号的 作用水平决定； ０ １ ０ ０ Ａ ０ ０ ０ ０ ０一 ２１ 维普资讯 http://www.cqvip.com 第７期 金鸿章等：小型便携式船舶姿态测量系统研究 １５５１ 前面设计的测量系统可提供多种观测量的选择，建 立如下观测模型： Ｙ＝Ｃ ＋ （８） 定义虚拟模型噪声和虚拟观测噪声为： ｆ孝 ＝△ （ｋ） ＋Ｊ＿ 【叩 ＝△Ｃ（ ） ＋ （１６） 式中：Ｙ为传感器或陀螺的输出值；Ｃ为观测矩阵，由传 感器或陀螺的输入输出特性决定； 为测量噪声，包括测 量误差及传感器的内部噪声。结合前面得到的系统方 虚拟噪声 （ｋ）补偿了状态模型误差△ （ｋ） ，虚 拟噪声叼（ｋ）补偿了观测方程误差ＡＣ（ｋ） 。通常模型 程，得到状态空间模型： ＝Ａ ＋ 一 （９） Ｙ＝ ＋ 至此，已将船舶运动参数的在线测量问题转化为线 性动态系统的状态估计问题。为便于在计算机上实现， 将模型离散化为： ＋ｌ＝ ＋Ｊ ｋ 、 一 （１０） Ｙ ＝ｃ ＋ 式中：西＝ （ｔ）｝ｔ＝ＡＴ； （ｔ）＝Ｌ ｛（５，一Ａ） ｝；Ｆ＝ ＡＴ 一 （Ｉ （ｔ）ｄｔ）・Ｂ；△ 为采样时间。 ＪＵ 实际的海情是不断变化和未知的，而传统Ｋａｌｍａｎ滤 波算法要求精确已知系统模型，包括噪声统计。若对上 述模型直接执行Ｋａｌｍａｎ滤波则会存在较大误差，甚至滤 波器发散。为了补偿６０（ｔ）统计特性随海情的变化以及 系统工作环境、惯性器件、电子伺服回路等受影响产生的 未知变化，本文采用了自适应滤波技术，实时修正系统模 型和噪声统计，以提高滤波精度。 考虑上述离散系统： 川＝ ＋Ｊ 一 （１１） Ｙｋ＝ ＋ 式中： 、Ｊ１、ｃ是已知矩阵； 、 是相互独立的白噪 声，统计特性为： Ｅｗ ＝ｑ（ｋ），ＣＯＹ［ ｆ，ｔＯｊ］＝ｑ０（ ）６ （１２） Ｅｖ ＝ｒ（ｋ），ＣＯＹ［ ， ］＝Ｒ。（ ）６ （１３） 考虑系统式（１１）带有未知模型误差，即真实系统不 是（１１），而是： ＋。＝（ ＋△ （ｋ）） ＋Ｊ Ｙ ＝（Ｃ＋ＡＣ（ｋ）） ＋ （１４） 式中：状态转移矩阵西的误差△西（ｋ）和观测阵ｃ的误差 ＡＣ（ｋ）是未知的，但西和ｃ是已知的。因此，即使噪声 统计式（１２）、（１３）是已知的，对带模型误差系统进行常 规Ｋａｌｍａｎ滤波也会使滤波器性能变坏。为了补偿模型 误差，将真实系统式（１１）改写为： ｒ ＋。＝ ＋ （ ） ｛【 一 ＝ｃ ＋ （ ） （１５） 误差项△西（ｋ），ＡＣ（ｋ）相对于西、ｃ而言是较小的，因此 可近似假设虚拟噪声 （ｋ）和 （ｋ）是带未知时变噪声统 计的相互独立的白噪声： Ｅ （ｋ）＝ （ｋ），ＣＯＹ［ （ｉ）， （ ）］＝ｑ（ｋ） （１７） Ｅ （ｋ）＝ｒ（ｋ），ＣＯＹ［ （ｉ）， （ ）］＝Ｒ（ｋ） （１８） 于是对真实系统（１１）的滤波问题转化为带未知时 变噪声统计系统（１５）的滤波问题，称这种引进虚拟噪声 补偿技术改进带未建模动态系统滤波性能的滤波器为鲁 棒Ｋａｌｍａｎ滤波器。 采用时变噪声统计递推估值器，得出鲁棒Ｋａｌｍａｎ滤 波算法如下： Ｉ ＝ 一ｌ＋ ｋ８ ｋ ｆ未…： 。＋亏 １． ｆ ｓ Ｙ 一ＣＥ 一ｌ—ｒ 一ｌ ｌ Ｐ 。＝ ＋０ ＼ｌ 一一 Ｐｖｋ：Ｐｖｋ＿１一ＫｋＣＰ ｋ，ｋ ｊ Ｋｋ＝Ｐｋ／ｋ－１Ｃ ａｉ Ｉ以 ：云Ｐ 一。云 ＋ 一。 １ ＝（１一ｄ 一。） 一。＋ｄ 一。［育 一 一。 一。］ Ｉ．１ 一 ｒ ＝（１一ｄ 一１）ｒ 一ｌ＋ｄ 一ｌ［Ｙ 一ｃ 一１］ Ｉｌ Ｒ ＝（１一ｄ 一１）Ｒ 一ｌ＋ｄ 一ｌ［￡ ￡ 一ｃ 一１ｃＴ］． 【 ：（１一ｄｋ）０ 一ｄｋ一。［ ￡｝ ＋ 一 一。 ］ （１９） 式中：ｄ ＝１—６／（１一ｂ¨ ），０＜ｂ＜１，ｂ为遗忘因子；初值 为茁【）／（】＝ ０，Ｐ【）／（】＝Ｐｏ， ＝一ｑ０，Ｑ０＝ｑ０，Ｆ０＝ＦＯ，Ｒ０＝Ｒ０。 从初值出发，就可实现状态估计和噪声统计。 ５仿真与实验 ５．１算法仿真与比较 根据上节研究的软件算法与传统Ｋａｌｍａｎ滤波算法 在不同海情下进行模拟，得到仿真结果。限于篇幅，仅给 出横摇结果： 维普资讯 http://www.cqvip.com １５５２ 仪器仪表学报 第２ ９卷 １）某型船１，有义波高１ ｍ，海浪平均周期５ Ｓ，浪向 角９０。，航速９ ｋｎ，如图５～６所示。 ２）某型船２，有义波高２ ｍ，海浪平均周期７ ｓ，浪向 角９０。，航速１８ ｋｎ，如图７～８所示。 一 已一 删 援 糍 蜒 ０ ５ １０ １５ ２０ ２５ ３０ 时间／ｓ 图５横摇角估计误差（船１） Ｆｉｇ．５ Ｅｓｔｉｍａｔｅｄ ｅｌ＇ｒｏｒ ｏｆ ｒｏｌｌｉｎｇ ａｎｇｌｅ 倒 糍 蜒 图６横摇角速度估计误差（船１） Ｆｉｇ．６ Ｅｓｔｉｍａｔｅｄ ｅｌ＇ｒｏｒ ｏｆ ｒｏｌｌｉｎｇ ａｎｇｌｅ ｖｅｌｏｃｉｔｙ 时间／ｓ 图７横摇角估计误差（船２） Ｆｉｇ．７ Ｅｓｔｉｍａｔｅｄ ｅｌ＇ｒｏｒ ｏｆ ｒｏｌｌｉｎｇ ａｎｇｌｅ ｌ・５ 譬 三 删 慧０．５ 倒 景 蠢。蜒 ０．５ 图中看出，随海情变化，船舶的横摇角频率也发生改 变，传统的Ｋａｌｍａｎ滤波算法随着模型的不精确程度增 加，其估计效果也呈现较大的误差。然而鲁棒Ｋａｌｍａｎ滤 波通过在线估计系统噪声方差，由此计算滤波增益，得到 较高的估计精度，同时能够有效地补偿传感器灵敏度、非 线性特性等随温度和工作环境变化产生的误差。船１与 ２在尺寸、排水量、航速以及海情条件等方面均有较大差 异，但都能得到较好的估计效果，说明此算法具有很好的 推广特点。另外，此算法并没有增加计算的迭代次数，运 算时间短，利于在计算机、单片机或ＤＳＰ上实现。 ５．２实验结果 本次实验在中国电子科技集团４９研究所进行。以 标定值１ ｇ对纵轴线加速度进行测试，结果见表１。 表１纵向加速度测试结果 Ｔａｂｌｅ １ Ｔｅｓｔ ｒｅｓｕｌｔ ｏｆ Ｉｏｇｎｉｔｕｄｉｎａｌ ａｃｃｅｌｅｒａｔｉｏｎ 以一组连续横摇角对系统测试，结果见表２。 初测结果表明本系统可以提供较稳定可靠的船舶姿 态参数。与传统船用姿态测量设备相比，测量误差略 大 ］，主要原因是由于传感器的响应速度和传感器本身 的测量精度等限制，可依试验要求选择精度更高、动态特 性更好的传感器替换以提高精度。 维普资讯 http://www.cqvip.com 第７期 金鸿章等：小型便携式船舶姿态测量系统研究 １５５３ 表２横摇角测试结果 Ｔａｂｌｅ２ Ｔｅｓｔ ｒｅｓｕｌｔ ｏｆ ｒｏｌｌｉｎｇ ａｎｇｌｅ ６结 论 本文提出了一种小型便携式船舶姿态测量系统的研 究思想，通过捷联方式直接敏感姿态参数，计算机进行姿 态解算获得船舶运动信息，可提供船舶ｌ５个运动量的数 据。针对本系统设计了一种新的辅助软件，能补偿系统 惯性器件的漂移及干扰等影响，且算法不受船型、海情等 条件限制，有利于推广。本系统体积小、携带方便、易于 装卸维护，特别适合小型船舶、船模试验等场合。初步试 测结果证明了系统的可行性。 参考文献 ＩＩＡＮＡＫＡ Ｓ．ＮＩＳＨＩＦＵＪＩ Ｓ．Ａｕｔｏｍａｔｉｃ ｏｎ－ｌｉｎｅ ｍｅａｓｕｒｅ－ ｍｅｎｔ ｏｆ ｄｙｎａｍｉｃ ｓｈｉｐ Ｓ ａｔｔｉｔｕｄｅ『Ｃ］．ＩＥＣＯＮ Ｐｒｏｃｅｅｄ． ｉｎｇｓ，１９９４，３：２００５－２０１０． ［２］ ＴＡＮＡＫＡ Ｓ．Ｏｎ ａｕｔｏｍａｔｉｃ ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ ｓｙｓｔｅｍ ｆｏｒ ｓｈｉｐｓ ｕｓｉｎｇ ｓｅｒｖｏ－ｔｙｐｅ ａｃｃｅｌｅｒｏｍｅｔｅｒｓ［Ｃ］．Ｔｒａｎｓ． ＳＩＣＥ，１９９１，２７（８）：８６１－８６９． ［３］ 黄德鸣，张树侠，孙枫．平台罗经［Ｍ］．北京：国防 工业出版社，１９９０． ＨＵＡＮＧ Ｄ Ｍ．ＺＨＡＮＧ ＳＨ Ｘ．ＳＵＮ Ｆ．Ｆｌａｔ ｃｏｍｐａｓｓ ［Ｍ］．Ｂｅｉｊｉｎｇ：Ｎａｔｉｏｎａｌ Ｄｅｆｅｎｃｅ Ｉｎｄｕｓｔｒｙ Ｐｒｅｓｓ，１９９０． ［４］ ＺＨＵ Ｒ。ＳＵＮ Ｄ，ＺＨ０Ｕ ＺＨ Ｙ．ｅｔ ａ１．Ａ ｌｉｎｅａｒ ｆｕｓｉｏｎ ａ１． ｇｏｒｉｔｈｍ ｆｏｒ ａｔｔｉｔｕｄｅ ｄｅｔｅｒｍｉｎａｔｉｏｎ ｕｓｉｎｇ ｌｏｗ ｃｏｓｔ ＭＥＭＳ－ ｂａｓｅｄ ｓｅｎｓｏｒｓ［Ｊ］．Ｍｅａｓｕｒｅｍｅｎｔ，２００７，４０：３２２，３２８． ［５］ 邓自立，王建国．带模型误差系统自适应Ｋａｌｍａｎ滤 波的虚拟噪声补偿技术［Ｊ］．信息与控制，１９８８，１： １ ４． ＤＥＮＧ Ｚ Ｌ．ＷＡＮＧ Ｊ Ｇ．Ａ ｆｉｃｔｉｔｉｏｕｓ ｎｏｉｓｅ ｃｏｍｐｅｎｓａｔｉｎｇ ｔｅｃｈｎｉｑｕｅ ｏｆ ａｄａｐｔｉｖｅ ｋａｌｍａｎ ｆｉｌｔｅｒｉｎｇ ｆｏｒ ｓｙｓｔｅｍｓ ｗｉｔｈ ｍｏｄｅｌ ｅｌＴｏｒｓ［Ｊ］．Ｉｎｆｏｒｍａｔｉｏｎ ａｎｄ Ｃｏｎｔｒｏｌ，１９８８，１：１－４． ［６］ ＧＥＢＲＥ—ＥＧＺＩＡＢＨＥＲ Ｄ，ＨＡＹＷＡＲＤ Ｒ Ｃ．ＰＯＷＥＬＬ Ｊ Ｄ．Ｄｅｓｉｇｎ ｏｆ ｍｕｌｔｉ—ｓｅｎｓｏｒ ａｔｔｉｔｕｄｅ ｄｅｔｅｒｍｉｎａｔｉｏｎ ｓｙｓｔｅｍｓ ［Ｃ］．ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎ ｏｎ Ａｅｒｏｓｐａｃｅ ａｎｄ Ｅｌｅｃｔｒｏｎｉｃ Ｓｙｓｔｅｍｓ，２００４，４０（２）：６２７－６４９． ［７］ ＸＩＥ Ｎ，ＱＩＡＮ Ｇ Ｌ，ＧＡＯ Ｈ Ｑ．Ａ ｍｅａｓｕｒｅｍｅｎｔ ｓｙｓｔｅｍ ｏｆ ｓｈｉｐ ｍｏｔｉｏｎｓ ｄｕｒｉｎｇ ｍｏｄｅｌ ｔｅｓｔｓ ａｎｄ ｆｕｌｌ ｓｃａｌｅ ｓｅａｋｅｅｐｉｎｇ ｔｉｒａｌｓ［Ｊ］．Ｊｏｕｒｎａｌ ｏｆ Ｓｈｉｐ Ｍｅｃｈａｎｉｃｓ，２００１，５（３）： ２６．３２． ［８］ ＰＲＡＤＯ Ｇ．Ｏｐｔｉｍａｌ ｅｓｔｉｍａｔｉｏｎ ｏｆ ｓｈｉｐ Ｓ ａｔｔｉｔｕｄｅｓ ａｎｄ ａｔ— ｔｉｔｕｄｅ ｒａｔｅｓ［Ｊ］．Ｏｃｅａｎ Ｅｎｇｉｎｅｅｒｉｎｇ，１９７９，ＯＥ－４（２）：５２— ５９． ［９］ 吴简彤，马文国，王利存，等．船舶捷联式惯导系统中 姿态矩阵算法研究［Ｊ］．导航，１９９７，１：８８．９４． ＷＵ Ｊ Ｔ．ＭＡ Ｗ Ｇ。ＷＡＮＧ Ｌ Ｃ．ｅｔ ａ１．Ｒｅｓｅａｒｃｈ ｏｎ ａ１． ｇｏｒｉｔｈｍ ｏｆ ａｔｔｉｔｕｄｅ ｍａｔｒｉｘ ｉｎ ｓｔｒａｐｄｏｗｎ ｉｎｅｒｔｉａｌ ｓｙｓｔｅｍ ｆｏｒ ｓｈｉｐ［Ｊ］．Ｎａｖｉｇａｔｉｏｎ，１９９７，１￣８８－９４ 作者简介 金鸿章，现为哈尔滨工程大学自动化 学院教授，博士生导师，主要研究方向为船 舶运动控制、复杂系统、智能控制。 Ｅ—ｍａｉｌ：ｈｅｕｊｉｎｈｚ＠１６３．ｃｏｍ Ｊｉｎ Ｈｏｎｇｚｈａｎｇ ｉｓ ａ ｐｒｏｆｅｓｓｏｒ ａｎｄ ｓｕｐｅｒｖｉ— ｓｏｒ ｆｏｒ ｄｏｃｔｏｒｉａｌ ｓｔｕｄｅｎｔ ｉｎ Ｈａｒｂｉｎ Ｅｎ￣ｎｅｅｒｉｎｇ ｒｅｓｅａｒｃｈ ｉｎｔｅｒｅｓｔｓ ｉｎｃｌｕｄｅ ｓｈｉｐ ｍｏｔｉｏｎ ｃｏｎｔｒｏｌ， ａｎｄ ｉｎｔｅｌｌｉｇｅｎｔ ｃｏｎｔｒｏ１． ＠１６３．ｃｏｍ 王帆，２００８年于哈尔滨工程大学获硕 士学位，现为哈尔滨工程大学博士研究生， 主要研究方向为船舶姿态测量与姿态控制。 Ｅ－ｍａｉｌ：ｗａｎｇｆａｎｈｅｕ＠１ ６３．ｃｏｒｎ Ｗａｎｇ Ｆａｎ ｒｅｃｅｉｖｅｄ ＭＳ ｄｅｇｒｅｅ ｆｒｏｍ Ｈａｒｂｉｎ Ｅｎｇｉｎｅｅｒｉｎｇ Ｕｎｉｖｅｒｓｉｔｙ ｉｎ ２００８，ａｎｄ ｎｏｗ ｈｅ ｉｓ ｓｔｕｄｙｉｎｇ ａｓ ａ ＰｈＤ ｓｔｕｄｅｎｔ．Ｈｉｓ ｒｅｓｅａｒｃｈ ｉｎｔｅｒｅｓｔｓ ａｒｅ ｓｈｉｐ ａｔｔｉ— ｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ ａｎｄ ｃｏｎｔｒｏ１． Ｅ－ｍａｉｌ－ｗａｎｇｆａｎｈｅｕ＠１６３，ｃｏｍ