变分配系数联邦Kalman滤波在塔机定位中的应用

王 昊，陈志梅，张井岗，孙 辉

(太原科技大学电子信息工程学院，太原030024)

信息融合(也称数据融合)是将来自多个传感器的多源信息进行综合处理以得出更准确、可靠的结论。Kalman滤波技术是多传感器信息融合技术之一，可及时实现对目标精确定位。Kalman滤波技术广泛应用于定位、导航系统[1-3]。普通Kalman滤波技术不仅对于标量估计的平稳系统，对于多输入多输出的非平稳的时变系统也出现无偏最小的方差估计[4-7]。但就结构复杂、关系性强烈的情况，普通Kalman滤波技术对其信息的处理效果并不理想。于是，联邦 Kalman滤波技术得到研究发展[8-10]，与普通Kalman滤波相比，联邦Kalman滤波技术增加了合理的关系约束。联邦Kalman滤波技术由多个局部Kalman滤波器组成，各局部滤波器应用不同方式对同一组变量进行检测，两滤波结果在联邦滤波过程中达到相互约束。

但目前联邦Kalman滤波循环中，反馈信息的分配系数取值并不十分精确，仅根据现象抽取经验值，运行过程中不进行调节。对于复杂关系、大噪声影响系统，分配系数的误差较大，将严重影响到最终效果。而且，Kalman滤波技术在工业领域占很大比例的重型机械中没有得到广泛应用。

对于启动、制动和进行耦合运动时，机构和结构都将承受强烈冲击振动的塔式起重机，精确定位便是灵活、高效控制塔机的一大前提。塔机系统中由于小车的加减速、负载的提升以及风力等外部干扰影响导致机臂、小车与负载不能精确到达目标位，严重影响了生产效率的提高、增加了事故发生的可能性，且负载定位为三维定位，与机臂、小车和绳长均有复杂且密切的关系[11]。合理分配系数受噪声很大影响，因此，在塔机定位中分配系数的取值非常重要。本文针对塔式起重机的运行系统，提出一种新的基于联邦Kalman滤波技术的塔机定位方法——变分配系数联邦Kalman滤波法，设计算法，使分配系数在联邦Kalman滤波系统的循环滤波过程中随噪声的不断变化而不断调节，实现各局部滤波器噪声相互约束，不会产生过大估计值偏差，实现塔机的精确定位。

1 联邦Kalman滤波

1.1 Kalman 滤波算法

Kalman滤波算法对系统描述归结为:状态方程、观测方程和最初条件。由于实时递推性质，系统的最初条件与实际相差远近并不重要。算法如下[12]:

系统状态方程与检测方程由下式表示:

其中，A为状态转移矩阵，B为量测矩阵。W为重要的过程噪声项——具有零均值、正定协方差矩阵为Q的高斯白噪声，它体现的是系统状态的实际性。检测方程中，V与W相似，为观测噪声。滤波器递推转变最重要的增益K:

k时刻状态与滤波误差协方差的最优估计值:

1.2 联邦Kalman滤波

联邦Kalman滤波框图如图1所示。将各局部Kalman滤波器的滤波结果进行联邦信息融合，得出k时刻最终的最优估计，再将最优估计反馈回局部滤波器应用于k+1时刻的滤波[2]。

图1 联邦Kalman滤波框图Fig.1 Federal Kalman filtering diagram

最终融合误差协方差P与最优估计的算法如下:

最优估计反馈，对各局部滤波器的估计值进行重置算法如下:

其中，bi为联邦信息融合分配系数，且b1+b2+…+bn=1(其中n为局部滤波器总数)。分配系数体现的是各局部滤波系统在最终联邦融合中所占比例。噪声越小，局部滤波器的滤波结果越接近于真实值，它的分配系数bi则越大。

2 塔机定位及Kalman滤波方法的改进

2.1 塔机结构特点及其联邦Kalman定位

对于塔式起重机，不仅存在机臂、小车、绳长和负载等一系列互相联系的运动性变量，而且塔机运行环境中振荡、风流等因素对系统产生的噪声也很大。塔机的结构模型如图2所示。

图2 塔式起重机结构图Fig.2 The structure of tower crane

如图2所示，γ为机臂转角，x为小车与臂根距离，L为吊绳的长度，H为负载高度，θ、Φ是吊绳偏离垂直位置的两个摆角。

影响塔机定位的因素为以下几点:

①机臂以臂根为圆心点左右转动，很明显会引起小车与吊绳的同时运动，因此确定机臂转角是定位重点之一。引入定位变量——机臂转角γ、转速v1.由式(1)、式(2)建立对机臂转角定位的状态方程与检测方程:

再经式(3)、式(4)、式(5)便可对不同时刻的机臂转角状态进行循环最优估计。

②小车在臂根和臂尖之间运动，带动吊绳前后运动，因此确定小车在机臂上的位置也很重要。于是引入定位变量——小车与臂根距离x、小车的运动速度v2.对小车的定位过程与上面机臂转角定位类似。

③吊绳通过根部收绳或放绳改进负载的高低。吊绳是与负载的直接链接物，因此吊绳长短同样相当重要。引入第三组定位变量——绳长L、吊绳下放(或上升)速度v3.对吊绳长度的定位过程与机臂转角定位亦相同。

④负载定位是塔机定位系统最直接、最重要的目标。但由于其同时受到机臂、小车和吊绳长度等众多因素影响，如图2所示产生了θ、Φ几个摆角，引起负载各位置变量很大的噪声，因此对负载定位需要同时充分考虑各影响因素;其次负载为三维运动体，对负载准确定位，便需要采用n个能充分反映负载当前位置的变量，同时也要尽量缩小n值以简化系统。于是，引入负载的直接定位变量——以臂根为圆心的负载运动转角γ'(近似于机臂转角γ)和转速v1'、负载与机架距离x'(近似于小车与臂根的距离x)和速度v2'、负载高度H(近似于机架高度与吊绳长度的差值200-L)与下放速度v3'.

负载的定位过程如下:

首先，将①、②、③中机臂、小车、吊绳各自检测到的数据经几何校正推导出负载的位置数据，聚集到同一Kalman滤波器中，建立一新滤波系统——负载定位系统，作为联邦Kalman滤波系统的第一个局部滤波器，根据经验取定其分配系数为b1，其状态与检测方程如(11).

在第一局部滤波器中，首先应用式(3)、式(4)、式(5)算法导出第一个局部最优估计:X1(k|k)=

其次，对④中负载的的一系列直接定位变量采用适当传感器直接采集到其测量值(γ'、v1'、x'、v2'、H、v3')，建立起联邦Kalman滤波系统的第二个局部滤波器，分配系数为b2=1-b1，其状态与检测方程如下:

系统式(11)、式(12)格式相似，但其主要差别在于噪声取值各不相同，联邦Kalman滤波技术的主要功能便是不同噪声系统的相互约束，产生精确定位。同样，在第二局部滤波器中，首先应用式(3)、式(4)、式(5)算法导出其局部最优估计:P2(k|k)与X2(k|k)=[γ'(k|k)v'1(k|k)x'(k|k)v'2(k|k)H'(k|k)v'3(k|k)]'.

再根据式(6)、式(7)即可导出整体联邦滤波系统的最优估计:最后根据式(8)、式(9)与两分配系数将整体最优估计反馈，对各局部滤波器估计值进行重置，进行下一步滤波:P1(k|k)=b－11·P(k|k)、P2(k|k)=b－12·P(k|k)、X1(k|k)=X2(k|k)=X(k|k).

2.2 联邦Kalman滤波定位方法的改进

由于联邦Kalman滤波系统中分配系数体现的是各局部滤波系统在最终联邦融合中所占比例。噪声越小，局部滤波器的滤波结果越接近于真实值，它的分配系数则越大，因此噪声所占比例决定分配系数大小。

目前仅仅应用经验或不同系统之间噪声标准差比例来确定其分配系数，并在整个滤波过程中应用此值。但是现实滤波系统中，不同时刻，系统之间状态噪声大小比例完全保持不变的可能性趋近于零。因此，当前联邦Kalman滤波技术中分配系数的取值特别重要，尤其在噪声值及其噪声变化值都较大的塔机负载定位中，会明显影响到最终定位效果。

因此，本文提出一种分配系数的分时确定法:就每时刻的噪声大小比例来确定此时刻的分配系数值，使分配系数在滤波系统中进行循环变动，实现各次滤波分配系数的精确性。具体分配系数的计算公式如下:

V1k、V2k为对k时刻两个局部滤波系统状态噪声协方差矩阵的对角线元素的提取并构成的新矩阵，状态噪声协方差矩阵是能评定某时刻系统噪声大小的权威标准。接下来，应用对V1k、V2k的计算求出k时刻的联邦分配系数:

式中‘tr’函数实现的是矩阵对角线元素的求和。式(15)、式(16)便能根据两局部滤波系统在k时刻的实际状态噪声大小与比例，确定适用于k时刻的精确的分配系数b1k、b2k，且满足b1k+b2k=1.用其取代上面滤波系统中的定值b1、b2，应用到k时刻的联邦滤波中，会明显提高负载在此时刻的定位精度。

3 仿真研究

为验证本方法的正确性及有效性，对塔式起重机系统进行了仿真研究。

对负载直接检测，建立原始联邦Kalman滤波模型，工作时对各变量的噪声取经验值，当各传感器都正常工作一段时间(定位采样次数k=300;周期T=1 s)后，仿真结果如图 3(a)、4(a)、5(a)所示。

图3 吊绳长度Kalman滤波的定位结果Fig.3 The positioning results of Kalman filter with the sling length

图4 小车与臂根距离Kalman滤波的定位结果Fig.4 The positioning result of Kalman filter with the distance between car and arm root

图5 机臂转角Kalman滤波的定位结果Fig.5 The positioning result of Kalman filter with the arm-turning angel

将独立的机臂、小车、吊绳检测数据相结合，建立一新的滤波系统，作为局部Kalman滤波器1.将上面对负载的直接检测装置作为局部滤波器2.采用改进的联邦Kalman滤波算法对系统进行仿真，仿真结果如图3(b)、4(b)、5(b)所示。从仿真结果可以看出，普通滤波方法定位结果虽在期望值周围运动，但定位次数超过100，便有发散趋势(平均偏离值可达10 m、15°)，精确度不够，而本文的方法在保留原有效果的基础上，定位结果更加接近于期望值，偏离值大幅度降低，无发散趋势，定位效果明显提高。

4 结论

通过理论研究与仿真结果可以看出，本文提出的方法可使联邦卡尔曼滤波最优估计值精度得到提高，是对现有联邦Kalman算法的一大改进[13-14]。改进的联邦Kalman滤波算法应用于类似塔机定位这样结构复杂、噪声严重的系统，定位效果明显改善，具有较好的工程应用价值。

[1]于德新，杨兆升.基于卡尔曼滤波的GPS/DR导航信息融合方法[J].交通运输工程学报，2006，6(2):65-69.

[2]韩志玲.基于GPS和GPRS的公交车辆管理系统的设计与实现[J].太原科技大学学报，2006，27(5):347-351.

[3]MA Z X，LIU Z Y，CHEN M.Application of Adaptive Kalman Filtering Based on Fuzzy Logic to the Integrated GPS/INS Navigation[J].Information and Control，2006，35(4):457-461.

[4]ANDREAS MAIER，GERT F，TTOMMER.Comparision of Centralized and Decentralized Kalman Filter for SAR/TRN/GPS/INS Integration[C〛//International Technical Meeting，2010，25(1):74-80.

[5]HU DANDAN，GAO QINGJI.Application of Fuzzy Federal Kalman Filtering in the Airport Automatic Docking Guidance System[C〛//World Congress on Intelligent Control and Automation(IEEE)，2008，25(7):3576-3580.

[6]HAO ZHENHAI，HUANG SHENGGUO.Data Fution Approach Based on High Precision Barometric Altimeter and GPS[C〛//School of Civil Aviation(IEEE)，2009，23(5):1-4.

[7]SUN XIAO-JUN，GAO YUAN，DENG ZI-LI，et al.Multi-model information fusion Kalman?ltering and white noise deconvolution[J].Information Fusion，2010，11(4):163-173.

[8]CHENG G，LIU W D，WEI S J.Methods for Estimation and Prediction of Maneuverable Target Based on Kalman Filtering[J].Computer Simulation，2006，23(1):8-10.

[9]赵明辉，陈志梅，孟文俊.基于遗传算法的起重机模糊控制研究[J].太原科技大学学报，2010，31(5):364-368.

[10]裴福俊，居鹤华，崔平远.基于联邦卡尔曼滤波的月球车自主导航方法[J].宇航学报，2009，30(4):1429-1434.

[11]卢恒惠，张盛，汪浩，等.基于联邦Kalman滤波的 Wi-Fi/GPS车辆组合定位系统[J].清华大学学报，2011，51(3):420-423.

[12]WU JIANHONG，ZHANG HONGCAI.Data fusion algorithm design of GPS/IMU based on fuzzy adaptive federated Kalman filter[J].Image and Signal Processing(IEEE)，2009，17(10):1-4.

[13]谷立臣，董建国，张优云，邱大谋.基于信息融合的塔机运行状态监测系统[J].工程机械，2000(3):12-14.

[14]张倩，欧壮杰.基于卡尔曼滤波的数据融合技术在系统航迹中的研究与应用[J].航空科学技术，2009(1):14-18.