肖志紅 魏仕倫

【摘要】 本文提出了一種適用于四旋翼飛行器巡線的控制算法，解決了其他巡線方式的巡線不精確問題。在帶有線性CCD的四旋翼飛行器上開發(fā)和實現(xiàn)了這一巡線算法，并進行多次巡線測試，驗證了該算法的可行性和有效性，且可以增強巡線控制系統(tǒng)的魯棒性。

【關(guān)鍵詞】 四旋翼飛行器 串級PID 巡線

Abstract：This paper proposes a control algorithm which is applicable for the quadrocopter， and solved the inaccurate problem of other patrol algorithms. Developed and realized this patrol algorithm on the aircraft with a linear CCD. We experimented this patrol algorithm for many times and verified the feasibility and validity of the algorithm， and this algorithm can enhance the robustness of the control system of the patrol.

Keywords： Quadrocopter Cascade PID control Patrol algorithm1

一、引言

四旋翼飛行器是近年興起的一種微型飛行器。相對于固定翼飛行器，四旋翼飛行器具有控制靈活、抗干擾能力強、飛行穩(wěn)定的優(yōu)點。在很多領(lǐng)域得到了應(yīng)用，例如：航拍[1]、電力線路檢測[2]、地勢偵查[3]等。因此，近年來四旋翼飛行器成為微型飛行器中的一個重要研究方向。

本文就如何設(shè)計四旋翼飛行器巡線算法進行了研究。整體設(shè)計結(jié)構(gòu)圖如圖（1）所示，采用主頻為72MHz的32位ARM處理器STM32F103作為主控?？紤]到該處理器的處理能力有限，決定采用數(shù)據(jù)處理量較少的線性CCD、超聲波傳感器以及自帶數(shù)字運動處理器（DMP：Digtial Motion Processor）的MPU6050傳感器進行數(shù)據(jù)采集。把采集的數(shù)據(jù)經(jīng)過位置環(huán)和姿態(tài)環(huán)處理，最后將處理好的數(shù)據(jù)進行融合再作為控制信號來控制電機，使四旋翼飛行器能夠精確巡線。

二、獲取四旋翼飛行器巡線所需的數(shù)據(jù)

2.1 角度、角速度、加速度數(shù)據(jù)的獲取

MPU6050是一款集成加速度計、陀螺儀的六軸姿態(tài)傳感器，可以采用I2C總線與主控板進行通信。主控板通過I2C總線讀取MPU6050的加速度和角速度的原始數(shù)據(jù)，然后采用卡爾曼濾波算法對獲取的加速度和角速度的原始數(shù)據(jù)數(shù)據(jù)進行濾波，從而準(zhǔn)確可靠的加速度和角速度數(shù)據(jù)。對于四旋翼飛行器姿態(tài)角的獲取，利用MPU6050的DMP，通過調(diào)用InvenSense公司的運動處理庫來獲取四元數(shù)，然后利用公式（1）將四元數(shù)轉(zhuǎn)化成歐拉角，從而獲取四旋翼飛行器的實時姿態(tài)[4-6]。

式（1）中q0、q1、q2、q3為四元數(shù)，？為俯仰角，θ翻滾角，ψ偏航角。

利用MPU6050的DMP可以非常方便的實現(xiàn)姿態(tài)解算，大大減輕了主控板的運算壓力，使主控板有更多的時間去處理控制算法以及其他任務(wù)。

2.2位置數(shù)據(jù)的獲取

對于位置數(shù)據(jù)的獲取，如圖（2）所示，采用超聲波傳感器來獲取Z軸（高度）數(shù)據(jù)，線性CCD來獲取四旋翼飛行器在X軸上的位置（選用X軸還是Y軸取決于線性CCD的位置，如果線性CCD的位置與MPU6050規(guī)定的X軸垂直那么就是獲取X軸上的位置，反之則是Y軸上的位置，本文默認(rèn)獲取X軸上的位置）。因為四旋翼飛行器巡線時對高度要求較高，所以需要精度高的傳感器。目前主要的高度傳感器有氣壓計和超聲波傳感器，氣壓計的精度為5～10cm，而超聲波的精度為1cm。顯然超聲波具有更高的精度，因此本文采用HC-SR04超聲波傳感器來獲取高度數(shù)據(jù)，使用卡爾曼濾波算法對原始數(shù)據(jù)進行濾波?？紤]到該處理器的處理能力有限，且需要處理多個任務(wù)，所以對于X軸位置數(shù)據(jù)的獲取，采用數(shù)據(jù)處理量較少的線性CCD來獲取四旋翼飛行器在X軸上的位置數(shù)據(jù)即可。

三、姿態(tài)環(huán)控制算法設(shè)計

3.1串級PID控制

當(dāng)四旋翼飛行器正常飛行時，如果突遇外界干擾，可能會使得MPU6050內(nèi)部的加速度傳感器或陀螺儀采集數(shù)據(jù)失真，從而使得獲取的歐拉角錯誤。因此只用角度單環(huán)的情況下，四旋翼飛行器很難在外界干擾的情況下穩(wěn)定飛行[7]。因此姿態(tài)環(huán)可采用串級PID算法進行控制，控制算法結(jié)構(gòu)圖如圖（3）所示，使用角速度作為內(nèi)環(huán)。角速度由陀螺儀采集數(shù)據(jù)輸出，采集值一般不會受到外界干擾，且角速度變化靈敏，當(dāng)受外力干擾時恢復(fù)迅速，可有效避免外界干擾造成的影響，增強了系統(tǒng)的魯棒性。

式（4）為角度環(huán)PID計算公式，式（5）為角速度環(huán)PID計算公式。）（_kPidOutAngle為角度環(huán)輸出，）（_tankPidOutc ePals為角速度環(huán)PID輸出，e（t）=期望角度-實際角度，e（t）=）（_kPidOutAngle-實際角速度。

四、巡線算法設(shè)計

4.1巡線算法設(shè)計

四旋翼飛行器巡線和定點類似，其巡線相當(dāng)于在X，Z軸方向上的定點。設(shè)計之初想把定點算法的設(shè)計思路應(yīng)用到巡線上，通過翻看大量關(guān)于四旋翼飛行器定點的文獻[8-9]發(fā)現(xiàn)傳統(tǒng)的四旋翼飛行器定點算法的設(shè)計思路都如圖（4）所示的控制算法結(jié)構(gòu)，將位置環(huán)的輸出作為角度環(huán)的輸入。把這一設(shè)計思路應(yīng)用到巡線上時發(fā)現(xiàn)四軸飛行器雖然能夠巡線，但是巡線不精確，四旋翼飛行器會在黑線附近大幅度等幅震蕩，達(dá)不到巡線的要求。

經(jīng)多次試驗并仔細(xì)分析，發(fā)現(xiàn)問題在于角度的變化對于位置偏差的修正過慢。當(dāng)位置發(fā)生變化時，角度的變化不能迅速對位置的偏差進行修正，所以導(dǎo)致傳統(tǒng)四旋翼飛行器定點算法應(yīng)用到巡線上造成巡線不精確的問題。本文對此將巡線控制算法的結(jié)構(gòu)改成如圖（5）所示的結(jié)構(gòu)，不再將位置環(huán)和姿態(tài)環(huán)進行串聯(lián)，而是將位置環(huán)和姿態(tài)環(huán)采用并聯(lián)的形式，將位置環(huán)的輸出直接作為控制量施加在電機上。采用這種巡線算法結(jié)構(gòu)大大提高了四旋翼對位置偏差的修正速度。

4.2 測試及分析

以X軸位置測試為例。本次測試位置環(huán)PID控制頻率為50HZ，期望X軸位置為0。將采集到的四旋翼飛行器X軸位置數(shù)據(jù)通過藍(lán)牙發(fā)送到PC機上，再通過上位機把接收到的數(shù)據(jù)繪制成波形圖。波形圖Y軸的單位為0.4cm/格，X軸的單位為200ms/格。由于線性CCD的測量范圍有限，能夠測量的最大范圍約為±30cm，超過這一范圍的位置在圖中表現(xiàn)為Y=±64。圖（6）為定點算法應(yīng)用到巡線上的位置波形圖，從該圖中可看出四旋翼飛行器的位置在0附近大幅度震蕩，不能在一個小范圍內(nèi)穩(wěn)定。圖（7）為本文改進的控制結(jié)構(gòu)的位置波形圖，從該圖中可看出四軸飛行器的位置經(jīng)過一大波震蕩后，近似歸為期望值0，且調(diào)節(jié)時間很短，能夠達(dá)到我們的穩(wěn)準(zhǔn)快要求，從而實現(xiàn)精確巡線。Z軸位置測試結(jié)果類似。

五、結(jié)束語

本文主要研究了四旋翼飛行器的巡線算法，參考傳統(tǒng)四旋翼飛行器定點算法的設(shè)計思路，發(fā)現(xiàn)了其在巡線應(yīng)用上的問題，并提出了一種適用于四旋翼飛行器巡線的算法，大大提高了四旋翼飛行器的巡線精度，且增強了系統(tǒng)的魯棒性。

參 考 文 獻

[1]黃鵬宇，曾路榮，楊川，彭遠(yuǎn)行，余成波.一種新型災(zāi)難救援四軸航拍飛行器設(shè)計[J].四川兵工學(xué)報，2014，06：124-128.

[2]李勇.無人飛行器在特高壓交流輸電線路巡視中的應(yīng)用模式研究[D].華北電力大學(xué)，2014.

[3]錢杰.四旋翼飛行器整體設(shè)計與航跡跟蹤地面站軟件設(shè)計[D].電子科技大學(xué)，2015.

[4]何瑜.四軸飛行器控制系統(tǒng)設(shè)計及其姿態(tài)解算和控制算法研究[D].電子科技大學(xué)，2015.

[5]王金紅，成怡，錢紅亮，謝飛.四軸飛行器姿態(tài)監(jiān)控系統(tǒng)設(shè)計[J].宇航計測技術(shù)，2014，06：9-13.

[6]朱陽，王偉成，王民慧.基于MPU9150的四軸飛行器姿態(tài)控制系統(tǒng)設(shè)計[J].制造業(yè)自動化，2015，10：46-48+71.

[7]陸偉男，蔡啟仲，李剛，鄭力.基于四軸飛行器的雙閉環(huán)PID控制[J].科學(xué)技術(shù)與工程，2014，33：127-131.

[8]孫驊.基于GPS與AHRS的四軸飛行器懸停算法研究[J].機械科學(xué)與技術(shù)，2013，04：481-487.

[9]白志君.四旋翼無人機室內(nèi)自主導(dǎo)航系統(tǒng)的研究與實現(xiàn)[D].廈門大學(xué)，2014.