[發(fā)明專利]一種基于改進(jìn)互補(bǔ)濾波的自平衡移動(dòng)機(jī)器人姿態(tài)快速收斂方法

有效

• 申請?zhí)枺?/td>: 201510833686.7
• 申請日: 2015-11-23
• 公開（公告）號(hào): CN105300381B
• 公開（公告）日: 2018-09-28
• 發(fā)明（設(shè)計(jì)）人: 周翟和;呂建新;趙慶濤;虞波;田祥瑞;姚睿;崔江
• 申請（專利權(quán)）人: 南京航空航天大學(xué)
• 主分類號(hào): G01C21/16
• 分類號(hào): G01C21/16
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于改進(jìn)互補(bǔ)濾波的自平衡移動(dòng)機(jī)器人姿態(tài)快速收斂方法，其特征在于，具體包括以下步驟：

步驟一：確定互補(bǔ)濾波器的初始化參數(shù)，包括互補(bǔ)濾波系數(shù)k_p、k_l等；

步驟二：實(shí)修改時(shí)濾波器參數(shù)k_p和k_l且k_p和k_l存在如下函數(shù)關(guān)系：

k_p+k_l＝1 (1)

k_l根據(jù)如下函數(shù)式實(shí)時(shí)優(yōu)化：

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

其中，t為移動(dòng)機(jī)器人運(yùn)行時(shí)間、σ_acc為加速度計(jì)所解算的俯仰角測量方差、σ_gro為陀螺儀所解算的俯仰角測量方差、Δθ為加速度計(jì)與陀螺儀所解算俯仰角的差值；

步驟三：由加速度計(jì)解算出重力場下的載體傾角，包括俯仰角θ_acc及橫滾角

其中，a_x、a_y、a_z分別為重力場下的x軸分量、y軸分量、z軸分量；

步驟四：以加速度計(jì)解算的傾角補(bǔ)償磁力計(jì)的輸出得到載體的偏航角ψ_m；

其中，θ_acc和為加速度計(jì)估計(jì)的俯仰角和橫滾角，m_x、m_y、m_z分別為磁力計(jì)輸出的x軸分量、y軸分量、z軸分量；

步驟五：根據(jù)歐拉角的方法對陀螺的輸出值進(jìn)行坐標(biāo)變換，解算出在導(dǎo)航坐標(biāo)系下的三軸姿態(tài)信息；設(shè)陀螺儀的輸出值為則歐拉角速率與三軸陀螺所測的角速率的關(guān)系如下：

其中，θ為俯仰角，λ為橫滾角，ψ為偏航角，分別為其對應(yīng)角的角速率；

所以根據(jù)陀螺儀所解算的俯仰角θ_gro為：

其中，dt為積分時(shí)間，θ_gro^*為上一時(shí)刻所解算的俯仰角；

步驟六：計(jì)算濾波器參數(shù)k_p和k_l與t、σ_acc、σ_gro的函數(shù)關(guān)系式，并帶入公式(7)中解算自平衡移動(dòng)機(jī)器人的最優(yōu)姿態(tài)角：

其中，Δt為采樣時(shí)間，θ_acc、ψ_m分別為加速度計(jì)測得的俯仰角、橫滾角及補(bǔ)償磁力計(jì)得到的偏航角，分別為陀螺儀測得對應(yīng)軸的角速率，θ_pitch、θ_roll、θ_yaw為經(jīng)過改進(jìn)互補(bǔ)濾波后的最優(yōu)三軸姿態(tài)角。

2.根據(jù)權(quán)利要求1所述的一種基于改進(jìn)互補(bǔ)濾波的自平衡移動(dòng)機(jī)器人姿態(tài)快速收斂方法，其特征在于步驟二中互補(bǔ)濾波器參數(shù)k_l是實(shí)時(shí)優(yōu)化的且函數(shù)表達(dá)式如下：

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

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

其中的計(jì)算公式如下：

其中，θ_acc、θ_gro分別為加速度計(jì)和陀螺儀每次俯仰角的解算值；分別為加速度計(jì)與陀螺儀最近10次所解算的俯仰角的均值；ξ為角度校正系數(shù)。

3.根據(jù)權(quán)利要求2所述的一種基于改進(jìn)互補(bǔ)濾波的自平衡移動(dòng)機(jī)器人姿態(tài)快速收斂方法，其特征在于：濾波器參數(shù)k_l和時(shí)間相關(guān)的函數(shù)關(guān)系式為：

其中a₁、a₂為常數(shù)；

濾波器參數(shù)k_l和加速度與陀螺儀計(jì)俯仰角測量方差相關(guān)的函數(shù)關(guān)系式為：

濾波器參數(shù)k_l和加速度計(jì)與陀螺儀解算的俯仰角的差值相關(guān)的函數(shù)關(guān)系式為：

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

其中a₃為常數(shù)。