針對2016年湖南省電子設計競賽的飛行器賽題，寫一篇四軸的方案，適合初學四軸的和想了解四軸的，大神級別的可以繞道。

1.方案設計

1.1姿態感測器的選擇

1.1.1加速度計

加速度檢測晶片有ADXL345、BMA180以及MPU6050等。ADXL345是ADI公司於2008年推出的具有SPI和IIC數字介面的三軸加速度計，有體積小、功耗低、可變數程、高分變率的特點。但是，在工作時電壓要穩定，否則會降低測量的精確度，因為每一個工作電壓點的零點漂移不一致，所以使用該晶片時，對供電系統的要求比較高。MPU6050是一款整合了3軸加速度計和3軸陀螺儀的六軸測量儀器，而且可以從第二個IIC介面連線其器感測器，比如磁力感測器，這樣可以從

1.1.2陀螺儀

角速度檢測的晶片有L3GD2H、IGT32000和MPU6060等，IGT3200是世界上第一個單晶片，IIC匯流排介面數字輸出，3軸MEMS陀螺儀積體電路，對3D滑鼠、遊戲和三維遠端控制有廣泛的應用，其測量的角速度精度為16位，解析度為14.375deg/s，最大測量範圍位2000deg/s，工作電壓2.6V。

1.1.3電子羅盤

電子羅盤有AK8975、HML5883L等，它是一種導航工具，能夠提供機體的航向和姿態。電子羅盤是一種重要的導航工具，能實時提供移動物體的航向和姿態。

1.1.4融合了角速度計和角速度計的模組

市面上有姿態角測量的模組，直接通過串列埠TX輸出尤拉角，模組內部帶有MPU，比如JY901，該模組融合了地磁測量，資料輸出頻率達到200Hz，含有GPS介面。姿態解算含有動態卡爾曼濾波，使用該模組可以直接串列埠讀資料，去除了主控姿態解算的壓力。但是不能再提高獲取原始資料的速度，而且在工作時不能有強烈的振動，否則，獲取的資料會出錯。

經過上面各種器件的比較，選取MPU9150作為姿態檢測晶片，其內部集成了MPU6050和AK8975，是一款九軸慣性感測晶片。這樣保證了角速度、加速度、電磁場的測量的精度，又使飛控板設計的面積大大小，減少了成本。

1.2循跡感測器的選擇

1.2.1反射式光電開關

反射式光電開光是一種紅外線產品，是一種小型光電元器件，能夠檢測出放射的光強的變化。它由一個紅外發射管和一個紅外接收管組成，發射波長是780nm-1mm，發射器內建一個校準鏡頭，經過反射射向接收器，當有物體經過，或者物體的顏色改變，都會改變反射光線的強度。白色和黑色對光的吸收程度有很大的差別，根據該原理可以識別軌跡。反射式光電開光在壁障，循跡中應用廣泛。但是光照因素對其檢測有很大的影響，不同的關照環境下反射的光線強度大有不同，會使除錯帶來麻煩。

1.2.1光流感測器

光流發法是影象分析的重要方法，特別是在視覺運動研究中有著舉足輕重的作用，它的原理是從二維的影象序列中檢測物體的運動、提取運動引數，並且分析物體運動的相關規律。PX4Flow是一款智慧光學流動感測器。感測器擁有原生 752×480畫素解析度，計算光學流的過程中採用了4倍分級和剪裁演算法，計算速度達到250Hz（白天，室外），具備非常高的感光度。與其他滑鼠感測器不同，它可以以120Hz（黑暗，室內）的計算速度在室內或者室外暗光環境下工作，而無需照明LED。你也可以對它重新程式設計，用於執行其他基礎的，高效率的低等級機器視覺任務。PX4FLOW模組使用USB和UART2輸出MAVLink協議包，可以使用地面站軟體QGroundControl從模組讀取資料，主控可以通過IIC埠來獲取相應的資料，

主要資料是x軸、y軸的移動速度和移動距離。光流演算法是基於每個畫素點變化來計算的，所以光流感測器測量時要求被測面有紋理特徵，特別不能是單色面，否則計算就不準確。而我們要循的軌跡是一條以白色為底面黑色的直線，所以光流感測器不太適合。

1.2.3攝像頭

攝像頭的功能是影象採集，通過對影象的分析來尋找軌跡，並計算出機體偏離軌跡的程度，所以用攝像頭循跡，是種很好的選擇。不同型號的攝像頭的解析度不同，一般解析度決定了攝像頭產品的價格，在循跡的應用中，對攝像頭的解析度要求不高，設計從成本的角度考慮，選擇OV2640攝像頭來進行影象採集。

1.3測高感測器的選擇

1.3.1氣壓計

在同一環境下，氣壓是隨高度的變化而變化的，每升高12m,大氣壓下降1mmHg（1毫升水銀柱）或者每上升9m,大氣壓降低100Pa。根據該原理，可以通過氣壓計測量不同位置的氣壓變化來計算出高度的變化。MS5611是由瑞士推出的一款I²C和SPI匯流排介面的高解析度氣壓感測器。MS5611提供了一個精確的24位數字溫度值和壓力值以及不同的操作模式，可以提高轉換速度並優化電流消耗，解析度達到10cm，體積尺寸是5mmX3mmX1mm，可以整合在移動裝置中。由於機體飛行高度只要求在0到4m的範圍內，所以，10cm的誤差對於定高來說還是比較大的。

1.3.2超聲波感測器

超聲波測距原理是超聲波發射裝置向某一方向發射超聲波，與此同時開始計時，超聲波在空氣中傳播，當碰到障礙物就立刻返回來，超聲波接收器收到反射波就停止計時。（超聲波在空氣中的傳播速度為340m/s，根據傳送到接收經過的時間t，就可以計算出發射點距障礙物的距離(s)，即：s=340t/2）常見的超聲波測距模組有US-100、URF02、HC_SR04等。US-100超聲波測距模組可測範圍是2cm~4.5m，擁有2.4~5.5V的寬電壓輸入範圍，靜態功耗低於2mA，內部自帶了溫度感測器，可對測距結果進行校正，內帶看門狗，工作穩定可靠，同時具有GPIO，串列埠兩種通訊方式，為了獲取資料的方便，選擇串列埠方式獲取資料。

2.硬體設計

2.1 結構與功能

飛行器由機架、電機、電調、螺旋槳、電池、飛控板、影象處理系統、超聲波模組、2.4G無線模組等組成，四旋翼飛行器結構形式如圖2.1所示。

機架為整個飛行器提供骨架支撐，採用 F330型號的機架，材質為尼龍和纖，硬度強，耐炸機。電機和槳提供動力，電調將直流電轉換成三相交流，為電機提供電力，電池則為整個系統提供電力。

飛控板則為飛行器的大腦，控制其飛行。飛控板應比賽要求則自主研發，採用TM4C123G為微處理器。飛控板中含有姿態檢測晶片，以獲取飛行姿態資料。影象處理系統為飛行器的眼睛，對地面軌跡影象經過二值化及濾波處理，再經過特定的計算得出飛飛行器相對地面軌跡的位置。最後以資料包通過串列埠傳送給飛控。

超聲波模組檢測飛行高度，以串列埠的形式發給飛控。2.4G無線模組是飛行器與地面站的溝通橋樑，通過地面站可以監控飛行器的飛行狀態，還可以用地面站調整PID引數。

2.2 TM4C123G基本應用系統

TM4C123G基本系統由時鐘系統、復位系統、SWD除錯及下載系統等組成。

時鐘系統為微控制器工作提供節拍，石英晶體振盪器比TM4C123G內部的RC振盪更精準、更穩定。設計採用外部16Mhz的晶體振盪器，再由TM4C123G內部的PLL將時鐘達到80Mhz，作為系統時鐘。具體電路如圖1.2.1所示，其中12pF的電容的作用是幫助晶體起振。OSC_0和OSC_1分別接TM4C123G的OSC0輸入管腳和OSC1輸出管腳。

復位系統包含手動復位和上電覆位。手動復位是當程式跑飛或者需要程式從頭開始執行時，通過按鍵來進行復位；上電覆位是微控制器在上電啟動時，為了能使CPU以系統各部件處於確定的狀態，並從初態開始工作，通過RC充電原理進行自動復位。復位電路如圖2.2.2所示，圖中100歐姆的電阻起保護電容的作用。

對於SWD除錯及下載系統，可以直接將下載器的訊號線接入TM4C123G的SWCLK和SWDIO引腳，再將下載器和目標晶片供地即可。下載介面採用1X4的單排針，引腳排序為+3.3V、SWCLK、GND、SWDIO。

2.3MPU9150電路設計

MPU9150和MPU有兩種通訊方式，一中是SPI，另一種是IIC，用SPI方式至少的用三根訊號線，而IIC只需要兩根線，為了降低設計的複雜性採用IIC通訊模式。MPU6050外圍元件的配置，可以通過它的資料手冊來確定。I2C介面採用的是OpenDrain機制，器件本身只能輸出低電平，不能主動輸出高電平，只能通過外部上拉電阻RP將訊號線拉至高電平，所以IIC總線上的上拉電阻是必須的。上拉電阻的大小會對時序有影響，對訊號的上升和下降時間也有影響。IIC上拉電阻有一個計算公式：Rmin={Vdd(min)-0.4V}/3mA,Vdd是供電電壓。Rmax=(T/0.874)*c,(T=1us100KHz,T=0.3us400KHz)，其中C是Buscapacitance。Rp最大值由匯流排最大容限（Cbmax)決定,Rp最小值由Vdd與上拉驅動電流（最大取3mA)決定。系統的Vdd是3.3V，MPU9105灌入埠的電流是3mA而TM4C123G的最小灌入電流是2mA，所以上拉驅動電流取3mA,Rmin≈1K。MPU9105的Cb=10pF，TM4C123G的Cb=50pF。通訊速率要求是400kHz，所以Rmax≈2k。所以上拉電阻選取常用值1.5k。具體電路如圖2.3.1所示。

2.4循跡單元的設計

影象處理對CPU的要求比較高，如果用主控TM4C123G來處理，由於多種任務的處理頻率和記憶體的限制，很難實現。所以採用協處理器(STM32F407)來對攝像頭資料進行處理，在通過串列埠將處理的資料傳給主控。為了方便安裝攝像頭，將影象處理做成一個單獨的模組，再通過連線線與主控相連。攝像頭和處理器的接線有很多，其中含有SCCB介面，是一種控制介面，控制影象感測器晶片的執行，和IIC介面相容；數字視訊介面（Digital Video Port），包含了訊號線和資料線，是個10位的數字介面，其支援8為位資料，ALIENTEK OV2640 模組採用預設的 8位連線方式。在佈線佈線時得注意，常用的設計規則有：(1)時鐘源到負載的連線儘量短，線應較寬。(2)訊號的負載過大時，在一個驅動器上不要驅動其他訊號，特別是其他時鐘。(3)注意不同時鐘之間，時鐘與其他訊號之間的干擾，避免多種訊號平行佈線（可採用遮蔽層隔離的辦法）(4)多個裝置需要嚴格的同步時鐘時，可採用H形網路，以保證時鐘源到每個負載的延遲相等。(5)驅動高頻時鐘的器件應很好地去耦。(6)遮蔽線和雙絞線一般能得到比線路板走線更好的效果。SCL和SDA屬於控制線，走線時，儘量不要和D0-D7的資料線在一起。D0-D7儘量並行走線，且長度儘量相等。

2、軟體設計

2.1姿態解算

姿態解算方法有四元數法和方向餘弦法，四元素法是通過四元素來記錄機體的狀態，再將四元數轉換成尤拉角，以尤拉角作為直觀的資料。

一種比較常見的四元數法是Mahony互補濾波演算法，特點是加速度對角速度進行補償，達到消除積分誤差的方法。從角速度計獲取的角度是通過對角度的積分而得到的，由於角度速度計短期測得的值比較準確，但時間一長，由於溫漂的問題，進過積分之後，得到的角度會有很大的誤差。加速度計短期值不太準確，噪聲比較大，但通過低通濾波之後，長期的值比較準確。根據兩者的特點，利用加速度計對角速度計進行補償來消除積累誤差。對機體姿態解算，需要得到x軸、y軸、z軸的角速度和角速度，還有解算的週期。加速度和角速度就可以通過MPU9150來獲取，週期就有主控的定時器產生。下面是姿態解算的步驟：

1.將加速度原始資料歸一化，轉換成單位加速度，公式如下：

norm = sqrt(ax*ax + ay*ay + az*az);

ax = ax / norm;

ay = ay / norm;

az = az / norm;

2.把四元數轉換換成“方向餘弦矩陣”中的第三列的三個元素。地理座標系的重力向量，轉到機體座標系，正好是這三個元素。所以vx、vy、vz，其實就是機體座標參照系上，換算出來的重力單位向量，公式如下：

vx = 2*(q1*q3 - q0*q2);

vy = 2*(q0*q1 + q2*q3);

vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

3.對加速度單位向量和重力單位向量進行叉積，該叉積的向量仍在機體座標系上，而陀螺儀的積分誤差也是在機體座標系上的，而且和叉積成正比，正好可以用來修正角速度計，所以對角速度計的修正直接體現在對機體的修正，公式如下：

ex = (ay*vz - az*vy);

ey = (az*vx - ax*vz);

ez = (ax*vy - ay*vx);

4.由於具體的比例係數未知的，所以採用PI調節來對角速度計進行補償，可以通過調節Kp、Ki兩個引數來控制加速度計對加速度計修正的速度。

exInt = exInt + ex*Ki;

eyInt = eyInt + ey*Ki;

ezInt = ezInt + ez*Ki;

gx = gx + Kp*ex + exInt;

gy = gy + Kp*ey + eyInt;

gz = gz + Kp*ez + ezInt;

5.得到了修正後的角速度就可以將其轉換成四元數，來記錄機體狀態，下面用一階龍格-庫塔法將角速度轉換成四元素。

q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;

q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;

q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;

q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

6.最後就是將四元素轉換成尤拉角了。

  Roll = atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));

Pitch = asin(2*(q1*q3-q0*q2);

Yaw = atan2(2*(q1*q2-q0*q3),2*(q0*q0-q1*q1)-1);

2.2姿態控制

談論四軸的姿態控制，首先得知道控制原理，如圖2.1所示，以電機1和電機4一側為前方，下面來分析四軸的基本運動。

1.垂直運動：垂直運動相對來說比較簡單。結構圖顯示相鄰的電機做反向運動，相對的電機做同向運動。因此有兩對電機轉向相反，可以平衡機身的反轉扭矩，當同時增大電機的轉速，是的總的拉力增大，當拉力比機體本身的重力大時，怎機體所受合力垂直向上，機體向上飛，當機體重力大於拉力，機體向上飛。

2.滾轉運動：滾轉運動是機體做左右翻滾的運動。以向右翻滾為例，為了使機體反向扭矩的平衡，不讓機體旋轉，又要使機體垂直的拉力不變。使電機3和電機4的轉速降低，電機1和電機2，的轉速增加，由於降低和增加的分別是一對轉速相反的電機，機體的反向扭矩依然平衡，使增加和降低的轉速一樣，則總的拉力沒有改變。但由於力的改變，機體會向右翻滾。

3.俯仰運動：由於結構的對稱性，原理和滾轉運動一樣，使電機2和電機3的轉速降低，電機1和電機4轉速增加，會使機體向上仰。

4.偏航運動：機體的偏航則利用機體反轉扭矩的不平衡。當電機1和電機3的轉速增加，電機2和電機4的轉速降低時，旋翼1和旋翼3產生的反扭矩大於旋翼2和旋翼4產生的反扭矩，機身便在總反扭矩的作用下繞z軸轉動，實現機體的偏航，轉向與電機1、電機3的轉向相反。

機體的前進後退和向左向右，就是由分別由俯仰運動和滾轉運動來實現。讓機體俯仰或翻滾到某一程度，則會使機體傾斜，拉力會產生一個與水平面平行的力，使機體往平行水平面的方向飛行。所以姿態的保持，即機體額尤拉角保持某個值不變，是姿態控制的主要要求。

數字控制器有很多種，最常用和經典的演算法那就是PID演算法了，機體姿態控制器採用串級PID。控制結構如圖2.2.1所示，圖中的角度就是姿態解算出來的尤拉角，角速度是直接從MPU9150讀出的值，內環是角速度控制，有加強穩定的作用，因為角度為位移量，不能突變，所以變換成角速度來突變；外環就是角度控制了，用來控制機體的姿態。串級控制輸入就是期望姿態，通過設定期望姿態就能控制機體的飛行方向。

執行機構是四個電機，四個電機是由PWM來控制的，下面是PWM的公式：

posture_value[0] = + out_roll - out_pitch + out_yaw ;

posture_value[1] = + out_roll + out_pitch - out_yaw ;

posture_value[2] = - out_roll + out_pitch + out_yaw ;

posture_value[3] = - out_roll - out_pitch - out_yaw ;

posture_value是儲存四個PWM的變化量，out_roll、out_pitch、out_yaw就是執行機構的輸入了。比如機體往左右時，out_pitch、out_yaw為零，out_roll為正值，則電機1和電機2轉速增加，電機3和電機4轉速降低。

2.3影象處理

要實現自主飛行，就必須使機體能夠感知周圍環境，感知器採用的是攝像頭，所以影象處理是也是設計的重要內容。在賽題要求中飛行環境不是很複雜，只有兩種顏色，白色和黑色，白色為地面，黑色為飛行的軌跡。所以影象處理大致可分為兩步，影象的二值化和對軌跡的計算。

2.3.1影象的二值化

二值化的基本原理是把影象的每個點的灰度通過適當的閥值設定成0或255，使整個影象呈現黑白的效果。影象的二值化有利於對影象進一步處理，使影象變得簡單，處理的資料量也減少了很多。為了能夠得到較好的二值化影象，一般採用連通和封閉的邊界來定義不交疊的區域。灰度大於或等於閥值的畫素置為255，用來表示特定點，小於則為0，用來表示背景。從OV2640獲取的圖片是RGB565的格式，其每個畫素點是16位的，首先得經過灰度化再二值化。將RGB565格式的圖片灰度化公式如下：

Red  = (PixVal & 0xF800) >> 8;

Green = (PixVal & 0x07E0) >> 3;

Blue  = (PixVal & 0x001F) << 3;

Grey  = (Red * 76 + Green * 150 + Blue * 30) >> 8。

其中PixVal就是畫素點的值，Red、Green、Blue為三種基色。Grey就是灰度值（0-255）。

得到灰度值之後就可以用OTSU或Kittle演算法進行二值化，Kittle演算法是一種快速的全域性閾值法，其效果雖然比OTSU差一點，但速度快很多倍，在循跡控制中要求協控處理影象的速度要快，所以選擇Kittle演算法。關於Kittle演算法有很多文獻資料，再此就不多說了。

2.3.1軌跡的計算方法

由於循跡的軌跡是直線，所以只要計算黑線在影象中心位置的偏移。對二值化後的影象進行掃描，計算每行的黑點橫座標的平均值，從某行進行分析，該平均值就體現出了該行連續黑點在整行的位置，把所有行的平均值再求平均之後就可得到整條黑線在影象的位置，該位置表現出左右的偏移。對於黑圓該方法同樣適用，將其看成二維的即可，和對每行進行掃描一樣，對每一列進行畫素點的掃描就可以得到圓在影象上前後的偏移，影象是機體上攝像頭獲取的，黑線或圓在影象的偏移，就是機體對於黑線或圓的偏移。

2.4循跡和定點

循跡和定點的控制原理相同，區別在於循跡控制是一維的，定點是二維的，所以可以使用同一種控制器，採用PID演算法，PID演算法具有很好的跟隨性，而且響應速度也很快。PID控制器由比例（P）、積分（I）和微分（D）組成，位置式PID公式如下：

u0為初始期望的輸出值，通過實際情況來估算出u0，一般為零，T是採用週期，即控制器兩次計算之間的間隔。e(n)是第n次期望和實際值得偏差，由於期望是黑線或者圓在影象正中心，所以期望值是不變的，偏差就是一個固定的值減去影象處理後的資料，這裡固定的值為128。

Kp、Kp/Ti、Kp*Td為控制器PID的引數，它們的作用如下：

1.比例引數的作用是加快系統的響應速度，太小系統響應太慢，太大會使系統振盪，參產生超調，比例環節是PID控制器中不可缺少的環節，調參時一開始就要估算出比例係數；

2.積分的作用是來消除系統誤差的，積分越大，消除靜差的時間越短，但也不能太大，否則會產生積分飽和的現象。從傳遞函式來分析，增加了積分環節，相位會有一個90度的延遲，而且控制器從一階變成了二階系統，會降低系統的穩定性；

3.微分的作用是改善系統的動態效能，在響應過程中可以抑制偏差向任何方向的變化，對偏差變化進行提前預知，防止超調，從某一角度分析可理解為微分環節在控制器中有相反的作用，當偏差越小，就會使調節器的調節作用越小，微分系數不能太大，不然會增加系統的調節時間。

經過對三個引數的分析，選取PD控制器最為合適，即將積分系數置為零，比例和微分設為非零合適置，在進行調參時，先把微分系數，比例設為一個合適的值，對機體進行測試，如果測得值在期望值附近產生振盪，則合理的比例係數就在此時比例係數的附近了。在除錯過程中，很難用個肉眼觀察出系統是否產生了等幅振盪，所以通過2.4G無線傳輸將機體對黑線的偏移傳輸給上位機，通過上位機來顯示波形，這就很明瞭了。如圖2.4.1就是選取比例係數時的波形圖，可以很直觀的看出其在128附近振盪。

比例係數選取好之後就要加入微分環節了，給個合適的微分引數，如果機體振盪更加嚴重了，那麼是微分系數太大了，得減小微分。如果沒有發生振盪，那麼微分就發揮了作用了，它會使振盪的幅度變小，即波形更加平緩，當然微分太小，其作用也是很小的。微分也有個缺點，那就是降低系統的響應，當然調試出一組折中的引數，系統的穩定性達到要求，系統的響應速度也要達到要求。如果在比例不變的情況下只改微分，達不到要求，那就要適當的改變比例係數了，利用比例係數和微分系數兩者的特點來進行選取。

2.5高度控制

定高函式的作用是使飛行器在指定高度飛行，在飛行過程中會有網子，這樣會影響超聲波測的得資料，因此在資料採集後需要濾除，濾除的方法採用低通濾波加滑動中值濾波。濾波之後採用串級PID對高度進行控制，其中速度控制是內環控制，外環是高度控制，PID的輸出量就是油門量，串級PID的結構和姿態控制的結構是一樣的，其中Z軸的速度就是對角速度計進行積分得到的，高度通過超聲波來測量。一階低通濾波的公式如下：

Y_n=A*X_n+(1-A)*Y_n-1

其中Y_n是濾波後的值，Y_n-1是上次濾波的值，X_n是當前測得的值，A為濾波係數，截止頻率f=A/(2πt)。在程式設計中實現方式如下：

height_lpf += 0.8*2*3.14f*t*(height - height_lpf);//低通濾波