[發明專利]一種基于動態窗口和冗余節點過濾的路徑優化方法

有效

• 申請號: 202110678202.1
• 申請日: 2021-06-18
• 公開（公告）號: CN113341991B
• 公開（公告）日: 2022-08-09
• 發明（設計）人: 宋永端;張聰毅;譚力琿;賴俊峰;王賽宇;張艷鍇
• 申請（專利權）人: 重慶大學;星際（重慶）智能裝備技術研究院有限公司;迪比（重慶）智能科技研究院有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 重慶晟軒知識產權代理事務所(普通合伙) 50238
• 代理人: 王海鳳

1.一種基于動態窗口和冗余節點過濾的路徑優化方法，其特征在于，包括以下步驟：

S100：利用已有的柵格地圖數據集，確定運動的起始位置和目標位置的坐標信息，在柵格地圖中標注目標節點和障礙物節點；

S200：利用A^*算法規劃出一條全局路徑，包括以下步驟：

S210：構建并且初始化兩個空表OpenList和CloseList，將起始位置設置為當前節點并存入OpenList列表；如果當前節點不是目標點，那么調用擴充節點函數選出與當前節點相鄰且可擴展的所有節點，并且將擴展的所有節點信息都存入OpenList列表；

S220：調用插入節點函數，遍歷可以擴展的所有節點，計算出這些節點對應的代價函數F，代價函數表示為：

F(n)＝G(n)+H(n)

其中，n表示當前節點；F(n)是當前節點n的代價函數；G(n)是移動機器人從初始節點到節點n的實際代價值；H(n)是從當前節點n到達目標點的代價值，選取這兩點之間的歐式距離代表H(n)，H(n)函數表示為：

其中，(x_g,y_g)代表目標節點在柵格地圖中的坐標，(x_n,y_n)代表當前節點在柵格地圖中的坐標；

S230：選出代價函數F(n)最小值所對應的節點，將其從OpenList列表中刪除，存入CloseList列表之中，同時將該節點設為當前節點連接至上一節點，重復S220，直至當前節點為目標點，導出該全局路徑；

S300：利用A^*算法規劃出的全局路徑進行全局優化，過濾掉冗余節點，包括以下步驟：

S310：將S200得到的CloseList列表中的路徑節點集記為P{P_i,1≤j≤n}，其中P₁為路徑的起點，P_n為路徑的終點，創建初始值只有P₁和P_n的關鍵點集合U{P₁,P_n}用于存放路徑優化之后的關鍵節點；

S320：對于節點集P{P_i,1≤j≤n}，令m＝2,3,4…n，由P₁開始依次連接P₂,P₃,…,P_m，判斷直線P₁P_m之間是否存在阻礙物節點，如果存在，則節點P_m-1為路徑的必經節點；如果直線P₁P_m沒有穿過障礙物節點，則判定P_m-1為冗余節點；

將所有必經節點都添加至集合U中，關鍵點選取完成之后，集合U{P₁,P_m-1,...,P_m+k,P_n}(2≤m≤n,1≤k≤n-m)包含了所有關鍵節點，假設U中的節點數目為r，即U{P₁,P₂,...,P_r},(1≤r≤n)；

S330：依次連接集合U中的所有節點，完成對路徑的全局優化；

S400：結合動態窗口法對S300優化后的全局路徑進行分段局部優化，得到最終的全局路徑，具體步驟如下：

S410：針對集合U，從起點P₁開始，依次將除開終點P_r之外的所有節點作為局部路徑規劃的起點，記作{S₁,S₂,...,S_r-1}；同時將集合U中的第二個節點P₂開始，依次將{P₂,P₃,...,P_r}記作局部路徑規劃的終點{D₁,D₂,...,D_r-1}，則可以將全局路徑分為S₁D₁,S₂D₂,...,S_r-1D_r-1共r-1段，記S₁在柵格地圖中的坐標值為D₁的坐標值為以此類推；

S420：初始化移動機器人的狀態參數集L(l)，l(x,y,yaw,v,w)記錄機器人運動在規劃路徑中的狀態參數，包括位置、航向角、線速度、角速度，設定機器人的初始線速度為v(m/s)，初始角速度為w(rad/s)和初始航向角yaw(rad)；

S430：計算S_iD_i的傾斜角度，計算公式如下：

其中，分別是D_i的橫縱坐標，分別是S_i的橫縱坐標；

將S_iD_i段路徑的傾斜角度轉換為弧度值作為移動機器人的初始航向角，斜率角度轉化為弧度值的公式為：

yaw_i＝α×180°÷π

得到在S_iD_i段路徑中機器人的所有狀態參數l_i(x_i,y_i,yaw_i,v_i,w_i),1≤i≤r，其中，x_i,y_i是節點S_i的橫縱坐標，v_i是S_iD_i段路徑中讀取的上一段路徑的終點處的線速度，w_i是S_iD_i段路徑中讀取的上一段路徑的終點處的角速度；

S440：按照S430中計算S_i+1D_i+1段路徑的傾斜角度α_i+1，同時將機器人上一個狀態參數l_i中的yaw_i轉換為角度值β，弧度值轉化為角度值的公式如下：

β＝yaw_i×π÷180°

比較α_i+1和β，如果|α_i+1-β|＜60°，則機器人保持最新的狀態l_i+1(x_i+1,y_i+1,yaw_i+1,v_i+1,w_i+1)，如果|α_i+1-β|≥60°，為避免移動機器人因為繞路轉彎造成了冗余路段和耗費不必要運動時間，則令機器人航向角

S450：重復步驟S440，直至移動機器人到達第r-1路段的終點D_r-1，得到記錄移動機器人所有的狀態參數集L(l₁,l₂,...,l_i,l_i+1,...,l_r)，完成局部路徑的優化。