技術(shù)領(lǐng)域
[0001] 本
發(fā)明屬于飛行器控制技術(shù)領(lǐng)域，特別適用于一種微型共軸雙旋翼飛行器控制裝置及方法。
背景技術(shù)
[0002] 目前，因自主
導(dǎo)航系統(tǒng)一般體型較大，要實現(xiàn)具有自主飛行能
力的飛行器的微型化難度很大，而且系統(tǒng)隨著自主化程度的提高以及功能的增多體積也隨之增大，研究集尺寸微型、功能強大于一體的飛行器還是一片空白。
發(fā)明內(nèi)容
[0003] 為解決已有技術(shù)存在的不足，本發(fā)明提供一種微型共軸雙旋翼飛行器控制裝置及方法，通過控制微型共軸雙旋翼飛行器的
舵機和
電機，以達到精確控制的目的。
[0004] 本發(fā)明的技術(shù)方案是這樣實現(xiàn)的：該裝置包括地面裝置和空中裝置兩部分；其中地面裝置包括視頻無線接收機、地面無線傳輸模
塊、地面處理器、顯示器和上位機；空中裝置包括有三軸磁航向
傳感器、三軸
陀螺儀、三軸
加速度傳感器、GPS、氣壓計、
超聲波傳感器、ARM處理器、微型無線攝像頭、視頻解碼模塊、視頻無線傳輸模塊和空中無線傳輸模塊；
[0005] 所述的地面裝置的連接是：視頻無線接收機輸出端連接顯示器的輸入端，地面無線傳輸模塊與地面處理器通過同步串口端連接；地面處理器與上位機通過異步串口端連接；
[0006] 所述的空中裝置安裝在飛行器上，空中裝置的連接是：三軸磁航向傳感器、三軸陀螺儀、三軸加速度傳感器、GPS、氣壓計和
超聲波傳感器的輸出端均連接ARM處理器相應(yīng)的輸入端，微型無線攝像頭的一個輸出端通過視頻解碼模塊連接ARM處理器的輸入端，微型無線攝像頭的另一個輸出端連接視頻無線傳輸模塊的輸入端，ARM處理器與空中無線傳輸模塊通過同步串口端連接；
[0007] 其中地面裝置和空中裝置通過其中地面無線傳輸模塊和空中無線傳輸模塊實現(xiàn)無線通訊，視頻無線接收機與視頻無線傳輸模塊實現(xiàn)無線通訊。
[0008] 所述的微型共軸雙旋翼飛行器控制裝置的控制方法，包括如下步驟：
[0009] 步驟一、裝置初始化；
[0010] 步驟二、采集各路
傳感器數(shù)據(jù)[0011] 依次采集三軸加速度傳感器，三軸磁航向傳感器、三軸
角速度陀螺儀、
超聲波傳感器、氣壓計和GPS的記錄數(shù)據(jù)：
[0012] (1)將三軸加速度傳感器三個軸向輸出的模擬
電壓信號經(jīng)AD轉(zhuǎn)換成
數(shù)字信號，對信號進行濾波，并儲存飛行器X、Y、Z軸上的加速度ax、ay、az；
[0013] (2)讀取三軸磁航向傳感器上地
磁場在三個軸上的數(shù)字磁場分量，并確定飛行器與地磁線所成夾角
[0014] 具體如下：
[0015] 如果Hy＞0，則：
[0016]
[0017] 如果Hy＜0，則：
[0018]
[0019] 如果Hy＝0，Hx＜0則：
[0020]
[0021] 如果Hy＝0，Hx＞0則：
[0022]
[0023] (3)將
俯仰、翻滾陀螺儀，
偏航陀螺儀輸出的模擬電壓信號經(jīng)AD轉(zhuǎn)換成數(shù)字信號，對信號進行濾波，并儲存飛行器俯仰角速度Ωx、翻滾角速度Ωy、偏航角速度Ωz；
[0024] (4)讀取數(shù)字氣壓計的氣壓值p，并確定絕對高度，方法如下式：
[0025]
[0026] 式中：p0為海平面氣壓，其值為1013.25hPa；H′為絕對高度，單位為米；
[0027] 如果是第一次測量氣壓值，則將H′賦值給外部變量Hs，計算飛行器飛行的相對高度：
[0028]
[0029] 在低空環(huán)境下采用超聲測距：發(fā)射方波，打開接收中斷，記錄發(fā)射與接收的時間間隔T，如果固定時間內(nèi)接收到返回接收信號，則計算相對高度H＝344*T/2，并代替公式(6)計算得到的相對高度，否則說明測量超過超聲波量程，關(guān)閉接收中斷，儲存高度值H；
[0030] (5)確定飛行器的經(jīng)度和緯度；
[0031] 讀取GPS信號，得到飛行器的經(jīng)度E和緯度N，并儲存，如果是第一次得到飛行器的經(jīng)度E和緯度N，則分別賦值給外部變量E0、N0，作為
起飛點坐標；
[0032] 步驟三、確定飛行模式、開始起飛、飛行高度、飛行速度和飛行目標點命令；
[0033] 將步驟二測到的各路傳感器數(shù)據(jù)組合成數(shù)據(jù)包后通過機載Zigbee無線發(fā)送給地面Zigbee部分，并且接收來自地面控制界面通過地面Zigbee發(fā)送過來的控制命令數(shù)據(jù)包，解析數(shù)據(jù)包后得到飛行模式、開始起飛、飛行高度、飛行速度和飛行目標點命令；
[0034] 步驟四、通過相對高度判斷飛行器是否起飛，當相對高度為零，飛行器沒有起飛；當相對高度為正值，飛行器已經(jīng)起飛，如果沒有起飛，轉(zhuǎn)入步驟四；如果已經(jīng)起飛，轉(zhuǎn)入步驟五；
[0035] 步驟五、空中裝置ARM處理器通過地面發(fā)過來的控制命令數(shù)據(jù)包選擇飛行器的飛行模式；
[0036] 把遙控模式、航線設(shè)定模式、自主循跡模式三種飛行模式賦予不同的數(shù)據(jù)，通過與地面發(fā)過來的控制命令數(shù)據(jù)包中相應(yīng)的數(shù)據(jù)進行比較，選擇相應(yīng)的飛行模式；
[0037] 步驟六、確定飛行器控制增量；
[0038] 飛行器有四個控制通道，分別為上升下降，前進后退，左右平移，左右擺動，飛行器的
姿態(tài)的控制可以分解成對這四個通道的控制，各個通道采用下例式計算出控制增量：
[0039] Δu(k)＝kp×ΔE(k)+ki×E(k)+kd[E(k)-2E(k-1)+E(k-2)]
[0040] +k11×Δa(k)+k12×a(k)+a13[a(k)-2a(k-1)+a(k-2)]
[0041] +k21×ΔΩ(k)+k22×Ω(k)+k23[Ω(k)-2Ω(k-1)+Ω(k-2)]
[0042] 式中：kp、k11、k21分別為被控量、加速度、角速度的比例系數(shù)；ki、k12、k22分別為被控量、加速度、角速度的積分系數(shù)；kd、k13、k23分別為被控量、加速度、角速度的微分系數(shù)；E(k)、a(k)、Ω(k)分別為被控量、加速度、角速度的系統(tǒng)偏差；ΔE(k)、Δa(k)、ΔΩ(k)分別為被控量、加速度、角速度的偏差變化率；ΔE(k)＝E(k)-E(k-1)，Δa(k)＝a(k)-a(k-1)，ΔΩ(k)＝Ω(k)-Ω(k-1)；k，k-1，k-2分別表示第k次，第k-1次，第k-2次；
[0043] 步驟七、控制飛行器的舵機和電機；
[0044] 通過飛行器上的ARM處理器控制PWM寄存器控制飛行器的舵機和電機；其中因舵機的轉(zhuǎn)角與控制舵機的脈沖寬度成線性關(guān)系，脈沖寬度又是由ARM處理器內(nèi)部PWM寄存器值來設(shè)置的，通過ARM處理器計算改變PWM寄存器值來控制飛行器左右平移和前進后退；飛行器上兩個電機形成轉(zhuǎn)速差時會產(chǎn)生旋轉(zhuǎn)力矩通過控制，該力矩可以控制飛行器左右方向擺動，當兩電機同比增速或減速時則能控制飛行器的上升和下降；
[0045] (1)舵機控制
[0046] 左右平移、前進后退運動相對
懸停狀態(tài)下兩舵機角度變化相互關(guān)系為：
[0047] 左平移：左舵機角度變化量為+θ左移，右舵機角度變化量為-θ左移[0048] 右平移：左舵機角度變化量為-θ右移，右舵機角度變化量為+θ右移[0049] 前進：左舵機角度變化量為-θ前進，右舵機角度變化量為-θ前進
[0050] 后退：左舵機角度變化量為+θ后退，右舵機角度變化量為+θ后退；
[0051] (2)電機控制
[0052] 兩個電機轉(zhuǎn)動方向相反，當兩個電機形成轉(zhuǎn)速差時會產(chǎn)生旋轉(zhuǎn)力矩，該力矩可以控制飛行器左右方向擺動，當向右方向轉(zhuǎn)動的電機轉(zhuǎn)速大于向左方向轉(zhuǎn)動的電機轉(zhuǎn)速時，飛機向右方向擺動；當向左方向轉(zhuǎn)動的電機轉(zhuǎn)速大于向右方向轉(zhuǎn)動的電機轉(zhuǎn)速時，飛機向左方向擺動，兩個電機同比增速，可以控制飛行器上升；兩個電機同比減速減時，可以控制飛行器的下降；
[0053] 電機采用增量式PID控制：
[0054] Δu1(k)＝kp×ΔE(k)+ki×E(k)+kd[E(k)-2E(k-1)+E(k-2)]
[0055] 式 中，Kp，Ki，Kd分 別 是PID 控 制 中 比 例，積 分，微 分 控 制 參 數(shù)；E(k)-2E(k-1)+E(k-2)為系統(tǒng)量變化率，在采集數(shù)據(jù)的時間點上，將得到的值作為PWM脈寬的變化量來控制電機；
[0056] 步驟八、通過GPS信號獲取飛機所在
位置，與目的地的經(jīng)緯度進行比較，如果兩者不等，飛機未到達目的地，轉(zhuǎn)入步驟二；如果兩者相等，飛機到達目的地，給定高度逐漸減小，飛行器降落。
[0057] 所述的步驟五，空中裝置ARM處理器通過地面發(fā)過來的控制命令數(shù)據(jù)包選擇飛行器的飛行模式，按如下步驟進行：
[0058] 飛行模式分為三種：遙控模式、航線設(shè)定模式、自主循跡模式；
[0059] (1)遙控模式
[0060] 計算出遙控信號左右舵機和前后電機對應(yīng)的通道的遙控脈沖寬度，進而處理得到飛行器左右舵機控制脈沖寬度和前后電機PWM
控制信號的周期和占空比；
[0061] (2)航線設(shè)定模式
[0062] 飛行器飛行的方向即與地磁線的夾角為：
[0063]
[0064] 飛行器偏移設(shè)定航線的距離為：
[0065]
[0066] 其中E、N分別為本次測得的經(jīng)度和緯度，En、Nn分別為飛行器由步驟三得到的下一個目標點的經(jīng)度和緯度，E0、N0分別作為起飛點坐標，其中l(wèi)1、l2和k3分別為飛行器偏移
設(shè)定航線在X，Y和Z軸的距離；
[0067] (3)自主循跡模式
[0068] 將微型無線攝像頭采集的
視頻信號提取白布上黑線，在圖像上形成的路徑的左右上下邊界，計算出飛行器偏移路徑的角度和距離，具體步驟如下：
[0069] ①采集圖像數(shù)據(jù)
[0070] 采取了降低圖像
分辨率的方法每行采集至少48個點，每幅
圖像采集至少40行，圖像分辨率至少為48×40；
[0072] 圖像采第1場的第1行數(shù)據(jù)用雙峰法計算出該行的閾值，將該圖像中大于等于閾值的點賦值為1，小于閾值的點賦值為0，實現(xiàn)二值化處理，第2行閾值引用第1行的閾值，并實現(xiàn)二值化處理，將所有值為1的點所對應(yīng)的數(shù)據(jù)相加取平均值，將所有值為0的點所對應(yīng)的數(shù)據(jù)相加取平均值，兩個平均值再取平均值便得到第2行的閾值，并與上一行的閾值進行比較，在允許的范圍內(nèi)更新，如此
迭代至最后一行，最后一行的閾值則作為下一場第一行的閾值，如此反復(fù)迭代；
[0073] ③判斷每行圖像二值化情況
[0074] 每行圖像從起點到終點進行判斷，如果該行僅一次出現(xiàn)值為“0-1-0”變化，則認為該行圖像二值化情況良好；
[0075] ④搜索路徑的左右邊界、上下邊界
[0076] 從第1行起依次搜索該場圖像的其他行的圖像二值化情況，如果連續(xù)3行是二值化情況良好，則停止搜索，并把該行作為路徑搜索起始行ist，以ist作為起始行向上即行標號增加的方向和向下即行標號減小的方向?qū)Ω餍羞M行路徑邊界的搜索；
[0077] 路徑左邊界向上搜索：
[0078] 將路徑左邊界向上一行延伸，如果上一行該處值為0的點，則向左搜索路徑左邊界，如果搜索超出邊界還沒搜索到則用邊界值代替；如果該處值為1的點，則向右搜索路徑左邊界：
[0079] 路徑右邊界向上搜索：
[0080] 將路徑右邊界向上一行延伸，如果上一行該處值為0的點，則向右搜索路徑右邊界；如果該處值為1的點，則向左搜索路徑右邊界；
[0081] 如果搜索到的路徑邊界出現(xiàn)左路徑邊界值大于右路徑邊界值，左右邊界太寬即為0點個數(shù)大于10個或左右邊界值之間有值為0的點，則嘗試跳過行，該行的路徑邊界值用前兩行邊界的平均值進行補充，如果向上路徑搜索過程中總跳躍次數(shù)達到3次或搜索達到頂端則結(jié)束搜索，同時可以得到路徑上邊界值b，并計算出上邊界左右兩邊界的中心值B；
[0082] 向下搜索同理可得到路徑下邊界值a，計算下邊界左右兩邊界的中心值A(chǔ)，如果搜索到的有效路徑長度小于3行時，則認為本場圖像路徑提取失敗，如果連續(xù)3場圖像路徑提取失敗則，則將起飛點坐標作為下一個目標點坐標，轉(zhuǎn)至步驟①；
[0083] ⑤飛行器飛行方向與路徑方向夾角大小為：
[0084]
[0085] 飛行器路徑在圖像上偏離的距離為：
[0086]
[0087] 其中a為路徑下邊界值，A為下邊界左右兩邊界的中心值，b為路徑上邊界值，B為上邊界左右兩邊界的中心值，M為行圖像采集的數(shù)據(jù)個數(shù)。
[0088] 其中Zigbee是IEEE802.15.4協(xié)議的代名詞，根據(jù)這個協(xié)議規(guī)定的是一種短距離、低功耗的無線通信技術(shù)。
[0089] 本發(fā)明的優(yōu)點：本微型共軸雙旋翼飛行器控制系統(tǒng)以微型四通道共軸雙旋翼
直升機為載體，該載體體積小、效率高，機載控制系統(tǒng)選用了
內(nèi)核ARM
微處理器作為CPU，提高了資源的利用率，三軸加速度傳感器、三軸磁航向傳感器、三軸角速度陀螺儀、超聲波傳感器、氣壓計，采用了芯片級的傳感器，采用GPS
定位模塊，微型無線攝像頭。無線通訊采用了Zigbee無線通訊方式，其能夠方便的實現(xiàn)
機器人群體的組網(wǎng)通訊。該系統(tǒng)能實現(xiàn)航拍和任務(wù)設(shè)定自主飛行以及地面尋跡自主飛行。自動手動可方便切換。
附圖說明：
[0090] 圖1本發(fā)明裝置結(jié)構(gòu)
框圖；
[0091] 圖2本發(fā)明地面無線傳輸模塊/空中無線傳輸模塊電原理圖；
[0092] 圖3本發(fā)明三軸磁航向傳感器電原理圖；
[0093] 圖4(a)本發(fā)明三軸陀螺儀-俯仰和翻滾陀螺儀電原理圖；
[0094] 圖4(b)本發(fā)明三軸陀螺儀-偏航陀螺儀電原理圖；
[0095] 圖5本發(fā)明三軸加速度傳感器電原理圖；
[0096] 圖6本發(fā)明GPS電原理圖；
[0097] 圖7本發(fā)明氣壓計電原理圖；
[0098] 圖8本發(fā)明ARM處理器電原理圖；
[0099] 圖9本發(fā)明視頻解碼模塊電原理圖；
[0100] 圖10本發(fā)明控制方法
流程圖。
具體實施方式
[0101] 本發(fā)明一種微型共軸雙旋翼飛行器控制裝置及方法結(jié)合施實例加以說明。
[0102] 超聲波傳感器選用SRF05模塊；
[0103] 微型無線攝像頭、視頻無線傳輸模塊和視頻無線接收機選用深圳市聯(lián)益達科技有限公司型號為RC211A+208CWAS無線攝像機的部分器件；地面無線傳輸模塊如圖1所示；三軸磁航向傳感器如圖2所示；三軸陀螺儀如圖3所示；三軸加速度傳感器如圖4(a)和圖4(B)所示；GPS如圖5所示；氣壓計如圖6所示；ARM處理器選用型號為STM32F103，如圖7所示；視頻解碼模塊如圖8所示；空無線傳輸模塊的電原理結(jié)構(gòu)和地面無線傳輸模塊相同，空無線傳輸模塊如圖1所示；
[0104] 地面處理器包括ARM處理器、MAX3232、串口、串口線，各部件的連接是：ARM處理器的USART1的RX、TX引腳與相應(yīng)的MAX3232引腳相連接，串口與相應(yīng)的MAX3232引腳相連接。
[0105] 該裝置包括地面裝置和空中裝置兩部分；其中地面裝置包括視頻無線接收機、地面無線傳輸模塊、地面處理器、顯示器和上位機；空中裝置包括有三軸磁航向傳感器、三軸陀螺儀、三軸加速度傳感器、GPS、氣壓計、超聲波傳感器、ARM處理器、微型無線攝像頭、視頻解碼模塊、視頻無線傳輸模塊和空中無線傳輸模塊；
[0106] 其中地面無線傳輸模塊如圖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)和電感(TL1)的一端相連接，電感(TL1)的另一端接地，電容(C71)的另一端與CC2520(U15)的RF-N引腳相連接；電感(TL3)的另一端與電容(C73)和電容(C74)的一端相連接，電容(C74)的另一端接地，電容(C73)的另一端與CC2520(U15)的RF-P引腳相連接；電容(C75)一端接地，另一端與32M晶振(Y4)和CC2520(U15)的SOSC32M-2引腳相連接；電容(C76)一端接地，另一端與32M晶振(Y4)和CC2520(U15)的SOSC32M-2引腳相連接。
[0107] 地面裝置器件之間的連接：地面無線傳輸模塊(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引腳相連接；地面處理器的串口通過串口線與上位機連接；視頻無線接收機輸出端連接顯示器的輸入端。
[0108] 空中裝置器件之間的連接：三軸磁航向傳感器(HMC5843)的I2C接口SCL、SDA引腳分別與ARM處理器(STM32F103RC)的PB10、PB11引腳相連接；三軸陀螺儀中的俯仰和翻滾陀螺儀(LPR530AL)的輸入引腳HP與ARM處理器(STM32F103RC)的PC13引腳相連接，輸出引腳4×OUX、4×OUY、Vref三個引腳分別與ARM處理器(STM32F103RC)的PC2、PC3、PA0引腳相連接；三軸陀螺儀中的偏航陀螺儀(LY330ALH)輸出引腳OUZ、Vref引腳分別與ARM處理器(STM32F103RC)的PC0、PC1引腳相連接；三軸加速度傳感器(MMA7260A)的輸出端ACC_X、ACC_Y、ACC_Y分別與ARM處理器(STM32F103RC)的PA1、PA2、PA3引腳相連接；GPS(G591BGPS模塊)的RXA、TXA和RXB、TXB引腳分別與ARM處理器(STM32F103RC)的PA9、PA10和PC10、PC11引腳相連接，輸出引腳1PPS引腳與ARM處理器(STM32F103RC)的PA12引腳相連接；超聲波傳感器(SRF05模塊)的輸入/輸出雙向引腳TRIGGER INPUT/ECHO2
OUTPUT與ARM處理器(STM32F103RC)的PB7引腳相連接；氣壓計的IC接口SCL、SDA引腳分別與ARM處理器(STM32F103RC)的PB10、PB11引腳相連接；空中無線傳輸模塊(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引腳相連接；微型無線攝像頭的一個輸出端與視頻解碼模塊(LMH1981)的輸入端相連接；視頻解碼模塊(LMH1981)的輸出端HSOUT、OEOUTA與ARM處理器(STM32F103RC)的PB6、PB5相連接，無線攝像頭的另一個輸出端與視頻無線傳輸模塊的輸入端相連接。
[0109] 所述的微型共軸雙旋翼飛行器控制裝置的控制方法，包括如下步驟，如圖10所示：
[0110] 一、裝置初始化；
[0111] 二、采集各路傳感器數(shù)據(jù)
[0112] 依次采集三軸加速度傳感器，三軸磁航向傳感器、三軸角速度陀螺儀、超聲波傳感器、氣壓計、GPS的數(shù)據(jù)：
[0113] (1)將三軸加速度傳感器三個軸向輸出的模擬電壓信號，實驗測量為Vx＝1.1231V，Vy＝1.7741V，Vz＝1.6734V，經(jīng)AD轉(zhuǎn)換成數(shù)字信號，對信號進行濾波，并儲存飛
2 2
行器X、Y、Z軸上的加速度ax、ay、az，實驗測量為ax＝-7.8m/s、ay＝1.52m/s、az＝0.23m/
2
s ；
[0114] (2)讀取三軸磁航向傳感器上
地磁場在三個軸上的數(shù)字磁場分量，并確定飛行器與地磁線所成夾角
[0115] 具體如下：
[0116] 如果Hy＞0，則：
[0117]
[0118] 如果Hy＜0，則：
[0119]
[0120] 如果Hy＝0，Hx＜0則：
[0121]
[0122] 如果Hy＝0，Hx＞0則：
[0123]
[0124] 實驗數(shù)據(jù)為
[0125] (3)將俯仰、翻滾陀螺儀，偏航陀螺儀輸出的模擬電壓信號經(jīng)AD轉(zhuǎn)換成數(shù)字信號，對信號進行濾波，并儲存飛行器俯仰、翻滾、偏航角速度Ωx、Ωy、Ωz，實驗數(shù)據(jù)為飛行器俯仰角速度：0.107rad/s；翻滾角速度：0.013rad/s；偏航角速度：0.361rad/s；
[0126] (4)讀取數(shù)字氣壓計的氣壓值p，并確定絕對高度，方法見下式：
[0127]
[0128] 式中：P0為海平面氣壓，其值為1013.25hPa；H′為絕對高度，單位為米，測量結(jié)果為42m；
[0129] 如果是第一次測量氣壓值，則將H′賦值給外部變量Hs，計算飛行器飛行的相對高度：
[0130]
[0131] 發(fā)射方波，打開接收中斷，用
定時器記錄發(fā)射與接收的時間間隔T(單位為秒)，如果固定時間內(nèi)接收到返回接收信號，則計算相對高度H＝344*T/2，并代替公式(6)計算得到的相對高度，否則說明測量超過超聲波量程，關(guān)閉接收中斷，儲存高度值H，測試實際在低空環(huán)境，所以采用超聲測距測量高度，測量得到T＝0.017s，計算得高度為2.92m；
[0132] (5)確定飛行器的經(jīng)度和緯度；
[0133] 讀取GPS信號，得到飛行器的經(jīng)度E和緯度N，并儲存，如果是第一次得到飛行器的經(jīng)度E和緯度N，則分別賦值給外部變量E0、N0分別作為起飛點坐標，本地經(jīng)緯度測量值為東經(jīng)121°25’50.1″北緯31°13’20.8″；
[0134] 三、確定飛行模式、開始起飛、飛行高度、飛行速度、飛行目標點命令；
[0135] 將步驟二測到的各路傳感器數(shù)據(jù)組合成數(shù)據(jù)包后通過機載Zigbee無線發(fā)送給地面Zigbee部分，并且接收來地面自控制界面通過地面Zigbee發(fā)送過來的控制命令數(shù)據(jù)包，解析數(shù)據(jù)包后得到飛行模式、開始起飛、飛行高度、飛行速度、飛行目標點命令；
[0136] 四、通過相對高度判斷飛行器是否起飛，當相對高度為零，飛行器沒有起飛；當相對高度為正值，飛行器已經(jīng)起飛，如果沒有起飛，轉(zhuǎn)入步驟四；如果已經(jīng)起飛，轉(zhuǎn)入步驟五；
[0137] 五、空中裝置ARM處理器通過地面發(fā)過來的控制命令數(shù)據(jù)包選擇飛行器的飛行模式，把三種飛行模式賦予不同的數(shù)據(jù)，通過與地面發(fā)過來的控制命令數(shù)據(jù)包中相應(yīng)的數(shù)據(jù)進行比較，選擇相應(yīng)的飛行模式；
[0138] 六、計算飛行器控制增量；
[0139] 飛行器的有四個控制通道，分別為上升下降，前進后退，左右平移，左右擺動，飛行器的姿態(tài)的控制可以分解成對這四個通道的控制，各個通道采用下例式計算出控制增量：
[0140] Δu(k)＝kp×ΔE(k)+ki×E(k)+kd[E(k)-2E(k-1)+E(k-2)]
[0141] +k11×Δa(k)+k12×a(k)+a13[a(k)-2a(k-1)+a(k-2)]
[0142] +k21×ΔΩ(k)+k22×Ω(k)+k23[Ω(k)-2Ω(k-1)+Ω(k-2)]
[0143] 式中：kp、k11、k21分別為被控量、加速度、角速度的比例系數(shù)，實驗時得到的值分別為：80、90、30；ki、k12、k22分別為被控量、加速度、角速度的積分系數(shù)，實驗時得到的值分別為：40、40、20；kd、k13、k23分別為被控量、加速度、角速度的微分系數(shù)，實驗時得到的值分別為：45、50、30；E(k)、a(k)、Ω(k)分別為被控量、加速度、角速度的系統(tǒng)偏差，分別計算得：65、0.29、0.0021；ΔE(k)、Δa(k)、ΔΩ(k)分別為被控量、加速度、角速度的偏差變化率，三個偏差分別為35、0.73、0.04；ΔE(k)＝E(k)-E(k-1)，Δa(k)＝a(k)-a(k-1)，ΔΩ(k)＝Ω(k)-Ω(k-1)；k，k-1，k-2分別表示第k次，第k-1次，第k-2次，經(jīng)過計算，控制增量Δu(k)為2347.57；
[0144] 七、舵機和電機的控制
[0145] 通過飛行器上的ARM7處理器控制PWM寄存器控制飛行器的舵機和電機；其中因舵機的轉(zhuǎn)角與控制舵機的脈沖寬度成線性關(guān)系，脈沖寬度又是由處理器內(nèi)部PWM寄存器值來設(shè)置的，通過處理器計算改變PWM寄存器值來控制飛行器左右平移和前進后退；兩個電機形成轉(zhuǎn)速差時會產(chǎn)生旋轉(zhuǎn)力矩通過控制，該力矩可以控制飛行器左右方向擺動，當兩電機同比增速或減速時則能控制飛行器的上升和下降；
[0146] (1)舵機控制
[0147] 左右平移、前進后退運動相對懸停狀態(tài)下兩舵機角度變化相互關(guān)系為：
[0148] 左平移：左舵機角度變化量為+θ左移，右舵機角度變化量為-θ左移[0149] 右平移：左舵機角度變化量為-θ右移，右舵機角度變化量為+θ右移[0150] 前進：左舵機角度變化量為-θ前進，右舵機角度變化量為-θ前進
[0151] 后退：左舵機角度變化量為+θ后退，右舵機角度變化量為+θ后退
[0152] 假設(shè)飛行器需要往X-Y平面的第一象限平移，則飛行器可以分解成前進和右平移，則最終左右舵機分別旋轉(zhuǎn)的角度為：
[0153] θ左舵＝90-θ前進+θ右移
[0154] θ右舵＝90-θ前進-θ右移
[0155] 在采集數(shù)據(jù)的時間點上，程序給出的處理結(jié)果是下螺旋槳前傾12°，分別輸出到兩個控制方向的舵機分別是左舵減小6°，右舵增加6°
[0156] (2)電機控制
[0157] 兩個電機轉(zhuǎn)動方向相反，當兩個電機形成轉(zhuǎn)速差時會產(chǎn)生旋轉(zhuǎn)力矩，該力矩可以控制飛行器左右方向擺動，當向右方向轉(zhuǎn)動的電機轉(zhuǎn)速大于向左方向轉(zhuǎn)動的電機轉(zhuǎn)速時，飛機向右方向擺動；當向左方向轉(zhuǎn)動的電機轉(zhuǎn)速大于向右方向轉(zhuǎn)動的電機轉(zhuǎn)速時，飛機向左方向擺動，兩個電機同比增速，可以控制飛行器上升；兩個電機同比減速減時，可以控制飛行器的下降；
[0158] 電機采用增量式PID控制：
[0159] Δu1(k)＝kp×ΔE(k)+ki×E(k)+kd[E(k)-2E(k-1)+E(k-2)]
[0160] 式中，Kp，Ki，Kd分別是PID控制中比例，積分，微分控制參數(shù)，控制中分別取值為80、50、20；E(k)為系統(tǒng)偏差；E(k)-2E(k-1)+E(k-2)為系統(tǒng)量變化率，在采集數(shù)據(jù)的時間點上，ΔE(k)＝31，E(k)＝46，E(k)-2E(k-1)+E(k-2)＝-24，計算得到Δu1(k)＝4300，得到的值作為PWM脈寬的變化量來控制電機；
[0161] 八、通過GPS信號獲取飛機所在位置，與目的地的經(jīng)緯度進行比較，如果兩者不等，飛機未到達目的地，轉(zhuǎn)入步驟二；如果兩者相等，飛機到達目的地，給定高度逐漸減小，飛行器降落，實驗中測得的飛行位置為東經(jīng)121°25’45.4″北緯31°13’17.9″，與目的地位置東經(jīng)121°25’40″北緯31°13’20″不等，飛機未到達目的地，轉(zhuǎn)入步驟二。
[0162] 飛行模式分為三種：遙控模式、航線設(shè)定模式、自主循跡模式；
[0163] (1)遙控模式
[0164] 定時器輸入捕捉方式計算出遙控信號左右舵機和前后電機對應(yīng)的通道的遙控脈沖寬度，進而處理得到左右舵機控制脈沖寬度和前后電機PWM控制信號的周期和占空比；
[0165] (2)航線設(shè)定模式
[0166] 飛行器飛行的方向(與地磁線的夾角)為：
[0167]
[0168] 飛行器偏移設(shè)定航線的距離為：
[0169]
[0170] 其中E、N分別為本次測得的經(jīng)度和緯度，En、Nn分別為飛行器由步驟(3)得到的下一個目標點的經(jīng)度和緯度，E0、N0分別作為起飛點坐標，
[0171] 飛行器飛行的高度H0和速度V0由步驟三得到；
[0172] (3)自主循跡模式
[0173] 對攝像頭視頻信號進行
數(shù)據(jù)采集，提取白布上黑線在圖像上形成的路徑的左右上下邊界，計算出飛行器偏移路徑的角度和距離，具體步驟如下：
[0174] ①采集圖像數(shù)據(jù)
[0175] 采取了降低圖像分辨率的方法每行采集48個點，每幅圖像采集40行，圖像分辨率為48*40；
[0176] ②計算閾值并二值化圖像
[0177] 圖像采第1場的第1行數(shù)據(jù)用雙峰法計算出該行的閾值，將該圖像中大于等于閾值的點賦值為1，小于閾值的點賦值為0，實現(xiàn)二值化處理，第2行閾值引用第1行的閾值，并實現(xiàn)二值化處理，將所有值為1的點所對應(yīng)的數(shù)據(jù)相加取平均值，將所有值為0的點所對應(yīng)的數(shù)據(jù)相加取平均值，兩個平均值再取平均值便得到第2行的閾值，并與上一行的閾值進行比較，在允許的范圍內(nèi)更新，如此迭代至最后一行，最后一行的閾值則作為下一場第一行的閾值，如此反復(fù)迭代；
[0178] ③判斷每行圖像二值化情況
[0179] 每行圖像從起點到終點進行判斷，如果該行僅一次出現(xiàn)值為“0-1-0”變化，則認為該行圖像二值化情況良好；
[0180] ④搜索路徑的左右邊界、上下邊界
[0181] 從第1行起依次搜索該場圖像的其他行的圖像二值化情況，如果連續(xù)3行是二值化情況良好，則停止搜索，并把該行作為路徑搜索起始行ist，以ist作為起始行向上(行標號增加的方向)和向下(行標號減小的方向)對各行進行路徑邊界的搜索；
[0182] 路徑左邊界向上搜索：
[0183] 將路徑左邊界向上一行延伸，如果上一行該處值為0的點，則向左搜索路徑左邊界，如果搜索超出邊界還沒搜索到則用邊界值代替；如果該處值為1的點，則向右搜索路徑左邊界：
[0184] 路徑右邊界向上搜索：
[0185] 將路徑右邊界向上一行延伸，如果上一行該處值為0的點，則向右搜索路徑右邊界；如果該處值為1的點，則向左搜索路徑右邊界；
[0186] 如果搜索到的路徑邊界出現(xiàn)左路徑邊界值大于右路徑邊界值，左右邊界太寬(為0點個數(shù)大于10個)或左右邊界值之間有值為0的點，則嘗試跳過行，該行的路徑邊界值用前兩行邊界的平均值進行補充，如果向上路徑搜索過程中總跳躍次數(shù)達到3次或搜索達到頂端則結(jié)束搜索，同時可以得到路徑上邊界值b，并計算出上邊界左右兩邊界的中心值B；
[0187] 向下搜索同理可得到路徑下邊界值a，計算下邊界左右兩邊界的中心值A(chǔ)，如果搜索到的有效路徑長度小于3行時，則認為本場圖像路徑提取失敗，如果連續(xù)3場圖像路徑提取失敗則，則將起飛點坐標作為下一個目標點坐標，轉(zhuǎn)至步驟①，返回起飛點；
[0188] ⑤飛行器飛行方向與路徑方向夾角大小為：
[0189]
[0190] 飛行器路徑在圖像上偏離的距離為：
[0191]
[0192] 其中a為路徑下邊界值，A為下邊界左右兩邊界的中心值，b為路徑上邊界值，B為上邊界左右兩邊界的中心值，M為行圖像采集的數(shù)據(jù)個數(shù)。
