)為本時(shí)間互補(bǔ)濾波器輸出的y方向的水平速度信息，v y〇i υ為上一時(shí)間 互補(bǔ)濾波器輸出的y方向水平速度信息，ayS加速度傳感器獲得的y方向的加速度，Κ 2為 y方向的速度偏差的比例系數(shù)。與機(jī)體坐標(biāo)系的X方向類(lèi)似，通過(guò)改變通過(guò)調(diào)節(jié)1的大小， 可以調(diào)節(jié)濾波器對(duì)于光流傳感器與加速度傳感器的置信系數(shù)。
[0033] 本發(fā)明的技術(shù)特點(diǎn)及效果：
[0034] 本發(fā)明采用互補(bǔ)濾波算法將采用光流法獲取的無(wú)人機(jī)速度信息與加速度傳感器 獲取的加速度信息進(jìn)行融合，實(shí)現(xiàn)在較長(zhǎng)時(shí)間范圍內(nèi)獲取無(wú)人機(jī)的高精度準(zhǔn)確速度信息， 滿(mǎn)足無(wú)人機(jī)室內(nèi)穩(wěn)定自主懸停的需要。
【附圖說(shuō)明】：
[0035] 圖1是本發(fā)明采用的互補(bǔ)濾波器結(jié)構(gòu)框圖；圖中A_el為加速度傳感器的測(cè)量值，VciptlMl為光流傳感器的測(cè)量值，v為互補(bǔ)濾波器輸出的輸?shù)乃俣戎怠?br>[0036] 圖2是采用無(wú)人機(jī)水平速度控制器結(jié)構(gòu)圖；圖中％為遙控器給定的水平加速度， vt為對(duì)a d積分后的給定速度，v為經(jīng)過(guò)互補(bǔ)濾波器融合后的無(wú)人機(jī)實(shí)際速度，a ,為速度控 制器輸出的給定加速度。
[0037] 圖3是手持無(wú)人機(jī)水平X、y方向光流傳感器的裸數(shù)據(jù)和融合后的數(shù)據(jù)，其中圖 3(a)為X方向的融合效果，圖3(b)位y方向的融合效果。
[0038] 圖4是懸停過(guò)程中目標(biāo)姿態(tài)角和實(shí)際姿態(tài)角的數(shù)據(jù)，其中圖4 (a)為給定的俯仰角 及測(cè)量俯仰角，圖4(b)為給定的滾轉(zhuǎn)角及測(cè)量滾轉(zhuǎn)角。
【具體實(shí)施方式】
[0039] 本發(fā)明所要解決的技術(shù)問(wèn)題是，提供一種基于光流傳感器與加速度傳感器數(shù)據(jù)融 合的無(wú)人機(jī)自主定位方法，實(shí)現(xiàn)室內(nèi)環(huán)境下無(wú)人機(jī)的定點(diǎn)懸停。
[0040] 本發(fā)明采用的技術(shù)方案是：采用光流傳感器與加速度傳感器數(shù)據(jù)融合的方法用于 無(wú)人機(jī)的定位系統(tǒng)中，包括如下步驟：
[0041] 利用安裝在四旋翼無(wú)人機(jī)底部的光流傳感器獲取無(wú)人機(jī)的速度信息及速度信息 處理，利用飛行控制器PCB板上的加速度傳感器獲取無(wú)人機(jī)的加速度位置信息，采用互補(bǔ) 濾波器將上述數(shù)據(jù)進(jìn)行融合，獲取較為精準(zhǔn)的無(wú)人機(jī)與地面的相對(duì)速度。
[0042] 所述的利用光流傳感器獲取無(wú)人機(jī)的速度信息及速度信息處理是：
[0043] 光流是外界影像由于相對(duì)運(yùn)動(dòng)在視網(wǎng)膜上形成的視在運(yùn)動(dòng)，一般可以通過(guò)光流算 法對(duì)視頻流進(jìn)行處理得到。當(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)系可以表示為：
[0045] 通過(guò)上述關(guān)系，便可以推算出無(wú)人機(jī)相對(duì)于地面的相對(duì)速度vq。
[0046] 光流法直接獲取的是無(wú)人機(jī)的水平速度信息，然而由于光流法會(huì)受到室內(nèi)光線(xiàn)強(qiáng) 度，飛行器飛行高度等因素影響，光流傳感器的讀數(shù)有較強(qiáng)的噪聲，單獨(dú)使用光流傳感器會(huì) 影響到控制的精度。
[0047] 利用加速度傳感器可以將無(wú)人機(jī)的加速度信息積分后獲取速度信息。但是，對(duì)加 速度信息進(jìn)行積分，會(huì)將加速度傳感器的噪聲也進(jìn)行積分，從而影響所獲取的速度信息的 精度。
[0048] 所述的采用互補(bǔ)濾波器進(jìn)行數(shù)據(jù)融合是：
[0049] 采用互補(bǔ)濾波算法將采用加速度傳感器和光流法獲取的無(wú)人機(jī)位置信息進(jìn)行融 合，可以獲取的無(wú)人機(jī)相對(duì)于地面的速度信息，且較為準(zhǔn)確，通過(guò)構(gòu)造單閉環(huán)比例積分控制 器，實(shí)現(xiàn)了無(wú)人機(jī)在室內(nèi)的定點(diǎn)懸停。以下將主要對(duì)基于互補(bǔ)濾波器的數(shù)據(jù)融合算法進(jìn)行 介紹：
[0050] 由于采用基于加速度傳感器和光流法的定位算法只能直接或間接獲取無(wú)人機(jī)的 水平位置的速度信息，所以只考慮水平方向無(wú)人機(jī)的水平速度信息融合方法。此外，由于水 平方向上X方向和y方向數(shù)據(jù)處理方法類(lèi)似，以下以X方向?yàn)槔M(jìn)行說(shuō)明。
[0051] 在理想狀態(tài)下，無(wú)人機(jī)X方向的水平速度信息vx與其對(duì)應(yīng)速度信息ax具有如下關(guān) 系：
[0053] 在實(shí)際測(cè)量過(guò)程中，由于傳感器自身精度及外界干擾的存在，測(cè)量結(jié)果往往包含 有大量的噪聲信號(hào)和干擾信息，這里將由光流法獲取的速度信息簡(jiǎn)化為如下形式：
[0055] 這里的Vciptlral_x為光流傳感器讀取的速度值，v x為無(wú)人機(jī)傳感器相對(duì)于地面的相 對(duì)速度真值；μ x為測(cè)量噪聲，為恒定值。
[0056] 采用互補(bǔ)濾波算法的結(jié)構(gòu)框圖如圖1所示，根據(jù)系統(tǒng)的結(jié)構(gòu)框圖，可以將系統(tǒng)的 輸出νχ〇?)寫(xiě)成如下形式：
[0058] 其中νχ〇?)為本狀態(tài)互補(bǔ)濾波器輸出的水平速度信息，ν χ〇? υ為上一時(shí)間點(diǎn)互補(bǔ)濾 波器輸出的水平速度信息。
[0059] 在實(shí)際控制系統(tǒng)中，通過(guò)調(diào)節(jié)&的大小，可以改變無(wú)人機(jī)互補(bǔ)濾波器對(duì)加速度傳 感器和光流傳感器的置信系數(shù)。當(dāng)Ki較大時(shí)，對(duì)于光流傳感器的置信系數(shù)較大，反之，對(duì)加 速度傳感器的置信系數(shù)較大。
[0060] 對(duì)于無(wú)人機(jī)集體坐標(biāo)系y方向，與X方的速度融合算法相同，可以表述為：
[0061] 光流法獲得的y方向的速度ig息為：
[0063] 上述Vc]ptw_y為光流傳感器讀取的速度值，v y為無(wú)人機(jī)傳感器相對(duì)于地面的相對(duì) 速度真值；μ y為速度測(cè)量的噪聲值，為恒定值；
[0064] 在融合算法中，令第一次的融合速度值與光流傳感器的裸數(shù)據(jù)值相等，BP :
[0065] vy⑴=v optical-y
[0066] 第一個(gè)時(shí)間點(diǎn)之后，在時(shí)域上的互補(bǔ)濾波器的輸出值vy〇i)，即融合后的水平速度 寫(xiě)成如下形式：
[0068] 其中vy〇i)S本時(shí)間點(diǎn)互補(bǔ)濾波器輸出的y方向的水平速度信息，vy〇i υ為上一時(shí) 間點(diǎn)互補(bǔ)濾波器輸出的y方向水平速度信息，ayS加速度傳感器獲得的y方向的加速度，Κ 2 為機(jī)體坐標(biāo)系y方向的速度偏差的比例系數(shù)，調(diào)整K2可以得到一個(gè)較為準(zhǔn)確的y方向的速 度信息。與機(jī)體坐標(biāo)系的X方向類(lèi)似，調(diào)整1(2也是在調(diào)節(jié)濾波器對(duì)于光流傳感器與加速度 傳感器的置信系數(shù)。
[0069] 下面結(jié)合實(shí)例和附圖對(duì)本發(fā)明基于光流傳感器和加速度傳感器數(shù)據(jù)融合的無(wú)人 機(jī)自主定位方法做出詳細(xì)說(shuō)明。
[0070] 本發(fā)明綜合考慮基于光流法的定位方法的優(yōu)點(diǎn)，充分利用光流法短時(shí)間內(nèi)定位精 度較高能夠在室內(nèi)工作和加速度傳感器頻率較快的優(yōu)勢(shì)，通過(guò)互補(bǔ)濾波方法將兩種定位算 法進(jìn)行融合，并在室內(nèi)環(huán)境中通過(guò)手持四旋翼無(wú)人機(jī)進(jìn)行了相應(yīng)的實(shí)驗(yàn)驗(yàn)證，實(shí)現(xiàn)室內(nèi)基 于多傳感器數(shù)據(jù)融合的四旋翼無(wú)人機(jī)精準(zhǔn)定位功能。
[0071] 本發(fā)明基于光流傳感器和加速度傳感器數(shù)據(jù)融合的無(wú)人機(jī)自主定位方法，包括以 下步驟：
[0072] 1)利用光流傳感器獲取無(wú)人機(jī)的速度信息及速度信息處理：
[0073] 光流法直接獲取的是無(wú)人機(jī)的水平速度信息，當(dāng)攜帶有光流傳感器的四旋翼無(wú)人 機(jī)飛行時(shí)，由于外界影像相對(duì)于光流傳感器運(yùn)動(dòng)，傳感器的感光平面上便形成了像素運(yùn)動(dòng)， 其速度表示為vflm?，它的值正比于無(wú)人機(jī)的相對(duì)速度vq，反比于無(wú)人機(jī)與地面的相對(duì)距離 h，可以表示為：
[0075] 由此便可以得到四旋翼無(wú)人機(jī)的水平速度信息。然而，由于受到光照條件，地面紋 理等因素的影響，通常由光流傳感器直接獲取的無(wú)人機(jī)的相對(duì)速度存在高頻率的高斯白噪 聲，若不進(jìn)行融合，會(huì)影響控制效果。
[0076] 2)利用加速度傳感器獲取無(wú)人機(jī)的加速度信息：
[0077] 加速度儀傳感器對(duì)光流傳感器的噪聲進(jìn)行補(bǔ)償，通過(guò)SPI總線(xiàn)，實(shí)現(xiàn)了微控制器 與加速度傳感器之間的通信，在1kHz頻率下利用可編程中斷讀取加速度傳感器的讀數(shù)，并 對(duì)其進(jìn)行積分得到無(wú)人機(jī)的速度信息。
[0078] 3)采用互補(bǔ)濾波器進(jìn)行數(shù)據(jù)融合：
[0079] 采用互補(bǔ)濾波算法將光流傳感器和加速度傳感器獲取的無(wú)人機(jī)水平速度信息進(jìn) 行融合，實(shí)現(xiàn)在室內(nèi)環(huán)境下獲取無(wú)人機(jī)的準(zhǔn)確速度信息的目的，滿(mǎn)足無(wú)人機(jī)室內(nèi)自主懸停 飛行控制的需要。以下將主要對(duì)基于互補(bǔ)濾波器的數(shù)據(jù)融合算法進(jìn)行介紹