四旋翼飛行器巡線算法的研究

肖志紅 魏仕倫

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

【關(guān)鍵詞】 四旋翼飛行器 串級(jí)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

一、引言

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

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

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

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

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

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

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

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

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

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

3.1串級(jí)PID控制

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

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

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

4.1巡線算法設(shè)計(jì)

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

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

4.2 測(cè)試及分析

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

五、結(jié)束語(yǔ)

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

參 考 文 獻(xiàn)

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

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

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

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

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

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

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

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

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