基于卡尔曼滤波的主动波浪补偿系统研究

３６　２０１５，５１（２１）　ＣｏｍｐｕｔｅｒＥｎｇｉｎｅｅｒｉｎｇ　ａｎｄＡｐｐｌｉｃａｔｉｏｎｓ计算机工程与应用　

基于卡尔曼滤波的主动波浪补偿系统研究　

解迎刚１，２，王晓　，曾佳佳　，侯金梦　

ＸＩＥ　Ｙｉｎｇｇａｎｇ　一，ＷＡＮＧ　Ｘｉａｏ　，　ＺＥＮＧ　Ｊｉａｊｉａ　，ＨＯＵ　Ｊｉｎｍｅｎｇ　

１．北京信息科技大学信息与通信工程学院，北京１００１０１　２．高动态导航技术北京市重点实验室，北京１００１０１　

３．北京科技大学计算机与通信工程学院，北京１０００８３　

１．Ｓｃｈｏｏｌ　ｏｆ　Ｉｎｆｏｒｍａｔｉｏｎ＆Ｃｏｍｍｕｎｉｃａｔｉｏｎ　Ｅｎｇｉｎｅｅｒｉｎｇ．Ｕｎｉｖｅｒｓｉｔｙ　ｏｆ　Ｂｅｉｊｉｎｇ　Ｉｎｆｏｒｍａｔｉｏｎ　Ｓｃｉｅｎｃｅ＆Ｔｅｃｈｎｏｌｏｇｙ，Ｂｅｉｊｉｎｇ　１００１０１，Ｃｈｉｎａ　

２．Ｂｅｉｊｉｎｇ　Ｋｅｙ　Ｌａｂｏｒａｔｏｒｙ　ｏｆＨｉｇｈ　Ｄｙｎａｍｉｃ　Ｎａｖｉｇａｔｉｏｎ　Ｔｅｃｈｎｏｌｏｇｙ，Ｂｅｉｊｉｎｇ　１００１０１，Ｃｈｉｎａ　３．Ｓｃｈｏｏｌ　ｏｆＣｏｍｐｕｔｅｒ＆Ｃｏｍｍｕｎｉｃａｔｉｏｎ　Ｅｎｇｉｎｅｅｒｉｎｇ，Ｕｎｉｖｅｒｓｉｔｙ　ｏｆ　Ｓｃｉｅｎｃｅ＆Ｔｅｃｈｎｏｌｏｇｙ　Ｂｅｉｊｉｎｇ，Ｂｅｉｊｉｎｇ　１０００８３，Ｃｈｉｎａ　

ＸＩＥ　Ｙｉｎｇｇａｎｇ，ＷＡＮＧ　Ｘｉａｏ，ＺＥＮＧ　Ｊｉａｊｉａ，ｅｔ　ａ１．Ａｃｔｉｖｅ　ｗａｖｅ　ｃｏｍｐｅｎｓａｔｉｏｎ　ｓｙｓｔｅｍ　ｂａｓｅｄ　ｏｎ　Ｋａｌｍａｎ　ｆｉｌｔｅｒ　ｔｏ　ｇｒｉｎｄ．　

Ｃｏｍｐｕｔｅｒ　Ｅｎｇｉｎｅｅｒｉｎｇ　ａｎｄ　Ａｐｐｌｉｃａｔｉｏｎｓ，２０１５，５１（２１）：３６－４０．　

Ａｂｓｔｒａｃｔ：Ｔｈｉｓ　ｐａｐｅｒ　ｃｏｎｄｕｃｔｓ　ａ　ｍｏｒｅ　ｉｎ—ｄｅｐｔｈ　ｅｘａｍｉｎｅ　ｆｏｒ　ｔｈｅ　ｈｅａｖｅ　ｍｏｔｉｏｎ　ｏｆ　ｔｈｅ　ｖｅｓｓｅｌ　ａｎｄ　ａｃｔｉｖｅ　ｈｅａｖｅ　ｃｏｍｐｅｎｓａｔｉｏｎ　

ｗｉｔｈ　ｔｈｅ　ｓｕｒｇｅ．Ｉｔ　ｉｓ　ｉｎｔｅｎｄｅｄ　ｔｏ　ｓｏｌｖｅ　ｔｈｅ　ｔｒａｉｌｉｎｇ　ｓｕｃｔｉｏｎ　ｈｏｐｐｅｒ　ｄｒｅｄｇｅｒｓ　ｒａｋｅ　ｔｕｂｅ　ｔｅｒｍｉｎａｌ　ｌｅｖｅｌ　ｈｅｉｇｈｔ　ｓｔａｂｉｌｉｔｙ．Ｉｔ　ｅｓｔａｂ．　１ｉｓｈｅｓ　ｃｏｍｐｅｎｓａｔｉｏｎ　ｗａｖｅ　ｆｏｒｅｃａｓｔｉｎｇ　ｓｙｓｔｅｍ，ｔｈｒｏｕｇｈ　ｔｈｅ　ｍｏｖｅｍｅｎｔ　ｏｆ　ｓｈｉｐ　ｍｏｔｉｏｎ　ｒｅｆｅｒｅｎｃｅ　ｕｎｉｔ　ｍｅａｓｕｒｉｎｇ　ｔｈｅ　ｖｅ￣ｉｃａｌ　ｖｅｌｏｃｉｔｙ　ａｎｄ　ａｃｃｅｌｅｒａｔｉｏｎ．Ｔｈｅ　ｍａｉｎ　ｃｏｎｔｒｏｌ　ｕｎｉｔ　ｉＳ　ｃｏｍｂｉｎｅｄ　ｗｉｔｈ　Ｋａｌｍａｎ　ｆｉｌｔｅｒ　ａｌｇｏｒｉｔｈｍ　ｔｏ　ｐｒｏｃｅｓｓ　ｄａｔａ　ｉｎ　ａｃｃｏｒｄａｎｃｅ　

ｗｉｔｈ　ｔｈｅ　ｍａｔｈｅｍａｔｉｃａｌ　ｍｏｄｅｌ　ｅｓｔａｂｌｉｓｈｅｄ．Ｉｔ　ａｄｊｕｓｔｓ　ｔｈｅ　ｓｐｅｅｄ　ｏｆ　ｔｈｅ　ｒｏｐｅ　ｔｈｒｏｕｇｈ　ｔｈｅ　ｉｍｐｌｅｍｅｎｔｉｎｇ　ａｇｅｎｃｉｅｓ　ｔｏ　ｃｏｍｐｌｅｔｅ　

ｔｈｅ　ｒｅｔｒａｃｔａｂ１　ｒａｋｅ　ｔｕｂｅ　ｔｅｒｍｉｎａ１　ｃｏｎｔｒｏ１　ｃｏｍｐｅｎｓａｔｉｏｎ，ＳＯ　ｔｈａｔ　ｔｈｅ　１ｅｖｅｌ　ｏｆ　ｔｈｅ　ｒａｋｅ　ｈｅａｄ　ｒｅｍａｉｎｓ　ｓｔａｂｌｅ　ｄｕｒｉｎｇ　ｏｐｅｒａｔｉｏｎ．　Ｗａｖｅ　ｃｏｍｐｅｎｓａｔｉｏｎ　ｐｒｅｄｉｃｔｉｏｎ　ｓｙｓｔｅｍ　ｅｓｔａｂｌｉｓｈｅｄ　ｗｉｔｈ　ｉｎｄｅｐｅｎｄｅｎｔ　ｌｅａｒｎｉｎｇ　ａｂｉｌｉｔｙ，ｃａｎ　ｕｓｅ　ｔｈｅ　ｍｅａｓｕｒｅｄ　ｄａｔａ　ｔｏ　ｉｍｐｒｏｖｅ　ｔｈｅ　ａｃｃｕｒａｃｙ　ｏｆ　ｓｅｌｆ－ｃｏｒｒｅｃｔｉｏｎ，ｔｈｅｒｅｆｏｒｅ　ｗｉｔｈ　ｈｉｇｈ　ｐｒａｃｔｉｃａｌ　ｖａｌｕｅ．　

Ｋｅｙ　ｗｏｒｄｓ：ａｃｔｉｖｅ　ｗａｖｅ　ｃｏｍｐｅｎｓａｔｉｏｎ；Ｋａｌｍａｎ　ｆｉｌｔｅｒ；ｐｒｅｄｉｃｔｉｏｎ　ｏｆ　ｗａｖｅ　ｃｏｍｐｅｎｓａｔｉｏｎ　ｓｙｓｔｅｍ　

摘要：针对船舶随涌浪进行的升沉运动进行较为深入的研究，旨在采用主动式波浪补偿解决耙吸式疏浚船耙管终　端水平高度稳定问题。建立波浪补偿预测系统，通过运动参考单元测量船舶运动竖直方向速度和加速度，主控制单　元根据所建立的数学模型结合卡尔曼滤波算法处理数据，通过执行机构调整绳索收放速度完成耙管终端控制补偿，　

使耙头水平高度在工作过程中保持稳定。所建立的波浪补偿预测系统具有自主学习能力，可以不断运用实测数据　自我修正以提高精度，具有很高的实用价值。　

关键词：主动式波浪补偿；卡尔曼滤波；波浪补偿预测系统　文献标志码：Ａ　中图分类号：ＴＰ３９１　ｄｏｉ：１０．３７７８￣．ｉｓｓｎ．１００２．８３３１．１５０２．００４２　

１　引言　

耙吸式疏浚船是一种边走边挖，且挖泥、装泥和卸　泥等全部工作都由自身来完成的疏浚船，耙吸式疏浚船　在工作过程中由于海浪造成的复杂波动状态（如纵摇、　

升沉运动等）会造成耙头位置运动频繁　。在海浪运动　中由于涌浪的影响，使得波浪运动过程无法用固定波形　

描述，从而无法对耙吸式疏浚船耙管终端水平高度进行　准确预测，造成工程船作业困难　。耙吸式疏浚船工作　

模式如图１所示。　

舰船在海洋环境下要进行安全有效的作业任务，操　

基金项目：北京信息科技大学学校科研基金项目（Ｎｏ．１５２５００８）；高动态导航技术北京市重点实验室开放课题（Ｎｏ．ＨＤＮ２０１５００５）；　国家自然科学基金（Ｎｏ．６１２６１１６０４９７）；北京市科技计划课题（Ｎｏ．Ｚ１３１１００００５３１３００９）。　作者简介：解迎刚（１９７８一），男，博士，讲师，研究领域为惯性导航、物联网及智能数据处理技术；王晓（１９９１一），男，研究生，研究　领域为物联网技术；曾佳佳（１９９ｌ一），女，研究生，研究领域为信息处理技术；侯金梦（１９９０一），女，研究生，研究领域为　智能决策技术。Ｅ—ｍａｉｌ：ｙｉｎｇｇａｎｇｘｉｅ＠１２６．ｃｏｍ　收稿日期：２０１５．０２．０３　修回日期：２０１５－０４—２７　文章编号：１００２．８３３１（２０１５）２１—００３６．０５　ＣＮＫＩ网络优先出版：２０１５－０８．０４，ｈｔｔｐ：／／ｗｗｗ．ｃｎｋｉ．ｎｅｔ／ｋｃｍｓ／ｄｅｔａｉｌ／１１．２１２７．ＴＰ．２０１５０８０４．１１０７．００１．ｈｔｍｌ

　ＣｏｍｐｕｔｅｒＥｎｇｉｎｅｅｒｉｎｇ　ａｎｄＡｐｐｌｉｃａｔｉｏｎｓ计算机工程与应用　

放绳索速度Ｖ＝　ｄｌ，代入公式（５）和（１）得　ｄｆ　

Ｖ＝　Ｌｆ　ｓｉｎ　ｄ　

、　出　

ＮｔＬｉｔａｎ伐　上√Ⅳｆ２＋三　一２ＮｉＬ　ｃｏｓａ　Ｕ　——＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝　

．“（　一１）ｔ。　（６）　Ｌ　３Ｎ　＋Ｌ　一２Ｎ｝Ｌｉ　ｃｏｓ强　

采用卡尔曼滤波，首先引入一个离散控制过程的系　

统，它由系统的过程方程和观测方程共同表示　。　

过程方程：　

（七）＝ＡＸ（ｋ—１）＋ＢＵ（ｋ）＋　（尼）　（７）　观测方程：　

Ｚ（ｋ）＝ｕｘ（ｋ）＋ｖ（ｋ）　（８）　

（７）、（８）两式中，Ｘ（ｋ）是ｋ时刻的系统状态，Ｕ（ｋ）是ｋ　

时刻对系统的控制量。　和　是系统参数，对于多模　

型系统，　、　为矩阵。ｚ（ｋ）是ｋ时刻的测量值，　是　

测量系统的参数，对于多测量系统，日为矩阵。Ｗ（ｋ）和　

Ｖ（ｋ）分别表示过程和测量的噪声，它们被假设成高斯白　

噪声，其协方差分别是Ｑ、　（这里假设它们不随系统状　

态变化而变化）。利用系统的过程模型，来预测下一状　

态的系统。假设现在的系统状态是ｋ，根据系统的模　型，可以基于系统的上一状态而预测出现在状态：　

Ｘ（ｋ］ｋ一１）＝ＡＸ（ｋ一１ｌｋ一１）＋　（　）　（９）　

公式（９）中，Ｘ（ｋｌｋ一１）是利用上一状态预测的结　

果，Ｘ（ｋ一１Ｉｋ一１）是上一状态最优的结果，Ｕ（ｋ）为现在　

状态的控制量，如果没有控制量，它可以为０。　目前，系统的结果已经更新，现在对与Ｘ（ｋｌｋ一１）相　

对应的协方差进行更新，用Ｐ表示：　

Ｐ（ｋｌｋ一１）＝ＡＰ（ｋ一１ｌｋ一１）Ａ　＋Ｑ　（１Ｏ）　式（１０）中，Ｐ（ｋｌｋ一１）是Ｘ（ｋｑｋ一１）对应的协方差，　

Ｐ（ｋ一１Ｉｋ一１）是ｘ（ｋ—ｌｌｋ一１）对应的协方差，Ａ　表示　

的转置矩阵，Ｑ是系统过程的协方差。式（９）、（１０）也　

就是对系统的预测。结合预测值和测量值可以得到现　

在状态（ｋ）的最优化估算值Ｘ（ｋｌｋ）：　

Ｘ（ｋＬｋ）＝Ｘ（ｋｌｋ一１）＋　（七）・（ｚ（　）一ＨＸ（ｋ　Ｌｋ一１））　（１１）　

Ｋｇ（ｋ）＝丽Ｐ　（ｋ　ｌｋ－而１）Ｈ＇　（１２）　

其中　为卡尔曼增益。为使得卡尔曼滤波不断运行下　

去直到系统过程结束更新ｋ状态下Ｘ（ｋｌｋ）的协方差：　

Ｐ（ｋｌｋ）：（Ｉ－Ｋ（ｋ）Ｈ）Ｐ（ｋｌｋ一１）　（１３）　

其中ｊ为１的矩阵，对于单模型单测量，Ｊ：１。当系统　

进入　＋１状态时，Ｐ（ｋｌｋ）就是式（７）的Ｐ（ｋ一１ｌｋ～１）。这　

样，算法就可以自回归地运算下去。　由上述卡尔曼滤波过程可知波浪补偿系统的状态　

方程，式（９）中　＝　（　为单位阵），易得：　Ｖ（ｎｌｎ一１）＝Ｖ（　一１ｌｎ一１）＋　Ｎ　Ｌ　ｔａｎ　——　＝＝＝＝兰　＝＝＝＝＝＝＝口（　一１）　＋　（，２）（１４）　ｒ———　——————：　—————————————————————一’’、。　一，’Ｕ　＼。，　、　Ｌ　Ｎ　＋Ｌ　一２Ｎ．ＬｉＣＯＳ值　

其中，Ｖ（ｎｌｎ一１）为第ｎ一１时刻对第ｎ时刻绳索收放速　

度　的预测值，Ｖ（ｎ一１ｌｎ一１）为第ｎ一１时刻的最优估计　

值；ｗ（ｎ）为高斯白噪声，描述了系统的不确定性，且有：　

Ｅ（　（，ｚ））＝０　

Ｅ［Ｈ，（　）’　（　）　］＝ｇ　（　—　）　ｑ为ｗ（ｎ）的方差强度。　（１５）　

—　：　竺　：＝：　一１）ｔ　为速度变化量。　√　＋　一２｜Ⅳ＝ｆ三ｆ　ｃｏｓ　

根据Ｋａｌｍａｎ滤波，在测量方差已知情况下能够从　

一系列存在测量噪声的数据中估计动态系统状态”　１。　

在本系统中第ｎ一１时刻对第ｎ时刻方差进行估计，加　

入过程误差Ｑ：　

Ｐ（ｎ［ｎ一１）＝ｐ（ｎ一１ｌｎ一１）＋Ｑ　（１６）　

公式（１　１）（１２）实现了对系统状态的基本预测，为使　预测精度进一步提高，还需具备自主学习的能力。引入　

卡尔曼增益　，其意义是作为权值衡量最优估计值与　

预测值的接近程度　，根据第ｎ一１时刻对第ｎ时刻方差　预测值占其与测量误差尺和的比例确定。　

Ｘｇ（　…）　

然后利用卡尔曼增益确定第ｎ时刻最优估计值在　

预测值和实测值问的位置。　由式（８）中日＝　（　为单位阵）易得：　

Ｖ（ｎｌｎ）＝Ｖ（ｎｌｎ一１）＋Ｋ（　）［ｚ（　）一Ｖ（ｎｌｎ一１）】　（１８）　

最后按最优估计值与绳索收放速度实测值（通过光　

电测速传感器测出）的接近程度［１一Ｋ（ｎ）］确定第　时　

刻的最优估计方差。　

Ｐ（ｎｌｎ）＝［１一Ｋ（　）］Ｐ（　Ｉ，２—１）　（１９）　

公式（１７）～（１９）实现了自主学习的过程。　以上式（７）～（１９）构成针对耙吸式疏浚船的主动波　

浪补偿系统数学模型。式中各个参量的含义如下所示：　

ａ为ＭＲＵ测量得到船舶升沉运动加速度，Ｕ为船　舶升沉运动速度的预测值，　为控制耙管位置绳索收放　速度预测值，ｚ为控制耙管位置绳索收放速度实测值，