本發(fā)明涉及冗余機(jī)器人，尤其涉及一種具有冗余自由度機(jī)械臂的逆運(yùn)動(dòng)學(xué)最小范數(shù)求解方法。

背景技術(shù)：

1、在面對(duì)地震搶險(xiǎn)、深海探測(cè)、核電站勘探等地形狹窄、環(huán)境惡劣的極端作業(yè)環(huán)境下，傳統(tǒng)關(guān)節(jié)型機(jī)械臂受到其結(jié)構(gòu)設(shè)計(jì)與環(huán)境條件的制約難以滿足任務(wù)要求，因而自由度更高、體型更加纖細(xì)的冗余自由度機(jī)械臂在合理的路徑規(guī)劃下可以形成不同機(jī)械臂構(gòu)型，以完成各種狹窄環(huán)境的作業(yè)需求，有望成為上述環(huán)境中的作業(yè)主力，具有很大的應(yīng)用前景。

2、機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解是機(jī)械臂控制的基礎(chǔ)與關(guān)鍵，主要應(yīng)用在具有關(guān)節(jié)型結(jié)構(gòu)的機(jī)械臂中，通過(guò)將機(jī)械臂末端執(zhí)行器的位置和姿態(tài)從三維笛卡爾空間映射到機(jī)械臂內(nèi)部的關(guān)節(jié)空間，求得對(duì)應(yīng)的關(guān)節(jié)轉(zhuǎn)角，以此降低機(jī)械臂控制難度。機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解自身已存在解的存在性、唯一性與合理性不確定的問(wèn)題，致使具有冗余自由度的機(jī)械臂構(gòu)型更加復(fù)雜，通常具有較多或無(wú)窮解法，逆運(yùn)動(dòng)學(xué)求解難度與求解時(shí)間將成倍增加。

3、目前，針對(duì)機(jī)械臂逆運(yùn)動(dòng)學(xué)的解法主要分為解析解與數(shù)值解兩大類(lèi)。解析解主要針對(duì)滿足pieper準(zhǔn)則的機(jī)械臂機(jī)構(gòu)，即機(jī)械臂三個(gè)相鄰關(guān)節(jié)軸線交于一點(diǎn)或相互平行。這樣的機(jī)械臂逆運(yùn)動(dòng)學(xué)方程主要為不高于四次的多項(xiàng)式，計(jì)算量小且較為精確。但在具有無(wú)窮逆解的冗余自由度機(jī)械臂求解過(guò)程中，給出的解往往不是最小范數(shù)解，且機(jī)械臂末端運(yùn)動(dòng)軌跡在運(yùn)動(dòng)到某些象限，關(guān)節(jié)轉(zhuǎn)角可能會(huì)出現(xiàn)跳變、反轉(zhuǎn)等情況。

4、數(shù)值解主要通過(guò)數(shù)值迭代求解得到逆運(yùn)動(dòng)學(xué)最小范數(shù)解。其中較為典型的是基于雅各比偽逆法求解機(jī)器人逆運(yùn)動(dòng)學(xué)，該方法主要通過(guò)機(jī)械臂在末端點(diǎn)工作空間的誤差，將其投影到每一個(gè)關(guān)節(jié)進(jìn)行比例控制，經(jīng)若干次迭代后收斂至其中一個(gè)可行解。但該方法計(jì)算量較大，當(dāng)機(jī)械臂在運(yùn)動(dòng)到奇異位形附近時(shí)，通過(guò)偽逆法求解的關(guān)節(jié)速度會(huì)急劇變大，使迭代出的計(jì)算結(jié)果不可控，且機(jī)械臂的關(guān)節(jié)轉(zhuǎn)角容易超出工作范圍。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明的目的在于提供一種具有冗余自由度機(jī)械臂的逆運(yùn)動(dòng)學(xué)最小范數(shù)求解方法，以提升最小范數(shù)解的解算效率與精準(zhǔn)度。

2、為實(shí)現(xiàn)上述目的，本發(fā)明采用如下技術(shù)方案：

3、一種具有冗余自由度機(jī)械臂的逆運(yùn)動(dòng)學(xué)最小范數(shù)求解方法，包括以下步驟：

4、步驟1、根據(jù)機(jī)械臂自身參數(shù)建立d-h坐標(biāo)系和d-h參數(shù)；

5、步驟2、根據(jù)步驟1得到的d-h坐標(biāo)系和d-h參數(shù)建立正運(yùn)動(dòng)學(xué)模型；

6、步驟3、基于正運(yùn)動(dòng)學(xué)模型建立機(jī)基于雅各比偽逆的械臂逆運(yùn)動(dòng)學(xué)模型；

7、步驟4、使用阻尼最小二乘法與梯度投影法求解并優(yōu)化步驟3得到的逆運(yùn)動(dòng)學(xué)模型，并對(duì)其進(jìn)行求解以得到機(jī)械臂的逆運(yùn)動(dòng)學(xué)最小范數(shù)。

8、進(jìn)一步的，所述步驟1根據(jù)機(jī)械臂自身參數(shù)建立d-h坐標(biāo)系和d-h參數(shù)的實(shí)現(xiàn)方法包括：

9、建立d-h坐標(biāo)系：建立d-h坐標(biāo)系：定義基座為第0個(gè)連桿，靠近基座側(cè)的關(guān)節(jié)命名為第一段關(guān)節(jié)，接下來(lái)依次為第二段關(guān)節(jié)、第三段關(guān)節(jié)，依次類(lèi)推，直到編號(hào)為2-n，每個(gè)關(guān)節(jié)上均建立兩個(gè)相互正交的笛卡爾坐標(biāo)系，其中原點(diǎn)與萬(wàn)向節(jié)十字軸重合，zi軸與第i關(guān)節(jié)的軸線方向平行，xi軸垂直于zi軸并指向其背離方向，yi軸根據(jù)笛卡爾坐標(biāo)系下的右手法則確定；

10、建立d-h參數(shù)：據(jù)機(jī)械臂自身結(jié)構(gòu)特征，在第{i}與第{i-1}關(guān)節(jié)的重心處依照坐標(biāo)軸xi與坐標(biāo)軸zi-1垂直相交原則，將尺寸參數(shù)抽象為建立d-h參數(shù)所需的連桿長(zhǎng)度ai、連桿扭角αi、關(guān)節(jié)距離di與關(guān)節(jié)轉(zhuǎn)角θi，建立d-h參數(shù)。

11、進(jìn)一步的，所述步驟2根據(jù)的d-h坐標(biāo)系和d-h參數(shù)建立正運(yùn)動(dòng)學(xué)模型的方法，包括：

12、根據(jù)機(jī)械臂d-h坐標(biāo)系和d-h參數(shù)，使用運(yùn)動(dòng)學(xué)空間內(nèi)相鄰兩坐標(biāo)系的齊次變換矩陣公式，建立表示機(jī)械臂末端位置、姿態(tài)與各關(guān)節(jié)變量之間函數(shù)關(guān)系的機(jī)械臂正運(yùn)動(dòng)學(xué)模型公式；

13、所述齊次變換矩陣公式如下所示：

14、

15、式中，cθi＝cos(θi)，sθi＝sin(θi)；cαi＝cos(αi)，sαi＝sin(ai)，ai表示連桿長(zhǎng)度，di表示關(guān)節(jié)距離，θi表示關(guān)節(jié)轉(zhuǎn)角，iti+1表示連桿坐標(biāo)系{i+1}相對(duì)于坐標(biāo)系{i}的齊次變換矩陣；

16、依據(jù)鏈?zhǔn)椒▌t將得到的齊次變換矩陣連續(xù)右乘，得到機(jī)械臂正運(yùn)動(dòng)學(xué)模型：

17、

18、式中，表示機(jī)械臂末端執(zhí)行器相對(duì)于基坐標(biāo)系的齊次變換矩陣，r3*3表示末端執(zhí)行器位置矩陣，r3*3表示末端執(zhí)行器姿態(tài)矩陣。

19、進(jìn)一步的，所述步驟3基于正運(yùn)動(dòng)學(xué)模型建立機(jī)基于雅各比偽逆的械臂逆運(yùn)動(dòng)學(xué)模型的方法，包括：

20、3.1在機(jī)器臂關(guān)節(jié)角度運(yùn)動(dòng)范圍內(nèi)隨機(jī)選取一組角度數(shù)據(jù)q0＝(θ1，θ2，…，θn)作為初始姿態(tài)，基于初始姿態(tài)，使用步驟2建立的正運(yùn)動(dòng)學(xué)模型計(jì)算得到機(jī)器臂末端執(zhí)行器位姿矩陣記為x，并計(jì)算機(jī)器臂末端執(zhí)行器當(dāng)前位姿與期望位姿的差值，記為dx；

21、3.2、建立機(jī)械臂基于雅各比偽逆的逆運(yùn)動(dòng)學(xué)

22、a、基于關(guān)節(jié)空間與末端執(zhí)行器工作空間的雅各比方程，建立用于表達(dá)當(dāng)前末端位姿與期望位姿的差值與關(guān)節(jié)角度增量之間的關(guān)系式dx；

23、dx＝j(luò)(0)dθ

24、其中，dx表示當(dāng)前末端位姿與期望位姿的差值，j(θ)表示機(jī)械臂關(guān)節(jié)運(yùn)動(dòng)的雅各比矩陣，dθ表示關(guān)節(jié)角度增量；

25、b、利用步驟a得到的關(guān)系式計(jì)算關(guān)節(jié)角度增量，根據(jù)關(guān)節(jié)角度增量使用迭代法建立逆運(yùn)動(dòng)學(xué)模型dθ：

26、dθ＝j(luò)+dx

27、其中，j+表示雅各比方程的偽逆方程，其表達(dá)式為：j+＝j(luò)t(jjt)-1；j表示雅各比矩陣，jt表示雅各比矩陣的轉(zhuǎn)置矩陣。

28、進(jìn)一步的，步驟4、結(jié)合阻尼最小二乘法與梯度投影法求解并優(yōu)化步驟3得到的逆運(yùn)動(dòng)學(xué)模型，以得到機(jī)械臂的逆運(yùn)動(dòng)學(xué)最小范數(shù)的方法，包括：

29、4.1、對(duì)偽逆方程j+進(jìn)行奇異值分解，得到最小奇異值：

30、j+＝utd+v

31、其中，ut和v為奇異值分解后得到的正交矩陣；d+是由雅各比矩陣偽逆j+的奇異值組成的對(duì)角矩陣，且滿足

32、其中，σm+1-i指分解得到的最小奇異值，用于表示當(dāng)前運(yùn)動(dòng)過(guò)程中機(jī)械臂構(gòu)型的奇異程度；

33、4.2、基于最小奇異值，構(gòu)建基于阻尼最小二乘法下的雅各比偽逆方程j#：

34、j#＝(jtj+λ2i)-1jt

35、

36、式中，i表示單位矩陣；j表示雅各比矩陣；jt表示雅各比矩陣的轉(zhuǎn)置矩陣；λ表示阻尼系數(shù)且λ≥0，通過(guò)引入變阻尼系數(shù)進(jìn)行自適應(yīng)調(diào)整，來(lái)降低機(jī)械臂的末端追蹤誤差；ε表示最小誤差范圍，判斷機(jī)器人是否進(jìn)入奇異狀態(tài)，σm表示雅各比矩陣的最小奇異值，由偽逆j+經(jīng)奇異值分解而來(lái)；λm表示預(yù)設(shè)最大阻尼值；

37、4.3、在雅各比偽逆方程j#中引入梯度投影法以避開(kāi)求解過(guò)程中超過(guò)關(guān)節(jié)轉(zhuǎn)動(dòng)極限的情況，在梯度投影法中設(shè)定與關(guān)節(jié)空間位置θ相關(guān)的指標(biāo)函數(shù)為：

38、

39、式中，h(θ)表示θi為機(jī)械臂連桿第i個(gè)關(guān)節(jié)的當(dāng)前轉(zhuǎn)角，θi表示第i個(gè)關(guān)節(jié)的轉(zhuǎn)角，θimax、θimin為第i個(gè)關(guān)節(jié)的上下轉(zhuǎn)角極限，ai表示關(guān)節(jié)的上下轉(zhuǎn)角極限中值，表示對(duì)h(θ)求偏導(dǎo)；

40、4.4、根據(jù)步驟4.2所得雅各比偽逆方程j#和步驟4.3的梯度投影法優(yōu)化基于雅各比偽逆的械臂逆運(yùn)動(dòng)學(xué)模型：

41、

42、式中，表示指標(biāo)函數(shù)h(θ)的偏導(dǎo)矩陣，in表示n階單位矩陣，k為優(yōu)化系數(shù)；

43、4.5、迭代求解步驟4.4所得基于雅各比偽逆的械臂逆運(yùn)動(dòng)學(xué)模型，得到關(guān)節(jié)轉(zhuǎn)角矩陣θi即機(jī)械臂的逆運(yùn)動(dòng)學(xué)最小范數(shù)。

44、進(jìn)一步的，上述方法還包括對(duì)得到的械臂的逆運(yùn)動(dòng)學(xué)最小范數(shù)進(jìn)行驗(yàn)證的步驟，具體的：

45、將求解得到的最小范數(shù)關(guān)節(jié)轉(zhuǎn)角矩陣θi帶入至步驟2得到的正運(yùn)動(dòng)學(xué)模型中，驗(yàn)證此時(shí)通過(guò)正運(yùn)動(dòng)學(xué)模型求解得到的末端執(zhí)行器位姿矩陣與期望位置之間的誤差是否在允許范圍內(nèi)，若是則表明逆運(yùn)動(dòng)學(xué)模型建立正確。

46、本發(fā)明通過(guò)基于經(jīng)典的雅各比偽逆法對(duì)求解過(guò)程進(jìn)行迭代，大幅減少了運(yùn)算量，保證機(jī)械臂關(guān)節(jié)在大部分工作區(qū)域內(nèi)運(yùn)動(dòng)速度最小。通過(guò)阻尼最小二乘法避開(kāi)運(yùn)動(dòng)過(guò)程中機(jī)械臂構(gòu)型的奇異位形，配合引入的梯度投影法避開(kāi)機(jī)械臂關(guān)節(jié)轉(zhuǎn)角極限位置，使求得的最小范數(shù)解具有較高的解算效率與精準(zhǔn)度。