[發明專利]一種非均勻采樣FMT*的移動機器人路徑規劃方法

有效

• 申請號: 202011251462.2
• 申請日: 2020-11-11
• 公開（公告）號: CN112286202B
• 公開（公告）日: 2021-07-27
• 發明（設計）人: 陳彥杰;吳錚;林依凡;黃益斌;何炳蔚;林立雄
• 申請（專利權）人: 福州大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 福州元創專利商標代理有限公司 35100
• 代理人: 郭東亮;蔡學俊

1.一種非均勻采樣FMT*的移動機器人路徑規劃方法，其特征在于：包括如下步驟：

步驟S1、地圖初始化，其方法為：將環境中所有的已知障礙物用均勻分布的點集合進行離散化表示，并假設障礙物數量為k，組成集合{C_i}，即

步驟S2、利用增量構造方法建立廣義Voronoi圖，即GVG，由廣義Voronoi邊和頂點組成；

步驟S3、尋找初始化路徑，其方法為：設置機器人的初始位置、目標位置，在路線圖GVG中尋找初始路徑，并離散化路徑，實現最優路徑與初始路徑同倫；

步驟S4、安全隧道建立，其方法為通過GVG建立過程得到距離障礙物的距離d，將其作為半徑R，并以圓心在路徑離散點，半徑R的連續圓形區域，建立覆蓋初始路徑的安全隧道；

步驟S5、非均勻采樣，其方法為：設置總采樣數N，在安全隧道內進行隨機采樣，得到采樣集合V_unvisited；

步驟S6、構造采樣集合V_unvisited、探索集合V_open、已探索集合V_closed，＝將機器人路徑的目標點x_goal放入V_unvisited，將初始點x_start放入探索集合V_open，代價初始化為0，記作c(x_start)＝0，已探索集合

步驟S7、尋找探索集合V_open中代價最小點作為節點Z，以Z為中心，r_n為半徑范圍內搜索采樣集合V_unvisited的節點，組成集合X_near，以函數Near(V_unvisited,Z,r_n)表示；

步驟S8、對于每個點x∈X_near，以x為中心，半徑r_n探索V_open中點，組成集合Y_near，以函數Near(V_open,x,r_n)表示；

步驟S9、計算所有點y∈Y_near到點x的代價，選取代價最小點y_min，表示為使樹中各個節點到子節點代價最小，從而實現算法的漸進最優；

步驟S10、判斷y_min與x是否有障礙物，若無則將x從V_unvisited放入V_open中，其代價為c(x)＝c(y_min)+Cost(y_min,x)，y_min為x的父節點；

步驟S11、探索完X_near后，將Z從V_open放入V_closed；

步驟S12、判斷Z是否是目標點，若是則返回路徑解，若不是則返回步驟S7繼續；

步驟S13、機器人接收到算法規劃的可行路徑解，并把可行路徑解中的路徑轉化為所需線速度和角速度，控制機器人移動到目標位置，路徑規劃結束。

2.根據權利要求1所述的一種非均勻采樣FMT*的移動機器人路徑規劃方法，其特征在于：所述步驟S2具體實現方式為：距離d_i(x)表示點x到第i個障礙物C_i的距離，定義為：定義方向為點x指向c₀，由梯度表示，其中c₀表示x到C_i的距離最近點；

若點x周圍有許多障礙物，則定義x到環境的距離為所定義的距離均為可視距離且不考慮被遮擋障礙物；

所述GVG的基本組成部分是每個點到障礙物C_i和C_j的等距集合，將集合定義為等距邊：

由公式一獲得的所有等距邊的交匯點為廣義Voronoi頂點；進一步地，GVG由廣義Voronoi邊和頂點組成；每個頂點和Voronoi邊都保留距離d，GVG形成的路線圖分布在所有的障礙物周圍，并且保證路線圖中的任意兩點之間存在一條有GVG邊緣和交匯點形成的路徑將兩點連接。