多旋翼無人飛行器嵌入式飛控開發(fā)實戰(zhàn)-基于STM32系列微控制器的代碼實現(xiàn)-課件奚海蛟-第5章無人機參數(shù)存儲 光流計OpticalFlow-PID位置算法

多旋翼無人機飛行器嵌入式飛控開發(fā)實戰(zhàn)光流計OpticalFlow1234氣壓計MS56115光流計OpticalFlow無人機數(shù)傳加速度計與陀螺儀MPU6050磁力計QMC5883L6遙控輸入7電機控制光流計OpticalFlow通過串口通信原理，將光流模塊采集到的數(shù)據(jù)，進(jìn)行檢驗以及計算，實現(xiàn)無人機定高、定點的目的。0102光流計OpticalFlowSTMicroelectronics的VL53L1X是一種先進(jìn)的ToF激光測距傳感器，壯大了FlightSense?產(chǎn)品系列陣容。PMW3901是一個光流專用集成電路，它可以在內(nèi)部計算光流，并在每幀之間提供像素差。VL53L1XVL53L1X系統(tǒng)由VL53L1X模塊和運行在該模塊上的驅(qū)動程序組成。PMW3901PMW3901基本上是一個跟蹤傳感器，類似于你可以在電腦鼠標(biāo)中找到的東西，其適用范圍在80毫米到無限寬之間。原理圖通過串口4與光流模塊連接。光流計OpticalFlow實現(xiàn)光流模塊采集數(shù)據(jù)，無人機定高、定點。練習(xí)簡述光流傳感器的運行原理01簡述光流傳感器的數(shù)據(jù)解析的過程。02遙控輸入1234氣壓計MS56115光流計OpticalFlow無人機數(shù)傳加速度計與陀螺儀MPU6050磁力計QMC5883L6遙控輸入7電機控制遙控輸入熟悉飛控系統(tǒng)的遙控接收機輸入接口信號的采集方法，能夠采集多路的遙控信號，能夠自行添加更多路的遙控數(shù)據(jù)。0102遙控輸入遙控器發(fā)送PWM脈沖，接收機接收信號，然后單片機捕獲接收到的信號。定時器TIM3和定時器TIM4來捕獲接收機的1—8通道的pwm值。接收機通道與飛控通道對應(yīng)連接原理圖遙控輸入能夠采集無人機多路的遙控信號。練習(xí)利用其他通道實現(xiàn)遙控器的輸入。01簡述遙控器輸入的過程。02電機控制1234氣壓計MS56115光流計OpticalFlow無人機數(shù)傳加速度計與陀螺儀MPU6050磁力計QMC5883L6遙控輸入7電機控制電機控制學(xué)習(xí)飛控系統(tǒng)的電調(diào)輸出接口的原理和使用；了解電調(diào)工作條件需求；熟悉使用輸出PWM驅(qū)動電調(diào)系統(tǒng)。0102電子調(diào)速器電調(diào)左端為輸入端，外側(cè)黑、紅線接電池，內(nèi)側(cè)黑、紅、白線接數(shù)字信號電路；右側(cè)三條輸出線接三相無刷電機。電調(diào)的作用就是把PWM方波信號調(diào)成三相交流電，任意改變接入電機的兩根線，即可改變電機的轉(zhuǎn)向。電子調(diào)速器與飛控連接圖電調(diào)的輸入信號由飛控板的引腳控制。由STM32定時器TIM1產(chǎn)生4路PWM方波脈沖，提供給四旋翼無人機的四個電調(diào)，控制無刷電機的轉(zhuǎn)速。原理圖電機控制使用輸出PWM驅(qū)動電調(diào)系統(tǒng)。練習(xí)簡述電調(diào)的原理。01簡述電機控制的實現(xiàn)過程。02實現(xiàn)數(shù)字濾波器-FIR以及IIR、卡爾曼濾波以及擴展卡爾曼濾波、無人機的姿態(tài)解算以及角速度、角度PID控制器。第7章無人機飛控算法1234基于擴展卡爾曼濾的算法融合5無人機姿態(tài)解算數(shù)字濾波器-FIR數(shù)字濾波器-IIR基于卡爾曼濾波的角度融合6角速度、角度PID控制器設(shè)計數(shù)字濾波器-FIR1234基于擴展卡爾曼濾的算法融合5無人機姿態(tài)解算數(shù)字濾波器-FIR數(shù)字濾波器-IIR基于卡爾曼濾波的角度融合6角速度、角度PID控制器設(shè)計將“光標(biāo)”飛控中傳感器采集到的數(shù)據(jù)信息通過數(shù)字濾波器—FIR濾除雜波，保證傳感器采集數(shù)據(jù)的準(zhǔn)確有效性。0102數(shù)字濾波器-FIRFIR濾波器FIR濾波器是非遞歸型濾波器的簡稱，又叫做有限長單位沖激響應(yīng)濾波器。在數(shù)字信號處理系統(tǒng)中較為基本的元件。數(shù)字濾波器系統(tǒng)函數(shù)為H(z)，脈沖響應(yīng)為h(n)，輸入時序為x(n)。

>

>設(shè)濾波器單位脈沖響應(yīng)的長度為N，系統(tǒng)函數(shù)為：>傳輸函數(shù)

直接型設(shè)FIR濾波器的單位沖擊響應(yīng)h(n)為一個長度為N的序列，則濾波器系統(tǒng)函數(shù)為：A表示這一系統(tǒng)輸入輸出關(guān)系的差分方程為：B

差分方程對應(yīng)的網(wǎng)絡(luò)結(jié)構(gòu)優(yōu)點：簡單直觀，乘法運算量較少。缺點：調(diào)整零點較難，并且具有相位延遲性。級聯(lián)型當(dāng)需要控制濾波器的傳輸零點時，可將H(z)分解為實系數(shù)二階因子的乘積形式。式中，H(z)為h(n)的z變換，β0k，β1k，β2k為實數(shù)。

級聯(lián)型結(jié)構(gòu)BA優(yōu)點調(diào)整零點比直接型方便。缺點H(z)中的系數(shù)比直接型多，因而需要的乘法器多。線性相位型結(jié)構(gòu)FIR濾波器的線性相位結(jié)構(gòu)有偶對稱和奇對稱，不論h(n)為偶對稱還是奇對稱都有：

當(dāng)N為奇數(shù)時，系統(tǒng)函數(shù)為：當(dāng)N為偶數(shù)時，系統(tǒng)函數(shù)：FIR直接型信號流01N為偶數(shù)02N為奇數(shù)頻率采樣型頻率采樣型結(jié)構(gòu)是一種用系數(shù)將濾波器參數(shù)化的一種實現(xiàn)結(jié)構(gòu)。

STEP1H(k)與系統(tǒng)函數(shù)之間的關(guān)系可用內(nèi)插公式為：其零點為：STEP2

信號流圖數(shù)字濾波器-FIR將“光標(biāo)”飛控中傳感器采集到的數(shù)據(jù)信息通過數(shù)字濾波器—FIR濾除雜波，保證傳感器采集數(shù)據(jù)的準(zhǔn)確有效性。練習(xí)簡述FIR數(shù)字濾波器運行原理。01重新導(dǎo)出22階漢明窗口濾波器。02數(shù)字濾波器-IIR1234基于擴展卡爾曼濾的算法融合5無人機姿態(tài)解算數(shù)字濾波器-FIR數(shù)字濾波器-IIR基于卡爾曼濾波的角度融合6角速度、角度PID控制器設(shè)計數(shù)字濾波器-IIR將“光標(biāo)”飛控中傳感器采集到的數(shù)據(jù)信息通過數(shù)字濾波器—IIR濾除雜波，保證傳感器采集數(shù)據(jù)的準(zhǔn)確有效性。0102數(shù)字濾波器-IIRIIR（InfiniteImpulseResponse）數(shù)字濾波器，又名無限脈沖響應(yīng)數(shù)字濾波器，或遞歸濾波器。IIR與FIR濾波器對比數(shù)字濾波器根據(jù)其沖激響應(yīng)函數(shù)的時域性分為兩種，無限長沖激響應(yīng)（IIR）數(shù)字濾波器和有限長沖激響應(yīng)（FIR）濾波器。IIR數(shù)字濾波器與FIR數(shù)字濾波器相比相位特性差且可以用于較少的結(jié)束獲得很高的選擇性但是具有結(jié)構(gòu)簡單具有經(jīng)濟高效的特點IIR濾波器差分方程的一般表達(dá)式:運算量小IIR數(shù)字濾波器設(shè)計的方法直接設(shè)計法01不使用模擬濾波器，直接進(jìn)行數(shù)字濾波設(shè)計的方法。不需要事先給定參數(shù)的方法。間接設(shè)計法02間接設(shè)計發(fā)是借助成熟的模擬濾波器設(shè)計方法設(shè)計的，根據(jù)數(shù)字濾波器設(shè)計指標(biāo)（事先給定參數(shù)）來設(shè)計相對應(yīng)的過渡模擬濾波器，再將過渡模擬濾波器轉(zhuǎn)換為數(shù)字濾波器。原型低通濾波器--巴特沃斯低通濾波器巴特沃斯低通濾波器其中，N為濾波器的次數(shù)，Ωc是截止頻率。計算出阻帶衰減：將巴特沃斯低通濾波器的振幅特性，直接帶入上式得到：可以解得次數(shù)N為：巴斯沃斯濾波器的傳遞函數(shù)巴斯沃斯低通濾波器的傳遞函數(shù)，可以由其振幅特性的分母多項式求得。其分母多項式為：根據(jù)S解開，可以得到極點。當(dāng)N為偶數(shù)的時候，求得：同理得當(dāng)N為奇數(shù)的時候，求得：利用歐拉公式求得的極點：巴斯沃斯濾波器的傳遞函數(shù)選取穩(wěn)定極點，取模擬濾波器的傳遞函數(shù)求得：設(shè)計完乘法器過后，可計算出其濾波系數(shù)：雙1次z變換的原理模擬濾波器的傳遞函數(shù)為：傳遞函數(shù)進(jìn)行拉普拉斯逆變換，可以得：其中，x(t)表示輸入，y(t)表示輸出。然后我們需要將其離散化，可得到：之后使用z變換，再將其簡化?？梢缘茫簭亩?，我們可以得到s平面到z平面的映射關(guān)系：然后，將關(guān)系式

與s=δ+jΩ帶入得到：可以得到Ω與ω的對應(yīng)關(guān)系為：數(shù)字濾波器-IIR將“光標(biāo)”飛控中傳感器采集到的數(shù)據(jù)信息通過數(shù)字濾波器—IIR濾除雜波，保證傳感器采集數(shù)據(jù)的準(zhǔn)確有效性。練習(xí)簡述IIR濾波器運行原理。01簡述IIR濾波器實現(xiàn)過程。02基于卡爾曼濾波的角度融合1234基于擴展卡爾曼濾的算法融合5無人機姿態(tài)解算數(shù)字濾波器-FIR數(shù)字濾波器-IIR基于卡爾曼濾波的角度融合6角速度、角度PID控制器設(shè)計通過學(xué)習(xí)卡爾曼濾波工作原理與實戰(zhàn)演練，掌握并運用卡爾曼濾波進(jìn)行濾波和數(shù)據(jù)融合。0102FreeRTOS+TCP客戶端TCP協(xié)議簡介卡爾曼濾波（Kalmanfiltering）是一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計的算法。由于觀測數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，所以最優(yōu)估計也可看作是濾波過程?？柭鼮V波原理系統(tǒng)的狀態(tài)方程和測量方程狀態(tài)方程：觀測方程：當(dāng)前狀態(tài)當(dāng)前狀態(tài)公式為：式中，

，B=[T0]，X(k|k-1)是利用k預(yù)測的結(jié)果，X(k-1|k-1)是k-1時刻的最優(yōu)結(jié)果。則有對應(yīng)于X(k│k-1)的協(xié)方差為：式中，P(k│k-1)是X(k│k-1)對應(yīng)的協(xié)方差，AT表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過程的協(xié)方差。即對系統(tǒng)的狀態(tài)更新。則狀態(tài)k的最優(yōu)化估算值X(k│k)為：其中H=[10]。卡爾曼增益(KalmanGain)卡爾曼增益公式為：此時，已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值X(k│k)。但是還要更新k狀態(tài)下X(k│k)的協(xié)方差：

可改寫為：

基于卡爾曼濾波的角度融合運用卡爾曼濾波進(jìn)行濾波和數(shù)據(jù)融合。練習(xí)簡述卡爾曼濾波運行原理。01簡述卡爾曼濾波黃金公式含義。02基于擴展卡爾曼濾的算法融合1234基于擴展卡爾曼濾的算法融合5無人機姿態(tài)解算數(shù)字濾波器-FIR數(shù)字濾波器-IIR基于卡爾曼濾波的角度融合6角速度、角度PID控制器設(shè)計基于擴展卡爾曼濾的算法融合通過學(xué)習(xí)擴展卡爾曼濾波工作原理與實戰(zhàn)演練，掌握并運用擴展卡爾曼濾波進(jìn)行濾波和數(shù)據(jù)融合。0102擴展卡爾曼濾擴展卡爾曼濾波器（ExtendedKalmanFilter）是標(biāo)準(zhǔn)卡爾曼濾波在非線性情形下的一種擴展形式，它是一種高效率的遞歸濾波器（自回歸濾波器）。EKF的基本思想是利用泰勒級數(shù)展開將非線性系統(tǒng)線性化，然后采用卡爾曼濾波框架對信號進(jìn)行濾波，因此它是一種次優(yōu)濾波。擴展卡爾曼濾波EKF卡曼濾波預(yù)測方程：A卡曼濾波更新方程：B

在卡爾曼濾波中假設(shè)AB其狀態(tài)轉(zhuǎn)移是線性的，因此我們可以直接用矩陣F表示其線性特征。其狀態(tài)和觀測都是高斯分布。推導(dǎo)卡爾曼濾波

FreeRTOS的套接字連接接收函數(shù)

求偏導(dǎo)令等于0推導(dǎo)卡爾曼增益將真實測量值代替之后化簡Σt第二種方法

計算聯(lián)合高斯分布求條件高斯分布A舒爾補恒等式展開對比C計算高斯聯(lián)合分布的指數(shù)二次項BD得到新的均值和方差求解EKF

基于擴展卡爾曼濾的算法融合運用擴展卡爾曼濾波進(jìn)行濾波和數(shù)據(jù)融合。練習(xí)簡述擴展卡爾曼濾波的原理。01簡述擴展卡爾曼濾波的實現(xiàn)過程。02無人機姿態(tài)解算1234基于擴展卡爾曼濾的算法融合5無人機姿態(tài)解算數(shù)字濾波器-FIR數(shù)字濾波器-IIR基于卡爾曼濾波的角度融合6角速度、角度PID控制器設(shè)計無人機姿態(tài)解算掌握歐拉角轉(zhuǎn)四元數(shù)、四元數(shù)轉(zhuǎn)方向余弦矩陣、靜態(tài)姿態(tài)角計算、方向余弦矩陣計算轉(zhuǎn)歐拉角、一節(jié)龍格庫塔法求解四元數(shù)以及通過磁力計與加速度計補償姿態(tài)角。0102無人機姿態(tài)解算無人機控制中，主要就是姿態(tài)解算和姿態(tài)控制部分，位置控制可用遠(yuǎn)程遙控控制，而姿態(tài)控制一般由無人機系統(tǒng)自動完成。無人機控制框圖無人機姿態(tài)解算框圖姿態(tài)解算方法的比較名詞解釋慣性測量單元IMU姿態(tài)航向參考系統(tǒng)AHRS地磁角速度重力MARG微機電系統(tǒng)MEMS可垂直起降TOL方向余弦矩陣DCM天文導(dǎo)航系統(tǒng)CNS全球?qū)Ш叫l(wèi)星系統(tǒng)GNSS自由度維數(shù)DOF無人駕駛飛行器UAV慣性導(dǎo)航系統(tǒng)INS擴展卡爾曼濾波EKF無損卡爾曼濾波UKF姿態(tài)數(shù)據(jù)重力0102地磁電子羅盤0503陀螺儀加速度計04導(dǎo)航的基本原則導(dǎo)航的基本原則就是保證兩個基本坐標(biāo)系的正確轉(zhuǎn)化，沒有誤差。只有實現(xiàn)了這個原則，載體才可以在自己的坐標(biāo)系中完成一系列動作而被轉(zhuǎn)換到地理坐標(biāo)系中看起來是正確的。AHRS和IMU的差異AHRS由加速度計、磁場計、陀螺儀構(gòu)成，AHRS的真正參考來自于地球的重力場和地球的磁場，它的靜態(tài)精度取決于對磁場的測量精度和對重力的測量精度，而陀螺儀決定了他的動態(tài)性能。IMU（Inertialmeasurementunit）學(xué)名慣性測量單元，所有的運動都可以分解為一個直線運動和一個旋轉(zhuǎn)運動，故這個慣性測量單元就是測量這兩種運動，直線運動通過加速度計可以測量，旋轉(zhuǎn)運動則通過陀螺。大地坐標(biāo)系與機體坐標(biāo)系關(guān)系定義三個單位向量歐拉角俯仰角（pitch）θ01滾轉(zhuǎn)角（橫滾角roll）φ02偏航角（方位角yaw）ψ03方向角轉(zhuǎn)動示意圖偏航角（方位角yaw）ψ俯仰角（pitch）θ滾轉(zhuǎn)角（橫滾角roll）φ飛機空間角的確定數(shù)學(xué)表達(dá)為求歐拉角姿態(tài)矩陣各次基本旋轉(zhuǎn)對應(yīng)的變換矩陣反求歐拉角四元數(shù)四元數(shù)是簡單的超復(fù)數(shù)。四元數(shù)運算四元數(shù)的形式：并且四元數(shù)與旋轉(zhuǎn)關(guān)系四元數(shù)是可以描述三維空間中剛體的旋轉(zhuǎn)計算四元數(shù)旋轉(zhuǎn)平面為垂直P點與P'點的共面且垂直于旋轉(zhuǎn)軸v，定義v0是四元數(shù)P的矢量部分P在旋轉(zhuǎn)平面上的投影矢量，v2是四元數(shù)P'的矢量部分P'在旋轉(zhuǎn)平面上的投影矢量，因此相當(dāng)于矢量v0圍繞旋轉(zhuǎn)軸v旋轉(zhuǎn)θ角度到v2。四元數(shù)的姿態(tài)估計經(jīng)運算可得

，從而得到D即為機體坐標(biāo)系b到大地坐標(biāo)系的方向余弦矩陣機體坐標(biāo)轉(zhuǎn)大地坐標(biāo)矩陣其中四元數(shù)求解方向余弦矩陣A將單位四元數(shù)代入，并得到方向余弦矩陣B采用一階龍格庫塔法，最終求得無人機姿態(tài)解算計算轉(zhuǎn)歐拉角、求解四元數(shù)以及通過磁力計與加速度計補償姿態(tài)角。角速度、角度PID控制器設(shè)計1234基于擴展卡爾曼濾的算法融合5無人機姿態(tài)解算數(shù)字濾波器-FIR數(shù)字濾波器-IIR基于卡爾曼濾波的角度融合6角速度、角度PID控制器設(shè)計角速度、角度PID控制器設(shè)計通過學(xué)習(xí)角速度、角度PID控制器設(shè)計以及參數(shù)調(diào)整，掌握PID的控制原理、參數(shù)調(diào)節(jié)以及在無人機中的應(yīng)用。0102PID算法在過程控制中，按偏差的比例（P）、積分（I）和微分（D）