本發(fā)明涉及機器人的逆運動學領域，具體為由萬向節(jié)串聯而成的超冗余機械臂的基于分段幾何法求解逆運動學的方法及系統。

背景技術：

超冗余機械臂相對于傳統機械臂有更多的自由度，在復雜多障礙的環(huán)境中體現出了極強的靈活性，因此被廣泛應用于核電領域、航天領域大型設備的檢修、維護、救援等任務。運動規(guī)劃是超冗余度機器人機構學研究中的一個重點和難點，然而逆運動學求解是運動規(guī)劃的必要環(huán)節(jié)。通常冗余機械臂的逆運動學求解方法可以分為三類：(1)代數法(2)迭代法(3)幾何法。由于超冗余自由度的存在，超冗余機械臂可以實現一些附加任務如：優(yōu)化關節(jié)速度、回避障礙物、回避奇異構型、回避關節(jié)極限、最小化驅動關節(jié)力矩。

Takegaki將超冗余機械臂的逆運動學問題歸結為確定隨時間變化的脊線構型的問題，基于運動學公式提出了確定機械臂最優(yōu)構型的新方法。Chirikjian針對超冗余機械臂的運動學規(guī)劃提出了脊線法的概念，奠定了超冗余機器人運動學的理論基礎，他采用脊線法來描述超冗余機械臂的宏觀構型，得到了許多重要的結論。Mochiyama采用空間曲線方法針對由雙自由度關節(jié)構成的超冗余剛性連桿機械臂的控制問題開展了深入研究。一些研究者將模式函數法應用到平面超冗余機器人的運動規(guī)劃當中。在平面脊線定義的曲率函數中有一個變量控制脊線在根部的方向，其他變量用來實現障礙物回避。同時也引入了簡單的平面脊線擬合方法即利用脊線的切線作為桿件的方向，這種方法有效降低了末端效應器位置的誤差。人工神經網絡法被廣泛應用于工業(yè)機械臂的逆解，但是所需要訓練集的大小會隨著自由度數目的變化而顯著變化，并且由于學習過程的延長可能無法滿足實時性的要求。

為了降低超冗余機械臂大量自由度引起的高計算量問題，因此該技術有必要進行改進。

技術實現要素：

為了解決上述技術問題，本發(fā)明的目的是提供一種基于分段幾何法求解超冗余機器人逆運動學的方法及系統。

本發(fā)明所采用的技術方案是：

本發(fā)明提供一種基于分段幾何法求解超冗余機器人逆運動學的方法，包括以下步驟：

系統將n自由度機器人的機械臂分段表示為包括肩部、肘部、腕部，其中n>＝8；

系統根據腕部末端節(jié)點T與抓取目標的位置一致，進而求解確定所述肩部的首端節(jié)點S₀、肩部與肘部的交叉節(jié)點E₀以及肘部與腕部的交叉節(jié)點W₀各節(jié)點的位置；

系統根據選取的臂型角求解所述肩部各關節(jié)角度；

系統求解所述肘部各關節(jié)角度；

系統利用肘部與腕部的交叉節(jié)點W₀及腕部末端節(jié)點T求解腕部各關節(jié)角度。

作為該技術方案的改進，所述肩部包括4個自由度，所述腕部包括2個自由度。

作為該技術方案的改進，所述系統利用空間圓弧參數求解所述肘部各關節(jié)角度。

作為該技術方案的改進，其還包括步驟：系統根據所求得的肩部與肘部的交叉節(jié)點E₀以及肘部與腕部的交叉節(jié)點W₀，求得空間圓弧參數α,φ的值。

作為該技術方案的改進，所述步驟系統根據腕部末端節(jié)點T與抓取目標的位置一致，進而求解確定所述肩部的首端節(jié)點S₀、肩部與肘部的交叉節(jié)點E₀以及肘部與腕部的交叉節(jié)點W₀各節(jié)點的位置，其中：

所述肘部與腕部的交叉節(jié)點W₀的位置可表示為，

其中，所述[p_x,p_y,p_z]^T是坐標系Σ_n的原點位置；D為在坐標

系{0}下期望的方向向量；l_e表示腕部最末端連桿的長度。

進一步地，所述肩部與肘部的交叉節(jié)點E₀的位置可表示為：

其中，肩部包括4個自由度，則S₂與E₀重合，所述l表示非末

端的連桿長度，所述n為機器人的總自由度。

進一步地，所述肘部各節(jié)點E₀,E₁,…,E_n/2-3可表示為：

其中，j＝1,…,n/2-3。

進一步地，系統根據求得的肩部與肘部的交叉節(jié)點E₀，并選取一臂型角ψ，進而求得肩部各關節(jié)角度。

另一方面，本發(fā)明還提供一種基于分段幾何法求解超冗余機器人逆運動學的系統，包括：

第一模塊，用于執(zhí)行步驟系統將n自由度機器人的機械臂分段表示為包括肩部、肘部、腕部，其中n>＝8；

第二模塊，用于執(zhí)行步驟系統根據腕部末端節(jié)點T與抓取目標的位置一致，進而求解確定所述肩部的首端節(jié)點S₀、肩部與肘部的交叉節(jié)點E₀以及肘部與腕部的交叉節(jié)點W₀各節(jié)點的位置；

第三模塊，用于執(zhí)行步驟系統根據選取的臂型角求解所述肩部各關節(jié)角度；

第四模塊，用于執(zhí)行步驟系統求解所述肘部各關節(jié)角度；

第五模塊，用于執(zhí)行步驟系統利用肘部與腕部的交叉節(jié)點W₀及腕部末端節(jié)點T求解腕部各關節(jié)角度。

本發(fā)明的有益效果是：本發(fā)明提供的基于分段幾何法求解超冗余機器人逆運動學的方法及系統，通過將多自由度機械臂的機器人運用幾何分段分解為三部分，基于目標位置，根據選取的各關鍵節(jié)點的位置的求解入手，根據超冗余機械臂期望的末端位置及姿態(tài)在3D空間確定超冗余機械臂的合理構型，其有很高的計算效率，方便快捷，且求解唯一，十分精準。

附圖說明

下面結合附圖對本發(fā)明的具體實施方式作進一步說明：

圖1是本發(fā)明一實施例的D-H坐標系示意圖；

圖2是本發(fā)明第二實施例的超冗余機械臂分段規(guī)則示意圖；

圖3是本發(fā)明第三實施例的分段幾何法規(guī)劃算法示意圖；

圖4是本發(fā)明第四實施例的相對于參考平面S₀E₀E₁的臂型角示意圖。

具體實施方式

需要說明的是，在不沖突的情況下，本申請中的實施例及實施例中的特征可以相互組合。

本發(fā)明針對超冗余機械臂的逆運動學求解的難題，提出了一種基于分段幾何法的超冗余機械臂逆運動學求解方法。該方法可以根據超冗余機械臂期望的末端位置及姿態(tài)在3D空間確定超冗余機械臂的合理構型。針對特定任務需求，該方法可以通過調整少量幾何參數獲得所需構型。所有的關節(jié)角度都是通過解析解的方式求解，有很高的計算效率，該方法適用于實時在線控制，同時該方法也適用于多種超冗余構型。

本發(fā)明提供一種基于分段幾何法求解超冗余機器人逆運動學的方法，包括以下步驟：

系統將n自由度機器人的機械臂分段表示為包括肩部、肘部、腕部，其中n＝2a，所述a為大于等于4的整數；因該類機械臂為萬向節(jié)構成的，一個萬向節(jié)由兩個自由度構成。

系統根據腕部末端節(jié)點T與抓取目標的位置一致，進而求解確定所述肩部的首端節(jié)點S₀、肩部與肘部的交叉節(jié)點E₀以及肘部與腕部的交叉節(jié)點W₀各節(jié)點的位置；

系統根據選取的臂型角求解所述肩部各關節(jié)角度；

系統求解所述肘部各關節(jié)角度；

系統利用肘部與腕部的交叉節(jié)點W₀及腕部末端節(jié)點T求解腕部各關節(jié)角度。

作為該技術方案的改進，所述肩部包括4個自由度，所述腕部包括2個自由度。

作為該技術方案的改進，所述系統利用空間圓弧參數求解所述肘部各關節(jié)角度。

作為該技術方案的改進，其還包括步驟：系統根據所求得的肩部與肘部的交叉節(jié)點E₀以及肘部與腕部的交叉節(jié)點W₀，求得空間圓弧參數α,φ的值。

作為該技術方案的改進，所述步驟系統根據腕部末端節(jié)點T與抓取目標的位置一致，進而求解確定所述肩部的首端節(jié)點S₀、肩部與肘部的交叉節(jié)點E₀以及肘部與腕部的交叉節(jié)點W₀各節(jié)點的位置，其中：

所述肘部與腕部的交叉節(jié)點W₀的位置可表示為，

其中，所述[p_x,p_y,p_z]^T是坐標系Σ_n的原點位置；D為在坐標系{0}下期望的方向向量；l_e表示腕部最末端連桿的長度。

進一步地，所述肩部與肘部的交叉節(jié)點E₀的位置可表示為：

其中，肩部包括4個自由度，則S₂與E₀重合，所述l表示非末端的連桿長度，所述n為機器人的總自由度。

進一步地，所述肘部各節(jié)點E₀,E₁,…,E_n/2-3可表示為：

其中，j＝1,…,n/2-3。

進一步地，系統根據求得的肩部與肘部的交叉節(jié)點E₀，并選取一臂型角ψ，進而求得肩部各關節(jié)角度。

另一方面，本發(fā)明還提供一種基于分段幾何法求解超冗余機器人逆運動學的系統，包括：

第一模塊，用于執(zhí)行步驟系統將n自由度機器人的機械臂分段表示為包括肩部、肘部、腕部，其中n>＝8；

第二模塊，用于執(zhí)行步驟系統根據腕部末端節(jié)點T與抓取目標的位置一致，進而求解確定所述肩部的首端節(jié)點S₀、肩部與肘部的交叉節(jié)點E₀以及肘部與腕部的交叉節(jié)點W₀各節(jié)點的位置；

第三模塊，用于執(zhí)行步驟系統根據選取的臂型角求解所述肩部各關節(jié)角度；

第四模塊，用于執(zhí)行步驟系統求解所述肘部各關節(jié)角度；

第五模塊，用于執(zhí)行步驟系統利用肘部與腕部的交叉節(jié)點W₀及腕部末端節(jié)點T求解腕部各關節(jié)角度。

算法整體原理的主要步驟如下：

參照圖1，是本發(fā)明一實施例的D-H坐標系示意圖。超冗余機械臂的D-H坐標系，類似于人類的手臂，將超冗余機械臂分為三段：肩部(shoulder S)，肘部(elbow E)，腕部(wrist W)，關鍵節(jié)點用符號S₀，E₀，W₀和T表示，參照圖2所示。

參照圖3，是本發(fā)明第三實施例的分段幾何法規(guī)劃算法示意圖。其中肩部包含前四個關節(jié)θ₁到θ₄構成，用于定位肘部初始點E₀，同時冗余一個自由度，而臂型角參數ψ用于表達這個冗余性。

參照圖4，是本發(fā)明第四實施例的相對于參考平面S₀E₀E₁的臂型角示意圖。當期望的肘部初始點E₀已知，所有的4個關節(jié)可以在特定的臂型角下求解。θ₅到θ_n-2構成肘部關節(jié)，類似于人類的手臂，肘部關節(jié)可以使機械臂末端工具在空間做大范圍運動，同時對于匹配特定的任務構型也有重要作用。為了求解肘部多關節(jié)角度，本方案采用基于空間圓弧的擬合法，因此所有的偶數關節(jié)運動角度相等，所有奇數關節(jié)運動角度相等。該處理方式可以有效避免關節(jié)內部奇異及避免關節(jié)超限運動。腕部關節(jié)由最后的萬向節(jié)構成(θ_n-1到θ_n)，用于確定末端效應器的指向。

為了便于討論定義了如下符號：

n:超冗余機械臂自由度數目.

θ_i:第i個關節(jié)角度,i＝1,2...n；

S_u:肩部萬向節(jié)位置，可表示為

E_v:肘部萬向節(jié)位置，可表示為

W₀:腕部萬向節(jié)位置，可表示為

T:工具坐標中心位置，可表示為T(x_T,y_T,z_T)；

O₀:坐標系{0}原點位置；

O_E:坐標系{E}原點位置,坐標系{E}的位置和S₂(E₀)的位置相同，坐標系{E}的姿態(tài)和坐標系{0}姿態(tài)相同；其中S₂和E₀表示同一個位置,E_n/2-3和W₀表示同一個位置。

首先，確定關鍵點T,W₀,S₂(E₀),S₀的值。關鍵點T,S₀的值分別與工具末端期望位置及基座位置相同，可以認為是已知的。末端效應器相對于坐標系{0}的指向用來求解點W₀,S₂(E₀)，然后根據腕部點和肘部點的位置可以求解空間圓弧參數的α,φ的值。

第二步，選擇臂型角求解肩部關節(jié)角度；

第三步，利用空間圓弧參數求解肘部關節(jié)角度；

第四步，利用腕部位置及效應器位置求解腕部關節(jié)角度。

關鍵點求解步驟如下：

假設給定的末端效應器位置及姿態(tài)為：

其中,[n_x,n_y,n_z]^T,[o_x,o_y,o_z]^T和[a_x,a_y,a_z]^T是單位向量代表末端效應器的x軸、y軸、z軸。[p_x,p_y,p_z]^T是坐標系Σ_n的原點位置；⁰R_n是3×3的旋轉矩陣。

S₀是全局坐標系原點，可認為其各坐標值均為零；

腕部點求解：在坐標系{0}下期望的方向向量設置為D，可以通過給定的旋轉矩陣⁰R_n求得，因此腕部點可以定義為：

其中，l_e表示腕部最末端連桿的長度。

肩部終點求解：在坐標系{0}下，肩部末端點(肘部起始點)的坐標可表示為

l表示非末端的連桿長度；除了末端桿l_e，其余的連桿都是等長的。空間圓弧參數求解：

肘部的擬合構型是空間圓弧，落在圓弧上的節(jié)點分別表示為E₀,E₁,…,E_n/2-3(W₀)。相應的，構成肘部的連桿就是空間圓弧的弦長，第一根弦長為E₀E₁,其在空間的向量可表示為：

然后可以用幾何參數來表達這個空間圓弧,如α,φ,定義如下：

α∈[0,2π]:向量和平面Y_E-Z_E,之間的夾角；

向量在平面Y_E-Z_E上的投影與the Y_E-axis之間的夾角；

φ∈[-π,π]:相鄰連桿之間的夾角。

基于空間圓弧參數α,φ,肘部節(jié)點E₀,E₁,…,E_n/2-3可以表示為：

其中j＝1,…,n/2-3。

根據上述E_n/2-3(W₀)和E₀(S₂)的位置已確定，空間圓弧參數α,φ可通過式(4)取邊界值j＝n/2-3進而代入(4)可求得。

肩部關節(jié)求解

肩部關節(jié)節(jié)點S₂相對于坐標系{0}的位置依賴于前四個關節(jié)角，即θ₁到θ₄。這實際上構成了一個4自由度的機械臂，由于S₂的位置已經求得，θ₁到θ₄同樣可以求得。顯然對于空間3個坐標定位冗余一個自由度，為了利用這個冗余的自由度，本方案定義了一個參數——臂型角ψ。參照圖4所示，基于臂型角ψ的S₁的空間位置定義為S_1ψ，當S_1ψ的位置確定后，θ₁,θ₂和θ₃,θ₄可以根據S_1ψ和S₂的空間位置求得。

求解S_1ψ的空間位置

參考平面定義為S₀E₀E₁，S₁可能的空間位置形成一個空間圓環(huán)如圖4所示，該空間圓環(huán)的中心定義為S_1c，同時落在平面S₀E₀E₁及空間圓環(huán)上的兩個點定義為S_1a和臂型角定義為從平面S₀S_1aS₂到平面S₀S_1ψS₂的夾角.S_1ψ的空間實際位置滿足以下條件：

a)S_1ψ在平面S_1ψS_1cS₁上；

b)點S_1ψ到坐標系{0}的距離為l；

c)臂型角ψ定義為S_1cS_1a與S_1cS_1ψ的夾角。

求解θ₁到θ_n：

θ_n-1＝arctan2(y_nr/l_ec_n,x_nr/l_ec_n) (16)

本發(fā)明提供的基于分段幾何法求解超冗余機器人逆運動學的方法及系統，通過將多自由度機械臂的機器人運用幾何分段分解為三部分，基于目標位置，根據選取的各關鍵節(jié)點的位置的求解入手，根據超冗余機械臂期望的末端位置及姿態(tài)在3D空間確定超冗余機械臂的合理構型，其有很高的計算效率，方便快捷，且求解唯一，十分精準。

以上是對本發(fā)明的較佳實施進行了具體說明，但本發(fā)明創(chuàng)造并不限于所述實施例，熟悉本領域的技術人員在不違背本發(fā)明精神的前提下還可做出種種的等同變形或替換，這些等同的變形或替換均包含在本申請權利要求所限定的范圍內。