一種基于線性規(guī)劃的實時動態(tài)航跡規(guī)劃方法
【專利摘要】本發(fā)明公開了一種基于線性規(guī)劃的實時動態(tài)航跡規(guī)劃方法，包括如下內(nèi)容：在導航坐標系中，根據(jù)障礙物的形態(tài)特征坐標，獲得障礙物邊界坐標點所包含的坐標點集合；根據(jù)建立的離散時間模型，獲得飛行器的動態(tài)參數(shù)約束集；基于障礙物邊界坐標點所包含的坐標點集合以及飛行動態(tài)參數(shù)約束集，根據(jù)最優(yōu)航跡目標函數(shù)，獲得在導航坐標系中的航跡坐標點集，解決了現(xiàn)有技術(shù)中無法自動實現(xiàn)航跡規(guī)劃的技術(shù)問題，進而實現(xiàn)了能夠自動實現(xiàn)航跡規(guī)劃的技術(shù)效果。
【專利說明】一種基于線性規(guī)劃的實時動態(tài)航跡規(guī)劃方法

【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及航跡規(guī)劃【技術(shù)領(lǐng)域】，尤其涉及一種基于線性規(guī)劃的實時動態(tài)航跡規(guī)劃方法。

【背景技術(shù)】
[0002]近年來,無人直升機(Unmanned Aerial Vehicle, UAV)因為具有垂直起降、自主飛行等特點，適合代替人在危險、惡劣的環(huán)境中工作。使其運用不管是在軍事方面還是民用方面都得到了空前的發(fā)展，這也是飛行器未來的一個發(fā)展趨勢。無人直升機的一大特點就在于其無人駕駛，這就要求任務規(guī)劃系統(tǒng)能夠?qū)o人直升機進行自主飛行控制。
[0003]航跡規(guī)劃是無人機自主飛行的一項關(guān)鍵技術(shù)，它是在一定約束條件下(如飛行路徑上的障礙物)，根據(jù)某個或者是某幾個性能指標(如:時間最短，路程最短，燃油最省)，計算出從出發(fā)位置到終止位置的最優(yōu)飛行軌跡。
[0004]由于無人機領(lǐng)域的發(fā)展，各類航跡規(guī)算法層出不窮，比如A*搜索算法、遺傳算法、模擬退火算法、蟻群算法、粒子群優(yōu)化算法、基于勢能函數(shù)的算法等等。
[0005]中國專利CN201210274571.5，基于稀疏A*算法和遺傳算法的航跡規(guī)劃方法，將SAS算法和遺傳算法相結(jié)合進行航跡規(guī)劃。一定程度減少了航跡規(guī)劃計算量。中國專利CN201310478422.5基于故障網(wǎng)格設計航跡規(guī)劃算法，采用分布式技術(shù)實現(xiàn)計算加速。中國專利CN201310228497.8基于蜂群協(xié)同覓食算法設計多無人機航跡規(guī)劃方法，提升航跡規(guī)劃的穩(wěn)定性，提高了搜索效率。
[0006]然后前面的這些專利雖然大都滿足了航跡規(guī)劃的實時性，最優(yōu)性等特點，但是缺乏使用性，具體在實際運用中操作復雜，繁瑣。
[0007]中國專利CN201110458232.8通過鼠標從高清地圖上點航點，然后再判斷航跡的飛行性，根據(jù)航跡判斷結(jié)果手動拖動航點改變航跡，不需要手動輸入航點，操作方便，實用性強。但是其航跡是通過手動選取的，然后在判斷可行性，不斷的修改得到一條可飛航跡，這嚴格意義上來說不能算是自動航跡規(guī)劃，航跡規(guī)劃只給定起始點和目標點，自動計算出從起點到終點的一條最優(yōu)可飛航跡。
[0008]因此，現(xiàn)有技術(shù)中飛行器存在無法自動實現(xiàn)航跡規(guī)劃的技術(shù)問題。


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

[0009]本申請實施例通過提供一種基于線性規(guī)劃的實時航跡規(guī)劃方法，解決了現(xiàn)有技術(shù)中無法自動實現(xiàn)航跡規(guī)劃的技術(shù)問題，進而實現(xiàn)了能夠自動實現(xiàn)航跡規(guī)劃的技術(shù)效果。
[0010]本申請實施例提供了一種基于線性規(guī)劃的實時動態(tài)航跡規(guī)劃方法，包括如下內(nèi)容:
[0011]S101.在導航坐標系中，根據(jù)障礙物的形態(tài)特征坐標，獲得障礙物邊界坐標點所包含的坐標點集合；
[0012]S102.根據(jù)建立的離散時間模型，獲得飛行器的動態(tài)參數(shù)約束集；
[0013]S104.基于障礙物邊界坐標點所包含的坐標點集合以及飛行動態(tài)參數(shù)約束集，根據(jù)最優(yōu)航跡目標函數(shù)，獲得在導航坐標系中的航跡坐標點集。
[0014]進一步地，在SlOl之前還有:
[0015]對障礙物邊界坐標點所包含的坐標點集，離散時間模型以及動態(tài)參數(shù)約束集進行初始化。
[0016]進一步地，SlOl具體包括:
[0017]在導航坐標系中，根據(jù)障礙物的形態(tài)特征坐標，判斷障礙物是否為線性障礙物；
[0018]若判斷獲得為線性障礙物時，根據(jù)線性障礙物的避障約束面所構(gòu)成的區(qū)域，獲取避障面所構(gòu)成的區(qū)域坐標點集合；
[0019]若判斷獲得為柱形障礙物時，獲取柱形障礙物邊界坐標點所包含的坐標點集合。
[0020]進一步地，S102具體為:根據(jù)建立的離散時間模型，飛行器的速度門限值以及加速度門限值，獲得飛行的速度約束集和加速度約束集。
[0021]進一步地，S103具體為:采用AMPL語言編寫成可調(diào)用程序?qū)ψ顑?yōu)航跡目標函數(shù)，障礙物邊界坐標點所包含的坐標點集合以及飛行動態(tài)參數(shù)約束集求解，獲得飛行器在導航坐標系中的航跡坐標點集。
[0022]進一步地，在S103之后還有:
[0023]對應導航坐標系建立三維航跡規(guī)劃坐標系；
[0024]將導航坐標系中飛行器的起點坐標作為三維航跡規(guī)劃坐標系中的原點；
[0025]對應導航坐標系中的航跡坐標點集，依次在三維航跡規(guī)劃坐標系中標出飛行器的航跡最優(yōu)點集。
[0026]本申請實施例中提供的一個或多個技術(shù)方案，至少具有如下技術(shù)效果或優(yōu)點:
[0027]由于采用了在導航坐標系中，根據(jù)障礙物的形態(tài)特征坐標，獲得障礙物邊界坐標點包含的坐標點集合；然后，根據(jù)建立的離散時間模型，獲得飛行器的動態(tài)參數(shù)約束集；基于障礙物邊界坐標點所包含的坐標點集合以及飛行動態(tài)參數(shù)約束集，根據(jù)最優(yōu)航跡目標，獲得在導航坐標系中由起點到達終點的航跡坐標點集，所以，有效解決了現(xiàn)有技術(shù)中無法自動實現(xiàn)航跡規(guī)劃的技術(shù)問題，進而實現(xiàn)了能夠自動實現(xiàn)航跡規(guī)劃的技術(shù)效果。

【專利附圖】

【附圖說明】
[0028]圖1為本發(fā)明實施例中基于線性規(guī)劃的實時動態(tài)航跡規(guī)劃方法的流程圖；
[0029]圖2a，圖2b為本發(fā)明實施例中柱形障礙物的模型規(guī)劃示意圖和線性障礙物的模型規(guī)劃示意圖；
[0030]圖3為本發(fā)明實施例中飛行器在線性障礙物的實際場景中的示意圖；
[0031]圖4為本發(fā)明實施例中航跡規(guī)劃在導航地圖上的效果示意圖；
[0032]圖5為本發(fā)明實施例中對應導航坐標系建立航跡規(guī)劃坐標系的示意圖。

【具體實施方式】
[0033]本申請實施例通過提供一種基于線性規(guī)劃的實時動態(tài)航跡規(guī)劃方法，解決了現(xiàn)有技術(shù)中無法自動實現(xiàn)航跡規(guī)劃的技術(shù)問題，實現(xiàn)了能夠自動實現(xiàn)航跡規(guī)劃的技術(shù)效果。
[0034]為了解決上述無法自動實現(xiàn)航跡規(guī)劃的技術(shù)問題，總體思路如下:
[0035]首先，在導航坐標系中，根據(jù)確定的障礙物的形態(tài)特征坐標，獲得障礙物邊界坐標所包含的坐標點集合；接著，根據(jù)建立的離散時間模型，獲得飛行器的動態(tài)參數(shù)約束集?’最后，根據(jù)上述獲得的障礙物邊界坐標所包含的坐標點集合以及飛行器的動態(tài)參數(shù)約束集，以及最優(yōu)航跡目標函數(shù)，從而獲得在導航坐標系中的航跡坐標點集，從而有效實現(xiàn)了自動航跡規(guī)劃的效果。
[0036]為了更好的理解上述技術(shù)方案，下面將結(jié)合說明書附圖以及具體的實施方式對上述技術(shù)方案進行詳細的說明。
[0037]如圖1所示，本發(fā)明提出的一種基于線性規(guī)劃的實時動態(tài)航跡規(guī)劃方法，具體包括如下步驟:
[0038]S101，在導航坐標系中根據(jù)障礙物的形態(tài)特征坐標，獲得障礙物邊界坐標點所包含的坐標點集合；
[0039]S102，根據(jù)建立的離散時間模型，獲得飛行器的動態(tài)參數(shù)約束集；
[0040]S103，基于障礙物邊界坐標點所包含的坐標點集合以及飛行動態(tài)參數(shù)約束集，根據(jù)最優(yōu)航跡目標函數(shù)，獲得在導航坐標系中由起點到達終點的航跡坐標點集。
[0041]在具體的實施方式中，首先，在進行由起點到達終點的航跡規(guī)劃之前，先對障礙物的信息以及飛行動態(tài)信息進行初始化，從而避免沒有初始化所規(guī)劃出現(xiàn)航跡并不是最優(yōu)路徑。
[0042]接著，對SlOl進行詳細描述。
[0043]在SlOl中，通過檢測障礙物的形態(tài)特征坐標，可以將障礙物分為兩類，一類是線性障礙物，一類是柱形障礙物。
[0044]具體地，像建筑物、電力塔、樹木等相對規(guī)則的障礙物抽象成多邊形柱形障礙物；像電力線，墻等細長的不能從其下面穿過或旁邊繞過的障礙物抽象成線性障礙物。
[0045]具體如圖2a，圖2b所示，根據(jù)在導航坐標系中的N邊型的N個頂點的經(jīng)度，緯度，高度信息，例如以頂面四邊形為例，獲取a、b、c、d四個點的經(jīng)緯度以及高度hi，建立柱形障礙物數(shù)學模型，求出底面每條邊的數(shù)學表達式:aiX+biy+Ci<0，I = 1...N;因此，柱形障礙物的邊界點所包含的坐標點的集合如下:
[0046]滿足aiX+bj+Ci ^ O
[0047]U a2x+b2y+c2 ^ O
[0048]......
[0049]U aNx+bNy+cN ( O
[0050]U z 彡 Zmin
[0051]U z ^ Zfflax
[0052]對于線性障礙物，如圖3所示為線性障礙物的實際場景，通過設定兩個可調(diào)參數(shù)，水平安全距離Dp和高度安全距離Dh，可以得到EF平行且距離為Dp兩條水平安全線L1, L2以及垂直方向距離為Dh的垂直安全線L。過L、L1和L、L2確定兩個約束面:
[0053]amx+bmy+cmz+dm = O, anx+bny+cnz+dn = O
[0054]線性障礙物的避障約束面所構(gòu)成的區(qū)域坐標點集如下:
[0055]滿足amx+bmy+cmz+dm 彡 O
[0056]U anx+bny+cnz+dn ^ O
[0057]為了滿足混合線性規(guī)劃的計算需求，將避障約束不等式間“或”的關(guān)系變成“與”的關(guān)系，采用Big-M方法并引入一組整數(shù)變量bk，避障約束可以表示為:
[0058]柱形障礙物:
[0059]

【權(quán)利要求】
1.一種基于線性規(guī)劃的實時動態(tài)航跡規(guī)劃方法，其特征在于，包括如下內(nèi)容: 5101.在導航坐標系中，根據(jù)障礙物的形態(tài)特征坐標，獲得障礙物邊界坐標點所包含的坐標點集合； 5102.根據(jù)建立的離散時間模型，獲得飛行器的動態(tài)參數(shù)約束集； S104.基于障礙物邊界坐標點所包含的坐標點集合以及飛行動態(tài)參數(shù)約束集，根據(jù)最優(yōu)航跡目標函數(shù)，獲得在導航坐標系中的航跡坐標點集。
2.根據(jù)權(quán)利要求1所述的基于線性規(guī)劃的實時動態(tài)航跡規(guī)劃方法，其特征在于，在SlOl之前還有: 對障礙物邊界坐標點所包含的坐標點集，離散時間模型以及動態(tài)參數(shù)約束集進行初始化。
3.根據(jù)權(quán)利要求1所述的基于線性規(guī)劃的實時動態(tài)航跡規(guī)劃方法，其特征在于，SlOl具體包括: 在導航坐標系中，根據(jù)障礙物的形態(tài)特征坐標，判斷障礙物是否為線性障礙物； 若判斷獲得為線性障礙物時，根據(jù)線性障礙物的避障約束面所構(gòu)成的區(qū)域，獲取避障面所構(gòu)成的區(qū)域坐標點集合； 若判斷獲得為柱形障礙物時，獲取柱形障礙物邊界坐標點所包含的坐標點集合。
4.根據(jù)權(quán)利要求1所述的基于線性規(guī)劃的實時動態(tài)航跡規(guī)劃方法，其特征在于，S102具體為:根據(jù)建立的離散時間模型，飛行器的速度門限值以及加速度門限值，獲得飛行的速度約束集和加速度約束集。
5.根據(jù)權(quán)利要求1所述的基于線性規(guī)劃的實時動態(tài)航跡規(guī)劃方法，其特征在于，S103具體為:采用AMPL語言編寫成可調(diào)用程序?qū)ψ顑?yōu)航跡目標函數(shù)，障礙物邊界坐標點所包含的坐標點集合以及飛行動態(tài)參數(shù)約束集求解，獲得飛行器在導航坐標系中的航跡坐標點集。
6.根據(jù)權(quán)利要求5所述的基于線性規(guī)劃的實時動態(tài)航跡規(guī)劃方法，其特征在于，在S103之后還有: 對應導航坐標系建立三維航跡規(guī)劃坐標系； 將導航坐標系中飛行器的起點坐標作為三維航跡規(guī)劃坐標系中的原點； 對應導航坐標系中的航跡坐標點集，依次在三維航跡規(guī)劃坐標系中標出飛行器的航跡最優(yōu)點集。
【文檔編號】G01C21/00GK104165627SQ201410429084
【公開日】2014年11月26日 申請日期:2014年8月27日 優(yōu)先權(quán)日:2014年8月27日
【發(fā)明者】葛樹志, 王剛, 周紅坤, 王維, 錢杰, 韓東斐, 薛遠奎 申請人:電子科技大學