專利名稱：：基于雙軸光電測(cè)量系統(tǒng)的地面車(chē)位置、姿態(tài)及航向測(cè)量方法
技術(shù)領(lǐng)域：
：本發(fā)明涉及地面車(chē)快速精確定位、定向、測(cè)姿方法，特別涉及利用光電測(cè)量系統(tǒng)的地面車(chē)快速精確定位定向技術(shù)，屬于導(dǎo)航定位
技術(shù)領(lǐng)域：
。
背景技術(shù)：
：陸地導(dǎo)航系統(tǒng)(LNS，LandNavigationSystem)已成為世界上發(fā)達(dá)國(guó)家陸軍裝備現(xiàn)代化的一個(gè)重要組成部分。LNS是信息化戰(zhàn)場(chǎng)實(shí)現(xiàn)體系對(duì)抗的基本要素，它不僅可以有效地提高路地戰(zhàn)車(chē)的作戰(zhàn)精度，還可以極大地提高戰(zhàn)車(chē)的快速反應(yīng)能力和作戰(zhàn)的機(jī)動(dòng)性、自主性。地面車(chē)LNS要解決的核心問(wèn)題是準(zhǔn)確快速的定位定向、姿態(tài)感知的技術(shù)。目前陸地戰(zhàn)車(chē)LNS大多數(shù)采用GPS和慣性導(dǎo)航系統(tǒng)(INS)或航向參考系統(tǒng)(DR)構(gòu)成的組合導(dǎo)航系統(tǒng)來(lái)實(shí)現(xiàn)導(dǎo)航定位。該方法存在的缺陷是在GPS受干擾或者戰(zhàn)時(shí)不能使用GPS的情況下，INS/DR系統(tǒng)由于隨時(shí)間累計(jì)的誤差而不能滿足陸地戰(zhàn)車(chē)所需要的定位定向及姿態(tài)精度。在現(xiàn)有陸地導(dǎo)航系統(tǒng)定位定向精度不能滿足特定任務(wù)要求的情況下，利用光電探測(cè)系統(tǒng)的載體位置、姿態(tài)和航向感知技術(shù)可以很好的解決地面車(chē)的定位定向及姿態(tài)感知問(wèn)題?，F(xiàn)有基于光電探測(cè)系統(tǒng)的地面車(chē)精確定位定向方法(申請(qǐng)?zhí)?00710063640.7)是一種基于光電測(cè)量系統(tǒng)單軸(航向軸)測(cè)量信息的算法，存在以下不足-無(wú)法解算得到地面車(chē)輛的姿態(tài)(橫滾、俯仰角)和高度信息。算法建立在平面模型基礎(chǔ)上，實(shí)際應(yīng)用中無(wú)法保證地面車(chē)水平，車(chē)體的姿態(tài)角會(huì)造成光電探測(cè)系統(tǒng)航向角測(cè)量耦合誤差，從而影響最終定位/定向精度。
發(fā)明內(nèi)容本發(fā)明要解決的技術(shù)問(wèn)題是針對(duì)現(xiàn)有陸地導(dǎo)航系統(tǒng)和基于單軸光學(xué)探測(cè)系統(tǒng)的地面車(chē)快速定位定向算法存在的問(wèn)題，發(fā)明了一種基于雙軸光電測(cè)量系統(tǒng)的地面車(chē)快速位置、姿態(tài)、航向測(cè)量方法?；陔p軸光電測(cè)量系統(tǒng)的地面車(chē)快速位置、姿態(tài)、航向測(cè)量方法(后簡(jiǎn)稱發(fā)明)，其
發(fā)明內(nèi)容主要包括1、發(fā)明提出車(chē)輛上裝有慣導(dǎo)系統(tǒng)INS和雙軸光電測(cè)量系統(tǒng)。車(chē)輛駛?cè)胫付òl(fā)射區(qū)域后，利用雙軸光電測(cè)量系統(tǒng)光軸依次瞄準(zhǔn)三個(gè)已知位置靶標(biāo)中心，利用雙軸光電測(cè)量系統(tǒng)可以測(cè)得此時(shí)光軸矢量在載體坐標(biāo)系的方位角和俯仰角。利用光電測(cè)量系統(tǒng)測(cè)量的載體坐標(biāo)系的方位角和俯仰角信息，慣導(dǎo)系統(tǒng)提供的車(chē)體初始姿態(tài)信息快速完成地面車(chē)位置、姿態(tài)、航向的測(cè)量。2、發(fā)明提出原始雙軸光電測(cè)量系統(tǒng)測(cè)量信息的修正算法。利用慣導(dǎo)提供的車(chē)體初始姿態(tài)信息修正1中測(cè)量得到的載體坐標(biāo)系下方位、俯仰角信息，得到鎖定光軸矢量在近似地理坐標(biāo)系下的方位角和俯仰角。3、發(fā)明提出車(chē)體姿態(tài)角、高度解算方法。發(fā)明提出利用2中計(jì)算得到的修正后方位角，耙標(biāo)的位置(含經(jīng)度、緯度)、車(chē)體初始位置/航向計(jì)算得到車(chē)體的位置誤差和航向誤差，并利用得到的位置/航向誤差修正車(chē)體初始位置/航向。發(fā)明提出利用2中計(jì)算得到的修正后俯仰角、靶標(biāo)的位置(含經(jīng)度、緯度、高度)、車(chē)體的初始高度、前面得到修正后的車(chē)體位置/航向計(jì)算得到車(chē)體的姿態(tài)角誤差和高度誤差，并利用得到的高度/姿態(tài)誤差修正車(chē)體初始高度/姿態(tài)。4、發(fā)明提出修正、迭代計(jì)算方法，修正迭代計(jì)算方法是算法實(shí)現(xiàn)高精度解算的關(guān)鍵。修正迭代計(jì)算方法為當(dāng)3計(jì)算得到誤差修正量未達(dá)到指定精度(大于預(yù)設(shè)的閾值)，由3計(jì)算得到的修正后的車(chē)體位置、姿態(tài)和航向作為初值，重復(fù)2、3、4進(jìn)行迭代解算；當(dāng)3計(jì)算得到的誤差修正量達(dá)到指定精度(小于等于預(yù)設(shè)的閾值)，輸出修正后的車(chē)體位置、姿態(tài)、航向作為地面車(chē)精確的位置、姿態(tài)、航向信息。發(fā)明的特點(diǎn)主要包括本發(fā)明提出利用雙軸光電測(cè)量系統(tǒng)。雙軸光電測(cè)量系統(tǒng)可以同時(shí)測(cè)量靶標(biāo)相對(duì)車(chē)體坐標(biāo)系的方位角、俯仰角信息。本發(fā)明提出了利用車(chē)體初始姿態(tài)對(duì)車(chē)體系下的方位角、俯仰角測(cè)量信息進(jìn)行修正，得到近似地理坐標(biāo)系內(nèi)的方位角、俯仰角信息。利用近似地理系下的方位角進(jìn)行誤差三角形法解算，可以避免有車(chē)體姿態(tài)耦合造成車(chē)體位置、航向解算誤差。本發(fā)明提出利用近似地理坐標(biāo)系下的俯仰角測(cè)量信息、航向角信息、位置信息，計(jì)算車(chē)體高度、姿態(tài)角的算法。本發(fā)明提出了以慣導(dǎo)位置/姿態(tài)/航向作為初始值的迭代計(jì)算方法。迭代算法中把載體系下的測(cè)量信息轉(zhuǎn)換到近似地理坐標(biāo)系，利用近似地理系下的角度測(cè)量信息進(jìn)行位置/姿態(tài)/航向的解算，可以保證位置/姿態(tài)/航向算法始終工作在小角度線形區(qū)域，并且通過(guò)不斷修正最終做到精確的解算，避免了由于姿態(tài)算法線形化近似帶來(lái)的誤差，同時(shí)也避免了由于姿態(tài)誤差造成的位置/航向計(jì)算誤差。圖1為發(fā)明實(shí)際應(yīng)用情景示意圖2為基于平面誤差三角形的快速定位定向算法示意圖3為發(fā)明中雙軸光電測(cè)量系統(tǒng)光軸矢量方位角、俯仰角測(cè)量原理示意圖4為發(fā)明中地面車(chē)位置、姿態(tài)、航向快速測(cè)量算法流程示意圖。具體實(shí)施方式-地面車(chē)上裝有導(dǎo)航系統(tǒng)INS或DR系統(tǒng)和雙軸光電測(cè)量系統(tǒng)，發(fā)射場(chǎng)安放三個(gè)位置精確的靶標(biāo)。INS或DR系統(tǒng)可以提供車(chē)輛粗略位置和姿態(tài)/航向信息(Am、％—鵬、、鵬、y。一鵬、《_鵬、幾_鵬)。發(fā)射場(chǎng)三個(gè)靶標(biāo)精確位置已知(々、A、A)。地面車(chē)輛駛?cè)胫付ǘㄎ粎^(qū)域(耙標(biāo)構(gòu)成的三角形中心區(qū)域，如圖1所示)，本發(fā)明算法實(shí)施主要分為初值給定、光學(xué)測(cè)量、迭代解算三部分*利用雙軸光電探測(cè)系統(tǒng)對(duì)三個(gè)靶標(biāo)進(jìn)行瞄準(zhǔn)測(cè)量首先由車(chē)體初始位置/航向信息(^—ws、p。鵬、/y鵬、^_鵬)、靶標(biāo)的位置信息(zl,、p,、A)通過(guò)公式(1)計(jì)算第一個(gè)靶標(biāo)相對(duì)光軸的角度(",/')，用于自動(dòng)轉(zhuǎn)動(dòng)光軸，靶標(biāo)點(diǎn)進(jìn)入光電探測(cè)系統(tǒng)的視野，此時(shí)可以采用人工或自動(dòng)方式對(duì)目標(biāo)進(jìn)行瞄準(zhǔn)(保證目標(biāo)中心位于視場(chǎng)中心)，雙軸光電探測(cè)平臺(tái)此時(shí)可以測(cè)量出光軸矢量相對(duì)車(chē)體坐標(biāo)系的方位/俯仰角(",-)e，如圖3所示。"=tan(-^-r=~)—W0鵬(仍1o鵬)一(1)"={[W—鵬)cosO?！i)]+—外—鵬)}ie〃￡)其中A為地球半徑。重復(fù)上過(guò)程完成三個(gè)靶標(biāo)的測(cè)量，得到一組測(cè)量信息(a,，A)c。*利用上步的測(cè)量信息解算車(chē)體位置、姿態(tài)、航向解算車(chē)體位置/姿態(tài)/航向需要的信息包括>初始位置/姿態(tài)/航向鵬、爐。_,船、&_鵬、(^_鵬、《—鵬、r?！X)>靶標(biāo)位置信息(4、A、>角度測(cè)量信息(a,，A)c經(jīng)過(guò)測(cè)量得到上信息后，通過(guò)本發(fā)明提出的算法可以精確解算出車(chē)體的位置/姿態(tài)/航向。算法解算過(guò)程如附圖4所示可分為五步，具體如下第一步計(jì)算初值車(chē)體的位置/航向/姿態(tài)",％,4,^,《,/。)賦值為車(chē)載慣導(dǎo)給出的(Vm、％_鵬、V鵬、/ws、《o」ws、6—/船)。第二步:將車(chē)體系下的測(cè)量信息轉(zhuǎn)化到近似地理坐標(biāo)系利用光電平臺(tái)鎖定靶標(biāo)i時(shí)得到的測(cè)量航向、俯仰角為(",，A)e，車(chē)體坐標(biāo)系的姿態(tài)角信息(W。、《、y0)。利用下公式計(jì)算近似地理坐標(biāo)系下的角度測(cè)量信息(",,A》-0、「100、〈cos;r00sin/0、cosAcos",00cos《—sin<90010cosAcos;、ool乂(osingcos6>0,0COS",上公式右邊全部為己知測(cè)量量，通過(guò)下公式可以取得0、(100、'cos700sin;K0、一cos/,sinor,cosy000cos《010cos/,cosa,=、o00、0sin<90'cos《。y0cosvc3由下公式計(jì)算得到近似地理坐標(biāo)系下的測(cè)量(a,.，A)e:A=sin-'(C3);,=tan-'(CVq);第三步平面解算車(chē)體位置、航向由上步計(jì)算得到的近似地理坐標(biāo)系下的測(cè)量信息(",，A)e，靶標(biāo)的位置(4、p,)，車(chē)體的初始位置/姿態(tài)/航向(;io，外,A)，^，《,;0,通過(guò)平面誤差三角形法解算地面車(chē)的位置(義，W和航向誤差A(yù)^。。平面誤差三角形解法為"基于光學(xué)探測(cè)系統(tǒng)的地面車(chē)定位定向法"(200710063640.7)提出，關(guān)于誤差三角形解算的詳細(xì)說(shuō)明見(jiàn)附錄。利用計(jì)算得到的(/^,A^)按照下公式更新車(chē)體的位置/姿態(tài)/航向d^A)，^AJ。)。A0=義P0=Py0,+A^0第四步:立體解算車(chē)體高度、姿態(tài)將上步更新后的車(chē)體坐標(biāo)系位置、姿態(tài)，靶標(biāo)的位置信息、近似地理系下俯仰角。0,<^0，^,^0,^),/0，；1/，^.，力/,^;)帶入公式(5)取1=1，2，3，可得關(guān)于A&、△《A^。的線性方程組，據(jù)此可計(jì)算車(chē)體的高度、姿態(tài)角誤差(AApA6。，A;g?！獈AA0-cos^/0A<90+sin《y0A;K0=-加-1(^~~^")十(5)其中《為靶標(biāo)到車(chē)的距離，計(jì)算公式為《={-;gcos(p。)]2+("-%)2}1/2&。由計(jì)算得到的位置高度、橫滾角、俯仰角誤差(AA,A《，A7。)修正車(chē)體坐標(biāo)系",外，A)，W《'^)。第五步:將修正后的位置姿態(tài)(^,^，4,^,《,0代入第二步中迭代解算，直至修正誤差量達(dá)到預(yù)定精度，得到車(chē)體精確的位置、姿態(tài)和航向"，外，4,v/。,《，;g。具體實(shí)施例子為了驗(yàn)證本發(fā)明提出的基于雙軸光學(xué)探測(cè)系統(tǒng)的地面車(chē)快速位置、姿態(tài)、航向測(cè)量方法的可行性，利用北航研制的雙軸光學(xué)探測(cè)系統(tǒng)，在北京十三陵水庫(kù)的河灘上進(jìn)行了本發(fā)明的驗(yàn)證實(shí)驗(yàn)。雙軸光學(xué)探測(cè)系統(tǒng)的角度測(cè)量精度為30"，實(shí)驗(yàn)中靶標(biāo)的位置精度為5cm。實(shí)驗(yàn)中過(guò)程中車(chē)體保持位置/姿態(tài)/航向不動(dòng)，通過(guò)不同靶標(biāo)的組合下的結(jié)算結(jié)果的重復(fù)性度量算法精度。實(shí)驗(yàn)結(jié)果如下表所示-<table>tableseeoriginaldocumentpage7</column></row><table>從結(jié)果可以看出，本專利提出的算法(在現(xiàn)有實(shí)驗(yàn)條件下)位置誤差優(yōu)于0.5m，航向角/姿態(tài)角的解算誤差優(yōu)于30"。實(shí)驗(yàn)結(jié)果驗(yàn)證了本發(fā)明所訴的方法是有效的，并且可以達(dá)到很高的解算精度，有廣闊的應(yīng)用前景。最后所應(yīng)說(shuō)明的是所有的不脫離本發(fā)明基本思路和范圍內(nèi)的修改或局部替換，均應(yīng)涵蓋在本發(fā)明的權(quán)利要求范圍中。附錄l基于平面誤差三角形的快速定位定向算法地面車(chē)停放位置為0，該點(diǎn)地理北向?yàn)?N，如下圖所示。A、B、C為靶標(biāo)。地面車(chē)在0時(shí)，車(chē)載設(shè)備給的車(chē)體縱軸北向偏角誤差為A^。光電探測(cè)平臺(tái)在地理系水平面內(nèi)測(cè)量到的靶標(biāo)方位角為a,。如附圖2所示，過(guò)A點(diǎn)做平行于北向的直線AD。再過(guò)A點(diǎn)作直線^4',該直線與AD間的夾角為^，,，艮P:Za"'=^ml。如果導(dǎo)航設(shè)備給出的北向誤差角A^-0，那么直線^4'和AO重合。同理，過(guò)B、C點(diǎn)分別作直線朋'和CC'，三條直線A4'、和CC'兩兩相交，形成三角形乂萬(wàn)'C'，這個(gè)三角形稱為誤差三角形，誤差三角形的大小與誤差角A^的大小有關(guān)。設(shè)A^的最大值為A^^。分別調(diào)整Z"^'及直線S^和CC'對(duì)應(yīng)的角度，使其在h^-Av^x,l,+A^皿]之間對(duì)應(yīng)連續(xù)變化，每次變化的角度增量為^^，其中n為角度分劃數(shù)，可根據(jù)系統(tǒng)的定向精度要求取。每取一組角度值，由相應(yīng)的直線^4'、s萬(wàn)'和cc'形成一個(gè)誤差三角形，而地面車(chē)的真實(shí)地理坐標(biāo)(x,少)在該三角形所確定的區(qū)域內(nèi)。當(dāng)取某組角度值時(shí)，得到的誤差三角形的邊長(zhǎng)之和最小，此時(shí)得到的ZDA4'、Z/)5&和^DCC'即可認(rèn)為是靶標(biāo)相對(duì)于車(chē)體的真實(shí)方位角，對(duì)比初始時(shí)刻車(chē)載設(shè)備給出的北向，就可以得出北向誤差，同時(shí)得到地面車(chē)的平面位置。權(quán)利要求1、基于雙軸光電測(cè)量系統(tǒng)的地面車(chē)位置、姿態(tài)、航向測(cè)量方法，其特征在于步驟如下(1)車(chē)輛上裝有慣導(dǎo)系統(tǒng)和雙軸光電測(cè)量系統(tǒng)，車(chē)輛駛?cè)胫付ǘㄎ粎^(qū)域，慣導(dǎo)系統(tǒng)為車(chē)輛提供位置、姿態(tài)、航向初始值；(2)利用雙軸光電測(cè)量系統(tǒng)分別鎖定三個(gè)已知位置靶標(biāo)，在鎖定靶標(biāo)時(shí)，測(cè)出三個(gè)光軸矢量相對(duì)車(chē)體坐標(biāo)系的方位角和俯仰角；(3)利用慣導(dǎo)系統(tǒng)提供的車(chē)體初始姿態(tài)將步驟(2)中測(cè)量的車(chē)體坐標(biāo)系下光軸矢量方位、俯仰角信息轉(zhuǎn)化到近似地理坐標(biāo)系下，得到三個(gè)光軸矢量在近似地理坐標(biāo)系下的方位角和俯仰角；(4)由上述三個(gè)靶標(biāo)的位置和光電測(cè)量系統(tǒng)鎖定靶標(biāo)時(shí)光軸矢量在近似地理坐標(biāo)系下的方位角計(jì)算慣導(dǎo)系統(tǒng)提供的車(chē)體位置和航向誤差；由靶標(biāo)位置、車(chē)體位置和航向及光電測(cè)量系統(tǒng)鎖定靶標(biāo)時(shí)光軸矢量在近似地理坐標(biāo)系下的俯仰角計(jì)算慣導(dǎo)系統(tǒng)提供的車(chē)體姿態(tài)和高度誤差；(5)當(dāng)步驟(4)所述車(chē)體位置、車(chē)體姿態(tài)、航向誤差和高度誤差修正量小于等于某一預(yù)先設(shè)定的閾值時(shí)，首先用(4)計(jì)算得到的所述車(chē)體位置和航向誤差以及所述車(chē)體姿態(tài)和高度誤差修正車(chē)體位置、姿態(tài)和航向，然后輸出車(chē)體精確的位置、姿態(tài)和航向信息；當(dāng)步驟(4)所述誤差修正量大于某一預(yù)先設(shè)定的閾值時(shí)，首先用(4)計(jì)算得到的所述車(chē)體位置和航向誤差以及所述車(chē)體姿態(tài)和高度誤差修正車(chē)體位置、姿態(tài)和航向，并以修正后的位置、姿態(tài)和航向作為初始值，重復(fù)步驟(3)(4)。2、根據(jù)權(quán)利要求l所述的基于雙軸光電測(cè)量系統(tǒng)的地面車(chē)位置、姿態(tài)、航向測(cè)量方法，其特征在于所述的雙軸光電測(cè)量系統(tǒng)測(cè)量維度為載體的方位、俯仰兩個(gè)軸向。3、根據(jù)權(quán)利要求l所述的基于雙軸光電測(cè)量系統(tǒng)的地面車(chē)位置、姿態(tài)、航向測(cè)量方法，其特征在于為了達(dá)到高精度的計(jì)算結(jié)果，方法中經(jīng)過(guò)步驟(3)，將光學(xué)系統(tǒng)測(cè)量信息變換到近似地理坐標(biāo)系，得到鎖定靶標(biāo)時(shí)光軸矢量在近似地理坐標(biāo)系下的方位角、俯仰角，從而抑制了由于車(chē)體停放不平造成的算法誤差。4、根據(jù)權(quán)利要求1所述的基于雙軸光電測(cè)量系統(tǒng)的地面車(chē)位置、姿態(tài)、航向測(cè)量方法，其特征在于為了達(dá)到高精度的計(jì)算結(jié)果，方法中采用了步驟(5)中所述的修正、迭代計(jì)算方法。5、根據(jù)權(quán)利要求l所述的基于雙軸光電測(cè)量系統(tǒng)的地面車(chē)位置、姿態(tài)、航向測(cè)量方法，其特征在于所述步驟(4)中提出的姿態(tài)和高度計(jì)算過(guò)程，采用了將非線性方程組在真值附近線性化的方法。全文摘要基于雙軸光電測(cè)量系統(tǒng)的地面車(chē)位置、姿態(tài)和航向測(cè)量方法車(chē)輛上裝有慣導(dǎo)系統(tǒng)和雙軸光電測(cè)量系統(tǒng)，車(chē)輛駛?cè)胫付▍^(qū)域，慣導(dǎo)系統(tǒng)提供車(chē)輛位置、姿態(tài)和航向初始值；利用雙軸光電測(cè)量系統(tǒng)鎖定三個(gè)已知位置靶標(biāo)，測(cè)出此時(shí)光軸矢量相對(duì)車(chē)體坐標(biāo)系的方位角和俯仰角；利用慣導(dǎo)提供的初始位置、姿態(tài)和航向及雙軸光電測(cè)量系統(tǒng)測(cè)量的三組方位角和俯仰角信息，通過(guò)特定的算法計(jì)算得到車(chē)輛的精確位置、姿態(tài)和航向。本發(fā)明實(shí)施簡(jiǎn)單、快速、精確，并且可以為多個(gè)車(chē)輛同時(shí)服務(wù)，是地面車(chē)位置、姿態(tài)、航向快速精確測(cè)量的新途徑。文檔編號(hào)G01C21/28GK101672655SQ20091009315公開(kāi)日2010年3月17日申請(qǐng)日期2009年9月25日優(yōu)先權(quán)日2009年9月25日發(fā)明者馮培德,孟慶季,魁李,鐘海波申請(qǐng)人:北京航空航天大學(xué)