天空工廠姿態(tài)解算及控制探究

姿態(tài)解算及控制天空工廠周歡班門(mén)弄斧拋磚引玉大綱幾個(gè)概念姿態(tài)解算姿態(tài)控制常見(jiàn)算法基礎(chǔ)知識(shí)PID實(shí)現(xiàn)姿態(tài)解算幾個(gè)概念方向余弦陣歐拉角四元數(shù)坐標(biāo)系坐標(biāo)系地球坐標(biāo)系機(jī)體坐標(biāo)系即我們的參考坐標(biāo)系即機(jī)體自身坐標(biāo)系歐拉角定義：構(gòu)件在三維空間中的有限轉(zhuǎn)動(dòng)，可依次用三個(gè)相對(duì)轉(zhuǎn)角表示，即進(jìn)動(dòng)角、章動(dòng)角和自旋角，這三個(gè)轉(zhuǎn)角統(tǒng)稱(chēng)為歐拉角。橫滾角俯仰角偏航角方向余弦陣坐標(biāo)系變換的關(guān)系定義：設(shè)有空間兩點(diǎn)，若以P1為始點(diǎn)，另一點(diǎn)P2為終點(diǎn)的線段稱(chēng)為有向線段.通過(guò)原點(diǎn)作一與其平行且同向的有向線段.將與Ox,Oy,Oz三個(gè)坐標(biāo)軸正向夾角分別記作α,β,γ.這三個(gè)角α,β,γ稱(chēng)為有向線段的方向角.其中0≤α≤π,0≤β≤π,0≤γ≤π.若有向線段的方向確定了，則其方向角也是唯一確定的。四元數(shù)定義：四元數(shù)是最簡(jiǎn)單的超復(fù)數(shù)。由實(shí)數(shù)加上三個(gè)元素i、j、k組成，而且它們有如下的關(guān)系：i^2=j^2=k^2=ijk=-1,每個(gè)四元數(shù)都是1、i、j和k的線性組合，即是四元數(shù)一般可表示為a+bi+cj+dk。單位四元數(shù)的對(duì)可以表示四維空間中的一個(gè)轉(zhuǎn)動(dòng)百度知道空間的位置可以由一個(gè)四元數(shù)陣確定姿態(tài)解算常見(jiàn)算法歐拉角法方向余弦法四元數(shù)法核心：從兩個(gè)坐標(biāo)系之間的變換陣求出姿態(tài)角方法：求姿態(tài)更新方程條件：加速度計(jì)——加速度

陀螺儀——角速度等效旋轉(zhuǎn)矢量法卡爾曼濾波歐拉角法微分方程解算歐拉角微分方程需要解三個(gè)微分方程但在用計(jì)算機(jī)進(jìn)行數(shù)值積分時(shí)，要進(jìn)行超越函數(shù)（三角函數(shù)）的運(yùn)算，從而加大了計(jì)算的工作量。當(dāng)載體的縱搖角（俯仰角）為90°時(shí)，將出現(xiàn)奇點(diǎn)，因此該方法不能進(jìn)行全姿態(tài)解算。方向余弦法本質(zhì)：直接求解方向余弦陣的微分，即隨時(shí)間矩陣的變化可以全角度解算姿態(tài)角，避免了歐拉法的退化四元數(shù)法空間姿態(tài)可以由四元數(shù)唯一表示與姿態(tài)角的關(guān)系等價(jià)四元數(shù)的更新難點(diǎn)：微分方程求解一階近似：歸一化初始對(duì)準(zhǔn)怎么獲得四元數(shù)？等效旋轉(zhuǎn)矢量法特別適用于角機(jī)動(dòng)頻繁激烈或存在嚴(yán)重角振動(dòng)的運(yùn)載體的姿態(tài)更新基于四元數(shù)，克服普通四元數(shù)更新算法只合適低動(dòng)態(tài)運(yùn)動(dòng)的特性幾種算法對(duì)比算法是否全姿態(tài)計(jì)算復(fù)雜度精度實(shí)用性歐拉法否（90°出現(xiàn)退化）微分方程+三角超越函數(shù)微分方程近似時(shí)有誤差小機(jī)動(dòng)條件方向余弦法是9個(gè)未知量微分方程同上計(jì)算實(shí)時(shí)性差四元數(shù)法是4個(gè)未知量微分方程同上現(xiàn)在最常用等效旋轉(zhuǎn)矢量法是微分方程誤差補(bǔ)償大機(jī)動(dòng)卡爾曼濾波卡爾曼濾波器由一系列遞歸數(shù)學(xué)公式描述。它們提供了一種高效可計(jì)算的方法來(lái)估計(jì)過(guò)程的狀態(tài)，并使估計(jì)均方誤差最小?？柭鼮V波器應(yīng)用廣泛且功能強(qiáng)大：它可以估計(jì)信號(hào)的過(guò)去和當(dāng)前狀態(tài)，甚至能估計(jì)將來(lái)的狀態(tài)，即使并不知道模型的確切性質(zhì)。知識(shí)基礎(chǔ)：狀態(tài)方程觀測(cè)方程狀態(tài)變量狀態(tài)空間方程核心目標(biāo)：離散卡爾曼濾波算法連續(xù)條件進(jìn)行離散化即可卡爾曼濾波算法關(guān)鍵建立系統(tǒng)的狀態(tài)空間方程多種方法四元數(shù)四元數(shù)的更新方程就是狀態(tài)方程，再建立觀測(cè)方程即可方向余弦法物理意義法卡爾曼濾波卡爾曼濾波floatkalman(unsignedcharaxis,floatnewAcc_Angle,floatnewReading_Gyro){ angle[axis]+=dt*(newReading_Gyro-gyro_Bias[axis]); P_00[axis]+=-dt*(P_10[axis]+P_01[axis])+dt*Q_00; P_01[axis]+=-dt*P_11[axis]; P_10[axis]+=-dt*P_11[axis]; P_11[axis]+=dt*Q_11; Err=newAcc_Angle-angle[axis]; K_0=P_00[axis]/(P_00[axis]+R); K_1=P_10[axis]/(P_00[axis]+R); angle[axis]+=K_0*Err; gyro_Bias[axis]+=K_1*Err; P_00[axis]-=K_0*P_00[axis]; P_01[axis]-=K_0*P_01[axis]; P_10[axis]-=K_1*P_00[axis]; P_11[axis]-=K_1*P_01[axis]; returnangle[axis];}姿態(tài)控制基礎(chǔ)知識(shí)比例調(diào)節(jié)積分調(diào)節(jié)微分調(diào)節(jié)在P調(diào)節(jié)中，調(diào)節(jié)器的輸出信號(hào)u與偏差信號(hào)e成比例，即，u=Kpe式中Kp稱(chēng)為比例增益比例調(diào)節(jié)P調(diào)節(jié)對(duì)偏差信號(hào)能做出及時(shí)反應(yīng)，沒(méi)有絲毫的滯后。比例調(diào)節(jié)的顯著特點(diǎn)就是有差調(diào)節(jié)。放大系數(shù)越小，余差就越大；放大系數(shù)越大，余差就越小。K過(guò)大，系統(tǒng)容易振蕩積分調(diào)節(jié)調(diào)節(jié)器的輸出信號(hào)的變化速度du/dt與偏差信號(hào)e成正比，或者說(shuō)調(diào)節(jié)器的輸出與偏差信號(hào)的積分成正比，即：只要偏差存在，調(diào)節(jié)器的輸出就會(huì)不斷變化，直到偏差為零調(diào)節(jié)器的輸出才穩(wěn)定下來(lái)不再變化。所以積分調(diào)節(jié)作用能自動(dòng)消除余差。穩(wěn)定性差，有滯后，必然產(chǎn)生超調(diào)微分控制調(diào)節(jié)器的輸出u與被調(diào)量或其偏差e對(duì)于時(shí)間的導(dǎo)數(shù)成正比，即式中，S2——微分時(shí)間。微分調(diào)節(jié)只與偏差的變化成比例，偏差變化越劇烈，由微分調(diào)節(jié)器給出的控制作用越大，從而及時(shí)地抑制偏差的增長(zhǎng)，提高系統(tǒng)的穩(wěn)定性。有超前控制的作用，但是對(duì)于系統(tǒng)擾動(dòng)會(huì)放大PID調(diào)節(jié)器//橫滾角有誤差向右滾為正

temp=FFRoll-roll; err_I_roll+=temp; temp2=Kp_roll*(temp+Ti_roll*err_I_roll+Td_roll*(temp-err_prew_roll));

if((m2>=min_v&&m2<=max_v)||(m4>=min_v&&m4<=max_v)) { m2=m2+temp2; m4=m4-temp2; } else { err_I_roll-=temp; } err_prew_roll=temp;PID實(shí)現(xiàn)謝謝！歡迎批評(píng)指正！假設(shè)我們要研究的對(duì)象是一個(gè)房間的溫度。根據(jù)你的經(jīng)驗(yàn)判斷，這個(gè)房間的溫度是恒定的，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度（假設(shè)我們用一分鐘來(lái)做時(shí)間單位）。假設(shè)你對(duì)你的經(jīng)驗(yàn)不是100%的相信，可能會(huì)有上下偏差幾度。我們把這些偏差看成是高斯白噪聲（White

Gaussian

Noise），也就是這些偏差跟前后時(shí)間是沒(méi)有關(guān)系的而且符合高斯分配（Gaussian

Distribution）。另外，我們?cè)诜块g里放一個(gè)溫度計(jì)，但是這個(gè)溫度計(jì)也不準(zhǔn)確的，測(cè)量值會(huì)比實(shí)際值偏差。我們也把這些偏差看成是高斯白噪聲。

好了，現(xiàn)在對(duì)于某一分鐘我們有兩個(gè)有關(guān)于該房間的溫度值：你根據(jù)經(jīng)驗(yàn)的預(yù)測(cè)值（系統(tǒng)的預(yù)測(cè)值）和溫度計(jì)的值（測(cè)量值）。下面我們要用這兩個(gè)值結(jié)合他們各自的噪聲來(lái)估算出房間的實(shí)際溫度值。

假如我們要估算k時(shí)刻的是實(shí)際溫度值。首先你要根據(jù)k-1時(shí)刻的溫度值，來(lái)預(yù)測(cè)k時(shí)刻的溫度。因?yàn)槟阆嘈艤囟仁呛愣ǖ模阅銜?huì)得到k時(shí)刻的溫度預(yù)測(cè)值是跟k-1時(shí)刻一樣的，假設(shè)是23度，同時(shí)該值的高斯噪聲的偏差是5度（5是這樣得到的：如果k-1時(shí)刻估算出的最優(yōu)溫度值的偏差是3，你對(duì)自己預(yù)測(cè)的不確定度是4度，他們平方相加再開(kāi)方，就是5）。然后，你從溫度計(jì)那里得到了k時(shí)刻的溫度值，假設(shè)是25度，同時(shí)該值的偏差是4度。

由于我們用于估算k時(shí)刻的實(shí)際溫度有兩個(gè)溫度值，分別是23度和25度。究竟實(shí)際溫度是多少呢？相信自己還是相信溫度計(jì)呢？究竟相信誰(shuí)多一點(diǎn)，我們可以用他們的covariance來(lái)判斷。因?yàn)镵g^2=5^2/(5^2+4^2)，所以Kg=0.78，我們可以估算出k時(shí)刻的實(shí)際溫度值是：23+0.78*(25-23)=24.56度。可以看出，因?yàn)闇囟扔?jì)的covariance比較?。ū容^相信溫度計(jì)），所以估算出的最優(yōu)溫度值偏向溫度計(jì)的值。

現(xiàn)在我們已經(jīng)得到k時(shí)刻的最優(yōu)溫度值了，下一步就是要進(jìn)入k+1時(shí)刻，進(jìn)行新的最優(yōu)估算。到現(xiàn)在為止，好像還沒(méi)看到什么自回歸的東西出現(xiàn)。對(duì)了，在進(jìn)入k+1時(shí)刻之前，我們還要算出k時(shí)刻那個(gè)最優(yōu)值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。這里的5就是上面的k時(shí)刻你預(yù)測(cè)的那