飛控算法與操作系統(tǒng)

2017飛控算法與操作系統(tǒng)南寧·中國電子教育學會·上海錫月科技有限公司無人機技術教學改革研討會01飛控算法兩種結構“×”字模式：Pitch和Roll與1,3、2,4兩組電機呈45°夾角?！笆弊帜Ｊ剑篜itch對應2,4電機的對軸，Roll對應1,3電機的對軸，夾角為0。穩(wěn)定靈活六種飛行狀態(tài)四旋翼垂直運動示意圖垂直運動在四旋翼完成4軸平衡的條件下，同時增加四個電機的輸出功率，旋翼轉速增加使得總的拉力增大，當總拉力足以克服四旋翼無人機受到的重力時，四旋翼飛行器便離地垂直上升；反之，同時減小四個電機的輸出功率，四旋翼飛行器則垂直下降，直至平衡落地，實現(xiàn)了沿z軸的垂直運動。當外界擾動量為零時，在旋翼產(chǎn)生的升力等于飛行器的自身的重力時，飛行器便保持懸停狀態(tài)。六種飛行狀態(tài)四旋翼俯仰運動示意圖俯仰運動在四旋翼完成4軸平衡的條件下，在圖5-6中，電機1、4的轉速上升，電機2、3的轉速下降（改變量大小應相等，在PID程序的實現(xiàn)中也有體現(xiàn)）。由于旋翼1、4的轉速即升力上升，旋翼2、3的轉速即升力下降，產(chǎn)生的不平衡力矩使機身繞Roll軸旋轉，同理，當電機1、4的轉速下降，電機2、3的轉速上升，機身便繞Roll軸向另一個方向運動，實現(xiàn)飛行器的俯仰運動。六種飛行狀態(tài)四旋翼翻滾運動示意圖翻滾運動在圖中，在四旋翼完成4軸平衡的條件下，提高3、4號電機的轉速，減慢1、2號電機的轉速，則可使機身繞Pitch軸的正向或者反向進行運動，實現(xiàn)飛行器的翻滾運動。

六種飛行狀態(tài)四旋翼航向運動示意圖航向運動在圖中，當電機1和電機3的轉速上升，電機2和電機4的轉速下降時，旋翼1和旋翼3對機身的反扭矩大于旋翼2和旋翼4對機身的反扭矩，機身便在富余反扭矩的作用下繞z軸轉動，實現(xiàn)飛行器的航向運動，轉向與電機1、電機3的轉向相反。六種飛行狀態(tài)四旋翼前后運動示意圖前后運動如果想要實現(xiàn)四旋翼無人機在水平面內(nèi)前后、左右的運動，必須在水平面內(nèi)對飛行器施加一定的力。在圖中，增加1、4號電機轉速，使拉力增大，相應減小2、3號電機轉速，使拉力減小，同時反扭矩仍然要保持平衡。按俯仰理論，飛行器首先發(fā)生一定程度的傾斜，從而使旋翼拉力產(chǎn)生水平分量，因此可以實現(xiàn)無人機的前飛運動。向后飛行與向前飛行正好相反。六種飛行狀態(tài)四旋翼前后運動示意圖側向運動與前后運動對稱，與翻滾運動同軸?？刂齐y點自身中心偏移自身氣流干擾外界氣流干擾多變量、非線性、強耦合欠驅動系統(tǒng)傳感器精度低、誤差積累PID控制器（比例-積分-微分控制器），由比例單元(P)、積分單元(I)和微分單元(D)組成[1]。透過Kp，Ki和Kd三個參數(shù)的設定。PID控制器主要適用于基本上線性，且動態(tài)特性不隨時間變化的系統(tǒng)。PID控制器自動控制系統(tǒng)的特點穩(wěn)定性（P和I降低系統(tǒng)穩(wěn)定性，D提高系統(tǒng)穩(wěn)定性）在平衡狀態(tài)下，系統(tǒng)受到某個干擾后，經(jīng)過一段時間其被控量可以達到某一穩(wěn)定狀態(tài)；準確性（P和I提高穩(wěn)態(tài)精度，D無作用）系統(tǒng)處于穩(wěn)態(tài)時，其穩(wěn)態(tài)誤差；快速性（P和D提高響應速度，I降低響應速度）：系統(tǒng)對動態(tài)響應的要求。一般由過渡時間的長短來衡量。自動控制系統(tǒng)的特點動態(tài)特性（暫態(tài)特性，由于系統(tǒng)慣性引起）系統(tǒng)突加給定量（或者負載突然變化）時，其系統(tǒng)輸出的動態(tài)響應曲線。延遲時間、上升時間、峰值時間、調(diào)節(jié)時間、超調(diào)量和振蕩次數(shù)。在通常情況下,上升時間和峰值時間用來評價系統(tǒng)的響應速度；超調(diào)量用來評價系統(tǒng)的阻尼程度；調(diào)節(jié)時間同時反應響應速度和阻尼程度；穩(wěn)態(tài)特性在參考信號輸出下，經(jīng)過無窮時間，其系統(tǒng)輸出與參考信號的誤差。影響因素：系統(tǒng)結構、參數(shù)和輸入量的形式等增量式PID與位置式PID增量式PID位置式PID特點：累加歷史數(shù)據(jù)輸出最終絕對加載量特點：不累計歷史數(shù)據(jù)，僅與最近三次采樣值有關只輸出控制增量分離四軸做平衡控制分離四軸做平衡控制（單級PID）純比例調(diào)節(jié)（P項）找到P0引起等幅震蕩加入微分，反向抑制震蕩，但存在一些靜差加入微弱的積分項，使靜差減小，在-1°~1°間010203分離四軸做平衡控制（單級PID）單側人為擾動后的自穩(wěn)數(shù)據(jù)分離四軸做平衡控制（單級PID）Kd過小的控制效果圖Kd過大的控制效果圖無法消除震蕩無法穩(wěn)定在某一值掩蓋了Kp調(diào)節(jié)作用使系統(tǒng)不存在分離四軸做平衡控制（單級PID）Ki過小的控制效果圖Ki過大的控制效果圖調(diào)整率緩慢引起震蕩單級PID

當前角度誤差=期望角度–實際角度;PID_P項=Kp*當前角度誤差;PID_I項+=Ki*當前角度誤差;PID_I項進行積分限幅;//當前角度的微分（原理上為當前角度誤差–上一次角度//誤差，換一個角度來講角度的微分就是角速度，所以一//般用陀螺儀的角速度數(shù)據(jù)來替代角度的微分）PID_D項=Kd*當前角速度的微分（陀螺儀數(shù)據(jù)）;單環(huán)PID總輸出=PID_P項+PID_I項+PID_D項;單環(huán)PID總輸出限幅;

編程思路串級PID

當前角度誤差=期望角度–實際角度;PID_P項=Kp*當前角度誤差;PID_I項+=Ki*當前角度誤差;PID_I項進行積分限幅;//當前角度的微分（原理上為當前角度誤差–上一次角度//誤差，換一個角度來講角度的微分就是角速度，所以一//般用陀螺儀的角速度數(shù)據(jù)來替代角度的微分）PID_D項=Kd*當前角速度的微分（陀螺儀數(shù)據(jù)）;單環(huán)PID總輸出=PID_P項+PID_I項+PID_D項;單環(huán)PID總輸出限幅;

編程思路單級PID串級PID輸出給電機期望角度比例+姿態(tài)解析角速度采樣積分微分比例積分微分+++++-內(nèi)環(huán)：角速度環(huán)外環(huán)：角度環(huán)-特點增加了控制變量，增加抗干擾性內(nèi)環(huán)粗調(diào)，外環(huán)細調(diào)內(nèi)環(huán)起到超前調(diào)節(jié)的總用，增加魯棒性提高了系統(tǒng)工作頻率，減小震蕩周期，調(diào)節(jié)時間縮短，系統(tǒng)的快速性增強串級PID當前角度誤差=期望角度–實際角度;外環(huán)PID_P項=Kp*當前角度誤差;外環(huán)PID_I項+=Ki*當前角度誤差;外環(huán)PID_I項進行積分限幅;外環(huán)PID_D項=Kd*（本次角度誤差–上次角度誤差）/控制周期;外環(huán)PID總輸出=外環(huán)PID_P項+外環(huán)PID_I項+外環(huán)PID_D項;外環(huán)PID總輸出限幅;當前角速度誤差=外環(huán)PID總輸出–當前角速度;（期望角速度為0）;內(nèi)環(huán)PID_P項=Kp*當前角速度誤差;內(nèi)環(huán)PID_I項+=Ki*當前角速度誤差;內(nèi)環(huán)PID_I項進行積分限幅;內(nèi)環(huán)PID_D項=Kd*（本次角速度誤差–上次角速度誤差）/控制周期;內(nèi)環(huán)PID總輸出=內(nèi)環(huán)PID_P項+內(nèi)環(huán)PID_I項+內(nèi)環(huán)PID_D項;內(nèi)環(huán)PID總輸出限幅;編程思路打舵實驗與PID調(diào)參角速度環(huán)打舵：觀察靜態(tài)穩(wěn)定性觀察響應率角度環(huán)打舵觀察靜態(tài)水平穩(wěn)定性觀察正負十度內(nèi)的跟蹤情況根據(jù)以上情況進行相應的PID調(diào)參位置控制中的PID——單神經(jīng)元串級PID傳統(tǒng)PID跟蹤正弦仿真神經(jīng)元PID跟蹤正弦仿真有超調(diào)跟蹤延時大幾乎無超調(diào)縮短跟蹤延時02操作系統(tǒng)使用操作系統(tǒng)的原因可移植性高大多代碼無需考慮CPU注重飛控算法本身弱化時間序列的設計給新增任務便利性任意剪裁不需要的任務工程管理性高給團隊合作與業(yè)務部署帶來便利版本管理清晰飛控任務部署RTOS定時器3中斷IMU解算姿態(tài)內(nèi)環(huán)控制45視覺數(shù)據(jù)接收10PC數(shù)據(jù)接收11位置內(nèi)環(huán)控制6250Hz位置融合7200Hz姿態(tài)融合8200Hz位置外環(huán)9125Hz數(shù)據(jù)上傳PC1240HZ超聲采集1320HZ電壓檢測1410HZ通信切換155HZ一鍵起飛162.5HZ定時器4中斷串口1、3中斷串口6中斷XXXNXXX任務優(yōu)先級觸發(fā)方式任務創(chuàng)建——示例兩種任務觸發(fā)方式——延時觸發(fā)每次任務執(zhí)行完畢，系統(tǒng)延時25ms，再次執(zhí)行來源于Task.c摘錄兩種任務觸發(fā)方式——中斷觸發(fā)等待中斷信號量到達，執(zhí)行以下任務時間到達，產(chǎn)生相應的信號量來源于Time3_Ahrs_Inner.c摘錄來源于Task.c摘錄內(nèi)環(huán)調(diào)參P太小：不能修正角速度誤差表現(xiàn)為很“軟”傾斜后難以修正，打舵響應也差。P太大：在平衡位置容易震蕩，打舵回中或給干擾（用手突加干擾）時會震蕩。合適的P：能較好的對打舵進行響應，又不太會震蕩，