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

有效

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

本發(fā)明提出的一種基于改進(jìn)互補(bǔ)濾波的自平衡移動(dòng)機(jī)器人姿態(tài)快速收斂方法，屬于運(yùn)動(dòng)控制與多傳感器數(shù)據(jù)融合技術(shù)領(lǐng)域，主要作用于移動(dòng)機(jī)器人起始運(yùn)動(dòng)階段，使之實(shí)現(xiàn)快速平穩(wěn)啟動(dòng)。姿態(tài)檢測(cè)模塊主要包括微控制器、陀螺儀、加速度計(jì)和磁力計(jì)等MEMS慣性傳感器。該系統(tǒng)利用加速度計(jì)重力場(chǎng)分量估計(jì)的傾角作為輔助信息，通過傾角補(bǔ)償?shù)玫接纱帕τ?jì)解算的偏航角，并運(yùn)用歐拉角姿態(tài)解算方法得到陀螺儀在三軸的角速率信息。通過設(shè)計(jì)一個(gè)參數(shù)靈活可調(diào)的互補(bǔ)濾波器對(duì)陀螺儀和輔助傳感器的數(shù)據(jù)進(jìn)行融合處理，實(shí)時(shí)解算出最優(yōu)姿態(tài)角。本發(fā)明不僅能夠使姿態(tài)檢測(cè)模塊快速響應(yīng)初始狀態(tài)的姿態(tài)角，而且能明顯抑制噪聲和漂移誤差，使移動(dòng)機(jī)器人實(shí)現(xiàn)快速平穩(wěn)啟動(dòng)，增強(qiáng)其穩(wěn)定性能。

技術(shù)領(lǐng)域

本發(fā)明提供的一種基于改進(jìn)互補(bǔ)濾波的自平衡移動(dòng)機(jī)器人姿態(tài)快速收斂方法，屬于數(shù)字濾波和多傳感器數(shù)據(jù)融合技術(shù)領(lǐng)域，主要作用于移動(dòng)機(jī)器人起始運(yùn)動(dòng)階段，使之實(shí)現(xiàn)快速平穩(wěn)啟動(dòng)。

背景技術(shù)

姿態(tài)解算的精度和速度將直接影響移動(dòng)或飛行控制算法的穩(wěn)定性、可靠性和實(shí)現(xiàn)的難易程度，所以，姿態(tài)解算是移動(dòng)或飛行控制實(shí)現(xiàn)的前提。隨著MEMS技術(shù)和計(jì)算機(jī)技術(shù)的發(fā)展，小型兩輪自平衡機(jī)器人姿態(tài)的測(cè)量普遍采用低成本的捷聯(lián)慣性測(cè)量單元IMU，其主要由低成本陀螺儀、加速度傳感器和電子羅盤組成。MEMS陀螺儀具有溫度漂移特性，加速度傳感器會(huì)受到自平衡機(jī)器人移動(dòng)過程中機(jī)體振動(dòng)的影響。因此，如何融合IMU多傳感器的數(shù)據(jù)，濾除外部干擾，得到高可靠性、高精度的姿態(tài)數(shù)據(jù)，是一項(xiàng)非常具有挑戰(zhàn)性的工作。互補(bǔ)濾波器算法簡(jiǎn)單可靠，能較好地結(jié)合陀螺儀角速度的動(dòng)態(tài)性能和加速度計(jì)的靜態(tài)精度，可以剔除高頻運(yùn)動(dòng)加速度，在低成本的INS導(dǎo)航系統(tǒng)中應(yīng)用廣泛。由于陀螺儀短時(shí)精度高，長(zhǎng)時(shí)間故障會(huì)引起漂移。而對(duì)于加速度計(jì)，短時(shí)間內(nèi)精度沒有陀螺儀高，但是長(zhǎng)時(shí)間卻能保持穩(wěn)定。同時(shí)根據(jù)上文的分析，陀螺顯示為高通特性，加速度計(jì)顯示低通特性，它們?cè)陬l域上可以相互補(bǔ)充，從而實(shí)現(xiàn)高精度的姿態(tài)測(cè)量。所以互補(bǔ)濾波算法能同時(shí)濾除低頻干擾與高頻干擾，實(shí)現(xiàn)傳感器數(shù)據(jù)融合。但姿態(tài)求解器傳遞函數(shù)C(s)往往取定值，則低通濾波系數(shù)G_L(s)和高通濾波系數(shù)G_H(s)為常量，且在最終時(shí)刻G_H(s)＞＞G_L(s)。不能滿足初始時(shí)刻姿態(tài)的快速收斂要求，具體表現(xiàn)為對(duì)陀螺儀和加速度計(jì)進(jìn)行互補(bǔ)濾波的角度值在初始階段很難快速跟上移動(dòng)機(jī)器人的初始真實(shí)角度值，動(dòng)態(tài)性能差，收斂慢甚至易摔倒，故未經(jīng)改進(jìn)的互補(bǔ)濾波技術(shù)還存在一定的缺陷。

發(fā)明內(nèi)容

本發(fā)明的目的在于針對(duì)上述存在的問題和不足，提出一種用于移動(dòng)機(jī)器人姿態(tài)檢測(cè)的能夠快速響應(yīng)、有更強(qiáng)適應(yīng)性的改進(jìn)互補(bǔ)濾波方法，且算法較為簡(jiǎn)單，對(duì)噪聲和漂移抑制明顯，不僅在數(shù)據(jù)上更具有平滑度，同時(shí)在大幅度的角度變化情況下，也能有很快的響應(yīng)速度并能得到高精準(zhǔn)的姿態(tài)角。

本發(fā)明具體過程如下：

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

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

k_p+k_I＝1 (1)

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

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

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