加速度與光流傳感器數據融合無人機水平速度控制方法
【技術領域】
[0001] 本發(fā)明涉及無人機定位方法，特別是涉及一種基于加速度傳感器與光流傳感器數 據融合的無人機自主定位方法。 技術背景
[0002] 無人機的定位問題主要是指利用自身傳感器確定無人機在飛行環(huán)境中相對于慣 性坐標系的位置和姿態(tài)信息。準確的位姿估計是實現四旋翼無人機安全飛行、軌跡規(guī)劃以 及目標跟蹤等復雜飛行任務的前提和基礎。
[0003]目前廣泛使用的無人機導航系統(tǒng)主要是基于GPS定位的方法，但是其定位精度較 低，且在室內幾乎沒有信號，故而在室內實現無人機的定位飛行并不能利用GPS傳感器。
[0004] 日本千葉大學將實時光流視覺系統(tǒng)用于四旋翼無人機的定位與控制。它采用了一 個垂直向下的攝像頭完成光流計算，并將其與慣導數據融合，采用了三層嵌套的卡爾曼濾 波技術，通過非線性控制器，實現了四旋翼無人機的室內外自主起飛、定點懸停、軌跡跟蹤、 自主降落等復雜任務，且控制效果較為良好。（會議：IEEE International Conference on Robotics and Automation ;著者：F. Kendoula，I. Fantoni，K. Nonami ;出版年月：2007 年； 文章題目：Three Nested Kalman Filters-based Algorithm for Real-time Estimation of Optical Flow, UAV Motion and Obstacles Detection ;頁碼：4746-4751)(期刊： Journal of Field Robotics ;著者：F. Kendoula, I. Fantoni, K. Nonami，出版年月：2010 年；文章題目：Guidance and Nonlinear Control System for Autonomous Flight of Minirotorcraft Unmanned Aerial Vehicles，頁碼：311 - 334) 〇
[0005] 瑞士蘇黎世聯邦理工學院的研究人員采用PX4FL0W光流傳感器作為位置測量單 元并用于室內外無人機的定位控制系統(tǒng)中，該研究機構雖實現了基于光流法的室內軌跡跟 蹤實驗，但從圖中可以看出，光流傳感器獲取的無人機速度信息經長距離積分作用后，在邊 長約為3米的連續(xù)兩圈矩形軌跡跟蹤過程中，其最大定位誤差大于0. 5米，在隨時間累加過 程中其位置信息存在較大的漂移（會議：IEEE International Conference on Robotics and Automation ;著者：Honegger D, Meier L，Tanskanen P ;出版年月：2013年；文章題目： An Open Source and Open Hardware Embedded Metric Optical Flow CMOS Camera for Indoor and Outdoor Applications ;頁碼：1736-1741) 〇
[0006] 此外，瑞士蘇黎世聯邦工學院通過光流法實現了四旋翼無人機在走廊中的避 障飛行的高難度任務，他們利用超前的190°魚眼攝像頭獲取光流信息，采用金字塔 Lucas-Kanada法完成光流計算，采用Shi-Tomasi角點檢測方法進行特征點提取，并將其與 慣導數據融合，消除了光流旋轉分量的影響。（會議：IEEE International Conference on Robotics and Automation ;著者：S. Zingg, D. Scaramuzza, S. Weiss, et al ;出版年月：2010 年；文章題目：MAV navigation through indoor corridors using optical flow ;頁碼： 3361-3368)〇

【發(fā)明內容】

[0007] 為克服現有技術的不足，提供一種新的基于方向信息的自適應二次噪聲點檢測方 法，該方法可以有效地降低了非噪聲點的誤判概率，并且能夠更有效地去除圖像中的椒鹽 噪聲，對不同強度噪聲去噪的魯棒性較強。為此，本發(fā)明采取的技術方案是，加速度與光流 傳感器數據融合無人機水平速度控制方法，利用安裝在四旋翼無人機底部的光流傳感器獲 取無人機的水平速度信息，并利用飛行控制器PCB板上的加速度傳感器獲取無人機的加速 度信息，采用互補濾波器將上述數據進行融合，獲取較為精準的無人機與地面的相對速度。
[0008] 當攜帶有光流傳感器的四旋翼無人機飛行時，外界影像相對于光流傳感器運動， 光流傳感器的感光平面上便形成了像素運動，其速度表示為VciptlMl，它的值正比于無人機的 相對速度vq，反比于無人機與地面的相對距離h，其關系可以表示為：
[0010] 通過上述關系，便可以推算出無人機相對于地面的相對速度vq。
[0011] 由于采用基于加速度傳感器和光流法的算法只能直接或間接獲取無人機的水平 的速度信息，所以只考慮水平方向無人機的水平速度信息融合方法，無人機的機體坐標系 的X方向上：
[0012] 在理想狀態(tài)下，無人機X方向的水平速度信息vx與其對應加速度信息a具有如下 關系：
[0014] ax為加速度信息a的X方向的加速度，上式符號上方的點表示一階導數，在實際測 量過程中，由于傳感器自身精度及外界干擾的存在，測量結果往往包含有大量的噪聲信號 和干擾信息，這里將由光流法獲取的速度信息簡化為如下形式：
[0016] 這里的Vciptlral_x為光流傳感器讀取的速度值，v x為無人機傳感器相對于地面的相 對速度真值；μ x為測量噪聲，為恒定值；
[0017] 飛行控制器獲取的無人機的飛行狀態(tài)信息是離散的信息，每一時間點的融合后的 水平位置速度信息可以表示為vxW，其中η為無人機信息的采樣次數。
[0018] 在融合算法中，令第一次的融合速度值與光流傳感器的裸數據值相等，即：
[0020] 在第一個時間點后，在時域上的互補濾波器的輸出值νχ〇?)，即融合后的水平速度 寫成如下形式：
[0022] 其中νχ〇?)為本時間互補濾波器輸出的X方向的水平速度信息，νχ〇?1)為上一時間互 補濾波器輸出的X方向的水平速度信息，ax為加速度信息a的X方向的加速度，K X方 向的速度偏差的比例系數；該濾波器利用光流傳感器的裸數據Vciptlral_x與上一時間的輸出 νχ〇? 1}做差作為并與比例系數1^構造比例反饋，以加速度傳感器的數據&：!的積分作為前向 通道，利用反饋通道補償積分得到的速度信息，通過測量值與上一時間點做差，消去光流傳 感器測量的速度值的測量噪聲μχ，從而得到一個較為準確的X方向的速度信息。
[0023] 在實際控制系統(tǒng)中，通過調節(jié)&的大小，改變無人機互補濾波器對加速度傳感器 和光流傳感器的置信系數；當Ki較大時，對于光流傳感器的置信系數較大，反之，對加速度 傳感器的置信系數較大。
[0024] 同理對于無人機的機體坐標系y方向，采用與X方向相同的速度融合算法，表述如 下：
[0025] 光流法獲得的y方向的速度信息可以表示為：
[0026] voptlcalj= νγ+μγ
[0027] 這里的VciptlraLy為光流傳感器讀取的速度值，v y為無人機傳感器相對于地面的相 對速度真值；μ y為測量噪聲，為恒定值；
[0028] 同樣，在融合算法中，令第一次的融合速度值與光流傳感器的裸數據值相等，BP :
[0029] vyW = v optlcal-y
[0030] 在第一個時間點后，在時域上的互補濾波器的輸出值vy〇i)，即融合后的水平速度 寫成如下形式：
[0031] vy(k)= / ay-K2(v optical-y Vy(k 1))
[0032] 其中vy〇i