[发明专利]一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法

说明书

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

技术领域

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

背景技术

两轮自平衡机器人为典型的非完整、非线性、欠驱动系统。为了实现两轮自平衡机器人运动过程中的平衡控制，必须具有能实时检测其姿态信息的检测系统，并将姿态信息传递给控制器，以实现对两轮自平衡机器人的精确控制。由于组成姿态检测系统的惯性传感器本身特性，它们受温度及噪声的影响很大，当惯性传感器长时间工作时，误差将随着时间累积导致系统无法正常工作。

目前两轮自平衡机器人姿态解算方法主要有卡尔曼滤波法、扩展卡尔曼滤波法、无味卡尔曼滤波法、粒子滤波法。卡尔曼滤波法姿态解算方法建模简单，实时性较好，但是忽略了非线性因素尤其是载体位移加速度对姿态测量信息的影响；扩展卡尔曼滤波法姿态解算方法实时性较好，但是线性化损失了部分二次项值，线性化误差较大。无味卡尔曼滤波法与粒子滤波精度较高，实时性较差，且不够简便。

发明内容

本发明主要解决的技术问题是针对上述背景技术的不足，提供了一种基于改进扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法。本发明为实现上述发明目的采用如下技术方案。

基于改进扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法，包括如下步骤：

步骤1：该两轮自平衡机器人是一种新型的交通工具，采用两轮并排固定的方式，由两个直流无刷电动机驱动，采用姿态感知系统、控制算法及机械装置自动协调控制车体平衡，无需人来控制平衡。人站在车上只要身体带动车体一起前倾(或后倾)就可以实现小车前进(或后退)。该两轮自平衡机器人利用STM32快速采集三轴陀螺仪三轴加速度计，采样频率1MHZ；同时，在采样间隔中，处理姿态数据与输出控制，频率为100HZ。俯仰角在运行中幅度为-10～10。

步骤2：陀螺仪能直接测量两轮自平衡机器人的姿态角速度，并通过积分运算获得角度信息，但是陀螺仪测量模型存在着相应的漂移误差，因此，结合测量模型与两轮自平衡机器人姿态测量系统，通过实验获得陀螺仪在零输入状态下的误差漂移特性，根据数据拟合模型，结合非线性最小二乘法拟合实验数据，建立陀螺仪误差数学模型。

步骤3：两轮自平衡机器人是由控制系统根据姿态信息驱动两轮同轴安装分别驱动的电机保持动态平衡。由于不存在固定转轴，一般角度或角速度传感器无法测量，而惯性测量单元能够测量转轴不固定运动系统的角速度和角度。而惯性传感器三轴陀螺仪三轴加速度能够组成低成本的捷联式惯导姿态检测系统。并根据两轮自平衡机器人姿态测量系统建立MEMS陀螺仪加速度计姿态解算数学模型。

步骤4：由于惯性传感器自身的性能和特点，单独使用陀螺仪或加速度计等惯性测量元件解算两轮自平衡机器人的姿态，都会存在严重的误差问题，因此利用MEMS惯性传感器误差模型和两轮自平衡机器人姿态模型构建卡尔曼滤波器。对系统姿态测量数据进行数据融合，抑制噪声干扰，补偿误差。根据非线性系统的微分数学模型：

得系统状态向量：

系统量测向量：