一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法
【专利摘要】本发明是一种基于改进扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法。针对现有两轮自平衡机器人的姿态解算方法不能很好的满足精度、实时性、简便性等要求，本发明方法利用改进的扩展卡尔曼滤波算法，有效的融合了惯性传感器姿态测量数据，补偿了陀螺仪随机漂移误差，减小了两轮自平衡机器人运动时位移加速度对姿态解算的影响。该姿态解算方法同时可以适用于两轮自平衡电动车。通过两轮自平衡机器人的静态实验、模拟平台实验以及实际动态实验，验证了该方法能提高两轮自平衡机器人的姿态解算精度。
【专利说明】一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人 姿态解算方法

【技术领域】
[0001] 本发明是一种基于改进扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法。 该发明适用于两轮自平衡机器人的姿态解算，同时也适用于两轮自平衡电动车。

【背景技术】
[0002] 两轮自平衡机器人为典型的非完整、非线性、欠驱动系统。为了实现两轮自平衡机 器人运动过程中的平衡控制，必须具有能实时检测其姿态信息的检测系统，并将姿态信息 传递给控制器，以实现对两轮自平衡机器人的精确控制。由于组成姿态检测系统的惯性传 感器本身特性，它们受温度及噪声的影响很大，当惯性传感器长时间工作时，误差将随着时 间累积导致系统无法正常工作。
[0003] 目前两轮自平衡机器人姿态解算方法主要有卡尔曼滤波法、扩展卡尔曼滤波法、 无味卡尔曼滤波法、粒子滤波法。卡尔曼滤波法姿态解算方法建模简单，实时性较好，但是 忽略了非线性因素尤其是载体位移加速度对姿态测量信息的影响；扩展卡尔曼滤波法姿态 解算方法实时性较好，但是线性化损失了部分二次项值，线性化误差较大。无味卡尔曼滤波 法与粒子滤波精度较高，实时性较差，且不够简便。


【发明内容】

[0004] 本发明主要解决的技术问题是针对上述【背景技术】的不足，提供了一种基于改进扩 展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法。本发明为实现上述发明目的采用如 下技术方案。
[0005] 基于改进扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法，包括如下步 骤：
[0006] 步骤1 :该两轮自平衡机器人是一种新型的交通工具，采用两轮并排固定的方式， 由两个直流无刷电动机驱动，采用姿态感知系统、控制算法及机械装置自动协调控制车体 平衡，无需人来控制平衡。人站在车上只要身体带动车体一起前倾（或后倾）就可以实现 小车前进（或后退）。该两轮自平衡机器人利用STM32快速采集三轴陀螺仪三轴加速度计， 采样频率IMHZ ;同时，在采样间隔中，处理姿态数据与输出控制，频率为100HZ。俯仰角在运 行中幅度为-10?10。
[0007] 步骤2 :陀螺仪能直接测量两轮自平衡机器人的姿态角速度，并通过积分运算获 得角度信息，但是陀螺仪测量模型存在着相应的漂移误差，因此，结合测量模型与两轮自平 衡机器人姿态测量系统，通过实验获得陀螺仪在零输入状态下的误差漂移特性，根据数据 拟合模型，结合非线性最小二乘法拟合实验数据，建立陀螺仪误差数学模型。
[0008] 步骤3 :两轮自平衡机器人是由控制系统根据姿态信息驱动两轮同轴安装分别驱 动的电机保持动态平衡。由于不存在固定转轴，一般角度或角速度传感器无法测量，而惯性 测量单元能够测量转轴不固定运动系统的角速度和角度。而惯性传感器三轴陀螺仪三轴加 速度能够组成低成本的捷联式惯导姿态检测系统。并根据两轮自平衡机器人姿态测量系统 建立MEMS陀螺仪加速度计姿态解算数学模型。
[0009] 步骤4 :由于惯性传感器自身的性能和特点，单独使用陀螺仪或加速度计等惯性 测量元件解算两轮自平衡机器人的姿态，都会存在严重的误差问题，因此利用MEMS惯性传 感器误差模型和两轮自平衡机器人姿态模型构建扩展卡尔曼滤波器，对系统姿态测量数据 进行数据融合，抑制噪声干扰，补偿误差。根据非线性系统的微分数学模型：

【权利要求】
1. 一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法，其特征在 于它的步骤如下： (1) 利用STM32交互采集处理两轮自平衡机器人惯性姿态测量数据； (2) 利用两轮自平衡机器人姿态测量系统建立MEMS陀螺仪误差数学模型； (3) 利用两轮自平衡机器人姿态测量系统建立MEMS MU姿态解算数学模型； (4) 利用MEMS IMU姿态解算数学模型构建扩展卡尔曼滤波器模型； (5) 利用改进的扩展卡尔曼滤波算法对两轮自平衡机器人姿态测量数据进行数据融 合； (6) 利用二阶近似解算减少非线性函数非线性截断误差； (7) 利用位移加速度判断公式，自适应量测噪声矩阵对两轮自平衡机器人姿态进行补 偿； (8) 利用两步量测更新的方法提高改进的扩展卡尔曼滤波的先验解算精度。
2. 根据权利要求1所述的一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人 姿态解算方法，其特征在于，所述步骤（4)为：利用MEMS惯性传感器误差模型和两轮自平衡 机器人姿态模型构建扩展卡尔曼滤波器： 系统状态向量：
系统量测向量：
3. 根据权利要求1所述的一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人 姿态解算方法，其特征在于，所述步骤（5)为：利用改进的扩展卡尔曼滤波算法对两轮自平 衡机器人姿态测量数据进行数据融合。该改进的扩展卡尔曼滤波算法为两阶两步量测的自 适应扩展卡尔曼滤波算法。其步骤为： 1)时间更新：分别得到状态矩阵非线性部分的Jacobian和Hessian矩阵；状态矩阵离 散化；对状态量与过程协方差进行二阶时间更新。 2) 量测更新第一步：对陀螺仪测量数据进行量测更新，得到更新后的状态量与协方差 量。 3) 自适应加速度计量测噪声矩阵：根据加速度计测量值判断是否具有外部加速度，同 时修正加速度计量测噪声矩阵。 4) 量测更新第二步：分别得到量测矩阵非线性部分的Jacobian和Hessian矩阵；对加 速度进行二阶量测更新。
4.根据权利要求1所述的一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人 姿态解算方法，其特征在于，所述步骤（7)为：利用以下方程考虑两轮自平衡机器人是否存 在非重力载体位移加速度： f(ax，ay，az) = I I |fa,k| |2_g| < 5 式中，S与加速度计的噪声有关。如果上式满足，则表明两轮自平衡机器人静止或保 持稳定状态，此时加速度计只受到重力加速度的作用，自适应量测噪声矩阵Rk, a:
否则：存在着非重力载体位移加速度的影响。
【文档编号】G01C21/20GK104316055SQ201410489632
【公开日】2015年1月28日 申请日期:2014年9月19日 优先权日:2014年9月19日
【发明者】周翟和, 胡佳佳, 虞波, 沈超, 赵庆涛 申请人:南京航空航天大学