[發明專利]一種可保證遠離障礙物的基于硬約束優化問題的機器人平滑軌跡規劃方法

有效

• 申請號: 202011254810.1
• 申請日: 2020-11-11
• 公開（公告）號: CN112327853B
• 公開（公告）日: 2021-09-28
• 發明（設計）人: 鄭培煒;成慧
• 申請（專利權）人: 中山大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 廣州粵高專利商標代理有限公司 44102
• 代理人: 王曉玲

本發明屬于移動機器人運動規劃領域，更具體地，涉及一種可保證遠離障礙物的基于硬約束優化問題的機器人平滑軌跡規劃方法。基于硬約束的二次規劃問題的方法，在最小化軌跡的三次導數的平方的積分來獲得平滑的軌跡的方法上加上了對膨脹區域侵入的懲罰項，作為優化問題方程的一部分，以此來得到一條平滑安全且盡量遠離障礙物的軌跡。即便在狹窄的通道場景也能保證有解，提高了原先方法的魯棒性。

技術領域

本發明屬于移動機器人運動規劃領域，更具體地，涉及一種可保證遠離障礙物的基于硬約束優化問題的機器人平滑軌跡規劃方法。

背景技術

移動機器人的運動規劃有廣泛的應用前景，如農藥噴灑、快遞分發、搜索救援等方面。機器人在執行任務時，往往需要在未知復雜場景中進行作業。外界擾動(風、人為干擾等)可能會讓機器人偏離其期望運動軌跡并與障礙物發生碰撞，破壞機器人的機械電氣部件。此外，期望運動軌跡越平滑，機器人底層控制器就越容易進行軌跡跟隨。因此，在規劃機器人的期望運動軌跡時，往往需要考慮規劃出一條平滑且遠離障礙物的軌跡，以獲得更好的軌跡跟隨效果并應對外界擾動。

移動機器人的軌跡規劃問題至少涉及到時間、空間等維度，通常都選擇將其構造成一個優化問題來解決。根據不同求解優化問題的方式可以將這些方法分類為：基于硬約束優化問題的方法、基于軟約束優化問題的方法與基于搜索的運動原語的方法。1.現有的基于硬約束的優化問題的方法規劃出來的軌跡不能保證遠離障礙物，由于硬約束優化問題通過構造成二次規劃問題來進行求解，而軌跡與障礙物的距離的約束很難表示成一個線性的約束條件。所以通常會考慮直接膨脹無人機來得到一條遠離障礙物的軌跡，但是在狹窄通道容易出現問題無解從而規劃失敗。2.現有基于軟約束的優化問題的方法規劃出來的軌跡不能保證軌跡能完成滿足所有約束。軟約束方法本質就是盡可能去滿足所有約束，對約束沒有硬性保證，其求解出來的軌跡可能會違背某些約束(如超過最大速度/加速度限制、軌跡過障礙物等)。需要人為提前找出合理的參數或規劃過程中在線迭代調整參數，容易造成求解時間不穩定。3.現有基于搜索的運動原語的方法需要構造搜索樹才能進行下一步的搜索求解工作，搜索樹的大小跟動作/ 狀態空間離散的顆粒度成正比，且離散顆粒度又會直接影響到求解出來的軌跡的平滑度等性質。想得到一條較優的軌跡就需要較小的離散顆粒度，也就需要較大的內存存儲搜索樹以及較長的搜索時間，這對計算機的計算力和內存要求較高。

中國專利CN110275526A，公開日為2019.09.24，公開了一種基于改進遺傳算法的移動機器人路徑規劃方法；在一定程度上提高了路徑規劃質量，但是其方法步驟復雜，計算繁瑣。

發明內容

本發明為克服上述現有技術中的至少一個缺陷，提供一種可保證遠離障礙物的基于硬約束優化問題的機器人平滑軌跡規劃方法，有效提高了魯棒性。

為解決上述技術問題，本發明采用的技術方案是：一種可保證遠離障礙物的基于硬約束優化問題的機器人平滑軌跡規劃方法，包括以下步驟：

S1.路徑搜索：使用基于圖搜索或基于采樣的路徑搜索算法找到一條當前位置到目標位置的路徑，該路徑沒有考慮動力學約束，其中路徑搜索算法包括 A*算法、RRT*算法；

在三維空間里進行規劃時，需要將連續空間離散成柵格地圖，在使用路徑搜索算法搜索出從起點到目標點的一條最短路徑。由于路徑搜索只考慮機器人起點與終點的位置，并不會考慮諸如當前速度、加速度以及無人機的動力學約束，這會導致找出的路徑違背機器人的動力學約束，如超過了機器人的最大加速度限制導致底層控制器無法進行軌跡跟隨。因此需要后續的軌跡優化工作；

S2.生成飛行走廊：在上一步找出的路徑上的每個離散點生成對應的飛行走廊，并刪除相同的飛行走廊，只留下不同的飛行走廊；所述的飛行走廊為：柵格地圖中包含該點的無障礙的區域所圍成的凸六面體，該凸六面體的邊只能與地圖坐標系的某一軸平行。