本發(fā)明涉及智能信息，尤其是指一種基于立體視覺的機械臂路徑規(guī)劃方法及裝置。

背景技術(shù)：

1、機器人作為和實際物理世界交互的實體，它所執(zhí)行的每一個動作都需要以規(guī)劃算法為基礎(chǔ)。機器人路徑規(guī)劃問題是在已知環(huán)境中，按照一定指標(biāo)，找到連接起始點到目標(biāo)點的路徑，該路徑由一串離散的機器人位姿路徑點組成。協(xié)作型機械臂需要具備在環(huán)境中自主規(guī)劃路徑的能力，既能夠基于已知環(huán)境信息進行全局路徑規(guī)劃，同時在遇到事先未知的障礙時能夠及時的進行局部避障路徑規(guī)劃。

2、在環(huán)境信息已知的情況下，現(xiàn)有的協(xié)作型機械臂的路徑規(guī)劃方法能夠?qū)崿F(xiàn)一定程度的自主路徑規(guī)劃，根據(jù)場景和目的不同可以具體分類分為全局規(guī)劃和局部規(guī)劃兩類。

3、在全局路徑規(guī)劃問題中，需要事先知道整個環(huán)境的障礙位置信息，以rrt和prm為代表的隨機采樣搜索型路徑規(guī)劃方法有較好的效果。隨機采樣搜索型路徑規(guī)劃方法避免了對復(fù)雜環(huán)境的建模，只需要進行碰撞檢測就能判斷生成的路徑是否可行。但是該方法直接獲得的路徑通常比較曲折，并且僅能獲得協(xié)作型機械臂在靜態(tài)已知環(huán)境中的任務(wù)執(zhí)行路徑，而對于事先未知的動態(tài)障礙，例如協(xié)作流水線上的工人，不能做到實時躲避。

4、在局部路徑規(guī)劃方法中，勢場法在協(xié)作型機械臂上有著較好的應(yīng)用。勢場法的求解方式簡潔，實時性較好，對障礙模型的依賴程度較低。但是勢場法也具有一些固有缺陷，它容易陷入局部極小值，無法生成復(fù)雜的機械臂軌跡，在面對復(fù)雜的動態(tài)環(huán)境時存在一些明顯的局限性。

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

1、為此，本發(fā)明所要解決的技術(shù)問題在于克服現(xiàn)有技術(shù)中協(xié)作型機械臂路徑規(guī)劃的效率低且容易與動態(tài)障礙發(fā)生碰撞的問題。

2、為解決上述技術(shù)問題，本發(fā)明提供了一種基于立體視覺的機械臂路徑規(guī)劃方法，包括：

3、獲取機械臂作業(yè)場景的環(huán)境地圖；

4、提取機械臂作業(yè)場景中的動態(tài)障礙點云；

5、將距離機械臂上控制點最近的動態(tài)障礙點云設(shè)置為最近障礙點，并對最近障礙點進行跟蹤；

6、基于機械臂作業(yè)場景的環(huán)境地圖，離線規(guī)劃機械臂的全局路徑；

7、在機械臂按照全局路徑行進的過程中，基于最近障礙點，采用人工勢場法令機械臂對動態(tài)障礙進行實時避障，輸出機械臂的運行速度。

8、優(yōu)選地，所述提取機械臂作業(yè)場景中的動態(tài)障礙點云，包括：

9、將機械臂設(shè)置為不同位姿，離線拍攝并存儲多張沒有動態(tài)障礙的原始深度圖；

10、在原始深度圖中濾除機械臂，得到濾除機械臂后的背景深度圖；

11、將多張濾除機械臂后的背景深度圖進行比對，保留其中深度最大值，得到靜態(tài)背景深度圖；

12、利用深度相機實時采集帶有動態(tài)障礙的深度圖，將帶有動態(tài)障礙的深度圖中的機械臂濾除后，與靜態(tài)背景深度圖進行比較，得到動態(tài)障礙深度圖；

13、根據(jù)深度相機的內(nèi)參對動態(tài)障礙深度圖進行轉(zhuǎn)換，得到相機坐標(biāo)系下的動態(tài)障礙點云。

14、優(yōu)選地，采用卡爾曼濾波對最近障礙點進行跟蹤。

15、優(yōu)選地，所述基于機械臂作業(yè)場景的環(huán)境地圖，離線規(guī)劃機械臂的全局路徑，包括：

16、基于rrt_connect算法在關(guān)節(jié)空間中離線規(guī)劃出機械臂的初始全局路徑；

17、利用貪心算法消除初始全局路徑中的冗余點；

18、采用三次樣條曲線對經(jīng)過貪心算法優(yōu)化后的全局路徑進行插值平滑處理，得到目標(biāo)全局路徑。

19、優(yōu)選地，在機械臂按照全局路徑行進的過程中，基于最近障礙點，采用人工勢場法令機械臂對動態(tài)障礙進行實時避障，輸出機械臂的運行速度，包括：

20、在全局路徑的各路徑點所處的關(guān)節(jié)空間中設(shè)置吸引速度勢場，機械臂按照全局路徑向下一路徑點行進時，根據(jù)機械臂當(dāng)前位置與下一路徑點之間的歐氏距離計算吸引速度；

21、在最近障礙點上設(shè)置排斥速度勢場，計算排斥速度；

22、將位于笛卡爾空間的排斥速度轉(zhuǎn)換至關(guān)節(jié)空間，得到機械臂的關(guān)節(jié)避障速度；將機械臂的關(guān)節(jié)吸引速度設(shè)置為吸引速度；關(guān)節(jié)吸引速度和關(guān)節(jié)避障速度疊加后得到機械臂的運行速度。

23、優(yōu)選地，所述在全局路徑的各路徑點所處的關(guān)節(jié)空間中設(shè)置吸引速度勢場，計算吸引速度，包括：

24、設(shè)置吸引速度勢場函數(shù)為：

25、

26、其中，dgoal為當(dāng)前機械臂關(guān)節(jié)空間的位置與下一路徑點之間的歐氏距離，是吸引速度勢場函數(shù)的變量；kgoal表示吸引速度的最大值，α表示臨界距離；f(dgoal)表示吸引速度勢場的大小；

27、計算吸引速度，公式為：

28、

29、其中，為吸引速度，為當(dāng)前機械臂關(guān)節(jié)空間的位置指向下一路徑點的方向向量。

30、優(yōu)選地，所述在最近障礙點上設(shè)置排斥速度勢場，計算排斥速度，包括：

31、設(shè)置排斥速度勢場函數(shù)為：

32、

33、其中，kobs表示排斥速度的最大值，ρ表示球形障礙檢測區(qū)域的半徑，β表示下降常數(shù)；dobs表示機械臂控制點和最近障礙點之間距離，fobs(dobs)表示排斥速度勢場的大??；

34、計算排斥速度，公式為：

35、

36、其中，為排斥速度，為最近障礙點指向機械臂控制點的方向向量。

37、優(yōu)選地，得到機械臂的運行速度后，將機械臂的關(guān)節(jié)避障速度投影到全局路徑上，對機械臂的運行速度進行優(yōu)化，包括：

38、對于全局路徑中的路徑點，假設(shè)從路徑點pl到路徑點pl+1的連線為將作為線性空間a，則線性空間a的投影矩陣pa為：

39、pa＝a(ata)-1at

40、其中，

41、將關(guān)節(jié)避障速度投影至線性空間a，得到避障投影速度為：

42、

43、其中，為關(guān)節(jié)避障速度，為避障投影速度；

44、優(yōu)化后的機械臂的運行速度為避障投影速度和關(guān)節(jié)吸引速度的合速度。

45、優(yōu)選地，在機械臂的運行過程中增加避障策略，所述避障策略包括：

46、當(dāng)關(guān)節(jié)吸引速度和避障投影速度同向時，機械臂的運行速度為關(guān)節(jié)吸引速度；

47、當(dāng)關(guān)節(jié)吸引速度和避障投影速度異向時，若避障投影速度的模值大于關(guān)節(jié)吸引速度的模值，則機械臂的運行速度為0；否則機械臂的運行速度為關(guān)節(jié)吸引速度和避障投影速度的合速度。

48、本發(fā)明還提供了一種基于立體視覺的機械臂路徑規(guī)劃裝置，包括：

49、地圖獲取模塊，用于獲取機械臂作業(yè)場景的環(huán)境地圖；

50、動態(tài)障礙點云提取模塊，用于提取機械臂作業(yè)場景中的動態(tài)障礙點云；

51、動態(tài)障礙追蹤模塊，用于將距離機械臂上控制點最近的動態(tài)障礙點云設(shè)置為最近障礙點，并對最近障礙點進行跟蹤；

52、全局路徑規(guī)劃模塊，用于基于靜態(tài)的機械臂作業(yè)場景的環(huán)境地圖，離線規(guī)劃機械臂的全局路徑；

53、實時避障模塊，用于在機械臂按照全局路徑行進的過程中，基于最近障礙點，采用人工勢場法令機械臂對動態(tài)障礙進行實時避障，輸出機械臂的運行速度。

54、本發(fā)明的上述技術(shù)方案相比現(xiàn)有技術(shù)具有以下有益效果：

55、本發(fā)明所述的一種基于立體視覺的機械臂路徑規(guī)劃方法，在獲取靜態(tài)環(huán)境和動態(tài)障礙信息的基礎(chǔ)上，首先基于靜態(tài)環(huán)境規(guī)劃全局路徑，再基于動態(tài)障礙信息，即最近障礙點，采用人工勢場法實現(xiàn)了機械臂對動態(tài)障礙的實時避障，提高了機械臂路徑規(guī)劃的效率，使機械臂具備自主避障的能力，能夠主動感知周圍動態(tài)的環(huán)境并自主做出規(guī)劃，同時提高了機械臂路徑規(guī)劃的效率。本發(fā)明顯著增強了機械臂的環(huán)境適應(yīng)能力和操作安全性，提高了機械臂的任務(wù)完成效率，降低了運行成本。