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