卡尔曼滤波在两轮自平衡代步车姿态检测中的应用 卡尔曼滤波在两轮自平衡代步车姿态检测中的应用 绍了一种MENS加速度计、陀螺仪与嵌入式微控制器相结合的两轮自平衡代步车姿态检测系统。针对加速度计和 陀螺仪测量分别存在噪声干扰和随机漂移误差，采用卡尔曼滤波实现传感器数据融合，补偿传感器测量误差， 得到车体姿态的最优估计。 引言 两轮自平衡代步车作为一种新型轻便的交通工具，具有运动灵活、结构简单、自平衡控制的特点。代步车在运动过程中需要不 断地对车体的运动速度、角度等姿态信息进行及时准确的采集与估计，以实现车体的运动规划调整和动态平衡控制。 常用的姿态检测传感器为加速度计和陀螺仪。加速度计用于测量与惯性有关的加速度，经转化后获得角度值，其误差小，精度 高，但动态角度测量噪声干扰较大。陀螺仪的作用是测量角速度信号，其本身极易受噪声干扰，同时由于温度变化、不稳定力 矩等因素，陀螺仪会产生随机漂移误差，并随着时间的推移而累加变大[1]。参考文献[2]采用互补滤波的方法，根据传感器输 出特性选择相应的滤波器，达到加速度计与陀螺仪信号相互补偿的作用，具有理论简单、响应快的特点，但对横向加速度和陀 螺仪漂移不够敏感，所以无法得到姿态信息的准确估计。本文提出了应用卡尔曼滤波算法实现加速度计与陀螺仪的检测信息融 合，得到俯仰角和俯仰角速度的最优估计，并在以STM32F103RET6为控制核心的两轮自平衡代步车姿态检测系统上加以验 证。 1 系统组成 本文所采用的姿态检测系统主要由加速度计、陀螺仪、微控制器、无刷电机等部分组成[2]。姿态检测系统框图如图1所示,陀螺 仪和加速度计信号送入微控制器中，通过卡尔曼滤波器对传感器数据进行补偿和融合,获得准确的姿态信号,再通过PID控制算 法输出控制量，经过三相逆变电路，生成PWM波驱动电机运转。 图1 姿态检测系统框图 1.1 加速度计 ADXL345是ADI公司2008年推出的采用MEMS技术的具有SPI和I2C总线数字输出功能的三轴加速度计，最大可感知±16g的加 速度，感应精度可达3.9 mg/LSB,倾角测量典型误差小于1°，具有小巧轻薄、超低功耗、可变量程、高分辨率等特点。 ADXL345可在倾斜敏感应用中测量静态重力加速度，也可在运动甚至振动环境中测量动态加速度。将加速度计垂直于车体俯 仰轴安装时，通过反正切函数运算能得到准确的俯仰角信息，其表达式为: 其中，θ为俯仰角；Ax和Ay分别为横轴和纵轴的重力加速度输出。图2为加速度计检测原理图，检测数据通过I2C总线（SCL和 SDA端口）送入微控制器。 图2 加速度计检测原理图 1.2 陀螺仪 ITG3205是单一芯片并以数字方式输出的三轴MEMS陀螺仪，将数字输出X、Y、Z轴角速度值传感器整合在单一电路上。图3 为陀螺仪检测原理图，检测数据通过高达400 kHz的I2C总线串行接口（SCL和SDA）与微控制器通信。 图3 陀螺仪检测原理图 1.3 微控制器 采用ST公司的STM32F103RET6作为控制芯片，它的最高工作频率可以高达72 MHz，并且片内集成了高速的存储器(高达 512K字节的闪存和64K字节的SRAM)，具有丰富的增强I/O端口和联接到两条APB总线的外设[3]。STM32F103RET6内部包含 3个12位的ADC、4个通用16位定时器、2个16位带死区控制和紧急刹车功能，用于电机控制的PWM高级控制定时器、2个I2C 总线接口利于检测数据的串行传输，适合应用于电机驱动和控制的场合。 2 卡尔曼滤波车体姿态信息融合 2.1 车体姿态模型 本系统采用的加速度计和陀螺仪都是三轴传感器，可以对车体运行过程中的俯仰、偏航、横滚状态的信息进行检测。本文主要 研究俯仰状态，应用卡尔曼滤波对俯仰角和俯仰角速度进行数据融合[45]。车体的俯仰角与俯仰角速度存在导数关系，所以选 取车体的俯仰角作为状态向量的一个元素。系统采用加速度计来估计陀螺仪的随机漂移gyro_bias，将此偏差作为另一个状态 向量。 车体的俯仰角与俯仰角速度存在如下等式关系： 式中，anglek为估算的k时刻俯仰角,anglek-1为k-1时刻的俯仰角估计值, gyrok-1为陀螺仪所测得的k-1时刻角速度； 

gyro_biask-1为k-1时刻陀螺仪的位偏差,dt为采样时间。 如果认为前一时刻与后一时刻的漂移误差变化很小，即： 可得系统状态方程为 其中，wk是过程噪声,并假定其符合均值为零、协方差矩阵为R的正态分布。 测量方程为: 其中,θk是k时刻加速度计的俯仰角观测值；vk是观测噪声，假设符合均值为零、协方差为R的正态分布。 2.2 卡尔曼滤波器设计 由式（4）、（5）得到的车体姿态的状态方程及观测方程，在应用卡尔曼滤波实现姿态信息融合的过程中，滤波器的状态由 以下两个变量描述：k时刻的状态估计x^k|k；度量状态估计值精确程度的误差相关矩阵Pk|k。其滤波算法框图如图4所示。 图4 卡尔曼滤波算法框图 其中,状态转换矩阵Ak=1-dt01；输入矩阵Bk=dt0；Hk是观测矩阵,Hk=(10)；过程噪声协方差矩阵Q如下式所示： Q_angle和Q_gyro分别是俯仰角状态和陀螺仪漂移状态的过程噪声的协方差，相对较小的取值，体现更高的信任程度，一般取 Q_gyro的值比Q_angle小三个数量级左右。观测噪声的协方差矩阵： R_angle为加速度计测量噪声vk的协方差，R_angle越大，则观测值中被卡尔曼滤波器视为噪声的成分越大。 卡尔曼滤波以最小均方误差估计为准则，通过一串含有噪声的测量数据，对系统的内部状态进行估计[6]。通过对上述算法的 循环使用，可得到车体姿态的最优估计。 3 实验验证 为了验证卡尔曼滤波融合车体姿态信息的可行性与有效性，将该数据融合算法移植到STM32F103RET6微控制器中，通过对 两轮自平衡代步车姿态检测系统电路板的调试，实现对姿态数据的实时检测与处理。软件设置卡尔曼滤波器初始参数如表1所 列。 表1 卡尔曼滤波器参数设置 实验结果如图5所示，在未经卡尔曼滤波器前，加速度计观测的俯仰角度存在较大的噪声干扰，角度输出为±10°之间；经卡尔 曼滤波后，角度输出为±5°之间，有效滤除噪声干扰信号。 图5 俯仰角滤波结果 图6中卡尔曼滤波器对陀螺仪工作过程中的随机漂移误差进行估计，并用估计结果来更正陀螺仪测量的角速率值，有效补偿陀 螺仪的漂移误差。经滤波后，系统达到更高的检测精度，满足实际需要。 图6 角速率滤波结果 结语 本文研究了利用卡尔曼滤波实现传感器数据融合的方法，并通过加速度计、陀螺仪、控制器等组成的姿态检测系统，完成了对 车体运行过程中的姿态信息的检测与处理。该检测方案也可以应用在其它轮式机器人中，具有较好的应用前景。 参考文献