一種無人機曲率連續(xù)可調(diào)路徑規(guī)劃方法
【專利說明】一種無人機曲率連續(xù)可調(diào)路徑規(guī)劃方法
[0001]
技術領域
[0002]本發(fā)明屬于無人機控制技術領域，尤其是涉及一種無人機曲率連續(xù)可調(diào)路徑規(guī)劃方法。
[0003]
【背景技術】
[0004]隨著智能控制與無人技術的發(fā)展，無人機(Unmanned Aerial Vehicle)在物流運輸、農(nóng)作物監(jiān)測、災難救援以及目標偵查等民用領域和軍事領域中發(fā)揮著越來越重要的應用價值。路徑規(guī)劃是無人機快速高效執(zhí)行任務的基礎。路徑規(guī)劃的目的是提供一條滿足無人機自身運動學要求的優(yōu)化路徑。
[0005]路徑規(guī)劃分為邏輯路徑規(guī)劃與物理路徑規(guī)劃問題。邏輯路徑規(guī)劃是在給定的區(qū)間上找到代價最小的起始點與終止點連接方式。物理路徑規(guī)劃的目的是給出滿足物理限制條件(無人機運動力學限制)的可行路徑，并且保證路徑最優(yōu):實時性、局部可調(diào)整性、精確性。
[0006]典型的邏輯路徑規(guī)劃算法有:人工勢場法(Artificial Potential Field)、TSP (Traveling Salesman Problem)、MTSP (Multiple Traveling Salesman Problem)、Dijkstra、A*、遺傳算法(Genetic Algorithm)、SOM (Self Organizing Mapping)等。但是這些算法沒有考慮物理可行性。
[0007]主流的物理路徑規(guī)劃算法有:DC (Dubins Curve)曲線、Bezier曲線、B-Spline、PH (Pythagorean Hodograph)曲線等。這些算法沒有同時考慮路徑曲率連續(xù)、曲率大小限制以及經(jīng)過所有路標點的要求。DC曲線利用直線與圓弧連接平面上兩點，保證路徑最短，但是連接點處曲率間斷Aezier與B-Spline為近似曲線，曲線逼近控制點，但是不經(jīng)過所有控制點；PH曲線曲率連續(xù)并且可以根據(jù)曲率極值反演曲線參數(shù)，但是該曲線仍屬于近似曲線，并且計算復雜度高，不適合實時路徑規(guī)劃。
[0008]

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

[0009]本發(fā)明的目的是提供一種曲率連續(xù)可調(diào)路徑規(guī)劃方法，以解決現(xiàn)有技術中存在的路徑無法滿足無人機運動學條件、路徑缺乏精確度以及算法復雜度高的問題。
[0010]本發(fā)明所采用的技術方案是:一種無人機曲率連續(xù)可調(diào)路徑規(guī)劃方法，其特征在于，包括以下步驟:
步驟1:初始化無人機任務地圖，標注各任務點的地理坐標，設定無人機的最小轉(zhuǎn)彎半徑；
步驟2:設定分段Catmull-Rom曲線的形狀參數(shù)分別為1、0 ;采用幾何算法連接各任務點，得到規(guī)劃路徑；曲率間斷點只存在于分段Catmull-Rom曲線的連接點處，計算各連接點是否滿足曲率連續(xù)條件，得到曲率間斷點Node[i], i=l、2、3...i ;
步驟3:標注曲率間斷點Node[i], i=l、2、3...1，在原任務點分布情況下，插入新的路標點或者移動關鍵路標點，完成所有連接點的曲率連續(xù)化，以達到路徑曲率連續(xù)，得到新路徑；
步驟4:計算步驟3中得到的新路徑曲線曲率值，記錄曲率值超過最大曲率限制的曲率線段OverCur [j],計算對應路徑曲線起始點為Point_sThod[j]、Point_fThod[j]；
步驟5:采用最小曲率圓過渡方法，利用曲率單調(diào)變化的Bezier曲線連接各曲率線段OverCur [j]的起始點與終點，完成曲率路徑曲線重規(guī)劃。
[0011]作為優(yōu)選，步驟3中所述的在原任務點分布情況下插入新的路標點或者移動關鍵路標點，采用的是最優(yōu)插值法，其具體實現(xiàn)包括以下子步驟:
步驟3.1:原始路徑點數(shù)目記為data_len，根據(jù)曲率突變大小，決定插入點的位置，記該位置在原任務點中的序號為ctlflag ；
步驟3.2:根據(jù)ctlflag的值，分析曲線長度受該插入點影響的曲線段；其具體分析過程包括以下子步驟:
步驟3.2.1:計算處于該點之前的長度受影響曲線段數(shù)seg_ahead ;若ctlflag > 5，則段數(shù)為3;否則段數(shù)為mod(ctlflag+l，3);
步驟3.2.2:計算處于該點之后的長度受影響曲線段數(shù)seg_after ;若len-ctrflag 多 7，其中 len-ctrflag=data_len_ctlflag，則段數(shù)為 3 ;否則段數(shù)為len-ctlflag-4；
步驟3.2.3:曲線長度受該插入點影響的曲線段總段數(shù)為seg_ahead+seg_after ；
步驟3.3:根據(jù)ctlflag的值，分析曲率值受該插入點影響的連接點個數(shù)；其具體分析過程包括以下子步驟:
步驟3.3.1:計算插入點前面曲率值受影響的連接點的個數(shù)Cur_ahead=seg_ahead_l ； 步驟3.3.2:計算插入點后面曲率值受影響的連接點的個數(shù)為:Cur_after=seg_after
-1 ;
步驟3.3.3:曲率值受該插入點影響的連接點個數(shù)為Cur_ahead+Cur_after+1 ；
步驟3.4:利用遺傳算法，優(yōu)化需要插入點的位置，該位置保證受影響的曲率值連續(xù)，并且優(yōu)化受影響的曲線長度值；
步驟3.5:重復上述步驟，完成所有連接點的曲率連續(xù)化。
[0012]作為優(yōu)選，步驟5中所述的最小曲率圓過渡方法，其具體實現(xiàn)過程包以下子步驟: 步驟5.1:分別計算Point_sThod[j]、Point_fThod[j]處曲率圓的圓心坐標Cen_s[j]、
Cen_f[j];其中曲率圓半徑為無人機最小轉(zhuǎn)彎半徑；
步驟5.2:以Point_sThod[j]為原點，建立局部坐標系，將Cen_f [j]轉(zhuǎn)化為局部坐標；步驟5.3:本方法提出的曲率圓過渡法包括C、S、C-C、C-S、S-S、S-C曲線，其中C、C-C、S-S曲線保證起始點與終點繞旋方向不變；S、C-S、S-C曲線使終點與起始點繞旋方向相反；根據(jù)以上結論，選擇轉(zhuǎn)移曲線類型，計算最小曲率圓過渡法兩個輸入?yún)?shù):形狀控制參數(shù)mCm彡I)、Bezier結構化參數(shù)theta，m由Cen_s [j]、Cen_f [j]的相對位置唯一決定，theta由m與曲率單調(diào)條件唯一決定，連接其起始點和終點；
步驟5.4:重復以上步驟，完成曲率路徑曲線重規(guī)劃。
[0013]本發(fā)明與現(xiàn)有技術相比，提出了一種經(jīng)過所有路標點的平滑路徑規(guī)劃方法，采用參數(shù)化的Catmull-Rom曲線，確保路徑經(jīng)過所有路標點；通過優(yōu)化插值與關鍵點轉(zhuǎn)移算法，保證路徑各點曲率連續(xù)；利用曲率單調(diào)平滑連接算法，控制路徑曲率極值范圍，使路徑在滿足無人機運動學條件前提下，具有精確性和可行性。
[0014]
【附圖說明】
[0015]圖1:為本發(fā)明實施例的方法流程圖；
圖2:為本發(fā)明實施例最優(yōu)插值不意圖；
圖3:為本發(fā)明實施例最優(yōu)插值方法流程圖；
圖4:為本發(fā)明實施例最小曲率過渡圓連接示意圖；
圖5:為本發(fā)明實施例最小曲率圓過渡方法流程圖；
圖6:為本發(fā)明實施例具體實施例示意圖。
[0016]
【具體實施方式】
[0017]為了便于本領域普通技術人員理解和實施本發(fā)明，下面結合附圖及實施例對本發(fā)明作進一步的詳細描述，應當理解，此處所描述的實施示例僅用于說明和解釋本發(fā)明，并不用于限定本發(fā)明。
[0018]請見圖1，本發(fā)明提供的一種無人機曲率連續(xù)可調(diào)路徑規(guī)劃方法，包括以下步驟: 步驟1:初始化無人機任務地圖，標注各任務點的地理坐標，設定無人機的最小轉(zhuǎn)彎半徑；
初始化地圖，設定無人機在該次任務中的目標點序列，根據(jù)實際地形，設定無人機飛行高度；在任務執(zhí)行過程中，無人機的速度與飛行高度不變。
[0019]步驟2:設定分段Catmull-Rom曲線的形狀參數(shù)分別為1、0 ;采用幾何算法連接各任務點，得到規(guī)劃路徑；曲率間斷點只存在于分段Catmull-Rom曲線的連接點處，計算各連接點是否滿足曲率連續(xù)條件，得到曲率間斷點Node[i], i=l、2、3...1 ；
步驟3:標注曲率間斷點Node[i], i=l、2、3...1，在原任務點分布情況下，插入新的路標點或者移動關鍵路標點，完成所有連接點的曲率連續(xù)化，以達到路徑曲率連續(xù)，得到新路徑；
在原任務點分布情況下插入新的路標點或者移動關鍵路標點，采用的是最優(yōu)插值法，請見圖2和圖3，其具體實現(xiàn)包括以下子步驟:
步驟3.1:原始路徑點數(shù)目記為data_len，根據(jù)曲率突變大小，決定插入點的位置，記該位置在原任務點中的序號為ctlflag ；
步驟3.2:根據(jù)ctlflag的值，分析曲