加速度與光流傳感器數(shù)據(jù)融合無(wú)人機(jī)水平速度控制方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及無(wú)人機(jī)定位方法，特別是涉及一種基于加速度傳感器與光流傳感器數(shù) 據(jù)融合的無(wú)人機(jī)自主定位方法。 技術(shù)背景
[0002] 無(wú)人機(jī)的定位問(wèn)題主要是指利用自身傳感器確定無(wú)人機(jī)在飛行環(huán)境中相對(duì)于慣 性坐標(biāo)系的位置和姿態(tài)信息。準(zhǔn)確的位姿估計(jì)是實(shí)現(xiàn)四旋翼無(wú)人機(jī)安全飛行、軌跡規(guī)劃以 及目標(biāo)跟蹤等復(fù)雜飛行任務(wù)的前提和基礎(chǔ)。
[0003]目前廣泛使用的無(wú)人機(jī)導(dǎo)航系統(tǒng)主要是基于GPS定位的方法，但是其定位精度較 低，且在室內(nèi)幾乎沒(méi)有信號(hào)，故而在室內(nèi)實(shí)現(xiàn)無(wú)人機(jī)的定位飛行并不能利用GPS傳感器。
[0004] 日本千葉大學(xué)將實(shí)時(shí)光流視覺(jué)系統(tǒng)用于四旋翼無(wú)人機(jī)的定位與控制。它采用了一 個(gè)垂直向下的攝像頭完成光流計(jì)算，并將其與慣導(dǎo)數(shù)據(jù)融合，采用了三層嵌套的卡爾曼濾 波技術(shù)，通過(guò)非線性控制器，實(shí)現(xiàn)了四旋翼無(wú)人機(jī)的室內(nèi)外自主起飛、定點(diǎn)懸停、軌跡跟蹤、 自主降落等復(fù)雜任務(wù)，且控制效果較為良好。（會(huì)議：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 ;頁(yè)碼：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，頁(yè)碼：311 - 334) 〇
[0005] 瑞士蘇黎世聯(lián)邦理工學(xué)院的研究人員采用PX4FL0W光流傳感器作為位置測(cè)量單 元并用于室內(nèi)外無(wú)人機(jī)的定位控制系統(tǒng)中，該研究機(jī)構(gòu)雖實(shí)現(xiàn)了基于光流法的室內(nèi)軌跡跟 蹤實(shí)驗(yàn)，但從圖中可以看出，光流傳感器獲取的無(wú)人機(jī)速度信息經(jīng)長(zhǎng)距離積分作用后，在邊 長(zhǎng)約為3米的連續(xù)兩圈矩形軌跡跟蹤過(guò)程中，其最大定位誤差大于0. 5米，在隨時(shí)間累加過(guò) 程中其位置信息存在較大的漂移（會(huì)議：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 ;頁(yè)碼：1736-1741) 〇
[0006] 此外，瑞士蘇黎世聯(lián)邦工學(xué)院通過(guò)光流法實(shí)現(xiàn)了四旋翼無(wú)人機(jī)在走廊中的避 障飛行的高難度任務(wù)，他們利用超前的190°魚眼攝像頭獲取光流信息，采用金字塔 Lucas-Kanada法完成光流計(jì)算，采用Shi-Tomasi角點(diǎn)檢測(cè)方法進(jìn)行特征點(diǎn)提取，并將其與 慣導(dǎo)數(shù)據(jù)融合，消除了光流旋轉(zhuǎn)分量的影響。（會(huì)議：IEEE International Conference on Robotics and Automation ;著者：S. Zingg, D. Scaramuzza, S. Weiss, et al ;出版年月：2010 年；文章題目：MAV navigation through indoor corridors using optical flow ;頁(yè)碼： 3361-3368)〇

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

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