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

摘要

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

主权项

1.一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法，其特征在于它的步骤如下：(1)利用STM32交互采集处理两轮自平衡机器人惯性姿态测量数据；(2)利用两轮自平衡机器人姿态测量系统建立MEMS陀螺仪误差数学模型；(3)利用两轮自平衡机器人姿态测量系统建立MEMS IMU姿态解算数学模型；(4)利用MEMS惯性传感器误差模型和两轮自平衡机器人姿态模型构建卡尔曼滤波器；(5)利用两阶两步量测的自适应扩展卡尔曼滤波算法对两轮自平衡机器人姿态测量数据进行数据融合，其步骤为：时间更新：分别得到状态矩阵非线性部分的Jacobian和Hessian矩阵；状态矩阵离散化；对状态量与过程协方差进行二阶时间更新；量测更新第一步：对陀螺仪测量数据进行量测更新，得到更新后的状态量与协方差量；自适应加速度计量测噪声矩阵：根据加速度计测量值判断是否具有外部加速度，同时修正加速度计量测噪声矩阵；量测更新第二步：分别得到量测矩阵非线性部分的Jacobian和Hessian矩阵；对加速度进行二阶量测更新；(6)利用二阶近似解算减少非线性函数非线性截断误差；(7)利用位移加速度判断公式，自适应量测噪声矩阵对两轮自平衡机器人姿态进行补偿；(8)利用两步量测更新的方法实现扩展卡尔曼滤波的量测更新，第一步实现陀螺仪量测更新，第二步实现加速度计量测更新。