本發(fā)明涉及無(wú)人機(jī)用空速估計(jì)和空速管故障檢測(cè)方法，屬于航空傳感器故障檢測(cè)領(lǐng)域，應(yīng)用于采用多傳感器(IMU，GPS，大氣數(shù)據(jù)機(jī)，空速管)的無(wú)人機(jī)平臺(tái)，能快速檢測(cè)空速管發(fā)生的故障。
背景技術(shù)：
：無(wú)人機(jī)由于其經(jīng)濟(jì)性和機(jī)動(dòng)性優(yōu)勢(shì)受到越來(lái)越廣泛的應(yīng)用。無(wú)人機(jī)是一個(gè)典型的多傳感器系統(tǒng)，包括IMU，GPS，空速管，磁羅盤等傳感器，這些傳感器為無(wú)人機(jī)的飛行控制和導(dǎo)航提供重要信息，因此，傳感器的可靠性是保證無(wú)人機(jī)正常飛行的基礎(chǔ)。無(wú)人機(jī)空速的測(cè)量對(duì)于無(wú)人機(jī)飛行至關(guān)重要，錯(cuò)誤的空速信息會(huì)輸出錯(cuò)誤的飛行包絡(luò)線，從而導(dǎo)致無(wú)人機(jī)墜毀。常規(guī)意義下的空速測(cè)量是利用空速管測(cè)量飛行動(dòng)壓，大氣數(shù)據(jù)計(jì)算機(jī)將其轉(zhuǎn)換為速度信號(hào)。而比起相對(duì)可靠的電路系統(tǒng)，空速管經(jīng)常會(huì)受到異物堵塞或結(jié)冰影響而不能正常輸出動(dòng)壓信息，從而導(dǎo)致空速輸出故障。因此，檢測(cè)并且隔離空速管的故障對(duì)于無(wú)人機(jī)飛行至關(guān)重要。對(duì)商用大型飛機(jī)而言，通常解決這一問(wèn)題的方法是采取硬件冗余技術(shù)，即多個(gè)空速管冗余使用提高整體的可靠性。然而，基于無(wú)人機(jī)的尺寸以及經(jīng)濟(jì)性考慮，此方案并不是最佳選擇。因此，利用基于解析冗余的故障檢測(cè)方法是無(wú)人機(jī)空速管故障檢測(cè)的有效方案。2012年IEEEWorkshopEESMS會(huì)議，M.Fravllini等人的論文“Modelbasedapproachesfortheairspeedestimationandfaultmonitoringofanunmannedaerialvehicles”利用攻角模型估計(jì)空速，并通過(guò)與空速管的測(cè)量值比較來(lái)檢測(cè)空速管是否發(fā)生故障。但是此方法要求準(zhǔn)確的無(wú)人機(jī)模型，即具體的空氣動(dòng)力參數(shù)，因此針對(duì)不同的無(wú)人機(jī)需要修改模型參數(shù)，方法使用受限。2011年IEEEAerospaceandElectronicSystems期刊，A.Cho等人的論文“WindestimationandairspeedcalibrationusingaUAVwithasingle-antennaGPSreceiverandpitottube”利用空速三角的幾何關(guān)系，并且融合了GPS和空速管輸出信息，估計(jì)出空速并檢測(cè)空速管故障。但是此方法在估計(jì)的過(guò)程中使用了空速管的信息，因此無(wú)法保證故障信號(hào)的獨(dú)立性?，F(xiàn)有的基于解析冗余的空速管故障檢測(cè)方法都是利用無(wú)人機(jī)空氣動(dòng)力模型，其模型中的參數(shù)需要通過(guò)辨識(shí)獲得，對(duì)于不同型號(hào)的無(wú)人機(jī)使用受限。同時(shí)，為了獲得準(zhǔn)確的估計(jì)結(jié)果，常常利用空速管的信息作為量測(cè)量，因此會(huì)導(dǎo)致故障信號(hào)與正常輸出信號(hào)分辨困難。技術(shù)實(shí)現(xiàn)要素：本發(fā)明解決的問(wèn)題是：無(wú)人機(jī)在飛行的過(guò)程中，空速管容易受異物堵塞或結(jié)冰而出現(xiàn)故障，通常的硬件冗余技術(shù)在在重量輕、結(jié)構(gòu)簡(jiǎn)單的無(wú)人機(jī)平臺(tái)使用困難，而基于無(wú)人機(jī)動(dòng)力學(xué)模型的解析冗余故障檢測(cè)方法又依賴特定的無(wú)人機(jī)型號(hào)，從而限制其適用范圍。因此建立不依賴特定尺寸和型號(hào)無(wú)人機(jī)的模型估計(jì)空速，同時(shí)估計(jì)空速與空速管輸出信息獨(dú)立，并通過(guò)與空速管輸出比較來(lái)檢測(cè)空速管故障。本發(fā)明的技術(shù)解決方案為：一種無(wú)人機(jī)空速估計(jì)和空速管故障檢測(cè)方法，包括以下步驟：步驟1：定義載體坐標(biāo)系oxbybzb和北東地導(dǎo)航坐標(biāo)系oxnynzn。步驟2：無(wú)人機(jī)飛控系統(tǒng)采集各傳感器輸出信息：IMU輸出角速率ω和加速度信息a，GPS輸出速度信息vgps，INS/GPS組合導(dǎo)航系統(tǒng)輸出姿態(tài)信息迎角/側(cè)滑角傳感器輸出風(fēng)角信息α與β，空速管輸出空速信息Vm。步驟3：利用IMU輸出解算無(wú)人機(jī)姿態(tài)信息同時(shí)融合GPS輸出信息對(duì)姿態(tài)信息進(jìn)行估計(jì)補(bǔ)償。步驟4：以角速率ω、加速度a以及姿態(tài)信息為系統(tǒng)輸入，建立狀態(tài)方程。步驟5：以GPS速度和迎角α、側(cè)滑角β為量測(cè)值，建立量測(cè)方程。步驟6：根據(jù)上述空速估計(jì)模型，利用EKF估計(jì)無(wú)人機(jī)的空速步驟7：獲取估計(jì)空速與空速管量測(cè)空速Vm之間的殘差r，利用累加圖檢測(cè)統(tǒng)計(jì)量是否超出閾值，并由此檢測(cè)空速管的故障。其中，步驟1所述的載體坐標(biāo)系oxbybzb和北東地導(dǎo)航坐標(biāo)系oxnynzn分別定義為：載體坐標(biāo)系oxbybzb定義：原點(diǎn)o位于飛機(jī)的質(zhì)心，oxb軸與載體運(yùn)動(dòng)方向的重心線重合，正向指向載體的運(yùn)動(dòng)方向，oyb軸垂直oxb軸指向載體的右側(cè)，ozb軸與oxb軸、oyb軸正交形成右手坐標(biāo)系。北東地導(dǎo)航坐標(biāo)系oxnynzn定義：原點(diǎn)與載體坐標(biāo)系的原點(diǎn)重合以消除坐標(biāo)原點(diǎn)的漂移，oxn軸指向當(dāng)?shù)乇弊游缇€，oyn軸與oxn軸垂直指向東，ozn軸與oxn軸、oyn軸垂直形成右手坐標(biāo)系向下。步驟2所述的無(wú)人機(jī)飛控系統(tǒng)采集各傳感器輸出信息為：(1)IMU輸出三軸角速率ω和三軸加速度a：ω＝[ωxωyωz]Ta＝[axayaz]T其中下標(biāo)x,y,z表示向量在載體坐標(biāo)系三軸上的投影。(2)GPS輸出絕對(duì)速度其中vn、ve、vd分別表示北向、東向和地向速度，上標(biāo)n表示在導(dǎo)航坐標(biāo)系的投影。(3)迎角/側(cè)滑角傳感器輸出風(fēng)角信息α與β分別為：迎角α計(jì)算方法：α=Pα1-Pα2K1(P3-P4)]]>側(cè)滑角β計(jì)算方法：β=Pβ1-Pβ2K1(P5-P6)]]>式中P3表示迎角探針左壓力，P4為右壓力，Pα1為上壓力，Pα2為下壓力；P5表示側(cè)滑角探針上壓力，P6為下壓力，Pβ1為左壓力，Pβ2為右壓力；K1表示轉(zhuǎn)換系數(shù)；(4)根據(jù)空速管測(cè)量的壓力信息計(jì)算量測(cè)空速Vm假設(shè)空速管外的氣流不可壓縮，并且依據(jù)伯努利方程可以計(jì)算得到風(fēng)速的量測(cè)值為：Vm=2(P0-P)/ρ]]>其中P0，P分別表示空速管測(cè)得的全壓和靜壓，ρ表示空氣密度。步驟3所述的利用IMU輸出解算無(wú)人機(jī)姿態(tài)信息同時(shí)融合GPS輸出信息對(duì)姿態(tài)信息進(jìn)行估計(jì)補(bǔ)償?shù)木唧w步驟為：a.根據(jù)速度微分方程、位置微分方程以及姿態(tài)微分方程實(shí)時(shí)解算INS系統(tǒng)的導(dǎo)航信息；速度微分方程：v·n=Cbna-(2ωien+ωenn)×vn+g]]>位置微分方程：L·=vnRM+hλ·=ve(RN+h)cosLh·=-vd]]>其中vn＝[vnvevd]T表示絕對(duì)速度，下標(biāo)n，e，d分別表示絕對(duì)速度向量在北向，動(dòng)向和地向的投影；a為加速度計(jì)輸出，為載體系b系到導(dǎo)航系n系的姿態(tài)轉(zhuǎn)移矩陣，為地球自轉(zhuǎn)角速率在北東地導(dǎo)航坐標(biāo)系下的投影，為北東地導(dǎo)航坐標(biāo)系系相對(duì)地球坐標(biāo)系的轉(zhuǎn)動(dòng)角速率在北東地導(dǎo)航坐標(biāo)系下的投影，且有：[Lλh]T表示位置向量，L,λ,h分別為北東地導(dǎo)航坐標(biāo)系的緯度、經(jīng)度、高度；RN為卯酉圈曲率半徑，Re為WGS84大地坐標(biāo)系地球參考橢球的赤道平面半徑，e為WGS84大地坐標(biāo)系地球參考橢球的橢圓度；RM為子午圈曲率半徑，g＝[00g]T，g表示重力加速度。姿態(tài)微分方程：C·bn=Cbnωnbb]]>其中：ωnbb=ωibb-Cnb(ωien+ωenn)]]>為載體坐標(biāo)系b系到導(dǎo)航坐標(biāo)系n系的方向余弦矩陣：Cnb=cos(ψ)sin(ψ)0-sin(ψ)cos(ψ)00011000cos(θ)sin(θ)0-sin(θ)cos(θ)cos(φ)0-sin(φ)010sin(φ)0cos(φ)]]>其中φ，θ，ψ表示姿態(tài)角在北東地導(dǎo)航坐標(biāo)系內(nèi)三個(gè)方向的投影，為的轉(zhuǎn)置矩陣，為陀螺儀的輸出，利用四階龍格庫(kù)塔捷聯(lián)導(dǎo)航解算算法計(jì)算新周期的速度、位置和姿態(tài)信息。b.利用卡爾曼濾波模塊解算INS/GPS組合導(dǎo)航系統(tǒng)的導(dǎo)航信息。組合導(dǎo)航系統(tǒng)的狀態(tài)方程為：X·=AX+GW]]>其中：X=(δΦn)T(δvn)T(δpn)TϵT▿TT]]>δΦn，δvn，δpn表示平臺(tái)誤差角、速度誤差以及位置誤差，上標(biāo)n表示三個(gè)誤差向量在北東地導(dǎo)航坐標(biāo)系的投影；ε＝[εxεyεz]T表示三軸陀螺常值漂移，▽＝[▽x▽y▽z]T表示三軸加計(jì)常值漂移，G表示系統(tǒng)的噪聲驅(qū)動(dòng)陣，W表示系統(tǒng)的噪聲矢量，上標(biāo)T表示矩陣的轉(zhuǎn)置矩陣。量測(cè)方程為：Z＝HX+V其中：Z＝[δvnTδpnT]Tδvn=vn-vgpsnδpn=pn-pgpsn]]>H=03×3I3×303×303×303×303×303×3I3×303×303×3]]>在建立了系統(tǒng)方程和量測(cè)方程后，將系統(tǒng)方程和量測(cè)方程離散化，再利用卡爾曼濾波器進(jìn)行濾波。利用卡爾曼濾波器進(jìn)行濾波時(shí)，四階龍格庫(kù)塔捷聯(lián)解算模塊的輸出周期為Ts，GPS接收機(jī)的輸出周期為Tgps，卡爾曼濾波的離散化周期為Tgps；當(dāng)沒(méi)有GPS接收機(jī)信息的輸出時(shí)，只進(jìn)行INS導(dǎo)航結(jié)算；在接收到衛(wèi)星接收機(jī)信息時(shí)，利用GPS與IMU的輸出進(jìn)行量測(cè)更新。c.利用估計(jì)得到的位置誤差δpn、速度誤差δvn和姿態(tài)誤差δΦn補(bǔ)償模對(duì)INS系統(tǒng)的導(dǎo)航解算結(jié)果進(jìn)行補(bǔ)償：vn＝vn-δvnpn＝pn-δpnCnb=Cnb·Cnt]]>Cnt=cos(δψ)sin(δψ)0-sin(δψ)cos(δψ)00011000cos(δθ)sin(δθ)0-sin(δθ)cos(δθ)cos(δφ)0-sin(δφ)010sin(δφ)0cos(δφ)]]>最后可得到校正后的姿態(tài)信息步驟4所述的以角速率ω、加速度a以及姿態(tài)信息為系統(tǒng)輸入，建立狀態(tài)方程的具體步驟為：空速微分方程：u·(t)v·(t)w·(t)=Cnb(t)00g+ax(t)ay(t)az(t)+wa(t)+0-w(t)v(t)w(t)0-u(t)-v(t)u(t)0(ωx(t)ωy(t)ωz(t)+wω(t))]]>風(fēng)速微分方程：μ·n(t)μ·e(t)μ·d(t)=wμ(t)]]>其中表示無(wú)人機(jī)空速在載體系三個(gè)軸方向上的投影，μ(t)＝[μn(t)μe(t)μd(t)]T表示風(fēng)速在導(dǎo)航系三個(gè)軸方向上的投影，wa(t)，wω(t)，wμ(t)分別表示加速度計(jì)，陀螺儀和風(fēng)速的噪聲矢量。根據(jù)上述微分方程建立空速狀態(tài)方程：x·(t)=fc(x(t),u(t),w(t))]]>其中：x(t)＝[u(t)v(t)w(t)μn(t)μe(t)μd(t)]Tu(t)＝[ωx(t)ωy(t)ωz(t)ax(t)ay(t)az(t)]Tw(t)＝[wω(t)Twa(t)Twμ(t)T]T步驟5所述的以GPS速度和迎角α、側(cè)滑角β為量測(cè)值，建立量測(cè)方程的具體步驟為：利用GPS接收機(jī)輸出的絕對(duì)速度與大氣數(shù)據(jù)機(jī)輸出的迎角/側(cè)滑角α(t)，β(t)作為量測(cè)量：vgpsn(t)=vn(t)ve(t)vd(t)=Cbn(t)u(t)v(t)w(t)+μn(t)μe(t)μd(t)]]>α(t)=tan-1(w(t)u(t))]]>β(t)=sin-1(v(t)u(t)2+v(t)2+w(t)2)]]>建立量測(cè)方程：y(t)＝h(x(t))+d(t)其中：d(t)＝[dgps(t)Tdα(t)dβ(t)]T表示GPS速度、迎角和側(cè)滑角噪聲。對(duì)狀態(tài)方程和量測(cè)方程進(jìn)行離散化，離散時(shí)間為Tgps：xk＝xk-1+Tgpsfc(xk-1,uk-1,wk-1)＝f(xk-1,uk-1,wk-1)yk＝y(tǒng)(kTgps)＝h(xk)+dk步驟6所述的利用EKF估計(jì)無(wú)人機(jī)的空速的步驟為：初值設(shè)置：表示初值的估計(jì)值，P0表示誤差協(xié)方差矩陣的初值。噪聲方差陣根據(jù)各傳感器的規(guī)格參數(shù)設(shè)置：Qk和Rk分別表示過(guò)程噪聲和量測(cè)噪聲的協(xié)方差矩陣。時(shí)間更新：x^k,k-1=f(x^k-1,k-1,uk-1,0)]]>Pk,k-1=Fk-1Pk-1,k-1Fk-1T+Lk-1Qk-1Lk-1T]]>和Pk,k-1分別表示狀態(tài)和誤差協(xié)方差矩陣的一步預(yù)測(cè)值。其中Fk-1和Lk-1分別表示轉(zhuǎn)移矩陣對(duì)狀態(tài)和誤差雅克比矩陣。量測(cè)更新：Kk=Pk,k-1HkT(HkPk,k-1HkT+Rk)-1]]>x^k,k=x^k,k-1+Kk(yk-h(x^k,k-1))]]>Pk,k=(I-KkHk)Pk,k-1(I-KkHk)T+KkRkKkT]]>其中：Hk=∂hk∂x|x^k,k-1]]>Kk表示卡爾曼濾波的增益，表示k時(shí)刻的狀態(tài)濾波結(jié)果，Pk,k表示k時(shí)刻誤差協(xié)方差矩陣的濾波結(jié)果。步驟7所述的空速管故障判斷方法為：k時(shí)刻殘差r(k)由第k時(shí)刻的估計(jì)空速與空速管量測(cè)空速Vm(k)做差所得：r(k)=Vm(k)-|V^(k)|=Vm(k)-(u^(k))2+(v^(k))2+(w^(k))2]]>基于殘差的累加圖統(tǒng)計(jì)量為：S+(k)=sup(0,S+(k-1)-τ(τ-2r(k))2σ02),S+(1)=0]]>其中σ0表示殘差r的標(biāo)準(zhǔn)差，τ表示故障檢驗(yàn)的敏感參數(shù)，表示故障幅度的期望值，通?；趯?shí)際情況考慮取τ＝σ0。針對(duì)上述統(tǒng)計(jì)量，將檢測(cè)閾值設(shè)計(jì)為最大統(tǒng)計(jì)量的1.5倍，即：Sth＝1.5max(S+(k))，k＝1,2,3.....一旦統(tǒng)計(jì)量S+(k)超過(guò)閾值Sth，則認(rèn)為發(fā)生故障。本發(fā)明的原理：本發(fā)明針對(duì)無(wú)人機(jī)空速管故障檢測(cè)問(wèn)題，提出一種基于估計(jì)空速與真實(shí)空速管輸出作比較的方法來(lái)確定故障的發(fā)生?？账俚墓烙?jì)使用了IMU測(cè)量的加速度和角速率作為方程輸入，GPS測(cè)量的速度位置、迎角/側(cè)滑角傳感器測(cè)量的風(fēng)角信息作為方程輸出，以空速三個(gè)方向的量作為狀態(tài)，并且基于無(wú)人機(jī)空速三角幾何關(guān)系對(duì)空速進(jìn)行估計(jì)。所得的估計(jì)值與空速管信息獨(dú)立，且估計(jì)過(guò)程與無(wú)人機(jī)空氣動(dòng)力參數(shù)無(wú)關(guān)?；诖耍瑢⒐烙?jì)值與空速管信息做差得到空速殘差，利用累加圖對(duì)空速管進(jìn)行故障檢測(cè)。本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于：(1)本發(fā)明利用IMU量測(cè)信息建立空速狀態(tài)方程，利用GPS和迎角/側(cè)滑角傳感器信息估計(jì)空速，不同于一般的空速估計(jì)方法基于無(wú)人機(jī)的空氣動(dòng)力學(xué)模型，需要對(duì)應(yīng)的空氣動(dòng)力參數(shù)。因此本發(fā)明適用范圍不局限于某一型號(hào)的無(wú)人機(jī)，具有廣泛的使用價(jià)值。(2)本發(fā)明中的空速估計(jì)過(guò)程是利用IMU輸出，GPS輸出以及迎角/側(cè)滑角傳感器輸出，而空速管的輸出信息所自帶的干擾以及故障不會(huì)影響空速的估計(jì)過(guò)程，因此相比于一般基于空速管輸出信息的空速估計(jì)方法，本方法具有較高的精度。附圖說(shuō)明圖1為本發(fā)明的無(wú)人機(jī)空速估計(jì)和空速管故障檢測(cè)方法步驟圖；圖2為“托爾”無(wú)人機(jī)第一次飛行試驗(yàn)結(jié)果展示，其中，圖2a為“托爾”無(wú)人機(jī)第一次飛行軌跡；圖2b為“托爾”無(wú)人機(jī)第一次飛行的估計(jì)空速與量測(cè)空速圖；圖3為“托爾”無(wú)人機(jī)第二次飛行試驗(yàn)結(jié)果展示，其中，圖3a為“托爾”無(wú)人機(jī)第二次飛行軌跡；圖3b為“托爾”無(wú)人機(jī)第二次飛行的估計(jì)空速與量測(cè)空速圖；圖4為“費(fèi)舍爾”無(wú)人機(jī)第一次飛行試驗(yàn)結(jié)果展示，其中，圖4a為“費(fèi)舍爾”無(wú)人機(jī)第一次飛行軌跡；圖4b為“費(fèi)舍爾”無(wú)人機(jī)第一次飛行的估計(jì)空速與量測(cè)空速圖；圖5為“托爾”無(wú)人機(jī)第一次飛行空速管故障檢測(cè)結(jié)果；圖6為“托爾”無(wú)人機(jī)第二次飛行空速管故障檢測(cè)結(jié)果；圖7為“費(fèi)舍爾”無(wú)人機(jī)第一次飛行空速管故障檢測(cè)結(jié)果。具體實(shí)施方式如圖1所示，本發(fā)明的具體實(shí)施包括以下步驟：步驟1：定義載體坐標(biāo)系oxbybzb和北東地導(dǎo)航坐標(biāo)系oxnynzn。載體坐標(biāo)系oxbybzb定義：原點(diǎn)o位于飛機(jī)的質(zhì)心，oxb軸與載體運(yùn)動(dòng)方向的重心線重合，正向指向載體的運(yùn)動(dòng)方向，oyb軸垂直oxb軸指向載體的右側(cè)，ozb軸與oxb軸、oyb軸正交形成右手坐標(biāo)系，以下簡(jiǎn)稱為b系。北東地導(dǎo)航坐標(biāo)系oxnynzn定義：原點(diǎn)與載體坐標(biāo)系的原點(diǎn)重合以消除坐標(biāo)原點(diǎn)的漂移，oxn軸指向當(dāng)?shù)乇弊游缇€，oyn軸與oxn軸垂直指向東，ozn軸與oxn軸、oyn軸垂直形成右手坐標(biāo)系向下，以下簡(jiǎn)稱為n系。步驟2：無(wú)人機(jī)飛控系統(tǒng)采集各傳感器輸出信息：(1)IMU輸出三軸角速率ω和三軸加速度a：ω＝[ωxωyωz]Ta＝[axayaz]T其中下標(biāo)x,y,z表示向量在載體坐標(biāo)系三軸上的投影。(2)GPS輸出絕對(duì)速度其中vn、ve、vd分別表示北向、東向和地向速度，上標(biāo)n表示在導(dǎo)航坐標(biāo)系的投影。(3)迎角/側(cè)滑角傳感器輸出風(fēng)角信息α與β分別為：迎角α計(jì)算方法：α=Pα1-Pα2K1(P3-P4)]]>側(cè)滑角β計(jì)算方法：β=Pβ1-Pβ2K1(P5-P6)]]>式中P3表示迎角探針左壓力，P4為右壓力，Pα1為上壓力，Pα2為下壓力；P5表示側(cè)滑角探針上壓力，P6為下壓力，Pβ1為左壓力，Pβ2為右壓力；K1表示轉(zhuǎn)換系數(shù)；(4)根據(jù)空速管測(cè)量的壓力信息計(jì)算量測(cè)空速Vm假設(shè)空速管外的氣流不可壓縮，并且依據(jù)伯努利方程可以計(jì)算得到風(fēng)速的量測(cè)值為：Vm=2(P0-P)/ρ]]>其中P0，P分別表示空速管測(cè)得的全壓和靜壓，ρ表示空氣密度。步驟3：利用IMU輸出解算無(wú)人機(jī)姿態(tài)信息同時(shí)融合GPS輸出信息對(duì)姿態(tài)信息進(jìn)行估計(jì)補(bǔ)償。a.根據(jù)速度微分方程、位置微分方程以及姿態(tài)微分方程實(shí)時(shí)解算INS系統(tǒng)的導(dǎo)航信息；速度微分方程：v·n=Cbna-(2ωien+ωenn)×vn+g]]>位置微分方程：L·=vnRM+hλ·=ve(RN+h)cosLh·=-vd]]>其中vn＝[vnvevd]T表示絕對(duì)速度，下標(biāo)n，e，d表示絕對(duì)速度向量在北東地導(dǎo)航坐標(biāo)系的投影；a為加速度計(jì)輸出，為載體系b系到導(dǎo)航系n系的姿態(tài)轉(zhuǎn)移矩陣，為地球自轉(zhuǎn)角速率在北東地導(dǎo)航坐標(biāo)系下的投影，為北東地導(dǎo)航坐標(biāo)系系相對(duì)地球坐標(biāo)系的轉(zhuǎn)動(dòng)角速率在北東地導(dǎo)航坐標(biāo)系下的投影，且有：[Lλh]T表示位置向量，L,λ,h分別為北東地導(dǎo)航坐標(biāo)系的緯度、經(jīng)度、高度；RN為卯酉圈曲率半徑，Re為WGS84大地坐標(biāo)系地球參考橢球的赤道平面半徑，e為WGS84大地坐標(biāo)系地球參考橢球的橢圓度；RM為子午圈曲率半徑，g＝[00g]T，g表示重力加速度。姿態(tài)微分方程：C·bn=Cbnωnbb]]>其中ωnbb=ωibb-Cnb(ωien+ωenn)]]>為載體坐標(biāo)系b系到導(dǎo)航坐標(biāo)系n系的方向余弦矩陣：Cnb=cos(ψ)sin(ψ)0-sin(ψ)cos(ψ)00011000cos(θ)sin(θ)0-sin(θ)cos(θ)cos(φ)0-sin(φ)010sin(φ)0cos(φ)]]>其中φ，θ，ψ表示姿態(tài)角在北東地導(dǎo)航坐標(biāo)系內(nèi)三個(gè)方向的投影，為的轉(zhuǎn)置矩陣，為陀螺儀的輸出，利用四階龍格庫(kù)塔捷聯(lián)導(dǎo)航解算算法計(jì)算新周期的速度、位置和姿態(tài)信息。b.利用卡爾曼濾波模塊解算INS/GPS組合導(dǎo)航系統(tǒng)的導(dǎo)航信息。組合導(dǎo)航系統(tǒng)的狀態(tài)方程為：X·=AX+GW]]>其中X＝[δΦnTδvnTδpnTεT▽T]TδΦn，δvn，δpn表示平臺(tái)誤差角、速度誤差以及位置誤差，上標(biāo)n表示在北東地導(dǎo)航坐標(biāo)系的投影；ε＝[εxεyεz]T表示三軸陀螺常值漂移，▽＝[▽x▽y▽z]T表示三軸加計(jì)常值漂移，G表示系統(tǒng)的噪聲驅(qū)動(dòng)陣，W表示系統(tǒng)的噪聲矢量，上標(biāo)T表示矩陣的轉(zhuǎn)置矩陣。量測(cè)方程為：Z＝HX+V其中Z＝[δvnTδpnT]Tδvn=vn-vgpsnδpn=pn-pgpsn]]>H=03×3I3×303×303×303×303×303×3I3×303×303×3]]>在建立了系統(tǒng)方程和量測(cè)方程后，將系統(tǒng)方程和量測(cè)方程離散化，再利用卡爾曼濾波器進(jìn)行濾波。利用卡爾曼濾波器進(jìn)行濾波時(shí)，四階龍格庫(kù)塔捷聯(lián)解算模塊的輸出周期為Ts，GPS接收機(jī)的輸出周期為Tgps，卡爾曼濾波的離散化周期為Tgps；當(dāng)沒(méi)有GPS接收機(jī)信息的輸出時(shí)，只進(jìn)行INS導(dǎo)航結(jié)算；在接收到衛(wèi)星接收機(jī)信息時(shí)，利用GPS與IMU的輸出進(jìn)行量測(cè)更新。c.利用估計(jì)得到的位置誤差δpn、速度誤差δvn和姿態(tài)誤差δΦn補(bǔ)償模對(duì)INS系統(tǒng)的導(dǎo)航解算結(jié)果進(jìn)行補(bǔ)償：vn＝vn-δvnpn＝pn-δpnCnb=Cnb·Cnt]]>Cnt=cos(δψ)sin(δψ)0-sin(δψ)cos(δψ)00011000cos(δθ)sin(δθ)0-sin(δθ)cos(δθ)cos(δφ)0-sin(δφ)010sin(δφ)0cos(δφ)]]>最后可得到校正后的姿態(tài)信息步驟4：以角速率ω、加速度a以及姿態(tài)信息為系統(tǒng)輸入，建立狀態(tài)方程?？账傥⒎址匠蹋簎·(t)v·(t)w·(t)=Cnb(t)00g+ax(t)ay(t)az(t)+wa(t)+0-w(t)v(t)w(t)0-u(t)-v(t)u(t)0(ωx(t)ωy(t)ωz(t)+wω(t))]]>風(fēng)速微分方程：μ·n(t)μ·e(t)μ·d(t)=wμ(t)]]>其中表示無(wú)人機(jī)空速在載體系三個(gè)軸方向上的投影，μ(t)＝[μn(t)μe(t)μd(t)]T表示風(fēng)速在導(dǎo)航系三個(gè)軸方向上的投影，wa(t)，wω(t)，wμ(t)分別表示加速度計(jì)，陀螺儀和風(fēng)速的噪聲矢量。根據(jù)上述微分方程建立空速狀態(tài)方程：x·(t)=fc(x(t),u(t),w(t))]]>其中：x(t)＝[u(t)v(t)w(t)μn(t)μe(t)μd(t)]Tu(t)＝[ωx(t)ωy(t)ωz(t)ax(t)ay(t)az(t)]Tw(t)＝[wω(t)Twa(t)Twμ(t)T]T步驟5：以GPS速度和迎角α、側(cè)滑角β為量測(cè)值，建立量測(cè)方程。利用GPS接收機(jī)輸出的絕對(duì)速度與大氣數(shù)據(jù)機(jī)輸出的迎角/側(cè)滑角α(t)，β(t)作為量測(cè)量：vgpsn(t)=vn(t)ve(t)vd(t)=Cbn(t)u(t)v(t)w(t)+μn(t)μe(t)μd(t)]]>α(t)=tan-1(w(t)u(t))]]>β(t)=sin-1(v(t)u(t)2+v(t)2+w(t)2)]]>建立量測(cè)方程：y(t)＝h(x(t))+d(t)其中：d(t)＝[dgps(t)Tdα(t)dβ(t)]T表示GPS速度、迎角和側(cè)滑角噪聲。對(duì)狀態(tài)方程和量測(cè)方程進(jìn)行離散化，離散時(shí)間為Tgps：xk＝xk-1+Tgpsfc(xk-1,uk-1,wk-1)＝f(xk-1,uk-1,wk-1)yk＝y(tǒng)(kTgps)＝h(xk)+dk步驟6：根據(jù)上述空速估計(jì)模型，利用EKF估計(jì)無(wú)人機(jī)的空速初值設(shè)置：表示初值的估計(jì)值，P0表示誤差協(xié)方差矩陣的初值。噪聲方差陣根據(jù)各傳感器的規(guī)格參數(shù)設(shè)置：Qk和Rk分別表示過(guò)程噪聲和量測(cè)噪聲的協(xié)方差矩陣。時(shí)間更新：x^k,k-1=f(x^k-1,k-1,uk-1,0)]]>Pk,k-1=Fk-1Pk-1,k-1Fk-1T+Lk-1Qk-1Lk-1T]]>和Pk,k-1分別表示狀態(tài)和誤差協(xié)方差矩陣的一步預(yù)測(cè)值。其中Fk-1和Lk-1分別表示轉(zhuǎn)移矩陣對(duì)狀態(tài)和誤差雅克比矩陣。量測(cè)更新：Kk=Pk,k-1HkT(HkPk,k-1HkT+Rk)-1]]>x^k,k=x^k,k-1+Kk(yk-h(x^k,k-1))]]>Pk,k=(I-KkHk)Pk,k-1(I-KkHk)T+KkRkKkT]]>其中Hk=∂hk∂x|x^k,k-1]]>Kk表示卡爾曼濾波的增益，表示k時(shí)刻的狀態(tài)濾波結(jié)果，Pk,k表示k時(shí)刻誤差協(xié)方差矩陣的濾波結(jié)果。步驟7：獲取估計(jì)空速與空速管量測(cè)空速Vm之間的殘差r，利用累加圖檢測(cè)統(tǒng)計(jì)量超出閾值，并由此判斷空速管的故障。k時(shí)刻殘差r(k)由第k時(shí)刻的估計(jì)空速與空速管量測(cè)空速Vm(k)做差所得：r(k)=Vm(k)-|V^(k)|=Vm(k)-(u^(k))2+(v^(k))2+(w^(k))2]]>基于殘差的累加圖統(tǒng)計(jì)量為：S+(k)=sup(0,S+(k-1)-τ(τ-2r(k))2σ02),S+(1)=0]]>其中σ0表示殘差r的標(biāo)準(zhǔn)差，τ表示故障檢驗(yàn)的敏感參數(shù)，表示故障幅度的期望值，通?；趯?shí)際情況考慮取τ＝σ0。針對(duì)上述統(tǒng)計(jì)量，將檢測(cè)閾值設(shè)計(jì)為最大統(tǒng)計(jì)量的1.5倍，即：Sth＝1.5max(S+(k))，k＝1,2,3.....一旦統(tǒng)計(jì)量S+(k)超過(guò)閾值Sth，則認(rèn)為發(fā)生故障。實(shí)施例以明尼蘇達(dá)大學(xué)無(wú)人機(jī)實(shí)驗(yàn)室的UltraStick25e無(wú)人機(jī)平臺(tái)實(shí)際飛行數(shù)據(jù)進(jìn)行空速估計(jì)和空速管故障檢測(cè)。平臺(tái)包含三架無(wú)人機(jī)，其中選擇“托爾”型和“費(fèi)舍爾”號(hào)作為實(shí)施對(duì)象。兩架無(wú)人機(jī)除了尺寸和重量略有差別以外，都具有相同的傳感器配置，包括GloasatEM-406A型GPS接收機(jī)，ADIS16405型IMU以及Goodrich公司的微型五孔探針。按照說(shuō)明書步驟1定義無(wú)人機(jī)的載體坐標(biāo)系和導(dǎo)航坐標(biāo)系。載體坐標(biāo)系oxbybzb定義：原點(diǎn)o位于飛機(jī)的質(zhì)心，oxb軸與載體運(yùn)動(dòng)方向的重心線重合，正向指向載體的運(yùn)動(dòng)方向，oyb軸垂直oxb軸指向載體的右側(cè)，ozb軸與oxb軸、oyb軸正交形成右手坐標(biāo)系。北東地導(dǎo)航坐標(biāo)系oxnynzn定義：原點(diǎn)與載體坐標(biāo)系的原點(diǎn)重合以消除坐標(biāo)原點(diǎn)的漂移，oxn軸指向當(dāng)?shù)乇弊游缇€，oyn軸與oxn軸垂直指向東，ozn軸與oxn軸、oyn軸垂直形成右手坐標(biāo)系向下。按照說(shuō)明書步驟2采集IMU，GPS，五孔探針以及空速管的輸出信息，傳感器精度如下表所示：表1無(wú)人機(jī)傳感器輸出誤差按照說(shuō)明書步驟3，根據(jù)IMU輸出信息以及GPS輸出信息估計(jì)并校正姿態(tài)信息。按照說(shuō)明書步驟4，以角速率，加速度以及姿態(tài)信息為系統(tǒng)輸入，建立空速狀態(tài)方程。按照說(shuō)明書步驟5，以GPS速度，迎角以及側(cè)滑角信息為量測(cè)量，建立量測(cè)方程。噪聲方差陣為：Qk=Q=diagσax2σay2σaz2σωx2σωy2σωz2σμx2σμy2σμz2]]>Rk=R=diagσα2σβ2σvx2σvy2σvz2]]>按照說(shuō)明書步驟6，根據(jù)空速模型，利用EKF估計(jì)空速。試驗(yàn)選用兩次“托爾”型無(wú)人機(jī)飛行數(shù)據(jù)和一次“費(fèi)舍爾”型無(wú)人機(jī)飛行數(shù)據(jù)進(jìn)行空速估計(jì)，飛行軌跡和空速估計(jì)結(jié)果如圖2至圖4所示。其中圖2a表示第一次“托爾”型無(wú)人機(jī)飛行軌跡，圖2b表示第一次“托爾”型無(wú)人機(jī)飛行的估計(jì)空速和量測(cè)空速，其中實(shí)線表示估計(jì)空速，虛線表示空速管量測(cè)空速，單位為m/s，橫坐標(biāo)是數(shù)據(jù)處理時(shí)間，間隔為0.02s；圖3a表示第二次“托爾”型無(wú)人機(jī)飛行軌跡，圖3b表示第二次“托爾”型無(wú)人機(jī)飛行的估計(jì)空速和量測(cè)空速，其中實(shí)線表示估計(jì)空速，虛線表示空速管量測(cè)空速，單位為m/s，橫坐標(biāo)是數(shù)據(jù)處理時(shí)間，間隔為0.02s；圖4a表示第一次“費(fèi)舍爾”型無(wú)人機(jī)飛行軌跡，圖4b表示第一次“費(fèi)舍爾”型無(wú)人機(jī)飛行的估計(jì)空速和量測(cè)空速，其中實(shí)線表示估計(jì)空速，虛線表示空速管量測(cè)空速，單位為m/s，橫坐標(biāo)是數(shù)據(jù)處理時(shí)間，間隔為0.02s；從三次飛行試驗(yàn)結(jié)果可以看出，本發(fā)明可以在沒(méi)有空速管信息的情況下準(zhǔn)確的估計(jì)出空速信息，尤其是針對(duì)不同型號(hào)的無(wú)人機(jī)，仍然可以準(zhǔn)確的估計(jì)出空速，統(tǒng)計(jì)結(jié)果如下表所示：表2無(wú)人機(jī)飛行試驗(yàn)統(tǒng)計(jì)結(jié)果結(jié)果表明，空速估計(jì)的誤差方差可以保持在較低的水平內(nèi)。按照說(shuō)明書步驟7,對(duì)空速管故障進(jìn)行檢測(cè)。由于飛行試驗(yàn)是在無(wú)故障條件下進(jìn)行的，因此需要對(duì)空速管輸出信息進(jìn)行故障注入，下表表示三個(gè)架次飛行數(shù)據(jù)故障注入信息：表3無(wú)人機(jī)空速管故障注入信息無(wú)人機(jī)型號(hào)飛行架次故障幅值(m/s)故障發(fā)生時(shí)間(k)閾值托爾10.601518000～18500335托爾20.65996000～6500139費(fèi)舍爾10.788513000～13500322故障檢測(cè)結(jié)果如圖5至圖7所示。圖5表示第一次“托爾”無(wú)人機(jī)飛行試驗(yàn)的累加圖統(tǒng)計(jì)量，虛線表示無(wú)故障時(shí)的累加圖統(tǒng)計(jì)量，實(shí)線表示有故障時(shí)的累加圖統(tǒng)計(jì)量，橫線表示預(yù)設(shè)閾值，橫坐標(biāo)表示數(shù)據(jù)處理時(shí)間，間隔為0.02s，在k＝18000～18500時(shí)，有故障注入，統(tǒng)計(jì)量為730，超出預(yù)設(shè)閾值335，表示檢測(cè)到故障發(fā)生；圖6表示第二次“托爾”無(wú)人機(jī)飛行試驗(yàn)的累加圖統(tǒng)計(jì)量虛線表示無(wú)故障時(shí)的累加圖統(tǒng)計(jì)量，實(shí)線表示有故障時(shí)的累加圖統(tǒng)計(jì)量，橫線表示預(yù)設(shè)閾值，橫坐標(biāo)表示數(shù)據(jù)處理時(shí)間，間隔為0.02s，在k＝6000～6500時(shí)，有故障注入，統(tǒng)計(jì)量為342，超出預(yù)設(shè)閾值139，表示檢測(cè)到故障發(fā)生；圖7表示第一次“費(fèi)舍爾”無(wú)人機(jī)飛行試驗(yàn)的累加圖統(tǒng)計(jì)量，虛線表示無(wú)故障時(shí)的累加圖統(tǒng)計(jì)量，實(shí)線表示有故障時(shí)的累加圖統(tǒng)計(jì)量，橫線表示預(yù)設(shè)閾值，橫坐標(biāo)表示數(shù)據(jù)處理時(shí)間，間隔為0.02s，在k＝13000～13500時(shí)，有故障注入，統(tǒng)計(jì)量為479，超出預(yù)設(shè)閾值322，表示檢測(cè)到故障發(fā)生；從上述三次飛行實(shí)驗(yàn)看，在故障注入時(shí)刻以及持續(xù)時(shí)間內(nèi)，累加圖統(tǒng)計(jì)量都超過(guò)了提前設(shè)定的閾值，提示故障發(fā)生，因此本發(fā)明有效的檢測(cè)了故障的發(fā)生。當(dāng)前第1頁(yè)1 2 3