一種飛行器三維導(dǎo)航方法及其裝置的制造方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及飛行器導(dǎo)航技術(shù)領(lǐng)域，尤其涉及一種飛行器三維導(dǎo)航方法及其裝置。
【背景技術(shù)】
[0002] 隨著科學(xué)技術(shù)的發(fā)展，由于人類不斷地"擺脫"地心引力，天空不再是少數(shù)人的探 索領(lǐng)域，慢慢會(huì)成為日常的人們生活的領(lǐng)域的一部分。就像現(xiàn)在的在地面上行駛的車輛，由 于地面空間有限和人口的膨脹，地方將不再夠用，以后的汽車將會(huì)在天空中翱翔。就現(xiàn)在而 言，飛行器（FlightVehicle)逐漸出現(xiàn)在人們的視野，各種各樣的飛行器用途也越來(lái)越廣。 在軍事方面，飛行器能完成組隊(duì)搬運(yùn)、目標(biāo)跟蹤、載人航天等任務(wù)；在民用方面，飛行器也能 夠運(yùn)送快遞，進(jìn)行高空航拍、三維地形掃描等任務(wù)?，F(xiàn)在，大部分飛行器還是需要人為控制， 例如需要控制者使用遙控器或者終端去控制飛行。如何讓控制者按照給定路線去控制飛行 器飛行成為了問題。
[0003] 給飛行器添加三維導(dǎo)航儀可以很好地解決飛行器跟隨三維路徑和指示路徑的問 題。目前，大部分的三維導(dǎo)航技術(shù)均是對(duì)周圍的場(chǎng)景進(jìn)行三維化，讓人感覺導(dǎo)航的世界與現(xiàn) 實(shí)世界相仿，從而提高導(dǎo)航的效果。這樣確實(shí)能夠使人更加容易地跟隨導(dǎo)航路徑的行走，但 是，這并不是使用路徑的三維導(dǎo)航。在路徑的實(shí)時(shí)顯示、數(shù)據(jù)存儲(chǔ)和提醒方面，現(xiàn)在的導(dǎo)航 儀還是使用二維的路徑，導(dǎo)航儀沒有就對(duì)路徑的三維數(shù)據(jù)或者變化作出有效的顯示或者提 示。這樣，在以后的三維導(dǎo)航發(fā)展當(dāng)中具有很大的局限性。例如：當(dāng)前飛行器在不同高度飛 行時(shí)，導(dǎo)航儀的路徑還是指示這左轉(zhuǎn)右轉(zhuǎn)，沒有考慮飛行器的高度進(jìn)行上下高低的導(dǎo)航，那 么，顯然，導(dǎo)航儀指示是不足夠的。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的在于克服現(xiàn)有技術(shù)的不足，本發(fā)明提供了一種飛行器三維導(dǎo)航方法 及其裝置，能夠更好地得到全面的三維路徑信息，讓飛行器的控制人員更加容易、更加方便 地控制飛行器。
[0005] 為了解決上述問題，本發(fā)明提出了一種飛行器三維導(dǎo)航方法，所述方法包括：
[0006] 獲取路徑的位矢、切矢、曲率、撓率數(shù)據(jù)；
[0007] 獲取飛行器實(shí)時(shí)的位置信息和姿態(tài)信息；
[0008] 根據(jù)飛行器實(shí)時(shí)的位置信息和姿態(tài)信息提供三維導(dǎo)航。
[0009] 優(yōu)選地，在所述獲取路徑的位矢、切矢、曲率、撓率數(shù)據(jù)的步驟之前，還包括：
[0010] 計(jì)算路徑的位矢、切矢、曲率、燒率數(shù)據(jù)。
[0011] 優(yōu)選地，所述計(jì)算路徑的位矢、切矢、曲率、撓率數(shù)據(jù)的步驟包括：
[0012] 根據(jù)起始點(diǎn)位置、目標(biāo)點(diǎn)位置、障礙物狀況，生成目標(biāo)路徑；
[0013] 根據(jù)目標(biāo)路徑的特性將目標(biāo)路徑分成若干個(gè)段路徑；
[0014] 計(jì)算各段路徑的切矢、曲率、撓率的參數(shù)方程；
[0015] 確定各段參數(shù)的分辨率，得到各個(gè)路徑點(diǎn)的位矢、切矢、曲率、撓率數(shù)據(jù)。
[0016] 優(yōu)選地，所述獲取路徑的位矢、切矢、曲率、撓率數(shù)據(jù)的步驟，包括：
[0017] 通過無(wú)線或者有線方式從地面站得到各個(gè)路徑點(diǎn)的位矢、切矢、曲率、撓率數(shù)據(jù)。
[0018] 優(yōu)選地，所述姿態(tài)信息通過加速度計(jì)和電子羅盤的信息融合獲得；所述位置信息 包括經(jīng)煒度信息和高度信息。
[0019] 相應(yīng)地，本發(fā)明還提供一種飛行器三維導(dǎo)航裝置，所述裝置包括：
[0020] 數(shù)據(jù)獲取模塊，用于獲取路徑的位矢、切矢、曲率、撓率數(shù)據(jù)；并獲取飛行器實(shí)時(shí)的 位置信息和姿態(tài)信息；
[0021] 導(dǎo)航模塊，用于根據(jù)飛行器實(shí)時(shí)的位置信息和姿態(tài)信息提供三維導(dǎo)航。
[0022] 優(yōu)選地，所述裝置還包括：計(jì)算模塊，用于計(jì)算路徑的位矢、切矢、曲率、撓率數(shù)據(jù)。
[0023] 優(yōu)選地，所述計(jì)算模塊包括：
[0024] 路徑生成單元，用于根據(jù)起始點(diǎn)位置、目標(biāo)點(diǎn)位置、障礙物狀況，生成目標(biāo)路徑；
[0025] 路徑分割單元，用于根據(jù)目標(biāo)路徑的特性將目標(biāo)路徑分成若干個(gè)段路徑；
[0026] 計(jì)算各段路徑的切矢、曲率、撓率的參數(shù)方程；
[0027] 確定各段參數(shù)的分辨率，得到各個(gè)路徑點(diǎn)的位矢、切矢、曲率、撓率數(shù)據(jù)
[0028] 優(yōu)選地，所述數(shù)據(jù)獲取模塊通過無(wú)線或者有線方式從地面站得到各個(gè)路徑點(diǎn)的位 矢、切矢、曲率、撓率數(shù)據(jù)。
[0029] 優(yōu)選地，所述姿態(tài)信息通過加速度計(jì)和電子羅盤的信息融合獲得；所述位置信息 包括經(jīng)煒度信息和高度信息。
[0030] 在本發(fā)明實(shí)施例中，通過地面站進(jìn)行飛行路徑的規(guī)劃，傳輸各個(gè)路徑點(diǎn)的位矢、切 矢、曲率、撓率這幾個(gè)幾何不變量到飛行器，飛行器在飛行過程中利用自身位置和姿態(tài)信息 對(duì)路徑的導(dǎo)航做出適當(dāng)?shù)奶幚恚瑸槿S路徑導(dǎo)航和顯示提供保障。能夠更好地提供路徑的 信息，讓飛行器的控制人員更加容易、更加方便地控制飛行器；不但更加直觀，使飛行器的 控制更加簡(jiǎn)單，而且提高飛行器控制的安全性。
【附圖說(shuō)明】
[0031] 為了更清楚地說(shuō)明本發(fā)明實(shí)施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對(duì)實(shí)施例或現(xiàn) 有技術(shù)描述中所需要使用的附圖作簡(jiǎn)單地介紹，顯而易見地，下面描述中的附圖僅僅是本 發(fā)明的一些實(shí)施例，對(duì)于本領(lǐng)域普通技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)的前提下，還可以 根據(jù)這些附圖獲得其它的附圖。
[0032] 圖1是本發(fā)明實(shí)施例的飛行器三維導(dǎo)航方法的流程示意圖；
[0033] 圖2是本發(fā)明實(shí)施例中根據(jù)飛行器實(shí)時(shí)的位置信息和姿態(tài)信息提供三維導(dǎo)航的 流程示意圖；
[0034] 圖3是本發(fā)明實(shí)施例中坐標(biāo)變換的流程示意圖；
[0035] 圖4是本發(fā)明實(shí)施例中導(dǎo)航儀的坐標(biāo)系確立示意圖；
[0036] 圖5是本發(fā)明實(shí)施例的飛行器三維導(dǎo)航裝置的結(jié)構(gòu)組成示意圖。
【具體實(shí)施方式】
[0037] 下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完 整地描述，顯然，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例?；?本發(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒有作出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他 實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。
[0038] 圖1是本發(fā)明實(shí)施例的飛行器三維導(dǎo)航方法的流程示意圖，如圖1所示，該方法包 括：
[0039] S1，獲取路徑的位矢、切矢、曲率、撓率數(shù)據(jù)；
[0040] S2,獲取飛行器實(shí)時(shí)的位置信息和姿態(tài)信息；
[0041] S3,根據(jù)飛行器實(shí)時(shí)的位置信息和姿態(tài)信息提供三維導(dǎo)航。
[0042] 進(jìn)一步地，在S1之前，還包括：
[0043] 計(jì)算路徑的位矢、切矢、曲率、撓率數(shù)據(jù)。
[0044] 計(jì)算路徑的位矢、切矢、曲率、撓率數(shù)據(jù)的步驟包括：
[0045] 根據(jù)起始點(diǎn)位置、目標(biāo)點(diǎn)位置、障礙物狀況，生成目標(biāo)路徑；
[0046] 根據(jù)目標(biāo)路徑的特性將目標(biāo)路徑分成若干個(gè)段路徑；
[0047] 計(jì)算各段路徑的切矢、曲率、撓率的參數(shù)方程；
[0048] 確定各段參數(shù)的分辨率，得到各個(gè)路徑點(diǎn)的位矢、切矢、曲率、撓率數(shù)據(jù)。
[0049] 具體實(shí)施中，地面站根據(jù)起始點(diǎn)位置（經(jīng)煒坐標(biāo)和高度），目標(biāo)點(diǎn)位置（經(jīng)煒坐 標(biāo)和高度）和障礙物狀況，生成目標(biāo)路徑。接著再將目標(biāo)路徑分成若干段，這個(gè)可以根據(jù) 計(jì)算機(jī)處理器的運(yùn)算速度、問題對(duì)路徑要求的計(jì)算精度等決定分的路徑的段數(shù)。這里如果 路徑是連續(xù)的，則路徑可以直接進(jìn)入下一步參數(shù)方程的轉(zhuǎn)化；如果路徑是離散的，則每一 段進(jìn)行插值以后在進(jìn)行參數(shù)化。將各段路徑分別轉(zhuǎn)換為一般參數(shù)（以下將t設(shè)置為參數(shù)， te[0，1])下的參數(shù)方程r(t)。每段參數(shù)方程的r(t)的格式如下：
[0050] r(t) = [x(t)y(t)z(t)]
[0051] 每一列x(t)、y(t)、z(t)分別代表這段中各個(gè)點(diǎn)的煒度，經(jīng)度，高度的參數(shù)方程。 對(duì)r⑴求一次導(dǎo)r'⑴，二次導(dǎo)r"⑴，三次導(dǎo)r" '⑴。

[0067] 然后再利用曲率、撓率公式，代入計(jì)算，即可得到各個(gè)點(diǎn)的曲率和撓率的數(shù)值。
[0068] 上述得到的參數(shù)方程均為連續(xù)方程，為了更好地將路徑傳輸給飛行器，接下來(lái)將 路徑離散化。首先需要確定參數(shù)t的分辨率。
[0069] 優(yōu)選地，分辨率可以根據(jù)曲率決定，曲率大的地方，t的分辨率大，控制點(diǎn)較多；曲 率小的地方，t的分辨率可以小一點(diǎn)，控制點(diǎn)可以稍微少一點(diǎn)。
[0070] 這里說(shuō)明采用參數(shù)方程的好處就是，更改路徑的分辨率，只需要更改t的分辨率 即可，并不需要重新計(jì)算曲率，撓率的公式。
[0071] 將各路徑段的參數(shù)t代入各路徑段的位矢參數(shù)方程r(t)、切矢參數(shù)方程r'（t)、曲 率參數(shù)方程κ(t)和撓率參數(shù)方程τ(t)，即可得到各個(gè)路徑點(diǎn)的位矢、切矢、曲率和撓率。
[0072] 在S1中，飛行器通過無(wú)線或者有線方式從地面站得到各個(gè)路徑點(diǎn)的位矢、切矢、 曲率、撓率數(shù)據(jù)，獲得獲得數(shù)據(jù)后，存儲(chǔ)在ROM或者SD卡中。
[0073] 每個(gè)路徑點(diǎn)的數(shù)據(jù)存儲(chǔ)方式為：
[0074] p= [r1X3r1Χ3κ1Χ1τ