本發(fā)明涉及機器人避障，具體為一種多自由度機械臂避障路徑規(guī)劃方法及系統(tǒng)。

背景技術：

1、目前，機械臂避障技術在多個領域取得了顯著進展。首先，傳感器技術的快速發(fā)展，如激光雷達、深度攝像頭和力傳感器，使得機械臂能夠更加精準地感知環(huán)境，實時檢測靜態(tài)和動態(tài)障礙物。其次，路徑規(guī)劃算法得到了不斷優(yōu)化，結(jié)合啟發(fā)式搜索算法和采樣算法，提高了路徑的規(guī)劃效率和可靠性。

2、現(xiàn)有技術中的，公開號為cn110125943b公開了一種多自由度機械臂避障路徑規(guī)劃方法，通過采用機械臂路徑規(guī)劃方法得到初始路徑；采集初始路徑中每個關節(jié)在每一預設時刻的實時信息；對所述初始路徑中的實時信息進行數(shù)學建模并映射，隨機選擇一條路徑，按預設的約束條件計算可行度、適應度，將適應度最大的路徑標記為合格路徑，重復，直至合格，將合格路徑記為第二代路徑集合；對第二代路徑集合進行交叉、變異、融合；得到第三代路徑集合將得到的第三代路徑集,重復，直至迭代次數(shù)為n；將最后一次迭代中得到適應度最大的路徑作為最終目標路徑。

3、現(xiàn)有技術不足：

4、現(xiàn)有技術存在計算效率與實時性不足問題，隨著機械臂自由度的增加，路徑規(guī)劃算法的計算復雜度也隨之上升，尤其在復雜環(huán)境中，障礙物增多，導致路徑的生成、評估和優(yōu)化需要大量計算資源，無法確定優(yōu)化路徑所屬的工作空間，實時性不強。

5、現(xiàn)有技術未考慮到機械臂在避障動作時自身的運行參數(shù)，通過隨機路徑的選擇，無法在動態(tài)變化的環(huán)境下確定機械臂的運動效率和安全性，難以及時調(diào)整路徑以應對突發(fā)障礙物出現(xiàn)或環(huán)境變化。

6、因此有必要提供一種多自由度機械臂避障路徑規(guī)劃方法及系統(tǒng)來解決所述問題。

7、在所述背景技術部分公開的上述信息僅用于加強對本公開的背景的理解，因此它可以包括不構(gòu)成對本領域普通技術人員已知的現(xiàn)有技術的信息。

技術實現(xiàn)思路

1、本發(fā)明的目的在于提供一種多自由度機械臂避障路徑規(guī)劃方法及系統(tǒng)，以解決上述背景技術中提出的問題。

2、為實現(xiàn)上述目的，本發(fā)明提供如下技術方案：

3、一種多自由度機械臂避障路徑規(guī)劃方法，具體步驟包括：

4、進一步地，求解機械臂的各個關節(jié)變換關系，所依據(jù)的方法為：

5、基于正向運動學運用運動學鏈的幾何關系求解，利用到機械臂相關聯(lián)的四個參數(shù)，四個參數(shù)分別為旋轉(zhuǎn)角度、連桿長度、連桿的偏移量以及扭轉(zhuǎn)角，對于所述旋轉(zhuǎn)角度、連桿長度、連桿偏移以及扭轉(zhuǎn)角，通過一個齊次變換矩陣來表示關節(jié)i和關節(jié)i-1的變換關系，對于關節(jié)點數(shù)量為n的機械臂，通過逐級乘積方式計算，將各個關節(jié)的齊次變換矩陣依次相乘，最終得到機械臂相對于基座的變換矩陣，所依據(jù)的邏輯為：

6、

7、其中，θi表示第i個關節(jié)點的旋轉(zhuǎn)角度，αi表示第i個關節(jié)點連接的連桿長度，di表示第i個關節(jié)點連接的連桿的偏移量，αi表示第i個關節(jié)點所連接的相鄰兩個連桿間的扭轉(zhuǎn)角，tii-1表示關節(jié)i變換到關節(jié)i-1的齊次變換矩陣，i為關節(jié)點的索引；

8、

9、其中，表示機械臂相對于基座的變換矩陣，表示關節(jié)n變換到關節(jié)n-1的齊次變換矩陣。

10、進一步地，求解各個關節(jié)三維坐標，根據(jù)所述機械臂的各個關節(jié)變換關系以及三維坐標生成機械臂的工作空間，所依據(jù)的公式為：

11、計算各個關節(jié)三維坐標，所依據(jù)的公式為：

12、xi＝αicosθi+αi+1cos(θi+θi+1)

13、yi＝αisinθi+αi+1sin(θi+θi+1)

14、zi＝αitanθi+αi+1tan(θi+θi+1)

15、其中，xi、yi、zi表示第i個關節(jié)點的橫坐標、縱坐標、豎坐標，αi+1表示第i+1個關節(jié)點所連接的相鄰兩個連桿間的扭轉(zhuǎn)角，θi+1表示第i+1個關節(jié)點的旋轉(zhuǎn)角度；

16、利用所述機械臂的各個關節(jié)變換關系以及三維坐標生成機械臂的工作空間，所依據(jù)的方法為：

17、計算末端執(zhí)行器的坐標并以其為基準點，遍歷所有關節(jié)點的角度組合，對每個關節(jié)點的旋轉(zhuǎn)角度作離散化處理，對每個離散化后的旋轉(zhuǎn)角度組合使用正向運動學計算相鄰關節(jié)點之間的變換關系，將計算得到的所有坐標位置記錄下來生成空間集合來表示機械臂的工作空間，所依據(jù)的公式為：

18、

19、

20、其中，θ表示將旋轉(zhuǎn)角度進行離散化處理后的角度集合，表示末端執(zhí)行器的坐標,坐標中的1表示末端執(zhí)行器的自由度，p表示空間集合用于表示機械臂的工作空間。

21、進一步地，生成與所述機械臂工作空間相對應的障礙物空間，建立機械臂的避障空間，所依據(jù)的方法為：

22、根據(jù)每個障礙物的坐標和大小，在機械臂的工作空間中定義其幾何體積，使用幾何工具表示每個障礙物占據(jù)的體積集合，這些集合在機械臂工作空間占據(jù)相應的區(qū)域，所述區(qū)域即為障礙物空間，再將所述障礙物空間與機械臂工作空間進行映射，通過布爾運算計算工作空間與障礙空間的幾何差集，得到機械臂的避障空間，所依據(jù)的公式為：

23、

24、其中，f表示機械臂的避障空間，w表示機械臂的工作空間，v(om)表示第m個障礙物品的體積幾何，om表示第m個障礙物品，q表示障礙物的總體數(shù)量。

25、進一步地，采用啟發(fā)式搜索算法，生成初步全局路徑，所依據(jù)的方法為：

26、利用啟發(fā)式搜索算法估算從當前節(jié)點到目標節(jié)點的代價以選擇最優(yōu)路徑，使用一個具體的估值函數(shù)，從起點開始將節(jié)點的鄰居節(jié)點放入開放列表，計算它們的總路徑代價，并根據(jù)最小的總路徑代價來選擇下一個擴展的節(jié)點，終止條件是當目標節(jié)點被擴展時，搜索結(jié)束，找到初步全局路徑，所依據(jù)的公式為：

27、f(j)＝g(j)+h(j)

28、

29、h(j)＝|xj-xgoal|+|yj-ygoal|+|zj-zgoal|

30、其中，f(j)表示從起點到目標節(jié)點且經(jīng)過節(jié)點j總路徑代價，g(j)表示從起點到節(jié)點j的實際代價，h(j)表示從節(jié)點j到目標節(jié)點的啟發(fā)估計，即為估計代價，用曼哈頓距離表示，xgoal、ygoal、zgoal表示目標節(jié)點的橫坐標、縱坐標、豎坐標，xj、yj、zj表示路徑節(jié)點的橫坐標、縱坐標、豎坐標；

31、對避障路徑長度進行最小化以及對安全距離最大化，選取路途障礙最少的避障路徑，滿足這一約束條件的最小值，所依據(jù)的公式為：

32、l＝f(j)min

33、

34、其中，l表示避障路徑的最小長度，lk表示路徑l上的第k個點，d(lk，om)表示路徑l上第k個點lk與所有障礙物間的最小距離，dmin表示定義最小的安全距離，k表示路徑l上點的索引。

35、進一步地，計算機械臂在運動中的速度、加速度以及關節(jié)運動的極限力矩，所依據(jù)的公式為：

36、

37、其中，v(t)表示機械臂在t時刻的速度，a(t)表示機械臂在t時刻的加速度，i表示慣性力矩，θi+1-θi表示路徑中第i+1個關節(jié)點的旋轉(zhuǎn)角度與第i個關節(jié)點的旋轉(zhuǎn)角度的差值，τmax表示機械臂的極限力矩。

38、進一步地，生成機械臂最大避障角，所依據(jù)的公式為：

39、τdynamic(t)＝τmax+δ*v(t)+ε*a(t)

40、

41、ki＝θi+δμi

42、其中，τdynamic(t)表示機械臂在t時刻的動態(tài)力矩，δ、ε分別表示機械臂速度、加速度對動態(tài)力矩作用的權重，kmax(t)表示機械臂在t時刻的最大障礙角，ki表示關節(jié)i的避障角，θi表示當?shù)趇個關節(jié)點的旋轉(zhuǎn)角度，δμi表示為確保避障而需要增加或減小的角度。

43、進一步地，利用每個關節(jié)避障角對比最大避障角以篩選出極端關節(jié)角，并統(tǒng)計其數(shù)量，所依據(jù)的邏輯為：

44、

45、其中，q表示判斷每個關節(jié)避障角是否為極端關節(jié)角的邏輯值，當q＝1時，表示關節(jié)i的避障角是極端關節(jié)角；當q＝0時，表示關節(jié)i的避障角不是極端關節(jié)角。

46、進一步地，根據(jù)所述極端關節(jié)角數(shù)量，計算冗余自由度，設置冗余自由度閾值，若冗余自由度超過閾值，則避障成功且路徑規(guī)劃有效，所依據(jù)的邏輯為：

47、r＝j-n

48、

49、其中，r表示冗余自由度，rmax表示冗余自由度閾值，j表示機械臂整體自由度，n表示極端關節(jié)角個數(shù)。

50、本發(fā)明另外還提供一種多自由度機械臂避障路徑規(guī)劃系統(tǒng)，所述路徑規(guī)劃系統(tǒng)用于執(zhí)行任意一項上述的多自由度機械臂避障路徑規(guī)劃方法，包括：

51、工作空間生成模塊，所述工作空間生成模塊用于基于正向運動學原理求解機械臂的各個關節(jié)變換關系以及各個關節(jié)三維坐標，根據(jù)所述機械臂的各個關節(jié)變換關系以及三維坐標生成機械臂的工作空間；

52、避障空間建立模塊，所述避障空間建立模塊用于在所述機械臂工作空間內(nèi)根據(jù)障礙物的體積進行建模，生成與所述機械臂工作空間相對應的障礙物空間，建立機械臂的避障空間；

53、路徑優(yōu)化模塊，所述路徑優(yōu)化模塊用于在所述建立的避障空間上，采用啟發(fā)式搜索算法，生成初步全局路徑，對初步全局路徑做優(yōu)化處理，所述優(yōu)化處理包括，對路徑長度進行最小化以及對安全距離最大化，選取路途障礙最少的避障路徑；

54、動態(tài)性能分析模塊，所述動態(tài)性能分析模塊用于計算機械臂在運動中的速度、加速度以及關節(jié)運動的極限力矩，利用所述機械臂的速度、加速度和極限力矩綜合生成機械臂最大避障角；

55、冗余自由度評估模塊，所述冗余自由度評估模塊用于利用每個關節(jié)避障角對比最大避障角以篩選出極端關節(jié)角，并統(tǒng)計其數(shù)量，根據(jù)所述極端關節(jié)角數(shù)量，計算冗余自由度，設置冗余自由度閾值，若冗余自由度超過閾值，則避障成功且路徑規(guī)劃有效。

56、與現(xiàn)有技術相比，本發(fā)明的有益效果是：

57、本發(fā)明通過采用啟發(fā)式搜索算法，優(yōu)化路徑長度和安全距離，可以提高路徑規(guī)劃的效率。結(jié)合正向運動學和齊次變換矩陣，快速求解機械臂的工作空間和避障空間，解決了現(xiàn)有技術在復雜環(huán)境中計算時間長和反應遲緩的問題，此外，通過路徑優(yōu)化模塊的引入，確保路徑長度的最小化和安全距離的最大化，大幅提升了運動的安全性，克服了以往技術中安全性考慮不足的缺陷；

58、本發(fā)明通過動態(tài)性能分析和冗余自由度評估，增強了機械臂在復雜及動態(tài)環(huán)境中的靈活性，路徑平滑處理則確保了運動的連貫性，解決了現(xiàn)有技術中路徑不夠平滑的問題。同時，通過評估極端關節(jié)角和靈活調(diào)整路徑的能力，應對障礙物的能力也得到了顯著提升，有效應對了障礙物密集環(huán)境中現(xiàn)有技術的不足；

59、本發(fā)明通過工作空間生成、避障空間建立、路徑優(yōu)化、動態(tài)性能分析和冗余自由度評估等方法步驟，實現(xiàn)了機械臂在復雜環(huán)境中的路徑規(guī)劃優(yōu)化和安全避障功能。