【
技術(shù)領(lǐng)域：
】本發(fā)明涉及自動(dòng)化控制技術(shù)，尤其涉及一種避障任務(wù)無(wú)關(guān)人工勢(shì)場(chǎng)引導(dǎo)的避障路徑規(guī)劃方法。
背景技術(shù)：
：路徑規(guī)劃算法的計(jì)算量取決于任務(wù)、環(huán)境的復(fù)雜性以及對(duì)規(guī)劃路徑質(zhì)量的要求，一個(gè)好的路徑規(guī)劃算法應(yīng)該兼顧對(duì)規(guī)劃速度和路徑質(zhì)量；另一方面，從航天任務(wù)操作環(huán)境的特點(diǎn)出發(fā)，在完成避障規(guī)劃任務(wù)的同時(shí)，如何降低機(jī)械臂其使用代價(jià)，也是避障規(guī)劃方法需要解決的難點(diǎn)問(wèn)題之一。依據(jù)機(jī)器人對(duì)環(huán)境信息的把握程度可把避障路徑規(guī)劃劃分為基于先驗(yàn)完全信息的全局路徑規(guī)劃和基于傳感器信息的局部路徑規(guī)劃。一般來(lái)說(shuō)，局部路徑規(guī)劃算法由傳感器實(shí)時(shí)采集環(huán)境信息，了解環(huán)境地圖信息，然后確定出所在地圖的位置及其局部的障礙物分布情況，從而進(jìn)行實(shí)時(shí)路徑規(guī)劃。該類(lèi)方法由于獲取的環(huán)境信息有限，所以容易陷入局部最小。全局路徑規(guī)劃算法可分為兩類(lèi)，一類(lèi)是基于環(huán)境地圖的路徑規(guī)劃算法，該類(lèi)算法首先需要建立環(huán)境地圖，然后進(jìn)行無(wú)障礙路徑搜索，但在高維空間建立環(huán)境地圖十分困難，所以不適用于冗余度機(jī)械臂避障路徑規(guī)劃；另一類(lèi)方法是基于隨機(jī)采樣的路徑規(guī)劃算法，該類(lèi)算法可以通過(guò)在機(jī)械臂關(guān)節(jié)空間進(jìn)行采樣來(lái)避免建立顯式的機(jī)械臂無(wú)障礙關(guān)節(jié)空間，適用于高維空間的路徑規(guī)劃，但該類(lèi)算法多數(shù)沒(méi)有對(duì)路徑進(jìn)行優(yōu)化，或者優(yōu)化效率低下。同時(shí)，對(duì)于全局路徑規(guī)劃而言，在環(huán)境發(fā)生改變時(shí)，很難快速做出反應(yīng)，一般需要通過(guò)與局部避障路徑規(guī)劃算法來(lái)完成任務(wù)。綜上所述，對(duì)空間機(jī)械臂的避障任務(wù)進(jìn)行算法改進(jìn)是非常重要的，一方面為保證規(guī)劃得出的路徑質(zhì)量，同時(shí)為使全局規(guī)劃算法能夠與局部算法有效結(jié)合，就需要使規(guī)劃出的路徑遠(yuǎn)離奇異位形，并且在規(guī)劃過(guò)程中考慮機(jī)械臂的使用代價(jià)；另一方面從路徑規(guī)劃的效率出發(fā)，希望機(jī)械臂能以較高的效率完成規(guī)定的操作程序，降低路徑規(guī)劃的時(shí)間代價(jià)。技術(shù)實(shí)現(xiàn)要素：有鑒于此，本發(fā)明實(shí)施例提出了一種避障任務(wù)無(wú)關(guān)人工勢(shì)場(chǎng)引導(dǎo)的避障路徑規(guī)劃方法，以降低機(jī)械臂的使用代價(jià)，提升與局部避障路徑規(guī)劃算法結(jié)合的能力，提升避障路徑規(guī)劃的效率。本發(fā)明實(shí)施例提出了一種避障任務(wù)無(wú)關(guān)人工勢(shì)場(chǎng)引導(dǎo)的避障路徑規(guī)劃方法，包括：考慮空間機(jī)械臂使用代價(jià)，選用機(jī)械臂能耗、末端速度、摩擦磨損作為優(yōu)化目標(biāo)，在此基礎(chǔ)上提出一種引入速度極限的最小奇異值指標(biāo)；考慮所述引入速度極限的雅各比矩陣最小奇異值、機(jī)械臂可操作度、條件數(shù)、關(guān)節(jié)極限，提出一種考慮機(jī)械臂操作代價(jià)的梯度勢(shì)場(chǎng)離線構(gòu)建方法；使用構(gòu)建的梯度勢(shì)場(chǎng)引導(dǎo)rrt算法中的隨機(jī)樹(shù)進(jìn)行拓展，規(guī)劃得出空間機(jī)械臂的無(wú)障礙運(yùn)行路徑。上述方法中，考慮空間機(jī)械臂使用代價(jià)，選用機(jī)械臂能耗、末端速度、摩擦磨損作為優(yōu)化目標(biāo)，在此基礎(chǔ)上提出一種引入速度極限的最小奇異值指標(biāo)，包括：考慮空間機(jī)械臂使用代價(jià)，選用機(jī)械臂能耗、末端速度、摩擦磨損作為優(yōu)化目標(biāo)，在此基礎(chǔ)上提出的引入速度極限的最小奇異值指標(biāo)為：其中，σi為機(jī)械臂雅克比矩陣奇異值，i＝1，2，3，...，n，n為機(jī)械臂自由度數(shù)；當(dāng)某幾個(gè)奇異值σj遠(yuǎn)小于其他奇異值σk時(shí)，即σj＜＜σk，j＝n，n-1，..，t，k＝t-1，t-2，..，2，1，對(duì)所有σj，j＝n，n-1，..，t，計(jì)算σjvj′tvj；其中vj為將雅克比矩陣奇異值分解后，j(q)＝u∑vt，矩陣v中對(duì)應(yīng)σj的第j個(gè)列向量，vj′為vj方向上的一個(gè)向量，vj′的每一分量都小于關(guān)節(jié)角速度極限，且vj′的某一分量等于關(guān)節(jié)角速度極限；σjmvjm′tvjm為所有σjvj′tvj中的最小值，j＝n，n-1，..，t。上述方法中，考慮所述引入速度極限的雅各比矩陣最小奇異值、機(jī)械臂可操作度、條件數(shù)、關(guān)節(jié)極限，提出一種考慮機(jī)械臂操作代價(jià)的梯度勢(shì)場(chǎng)的離線構(gòu)建方法，包括：考慮所述引入速度極限的雅各比矩陣最小奇異值、機(jī)械臂可操作度、條件數(shù)、關(guān)節(jié)極限，離線構(gòu)建一種考慮機(jī)械臂操作代價(jià)的梯度勢(shì)場(chǎng)，包括：由所述引入速度極限的雅各比矩陣最小奇異值，建立考慮最小奇異值及關(guān)節(jié)速度極限的勢(shì)場(chǎng)表達(dá)式e1，為：式中，α1和β1為任意正數(shù)，用于調(diào)節(jié)勢(shì)場(chǎng)幅值與作用范圍；k為任意正數(shù)，表示任務(wù)要求末端速度大??；k′為所述σjvj′tvj的最小值σjmvjm′tvjm；為機(jī)械臂所有關(guān)節(jié)中最小的速度極限；σr為機(jī)械臂雅克比矩陣最小奇異值；考慮可操作度建立的勢(shì)場(chǎng)表達(dá)式e2如下：其中ω為機(jī)械臂可操作度，κ為條件數(shù)；α2和β2為任意正數(shù)，α2用于調(diào)整勢(shì)函數(shù)的幅值，β2用于調(diào)整引力勢(shì)場(chǎng)e2的作用范圍；由關(guān)節(jié)極限建立的勢(shì)場(chǎng)為e3：.其中，α為任意正數(shù)；n為機(jī)械臂自由度數(shù)；qi為第i個(gè)關(guān)節(jié)的關(guān)節(jié)角，qimin和qimax分別為第i個(gè)關(guān)節(jié)角的最小值和最大值；將所述三個(gè)勢(shì)場(chǎng)合并，所述合勢(shì)場(chǎng)定義e(q)為：e(q)＝e1+e2+e3計(jì)算得出勢(shì)場(chǎng)的梯度表達(dá)式：通過(guò)上式的離散形式，能夠求出n個(gè)梯度勢(shì)場(chǎng)，其中關(guān)于q1的所述梯度勢(shì)場(chǎng)表達(dá)式為：至均可通過(guò)類(lèi)似表達(dá)式求得。上述方法中，使用構(gòu)建的梯度勢(shì)場(chǎng)引導(dǎo)rrt算法中的隨機(jī)樹(shù)進(jìn)行拓展，規(guī)劃得出空間機(jī)械臂的運(yùn)行路徑，包括：將所述離線建立的梯度勢(shì)場(chǎng)融入rrt算法，在每一次拓展時(shí)使隨機(jī)樹(shù)有概率朝向梯度下降的方向拓展；所述一種考慮使用代價(jià)優(yōu)化的避障任務(wù)無(wú)關(guān)人工勢(shì)場(chǎng)引導(dǎo)的rrt改進(jìn)算法，算法中涉及符號(hào)表示如下：qx和cx：分別表示某構(gòu)型空間位置及其對(duì)應(yīng)笛卡爾坐標(biāo)系位置，下標(biāo)x用于說(shuō)明二者之間的對(duì)應(yīng)關(guān)系；t：為一系列節(jié)點(diǎn)node的集合；隨機(jī)樹(shù)t包含以下幾個(gè)屬性：(1)根節(jié)點(diǎn)noderoot；(2)構(gòu)型節(jié)點(diǎn)集node：包含一系列節(jié)點(diǎn)node的集合，按其rank由高到低排列；node：構(gòu)成隨機(jī)樹(shù)的基本單元，每個(gè)節(jié)點(diǎn)都包含以下幾個(gè)屬性：(1)node.q：節(jié)點(diǎn)node代表的構(gòu)型位置；(2)node.rank：節(jié)點(diǎn)node的rank值，rank值越大說(shuō)明構(gòu)型node.q處勢(shì)場(chǎng)值較小且距離障礙物較遠(yuǎn)，rank的計(jì)算方法如下：其中，d1為機(jī)械臂末端在當(dāng)前構(gòu)型下距目標(biāo)位置的距離；d2為機(jī)械臂當(dāng)前構(gòu)型下距離障礙物的距離；dsafe為具體避障規(guī)劃任務(wù)限定的機(jī)械臂距離障礙物的安全距離；(3)node.fcount：用于記錄隨機(jī)樹(shù)節(jié)點(diǎn)node拓展失敗的次數(shù)；(4)node.parent：構(gòu)型node的父節(jié)點(diǎn)，隨機(jī)樹(shù)t的根節(jié)點(diǎn)noderoot的父節(jié)點(diǎn)為；(5)node.child：構(gòu)型node的子節(jié)點(diǎn)；qset：qset用于記錄進(jìn)行過(guò)碰撞檢測(cè)的構(gòu)型位置及其距離障礙物的距離，用于估計(jì)新采樣構(gòu)型距障礙物的距離；d3：隨機(jī)樹(shù)的最小拓展步長(zhǎng)；算法中使用的函數(shù)如下：t.init(nodeinit)：初始化隨機(jī)樹(shù)t；t.add(q1，node2)：向隨機(jī)樹(shù)t中加入以q1為構(gòu)型的節(jié)點(diǎn)，node2為該節(jié)點(diǎn)的父節(jié)點(diǎn)；t.random_pick(n)：從隨機(jī)樹(shù)t中依rank值選出對(duì)應(yīng)的構(gòu)型節(jié)點(diǎn)node，n用于調(diào)整高rank值節(jié)點(diǎn)被選出的概率，n越大，該概率越高；t.remove(node)：從隨機(jī)樹(shù)t中移除構(gòu)型節(jié)點(diǎn)node，及其所有子節(jié)點(diǎn)；t.closet(q)：從隨機(jī)樹(shù)t中選出距離構(gòu)型q最近的節(jié)點(diǎn)；t.judgefcount(node)：判斷節(jié)點(diǎn)node拓展失敗次數(shù)是否超過(guò)閾值，如果超過(guò)，刪除該節(jié)點(diǎn)；random_sample(n)：在構(gòu)型空間中隨機(jī)選取n個(gè)構(gòu)型q；random_num()：產(chǎn)生[0，1]區(qū)間內(nèi)的隨機(jī)數(shù)；lowest_potential(a)：從集合a中選出勢(shì)場(chǎng)值最低的構(gòu)型；get_config(q，d)：給定當(dāng)前構(gòu)型位置，確定新節(jié)點(diǎn)的位置，方向隨機(jī)；get_alpha(q，d)：給定拓展方向，計(jì)算該方向上隨機(jī)樹(shù)拓展的最大步長(zhǎng)；collision_detection(q)：對(duì)處于構(gòu)型q下的機(jī)械臂進(jìn)行碰撞檢測(cè)，返回距離障礙物的最近距離，若發(fā)生碰撞則返回0；grad(q)：返回構(gòu)型q處的勢(shì)場(chǎng)梯度；所述避障任務(wù)無(wú)關(guān)人工勢(shì)場(chǎng)引導(dǎo)的rrt改進(jìn)算法如下所示：所述算法中，case1的偽代碼如下所示：所述算法中，case2的偽代碼如下所示：由上述算法能夠?yàn)闄C(jī)械臂規(guī)劃出一條無(wú)障礙路徑；所述運(yùn)行路徑即為由避障任務(wù)無(wú)關(guān)人工勢(shì)場(chǎng)引導(dǎo)的rrt改進(jìn)算法得出的路徑；所述路徑能夠使空間機(jī)械臂在完成避障路徑規(guī)劃問(wèn)題并保證路徑規(guī)劃效率的前提下降低機(jī)械臂使用代價(jià)，提升運(yùn)行過(guò)程中與局部路徑規(guī)劃算法結(jié)合的能力?！靖綀D說(shuō)明】為了更清楚地說(shuō)明本發(fā)明實(shí)施例的技術(shù)方案，下面將對(duì)實(shí)施例中所需要使用的附圖作簡(jiǎn)單地介紹，顯而易見(jiàn)地，下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例，對(duì)于本領(lǐng)域普通技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)性的前提下，還可以根據(jù)這些附圖獲得其它的附圖。圖1是本發(fā)明實(shí)施例所提出的避障任務(wù)無(wú)關(guān)人工勢(shì)場(chǎng)引導(dǎo)的避障路徑規(guī)劃方法的流程示意圖；圖2是二自由度機(jī)械臂關(guān)節(jié)速度極限示意圖；圖3是利用本發(fā)明實(shí)施例所提出的避障任務(wù)無(wú)關(guān)人工勢(shì)場(chǎng)引導(dǎo)的避障路徑規(guī)劃算法的流程圖；圖4是基于knn的碰撞檢測(cè)策略流程示意圖；圖5是本發(fā)明實(shí)施例中八自由度空間機(jī)械臂dh坐標(biāo)系示意圖；圖6是本發(fā)明實(shí)施例中八自由度機(jī)械臂仿真模型；圖7是利用本發(fā)明實(shí)施例所提出的避障任務(wù)無(wú)關(guān)人工勢(shì)場(chǎng)引導(dǎo)的避障路徑規(guī)劃算法規(guī)劃得出的無(wú)障礙路徑示意圖?！揪唧w實(shí)施方式】為了更好的理解本發(fā)明的技術(shù)方案，下面結(jié)合附圖對(duì)本發(fā)明實(shí)施例進(jìn)行詳細(xì)描述。應(yīng)當(dāng)明確，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例。基于本發(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒(méi)有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其它實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。避障任務(wù)無(wú)關(guān)人工勢(shì)場(chǎng)引導(dǎo)的避障路徑規(guī)劃方法主要包括三個(gè)部分：考慮空間機(jī)械臂使用代價(jià)，選用機(jī)械臂能耗、末端速度、摩擦磨損作為優(yōu)化目標(biāo)，在此基礎(chǔ)上提出一種引入速度極限的最小奇異值指標(biāo)；考慮所述引入速度極限的雅各比矩陣最小奇異值、機(jī)械臂可操作度、條件數(shù)、關(guān)節(jié)極限，提出一種考慮機(jī)械臂操作代價(jià)的梯度勢(shì)場(chǎng)離線構(gòu)建方法；使用構(gòu)建的梯度勢(shì)場(chǎng)引導(dǎo)rrt算法中的隨機(jī)樹(shù)進(jìn)行拓展，規(guī)劃得出空間機(jī)械臂的無(wú)障礙運(yùn)行路徑。本發(fā)明實(shí)施例給出一種避障任務(wù)無(wú)關(guān)人工勢(shì)場(chǎng)引導(dǎo)的避障路徑規(guī)劃方法，請(qǐng)參考圖1，其為本發(fā)明實(shí)施例所提出的避障任務(wù)無(wú)關(guān)人工勢(shì)場(chǎng)引導(dǎo)的避障路徑規(guī)劃方法的流程示意圖，如圖1所示，該方法包括以下步驟：步驟101，考慮空間機(jī)械臂使用代價(jià)，選用機(jī)械臂能耗、末端速度、摩擦磨損作為優(yōu)化目標(biāo)，在此基礎(chǔ)上提出一種引入速度極限的最小奇異值指標(biāo)。具體的，在完成避障路徑規(guī)劃的前提下，應(yīng)當(dāng)盡量減小使用不當(dāng)給機(jī)械臂帶來(lái)的損耗，延緩其部件性能衰減，在這一過(guò)程中，機(jī)械臂的使用代價(jià)可以由軸承間隙、齒輪位置誤差、軸承運(yùn)動(dòng)誤差等影響因素來(lái)表征。這些影響因素能夠通過(guò)等中間響應(yīng)層因素間接反映。所以選用機(jī)械臂能耗、末端速度、摩擦磨損等間接因素作為優(yōu)化目標(biāo)。機(jī)械臂能耗與關(guān)節(jié)角速度的關(guān)系可以由下式表示：其中，為第i個(gè)關(guān)節(jié)的角速度；τi(t)為第i個(gè)關(guān)節(jié)的關(guān)節(jié)力矩；t0為任務(wù)開(kāi)始時(shí)間；tf為任務(wù)終止時(shí)間。機(jī)械臂雅各比矩陣的最小奇異值能夠用來(lái)作為衡量所需關(guān)節(jié)速度上限的指標(biāo)，即：σr為雅各比矩陣最小奇異值，為機(jī)械臂末端速度，為機(jī)械臂關(guān)節(jié)角速度；上式表明，在任務(wù)所需末端速度不變的情況下，σr越小，則需求的越大；所以從降低能耗的角度出發(fā)，應(yīng)當(dāng)使雅各比矩陣最小奇異值盡可能大；另一方面，從與局部路徑規(guī)劃算法的結(jié)合方面出發(fā)，σr越大，雅各比矩陣越遠(yuǎn)離病態(tài)，當(dāng)使用偽逆法求解機(jī)械臂關(guān)節(jié)角速度時(shí)，精度越高；反之，當(dāng)機(jī)械臂處于奇異位型時(shí)，偽逆法求解得到的關(guān)節(jié)角速度趨于無(wú)窮，無(wú)法正常運(yùn)作。機(jī)械臂可操作度定義為：其中，j(q)為機(jī)械臂在構(gòu)型q處的雅各比矩陣；在末端速度一定的情況下，可操作度越大，則需求的關(guān)節(jié)角速度相對(duì)越小；與最小奇異值指標(biāo)不同的是，可操作度衡量了機(jī)械臂各個(gè)操作方向上的靈敏度。此外，當(dāng)機(jī)械臂關(guān)節(jié)長(zhǎng)時(shí)間處于關(guān)節(jié)極限位置時(shí)，會(huì)加劇關(guān)節(jié)的磨損。所以應(yīng)當(dāng)在任務(wù)完成的前提下，使機(jī)械臂關(guān)節(jié)遠(yuǎn)離極限位置。綜上所述，選取機(jī)械臂雅各比矩陣最小奇異值，機(jī)械臂可操作度，機(jī)械臂關(guān)節(jié)極限等指標(biāo)作為路徑規(guī)劃時(shí)考慮的優(yōu)化目標(biāo)。通過(guò)在路徑規(guī)劃過(guò)程中選擇相應(yīng)的構(gòu)型節(jié)點(diǎn)，完成對(duì)這些指標(biāo)進(jìn)行優(yōu)化，一方面降低機(jī)械臂使用代價(jià)，另一方面使機(jī)械臂在避障路徑規(guī)劃過(guò)程中能與局部避障算法有效的結(jié)合。將機(jī)械臂雅各比矩陣j(q)mxn進(jìn)行奇異值分解，得到：其中，∑＝utj(q)v＝diag(σ1(q)，σ2(q)，...，σr(q))，最小奇異值為：σr(q)上式可以寫(xiě)做：j(q)＝u1σ1v1t+u2σ2v2t+…+umσmvmt則有可將視為將關(guān)節(jié)角速度向rnx1空間的一組基v1，v2，v3，...，vn進(jìn)行投影，上式可以理解為在rmx1空間內(nèi)將u1，u2，u3，...，um進(jìn)行線性組合，若存在某奇異值σi＝0，則無(wú)論關(guān)節(jié)角速度如何變化，得出的機(jī)械臂末端速度在ui方向都不會(huì)存在速度分量；同理，若存在奇異值σi→0，要在ui方向產(chǎn)生速度分量，則需要在機(jī)械臂在執(zhí)行任務(wù)的過(guò)程中，有時(shí)希望機(jī)械臂末端在任意方向上的速度大小都能夠達(dá)到k，以確保機(jī)械臂能夠完成任務(wù)，此時(shí)僅對(duì)最小奇異值σr加以約束不能夠保證滿足該條件，應(yīng)當(dāng)同時(shí)考慮關(guān)節(jié)速度極限在v1，v2，v3，...，vn上的分量，如圖2所示。為便于說(shuō)明，以一個(gè)二自由度機(jī)械臂為模型進(jìn)行解釋。如圖2所示，為角速度極限范圍內(nèi)的一個(gè)關(guān)節(jié)角速度，v1和v2為jtj的一組特征向量，在r2空間內(nèi)為一組標(biāo)準(zhǔn)正交基，其對(duì)應(yīng)的特征向量分別為σ1與σ2，其中σ1＞σ2，但所以不能確定與之間的具體大小關(guān)系，也就無(wú)法確定和之間大小關(guān)系。即僅確保最小奇異值方向的速度kur對(duì)應(yīng)的關(guān)節(jié)角速度滿足角速度約束，并不一定保證其他方向的角速度滿足約束。在各關(guān)節(jié)角速度極限近似相等，某奇異值遠(yuǎn)小于其他奇異值σr＜＜σii＝1，2，3...，r-1時(shí)?？紤]極限情況，若最小奇異值對(duì)應(yīng)的關(guān)節(jié)速度方向能取到關(guān)節(jié)速度極限的最大值，即且次最小奇異值方向?qū)?yīng)的關(guān)節(jié)速度方向vr-1同時(shí)取到關(guān)節(jié)速度極限最小值，在時(shí)，不妨取其中m和n均為常數(shù)。mvr在vr上的投影為nvr-1在vr-1上的投影為n為機(jī)械臂自由度數(shù)。若要求由σrm＞k能導(dǎo)出σr-1n＞k，只需σr-1n＞σrm，即若機(jī)械臂自由度n＝8，則即可。即對(duì)于一個(gè)8自由度機(jī)械臂，在情況下，只要滿足：則其他方向末端速度均能達(dá)到要求速度大小k。式中，σr為雅各比矩陣最小奇異值，vr為σr對(duì)應(yīng)的特征向量；vr′為vr方向上的一個(gè)向量，vr′的每一分量都小于關(guān)節(jié)角速度極限，且vr′的某一分量取到關(guān)節(jié)角速度極限；k為任務(wù)要求的最低末端速度，需要留出一定余量。在各關(guān)節(jié)角速度極限近似相等，某幾個(gè)奇異值遠(yuǎn)小于其他奇異值σj＜＜σi時(shí)，則對(duì)于較小的幾個(gè)奇異值，分別計(jì)算σjvj′tvj，取其中的最小值σjmvjm′tvjm＞k，即保證即可。步驟102，考慮所述引入速度極限的雅各比矩陣最小奇異值、機(jī)械臂可操作度、條件數(shù)、關(guān)節(jié)極限，提出一種考慮機(jī)械臂操作代價(jià)的梯度勢(shì)場(chǎng)離線構(gòu)建方法。由上述討論可建立考慮最小奇異值及關(guān)節(jié)速度極限的勢(shì)場(chǎng)表達(dá)式e1，為：式中，α1和β1為任意正數(shù)，用于調(diào)節(jié)勢(shì)場(chǎng)幅值與作用范圍；k為任意正數(shù)，表示任務(wù)要求末端速度大小；k′為步驟101中σjvj′tvj的最小值σjmvjm′tvjm；為機(jī)械臂所有關(guān)節(jié)中最小的速度極限；σr為機(jī)械臂雅克比矩陣最小奇異值；當(dāng)遠(yuǎn)大于k時(shí)，說(shuō)明σr足夠大，此時(shí)e1→0；只有當(dāng)接近k時(shí)，才考慮k-k′，；當(dāng)k′遠(yuǎn)離k時(shí)，e1增大，產(chǎn)生斥力，使機(jī)械臂遠(yuǎn)離該構(gòu)型位置。謝碧云等人于2010年提出一種基于條件數(shù)約束的方向可操作度(dmccn)，并以此為路徑規(guī)劃過(guò)程中的優(yōu)化目標(biāo)，對(duì)平面3r機(jī)械臂進(jìn)行仿真實(shí)驗(yàn)。該指標(biāo)當(dāng)條件數(shù)較小時(shí)，指標(biāo)主要受方向可操作度的影響；條件數(shù)不斷增大時(shí)，方向可操作度對(duì)指標(biāo)的影響逐漸削弱，條件數(shù)對(duì)指標(biāo)的影響不斷增大。dmccn的定義為：式中，為條件數(shù)，定義為雅各比矩陣j(q)最大奇異值與最小奇異值的比值；κmax為任務(wù)要求的最大條件數(shù)；u為沿末端運(yùn)動(dòng)速度方向的單位矢量；定義為機(jī)械臂的方向可操作度；α為一正數(shù)，用于調(diào)節(jié)條件數(shù)及方向可操作度的靈敏度。可以將出現(xiàn)較大條件數(shù)的情況分為兩種，σmin較小的情況在依最小奇異值建立勢(shì)場(chǎng)時(shí)已經(jīng)加以考慮；σmax較大對(duì)機(jī)械臂末端運(yùn)動(dòng)沒(méi)有影響。并且，對(duì)于可操作度ω較小的情況在之前最小奇異值建立勢(shì)場(chǎng)時(shí)也加以考慮，故此時(shí)只考慮可操作度較大的情況。所以在考慮機(jī)械臂末端各方向運(yùn)動(dòng)能力時(shí)，期望機(jī)械臂朝向條件數(shù)較小且可操作度較大的構(gòu)型移動(dòng)。由于勢(shì)場(chǎng)與任務(wù)無(wú)關(guān)，與dmccn不同，更多的考慮可操作度的各向同性，表達(dá)式如下：考慮可操作度建立的勢(shì)場(chǎng)表達(dá)式e2如下：其中ω為機(jī)械臂可操作度，κ為條件數(shù)；α2和β2為任意正數(shù)，α2用于調(diào)整勢(shì)函數(shù)的幅值，β2用于調(diào)整引力勢(shì)場(chǎng)e2的作用范圍；冗余度空間機(jī)械臂在關(guān)節(jié)極限范圍內(nèi)，其末端執(zhí)行器能夠到達(dá)工作空間中的任意目標(biāo)位置，機(jī)械臂關(guān)節(jié)極限可以表示為：qimin≤qi≤qimaxi＝1，2，..，n其中，qimin為第i個(gè)關(guān)節(jié)的關(guān)節(jié)角下限，qimax為第i個(gè)關(guān)節(jié)的關(guān)節(jié)角上限，n為機(jī)械臂自由度數(shù)。由以上分析可知，希望使機(jī)械臂在運(yùn)行過(guò)程中盡量遠(yuǎn)離關(guān)節(jié)角上下限，且當(dāng)機(jī)械臂遠(yuǎn)離關(guān)節(jié)限位時(shí)，對(duì)機(jī)械臂的運(yùn)行無(wú)影響。由關(guān)節(jié)極限建立的勢(shì)場(chǎng)為e3：.其中，α為任意正數(shù)；n為機(jī)械臂自由度數(shù)；qi為第i個(gè)關(guān)節(jié)的關(guān)節(jié)角，qimin和qimax分別為第i個(gè)關(guān)節(jié)角的最小值和最大值；由上式得知，在當(dāng)前關(guān)節(jié)位置距關(guān)節(jié)角下限較近時(shí)，dimin值較大，dimax值較小，勢(shì)場(chǎng)主要由式中的第一項(xiàng)決定；若當(dāng)前關(guān)節(jié)位置距關(guān)節(jié)角上下限都較遠(yuǎn)時(shí)，dimin和dimax的值都較小，勢(shì)場(chǎng)趨近于0。將以上三個(gè)勢(shì)場(chǎng)合并，共同作用于隨機(jī)樹(shù)的拓展，以實(shí)現(xiàn)對(duì)優(yōu)化目標(biāo)的優(yōu)化。將所述三個(gè)勢(shì)場(chǎng)合并，所述合勢(shì)場(chǎng)定義e(q)為：e(q)＝e1+e2+e3計(jì)算得出勢(shì)場(chǎng)的梯度表達(dá)式：通過(guò)上式的離散形式，能夠求出n個(gè)梯度勢(shì)場(chǎng)，其中關(guān)于q1的所述梯度勢(shì)場(chǎng)表達(dá)式為：至均可通過(guò)類(lèi)似表達(dá)式求得。步驟103，使用構(gòu)建的梯度勢(shì)場(chǎng)引導(dǎo)rrt算法中的隨機(jī)樹(shù)進(jìn)行拓展，規(guī)劃得出空間機(jī)械臂的無(wú)障礙運(yùn)行路徑。算法中涉及符號(hào)表示如下：qx和cx：分別表示某構(gòu)型空間位置及其對(duì)應(yīng)笛卡爾坐標(biāo)系位置，下標(biāo)x用于說(shuō)明二者之間的對(duì)應(yīng)關(guān)系；t：為一系列節(jié)點(diǎn)node的集合；隨機(jī)樹(shù)t包含以下幾個(gè)屬性：(1)根節(jié)點(diǎn)noderoot；(2)構(gòu)型節(jié)點(diǎn)集node：包含一系列節(jié)點(diǎn)node的集合，按其rank由高到低排列；node：構(gòu)成隨機(jī)樹(shù)的基本單元，每個(gè)節(jié)點(diǎn)都包含以下幾個(gè)屬性：(1)node.q：節(jié)點(diǎn)node代表的構(gòu)型位置；(2)node.rank：節(jié)點(diǎn)node的rank值，rank值越大說(shuō)明構(gòu)型node.q處勢(shì)場(chǎng)值較小且距離障礙物較遠(yuǎn)，rank的計(jì)算方法如下：其中，d1為機(jī)械臂末端在當(dāng)前構(gòu)型下距目標(biāo)位置的距離；d2為機(jī)械臂當(dāng)前構(gòu)型下距離障礙物的距離；dsafe為機(jī)械臂距離障礙物的安全距離；由上式可知，只有當(dāng)機(jī)械臂距離障礙物距離小于安全距離時(shí)才對(duì)其進(jìn)行考慮，并且dsafe隨實(shí)際任務(wù)的不同可以進(jìn)行調(diào)整；(3)node.fcount：用于記錄隨機(jī)樹(shù)節(jié)點(diǎn)node拓展失敗的次數(shù)；(4)node.parent：構(gòu)型node的父節(jié)點(diǎn)，隨機(jī)樹(shù)t的根節(jié)點(diǎn)noderoot的父節(jié)點(diǎn)為；(5)node.child：構(gòu)型node的子節(jié)點(diǎn)：qset：qset用于記錄進(jìn)行過(guò)碰撞檢測(cè)的構(gòu)型位置及其距離障礙物的距離，用于估計(jì)新采樣構(gòu)型距障礙物的距離；d3：隨機(jī)樹(shù)的最小拓展步長(zhǎng)；算法中使用的函數(shù)如下：t.init(nodeinit)：初始化隨機(jī)樹(shù)t；t.add(q1，node2)：向隨機(jī)樹(shù)t中加入以q1為構(gòu)型的節(jié)點(diǎn)，node2為該節(jié)點(diǎn)的父節(jié)點(diǎn)；t.random_pick(n)：從隨機(jī)樹(shù)t中依rank值選出對(duì)應(yīng)的構(gòu)型節(jié)點(diǎn)node，n用于調(diào)整高rank值節(jié)點(diǎn)被選出的概率，n越大，該概率越高；t.remove(node)：從隨機(jī)樹(shù)t中移除構(gòu)型節(jié)點(diǎn)node，及其所有子節(jié)點(diǎn)；t.closet(q)：從隨機(jī)樹(shù)t中選出距離構(gòu)型q最近的節(jié)點(diǎn)；t.judgefcount(node)：判斷節(jié)點(diǎn)node拓展失敗次數(shù)是否超過(guò)閾值，如果超過(guò)，刪除該節(jié)點(diǎn)；random_sample(n)：在構(gòu)型空間中隨機(jī)選取n個(gè)構(gòu)型q；random_num()：產(chǎn)生[0，1]區(qū)間內(nèi)的隨機(jī)數(shù)；lowest_potential(a)：從集合a中選出勢(shì)場(chǎng)值最低的構(gòu)型；get_config(q，d)：給定當(dāng)前構(gòu)型位置，確定新節(jié)點(diǎn)的位置，方向隨機(jī)；get_alpha(q，d)：給定拓展方向，計(jì)算該方向上隨機(jī)樹(shù)拓展的最大步長(zhǎng)；collision_detection(q)：對(duì)處于構(gòu)型q下的機(jī)械臂進(jìn)行碰撞檢測(cè)，返回距離障礙物的最近距離，若發(fā)生碰撞則返回0；grad(q)：返回構(gòu)型q處的勢(shì)場(chǎng)梯度；所述算法通過(guò)在路徑規(guī)劃的過(guò)程中引入勢(shì)場(chǎng)，使機(jī)械臂的某些目標(biāo)達(dá)到優(yōu)化，提高與其他局部規(guī)劃算法結(jié)合的能力，同時(shí)，借鑒一種基于knn算法的碰撞檢測(cè)思想，以提升路徑規(guī)劃中碰撞檢測(cè)的效率；所述算法的說(shuō)明可以從隨機(jī)性和確定性兩個(gè)方面進(jìn)行展開(kāi)。算法整體流程如圖3所示。在確定性拓展方面，其目的在于能夠使機(jī)械臂朝向目標(biāo)位置生長(zhǎng)，將確定性拓展的情況記為case1；在case1中，首先，每次從隨機(jī)樹(shù)t中隨機(jī)挑選一個(gè)節(jié)點(diǎn)，擁有較高rank值的節(jié)點(diǎn)被選出的概率較高，某節(jié)點(diǎn)若擁有較高的rank值則表明該節(jié)點(diǎn)距目標(biāo)位置可能較近，同時(shí)機(jī)械臂在該節(jié)點(diǎn)處有較好的運(yùn)動(dòng)性能并且距離障礙物較遠(yuǎn)；其次，對(duì)于選出的節(jié)點(diǎn)，使用隨機(jī)拓展的方式或沿勢(shì)場(chǎng)梯度的方向進(jìn)行拓展，其中沿勢(shì)場(chǎng)梯度下降只在機(jī)械臂距離目標(biāo)位置較遠(yuǎn)時(shí)才使用，這是因?yàn)樵趯?shí)際使用中勢(shì)場(chǎng)間隔較大，所以當(dāng)機(jī)械臂靠近目標(biāo)位置時(shí)，依舊采用梯度下降方向作為引導(dǎo)，可能會(huì)使機(jī)械臂在目標(biāo)位置附近反復(fù)震蕩；最后，對(duì)拓展之后的節(jié)點(diǎn)判斷其距離笛卡爾目標(biāo)的距離，以確保每一次生長(zhǎng)都能夠靠近目標(biāo)位置；case1的偽代碼如下所示：在隨機(jī)拓展方面，目的在于使路徑規(guī)劃算法達(dá)到概率完備，能夠使機(jī)械臂繞開(kāi)障礙物，將隨機(jī)拓展的情況記為case2：在case2中，首先在構(gòu)型空間中隨機(jī)采樣n次，從中選出勢(shì)場(chǎng)值最小的構(gòu)型qh；傳統(tǒng)的隨機(jī)性拓展只從隨機(jī)樹(shù)t中選取選擇距離qh最近的節(jié)點(diǎn)進(jìn)行拓展，在此基礎(chǔ)上以一定概率選取rank值較高的節(jié)點(diǎn)，目的在于在目標(biāo)優(yōu)化的同時(shí)提升算法收斂的能力；將選出的節(jié)點(diǎn)作為noderand，以noderand.q→qh方向?qū)㈦S機(jī)樹(shù)進(jìn)行拓展，并且不考慮新添加節(jié)點(diǎn)距目標(biāo)構(gòu)型的距離，這樣做能夠充分保留算法的“隨機(jī)性”；case2的偽代碼如下所示：此外，本算法還引入基于概率的碰撞檢測(cè)策略與該算法進(jìn)行結(jié)合；原策略通過(guò)記錄每一個(gè)節(jié)點(diǎn)的構(gòu)型的哈希值及碰撞狀態(tài)，當(dāng)?shù)玫阶銐虻臉颖竞?，以knn算法估計(jì)新構(gòu)型的碰撞狀態(tài)；由以上分析可以綜合得出改進(jìn)的rrt算法的整體流程，如下所示：核心函數(shù)偽代碼及說(shuō)明：該函數(shù)中init(qset)用于初始化構(gòu)型節(jié)點(diǎn)及其距障礙物距離，用于之后的碰撞檢測(cè)策略；該函數(shù)主要用于隨機(jī)生成哈希表參數(shù)。用于判斷節(jié)點(diǎn)node的fcount是否高于閾值，若高于閾值，則從樹(shù)t中移除節(jié)點(diǎn)node，且返回1；否則返回0；該函數(shù)用于從隨機(jī)樹(shù)中選出一個(gè)節(jié)點(diǎn)用于拓展。首先從隨機(jī)樹(shù)中選出n-1個(gè)節(jié)點(diǎn)以及t.node[1]，共n個(gè)節(jié)點(diǎn)，再?gòu)倪@n個(gè)節(jié)點(diǎn)中選出一個(gè)節(jié)點(diǎn)；這樣能夠保證距離目標(biāo)最近的節(jié)點(diǎn)每次都有較大概率被選出同時(shí)保留隨機(jī)性；在實(shí)際使用中，可以從隨機(jī)樹(shù)中選出rank值較高的2、3個(gè)節(jié)點(diǎn)，再任意選擇其他節(jié)點(diǎn)，以此提高在靠近目標(biāo)位置附近的解的隨機(jī)性。傳統(tǒng)碰撞檢測(cè)策略在用于rrt，prm等隨機(jī)采樣路徑規(guī)劃算法時(shí)，每產(chǎn)生一個(gè)新的節(jié)點(diǎn)，都需要進(jìn)行一次碰撞檢測(cè)，以確保新加入的節(jié)點(diǎn)沒(méi)有發(fā)生碰撞；pan等人在2016年提出一種基于概率的碰撞檢測(cè)策略，通過(guò)記錄之前查詢點(diǎn)的構(gòu)型位置及碰撞狀態(tài)，來(lái)估計(jì)新構(gòu)型位置的碰撞狀態(tài)，通過(guò)這一策略，能夠減少執(zhí)行碰撞檢測(cè)算法的次數(shù)，以提升路徑規(guī)劃算法的效率。該策略的整體流程如圖4所示。首先，通過(guò)lsh-basedknn算法找出新構(gòu)型位置附近的已查詢的其他構(gòu)型節(jié)點(diǎn)；接著，由“距離條件”判斷這些節(jié)點(diǎn)是否距離新構(gòu)型足夠近，如果足夠近則通過(guò)這些節(jié)點(diǎn)估計(jì)新構(gòu)型的碰撞狀態(tài)，否則執(zhí)行碰撞檢測(cè)；對(duì)于計(jì)算得出的碰撞概率，由“模糊條件”判斷是否足夠確定新構(gòu)型的碰撞結(jié)果，若估計(jì)出的新構(gòu)型碰撞或者不碰撞的可能性較為接近，則說(shuō)明該構(gòu)型可能處于臨界碰撞狀態(tài)，需要執(zhí)行碰撞檢測(cè)；最后將得到的估計(jì)結(jié)果記錄、保存。所述算法主要利用碰撞檢測(cè)算法計(jì)算隨機(jī)樹(shù)拓展的步長(zhǎng)；所以，對(duì)該碰撞檢測(cè)策略做了如下改動(dòng)：(1)首先，在記錄節(jié)點(diǎn)時(shí)，使用距障礙物距離代替碰撞狀態(tài)，以此估計(jì)新構(gòu)型x距離障礙物的距離；(2)其次，在記錄距離時(shí)，只有當(dāng)執(zhí)行碰撞檢測(cè)時(shí)才進(jìn)行記錄，對(duì)于估計(jì)的距離值不進(jìn)行記錄，這是為了保證估計(jì)結(jié)果不會(huì)漂移；(3)最后，通過(guò)對(duì)原算法的分析可知，模糊條件主要是為了在臨界碰撞位置保證機(jī)械臂的安全性，但該條件需要計(jì)算概率，在本算法中并不適用，所以為了保證機(jī)械臂的安全性，只有當(dāng)估計(jì)出的距離大于某一閾值時(shí)才使用估計(jì)結(jié)果，即當(dāng)機(jī)械臂距障礙物較近時(shí)，執(zhí)行碰撞檢測(cè)，以保證機(jī)械臂與障礙物不發(fā)生碰撞。用于具體實(shí)施例中，以一個(gè)八自由度的模塊化機(jī)械臂為具體研究對(duì)象進(jìn)行了仿真及實(shí)物實(shí)驗(yàn)研究，其中本發(fā)明實(shí)施例中八自由度空間機(jī)械臂dh坐標(biāo)系示意圖如圖5所示。表一為空間機(jī)械臂dh參數(shù)表，如下所示：表一連桿iαi-1ai-1diθi100220-π/22-π/20-110-π/23-π/2024504π13590050130-90-π/26-π/2030007π/20008-π/20850機(jī)械臂關(guān)節(jié)角變化范圍：八自由度機(jī)械臂仿真模型如圖6所示。其中機(jī)械臂初始位姿：目標(biāo)位置為：cgoal＝[150300600]由所述算法得出的無(wú)障礙路徑示意圖如圖7所示。由圖中能夠看出，該算法能夠規(guī)劃出一條從初始構(gòu)型到目標(biāo)位置的無(wú)障礙路徑，并且機(jī)械臂在運(yùn)行過(guò)程中與障礙物始終保持一定的安全距離，說(shuō)明該算法的有效性。所述算法的優(yōu)化性能通過(guò)三個(gè)指標(biāo)對(duì)優(yōu)化性能進(jìn)行比較：obj1表示產(chǎn)生末端速度最弱方向的能產(chǎn)生的最高速度的能力；該值越小，說(shuō)明該方向產(chǎn)生末端速度的能力較強(qiáng)；式中num為該路徑上的節(jié)點(diǎn)個(gè)數(shù)，obj1為各個(gè)節(jié)點(diǎn)上(k-k′)的平均；obj2表示機(jī)械臂各個(gè)節(jié)點(diǎn)上可操作度的平均；該值越大，說(shuō)明機(jī)械臂在各個(gè)方向上的運(yùn)動(dòng)能力整體較好，產(chǎn)生同樣的末端速度所需的關(guān)節(jié)角速度較小，在一定程度上能夠降低機(jī)械臂的能耗；obj3表示機(jī)械臂各個(gè)節(jié)點(diǎn)上條件數(shù)的平均；該值同樣用來(lái)衡量機(jī)械臂各方向上的運(yùn)動(dòng)能力，可以看作是對(duì)obj2的補(bǔ)充。表二為原算法規(guī)劃路徑得出的指標(biāo)，如下所示：表二組別第1組第2組第3組第4組第5組第6組第7組第8組obj126.3343.0630.9331.6416.7638.5815.0119.88obj24.453.134.033.894.243.884.325.18obj315.3715.9311.5111.979.3812.6610.0810.59組別第9組第10組第11組第12組第13組第14組第15組平均值obj146.2113.2420.9424.4224.7928.959.4426.01obj23.135.654.234.074.404.125.764.30obj317.287.969.437.7010.1810.58.1611.25表三為所述算法規(guī)劃路徑得出優(yōu)化后的指標(biāo)，如下所示：表三組別第1組第2組第3組第4組第5組第6組第7組第8組obj116.0724.33-3.828.663.2917.4720.9616.65obj25.155.085.795.806.015.514.714.96obj38.8710.666.908.416.218.9910.0513.39組別第9組第10組第11組第12組第13組第14組第15組平均值obj16.6214.4221.9313.154.5314.429.7912.56obj25.944.584.564.985.545.004.905.23obj37.389.5011.898.966.309.3011.489.22由實(shí)驗(yàn)結(jié)果看出，原算法與所述算法在路徑運(yùn)動(dòng)性能優(yōu)化方面，具有較明顯的優(yōu)化效果。由原算法得出的obj1、obj2、obj3的平均值分別為26.01，4.30，11.25由所述算法，經(jīng)過(guò)在同一實(shí)驗(yàn)條件下進(jìn)行15次實(shí)驗(yàn)，得出的obj1、obj2、obj3平均值分別為12.56、5.23和9.22；由obj1、obj2、obj3的特點(diǎn)可知，所述算法對(duì)以上三項(xiàng)指標(biāo)都進(jìn)行了一定的優(yōu)化，obj1減小了13.45，說(shuō)明機(jī)械臂以該路徑運(yùn)行時(shí)，有更高的能力產(chǎn)生較大的末端速度；obj2上升了0.93，obj3下降了2.03說(shuō)明機(jī)械臂使用所述路徑規(guī)劃算法，能使各方向的運(yùn)動(dòng)能力更加均衡，并使各方向整體運(yùn)動(dòng)性能得到提升。在與關(guān)節(jié)角極限的距離方面，使用的指標(biāo)為：m＝min(qidist)qidist＝min(min_dist(qi，qsub)，min_dist(qi，qsup))qsub與qsup分別為關(guān)節(jié)角的下極限與上極限，均為n維向量；min_dist(q，qsub)用來(lái)計(jì)算q與qsub每一個(gè)維度上的距離，并取其中的最小值；式中i為整個(gè)路徑關(guān)節(jié)序列的組數(shù)，m則用于取qidist中的最小值；表四為原算法規(guī)劃路徑得出的與關(guān)節(jié)極限的距離，如下所示：表四組別第1組第2組第3組第4組第5組第6組第7組第8組m0.65290.09640.00430.05330.84520.17460.00920.3971組別第9組第10組第11組第12組第13組第14組第15組平均值m0.06910.78980.01290.00220.22100.01880.76600.2742表五為所述算法規(guī)劃路徑得出的與關(guān)節(jié)極限的距離，如下所示：表五組別第1組第2組第3組第4組第5組第6組第7組第8組m0.76280.73690.57560.88800.78660.65501.27900.6994組別第9組第10組第11組第12組第13組第14組第15組平均值m0.08160.25590.06990.04120.12650.37720.34100.5118若取關(guān)節(jié)角距離極限3度為標(biāo)準(zhǔn)，換算成弧度值為0.052，則由所述算法規(guī)劃得出的路徑在此安全范圍外的只有1組；由原方法得出的路徑在范圍外有5組；取關(guān)節(jié)角極限5度為標(biāo)準(zhǔn)，弧度值為0.087，則所述方法有3組在安全范圍外，原方法有7組；并且，由所述方法規(guī)劃得出的路徑關(guān)節(jié)極限最小值為0.0412，為2～3度之間，原方法得出的最小值為0.0022，該值小于1度。由以上分析得出，所述方法在避關(guān)節(jié)極限方面同樣具有明顯的效果。以上所述僅為本發(fā)明的較佳實(shí)施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)，所做的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明保護(hù)的范圍之內(nèi)。當(dāng)前第1頁(yè)12