用容积卡尔曼滤波算法进行集装箱吊具姿态估计
李胜永 季本山 张智华 张江南
摘要：为提高非线性模型下集装箱吊具姿态的估计精度，建立一个基于四元数和低精度高噪声传感器的集装箱吊具姿态估计非线性模型，运用容积卡尔曼滤波算法进行姿态估计.试验数据与高精度姿态传感器数据比较表明，采用容积卡尔曼滤波算法可以有效提高集装箱吊具姿态估计精度和稳定性.
关键词：集装箱吊具； 姿态估计； 容积卡尔曼滤波（CKF）； 四元数
中图分类号： U693.3
文献标志码： A
Abstract：In order to improve the estimation accuracy of container spreader attitude in a nonlinear model， a nonlinear model based on the quaternion and the sensor with low accuracy and high noise is established to estimate the container spreader attitude by the Cubature Kalman Filtering （CKF） algorithm. Through comparing the test data with the highaccuracy attitude sensor data， it shows that the CKF algorithm can effectively improve the accuracy and stability of the container spreader attitude estimation.
Key words：container spreader； attitude estimation； Cubature Kalman Filtering （CKF）； quaternion
0引言
集装箱吊具姿态调整是集装箱装卸效率提高的瓶颈，涉及到对位、着箱和防摇等3个重要环节.集装箱吊具姿态通常用旋转角、倾斜角和滚转角表示，它为操作司机提供吊具姿态信息，引导集装箱装卸操作，同时为控制系统提供姿态参数数据，保证装卸过程安全.
目前保证集装箱吊具姿态的措施有：
（1）通过力矩电机或液压油缸增加钢丝绳收放阻尼，不断地减小集装箱与吊具摇摆幅度，并目测姿态，完成装卸操作.该措施的缺点是只关注倾斜角，用于集装箱吊具防摇的精度低，不能集成到控制系统中.
（2）通过滑轮组八绳姿态纠正，使钢丝绳在小车方向的水平分力足够大，依靠此分力实现吊具在小车方向的倾斜角姿态纠正.该措施用于吊具防摇，但结构复杂，不能集成到控制系统中.
（3）通过各种传感器和检测元件检测吊具相对于小车的位置信息和吊具姿态信息，并反馈到控制系统.[1]经控制器处理后将姿态纠正信号输出到执行机构，属于较为典型的“控制信号反馈控制”闭环控制系统，多采用PID控制器.
目前大规模使用电子姿态控制系统（上述第3种措施）进行集装箱装卸，其缺点是：由于集装箱吊具利用钢丝绳和小车运行机构实现力传动，属于典型大惯性柔性负载，其姿态调整控制系统为时变非线性系统，姿态传感器精度低、噪声高，姿态调整耗时费力，简单的“控制信号反馈控制”闭环会出现噪声信号放大，从而导致姿态调整频繁，效率低下，操作司机可控感差.
随着控制理论和电子技术的发展，更多时变非线性系统分析方法用于各个领域与行业.[2]原较多用于飞机、卫星和导弹姿态控制的容积卡尔曼滤波（Cubature Kalman Filtering， CKF）算法在其他多个领域时变非线性的姿态和定位系统中取得很好效果.ARASARATNAM[3]提出的CKF算法构造2n个具有相同权重的容积点，由非线性系统方程传播这些点进而进行状态估计，保持原系统的时变性和非线性，同时具有二阶精度[4].王宏健等[5]将CKF算法应用于水下航行器的姿态和定位估计，取得很好效果；LIU等[6]将CKF算法运用于列车姿态和定位中，取得很好的精度.
鉴于此，本研究将CKF姿态估计环节引入控制系统（见图1）中，其基本思想是：运用CKF算法对上一环节的姿态估计和姿态测量信号进行计算，得出最优姿态估计，提供给控制器进行姿态调整控制.[78]该方法理论上可以达到两个效果：（1）提供给控制器的姿态控制信号是在原有姿态发展趋势上的最优估计，具有合理的超前分析和预判性质，使得控制系统具有姿态调整收敛属性，系统变得平稳；（2）降低集装箱吊具姿态的控制系统外的被动扰动（机械振动和风速影响）和测量监测单元的传感器自身噪声干扰，避免这些扰动或干扰在重复姿态调整过程中被逐渐放大，从而影响系统稳定性.
集装箱吊具姿态控制系统是大惯性柔性负载，目前在此领域内尚未见到CKF算法的应用.因此，本文用CKF算法对基于四元数法的集装箱吊具姿态模型进行求解，利用三轴陀螺仪和三轴加速度传感器从集装箱吊具姿态控制试验装置上获取传感器数据，将CKF算法应用于集装箱吊具姿态估计.为找到对比对象，将法国SBG Systems公司生产的SBG AHRS传感器作为集装箱吊具姿态监测标准，对比分析基于CKF算法的低精度、高噪声普通姿态监测传感器对集装箱吊具姿态的估计.
2集装箱吊具姿态估计系统建模
3试验结果及分析
试验中，将CKF算法集成到SMT32F103TB处理器中，利用串口将集装箱姿态数据传输到上位机，监控其状态并记录在csv格式文件中.上位机采样频率为400 Hz，状态变量xi初值为（0，0，0，1，0，0，0，0，0，0）T ，误差协方差矩阵的初值为100I10×10，差分区间为1.试验模型的初值阶段设置为静止启动阶段，计算协方矩阵和过程噪声协方差矩阵.此外，在同等条件下独立得到低精度、高噪声数据传感器情况下CKF算法的姿态估计和高精度SBG AHRS传感器输出的姿态.将两者进行对比分析，总结低精度、高噪声数据传感器情况下CKF算法的姿态估计误差.
试验结果见图3～8.
结果分析：
（1）旋转角、倾斜角和滚转角在姿态变化剧烈阶段具有较好的收敛性和较高的精度，体现出CKF在时变非线性状态下良好的滤波效果，其姿态估计不仅仅依赖于传感器观测系统.
（2）倾斜角在集装箱吊具平稳阶段出现时延特性的滞后误差，这是因为机械机构系统扰动噪声使集装箱吊具姿态发生微变，CKF算法机理在随机突发扰动阶段的劣势
导致上述特性出现.
不过，集装箱吊具平稳阶段主要关注质心的均布情况，反映在滚转角的监测上，这是引入集装箱吊具姿态估计方法的一个附加优势.
（3）各姿态角在整个集装箱吊具姿态估计曲线中不似SBG AHRS传感器曲线出现尖波，原因在于CKF算法机理可以有效抑制随机扰动，且直接反映到控制系统的控制器，使得集装箱吊具姿态调整更趋平稳.
（4）在集装箱吊具姿态控制过程中通常使用内部集成PID模块的PLC.PID模块具有自适应PID控制算法，根据文献[11]，PID控制系统在低频时鲁棒稳定性条件与PID参数无关.集装箱吊具是大惯
性柔性负载，在正常工作过程中通过试验测定的倾斜角频率为0. 04～1.02 Hz，旋转角交变频率为0.1～2 Hz，滚转角交变频率为0.2～0.8 Hz，根据文献[11]中的鲁棒性低频界限4 Hz，无论是SBG AHRS传感器还是采用CKF算法的低精度高噪声的通用传感器都可得到较好的鲁棒稳定性.需要指出的是，CKF算法通过对上一时刻姿态数值和这一时刻姿态测量作出最优估计，可以有效降低集装箱吊具系统扰动和传感器系统测量噪声，提高整个集装箱吊具系统的鲁棒稳定性（见图3，5和7）.
（5）在图4，6和8中：倾斜角的估计误差最大达到11.2°，超过3°的误差自纠正时间达3.1 s；旋转角估计误差达到7.3°，超过3°的误差自纠正时间为3.9 s；滚转角最大误差为0.5°，超过0.2°的误差自纠正时间达3.3 s.在大量的实物模型试验过程中上述估计误差均出现过多次，在进行相同系统参数MATLAB仿真过程中反而得到3°以内的误差.对比仿真环境与实物试验环境，可将出现上述问题的原因归结为：首先，实物仿真环境的模拟吊具质量较小，控制系统机械扰动和惯性扰动作用显著，超出文献[11]中的鲁棒稳定性条件.其次，SBG AHRS传感器具备100 Hz三维加速度和方向角刷新输出频率，高分辨率（8 mg/LSB）理论上能够分辨仅为10′的角度变化.而MPU6050分辨率为2 mg/LSB，可以分辨50′左右的角度变化.精度的差别带来累积测量误差，尤其在脱离大惯性体环境条件下表现突出.
4结束语
将CKF算法用于由低精度、高噪声传感器组成的集装箱吊具姿态估计系统中，并通过集装箱吊具模型反复进行试验数据验证.试验结果表明，CKF算法可以对传感器观测噪声和吊具姿态扰动噪声进行很好的过滤，比SBG AHRS高精度姿态传感器表现出更好的姿态估计数据精度，适用于当前集装箱吊具电子防摇系统、着箱和定位.
参考文献：
[1]金晓春， 李映新， 张寒， 等. 集装箱吊具智能防摇装置性能分析[J]. 铁道货运， 2013（11）： 4752.
[2]冯爱国， 徐晓苏. GPS辅助姿态计算的捷联陀螺罗经实现[J]. 上海海事大学学报， 2010， 31（4）： 1719.
[3]ARASARATNAM I. Cubature Kalman filtering： theory and applications[D]. Hamilton： McMaster University， 2009.
[4]FEMANDEZPRADES C， VILAVALLS J. Bayesian nonlinear filtering using quadrature and cubature rules applied to sensor data fusion for positioning[C]//IEEE Int Conf Communications， 2010： 15.
[5]王宏健， 傅桂霞， 李娟， 等. 基于强跟踪CKF的无人水下航行器SLAM[J]. 仪器仪表学报， 2013， 34（11）： 25422546.
[6]LIU Jiang， CAI Baigen， TANG Tao， et al. A CKF based GNSS/INS train integrated positioning method[C]//Int Conf Mechatronics and Automation， 2010： 16861689.
[7]孙妍， 鲁涤强， 陈启军. 一种基于强跟踪的改进容积卡尔曼滤波器[J]. 华中科技大学学报： 自然科学版， 2013， 41（S）： 451454.
[8]钱华明， 黄蔚， 葛磊， 等. 基于四元数平方根容积卡尔曼滤波的姿态估计[J]. 北京航空航天大学学报， 2013， 39（5）： 645648.
[9]张荣辉， 贾宏光， 陈涛， 等. 基于四元数法的捷联式惯性导航系统的姿态解算[J]. 光学精密工程， 2008， 16（10）： 19631969.
[10]SHUSTER M D. Survey of attitude representations[J]. J Astronautical Sciences， 1993， 41（4）： 439517.
[11]郑冰， 王诗宓. PID控制系统的鲁棒性分析与设计[J]. 信息与控制， 1989（6）： 1518.
（编辑贾裙平）
摘要：为提高非线性模型下集装箱吊具姿态的估计精度，建立一个基于四元数和低精度高噪声传感器的集装箱吊具姿态估计非线性模型，运用容积卡尔曼滤波算法进行姿态估计.试验数据与高精度姿态传感器数据比较表明，采用容积卡尔曼滤波算法可以有效提高集装箱吊具姿态估计精度和稳定性.
关键词：集装箱吊具； 姿态估计； 容积卡尔曼滤波（CKF）； 四元数
中图分类号： U693.3
文献标志码： A
Abstract：In order to improve the estimation accuracy of container spreader attitude in a nonlinear model， a nonlinear model based on the quaternion and the sensor with low accuracy and high noise is established to estimate the container spreader attitude by the Cubature Kalman Filtering （CKF） algorithm. Through comparing the test data with the highaccuracy attitude sensor data， it shows that the CKF algorithm can effectively improve the accuracy and stability of the container spreader attitude estimation.
Key words：container spreader； attitude estimation； Cubature Kalman Filtering （CKF）； quaternion
0引言
集装箱吊具姿态调整是集装箱装卸效率提高的瓶颈，涉及到对位、着箱和防摇等3个重要环节.集装箱吊具姿态通常用旋转角、倾斜角和滚转角表示，它为操作司机提供吊具姿态信息，引导集装箱装卸操作，同时为控制系统提供姿态参数数据，保证装卸过程安全.
目前保证集装箱吊具姿态的措施有：
（1）通过力矩电机或液压油缸增加钢丝绳收放阻尼，不断地减小集装箱与吊具摇摆幅度，并目测姿态，完成装卸操作.该措施的缺点是只关注倾斜角，用于集装箱吊具防摇的精度低，不能集成到控制系统中.
（2）通过滑轮组八绳姿态纠正，使钢丝绳在小车方向的水平分力足够大，依靠此分力实现吊具在小车方向的倾斜角姿态纠正.该措施用于吊具防摇，但结构复杂，不能集成到控制系统中.
（3）通过各种传感器和检测元件检测吊具相对于小车的位置信息和吊具姿态信息，并反馈到控制系统.[1]经控制器处理后将姿态纠正信号输出到执行机构，属于较为典型的“控制信号反馈控制”闭环控制系统，多采用PID控制器.
目前大规模使用电子姿态控制系统（上述第3种措施）进行集装箱装卸，其缺点是：由于集装箱吊具利用钢丝绳和小车运行机构实现力传动，属于典型大惯性柔性负载，其姿态调整控制系统为时变非线性系统，姿态传感器精度低、噪声高，姿态调整耗时费力，简单的“控制信号反馈控制”闭环会出现噪声信号放大，从而导致姿态调整频繁，效率低下，操作司机可控感差.
随着控制理论和电子技术的发展，更多时变非线性系统分析方法用于各个领域与行业.[2]原较多用于飞机、卫星和导弹姿态控制的容积卡尔曼滤波（Cubature Kalman Filtering， CKF）算法在其他多个领域时变非线性的姿态和定位系统中取得很好效果.ARASARATNAM[3]提出的CKF算法构造2n个具有相同权重的容积点，由非线性系统方程传播这些点进而进行状态估计，保持原系统的时变性和非线性，同时具有二阶精度[4].王宏健等[5]将CKF算法应用于水下航行器的姿态和定位估计，取得很好效果；LIU等[6]将CKF算法运用于列车姿态和定位中，取得很好的精度.
鉴于此，本研究将CKF姿态估计环节引入控制系统（见图1）中，其基本思想是：运用CKF算法对上一环节的姿态估计和姿态测量信号进行计算，得出最优姿态估计，提供给控制器进行姿态调整控制.[78]该方法理论上可以达到两个效果：（1）提供给控制器的姿态控制信号是在原有姿态发展趋势上的最优估计，具有合理的超前分析和预判性质，使得控制系统具有姿态调整收敛属性，系统变得平稳；（2）降低集装箱吊具姿态的控制系统外的被动扰动（机械振动和风速影响）和测量监测单元的传感器自身噪声干扰，避免这些扰动或干扰在重复姿态调整过程中被逐渐放大，从而影响系统稳定性.
集装箱吊具姿态控制系统是大惯性柔性负载，目前在此领域内尚未见到CKF算法的应用.因此，本文用CKF算法对基于四元数法的集装箱吊具姿态模型进行求解，利用三轴陀螺仪和三轴加速度传感器从集装箱吊具姿态控制试验装置上获取传感器数据，将CKF算法应用于集装箱吊具姿态估计.为找到对比对象，将法国SBG Systems公司生产的SBG AHRS传感器作为集装箱吊具姿态监测标准，对比分析基于CKF算法的低精度、高噪声普通姿态监测传感器对集装箱吊具姿态的估计.
2集装箱吊具姿态估计系统建模
3试验结果及分析
试验中，将CKF算法集成到SMT32F103TB处理器中，利用串口将集装箱姿态数据传输到上位机，监控其状态并记录在csv格式文件中.上位机采样频率为400 Hz，状态变量xi初值为（0，0，0，1，0，0，0，0，0，0）T ，误差协方差矩阵的初值为100I10×10，差分区间为1.试验模型的初值阶段设置为静止启动阶段，计算协方矩阵和过程噪声协方差矩阵.此外，在同等条件下独立得到低精度、高噪声数据传感器情况下CKF算法的姿态估计和高精度SBG AHRS传感器输出的姿态.将两者进行对比分析，总结低精度、高噪声数据传感器情况下CKF算法的姿态估计误差.
试验结果见图3～8.
结果分析：
（1）旋转角、倾斜角和滚转角在姿态变化剧烈阶段具有较好的收敛性和较高的精度，体现出CKF在时变非线性状态下良好的滤波效果，其姿态估计不仅仅依赖于传感器观测系统.
（2）倾斜角在集装箱吊具平稳阶段出现时延特性的滞后误差，这是因为机械机构系统扰动噪声使集装箱吊具姿态发生微变，CKF算法机理在随机突发扰动阶段的劣势
导致上述特性出现.
不过，集装箱吊具平稳阶段主要关注质心的均布情况，反映在滚转角的监测上，这是引入集装箱吊具姿态估计方法的一个附加优势.
（3）各姿态角在整个集装箱吊具姿态估计曲线中不似SBG AHRS传感器曲线出现尖波，原因在于CKF算法机理可以有效抑制随机扰动，且直接反映到控制系统的控制器，使得集装箱吊具姿态调整更趋平稳.
（4）在集装箱吊具姿态控制过程中通常使用内部集成PID模块的PLC.PID模块具有自适应PID控制算法，根据文献[11]，PID控制系统在低频时鲁棒稳定性条件与PID参数无关.集装箱吊具是大惯
性柔性负载，在正常工作过程中通过试验测定的倾斜角频率为0. 04～1.02 Hz，旋转角交变频率为0.1～2 Hz，滚转角交变频率为0.2～0.8 Hz，根据文献[11]中的鲁棒性低频界限4 Hz，无论是SBG AHRS传感器还是采用CKF算法的低精度高噪声的通用传感器都可得到较好的鲁棒稳定性.需要指出的是，CKF算法通过对上一时刻姿态数值和这一时刻姿态测量作出最优估计，可以有效降低集装箱吊具系统扰动和传感器系统测量噪声，提高整个集装箱吊具系统的鲁棒稳定性（见图3，5和7）.
（5）在图4，6和8中：倾斜角的估计误差最大达到11.2°，超过3°的误差自纠正时间达3.1 s；旋转角估计误差达到7.3°，超过3°的误差自纠正时间为3.9 s；滚转角最大误差为0.5°，超过0.2°的误差自纠正时间达3.3 s.在大量的实物模型试验过程中上述估计误差均出现过多次，在进行相同系统参数MATLAB仿真过程中反而得到3°以内的误差.对比仿真环境与实物试验环境，可将出现上述问题的原因归结为：首先，实物仿真环境的模拟吊具质量较小，控制系统机械扰动和惯性扰动作用显著，超出文献[11]中的鲁棒稳定性条件.其次，SBG AHRS传感器具备100 Hz三维加速度和方向角刷新输出频率，高分辨率（8 mg/LSB）理论上能够分辨仅为10′的角度变化.而MPU6050分辨率为2 mg/LSB，可以分辨50′左右的角度变化.精度的差别带来累积测量误差，尤其在脱离大惯性体环境条件下表现突出.
4结束语
将CKF算法用于由低精度、高噪声传感器组成的集装箱吊具姿态估计系统中，并通过集装箱吊具模型反复进行试验数据验证.试验结果表明，CKF算法可以对传感器观测噪声和吊具姿态扰动噪声进行很好的过滤，比SBG AHRS高精度姿态传感器表现出更好的姿态估计数据精度，适用于当前集装箱吊具电子防摇系统、着箱和定位.
参考文献：
[1]金晓春， 李映新， 张寒， 等. 集装箱吊具智能防摇装置性能分析[J]. 铁道货运， 2013（11）： 4752.
[2]冯爱国， 徐晓苏. GPS辅助姿态计算的捷联陀螺罗经实现[J]. 上海海事大学学报， 2010， 31（4）： 1719.
[3]ARASARATNAM I. Cubature Kalman filtering： theory and applications[D]. Hamilton： McMaster University， 2009.
[4]FEMANDEZPRADES C， VILAVALLS J. Bayesian nonlinear filtering using quadrature and cubature rules applied to sensor data fusion for positioning[C]//IEEE Int Conf Communications， 2010： 15.
[5]王宏健， 傅桂霞， 李娟， 等. 基于强跟踪CKF的无人水下航行器SLAM[J]. 仪器仪表学报， 2013， 34（11）： 25422546.
[6]LIU Jiang， CAI Baigen， TANG Tao， et al. A CKF based GNSS/INS train integrated positioning method[C]//Int Conf Mechatronics and Automation， 2010： 16861689.
[7]孙妍， 鲁涤强， 陈启军. 一种基于强跟踪的改进容积卡尔曼滤波器[J]. 华中科技大学学报： 自然科学版， 2013， 41（S）： 451454.
[8]钱华明， 黄蔚， 葛磊， 等. 基于四元数平方根容积卡尔曼滤波的姿态估计[J]. 北京航空航天大学学报， 2013， 39（5）： 645648.
[9]张荣辉， 贾宏光， 陈涛， 等. 基于四元数法的捷联式惯性导航系统的姿态解算[J]. 光学精密工程， 2008， 16（10）： 19631969.
[10]SHUSTER M D. Survey of attitude representations[J]. J Astronautical Sciences， 1993， 41（4）： 439517.
[11]郑冰， 王诗宓. PID控制系统的鲁棒性分析与设计[J]. 信息与控制， 1989（6）： 1518.
（编辑贾裙平）
