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

【發明內容】

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

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