CN105300381A - 一种基于改进互补滤波的自平衡移动机器人姿态快速收敛方法 - Google Patents

Abstract

本发明提出的一种基于改进互补滤波的自平衡移动机器人姿态快速收敛方法，属于运动控制与多传感器数据融合技术领域，主要作用于移动机器人起始运动阶段，使之快速平稳启动。姿态检测模块主要包括微控制器、陀螺仪、加速度计和磁力计等MEMS惯性传感器。该系统利用加速度计重力场分量估计的倾角作为辅助信息，通过倾角补偿得到由磁力计解算的偏航角，并运用欧拉角姿态解算方法得到陀螺仪在三轴的角速率信息。通过设计一个参数灵活可调的互补滤波器对陀螺仪和辅助传感器的数据进行融合处理，实时解算出最优姿态角。本发明不仅能够使姿态检测模块快速响应初始状态的姿态角，而且能明显抑制噪声和漂移误差，使移动机器人快速平稳启动，增强其稳定性能。

Description

一种基于改进互补滤波的自平衡移动机器人姿态快速收敛方法

技术领域

本发明提供的一种基于改进互补滤波的自平衡移动机器人姿态快速收敛方法，属于数字滤波和多传感器数据融合技术领域，主要作用于移动机器人起始运动阶段，使之实现快速平稳启动。

背景技术

姿态解算的精度和速度将直接影响移动或飞行控制算法的稳定性、可靠性和实现的难易程度，所以，姿态解算是移动或飞行控制实现的前提。随着MEMS技术和计算机技术的发展，小型两轮自平衡机器人姿态的测量普遍采用低成本的捷联惯性测量单元IMU，其主要由低成本陀螺仪、加速度传感器和电子罗盘组成。MEMS陀螺仪具有温度漂移特性，加速度传感器会受到自平衡机器人移动过程中机体振动的影响。因此，如何融合IMU多传感器的数据，滤除外部干扰，得到高可靠性、高精度的姿态数据，是一项非常具有挑战性的工作。互补滤波器算法简单可靠，能较好地结合陀螺仪角速度的动态性能和加速度计的静态精度，可以剔除高频运动加速度，在低成本的INS导航系统中应用广泛。由于陀螺仪短时精度高，长时间故障会引起漂移。而对于加速度计，短时间内精度没有陀螺仪高，但是长时间却能保持稳定。同时根据上文的分析，陀螺显示为高通特性，加速度计显示低通特性，它们在频域上可以相互补充，从而实现高精度的姿态测量。所以互补滤波算法能同时滤除低频干扰与高频干扰，实现传感器数据融合。但姿态求解器传递函数C(s)往往取定值，则低通滤波系数G_L(s)和高通滤波系数G_H(s)为常量，且在最终时刻G_H(s)＞＞G_L(s)。不能满足初始时刻姿态的快速收敛要求，具体表现为对陀螺仪和加速度计进行互补滤波的角度值在初始阶段很难快速跟上移动机器人的初始真实角度值，动态性能差，收敛慢甚至易摔倒，故未经改进的互补滤波技术还存在一定的缺陷。

发明内容

本发明的目的在于针对上述存在的问题和不足，提出一种用于移动机器人姿态检测的能够快速响应、有更强适应性的改进互补滤波方法，且算法较为简单，对噪声和漂移抑制明显，不仅在数据上更具有平滑度，同时在大幅度的角度变化情况下，也能有很快的响应速度并能得到高精准的姿态角。

本发明具体过程如下：

步骤一：确定互补滤波器的初始化参数，包括互补滤波系数k_p、k_I等；

步骤二：由加速度计解算出重力场下的载体倾角，包括俯仰角θ_acc及横滚角

步骤三：以加速度计解算的倾角补偿磁力计的输出得到载体的偏航角ψ_m；

其中，θ_acc和为加速度计估计的俯仰角和横滚角；

步骤四：根据欧拉角的方法对陀螺的输出值进行坐标变换，解算出在导航坐标系下的三轴姿态信息；设陀螺仪的输出值为则欧拉角速率与三轴陀螺所测的角速率的关系如下：

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\γ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{ccc}cos\mathrm{\γ}& 0& sin\mathrm{\γ}\\ sin\mathrm{\γ}& 1& -cos\mathrm{\γ}\\ sin\mathrm{\γ}/cos\mathrm{\θ}& 0& -cos\mathrm{\γ}/cos\mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b\end{array}\right]---\left(3\right)$

其中，θ为俯仰角，λ为横滚角，ψ为偏航角，分别为其对应角的角速率；

所以根据陀螺仪所解算的俯仰角θ_gro为：

${\mathrm{\θ}}_{gro}={\mathrm{\θ}}_{gro}+\stackrel{\·}{\mathrm{\θ}}\·dt---\left(4\right)$

其中，dt为积分时间；

步骤五：实时修改滤波器参数k_p和k_I。k_p和k_I存在如下函数关系：

k_p+k_I＝1(5)

k_I根据如下函数式实时优化：

k_I＝f₁(t)+f₂(σ_acc，σ_gro)+f₃(Δθ)(6)

其中，t为移动机器人运行时间、σ_acc为加速度计所解算的俯仰角测量方差、σ_gro为陀螺仪所解算的俯仰角测量方差、Δθ为加速度计与陀螺仪所解算俯仰角的差值；

滤波器参数k_I和时间相关的函数关系式为：

$f_1\left(t\right)=\left\{\begin{array}{c}{a}_1,t\≤t_1\\ a_2,t>t_1\end{array}\right.---\left(7\right)$

其中常数a₁、a₂根据实验调试灵活取值；

滤波器参数k_I和加速度与陀螺仪计俯仰角测量方差相关的函数关系式为：

$f_2({\mathrm{\σ}}_{acc},{\mathrm{\σ}}_{gro})=0.001*{\mathrm{\σ}}_{acc}+\sqrt{{\mathrm{\σ}}_{gro}}---\left(8\right)$

滤波器参数k_I和加速度计与陀螺仪解算的俯仰角的差值相关的函数关系式为：

f₃(Δθ)＝a₃·Δθ(9)

其中常数a₃根据实验调试灵活取值。

在公式(8)与(9)中σ_acc、σ_gro、Δθ的计算公式如下：

${\mathrm{\σ}}_{acc}=\underset{j=1}{\overset{10}{\Σ}}{({\mathrm{\θ}}_{acc}-{\stackrel{\‾}{\mathrm{\θ}}}_{acc})}^2---\left(10\right)$

${\mathrm{\σ}}_{gro}=\underset{j=1}{\overset{10}{\Σ}}{({\mathrm{\θ}}_{gro}-{\stackrel{\‾}{\mathrm{\θ}}}_{gro})}^2---\left(11\right)$

$\mathrm{\Δ}\mathrm{\θ}=(\underset{j=1}{\overset{10}{\Σ}}{\mathrm{\θ}}_{acc}-\underset{j=1}{\overset{10}{\Σ}}{\mathrm{\θ}}_{gro})\·\mathrm{\ξ}---\left(12\right)$

其中的计算公式如下：

${\stackrel{\‾}{\mathrm{\θ}}}_{acc}=\underset{j=1}{\overset{10}{\Σ}}\left({\mathrm{\θ}}_{acc}\right)/10---\left(13\right)$

${\stackrel{\‾}{\mathrm{\θ}}}_{gro}=\underset{j=1}{\overset{10}{\Σ}}\left({\mathrm{\θ}}_{gro}\right)/10---\left(14\right)$

其中，θ_acc、θ_gro分别为加速度计和陀螺仪每次俯仰角的解算值；分别为加速度计与陀螺仪最近10次所解算的俯仰角的均值；ξ为角度校正系数；

步骤六：运用步骤五中参数可调的互补滤波器解算自平衡移动机器人的最优姿态角，利用如下公式融合处理：

其中，Δt为采样时间，θ_acc、ψ_m分别为加速度计测得的俯仰角、横滚角及补偿磁力计得到的偏航角，分别为陀螺仪测得对应轴的角速率，θ_pitch、θ_roll、θ_yaw为经过改进互补滤波后的最优三轴姿态角。

附图说明

图1改进互补滤波流程图

图2自平衡机器人坐标系示意图

图3改进互补滤波仿真图

图4改进互补滤波仿真局部图

图5一般互补滤波试验论证图

图6改进互补滤波试验论证图

具体实施方式

参照说明书附图对本发明的一种基于改进互补滤波的自平衡移动机器人姿态快速收敛方法作以下详细地说明。

如图1所示，详细的阐述了改进互补滤波的算法流程，最终解算的互补滤波角度具有很高的精度，且在初始时刻实时响应性较好。

在图2中，简要的描述了自平衡机器人坐标系示意图，其中，θ为俯仰角，λ为横滚角，ψ为偏航角。

在Matlab仿真中，取σ_acc＝0.2，σ_gro＝0.04，Δθ为符和高斯分布的方差为1，均值为0的白噪声。由图3和图4仿真可知，改进后的互补滤波在初始时刻具有更快的收敛性。

试验采用微控制器STM32F103、姿态检测模块MPU6050，并在设定好定时器的中断时间为5ms，在中断程序中执行改进互补滤波算法。

步骤一：确定互补滤波器的初始化参数，包括互补滤波系数k_p、k_I等；

步骤二：由加速度计解算出重力场下的载体倾角，包括俯仰角θ_acc及横滚角

步骤三：以加速度计解算的倾角补偿磁力计的输出得到载体的偏航角ψ_m；

其中，θ_acc和为加速度计估计的俯仰角和横滚角；

步骤四：根据欧拉角的方法对陀螺的输出值进行坐标变换，解算出在导航坐标系下的三轴姿态信息；设陀螺仪的输出值为则欧拉角速率与三轴陀螺所测的角速率的关系如下：

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\γ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{ccc}cos\mathrm{\γ}& 0& sin\mathrm{\γ}\\ sin\mathrm{\γ}& 1& -cos\mathrm{\γ}\\ sin\mathrm{\γ}/cos\mathrm{\θ}& 0& -cos\mathrm{\γ}/cos\mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b\end{array}\right]---\left(18\right)$

其中，θ为俯仰角，λ为横滚角，ψ为偏航角，分别为其对应角的角速率；

所以根据陀螺仪所解算的俯仰角θ_gro为：

${\mathrm{\θ}}_{gro}={\mathrm{\θ}}_{gro}+\stackrel{\·}{\mathrm{\θ}}\·dt---\left(19\right)$

其中，dt为积分时间；

步骤五：实时修改滤波器参数k_p和k_I。k_p和k_I存在如下函数关系：

k_p+k_I＝1(20)

k_I根据如下函数式实时优化：

k_I＝f₁(t)+f₂(σ_acc，σ_gro)+f₃(Δθ)(21)

其中，t为移动机器人运行时间、σ_acc为加速度计所解算的俯仰角测量方差、σ_gro为陀螺仪所解算的俯仰角测量方差、Δθ为加速度计与陀螺仪所解算俯仰角的差值；

滤波器参数k_I和时间相关的函数 $f_1\left(t\right)=\left\{\begin{array}{c}{a}_1,t\≤t_1\\ a_2,t>t_1\end{array}\right.,$ 其中常数a₁、a₂根据实验调试灵活取值；滤波器参数k_I和加速度与陀螺仪计俯仰角测量方差相关的函数滤波器参数k_I和加速度计与陀螺仪解算的俯仰角的差值相关的函数f₃(Δθ)＝a₃·Δθ，其中常数a₃根据实验调试灵活取值。

在公式(2)中σ_acc、σ_gro、Δθ的计算公式如下：

${\mathrm{\σ}}_{acc}=\underset{j=1}{\overset{10}{\Σ}}{({\mathrm{\θ}}_{acc}-{\stackrel{\‾}{\mathrm{\θ}}}_{acc})}^2---\left(22\right)$

${\mathrm{\σ}}_{gro}=\underset{j=1}{\overset{10}{\Σ}}{({\mathrm{\θ}}_{gro}-{\stackrel{\‾}{\mathrm{\θ}}}_{gro})}^2---\left(23\right)$

$\mathrm{\Δ}\mathrm{\θ}=(\underset{j=1}{\overset{10}{\Σ}}{\mathrm{\θ}}_{acc}-\underset{j=1}{\overset{10}{\Σ}}{\mathrm{\θ}}_{gro})\·\mathrm{\ξ}---\left(24\right)$

其中的计算公式如下：

${\stackrel{\‾}{\mathrm{\θ}}}_{acc}=\underset{j=1}{\overset{10}{\Σ}}\left({\mathrm{\θ}}_{acc}\right)/10---\left(25\right)$

${\stackrel{\‾}{\mathrm{\θ}}}_{gro}=\underset{j=1}{\overset{10}{\Σ}}\left({\mathrm{\θ}}_{gro}\right)/10---\left(26\right)$

其中，θ_acc、θ_gro分别为加速度计和陀螺仪每次俯仰角的解算值；分别为加速度计与陀螺仪最近10次所解算的俯仰角的均值；ξ为角度校正系数；

步骤六：运用步骤五中参数可调的互补滤波器解算自平衡移动机器人的最优姿态角，利用如下公式融合处理：

其中，Δt为采样时间，θ_acc、ψ_m分别为加速度计测得的俯仰角、横滚角及补偿磁力计得到的偏航角，分别为陀螺仪测得对应轴的角速率，θ_pitch、θ_roll、θ_yaw为经过改进互补滤波后的最优三轴姿态角。

经试验调试，选定说明书中公式(7)、(9)的参数a₁、a₂、a₃，则可得出以下函数：

$f_1\left(t\right)=\left\{\begin{array}{c}0.2,t\≤1200ms\\ 0.001,t>1200ms\end{array}\right.,f_2\left({\mathrm{\σ}}_{acc},{\mathrm{\σ}}_{gro}\right)=0.001*{\mathrm{\σ}}_{acc}+\sqrt{{\mathrm{\σ}}_{gro}},$ f₃(Δθ)＝0.2*Δθ

其中，t为移动机器人运行时间、σ_acc为加速度计所解算的俯仰角测量方差、σ_gro为陀螺仪所解算的俯仰角测量方差、Δθ为加速度计与陀螺仪所解算俯仰角的差值；

在实验论证中，在中断每5ms时间内采集一次加速度计、陀螺仪、磁力计的输出值和互补滤波后姿态角。如图5所示，对于一般互补滤波，对陀螺仪和加速度计进行互补滤波的角度值在初始阶段很难快速跟上移动机器人的初始真实角度值，动态性能差，收敛慢；当在移动机器人中运用本发明所述的改进互补滤波算法，如图6所示，能很好解决互补滤波在初始时刻的姿态收敛问题，具体变现为启动平稳迅速，无磕绊不适感。

表1一般互补滤波和改进互补滤波姿态收敛时间的比较

将机器人在初始时刻加速度计所解算的姿态角作为参考值，分别根据改进前后的互补滤波进行实验，得到滤波后的角度第一次和参考值相等的时间，允许5％的误差率。根据实验得出表1，对于一般互补滤波，机器人姿态收敛时间均值为192ms；而对于改进互补滤波，机器人姿态收敛时间约为99ms，较一般互补滤波收敛时间减少48.4％，姿态收敛速度大大提高。

Claims (4)

1.一种基于改进互补滤波的自平衡移动机器人姿态快速收敛方法，其特征在于，具体包括以下步骤：

步骤一：确定互补滤波器的初始化参数，包括互补滤波系数k_p、k_I等；

步骤二：实时修改滤波器参数k_p和k_I。k_p和k_I存在如下函数关系：

k_p+k_I＝1(1)

k_I根据如下函数式实时优化：

k_I＝f₁(t)+f₂(σ_acc，σ_gro)+f₃(Δθ)(2)

其中，t为移动机器人运行时间、σ_acc为加速度计所解算的俯仰角测量方差、σ_gro为陀螺仪所解算的俯仰角测量方差、Δθ为加速度计与陀螺仪所解算俯仰角的差值；

步骤三：由加速度计解算出重力场下的载体倾角，包括俯仰角θ_acc及横滚角

步骤四：以加速度计解算的倾角补偿磁力计的输出得到载体的偏航角ψ_m；

其中，θ_acc和为加速度计估计的俯仰角和横滚角；

步骤五：根据欧拉角的方法对陀螺的输出值进行坐标变换，解算出在导航坐标系下的三轴姿态信息；设陀螺仪的输出值为则欧拉角速率与三轴陀螺所测的角速率的关系如下：

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\γ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{ccc}cos\mathrm{\γ}& 0& sin\mathrm{\γ}\\ sin\mathrm{\γ}& 1& -cos\mathrm{\γ}\\ \sin \mathrm{\γ}/cos\mathrm{\θ}& 0& -\cos \mathrm{\γ}/cos\mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{{\mathrm{\ω}}_n^b}_{bx}\\ {{\mathrm{\ω}}_n^b}_{by}\\ {{\mathrm{\ω}}_n^b}_{bz}\end{array}\right]---\left(5\right)$

其中，θ为俯仰角，λ为横滚角，ψ为偏航角，分别为其对应角的角速率；

所以根据陀螺仪所解算的俯仰角θ_gro为：

${\mathrm{\θ}}_{gro}={\mathrm{\θ}}_{gro}+\stackrel{\·}{\mathrm{\θ}}\·dt---\left(6\right)$

其中，dt为积分时间；

步骤六：计算滤波器参数k_p和k_I与t、σ_acc、σ_gro的函数关系式，并带入公式(7)中解算自平衡移动机器人的最优姿态角：

其中，Δt为采样时间，θ_acc、ψ_m分别为加速度计测得的俯仰角、横滚角及补偿磁力计得到的偏航角，分别为陀螺仪测得对应轴的角速率，θ_pitch、θ_roll、θ_yaw为经过改进互补滤波后的最优三轴姿态角。

2.根据权利要求1所述的一种基于改进互补滤波的自平衡移动机器人姿态快速收敛方法，其特征在于步骤二中互补滤波器参数k_I是实时优化的且函数表达式如下：

k_I＝f₁(t)+f₂(σ_acc，σ_gro)+f₃(Δθ)(8)

在公式(4)中σ_acc、σ_gro、Δθ的计算公式如下：

${\mathrm{\σ}}_{acc}=\underset{i=1}{\overset{10}{\Σ}}{({\mathrm{\θ}}_{acc}-{\stackrel{\‾}{\mathrm{\θ}}}_{acc})}^2---\left(9\right)$

${\mathrm{\σ}}_{gro}=\underset{j=1}{\overset{10}{\Σ}}{({\mathrm{\θ}}_{gro}-{\stackrel{\‾}{\mathrm{\θ}}}_{gro})}^2---\left(10\right)$

$\mathrm{\Δ}\mathrm{\θ}=(\underset{i=1}{\overset{10}{\Σ}}{\mathrm{\θ}}_{acc}-\underset{j=1}{\overset{10}{\Σ}}{\mathrm{\θ}}_{gro})\·\mathrm{\ξ}---\left(11\right)$

其中的计算公式如下：

${\stackrel{\‾}{\mathrm{\θ}}}_{acc}=\underset{i=1}{\overset{10}{\Σ}}\left({\mathrm{\θ}}_{acc}\right)/10---\left(12\right)$

${\stackrel{\‾}{\mathrm{\θ}}}_{gro}=\underset{i=1}{\overset{10}{\Σ}}\left({\mathrm{\θ}}_{gro}\right)/10---\left(13\right)$

其中，θ_acc、θ_gro分别为加速度计和陀螺仪每次俯仰角的解算值；分别为加速度计与陀螺仪最近10次所解算的俯仰角的均值；ξ为角度校正系数。

3.根据权利要求2所述的一种基于改进互补滤波的自平衡移动机器人姿态快速收敛方法，其特征在于：滤波器参数k_I和时间相关的函数关系式为：

$f_1\left(t\right)=\left\{\begin{array}{c}{a}_1,t\≤t_1\\ a_2,t>t_1\end{array}\right.---\left(14\right)$

其中常数a₁、a₂根据实验调试灵活取值；

滤波器参数k_I和加速度与陀螺仪计俯仰角测量方差相关的函数关系式为：

$f_2({\mathrm{\σ}}_{acc},{\mathrm{\σ}}_{gro})=0.001*{\mathrm{\σ}}_{acc}+\sqrt{{\mathrm{\σ}}_{gro}}---\left(15\right)$

滤波器参数k_I和加速度计与陀螺仪解算的俯仰角的差值相关的函数关系式为：

f₃(Δθ)＝a₃·Δθ(16)

其中常数a₃根据实验调试灵活取值。

4.根据权利要求1所述的一种基于改进互补滤波的自平衡移动机器人姿态快速收敛方法，其特征在于步骤六中自平衡移动机器人最优姿态角的求解算法：

自平衡移动机器人的最优三轴姿态角采用参数可调的互补滤波器解算，即将公式(17)与(18)代入公式(19)中，

k_I＝f₁(t)+f₂(σ_acc，σ_gro)+f₃(Δθ)(17)

k_p＝1-f₁(t)+f₂(σ_acc，σ_gro)+f₃(Δθ)(18)

从而得到自平衡移动机器人的最优姿态角。