CN104316055B - 一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

步骤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表示重力加速度。

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

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

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

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

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

步骤6：常规扩展卡尔曼滤波是对均值与方差的非线性变换的一阶近似解算，由于扩展卡尔曼对非线性函数采用非线性截断的策略，不可避免地使滤波精度和收敛性都受到了一定的影响。而二阶扩展卡尔曼滤波较一阶扩展卡尔曼滤波，精度与实时性都能够得到满足。由于只需对非线性部分做泰勒展开处理，因此计算量满足两轮自平衡机器人实时处理性要求。

步骤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为量测更新第一步得到的陀螺仪协方差。

步骤8：考虑到扩展卡尔曼滤波器其线性化参考点是围绕先验解算的状态，因此先验解算优劣对于扩展卡尔曼滤波具有非常重要的作用。且两轮自平衡机器人姿态测量系统模型分别具有非线性与线性成分，因此采用第一步陀螺仪量测更新、第二步加速度计量测更新的两步量测更新方法来提高滤波精度。

本发明采用上述发明技术路线方案，具有以下有益效果：本发明抑制噪声干扰、融合加速度计与陀螺仪数据；采用传感器误差模型补偿随机漂移误差；使用自适应量测噪声矩阵减小非重力位移加速度因素对姿态解算的影响。本发明为两轮自平衡机器人提供了一个低成本，精度高的姿态测量系统。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1惯性传感器陀螺仪输出随机漂移图

图2(a)两轮自平衡机器人静态竖直放置横滚角角度偏差修正图

图2(b)两轮自平衡机器人静态竖直放置静态俯仰角角度偏差修正图

图3(a)动态模拟平台横滚角角度偏差修正图

图3(b)动态模拟平台俯仰角角度偏差修正图

图4(a)两轮自平衡机器人动态运动横滚角角度偏差修正图

图4(b)两轮自平衡机器人动态运动俯仰角角度偏差修正图

具体实施方式

本发明适用于两轮自平衡机器人的姿态解算，同时也适用两轮自平衡电动车。对于不同的惯性传感器，具有不同的随机漂移特性，因此首先确定惯性传感器的误差模型与姿态解算模型，利用改进的扩展卡尔曼滤波对采样得到的三轴陀螺仪加速度姿态测量数据进行数据融合，得到两轮自平衡机器人的姿态信息。下面为本发明的具体技术实施方案，包括如下步骤：

步骤1，利用陀螺仪对两轮自平衡机器人进行姿态解算，得横滚角γ与俯仰角θ与三轴角速度ω_x、ω_y、ω_z关系：

步骤2，为了减少传感器本身特性对姿态解算产生的影响，对陀螺仪在零输入静止情况下进行2h数据采样，采样结果如附图1。在理想状态下，将是一个恒定的数值0。实际输出数据(由附图1红线表示)，其均值随时间呈指数非线性形式变化。该漂移误差主要是由温度引起的，通过非线性最小二乘法曲线拟合，最后趋于稳定(由图1蓝色点划线表示)。拟合模型：

通过调整C₁、C₂、T三参数使得拟合曲线残差平方二范数之和最小。

步骤3，利用捷联惯导理论，考虑两轮自平衡机器人处于水平静止或稳定状态时，两轮自平衡机器人在导航坐标系中加速度为零，得

其中f^b是加速度计输出量，当载体加速度运动时，存在额外的载体位移加速度，加速度计无法区分重力加速度和非重力的载体位移加速度，所以必须消除这种影响，上式才能成立。为了消除非重力载体位移加速度对姿态解算测量的影响，需要利用陀螺仪的特性修正其测量值。

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

以非线性系统的数学模型为例考虑扩展卡尔曼滤波器状态方程：

式中，x是系统状态向量；z是系统观测向量，分别为：

其中，ε_x、ε_y、ε_z分别是陀螺仪x轴、y轴、z轴的误差拟合模型；分别是加速度计x轴、y轴的量测值；gyro_x、gyro_y、gyro_z分别是陀螺仪x轴、y轴、z轴的量测值。并且状态向量f为：

式中，参数T_x、C_1x、C_2x、T_y、C_1y、C_2y、T_z、C_1z、C_2z分是陀螺仪x轴、y轴、z轴误差拟合模型参数。非线性量测向量h为：

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

步骤5-1，采样三轴陀螺仪三轴加速度计姿态测量数据。

步骤5-2，采样数据时间更新过程：

步骤5-2，量测更新第一步：陀螺仪量测更新

步骤5-3，自适应量测噪声矩阵R_k，a：满足方程

否则：

步骤5-4，量测更新第二步：加速度量测更新

步骤5-5，进入下一时刻，重复步骤5-1到5-4，得到两轮自平衡机器人姿态数据。

式中，α₁、α₂、α₃为常量系数，调整滤波效果，r_c，1、r_c，2为常量确保量测噪声矩阵不至于太小而导致滤波器发散，通过r_k，1和r_k，2自适应来调整加速度计量测值的可信度，从而改善非重力位移加速度对姿态解算的影响。量测方程满足以下关系：

其中，h(x_k)_a、h(x_k)_g分别是加速度计与陀螺仪的量测矩阵，H_k，g是h(x_k)_g的线性形式。e_i＝(0 ... 0 1 0 ... 0)^T是单位矢量。矩阵F_x(x_k)，H_x，a(x_k)，H_xx，a(x_k)分别是状态矩阵与量测矩阵非线性部分的Jacobian和Hessian矩阵，由于只需对非线性部分做泰勒展开处理，因此计算量满足两轮自平衡机器人实时处理性要求。

实施例：

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

(1)静态实验：将两轮自平衡机器人竖直放置，在零输入状态下，进行静态实验。此时认为不存在非重力位移加速度的影响，获得如图2(a)(b)所示曲线。

通过对比由加速度计解算的姿态角与改进的扩展卡尔曼滤波融合的姿态角，经过改进扩展卡尔曼滤波融合后的姿态角，方差明显减小，输出平稳，且均值未偏离加速度计解算的原始角度值。从静态实验结果可以看出，本发明能够有效滤除噪声干扰，消除随机误差。

(2)模拟平台实验：两轮自平衡机器人的加速过程主要发生在俯仰过程，所以该实验主要对俯仰轴施加非重力载体位移加速度。由于两轮自平衡机器人在运动中较难保持一定的俯仰角与横滚角不变，因此现利用模拟运动平台，在静态测试姿态角不变的基础上，模拟两轮自平衡机器人在加速度计俯仰轴方向上受到水平加速度。如图3(a)(b)所示。

由于主要是加速度计俯仰轴上受到非重力载体位移加速度。因此，经过扩展卡尔曼滤波修正的俯仰角偏差较大，说明非重力载体位移加速度对姿态信息具有一定测量影响。

(3)实际动态实验：两轮自平衡机器人实际运行过程中，利用改进的扩展卡尔曼滤波对姿态角进行补偿，如图4(a)(b)所示。

通过对比实验结果，改进的扩展卡尔曼滤波能很好的抑制非重力载体位移加速度对姿态测量的影响。且非重力载体位移加速度越大，补偿的效果越明显。

Claims (3)

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为量测更新第一步得到的陀螺仪协方差。