冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)驅(qū)動(dòng)力優(yōu)化方法及軸組控制驗(yàn)證平臺(tái)的制作方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)控制技術(shù)領(lǐng)域，涉及一種冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)的 驅(qū)動(dòng)力優(yōu)化分配方法及軸組控制驗(yàn)證平臺(tái)。
【背景技術(shù)】
[0002] 仿生咀嚼機(jī)器人可以模擬人類(lèi)下頌運(yùn)動(dòng)行為、再現(xiàn)其生物力學(xué)環(huán)境，其研究成果 在口腔醫(yī)學(xué)和食品科學(xué)等領(lǐng)域具有廣闊應(yīng)用前景。人體口頌系統(tǒng)下頌驅(qū)動(dòng)肌肉數(shù)目明顯多 于下頌運(yùn)動(dòng)的自由度，所以口頌系統(tǒng)具有冗余驅(qū)動(dòng)的特性。在顳下頌關(guān)節(jié)及關(guān)節(jié)盤(pán)等組織 的約束下，左右兩側(cè)顳下頌各約束兩個(gè)自由度，下頌運(yùn)動(dòng)只具有四個(gè)自由度。冗余驅(qū)動(dòng)的特 性具有許多優(yōu)勢(shì)，可以改善工作空間中的奇異位形，解決奇異點(diǎn)導(dǎo)致的運(yùn)動(dòng)精度降低、剛度 減小和驅(qū)動(dòng)關(guān)節(jié)無(wú)法實(shí)施控制等問(wèn)題。但由于冗余驅(qū)動(dòng)力的存在，使得逆動(dòng)力學(xué)方程不存 在唯一解，這增大了并聯(lián)機(jī)構(gòu)控制的難度，因此，需要找到一種適合冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)力控 制的方法。
[0003] 冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)的控制方法一般有兩種，即運(yùn)動(dòng)學(xué)控制與動(dòng)力學(xué)控制。運(yùn)動(dòng)學(xué) 控制比較簡(jiǎn)單，一般應(yīng)用在精度不高，速度較慢的場(chǎng)合。冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)控制中易存在的 位置誤差以及瞬時(shí)反向驅(qū)動(dòng)，使得機(jī)構(gòu)中各個(gè)支鏈間容易產(chǎn)生較大的內(nèi)力，過(guò)大的內(nèi)力在 高速和大負(fù)載的情況下會(huì)造成桿件的嚴(yán)重破壞。針對(duì)冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)的研究主要集中在 機(jī)構(gòu)設(shè)計(jì)、奇異性分析等方面，通過(guò)機(jī)構(gòu)優(yōu)化來(lái)提高性能。然而，針對(duì)運(yùn)動(dòng)性能起關(guān)鍵作用 的動(dòng)力學(xué)控制研究較少，從某種程度上限制了冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)的性能提升及推廣應(yīng)用。 本發(fā)明旨在提出一種冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)驅(qū)動(dòng)力優(yōu)化分配方法，解決機(jī)構(gòu)內(nèi)力分配不均的問(wèn) 題。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明是為了解決冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)普遍存在的驅(qū)動(dòng)力分配不均，機(jī)構(gòu)內(nèi)力過(guò) 大，運(yùn)動(dòng)控制難等問(wèn)題，提出了一種冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)驅(qū)動(dòng)力優(yōu)化分配方法及對(duì)此方法的 軸組控制驗(yàn)證平臺(tái)。
[0005] 為了達(dá)到上述目的，本發(fā)明的技術(shù)方案為：
[0006] -種冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)驅(qū)動(dòng)力優(yōu)化分配方法，包括以下步驟：
[0007] 第一步，建立機(jī)器人坐標(biāo)系描述機(jī)器人的運(yùn)動(dòng)，建立3個(gè)坐標(biāo)系描述冗余驅(qū)動(dòng)并聯(lián) 機(jī)構(gòu)的運(yùn)動(dòng)，如圖7所示。含有點(diǎn)接觸高副的冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)具有4個(gè)自由度，廣義坐標(biāo)只 有4個(gè)，采用四個(gè)獨(dú)立位置參數(shù)X L，YL，隊(duì)，γ L描述動(dòng)平臺(tái)的位姿。
[0008] 第二步，建立運(yùn)動(dòng)學(xué)方程;建立并聯(lián)6PUS機(jī)構(gòu)支鏈及點(diǎn)接觸高副約束方程，并完成 冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)的運(yùn)動(dòng)學(xué)計(jì)算。
[0009] 第三步，建立動(dòng)力學(xué)方程;在運(yùn)動(dòng)學(xué)方程的基礎(chǔ)上，求解機(jī)器人的動(dòng)能與勢(shì)能，建 立冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)的動(dòng)力學(xué)模型。
[0010] 第四步，建立拉格朗日方程;采用第一組與第二組第一類(lèi)拉格朗日方程，通過(guò)引入 6個(gè)拉格朗日算子求解驅(qū)動(dòng)力/力矩，由于冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)特性，由四個(gè)方程求解6個(gè)未知 數(shù)，驅(qū)動(dòng)力/力矩有無(wú)限組解。
[0011] 第五步，偽逆優(yōu)化方法求解驅(qū)動(dòng)力；采用偽逆矩陣對(duì)六個(gè)拉格朗日算子λ,(i = 1， 2，···，6)進(jìn)行優(yōu)化，通過(guò)聯(lián)立方程消除拉格朗日算子，求得驅(qū)動(dòng)力/力矩的最小二范數(shù)解，得 到最優(yōu)驅(qū)動(dòng)力。六個(gè)拉格朗日算子M(i = l，2,…，6)由公式(26)獲得，但是很明顯只有四個(gè) 方程，因此，采用偽逆矩陣進(jìn)行優(yōu)化，得到六個(gè)拉格朗日算子的數(shù)值解。
[0012] 求得冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)的最優(yōu)化驅(qū)動(dòng)力，同時(shí)該方法可以推廣用在其他冗余驅(qū)動(dòng) 并聯(lián)機(jī)構(gòu)的驅(qū)動(dòng)力優(yōu)化分配問(wèn)題。
[0013] 一種運(yùn)行上述方法的冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)軸組控制驗(yàn)證平臺(tái)，包括冗余驅(qū)動(dòng)控制模 塊、軸組運(yùn)動(dòng)控制模塊、伺服驅(qū)動(dòng)、傳感系統(tǒng)及冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)本體，如圖3所示;所述冗 余驅(qū)動(dòng)控制模塊位于上位機(jī)PC中，冗余驅(qū)動(dòng)控制模塊運(yùn)行上述方法進(jìn)行驅(qū)動(dòng)力優(yōu)化分配， 發(fā)出控制指令;軸組運(yùn)動(dòng)控制模塊位于下位機(jī)軸組運(yùn)動(dòng)控制器中，軸組運(yùn)動(dòng)控制器與伺服 驅(qū)動(dòng)位于控制柜中，軸組運(yùn)動(dòng)控制器接收冗余驅(qū)動(dòng)控制模塊發(fā)出的控制指令，并將控制指 令通信至伺服驅(qū)動(dòng);傳感系統(tǒng)與軸組運(yùn)動(dòng)控制模塊相連，傳感系統(tǒng)采集冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu) 本體的數(shù)據(jù)，并將采集的數(shù)據(jù)反饋給軸組運(yùn)動(dòng)控制模塊，實(shí)現(xiàn)對(duì)伺服電機(jī)的閉環(huán)控制，保證 冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)電機(jī)的驅(qū)動(dòng)力矩;所述的采集的數(shù)據(jù)包括冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)的受力、速 度、位置等信息。
[0014] 所述冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)本體采用專(zhuān)利"一種具有仿生顳下頌關(guān)節(jié)的冗余驅(qū)動(dòng)咀嚼 機(jī)器人"，專(zhuān)利號(hào)：ZL201310602874.X。
[0015]所述的冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)本體包括6條PUS并聯(lián)支鏈和2個(gè)點(diǎn)接觸高副HKP(Higher Kinematic Pair)，機(jī)構(gòu)形式為6PUS-2HKP，如圖1所示。
[0016] 所述的一條PUS驅(qū)動(dòng)支鏈包括伺服電機(jī)1、聯(lián)軸器2、滾珠絲杠3、萬(wàn)向節(jié)4、移動(dòng)副5、 驅(qū)動(dòng)桿6和球副7;伺服電機(jī)1固定在上固定板9上，通過(guò)聯(lián)軸器2帶動(dòng)滾珠絲杠3轉(zhuǎn)動(dòng)，滾珠絲 杜3轉(zhuǎn)動(dòng)帶動(dòng)移動(dòng)副5沿導(dǎo)軌直線運(yùn)動(dòng)，移動(dòng)副5通過(guò)萬(wàn)向節(jié)4與驅(qū)動(dòng)桿6連接，驅(qū)動(dòng)桿6通過(guò) 球副7與動(dòng)平臺(tái)8連接，六條PUS驅(qū)動(dòng)支鏈最后固定在下固定板10上。通過(guò)電機(jī)驅(qū)動(dòng)6PUS機(jī) 構(gòu)，進(jìn)而驅(qū)動(dòng)動(dòng)平臺(tái)8在三維空間平動(dòng)或轉(zhuǎn)動(dòng)。
[0017] 所述的一個(gè)HKP包括約束塊12和約束桿13;約束塊12下表面為約束曲面，約束桿13 安裝在動(dòng)平臺(tái)8上，約束桿13與約束曲面在運(yùn)動(dòng)過(guò)程中保持接觸，形成點(diǎn)接觸高副;兩個(gè)點(diǎn) 接觸高副可以約束動(dòng)平臺(tái)兩個(gè)自由度。因此，整個(gè)6PUS-2HKP冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)是由六臺(tái)電 機(jī)驅(qū)動(dòng)，只有四個(gè)自由度的冗余驅(qū)動(dòng)機(jī)構(gòu)，其運(yùn)動(dòng)特性與一般的并聯(lián)機(jī)構(gòu)有明顯差異。
[0018] 本發(fā)明提出一種冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)的軸組控制驗(yàn)證平臺(tái)，驅(qū)動(dòng)力優(yōu)化分配方法可 適用于冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)控制領(lǐng)域，能夠解決冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)普遍存在的驅(qū)動(dòng)力分 配不均，機(jī)構(gòu)內(nèi)力過(guò)大，運(yùn)動(dòng)控制難以及由于采用點(diǎn)接觸高副從而增加系統(tǒng)控制的復(fù)雜性 等問(wèn)題，可以對(duì)冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)驅(qū)動(dòng)力優(yōu)化分配方法進(jìn)行實(shí)驗(yàn)驗(yàn)證。
【附圖說(shuō)明】
[0019] 圖1是冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)原理圖。
[0020] 圖2是冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)驅(qū)動(dòng)力優(yōu)化分配方法流程圖。
[0021 ]圖3是冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)軸組控制驗(yàn)證平臺(tái)示意圖。
[0022]圖4是冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)側(cè)視圖。
[0023]圖5是冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)正視圖。
[0024]圖6是點(diǎn)接觸高副HKP示意圖。
[0025] 圖7是冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)坐標(biāo)系示意圖。
[0026] 圖中：1伺服電機(jī);2聯(lián)軸器;3滾珠絲杠;4萬(wàn)向節(jié);5移動(dòng)副;6驅(qū)動(dòng)桿;7球副；8動(dòng)平 臺(tái)；9上固定板;10下固定板;11點(diǎn)接觸尚副位置;12約束塊;13約束桿。
【具體實(shí)施方式】
[0027]下面結(jié)合技術(shù)方案和附圖詳細(xì)敘述本發(fā)明的具體實(shí)施內(nèi)容。
[0028]所述冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)驅(qū)動(dòng)力優(yōu)化分配方法流程圖見(jiàn)圖2,具體步驟如下：
[0029]第一步，建立機(jī)器人坐標(biāo)系
[0030]建立3個(gè)坐標(biāo)系描述冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)的運(yùn)動(dòng)，如圖7所示;三個(gè)坐標(biāo)系為基坐標(biāo) 系Ob-XbYbZb、瞬時(shí)坐標(biāo)系Ol-XlYlZl和動(dòng)平臺(tái)坐標(biāo)系Om-XmYmZm。所述的Ob-XbYbZb在基座上;所 述的Ol-XlYlZl在左側(cè)點(diǎn)接觸高副的約束桿13上。0 L-XL YlZl隨機(jī)器人的運(yùn)動(dòng)而沿約束曲面滑 動(dòng)或轉(zhuǎn)動(dòng)，在0l-XlYlZl中，Zl始終垂直于約束曲面，Xl相切于約束曲面，Yl與兩者正交;所述 的0μ-ΚμΥμΖμ設(shè)置在初始位置瞬時(shí)坐標(biāo)系Σ b的位置。
[0031 ] 通常情況下，機(jī)器人的運(yùn)動(dòng)一般用6個(gè)運(yùn)動(dòng)學(xué)參數(shù)描述，即：XL，YL，ZL，，&，γ L。由 于點(diǎn)接觸高副的存在，限制了下頌結(jié)構(gòu)的位置和姿態(tài)部分自由度。含有點(diǎn)接觸高副的冗余 驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)具有4個(gè)自由度，廣義坐標(biāo)有4個(gè)，采用四個(gè)獨(dú)立位置參數(shù)Xl，Yl，&，γ l來(lái)描述 位姿。
[0032] 第二步，建立運(yùn)動(dòng)學(xué)方程
[0033] 建立并聯(lián)6PUS機(jī)構(gòu)支鏈及點(diǎn)接觸高副約束方程，并完成冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)的運(yùn)動(dòng) 學(xué)計(jì)算。
[0034] 對(duì)于6自由度的并聯(lián)機(jī)器人來(lái)說(shuō)，運(yùn)動(dòng)學(xué)一般可以寫(xiě)成：
[0035] Qi = fi(XL,YL,ZL,aL,pL, yL) (1)
[0036] 其中及二說(shuō)^:^…他^是各關(guān)節(jié)空間參數(shù)^二⑴幾義…及^-是末端執(zhí) 行器直角坐標(biāo)空間參數(shù)。
[0037] 2.1點(diǎn)接觸高副HKP約束方程
[0038]左右兩側(cè)HKP約束曲面的約束方程的數(shù)學(xué)一般表達(dá)式分別為
[0039] fL(XL,YL,ZL)=0 (2)
[0040] fR(XR,YR,ZR)=0 (3)
[00411 其中，fL和fR分別是左右兩側(cè)約束方程，Xl，Yl，Z L為左側(cè)HKP的坐標(biāo)，XR，YR，ZR為右側(cè) HKP的坐標(biāo)。
[0042] 2.2并聯(lián)機(jī)構(gòu)6PUS支鏈約束方程
[0043]機(jī)器人單條支鏈?zhǔn)疽鈭D見(jiàn)圖7，。
[0044] 向量BiMi存在如下關(guān)系：
[0045] BiMi = BiCi+CiMi (4)
[0046] 根據(jù)上述約束條件，每個(gè)滑塊的移動(dòng)量Qi的計(jì)算公式：
[0047]
[0048] 其中，G是移動(dòng)副的位置，81每條支鏈支撐板的固定位置，U是每條驅(qū)動(dòng)桿的長(zhǎng)度， 肌肉連接點(diǎn)Mi在Ob-XbYbZb的坐標(biāo)可以通過(guò)坐標(biāo)轉(zhuǎn)換矩陣％獲得。
[0049] 動(dòng)平臺(tái)8的6個(gè)位置參數(shù)關(guān)系表達(dá)式為：
[0050]
[0051] 其中上幾，2[，丸，&，丫[為6個(gè)位置參數(shù)上幾，&，丫[為4個(gè)獨(dú)立位置參數(shù)0 [0052]由上述公式(5)和(6)求得冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)的運(yùn)動(dòng)學(xué)關(guān)系表達(dá)式為：
[0053]
[0054]其中，T為運(yùn)動(dòng)學(xué)變換矩陣，TM為位置變換矩陣；由公式(7)可知，已知4個(gè)獨(dú)立位置 參數(shù)XL，YL，i3L，yL，用％表示，便可以通過(guò)運(yùn)動(dòng)學(xué)計(jì)算得到6個(gè)滑塊的位移變化量(^(? = 1， 2,…，6) 〇
[0055] 即對(duì)于冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)來(lái)說(shuō)，
[0056] [Qi]6xi = [Τμ]θχ4 · [Wm]4xi (8)
[0057] 第三步，建立動(dòng)力學(xué)方程
[0058]在運(yùn)動(dòng)學(xué)方程的基礎(chǔ)上，求解機(jī)器人的動(dòng)能與勢(shì)能，建立冗余驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)的動(dòng) 力學(xué)模型。
[0059] 3.1機(jī)器人動(dòng)能計(jì)算
[0060]動(dòng)能計(jì)算如下式：
[0061] 1=1

[0062] 其中，KP是動(dòng)平臺(tái)的動(dòng)能，Kci是滑塊的動(dòng)能，Kcmi是支鏈驅(qū)動(dòng)桿CiMi的動(dòng)能，具體各 項(xiàng)計(jì)算加下.
[0063]
[0064] 其中，mP是動(dòng)平臺(tái)的質(zhì)量，vT 是動(dòng)平臺(tái)在固定坐標(biāo)系下的平移速度，
I動(dòng)平臺(tái)在固定坐標(biāo)系下的轉(zhuǎn)動(dòng)速度，且滿(mǎn)足
[0065]
(1 1)
[0066] 1邱是動(dòng)平臺(tái)在固定坐標(biāo)系下的慣性矩陣，計(jì)算如下：
[0067] IPB = BRp · IPM · bRpt (12)
[0068] 其中，IpM是下頌機(jī)構(gòu)關(guān)于點(diǎn)在動(dòng)坐標(biāo)系中的慣性矩陣，IpB是下頌機(jī)構(gòu)關(guān)于點(diǎn) 在固定坐標(biāo)系中的慣性矩陣。
[0069]滑塊動(dòng)能如下：
[0070]
(13)
[0071] 其中，Vs是滑塊的速度。
[0072]驅(qū)動(dòng)桿的動(dòng)能推導(dǎo)：
[0073]
(14)
[0074] 其中，是桿的重量，V? i是驅(qū)動(dòng)桿質(zhì)量中心的速度矢量，I? b是桿在固定坐標(biāo)系 下的慣性矩陣，可以用下式計(jì)算：
[0075] IcmB = BRLi · IcmM · BRLiT (1