[發(fā)明專利]一種帶有底盤誤差的機械臂路徑規(guī)劃方法

有效

• 申請?zhí)枺?/td>: 202010191628.X
• 申請日: 2020-03-18
• 公開（公告）號: CN111251306B
• 公開（公告）日: 2022-11-29
• 發(fā)明（設計）人: 謝遠龍;王書亭;蔣立泉;李耀仲;景偉;楊梓桐;蔣立宇;章小龍;劉超;孫浩東
• 申請（專利權(quán)）人: 廣東省智能機器人研究院
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 廣州粵高專利商標代理有限公司 44102
• 代理人: 羅曉林;楊桂洋

1.一種帶有底盤誤差的機械臂路徑規(guī)劃方法，包括以下步驟：

獲取移動底盤在運動空間中實際位置與目標位置的誤差值，并依據(jù)誤差值，重構(gòu)構(gòu)型空間，并對起始位置和目標位置進行修正；

初始化起始位置和目標位置，創(chuàng)建以修正后的起始位置為根節(jié)點的隨機樹，隨機樹包括父節(jié)點和子節(jié)點；

調(diào)用具備偏向概率采樣的隨機函數(shù)，在構(gòu)形空間中產(chǎn)生一個隨機點x_rand；

遍歷隨機樹的所有子節(jié)點，搜索與隨機點距離最近的節(jié)點x_near；

生成新節(jié)點x_new，調(diào)用回歸過濾機制，將不符合要求的節(jié)點刪除，不擴展到隨機樹中，將符合要求的節(jié)點擴展到隨機樹中；

調(diào)用隨機點邊界擴展機制，更新邊界節(jié)點的狀態(tài)信息，確定邊界節(jié)點的可擴展性；

確定機械臂在工作空間中的位姿，對新節(jié)點進行碰撞檢測；

擴展并更新隨機樹；

檢查是否滿足終止條件，獲取機械臂規(guī)劃路徑；

所述回歸過濾機制具體為：

對于新節(jié)點x_new，與該新節(jié)點x_new直接相連的節(jié)點為x_near，距離為步長ε，當新節(jié)點x_new與隨機樹中所有節(jié)點的最短距離小于步長ε時，即與新節(jié)點x_new距離最近的節(jié)點不為x_near，則當前x_new節(jié)點是回歸的，定義為回歸節(jié)點，將該回歸節(jié)點直接過濾，不擴展到隨機樹中,當新節(jié)點與隨機樹中所有節(jié)點的最短距離等于或大于步長ε時，該新節(jié)點符合要求，將該新節(jié)點擴展到隨機樹中；

所述隨機點邊界擴展機制具體為：

邊界節(jié)點是指其擴展的節(jié)點有一定概率與障礙物發(fā)生碰撞的節(jié)點；

為每一個新擴展的節(jié)點定義一個狀態(tài)值δ，δ值隨著隨機樹的擴展不斷更迭，新擴展的節(jié)點狀態(tài)值δ初始為0；

定義當前有m條邊與邊界節(jié)點連接，若擴展的子節(jié)點與障礙物邊界碰撞，若邊界節(jié)點的狀態(tài)值為0則將δ值更新為1/2，否則將δ值增加1/(m+1)；若擴展的子節(jié)點與障礙物邊界不發(fā)生碰撞，則該節(jié)點的狀態(tài)值δ將減少1/(m+1)，若減少后的狀態(tài)值δ小于0，則δ值更新為0；

定義邊界節(jié)點在擴展過程中與邊界發(fā)生碰撞的概率閾值為CP，如果狀態(tài)值δ＞CP，則該邊界節(jié)點將不再擴展任何子節(jié)點；如果狀態(tài)值δ≤CP，則將該節(jié)點擴展到隨機樹中；

所述終止條件具體為：

將新節(jié)點擴展到隨機樹后，判斷該新節(jié)點與目標點間的距離，若距離小于步長ε，且新節(jié)點與目標點的連線不穿過障礙物，則終止隨機樹的擴展過程；否則繼續(xù)擴展隨機樹。

2.根據(jù)權(quán)利要求1所述的帶有底盤誤差的機械臂路徑規(guī)劃方法，其特征在于，所述依據(jù)誤差值，重構(gòu)構(gòu)型空間，具體為：

e(x,y,θ)＝E(x,y,θ)

其中E(*)為環(huán)境匹配函數(shù)，e(*)為誤差函數(shù)，

C_e(x,y,θ)←C(x,y,θ)∩C(e(x,y,θ))

p_e(x,y,θ)←p(x,y,θ)∩Ce(x,y,θ)

其中C為構(gòu)型空間，Ce為考慮誤差之后的空間重構(gòu)，p為示教點位置，p_e為p修正后在空間中的映射位置。

3.根據(jù)權(quán)利要求2所述的帶有底盤誤差的機械臂路徑規(guī)劃方法，其特征在于，所述創(chuàng)建的隨機樹具體為：

隨機樹以起始點為根節(jié)點，以指定步長ε為父節(jié)點與子節(jié)點間的固定距離，父節(jié)點與子節(jié)點連線方向任意，父節(jié)點可擴展任意數(shù)量的子節(jié)點，隨機樹的所有節(jié)點都位于構(gòu)形空間的自由空間中。

4.根據(jù)權(quán)利要求3所述的帶有底盤誤差的機械臂路徑規(guī)劃方法，其特征在于所述調(diào)用具備偏向概率采樣的隨機函數(shù)具體為：

P＝rand(1)

其中，P表示0～1之間生成的隨機數(shù)，表示偏向概率，rand(x)表示生成在[0，x]區(qū)間的隨機樹，x_max、y_max分別表示構(gòu)形空間中x、y方向的最大值，(x_goal，y_goal)表示目標點。