本發(fā)明涉及超冗余機器人的逆運動學領(lǐng)域，具體為一種基于模式函數(shù)的超冗余機械臂混合逆向求解方法及系統(tǒng)。

背景技術(shù)：

超冗余機械臂具有極強的靈活性，除了執(zhí)行空間常規(guī)任務(wù)外，還特別適用于狹小空間開展作業(yè)，逐漸被應(yīng)用于航天、核電站、醫(yī)療等諸多領(lǐng)域。但由于大量自由度的存在，該類機械臂的運動學逆解變得十分復雜，因此運動逆解及規(guī)劃一直以來都是超冗余度機器人運動學研究中的一個重點和難點，然而超冗余機械臂的逆運動學求解卻是運動規(guī)劃的必要環(huán)節(jié)。通常冗余機械臂的逆運動學求解方法可以分為三類：(1)廣義逆法(2)人工神經(jīng)網(wǎng)絡(luò)法(3)幾何法。然而隨著自由度的大量增加其運動學逆解及路徑規(guī)劃問題也變得更加復雜。人工神經(jīng)網(wǎng)絡(luò)法被廣泛應(yīng)用于工業(yè)機械臂的逆解，但是所需要訓練集的大小會隨著自由度數(shù)目的變化而顯著變化，并且由于學習過程的延長可能無法滿足實時性的要求。Chirikjian針對超冗余機械臂的運動學規(guī)劃提出了脊線法的概念，奠定了超冗余機器人運動學的理論基礎(chǔ)，他采用脊線法來描述超冗余機械臂的宏觀構(gòu)型，得到了許多重要的結(jié)論。在該方法中，脊線被定義為分段連續(xù)的曲線，用該曲線來表達超冗余機械臂的宏觀幾何特征。脊線被一系列本征的模式函數(shù)所擬合，這些模式函數(shù)可以根據(jù)需要任意選擇并且可以在位置級得出有效的運動學逆解。一旦模式脊線針對假定的末端效應(yīng)器位置所確定，就可以確定各種各樣的擬合算法，求出機械臂在脊線上的關(guān)節(jié)點。Fahimi擴展了模式函數(shù)法在三維空間超冗余機械臂的應(yīng)用，引入了新的模式函數(shù)擴展了工作空間，并采用了針對萬向節(jié)關(guān)節(jié)構(gòu)型的遞歸擬合算法求解每個連桿的非線性代數(shù)方程，從而避免了同時求大量非線性方程的弊端。為了降低超冗余機械臂大量自由度引起的高計算量問題，因此該技術(shù)有必要進行改進。

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

為了解決上述技術(shù)問題，本發(fā)明的目的是提供一種基于模式函數(shù)的超冗余機械臂混合逆向求解方法及系統(tǒng)。

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

本發(fā)明提供一種基于模式函數(shù)的超冗余機械臂混合逆向求解方法，包括以下步驟；

系統(tǒng)根據(jù)期望的機械臂末端位置及各關(guān)節(jié)的構(gòu)型確定模式函數(shù)，并求得空間脊線；

系統(tǒng)利用機械臂末端點與脊線末端點重合，并通過末段連桿匹配機械臂期望指向，進而求得末端萬向節(jié)節(jié)點U_2N在空間的位置U_2N；

系統(tǒng)根據(jù)求得的末端萬向節(jié)節(jié)點U_2N的位置，基于單段連桿長，將點U_2N-1擬合至所述空間脊線上，進而確定點U_2N-1的位置；

系統(tǒng)根據(jù)求得的點U_2N-1的位置，基于等效連桿長度，進而依次確定各奇數(shù)萬向節(jié)的節(jié)點位置；

系統(tǒng)根據(jù)空間位置要求確定各臂型角，并求得各偶數(shù)萬向節(jié)的節(jié)點位置；

系統(tǒng)根據(jù)所求得的各萬向節(jié)節(jié)點位置，進而求解各關(guān)節(jié)的角度。

作為該技術(shù)方案的改進，所述空間脊線可表示為：

其中，u(σ)＝[sinφ(σ)cosψ(σ),cosφ(σ)cosψ(σ),sinψ(σ)]，u(σ)是在σ處正切于曲線的單位向量；所述s∈[0,1]，其表示脊線的長度參數(shù)；l是脊線的實際長度。

作為該技術(shù)方案的改進，其還包括系統(tǒng)根據(jù)機械臂末端歐拉角α,β,γ求得末端空間方向向量k：

其中，O₀為坐標原點，T為機械臂末端點，L表示連桿長度。

作為該技術(shù)方案的改進，所述步驟系統(tǒng)根據(jù)求得的末端萬向節(jié)節(jié)點U_2N的位置，基于單段連桿長，將點U_2N-1擬合至所述空間脊線上，進而確定點U_2N-1的位置，其包括：

其中，表示第(2N)個萬向節(jié)(x_2N,y_2N,z_2N)構(gòu)成的矢量，表示第(2N-1)個萬向節(jié)(x_2N-1,y_2N-1,z_2N-1)構(gòu)成的矢量；

表示第(2N-1)個連桿構(gòu)成的矢量,且

作為該技術(shù)方案的改進，所述萬向節(jié)節(jié)點U_2N-1在脊線上的空間位置可表示為：

其中，f(s_k)＝0，該式中s_k表示脊線起點到當前點的弧長，且s_k∈[0,1]。

作為該技術(shù)方案的改進，所述步驟系統(tǒng)根據(jù)求得的點U_2N-1的位置，基于等效連桿長度，進而依次確定各奇數(shù)萬向節(jié)的節(jié)點位置，其包括：

其中，ρ_2i-1為等效連桿的長度，其表示第2i-1個萬向節(jié)點與第2i+1個萬向節(jié)點之間的等效臂長。

進一步地，所述步驟系統(tǒng)根據(jù)空間位置要求確定各臂型角，并求得各偶數(shù)萬向節(jié)的節(jié)點位置，其可表示為：

其中，在以為圓心且為半徑的圓上，且與向量的夾角為臂型角ψ_2i-1。

進一步地，所述步驟系統(tǒng)根據(jù)所求得的各萬向節(jié)節(jié)點位置，進而求解各關(guān)節(jié)的角度，其包括俯仰-偏航型萬向節(jié)求解和偏航-俯仰型萬向節(jié)求解。

另一方面，本發(fā)明還提供一種基于模式函數(shù)的超冗余機械臂混合逆向求解系統(tǒng)，包括：

第一模塊，用于執(zhí)行步驟系統(tǒng)根據(jù)期望的機械臂末端位置及各關(guān)節(jié)的構(gòu)型確定模式函數(shù)，并求得空間脊線；

第二模塊，用于執(zhí)行步驟系統(tǒng)利用機械臂末端點與脊線末端點重合，并通過末段連桿匹配機械臂期望指向，進而求得末端萬向節(jié)節(jié)點U_2N在空間的位置U_2N；

第三模塊，用于執(zhí)行步驟系統(tǒng)根據(jù)求得的末端萬向節(jié)節(jié)點U_2N的位置，基于單段連桿長，將點U_2N-1擬合至所述空間脊線上，進而確定點U_2N-1的位置；

第四模塊，用于執(zhí)行步驟系統(tǒng)根據(jù)求得的點U_2N-1的位置，基于等效連桿長度，進而依次確定各奇數(shù)萬向節(jié)的節(jié)點位置；

第五模塊，用于執(zhí)行步驟系統(tǒng)根據(jù)空間位置要求確定各臂型角，并求得各偶數(shù)萬向節(jié)的節(jié)點位置；

第六模塊，用于執(zhí)行步驟系統(tǒng)根據(jù)所求得的各萬向節(jié)節(jié)點位置，進而求解各關(guān)節(jié)的角度。

本發(fā)明的有益效果是：本發(fā)明提供的基于模式函數(shù)的超冗余機械臂混合逆向求解方法及系統(tǒng)，該方案統(tǒng)籌考慮了超冗余機械臂的可解性與靈活性，首先通過關(guān)節(jié)點與脊線匹配的方式給定有限的約束條件，然后基于奇數(shù)萬向節(jié)點與偶數(shù)萬向節(jié)點，根據(jù)附加任務(wù)優(yōu)化特定的臂型角參數(shù)值以達到求解關(guān)節(jié)角度，實現(xiàn)超冗余機械臂的空間合理規(guī)劃的目標。

該混合逆向擬合法用獨立的末端萬向節(jié)匹配效應(yīng)器的方向向量，不僅保證位置準確可達而且可以保證末端效應(yīng)器的指向；其余關(guān)節(jié)優(yōu)先通過雙段擬合規(guī)則擬合到脊線上，這樣既能保證超冗余機械臂在宏觀構(gòu)型上滿足脊線要求，又將雙段4DOF相對空間位置的一個冗余自由度轉(zhuǎn)化為可直觀調(diào)節(jié)的臂型角，在合理有效逆解的同時進一步釋放了超冗余機械臂的冗余特性。

附圖說明

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

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

圖2是本發(fā)明第二實施例的整體原理圖；

圖3是本發(fā)明第三實施例的脊線整體構(gòu)型及位置向量示意圖；

圖4是本發(fā)明第四實施例的位置求解原理示意圖；

圖5是本發(fā)明第五實施例的兩段機械臂長度與轉(zhuǎn)角關(guān)系示意圖；

圖6是本發(fā)明第六實施例的參考面臂型角示意圖；

圖7是本發(fā)明第七實施例的求各關(guān)節(jié)角度的示意圖。

具體實施方式

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

參照圖1-7，是本發(fā)明實施例的示意圖。本發(fā)明提供一種基于模式函數(shù)的超冗余機械臂混合逆向求解方法，包括以下步驟；

系統(tǒng)根據(jù)期望的機械臂末端位置及各關(guān)節(jié)的構(gòu)型確定模式函數(shù)，并求得空間脊線；

系統(tǒng)利用機械臂末端點與脊線末端點重合，并通過末段連桿匹配機械臂期望指向，進而求得末端萬向節(jié)節(jié)點U_2N在空間的位置U_2N；

系統(tǒng)根據(jù)求得的末端萬向節(jié)節(jié)點U_2N的位置，基于單段連桿長，將點U_2N-1擬合至所述空間脊線上，進而確定點U_2N-1的位置；

系統(tǒng)根據(jù)求得的點U_2N-1的位置，基于等效連桿長度，進而依次確定各奇數(shù)萬向節(jié)的節(jié)點位置；

系統(tǒng)根據(jù)空間位置要求確定各臂型角，并求得各偶數(shù)萬向節(jié)的節(jié)點位置；

系統(tǒng)根據(jù)所求得的各萬向節(jié)節(jié)點位置，進而求解各關(guān)節(jié)的角度。

作為該技術(shù)方案的改進，所述空間脊線可表示為：

其中，u(σ)＝[sinφ(σ)cosψ(σ),cosφ(σ)cosψ(σ),sinψ(σ)]，u(σ)是在σ處正切于曲線的單位向量；所述s∈[0,1]，其表示脊線的長度參數(shù)；l是脊線的實際長度。

作為該技術(shù)方案的改進，其還包括系統(tǒng)根據(jù)機械臂末端歐拉角α,β,γ求得末端空間方向向量k：

其中，O₀為坐標原點，T為機械臂末端點，L表示連桿長度。

作為該技術(shù)方案的改進，所述步驟系統(tǒng)根據(jù)求得的末端萬向節(jié)節(jié)點U_2N的位置，基于單段連桿長，將點U_2N-1擬合至所述空間脊線上，進而確定點U_2N-1的位置，其包括：

其中，表示第(2N)個萬向節(jié)(x_2N,y_2N,z_2N)構(gòu)成的矢量，表示第(2N-1)個萬向節(jié)(x_2N-1,y_2N-1,z_2N-1)構(gòu)成的矢量；表示第(2N-1)個連桿構(gòu)成的矢量,且

作為該技術(shù)方案的改進，所述萬向節(jié)節(jié)點U_2N-1在脊線上的空間位置可表示為：

其中，f(s_k)＝0，該式中s_k表示脊線起點到當前點的弧長，且s_k∈[0,1]。作為該技術(shù)方案的改進，所述步驟系統(tǒng)根據(jù)求得的點U_2N-1的位置，基于等效連桿長度，進而依次確定各奇數(shù)萬向節(jié)的節(jié)點位置，其包括：

其中，ρ_2i-1為等效連桿的長度，其表示第2i-1個萬向節(jié)點與第2i+1個萬向節(jié)點之間的等效臂長。

進一步地，所述步驟系統(tǒng)根據(jù)空間位置要求確定各臂型角，并求得各偶數(shù)萬向節(jié)的節(jié)點位置，其可表示為：

其中，在以為圓心且為半徑的圓上，且與向量的夾角為臂型角ψ_2i-1。

進一步地，所述步驟系統(tǒng)根據(jù)所求得的各萬向節(jié)節(jié)點位置，進而求解各關(guān)節(jié)的角度，其包括俯仰-偏航型萬向節(jié)求解和偏航-俯仰型萬向節(jié)求解。

為了解決由于超冗余的自由度存在，機械臂的逆解是無窮多的，本算法基于常規(guī)的期望約束(末端位置，工具指向、避障要求、避關(guān)節(jié)超限、穿越狹小空間等)提出的求解超冗余機械臂的方法，主要步驟如下：

第一步：求宏觀脊線。根據(jù)期望的末端位置及中間的構(gòu)型要求選定模式函數(shù)，通過對模式函數(shù)的數(shù)值積分求得脊線在空間的整體形態(tài)。

第二步：匹配末端方向向量。此時脊線的末端點也正是期望的位置，原則上可以采用脊線末端點的切線方向作為機械臂的指向，但由于臂桿有一定長度，當擬合到脊線上后，末端方向向量與脊線的切向向量并不相同，因此基于末端點引入末端方向向量k控制末端臂桿指向,求得U_2N點在空間位置，此時向量與方向向量k共線。

第三步：單段擬合到脊線。由于脊線有一定弧度，通常情況下以U_2N點為起點的在匹配方向向量后，U_2N點難以落在脊線上，本方案通過單段臂桿的逆向擬合將U_2N-1點擬合到脊線上。

第四步：雙段擬合到脊線上求奇數(shù)節(jié)點位置。U₁到U_2N-2之間奇數(shù)萬向節(jié)的節(jié)點通過雙段逆向擬合的方法擬合到脊線上。

第五步：臂型角求偶數(shù)節(jié)點位置。當奇數(shù)節(jié)點確定后，可以默認設(shè)置臂型角或根據(jù)空間位置要求確定臂型角后，求解偶數(shù)萬向節(jié)節(jié)點(U₂,U₄,U₆…U_2N-2)的位置。

第六步：求各關(guān)節(jié)角度。當所有萬向節(jié)位置確定后，根據(jù)具體關(guān)節(jié)構(gòu)型(Yaw-Pitch型，Pitch-Yaw型)求解組成萬向節(jié)關(guān)節(jié)的角度。

(1)基于模式函數(shù)的整體構(gòu)型

由于離散的分段機械臂有較多的連桿組成，這樣它在空間的構(gòu)型可以近似的由空間脊線來模擬，這條脊線可以認為是一條分段連續(xù)曲線——模式脊線，模式脊線法的基本原理是微分幾何學，用它來描述超冗余機械臂的宏觀幾何特征。通過建立超冗余機械臂基于模式脊線的運動學模型，從而使的復雜的超冗余機械臂規(guī)劃問題轉(zhuǎn)化為模式脊線的運動學問題。基于模式脊線的空間宏觀構(gòu)型從而求得超冗余機械臂的各個關(guān)節(jié)的運動學解，因此可以方便有效的解決超冗余度機器人逆解問題，為空間超冗余度機器人的運動學規(guī)劃提供了理論基礎(chǔ)。模式脊線法的關(guān)鍵在于求得匹配脊線構(gòu)型的模式參數(shù)，使得模式脊線合理表達超冗余度機器人的主要特征，同時，通過修正該模式函數(shù)法使其在起始點和末端點能夠在切線方向，保證了該脊線是一條平滑的曲線，并且具有更合理的工作空間。

空間脊線上的任意點可以表示成末端在曲線上的空間位置向量，

u(σ)＝[sinφ(σ)cosψ(σ),cosφ(σ)cosψ(σ),sinψ(σ)] (1)

因此空間脊線可以表達如下：

其中s∈[0,1]是曲線的長度參數(shù)，u(σ)是在σ處正切于曲線的單位向量，l是曲線的實際長度。

本方案選擇三個模式函數(shù)來定義曲線參數(shù)：

φ(s)＝a₁sin(2πs)+a₂(1-cos(2πs))+b_1φ(1-sin(πs/2))+b_2φ(sin(πs/2)) (3)

ψ(s)＝a₃(1-cos(2πs))+b_1ψ(1-sin(πs/2))+b_2ψ(sin(πs/2)) (4)

其中b_1φ＝φ(0),b_1ψ＝ψ(0),b_2φ＝φ(1),b_2ψ＝ψ(1)分別與起始點和末端點的方位角子午角相關(guān)，表征了起始點和終止點的方向。公式(2)可表示為：

函數(shù)φ(s)和ψ(s)可以表達成如下模式函數(shù)的線性組合：

其中：f_i(s)是模式函數(shù)，a_i是模式協(xié)同參數(shù)，n₁是針對φ(s)模式函數(shù)的數(shù)目，g_i(s)以及b_iφ，b_iψ用來確定脊線起點和終點的方向。該模式函數(shù)法將逆運動學問題轉(zhuǎn)化為求解滿足任務(wù)要求的a_i的問題。參照圖2-3，例如，x(1)＝x(T)，其中x(T)是脊線末端點的期望位置向量。

將式子(3)、(4)分別代入(6)、(7)，再統(tǒng)一代入式(5)，當s＝1時可以得到脊線的正運動學方程：

x(1)＝x(T) (8)

其中x(T)是脊線末端點的期望位置向量，式(8)可以根據(jù)模式協(xié)同參數(shù)a_i的值通過數(shù)值解得方式求解。

通過式(9)的迭代近似解可以求得模式協(xié)同參數(shù)向量：

其中α是控制收斂速度的常數(shù)，m表示迭代次數(shù)，3×3的模型雅克比矩陣J_a(a,s)取s＝1時的值。通過(3)、(4)、(5)可以求得x(1)各元素的偏微分，利用數(shù)值積分計算具體值；式(9)的數(shù)值解與機械臂關(guān)節(jié)的數(shù)目無關(guān)。

在求得空間脊線后，本方案提出了混合逆向擬合法將完全解放末端姿態(tài)，使得超冗余機械臂的末端完全匹配期望的空間位置及指向后，再將超冗余機械臂的節(jié)點以單段或雙段逆向的方式擬合到空間脊線上。單段逆向擬合能保證超冗余機械臂的萬向節(jié)擬合到脊線上，雙段逆向擬合方法通過定義臂型角使得超冗余機械臂在空間運動狀態(tài)，不再完全受限于空間脊線，而是在全局位置符合脊線位形的情況下，有了調(diào)整局部姿態(tài)的能力，既實現(xiàn)了末端效應(yīng)器工作空間內(nèi)任意位置及指向可達，又能夠保證超冗余機械臂在符合期望脊線構(gòu)型的情況下具備局部調(diào)整姿態(tài)的能力。

(2)方向向量擬合

k是根據(jù)末端歐拉角α,β,γ求得的空間方向向量：

(3)單段逆向擬合

參照圖4，設(shè)定超冗余機械臂腕部末端與模式脊線的末端重合；在擬合方向向量k得到U_2N后為了保證臂桿長度，將遞歸計算脊線上的點使得臨近關(guān)節(jié)點之間的空間距離滿足桿長關(guān)系：

式(11)中

——第(2N)^th個萬向節(jié)(x_2N,y_2N,z_2N)；

——第(2N-1)^th個萬向節(jié)(x_2N-1,y_2N-1,z_2N-1)；

——第(2N-1)^th個連桿,

以上公式中，關(guān)節(jié)點的位置都是脊線弧長s_k的函數(shù)，保證點在空間脊線上，在區(qū)間[0,1]內(nèi)搜索，分別以式f(s_k)＝0為判斷條件，可以有效的得到U_2N-1關(guān)節(jié)點在脊線上的空間位置。

(4)雙段逆向擬合

區(qū)別于傳統(tǒng)的單段逆向擬合，本方案將其余關(guān)節(jié)采用雙段逆向擬合的方式求解超冗余機械臂的逆解。傳統(tǒng)單段逆向擬合可以保證很好的匹配空間脊線的形態(tài)，但由于脊線是通過模式函數(shù)定性確定的空間構(gòu)型，往往難以滿足實際工況需求，也因此受限于脊線的約束；雙段逆向擬合的方法進一步將超冗余機械臂的冗余性與直觀可控的參數(shù)(雙段臂桿長度、臂型角)關(guān)聯(lián)，相對于傳統(tǒng)的單段逆向擬合法，在結(jié)合實際工況及機械臂構(gòu)型及關(guān)節(jié)角度可解可控的情況下，更充分的利用了超冗余機械臂的冗余性。

由于萬向節(jié)關(guān)節(jié)1關(guān)節(jié)2的運動不改變兩節(jié)機械臂的起點和終點連線長度，僅改變終點在空間中的位置，因此假定θ₁＝θ₂＝0進行分析，參照圖5所示，兩節(jié)機械臂長度ρ僅與關(guān)節(jié)角θ₃,θ₄的變化有關(guān)；

其中，s_i＝sin(θ_i),c_i＝cos(θ_i)。

可得：

min(2L²(1+c₃c₄))≤ρ²≤4L² (15)

當θ₃＝θ₄＝90°時

1.42L≤ρ≤2.00L (16)

以n DOF超冗余機械臂為例，分為2N-1位置段，選定位置參數(shù)ρ_2i-1作為位置段長度調(diào)節(jié)參數(shù)。默認設(shè)定ρ₁＝...＝ρ_2N-2＝1.7L，將ρ_2N-1作為待確定值，當ρ_2N-1滿足(16)時，則可以進行整體位置逆解。

設(shè)定超冗余機械臂的末端T與模式脊線的末端重合；為了滿足4DOF等效關(guān)節(jié)臂桿長度，將遞歸計算脊線上的點使得臨近關(guān)節(jié)點之間的空間距離滿足桿長關(guān)系：

式(17)中

——第(2i+1)^th萬向節(jié)構(gòu)成的矢量，坐標為(x_2i+1,y_2i+1,z_2i+1)；

——第(2i-1)^th萬向節(jié)構(gòu)成的矢量，坐標為(x_2i-1,y_2i-1,z_2i-1)；

——第(2i-1)^th等效臂桿i.e.長度為

以上公式中，關(guān)節(jié)點的位置都是脊線弧長s_k的函數(shù)，保證點在空間脊線上，通過逐步掃描法和二分法在區(qū)間[0,1]內(nèi)搜索，以式f(s_k)＝0為判斷條件，可以有效的得到各個關(guān)節(jié)點的空間位置。

通過以上公式可以求得ρ_2i-1各萬向節(jié)節(jié)節(jié)點在模式脊線上的空間位置；當逆向擬合到U₁～U₃萬向節(jié)節(jié)點時需要對U₃節(jié)點的合理性進行判斷：

當ρ_2N-1≥2L時，說明構(gòu)型的實際空間結(jié)構(gòu)的最大長度不能滿足U₃的位置需求，因此需要調(diào)節(jié)前兩段長度，將ρ₁＝ρ₁+△,...ρ_2N-2＝ρ_2N-2+△(△是一個可調(diào)節(jié)參數(shù))后再次進行逆解計算，直到|ρ_2N-1-1.7L|≤δ(δ是一個小的閾值)。

當ρ_2N-1≤1.42L時，說明構(gòu)型的實際空間結(jié)構(gòu)的最小長度不能滿足U₃位置需求，因此需要調(diào)節(jié)前兩段長度，將ρ₁＝ρ₁-△,...ρ_2N-2＝ρ_2N-2-△(△是一個可調(diào)節(jié)參數(shù))后再次進行逆解計算，直到|ρ_2N-1-1.7L|≤δ(δ是一個小的閾值)。

當確定合理的位置參數(shù)ρ_2i-1后，可以通過位置擬合，求得等效臂桿段節(jié)點的空間坐標。

(5)基于臂型角求解姿態(tài)

當確定機械臂分節(jié)點的位置后可以采用本方案提出的臂型角參數(shù)化求解各個關(guān)節(jié)的值，因為臂型角是可以調(diào)節(jié)的變量，因此可以有針對性的實現(xiàn)局部的姿態(tài)調(diào)整，例如通過調(diào)整局部姿態(tài)實現(xiàn)狹小空間的穿越或者實現(xiàn)障礙物的避障規(guī)劃。

基于臂型角的位置求解

參照圖6，以空間面U₁U_2i-1U_2i+1為參考平面，以為圓心，以矢量為Z軸，以為X軸，則按照右手定則可定義Y軸，在此坐標系中，X軸與Y軸正方向的夾角定義為臂型角ψ(ψ∈[0°,360°))。所以在平面上；到原點的距離為桿長L；在以為圓心且為半徑的圓上與向量的夾角為臂型角ψ_2i-1。

a)在平面上：

b)到U_2i-1的距離為桿長L：

c)與向量的夾角為臂型角ψ_2i-1：

聯(lián)立方程可得：

由(20)可以求得x,y,z的值。

(6)基于關(guān)節(jié)擬合點與角度求解

參照圖7，是本發(fā)明一實施例的通過點與坐標系關(guān)系的正向遞推求得各個關(guān)節(jié)角度示意圖。

1)PY(pitch-yaw)型萬向節(jié)求解

通過建立DH的關(guān)系可以求得

通過逆向擬合法可以求得U_Ji在0坐標系下的位置：

為了求得θ_4i-3,θ_4i-2的角度，需要將U_Ji的位置參考坐標轉(zhuǎn)換到{4i-4}坐標系下，由于在逆向擬合時無法確定U_2i的姿態(tài)，因此僅用位置矩陣進行運算：當開始求解θ_4i-3,θ_4i-2時，關(guān)節(jié)θ₁到θ_4i-4的所有角度都已經(jīng)求得了，因此⁰T_4i-4(⁰T_2i-4＝f(θ₁...θ_4i-4))的所有元素都是已知的，⁰T_4i-2僅位置部分已知，因此可以通過(23)求解^4i-4T_4i-2的位置部分，如下：

由(21)(23)可得：

解得：

θ_4i-3＝arctan2(y_4i-2/Lcos(θ_4i-2),x_4i-2/Lcos(θ_4i-2)) (26)

2)YP(yaw-pitch)型萬向節(jié)求解

通過建立DH的關(guān)系可以求得

通過單段逆向擬合可以求得U_2i+1在{0}坐標系下的位置：

為了求得θ_4i-1,θ_4i的角度，需要將U_2i+1的位置坐標轉(zhuǎn)換到{4i-2}坐標系下。當開始求解θ_4i-1,θ_4i時，關(guān)節(jié)θ₁到θ_4i-2的所有角度都已經(jīng)求得了，因此⁰T_4i-2(⁰T_4i-2＝f(θ₁...θ_4i-2))的所有元素都是已知的，⁰T_4i僅位置部分已知，因此可以通過(29)求解^4i-2T_4i的位置部分，如下：

由(27)(29)可得：

θ_4i＝arctan2(y_4i/Lcos(θ_4i-1),x_4i/Lcos(θ_4i-1)) (31)

同理，根據(jù)DH坐標系YPPY…YPPY結(jié)構(gòu)規(guī)則，可以依次求出各個關(guān)節(jié)的角度。

另一方面，本發(fā)明還提供一種基于模式函數(shù)的超冗余機械臂混合逆向求解系統(tǒng)，包括：

第一模塊，用于執(zhí)行步驟系統(tǒng)根據(jù)期望的機械臂末端位置及各關(guān)節(jié)的構(gòu)型確定模式函數(shù)，并求得空間脊線；

第二模塊，用于執(zhí)行步驟系統(tǒng)利用機械臂末端點與脊線末端點重合，并通過末段連桿U_2NT匹配機械臂期望指向k，進而求得末端萬向節(jié)節(jié)點U_2N在空間的位置U_2N；

第三模塊，用于執(zhí)行步驟系統(tǒng)根據(jù)求得的末端萬向節(jié)節(jié)點U_2N的位置，基于單段連桿長，將點U_2N-1擬合至所述空間脊線上，進而確定點U_2N-1的位置；

第四模塊，用于執(zhí)行步驟系統(tǒng)根據(jù)求得的點U_2N-1的位置，基于等效連桿長度，進而依次確定各奇數(shù)萬向節(jié)的節(jié)點位置；

第五模塊，用于執(zhí)行步驟系統(tǒng)根據(jù)空間位置要求確定各臂型角，并求得各偶數(shù)萬向節(jié)的節(jié)點位置；

第六模塊，用于執(zhí)行步驟系統(tǒng)根據(jù)所求得的各萬向節(jié)節(jié)點位置，進而求解各關(guān)節(jié)的角度。

本發(fā)明提供的基于模式函數(shù)的超冗余機械臂混合逆向擬合逆解方法，提出了基于模式函數(shù)的混合逆向擬合法解決超冗余機械臂在3D空間的逆運動學問題。在機械臂末端位置及指向確定后，通過該方法可以求得超冗余機械臂的合理關(guān)節(jié)角度，而且在求解過程中可以進一步考慮避障、避關(guān)節(jié)奇異等附加任務(wù)。

該方法基于末端點位姿信息，確定中間萬向節(jié)節(jié)點的位置，在確定過程中可以考慮附加任務(wù)例如工具端指向，回避空間障礙物，回避機械臂內(nèi)部奇異及邊界奇異，回避關(guān)節(jié)運動超限等；中間萬向節(jié)節(jié)點一旦確定就可以根據(jù)關(guān)節(jié)的配置類型求解當前位置下的關(guān)節(jié)角度。該方法分三大步驟：首先由根據(jù)整體空間環(huán)境確定的模式函數(shù)求得超冗余機械臂的空間脊線；然后基于脊線的宏觀構(gòu)型，將超冗余機械臂的各個臂桿按照末段擬合方向向量、次段擬合到脊線、其余雙段擬合到脊線同時考慮附加任務(wù)的順序，依次求解超冗余機械臂的萬向節(jié)的空間節(jié)點位置；最后當各個萬向節(jié)空間位置確定后，根據(jù)萬向節(jié)的具體構(gòu)型可求解相應(yīng)的關(guān)節(jié)角度。該混合逆向擬合法用獨立的末端萬向節(jié)匹配效應(yīng)器的方向向量，不僅保證位置準確可達而且可以保證末端效應(yīng)器的指向；其余關(guān)節(jié)優(yōu)先通過雙段擬合規(guī)則擬合到脊線上，這樣既能保證超冗余機械臂在宏觀構(gòu)型上滿足脊線要求，又將雙段4DOF相對空間位置的一個冗余自由度轉(zhuǎn)化為可直觀調(diào)節(jié)的臂型角，在合理有效逆解的同時進一步釋放了超冗余機械臂的冗余特性。

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