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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

其中，O₀為坐標(biāo)原點(diǎn)，T為機(jī)械臂末端點(diǎn)，L表示連桿長(zhǎng)度。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

附圖說明

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

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

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

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

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

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

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

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

具體實(shí)施方式

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

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

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

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

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

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

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

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

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

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

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

其中，O₀為坐標(biāo)原點(diǎn)，T為機(jī)械臂末端點(diǎn)，L表示連桿長(zhǎng)度。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

因此空間脊線可以表達(dá)如下：

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

本方案選擇三個(gè)模式函數(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)分別與起始點(diǎn)和末端點(diǎn)的方位角子午角相關(guān)，表征了起始點(diǎn)和終止點(diǎn)的方向。公式(2)可表示為：

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

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

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

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

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

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

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

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

(2)方向向量擬合

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

(3)單段逆向擬合

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

式(11)中

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

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

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

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

(4)雙段逆向擬合

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

由于萬向節(jié)關(guān)節(jié)1關(guān)節(jié)2的運(yùn)動(dòng)不改變兩節(jié)機(jī)械臂的起點(diǎn)和終點(diǎn)連線長(zhǎng)度，僅改變終點(diǎn)在空間中的位置，因此假定θ₁＝θ₂＝0進(jìn)行分析，參照?qǐng)D5所示，兩節(jié)機(jī)械臂長(zhǎng)度ρ僅與關(guān)節(jié)角θ₃,θ₄的變化有關(guān)；

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

可得：

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

當(dāng)θ₃＝θ₄＝90°時(shí)

1.42L≤ρ≤2.00L (16)

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

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

式(17)中

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

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

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

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

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

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

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

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

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

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

基于臂型角的位置求解

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

a)在平面上：

b)到U_2i-1的距離為桿長(zhǎng)L：

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

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

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

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

參照?qǐng)D7，是本發(fā)明一實(shí)施例的通過點(diǎn)與坐標(biāo)系關(guān)系的正向遞推求得各個(gè)關(guān)節(jié)角度示意圖。

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

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

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

為了求得θ_4i-3,θ_4i-2的角度，需要將U_Ji的位置參考坐標(biāo)轉(zhuǎn)換到{4i-4}坐標(biāo)系下，由于在逆向擬合時(shí)無法確定U_2i的姿態(tài)，因此僅用位置矩陣進(jìn)行運(yùn)算：當(dāng)開始求解θ_4i-3,θ_4i-2時(shí)，關(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}坐標(biāo)系下的位置：

為了求得θ_4i-1,θ_4i的角度，需要將U_2i+1的位置坐標(biāo)轉(zhuǎn)換到{4i-2}坐標(biāo)系下。當(dāng)開始求解θ_4i-1,θ_4i時(shí)，關(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坐標(biāo)系YPPY…YPPY結(jié)構(gòu)規(guī)則，可以依次求出各個(gè)關(guān)節(jié)的角度。

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

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

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

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

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

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

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

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

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

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