本發(fā)明涉及機(jī)械臂控制領(lǐng)域，具體涉及一種機(jī)器人運(yùn)動(dòng)路徑規(guī)劃方法。

背景技術(shù)：

信息技術(shù)的發(fā)展，信息技術(shù)被廣泛的應(yīng)用到人們的生活方式與工作方式中。如有更有效的利用現(xiàn)有的信息技術(shù)，人們一直沒(méi)有停止探索。機(jī)器人領(lǐng)域是集計(jì)算機(jī)、機(jī)械、傳感技術(shù)、信息處理技術(shù)、圖像處理與識(shí)別技術(shù)、語(yǔ)言識(shí)別與處理技術(shù)、控制技術(shù)和通信技術(shù)等于一體的系統(tǒng)?，F(xiàn)有的Player、MOOS、CARMEN、YARP、Orocos、微軟RoboticsStudio等操作系統(tǒng)無(wú)法滿(mǎn)足實(shí)際應(yīng)用的需求。ROS操作系統(tǒng)(RobotOperatingSystem)是一種開(kāi)源機(jī)器人操作系統(tǒng)，能夠提供類(lèi)似于操作系統(tǒng)的功能，為機(jī)器人應(yīng)用系統(tǒng)提供硬件抽象、底層驅(qū)動(dòng)、消息傳遞和包管理，以及一些輔助開(kāi)發(fā)工具，例如建立、編寫(xiě)和運(yùn)行多機(jī)通信系統(tǒng)整合的程序。ROS操作系統(tǒng)是能為異質(zhì)計(jì)算機(jī)集群提供例如操作系統(tǒng)功能的軟件平臺(tái)。2007年，斯坦福大學(xué)人工智能實(shí)驗(yàn)室與機(jī)器人技術(shù)公司W(wǎng)illow Garage合作開(kāi)發(fā)出了機(jī)器人操作系統(tǒng)，2008年以后由Willow Garage推廣。ROS操作系統(tǒng)平臺(tái)是一個(gè)非常大的共享平臺(tái)，人們可以借由這個(gè)系統(tǒng)進(jìn)行所處領(lǐng)域的代碼交流，因此世界上有非常多的機(jī)構(gòu)在對(duì)ROS操作系統(tǒng)進(jìn)行開(kāi)發(fā)和維護(hù)。ROS操作系統(tǒng)的設(shè)計(jì)目標(biāo)就是為了提高代碼的復(fù)用性，所以采用一種分布式的進(jìn)程架構(gòu)，使得程序具有高度的獨(dú)立性和低耦合性。

CN201310734882.X公開(kāi)了一種機(jī)械手精確控制系統(tǒng)，它包括視頻采集模塊、圖像處理模塊、機(jī)械手運(yùn)動(dòng)定位點(diǎn)匹配模塊、運(yùn)動(dòng)差異計(jì)算模塊、模糊控制器、機(jī)械手運(yùn)動(dòng)驅(qū)動(dòng)器、路徑規(guī)劃模塊和運(yùn)動(dòng)目標(biāo)位置輸入模塊。一種機(jī)械手精確控制方法，其具體技術(shù)方案為：1、確定機(jī)械手運(yùn)動(dòng)目標(biāo)位置；2、視頻采集模塊和圖像處理模塊判斷當(dāng)前位置；3、路徑規(guī)劃模塊，對(duì)計(jì)算從起始位置坐標(biāo)到運(yùn)動(dòng)目標(biāo)位置坐標(biāo)之間的路徑；4、提取機(jī)械手各個(gè)關(guān)節(jié)節(jié)點(diǎn)在不同時(shí)刻的運(yùn)動(dòng)控制信號(hào)；5、啟動(dòng)視頻采集設(shè)備；6、獲取圖像中的定位點(diǎn)的坐標(biāo)值；7、模糊控制器產(chǎn)生控制信號(hào)；8、機(jī)械手運(yùn)動(dòng)控制信號(hào)驅(qū)動(dòng)機(jī)械手繼續(xù)運(yùn)動(dòng)；9、視頻采集設(shè)備獲取機(jī)械手坐標(biāo)值，并與規(guī)劃坐標(biāo)值進(jìn)行匹配。該發(fā)明實(shí)現(xiàn)了實(shí)時(shí)的精確調(diào)整機(jī)械手的運(yùn)動(dòng)路線(xiàn)。但運(yùn)算復(fù)雜，沒(méi)有提及如何有效的避障。

CN201410524184.1公開(kāi)一種基于ROS(機(jī)器人操作系統(tǒng)，Robot Operating System)的移動(dòng)機(jī)器人室內(nèi)環(huán)境探索系統(tǒng)與控制方法。該系統(tǒng)基于ROS，能夠?qū)崿F(xiàn)機(jī)器人對(duì)室內(nèi)環(huán)境的自主探索與定位。該系統(tǒng)主要采用iRobot差分驅(qū)動(dòng)底盤(pán)和UTM30LX激光雷達(dá)。上位機(jī)可實(shí)時(shí)得到機(jī)器人的位置定位信息與室內(nèi)環(huán)境探索路徑軌跡，并由激光雷達(dá)的數(shù)據(jù)匹配得到由局部地圖連接形成的全局地圖。該方法沒(méi)有提及如何有效的避障，對(duì)于機(jī)械臂的精確控制，沒(méi)有提及相關(guān)的處理方法。

技術(shù)實(shí)現(xiàn)要素：

針對(duì)現(xiàn)有技術(shù)的不足，本發(fā)明提供一種機(jī)器人運(yùn)動(dòng)路徑規(guī)劃方法，該方法基于ROS操作系統(tǒng)，機(jī)器人通過(guò)跟隨人體，采集人體手臂的運(yùn)動(dòng)信息，實(shí)現(xiàn)高自由度的路徑規(guī)劃，避障效果好，工作效率高，運(yùn)算量少。

本發(fā)明為實(shí)現(xiàn)上述目的，采用以下技術(shù)方案：機(jī)器人運(yùn)動(dòng)路徑規(guī)劃方法，其中，所述機(jī)器人包括主機(jī)、機(jī)械臂、微控制器和攝像頭；所述主機(jī)和微控制器連接；所述微控制器連接機(jī)械臂上的舵機(jī)，用于控制機(jī)械臂的運(yùn)動(dòng)；所述主機(jī)連接攝像頭；所述主機(jī)裝有ROS操作系統(tǒng)；所述ROS操作系統(tǒng)中裝有Moveit工具和OMPL(開(kāi)源運(yùn)動(dòng)規(guī)劃庫(kù))。

機(jī)器人機(jī)械臂運(yùn)動(dòng)路徑規(guī)劃方法，具體過(guò)程為：

1)攝像頭采集周?chē)h(huán)境的深度信息，運(yùn)用ROS操作系統(tǒng)將其轉(zhuǎn)換為空間點(diǎn)云數(shù)據(jù)，然后運(yùn)用OpenCV的特征識(shí)別功能從空間點(diǎn)云數(shù)據(jù)中獲取人體框架和空間姿態(tài)信息；

2)根據(jù)步驟1)獲得的人體位置和空間姿態(tài)信息，機(jī)器人主機(jī)控制電機(jī)的運(yùn)轉(zhuǎn)，機(jī)器人跟隨人體的運(yùn)動(dòng)，實(shí)現(xiàn)機(jī)器人跟隨人體的路徑規(guī)劃；

3)機(jī)器人在跟隨人體運(yùn)動(dòng)過(guò)程中，攝像頭重點(diǎn)標(biāo)記采集人手臂圖像信息，并運(yùn)用ROS操作系統(tǒng)將人手臂圖像信息在特定空間坐標(biāo)系中轉(zhuǎn)換為從肩到手的空間坐標(biāo)；

4)人手臂運(yùn)動(dòng)時(shí)，攝像頭重點(diǎn)標(biāo)記采集人手臂運(yùn)動(dòng)姿態(tài)圖像，將運(yùn)動(dòng)姿態(tài)圖像結(jié)合空間坐標(biāo)在ROS操作系統(tǒng)中轉(zhuǎn)換為機(jī)械臂關(guān)節(jié)的空間姿態(tài)坐標(biāo)；

5)根據(jù)機(jī)械臂關(guān)節(jié)的空間姿態(tài)坐標(biāo)、機(jī)械臂當(dāng)前的坐標(biāo)結(jié)合機(jī)械臂的關(guān)節(jié)位置，開(kāi)源運(yùn)動(dòng)規(guī)劃庫(kù)和Moveit工具規(guī)劃?rùn)C(jī)械臂從當(dāng)前的空間坐標(biāo)運(yùn)動(dòng)到指定的位置坐標(biāo)的運(yùn)動(dòng)軌跡，實(shí)現(xiàn)自由路徑規(guī)劃。

進(jìn)一步地，所述步驟2)過(guò)程中，機(jī)器人根據(jù)采集的人體位置和空間姿態(tài)信息，調(diào)整電機(jī)的運(yùn)行速度，保持人體與機(jī)器人的距離。

進(jìn)一步地，所述步驟2)過(guò)程中，機(jī)器人攝像頭中出現(xiàn)多個(gè)人體時(shí)，以不同的數(shù)據(jù)狀態(tài)標(biāo)識(shí)不同人，跟隨指定的人。

進(jìn)一步地，所述步驟2)過(guò)程中，機(jī)器人沒(méi)有找到跟隨的人體時(shí)，根據(jù)機(jī)器人主機(jī)記載的數(shù)據(jù)，自身進(jìn)行旋轉(zhuǎn)尋找目標(biāo)。

進(jìn)一步地，步驟5)實(shí)現(xiàn)自由路徑規(guī)劃過(guò)程中，攝像頭拍攝的空間圖像信息中存在空間障礙物，ROS操作系統(tǒng)將空間圖像信息中的障礙物轉(zhuǎn)換為3D空間的點(diǎn)云數(shù)據(jù)。

進(jìn)一步地，所述主機(jī)根據(jù)3D空間的點(diǎn)云數(shù)據(jù)信息，控制開(kāi)源運(yùn)動(dòng)規(guī)劃庫(kù)和Moveit工具重新規(guī)劃?rùn)C(jī)械臂的運(yùn)動(dòng)軌跡，從而微處理器控制機(jī)械臂在運(yùn)動(dòng)過(guò)程中避障。

進(jìn)一步地，所述機(jī)械臂根據(jù)步驟5)的路徑規(guī)劃，進(jìn)行移動(dòng)過(guò)程中，ROS操作系統(tǒng)通過(guò)不斷的計(jì)算從機(jī)械臂當(dāng)前的坐標(biāo)到指定的位置坐標(biāo)之間的路徑，給出在運(yùn)動(dòng)過(guò)程中機(jī)械臂各個(gè)關(guān)節(jié)的坐標(biāo)，確認(rèn)機(jī)械臂各個(gè)關(guān)節(jié)的實(shí)際的空間姿態(tài)與路徑規(guī)劃中的空間姿態(tài)之間的坐標(biāo)差值。

進(jìn)一步地，所述機(jī)械臂各個(gè)關(guān)節(jié)的實(shí)際的空間姿態(tài)與路徑規(guī)劃中的空間姿態(tài)之間的坐標(biāo)差值大于預(yù)先設(shè)定的范圍時(shí)，開(kāi)源運(yùn)動(dòng)規(guī)劃庫(kù)和Moveit工具通過(guò)逆運(yùn)動(dòng)學(xué)解算機(jī)械臂的空間姿態(tài)，從而調(diào)整機(jī)械臂關(guān)節(jié)角度，控制機(jī)械臂的舵機(jī)進(jìn)行轉(zhuǎn)動(dòng)，調(diào)整機(jī)械臂的運(yùn)動(dòng)角度，調(diào)整路徑的偏差。

進(jìn)一步地，所述機(jī)械臂各個(gè)關(guān)節(jié)的實(shí)際的空間姿態(tài)與路徑規(guī)劃中的空間姿態(tài)之間的坐標(biāo)差值小于預(yù)先設(shè)定的范圍，則主機(jī)發(fā)出運(yùn)動(dòng)終止的信號(hào)，機(jī)械臂完成此次任務(wù)。

進(jìn)一步地，所述機(jī)器人還設(shè)置有預(yù)警裝置，所述步驟4)中攝像頭采集人手臂運(yùn)動(dòng)姿態(tài)圖像時(shí)，攝像頭與人手臂的距離超出預(yù)先設(shè)定的距離時(shí)，進(jìn)行預(yù)警。

進(jìn)一步地，所述機(jī)器人還設(shè)置有底盤(pán)微控制器、碰撞傳感器和底部懸空傳感器，機(jī)器人在行走過(guò)程中出現(xiàn)地面的障礙物和地面極度不平時(shí)，碰撞傳感器能檢測(cè)到地面上高度較低的障礙物，底部懸空傳感器檢測(cè)底盤(pán)到地面的距離來(lái)解決地面不平的障礙，底盤(pán)微控制器調(diào)整機(jī)器人穩(wěn)定行走。

進(jìn)一步地，所述機(jī)器人還設(shè)置有跟隨模式選擇裝置，用于設(shè)定攝像頭與人手臂的拍攝位置和距離；所述拍攝位置包括：人手臂的左側(cè)方、右側(cè)方和正前方。

進(jìn)一步地，所述主機(jī)為Intel雙核四線(xiàn)程i7工業(yè)微型主機(jī)。

進(jìn)一步地，所述攝像頭為RGBD深度攝像頭。

本發(fā)明的有益效果：

本發(fā)明提供的一種機(jī)器人運(yùn)動(dòng)路徑規(guī)劃方法，該方法基于ROS操作系統(tǒng)，該方法把圖像信息用點(diǎn)云數(shù)據(jù)表示障礙物的位置和形狀大小，有效的實(shí)現(xiàn)路徑規(guī)劃中避障，減少了計(jì)算量；采用ROS操作系統(tǒng)，能夠使運(yùn)動(dòng)獨(dú)立性增強(qiáng)；運(yùn)用ROS操作系統(tǒng)中自帶工具，減少運(yùn)算的復(fù)雜程度，規(guī)劃運(yùn)動(dòng)軌跡簡(jiǎn)單，運(yùn)動(dòng)控制也更簡(jiǎn)單；運(yùn)用底盤(pán)微控制器、碰撞傳感器和底部懸空傳感器，來(lái)檢測(cè)環(huán)境中的障礙物和極度不平的地面，碰撞傳感器能檢測(cè)到地面上高度較低的障礙物，彌補(bǔ)了視覺(jué)攝像頭看不到視角外的障礙物，底部懸空傳感器檢測(cè)底盤(pán)到地面的距離，防止機(jī)器人跌落；該方法提高了機(jī)器人在運(yùn)動(dòng)規(guī)劃過(guò)程中的自由度、精確度，在時(shí)間和空間上及數(shù)據(jù)上進(jìn)行融合以提高系統(tǒng)的適應(yīng)性和環(huán)境空間識(shí)別能力，使得機(jī)器人能按照正確的規(guī)劃軌跡進(jìn)行運(yùn)動(dòng)，從而高自由度的實(shí)現(xiàn)機(jī)械臂運(yùn)動(dòng)路徑規(guī)劃，工作效率高，運(yùn)算量少。

具體實(shí)施方式

下面以具體實(shí)施例對(duì)本發(fā)明作進(jìn)一步的說(shuō)明，但本發(fā)明不受下述實(shí)施例的限定。

實(shí)施例1

本發(fā)明提供機(jī)器人機(jī)械臂運(yùn)動(dòng)路徑規(guī)劃方法，其中，所述機(jī)器人包括主機(jī)、機(jī)械臂、微控制器和攝像頭；所述主機(jī)和微控制器連接；所述微控制器連接機(jī)械臂上的舵機(jī)，用于控制機(jī)械臂的運(yùn)動(dòng)；所述主機(jī)連接攝像頭；所述主機(jī)裝有ROS操作系統(tǒng)；所述ROS操作系統(tǒng)中裝有Moveit工具和OMPL(開(kāi)源運(yùn)動(dòng)規(guī)劃庫(kù))；所述主機(jī)為Intel雙核四線(xiàn)程i7工業(yè)微型主機(jī)；所述攝像頭為RGBD深度攝像頭；所述微控制器可以控制舵機(jī)在空間7自由度旋轉(zhuǎn)和運(yùn)動(dòng)。

機(jī)器人運(yùn)動(dòng)路徑規(guī)劃方法，具體過(guò)程為：

1)攝像頭采集周?chē)h(huán)境的深度信息，運(yùn)用ROS操作系統(tǒng)將其轉(zhuǎn)換為空間點(diǎn)云數(shù)據(jù)，然后運(yùn)用OpenCV的特征識(shí)別功能從空間點(diǎn)云數(shù)據(jù)中獲取人體框架和空間姿態(tài)信息；

2)根據(jù)步驟1)獲得的人體位置和空間姿態(tài)信息，機(jī)器人主機(jī)控制電機(jī)的運(yùn)轉(zhuǎn)，機(jī)器人跟隨人體的運(yùn)動(dòng)，實(shí)現(xiàn)人體跟隨的路徑規(guī)劃；機(jī)器人根據(jù)采集的人體位置和空間姿態(tài)信息，調(diào)整電機(jī)的運(yùn)行速度，保持人體與機(jī)器人的距離；機(jī)器人攝像頭中出現(xiàn)多個(gè)人體時(shí)，以不同的數(shù)據(jù)狀態(tài)標(biāo)識(shí)不同人，跟隨指定的人；機(jī)器人沒(méi)有找到跟隨的人體時(shí)，根據(jù)機(jī)器人主機(jī)記載的數(shù)據(jù)，自身進(jìn)行旋轉(zhuǎn)尋找目標(biāo)；

3)機(jī)器人在人體跟隨的運(yùn)動(dòng)過(guò)程中，攝像頭重點(diǎn)標(biāo)記采集人手臂圖像信息，并運(yùn)用ROS操作系統(tǒng)將人手臂圖像信息在特定空間坐標(biāo)系中轉(zhuǎn)換為從肩到手的空間坐標(biāo)；

4)人手臂運(yùn)動(dòng)時(shí)，攝像頭重點(diǎn)標(biāo)記采集人手臂運(yùn)動(dòng)姿態(tài)圖像，將運(yùn)動(dòng)姿態(tài)圖像結(jié)合空間坐標(biāo)在ROS操作系統(tǒng)中轉(zhuǎn)換為機(jī)械臂關(guān)節(jié)的空間姿態(tài)坐標(biāo)；

5)根據(jù)機(jī)械臂關(guān)節(jié)的空間姿態(tài)坐標(biāo)、機(jī)械臂當(dāng)前的坐標(biāo)結(jié)合機(jī)械臂的關(guān)節(jié)位置，開(kāi)源運(yùn)動(dòng)規(guī)劃庫(kù)和Moveit工具規(guī)劃?rùn)C(jī)械臂從當(dāng)前的空間坐標(biāo)運(yùn)動(dòng)到指定的位置坐標(biāo)的運(yùn)動(dòng)軌跡，實(shí)現(xiàn)自由路徑規(guī)劃；若攝像頭拍攝的空間圖像信息中存在空間障礙物，ROS操作系統(tǒng)將空間圖像信息中的障礙物轉(zhuǎn)換為3D空間的點(diǎn)云數(shù)據(jù)；

6)主機(jī)根據(jù)3D空間的點(diǎn)云數(shù)據(jù)信息，控制開(kāi)源運(yùn)動(dòng)規(guī)劃庫(kù)和Moveit工具重新規(guī)劃?rùn)C(jī)械臂的運(yùn)動(dòng)軌跡，從而微處理器控制機(jī)械臂在運(yùn)動(dòng)過(guò)程中避障。

進(jìn)一步地，所述機(jī)械臂根據(jù)步驟5)的路徑規(guī)劃，進(jìn)行移動(dòng)過(guò)程中，ROS操作系統(tǒng)通過(guò)不斷的計(jì)算從機(jī)械臂當(dāng)前的坐標(biāo)到指定的位置坐標(biāo)之間的路徑，給出在運(yùn)動(dòng)過(guò)程中機(jī)械臂各個(gè)關(guān)節(jié)的坐標(biāo)，確認(rèn)機(jī)械臂各個(gè)關(guān)節(jié)的實(shí)際的空間姿態(tài)與路徑規(guī)劃中的空間姿態(tài)之間的坐標(biāo)差值。所述機(jī)械臂各個(gè)關(guān)節(jié)的實(shí)際的空間姿態(tài)與路徑規(guī)劃中的空間姿態(tài)之間的坐標(biāo)差值大于預(yù)先設(shè)定的范圍時(shí)，開(kāi)源運(yùn)動(dòng)規(guī)劃庫(kù)和Moveit工具通過(guò)逆運(yùn)動(dòng)學(xué)解算機(jī)械臂的空間姿態(tài)，從而調(diào)整機(jī)械臂關(guān)節(jié)角度，控制機(jī)械臂的舵機(jī)進(jìn)行轉(zhuǎn)動(dòng)，調(diào)整機(jī)械臂的運(yùn)動(dòng)角度，調(diào)整路徑的偏差。所述機(jī)械臂各個(gè)關(guān)節(jié)的實(shí)際的空間姿態(tài)與路徑規(guī)劃中的空間姿態(tài)之間的坐標(biāo)差值小于預(yù)先設(shè)定的范圍，則主機(jī)發(fā)出運(yùn)動(dòng)終止的信號(hào)，機(jī)械臂完成此次任務(wù)。

進(jìn)一步地，所述機(jī)器人還設(shè)置有預(yù)警裝置，所述步驟4)中攝像頭采集人手臂運(yùn)動(dòng)姿態(tài)圖像時(shí)，攝像頭與人手臂的距離超出預(yù)先設(shè)定的距離時(shí)，進(jìn)行預(yù)警。進(jìn)一步地，所述機(jī)器人還設(shè)置有跟隨模式選擇裝置，用于設(shè)定攝像頭與人手臂的拍攝位置和距離；所述拍攝位置包括：人手臂的左側(cè)方、右側(cè)方和正前方。

進(jìn)一步地，機(jī)器人的底盤(pán)的控制主要由微型主機(jī)連接底盤(pán)微控制器，根據(jù)主機(jī)跟隨人體運(yùn)動(dòng)發(fā)送線(xiàn)速度和角速度指令到微控制器，微控制器將速度指令轉(zhuǎn)換成驅(qū)動(dòng)電機(jī)的PWM進(jìn)行調(diào)速和轉(zhuǎn)向；機(jī)器人的底盤(pán)還包括碰撞傳感器和底部懸空傳感器，來(lái)檢測(cè)地面環(huán)境中的障礙物和極度不平的地面，所述碰撞傳感器能檢測(cè)到地面上高度較低的障礙物，彌補(bǔ)了視覺(jué)攝像頭看不到視角外的障礙物，所述底部懸空傳感器檢測(cè)底盤(pán)到地面的距離，來(lái)解決地面不平帶來(lái)的障礙，有效防止機(jī)器人跌落。

在上述過(guò)程中，機(jī)器人的RGBD攝像頭能捕捉的范圍內(nèi)，人體運(yùn)動(dòng)，機(jī)器人采集圖像信息，并轉(zhuǎn)換為空間點(diǎn)云數(shù)據(jù)，利用OpenCV的特征識(shí)別功能獲取人體框架和空間姿態(tài)信息，從而確定人的位置和人的運(yùn)動(dòng)狀態(tài)；機(jī)器人主機(jī)控制電機(jī)的旋轉(zhuǎn)方向與速度，與人體保持一定的距離，跟隨人體運(yùn)動(dòng)，在這一過(guò)程中，機(jī)器人攝像頭采集周邊的環(huán)境以及人體手臂的工作情況。人揮動(dòng)手臂，機(jī)械臂也隨人手臂的動(dòng)作而動(dòng)作，達(dá)到運(yùn)用機(jī)械臂在人手臂不適宜工作的環(huán)境中工作的用途，例如有腐蝕性、刺激性的工作環(huán)境等。

在跟隨人體和肢體的運(yùn)動(dòng)過(guò)程中，采用RGBD攝像頭根據(jù)跟隨模式選擇裝置的設(shè)定拍攝人手臂圖像，采集人手臂圖像分析后得到人手臂的骨架坐標(biāo)，攝像頭拍攝人手臂的相應(yīng)動(dòng)作，將運(yùn)動(dòng)中的人手臂關(guān)節(jié)姿態(tài)轉(zhuǎn)換為機(jī)械臂關(guān)節(jié)的空間姿態(tài)坐標(biāo)，在運(yùn)動(dòng)中出現(xiàn)障礙物時(shí)，圖像信息用能表示位置和顏色的點(diǎn)群表示即點(diǎn)云數(shù)據(jù)，根據(jù)點(diǎn)云數(shù)據(jù)就能知曉障礙物的位置及形狀大小，ROS操作系統(tǒng)中的開(kāi)源運(yùn)動(dòng)規(guī)劃庫(kù)和Moveit工具通過(guò)運(yùn)算空間姿態(tài)坐標(biāo)和點(diǎn)云數(shù)據(jù)，以及人體和人體手臂的點(diǎn)云數(shù)據(jù)，規(guī)劃?rùn)C(jī)器人和機(jī)械臂的運(yùn)動(dòng)軌跡，實(shí)現(xiàn)機(jī)器人和機(jī)械臂的自由度規(guī)劃和運(yùn)動(dòng)達(dá)到指定的位置，并成功實(shí)現(xiàn)避障。

假設(shè)給定一個(gè)機(jī)械臂結(jié)構(gòu)(假設(shè)有N個(gè)關(guān)節(jié))，給定一個(gè)目標(biāo)(比如終端移到xyz)，給定一個(gè)環(huán)境，開(kāi)源運(yùn)動(dòng)規(guī)劃庫(kù)和Moveit工具在自由路徑規(guī)劃與運(yùn)動(dòng)中規(guī)劃一個(gè)軌跡，包含M個(gè)數(shù)組，每一個(gè)數(shù)組長(zhǎng)度是N，也就是一個(gè)完整的關(guān)節(jié)位置；沿著這個(gè)軌跡依次移動(dòng)關(guān)節(jié)，就可以最終把終端移到xyz，將終端的位置通過(guò)逆運(yùn)動(dòng)學(xué)解算轉(zhuǎn)變成關(guān)節(jié)角度，實(shí)現(xiàn)舵機(jī)角度的轉(zhuǎn)動(dòng)，從而控制機(jī)械臂的運(yùn)動(dòng)角度。若在機(jī)械臂關(guān)節(jié)移動(dòng)過(guò)程中，出現(xiàn)機(jī)械臂各個(gè)關(guān)節(jié)的實(shí)際的空間姿態(tài)與路徑規(guī)劃中的空間姿態(tài)之間的坐標(biāo)差值大于預(yù)先設(shè)定的范圍時(shí)，開(kāi)源運(yùn)動(dòng)規(guī)劃庫(kù)和Moveit工具通過(guò)逆運(yùn)動(dòng)學(xué)解算機(jī)械臂的空間姿態(tài)，從而調(diào)整機(jī)械臂關(guān)節(jié)角度，控制機(jī)械臂的舵機(jī)進(jìn)行轉(zhuǎn)動(dòng)，調(diào)整機(jī)械臂的運(yùn)動(dòng)角度，調(diào)整路徑的偏差；出現(xiàn)機(jī)械臂各個(gè)關(guān)節(jié)的實(shí)際的空間姿態(tài)與路徑規(guī)劃中的空間姿態(tài)之間的坐標(biāo)差值小于預(yù)先設(shè)定的范圍，則主機(jī)發(fā)出運(yùn)動(dòng)終止的信號(hào)，機(jī)械臂完成此次任務(wù)。若攝像頭采集人手臂運(yùn)動(dòng)姿態(tài)圖像時(shí)，攝像頭與人手臂的距離超出預(yù)先設(shè)定的距離時(shí)，進(jìn)行預(yù)警。

若機(jī)器人視線(xiàn)中出現(xiàn)不同的人體，則采用不同的數(shù)據(jù)狀態(tài)進(jìn)行標(biāo)識(shí)，跟隨指定的人體的運(yùn)動(dòng)，并與人體保持一定的距離，例如40cm；若在運(yùn)動(dòng)過(guò)程中，失去跟隨的人體目標(biāo)，則根據(jù)主機(jī)內(nèi)存儲(chǔ)的相關(guān)信息，機(jī)器人自身進(jìn)行旋轉(zhuǎn)并尋找目標(biāo)；在旋轉(zhuǎn)兩周后仍未找到目標(biāo)，則發(fā)出警報(bào)進(jìn)行預(yù)警，尋求幫助，并停止工作。

以上所述僅為本發(fā)明的優(yōu)選實(shí)施例而已，并不用于限制本發(fā)明，對(duì)于本領(lǐng)域的技術(shù)人員來(lái)說(shuō)，本發(fā)明可以有各種更改和變化。凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。