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