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

摘要

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

主权项

1.一种基于改进互补滤波的自平衡移动机器人姿态快速收敛方法，其特征在于，具体包括以下步骤：步骤一：确定互补滤波器的初始化参数，包括互补滤波系数kp、kl等；步骤二：实修改时滤波器参数kp和kl且kp和kl存在如下函数关系：kp+kl＝1    (1)kl根据如下函数式实时优化：kl＝f1(t)+f2(σacc，σgro)+f3(Δθ)        (2)其中，t为移动机器人运行时间、σacc为加速度计所解算的俯仰角测量方差、σgro为陀螺仪所解算的俯仰角测量方差、Δθ为加速度计与陀螺仪所解算俯仰角的差值；步骤三：由加速度计解算出重力场下的载体倾角，包括俯仰角θ_acc及横滚角其中，ax、ay、az分别为重力场下的x轴分量、y轴分量、z轴分量；步骤四：以加速度计解算的倾角补偿磁力计的输出得到载体的偏航角ψm；其中，θ_acc和为加速度计估计的俯仰角和横滚角，m_x、m_y、m_z分别为磁力计输出的x轴分量、y轴分量、z轴分量；步骤五：根据欧拉角的方法对陀螺的输出值进行坐标变换，解算出在导航坐标系下的三轴姿态信息；设陀螺仪的输出值为则欧拉角速率与三轴陀螺所测的角速率的关系如下：其中，θ为俯仰角，λ为横滚角，ψ为偏航角，分别为其对应角的角速率；所以根据陀螺仪所解算的俯仰角θgro为：其中，dt为积分时间，θgro*为上一时刻所解算的俯仰角；步骤六：计算滤波器参数kp和kl与t、σacc、σgro的函数关系式，并带入公式(7)中解算自平衡移动机器人的最优姿态角：其中，Δt为采样时间，θ_acc、ψ_m分别为加速度计测得的俯仰角、横滚角及补偿磁力计得到的偏航角，分别为陀螺仪测得对应轴的角速率，θ_pitch、θ_roll、θ_yaw为经过改进互补滤波后的最优三轴姿态角。