本發(fā)明涉及一種基于位置和姿態(tài)測(cè)量系統(tǒng)的柔性桿臂測(cè)量方法，可用于測(cè)量載機(jī)存在撓曲變形時(shí)多載荷之間的柔性基線長(zhǎng)度，提高多載荷之間相對(duì)的位置姿態(tài)精度。
背景技術(shù)：
：高精度POS由慣性測(cè)量單元(InertialmeasurementUnit,IMU)、導(dǎo)航計(jì)算機(jī)系統(tǒng)(POSComputerSystem，PCS)和GPS組成。高精度POS可以為高分辨率航空遙感系統(tǒng)提供高頻、高精度的時(shí)間、空間及精度信息，通過運(yùn)動(dòng)誤差補(bǔ)償提高成像精度和效率，是實(shí)現(xiàn)高分辨率成像的關(guān)鍵。我國(guó)在單POS成像方面取得了一定進(jìn)展，但是由于對(duì)地觀測(cè)載荷的需求牽引，如集成高分辨測(cè)繪相機(jī)、全譜段成像光譜儀、SAR于同一載體的多任務(wù)載荷，機(jī)載分布式陣列天線SAR和柔性多基線干涉SAR以及艦載稀疏陣列成像雷達(dá)等，多個(gè)或多種載荷安裝在飛機(jī)不同位置，采用傳統(tǒng)的單一POS系統(tǒng)無法實(shí)現(xiàn)多點(diǎn)的高精度位置姿態(tài)測(cè)量以及各載荷數(shù)據(jù)的時(shí)間統(tǒng)一。同時(shí)對(duì)于集成多個(gè)載荷的航空遙感系統(tǒng)和陣列載荷，由于飛機(jī)機(jī)體和柔性桿臂的撓曲變形、振動(dòng)等因素，單個(gè)POS無法測(cè)量分布在飛機(jī)不同位置多個(gè)載荷的位置速度姿態(tài)信息。如果每個(gè)載荷都安裝一個(gè)POS，不僅重量、成本增加，而且不同POS之間存在不同系統(tǒng)誤差，使得多個(gè)載荷之間的數(shù)據(jù)難以融合，因此迫切需要建立高精度分布式時(shí)空基準(zhǔn)系統(tǒng)，為高性能航空遙感系統(tǒng)中所有載荷提供高精度的時(shí)間、空間信息。現(xiàn)有的柔性桿臂測(cè)量方法(公開號(hào)：CN102322873)搭建了一種柔性桿臂測(cè)試環(huán)境并給出柔性桿臂測(cè)量精度驗(yàn)證方法，沒有提供詳細(xì)的柔性桿臂測(cè)量算法，沒有解決動(dòng)態(tài)條件下子IMU初始對(duì)準(zhǔn)精度不高的問題，會(huì)直接限制子系統(tǒng)的位置姿態(tài)測(cè)量精度。針對(duì)柔性基線測(cè)量特性對(duì)測(cè)量精度要求較高的問題，在提高子IMU初始對(duì)準(zhǔn)的姿態(tài)精度的同時(shí)，也要考慮子系統(tǒng)傳遞對(duì)準(zhǔn)問題，提高整體系統(tǒng)的實(shí)時(shí)導(dǎo)航精度，實(shí)現(xiàn)柔性桿臂的精確測(cè)量。技術(shù)實(shí)現(xiàn)要素：本發(fā)明的技術(shù)解決問題是：克服現(xiàn)有技術(shù)的不足，提出了一種基于位置和姿態(tài)測(cè)量系統(tǒng)的柔性桿臂測(cè)量方法，克服傳統(tǒng)初始對(duì)準(zhǔn)方法動(dòng)態(tài)條件下對(duì)準(zhǔn)精度低的缺點(diǎn)，使得具有精度高、抗干擾能力強(qiáng)的特點(diǎn)，可用于測(cè)量載機(jī)存在撓曲變形時(shí)多載荷之間的柔性基線長(zhǎng)度，提高多載荷之間相對(duì)的位置姿態(tài)精度。本發(fā)明技術(shù)解決方案是：一種基于位置和姿態(tài)測(cè)量系統(tǒng)的柔性桿臂測(cè)量方法，實(shí)現(xiàn)步驟如下：(1)首先搭建柔性桿臂的試驗(yàn)環(huán)境，將高精度主慣性測(cè)量單元(主IMU)及多個(gè)低精度子慣性測(cè)量單元(子IMU)安裝在柔性桿臂結(jié)構(gòu)架的對(duì)應(yīng)安裝節(jié)點(diǎn)上，系統(tǒng)上電；(2)然后高精度主慣性測(cè)量單元完成初始對(duì)準(zhǔn)工作，并采用卡爾曼濾波方法估計(jì)出分布式POS主系統(tǒng)的位置、速度、姿態(tài)組合導(dǎo)航信息，完成主IMU組合導(dǎo)航；(3)同時(shí)低精度子慣性測(cè)量單元采用基于Rodrigues參數(shù)姿態(tài)描述的濾波方法進(jìn)行動(dòng)態(tài)基座下初始對(duì)準(zhǔn)，估計(jì)出準(zhǔn)確的姿態(tài)信息，完成子IMU初始對(duì)準(zhǔn)；(4)最后，子IMU使用步驟(2)中主IMU高精度組合導(dǎo)航結(jié)果，進(jìn)行傳遞對(duì)準(zhǔn)，得到準(zhǔn)確的子系統(tǒng)組合導(dǎo)航信息，解算出主/子IMU之間準(zhǔn)確的基線長(zhǎng)度，實(shí)現(xiàn)柔性桿臂測(cè)量。步驟(2)所述的卡爾曼濾波方法中狀態(tài)變量X共有18維，包括東北天向的失準(zhǔn)角東北天向的速度誤差δVE、δVN、δVU，緯度、經(jīng)度、高度誤差δL、δλ、δH，x,y,z三個(gè)軸向的陀螺儀常值漂移誤差εx、εy、εz，x,y,z三個(gè)軸向的加計(jì)常值漂移誤差△x、△y、△z，x,y,z軸向桿臂長(zhǎng)度誤差△rx、△ry、△rz；量測(cè)量Z是經(jīng)過桿臂補(bǔ)償后的位置誤差和速度誤差，桿臂為GPS與主IMU之間的剛性桿臂。步驟(3)所述的基于Rodrigues參數(shù)姿態(tài)描述的濾波方法，利用姿態(tài)陣分解，將動(dòng)態(tài)姿態(tài)的估計(jì)問題轉(zhuǎn)化為一個(gè)常值姿態(tài)的估計(jì)，借助經(jīng)典Rodrigues參數(shù)和姿態(tài)陣之間的凱萊變換，建立了在慣性坐標(biāo)系內(nèi)描述的二階非線性的誤差模型，采用非線性卡爾曼濾波算法完成更新，估計(jì)出較準(zhǔn)確的姿態(tài)信息。步驟(4)所述的傳遞對(duì)準(zhǔn)，利用主IMU精確的組合導(dǎo)航信息作為子IMU傳遞對(duì)準(zhǔn)的基準(zhǔn)，通過計(jì)算主IMU和子IMU直接的量測(cè)差反映子IMU的誤差，同時(shí)將主IMU和子IMU之間的撓曲變形角作為狀態(tài)變量建立撓曲變形模型，提高桿臂補(bǔ)償精度，并借助“速度+姿態(tài)”的匹配方法實(shí)現(xiàn)桿臂修正，對(duì)撓曲變形的噪聲進(jìn)行積分平滑?！八俣?姿態(tài)”的匹配方法實(shí)現(xiàn)如下：當(dāng)量測(cè)量選用主、子慣性測(cè)量單元的速度誤差和姿態(tài)誤差進(jìn)行傳遞對(duì)準(zhǔn)時(shí)，量測(cè)值為主、子慣性測(cè)量單元的姿態(tài)角誤差和速度誤差，量測(cè)方程為Z＝HX+η，式中Z為量測(cè)變量，H為量測(cè)矩陣，η為量測(cè)噪聲，Z＝[δψδθδγδVEδVNδVU]T其中δψ，δθ，δγ為系統(tǒng)的航向角誤差、俯仰角誤差、橫滾角誤差，即三個(gè)姿態(tài)誤差，δVE,δVN,δVU為系統(tǒng)的東向、北向、天向速度誤差，即三個(gè)速度誤差。所述步驟(3)中子IMU初始對(duì)準(zhǔn)采用基于Rodrigues參數(shù)姿態(tài)描述的濾波方法完成，具體步驟如下：實(shí)時(shí)姿態(tài)陣式中：nt系為實(shí)時(shí)導(dǎo)航坐標(biāo)系，即載體時(shí)變位置東北天地理坐標(biāo)系；in為導(dǎo)航慣性系，與動(dòng)基座對(duì)準(zhǔn)開始時(shí)刻的n系重合；b為載體坐標(biāo)系；ib為載體慣性系，與對(duì)準(zhǔn)開始時(shí)刻的b系重合。是運(yùn)動(dòng)的nt系相對(duì)于慣性系in的姿態(tài)陣，可由GPS輸出位置信息解析計(jì)算；可利用陀螺輸出進(jìn)行姿態(tài)跟蹤。因此，慣性系動(dòng)基座對(duì)準(zhǔn)的核心任務(wù)是對(duì)常值姿態(tài)陣的估計(jì)。利用牛頓第二定律和哥氏定理，經(jīng)過簡(jiǎn)易推導(dǎo)可得到慣性系比力方程如下：vin·(t)+ωiein×vin(t)-gin(t)=fin(t)]]>式中：為t時(shí)刻載體對(duì)地速度在慣性系in內(nèi)的投影；為t時(shí)刻載體所在位置重力加速度在in系內(nèi)投影；為t時(shí)刻理想比力值。對(duì)式兩端分別進(jìn)行積分，并記∫0tk[vin·(τ)+(ωiein×vin(τ))-gin(τ)]dτ=Vrin(tk)]]>∫0tkfin(τ)dτ=Cibin∫0tkCbibfb(τ)dτ=CibinVmib(tk)]]>利用GPS輸出可完成式中的求解式中：dVr1kin=∫tk-1tkvin·(t)dt=vin(tk)-vin(tk-1)]]>dVr2kin=∫tk-1tkvin(t)dt=∫tk-1tkCntinvnt(t)dt]]>dVr3kin=-∫tk-1tkgin(t)dt=-∫tk-1tkCntingnt(t)dt]]>進(jìn)一步，在tk-1至tk更新周期內(nèi)，假設(shè)為常矢量，導(dǎo)航系內(nèi)對(duì)地速度為線性函數(shù)，即：Cntin=Cntk-1in[I+(t-tk-1)(ωinntk/k-1ntk/k-1)]]]>vnt(t)=vntk-1(tk-1)+vntk(tk)-vntk-1(tk-1)T(t-tk-1)]]>式中：t∈[tk-1,tk]；T＝tk-tk-1為GPS量測(cè)更新周期；整理可得：dVr2kin=Cntk-1in[IT2+T26(ωinntk/k-1ntk/k-1×)]vntk-1(tk-1)+Cntk-1in[IT2+T23(ωinntk/k-1ntk/k-1×)]vntk(tk)]]>dVr2kin=-Cntk-1in[IT+T22(ωinntk/k-1ntk/k-1×)]gntk|k-1]]>式中利用捷聯(lián)慣導(dǎo)姿態(tài)、速度二子樣更新算法，可以實(shí)現(xiàn)對(duì)上式中的求解。進(jìn)一步，考慮陀螺儀隨機(jī)常值漂移εb和加速度計(jì)隨機(jī)常值零偏的影響，推導(dǎo)可得：Vmib^(tk)=Vmib(tk)+δVmib(tk)]]>δVmib·=Cbib^▿b+fib^×φib,δVmib(0)=0]]>φib·=-Cbib^ϵb,φib(0)=0]]>式中為姿態(tài)誤差角；為加速度計(jì)慣性系比力積分誤差。求解常值姿態(tài)陣的觀測(cè)方程為：Vmib^(tk)-δVmib(tk)=CinibVrin(tk)]]>進(jìn)一步，用經(jīng)典Rodrigues參數(shù)法來等價(jià)描述姿態(tài)陣記對(duì)應(yīng)Rodrigues參數(shù)為l，則二者滿足凱萊變換關(guān)系式，即整理得：dtk=stk×l+l×δvm,tkib+ωv]]>式中ωv包含慣性器件測(cè)量噪聲的積分和隨機(jī)擾動(dòng)的積分，且有綜上，慣性系動(dòng)基座對(duì)準(zhǔn)可選取如下15維狀態(tài)；X=lT(δVmib)T(φib)T(ϵb)T(▿b)TT]]>由上述推導(dǎo)，可得系統(tǒng)方程及量測(cè)方程分別為：l·δVmin·φib·ϵb·▿b·T=0000000fib^×0Cbib^000-Cbib^00000000000lδVminφibϵb▿b+0Cbib^ωa-Cbib^ωg00]]>dtk=h(Xk)+ωv=stk×l+(I+l×)δVmib+ωv]]>利用上式設(shè)計(jì)濾波算法，可以實(shí)現(xiàn)對(duì)l的估計(jì)，進(jìn)而得到姿態(tài)陣即可實(shí)現(xiàn)動(dòng)基座條件下的初始對(duì)準(zhǔn)。本發(fā)明的原理是：首先搭建柔性桿臂的試驗(yàn)環(huán)境，將高精度主慣性測(cè)量單元(主IMU)及多個(gè)低精度子慣性測(cè)量單元(子IMU)安裝在柔性桿臂結(jié)構(gòu)架的對(duì)應(yīng)安裝節(jié)點(diǎn)上，系統(tǒng)上電。IMU進(jìn)行初始對(duì)準(zhǔn)，使用桿臂補(bǔ)償后的GPS數(shù)據(jù)進(jìn)行捷聯(lián)組合解算，實(shí)現(xiàn)位置、速度、姿態(tài)信息的輸出；子IMU結(jié)合主系統(tǒng)輸出的組合導(dǎo)航信息，采用基于Rodrigues參數(shù)姿態(tài)描述的濾波方法進(jìn)行初始對(duì)準(zhǔn)；并采用基于“速度+姿態(tài)”的非線性濾波匹配方法，建立含撓曲桿臂誤差的傳遞對(duì)準(zhǔn)模型，借助主IMU位置速度信息姿態(tài)信息進(jìn)行傳遞對(duì)準(zhǔn)，獲取子系統(tǒng)精確的位置、速度、姿態(tài)信息及主/子IMU之間的相對(duì)關(guān)系，實(shí)現(xiàn)柔性桿臂測(cè)量。本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于：本發(fā)明針對(duì)子IMU動(dòng)態(tài)條件初始對(duì)準(zhǔn)問題，采用基于Rodrigues參數(shù)姿態(tài)描述的濾波方法進(jìn)行初始對(duì)準(zhǔn)，克服傳統(tǒng)初始對(duì)準(zhǔn)方法對(duì)準(zhǔn)精度低的缺點(diǎn)，提高了IMU初始姿態(tài)精度；將主/子IMU之間的撓曲變形誤差作為狀態(tài)變量建立撓曲變形模型，采用基于“位置+姿態(tài)”的傳遞對(duì)準(zhǔn)方法得到較高精度的組合導(dǎo)航結(jié)果，克服傳統(tǒng)慣性/衛(wèi)星組合導(dǎo)航方法受桿臂變化影響大的不足，提高系統(tǒng)對(duì)柔性桿臂的測(cè)量精度。附圖說明圖1為本發(fā)明的系統(tǒng)數(shù)據(jù)處理流程圖；圖2為本發(fā)明的系統(tǒng)組成示意圖；圖3為本發(fā)明的基于Rodrigues參數(shù)姿態(tài)描述的濾波方法初始對(duì)準(zhǔn)流程圖；圖4為本發(fā)明的傳遞對(duì)準(zhǔn)流程圖。具體實(shí)施方式如圖1、2所示，本發(fā)明的具體實(shí)施步驟如下：1、將位置和姿態(tài)測(cè)量系統(tǒng)(POS)的慣性測(cè)量單元安裝到柔性桿臂結(jié)構(gòu)架的對(duì)應(yīng)節(jié)點(diǎn)上，搭建實(shí)驗(yàn)環(huán)境。其中柔性桿臂結(jié)構(gòu)架由柔性桿臂和穩(wěn)定基座組成，柔性桿臂的正中央是主IMU安裝節(jié)點(diǎn)，兩端是子IMU安裝節(jié)點(diǎn)，將柔性桿臂固定在穩(wěn)定基座上，如圖2所示。搭建實(shí)驗(yàn)需要的柔性桿臂環(huán)境，安裝慣性測(cè)量單元，如圖2所示，啟動(dòng)分布式POS測(cè)量系統(tǒng)進(jìn)行測(cè)量。2、主IMU初始對(duì)準(zhǔn)及組合導(dǎo)航(1)主IMU初始對(duì)準(zhǔn)采用傳統(tǒng)解析式方法完成：a、載體坐標(biāo)系下：重力加速度g和地球自轉(zhuǎn)角速度ωie可通過加速度計(jì)和陀螺儀的輸出獲得。b、導(dǎo)航坐標(biāo)系下：當(dāng)?shù)氐慕?jīng)度λ，緯度L可通過GPS數(shù)據(jù)獲得，重力加速度g和地球自轉(zhuǎn)角速度ωie在地理坐標(biāo)系下的分量都是可以確定的，表示如下：gn=00gωien=ωiexnωieynωiezn=0ωiecosLωiesinL]]>c、捷聯(lián)矩陣由下式求出：Cbn=(gn)T(ωien)T(gn×ωien)T-1(gb)T(ωieb)T(gb×ωieb)T]]>(2)主系統(tǒng)實(shí)時(shí)導(dǎo)航，包括捷聯(lián)解算和卡爾曼濾波：a、捷聯(lián)解算：以上一時(shí)刻的位置、速度、姿態(tài)作為作為當(dāng)前捷聯(lián)解算的初始值，結(jié)合當(dāng)前時(shí)刻的主IMU數(shù)據(jù)，獲得當(dāng)前時(shí)刻的慣性導(dǎo)航結(jié)果。主要模塊包括姿態(tài)矩陣更新、姿態(tài)計(jì)算、速度計(jì)算、位置矩陣更新和位置計(jì)算，說明如下：①姿態(tài)矩陣更新與姿態(tài)計(jì)算采用四元數(shù)法更新姿態(tài)矩陣Cnb=Tγ·Tθ·Tψ=cosγcosψ-sinγsinθsinψcosγsinψ+sinγsinθcosψ-sinγcosθ-cosθsinψcosθcosψsinθsinγcosψ+cosγsinθsinψsinγsinψ-cosγsinθcosψcosγcosθ]]>初始四元數(shù)計(jì)算公式為：q=q0q1q2q3=cosψ2cosθ2cosγ2-sinψ2sinθ2sinγ2cosψ2sinθ2cosγ2-sinψ2cosθ2sinγ2cosψ2cosθ2sinγ2+sinψ2sinθ2cosγ2cosψ2sinθ2sinγ2+sinψ2cosθ2cosγ2]]>即可由下式進(jìn)行姿態(tài)更新計(jì)算：Cnb=q02+q12-q22-q322(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q02-q12+q22-q322(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)q02-q12-q22+q32=T11T12T13T21T22T23T31T32T33]]>航向角ψ為IMU坐標(biāo)系y軸在導(dǎo)航坐標(biāo)系水平面(XY面)的投影與導(dǎo)航坐標(biāo)系y軸的夾角，從導(dǎo)航坐標(biāo)系y軸起算，“逆時(shí)針”為正，有效范圍為[0°，360°]；俯仰角θ為IMU坐標(biāo)系y軸與導(dǎo)航坐標(biāo)系水平面(XY面)間的夾角，以載荷抬頭為正，即IMU坐標(biāo)系y軸矢量指向高于水平面為正，反之為負(fù)，有效范圍為[-90°，90°]；橫滾角γ定義為IMU右傾為正(以IMU坐標(biāo)系y軸矢量指向?yàn)榍?，IMU坐標(biāo)系x軸指向?yàn)橛?，左傾為負(fù)，有效范圍為[-180°，180°]。姿態(tài)更新后由下式計(jì)算結(jié)果：②速度計(jì)算由下式計(jì)算速度更新：Vxn·Vyn·Vzn·=aibxnaibynaibzn+02ωiezn+ωenzn-2(ωieyn+ωenyn)-2(ωiezn+ωenzn)02ωiexn+ωenxn2ωieyn+ωenyn-2(ωiexn+ωenxn)0VxnVynVzn-00g]]>式中為導(dǎo)航坐標(biāo)系下沿x,y,z三個(gè)軸的速度增量，為載體坐標(biāo)系相對(duì)慣性空間的加速度在x,y,z三個(gè)軸的投影，為導(dǎo)航坐標(biāo)系下地球自傳角速度沿x,y,z三個(gè)軸方向上的投影，由上式求出加速度澤③位置矩陣更新與位置計(jì)算由下列微分方程進(jìn)行位置矩陣更新：Cne·=CneΩenn,Ωenn=0-ωenznωenynωenzn0-ωenxn-ωenynωenxn0]]>式中分別為導(dǎo)航坐標(biāo)系下導(dǎo)航坐標(biāo)系相對(duì)地球坐標(biāo)系的轉(zhuǎn)動(dòng)角速率沿x,y,z三個(gè)軸方向的投影，采用一階歐拉法進(jìn)行位置矩陣更新，速發(fā)表達(dá)式為：Cne(t+T)=Cne(t)+TCne(t)Ωenn(t)]]>式中T為慣性導(dǎo)航系統(tǒng)采樣周期。完成位置矩陣更新后，即可計(jì)算出導(dǎo)航位置參數(shù)，記有：L=sin-1(C33)λ=tan-1(C32C31)]]>高度H由于純慣性導(dǎo)航系統(tǒng)的高度計(jì)算通道是發(fā)散的，要使用外界高度信息對(duì)捷聯(lián)解算算法的高度通道進(jìn)行阻尼。3、子IMU初始對(duì)準(zhǔn)：利用子IMU數(shù)據(jù)和主系統(tǒng)數(shù)據(jù)，采用基于Rodrigues參數(shù)姿態(tài)描述的濾波方法進(jìn)行初始對(duì)準(zhǔn)，如圖3所示，具體步驟如下：實(shí)時(shí)姿態(tài)陣式中：nt系為實(shí)時(shí)導(dǎo)航坐標(biāo)系，即載體時(shí)變位置東北天地理坐標(biāo)系；in為導(dǎo)航慣性系，與動(dòng)基座對(duì)準(zhǔn)開始時(shí)刻的n系重合；b為載體坐標(biāo)系；ib為載體慣性系，與對(duì)準(zhǔn)開始時(shí)刻的b系重合。是運(yùn)動(dòng)的nt系相對(duì)于慣性系in的姿態(tài)陣，可由GPS輸出位置信息解析計(jì)算；可利用陀螺輸出進(jìn)行姿態(tài)跟蹤。因此，慣性系動(dòng)基座對(duì)準(zhǔn)的核心任務(wù)是對(duì)常值姿態(tài)陣的估計(jì)。利用牛頓第二定律和哥氏定理，經(jīng)過簡(jiǎn)易推導(dǎo)可得到慣性系比力方程如下：vin·(t)+ωiein×vin(t)-gin(t)=fin(t)]]>式中：為t時(shí)刻載體對(duì)地速度在慣性系in內(nèi)的投影；為t時(shí)刻載體所在位置重力加速度在in系內(nèi)投影；為t時(shí)刻理想比力值。對(duì)式兩端分別進(jìn)行積分，并記：∫0tk[vin·(τ)+(ωiein×vin(τ))-gin(τ)]dτ=Vrin(tk)]]>∫0tkfin(τ)dτ=Cibin∫0tkCbibfb(τ)dτ=CibinVmib(tk)]]>利用GPS輸出可完成式中的求解式中：dVr1kin=∫tk-1tkvin·(t)dt=vin(tk)-vin(tk-1)]]>dVr2kin=∫tk-1tkvin(t)dt=∫tk-1tkCntinvnt(t)dt]]>dVr3kin=-∫tk-1tkgin(t)dt=-∫tk-1tkCntingnt(t)dt]]>進(jìn)一步，在tk-1至tk更新周期內(nèi)，假設(shè)為常矢量，導(dǎo)航系內(nèi)對(duì)地速度為線性函數(shù)，即：Cntin=Cntk-1in[I+(t-tk-1)(ωinntk/k-1ntk/k-1)]]]>vnt(t)=vntk-1(tk-1)+vntk(tk)-vntk-1(tk-1)T(t-tk-1)]]>式中：t∈[tk-1,tk]；T＝tk-tk-1為GPS量測(cè)更新周期；整理可得：dVr2kin=Cntk-1in[IT2+T26(ωinntk/k-1ntk/k-1×)]vntk-1(tk-1)+Cntk-1in[IT2+T23(ωinntk/k-1ntk/k-1×)]vntk(tk)]]>dVr2kin=-Cntk-1in[IT+T22(ωinntk|k-1ntk|k-1×)]gntk|k-1]]>式中利用捷聯(lián)慣導(dǎo)姿態(tài)、速度二子樣更新算法，可以實(shí)現(xiàn)對(duì)上式中的求解。進(jìn)一步，考慮陀螺儀隨機(jī)常值漂移εb和加速度計(jì)隨機(jī)常值零偏的影響，推導(dǎo)可得：Vmib^(tk)=Vmib(tk)+δVmib(tk)]]>δVmib·=Cbib^▿b+fib^×φib,δVmib(0)=0]]>φib·=-Cbib^ϵb,φib(0)=0]]>式中為姿態(tài)誤差角；為加速度計(jì)慣性系比力積分誤差。求解常值姿態(tài)陣的觀測(cè)方程為：Vmib^(tk)-δVmib(tk)=CinibVrin(tk)]]>進(jìn)一步，用經(jīng)典Rodrigues參數(shù)法來等價(jià)描述姿態(tài)陣記對(duì)應(yīng)Rodrigues參數(shù)為l，則二者滿足凱萊變換關(guān)系式，即整理得：dtk=stk×l+l×δvm,tkib+ωv]]>式中ωv包含慣性器件測(cè)量噪聲的積分和隨機(jī)擾動(dòng)的積分，且有綜上，慣性系動(dòng)基座對(duì)準(zhǔn)可選取如下15維狀態(tài)；X=lT(δVmib)T(φib)T(ϵb)T(▿b)TT]]>由上述推導(dǎo)，可得系統(tǒng)方程及量測(cè)方程分別為：l·δVmin·φib·ϵb·▿b·T=0000000fib^×0Cbib^000-Cbib^00000000000lδVminφibϵb▿b+0Cbib^ωa-Cbib^ωg00]]>dtk=h(Xk)+ωv=stk×l+(I+l×)δVmib+ωv]]>式中l(wèi)為Rodrigues參數(shù)，包含三個(gè)速度誤差，包含三個(gè)角度誤差，εb包含三個(gè)陀螺儀常值漂移誤差，包含三個(gè)加速度計(jì)常值漂移誤差。利用上式設(shè)計(jì)濾波算法，可以實(shí)現(xiàn)對(duì)l的估計(jì)，進(jìn)而得到姿態(tài)陣即可實(shí)現(xiàn)動(dòng)基座條件下的初始對(duì)準(zhǔn)。4、子系統(tǒng)建立含撓曲桿臂誤差的傳遞對(duì)準(zhǔn)模型，傳遞對(duì)準(zhǔn)采用基于“速度+姿態(tài)”的非線性濾波匹配方法。其原理是利用主POS的高精度速度、姿態(tài)信息與子POS的速度、姿態(tài)信息之差對(duì)主、子POS間的姿態(tài)誤差角進(jìn)行估計(jì)并修正。濾波器的模型包括狀態(tài)方程和量測(cè)方程。如圖4所示，具體步驟如下：在考慮撓性桿臂誤差的情況下，則量測(cè)方程可進(jìn)一步描述為：y＝Hx0+v+fs其中fs為撓曲變形產(chǎn)生的量測(cè)誤差，在此作為傳感器小故障處理。下面簡(jiǎn)要分析fs的產(chǎn)生和傳播機(jī)理?？紤]主、子IMU之間的三軸撓曲變形角θ滿足二階馬爾科夫過程的情況:θ··x+2βxθ·x+βx2θx=ηxθ··y+2βyθ·y+βy2θy=ηyθ··z+2βzθ·z+βz2θz=ηz]]>其中，τi為三軸撓曲變形過程的相關(guān)時(shí)間；ηi為白噪聲，其方差為：為三個(gè)撓曲變形角的方差。主、子IMU的載體系之間的關(guān)系在小角度時(shí)可由如下方向余弦陣來確定：Cba=1-μzμyμz1-μz-μyμx1=I+μ×]]>式中：μx=ρx+θxμy=ρy+θyμz=ρz+θz]]>ρx、ρy、ρz和θx、θy、θz分別為子IMU相對(duì)主IMU的固定安裝誤差角和撓曲變形角在b系各個(gè)軸上的分量。主IMU導(dǎo)航系n和子IMU計(jì)算導(dǎo)航系n1之間的關(guān)系在小角度下可由如下方向余弦陣來確定：Cnn1=1φz-φy-φz1φxφy-φx1=1-φ×]]>其中，φ×為n1系相對(duì)n系的失準(zhǔn)角φx、φy、φz組成的斜對(duì)稱陣。進(jìn)一步，根據(jù)傳遞對(duì)準(zhǔn)的原理可得：Can=Ta,Cbn1=Cnn1CanCba=Tb]]>忽略二階小量乘積，取近似值有：Tb＝Ta+Ta(μ×)-(φ×)Ta設(shè)主、子IMU計(jì)算出的航向角、俯仰角及橫滾角分別為ψ、θ、γ和ψx、θy、γz。Ti=Ti(11)Ti(12)Ti(13)Ti(21)Ti(22)Ti(23)T(31)Ti(32)Ti(33),i=a,b]]>則有：tan(ψ+δψ)=-Ta(12)+Ta(22)φz-Ta(32)φy-Ta(11)μz+Ta(13)μxTa(22)-Ta(12)φz+Ta(32)φx-Ta(21)μz+Ta(23)μx]]>tan(γ+δγ)=-Ta(31)+Ta(11)φy-Ta(21)φx+Ta(32)μz-Ta(33)μyTa(33)+Ta(13)φy-Ta(23)φx+Ta(31)μy-Ta(32)μx]]>sin(θ+δθ)=Ta(32)+Ta(12)φy-Ta(22)φx-Ta(31)μz+Ta(33)μx]]>兩邊按泰勒級(jí)數(shù)展開，均取前兩項(xiàng)并忽略二階小量，整理得到：δψ=Ta(12)Ta(32)(Ta(12))2+(Ta(22))2φx+Ta(22)Ta(32)(Ta(12))2+(Ta(22))2φy-φz+Ta(12)Ta(23)-Ta(13)Ta(22)(Ta(12))2+(Ta(22))2μx+Ta(11)Ta(22)-Ta(12)Ta(21)(Ta(12))2+(Ta(22))2μz]]>δψ=-Ta(22)1-(Ta(32))2φx+Ta(12)1-(Ta(32))2φy-Ta(31)1-(Ta(32))2μz+Ta(33)1-(Ta(32))2μx]]>δγ=Ta(21)Ta(33)-Ta(31)Ta(23)(Ta(33))2+(Ta(31))2φx+Ta(31)Ta(13)-Ta(11)Ta(33)(Ta(33))2+(Ta(31))2φy-Ta(31)Ta(32)(Ta(33))2+(Ta(31))2μx+μy-Ta(32)Ta(33)(Ta(33))2+(Ta(31))2μz]]>綜上，式中含有μi的項(xiàng)即為由于撓曲變形和固定安裝誤差角引起的量測(cè)誤差fs，可知：fs=H2H303×303×3ρθ]]>其中，H2=H3=Ta(12)Ta(23)-Ta(13)Ta(22)(Ta(12))2+(Ta(22))20Ta(11)Ta(22)-Ta(12)Ta(21)(Ta(12))2+(Ta(22))2Ta(33)1-(Ta(32))20-Ta(31)1-(Ta(32))2-Ta(31)Ta(32)(Ta(33))2+(Ta(31))21-Ta(32)Ta(33)(Ta(33))2+(Ta(31))2]]>分析了量測(cè)誤差fs的產(chǎn)生和傳播機(jī)理后，進(jìn)一步，為了準(zhǔn)確地對(duì)其進(jìn)行檢測(cè)和補(bǔ)償，必須將引起量測(cè)誤差的fs的固定安裝誤差角與撓曲變形角納入系統(tǒng)模型的狀態(tài)變量之中，設(shè)計(jì)卡爾曼濾波器，對(duì)其估計(jì)。在此情況下，定義新的傳遞對(duì)準(zhǔn)模型系統(tǒng)狀態(tài)變量如下：x=φxφyφzδVxδVyδVzδLδλδhϵxϵyϵz▿x▿y▿zρxρyρzθxθyθzθ·xθ·yθ·zT]]>從而可以得到含撓性桿臂誤差的傳遞對(duì)準(zhǔn)模型為：x·=Fx+Gwy=Hx+v]]>其中，F(xiàn)=F1F2F3Cbn103×303×303×303×3F4F5F603×3Cbn103×303×303×303×3F7F803×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×3I3×303×303×303×303×303×303×3B1B2]]>B1=-βx2000-βy2000-βz2,B2=-2βx000-2βy000-2βz,G=Cbn103×303×303×3Cbn103×3015×3015×3015×303×303×3I3×3]]>H=H103×303×9H2H303×303×3I3×303×903×303×303×3]]>利用卡爾曼濾波估計(jì)的結(jié)果對(duì)系統(tǒng)狀態(tài)變量進(jìn)行反饋校正，得到子系統(tǒng)的組合導(dǎo)航結(jié)果，進(jìn)而可以得到主IMU和子IMU之間準(zhǔn)確的基線長(zhǎng)度。當(dāng)前第1頁1 2 3