基于信号融合的自平衡车姿态测量方法研究

吴学勤+许耀华+李娟娟+王建锋+刘新雨

摘 要：针对自平衡车姿态角度测量问题，本文叙述了基于卡尔曼滤波和互补滤波的自平衡车MEMS IMU单轴融合算法的原理，分析了平衡车车身翻滚运动对融合算法计算结果的影响，最后分析了卡尔曼滤波法和互补滤波法的动态和静态收敛速度。为了验证各算法的效果，本文搭建了基于飞思卡尔K60单片机的信号采集平台进行实验。实验表明在采用低成本MEMS IMU的自平衡车俯仰角计算中，两种融合算法的效果接近，但是互补滤波法在静态时的收敛速度较快，同时考虑计算时效性，采用基于互补滤波的单轴融合算法较为合适。

关键字：自平衡车；信号融合；卡尔曼滤波；互补滤波；姿态角

中图分类号：TP273 文献标志码：A

Research on Signal-fusion Based Attitude Angle Measurement of Self-balanced Car

WU Xue-qin1，XU Yao-hua1，LI Juan-juan1，WANG Jian-feng2，LIU Xin-yu1

（1.School of Automobile，Changan University，Xian，Shaanxi 710064；

2.Road Traffic Intelligence Detection and Engineering Technology Research Center of Shaanxi （Changan University），Xian，Shaanxi 710064，China）

Abstract：The principle of algorithms based on Kalman filtering and complementary filtering are presented to measure the attitude angle of a self-balance car.The impact of rolling motion of car is evaluated.The speed of convergence of Kalman filtering and complementary filtering is analyzed.A K60 microcontroller based experimental platform is developed to validate the efficiency of algorithms.Result of experiment shows that the performance of the two signal-fusion algorithms are approaching.But after considering the computational efficiency，we can conclude that the complementary filtering based algorithm is more suitable.

Key words：balance-car；signal fusion；Kalman filter；complementary filter；attitude angle

1 引 言

二輪直立式自平衡车具有体积、重量小，运动灵活的特点，在个人交通领域有重大的市场。二轮直立式自平衡车的结构与倒立摆原理相似，其重心位于车轮支点上方。自平衡车本身无法实现平衡，必须依靠电机不断为其提供修正力矩来保持车辆的直立。为了实现该目的，自平衡车上的控制单元必须实时地获取车辆当前的车辆姿态参数，包括车辆的俯仰角、俯仰角速度等信息[1]。

一般情况下，自平衡车依靠所安装的IMU来测量车身的俯仰角和俯仰角速度。但是由于传感器本身以及安装位置等原因，由这些传感器直接采集到的信号包含有噪声。

目前有多种算法可以实现对陀螺仪和加速度计信号进行数据融合，从而滤除角度计算结果中的噪声，比较常用的方法有互补滤波法和卡尔曼滤波法[2-3]。本文对这两种测量方法进行对比，介绍各方法的原理，并用分析其在平衡车姿态角测量中的效果。

2 姿态角测量误差分析

在工程测量中，传感器的由于受到外界和自身干扰因素的影响，其输出信号必然包含各种噪声。

目前，自平衡车上普遍采用六轴MEMS IMU，包括以三个相互垂直的方向为敏感轴的三轴加速度计和对围绕这三轴的旋转运动敏感的三轴陀螺仪[4]。为了之后分析各数据融合方法的效果，这里先分析传感器的数学模型。

平衡车的车身坐标系及倾角示如图1.1和图1.2所示。

对于三轴加速度计信号，其数学描述为：

ami=abi-gbi+vai（1）

式中，ami轴加速度计中沿X、Y或Z轴方向的加速度信号输出，abi沿该轴方向的加速度分量，gbi力加速度在该轴的投影，vai噪声。

对于三轴陀螺仪，其数学描述为：

ωmi=ωti+boi+bi+ωai（2）

式中，ωmi为陀螺仪所测角速率，ωti为角速率真值，boi为角速率常漂值，bi为时变漂移量，ωai为白噪声。

假如单独处理加速度计和陀螺仪的信号，并且小车只绕其Y轴转动，可用以下公式分别计算车身当前的倾角

θa（t）=arctanamxamz=arctanabx-gbx+vaxabz-gbz+vaz（3）

θg（t）=∫t0ωmy（t）dt+θg（0）=∫t0ωty（t）dt

+boyt+∫t0by（t）dt+θg（0）+e（4）

公式（3）表明，单独依靠加速度计的信息计算倾角时，但是由于加速度计的测量结果不可避免地受到车体加速度和传感器噪声的影响，使得endprint

abx、abz、vax和vaz

不为零，因此其计算结果将偏离真值；

公式（4）表明，单独依靠陀螺仪计算角度时，其计算结果将包含常漂量b0和时变漂移b的一次项，使得积分误差将随积分时间而累积，最终使得结果失真。值得注意的是，从频域角度上看，陀螺仪的计算结果经过积分后，相当于进行了低通滤波，因此白噪声ωai对角度测量误差的影响较小，此时计算结果的误差主要集中在低频段。

3 测量方法原理介绍

以上分析表明，单独依靠加速度计或陀螺仪信号测量自平衡车的姿态角是不可靠的。需要采用融合的方法进行处理，以得到较好的结果。

3.1 一阶互补滤波

互补滤波通过对加速度计信号进行低通滤波，降低其高频噪声，对陀螺仪信号进行高通滤波，降低其低频噪声，最后加权融合的方法，实现两种传感器信息的融合[5]。一阶互补滤波的传感器信息融合原理如图2所示。

图2 互补滤波原理方框图

将公式（3）和（4）计算结果的成分简化为如下形式

θa（t）=θt（t）+na（t）（5）

θg（t）=θt（t）+ng（t）（6）

则根据图2所示的原理，平衡车倾角估计值t的拉普拉斯变换结果为

t（s）=1τs+1θ（s）+τsτs+1θ（s）

+1τs+1Na（s）+τsτs+1Ng（s）（7）

式中，τ为滤波器时间常数，τ的取值取决于传感器的噪声分布特性。

忽略噪声项，将公式（7）转为微分方程，得到

（t）+τd（t）dt=θ（t）+τdθ（t）dt（8）

为了得到公式（8）的离散形式，便于在单片机中处理，令dθ（t）dt=θ（k）-θ（k-1）T，d（t）dt=（k）-（k-1）T，代入公式（8）得到一阶互补滤波的离散形式

（k）=τT+τ（k-1）+TT+τθ（k）

+τT+τ[θ（k）-θ（k-1）]（9）

式中，T为传感器信号更新的时间间隔。代入公式（3）和（4），用ωm（k）替换θ（k）-θ（k-1），用θa（k）替换θ（k），就能得到用于信息融合的计算公式。在实际计算中，τT+τ的值可通过实验调整得到。将τT+τ替换为一个常系数k，然后对其进行修改，并观察角度曲线的变化，以此确定最佳滤波参数。

3.2 卡尔曼滤波

卡尔曼滤波通过实时更新均值和协方差矩阵执行滤波过程，是一种基于模型的线性滤波器[6]。本文所采用的卡尔曼滤波具体计算过程如下：

系统所对应的状态方程和观测方程为

xk=Fxk-1+Guk-1+wk-1（10）

公式（10）中，ωb为陀螺仪角速率的漂移量，对应于公式（2）中的b0、b两项。由于b项为时变项，因此ωb亦为时变项。通过添加一定的状态方程噪声，可以使算法跟踪=该参数的变化 [7]。

系统所对应的状态方程为

wR=[n 0 a]=[θa]k=[1 0]θωbk+vk（11）

在公式（11）中，系统以加速度传感器对应的车身倾角计算值θa作为观测量。

将公式（10）和（11）写为矩阵代数形式

xk=Fxk-1+Guk-1+wk-1

yk=Hkxk+vk

则离散卡尔曼滤波器的递推过程每一步的计算公式为：

P-k=Fk-1P+k-1FTk-1+Qk-1（11a）

Kk=P-KHTk（HkP-kHTk+Rk）-1（11b）

=Fk-1+k-1+Gk-1uk-1（11c）

+k=-k+Kk（yk-Hk-k）（11d）

P+k=（1-KkHk）P+k（I-KkHk）T+KkRkKTk（11f）

式中，Qk=E（wkwTk），Rk=E（vkvTk），Pk为状态向量的协方差矩阵，上标的正负号分别表示后验估计和先验估计。通过重复公式（11）所示的递推计算过程，即可实现对状态参数的实时估计。

3.3 IMU单轴融合算法可靠性分析

自平衡车载地面上行驶时，由于地面不可能完全水平，因此平衡车的车身难免存在绕X轴的翻滚运动。这种翻滚会照成重力加速度在三轴投影分量的变化，造成在公式（3）的计算结果中包含这一扰动量。以下对该运动对IMU单轴融合算法计算结果的影响进行分析。

由于IMU传感器固定在平衡车车体上，因此需要分析重力在车体坐标系三个轴向上投影分量的变化。这里用欧拉角法对车体坐标系相对世界坐标系的转动进行描述。其中，所述的世界坐标系原点与车体坐标系原点重合，其垂直轴Zw垂直于水平面，其纵轴Xw正方向为车体坐标系X在水平面上投影，Yw轴所在方向Xw与Zw方向向量的叉乘。用欧拉角描述的车系坐标系相对世界坐标系的旋转矩阵为[8]

在该矩阵中，向量n、o和a分别表示导航坐标系Xw、Yw、Zw轴的方向向量在车体坐标系中的投影[8]，为翻滚角，θ为俯仰角，ψ为偏航角。由于重力加速度在导航坐标系中的向量与导航坐标系的Zn轴重合，因此重力加速度在车体坐标系中的表达为[gx gy gz]T=ag。

令偏航角ψ=0，得到重力加速度在车体坐标系X轴和Z轴向上的投影为

将与公式（13）代入公式（3）中，并且令车体运动加速度和噪声项为零，则可以判断出车身的翻滚运動对角度的估计不会造成影响。

4 实验验证

4.1 实验平台搭建

本文采用的IMU传感器型号为MPU6050，由IvenSense公司推出，上面集成了三轴角速度机、三轴陀螺仪、温度传感器和一个数字运动处理模块（Digital Motion Processor，DMP），并通过I2C总线直接输出传感器的数字信号。本文所搭建的实验平台结构框图如图3所示。endprint