微型仿生撲翼機(jī)控制器設(shè)計(jì)

賀雪晨，周超英，汪超

（哈爾濱工業(yè)大學(xué)深圳研究生院機(jī)電工程及自動(dòng)化學(xué)院，廣東深圳518055）

仿生撲翼飛行器是一種模仿鳥類或昆蟲飛行方式的新型仿生飛行器[1]。與采用傳統(tǒng)飛行方式的飛行器相比，仿生撲翼飛行器具有體積小、重量輕、隱蔽性好和靈活性高等優(yōu)點(diǎn)[2]，特別是在小尺度、低雷諾數(shù)條件下，相較于其他氣動(dòng)布局的飛行器有著難以比擬的優(yōu)勢(shì)，在民用和軍事領(lǐng)域均有廣泛的應(yīng)用前景。因此，近幾十年來仿生撲翼飛行器的發(fā)展十分迅速，國(guó)內(nèi)外的科研工作者對(duì)這一領(lǐng)域的研究也十分重視。

隨著撲翼飛行器的小型化，勢(shì)必要求其控制器做到盡量微型化。文中設(shè)計(jì)一款微型控制器，并可解算出撲翼機(jī)的姿態(tài)角，采用PD控制器控制尾翼舵機(jī)，使撲翼機(jī)具有良好的飛行穩(wěn)定性，滿足微型撲翼機(jī)的控制要求。

1 硬件設(shè)計(jì)部分

1.1 總體方案

微型撲翼控制器可利用各傳感器的數(shù)據(jù)解算出撲翼機(jī)的當(dāng)前姿態(tài)，并解析接收機(jī)信號(hào)得到控制量，控制執(zhí)行機(jī)構(gòu)。主要由以下幾個(gè)部分組成：

1）微處理器：撲翼控制器的核心單元，用來解算姿態(tài)角、解析遙控信號(hào)、控制執(zhí)行機(jī)構(gòu)；

2）姿態(tài)傳感器：包含加速度計(jì)、磁力計(jì)和陀螺儀，提供姿態(tài)解算的原始數(shù)據(jù)；

3）通訊模塊：遙控器通過接收機(jī)與控制器進(jìn)行信息通信。

1.2 電路設(shè)計(jì)

MPU6050集成了加速度計(jì)和陀螺儀兩個(gè)傳感器，磁力計(jì)則選用HMC5883L，氣壓計(jì)BMP180作為拓展功能，三個(gè)傳感器均采用I2C總線與微處理器進(jìn)行數(shù)據(jù)傳送。

圖1 控制器框圖

微處理器的SCL和SDA與MPU6050的I2C接口相連，而MPU6050的AUX_CL和AUX_DA直接連接磁力計(jì)HMC5883L，此時(shí)可將MPU6050的AUX接口設(shè)置成直連式，微處理器便可直接訪問HMC5883L，兩個(gè)傳感器共同組成9軸傳感器，BMP180也可以通過I2C總線直接與微處理器相連。姿態(tài)傳感器電路如圖2所示。

圖3為文中所設(shè)計(jì)的控制器：

圖2 姿態(tài)傳感器電路圖

圖3 控制器

2 軟件設(shè)計(jì)部分

在上述已完成的硬件基礎(chǔ)上進(jìn)行軟件設(shè)計(jì)，軟件的主要功能為：將接收到的遙控?cái)?shù)據(jù)解析為電機(jī)控制量和目標(biāo)姿態(tài)角，其中電機(jī)控制量直接轉(zhuǎn)換成對(duì)應(yīng)占空比的PWM信號(hào)輸出給電機(jī)驅(qū)動(dòng)模塊，讀取各傳感器的數(shù)據(jù)并進(jìn)行姿態(tài)解算，得到飛行器當(dāng)前的姿態(tài)數(shù)據(jù)，將實(shí)際姿態(tài)角與目標(biāo)姿態(tài)角的差值輸入給PD控制器，最終得到調(diào)節(jié)后的控制量來控制尾翼舵機(jī)?？驁D如圖4所示。

圖4 軟件設(shè)計(jì)框圖

2.1 姿態(tài)解算

由于飛行器飛行高度低，飛行速度小，可忽略地球自轉(zhuǎn)等因素的影響[3]。在空間內(nèi)僅定義兩個(gè)不同的坐標(biāo)系，分別為以載體旋轉(zhuǎn)中心為原點(diǎn)的參考坐標(biāo)系OnOn和以飛行器載體中心為原點(diǎn)的機(jī)體坐標(biāo)系Ob。

飛行器在參考坐標(biāo)系中的姿態(tài)一般用俯仰角θ、偏航角ψ、滾轉(zhuǎn)角?來表示。本文的計(jì)算過程采用四元數(shù)算法，各軸角的表示采用歐拉角。在計(jì)算過程中利用修正漂移后的角速度求出四元數(shù)，將四元數(shù)轉(zhuǎn)換成方向余弦矩陣中的某幾個(gè)元素，再利用方向余弦矩陣與歐拉角的數(shù)學(xué)關(guān)系計(jì)算出姿態(tài)角，從而實(shí)現(xiàn)姿態(tài)解算。

參考坐標(biāo)系和機(jī)體坐標(biāo)系之間的旋轉(zhuǎn)可用方向余弦矩陣表示，其歐拉角表示方法為：

繞空間任意軸旋轉(zhuǎn)的方向余弦矩陣的四元數(shù)表示方法為：

對(duì)比式（1）和式（2）的相應(yīng)元素可得：

陀螺儀由于本身精度問題，測(cè)得的角速度存在一定誤差，在積分過程中這個(gè)誤差會(huì)一直累加，而加速度計(jì)和磁力計(jì)的長(zhǎng)期測(cè)量值是準(zhǔn)確的，因此可用加速度計(jì)和磁力計(jì)對(duì)陀螺儀做補(bǔ)償修正。姿態(tài)模塊共輸出9個(gè)數(shù)據(jù)：三軸角速度、三軸加速度、三軸磁場(chǎng)強(qiáng)度，以這9個(gè)數(shù)據(jù)作為姿態(tài)解算的輸入值，可算出方向余弦矩陣，從而根據(jù)式（3）求解出θ、ψ、?。整個(gè)解算流程如圖5所示。

圖5 姿態(tài)解算流程圖

根據(jù)加速度計(jì)的測(cè)量值，求解出姿態(tài)誤差向量ea：

其中g(shù)b為加速度計(jì)的實(shí)際測(cè)量方向，gn為n系中的重力方向，和分別為在b系中加速度計(jì)測(cè)量出的重力方向和陀螺儀計(jì)算出的重力方向，則：

根據(jù)磁力計(jì)的測(cè)量值，求解出姿態(tài)誤差向量em：

記磁力計(jì)實(shí)際測(cè)量出的地磁力方向?yàn)閙b，經(jīng)過（加速度計(jì)矯正后的旋轉(zhuǎn)矩陣）旋轉(zhuǎn)之后得到mn。已知地磁力在Oxy（n系）平面內(nèi)的向量大小相等，而沿z軸方向上分量大小不改變。處理之后的mn經(jīng)過旋轉(zhuǎn)回到b系中得到Lb，再與mb做向量積求得誤差向量em。

則姿態(tài)誤差向量e：

當(dāng)偏差角度很小時(shí)，陀螺儀的角速度誤差與加速度計(jì)和磁力計(jì)求得的誤差成正比，因而可用求得的誤差修正陀螺儀偏差，設(shè)修正后的角速度為w，則：

利用一階龍格-庫(kù)塔法（Runge-Kutta）更新四元數(shù)q，并將四元數(shù)單位化，設(shè)單位化后的四元數(shù)為，則：

將代入式（3）即可得姿態(tài)角θ、ψ、?。

2.2 PD控制器

俯仰和滾轉(zhuǎn)是撲翼機(jī)中最基本、最重要的控制，能夠保持撲翼機(jī)的飛行穩(wěn)定性。撲翼機(jī)通過尾翼的上下擺動(dòng)和沿機(jī)體軸的轉(zhuǎn)動(dòng)來調(diào)節(jié)尾翼與來流方向的夾角，從而改變尾翼上產(chǎn)生的氣動(dòng)力對(duì)機(jī)體中心的力矩大小。

俯仰角對(duì)保持撲翼機(jī)的穩(wěn)定飛行起著至關(guān)重要的作用。俯仰角速度q的引入，增大了撲翼機(jī)縱向短周期運(yùn)動(dòng)的阻尼，改善縱向通道的阻尼特性?？v向通道控制框圖如圖6所示。

秦安栽培的花椒品種主要有大紅袍、油椒、豆椒、“秦安一號(hào)”等。由于品種更新緩慢，花椒種質(zhì)資源相對(duì)老化，花椒品質(zhì)已不能適應(yīng)市場(chǎng)需求。許多椒園瀕臨衰老退化，樹勢(shì)衰弱，產(chǎn)量低，果實(shí)色澤暗淡，花椒質(zhì)量差，枝條容易枯死，普遍存在病蟲害嚴(yán)重，椒樹壽命不長(zhǎng)，椒園經(jīng)濟(jì)效益不高等。因此，老椒園的改造也是亟待解決的問題之一。

圖6 俯仰角控制框圖

俯仰角閉環(huán)控制律如式（10）：

式中θd為目標(biāo)俯仰角，θ為實(shí)際俯仰角，Kθ和Kq分別為比例調(diào)節(jié)系數(shù)和微分調(diào)節(jié)系數(shù)。

與俯仰角的姿態(tài)控制不同的是，為保持撲翼機(jī)的平穩(wěn)飛行，應(yīng)使目標(biāo)滾轉(zhuǎn)角?d等于零，引入滾轉(zhuǎn)角速度p增大撲翼機(jī)的滾轉(zhuǎn)阻尼，滾轉(zhuǎn)角的姿態(tài)控制亦采用PD控制器，其控制框圖如圖7所示。

圖7 滾轉(zhuǎn)角控制框圖

滾轉(zhuǎn)角閉環(huán)控制律如式（11）：

式中目標(biāo)滾轉(zhuǎn)角?d為0，?為實(shí)際滾轉(zhuǎn)角，K?和Kp分別為比例調(diào)節(jié)系數(shù)和微分調(diào)節(jié)系數(shù)。

3 裝配及試飛

文中所設(shè)計(jì)的微型撲翼控制器的質(zhì)量?jī)H為4.09 g，整體尺寸為27 mm*27 mm*2 mm，可將其裝配在微型撲翼飛行樣機(jī)上，如圖8所示。

圖8 裝配在撲翼機(jī)上的控制器

控制器可根據(jù)遙控器搖桿量的大小直接控制直流電機(jī)的轉(zhuǎn)速，從而控制撲翼機(jī)的撲動(dòng)頻率。當(dāng)撲翼機(jī)的滾轉(zhuǎn)角不為0時(shí)。由于滾轉(zhuǎn)姿態(tài)的自動(dòng)調(diào)節(jié)作用，尾翼會(huì)沿機(jī)體軸向機(jī)體滾轉(zhuǎn)方向同向轉(zhuǎn)動(dòng)，在氣動(dòng)力的作用下，產(chǎn)生與滾轉(zhuǎn)方向相反的滾轉(zhuǎn)力矩，從而調(diào)節(jié)滾轉(zhuǎn)姿態(tài)。

實(shí)際飛行過程中，撲翼飛行樣機(jī)可連續(xù)飛行15～20 s，實(shí)驗(yàn)試飛效果良好。圖9為飛行中的撲翼機(jī)。

4 結(jié)束語(yǔ)

文中設(shè)計(jì)了一款微型撲翼機(jī)控制器，采用STM32為主處理器，并設(shè)有陀螺儀、加速度計(jì)和磁力計(jì)，設(shè)計(jì)制作了控制器的硬件部分。在此基礎(chǔ)上進(jìn)行軟件設(shè)計(jì)，解析遙控器信號(hào)得到直流電機(jī)的控制量和目標(biāo)姿態(tài)角，采用基于四元數(shù)互補(bǔ)濾波的姿態(tài)解算方法求解撲翼機(jī)的姿態(tài)角。俯仰和滾轉(zhuǎn)姿態(tài)采用比例微分控制器，計(jì)算出舵機(jī)控制量，以保持撲翼機(jī)的飛行穩(wěn)定性。

該控制器具有質(zhì)量輕、尺寸小、與撲翼機(jī)契合度高的優(yōu)點(diǎn)，但目前采用的控制算法較為簡(jiǎn)單，只能實(shí)現(xiàn)手動(dòng)地遙控飛行，今后還需對(duì)控制算法進(jìn)行深入研究，以達(dá)到更好的控制效果。