基于四旋翼機(jī)器人平臺(tái)的非線性濾波算法研究*

陳 俊, 強(qiáng) 俊

(1.安徽機(jī)電職業(yè)技術(shù)學(xué)院 電氣工程系，安徽 蕪湖 241000；2.安徽工程大學(xué) 計(jì)算機(jī)與信息學(xué)院，安徽 蕪湖 241000)

基于四旋翼機(jī)器人平臺(tái)的非線性濾波算法研究*

陳 俊1, 強(qiáng) 俊2

(1.安徽機(jī)電職業(yè)技術(shù)學(xué)院 電氣工程系，安徽 蕪湖 241000；2.安徽工程大學(xué) 計(jì)算機(jī)與信息學(xué)院，安徽 蕪湖 241000)

以四旋翼機(jī)器人姿態(tài)控制為研究目標(biāo)，用四元數(shù)進(jìn)行姿態(tài)描述，非線性濾波算法為主要研究對(duì)象，基于Matlab仿真平臺(tái)，對(duì)四旋翼機(jī)器人姿態(tài)控制進(jìn)行設(shè)計(jì)，并以非線性濾波算法的優(yōu)化為重點(diǎn)，結(jié)合常用的濾波算法進(jìn)行比較，實(shí)驗(yàn)結(jié)果表明：RPF更適用于機(jī)器人的姿態(tài)角度更新計(jì)算。

四旋翼；Matlab仿真；四元數(shù)；非線性濾波；RPF

近年來，隨著微機(jī)電系統(tǒng)、新型材料(玻纖、碳纖等)以及傳感技術(shù)的蓬勃發(fā)展，飛行機(jī)器人取得了飛速的發(fā)展和應(yīng)用，伴隨著其負(fù)載能力、續(xù)航能力的提高，飛行機(jī)器人正不斷開拓功能，延伸到各個(gè)應(yīng)用領(lǐng)域[1]。

四旋翼機(jī)器人伴普及，逐步成為飛行機(jī)器人領(lǐng)域的標(biāo)準(zhǔn)研究平臺(tái)。一般四旋翼飛行器直觀上來看是由4個(gè)獨(dú)立的螺旋槳通過十字框架結(jié)構(gòu)相連，位于機(jī)身前方的螺旋槳為1號(hào)螺旋槳，奇數(shù)序的螺旋槳按逆時(shí)針方向旋轉(zhuǎn)，偶數(shù)序的按順時(shí)針方向旋轉(zhuǎn)，機(jī)器人的定位、啟動(dòng)、升降或是航向均是由這4個(gè)螺旋槳協(xié)同工作實(shí)現(xiàn)[2]。因此，如何結(jié)合機(jī)器人的當(dāng)前姿態(tài)，對(duì)各螺旋槳的轉(zhuǎn)速進(jìn)行實(shí)時(shí)控制，是實(shí)現(xiàn)四旋翼飛行器機(jī)體穩(wěn)定可控的關(guān)鍵問題。

四旋翼飛行器是典型的欠驅(qū)動(dòng)系統(tǒng)，輸入量只有各螺旋槳的拉力，輸出量則包括了機(jī)身的速度、加速度、姿態(tài)(傾斜角度)、水平位移以及高度等。在飛行器的姿態(tài)解算中，依靠的最重要部件就是慣性器件——加速度計(jì)和陀螺儀。加速度計(jì)具有良好的低頻特性，可以測(cè)量低速的靜態(tài)加速度；陀螺儀具有較好的高頻特性，可以測(cè)量高速旋轉(zhuǎn)運(yùn)動(dòng)。所以，要想實(shí)現(xiàn)對(duì)飛行器的精準(zhǔn)控制，就要結(jié)合先進(jìn)的濾波技術(shù)，得到精確的姿態(tài)解算結(jié)果，這是飛行器運(yùn)行的最關(guān)鍵問題[3]。

1 常用的姿態(tài)描述方法及其分析

常用的姿態(tài)描述方法包括了歐拉角法、方向余弦陣法、羅德里格參數(shù)法、四元數(shù)法等。歐拉角法是將機(jī)體的運(yùn)動(dòng)轉(zhuǎn)化為三次定軸轉(zhuǎn)動(dòng)，分別為繞z軸轉(zhuǎn)ψ，繞y軸轉(zhuǎn)θ，繞x軸轉(zhuǎn)φ，從而建立歐拉角微分方程，結(jié)合tait-bryan公式，p，q，r為3個(gè)坐標(biāo)軸方向的角速度，則可以得到如下解

(1)

方向余弦陣可以做到與姿態(tài)變換的一一對(duì)應(yīng)，基于李代數(shù)中的反對(duì)稱矩陣計(jì)算，可建立其微分方程公式

(2)

(3)

這里，ω為旋轉(zhuǎn)軸單位矢量，θ為轉(zhuǎn)軸方向上轉(zhuǎn)過的角度。此方法計(jì)算量過大，對(duì)于有高實(shí)時(shí)性要求的飛行機(jī)器人來說，不適用，并且，在轉(zhuǎn)向θ比較小的情況下，會(huì)使得ω產(chǎn)生較大的誤差。

四元數(shù)法相對(duì)來說，具有計(jì)算量較小且可實(shí)現(xiàn)全姿態(tài)工作的優(yōu)勢(shì)。四元數(shù)是由四個(gè)元構(gòu)成，其基本結(jié)構(gòu)為Q=q0+q1i+q2j+q3k

(4)

(5)

由于機(jī)身是運(yùn)動(dòng)的，所以用四元數(shù)描述對(duì)象姿態(tài)時(shí)，Q本身也是動(dòng)態(tài)的，所以可建立Q對(duì)時(shí)間的微分方程：

(6)

陀螺的測(cè)量誤差會(huì)隨時(shí)間累積放大，工作時(shí)間越長(zhǎng)，產(chǎn)生漂移越大，使得精度降低，故而需要借助其他姿態(tài)測(cè)量傳感器進(jìn)行修正，所以，姿態(tài)確定系統(tǒng)基本都是以陀螺和其他敏感器組合而成。由于姿態(tài)敏感器的輸出有測(cè)量誤差，且不一定是載體姿態(tài)參數(shù)，所以需要先進(jìn)的濾波算法處理測(cè)量數(shù)據(jù)。如何使用先進(jìn)的濾波算法，克服不確定因素對(duì)姿態(tài)估計(jì)的影響，從而實(shí)現(xiàn)對(duì)姿態(tài)變換的精確描述，是本文的主要任務(wù)。

2 非線性濾波算法

濾波理論從本質(zhì)上來說就是利用系統(tǒng)觀測(cè)信號(hào)，估計(jì)狀態(tài)概率分布函數(shù)。目前應(yīng)用最廣泛的卡爾曼濾波是以最小均方誤差為準(zhǔn)則，建立的一個(gè)線性濾波器。然而，實(shí)際應(yīng)用中通常是非線性狀態(tài)，在此前提下研究姿態(tài)確定系統(tǒng)的濾波問題。貝葉斯最優(yōu)估計(jì)給出了非線性函數(shù)濾波最優(yōu)解，但其系統(tǒng)模型具有非線性特性，算法中的加權(quán)積分部分后驗(yàn)概率密度函數(shù)難以求得，因而在實(shí)際應(yīng)用中難有作為[4]。

基于此，不少貝葉斯逼近次優(yōu)濾波算法被相繼提出，主要可分為兩類：基于系統(tǒng)狀態(tài)分布為高斯分布的高斯非線性濾波和基于蒙特卡洛采樣逼近思想的非高斯濾波，這其中，以擴(kuò)展卡爾曼濾波(EKF)、粒子濾波(PF)、差值濾波等最具代表性。由于四旋翼飛行器屬于非線性非高斯系統(tǒng)，粒子濾波(PF)就是利用蒙特卡洛采樣直接逼近系統(tǒng)的狀態(tài)后驗(yàn)概率密度函數(shù)，這從一定程度上實(shí)現(xiàn)了非線性非高斯系統(tǒng)的全局近似最優(yōu)估計(jì)。我們對(duì)此算法做著重分析研究。

2.1 蒙特卡洛采樣思想

2.2 序貫重要性重采樣算法(SIR)

依據(jù)貝葉斯重要性采樣，考慮到其遞推的特點(diǎn)，為避免每次新的觀測(cè)值都重新采樣計(jì)算樣本權(quán)值，序貫重要性采樣(SIS)算法被提出，其基本思想是從一個(gè)重要性概率密度中抽取一組帶有權(quán)值的樣本，以用于代表系統(tǒng)對(duì)象的后驗(yàn)概率密度函數(shù)，最后通過這組樣本的加權(quán)求和做狀態(tài)估計(jì)值。

SIS的問題在于，這組樣本在經(jīng)過迭代計(jì)算后會(huì)產(chǎn)生退化問題，具體表現(xiàn)為大部分樣本數(shù)據(jù)(粒子)的權(quán)值極小至可忽略，致使整體樣本權(quán)值的方差逐步增大，導(dǎo)致大量的運(yùn)算做了“無用功”，我們稱之為樣本退化現(xiàn)象?；诖?，序貫重要性重采樣算法(SIR)應(yīng)運(yùn)而生。它針對(duì)原有樣本集合粒子的離散分布，采取隨機(jī)選取復(fù)制的方式，以“權(quán)值較高粒子獲得較大概率復(fù)制”為原則，避免了退化問題，然而，這種方式使得許多權(quán)值較小的粒子失去了被復(fù)制的機(jī)會(huì)，造成粒子多樣性的丟失，從而使得濾波精度下降。所以，粒子濾波的重點(diǎn)在于：在合理的重要性概率密度選取下，不至于使粒子多樣性丟失的重采樣的優(yōu)化。

2.3 粒子濾波算法的優(yōu)化

如前文所述，SIR是系統(tǒng)狀態(tài)在離散近似分布下，依據(jù)粒子的權(quán)值大小進(jìn)行復(fù)制更新，使得粒子中原本處于“少數(shù)派”的高權(quán)重粒子得到了復(fù)制，而低權(quán)重的粒子得到了“忽略”，從而抑制了粒子多樣性?；诖?，引入Musso提出的正則化粒子濾波(RPF)用于姿態(tài)估計(jì)。該算法與SIR最大的不同在于，它將在系統(tǒng)狀態(tài)的連續(xù)近似分布下進(jìn)行采樣，從而有效解決了重采樣導(dǎo)致的粒子貧化問題。

RPF的基本思路是：

(1) 初始化。產(chǎn)生服從初始概率密度p(x0)的N個(gè)粒子，記為x0(i)，i=1,2,…,N。

(3) 量測(cè)更新。更新并歸一化粒子權(quán)重ωk(i)，計(jì)算狀態(tài)估計(jì)值xk和方差Pk：

(7)

(8)

(9)

(10)

(5) 狀態(tài)更新。

(11)

(12)

RPF算法在一定程度上較好解決了粒子貧化問題，雖然增加了核密度函數(shù)采樣，但計(jì)算量并未明顯增大，在系統(tǒng)干擾不大的情況下，可提高姿態(tài)解算精度[5]。

3 仿真實(shí)驗(yàn)

基于Matlab R2015b版本仿真平臺(tái)，考慮到本文采用了粒子濾波的重采樣和正則化方法，同時(shí)抑制了粒子退化和多樣性貧化問題，所以粒子數(shù)目可有效降低，初始粒子總數(shù)N為500[6]。

對(duì)陀螺儀以500 hz采樣頻率采集10 s，假定噪聲分布為非高斯情況，設(shè)置系統(tǒng)初始狀態(tài)x=0.1，仿真運(yùn)行時(shí)間為5 000個(gè)節(jié)點(diǎn)，初始過程噪聲方差Q=10，初始測(cè)量噪聲方差R=5，后驗(yàn)誤差初值P=5。仿真實(shí)驗(yàn)結(jié)果如圖1和圖2所示。為了顯示清楚，特將100個(gè)節(jié)點(diǎn)下的仿真結(jié)果如圖1所示。

可以看到，在系統(tǒng)干擾不是很大的情況下，RPF算法在姿態(tài)解算中更好地解決了粒子多樣性缺失呈現(xiàn)的貧化問題，且在運(yùn)算速度上與SIR保持相當(dāng)，從誤差圖上來看，RPF的姿態(tài)解算精度比SIR有較為顯著的提升，且并沒有犧牲系統(tǒng)的實(shí)時(shí)性。

圖1 RPF和SIR濾波實(shí)驗(yàn)對(duì)比及誤差對(duì)比圖(100節(jié)點(diǎn))Fig.1 The error comparison of RPF and SIR filtering experiment (100 nodes)

圖2 RPF和SIR濾波實(shí)驗(yàn)對(duì)比及誤差對(duì)比圖(5000節(jié)點(diǎn))Fig.2 The error comparison of RPF and SIR filtering experiment (5000 nodes)

4 結(jié)束語

以四旋翼飛行機(jī)器人的姿態(tài)解算為對(duì)象，分析了姿態(tài)解算的可實(shí)施方法，以飛行器的姿態(tài)確定系統(tǒng)為切入點(diǎn)，提出了將RPF融入姿態(tài)解算，并與SIR算法作比較，較好地解決了粒子退化和多樣性貧化問題，對(duì)四旋翼飛行機(jī)器人姿態(tài)解算問題提供了較有價(jià)值的解決途徑。

[1] CORKE P I,ROBOTICS.Vision & Control:Fundamental Algorithms in MATLAB[M].Berlin:Springer-Verlag,2011

[2] KITAGAWA G.Monte Carlo Filter and Smoother for Non-Gaussian Nonlinear State Space Models[J].Journal of Computational & Graphical Statistics,2012,5(1):1-25

[3] MELLINGER D, KUMAR V.Minimum Snap Trajectory Generation and Control for Quadrotors[C]∥Robotics and Automation(ICRA).Shanghai,China,2011

[4] 喬相偉.基于四元數(shù)非線性濾波的飛行器姿態(tài)確定算法研究[D].哈爾濱：哈爾濱工程大學(xué),2011

QIAO X W.Attitude Determination Algorithm Based on Quaternion Nonlinear Filter Foe Spacecraft[D].Harbin:Harbin Engineering University,2011

[5] 曹蓓.粒子濾波改進(jìn)算法及其應(yīng)用研究[D].西安：中國科學(xué)院,2012

CAO B.Research on Improved Algorithms and Applications Based on Particle Filter[D].Xi’an:University of Chinese Academy of Sciences,2012

[6] 陳俊.智能算法在足球機(jī)器人定向運(yùn)動(dòng)中的應(yīng)用[J].重慶工商大學(xué)學(xué)報(bào)(自然科學(xué)版),2013,30(3):46-49

CHEN J.Application of Intelligence Algorithm to Oriented Movement of Soccer Robot[J].Chongqing Technology and Business University(Natural Science Edition),2013,30(3):46-49

責(zé)任編輯：李翠薇

Research on Nonlinear Filtering Algorithm Based on Quadrotors

CHEN Jun1, QIANG Jun2

(1.Anhui Mechanical College of Mechanic and Electrical Engineering, Anhui Wuhu 241000, China; 2. School of Computer and Information Science, Anhui Polytechnic University, Anhui Wuhu 241000, China)

This paper takes the attitude control of quadrotors as the research object, takes quaternion as pose description, uses nonlinear filtering algorithm as the main research target, based on Matlab simulation platform, makes the attitude control design for the quadrotors, takes the optimization of nonlinear filtering algorithm as the emphasis, and makes comparison by combining usual filtering algorithms.The experimental results show that RPF is more applicable to attitude angel renew calculation.

quadrotors; Matlab simulation; quaternion; nonlinear filtering; RPF

10.16055/j.issn.1672-058X.2017.0003.014

2016-08-20；

2016-10-20. * 基金項(xiàng)目：安徽高校自然科學(xué)研究重點(diǎn)項(xiàng)目(KJ2015A353)；安徽高校自然科學(xué)研究重點(diǎn)項(xiàng)目(KJ2016A802)；安徽省高等教育提升計(jì)劃省級(jí)自然科學(xué)研究一般項(xiàng)目(TSKJ2015B16)；安徽工程大學(xué)計(jì)算機(jī)應(yīng)用重點(diǎn)實(shí)驗(yàn)室開放基金(JSJKF201501).

陳俊(1982-)，男，安徽蕪湖人，副教授，碩士研究生，從事智能控制和自動(dòng)檢測(cè)研究.

TP181

A

1672-058X(2017)03-0077-05