微型仿生撲翼機控制器設計

賀雪晨，周超英，汪超

（哈爾濱工業大學深圳研究生院機電工程及自動化學院，廣東深圳518055）

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

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

1 硬件設計部分

1.1 總體方案

微型撲翼控制器可利用各傳感器的數據解算出撲翼機的當前姿態，并解析接收機信號得到控制量，控制執行機構。主要由以下幾個部分組成：

1）微處理器：撲翼控制器的核心單元，用來解算姿態角、解析遙控信號、控制執行機構；

2）姿態傳感器：包含加速度計、磁力計和陀螺儀，提供姿態解算的原始數據；

3）通訊模塊：遙控器通過接收機與控制器進行信息通信。

1.2 電路設計

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

圖1 控制器框圖

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

圖3為文中所設計的控制器：

圖2 姿態傳感器電路圖

圖3 控制器

2 軟件設計部分

在上述已完成的硬件基礎上進行軟件設計，軟件的主要功能為：將接收到的遙控數據解析為電機控制量和目標姿態角，其中電機控制量直接轉換成對應占空比的PWM信號輸出給電機驅動模塊，讀取各傳感器的數據并進行姿態解算，得到飛行器當前的姿態數據，將實際姿態角與目標姿態角的差值輸入給PD控制器，最終得到調節后的控制量來控制尾翼舵機。框圖如圖4所示。

圖4 軟件設計框圖

2.1 姿態解算

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

飛行器在參考坐標系中的姿態一般用俯仰角θ、偏航角ψ、滾轉角?來表示。本文的計算過程采用四元數算法，各軸角的表示采用歐拉角。在計算過程中利用修正漂移后的角速度求出四元數，將四元數轉換成方向余弦矩陣中的某幾個元素，再利用方向余弦矩陣與歐拉角的數學關系計算出姿態角，從而實現姿態解算。

參考坐標系和機體坐標系之間的旋轉可用方向余弦矩陣表示，其歐拉角表示方法為：

繞空間任意軸旋轉的方向余弦矩陣的四元數表示方法為：

對比式（1）和式（2）的相應元素可得：

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

圖5 姿態解算流程圖

根據加速度計的測量值，求解出姿態誤差向量ea：

其中gb為加速度計的實際測量方向，gn為n系中的重力方向，和分別為在b系中加速度計測量出的重力方向和陀螺儀計算出的重力方向，則：

根據磁力計的測量值，求解出姿態誤差向量em：

記磁力計實際測量出的地磁力方向為mb，經過（加速度計矯正后的旋轉矩陣）旋轉之后得到mn。已知地磁力在Oxy（n系）平面內的向量大小相等，而沿z軸方向上分量大小不改變。處理之后的mn經過旋轉回到b系中得到Lb，再與mb做向量積求得誤差向量em。

則姿態誤差向量e：

當偏差角度很小時，陀螺儀的角速度誤差與加速度計和磁力計求得的誤差成正比，因而可用求得的誤差修正陀螺儀偏差，設修正后的角速度為w，則：

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

將代入式（3）即可得姿態角θ、ψ、?。

2.2 PD控制器

俯仰和滾轉是撲翼機中最基本、最重要的控制，能夠保持撲翼機的飛行穩定性。撲翼機通過尾翼的上下擺動和沿機體軸的轉動來調節尾翼與來流方向的夾角，從而改變尾翼上產生的氣動力對機體中心的力矩大小。

俯仰角對保持撲翼機的穩定飛行起著至關重要的作用。俯仰角速度q的引入，增大了撲翼機縱向短周期運動的阻尼，改善縱向通道的阻尼特性。縱向通道控制框圖如圖6所示。

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

圖6 俯仰角控制框圖

俯仰角閉環控制律如式（10）：

式中θd為目標俯仰角，θ為實際俯仰角，Kθ和Kq分別為比例調節系數和微分調節系數。

與俯仰角的姿態控制不同的是，為保持撲翼機的平穩飛行，應使目標滾轉角?d等于零，引入滾轉角速度p增大撲翼機的滾轉阻尼，滾轉角的姿態控制亦采用PD控制器，其控制框圖如圖7所示。

圖7 滾轉角控制框圖

滾轉角閉環控制律如式（11）：

式中目標滾轉角?d為0，?為實際滾轉角，K?和Kp分別為比例調節系數和微分調節系數。

3 裝配及試飛

文中所設計的微型撲翼控制器的質量僅為4.09 g，整體尺寸為27 mm*27 mm*2 mm，可將其裝配在微型撲翼飛行樣機上，如圖8所示。

圖8 裝配在撲翼機上的控制器

控制器可根據遙控器搖桿量的大小直接控制直流電機的轉速，從而控制撲翼機的撲動頻率。當撲翼機的滾轉角不為0時。由于滾轉姿態的自動調節作用，尾翼會沿機體軸向機體滾轉方向同向轉動，在氣動力的作用下，產生與滾轉方向相反的滾轉力矩，從而調節滾轉姿態。

實際飛行過程中，撲翼飛行樣機可連續飛行15～20 s，實驗試飛效果良好。圖9為飛行中的撲翼機。

4 結束語

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

該控制器具有質量輕、尺寸小、與撲翼機契合度高的優點，但目前采用的控制算法較為簡單，只能實現手動地遙控飛行，今后還需對控制算法進行深入研究，以達到更好的控制效果。