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

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

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

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

編程思路串級(jí)PID

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

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