|
四軸飛行器是一種特殊結構的無人機（Unmanned AerialVehicle,UAV),可以只通過調節四個旋翼的轉速,實現對飛行姿態的控制,它可為海上、廢墟等不適合人員進入的環境,提供偵察、救援、繪圖等服務。UAV 多數使用GPS 或慣性導航系統,然而,在GPS 信號不足或室內環境下,慣性導航系統誤差又隨時間積累,所以本身不能長時間地提供高精度的導航,需要輔助導航 。視覺導航方法,即由機載的視覺傳感器獲得周圍環境的圖像信息,然后通過圖像算法及相機的位置標定解算出載體的位置,有更大的發展空間,它成本低、質量輕,且易于實現。 視覺導航方法主要集中分為兩類：一類是以視覺伺服為主的導航方法,它基于視覺傳感器,直接利用圖像特征控制飛行器到指定位置,通常有兩個控制環,外環控制實時特征與目標特征間的誤差,估計飛行器的理想速度,然后,通過內環控制電機轉速到理想速度,最后,使特征的圖像匹配到目標位置。另一類是視覺輔助導航方法,即融合視覺傳感器和慣性傳感器給出載體位置,融合的方法多見Kalman 濾波方法。 本文基于四軸飛行器的數學模型,研究擴展卡爾曼濾波（ Extended Kalman Filter, EKF) 和不敏卡爾曼濾波（Unscented Kalman Filter,UKF)兩種非線性濾波算法在四軸飛行器視覺輔助導航方法中的應用情況,并對比Kalman 方法,UKF 和EKF 算法融合四軸飛行器多傳感器信息的效果。 1 四軸飛行器數學模型 四軸飛行器的動力學模型滿足如下假設：四軸飛行器是均勻對稱的剛體結構;慣性系的原點與飛行器的幾何中心一致;飛行器所受阻力及重力與飛行高度無關,保持不變;四軸各向拉力與電機的轉速成正比。四軸飛行器的結構模型如圖1 所示。 圖1 四軸飛行器的數學模型示意圖 圖1 中涉及到慣性坐標系－ E 系和載體坐標系－ B 系,φ,θ,ψ 分別表示慣性坐標系下載體的歐拉角,定義Θ =[φ θ ψ]T 。則根據坐標系變換原則,載體系到慣性系的變換矩陣R 如式1 所示,其中c 表示余弦函數, s 表示正弦函數。 四軸飛行器電機的推力Ｆ 與它的轉速ω 的平方成正比： 其中： i = 1,2,3,4 分別代表4 個電機, k 為推力系數。則四軸飛行器的總推力為： 定義P = [X Y Z] T表示載體在慣性系下的位置矢量,根據牛頓第二定律知,載體的運動加速度與所受力成正比： 則載體在慣性系下的線運動方程為： 定義p,q,r 表示四軸飛行器在載體系中的運動角速度,則歐拉角與運動角速度的關系為： 角運動方程為： 其中： I 為慣性矩陣, MR為旋轉力矩, M 為飛行器所受力矩,由于四軸飛行器是對稱的剛體,且其重心與幾何中心重合,所以慣性矩陣為對角陣,對角線元素為Ix、Iy、Iz , 考慮沿水平方向的旋轉效應,旋轉力矩為： 飛行器所受力矩為： 則將式（6)展開為： 定義控制量如下： 2 非線性濾波算法 2.1 擴展卡爾曼濾波 EKF 算法利用泰勒展開的一次項來對非線性方程作線性化處理, 再結合經典的卡爾曼濾波進行濾波估計。EKF算法簡單,易于實現。 設系統的狀態方程為： 其中： x 為系統狀態矢量, z 為系統觀測量; w（k),v（k) 為不相關零均值的高斯噪聲; Q（k),R（k) 分別表示其方差矩陣。φ（x),r（x) 表示泰勒展開算子,即線性化過程。 測量更新： 其中J 為雅可比算子。 2.2 不敏卡爾曼濾波(UKF) 不敏卡爾曼濾波是基于采樣方法的近似法,用帶有權值的樣本集來近似目標的狀態后驗概率密度,采用的是確定的樣本點,因而避免由線性化而導致的誤差。設系統的狀態方程為： 其中： xk ∈Ｒn 為系統狀態矢量, zk ∈Ｒm 為測量量, qk－1 ∈Ｒn和rk－1 ∈ Ｒm是高斯噪聲,且分別qk－1 ~ N（0,Qk－1 ),rk－1 ~N（0,Rk－1 ) 分布。初始狀態x0 的均值和協方差分別記為m0和P0 。UKF 算法的根本是無跡變換（Unscented Transform,UT),其過程如下： 先根據系統狀態的均值x 和方差矩陣Pxx 選擇一組樣本點x0 ,…,xi 和相應權值w0 ,…,wi ： 其中參數k 是標量的常數。 由樣本點x0 ,…,xi 經過非線性函數得到一組新的樣本點z0 ,…,zi ,zi = h（xi )（i = 0,…,2n)。 用這些帶有權值的新樣本點計算觀測狀態的均值和方差 UKF 濾波算法同樣包含時間更新與測量更新兩個過程： 時間更新：由k － 1 預測值經過上述的無跡變換過程計算k 時刻預測狀態 的均值 和方差矩陣 測量更新：計算k 時刻測量量zk 及其均值zk 、方差矩陣Pzz,k 及協方差矩陣Pxz,k 。 3 仿真模型建立 四軸飛行器狀態矢量 , 四軸飛行器的速度和歐拉角由慣性導航單元（ＩnertialＭeasurement Unit, ＩＭU)測量及解算。根據四軸飛行器的運動學方程,建立如式（25)所示的狀態方程： 4 仿真結果 根據四軸飛行器的數學模型及各濾波算法的遞推公式,使用Ｍatlab 對系統進行動態仿真。四軸飛行器的空間狀態初始值設為：三個方向的位置初值x（0) = 20 m, y（0) =30 m, z（0) =2 m, 三個方向線速度、角速度和角加速初值為零。圖2（a) ~ （c)分別為X,Y,Z 三個方向的載體位置曲線。圖中的真實值為視覺導航系統給出的位置估計。 圖2 X、Y、Z 方向位置曲線 Kalman 濾波在對非線性特征的四軸飛行器的位置濾波上存在明顯的濾波偏差,相比之下,EKF 和UKF 有較好的濾波效果,三個方向的估計值誤差曲線如圖５、圖6、圖7 所示。從X,Y,Z 三個方向的估計誤差曲線圖上,更能直觀地看出三種濾波算法的濾波結果,濾波估計誤差的均方差如表1 所示。 圖3 X、Y、Z 方向估計誤差曲線 Kalman 濾波非線性環境下濾波性能較差, 甚至會出現濾波發散點,而EKF 和UKF 能很好地處理四軸飛行器的非線性特性,從誤差均方差上看,UKF 優于EKF,這也是由于EKF是將非線性化的系統線性化造成的偏差,而UKF 是基于樣本點的濾波方式,對噪聲的適應性更強。 5 結語 本文研究EKF 和UKF 算法在四軸飛行器視覺組合導航中的應用,針對四軸飛行器的非線性特點,傳統的Kalman 濾波方法存在一定的的誤差,而EKF 和UKF 在融合視覺位置信息時具有明顯的優勢,且UKF 比EKF 更適應于四軸飛行器的組合導航系統。 |
