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

权利要求书

1.一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法，其特征在于它的步骤如下：

(1)利用STM32交互采集处理两轮自平衡机器人惯性姿态测量数据；

(2)利用两轮自平衡机器人姿态测量系统建立MEMS陀螺仪误差数学模型；

(3)利用两轮自平衡机器人姿态测量系统建立MEMS IMU姿态解算数学模型；

(4)利用MEMS惯性传感器误差模型和两轮自平衡机器人姿态模型构建卡尔曼滤波器；

(5)利用两阶两步量测的自适应扩展卡尔曼滤波算法对两轮自平衡机器人姿态测量数据进行数据融合，其步骤为：

时间更新：分别得到状态矩阵非线性部分的Jacobian和Hessian矩阵；状态矩阵离散化；对状态量与过程协方差进行二阶时间更新；

量测更新第一步：对陀螺仪测量数据进行量测更新，得到更新后的状态量与协方差量；

自适应加速度计量测噪声矩阵：根据加速度计测量值判断是否具有外部加速度，同时修正加速度计量测噪声矩阵；

量测更新第二步：分别得到量测矩阵非线性部分的Jacobian和Hessian矩阵；对加速度进行二阶量测更新；

(6)利用二阶近似解算减少非线性函数非线性截断误差；

(7)利用位移加速度判断公式，自适应量测噪声矩阵对两轮自平衡机器人姿态进行补偿；

(8)利用两步量测更新的方法实现扩展卡尔曼滤波的量测更新，第一步实现陀螺仪量测更新，第二步实现加速度计量测更新。

2.根据权利要求1所述的一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法，其特征在于，所述步骤(4)为：利用MEMS惯性传感器误差模型和两轮自平衡机器人姿态模型构建扩展卡尔曼滤波器：

系统状态向量：

系统量测向量：

其中，ω_x，ω_y，ω_z分别表示陀螺仪x，y，z轴的角速率；γ，θ分别表示横滚角和俯仰角；ε_x，ε_y，ε_z分别表示陀螺仪x，y，z轴误差拟合模型；T_x，C_1x，C_2x表示陀螺仪x轴误差拟合模型参数；T_y，C_1y，C_2y表示陀螺仪y轴误差拟合模型参数；T_z，C_1z，C_2z表示陀螺仪z轴误差拟合模型参数；g表示重力加速度。

3.根据权利要求1所述的一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法，其特征在于，所述步骤(7)为：利用以下方程考虑两轮自平衡机器人是否存在非重力载体位移加速度：

f(a_x，a_y，a_z)＝|||f_a，k||₂-g|＜δ

式中，δ与加速度计的噪声有关，如果上式满足，则表明两轮自平衡机器人静止或保持稳定状态，此时加速度计只受到重力加速度的作用，自适应量测噪声矩阵R_k，a：

否则：存在着非重力载体位移加速度的影响：

其中，α₁，α₂，α₃为常量系数，调整滤波效果；r_c，1和r_c，2为常量，用于确保量测噪声矩阵不至于太小而导致滤波器发散；r_k，1和r_k，2为k时刻的自适应调节因子，调整加速度计测量值可信度，从而改善非重力加速度对姿态估计的影响；为量测矩阵的Hessian矩阵；表示加速度计量测矩阵；P_k，g为量测更新第一步得到的陀螺仪协方差。