專利名稱：一種微型共軸雙旋翼飛行器控制裝置及方法
技術(shù)領(lǐng)域：
本發(fā)明屬于飛行器控制技術(shù)領(lǐng)域，特別適用于一種微型共軸雙旋翼飛行器控制裝 置及方法。
背景技術(shù)：
目前，因自主導(dǎo)航系統(tǒng)一般體型較大，要實(shí)現(xiàn)具有自主飛行能力的飛行器的微型 化難度很大，而且系統(tǒng)隨著自主化程度的提高以及功能的增多體積也隨之增大，研究集尺 寸微型、功能強(qiáng)大于一體的飛行器還是一片空白。

發(fā)明內(nèi)容
為解決已有技術(shù)存在的不足，本發(fā)明提供一種微型共軸雙旋翼飛行器控制裝置及 方法，通過(guò)控制微型共軸雙旋翼飛行器的舵機(jī)和電機(jī)，以達(dá)到精確控制的目的。本發(fā)明的技術(shù)方案是這樣實(shí)現(xiàn)的該裝置包括地面裝置和空中裝置兩部分；其中 地面裝置包括視頻無(wú)線接收機(jī)、地面無(wú)線傳輸模塊、地面處理器、顯示器和上位機(jī)；空中裝 置包括有三軸磁航向傳感器、三軸陀螺儀、三軸加速度傳感器、GPS、氣壓計(jì)、超聲波傳感器、 ARM處理器、微型無(wú)線攝像頭、視頻解碼模塊、視頻無(wú)線傳輸模塊和空中無(wú)線傳輸模塊；所述的地面裝置的連接是視頻無(wú)線接收機(jī)輸出端連接顯示器的輸入端，地面無(wú) 線傳輸模塊與地面處理器通過(guò)同步串口端連接；地面處理器與上位機(jī)通過(guò)異步串口端連 接；所述的空中裝置安裝在飛行器上，空中裝置的連接是三軸磁航向傳感器、三軸陀 螺儀、三軸加速度傳感器、GPS、氣壓計(jì)和超聲波傳感器的輸出端均連接ARM處理器相應(yīng)的 輸入端，微型無(wú)線攝像頭的一個(gè)輸出端通過(guò)視頻解碼模塊連接ARM處理器的輸入端，微型 無(wú)線攝像頭的另一個(gè)輸出端連接視頻無(wú)線傳輸模塊的輸入端，ARM處理器與空中無(wú)線傳輸 模塊通過(guò)同步串口端連接；其中地面裝置和空中裝置通過(guò)其中地面無(wú)線傳輸模塊和空中無(wú)線傳輸模塊實(shí)現(xiàn) 無(wú)線通訊，視頻無(wú)線接收機(jī)與視頻無(wú)線傳輸模塊實(shí)現(xiàn)無(wú)線通訊。所述的微型共軸雙旋翼飛行器控制裝置的控制方法，包括如下步驟步驟一、裝置初始化；步驟二、采集各路傳感器數(shù)據(jù)依次采集三軸加速度傳感器，三軸磁航向傳感器、三軸角速度陀螺儀、超聲波傳感 器、氣壓計(jì)和GPS的記錄數(shù)據(jù)(1)將三軸加速度傳感器三個(gè)軸向輸出的模擬電壓信號(hào)經(jīng)AD轉(zhuǎn)換成數(shù)字信號(hào)，對(duì) 信號(hào)進(jìn)行濾波，并儲(chǔ)存飛行器X、Y、ζ軸上的加速度ax、ay、az ；(2)讀取三軸磁航向傳感器上地磁場(chǎng)在三個(gè)軸上的數(shù)字磁場(chǎng)分量，并確定飛行器 與地磁線所成夾角δ ；具體如下
6
如果Hy >0，則d = 90-[arctan(/ic///;0]*180/;z·如果Hy <0，則5 = 270-[arctan(/ic//^)]*180/;r如果Hy = 0，Hx < 0 則a = 180如果Hy = 0，Hx > 0 則a = 0(3)將俯仰、翻滾陀螺儀，偏航陀螺儀輸出的模擬電壓信號(hào)經(jīng)AD轉(zhuǎn)換成數(shù)字信號(hào)， 對(duì)信號(hào)進(jìn)行濾波，并儲(chǔ)存飛行器俯仰角速度Ωχ、翻滾角速度Qy、偏航角速度Ωζ;(4)讀取數(shù)字氣壓計(jì)的氣壓值P，并確定絕對(duì)高度，方法如下式 H=44330*(1-(p/P0)1/5.255)式中ρ0為海平面氣壓，其值為1013. 25hPa ；H'為絕對(duì)高度，單位為米；如果是第一次測(cè)量氣壓值，則將H'賦值給外部變量Hs，計(jì)算飛行器飛行的相對(duì)高 度 H=44330*(1-(P/P0)1/5.255)-Hs
在低空環(huán)境下采用超聲測(cè)距發(fā)射方波，打開(kāi)接收中斷，記錄發(fā)射與接收的時(shí)間間 隔T，如果固定時(shí)間內(nèi)接收到返回接收信號(hào)，則計(jì)算相對(duì)高度H = 344*T/2，并代替公式(6) 計(jì)算得到的相對(duì)高度，否則說(shuō)明測(cè)量超過(guò)超聲波量程，關(guān)閉接收中斷，儲(chǔ)存高度值H ；(5)確定飛行器的經(jīng)度和緯度；讀取GPS信號(hào)，得到飛行器的經(jīng)度E和緯度N，并儲(chǔ)存，如果是第一次得到飛行器的 經(jīng)度E和緯度N，則分別賦值給外部變量Ε。N0,作為起飛點(diǎn)坐標(biāo)；步驟三、確定飛行模式、開(kāi)始起飛、飛行高度、飛行速度和飛行目標(biāo)點(diǎn)命令；將步驟二測(cè)到的各路傳感器數(shù)據(jù)組合成數(shù)據(jù)包后通過(guò)機(jī)載Zigbee無(wú)線發(fā)送給地 面Zigbee部分，并且接收來(lái)自地面控制界面通過(guò)地面Zigbee發(fā)送過(guò)來(lái)的控制命令數(shù)據(jù)包， 解析數(shù)據(jù)包后得到飛行模式、開(kāi)始起飛、飛行高度、飛行速度和飛行目標(biāo)點(diǎn)命令；步驟四、通過(guò)相對(duì)高度判斷飛行器是否起飛，當(dāng)相對(duì)高度為零，飛行器沒(méi)有起飛； 當(dāng)相對(duì)高度為正值，飛行器已經(jīng)起飛，如果沒(méi)有起飛，轉(zhuǎn)入步驟四；如果已經(jīng)起飛，轉(zhuǎn)入步驟 五；步驟五、空中裝置ARM處理器通過(guò)地面發(fā)過(guò)來(lái)的控制命令數(shù)據(jù)包選擇飛行器的飛 行模式；把遙控模式、航線設(shè)定模式、自主循跡模式三種飛行模式賦予不同的數(shù)據(jù)，通過(guò)與 地面發(fā)過(guò)來(lái)的控制命令數(shù)據(jù)包中相應(yīng)的數(shù)據(jù)進(jìn)行比較，選擇相應(yīng)的飛行模式；步驟六、確定飛行器控制增量；
飛行器有四個(gè)控制通道，分別為上升下降，前進(jìn)后退，左右平移，左右擺動(dòng)，飛行器 的姿態(tài)的控制可以分解成對(duì)這四個(gè)通道的控制，各個(gè)通道采用下例式計(jì)算出控制增量Δ u (k) = kp X Δ E (k) +Iii X E (k) +kd [Ε (k) -2E (k-1) +E (k_2)]+kn X Δ a (k) +k12 X a (k) +a13 [a (k) -2a (k_l) +a (k_2)]
+k21 X Δ Ω (k) +k22 X Ω (k) +k23 [ Ω (k) -2 Ω (k_l) + Ω (k_2)]式中kp、kn、k21分別為被控量、加速度、角速度的比例系數(shù)；‘ k12、k22分別 為被控量、加速度、角速度的積分系數(shù)；kd、k13、k23分別為被控量、加速度、角速度的微分 系數(shù)；E(k)、a(k)、Ω (k)分別為被控量、加速度、角速度的系統(tǒng)偏差；ΔΕ(10、Aa(k)、 Δ Ω (k)分別為被控量、加速度、角速度的偏差變化率；AE(k) =E(k)-E(k_l)，Aa(k)= a(k)-a(k-l), Δ Ω (k) = Ω (k)-Q (k-1) ；k, k_l，k_2 分別表示第 k 次，第 k_l 次，第 k_2 次；步驟七、控制飛行器的舵機(jī)和電機(jī)；通過(guò)飛行器上的ARM處理器控制PWM寄存器控制飛行器的舵機(jī)和電機(jī)；其中因舵 機(jī)的轉(zhuǎn)角與控制舵機(jī)的脈沖寬度成線性關(guān)系，脈沖寬度又是由ARM處理器內(nèi)部PWM寄存器 值來(lái)設(shè)置的，通過(guò)ARM處理器計(jì)算改變PWM寄存器值來(lái)控制飛行器左右平移和前進(jìn)后退；飛 行器上兩個(gè)電機(jī)形成轉(zhuǎn)速差時(shí)會(huì)產(chǎn)生旋轉(zhuǎn)力矩通過(guò)控制，該力矩可以控制飛行器左右方向 擺動(dòng)，當(dāng)兩電機(jī)同比增速或減速時(shí)則能控制飛行器的上升和下降；(1)舵機(jī)控制左右平移、前進(jìn)后退運(yùn)動(dòng)相對(duì)懸停狀態(tài)下兩舵機(jī)角度變化相互關(guān)系為左平移左舵機(jī)角度變化量為+ θ左移，右舵機(jī)角度變化量為_(kāi) θ左移右平移左舵機(jī)角度變化量為_(kāi) θ右移，右舵機(jī)角度變化量為+ θ拂前進(jìn)左舵機(jī)角度變化量為_(kāi) θ纖，右舵機(jī)角度變化量為_(kāi) θ纖后退左舵機(jī)角度變化量為+ θ后退，右舵機(jī)角度變化量為+ θ后退；(2)電機(jī)控制兩個(gè)電機(jī)轉(zhuǎn)動(dòng)方向相反，當(dāng)兩個(gè)電機(jī)形成轉(zhuǎn)速差時(shí)會(huì)產(chǎn)生旋轉(zhuǎn)力矩，該力矩可以 控制飛行器左右方向擺動(dòng)，當(dāng)向右方向轉(zhuǎn)動(dòng)的電機(jī)轉(zhuǎn)速大于向左方向轉(zhuǎn)動(dòng)的電機(jī)轉(zhuǎn)速時(shí)， 飛機(jī)向右方向擺動(dòng)；當(dāng)向左方向轉(zhuǎn)動(dòng)的電機(jī)轉(zhuǎn)速大于向右方向轉(zhuǎn)動(dòng)的電機(jī)轉(zhuǎn)速時(shí)，飛機(jī)向 左方向擺動(dòng)，兩個(gè)電機(jī)同比增速，可以控制飛行器上升；兩個(gè)電機(jī)同比減速減時(shí)，可以控制 飛行器的下降；電機(jī)采用增量式PID控制Δ U1 (k) = kp X Δ E (k) +Iii X E (k) +kd [Ε (k) -2E (k-1) +E (k_2)]式中，Kp, Ki, Kd分別是PID控制中比例，積分，微分控制參數(shù)； E(k)-2E(k-l)+E(k-2)為系統(tǒng)量變化率，在采集數(shù)據(jù)的時(shí)間點(diǎn)上，將得到的值作為PWM脈寬 的變化量來(lái)控制電機(jī)；步驟八、通過(guò)GPS信號(hào)獲取飛機(jī)所在位置，與目的地的經(jīng)緯度進(jìn)行比較，如果兩者 不等，飛機(jī)未到達(dá)目的地，轉(zhuǎn)入步驟二 ；如果兩者相等，飛機(jī)到達(dá)目的地，給定高度逐漸減 小，飛行器降落。所述的步驟五，空中裝置ARM處理器通過(guò)地面發(fā)過(guò)來(lái)的控制命令數(shù)據(jù)包選擇飛行 器的飛行模式，按如下步驟進(jìn)行
飛行模式分為三種遙控模式、航線設(shè)定模式、自主循跡模式；(1)遙控模式計(jì)算出遙控信號(hào)左右舵機(jī)和前后電機(jī)對(duì)應(yīng)的通道的遙控脈沖寬度，進(jìn)而處理得到 飛行器左右舵機(jī)控制脈沖寬度和前后電機(jī)PWM控制信號(hào)的周期和占空比；(2)航線設(shè)定模式飛行器飛行的方向即與地磁線的夾角為
β — β沒(méi)=arctan“ N-Nn飛行器偏移設(shè)定航線的距離為L(zhǎng) 機(jī)V +hV+iX)-^+^ +V)
其中Ε、N分別為本次測(cè)得的經(jīng)度和緯度，En、Nn分別為飛行器由步驟三得到的 下一個(gè)目標(biāo)點(diǎn)的經(jīng)度和緯度，E0, N0分別作為起飛點(diǎn)坐標(biāo)彳=概-E0)2+ (Nn - N0)2 ,
I2 = yl(E-E0)2+(N-N0)2 J3 = yl(En -Ef + (Nn 一N)2 ,其中 1” I2 和 k3 分別為飛行器偏移
設(shè)定航線在X，Y和Z軸的距離；(3)自主循跡模式將微型無(wú)線攝像頭采集的視頻信號(hào)提取白布上黑線，在圖像上形成的路徑的左右 上下邊界，計(jì)算出飛行器偏移路徑的角度和距離，具體步驟如下①采集圖像數(shù)據(jù)采取了降低圖像分辨率的方法每行采集至少48個(gè)點(diǎn)，每幅圖像采集至少40行，圖 像分辨率至少為48X40 ；②計(jì)算閾值并二值化圖像圖像采第1場(chǎng)的第1行數(shù)據(jù)用雙峰法計(jì)算出該行的閾值，將該圖像中大于等于閾 值的點(diǎn)賦值為1，小于閾值的點(diǎn)賦值為0，實(shí)現(xiàn)二值化處理，第2行閾值引用第1行的閾值， 并實(shí)現(xiàn)二值化處理，將所有值為1的點(diǎn)所對(duì)應(yīng)的數(shù)據(jù)相加取平均值，將所有值為0的點(diǎn)所對(duì) 應(yīng)的數(shù)據(jù)相加取平均值，兩個(gè)平均值再取平均值便得到第2行的閾值，并與上一行的閾值 進(jìn)行比較，在允許的范圍內(nèi)更新，如此迭代至最后一行，最后一行的閾值則作為下一場(chǎng)第一 行的閾值，如此反復(fù)迭代；③判斷每行圖像二值化情況每行圖像從起點(diǎn)到終點(diǎn)進(jìn)行判斷，如果該行僅一次出現(xiàn)值為“0-1-0”變化，則認(rèn)為 該行圖像二值化情況良好；④搜索路徑的左右邊界、上下邊界從第1行起依次搜索該場(chǎng)圖像的其他行的圖像二值化情況，如果連續(xù)3行是二值 化情況良好，則停止搜索，并把該行作為路徑搜索起始行ist，以ist作為起始行向上即行標(biāo) 號(hào)增加的方向和向下即行標(biāo)號(hào)減小的方向?qū)Ω餍羞M(jìn)行路徑邊界的搜索；路徑左邊界向上搜索將路徑左邊界向上一行延伸，如果上一行該處值為0的點(diǎn)，則向左搜索路徑左邊 界，如果搜索超出邊界還沒(méi)搜索到則用邊界值代替；如果該處值為1的點(diǎn)，則向右搜索路徑左邊界路徑右邊界向上搜索將路徑右邊界向上一行延伸，如果上一行該處值為0的點(diǎn)，則向右搜索路徑右邊 界；如果該處值為1的點(diǎn)，則向左搜索路徑右邊界； 如果搜索到的路徑邊界出現(xiàn)左路徑邊界值大于右路徑邊界值，左右邊界太寬即為 0點(diǎn)個(gè)數(shù)大于10個(gè)或左右邊界值之間有值為0的點(diǎn)，則嘗試跳過(guò)行，該行的路徑邊界值用前 兩行邊界的平均值進(jìn)行補(bǔ)充，如果向上路徑搜索過(guò)程中總跳躍次數(shù)達(dá)到3次或搜索達(dá)到頂 端則結(jié)束搜索，同時(shí)可以得到路徑上邊界值b，并計(jì)算出上邊界左右兩邊界的中心值B ；向下搜索同理可得到路徑下邊界值a，計(jì)算下邊界左右兩邊界的中心值A(chǔ)，如果搜 索到的有效路徑長(zhǎng)度小于3行時(shí)，則認(rèn)為本場(chǎng)圖像路徑提取失敗，如果連續(xù)3場(chǎng)圖像路徑提 取失敗則，則將起飛點(diǎn)坐標(biāo)作為下一個(gè)目標(biāo)點(diǎn)坐標(biāo)，轉(zhuǎn)至步驟①；⑤飛行器飛行方向與路徑方向夾角大小為e =
b-a飛行器路徑在圖像上偏離的距離為
1L = -(M-A-B)其中a為路徑下邊界值，A為下邊界左右兩邊界的中心值，b為路徑上邊界值，B為 上邊界左右兩邊界的中心值，M為行圖像采集的數(shù)據(jù)個(gè)數(shù)。其中Zigbee是IEEE802. 15. 4協(xié)議的代名詞，根據(jù)這個(gè)協(xié)議規(guī)定的是一種短距離、 低功耗的無(wú)線通信技術(shù)。本發(fā)明的優(yōu)點(diǎn)本微型共軸雙旋翼飛行器控制系統(tǒng)以微型四通道共軸雙旋翼直升 機(jī)為載體，該載體體積小、效率高，機(jī)載控制系統(tǒng)選用了內(nèi)核ARM微處理器作為CPU，提高 了資源的利用率，三軸加速度傳感器、三軸磁航向傳感器、三軸角速度陀螺儀、超聲波傳感 器、氣壓計(jì)，采用了芯片級(jí)的傳感器，采用GPS定位模塊，微型無(wú)線攝像頭。無(wú)線通訊采用了 Zigbee無(wú)線通訊方式，其能夠方便的實(shí)現(xiàn)機(jī)器人群體的組網(wǎng)通訊。該系統(tǒng)能實(shí)現(xiàn)航拍和任 務(wù)設(shè)定自主飛行以及地面尋跡自主飛行。自動(dòng)手動(dòng)可方便切換。


圖1本發(fā)明裝置結(jié)構(gòu)框圖；圖2本發(fā)明地面無(wú)線傳輸模塊/空中無(wú)線傳輸模塊電原理圖；圖3本發(fā)明三軸磁航向傳感器電原理圖；圖4 (a)本發(fā)明三軸陀螺儀_俯仰和翻滾陀螺儀電原理圖；圖4 (b)本發(fā)明三軸陀螺儀_偏航陀螺儀電原理圖；圖5本發(fā)明三軸加速度傳感器電原理圖；圖6本發(fā)明GPS電原理圖；圖7本發(fā)明氣壓計(jì)電原理圖；圖8本發(fā)明ARM處理器電原理圖；圖9本發(fā)明視頻解碼模塊電原理圖10本發(fā)明控制方法流程圖。
具體實(shí)施例方式本發(fā)明一種微型共軸雙旋翼飛行器控制裝置及方法結(jié)合施實(shí)例加以說(shuō)明。超聲波傳感器選用SRF05模塊；微型無(wú)線攝像頭、視頻無(wú)線傳輸模塊和視頻無(wú)線接收機(jī)選用深圳市聯(lián)益達(dá)科技有 限公司型號(hào)為RC211A+208CWAS無(wú)線攝像機(jī)的部分器件；地面無(wú)線傳輸模塊如圖1所示；三 軸磁航向傳感器如圖2所示；三軸陀螺儀如圖3所示；三軸加速度傳感器如圖4(a)和圖 4(B)所示；GPS如圖5所示；氣壓計(jì)如圖6所示；ARM處理器選用型號(hào)為STM32F103，如圖7 所示；視頻解碼模塊如圖8所示；空無(wú)線傳輸模塊的電原理結(jié)構(gòu)和地面無(wú)線傳輸模塊相同， 空無(wú)線傳輸模塊如圖1所示；地面處理器包括ARM處理器、MAX3232、串口、串口線，各部件的連接是ARM處理器 的USARTl的RX、TX引腳與相應(yīng)的MAX3232引腳相連接，串口與相應(yīng)的MAX3232引腳相連接。該裝置包括地面裝置和空中裝置兩部分；其中地面裝置包括視頻無(wú)線接收機(jī)、地 面無(wú)線傳輸模塊、地面處理器、顯示器和上位機(jī)；空中裝置包括有三軸磁航向傳感器、三軸 陀螺儀、三軸加速度傳感器、GPS、氣壓計(jì)、超聲波傳感器、ARM處理器、微型無(wú)線攝像頭、視頻 解碼模塊、視頻無(wú)線傳輸模塊和空中無(wú)線傳輸模塊；其中地面無(wú)線傳輸模塊如圖1所示，包括CC2520(U15)、電容(C66、C69、C70、C71、 C72、C73、C74、C75、C76)、電阻(R43)、電感(TL1、TL2、TL3)、天線(T2)和晶振(Y4)，各部件 的連接是電容(C66) —端接地，另一端與CC2520(U15)的DCOUPL引腳相連接；電阻(R43) 一端接地，一端與CC2520(U15)的RBIAS引腳相連接；天線(T2)與電容(C70)和電感(TL2) 的一端相連接，電容(C70)的另一端接地，電感(TL2)的另一端與電容(C69)、電容(C72)和 電感(TL3)的一端相連接，電容(C69)的另一端接地，電容(C72)的另一端與電容(C71)和 電感(TLl)的一端相連接，電感(TLl)的另一端接地，電容(C71)的另一端與CC2520(U15) 的RF-N引腳相連接；電感(TL3)的另一端與電容(C73)和電容(C74)的一端相連接，電容 (C74)的另一端接地，電容(C73)的另一端與CC2520(U15)的RF-P引腳相連接；電容(C75) 一端接地，另一端與32M晶振(Y4)和CC2520(U15)的S0SC32M-2引腳相連接；電容(C76) 一端接地，另一端與32M晶振(Y4)和CC2520(U15)的S0SC32M-2引腳相連接。地面裝置器件之間的連接地面無(wú)線傳輸模塊(C2520)的輸入引腳RESETn、VREG_ EN引腳分別與地面處理器中的ARM處理器(STM32F103RC)的PC4、PB6引腳相連接，SPI接 口 的 SCLK、SO、Si、CSn 引腳分別與 ARM 處理器(STM32F103RC)的 PA5、PA6、PA7、PA4 引腳 相連接，輸入/輸出雙向引腳GPI00、GPI01、GPI02、GPI03、GPI04、GPI05分別與ARM處理器 (STM32F103RC)的PB0、PC5、PB15、PB14、PB13、PB12引腳相連接；地面處理器的串口通過(guò)串 口線與上位機(jī)連接；視頻無(wú)線接收機(jī)輸出端連接顯示器的輸入端?？罩醒b置器件之間的連接三軸磁航向傳感器(HMC5843)的I2C接口 SCL、SDA引 腳分別與ARM處理器(STM32F103RC)的PB10、PB11引腳相連接；三軸陀螺儀中的俯仰和翻 滾陀螺儀(LPR530AL)的輸入引腳HP與ARM處理器(STM32F103RC)的PC13引腳相連接， 輸出引腳4X0UX、4X0UY、Vref三個(gè)引腳分別與ARM處理器(STM32F103RC)的PC2、PC3、
11PAO引腳相連接；三軸陀螺儀中的偏航陀螺儀(LY330ALH)輸出引腳OUZ、Vref引腳分別 與ARM處理器(STM32F103RC)的PCO、PCl引腳相連接；三軸加速度傳感器(MMA7260A)的 輸出端 ACC_X、ACC_Y、ACC_Y 分別與 ARM 處理器(STM32F103RC)的 PA1、PA2、PA3 引腳相連 接；GPS(G591BGPS 模塊)的 RXA、TXA 和 RXB、TXB 引腳分別與 ARM 處理器(STM32F103RC)的 PA9、PAlO和PC10、PCll引腳相連接，輸出引腳IPPS引腳與ARM處理器(STM32F103RC)的 PA12引腳相連接；超聲波傳感器(SRF05模塊)的輸入/輸出雙向引腳TRIGGER INPUT/ECHO OUTPUT與ARM處理器(STM32F103RC)的PB7引腳相連接；氣壓計(jì)的I2C接口 SCL、SDA引腳 分別與ARM處理器(STM32F103RC)的PB10、PBll引腳相連接；空中無(wú)線傳輸模塊(C2520) 的輸入引腳RESETn、VREG_EN引腳分別與ARM處理器(STM32F103RC)的PC4、PA11引腳相連 接，SPI 接口的 SCLK、SO、Si、CSn 引腳分別與 ARM 處理器(STM32F103RC)的 PA5、PA6、PA7、 PA4引腳相連接，輸入/輸出雙向引腳GPI00、GPI01、GPI02、GPI03、GPI04、GPI05分別與ARM 處理器(STM32F103RC)的PB0、PC5、PB15、PB14、PB13、PB12引腳相連接；微型無(wú)線攝像頭的 一個(gè)輸出端與視頻解碼模塊(LMH1981)的輸入端相連接；視頻解碼模塊(LMH1981)的輸出 端HSOUT、OEOUTA與ARM處理器(STM32F103RC)的PB6、PB5相連接，無(wú)線攝像頭的另一個(gè)輸 出端與視頻無(wú)線傳輸模塊的輸入端相連接。
示

所述的微型共軸雙旋翼飛行器控制裝置的控制方法，包括如下步驟，如圖10所
一、裝置初始化；
二、采集各路傳感器數(shù)據(jù)
依次采集三軸加速度傳感器，三軸磁航向傳感器、三軸角速度陀螺儀、超聲波傳感 器、氣壓計(jì)、GPS的數(shù)據(jù)(1)將三軸加速度傳感器三個(gè)軸向輸出的模擬電壓信號(hào)，實(shí)驗(yàn)測(cè)量為Vx = 1. 1231V，Vy = 1. 7741V, Vz = 1. 6734V，經(jīng)AD轉(zhuǎn)換成數(shù)字信號(hào)，對(duì)信號(hào)進(jìn)行濾波，并儲(chǔ)存飛
行器 X、Y、Z 軸上的加速度 ax、ay、az，實(shí)驗(yàn)測(cè)量為 ax = -7. 8m/s2、ay = 1. 52m/s2、az = 0. 23m/
2
S ；(2)讀取三軸磁航向傳感器上地磁場(chǎng)在三個(gè)軸上的數(shù)字磁場(chǎng)分量，并確定飛行器 與地磁線所成夾角具體如下如果Hy >0，則5 = 90- [arctan(/ic / Hy)] *\%Q/n(1)如果Hy <0，則5 = 270 - [arctan(i/x / Hy)] * 180 / π(2)如果Hy = 0，Hx < 0 則5 = 180(3)如果Hy = 0，Hx > 0 則3 = 0(4)實(shí)驗(yàn)數(shù)據(jù)為3=90-[arctan(l77.35 / 2.61)] * 180 / ττ=41.573 °(3)將俯仰、翻滾陀螺儀，偏航陀螺儀輸出的模擬電壓信號(hào)經(jīng)AD轉(zhuǎn)換成數(shù)字信號(hào)， 對(duì)信號(hào)進(jìn)行濾波，并儲(chǔ)存飛行器俯仰、翻滾、偏航角速度Ωχ、Qy、Ω ζ，實(shí)驗(yàn)數(shù)據(jù)為飛行器俯
12仰角速度0. 107rad/s ；翻滾角速度0. 013rad/s ；偏航角速度0. 361rad/s ；(4)讀取數(shù)字氣壓計(jì)的氣壓值P，并確定絕對(duì)高度，方法見(jiàn)下式
(丄、
,(η ^1255H =44330χ 1-(5)
UoJ
ν/式中P0為海平面氣壓，其值為1013. 25hPa ；H'為絕對(duì)高度，單位為米，測(cè)量結(jié)果 為 42m ；如果是第一次測(cè)量氣壓值，則將H'賦值給外部變量Hs，計(jì)算飛行器飛行的相對(duì)高 度
(丄、// = 44330X 1-丄 -Hs(6)
V/發(fā)射方波，打開(kāi)接收中斷，用定時(shí)器記錄發(fā)射與接收的時(shí)間間隔T(單位為秒)，如 果固定時(shí)間內(nèi)接收到返回接收信號(hào)，則計(jì)算相對(duì)高度H = 344*Τ/2，并代替公式(6)計(jì)算得 到的相對(duì)高度，否則說(shuō)明測(cè)量超過(guò)超聲波量程，關(guān)閉接收中斷，儲(chǔ)存高度值H，測(cè)試實(shí)際在低 空環(huán)境，所以采用超聲測(cè)距測(cè)量高度，測(cè)量得到T = 0. 017s，計(jì)算得高度為2. 92m ；(5)確定飛行器的經(jīng)度和緯度；讀取GPS信號(hào)，得到飛行器的經(jīng)度E和緯度N，并儲(chǔ)存，如果是第一次得到飛行器的 經(jīng)度E和緯度N，則分別賦值給外部變量Ε。N0分別作為起飛點(diǎn)坐標(biāo)，本地經(jīng)緯度測(cè)量值為 東經(jīng) 121° 25，50. 1〃 北緯 31° 13，20.8〃 ；三、確定飛行模式、開(kāi)始起飛、飛行高度、飛行速度、飛行目標(biāo)點(diǎn)命令；將步驟二測(cè)到的各路傳感器數(shù)據(jù)組合成數(shù)據(jù)包后通過(guò)機(jī)載Zigbee無(wú)線發(fā)送給地 面Zigbee部分，并且接收來(lái)地面自控制界面通過(guò)地面Zigbee發(fā)送過(guò)來(lái)的控制命令數(shù)據(jù)包， 解析數(shù)據(jù)包后得到飛行模式、開(kāi)始起飛、飛行高度、飛行速度、飛行目標(biāo)點(diǎn)命令；四、通過(guò)相對(duì)高度判斷飛行器是否起飛，當(dāng)相對(duì)高度為零，飛行器沒(méi)有起飛；當(dāng)相 對(duì)高度為正值，飛行器已經(jīng)起飛，如果沒(méi)有起飛，轉(zhuǎn)入步驟四；如果已經(jīng)起飛，轉(zhuǎn)入步驟五；五、空中裝置ARM處理器通過(guò)地面發(fā)過(guò)來(lái)的控制命令數(shù)據(jù)包選擇飛行器的飛行模 式，把三種飛行模式賦予不同的數(shù)據(jù)，通過(guò)與地面發(fā)過(guò)來(lái)的控制命令數(shù)據(jù)包中相應(yīng)的數(shù)據(jù) 進(jìn)行比較，選擇相應(yīng)的飛行模式；六、計(jì)算飛行器控制增量；飛行器的有四個(gè)控制通道，分別為上升下降，前進(jìn)后退，左右平移，左右擺動(dòng)，飛行 器的姿態(tài)的控制可以分解成對(duì)這四個(gè)通道的控制，各個(gè)通道采用下例式計(jì)算出控制增量Δ U (k) = kp X Δ E (k) +Iii X E (k) +kd [Ε (k) -2E (k_l) +E (k_2)]
+kn X Δ a (k) +k12 X a (k) +a13 [a (k) -2a (k_l) +a (k_2)]+k21 X Δ Ω (k) +k22 X Ω (k) +k23 [ Ω (k) -2 Ω (k_l) + Ω (k_2)]式中kp、kn、k21分別為被控量、加速度、角速度的比例系數(shù)，實(shí)驗(yàn)時(shí)得到的值分別 為80、90、30 ；k” k12、k22分別為被控量、加速度、角速度的積分系數(shù)，實(shí)驗(yàn)時(shí)得到的值分別 為40、40、20 ；kd、k13、k23分別為被控量、加速度、角速度的微分系數(shù)，實(shí)驗(yàn)時(shí)得到的值分別為=45,50,30 ；E(k)、a(k)、Ω (k)分別為被控量、加速度、角速度的系統(tǒng)偏差，分別計(jì)算得 65、0.29、0.0021 ;AE(k)、Aa(k)、Δ Ω (k)分別為被控量、加速度、角速度的偏差變化率， 三個(gè)偏差分別為 35、0· 73、0· 04 ； AE(k) = E(k)-E(k_l)，Aa(k) = a(k)-a(k_l)，Δ Ω (k) =Ω (k)-Q (k-1) ；k, k-1，k-2分別表示第k次，第k-1次，第k-2次，經(jīng)過(guò)計(jì)算，控制增量 Au(k)為 2347. 57 ；七、舵機(jī)和電機(jī)的控制通過(guò)飛行器上的ARM7處理器控制PWM寄存器控制飛行器的舵機(jī)和電機(jī)；其中因舵 機(jī)的轉(zhuǎn)角與控制舵機(jī)的脈沖寬度成線性關(guān)系，脈沖寬度又是由處理器內(nèi)部PWM寄存器值來(lái) 設(shè)置的，通過(guò)處理器計(jì)算改變PWM寄存器值來(lái)控制飛行器左右平移和前進(jìn)后退；兩個(gè)電機(jī) 形成轉(zhuǎn)速差時(shí)會(huì)產(chǎn)生旋轉(zhuǎn)力矩通過(guò)控制，該力矩可以控制飛行器左右方向擺動(dòng)，當(dāng)兩電機(jī) 同比增速或減速時(shí)則能控制飛行器的上升和下降；(1)舵機(jī)控制左右平移、前進(jìn)后退運(yùn)動(dòng)相對(duì)懸停狀態(tài)下兩舵機(jī)角度變化相互關(guān)系為左平移左舵機(jī)角度變化量為+ θ &移，右舵機(jī)角度變化量為_(kāi) θ左移右平移左舵機(jī)角度變化量為_(kāi) θ $移，右舵機(jī)角度變化量為+ θ拂前進(jìn)左舵機(jī)角度變化量為_(kāi) θ胃進(jìn)，右舵機(jī)角度變化量為_(kāi) θ纖后退左舵機(jī)角度變化量為+ θ g退，右舵機(jī)角度變化量為+ θ ^ia假設(shè)飛行器需要往X-Y平面的第一象限平移，則飛行器可以分解成前進(jìn)和右平 移，則最終左右舵機(jī)分別旋轉(zhuǎn)的角度為θ左舵=90-θ前進(jìn)+θ右移θ右舵=90-θ前進(jìn)-θ右移在采集數(shù)據(jù)的時(shí)間點(diǎn)上，程序給出的處理結(jié)果是下螺旋槳前傾12°，分別輸出到 兩個(gè)控制方向的舵機(jī)分別是左舵減小6°，右舵增加6°(2)電機(jī)控制兩個(gè)電機(jī)轉(zhuǎn)動(dòng)方向相反，當(dāng)兩個(gè)電機(jī)形成轉(zhuǎn)速差時(shí)會(huì)產(chǎn)生旋轉(zhuǎn)力矩，該力矩可以 控制飛行器左右方向擺動(dòng)，當(dāng)向右方向轉(zhuǎn)動(dòng)的電機(jī)轉(zhuǎn)速大于向左方向轉(zhuǎn)動(dòng)的電機(jī)轉(zhuǎn)速時(shí)， 飛機(jī)向右方向擺動(dòng)；當(dāng)向左方向轉(zhuǎn)動(dòng)的電機(jī)轉(zhuǎn)速大于向右方向轉(zhuǎn)動(dòng)的電機(jī)轉(zhuǎn)速時(shí)，飛機(jī)向 左方向擺動(dòng)，兩個(gè)電機(jī)同比增速，可以控制飛行器上升；兩個(gè)電機(jī)同比減速減時(shí)，可以控制 飛行器的下降；電機(jī)采用增量式PID控制Δ U1 (k) = kp X Δ E (k) +Iii X E (k) +kd [Ε (k) -2E (k-1) +E (k-2)]式中，Kp, Ki, Kd分別是PID控制中比例，積分，微分控制參數(shù)，控制中分別取值為 80,50,20 ；E(k)為系統(tǒng)偏差；E(k)-2E(k-l)+E(k-2)為系統(tǒng)量變化率，在采集數(shù)據(jù)的時(shí)間點(diǎn) 上，AE(k) =31，E(k) = 46，E(k)-2E(k-l)+E(k-2) = -24，計(jì)算得到 Δ U1 (k) = 4300，得 到的值作為PWM脈寬的變化量來(lái)控制電機(jī)；八、通過(guò)GPS信號(hào)獲取飛機(jī)所在位置，與目的地的經(jīng)緯度進(jìn)行比較，如果兩者不 等，飛機(jī)未到達(dá)目的地，轉(zhuǎn)入步驟二 ；如果兩者相等，飛機(jī)到達(dá)目的地，給定高度逐漸減小， 飛行器降落，實(shí)驗(yàn)中測(cè)得的飛行位置為東經(jīng)121° 25’45.4〃北緯31° 13’17.9〃，與目的 地位置東經(jīng)121° 25，40〃北緯31° 13，20〃不等，飛機(jī)未到達(dá)目的地，轉(zhuǎn)入步驟二。
飛行模式分為三種遙控模式、航線設(shè)定模式、自主循跡模式；(1)遙控模式定時(shí)器輸入捕捉方式計(jì)算出遙控信號(hào)左右舵機(jī)和前后電機(jī)對(duì)應(yīng)的通道的遙控脈 沖寬度，進(jìn)而處理得到左右舵機(jī)控制脈沖寬度和前后電機(jī)PWM控制信號(hào)的周期和占空比；(2)航線設(shè)定模式飛行器飛行的方向(與地磁線的夾角)為
E-E^ = arctan “
N-Nn飛行器偏移設(shè)定航線的距離為L(zhǎng) _ MV+h%2+/Λ2)-(// +hA +/7)
一 2/,其中Ε、N分別為本次測(cè)得的經(jīng)度和緯度，En、Nn分別為飛行器由步驟(3)得到 的下一個(gè)目標(biāo)點(diǎn)的經(jīng)度和緯度，E0, N0分別作為起飛點(diǎn)坐標(biāo)，Z1 = ^(En-E0)2+(Nn-N0)2， l2=^E-E0f HN-N0Y %^{En_EfHNn_Nf ；飛行器飛行的高度Htl和速度Vtl由步驟三得到；(3)自主循跡模式對(duì)攝像頭視頻信號(hào)進(jìn)行數(shù)據(jù)采集，提取白布上黑線在圖像上形成的路徑的左右上 下邊界，計(jì)算出飛行器偏移路徑的角度和距離，具體步驟如下①采集圖像數(shù)據(jù)采取了降低圖像分辨率的方法每行采集48個(gè)點(diǎn)，每幅圖像采集40行，圖像分辨率 為 48*40 ；②計(jì)算閾值并二值化圖像圖像采第1場(chǎng)的第1行數(shù)據(jù)用雙峰法計(jì)算出該行的閾值，將該圖像中大于等于閾 值的點(diǎn)賦值為1，小于閾值的點(diǎn)賦值為0，實(shí)現(xiàn)二值化處理，第2行閾值引用第1行的閾值， 并實(shí)現(xiàn)二值化處理，將所有值為1的點(diǎn)所對(duì)應(yīng)的數(shù)據(jù)相加取平均值，將所有值為0的點(diǎn)所對(duì) 應(yīng)的數(shù)據(jù)相加取平均值，兩個(gè)平均值再取平均值便得到第2行的閾值，并與上一行的閾值 進(jìn)行比較，在允許的范圍內(nèi)更新，如此迭代至最后一行，最后一行的閾值則作為下一場(chǎng)第一 行的閾值，如此反復(fù)迭代；③判斷每行圖像二值化情況每行圖像從起點(diǎn)到終點(diǎn)進(jìn)行判斷，如果該行僅一次出現(xiàn)值為“0-1-0”變化，則認(rèn)為 該行圖像二值化情況良好；④搜索路徑的左右邊界、上下邊界從第1行起依次搜索該場(chǎng)圖像的其他行的圖像二值化情況，如果連續(xù)3行是二值 化情況良好，則停止搜索，并把該行作為路徑搜索起始行ist，以ist作為起始行向上(行標(biāo) 號(hào)增加的方向)和向下(行標(biāo)號(hào)減小的方向)對(duì)各行進(jìn)行路徑邊界的搜索；路徑左邊界向上搜索將路徑左邊界向上一行延伸，如果上一行該處值為0的點(diǎn)，則向左搜索路徑左邊
界，如果搜索超出邊界還沒(méi)搜索到則用邊界值代替；如果該處值為1的點(diǎn)，則向右搜索路徑
15左邊界路徑右邊界向上搜索將路徑右邊界向上一行延伸，如果上一行該處值為0的點(diǎn)，則向右搜索路徑右邊 界；如果該處值為1的點(diǎn)，則向左搜索路徑右邊界；如果搜索到的路徑邊界出現(xiàn)左路徑邊界值大于右路徑邊界值，左右邊界太寬(為 0點(diǎn)個(gè)數(shù)大于10個(gè))或左右邊界值之間有值為0的點(diǎn)，則嘗試跳過(guò)行，該行的路徑邊界值用 前兩行邊界的平均值進(jìn)行補(bǔ)充，如果向上路徑搜索過(guò)程中總跳躍次數(shù)達(dá)到3次或搜索達(dá)到 頂端則結(jié)束搜索，同時(shí)可以得到路徑上邊界值b，并計(jì)算出上邊界左右兩邊界的中心值B ；向下搜索同理可得到路徑下邊界值a，計(jì)算下邊界左右兩邊界的中心值A(chǔ)，如果搜 索到的有效路徑長(zhǎng)度小于3行時(shí)，則認(rèn)為本場(chǎng)圖像路徑提取失敗，如果連續(xù)3場(chǎng)圖像路徑提 取失敗則，則將起飛點(diǎn)坐標(biāo)作為下一個(gè)目標(biāo)點(diǎn)坐標(biāo)，轉(zhuǎn)至步驟①，返回起飛點(diǎn)；⑤飛行器飛行方向與路徑方向夾角大小為
ΛB-A(9 = arctan-
b-a飛行器路徑在圖像上偏離的距離為L(zhǎng) = ^(M-A-B)其中a為路徑下邊界值，A為下邊界左右兩邊界的中心值，b為路徑上邊界值，B為 上邊界左右兩邊界的中心值，M為行圖像采集的數(shù)據(jù)個(gè)數(shù)。
1權(quán)利要求
一種微型共軸雙旋翼飛行器控制裝置，其特征在于該裝置包括地面裝置和空中裝置兩部分；其中地面裝置包括視頻無(wú)線接收機(jī)、地面無(wú)線傳輸模塊、地面處理器、顯示器和上位機(jī)；空中裝置包括有三軸磁航向傳感器、三軸陀螺儀、三軸加速度傳感器、GPS、氣壓計(jì)、超聲波傳感器、ARM處理器、微型無(wú)線攝像頭、視頻解碼模塊、視頻無(wú)線傳輸模塊和空中無(wú)線傳輸模塊；所述的地面裝置的連接是視頻無(wú)線接收機(jī)輸出端連接顯示器的輸入端，地面無(wú)線傳輸模塊與地面處理器通過(guò)同步串口端連接；地面處理器與上位機(jī)通過(guò)異步串口端連接；所述的空中裝置安裝在飛行器上，空中裝置的連接是三軸磁航向傳感器、三軸陀螺儀、三軸加速度傳感器、GPS、氣壓計(jì)和超聲波傳感器的輸出端均連接ARM處理器相應(yīng)的輸入端，微型無(wú)線攝像頭的一個(gè)輸出端通過(guò)視頻解碼模塊連接ARM處理器的輸入端，微型無(wú)線攝像頭的另一個(gè)輸出端連接視頻無(wú)線傳輸模塊的輸入端，ARM處理器與空中無(wú)線傳輸模塊通過(guò)同步串口端連接；其中地面裝置和空中裝置通過(guò)其中地面無(wú)線傳輸模塊和空中無(wú)線傳輸模塊實(shí)現(xiàn)無(wú)線通訊，視頻無(wú)線接收機(jī)與視頻無(wú)線傳輸模塊實(shí)現(xiàn)無(wú)線通訊。
2.采用權(quán)利要求1所述的微型共軸雙旋翼飛行器控制裝置的控制方法，其特征在于， 包括如下步驟步驟一、裝置初始化；步驟二、采集各路傳感器數(shù)據(jù)；依次采集三軸加速度傳感器，三軸磁航向傳感器、三軸角速度陀螺儀、超聲波傳感器、 氣壓計(jì)和GPS的記錄數(shù)據(jù)(1)將三軸加速度傳感器三個(gè)軸向輸出的模擬電壓信號(hào)經(jīng)AD轉(zhuǎn)換成數(shù)字信號(hào)，對(duì)信號(hào) 進(jìn)行濾波，并儲(chǔ)存飛行器X、Y、Z軸上的加速度ax、ay、az ；(2)讀取三軸磁航向傳感器上地磁場(chǎng)在三個(gè)軸上的數(shù)字磁場(chǎng)分量，并確定飛行器與地 磁線所成夾角(3)將俯仰、翻滾陀螺儀，偏航陀螺儀輸出的模擬電壓信號(hào)經(jīng)AD轉(zhuǎn)換成數(shù)字信號(hào)，對(duì)信 號(hào)進(jìn)行濾波，并儲(chǔ)存飛行器俯仰角速度Ωχ、翻滾角速度Qy、偏航角速度Ωζ;(4)讀取數(shù)字氣壓計(jì)的氣壓值P，并確定絕對(duì)高度，方法如下式( 式中Ρ0為海平面氣壓，其值為1013.25hPa;H'為絕對(duì)高度，單位為米；如果是第一次測(cè)量氣壓值，則將H'賦值給外部變量Hs，計(jì)算飛行器飛行的相對(duì)高度 發(fā)射方波，打開(kāi)接收中斷，記錄發(fā)射與接收的時(shí)間間隔Τ，如果固定時(shí)間內(nèi)接收到返回 接收信號(hào)，則計(jì)算相對(duì)高度Η = 344ΧΤ/2，并代替公式(6)計(jì)算得到的相對(duì)高度，否則說(shuō)明 測(cè)量超過(guò)超聲波量程，關(guān)閉接收中斷，儲(chǔ)存高度值H;(5)確定飛行器的經(jīng)度和緯度；讀取GPS信號(hào)，得到飛行器的經(jīng)度E和緯度N，并儲(chǔ)存，如果是第一次得到飛行器的經(jīng)度 E和緯度N，則分別賦值給外部變量&、N0,作為起飛點(diǎn)坐標(biāo)；步驟三、確定飛行模式、開(kāi)始起飛、飛行高度、飛行速度和飛行目標(biāo)點(diǎn)命令； 將步驟二測(cè)到的各路傳感器數(shù)據(jù)組合成數(shù)據(jù)包后通過(guò)機(jī)載Zigbee無(wú)線發(fā)送給地面 Zigbee部分，并且接收來(lái)自地面控制界面通過(guò)地面Zigbee發(fā)送過(guò)來(lái)的控制命令數(shù)據(jù)包，解 析數(shù)據(jù)包后得到飛行模式、開(kāi)始起飛、飛行高度、飛行速度和飛行目標(biāo)點(diǎn)命令；步驟四、通過(guò)相對(duì)高度判斷飛行器是否起飛，當(dāng)相對(duì)高度為零，飛行器沒(méi)有起飛；當(dāng)相 對(duì)高度為正值，飛行器已經(jīng)起飛，如果沒(méi)有起飛，轉(zhuǎn)入步驟四；如果已經(jīng)起飛，轉(zhuǎn)入步驟五； 步驟五、空中裝置ARM處理器通過(guò)地面發(fā)過(guò)來(lái)的控制命令數(shù)據(jù)包選擇飛行器的飛行模式；把遙控模式、航線設(shè)定模式、自主循跡模式三種飛行模式賦予不同的數(shù)據(jù)，通過(guò)與地面 發(fā)過(guò)來(lái)的控制命令數(shù)據(jù)包中相應(yīng)的數(shù)據(jù)進(jìn)行比較，選擇相應(yīng)的飛行模式； 步驟六、確定飛行器控制增量；飛行器有四個(gè)控制通道，分別為上升下降，前進(jìn)后退，左右平移，左右擺動(dòng)，飛行器的姿 態(tài)的控制可以分解成對(duì)這四個(gè)通道的控制，各個(gè)通道采用下例式計(jì)算出控制增量 Δ u (k) = kp X Δ E (k) +Iii X E (k) +kd [Ε (k) -2E (k_l) +E (k_2)] +kn X Δ a (k) +k12 X a (k) +a13 [a (k) -2a (k_l) +a (k_2)] +k21X Δ Ω (k)+k22X Ω (k) +k23 [ Ω (k) -2 Ω (k-D + Ω (k_2)] 式中kp、kn、k21分別為被控量、加速度、角速度的比例系數(shù)；k” k12、k22分別為被控量、 加速度、角速度的積分系數(shù)；kd、k13、k23分別為被控量、加速度、角速度的微分系數(shù)；E(k)、 a(k)、Ω (k)分別為被控量、加速度、角速度的系統(tǒng)偏差；ΔΕ(10、Aa(k)、Δ Ω (k)分別為 被控量、加速度、角速度的偏差變化率，ΔΕΟΟ = E(k)-E(k_l)，Aa(k) = a(k)-a(k_l)， Δ Ω (k) = Ω (10-Ω (k-1) ；k、k-l、k_2 分別表示第 k 次、第 k-Ι 次、第 k-2 次； 步驟七、控制飛行器的舵機(jī)和電機(jī)；通過(guò)飛行器上的ARM處理器控制PWM寄存器控制飛行器的舵機(jī)和電機(jī)；其中因舵機(jī)的 轉(zhuǎn)角與控制舵機(jī)的脈沖寬度成線性關(guān)系，脈沖寬度又是由ARM處理器內(nèi)部PWM寄存器值來(lái) 設(shè)置的，通過(guò)ARM處理器計(jì)算改變PWM寄存器值來(lái)控制飛行器左右平移和前進(jìn)后退；飛行 器上兩個(gè)電機(jī)形成轉(zhuǎn)速差時(shí)會(huì)產(chǎn)生旋轉(zhuǎn)力矩通過(guò)控制，該力矩可以控制飛行器左右方向擺 動(dòng)，當(dāng)兩電機(jī)同比增速或減速時(shí)則能控制飛行器的上升和下降；(1)舵機(jī)控制；左右平移、前進(jìn)后退運(yùn)動(dòng)相對(duì)懸停狀態(tài)下兩舵機(jī)角度變化相互關(guān)系為 左平移左舵機(jī)角度變化量為+ θ &移，右舵機(jī)角度變化量為-θ左移 右平移左舵機(jī)角度變化量為_(kāi) θ ，右舵機(jī)角度變化量為+ θ拂 前進(jìn)左舵機(jī)角度變化量為_(kāi) θ胃進(jìn)，右舵機(jī)角度變化量為-θ纖 后退左舵機(jī)角度變化量為+ θ β;ι，右舵機(jī)角度變化量為+ θ ^ia ；(2)電機(jī)控制；電機(jī)采用增量式PID控制Δ U1 (k) = kp X Δ E (k) +Iii X E (k) +kd [Ε (k) -2E (k-1) +E (k-2)]式中，Kp, Ki, Kd分別是PID控制中比例、積分、微分控制參數(shù);E(k)-2E(k-l)+E(k-2) 為系統(tǒng)量變化率，在采集數(shù)據(jù)的時(shí)間點(diǎn)上，將得到的值作為PWM脈寬的變化量來(lái)控制電機(jī)； 步驟八、通過(guò)GPS信號(hào)獲取飛機(jī)所在位置，與目的地的經(jīng)緯度進(jìn)行比較，如果兩者不 等，飛機(jī)未到達(dá)目的地，轉(zhuǎn)入步驟二 ；如果兩者相等，飛機(jī)到達(dá)目的地，給定高度逐漸減小， 飛行器降落。
3.按照權(quán)利要求2所述的微型共軸雙旋翼飛行器控制裝置的控制方法，其特征在于所 述的步驟五中空中裝置ARM處理器通過(guò)地面發(fā)過(guò)來(lái)的控制命令數(shù)據(jù)包選擇飛行器的飛行 模式，按如下步驟進(jìn)行飛行模式分為三種遙控模式、航線設(shè)定模式、自主循跡模式；(1)遙控模式計(jì)算出遙控信號(hào)左右舵機(jī)和前后電機(jī)對(duì)應(yīng)的通道的遙控脈沖寬度，進(jìn)而處理得到飛行 器左右舵機(jī)控制脈沖寬度和前后電機(jī)PWM控制信號(hào)的周期和占空比；(2)航線設(shè)定模式飛行器飛行的方向即與地磁線的夾角為E-E θ = arctan-—N-N17飛行器偏移設(shè)定航線的距離為L(zhǎng) _ 扭 hV +h%2+i2%2)-(h4 +12 +C) ~Tix其中Ε、N分別為本次測(cè)得的經(jīng)度和緯度，En、Nn分別為飛行器由步驟三得到的下 一個(gè)目標(biāo)點(diǎn)的經(jīng)度和緯度，Etl、Ntl分別作為起飛點(diǎn)坐標(biāo)，纟=^j(En-E0)2+(Nn-N0)2，I2=^I(E-E0)2+(N-N0)2 ,I3=^(En-E)2+(Nn-N)2，其中 1” I2 和 I3 分別為飛行器偏移設(shè)定航線在X，Y和Z軸的距離；(3)自主循跡模式；將微型無(wú)線攝像頭采集的視頻信號(hào)提取白布上黑線，在圖像上形成的路徑的左右上下 邊界，計(jì)算出飛行器偏移路徑的角度和距離，具體步驟如下①采集圖像數(shù)據(jù)；采取了降低圖像分辨率的方法每行采集至少48個(gè)點(diǎn)，每幅圖像采集至少40行，圖像分 辨率至少為48X40 ；②計(jì)算閾值并二值化圖像；圖像采第1場(chǎng)的第1行數(shù)據(jù)用雙峰法計(jì)算出該行的閾值，將該圖像中大于等于閾值的 點(diǎn)賦值為1，小于閾值的點(diǎn)賦值為0，實(shí)現(xiàn)二值化處理，第2行閾值引用第1行的閾值，并實(shí) 現(xiàn)二值化處理，將所有值為1的點(diǎn)所對(duì)應(yīng)的數(shù)據(jù)相加取平均值，將所有值為0的點(diǎn)所對(duì)應(yīng)的 數(shù)據(jù)相加取平均值，兩個(gè)平均值再取平均值便得到第2行的閾值，并與上一行的閾值進(jìn)行 比較，在允許的范圍內(nèi)更新，如此迭代至最后一行，最后一行的閾值則作為下一場(chǎng)第一行的 閾值，如此反復(fù)迭代；③判斷每行圖像二值化情況；每行圖像從起點(diǎn)到終點(diǎn)進(jìn)行判斷，如果該行僅一次出現(xiàn)值為“0-1-0”變化，則認(rèn)為該行圖像二值化情況良好；④搜索路徑的左右邊界、上下邊界；從第1行起依次搜索該場(chǎng)圖像的其他行的圖像二值化情況，如果連續(xù)3行是二值化情 況良好，則停止搜索，并把該行作為路徑搜索起始行ist，以ist作為起始行向上即行標(biāo)號(hào)增 加的方向和向下即行標(biāo)號(hào)減小的方向?qū)Ω餍羞M(jìn)行路徑邊界的搜索；路徑左邊界向上搜索將路徑左邊界向上一行延伸，如果上一行該處值為0的點(diǎn)，則向左搜索路徑左邊界，如 果搜索超出邊界還沒(méi)搜索到則用邊界值代替；如果該處值為1的點(diǎn)，則向右搜索路徑左邊 界路徑右邊界向上搜索將路徑右邊界向上一行延伸，如果上一行該處值為0的點(diǎn)，則向右搜索路徑右邊界；如 果該處值為1的點(diǎn)，則向左搜索路徑右邊界；如果搜索到的路徑邊界出現(xiàn)左路徑邊界值大于右路徑邊界值，左右邊界太寬即為0點(diǎn) 個(gè)數(shù)大于10個(gè)或左右邊界值之間有值為0的點(diǎn)，則嘗試跳過(guò)行，該行的路徑邊界值用前兩 行邊界的平均值進(jìn)行補(bǔ)充，如果向上路徑搜索過(guò)程中總跳躍次數(shù)達(dá)到3次或搜索達(dá)到頂端 則結(jié)束搜索，同時(shí)可以得到路徑上邊界值b，并計(jì)算出上邊界左右兩邊界的中心值B ；向下搜索同理可得到路徑下邊界值a，計(jì)算下邊界左右兩邊界的中心值A(chǔ)，如果搜索到 的有效路徑長(zhǎng)度小于3行時(shí)，則認(rèn)為本場(chǎng)圖像路徑提取失敗，如果連續(xù)3場(chǎng)圖像路徑提取失 敗，則將起飛點(diǎn)坐標(biāo)作為下一個(gè)目標(biāo)點(diǎn)坐標(biāo)，轉(zhuǎn)至步驟①；⑤飛行器飛行方向與路徑方向夾角大小為 θ = arctan B-A/b-a飛行器路徑在圖像上偏離的距離為L(zhǎng) = 1/2(M-A-B)其中a為路徑下邊界值，A為下邊界左右兩邊界的中心值，b為路徑上邊界值，B為上邊 界左右兩邊界的中心值，M為行圖像采集的數(shù)據(jù)個(gè)數(shù)。
全文摘要
一種微型共軸雙旋翼飛行器控制裝置及方法，屬于飛行器控制技術(shù)領(lǐng)域。該裝置包括地面裝置和空中裝置兩部分；其中地面裝置包括視頻無(wú)線接收機(jī)、地面無(wú)線傳輸模塊、地面處理器、顯示器和上位機(jī)；空中裝置包括有三軸磁航向傳感器、三軸陀螺儀、三軸加速度傳感器、GPS、氣壓計(jì)、超聲波傳感器、ARM處理器、微型無(wú)線攝像頭、視頻解碼模塊、視頻無(wú)線傳輸模塊和空中無(wú)線傳輸模塊。本發(fā)明的優(yōu)點(diǎn)提高了資源的利用率。其能夠方便的實(shí)現(xiàn)飛行器群體的組網(wǎng)通訊。該系統(tǒng)能實(shí)現(xiàn)航拍和任務(wù)設(shè)定自主飛行以及地面尋跡自主飛行，自動(dòng)手動(dòng)可方便切換。
文檔編號(hào)G05D1/12GK101916115SQ20101023758
公開(kāi)日2010年12月15日 申請(qǐng)日期2010年7月27日 優(yōu)先權(quán)日2010年7月27日
發(fā)明者孟祥飛, 張青春, 李龍, 竇志龍, 羅凱, 蔡衍, 郭江龍, 陳述平, 陳金湖 申請(qǐng)人:東北大學(xué)