本發(fā)明屬于無人機(jī)
技術(shù)領(lǐng)域：
，尤其涉及一種航向信息采集系統(tǒng)。
背景技術(shù)：
：無人機(jī)現(xiàn)有的航向檢測與控制，主要有如下幾種方法：(1)衛(wèi)星定位導(dǎo)航，采用衛(wèi)星接收機(jī)，接收超過6顆以上的衛(wèi)星信號，可運(yùn)算出無人機(jī)的三維坐標(biāo)位置及航向等飛行參數(shù)。這種方法對通信衛(wèi)星高度依賴，當(dāng)無人機(jī)在崇山峻嶺、密集建筑群等低空飛行以及在室內(nèi)飛行時，會出現(xiàn)信號被遮擋或被中斷；而且開放給民用的衛(wèi)星測定數(shù)據(jù)，誤差較大。(2)慣性導(dǎo)航，采用高精度的MEMS微機(jī)電陀螺儀采集三軸角速率、三軸加速度及三軸地球磁力信號，共計九自由度參數(shù)，遞推解算出俯仰、橫滾及航向角度。這種方法要用到地球磁力的采集傳感器，該磁力信號微弱、易受電磁信息干擾或物體屏蔽，造成所計算得出的航向信息精度及可靠性不高；而且計算誤差會隨著時間積累，而引起精度下降。(3)天文導(dǎo)航，根據(jù)天體來測定無人機(jī)位置和航向，但造價昂貴，檢測與計算系統(tǒng)十分復(fù)雜，很難應(yīng)用在小型無人機(jī)系統(tǒng)中。(4)多普勒導(dǎo)航，利用多普勒效應(yīng)進(jìn)行自主式位置及航向推算，能運(yùn)算出高精度的位置、速度和偏流角等參數(shù)，不過，定位誤差隨著時間推移會增大，且當(dāng)飛機(jī)姿態(tài)超過限度，多普勒雷達(dá)因收不到回波而不能工作，會產(chǎn)生導(dǎo)航失效。(5)視覺導(dǎo)航，根據(jù)圖像中每個像素點的灰度值，挖掘出航向參數(shù)值，這種技術(shù)涉及圖像識別及模式匹配等技術(shù)，要求傳感器精度高，圖像處理速度快；而且造價很高，目前還沒有見到成熟和推廣的應(yīng)用。上述5中方法中，天文導(dǎo)航和多普勒導(dǎo)航，體積大、重量重，很難用在小型無人飛機(jī)上；視覺導(dǎo)航要求運(yùn)算速度快、技術(shù)難度大、造價高，也難以用于小型無人機(jī)上。低空及室內(nèi)飛行的無人機(jī)航向控制，常用衛(wèi)星定位導(dǎo)航結(jié)合慣性導(dǎo)航的綜合導(dǎo)航模式。這幾種導(dǎo)航方式需要用到各種各樣的陀螺儀檢測裝置，如：液浮陀螺儀，撓性陀螺儀，靜電陀螺儀，激光陀螺儀，光纖陀螺儀，微機(jī)電陀螺儀，等等。低空及室內(nèi)飛行的無人機(jī)的航向信息，常用微機(jī)電MEMS陀螺儀，該陀螺儀主要由微慣性測量單元和數(shù)字磁羅盤組成，微慣性測量單元可以檢測出三軸角速率和三軸加速度信息，數(shù)字磁羅盤可以檢測出三軸地球磁力信息。三軸角速率和三軸加速度參數(shù)經(jīng)過諸如求解四元數(shù)微分方程的四階龍格-庫塔方法，或采用當(dāng)?shù)氐乩淼膶?dǎo)航坐標(biāo)系n與無人機(jī)載體坐標(biāo)系b之間的方向余弦變換等算法，即可遞推解算出無人機(jī)的俯仰、橫滾角度，結(jié)合三軸磁力參數(shù)即可解算出航向角度信息，其結(jié)構(gòu)框圖如圖1所示。然而，地磁信息相對微弱，且易受電磁信息干擾或大型物體屏蔽，造成檢測出來的航向信息參數(shù)精度不高，且會隨著時間的推移，引起累積誤差，所以信號的可靠性也受到制約。光流，是對產(chǎn)生視覺感知的光信號的一種運(yùn)動規(guī)律的發(fā)掘，主要研究相鄰兩幅圖片之間具有相同或相似灰度值的像素點之間的對應(yīng)關(guān)系，它不受磁場信息的干擾，用之替代數(shù)字磁羅盤，能夠提高航向測量的精度及可靠性。技術(shù)實現(xiàn)要素：：為解決當(dāng)前無人機(jī)導(dǎo)航中存在的問題，本發(fā)明提供了一種航向信息采集系統(tǒng)。本發(fā)明是采用如下技術(shù)方案實現(xiàn)的：一種航向信息采集系統(tǒng)，包括主控制器，所述主控制器連接有CMOS圖像傳感器、六自由度陀螺儀和測距傳感器：CMOS圖像傳感器直接連接主控制器，CMOS圖像傳感器通過幀捕獲器獲取圖像數(shù)據(jù)，經(jīng)過實時處理，存入主控制器中，通過主控制計算出光流值；六自由度陀螺儀通過SPI總線與主控芯片連接，六自由度陀螺儀采集三軸角速率及三軸加速度信息，通過主控制器解算出姿態(tài)信息；測距傳感器通過串口與主控芯片連接，測距傳感器測出鏡頭與被測圖像之間的距離；主控制器通過I2C總線連接有飛控系統(tǒng)；主控制器計算并確定無人機(jī)的航向信息，通過I2C總線，傳送到飛控系統(tǒng)中，實現(xiàn)航向控制。進(jìn)一步的改進(jìn)，CMOS圖像傳感器型號為MT9V034、六自由度陀螺儀的型號為MPU6000、測距傳感器MB1240，主控制器包括DMA直接存儲器、存儲器和CORTEX-M4微控制器；CORTEX-M4微控制器的主控芯片為STM32M427；所述CMOS圖像傳感器、六自由度陀螺儀和測距傳感器采用DMA讀取模式，將采集到的信息存儲于主控制器中。進(jìn)一步的改進(jìn)，所述主控制器解算出姿態(tài)信息的方法為：對六自由度陀螺儀及其SPI通信協(xié)議進(jìn)行初始化，設(shè)置陀螺儀數(shù)據(jù)讀寫函數(shù)初值，確定傳輸?shù)刂罚M(jìn)行數(shù)據(jù)傳輸，數(shù)據(jù)傳輸完畢再進(jìn)行數(shù)據(jù)轉(zhuǎn)換處理，實現(xiàn)三軸角速率及三軸加速度信息的一次采集；將陀螺儀測量出的重力加速度fg及重力加速度在無人機(jī)本體的X軸上的分量xg、Y軸上的分量yg及Z軸上的分量zg代入以下公式，解出無人機(jī)本體的俯仰角θ和橫滾角φ；進(jìn)一步的改進(jìn)，所述主控制器計算光流值的方法為：CMOS圖像傳感器朝向地面固定在無人機(jī)的載體上，通過無人機(jī)載體的俯仰角θ和橫滾角φ確定姿態(tài)；CMOS圖像傳感器輸出的圖像數(shù)據(jù)包括：幀同步信號、行同步信號、像素輸出時鐘和圖像數(shù)據(jù)；步驟1)確定連續(xù)2幀圖片的最佳匹配塊步驟2)計算最佳匹配塊中的亞像素級像素偏移量步驟3)計算在X和Y軸上的光流值分量。進(jìn)一步的改進(jìn)，所述主控制器確定連續(xù)2幀圖片的最佳匹配塊的方法為對圖像數(shù)據(jù)進(jìn)行灰度變換，再對灰度圖像進(jìn)行快速均值濾波，再進(jìn)行銳化處理，得到濾波銳化后的像素圖片；使用式(4)計算SAD值，確定出連續(xù)2幀濾波銳化后的像素圖片的最佳匹配塊；SAD(x,y)=Σi=1nΣj=1n|S(i,j)-R(i+x,j+y)|,-d≤x,y≤d---(4)]]>式(4)式中：x、y分別為參考幀中的預(yù)測塊與當(dāng)前幀中的當(dāng)前塊在水平、垂直方向的偏移值，i、j分別為當(dāng)前塊內(nèi)某像素的水平、垂直坐標(biāo)，S為當(dāng)前塊內(nèi)某像素的亮度值，R為預(yù)測塊內(nèi)對應(yīng)像素的亮度值，d為單方向最大搜索距離，SAD值即為當(dāng)前塊與預(yù)測塊之間每個像素點亮度差值的絕對值之和。進(jìn)一步的改進(jìn)，所述計算最佳匹配塊中的亞像素級像素偏移量的方法為：非整像素點(x，y)在鄰近的4個像素點分別為(i，j)、(i+1，j)、(i，j+1)、(i+1，j+1)，將各像素點的灰度值的雙線性插值代入式(5)：g(x，y)＝A00+A10α+A01β+A11αβ(5)其中：A00＝g(i，j)，A10＝g(i+1，j)-g(i，j)，A01＝g(i，j+1)-g(i，j)，A11＝g(i+1，j+1)+g(i，j)-g(i，j+1)-g(i+1，j)，α、β分別為點(x，y)到點(i，j)的距離的水平、垂直分量。解出的α、β值即為最佳匹配塊中亞像素級的像素偏移量。進(jìn)一步的改進(jìn)，所述計算計算X和Y軸上的光流值分量的方法為：用六自由度陀螺儀測得的三軸角速度，按式(7)計算得到圖像平面在X軸方向的光流分量vx、Y軸方向的光流分量vy：vx=-Txfz-ωyf+ωzyvy=-Tyfz-ωxf+ωzx---(7)]]>式(7)中，x、y分別為X、Y軸方向的像素偏移量；Tx、Ty為圖像平面在X、Y軸方向的平移變換系數(shù)；z為鏡頭與被測圖像之間的距離，f為鏡頭的焦距；ωx為X軸方向上的旋轉(zhuǎn)角速率、ωy及為Y軸方向上的旋轉(zhuǎn)角速率、ωz為Z軸方向上的旋轉(zhuǎn)角速率。進(jìn)一步的改進(jìn)，所述主控制器確定航向的方法為：1)航向角計算：先將無人機(jī)平放在地面上，確定正北朝向，進(jìn)行初始化設(shè)置；從初始化開始，采用式(8)和(9)不斷運(yùn)算相鄰兩幅圖像之間的航向增量角度Δψi，tanΔψb=vyvx---(8)]]>Δψi＝Δψbcosφcosθ(9)其中Δψb為圖像平面的航向增量角、vx為X軸方向的光流分量、vy為Y軸方向的光流分量；θ為姿態(tài)俯仰角、φ為橫滾角，Δψb圖像平面的航向增量角；通過遞推方法，不斷累加航向增量角度，采用式(10)算得瞬時航向角度值ψ：ψ=Σi=0nΔψi---(10)]]>2)航向控制：將瞬時航向角ψ與航向的指令角ψn進(jìn)行比較，得出誤差值Δψ，輸入到PID控制器的誤差輸入端，即可通過比例、積分、微分三個調(diào)節(jié)環(huán)節(jié)，運(yùn)算輸出無人機(jī)的舵機(jī)控制信號，促使無人機(jī)飛行的瞬時航向角能夠跟隨指令航向角，從而實現(xiàn)航向控制。與現(xiàn)有技術(shù)相比，本發(fā)明的優(yōu)點在于：(1)圖像檢測信息強(qiáng)且不易產(chǎn)生漂移，而地球磁場信息相對非常微弱且容易產(chǎn)生漂移，因此實時檢測航向信息，不會形成累積誤差，精度較高；(2)圖像檢測信息不受磁場干擾，不受大型物體屏蔽，而數(shù)字磁羅盤信息則相反，因此，本發(fā)明檢測出的航向角度值，抗干擾性能強(qiáng)；(3)圖像檢測信息比微機(jī)電MEMS檢測的信息較穩(wěn)定可靠，航向測控的可靠性高。(4)與采用數(shù)字磁羅盤檢測方法相比，本方法可以實現(xiàn)在磁力傳感器失效的情況下，不間斷地對小型無人機(jī)進(jìn)行航向角的測控。(5)本發(fā)明采用圖像傳感與距離傳感相結(jié)合的模型，實現(xiàn)光流的獲取，與Lucas-Kanade算法相比，計算量小且可以快速的獲取光流信息，可以在嵌入式系統(tǒng)上快速運(yùn)行。附圖說明圖1為基于MEMS陀螺的航向姿態(tài)測量系統(tǒng)結(jié)構(gòu)框圖；圖2為本發(fā)明的航向信息采集總體結(jié)構(gòu)框圖圖3為六自由度信息采集電路圖圖4為六自由度陀螺儀信息采集流程圖圖5為光流圖像傳感器電路圖圖6為圖像傳感器數(shù)據(jù)采集流程圖圖7為無人機(jī)航向角遞推運(yùn)算流程圖具體實施方式如圖2所示，航向信息采集系統(tǒng)中的傳感器采用了CMOS圖像傳感器MT9V034、六自由度陀螺儀MPU6000及測距傳感器MB1240，這3種傳感器分別與嵌入式微控制器CORTEX-M4主控芯片STM32M427連接，各自實現(xiàn)如下功能：1)CMOS圖像傳感器直接連接主控微處理器，通過幀捕獲器獲取圖像數(shù)據(jù)，經(jīng)過實時處理，存儲于主控芯片的存儲器中，主控芯片啟動流模塊，利用算法選擇出連續(xù)兩幀的最佳匹配塊，計算最佳匹配塊的亞像素級像素偏移量，計算出兩幀之間的光流值，再結(jié)合其他的傳感信息，即可解算出航向信息；2)六自由度陀螺儀MPU6000采集三軸角速率及三軸加速度信息通過SPI總線與主控芯片連接，經(jīng)過主控芯片的遞推運(yùn)算，可解算出系統(tǒng)的姿態(tài)信息，用于航向的求解；3)測距傳感器MB1240檢測鏡頭與被測圖像之間的距離，通過串口與主控芯片連接，供主控芯片求解航向信息。這3種傳感器信息采用DMA讀取模式，能提高存取速度，將采集到的信息直接存儲于主控芯片的存儲器中。主控芯片處理運(yùn)算所采集的信息，求得光流值及航向等信息，通過I2C總線，傳送到飛控系統(tǒng)中，用于實現(xiàn)航向控制。(2)六自由度陀螺信息采集。采用六自由度MEMS陀螺MPU6000，檢測無人機(jī)的三軸角速率及三軸加速度信息，該六自由度信息檢測電路如圖3所示。六自由度陀螺儀MPU6000與主控芯片之間采取SPI通信進(jìn)行數(shù)據(jù)采集，主控芯片采用CORTEX-M4芯片STM32F427，其信息采集的總體結(jié)構(gòu)如圖2所示。首先對該陀螺儀及其SPI通信協(xié)議進(jìn)行初始化，設(shè)置陀螺儀數(shù)據(jù)讀寫函數(shù)初值，接著確定傳輸?shù)刂?，進(jìn)行數(shù)據(jù)傳輸，待數(shù)據(jù)傳輸完畢再進(jìn)行數(shù)據(jù)轉(zhuǎn)換處理，實現(xiàn)三軸角速率及三軸加速度信息的一次采集，程序流程圖如圖4所示。(3)陀螺的姿態(tài)角度的遞推運(yùn)算。把如圖4所示采集到的六自由度信息，通過當(dāng)?shù)氐乩淼膶?dǎo)航坐標(biāo)系與無人機(jī)載體坐標(biāo)系之間的變換關(guān)系，采用方向余弦矩陣，進(jìn)行相互轉(zhuǎn)換，并遞推運(yùn)算出無人機(jī)姿態(tài)角度，即：無人機(jī)的俯仰角度及橫滾角度。導(dǎo)航坐標(biāo)系相對于載體坐標(biāo)系之間存在如下式(1)所示的變換關(guān)系。Cnb=cosψcosθsinψcosθ-sinθcosψsinθsinφ-sinψcosφsinψsinθsinφ+cosψcosφcosθsinφcosψsinθcosφ+sinψsinφsinψsinθcosφ-cosψsinφcosθcosφ---(1)]]>式(1)中：為導(dǎo)航坐標(biāo)系n相對于載體坐標(biāo)系b的方向余弦，θ、φ及ψ分別是無人機(jī)本體的俯仰角、橫滾角及航向角。由陀螺儀MPU6000測量出重力加速度fg在無人機(jī)本體的X、Y及Z軸上的分量為分別為xg、yg、zg，則根據(jù)公式(1)所示的導(dǎo)航坐標(biāo)系n相對于載體坐標(biāo)系b的方向余弦的變換關(guān)系，有xgygzg=Cnb00fg---(2)]]>把公式(1)中的代入公式(2)中去，可得出如下等式(3)。-sinθ·fg=xgcosθsinφ·fg=ygcosθcosφ·fg=zg---(3)]]>求解等式(3)，可解出無人機(jī)本體的俯仰角θ和橫滾角φ。(4)計算CMOS圖像傳感器的光流值。本發(fā)明采用CMOS圖像傳感器PX4FLOW，配有CMOS圖像傳感器MT9V034，擁有原生752×480像素分辨率，計算光流的過程中采用4倍分級和剪裁算法。在白天的室外工作環(huán)境中，計算速度能達(dá)到250Hz，具備很高的感光度；而在室內(nèi)或者暗光的工作環(huán)境中，計算速度也能達(dá)到120Hz，而無需額外照明。把CMOS圖像傳感器，固定在無人機(jī)的載體上，正面朝下，對著地面，通過無人機(jī)載體的俯仰角θ和橫滾角φ確定其姿態(tài)。CMOS圖像傳感器MT9V034的信息采集如圖5所示，圖像信息通過I2C通信接口傳送到主控芯片STM32F427中，之間的連接如圖2的信息采集總體結(jié)構(gòu)所示。該圖像傳感器具有36萬像素，采用整幀曝光，同一時間將整個傳感器暴露出來，在對運(yùn)行中的物體進(jìn)行采集。CMOS圖像傳感器輸出的圖像數(shù)據(jù)包括：幀同步信號、行同步信號、像素輸出時鐘、圖像數(shù)據(jù)。輸出的圖像數(shù)據(jù)轉(zhuǎn)化為二維的坐標(biāo)偏移量并以像素的形式存儲在主控芯片STM32F427的存儲器中。圖像傳感器數(shù)據(jù)采集與處理流程如圖6所示，首先對圖像傳感器及I2C總線進(jìn)行配置及初始化，設(shè)置圖像數(shù)據(jù)讀寫函數(shù)初值，接著確定傳輸?shù)刂?，進(jìn)行數(shù)據(jù)傳輸，發(fā)送讀寫地址接收1幀圖像信息，待1幀的圖像數(shù)據(jù)接收完畢，再進(jìn)行數(shù)據(jù)轉(zhuǎn)換處理，并將獲得的圖像信息傳輸?shù)街骺匦酒拇鎯ζ髦?，接著可以進(jìn)行下一幅圖像信息的采集工作。由于相鄰的兩幅圖像存在相同的特征，這些特征點的位置變化信息采用光流算法，可判斷出物體水平的平均運(yùn)動，這個運(yùn)動結(jié)果可被轉(zhuǎn)化為二維的坐標(biāo)偏移量，并以像素形式存儲在特定的寄存器中，實現(xiàn)對物體運(yùn)動的檢測。為了保證光流計算的運(yùn)算快速性和準(zhǔn)確性，在進(jìn)行光流計算之前，需要對所采集得到的圖像進(jìn)行數(shù)據(jù)轉(zhuǎn)換，采用的方法是：先對圖像數(shù)據(jù)進(jìn)行灰度變換，再對灰度圖像進(jìn)行快速均值濾波，得到比較少的噪聲灰度圖，然后對濾波后的圖像進(jìn)行銳化處理，使圖像的邊緣信息更加明顯，如此可得到經(jīng)過濾波銳化后的像素圖片，以便對之進(jìn)行后續(xù)的匹配算法及光流計算。主控芯片STM32F427中集成有硬件FPU浮點運(yùn)算單元，擁有M4擴(kuò)展指令集和浮點運(yùn)算，可用于光流計算。光流計算采用塊匹配算法，其原理是將視頻圖像的每一幀分成多個不重疊的塊，認(rèn)為塊內(nèi)所有像素位移矢量相同，選擇當(dāng)前幀的某一塊，依據(jù)一定的匹配原則，在參考幀的給定范圍搜索最為相似的塊，根據(jù)相鄰兩塊的相對位置計算出運(yùn)動位移，即每個像素點的運(yùn)動矢量。光流計算采用最小絕對誤差之和(SumofAbsoluteDifferences,SAD)的塊匹配算法，其光流檢測步驟如下。1)確定最佳匹配塊。采用SAD塊匹配算法，其計算公式如式(4)所示，用于確定出連續(xù)2幀圖片的最佳匹配塊。SAD(x,y)=Σi=1nΣj=1n|S(i,j)-R(i+x,j+y)|,-d≤x,y≤d---(4)]]>式(4)式中：x、y分別為參考幀中的預(yù)測塊與當(dāng)前幀中的當(dāng)前塊在水平、垂直方向的偏移值，i、j分別為當(dāng)前塊內(nèi)某像素的水平、垂直坐標(biāo)，S為當(dāng)前塊內(nèi)某像素的亮度值，R為預(yù)測塊內(nèi)對應(yīng)像素的亮度值，d為單方向最大搜索距離，SAD值即為當(dāng)前塊與預(yù)測塊之間每個像素點亮度差值的絕對值之和。兩塊之間的SAD值越小，說明兩塊之間的相似度越大，反之，兩塊之間的SAD值越大，則相似度越小。選擇搜索區(qū)域內(nèi)相似度最大的匹配塊作為最佳匹配塊。2)最佳匹配塊中的亞像素級像素偏移量計算。采用雙線性插值法將最佳匹配塊中的像素進(jìn)行亞像素細(xì)化，假設(shè)非整像素點(x，y)在鄰近的4個像素點分別為(i，j)、(i+1，j)、(i，j+1)、(i+1，j+1)，則，亞像素點(x，y)上灰度值的雙線性插值可用如下式(5)來計算得出。g(x，y)＝A00+A10α+A01β+A11αβ(5)式(5)中，A00＝g(i，j)，A10＝g(i+1，j)-g(i，j)，A01＝g(i，j+1)-g(i，j)，A11＝g(i+1，j+1)+g(i，j)-g(i，j+1)-g(i+1，j)，α、β分別為點(x，y)到點(i，j)的距離的水平、垂直分量。根據(jù)式(5)，即可計算出最佳匹配塊中亞像素級的像素偏移量。3)光流計算。由測距傳感器測出的鏡頭與景物之間的距離z及鏡頭的焦距f，可求出圖像平面在X、Y軸方向的光流分量如式(6)所示。vx=-Txfzvy=-Tyfz---(6)]]>式(6)中，Tx、Ty為圖像平面在X、Y軸方向的平移變換系數(shù)，z為鏡頭與景物之間的距離。由于鏡頭存在X、Y及Z三軸方向上的旋轉(zhuǎn)運(yùn)動，這三個方向上的旋轉(zhuǎn)角速率ωx、ωy及ωz會產(chǎn)生附加光流量值，需要進(jìn)行旋轉(zhuǎn)補(bǔ)償，補(bǔ)償后可得圖像平面在X、Y軸方向的光流分量vx、vy如式(7)所示。vx=-Txfz-ωyf+ωzyvy=-Tyfz-ωxf+ωzx---(7)]]>式(7)中，x、y分別為X、Y軸方向的像素偏移量。(5)航向計算。1)航向增量角度計算。根據(jù)式(7)計算得出的圖像平面在X、Y軸方向的光流分量vx、vy值，可以計算出圖像平面的航向增量角Δψb如式(8)所示。tanΔψb=vyvx---(8)]]>即：Δψb=arctanvyvx]]>考慮到拍照時刻，鏡頭隨著無人機(jī)本體，具有姿態(tài)俯仰角θ及橫滾角φ，因此需要把ψb經(jīng)過方向余弦變換，轉(zhuǎn)換到當(dāng)?shù)氐乩淼膶?dǎo)航坐標(biāo)系n的平面上，獲得航向增量角值Δψ，如下式(9)所示。Δψi＝Δψbcosφcosθ(9)2)航向角計算。從航向角等于0開始進(jìn)行初始化操作，把無人機(jī)平放在地面上，確定正北朝向，進(jìn)行初始化設(shè)置，此時復(fù)位光流的初始值是vx0=0vy0=0]]>此時，初始航向角ψ0＝0。從初始化開始，采用式(9)不斷運(yùn)算相鄰兩幅圖像之間的航向增量角度Δψi，通過遞推方法，不斷累加航向增量角度，即可算得瞬時航向角度值ψ如式(10)所示。ψ=Σi=0nΔψi---(10)]]>其迭代流程如圖7所示。(6)航向控制。如圖7所示迭代計算得出的無人機(jī)的瞬時航向角ψ之后，將之與航向的指令角ψn進(jìn)行比較，得出誤差值Δψ，輸入到PID控制器的誤差輸入端，即可通過比例、積分、微分三個調(diào)節(jié)環(huán)節(jié)，運(yùn)算輸出無人機(jī)的舵機(jī)控制信號，促使無人機(jī)飛行的瞬時航向角能夠跟隨指令航向角，從而實現(xiàn)航向控制。以上實例的說明只是用于幫助理解本發(fā)明的核心思想；同時，對于本領(lǐng)域的一般技術(shù)人員，依據(jù)本發(fā)明的思想，在具體實施方式及應(yīng)用范圍上均會有改變之處，綜上所述，本說明書內(nèi)容不應(yīng)理解為對本發(fā)明的限制。當(dāng)前第1頁1 2 3