一種應用於四旋翼機器人平臺的非線性濾波算法的製作方法

2023-09-27 07:51:00

本發明屬於四旋翼機器人技術領域，具體涉及一種應用於四旋翼機器人平臺的非線性濾波算法。

背景技術：

近年來，隨著微機電系統、新型材料(玻纖、碳纖等)以及傳感技術的蓬勃發展，飛行機器人取得了飛速的發展和應用，伴隨著其負載能力、續航能力的提高，飛行機器人正不斷開拓功能，延伸到各個應用領域。

四旋翼機器人伴隨著其快速的普及，逐步成為飛行機器人領域的標準研究平臺。一般四旋翼飛行器直觀上來看是由4個獨立的螺旋槳通過十字框架結構相連，位於機身前方的螺旋槳為1號螺旋槳，奇數序的螺旋槳按逆時針方向旋轉，偶數序的按順時針方向旋轉，機器人的定位、啟動、升降或是航向均是由這四個螺旋槳協同工作實現。因此，如何結合機器人的當前姿態，對各螺旋槳的轉速進行實時控制，是實現四旋翼飛行器機體穩定可控的關鍵問題。

四旋翼飛行器是典型的欠驅動系統，輸入量只有各螺旋槳的拉力，輸出量則包括了機身的速度、加速度、姿態(傾斜角度)、水平位移以及高度等等。在飛行器的姿態解算中，依靠的最重要部件就是慣性器件——加速度計和陀螺儀。加速度計具有良好的低頻特性，可以測量低速的靜態加速度；陀螺儀具有較好的高頻特性，可以測量高速旋轉運動。所以，要想實現對飛行器的精準控制，就要結合先進的濾波技術，得到精確的姿態解算結果，這是飛行器運行的最關鍵問題。

技術實現要素：

根據以上現有技術的不足，本發明所要解決的技術問題是提出一種應用於四旋翼機器人平臺的非線性濾波算法，得到精確的姿態解算結果。

為了解決上述技術問題，本發明採用的技術方案為：一種應用於四旋翼機器人平臺的非線性濾波算法，其特徵在於,包括如下步驟：步驟一：利用姿態敏感器的多次測量輸出多個測量結果，作為服從初始概率密度p(x0)的n個粒子，記為x0(i)，i＝1,2,…,n；

步驟二：系統狀態轉移概率密度中抽取更新粒子，xk(i)～p(xk(i)|xk-1(i))；

步驟三：更新並歸一化粒子權重ωk(i)，計算狀態估計值xk和方差pk；

步驟四：從步驟三中的ωk(i)和xk(i)中進行重採樣得到

步驟五：由更新狀態估計值和方差pk。

作為一種優選實施方式：所述步驟三中，狀態估計值xk和方差pk的具體求解方式為：

所述步驟四中，具體求解方式為：

其中，kh(·)為核密度函數，h為核帶寬，為正數，n是維數，由epanechnikov核函數抽取樣本ek(i)，計算所述步驟五中，狀態估計值和方差pk為：

本發明有益效果是:在系統幹擾不是很大的情況下，本發明濾波算法在姿態解算中更好的解決了粒子多樣性缺失呈現的貧化問題，且在運算速度上與sir保持相當，從誤差圖上來看，rpf的姿態解算精度比sir有較為顯著的提升，且並沒有犧牲系統的實時性。

附圖說明

下面對本說明書附圖所表達的內容及圖中的標記作簡要說明：

圖1是本發明具體實施方式的rpf和sir濾波實驗100節點下對比及誤差對比圖。

圖2是本發明具體實施方式的rpf和sir濾波實驗5000節點下對比及誤差對比圖。

具體實施方式

下面通過對實施例的描述，本發明的具體實施方式如所涉及的各構件的形狀、構造、各部分之間的相互位置及連接關係、各部分的作用及工作原理、製造工藝及操作使用方法等，作進一步詳細的說明，以幫助本領域技術人員對本發明的發明構思、技術方案有更完整、準確和深入的理解。

常用的姿態描述方法包括了歐拉角法、方向餘弦陣法、羅德裡格參數法、四元數法等。歐拉角法是將機體的運動轉化為三次定軸轉動，分別為繞z軸轉ψ，繞y軸轉θ，繞x軸轉φ，從而建立歐拉角微分方程，結合tait-bryan公式，p、q、r為三個坐標軸方向的角速度，則可以得到如下解：

從解可以看到，由於其自身缺陷，無法覆蓋整個歐式空間，當時，無解，陷入萬向節死鎖。

方向餘弦陣可以做到與姿態變換的一一對應，基於李代數中的反對稱矩陣計算，可建立其微分方程公式：

r＝e[ωs]tr0＝e[ω]θr0(3)

這裡，ω為旋轉軸單位矢量，θ為轉軸方向上轉過的角度。此方法計算量過大，對於有高實時性要求的飛行機器人來說，不適用，並且，在轉向θ比較小的情況下，會使得ω產生較大的誤差。

四元數法相對來說，具有計算量較小且可實現全姿態工作的優勢。四元數是由四個元構成，其基本結構為：

q＝q0+q1i+q2j+q3k(4)

這裡，q0～q3為實數，i、j、k為單位向量，且相互正交，虛單位為它既可以看作是四維空間向量，也可以看作是一個超複數。如果四元數q能夠確定，則可以根據公式(5)中的四元數矩陣與方向餘弦陣中元素的對應關係，再轉換為歐拉角(俯仰角θ、滾動角φ以及偏航角ψ)，從而解得姿態。所以，相比於直接用方向餘弦陣求解，只需求得q0～q3這四個量，計算量顯然小得多，工程上更易於實現。

由於機身是運動的，所以用四元數描述對象姿態時，q本身也是動態的，所以可建立q對時間的微分方程：

這裡，角速度ω＝[ωxωyωz]t，可由陀螺儀測量處理後得到。而後，通過四階-龍格庫塔法求更新解q(t+t)。至此，陀螺儀所得數據成為機身姿態更新的關鍵要素。

陀螺的測量誤差會隨時間累積放大，工作時間越長，會產生漂移越大，使得精度降低，故而需要藉助其他姿態測量傳感器進行修正，所以，姿態確定系統基本都是以陀螺和其他敏感器組合而成。由於姿態敏感器的輸出有測量誤差，且不一定是載體姿態參數，所以需要先進的濾波算法處理測量數據。如何使用先進的濾波算法，克服不確定因素對姿態估計的影響，從而實現對姿態變換的精確描述。

本發明採用正則化粒子濾波用於姿態估計，該算法在系統狀態的連續近似分布下進行採樣，從而有效解決了重採樣導致的粒子貧化問題。

正則化粒子濾波的基本思路是：

1)初始化。產生服從初始概率密度p(x0)的n個粒子，記為x0(i)，i＝1,2,…,n；

2)時間更新。系統狀態轉移概率密度中抽取更新粒子，xk(i)～p(xk(i)|xk-1(i))；

3)量測更新。更新並歸一化粒子權重ωk(i)，計算狀態估計值xk和方差pk；

4)正則化重採樣。從以上兩步中的xk(i)和ωk(i)中進行重採樣，依據式(9)重採樣得到

這裡，kh(·)即為核密度函數，h為核帶寬且為正數，n是維數。由epanechnikov核函數抽取樣本ek(i)，計算

5)狀態更新。

rpf算法在一定程度上較好解決了粒子貧化問題，雖然增加了核密度函數採樣，但計算量並未明顯增大，在系統幹擾不大的情況下，可提高姿態解算精度。

基於matlabr2015b版本仿真平臺，考慮到本發明採用了粒子濾波的重採樣和正則化方法，同時抑制了粒子退化和多樣性貧化問題，所以粒子數目可有效的降低，初始粒子總數n為500。

對陀螺儀以500hz採樣頻率採集10s，假定噪聲分布為非高斯情況，設置系統初始狀態x＝0.1，仿真運行時間為5000個節點，初始過程噪聲方差q＝10，初始測量噪聲方差r＝5，後驗誤差初值p＝5。仿真實驗結果如圖1和圖2所示。為了顯示清楚，特將100個節點下的仿真結果如圖1所示，5000個節點下的仿真結果如圖2所示。

可以看到，在系統幹擾不是很大的情況下，正則化粒子濾波算法在姿態解算中更好的解決了粒子多樣性缺失呈現的貧化問題，且在運算速度上與sir保持相當，從誤差圖上來看，rpf的姿態解算精度比sir有較為顯著的提升，且並沒有犧牲系統的實時性。

上面對本發明進行了示例性描述，顯然本發明具體實現並不受上述方式的限制，只要採用了本發明的方法構思和技術方案進行的各種非實質性的改進，或未經改進將本發明的構思和技術方案直接應用於其它場合的，均在本發明的保護範圍之內。本發明的保護範圍應該以權利要求書所限定的保護範圍為準。