一种基于改进互补滤波的自平衡移动机器人姿态快速收敛方法
【技术领域】
[0001] 本发明提供的一种基于改进互补滤波的自平衡移动机器人姿态快速收敛方法，属 于数字滤波和多传感器数据融合技术领域，主要作用于移动机器人起始运动阶段，使之实 现快速平稳启动。
【背景技术】
[0002] 姿态解算的精度和速度将直接影响移动或飞行控制算法的稳定性、可靠性和实现 的难易程度，所以，姿态解算是移动或飞行控制实现的前提。随着MEMS技术和计算机技术 的发展，小型两轮自平衡机器人姿态的测量普遍采用低成本的捷联惯性测量单元IMU，其主 要由低成本陀螺仪、加速度传感器和电子罗盘组成。MEMS陀螺仪具有温度漂移特性，加速度 传感器会受到自平衡机器人移动过程中机体振动的影响。因此，如何融合MU多传感器的 数据，滤除外部干扰，得到高可靠性、高精度的姿态数据，是一项非常具有挑战性的工作。互 补滤波器算法简单可靠，能较好地结合陀螺仪角速度的动态性能和加速度计的静态精度， 可以剔除高频运动加速度，在低成本的INS导航系统中应用广泛。由于陀螺仪短时精度高， 长时间故障会引起漂移。而对于加速度计，短时间内精度没有陀螺仪高，但是长时间却能保 持稳定。同时根据上文的分析，陀螺显示为高通特性，加速度计显示低通特性，它们在频域 上可以相互补充，从而实现高精度的姿态测量。所以互补滤波算法能同时滤除低频干扰与 高频干扰，实现传感器数据融合。但姿态求解器传递函数C(S)往往取定值，则低通滤波系 数GJs)和高通滤波系数G h(S)为常量，且在最终时刻Gh(S) >> GJs)。不能满足初始时 刻姿态的快速收敛要求，具体表现为对陀螺仪和加速度计进行互补滤波的角度值在初始阶 段很难快速跟上移动机器人的初始真实角度值，动态性能差，收敛慢甚至易摔倒，故未经改 进的互补滤波技术还存在一定的缺陷。
【发明内容】
[0003] 本发明的目的在于针对上述存在的问题和不足，提出一种用于移动机器人姿态检 测的能够快速响应、有更强适应性的改进互补滤波方法，且算法较为简单，对噪声和漂移抑 制明显，不仅在数据上更具有平滑度，同时在大幅度的角度变化情况下，也能有很快的响应 速度并能得到高精准的姿态角。
[0004] 本发明具体过程如下：
[0005] 步骤一：确定互补滤波器的初始化参数，包括互补滤波系数kp、1^等；
[0006] 步骤二：由加速度计解算出重力场下的载体倾角，包括俯仰角Θ a。。及横滚角Rcc;
[0007]
[0008] 步骤三：以加速度计解算的倾角补偿磁力计的输出得到载体的偏航角itm;
[0010] 其中，0a。。和L为加速度计估计的俯仰角和横滚角；
[0011] 步骤四：根据欧拉角的方法对陀螺的输出值进行坐标变换，解算出在导航坐标系 下的三轴姿态信息；设陀螺仪的输出值为则欧拉角速率与三轴陀螺所测的 角速率的关系如下：
[0013] 其中，Θ为俯仰角，λ为横滚角，φ为偏航角，6、乂、0分别为其对应角的角速 率；
[0014] 所以根据陀螺仪所解算的俯仰角Θ gra为：
[0016] 其中，dt为积分时间；
[0017] 步骤五：实时修改滤波器参数kp和k 1<3 <和k :存在如下函数关系：
[0018] kp+kj= 1 (5)
[0019] Ic1根据如下函数式实时优化：
[0020] k:= f !(^+^(o acc, 〇gro)+f3(A θ) (6)
[0021] 其中，t为移动机器人运行时间、〇 a。。为加速度计所解算的俯仰角测量方差、〇 gra 为陀螺仪所解算的俯仰角测量方差、A Θ为加速度计与陀螺仪所解算俯仰角的差值；
[0022] 滤波器参数Ic1和时间相关的函数关系式为：
[0024] 其中常数&1、a2根据实验调试灵活取值；
[0025] 滤波器参数4和加速度与陀螺仪计俯仰角测量方差相关的函数关系式为：
[0027] 滤波器参数Ic1和加速度计与陀螺仪解算的俯仰角的差值相关的函数关系式为：
[0028] f3 (Δ Θ ) = a3 · Δ Θ (9)
[0029] 其中常数a3根据实验调试灵活取值。
[0030] 在公式⑶与（9)中σ a。。、σ gra、Δ Θ的计算公式如下：
[0034] 其中^^的计算公式如下：
[0037] 其中，θ_、0gra分别为加速度计和陀螺仪每次俯仰角的解算值；瓦".、分别为 加速度计与陀螺仪最近10次所解算的俯仰角的均值；ξ为角度校正系数；
[0038] 步骤六：运用步骤五中参数可调的互补滤波器解算自平衡移动机器人的最优姿态 角，利用如下公式融合处理：
[0040] 其中，At为采样时间，0a。。、也别为加速度计测得的俯仰角、横滚角及补 偿磁力计得到的偏航角，么，0分别为陀螺仪测得对应轴的角速率，9 piteh、0roll、Θ胃为 经过改进互补滤波后的最优三轴姿态角。
【附图说明】
[0041] 图1改进互补滤波流程图
[0042] 图2自平衡机器人坐标系示意图
[0043] 图3改进互补滤波仿真图
[0044] 图4改进互补滤波仿真局部图
[0045] 图5 -般互补滤波试验论证图
[0046] 图6改进互补滤波试验论证图
【具体实施方式】
[0047] 参照说明书附图对本发明的一种基于改进互补滤波的自平衡移动机器人姿态快 速收敛方法作以下详细地说明。
[0048] 如图1所示，详细的阐述了改进互补滤波的算法流程，最终解算的互补滤波角度 具有很高的精度，且在初始时刻实时响应性较好。
[0049] 在图2中，简要的描述了自平衡机器人坐标系示意图，其中，Θ为俯仰角，λ为横 滚角，Φ为偏航角。
[0050] 在Matlab仿真中，取〇 _= 0. 2, 〇 gro= 0. 04, Δ Θ为符和高斯分布的方差为1， 均值为〇的白噪声。由图3和图4仿真可知，改进后的互补滤波在初始时刻具有更快的收 敛性。
[0051] 试验采用微控制器STM32F103、姿态检测模块MPU6050,并在设定好定时器的中断 时间为5ms，在中断程序中执行改进互补滤波算法。
[0052] 步骤一：确定互补滤波器的初始化参数，包括互补滤波系数kp、4等；
[0053] 步骤二：由加速度计解算出重力场下的载体倾角，包括俯仰角Θ a。。及横滚角代
[0055] 步骤三：以加速度计解算的倾角补偿磁力计的输出得到载体的偏航角itm;
[0057] 其中，0a。。和L为加速度计估计的俯仰角和横滚角；
[0058] 步骤四：根据欧拉角的方法对陀螺的输出值进行坐标变换，解算出在导航坐标系 下的三轴姿态信息；设陀螺仪的输出值为((^,，<_4&)，则欧拉角速率与三轴陀螺所测的 角速率的关系如下：
[0060] 其中，Θ为俯仰角，λ为横滚角，φ为偏航角，i、0分别为其对应角的角速 率；
[0061] 所以根据陀螺仪所解算的俯仰角Θ gra为：
[0063] 其中，dt为积分时间；
[0064] 步骤