本發(fā)明屬于機(jī)械臂路徑規(guī)劃，涉及六自由度機(jī)械臂在有障礙物環(huán)境下的路徑規(guī)劃算法，具體涉及一種融合天牛須算法的rrt-connect機(jī)械臂路徑規(guī)劃方法。

背景技術(shù)：

1、制造業(yè)中的工業(yè)機(jī)器人將成為智能制造的主力軍。它可以代替人或者和人一起協(xié)作完成工作，由于應(yīng)用場景變得越多、越復(fù)雜，對機(jī)械臂的安全性和智能性提出了更高的要求。路徑規(guī)劃是機(jī)械臂控制的重要內(nèi)容之一，即為從起始點(diǎn)到目標(biāo)點(diǎn)選擇一條路徑，使得機(jī)器人能夠安全、快速到達(dá)目標(biāo)點(diǎn)，保證機(jī)器人不與障礙物發(fā)生碰撞。

2、rrt算法能夠有效地對高維空間進(jìn)行搜索，并且無需對構(gòu)型空間障礙物進(jìn)行準(zhǔn)確的描述，因此rrt算法被廣泛應(yīng)用于機(jī)械臂的路徑規(guī)劃中。但該算法存在采樣隨機(jī)性強(qiáng)，搜索擴(kuò)展時間長，收斂速度慢等問題。許多學(xué)者對其進(jìn)行了改進(jìn)。kuffner等提出改進(jìn)的rrt-connect算法從起始點(diǎn)和目標(biāo)點(diǎn)同時擴(kuò)展兩棵隨機(jī)樹，直至連接在一起，加快了收斂速度，減少了搜索時間。但在有障礙物場景下的路徑搜索過程中冗余節(jié)點(diǎn)多，規(guī)劃出路徑較長。

3、因此，亟需一種新的機(jī)械臂路徑規(guī)劃方法來實(shí)現(xiàn)路徑的最優(yōu)規(guī)劃，同時降低冗余節(jié)點(diǎn)數(shù)量。

技術(shù)實(shí)現(xiàn)思路

1、有鑒于此，本發(fā)明的目的在于提供一種融合天牛須算法的rrt-connect機(jī)械臂路徑規(guī)劃方法，通過在起始點(diǎn)和目標(biāo)點(diǎn)之間增加第三點(diǎn)，生成四棵隨機(jī)樹，同時進(jìn)行雙向搜索，其次是給節(jié)點(diǎn)擴(kuò)展引入天牛須算法，增強(qiáng)節(jié)點(diǎn)擴(kuò)展的目標(biāo)導(dǎo)向性，減少無效搜索，直至所有隨機(jī)樹連在一起，選出從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑。

2、為達(dá)到上述目的，本發(fā)明提供如下技術(shù)方案：

3、一種融合天牛須算法的rrt-connect機(jī)械臂路徑規(guī)劃方法，將給定起始點(diǎn)和目標(biāo)點(diǎn)間的中點(diǎn)作為第三節(jié)點(diǎn)，若起始點(diǎn)和目標(biāo)點(diǎn)的中點(diǎn)存在障礙物，選取起始點(diǎn)和目標(biāo)點(diǎn)連線中垂線上距離連線中點(diǎn)向上或向下平移一個或多個單位距離點(diǎn)；使得融合天牛須的rrt-connect算法可以從起始點(diǎn)向第三節(jié)點(diǎn)、第三節(jié)點(diǎn)向起始點(diǎn)以及第三節(jié)點(diǎn)向目標(biāo)點(diǎn)、目標(biāo)點(diǎn)向第三節(jié)點(diǎn)生長出四棵樹；同時結(jié)合天牛須算法，將天牛須算法的左右兩根觸角感知?dú)馕稘舛鹊哪芰θ诤系絩rt-connect算法。該方法具體包括以下步驟：

4、步驟1：在已知環(huán)境地圖中，設(shè)置機(jī)械臂路徑規(guī)劃的起始點(diǎn)、目標(biāo)點(diǎn)及障礙物；

5、步驟2：添加第三節(jié)點(diǎn)，取起始點(diǎn)和目標(biāo)點(diǎn)的中點(diǎn)，若中點(diǎn)處有障礙物，則選取起始點(diǎn)和目標(biāo)點(diǎn)連線中垂線上距離連線中點(diǎn)向上或向下一個或多個單位距離位置點(diǎn)；

6、步驟3：將起始點(diǎn)作為第一隨機(jī)樹，將以起始點(diǎn)為目標(biāo)的第三節(jié)點(diǎn)作為第二隨機(jī)樹，將目標(biāo)點(diǎn)作為第三隨機(jī)樹，將以目標(biāo)點(diǎn)為目標(biāo)的第三節(jié)點(diǎn)作為第四隨機(jī)樹；第一隨機(jī)樹與第二隨機(jī)樹雙向擴(kuò)展，第三隨機(jī)樹與第四隨機(jī)樹雙向擴(kuò)展；

7、步驟4：第一隨機(jī)樹至第四隨機(jī)樹均按照步驟5～步驟6進(jìn)行重新拓展，獲得規(guī)劃路徑；

8、步驟5：將每棵隨機(jī)樹的節(jié)點(diǎn)當(dāng)作一只天牛，每只天牛有左右兩個觸須，比較左右觸須適應(yīng)度函數(shù)y＝f(x)即左須適應(yīng)度yleft與右須適應(yīng)度yright大小，確定運(yùn)動方向；

9、步驟6：每棵隨機(jī)樹沿著確定的方向按照擴(kuò)展節(jié)點(diǎn)的步長step和左右觸須坐標(biāo)值，獲取新節(jié)點(diǎn)qnew，并判斷新節(jié)點(diǎn)和最近節(jié)點(diǎn)連線間是否有障礙物，若有，則刪除該新節(jié)點(diǎn)，改變起始節(jié)點(diǎn)天牛的朝向并返回步驟5，反之，將新節(jié)點(diǎn)加入到隨機(jī)樹的路徑中，直到最后隨機(jī)樹新節(jié)點(diǎn)與雙向樹的另一棵樹的新節(jié)點(diǎn)的距離小于步長step，且不經(jīng)過障礙物，完成四棵樹的雙向擴(kuò)展，進(jìn)入步驟7；

10、步驟7：從擴(kuò)展的四棵隨機(jī)樹的路徑點(diǎn)中選擇從起始點(diǎn)經(jīng)過第三節(jié)點(diǎn)到目標(biāo)點(diǎn)避開障礙物的最優(yōu)路徑。

11、進(jìn)一步，步驟2中，第三節(jié)點(diǎn)的獲取按以下公式計算：

12、如果中點(diǎn)不經(jīng)過障礙物：

13、

14、如果中點(diǎn)經(jīng)過障礙物：

15、

16、其中，起始點(diǎn)坐標(biāo)為(x1,y1)，目標(biāo)點(diǎn)坐標(biāo)為(x2,y2)，第三節(jié)點(diǎn)坐標(biāo)為(x3,y3)，n代表移動n個單位距離。

17、進(jìn)一步，步驟5中，左觸須坐標(biāo)為：xleft＝x+d×v；

18、右觸須坐標(biāo)為：xright＝x-d×v；

19、其中，x的初始值為初始位置天牛的質(zhì)心，d為給定的左右兩觸須間的距離，v為天牛朝向向量。

20、進(jìn)一步，步驟5中，天牛朝向向量v的表達(dá)式為：

21、

22、其中，rands(n,1)表示隨機(jī)生成0～1之間的n維隨機(jī)向量，能隨機(jī)改變天牛的朝向。

23、進(jìn)一步，步驟5中，左須的適應(yīng)度函數(shù)值為yleft＝f(xleft)，右須的適應(yīng)度函數(shù)值為yright＝f(xright)；

24、當(dāng)f(xleft)＜f(xright)時，

25、當(dāng)f(xleft)＞f(xright)時，

26、其中，step表示天牛的移動步長，t為迭代次數(shù)，xt和xt-1分別表示x在第t次迭代和t-1次迭代時的天牛質(zhì)心值即擴(kuò)展的新節(jié)點(diǎn)的位置。

27、本發(fā)明的有益效果在于：

28、(1)本發(fā)明通過增加第三點(diǎn)，可以同時生成四棵隨機(jī)樹，能夠快速進(jìn)行路徑搜索，減少路徑規(guī)劃的時間，提高路徑規(guī)劃的搜索效率。

29、(2)本發(fā)明通過將四棵隨機(jī)樹的每個節(jié)點(diǎn)當(dāng)作一只天牛，通過比較天牛左右觸須適應(yīng)度函數(shù)值，確定前進(jìn)的方向，增強(qiáng)了節(jié)點(diǎn)擴(kuò)展的目標(biāo)導(dǎo)向性，減少無效搜索。

30、本發(fā)明的其他優(yōu)點(diǎn)、目標(biāo)和特征在某種程度上將在隨后的說明書中進(jìn)行闡述，并且在某種程度上，基于對下文的考察研究對本領(lǐng)域技術(shù)人員而言將是顯而易見的，或者可以從本發(fā)明的實(shí)踐中得到教導(dǎo)。本發(fā)明的目標(biāo)和其他優(yōu)點(diǎn)可以通過下面的說明書來實(shí)現(xiàn)和獲得。

技術(shù)特征：

1.一種融合天牛須算法的rrt-connect機(jī)械臂路徑規(guī)劃方法，其特征在于，將給定起始點(diǎn)和目標(biāo)點(diǎn)間的中點(diǎn)作為第三節(jié)點(diǎn)，若起始點(diǎn)和目標(biāo)點(diǎn)的中點(diǎn)存在障礙物，選取起始點(diǎn)和目標(biāo)點(diǎn)連線中垂線上距離連線中點(diǎn)向上或向下平移一個或多個單位距離點(diǎn)；使得融合天牛須的rrt-connect算法可以從起始點(diǎn)向第三節(jié)點(diǎn)、第三節(jié)點(diǎn)向起始點(diǎn)以及第三節(jié)點(diǎn)向目標(biāo)點(diǎn)、目標(biāo)點(diǎn)向第三節(jié)點(diǎn)生長出四棵樹；同時結(jié)合天牛須算法，將天牛須算法的左右兩根觸角感知?dú)馕稘舛鹊哪芰θ诤系絩rt-connect算法；該方法具體包括以下步驟：

2.根據(jù)權(quán)利要求1所述的rrt-connect機(jī)械臂路徑規(guī)劃方法，其特征在于，步驟2中，第三節(jié)點(diǎn)的獲取按以下公式計算：

3.根據(jù)權(quán)利要求1所述的rrt-connect機(jī)械臂路徑規(guī)劃方法，其特征在于，步驟5中，

4.根據(jù)權(quán)利要求3所述的rrt-connect機(jī)械臂路徑規(guī)劃方法，其特征在于，步驟5中，天牛朝向向量v的表達(dá)式為：

5.根據(jù)權(quán)利要求1所述的rrt-connect機(jī)械臂路徑規(guī)劃方法，其特征在于，步驟5中，左須的適應(yīng)度函數(shù)值為yleft＝f(xleft)，右須的適應(yīng)度函數(shù)值為yright＝f(xright)；

技術(shù)總結(jié)
本發(fā)明涉及一種融合天牛須算法的RRT?Connect機(jī)械臂路徑規(guī)劃方法，屬于機(jī)械臂路徑規(guī)劃技術(shù)領(lǐng)域。該方法將給定起始點(diǎn)和目標(biāo)點(diǎn)間的中點(diǎn)作為第三節(jié)點(diǎn)，若起始點(diǎn)和目標(biāo)點(diǎn)的中點(diǎn)存在障礙物，選取起始點(diǎn)和目標(biāo)點(diǎn)連線中垂線上距離連線中點(diǎn)向上或向下平移一個或多個單位距離點(diǎn)；使得融合天牛須的RRT?Connect算法可以從起始點(diǎn)向第三節(jié)點(diǎn)、第三節(jié)點(diǎn)向起始點(diǎn)以及第三節(jié)點(diǎn)向目標(biāo)點(diǎn)、目標(biāo)點(diǎn)向第三節(jié)點(diǎn)生長出四棵樹。同時結(jié)合天牛須算法，將天牛須算法的左右兩根觸角感知?dú)馕稘舛鹊哪芰θ诤系絉RT?Connect算法，減少擴(kuò)展的冗余點(diǎn)個數(shù)，縮短路徑長度，提高搜索效率。

技術(shù)研發(fā)人員：劉想德,蔣菲,張毅,楊超旋,熊慧
受保護(hù)的技術(shù)使用者：重慶郵電大學(xué)
技術(shù)研發(fā)日：
技術(shù)公布日：2025/1/2