余庆辉，蔡志端，张文莉，程似锦，吴海霞

（湖州师范学院工学院，浙江湖州313000）

数据融合的两轮自平衡车姿态角度检测

余庆辉，蔡志端，张文莉，程似锦，吴海霞

（湖州师范学院工学院，浙江湖州313000）

根据陀螺仪和加速计两类惯性传感器在姿态角度检测中的优缺点，提出了将多传感器数据加权和卡尔曼滤波相结合的两轮自平衡车姿态角度检测数据融合算法.首先对传感器输出进行分组加权运算，然后采用卡尔曼滤波算法对加权计算结果进行估计.实验结果表明，该检测方法可行有效，相对单一传感器测量，可提升姿态角度检测精度.

自平衡车；角度测量；数据加权；卡尔曼滤波；数据融合

两轮自平衡车是一种可自行保持平衡运动的运输载具.其以轻巧、便捷、节能、转向灵活等优点得到了广泛应用.两轮自平衡车是一种强耦合、欠驱动、类似于倒立摆的非线性系统［1-2］.平衡车在运动过程中控制器根据车体姿态角实时向两车轮驱动电路发控制信号，控制车轮的速度与方向，以便通过两轮差速保持车体平衡运行状态［3-4］.因此，两轮自平衡车姿态角的测量是车体平衡控制的基础与关键.近年来，许多学者对两轮自平衡车控制及其姿态角检测技术进行了大量研究.文献［1-5］研究了平衡车的控制技术；文献［6-12］研究了平衡车姿态角的检测技术，且都取得了有效的成果.本文主要研究车体姿态角的传感器测量方法及基于数据融合算法的最优值估计技术，并进行实验研究与分析.

1 两轮自平衡车姿态角度检测方法

图1为本文所设计的两轮自平衡车控制系统图.控制系统主要由姿态检测传感器组、单片机核心控制芯片、电机驱动电路、车轮电机、光电编码器等组成.系统采用陀螺仪和加速度计传感器组同时获取车体姿态角度，并送单片机A/D转换端口，完成角度值的采集.单片机通过程序设计数据融合算法，计算车体姿态角的最优估计值，以提升姿态角检测精度.同时单片机根据姿态角度检测值，通过PID控制算法计算电机控制信号，并通过PWM口输出脉宽调制信号至电机驱动电路，实现对两轮电机速度的控制，并最终实现车体平衡运动.在电机平衡控制过程中，需要知道车体姿态角与车轮电机控制信号间的关系.图2为车体倾斜运动过程的示意图.假设平衡车质量为m，重心高度为l，车体姿态角为θ并以α加速度向前运动.为使θ倾角度变为零，需要外力F产生加速度x（t）［67］.姿态角度θ一般比较小，可近似认为sinθ≈θ，车体运动方程为［67］：

设车轮角速度为ω（t），根据文献［3-4］得：

由（2）式和（3）式可知，通过控制电机速度ω，使得

从而保持电机平衡运动.

Fig. 1 The control structure diagram of two- wheel self- balanced vehicle

Fig. 2 Diagrammatic sketch of two- wheel self- balanced vehicle

本文陀螺仪采用AD公司的ADIS6100，加速度计采用飞思卡尔的MMA7260.陀螺仪的输出为旋转角速度值，对角速度进行积分运算得到角度值：

在程序设计过程中可采用累加的方法代替积分运算：

加速度计的输出是重力加速度在敏感轴上的分量值，通过分量值反三角函数计算，可测得角度值：

2 姿态角度数据融合算法

在角度检测过程中，由于陀螺仪和加速度计分别存在低频积分误差干扰和高频振动干扰，单独使用其中一类传感器进行姿态角度检测都会产生较大的测量误差，所以本文采用多类多传感器同时检测角度，并通过数据融合算法估算更精确的车体姿态角度，以弥补单类、单个传感器测量误差.图3为本文所提出的姿态角度数据融合算法框图.首先在车体上同时安装多个陀螺仪和加速度传感器，再对同类多传感器进行数据加权处理得到各类传感器的初始估计值，最后采用卡尔曼滤波算法对两类传感器初始估计值进行最终估计，得到两轮自平衡车姿态角度的最终估计值.多传感器数据加权处理过程中加权值由各传感器实时测量误差进行分配，分配方案为：

式中：λa、λb分别为a、b两个传感器加权值；Δθa、Δθb分别为两个传感器的测量误差.

卡尔曼滤波是由R.E.Kalman提出的以最小均方误差估计的实时递推的最优估计算法.其算法以系统当前状态值作为输入量，以估计值作为输出量.输入输出量之间由时间更新和观测更新统一起来.由前一状态计算得到的估计值和当前状态的测量实际值更新状态量的估计，最终计算当前状态的最优估计值.

设系统状态方程为：

其中：A、B、H为系统系数矩阵；x（k）、x（k-1）分别为k和k-1时刻系统状态量；u（k）、z（k）分别为输入量与输出量.

卡尔曼滤波算法估计值方程为：

Fig. 3 The total structure diagram of data fusion algorithm

卡尔曼滤波算法过程中的修正方程为：

其中：x^（k）为当前最优估计状态值；x^（k，k-1）为由前一状态计算得到的估计值；P为状态对应的协方差；Q为系统过程的协方差.

3 实验与结果分析

为了方便快捷获取实验数据，在平衡车主控制器上接入ESP8266 WIFI无线模块.实验过程中，无线通信模块与计算机联接通信，实验数据实时上传至上位机软件并保存.获取实验数据后，通过Matlab对数据进行分析处理，并对本文提出的算法进行验证分析.图4～图6为实验结果.图4为静止状态和动态下陀螺仪和加速度计两类传感器直接测量值在时间域中的变化曲线.其中，x1、x2分别为陀螺仪和加速度计在静态下输出角度值；x3、x4分别为动态下陀螺仪和加速度计输出角度值.由图4可知，无论在静态还是在动态，传感器的输出都存在干扰误差.陀螺仪存在低频干扰误差，加速度计存在高频干扰误差.所以，单独使用一类传感器进行两轮自平衡车姿态角测量是不够精确的，必须同时应用多传感器并采用数据融合算法对车体姿态角进行优化估算，以提升角度测量精度.图5和图6分别为静态和动态下数据融合算法角度测量效果.其中，x1、x2为陀螺仪和加速度计直接测量值；x3为本文的最终估计值.实验结果表明，无论是在静态还是动态环境下，本文所提出的算法都能有效去除加速度计高频毛刺干扰误差和陀螺仪低频误差，提升车体姿态角测量精度.

Fig. 4 Angle curve diagram in static and dynam ic state

Fig. 5 Static angle test result after data fusion algorithm

4 结论

本文分析了两轮自平衡车姿态角度测量的原理以及角度测量精度的提升技术，研究了结合多传感器数据加权和卡尔曼滤波的数据融合最优估计算法，并将其应用于两轮自平衡车姿态角的实时检测中.通过Matlab软件对本文提出的算法进行实验验证与分析.结果表明，本文提出的车体姿态角度检测方法与数据融合算法可行有效，能够高精度地测量出平衡车姿态角度，为平衡车控制性能的提升提供了良好的条件.

Fig. 6 Dynam ic angle test result after data fusion algorithm

［1］LIN S C，TAI C H，HUANG S C.Adaptive robust self-balancing and steering of a two wheeled human transportation vehicle［J］.Intel Robot System，2011，27（2）：103-123.

［2］XU J X，GUO Z Q，LEE T H.Design and implementation of a takagi-sugeno type fuzzy logic controller on a twowheeled mobile robot［J］.IEEE Transaction on Industrial Electronics，2013，60（12）：5 717-5 728.

［3］LIN S C，TAI C H，HUANG S C.Development of self-balancing human transportation vehicle for the teaching of feedback control［J］.IEEE Transactions on Education，2009，26（2）：1-52.

［4］梁文宇，周惠兴，曹荣敏，等.双轮载人自平衡控制系统研究综述［J］.控制工程，2010，17（1）：139-144.

［5］武俊峰，孙雷.两轮自平衡机器人的控制方法研究［J］.哈尔滨理工大学学报，2014，34（6）：22-26.

［6］孟腾，章政.基于陀螺仪与加速度计的二轮自平衡控制系统设计［J］.传感器与微系统，2014，33（10）：61-64.

［7］马思远，鲁庭勇，张丽君.两轮自平衡车运动姿态的测量和控制［J］.测控技术，2015，34（4）：71-73.

［8］郜园园，阮晓钢，宁洪军，等.两轮自平衡机器人惯性传感器滤波问题的研究［J］.传感器学报，2010，23（5）：696-700.

［9］姜香菊，刘二林.两轮自平衡机器人角度检测数据融合算法［J］.计算机工程与应用，2013，49（8）：203-205.

［10］张吉昌，程凯，郑荣儿.单轴双轮自平衡车姿态检测方案设计［J］.中国海洋大学学报，2009，39（9）：467-470.

［11］魏文，段晨东，高精隆，等.基于数据融合的两轮自平衡小车控制系统设计［J］.电子技术应用，2013，39（5）：79-81.

［12］傅忠云，朱海霞，孙金秋，等.两轮载人自平衡车姿态测量单元设计［J］.科学技术与工程，2015，15（15）：66-71.

Attitude Measurement of Two-Wheel Self-Balanced Vehicle Based on Data Fusion Algorithm

YU Qinghui，CAI Zhiduan，ZHANG Wenli，CHENG Sijin，WU Haixia
（School of Engineering，Huzhou University，Huzhou 313000，China）

Attitude measurement is the critical step in two-wheel self-balancing vehicle control. The advantages and disadvantages of the gyroscope and accelerometer sensors are analyzed.Data fusion algorithm for Attitude angle measurement is proposed based on multi-sensor data weighting method and kalman filter algorithm.Firstly，the output of the sensor is processed by weighting operation.Lastly，a kalman filter is proposed based on the measurement result.The experimental results show that the algorithm in the article is effective and more reliable than single sensor.The algorithm can enhance the precision of the self-balanced vehicle attitude angle measurement.

self-balanced vehicle；angle measurement；data weighting；Kalman filter；data fusion

TP291

A

1009-1734（2016）04-0061-04

［责任编辑 高俊娥］

2016-03-05

国家级大学生创新创业训练计划项目（201510347009）.

蔡志端，讲师，研究方向：永磁同步电机控制技术、功率变换器故障诊断与容错控制.E-mail：czddule@163.com