[發明專利]一種基于局部搜索算法的機械臂自主避障路徑生成方法

在審

• 申請號: 202210017496.8
• 申請日: 2022-01-07
• 公開（公告）號: CN114193457A
• 公開（公告）日: 2022-03-18
• 發明（設計）人: 馬建濤;韓峰濤;庹華;于文進;張航;何剛;韓建歡
• 申請（專利權）人: 珞石（北京）科技有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 北京世譽鑫誠專利代理有限公司 11368
• 代理人: 劉秀珍

1.一種基于局部搜索算法的機械臂自主避障路徑生成方法，其特征在于，包括如下步驟：

步驟S1，初始化障礙物相關信息，根據初始化后的信息生成障礙物的邊界框；

步驟S2，將當前關節角度一個鄰域內的關節點離散化；

步驟S3，從當前角度出發，依據預測的關節角度變化律獲得下一步的搜索路徑點；判斷從當前關節角度出發能否獲得可行路徑點，如果能夠獲得可行路徑點，則從可行路徑點中篩選最優路徑點，進而判斷是否到達終點；如果不能獲得可行路徑點則進行路徑回溯，返回前一個路徑點重新進入S2向前搜索；

步驟S4，如果判斷到達終點，則將所有最優路徑點平滑連接起來，生成最終的平滑路徑，如果判斷沒有到達終點，則前進一步返回步驟S2，搜索下一個路徑點。

2.如權利要求1所述的基于局部搜索算法的機械臂自主避障路徑生成方法，其特征在于，在所述步驟S1，所述初始化障礙物相關信息，包括：初始化路徑起始點、終點、角度極限、障礙物邊界信息。

3.如權利要求1所述的基于局部搜索算法的機械臂自主避障路徑生成方法，其特征在于，所述生成障礙物的邊界框，包括：選擇由圓形和矩形元素描述障礙物的邊界框；其中圓形由球心O_i(xo_i,yo_i,zo_i)和半徑ro_i組成，矩形由長方體的各個頂點(xc_i,yc_i,zc_i)表示，障礙物的邊界框由上述元素組成的圓柱形，球形和多面體進行描述。

4.如權利要求1所述的基于局部搜索算法的機械臂自主避障路徑生成方法，其特征在于，在所述步驟S2中，

首先，每個關節角度滿足約束：

其中，q_i,分別表示第i個關節的關節角度，關節角度上限和下限；m表示關節總個數；

式子(1)表示每個關節角度都必須處于其限定范圍內，是機械臂處于任何姿態，均需要遵守的固有約束；

依據機械臂的具體狀態，對其關節角做出了進一步的約束：

其中，q_i,c,分別表示第i個關節第c步關節角的角度，角度上限和下限，c表示從起始開始的步數序號；

r_i,c分別表示第i個關節初始關節角度上限，第c-1步的關節角度上限，關節角極限變化范圍；

分別表示第i個關節初始關節角度下限，第c-1步的關節角度下限；

確定離散關節點時的離散步長：

其中，n_s表示離散關節點時劃分的步數，dq_i,c表示第i個關節角第c步的離散步長。

5.如權利要求1所述的基于局部搜索算法的機械臂自主避障路徑生成方法，其特征在于，在所述步驟S3中，

步驟S31，每次搜索只選擇距離當前點位一個離散步長的離散點位作為目標路徑點，每次搜索時每個關節角度可能的結果為：

其中，表示第i個關節角第c步可能的搜索結果，q_i,c-1表示第i個關節角第c-1步的最優路徑點；

步驟S32，對于上式中可能的搜索結果，確定搜索規則，尋找最優路徑點。

6.如權利要求5所述的基于局部搜索算法的機械臂自主避障路徑生成方法，其特征在于，所述確定搜索規則，包括：

式子(6)可以進一步地表示為：

其中，sn_i,c為符號標志位，其可能的值為-1，0，1，其中i表示關節角序號，c表示步數序號；

此時，每次路徑點的搜索過程轉化為sn_i,c的遍歷過程，采用基于預測關節角度變化律的方法確定sn_i,c的遍歷準則，使得能夠優先遍歷到最優路徑點，加快搜索速度。