基于数据融合算法的四轮机器人姿态估计

陈 佳，游晓明

(上海工程技术大学 电子电气工程学院，上海 201620)

基于数据融合算法的四轮机器人姿态估计

陈 佳，游晓明

(上海工程技术大学 电子电气工程学院，上海 201620)

针对四轮机器人传感器实际获取数据误差过大的问题，设计了一种数据融合算法来进行四轮机器人姿态估计并进行实现。文中设计了一种联合卡尔曼滤波和一阶互补滤波的数据融合模型，并将其应用于机器人角度传感器上来处理实际得到的数据，使其更加贴合理想值从而方便后续使用。在3种不同环境下进行实验，实验结果证明，这种数据融合方法能较准确地进行角度校正、平滑数据，且计算量小、效率较高，适用于实时数据的处理，具有实际应用前景。

机器人；数据融合；卡尔曼滤波；一阶互补滤波；角度校正

定位技术能够获取移动机器人在任何时刻的相对位置，是实现导航和保证机器人自主导航的关键[1]。感知环境是移动机器人定位的重要一环，对得到的粗糙数据进行处理可以提高传感器数据的准备性。文献[2]指出以数据融合方式处理数据，符合机器人的高效化与精准化发展趋势，文献[3]指出这种方式在室内定位具有优势。 针对于数据融合的研究已取得较大成就，现有的传感器数据融合算法主要包括：加权平均法、卡尔曼滤波[4]、贝叶斯推理[5]、粒子滤波[6]、模糊逻辑[7]、神经网络[8]等算法。考虑到移动机器人动态收集数据的效率要求，卡尔曼滤波与一阶互补滤波被选用建立数据融合模型。卡尔曼滤波适用于动态环境中的传感器信息实时融合，具有依赖于前一状态的递推特性，这种递推特性加快了其运算速度，可用于非线性噪声情况下[9]。文献[10]指出卡尔曼滤波具有自适性，可以适应动态环境。一阶互补滤波的结构非常简单，不需要繁杂的计算就能得出相对比较好的融合效果。

由此可见，针对机器人行动状态的动态变化，数据融合模型的子项选用卡尔曼滤波和一阶互补滤波具有实际应用意义。

1 相关工作

1.1 卡尔曼滤波算法

卡尔曼滤波可用于解决在最优估计、预测、噪声滤除和最优操作等方面的问题[11]。而卡尔曼滤波器是一种由数字计算机实现的递推算法，每个递推周期中包含被估计的量的时间更新周期和量测周期两个过程。时间更新由上一步的量测更新结果和设计卡尔曼滤波器时的先验信息确定。因此，量测更新可看作是卡尔曼滤波的输入，估计值可看作是输出[12]。输入输出之间由时间更新和量测更新算法联系。

进行卡尔曼滤波首先需要构造考虑噪声的数学模型，当噪声是高斯白噪声时，卡尔曼滤波可以给出最佳估计值[13]。然后再通过量测信息与前一步估计信息的加权平均得到现在的最优估计。其中权值等参数根据过程的进展而改变，其同样是个动态的过程

X[K]=X[K-1]+K[K]×(Z-X[K-1])

(1)

式(1)是卡尔曼滤波算法的核心，体现卡尔曼滤波前后状态的依赖性及加权平均的思想。算法通过结合当前的测量值对上一时刻预测的最优估计再次进行校准，从而得到更接近实际的更优化数据。

经过卡尔曼滤波，传感器所获取的受外界噪声、震动干扰的粗糙数据得以处理，趋于平稳，效果得以改善。

1.2 一阶互补滤波算法

一阶互补滤波是加权平均法的一种简单形式，通过预先设置权值来对不同传感器信息进行加权平均

angle=K×angle_h+(1-K)×angle_i

(2)

式(2)是一阶互补滤波公式，电子罗盘取值angle_h和陀螺仪积分所得角度angle_i，借由权值K进行融合后的角度可得到融合角度angle。通过改变K的大小，可以对陀螺仪和电子罗盘在角度获取中所占的比重进行调节。

一阶互补滤波运算所需时间较短，适用于需要在行走过程中不断进行角度修正的移动机器人平台。

2 姿态估计模型及参数设计

2.1 数据融合模型

针对于多传感器的数据融合模型搭建有很多种分类方式，从多传感器系统的信息流通形式和综合处理层次角度进行分类，可分为：集中式、分布式、混合式和多级式4种[14]。

文献[15]指出，分布式数据融合模型有着计算简单，插口可添加，各部分可单独处理等优点，适用于多角度传感器的四轮移动机器人。

图1 分布式数据融合系统框图

图1是角度校正的数据融合模型。各传感器的数据先进行单独处理，最后再进行数据融合。通过处理，各个传感器获取的角度信息都得到了一定程度上的校准，可信度得以提高，之后再进行不带反馈的数据融合，在得到相对准确信息的同时，提高了运行效率。

2.2 参数设计

结合卡尔曼滤波与一阶互补滤波的数据融合模型需要对以下参数进行设定：

(1)卡尔曼滤波参数设计。卡尔曼滤波中需要事先设定的参数是测量方差R和过程方差Q。在实际测量时，可以发现测量值Z的浮动范围约为±5，故测量方差R设为5。Q是过程方差，一般与系统初始状态是无关的，它的值无法计算得到，但经过仿真对比，可发现Q的值较小时，最终的滤波效果很好，但需要更多时间来完成滤波。相对的，当Q比较大时，卡尔曼滤波的速度能显著提高，但会出现比较大的误差。通过实验仿真,Q值被确定为0.004 5；

(2)一阶互补滤波参数设计。一阶互补滤波形式简单，使用方便，需要预先设定的参数是权值K。通过在不同环境下的实验可以发现机器人受磁场的影响很大，在多次实验后可以确定陀螺仪积分得到的角度所占权值比较大，为0.995；电子罗盘所占权值很小，为0.005。根据上述公式，可将权值K设为0.005。

3 实验结果

3.1 卡尔曼滤波结果

图2是卡尔曼滤波修正的前后对比图， 当机器人运动时，由于外界环境的干扰，陀螺仪直接获取的数据会出现较大的振荡。如图可见在使用卡尔曼滤波后，则在短时间内得到明显修正，贴合实际值，效率较高。

图2 陀螺仪卡尔曼滤波修正对比图

3.2 不同环境下实验结果

为验证模型的通用性，分别在实验室内、走廊、室外3个场景里进行移动机器人自转实验。

3.2.1 实验室内

如图3所示，随着自转的进行，机器人的朝向角度会产生一定的角度漂移，但误差被控制在15°以内，较好地消除了实验室里仪器、地下电缆的干扰，可以得到较为精确的角度信息。

图3 实验室内角度修正图

3.2.2 走廊

走廊里存在的干扰比较少，从图4中可以看出测量值的变化趋势贴合实际值，基本呈一条直线。在长时间运转后出现的角度偏差在容许范围内，可以准确体现机器人的朝向方位。

图4 走廊角度修正图

3.2.3 室外

在室外进行实验的干扰最少，如图5所示，测量值与实际值基本重合，可见经由数据融合后获得的角度值与实际角度值趋于一致，效果较好。

图5 室外角度修正图

4 结束语

为改善传感器实际获得的粗糙的数据，本文设计了一种结合卡尔曼滤波与一阶互补滤波的数据融合模型，并通过自行编程对四轮机器人的实时姿态信息进行软件上的二次处理。

实验证明这种数据融合方法能一定程度上对数据进行校正，使其贴近理想值。且计算量小、效率较高，

适用于实时数据的处理。为机器人的后续功能的实现提供了相对理想的前提条件。参数的设置是数据融合模型的一个值得探究的方向。本次的数据融合模型参数采用的是预设数值，在后续研究中将继续以参数设置作为重点，尝试用神经网络等算法训练参数或是设计自适应参数调整模型来改良现在的数据融合模型。

[1] 张燕,夏庆锋,谢鹏飞.基于多传感器的姿态测量系统设计[J].机器人技术与应用,2013(3): 30-33.

[2] 惠黎明.基于多传感器探测精度的正确融合航迹判断[J].电子科技,2014,27(6):117-120.

[3] 夏俊,俞晖,罗汉文.基于多传感器数据融合的室内定位算法[J].上海师范大学学报:自然科学版,2015,44(1):65-72.

[4] 王锦章,孙菲,刘先康,等.基于卡尔曼滤波的航迹信息预处理技术[J].电子科技,2013,26(8): 22-25.

[5] 张燕,朱明敏,宋苏鸣.基于混合方式的贝叶斯网络结构学习[J].电子科技,2014,27(10):115-118.

[6] 李家森,席志红,赵彦青.北斗/INS组合导航中人工鱼群粒子滤波的应用[J].电子科技,2013,26(6):157-161.

[7] Deng Wenqian,Li Xuemei,Gao Xifeng,et al.A modified fuzzy C-means algorithm for brain MR image segmentation and bias field correction[J].计算机科学技术学报:英文版,2016(3):501-511.

[8] 赵小川.传感器信息融合—Matlab程序实现[M].北京:机械工业出版社,2014.

[9] Duan Jianmin,Shi Hui,Liu Dan,et al.Square root cubature Kalman filter-Kalman filter algorithm for intelligent vehicle position estimate[J].Procedia Engineering,2016(137): 267-276.

[10] 刘延飞,郭锁利,侯荣昌.卡尔曼滤波在GPS定位误差处理中的应用[J].电子科技,2011,24(9):140-148.

[11] Zolghadr J, Cai Y. Locating a two-wheeled robot using extended kalman filter[J]. Tehnicki Vjesnik-Technical Gazette, 2015, 22(6):1481-1488.

[12] 陈锋.卡尔曼滤波与卡尔曼预测方法的改进及其在结构损伤识别中的应用[D].广州:厦门大学,2014.

[13] 王襄.基于卡尔曼滤波融合的移动机器人定位算法[J].电子测试,2016(3):39-40.

[14] 卞鸿巍,李安,覃方君,等.现代信息融合技术在组合导航中的应用[M].北京:国防工业出版社,2010.

[15] 魏鹏,路赞赞.无线传感器网络分布式多传感器目标检测[J].电子科技,2014,27(3):143-146.

Design and Implementation of Four-wheels-robot Attitude Estimation Based on Data Fusion Algorithm

CHEN Jia,YOU Xiaoming

(School of Electronic and Electrical Engineering,Shanghai University of Engineering Science,Shanghai 201620,China)

To solve the problem about the error of sensors for four wheeled robot, a kind of data fusion algorithm is designed to estimate the posture of the robot. First of all, the Calman filter and the first-order complementary filter are used together as a type of data fusion model. Then the model is used to deal with the actual data, making it more ideal. And later function can be realized through it. Secondly, the data fusion model is proposed. Finally, the experiment is carried out under three kinds of conditions. The experimental results show that this data fusion method can correct the angle more accurately, smooth the data. It just needs small amount of calculation with higher efficiency. It is suitable for real-time data processing with a great prospect of practical application.

robot; data fusion; Calman filter; first-order complementary filter; angle correction

2017- 01- 03

国家自然科学基金(61673258)

陈佳(1993-)，女，硕士研究生。研究方向：嵌入式控制系统与智能算法等。游晓明(1963-)，女，博士，教授。研究方向：嵌入式系统等。

10.16180/j.cnki.issn1007-7820.2017.12.013

TP274.2

A

1007-7820(2017)12-048-03