Pixhawk-姿态解算-互补滤波
版權聲明：本文為博主原創文章，未經博主允許不得轉載。
目錄(?)[+]
? ? ?根深方能葉茂 在等待的日子里，刻苦讀書，謙卑做人，養得深根，日后才能枝葉茂盛。 --Better(根爺)
? ??鑒于串級PID在pixhawk系統中的重要性，無論是誤差的補償，如姿態解算；還是控制的實現，如姿態控制，位置控制，靠的都是串級的pid，這里我們先對串級pid做一個介紹，后面會再接著分析，姿態的控制以及位置的解算和控制。他們的分析都還將從原理框圖和源碼注釋上說明，就是把自己平時的一點整理與大家交流一下，也希望大神能帶我飛。
? ? 這一部分說三部分內容：
? ? ?1、pid的介紹
? ? ?2、pid調參
? ? ?3、串級pid
? ? ?1、pid的介紹
? ? 在工業應用中PID及其衍生算法是應用最廣泛的算法之一，是當之無愧的萬能算法，如果能夠熟練掌握PID算法的設計與實現過程，對于一般的研發人員來講，應該是足夠應對一般研發問題了，而難能可貴的是，在我所接觸的控制算法當中，PID控制算法又是最簡單，最能體現反饋思想的控制算法，可謂經典中的經典。經典的未必是復雜的，經典的東西常常是簡單的，而且是最簡單的，簡單的不是原始的，簡單的也不是落后的，簡單到了美的程度?，F在雖然已經演變出很多智能的算法，如蟻群，神經網絡等，感興趣可以看一下劉金琨老師的《先進pid控制》，但是在實際應用中還是以串級pid為主，因為它可靠。
? ? 先看看PID算法的一般形式：
? ?PID的流程簡單到了不能再簡單的程度，通過誤差信號控制被控量，而控制器本身就是比例、積分、微分三個環節的加和。這里我們規定（在t時刻）：
?? 1.輸入量為rin(t);
?? 2.輸出量為rout(t);
?? 3.偏差量為err(t)=rin(t)-rout(t);
?? pid的控制規律為
? ?1.說明一下反饋控制的原理，通過上面的框圖不難看出，PID控制其實是對偏差的控制過程;
?? 2.如果偏差為0,則比例環節不起作用，只有存在偏差時，比例環節才起作用。
?? 3.積分環節主要是用來消除靜差，所謂靜差，就是系統穩定后輸出值和設定值之間的差值，積分環節實際上就是偏差累計的過程，把累計的誤差加到原有系統上以抵消系統造成的靜差。
?? 4.而微分信號則反應了偏差信號的變化規律，或者說是變化趨勢，根據偏差信號的變化趨勢來進行超前調節，從而增加了系統的快速性。
? ?
? ? 下面將對PID連續系統離散化，從而方便在處理器上實現。下面把連續狀態的公式再貼一下：
? ??假設采樣間隔為T，則在第K T時刻：
? ? 偏差err(K)=rin(K)-rout(K);
? ? 積分環節用加和的形式表示，即err(K)+err(K+1)+……;
? ? 微分環節用斜率的形式表示，即[err(K)-err(K-1)]/T;
? ? 從而形成如下PID離散表示形式：
? ? 則u(K)可表示成為：
? ?至于說Kp、Ki、Kd三個參數的具體表達式，我想可以輕松的推出了，這里節省時間，不再詳細表示了。
其實到這里為止，PID的基本離散表示形式已經出來了。目前的這種表述形式屬于位置型PID，另外一種表述方式為增量式PID，由U上述表達式可以輕易得到：
? ? 那么：
? ? 這就是離散化PID的增量式表示方式，由公式可以看出，增量式的表達結果和最近三次的偏差有關，這樣就大大提高了系統的穩定性。需要注意的是最終的輸出結果應該為
???????u(K)+增量調節值;
? ? ?PID的離散化過程基本思路就是這樣，下面是將離散化的公式轉換成為C語言，從而實現微控制器的控制作用。
那么如何用c語言進行表達？下面將對pid一種常見的形式進行c語言的描述，注意他們的演變過程，在pixhawk中也用到這其中的一些注意事項，如積分分離。
? ? ?
位置型PID的C語言實現：
第一步：定義PID變量結構體，代碼如下：
struct _pid{
?? ?float SetSpeed;?? ??? ??? ?//定義設定值
?? ?float ActualSpeed;?? ??? ?//定義實際值
?? ?float err;?? ??? ??? ??? ?//定義偏差值
?? ?float err_last;?? ??? ??? ?//定義上一個偏差值
?? ?float Kp,Ki,Kd;?? ??? ??? ?//定義比例、積分、微分系數
?? ?float voltage;?? ??? ???//定義電壓值（控制執行器的變量）
?? ?float integral;?? ??? ??? ?//定義積分值
}pid;
? ?控制算法中所需要用到的參數在一個結構體中統一定義，方便后面的使用。
第二部：初始化變量，代碼如下：
void PID_init(){
?? ?printf("PID_init begin \n");
?? ?pid.SetSpeed=0.0;
?? ?pid.ActualSpeed=0.0;
?? ?pid.err=0.0;
?? ?pid.err_last=0.0;
?? ?pid.voltage=0.0;
?? ?pid.integral=0.0;
?? ?pid.Kp=0.2;
?? ?pid.Ki=0.015;
?? ?pid.Kd=0.2;
?? ?printf("PID_init end \n");
}
? ? 統一初始化變量，尤其是Kp,Ki,Kd三個參數，調試過程當中，對于要求的控制效果，可以通過調節這三個量直接進行調節。
第三步：編寫控制算法，代碼如下：
float PID_realize(float speed){
??? pid.SetSpeed=speed;
??? pid.err=pid.SetSpeed-pid.ActualSpeed;
??? pid.integral+=pid.err;//位置式pid是對積分的持續累加，容易造成積分飽和，是系統過調
??? pid.voltage=pid.Kp*pid.err+pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
??? pid.err_last=pid.err;
??? pid.ActualSpeed=pid.voltage*1.0;
??? return pid.ActualSpeed;
}
注意：這里用了最基本的算法實現形式，沒有考慮死區問題，沒有設定上下限，只是對公式的一種直接的實現，后面的介紹當中還會逐漸的對此改進。
?? 到此為止，PID的基本實現部分就初步完成了。下面是測試代碼：
int main(){
??? printf("System begin \n");
??? PID_init();
??? int count=0;
??? while(count<1000)
??? {
??? ??? float speed=PID_realize(200.0);
??? ??? printf("%f\n",speed);
??? ??? count++;
??? }
return 0;
}
增量型PID的C語言實現：
實現過程仍然是分為定義變量、初始化變量、實現控制算法函數、算法測試四個部分，
#include<stdio.h>
#include<stdlib.h>
struct _pid{
?? ?float SetSpeed;?? ??? ??? ?//定義設定值
?? ?float ActualSpeed;?? ??? ?//定義實際值
?? ?float err;?? ??? ??? ??? ?//定義偏差值
?? ?float err_next;?? ??? ??? ?//定義上一個偏差值
?? ?float err_last;?? ??? ??? ?//定義最上前的偏差值
?? ?float Kp,Ki,Kd;?? ??? ??? ?//定義比例、積分、微分系數
}pid;
void PID_init(){
?? ?pid.SetSpeed=0.0;
?? ?pid.ActualSpeed=0.0;
?? ?pid.err=0.0;
?? ?pid.err_last=0.0;
?? ?pid.err_next=0.0;
?? ?pid.Kp=0.2;
?? ?pid.Ki=0.015;
?? ?pid.Kd=0.2;
}
float PID_realize(float speed){
?? ?pid.SetSpeed=speed;
?? ?pid.err=pid.SetSpeed-pid.ActualSpeed;
?? ?float incrementSpeed=pid.Kp*(pid.err-pid.err_next)+pid.Ki*pid.err+pid.Kd*(pid.err-2*pid.err_next+pid.err_last);//只和前后三次的誤差值有關，也方便計算
?? ?pid.ActualSpeed+=incrementSpeed;
?? ?pid.err_last=pid.err_next;
?? ?pid.err_next=pid.err;
?? ?return pid.ActualSpeed;
}
int main(){
?? ?PID_init();
?? ?int count=0;
?? ?while(count<1000)
?? ?{
?? ??? ?float speed=PID_realize(200.0);
?? ??? ?printf("%f\n",speed);
?? ??? ?count++;
?? ?}
?? ?return 0;
}
積分分離的PID控制算法C語言實現：
? ? 在普通PID控制中，引入積分環節的目的，主要是為了消除靜差，提高控制精度。但是在啟動、結束或大幅度增減設定時，短時間內系統輸出有很大的偏差，會造成PID運算的積分積累，導致控制量超過執行機構可能允許的最大動作范圍對應極限控制量，從而引起較大的超調，甚至是震蕩，這是絕對不允許的。
?? 為了克服這一問題，引入了積分分離的概念，其基本思路是當被控量與設定值偏差較大時，取消積分作用; 當被控量接近給定值時，引入積分控制，以消除靜差，提高精度。其具體實現代碼如下：
??? pid.Kp=0.2;
??? pid.Ki=0.04;
??? pid.Kd=0.2;? //初始化過程
if(abs(pid.err)>200)
?? ?{
?? ?index=0;
?? ?}else{
?? ?index=1;
?? ?pid.integral+=pid.err;
?? ?}
?? ?pid.voltage=pid.Kp*pid.err+index*pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);????
//算法具體實現過程可參考上面的
抗積分飽和的PID控制算法C語言實現：
? ??所謂的積分飽和現象是指如果系統存在一個方向的偏差，PID控制器的輸出由于積分作用的不斷累加而加大，從而導致執行機構達到極限位置，若控制器輸出U(k)繼續增大，執行器開度不可能再增大，此時計算機輸出控制量超出了正常運行范圍而進入飽和區。一旦系統出現反向偏差，u(k)逐漸從飽和區退出。進入飽和區越深則退出飽和區時間越長。在這段時間里，執行機構仍然停留在極限位置而不隨偏差反向而立即做出相應的改變，這時系統就像失控一樣，造成控制性能惡化，這種現象稱為積分飽和現象或積分失控現象。
??? 防止積分飽和的方法之一就是抗積分飽和法，該方法的思路是在計算u(k)時，首先判斷上一時刻的控制量u(k-1)是否已經超出了極限范圍： 如果u(k-1)>umax，則只累加負偏差; 如果u(k-1)<umin，則只累加正偏差。從而避免控制量長時間停留在飽和區。直接貼出代碼，不懂的看看前面幾節的介紹。
float PID_realize(float speed){
?? ?int index;
?? ?pid.SetSpeed=speed;
?? ?pid.err=pid.SetSpeed-pid.ActualSpeed;
???if(pid.ActualSpeed>pid.umax)? //灰色底色表示抗積分飽和的實現
?? ?{
?? ????if(abs(pid.err)>200)?? ? ?//藍色標注為積分分離過程
?? ??? ?{
?? ??? ??? ?index=0;
?? ??? ?}else{
?? ??? ??? ?index=1;
?? ??? ??? ?if(pid.err<0)
?? ??? ??? ?{//如果超上限要嘛加負值要嘛就不加了，免得進入飽和區
?? ??? ??? ? ?pid.integral+=pid.err;?? ??? ???
????????????????????}
?? ??? ?}
?? ?}else if(pid.ActualSpeed<pid.umin){
?? ??? ?if(abs(pid.err)>200)?? ???//積分分離過程
?? ??? ?{
?? ??? ??? ?index=0;
?? ??? ?}else{
?? ??? ??? ?index=1;
?? ??? ??? ?if(pid.err>0)
?? ??? ??? ?{//如果超下限要嘛加正值要嘛就不加了免得進入飽和區
?? ??? ??? ?pid.integral+=pid.err;
?? ??? ??? ?}
?? ??? ?}
?? ?}else{
?? ??? ?if(abs(pid.err)>200)?? ??? ??? ??? ??? ?//積分分離過程
?? ??? ?{
?? ??? ??? ?index=0;
?? ??? ?}else{
?? ??? ??? ?index=1;
?? ??? ??? ?pid.integral+=pid.err;
?? ??? ?}
?? ?}
?? ?pid.voltage=pid.Kp*pid.err+index*pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
?? ?pid.err_last=pid.err;
?? ?pid.ActualSpeed=pid.voltage*1.0;
?? ?return pid.ActualSpeed;
}
變積分的PID控制算法C語言實現：
? ?變積分PID可以看成是積分分離的PID算法的更一般的形式。在普通的PID控制算法中，由于積分系數ki是常數，所以在整個控制過程中，積分增量是不變的。但是，系統對于積分項的要求是，系統偏差大時，積分作用應該減弱甚至是全無，而在偏差小時，則應該加強。積分系數取大了會產生超調，甚至積分飽和，取小了又不能短時間內消除靜差。因此，根據系統的偏差大小改變積分速度是有必要的。
?? 變積分PID的基本思想是設法改變積分項的累加速度，使其與偏差大小相對應：偏差越大，積分越慢; 偏差越小，積分越快。
?? 這里給積分系數前加上一個比例值index：
?? 當abs(err)<180時，index=1;
?? 當180<abs(err)<200時，index=（200-abs(err)）/20;
?? 當abs(err)>200時，index=0;
???最終的比例環節的比例系數值為ki*index;
?float PID_realize(float speed){
?? ?float index;
?? ?pid.SetSpeed=speed;
?? ?pid.err=pid.SetSpeed-pid.ActualSpeed;
?? ?if(abs(pid.err)>200)?? ??? ?? ?//變積分過程
?? ?{
?? ?index=0.0;
?? ?}else if(abs(pid.err)<180){
?? ?index=1.0;
?? ?pid.integral+=pid.err;
?? ?}else{
?? ?index=(200-abs(pid.err))/20;
?? ?pid.integral+=pid.err;
?? ?}
?? ?pid.voltage=pid.Kp*pid.err+index*pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
?? ?pid.err_last=pid.err;
?? ?pid.ActualSpeed=pid.voltage*1.0;
?? ?return pid.ActualSpeed;
}
? ? 最后給出大家專家系統中控制經驗，自己理解吧。
? ? 反應系統性能的兩個參數是系統誤差e和誤差變化律ec，
? ? 首先我們規定一個誤差的極限值，假設為Mmax；規定一個誤差的比較大的值，假設為Mmid；規定一個誤差的較小值，假設為Mmin；
? ? ?e*ec>0??誤差在朝向誤差絕對值增大的方向變化(可以理解成速度和加速度)
? ? ? ? ? ? ?若此時 abs(e)>Mmid?：誤差較大?強控制?
? ? ? ? ? ? ?若此時?abs(e)<Mmid?：誤差絕對值本身并不是很大?一般的控制作用
? ? ?e*ec<0??誤差在朝向誤差絕對值減小的方向變化
? ? ? ? ? ? ?若此時 e*err(k-1)>0或者e=0?：誤差的絕對值向減小的方向變化，或者已經達到平衡狀態，
?? ? ? ? ? ? ? ? ? ?此時保持控制器輸出不變即可。?
? ? ? ? ? ? ?若此時e*err(k-1)<0?：?誤差處于極限狀態。如果誤差的絕對值>min，強控制 （調節幅度比較大） ? ? ? ? ? ? ? ? ? ? ? 如果此時誤差絕對值較小，可以考慮實施較弱控制作用。
? ? ?當abs（e）>Mmax時，說明誤差的絕對值已經很大了，都應該考慮控制器的輸入應按最大（或最小） 輸出，以 ? ? ? ? ? ? ? ? ? ? ? ? ?達到迅速調整誤差的效果，使誤差絕對值以最大的速度減小。
? ? ?當abs(e)<Mmin時，說明誤差絕對值很小，此時加入積分，減小靜態誤差。
2、pid調參你怎么看
1）．PID調試一般原則?
a.在輸出不振蕩時，增大比例增益P。?
b.在輸出不振蕩時，減小積分時間常數Ti。?
c.在輸出不振蕩時，增大微分時間常數Td。
2）．一般步驟
?a.確定比例增益P ：確定比例增益P 時，首先去掉PID的積分項和微分項，一般是令Ti=0、Td=0（具體見PID的參數設定說明），使PID為純比例調節。輸入設定為系統允許的最大值的60%~70%，由0逐漸加大比例增益P，直至系統出現振蕩；再反過來，從此時的比例增益P逐漸減小，直至系統振蕩消失，記錄此時的比例增益P，設定PID的比例增益P為當前值的60%~70%。比例增益P調試完成。
b.確定積分時間常數Ti比例增益P確定后，設定一個較大的積分時間常數Ti的初值，然后逐漸減小Ti，直至系統出現振蕩，之后在反過來，逐漸加大Ti，直至系統振蕩消失。記錄此時的Ti，設定PID的積分時間常數Ti為當前值的150%~180%。積分時間常數Ti調試完成。
?d.系統空載、帶載聯調，再對PID參數進行微調，直至滿足要求：理想時間兩個波，前高后低4比
3、串級pid簡介
? ? 串級pid內外兩環并聯調節，這樣的好處的是增加系統的穩定性，抗干擾。同時調節系統緩慢過度，注意外環都是本身誤差，內環是速度，如位置控制外環是位置，內環是速度，是因為位置改變的實現是靠三個方向的速度積分出來的。同樣姿態控制中，外環是角度差，內環是加速度，是因為角度的實現是靠角速度過渡來的，他們都是這樣的一個過渡過程。實際中如果你追求響應的快捷，你也可以直接控制內環，或者直接控制姿態。
? ? 串級PID兩個PID控制算法，只不過把他們串起來了(更精確的說是套起來)。那這么做有什么用？答案是，它增強了系統的抗干擾性(也就是增強穩定性)，因為有兩個控制器控制飛行器，它會比單個控制器控制更多的變量，使得飛行器的適應能力更強。畫出串級PID的原理框圖，
? ? 在整定串級PID時的經驗則是：先整定內環PID，再整定外環P。因為內環靠近輸出，效果直接。
? ? 內環P：從小到大，拉動四軸越來越困難，越來越感覺到四軸在抵抗你的拉動；到比較大的數值時，四軸自己會高頻震動，肉眼可見，此時拉扯它，它會快速的振蕩幾下，過幾秒鐘后穩定；繼續增大，不用加人為干擾，自己發散翻機。
? ? 特別注意：只有內環P的時候，四軸會緩慢的往一個方向下掉，這屬于正常現象。這就是系統角速度靜差。
? ? 內環I：前述PID原理可以看出，積分只是用來消除靜差，因此積分項系數個人覺得沒必要弄的很大，因為這樣做會降低系統穩定性。從小到大，四軸會定在一個位置不動，不再往下掉；繼續增加I的值，四軸會不穩定，拉扯一下會自己發散。
? ? 特別注意：增加I的值，四軸的定角度能力很強，拉動他比較困難，似乎像是在釘釘子一樣，但是一旦有強干擾，它就會發散。這是由于積分項太大，拉動一下積分速度快，給??的補償非常大，因此很難拉動，給人一種很穩定的錯覺。
? ? 內環D：這里的微分項D為標準的PID原理下的微分項，即本次誤差-上次誤差。在角速度環中的微分就是角加速度，原本四軸的震動就比較強烈，引起陀螺的值變化較大，此時做微分就更容易引入噪聲。因此一般在這里可以適當做一些滑動濾波或者IIR濾波。從小到大，飛機的性能沒有多大改變，只是回中的時候更加平穩；繼續增加D的值，可以肉眼看到四軸在平衡位置高頻震動(或者聽到電機發出滋滋的聲音)。前述已經說明D項屬于輔助性項，因此如果機架的震動較大，D項可以忽略不加。
? ?外環P：當內環PID全部整定完成后，飛機已經可以穩定在某一位置而不動了。此時內環P，從小到大，可以明顯看到飛機從傾斜位置慢慢回中，用手拉扯它然后放手，它會慢速回中，達到平衡位置；繼續增大P的值，用遙控器給不同的角度給定，可以看到飛機跟蹤的速度和響應越來越快；繼續增加P的值，飛機變得十分敏感，機動性能越來越強，有發散的趨勢。
4、最后給你貼上pixhawk有關pid的源碼，就是位置式的很簡單，自己理解一下吧。需要說明的是位置式pid容易導致積分的飽和，所以在積分上過了很多處理。如在位置控制中，推力的積分量就是進行了飽和處理。
[html] view plaincopyprint?
總結
以上是生活随笔為你收集整理的Pixhawk-姿态解算-互补滤波的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： linux下使用rdesktop连接远程
- 下一篇： Main函数中的argc和argv应用举