本發(fā)明涉及通用航空管制領(lǐng)域技術(shù)，特別涉及一種多形態(tài)障礙的無(wú)人機(jī)路徑規(guī)劃方法及系統(tǒng)。
背景技術(shù)：
：執(zhí)行偵察、空中攝影任務(wù)的無(wú)人機(jī)飛行，通常會(huì)有部分行程甚至整個(gè)行程都是“穿越障礙飛行”，考慮到無(wú)人機(jī)以及其執(zhí)行任務(wù)的特征，對(duì)無(wú)人機(jī)避障算法有整體性和實(shí)時(shí)性的雙重要求。在穿越障礙飛行中，“穿山飛行”和“穿城飛行”也會(huì)有所區(qū)別。尤其是在都市的樓宇越建越高，無(wú)人機(jī)城市飛行的需求也變多的情況下，將“穿山飛行”的路徑規(guī)劃算法簡(jiǎn)單地套用在“穿城飛行”的任務(wù)中不合適的。樓宇避障和山川避障有許多不同點(diǎn)，一是山川高低起伏延綿，高度漸變，能用很簡(jiǎn)單的方法防止側(cè)撞，而樓宇的高低差垂直，屬于高度突變，無(wú)人機(jī)受自已最大曲率等物理?xiàng)l件和硬件條件的限制，面對(duì)樓宇這樣的障礙，往往雖然測(cè)到并采取了措施，但依然無(wú)法及時(shí)避障；二是對(duì)于穿城飛行，有飛機(jī)離樓宇很近的需求(比如拍照、送貨、救援)等等，而對(duì)于穿山飛行沒(méi)有這樣的需求。適用于無(wú)人機(jī)飛行的路徑應(yīng)該滿足無(wú)人機(jī)曲率約束的，曲率連接的可飛行路徑。因此，一些研究者將適用于無(wú)人機(jī)飛行的曲線直接用于無(wú)人機(jī)的路徑規(guī)劃中。比如，Shanmugavel團(tuán)隊(duì)和Nikolos團(tuán)隊(duì)分別將Dubins曲線、Clothoid曲線以及B樣條曲線用于無(wú)人機(jī)的路徑規(guī)劃。然而，Dubins路徑雖然滿足無(wú)人機(jī)的曲率約束，但曲率不連接，不利于無(wú)人機(jī)飛行控制跟蹤實(shí)現(xiàn)；Clothoid曲線雖然曲率連續(xù)，但要生成滿足約束條件的無(wú)人機(jī)可飛行路徑，該復(fù)合路徑生成過(guò)程復(fù)雜且靈活性差。B樣條曲線也具有曲率連續(xù)的特性，但由于該曲線上的曲率沒(méi)有確定的有理表達(dá)式，使路徑上每一點(diǎn)不易滿足無(wú)人機(jī)的最大曲率約束。針對(duì)上述曲線的局限性，有學(xué)者提出了用PythagoreanHodograph曲線來(lái)規(guī)劃路徑。PH曲線具有曲率連續(xù)、曲線平滑以及曲線長(zhǎng)度和曲率均有有理表達(dá)式的特點(diǎn)，生成的路徑能滿足無(wú)人機(jī)的最大曲率約束且曲率連續(xù)，便于無(wú)人機(jī)底層的飛行控制跟蹤實(shí)現(xiàn)。為了方便PH路徑的生成，對(duì)障礙的建模通常是對(duì)它們?cè)O(shè)置一個(gè)安全圓，即假設(shè)障礙物被安全圓包圍。但這樣對(duì)于樓宇避障的路徑規(guī)劃任務(wù)會(huì)產(chǎn)生一個(gè)問(wèn)題，樓宇的形狀通常為矩形，對(duì)它們?cè)O(shè)置安全圓，容易導(dǎo)致安全區(qū)域被人為放大，造成空間不必要的浪費(fèi)。而且，相鄰樓宇的安全圓有可能重疊，對(duì)于這種情況，只能對(duì)重疊的安全圓聯(lián)合起來(lái)再設(shè)置一個(gè)安全圓，造成空間的進(jìn)一步浪費(fèi)，有可能僅是對(duì)障礙建模后，整個(gè)路徑規(guī)劃就無(wú)法進(jìn)行下去(見(jiàn)圖1)。然而，安全圓算法是目前計(jì)算最簡(jiǎn)便的方式，如果直接用多邊形來(lái)表達(dá)障礙物，那么生成有曲率限制的路徑就變成了NP難題，這樣，安全空間的節(jié)省與計(jì)算的簡(jiǎn)便就成了一對(duì)矛盾的概念。因此，對(duì)樓宇障礙物進(jìn)行動(dòng)態(tài)建模，尋找既能節(jié)省安全空間又能快速計(jì)算的路徑規(guī)劃方法，在無(wú)人機(jī)樓宇路徑規(guī)劃中就變得十分重要。技術(shù)實(shí)現(xiàn)要素：有鑒于此，有必要提供一種能夠規(guī)劃出效率更高、彎曲能量更小的路徑，也解決了靜態(tài)安全圓造成的空間浪費(fèi)和多邊形障礙的NP題之間矛盾問(wèn)題的多形態(tài)障礙的無(wú)人機(jī)路徑規(guī)劃方法及系統(tǒng)。一種多形態(tài)障礙的無(wú)人機(jī)路徑規(guī)劃方法，所述多形態(tài)障礙的無(wú)人機(jī)路徑規(guī)劃方法包括如下步驟：S1、繪制初始矩形障礙圖及初始路徑，將初始障礙圖和初始路徑進(jìn)行重疊對(duì)比，提取出與初始路徑有重合的矩形障礙；S2、對(duì)與初始路徑有重合的障礙，根據(jù)其與初始路徑交匯點(diǎn)的特征，繪制動(dòng)態(tài)安全圓，并指定初始路徑的調(diào)整方向；S3、在每?jī)蓚€(gè)相鄰安全圓之間的路徑上取一點(diǎn)作為航姿點(diǎn)，組成初始路徑的動(dòng)態(tài)航姿點(diǎn)集，將初始路徑分割成若干段路徑段，以相鄰航姿點(diǎn)為起點(diǎn)和終點(diǎn)，根據(jù)中間安全圓特征及其調(diào)整方向在路徑段上設(shè)置一個(gè)新航姿點(diǎn)；S4、保持起點(diǎn)和終點(diǎn)的特征不變，依次調(diào)整每條路徑段曲率，使路徑段經(jīng)過(guò)新航姿點(diǎn)，釋放所有的安全圓和航姿點(diǎn)，將調(diào)整后的路徑段恢復(fù)成一整條符合曲率約束和安全約束的路徑。一種多形態(tài)障礙的無(wú)人機(jī)路徑規(guī)劃系統(tǒng)，所述多形態(tài)障礙的無(wú)人機(jī)路徑規(guī)劃系統(tǒng)包括以下功能模塊：重疊對(duì)比模塊，用于繪制初始矩形障礙圖及初始路徑，將初始障礙圖和初始路徑進(jìn)行重疊對(duì)比，提取出與初始路徑有重合的矩形障礙；動(dòng)態(tài)調(diào)整模塊，用于對(duì)與初始路徑有重合的障礙，根據(jù)其與初始路徑交匯點(diǎn)的特征，繪制動(dòng)態(tài)安全圓，并指定初始路徑的調(diào)整方向；航姿點(diǎn)確定模塊，用于在每?jī)蓚€(gè)相鄰安全圓之間的路徑上取一點(diǎn)作為航姿點(diǎn)，組成初始路徑的動(dòng)態(tài)航姿點(diǎn)集，將初始路徑分割成若干段路徑段，以相鄰航姿點(diǎn)為起點(diǎn)和終點(diǎn)，根據(jù)中間安全圓特征及其調(diào)整方向在路徑段上設(shè)置一個(gè)新航姿點(diǎn)；安全路徑規(guī)劃模塊，用于保持起點(diǎn)和終點(diǎn)的特征不變，依次調(diào)整每條路徑段曲率，使路徑段經(jīng)過(guò)新航姿點(diǎn)，釋放所有的安全圓和航姿點(diǎn)，將調(diào)整后的路徑段恢復(fù)成一整條符合曲率約束和安全約束的路徑。本發(fā)明提供了一種多形態(tài)障礙的無(wú)人機(jī)路徑規(guī)劃方法及系統(tǒng)，其通過(guò)路徑與其相交的障礙矩形的特征繪制安全圓，并指定初始路徑的調(diào)整方向；根據(jù)安全圓和路徑的特征設(shè)置航姿點(diǎn)，并根據(jù)安全圓及其所對(duì)應(yīng)障礙的特征指定新的航姿點(diǎn)。保持起點(diǎn)和終點(diǎn)的特征不變，依次調(diào)整每條路徑段曲率，使路徑段經(jīng)過(guò)新航姿點(diǎn)，釋放所有的安全圓和航姿點(diǎn)，從而得到一條符合曲率約束和安全約束的路徑。通過(guò)設(shè)定“動(dòng)態(tài)安全圓”、“動(dòng)態(tài)航姿點(diǎn)”，從而能夠規(guī)劃出效率更高、彎曲能量更小的路徑，也解決了靜態(tài)安全圓造成的空間浪費(fèi)和多邊形障礙的NP題之間的矛盾問(wèn)題。附圖說(shuō)明圖1是本發(fā)明一較佳實(shí)施例的多形態(tài)障礙的無(wú)人機(jī)路徑規(guī)劃方法流程圖；圖2是圖1中步驟S1的子流程圖；圖3是圖1中步驟S2的子流程圖；圖4是圖1中步驟S3的子流程圖；圖5是本發(fā)明實(shí)施例中多形態(tài)障礙的無(wú)人機(jī)路徑規(guī)劃方法的工作流程示意圖；圖6為本發(fā)明實(shí)施例中城市樓宇障礙圖模型；圖7為本發(fā)明實(shí)施例中初始PH路徑與障礙相交圖；圖8為本發(fā)明實(shí)施例中障礙相對(duì)坐標(biāo)系；圖9為本發(fā)明實(shí)施例中第一種情況下動(dòng)態(tài)安全圓建模及調(diào)整方向指定；圖10為本發(fā)明實(shí)施例中第二種情況安全圓建模及調(diào)整方向指定；圖11為本發(fā)明實(shí)施例中第三種情況的安全圓建模及調(diào)整方向指定；圖12為本發(fā)明實(shí)施例中動(dòng)態(tài)航姿點(diǎn)的設(shè)置；圖13為本發(fā)明實(shí)施例中基于新設(shè)航姿點(diǎn)的路徑段調(diào)整。具體實(shí)施方式為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖及實(shí)施例，對(duì)本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說(shuō)明，應(yīng)當(dāng)理解，此處所描述的具體實(shí)施例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。如圖1所示，本發(fā)明實(shí)施例提供一種多形態(tài)障礙的無(wú)人機(jī)路徑規(guī)劃方法，其特征在于，所述多形態(tài)障礙的無(wú)人機(jī)路徑規(guī)劃方法包括如下步驟：S1、繪制初始矩形障礙圖及初始路徑，將初始障礙圖和初始路徑進(jìn)行重疊對(duì)比，提取出與初始路徑有重合的矩形障礙。其中，如圖2所示，步驟S1包括以下分步驟：S11、繪制初始路徑周圍的障礙，每一個(gè)障礙用矩形表示，并保存初始障礙圖；S12、確定起始點(diǎn)和目標(biāo)點(diǎn)的位姿，繪制一條初始PH路徑；S13、修正初始PH路徑，使其滿足無(wú)人機(jī)的曲率約束。具體的，如圖6所示，考慮到無(wú)人機(jī)飛行區(qū)域的樓宇分布，繪制障礙圖模型，為了保證模型與真實(shí)情況盡量相同且建模方法簡(jiǎn)單，所有的樓宇都用一個(gè)矩形表示。用P(x，y，θ)來(lái)表示無(wú)人機(jī)在特定位置的特定姿態(tài)，其中，(x，y)為無(wú)人機(jī)位置，θ為該位置曲線切線的方向角。因此無(wú)人機(jī)的起始點(diǎn)可表示為Ps(xs，ys，θs)，終點(diǎn)為Pf(xf，yf，θf(wàn))，在路徑規(guī)劃前，這兩個(gè)點(diǎn)的位姿參數(shù)(xs，ys，θs)和(xf，yf，θf(wàn))為已知。用r(t)表示路徑，則連接Ps和Pf的路徑可寫成如下形式：其中，r(0)表示起點(diǎn)，r(1)表示終點(diǎn)。考慮到無(wú)人機(jī)安全性約束∏safe和曲率約束∏k，則路徑規(guī)劃的等式或修正為如下形式：對(duì)于任意多項(xiàng)式曲線r(t)＝{x(t)，y(t)}，若其滿足條件x′2(t)+y′2(t)＝σ2(t)(3)則多項(xiàng)式曲線r(t)稱為PH曲線，σ(t)是一個(gè)多項(xiàng)式。基于數(shù)值穩(wěn)定性的考慮，PH路徑可以寫成Bezier形式。Bezier形式下的n階多項(xiàng)式可寫為如下形式：r(t)=Σk=0nbknktk(1-t)(n-k),t∈[0,1]---(4)]]>其中，bk＝(xk，yk)，k是控制點(diǎn)，5次PH曲線已經(jīng)具備了曲率連續(xù)的特征，因此可作為路徑規(guī)劃的曲線。當(dāng)n＝5時(shí)，PH曲線為5次多項(xiàng)式曲線，上式可寫為r(t)=Σk=05bk5ktk(1-t)(5-k)=b0(1-t)5+5b1t(1-t)4+10b2t2(1-t)3+10b3t3(1-t)2+5b4t4(1-t)+b5t5---(5)]]>對(duì)上式求導(dǎo)，可得：r′(t)=5(b1-b0)(1-t)4+20(b2-b1)t(1-t)3+30(b4-b3)t3(1-t)2+5(b5-b4)t4---(6)]]>將t＝0和t＝1時(shí)的位置坐標(biāo)代入到式(5)和(6)中，可得b0=r(0)=(xs,ys)b5=r(1)=(xf,yf)b1=b0+15(cosθs,sinθs)b4=b5+15(cosθf,sinθf)---(7)]]>由上式可以看出，控制點(diǎn)(b0，b1，b4，b5)可由起始和終止的位姿點(diǎn)確定，最終，初始曲線的生成可簡(jiǎn)化為尋找(b2，b3)使得等式(5)滿足等式(3)給出的PH曲線的條件，最終可以得到四個(gè)解。選擇四條曲線中彈性彎曲能量最小的作為初始的PH曲線，彈性彎曲能量E的表達(dá)式如下E＝∫κ2ds(8)其中，K為每一點(diǎn)的曲率，S為整條初始曲線。S2、對(duì)與初始路徑有重合的障礙，根據(jù)其與初始路徑交匯點(diǎn)的特征，繪制動(dòng)態(tài)安全圓，并指定初始路徑的調(diào)整方向；其中，如圖3所示，步驟S2包括以下分步驟：S21、以矩形的中心點(diǎn)為坐標(biāo)原點(diǎn)，建立矩形障礙物的相對(duì)坐標(biāo)系；S22、根據(jù)路徑與矩形的交點(diǎn)以及矩形的頂點(diǎn)構(gòu)建相交多邊形，在相交多邊形中劃分直角三角形；S23、篩選出面積最大的直角三角形，繪制該三角形的外接圓，即動(dòng)態(tài)安全圓，以安全圓被路徑分為兩個(gè)部分中包含梯形面積較多部分的方向作為路徑的調(diào)整方向。如圖7所示，將無(wú)人機(jī)初始規(guī)劃路徑與障礙圖重合，只對(duì)與路徑有交集的障礙物進(jìn)行動(dòng)態(tài)安全圓的建模。對(duì)與路徑無(wú)交集的障礙物則不用考慮，從而有效節(jié)省路徑規(guī)劃計(jì)算的時(shí)間。對(duì)與路徑交集的障礙物(矩形)，設(shè)其長(zhǎng)為2L，寬為2W，以矩形的中心點(diǎn)為坐標(biāo)原點(diǎn)，長(zhǎng)軸方向?yàn)閄軸，短軸方向?yàn)閅軸，建立矩形障礙物的相對(duì)坐標(biāo)系，如圖8所示。對(duì)障礙物的安全圓建模分以下三種情況：(1)如圖9所示，當(dāng)路徑與矩形的交點(diǎn)在矩形的相鄰邊上：這種情況下，兩個(gè)交點(diǎn)和矩形的一個(gè)頂點(diǎn)構(gòu)成一個(gè)直角三角形(如圖9的圖形填充部分)。為這個(gè)直角三角形繪制外接圓，將此圓作為該矩形障礙物的安全圓(如圖9中的圓)。從幾何學(xué)的角度來(lái)看，路徑將安全圓分成了兩個(gè)部分，計(jì)算兩個(gè)部分中所包含的三角形的面積大小，選擇包含面積大的那個(gè)部分，作為路徑的調(diào)整方向(如圖9的箭頭)。(2)如圖10所示，當(dāng)路徑與矩形的交點(diǎn)在矩形的對(duì)邊上，且兩個(gè)交點(diǎn)位于相鄰的相限：這種情況下，兩個(gè)交點(diǎn)和矩形的兩個(gè)頂點(diǎn)構(gòu)成一個(gè)直角梯形(如圖10的圖形填充部分)。連接直角梯形的兩對(duì)角構(gòu)成兩個(gè)直角三角形，選擇面積大的直角三角形繪制外接圓，就可以保證此外接圓將整個(gè)梯形完全包含在內(nèi)，將此圓作為該矩形障礙物的安全圓(如圖10中的圓)。與第一種情況相同，計(jì)算被路徑分為的安全圓的兩個(gè)部分中包含梯形面積多的那個(gè)部分作為路徑的調(diào)整方向(如圖10的箭頭)。(3)如圖11所示，當(dāng)路徑與矩形的交點(diǎn)在矩形的對(duì)邊上，且兩個(gè)交點(diǎn)位于不相鄰的相限：這種情況下，兩個(gè)交點(diǎn)和矩形的頂點(diǎn)構(gòu)成兩個(gè)直角梯形(如圖11中的兩種圖形填充部分)。采用第二種情況中的繪制方式，可以繪制出兩個(gè)外接圓，將兩個(gè)外接圓均作為安全圓(如圖11中的圓)。每個(gè)安全圓的調(diào)整方向依據(jù)與第二種情況相同，那么兩個(gè)安全圓的調(diào)整方向是相反的(如圖11的兩個(gè)箭頭)，因此這種情況下的路徑會(huì)產(chǎn)生兩種修正路徑。S3、在每?jī)蓚€(gè)相鄰安全圓之間的路徑上取一點(diǎn)作為航姿點(diǎn)，組成初始路徑的動(dòng)態(tài)航姿點(diǎn)集，將初始路徑分割成若干段路徑段，以相鄰航姿點(diǎn)為起點(diǎn)和終點(diǎn)，根據(jù)中間安全圓特征及其調(diào)整方向在路徑段上設(shè)置一個(gè)新航姿點(diǎn)。其中，如圖4所示，步驟S3包括以下分步驟：S31、沿修正后的初始路徑的延伸方向，計(jì)算相鄰兩個(gè)安全圓中初始路徑自前一個(gè)安全圓的出點(diǎn)至后一個(gè)安全圓的入點(diǎn)之間的路勁長(zhǎng)度，取該路徑的中點(diǎn)為航姿點(diǎn)；S32、繪制初始路徑自前一個(gè)安全圓的出點(diǎn)至后一個(gè)安全圓的入點(diǎn)之間的直線線段，做該直線線段的垂直平分線，設(shè)置所述垂直平分線沿調(diào)整方向與安全圓相交的交點(diǎn)為新航姿點(diǎn)。具體的，通過(guò)一系列航姿點(diǎn)的無(wú)人機(jī)路徑規(guī)劃問(wèn)題還可以用下式描述：其中，i＝1，…，n為航姿點(diǎn)的數(shù)目，當(dāng)i＝1時(shí)，航姿點(diǎn)為起點(diǎn)，當(dāng)i＝n時(shí)，航姿點(diǎn)為終點(diǎn)，無(wú)論路徑的特征如何，起點(diǎn)和終點(diǎn)都是固定的航姿點(diǎn)。在所有的安全圓確定好之后，根據(jù)路徑相交的特征，在每個(gè)兩相鄰安全圓之間的路徑段上設(shè)置一個(gè)航姿點(diǎn)。以第N個(gè)航姿點(diǎn)為例，由于它在第N-1和第N個(gè)安全圓之間，計(jì)算從第N-1個(gè)安全圓的出點(diǎn)到第N個(gè)安全圓的入點(diǎn)之間的路徑的長(zhǎng)度，取中點(diǎn)為第N個(gè)航姿點(diǎn)，如圖12所示。根據(jù)動(dòng)態(tài)航姿點(diǎn)的設(shè)置方式可以得出航姿點(diǎn)的數(shù)量比安全圓多1個(gè)，相鄰兩個(gè)航姿點(diǎn)之間必然有一個(gè)安全圓，而路徑被航姿點(diǎn)分成了n-1個(gè)路徑段。在調(diào)整路徑時(shí)，假設(shè)每個(gè)航姿點(diǎn)的位置(x,y)和曲線切線方向角θ均衡定不變，那么調(diào)整路徑的曲率的問(wèn)題就等價(jià)為調(diào)整n-1個(gè)路徑段的曲率問(wèn)題?？紤]到相鄰兩個(gè)航姿點(diǎn)m和m+1之間的路徑段和其間的安全圓m，設(shè)置路徑對(duì)安全圓的入點(diǎn)為PIN，出點(diǎn)為POUT，做線段PIN-POUT的垂直平分線，順著調(diào)整方向，與安全圓交于M點(diǎn)，設(shè)置這個(gè)M點(diǎn)為新的航姿點(diǎn)。S4、保持起點(diǎn)和終點(diǎn)的特征不變，依次調(diào)整每條路徑段曲率，使路徑段經(jīng)過(guò)新航姿點(diǎn)，釋放所有的安全圓和航姿點(diǎn)，將調(diào)整后的路徑段恢復(fù)成一整條符合曲率約束和安全約束的路徑，如圖13所示。如圖6所示，本發(fā)明包括如下具體步驟：步驟1.根據(jù)GPS提供的城市圖，繪制初始路徑周圍的障礙，每一個(gè)障礙用矩形表示，并保存初始障礙圖。步驟2.確定起始點(diǎn)和目標(biāo)點(diǎn)的位姿(即位置和方向角)，繪制一條初始的PH路徑。步驟3.修正初始路徑，使其滿足無(wú)人機(jī)的曲率約束。步驟4.對(duì)比初始障礙圖和修正的初始路徑，判斷它們之間有沒(méi)有重合點(diǎn)。如果沒(méi)有，則該路徑即為滿足曲率約束和安全約束的路徑，進(jìn)入步驟11；如果有，則進(jìn)入步驟5。步驟5.提取出與路徑有重合的矩形障礙。步驟6.對(duì)提取出的障礙，根據(jù)其與路徑交匯點(diǎn)的特征，繪制安全圓，并指定路徑的調(diào)整方向。步驟7.在每?jī)蓚€(gè)相鄰安全圓之間的路徑上取一點(diǎn)作為航姿點(diǎn)，組成路徑的動(dòng)態(tài)航姿點(diǎn)集，將路徑分割成若干段路徑段。步驟8.以相鄰航姿點(diǎn)為起點(diǎn)和終點(diǎn)，根據(jù)中間安全圓特征及其調(diào)整方向，設(shè)置一個(gè)新航姿點(diǎn)。步驟9.保持起點(diǎn)和終點(diǎn)的特征不變，依次調(diào)整每條路徑段曲率，使路徑段經(jīng)過(guò)新航姿點(diǎn)，從而避開(kāi)動(dòng)態(tài)安全圓。返回步驟3。步驟10.釋放所有的安全圓和航姿點(diǎn)，將調(diào)整后的路徑段恢復(fù)成一整條調(diào)整后路徑。步驟11.路徑規(guī)劃結(jié)束，輸入符合曲率約束和安全約束的路徑?；谏鲜龆嘈螒B(tài)障礙的無(wú)人機(jī)路徑規(guī)劃方法，本發(fā)明還提供一種多形態(tài)障礙的無(wú)人機(jī)路徑規(guī)劃系統(tǒng)，所述多形態(tài)障礙的無(wú)人機(jī)路徑規(guī)劃系統(tǒng)包括以下功能模塊：重疊對(duì)比模塊，用于繪制初始矩形障礙圖及初始路徑，將初始障礙圖和初始路徑進(jìn)行重疊對(duì)比，提取出與初始路徑有重合的矩形障礙；動(dòng)態(tài)調(diào)整模塊，用于對(duì)與初始路徑有重合的障礙，根據(jù)其與初始路徑交匯點(diǎn)的特征，繪制動(dòng)態(tài)安全圓，并指定初始路徑的調(diào)整方向；航姿點(diǎn)確定模塊，用于在每?jī)蓚€(gè)相鄰安全圓之間的路徑上取一點(diǎn)作為航姿點(diǎn)，組成初始路徑的動(dòng)態(tài)航姿點(diǎn)集，將初始路徑分割成若干段路徑段，以相鄰航姿點(diǎn)為起點(diǎn)和終點(diǎn)，根據(jù)中間安全圓特征及其調(diào)整方向在路徑段上設(shè)置一個(gè)新航姿點(diǎn)；安全路徑規(guī)劃模塊，用于保持起點(diǎn)和終點(diǎn)的特征不變，依次調(diào)整每條路徑段曲率，使路徑段經(jīng)過(guò)新航姿點(diǎn)，釋放所有的安全圓和航姿點(diǎn)，將調(diào)整后的路徑段恢復(fù)成一整條符合曲率約束和安全約束的路徑。其中，所述重疊對(duì)比模塊包括以下功能子模塊：障礙圖繪制子模塊，用于繪制初始路徑周圍的障礙，每一個(gè)障礙用矩形表示，并保存初始障礙圖；初始路勁繪制模塊，用于確定起始點(diǎn)和目標(biāo)點(diǎn)的位姿，繪制一條初始PH路徑；路徑修正模塊，用于修正初始PH路徑，使其滿足無(wú)人機(jī)的曲率約束。其中，所述動(dòng)態(tài)調(diào)整模塊包括以下功能子模塊：坐標(biāo)系建立子模塊，用于以矩形的中心點(diǎn)為坐標(biāo)原點(diǎn)，建立矩形障礙物的相對(duì)坐標(biāo)系；繪制三角形子模塊，用于根據(jù)路徑與矩形的交點(diǎn)以及矩形的頂點(diǎn)構(gòu)建相交多邊形，在相交多邊形中劃分直角三角形；構(gòu)建安全圓子模塊，用于篩選出面積最大的直角三角形，繪制該三角形的外接圓，即動(dòng)態(tài)安全圓，以安全圓被路徑分為兩個(gè)部分中包含梯形面積較多部分的方向作為路徑的調(diào)整方向。其中，所述航姿點(diǎn)確定模塊包括以下功能子模塊：航姿點(diǎn)確定子模塊，用于沿修正后的初始路徑的延伸方向，計(jì)算相鄰兩個(gè)安全圓中初始路徑自前一個(gè)安全圓的出點(diǎn)至后一個(gè)安全圓的入點(diǎn)之間的路勁長(zhǎng)度，取該路徑的中點(diǎn)為航姿點(diǎn)；新航姿點(diǎn)確定子模塊，用于繪制初始路徑自前一個(gè)安全圓的出點(diǎn)至后一個(gè)安全圓的入點(diǎn)之間的直線線段，做該直線線段的垂直平分線，設(shè)置所述垂直平分線沿調(diào)整方向與安全圓相交的交點(diǎn)為新航姿點(diǎn)。本發(fā)明首次提出了“動(dòng)態(tài)安全圓”、“動(dòng)態(tài)航姿點(diǎn)”的概念，所述障礙物的安全圓模型是動(dòng)態(tài)的，隨著路徑的調(diào)整而變化，且安全圓模型的大小、位置不僅取決于障礙物的特征，也取決于路徑的特征。這種動(dòng)態(tài)的安全圓通常比靜態(tài)的安全圓的面積要小，可以有效地解決靜態(tài)安全圓造成的空間浪費(fèi)問(wèn)題。根據(jù)路徑與其相交的障礙矩形的特征繪制安全圓，使路徑避開(kāi)安全圓和避開(kāi)矩形障礙成為等價(jià)命題。根據(jù)安全圓和路徑的特征設(shè)置航姿點(diǎn)，根據(jù)安全圓及其所對(duì)應(yīng)障礙的特征指定新的航姿點(diǎn)。航姿點(diǎn)將路徑劃分為路徑段，使調(diào)整每個(gè)路徑段和調(diào)整整條路徑成為等價(jià)命題。在每一次調(diào)整路徑時(shí)，安全圓和航姿點(diǎn)均跟據(jù)路徑和障礙物的特征動(dòng)態(tài)變化，從而使路徑調(diào)整變得更加靈活。本發(fā)明提供了一種多形態(tài)障礙的無(wú)人機(jī)路徑規(guī)劃方法及系統(tǒng)，其通過(guò)路徑與其相交的障礙矩形的特征繪制安全圓，并指定初始路徑的調(diào)整方向；根據(jù)安全圓和路徑的特征設(shè)置航姿點(diǎn)，并根據(jù)安全圓及其所對(duì)應(yīng)障礙的特征指定新的航姿點(diǎn)。保持起點(diǎn)和終點(diǎn)的特征不變，依次調(diào)整每條路徑段曲率，使路徑段經(jīng)過(guò)新航姿點(diǎn)，釋放所有的安全圓和航姿點(diǎn)，從而得到一條符合曲率約束和安全約束的路徑。通過(guò)設(shè)定“動(dòng)態(tài)安全圓”、“動(dòng)態(tài)航姿點(diǎn)”，從而能夠規(guī)劃出效率更高、彎曲能量更小的路徑，也解決了靜態(tài)安全圓造成的空間浪費(fèi)和多邊形障礙的NP題之間的矛盾問(wèn)題。本發(fā)明應(yīng)用在低空無(wú)人機(jī)飛躍山地和城市樓宇的情況下，能夠具有較高的避障效率，能充分提高無(wú)人機(jī)飛行的效能。因此，可應(yīng)用于無(wú)人機(jī)路徑規(guī)劃的實(shí)際任務(wù)應(yīng)用。以上裝置實(shí)施例與方法實(shí)施例是一一對(duì)應(yīng)的，裝置實(shí)施例簡(jiǎn)略之處，參見(jiàn)方法實(shí)施例即可。本說(shuō)明書(shū)中各個(gè)實(shí)施例采用遞進(jìn)的方式描述，每個(gè)實(shí)施例重點(diǎn)說(shuō)明的都是與其他實(shí)施例的不同之處，各個(gè)實(shí)施例之間相同相似部分互相參見(jiàn)即可。專業(yè)人員還可以進(jìn)一步意識(shí)到，結(jié)合本文中所公開(kāi)的實(shí)施例描述的各示例的單元及算法步驟，能夠以電子硬件、計(jì)算機(jī)軟件或者二者的結(jié)合來(lái)實(shí)現(xiàn)，為了清楚地說(shuō)明硬件和軟件的可互換性，在上述說(shuō)明中已經(jīng)按照功能性一般性地描述了各示例的組成及步驟。這些功能究竟以硬件還是軟件方式來(lái)執(zhí)行，取決于技術(shù)方案的特定應(yīng)用和設(shè)計(jì)約束條件。專業(yè)技術(shù)人員可以對(duì)每個(gè)特定的應(yīng)用來(lái)使用不同方法來(lái)實(shí)現(xiàn)所描述的功能，但是這種實(shí)現(xiàn)不應(yīng)超過(guò)本發(fā)明的范圍。結(jié)合本文中所公開(kāi)的實(shí)施例描述的方法或算法的步驟可以直接用硬件、處理器執(zhí)行的軟件模塊，或者二者的結(jié)合來(lái)實(shí)施。軟件模塊可以置于隨機(jī)儲(chǔ)存器、內(nèi)存、只讀存儲(chǔ)器、電可編程ROM、電可檫除可編程ROM、寄存器、硬盤、可移動(dòng)磁盤、CD-ROM、或
技術(shù)領(lǐng)域：
內(nèi)所公知的任意其他形式的存儲(chǔ)介質(zhì)中。上面結(jié)合附圖對(duì)本發(fā)明的實(shí)施例進(jìn)行了描述，但是本發(fā)明并不局限于上述的具體實(shí)施方式，上述的具體實(shí)施方式僅僅是示意性的，而不是限制性的，本領(lǐng)域的普通技術(shù)人員在本發(fā)明的啟示下，在不脫離本發(fā)明宗旨和權(quán)利要求所保護(hù)的范圍情況下，還可做出很多形式，這些均屬于本發(fā)明的保護(hù)之內(nèi)。當(dāng)前第1頁(yè)1 2 3