[發明專利]一種基于Smooth?RRT算法的水下機器人路徑優化方法

在審

• 申請號: 201711078252.6
• 申請日: 2017-11-06
• 公開（公告）號: CN107883961A
• 公開（公告）日: 2018-04-06
• 發明（設計）人: 于立君;魏智紅;王輝;董澤全;高菁;張一;丁瑩;王正安;胡羽坤;張雪
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G05D1/02
• 代理公司: 暫無信息
• 代理人: 暫無信息

技術領域

本發明涉及優化領域，具體涉及一種基于Smooth-RRT算法的水下機器人路徑優化方法。

背景技術

機器人路徑規劃可分為全局路徑規劃和局部路徑規劃，當對環境未知或部分未知時采用 局部路徑規劃。由于復雜的海洋環境僅利用海圖的全局路徑規劃，不能對未知海底地形和船 只等做出及時有效的反應。路徑規劃是機器人研究的一個重要方向，是指依據某一性能指標， 機器人在所處的環境中搜索到一條可避開障礙從初始狀態到目標狀態的最優或次優路徑。傳 統的路徑規劃算法有柵格法、人工勢場法、多邊形擬合法、遺傳算法等，但這些方法都需要 對確定空間內的障礙物進行建模，不適合解決復雜環境中的路徑規劃。快速擴展樹(RRT)算法 在狀態空間中對采樣點進行碰撞檢測，不需對空間進行建模能夠快速有效地搜索高維空間。

RRT算法在完成對未知環境探索和規劃的同時，存在以下問題：在采樣空間內隨機采樣， 因此同一個任務被多次規劃，每次產生的路徑都不同，影響任務完成的效率，并對機器人機 體造成損害。許多研究者提出了相關的改進算法如：以預測模型為基礎動態RRT算法，減少 了規劃時間；KD樹概念，提高了搜索效率。然而這些改進方法沒有考慮水下機器人的運動 角度限制、沒有對產生的路徑進行平滑處理，極易產生階梯形和鋸齒形路段，對機器人機體 造成損害。為此本發明提出一種基于Smooth-RRT算法的路徑優化方法，加入角度因子的同 時利用貪心算法對規劃路徑進行處理，使產生的路徑更加光滑順暢、趨于實用，從而得到水 下機器人的最優規劃路徑。

發明內容

本發明的目的在于提供一種基于Smooth-RRT算法的水下機器人路徑優化方法，本方法 實現了改進RRT算法對路徑的優化利用，使其滿足水下機器人操作性能所需，同時考慮對改 進RRT生成路徑進行平滑處理。這樣不僅產生的路徑更加光滑順暢，同時大幅度縮減了水下 機器人的操舵次數。

本發明的目的是這樣實現的：

本發明的目的在于提供一種基于Smooth-RRT算法的水下機器人路徑優化方法，其特征 在于，包含以下步驟：

步驟一：初始化T₁＝x_init，x_init為初始位置；

步驟二：判斷|x_init-x_goal|≤ρ，若是，轉到步驟十一，不是，則轉到步驟三；x_goal為目標 位置；

步驟三：生成隨機點x_rand；

步驟四：給定一個0到1的偏置變量Bias，生成一個0到1的隨機數rand；

步驟五：如果rand<Bias,則x_rand＝x_goal；否則，x_rand不變；

步驟六：找出x_near使D(x_new,x_rand)≤D(x,x_rand)；x_near為距離x_rand最近的點；x_new為擴展的 新的探索節點；

步驟七：根據當前位置的父節點x_near-1、當前位置x_near和x_rand得出方向改變量中的θ；若 θ＜θ_max則執行x_near+ρ·θ,，否則執行(x_rand-x_near)·θ_max；ρ為固定步長；θ_max為最大轉角用 于約束條件限制探索點的范圍；θ為方向改變量；

步驟八：在x_new和x_rand的連線上求x_new使D(x_new,x_near)＝ρ,并且x_new∈C_free，若存在這樣的 x_new，轉到步驟九，若不存在，轉到步驟三；

步驟九：在擴展樹上增加節點，T_k+1＝T_k+x_new；

步驟十：判斷|x_new-x_goal|≤ρ,若是，轉到步驟十一，不是，則轉到步驟三；

步驟十一：利用貪心算法對路徑進行平滑處理；