本發(fā)明屬于路徑規(guī)劃，尤其涉及一種在賦成本地圖上尋找低成本、多約束光滑路徑方法。
背景技術：
1、隨著科技的飛速發(fā)展，機器人技術已經(jīng)廣泛應用于工業(yè)自動化、服務行業(yè)、醫(yī)療輔助等多個領域。機器人導航作為實現(xiàn)機器人自主移動和任務執(zhí)行的基礎技術，一直是機器人領域的核心內(nèi)容之一。路徑規(guī)劃是機器人導航中至關重要的一環(huán)，它涉及到如何在復雜環(huán)境中為機器人設計一條從起點到終點的合理有效的行進路線。合理有效的路徑規(guī)劃不僅能夠提高移動效率，還能確保安全性和節(jié)省能源消耗。
2、當前，應對賦成本地圖路徑規(guī)劃問題的研究不廣泛、不深入。常見的處理方式多集中于環(huán)境建模以及地圖預處理。如將實際場景轉化為柵格地圖，每個單元格包含障礙物信息、地形特征等因素。對于特定應用，還可能需要對某些單元格賦予不同的成本值。經(jīng)典的路徑規(guī)劃算法，如迪杰斯特拉算法和a?star搜索算法，雖然在許多情況下都非常有效，但它們通常假設路徑是由直線段組成的，并且不考慮路徑的曲率問題。一些依賴于樣條生成光滑路徑的方法，如貝塞爾曲線等，并不一定可以和機器人本身的運動特性相適應，在現(xiàn)實中，會出現(xiàn)偏離路線造成誤差等情況。啟發(fā)式算法利用模擬退火法、遺傳算法等優(yōu)化技術，對初步搜索得到的路徑進行局部調(diào)整，用以滿足曲率連續(xù)等約束，但算法依賴參數(shù)與初始解的選擇，同時受限于概率參數(shù)，收斂速度慢，可解釋性不強。
3、這些方法的變體改進，多數(shù)只針對某一特定約束進行優(yōu)化且依賴原始條件的假設，不能考慮實際條件下的應用。算法改進措施會提高計算復雜度，導致在實時性要求較高的應用場景中，難以滿足實時更新和快速響應的需求。
4、現(xiàn)有方法未考慮由于地形、材質(zhì)等導致機器人在不同區(qū)域經(jīng)過同樣長度距離所承受代價不同的情況。
5、現(xiàn)有方法難以同時兼顧實際任務需求與自身的運動特性，如實際任務中通常需要經(jīng)過一系列途徑點來滿足特定需求。生成的路線在滿足光滑的同時需要保證曲線間線段的長度來保證精準控制；控制曲線的最大曲率降低器械的磨損。
6、現(xiàn)有方法的優(yōu)化過程加大計算復雜度，時間成本高，難以快速響應。一些算法能夠生成滿足基本約束條件的路徑，但在保持路徑平滑性和連續(xù)性依舊表現(xiàn)不佳。
技術實現(xiàn)思路
1、為解決以上技術問題，本發(fā)明提出一種在賦成本地圖上尋找低成本、多約束光滑路徑方法，本發(fā)明的技術方案如下：
2、構造數(shù)學模型，用于對連接起點和終點的路徑進行平滑化得到平滑路徑，平滑路徑由線段、圓弧和緩和曲線構成；
3、在數(shù)學模型基礎上提出來一個兩階段算法，第一階段在地圖上進行取點采樣，構造圖結構，使用迪杰斯特拉算法得到途徑點的連接次序；第二階段在第一階段的基礎上，逐個處理途徑點，生成滿足條件的控制點集p，最后計算得到圓弧半徑集r，在集合p、r下，路徑被唯一確定。
4、本發(fā)明具有以下有益效果：
5、1.本發(fā)明的方法解決賦成本地圖的路徑規(guī)劃問題，該方法能夠在成本地圖上生成經(jīng)濟可行的路線；同時在測試中運行時間短、效果更好。
6、2.本發(fā)明的方法克服了傳統(tǒng)光滑路徑算法由多條光滑路徑疊加組成的缺點，通過緩和曲線實現(xiàn)線段和圓弧之間的無縫過渡，保證了整體路徑曲率的連續(xù)性；同時考慮了真實任務途徑點和避障要求。
7、3.本發(fā)明提出的全新數(shù)學模型，考慮了機器人的動力學因素和系統(tǒng)控制的精度要求，引入了曲線之間線段的最小長度和曲線的最大曲率等約束條件。
1.一種在賦成本地圖上尋找低成本、多約束光滑路徑方法，其特征在于，所述方法具體包括：
2.根據(jù)權利要求1所述的一種在賦成本地圖上尋找低成本、多約束光滑路徑方法，其特征在于，數(shù)學模型用以表述并解決如下問題：根據(jù)給定的賦成本地圖，找到一條連接起點與終點的平滑路徑，其中該平滑路徑應滿足以下優(yōu)化準則與約束：（1）經(jīng)過所有給定的途徑點；（2）避開給定的障礙物；（3）路徑滿足曲率連續(xù)的要求；（4）路徑有最大曲率約束，即對圓弧有最小半徑限制；（5）曲線間的線段長度大于該類型線段最小長度；（6）在上述標準和約束下，降低路徑的成本。
3.根據(jù)權利要求2所述的一種在賦成本地圖上尋找低成本、多約束光滑路徑方法，其特征在于，數(shù)學模型的目標函數(shù)為：
4.根據(jù)權利要求3所述的一種在賦成本地圖上尋找低成本、多約束光滑路徑方法，其特征在于，整個平滑路徑由線段、圓弧和緩和曲線構成，其中，一條緩和曲線可以通過以下參數(shù)被唯一確定：
5.根據(jù)權利要求4所述的一種在賦成本地圖上尋找低成本、多約束光滑路徑方法，其特征在于，緩和曲線中，線段oq與長虛線圓弧相切于點q，圓的半徑等于r，在線段oq和長虛線圓弧中插入一條長度為l的緩和曲線，即曲線，將線段與圓弧切點q外移，將圓弧內(nèi)移，通過移動將圓弧和線段切點轉為圓弧和緩和曲線、線段和緩和曲線的切點，假設緩和曲線起點位于原點，初始方向角為0，容易得到，表示直線延長線夾角補角，由數(shù)學性質(zhì)知其也等于圓心角，與前文模型約束中的含義一致，假設，由緩和曲線性質(zhì)，得到和的值是l和r的函數(shù)，將兩者的數(shù)值分別記為和，分別表示圓弧內(nèi)移值和切點外移值，此處v與前文模型約束中的含義一致，表示外移值，這里，當緩和曲線時，由曲線性質(zhì)知曲線上任一點曲率半徑與該點曲線長度乘積為常數(shù)，記，那么當，得到：
6.根據(jù)權利要求5所述的一種在賦成本地圖上尋找低成本、多約束光滑路徑方法，其特征在于，光滑化具體為：在線段和圓弧中插入緩和曲線，在兩條線段中間首先插入半徑為r的圓弧，線段交點為p，切點分別為和，此時圓弧圓心角為，表示直線延長線夾角補角，假設此時圓心為，將圓弧沿著方向移動個單位，同時圓心角從兩端減小，這樣得到處理后的圓弧；將切點和分別沿著和移動個單位得到點和作為緩和曲線的起點，以插入緩和曲線的方式完成線段與圓弧的光滑過渡。
7.根據(jù)權利要求1所述的一種在賦成本地圖上尋找低成本、多約束光滑路徑方法，其特征在于，兩階段算法輸入為：包含邊界、障礙物的地圖信息、約束數(shù)據(jù)以及途徑點的位置坐標，約束數(shù)據(jù)包括z、、算法參數(shù)取點間最小距離、增量代價閾值，其中，默認，為無障礙區(qū)域面積，增量代價閾值默認取1。
8.根據(jù)權利要求7所述的一種在賦成本地圖上尋找低成本、多約束光滑路徑方法，其特征在于，在第一階段算法包括：
9.根據(jù)權利要求8所述的一種在賦成本地圖上尋找低成本、多約束光滑路徑方法，其特征在于，步驟1包括：首先在所有點中尋找任意兩個點之間的距離最小的點對，將此兩點連接起來作為初始基線；其次找第三點從而創(chuàng)建第一個三角形，以基線的兩個端點作為向量的起點，依次將其余的未構網(wǎng)的點作為向量的終點，計算兩個向量間夾角的余弦值，其中和分別為基線的兩個端點，為其余未構網(wǎng)的點，尋找余弦值最小的點構造首個三角形，以首三角形的三條邊為基線，尋找與基線構成三角形的可擴展點，可拓展點在三角形外部，在所有可擴展點中尋找余弦值最小的點且構成的邊不能為重復擴展邊，重復找點構邊的步驟直到所有基線處理完畢。
10.根據(jù)權利要求9所述的一種在賦成本地圖上尋找低成本、多約束光滑路徑方法，其特征在于，步驟2包括：兩途徑點間成本由在圖上使用迪杰斯特拉算法所確定的最短距離確定，該算法在圖結構中，對于設置好兩點找到一條最短路徑，首先從一點開始，將該點的距離設置為0，相鄰節(jié)點設置為邊的權重，其他節(jié)點的距離設置為∞，表示還未找到到達該節(jié)點的路徑，然后從未訪問的節(jié)點中，選擇距離起始點最近的節(jié)點，然后將其標記為已訪問，對選中節(jié)點的所有鄰居節(jié)點，檢查從起始點經(jīng)過當前節(jié)點到達鄰居節(jié)點的路徑是否比當前已知最短路徑更短，如果是，則更新鄰居節(jié)點的最短路徑值，重復上述步驟，直到所有節(jié)點都被訪問，或剩余未訪問節(jié)點與源節(jié)點不連通，算法完成后，兩點間的最短路徑被確定。
11.根據(jù)權利要求7所述的一種在賦成本地圖上尋找低成本、多約束光滑路徑方法，其特征在于，算法第二階段包括：將對第一階段中得到的連接次序逐個處理；首先根據(jù)途徑點的趨勢，將該點以線段的形式進行平滑過渡，確定次序后，存在三個點的連接次序為、，線段的方向為，趨勢線表示按照連接順序生成趨勢，途徑點通過線段完成過渡，即在趨勢線方向上生成兩個信息節(jié)點，其中趨勢線的方向由障礙物的位置和線段的長度調(diào)整控制，當原趨勢線方向?qū)е峦緩秸系K物時會對方向進行調(diào)整，其調(diào)整方法為以點為中心，擺動趨勢線直至過渡線段不經(jīng)過障礙，擺動方式為：記，方向角，時，0，擺動調(diào)整角度為，線段長度參數(shù)由約束中最小線段長度與最大曲率決定，設置為2，此時將同步加入圖結構中。
12.根據(jù)權利要求11所述的一種在賦成本地圖上尋找低成本、多約束光滑路徑方法，其特征在于，將同步加入圖結構中具體方法為：以為例，（1）若落在點集v中一點上，將替換為，其余不變；（2）若落在邊集e中權重為的邊上，則圖結構中加入點，去掉的邊，產(chǎn)生和的兩條邊，兩條邊的權重分別為：、，為兩點間的距離；（3）若落到以為頂點的三角形中，則在圖結構中加入點，產(chǎn)生三條新邊、和，權重和構造初始圖結構方式相同，為其在地圖成本函數(shù)的線積分，即分別為和。
13.根據(jù)權利要求12所述的一種在賦成本地圖上尋找低成本、多約束光滑路徑方法，其特征在于，第二階段中，尋找信標點和關鍵點的具體方法為：若起點為s，終點為e，途徑點按連接次序為、，經(jīng)過上一階段的過渡后，此時途徑點以線段方式完成銜接，目前節(jié)點按連接次序為s、、、、e，其中、、為上步中根據(jù)趨勢線生成的信息節(jié)點，所有信息節(jié)點數(shù)量為，2表示起點和終點共兩個，表示途徑點數(shù)量，每個途徑點都會產(chǎn)生兩個信息節(jié)點，故總數(shù)必為偶數(shù)個，在此一次兩兩一組進行處理，即分組為：、，以為例，通過迪杰斯特拉算法生成圖上的最短路徑localpath，由分組初始點向分組終點逐點連線檢驗，當以線段方式進行連接后若代價顯著增加，代價計算方式與圖結構相同，為在地圖成本函數(shù)上的線積分，若增量超過原代價的設定閾值倍，則將該檢查節(jié)點的前置節(jié)點同時設置為信標點與新起點，繼續(xù)逐點檢驗直到結束，若連線通過障礙區(qū)域，該線積分為無窮大，在得到所有信標點后，對所有信標點進行驗算是否滿足光滑化的生成條件以及生成后是否經(jīng)過禁止區(qū)域，若不滿足則會進行調(diào)整，即沿障礙物方向進行后移至光滑化后不經(jīng)過障礙物為止，生成最終關鍵點。
14.根據(jù)權利要求13所述的一種在賦成本地圖上尋找低成本、多約束光滑路徑方法，其特征在于，通過趨勢線延申和逐個尋找關鍵點的方式，得到模型的關鍵點集合，而后通過點集生成光滑路徑，各段插入半徑大小由約束以及地圖信息所確定，這里使用粒子群pso算法確定最佳半徑，pso算法初始化為一組隨機解，然后通過迭代找到最優(yōu)解。在每一次的迭代中，通過參考每個半徑的個體最優(yōu)pbest和所有半徑的全局最優(yōu)gbest來更新自己，在找到這兩個最優(yōu)值后，通過下面的公式來更新自己的速度和位置：