飛控算法講解

SuccessAlwaysBelongsToThosePrepared!DroneTechnologySeminarInshanghai無人機技術研討會錫月科技曹君超飛控算法01兩種結構“×”字模式：Pitch和Roll與1,3、2,4兩組電機呈45°夾角。“十”字模式：Pitch對應2,4電機的對軸，Roll對應1,3電機的對軸，夾角為0。穩(wěn)定靈活四旋翼飛行狀態(tài)思考一下：四個電機轉速完全相同，無人機能平衡嘛？答：四旋翼重心不一定在中心上，所以無法平衡。達到平衡狀態(tài)后不改變四個電機的轉速，無人機能一直平衡下去嘛？

答：因為有外界的干擾，四旋翼四個旋翼的轉速要一直處于調節(jié)狀態(tài)才能保持平衡。六種飛行狀態(tài)四旋翼垂直運動示意圖垂直運動在四旋翼完成4軸平衡的條件下，同時增加四個電機的輸出功率，旋翼轉速增加使得總的拉力增大，當總拉力足以克服四旋翼無人機受到的重力時，四旋翼飛行器便離地垂直上升；反之，同時減小四個電機的輸出功率，四旋翼飛行器則垂直下降，直至平衡落地，實現(xiàn)了沿z軸的垂直運動。當外界擾動量為零時，在旋翼產生的升力等于飛行器的自身的重力時，飛行器便保持懸停狀態(tài)。六種飛行狀態(tài)四旋翼俯仰運動示意圖俯仰運動

在四旋翼完成4軸平衡的條件下，在圖中，電機1、4的轉速上升，電機2、3的轉速下降（改變量大小應相等，在PID程序的實現(xiàn)中也有體現(xiàn)）。由于旋翼1、4的轉速即升力上升，旋翼2、3的轉速即升力下降，產生的不平衡力矩使機身繞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)四旋翼無人機在水平面內前后、左右的運動，必須在水平面內對飛行器施加一定的力。在圖中，增加1、4號電機轉速，使拉力增大，相應減小2、3號電機轉速，使拉力減小，同時反扭矩仍然要保持平衡。按俯仰理論，飛行器首先發(fā)生一定程度的傾斜，從而使旋翼拉力產生水平分量，因此可以實現(xiàn)無人機的前飛運動。向后飛行與向前飛行正好相反。六種飛行狀態(tài)四旋翼前后運動示意圖側向運動與前后運動對稱，與翻滾運動同軸。無人機狀態(tài)空間方程控制原理Throttle_Info.M1=-pidRatePitch.value-pidRateRoll.value+pidAccHeight.value-pidRateYaw.value+throttleBasic;

Throttle_Info.M2=+pidRatePitch.value-pidRateRoll.value+pidAccHeight.value+pidRateYaw.value+throttleBasic;Throttle_Info.M3=+pidRatePitch.value+pidRateRoll.value+pidAccHeight.value-pidRateYaw.value+throttleBasic;

Throttle_Info.M4=-pidRatePitch.value+pidRateRoll.value+pidAccHeight.value+pidRateYaw.value+throttleBasic;X型的四旋翼每個對軸由四個電機同時控制，根據電機序號的標定，我們可以將3個歐拉角的控制量分解到4個電機上，高度的控制量四個電機是一致的。最后需要給每個電機加上一個油門基礎量。四旋翼控制的油門基礎量油門基礎量的選擇：大約占電機最高轉速的70%左右。能夠提供給四旋翼一個起飛的油門，注意并不是靠地效的反作用力是四旋翼脫離地面的量。油門基礎量與控制量的占比：

油門基礎量占比為80%左右或以上，控制量只是根據外界干擾和重心偏移所修正的一個油門轉速，所以只需要較小的控制量就能使四旋翼保持平衡。思考：如果油門基礎量較小，控制量較大，會發(fā)生什么情況？答：一旦控制飛行時，調節(jié)平衡能力就較差，且受到外界干擾后，控制量較大，四旋翼處于較不穩(wěn)定狀態(tài)。控制難點自身中心偏移自身氣流干擾外界氣流干擾多變量、非線性、強耦合欠驅動系統(tǒng)傳感器精度低、誤差積累PID控制器（比例-積分-微分控制器），由比例單元(P)、積分單元(I)和微分單元(D)組成。透過Kp，Ki和Kd三個參數的設定。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)受到某個干擾后，經過一段時間其被控量可以達到某一穩(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)響應曲線。延遲時間、上升時間、峰值時間、調節(jié)時間、超調量和振蕩次數。在通常情況下,上升時間和峰值時間用來評價系統(tǒng)的響應速度；超調量用來評價系統(tǒng)的阻尼程度；調節(jié)時間同時反應響應速度和阻尼程度；穩(wěn)態(tài)特性在參考信號輸出下，經過無窮時間，其系統(tǒng)輸出與參考信號的誤差。影響因素：系統(tǒng)結構、參數和輸入量的形式等分離四軸做平衡控制分離四軸做平衡控制（單級PID）純比例調節(jié)（P項）找到P0引起等幅震蕩加入微分，反向抑制震蕩，但存在一些靜差加入微弱的積分項，使靜差減小，在-1°~1°間010203分離四軸做平衡控制（單級PID）單側人為擾動后的自穩(wěn)數據分離四軸做平衡控制（單級PID）Kd過小的控制效果圖Kd過大的控制效果圖無法消除震蕩無法穩(wěn)定在某一值掩蓋了Kp調節(jié)作用使系統(tǒng)不存在分離四軸做平衡控制（單級PID）Ki過小的控制效果圖Ki過大的控制效果圖調整率緩慢引起震蕩單級PID

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

編程思路單級PID例程請見：Single-loopPID_control

CUSTOMIZEConvenientlyiteratetop-linealignmentsforwirelessmetrics.姿態(tài)控制要點輸出給電機期望角度比例+姿態(tài)解析角速度采樣積分微分比例積分微分+++++-內環(huán)：角速度環(huán)外環(huán)：角度環(huán)-特點增加了控制變量，增加抗干擾性內環(huán)粗調，外環(huán)細調內環(huán)起到超前調節(jié)的總用，增加魯棒性提高了系統(tǒng)工作頻率，減小震蕩周期，調節(jié)時間縮短，系統(tǒng)的快速性增強串級PID

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

編程思路串級PID輸出給電機期望角度比例+姿態(tài)解析角速度采樣積分微分比例積分微分+++++-內環(huán)：角速度環(huán)外環(huán)：角度環(huán)-特點增加了控制變量，增加抗干擾性內環(huán)粗調，外環(huán)細調內環(huán)起到超前調節(jié)的總用，增加魯棒性提高了系統(tǒng)工作頻率，減小震蕩周期，調節(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）;內環(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總輸出限幅;編程思路打舵實驗與PID調參角速度環(huán)打舵：觀察靜態(tài)穩(wěn)定性觀察響應率角度環(huán)打舵觀察靜態(tài)水平穩(wěn)定性觀察正負十度內的跟蹤情況根據以上情況進行相應的PID調參例程請見：Doubleclosed-loopPID_control

CUSTOMIZEConvenientlyiteratetop-linealignmentsforwirelessmetrics.PID參數整定要點角速度環(huán)打舵：觀察靜態(tài)穩(wěn)定性觀察響應率角度環(huán)打舵觀察靜態(tài)水平穩(wěn)定性觀察正負十度內的跟蹤情況P太小：不能修正角速度誤差表現(xiàn)為很“軟”傾斜后難以修正，打舵響應也差。P太大：在平衡位置容易震蕩，打舵回中或給干擾（用手突加干擾）時會震蕩。合適的P：能較好的對打舵進行響應，又不太會震蕩，但是舵量回中后會回彈好幾下才能停止（沒有D）。D的效果：加快打舵響應，最大的作用是能很好地抑制舵量回中后的震蕩，可謂立竿見影。D太大：會在橫滾俯仰混控時表現(xiàn)出來（盡管在“烤四軸”時的表現(xiàn)可能很好），具體表現(xiàn)是四軸抓在手里推油門會抽搐。如果這樣，只能回到“烤四軸”降低D，同時P也只能跟著降低。D調整完后可以再次加大P值，以能夠跟隨打舵為判斷標準。加入I：由于重心偏高，質量不均造成偏離，I可以一定程度上修正偏離。

CUSTOMIZEConvenientlyiteratetop-linealignmentsforwirelessmetrics.高度控制要點懸停油門=加速度環(huán)控制量（積分起主導作用）+電機參數表單電壓下所對應的懸停油門注*（如果電機參數曲線為各個電壓下，可由參數表直接實時算出懸停油門）此時輸出Z軸的升力F=mg

當需要輸出a（OriginalAccZ.value）的加速度時，輸出力F1=m(g+a)注*（如果整機質量已知，即可算出每個電機需要提供的升力，并通過電機參數表曲線將升力轉換為對應油門）此處因為要通用于不同軸距的多個機型，所以m、F與油門的關系量都未知，因此有以下關系式：F1/F=1+a/g因此此時應輸出：懸停油門*(1+a/g)*/UAVThrust.HeightThrust=OriginalAccZ.value+UAVThrust.BasicThrust;觀測量反饋算法這個算法需要明確的是所需的高度信息是地理坐標系下的相對高度，整個算法的核心思想是由地理坐標系下的加速度通過積分，來獲得速度、位置信息，而這個數據的精確程度是由機體測量的加速度通過減去偏差，再轉換到地理坐標系求得的。這里超聲波的作用就是計算一個校正系數來對加速度偏移量進行校正?？柭鼮V波器卡爾曼黃金五條公式觀測量反饋濾波高度數據融合加計數據以60HZ頻率從US100超聲波模塊獲取原始高度數據高度數據獲取中位值濾波簡單濾波處理實時高度以500HZ頻率從MPU6500獲取原始加速計數據加速計數據獲取均值濾波簡單濾波處理實時速度用于控制用于控制像素點到實際距離的轉換像素點焦距實際高度實際距離需要已知條件：1、CCD或CMOS大小2、焦距3、像素4、同步的高度數據IMU與圖像數據融合卡爾曼濾波觀測量反饋濾波三軸加速計數據加速計數據XY軸數據攝像頭數據（轉換到單位M）XY二維圖像位置信息濾波獲得x軸的實時位置和速度信息X軸位置和速度濾波獲得y軸的實時位置和速度信息Y軸位置和速度濾波獲得z軸的實時位置和速度信息Z軸位置和速度1、融合頻率合適過高會產生高延遲2、R參數為傳感器誤差的協(xié)方差3、Q參數為預測量的協(xié)方差4、R參數不變，增大Q是相信預測模型，反之則相信觀測量?？柭鼮V波要點

CUSTOMIZEConvenien