專利名稱：一種無人機側(cè)向領(lǐng)航方法
技術(shù)領(lǐng)域：
本發(fā)明屬于無人機導航技術(shù)領(lǐng)域，具體地說，是指一種無人機側(cè)向領(lǐng)航方法。
技術(shù)背景無人機導航的基本任務(wù)是精確確定飛機的空間位置，并使其能夠沿期望的軌跡飛行。目 前的區(qū)域?qū)Ш椒椒?，能夠允許飛機在臺基導航設(shè)備的基準臺覆蓋范圍內(nèi)或在自主導航設(shè)備能 力有限范圍內(nèi)或兩者配合下按任何希望的飛行路徑飛行。其中的臺基導航設(shè)備，包括傳統(tǒng)的 以地面電臺為基礎(chǔ)的陸基導航設(shè)備和衛(wèi)星導航系統(tǒng)為基礎(chǔ)的星基導航設(shè)備。在航線結(jié)構(gòu)上， 區(qū)域?qū)Ш降暮骄€就是由航路點系列組成的連線，這些航路點是脫離電臺臺址而自行設(shè)定的任何地理位置；在定位方法上，區(qū)域?qū)Ш蕉ǖ氖秋w機在地球上的絕對位置；在導航算法上，區(qū) 域?qū)Ш桨达w機計劃轉(zhuǎn)換到航線坐標，計算出向前方航路點飛行的待飛距離和航跡的側(cè)向偏移， 所有的計算在大圓航線上進行。目前無人機側(cè)向領(lǐng)航控制系統(tǒng)的工作原理如圖l所示。該側(cè)向領(lǐng)航方法根據(jù)期望航線以 及由傳感器獲得的飛機位置、高度、航向角信息，連續(xù)實時計算飛機相對于期望航線的側(cè)偏 距及航向角偏差等領(lǐng)航參數(shù)，并同由姿態(tài)運動傳感器獲得的飛機姿態(tài)運動信息一同輸入至側(cè) 向控制回路得到舵偏度指令，由舵回路根據(jù)舵偏度指令控制無人機飛行，最終實現(xiàn)引導無人 機沿著期望航線飛行。上述的領(lǐng)航方法在計算飛機側(cè)偏距時，通常以地球圓球模型進行計算，計算精度在十幾 到幾十米左右。由于地球繞其極軸轉(zhuǎn)動，所以赤道各處的地球半徑較其極軸方向的半徑長， 地球類似于一旋轉(zhuǎn)橢球體。因此現(xiàn)有的這種領(lǐng)航方法對于精度要求較高的無人機是不適用的。 另外，由于無人機側(cè)向領(lǐng)航方法給出的是側(cè)偏距及航向角偏差參數(shù)，在常值側(cè)風干擾下將會 有出現(xiàn)控制穩(wěn)態(tài)偏差，若能夠提供側(cè)偏移速度信息則可將控制的穩(wěn)態(tài)誤差消除。因此，有必 要考慮地球的橢球體模型，研究能夠提供高精度的側(cè)偏距信息，并提供側(cè)偏移速度信息的領(lǐng) 航方法。發(fā)明內(nèi) 容本發(fā)明的目的是提出一種無人機側(cè)向領(lǐng)航方法，根據(jù)無人機的位置信息以及期望航線得 到無人機的側(cè)偏距以及側(cè)偏移速度，進而引導無人機沿期望航線飛行。本發(fā)明的無人機側(cè)向領(lǐng)航方法通過以下步驟實現(xiàn)步驟一根據(jù)無人機的當前地理緯度^、航線從點(Z。，A)的地理諱度5。、航線到點 (A，A)的地理緯度B,以及無人機的高度H信息，由式(1)、 (2)、 (3)計算無人機的地心諱度A(.、航線從點地心諱度^,。、航線到點地心緯度S<formula>formula see original document page 0</formula>(1)(2)(3)上式中依據(jù)WGS—84坐標系，地球長半軸i 。 = 6378137.0m ，地球短半軸 i A = 6356752.3m 。步驟二根據(jù)步驟一中得到的無人機的地心煒度A:(.、航線從點地心絆度^M。、航線到 點地心緯度5,1以及無人機的地理經(jīng)度丄、航線從點(",S。)地理經(jīng)度丄。、航線到點(丄pg)地 理經(jīng)度A、無人機的高度H信息，由式(4)、 (5)、 (6)計算無人機當前位置點在WGS—84 坐標系中的天向分量k ，航線從點在WGS—84坐標系中的天向分量k。，航線到點在WGS_84 坐標系中的天向分量k,。<formula>formula see original document page 0</formula>步驟三根據(jù)步驟二中得到的天向分量k、 k。、 k,以及無人機當前地理緯度B，由式(7) 計算側(cè)偏距A。<formula>formula see original document page 0</formula>依據(jù)WGS—84坐標系，式中的地球橢偏度/ = 0.003352811 ; ka = arccoski xk0步驟四根據(jù)無人機當前位置(丄，巧、航線從點(i:。，^)、航線到點(L^、)信息，由式(S)、(9)、 (10)、計算無人機當前點在當?shù)厮阶鴺讼抵械谋毕蚍至縥，航線從點在當?shù)厮阶?標系中的天向分量k"航線到點在當?shù)厮阶鴺讼抵械奶煜蚍至縦;:<formula>formula see original document page 6</formula>步驟五根據(jù)步驟四中得到的無人機當前點在當?shù)厮阶鴺讼抵械谋毕蚍至縥，航線從 點在當?shù)厮阶鴺讼抵械奶煜蚍至縦,，航線到點在當?shù)厮阶鴺讼抵械奶煜蚍至縦3，結(jié)合無人機的北向地速^ ，東向地速^，由式(11)計算側(cè)偏移速度A^:<formula>formula see original document page 6</formula>步驟六將步驟四及步驟五中得到的飛機的側(cè)偏距A及側(cè)偏移速度"^輸出至側(cè)向控制 回路得到舵偏度指令，由舵回路根據(jù)舵偏度指令控制無人機飛行，最終實現(xiàn)引導飛機沿著期 望航線飛行。本發(fā)明提出的無人機側(cè)向領(lǐng)航方法的優(yōu)點在于(1)根據(jù)無人機的位置信息以及無人機 的期望航線得到無人機的側(cè)偏距以及側(cè)偏移速度，能夠引導無人機沿期望航線飛行，在常值 惻風千擾下為控制穩(wěn)態(tài)誤差的消除提供前提；(2)本方法在計算過程中考慮了地球為橢球體， 計算結(jié)果精度高，適用于對于精度要求較高的無人機領(lǐng)航。附圖
l是現(xiàn)有的無人機側(cè)向領(lǐng)航控制系統(tǒng)工作原理示意圖；圖2是本發(fā)明的無人機側(cè)向領(lǐng)航控制系統(tǒng)工作原理示意圖；圖3是不同垂線和諱度之間的關(guān)系示意圖；圖4是S, >^時側(cè)偏距、側(cè)偏移速度及地速的幾何關(guān)系示意圖；圖5是A <^時側(cè)偏距、側(cè)偏移速度及地速的幾何關(guān)系示意圖。
具體實施方式
下面結(jié)合附圖對本發(fā)明的無人機惻向領(lǐng)航方法作進一步說明。本發(fā)明的目的是提出一種無人機側(cè)向領(lǐng)航方法，根據(jù)無人機的位置信息以及期望航線得 到無人機的側(cè)偏距以及側(cè)偏移速度，進而引導無人機沿期望航線飛行。本發(fā)明的無人機側(cè)向 領(lǐng)航方法控制系統(tǒng)的工作原理如圖2所示。本發(fā)明的側(cè)向領(lǐng)航方法根據(jù)期望航線以及由傳感器獲得的無人機位置、高度及地速信息，連續(xù)實時計算飛機相對于期望航線的側(cè)偏距及側(cè)偏 移速度等領(lǐng)航參數(shù)，并同由姿態(tài)運動傳感器獲得的飛機姿態(tài)運動信息一同輸入至側(cè)向控制回 路得到舵偏度指令，由舵回路根據(jù)舵偏度指令控制無人機飛行，最終實現(xiàn)引導無人機沿著期 望航線飛行。為了描述無人機相對于地球的位置，進行無人機的導航定位，首先需要進行參考橢球系 的選取。由于衛(wèi)星技術(shù)和遙測技術(shù)的發(fā)展，目前已可利用衛(wèi)星測量的辦法取得全球性的大地 測量數(shù)據(jù)，從而擬和出全球大地坐標系。WGS_84坐標系正是于1984年制定的全球大地坐 標系，適應于全球定位。選取WGS一84坐標系，即確定了地球長半軸A、短半軸A和地球 橢偏度/參數(shù)，依據(jù)該坐標系，地球長半軸i 。 =6378137.0m，短半軸i 6 = 6356752.3m ，地 球橢偏度/ =0.003352811 。依據(jù)如下步驟計算無人機的側(cè)偏距A^及側(cè)偏移速度D^ :步驟一計算無人機的地心緯度S、航線從點地心諱度^.。、航線到點地心諱度^^。 根據(jù)無人機當前地理緯度^，航線從點地理緯度A，航線到點地理緯度A以及無人機的 高度H，可以計算無人機的地心緯度A,.、航線從點地心緯度5w.。、航線到點地心諱度5,,(1， 如式(1)、 (2)、 (3):^'05ATI-妙.妙o(1)(2)(3)地球表面某點常用的垂線和諱度有地理垂線和地理諱度、地心垂線和地心諱度。其中， 地理垂線是指參考橢球上某點處的法線，地理垂線與赤道平面的夾角為地理諱度；地心垂線 是指參考橢球上某點到地球中心的連線，地心垂線與赤道平面的夾角為地心諱度，如圖3所 示。設(shè)無人機當前位置為C，地心為O，圖3中CA為地理垂線，地理諱度為S ， CO為地 心垂線，地心緯度為Bw.。步驟二:計算無人機當前位置C在WGS_84坐標系中的天向分量k ，航線從點(丄。，B。)在 WGS—84坐標系中的天向分量k。，航線到點(A,A )在WGS—84坐標系中的天向分量k,。根據(jù)無人機的地心諱度5『、地理經(jīng)度￡ 、航線從點地心緯度S/:T.Q 、航線從點地理經(jīng)度Z。、 航線到點地心緯度^ ,、航線到點地理經(jīng)度A以及無人機的高度H，可以計算得到無人機當 前位置C在WGS_84坐標系中的天向分量k ，航線從點(A)，化)在WGS—84坐標系中的天 向分量k。，航線到點仏,A)在WGS—84坐標系中的天向分量、，如式(4)、 (5)、 (6):cos(A,(.).cos(丄) cos(5/;r)'sin(i)— sin(D cos(5w.0)'cos(Z0) cos(^(.o).sin(丄o)— sin(D cos(Dcos(l!) cos(D sin(丄,)— sin(5iri) _步驟三計算側(cè)偏距A。根據(jù)步驟二中所求得的無人機天向分量k、 k。、 kt，以及無人機當前地理緯度S，計算 側(cè)偏距A。-「 1(4)(5)(6)1 +21 + (1-/2).妙1、(7)其中，a = arccosk, x kn、|kxk0'k步驟四計算無人機當前位置C在當?shù)厮阶鴺讼抵械谋毕蚍至縥，航線從點在當?shù)厮?平坐標系中的天向分量^ ，航線到點在當?shù)厮阶鴺讼抵械奶煜蚍至縦3 。根據(jù)無人機當前地理緯度S、地理經(jīng)度丄、航線從點地理緯度5。、航線從點地理經(jīng)度丄。、 航線到點地理緯度A、航線到點地理經(jīng)度丄,，可以計算無人機當前點在當?shù)厮阶鴺讼抵械?北向分量j，航線從點在當?shù)厮阶鴺讼抵械奶煜蚍至縦,，航線到點在當?shù)厮阶鴺讼抵械?天向分量k;:—sin(外cos(丄)-sin(外sin(丄) (8)cos⑨ cos(S。)-cos(丄。)k, = cos08o)-sin(丄o) (9) sin(A) cos(g).cos(l!)，k3= cos(^).sin(丄,) (10) sin(A)步驟五計算側(cè)偏移速度"a。圖4及圖5分別給出了 A > &和A < S。時無人機相對于期望航線的側(cè)偏距、側(cè)偏移速度及地速的幾何關(guān)系示意圖。其中a^i;為當?shù)厮阶鴺讼?，其原點O位于無人機質(zhì)心，OIg軸指向當?shù)卣?，Oi;軸指向當?shù)卣龞|；^表示地速；A表示側(cè)偏距，定義無人機在期望航線的欲飛方向的右側(cè)為正；A^為側(cè)偏移速度，定義使A正向增加時為正；^為當?shù)厮阶?標系內(nèi)，垂直于當前航線，沿側(cè)偏距A增大的方向與北方向之間的夾角。根據(jù)無人機當前位置點在當?shù)厮阶鴺讼抵械谋毕蚍至縥 ，航線從點(z。,5。)在當?shù)厮阶鴺讼抵械奶煜蚍至縦2，航線到點仏,《)在當?shù)厮阶鴺讼抵械奶煜蚍至縦3，可以計算夾角p。p = arccoskjk，(11) 東向地速^，以(12) 東向地速J^，以(13)|k3xk2由圖4可以看出^>5。時，側(cè)偏移速度Z^與無人機的北向地速^ 及夾角P的關(guān)系為由圖5可以看出^<^。時，側(cè)偏移速度Z^與無人機的北向地速^ 及夾角P的關(guān)系為步驟六將步驟四及步驟五中得到的飛機的側(cè)偏距Dz及側(cè)偏移速度DZrf輸出至側(cè)向控制 回路得到舵偏度指令，由舵回路根據(jù)舵偏度指令來控制無人機飛行，最終實現(xiàn)引導無人機沿 著期望航線飛行。本發(fā)明提供的側(cè)向領(lǐng)航方法根據(jù)期望航線以及由傳感器獲得的無人機位置、高度、地速 信息，連續(xù)實時計算無人機相對于期望航線的側(cè)偏距"z及側(cè)偏移速度Z^等領(lǐng)航參數(shù)，并同由姿態(tài)運動傳感器獲得的無人機姿態(tài)運動信息一同輸入至側(cè)向控制回路得到舵偏度指令，由 舵回路根據(jù)舵偏度指令來控制無人機飛行，最終實現(xiàn)引導無人機沿著期望航線飛行。本發(fā)明 的領(lǐng)航方法在常值側(cè)風干擾下為控制穩(wěn)態(tài)誤差的消除提供前提，并在計算過程中考慮了地球 為橢球體，所得到的領(lǐng)航精度高，適用于對于精度要求較高的無人機領(lǐng)航。
權(quán)利要求
1、一種無人機側(cè)向領(lǐng)航方法，其特征在于步驟一、根據(jù)無人機的當前地理緯度B、航線從點(L0，B0)的地理緯度B0、航線到點(L1，B1)的地理緯度B1以及無人機的高度H信息，計算無人機的地心緯度BEC、航線從點地心緯度BEC0、航線到點地心緯度BEC全文摘要
本發(fā)明提供了一種無人機側(cè)向領(lǐng)航方法，該方法根據(jù)期望航線以及無人機位置、高度、地速信息，連續(xù)實時計算無人機相對于期望航線的側(cè)偏距D_Z及側(cè)偏移速度D_Zd等領(lǐng)航參數(shù)，并同無人機姿態(tài)運動信息一同輸入至側(cè)向控制回路得到舵偏度指令，最終引導無人機沿著期望航線飛行。本發(fā)明的領(lǐng)航方法在常值側(cè)風干擾下為控制穩(wěn)態(tài)誤差的消除提供前提，并在計算過程中考慮了地球為橢球體，所得到的領(lǐng)航精度高，適用于對于精度要求較高的無人機領(lǐng)航。
文檔編號G01C21/20GK101266150SQ20081010623
公開日2008年9月17日 申請日期2008年5月9日 優(yōu)先權(quán)日2008年5月9日
發(fā)明者崔中興, 方曉星, 瑛 王, 王養(yǎng)柱, 波 田 申請人:北京航空航天大學