一種腱驅(qū)動(dòng)機(jī)械手位置和腱張力的混合控制方法及控制裝置制造方法
【專(zhuān)利摘要】本發(fā)明公開(kāi)一種腱驅(qū)動(dòng)機(jī)械手位置和腱張力的混合控制方法及控制裝置，方法包括：進(jìn)行機(jī)械手自由空間中的關(guān)節(jié)角位置路徑規(guī)劃，由手指指尖當(dāng)前位置和物體期望接觸點(diǎn)確定運(yùn)動(dòng)軌跡，設(shè)定期望關(guān)節(jié)角位置；由操作對(duì)象特性設(shè)定期望接觸力及腱張力閾值；將腱張力值與閾值比較，小于力閾值時(shí)機(jī)械手位于自由空間，比較期望角位置和實(shí)際測(cè)量關(guān)節(jié)角位置之間偏差，采用位置控制律得到位置補(bǔ)償量，把補(bǔ)償量輸入到腱驅(qū)動(dòng)器進(jìn)行控制；腱張力大于閾值時(shí)機(jī)械手位于約束空間，對(duì)規(guī)劃的關(guān)節(jié)力矩進(jìn)行腱張力分配，比較期望腱張力和實(shí)際腱張力差別，根據(jù)兩者差值由張力控制器把力的誤差轉(zhuǎn)化為位置補(bǔ)償量，把該補(bǔ)償量與位置通道的位置補(bǔ)償量求和后輸入到腱驅(qū)動(dòng)器進(jìn)行控制。
【專(zhuān)利說(shuō)明】一種腱驅(qū)動(dòng)機(jī)械手位置和腱張力的混合控制方法及控制裝

【技術(shù)領(lǐng)域】
:
[0001]本發(fā)明涉及一種腱驅(qū)動(dòng)機(jī)械手位置和腱張力的混合控制方法及控制裝置，其屬于機(jī)器人控制領(lǐng)域。

【背景技術(shù)】
:
[0002]機(jī)械手為模仿手、臂的特定功能的一種自動(dòng)機(jī)械，因此泛指機(jī)械臂、末端執(zhí)行器、靈巧手手指等多關(guān)節(jié)多連桿操作機(jī)構(gòu)。腱驅(qū)動(dòng)型機(jī)械手是利用腱繩進(jìn)行傳動(dòng)的機(jī)械手，允許驅(qū)動(dòng)器放置于機(jī)械手結(jié)構(gòu)體的外部，可以減小機(jī)械手體積和重量，從而提高了機(jī)械手的靈巧性，同時(shí)也在驅(qū)動(dòng)器選型方面給機(jī)構(gòu)設(shè)計(jì)者提供了更多靈活性。
[0003]由于腱只可以傳遞張力，因此為了獲得完全獨(dú)立的自由度控制，必須要保證驅(qū)動(dòng)器的數(shù)量多于自由度的個(gè)數(shù)。有多種腱配置方式，在配置合理的情況下，N+1型腱能夠獨(dú)立控制N個(gè)D0F，同時(shí)保證腱具有正張力。該腱配置方式簡(jiǎn)化了機(jī)構(gòu)，但由于這種方式在減少腱繩數(shù)量的同時(shí)引入了關(guān)節(jié)位置和腱繩的耦合問(wèn)題，因此控制器的設(shè)計(jì)非常復(fù)雜。在裝配應(yīng)用中機(jī)械手需要與非結(jié)構(gòu)環(huán)境物理接觸，因此機(jī)械手的力矩控制能力非常重要。研究者們提出多種控制策略來(lái)解決耦合腱驅(qū)動(dòng)機(jī)械手的力矩控制問(wèn)題。這些控制策略由張力分配算法和控制律兩部分組成。張力分配算法是確定一組腱張力的過(guò)程，目的是產(chǎn)生期望的一組關(guān)節(jié)力矩，同時(shí)解決驅(qū)動(dòng)的冗余問(wèn)題。該冗余可以生成張力的零空間，用來(lái)保證所有腱具有正張力。
[0004]目前已有的控制律根據(jù)是否使用腱張力反饋進(jìn)行分類(lèi)。無(wú)腱張力反饋的控制方法包括計(jì)算力矩法、其它能夠預(yù)測(cè)或估計(jì)系統(tǒng)參數(shù)的智能方法，這些方法對(duì)于能夠精確建模運(yùn)動(dòng)和力矩關(guān)系的系統(tǒng)是可行的，但機(jī)械手需要抓握或操作的是各種可能的物體，其接觸表面特性差別很大，因此這些方法只能獲得粗略的腱張力控制，經(jīng)常產(chǎn)生更高的內(nèi)張力，導(dǎo)致腱的磨損，增加摩擦力降低性能，因此這些方法難以實(shí)現(xiàn)精確的操作。采用張力反饋的腱空間控制器忽略腱的動(dòng)力學(xué)，利用張力分配算法將期望的關(guān)節(jié)力矩轉(zhuǎn)換為期望的腱張力，然后為每根腱使用獨(dú)立的張力調(diào)節(jié)器。例如Salisbury和Craig在Stanford/JPL手上實(shí)現(xiàn)了腱空間控制律，Starr在Stanford/JPL手通過(guò)采用腱-導(dǎo)管間歇模型實(shí)現(xiàn)了類(lèi)似算法，2N型P0STECH手和Utah/MIT手等靈巧手也采用了該算法。然而采用該種方式的控制器在手指動(dòng)力學(xué)中引入了瞬態(tài)耦合，即某個(gè)關(guān)節(jié)的控制或擾動(dòng)可能導(dǎo)致另一個(gè)關(guān)節(jié)的不期望的響應(yīng)。
[0005]為了穩(wěn)定可靠地抓握物體，腱驅(qū)動(dòng)機(jī)械手的控制應(yīng)能合理的調(diào)節(jié)關(guān)節(jié)的柔順性和力矩。相對(duì)于傳統(tǒng)的齒輪傳動(dòng)，腱驅(qū)動(dòng)的難點(diǎn)在于設(shè)計(jì)一個(gè)控制器使其能夠按照期望的位置運(yùn)動(dòng)同時(shí)保證腱的張力保持在期望的范圍之內(nèi)。腱只能傳遞張力，而且有一定的延遲，以及機(jī)構(gòu)對(duì)腱的摩擦以及腱的其它未建模動(dòng)力學(xué)等因素也都影響著腱驅(qū)動(dòng)機(jī)械手操作控制算法的設(shè)計(jì)。總的來(lái)說(shuō)，現(xiàn)有的控制算法，仍不能完全滿足機(jī)械手靈巧操作和強(qiáng)力抓持操作的要求。
【發(fā)明內(nèi)容】
:
[0006]本發(fā)明的目的:針對(duì)腱驅(qū)動(dòng)機(jī)械手，提供一種力和位置控制方法和設(shè)備，該方法能夠減小腱運(yùn)動(dòng)和關(guān)節(jié)運(yùn)動(dòng)之間耦合的影響，提高系統(tǒng)的動(dòng)態(tài)特性和位置精度，減小從自由空間到約束空間的沖擊，有效的實(shí)現(xiàn)腱驅(qū)動(dòng)機(jī)械手的操作控制，提高機(jī)械手的靈巧性。
[0007]本發(fā)明采用如下技術(shù)方案:一種腱驅(qū)動(dòng)機(jī)械手位置和腱張力的混合控制方法，其包括如下步驟:
[0008]步驟1，由路徑規(guī)劃模塊進(jìn)行機(jī)械手自由空間中的關(guān)節(jié)角位置路徑規(guī)劃，根據(jù)手指指尖當(dāng)前位置和物體的期望接觸點(diǎn)確定從初始點(diǎn)到接觸點(diǎn)的運(yùn)動(dòng)軌跡，并且設(shè)定期望關(guān)節(jié)角位置；根據(jù)操作對(duì)象特性設(shè)定期望接觸力，并且設(shè)置腱張力閾值；
[0009]步驟2，根據(jù)腱張力傳感器測(cè)量得到的腱張力值與腱張力閾值比較確定靈巧手和物體之間的接觸狀態(tài)，若腱張力值小于腱張力閾值說(shuō)明機(jī)械手位于自由空間，此時(shí)由位置比較模塊比較期望關(guān)節(jié)角位置和實(shí)際測(cè)量關(guān)節(jié)角位置之間的偏差，并且由位置解算模塊解算為腱的長(zhǎng)度偏差，由一個(gè)位置控制模塊通過(guò)合適的控制律得到腱位置偏差，把該偏差指令輸送給鍵驅(qū)動(dòng)器進(jìn)行控制；
[0010]步驟3，若腱張力值大于閾值說(shuō)明機(jī)械手位于約束空間，此時(shí)首先由接觸力到關(guān)節(jié)力矩轉(zhuǎn)換模塊通過(guò)力雅可比矩陣把期望的接觸力轉(zhuǎn)換為關(guān)節(jié)力矩；進(jìn)一步由腱張力分配模塊轉(zhuǎn)換為腱空間的力，同時(shí)進(jìn)行腱張力分配；然后由腱張力比較器比較期望腱張力和實(shí)際腱張力的差別，根據(jù)這個(gè)偏差由張力控制模塊基于合適的控制律把張力的偏差轉(zhuǎn)化為位置的偏差，把該偏差與位置控制通道的位置偏差求和后輸送給驅(qū)動(dòng)器進(jìn)行控制。
[0011]本發(fā)明還采用如下技術(shù)方案:一種腱驅(qū)動(dòng)機(jī)械手位置和腱張力的混合控制裝置，其包括控制器、與控制器相連的腱驅(qū)動(dòng)器、與腱驅(qū)動(dòng)器相連的腱繩、安裝于腱繩上的腱張力傳感器、腱驅(qū)動(dòng)機(jī)械手單指機(jī)構(gòu)及關(guān)節(jié)角位置傳感器，所述腱張力傳感器和關(guān)節(jié)角位置傳感器與控制器相連進(jìn)而將采集的信息發(fā)送給控制器，所述控制器發(fā)送控制指令給腱驅(qū)動(dòng)器，所述控制器包括路徑規(guī)劃模塊、期望接觸力模塊、位置解算模塊、位置控制模塊、位置比較模塊、接觸力到關(guān)節(jié)力矩轉(zhuǎn)換模塊、腱張力分配模塊及張力控制模塊，路徑規(guī)劃模塊輸入手指指尖當(dāng)前位置和物體的期望接觸點(diǎn)，路徑規(guī)劃模塊輸出期望關(guān)節(jié)角位置到位置比較模塊和腱張力分配模塊，位置比較模塊的另一個(gè)輸入為實(shí)際測(cè)量關(guān)節(jié)角位置，位置比較模塊輸出位置偏差到位置結(jié)算模塊，位置解算模塊輸出到位置控制模塊，期望接觸力輸入到接觸力到關(guān)節(jié)力矩轉(zhuǎn)換模塊，轉(zhuǎn)換結(jié)果輸出到轉(zhuǎn)換腱張力分配模塊，腱張力模塊輸出的結(jié)果和腱張力傳感器測(cè)量的結(jié)果進(jìn)行比較，結(jié)果輸出到張力控制模塊，張力控制的輸出和位置控制的輸出求和然后輸出到腱驅(qū)動(dòng)控制器，控制機(jī)構(gòu)動(dòng)作。
[0012]所述腱驅(qū)動(dòng)機(jī)械手單指機(jī)構(gòu)包括側(cè)擺關(guān)節(jié)、基關(guān)節(jié)、中關(guān)節(jié)、連桿機(jī)構(gòu)、基座、近指節(jié)、中指節(jié)及遠(yuǎn)指節(jié)，所述基座與近指節(jié)通過(guò)基關(guān)節(jié)轉(zhuǎn)動(dòng)連接，近指節(jié)與中指節(jié)通過(guò)中關(guān)節(jié)轉(zhuǎn)動(dòng)連接，遠(yuǎn)指節(jié)通過(guò)連桿機(jī)構(gòu)與中指節(jié)耦合，所述腱繩的另一端依次穿過(guò)基座、近指節(jié)、中指節(jié)、遠(yuǎn)指節(jié)并與遠(yuǎn)指節(jié)固接，所述腱張力傳感器位于鍵繩上的鍵驅(qū)動(dòng)器和腱驅(qū)動(dòng)機(jī)械手單指機(jī)構(gòu)中間的位置上，所述關(guān)節(jié)角位置傳感器固定在側(cè)擺關(guān)節(jié)、基關(guān)節(jié)及中關(guān)節(jié)上。
[0013]本發(fā)明具有如下有益效果:本發(fā)明設(shè)計(jì)了一種基于腱張力反饋的腱驅(qū)動(dòng)機(jī)械手操作控制的方法，該方法可有效減輕機(jī)械手接觸物體時(shí)的沖擊，并且能夠?qū)崿F(xiàn)穩(wěn)定的抓取。該方法可進(jìn)一步應(yīng)用于以腱為傳動(dòng)方式的多關(guān)節(jié)機(jī)械臂、靈巧手單指等多關(guān)節(jié)多連桿機(jī)構(gòu)中，因此該實(shí)現(xiàn)方法具有廣闊的應(yīng)用前景。

【專(zhuān)利附圖】

【附圖說(shuō)明】
:
[0014]圖1為膜驅(qū)動(dòng)機(jī)械手單指系統(tǒng)組成圖。
[0015]圖2為腱驅(qū)動(dòng)機(jī)械手單指位置和腱張力混合控制框圖。
[0016]圖3為腱繩運(yùn)動(dòng)狀態(tài)簡(jiǎn)化圖。
[0017]其中:
[0018]1-控制器；2_腱驅(qū)動(dòng)器；3_腱繩；4_腱張力傳感器；5_腱驅(qū)動(dòng)機(jī)械手單指機(jī)構(gòu)；
6-關(guān)節(jié)角位置傳感器；11_側(cè)擺關(guān)節(jié)；12_基關(guān)節(jié)；13_中關(guān)節(jié)；14_連桿機(jī)構(gòu)；15_基座；16-近指節(jié)；17_中指節(jié)；18_遠(yuǎn)指節(jié)。

【具體實(shí)施方式】
:
[0019]機(jī)械手泛指機(jī)械臂、末端執(zhí)行器、靈巧手手指等多關(guān)節(jié)多連桿操作機(jī)構(gòu)，這兒主要采用了基于腱驅(qū)動(dòng)的靈巧手單手指的柔順控制為例結(jié)合【專(zhuān)利附圖】

【附圖說(shuō)明】腱驅(qū)動(dòng)機(jī)械手位置和腱張力的混合控制裝置及其控制方法的具體實(shí)施方法。但并不因此將本發(fā)明限制在實(shí)施范圍之內(nèi)。
[0020]如圖1所示，腱驅(qū)動(dòng)機(jī)械手單指系統(tǒng)主要由控制器1、與控制器I相連的腱驅(qū)動(dòng)器
2、與腱驅(qū)動(dòng)器2相連的腱繩3、安裝于腱繩3上的腱張力傳感器4、腱驅(qū)動(dòng)機(jī)械手單指機(jī)構(gòu)5及關(guān)節(jié)角位置傳感器6組成。其中腱驅(qū)動(dòng)機(jī)械手單指機(jī)構(gòu)5包括側(cè)擺關(guān)節(jié)11、基關(guān)節(jié)12、中關(guān)節(jié)13、連桿機(jī)構(gòu)14、基座15、近指節(jié)16、中指節(jié)17及遠(yuǎn)指節(jié)18。其中基座15與近指節(jié)16通過(guò)基關(guān)節(jié)12轉(zhuǎn)動(dòng)連接，近指節(jié)16與中指節(jié)17通過(guò)中關(guān)節(jié)13轉(zhuǎn)動(dòng)連接，遠(yuǎn)指節(jié)18通過(guò)連桿機(jī)構(gòu)14與中指節(jié)17耦合。其通過(guò)采用“N+1 ”型腱配置方式，具有3個(gè)獨(dú)立自由度，其主要實(shí)現(xiàn)繞側(cè)擺關(guān)節(jié)11、基關(guān)節(jié)12和中關(guān)節(jié)13的旋轉(zhuǎn)運(yùn)動(dòng)。其中腱繩3共包括有四根，且腱繩3的另一端均依次穿過(guò)基座15、近指節(jié)16、中指節(jié)17、遠(yuǎn)指節(jié)18并與遠(yuǎn)指節(jié)18固接。在每一根鍵繩3上位于鍵驅(qū)動(dòng)器2和腱驅(qū)動(dòng)機(jī)械手單指機(jī)構(gòu)5中間的位置上均裝有腱張力傳感器4，其中關(guān)節(jié)角位置傳感器6固定在側(cè)擺關(guān)節(jié)11、基關(guān)節(jié)12及中關(guān)節(jié)13上。腱張力傳感器4和關(guān)節(jié)角位置傳感器6與控制器I相連進(jìn)而將采集的信息發(fā)送給控制器1，控制器I發(fā)送控制指令給腱驅(qū)動(dòng)器2。
[0021]如圖2所示，腱驅(qū)動(dòng)機(jī)械手單指系統(tǒng)的控制器I包括路徑規(guī)劃模塊、期望接觸力模塊、位置解算模塊、位置控制模塊、位置比較模塊、接觸力到關(guān)節(jié)力矩轉(zhuǎn)換模塊、腱張力分配模塊、張力控制模塊。其中控制器接收來(lái)自關(guān)節(jié)角位置傳感器、腱張力傳感器的信息和期望的位置和接觸力信息，然后控制器各模塊按照后面所提步驟的調(diào)度運(yùn)行，通過(guò)操作處理把位置控制模塊和張力控制模塊的輸出求和后作為腱位置偏差輸入到腱驅(qū)動(dòng)器。腱驅(qū)動(dòng)器包括無(wú)刷直流電機(jī)和把旋轉(zhuǎn)運(yùn)動(dòng)轉(zhuǎn)換為直線運(yùn)動(dòng)的滾珠絲杠、螺母等組件，根據(jù)控制器輸入的位置偏差驅(qū)動(dòng)腱繩拉伸，導(dǎo)致腱驅(qū)動(dòng)機(jī)械手單指機(jī)構(gòu)相應(yīng)運(yùn)動(dòng)，實(shí)現(xiàn)所需的位置和力的調(diào)整。
[0022]圖2控制器中路徑規(guī)劃模塊為機(jī)械手單指的運(yùn)動(dòng)提供期望關(guān)節(jié)角位置Θ d ;位置解算模塊實(shí)現(xiàn)關(guān)節(jié)空間到腱空間的轉(zhuǎn)換運(yùn)算；位置控制模塊對(duì)機(jī)械手單指的腱位置進(jìn)行修正，輸出量ΛX作為腱端的位置偏差輸入到腱驅(qū)動(dòng)器中；腱驅(qū)動(dòng)器有4個(gè)輸入端，為位置控制器輸出的腱端位置偏差量V，控制器的輸入量為4個(gè)關(guān)節(jié)角位置傳感器給出的角位移量0)3和腱張力傳感器張力值fb。對(duì)于當(dāng)前的腱驅(qū)動(dòng)機(jī)械手單指系統(tǒng)，變量9d、0b、0r> 0e都是二維的列向量，fb> fd> Δ X都是四維的列向量。
[0023]因?yàn)殡熘荒軅鬟f張力，因此單獨(dú)的位置控制很難保證腱張力時(shí)刻大于0，而且腱本身有一定的柔韌性，對(duì)力的傳遞有遲滯性?？刂破髟诔跏紩r(shí)刻時(shí)，給腱繩施加力A ^ fmin(其中4為4根腱的張力，fmin主要為保證腱在初始時(shí)刻具有預(yù)張緊。)保證腱在初始時(shí)刻具有預(yù)張緊，這時(shí)腱可以等效為剛體。控制器各模塊按照如下具體步驟運(yùn)行。
[0024]步驟1，由路徑規(guī)劃模塊進(jìn)行機(jī)械手單指自由空間中的關(guān)節(jié)角位置路徑規(guī)劃，根據(jù)手指指尖當(dāng)前位置和物體的期望接觸點(diǎn)確定從初始點(diǎn)到接觸點(diǎn)的運(yùn)動(dòng)軌跡，并且設(shè)定期望關(guān)節(jié)角位置；根據(jù)操作對(duì)象特性設(shè)定期望接觸力，并且設(shè)定腱張力閾值。
[0025]步驟2，根據(jù)腱張力傳感器測(cè)量得到的腱張力值與腱張力閾值比較確定靈巧手和物體之間的接觸狀態(tài)，若腱張力值小于力閾值說(shuō)明機(jī)械手位于自由空間，此時(shí)由位置比較模塊比較期望關(guān)節(jié)角位置和實(shí)際測(cè)量關(guān)節(jié)角位置之間的偏差，并且由位置解算模塊解算為腱的長(zhǎng)度偏差，由一個(gè)位置控制模塊通過(guò)合適的控制得到腱位置偏差，把該偏差指令輸送給鍵驅(qū)動(dòng)器進(jìn)行控制；
[0026]步驟3，若腱張力值大于閾值說(shuō)明機(jī)械手位于約束空間，此時(shí)首先由接觸力到關(guān)節(jié)力矩轉(zhuǎn)換模塊通過(guò)力雅可比矩陣把期望的接觸力轉(zhuǎn)換為關(guān)節(jié)力矩；進(jìn)一步由腱張力分配模塊轉(zhuǎn)換為腱空間的力，同時(shí)進(jìn)行腱張力分配；然后由腱張力比較器比較期望腱張力和實(shí)際腱張力的差別，根據(jù)這個(gè)偏差由張力控制模塊基于合適的控制律把張力的偏差轉(zhuǎn)化為位置的偏差，把該偏差與位置控制通道的位置偏差求和后輸送給驅(qū)動(dòng)器進(jìn)行控制。
[0027]控制器各模塊的實(shí)現(xiàn)如下:
[0028]I)路徑規(guī)劃模塊
[0029]機(jī)械手手指運(yùn)動(dòng)路徑規(guī)劃是指根據(jù)手指指尖當(dāng)前位置和物體的期望接觸點(diǎn)確定從初始點(diǎn)到接觸點(diǎn)的運(yùn)動(dòng)軌跡。目前路徑規(guī)劃主要分為:笛卡爾空間的路徑規(guī)劃和關(guān)節(jié)空間的路徑規(guī)劃。對(duì)于多關(guān)節(jié)的手指，指尖的笛卡爾空間運(yùn)動(dòng)軌跡很難用時(shí)間的變量表示，并且計(jì)算比較復(fù)雜。相比而言，在關(guān)節(jié)空間，除了耦合關(guān)節(jié)外，其他的關(guān)節(jié)的角位移都是相對(duì)獨(dú)立的，可以分別進(jìn)行路徑規(guī)劃，規(guī)劃的方程相對(duì)簡(jiǎn)單而且計(jì)算量小、實(shí)時(shí)性好，所以這里采用了關(guān)節(jié)空間的路徑規(guī)劃。機(jī)械手自由空間中的關(guān)節(jié)角位置路徑規(guī)劃可采用一般多關(guān)節(jié)機(jī)械臂的關(guān)節(jié)角位置路徑規(guī)劃類(lèi)似的方法進(jìn)行，如拋物線過(guò)渡的線性插值方法、三次多項(xiàng)式路徑規(guī)劃、加加速度有界路徑規(guī)劃等等。
[0030]為了使靈巧手手指能夠順利的到達(dá)目的位置，要求規(guī)劃路徑的導(dǎo)數(shù)，即速度，有一個(gè)增加、不變、遞減的過(guò)程。本實(shí)施方案采用拋物線過(guò)渡的線性插值方法，該方法具有運(yùn)動(dòng)特性好，易于實(shí)現(xiàn)等優(yōu)點(diǎn)。規(guī)劃的方程為:
θΜ+-α?-\ 0<i<ih

2
[0031]θ?{?)=< 0db+atb{t-tb\ tb<t<tf-tb⑴
I ，

^dj -t)", tf —tb <t <tf
[0032]式中Θ d(l是關(guān)節(jié)的初始角位移，Θ df是關(guān)節(jié)最后到達(dá)的期望角位移，tf是到達(dá)期望位置的時(shí)間，a是加速度。在實(shí)際中，先估計(jì)待抓物體的笛卡爾位置，然后通過(guò)逆運(yùn)動(dòng)學(xué)解出各關(guān)節(jié)所要到達(dá)的角位移，最后根據(jù)式(I)對(duì)手指各關(guān)節(jié)進(jìn)行路徑規(guī)劃。
[0033]2)位置解算模塊
[0034]位置解算模塊的輸入量為:
[0035]Θ e = Θ d- Θ b(2)
[0036]其中表規(guī)劃的期望關(guān)節(jié)角位置，關(guān)節(jié)角位置傳感器反饋的關(guān)節(jié)角位置。
[0037]因該靈巧手采用“N+1 ”型腱驅(qū)動(dòng)，其位置控制有別于傳統(tǒng)的齒輪傳動(dòng)結(jié)構(gòu)，4根腱的位置對(duì)應(yīng)3個(gè)獨(dú)立關(guān)節(jié)，本實(shí)施方案采用幾何建模的方法實(shí)現(xiàn)關(guān)節(jié)角位置到腱位置的解算，即在各關(guān)節(jié)處對(duì)腱的運(yùn)動(dòng)路徑做幾何簡(jiǎn)化，得到每根腱的位置與關(guān)節(jié)角位置的關(guān)系。
[0038]對(duì)于如圖3所示的連桿由虛線位置轉(zhuǎn)到實(shí)線位置，關(guān)節(jié)轉(zhuǎn)動(dòng)Λ Θ角，腱繩Χ3由虛線位置運(yùn)動(dòng)到實(shí)線位置，關(guān)節(jié)處對(duì)應(yīng)的腱繩長(zhǎng)度由I1變?yōu)镮2，根據(jù)余弦定理可知:
I1 - +/;'1 -2/,/; cos(a + 句
[0039]\ 1-
/]=七2+r22-2WOS ⑷⑴
[0040]則Δ X3 = Δ I = I2-11O
[0041]其它三根腱采用同樣的方法，這樣就得到了關(guān)節(jié)角位置到四根腱的位置的轉(zhuǎn)換方程。
[0042]3)位置控制模塊
[0043]位置控制模塊主要是通過(guò)設(shè)計(jì)合適的控制器以滿足待定的位置精度要求?？刂破骺梢圆捎貌煌目刂扑惴?如滑模變結(jié)構(gòu)、PID、神經(jīng)網(wǎng)絡(luò)、模糊以及它們的組合算法等)構(gòu)建。其中，PID控制具有原理簡(jiǎn)單、易于實(shí)現(xiàn)，魯棒性強(qiáng)和適應(yīng)范圍廣等特點(diǎn)，是一種應(yīng)用廣泛的控制規(guī)律。
[0044]得到腱位移的ΛΧ(Λ Θ )后，為了改善位置控制的調(diào)節(jié)時(shí)間，增加系統(tǒng)阻尼，減少超調(diào)量，本實(shí)施方案采用了一個(gè)H)控制器，根據(jù)公式(3)最終得到位置控制律表達(dá)式如下:
[0045]Axn=KrMA,-&b) + K,n S ?(4)


at
[0046]Kpl為ro控制器增益，Kdl為ro控制器微分項(xiàng)增益。
[0047]前述控制算法滑模變結(jié)構(gòu)、PID、神經(jīng)網(wǎng)絡(luò)、模糊以及它們的組合算法等也可實(shí)現(xiàn)類(lèi)似的控制功能，這里不再詳細(xì)描述。
[0048]4)接觸力到關(guān)節(jié)力矩轉(zhuǎn)換模塊
[0049]在腱驅(qū)動(dòng)靈巧手系統(tǒng)中，腱之間存在耦合，為了避免解耦的麻煩，在關(guān)節(jié)空間規(guī)劃期望的關(guān)節(jié)力矩，然后轉(zhuǎn)換成腱空間的張力。
[0050]根據(jù)手指靜力學(xué)特性，事先規(guī)劃出期望的接觸力Fd，即可通過(guò)雅可比矩陣J反解出使手指指尖在接觸到物體并保持靜態(tài)平衡時(shí)所需的關(guān)節(jié)力矩τ d:
[0051]Td = J1Fd(5)
[0052]5)腱張力分配模塊
[0053]由于腱只傳遞張力，為了避免腱在運(yùn)動(dòng)過(guò)程中出現(xiàn)松弛，引入腱張力控制器確保腱張力的正定性。腱張力控制器算法的基礎(chǔ)是η個(gè)關(guān)節(jié)力矩τ和n+1個(gè)腱張力f之間的關(guān)系:
(Λ網(wǎng)，、
[0054]=PfP=(6)
[0055]其中t表示腱的內(nèi)部張力；R e R3*3是由三個(gè)獨(dú)立關(guān)節(jié)的半徑組成的對(duì)角陣；B表示腱的結(jié)構(gòu)矩陣；WT正交于RB的行空間且正定。
[0056]系統(tǒng)中腱張力可控的充要條件為P可逆，要求RB行滿秩。由式(6)可以得到:
[0057]f = P(7)

Kt J
[0058]令 P-1 = LA a]
Η
[0059]f = [^ Ω] =At+ at(8)

KtJ
[0060]設(shè)定腱張力取值范圍[fmin fmax]，fmin保證腱始終處于繃緊的狀態(tài)，fmax保護(hù)腱不超負(fù)荷。令A(yù)i和Si分別代表A的行向量和a的列向量。腱張力分配算法的步驟如下。
[0061]a)首先根據(jù)fmin確定t的取值，則有:
[0062]fi = Ai τ +ait 彡 fmin(9)
[0063]t = rmx—~—(10)

α:
[0064]b)其次將內(nèi)部張力值t代入張力分配等式⑶；
[0065]c)令為張力中最小的元素，fh為張力中的最大元素；
[0066]d)確定是否張力，…，4超過(guò)上界fmax，如果fh ( fmax,表明沒(méi)有超界，給各根腱分配張力f\，...，fn+1并結(jié)束分配過(guò)程。
[0067]e)如果出現(xiàn)fh > fmax,則采用比例縮放的方式構(gòu)造一個(gè)新的方程:
[0068]f = [A a]K I I )
[0069]由前述過(guò)程中得到的和fh及其對(duì)應(yīng)位置處相關(guān)參量計(jì)算得到式(11)中參量:
[0070]d = CahAfa1Ah) τ
a /'_ η /'
[0071]a = h min-1JjmL..d
f Δ — f A
[0072]t = Jmax 1~Jmin h τ

d
[0073]f)令&為利用式(11)所計(jì)算出的張力最小的元素，fh為其中的張力最大元素，
[0074]g)若fh彡fmax,并且^fniin,給各根腱分配張力f\，...，fn+1，并結(jié)束分配過(guò)程。否貝1J，重新進(jìn)行e)、f)、g)過(guò)程，直到滿足要求為止。
[0075]最終綜合公式(5)?(11)可得腱張力分配算法表達(dá)如下:

f a I1F、
[0076]L=P-1d(12)
[0077]6)張力控制模塊
[0078]最終得到腱張力控制模塊系統(tǒng)表達(dá)式如下:


(U f — ? \
[0079]Axi2=Kp2(fd-fb) + Kd2 Kh,dt'b)(13)
[0080]κρ2為力控制ro控制器增益，Kd2為力控制ro控制器微分項(xiàng)增益。
[0081]公式(4)和(13)相加得到機(jī)械手單指位置/腱張力混合控制系統(tǒng)控制律:
[0082]
ISxi= IsxajT ISxil
=卜Δ紙-Θ,) + Κ?1 —H +卜(Λ -/,) + ^/(/^~Λ)](14)
[0083]通過(guò)合適的機(jī)構(gòu)參數(shù)，可以確定相應(yīng)的映射矩陣；根據(jù)具體的性能指標(biāo)要求，可以確定相應(yīng)的控制器參數(shù)。下面是具體實(shí)施實(shí)例一組相關(guān)參數(shù)設(shè)置。
[0084]根據(jù)如圖2中所示腱的排列可以得出結(jié)構(gòu)矩陣B，進(jìn)而得到腱映射矩陣P，如下:
'-1 —11 I I「-0.06 -0.06 0.06 0.06 '
[0085]B= 1-11-1P= 0.04 -0.04 0.04 -0.04
0 0 1—1」L 0.5 0.5 0.5 0.5
[0086]同時(shí)根據(jù)任務(wù)要求及腱的性能設(shè)置腱張力范圍fmin = 8N, fmax = 200N。
[0087]另外根據(jù)系統(tǒng)參數(shù)，腱長(zhǎng)和各關(guān)節(jié)之間的關(guān)系如下:
Ar1 = 13.9 -^/129.01 -128.4cos(l 19,99-θ2)\- Α.?2ΘΧ
Δχ9 = —4久—4,720
[0088]J Λν； = 14.9 - ^/l 43.41 -142.78 cos(123.98 - ) +
13.5-^/123.25-123.07 cos(l 18.65-^)+4.72^
Av4 =-4^-4^-4.72^
[0089]具體腱驅(qū)動(dòng)機(jī)械手系統(tǒng)的控制器采用了通用PC、數(shù)據(jù)采集卡結(jié)合電機(jī)驅(qū)動(dòng)器來(lái)實(shí)現(xiàn)，另外以嵌入式處理器、DSP等為運(yùn)算核心結(jié)合信號(hào)調(diào)理和電機(jī)驅(qū)動(dòng)的控制器結(jié)構(gòu)形式也是可行的。
[0090]根據(jù)上述結(jié)構(gòu)參數(shù)，進(jìn)行了位置控制器和張力控制器的ro參數(shù)調(diào)節(jié)。采用該方法所實(shí)現(xiàn)系統(tǒng)獲得了良好的性能，能夠有效的解耦腱運(yùn)動(dòng)和關(guān)節(jié)運(yùn)動(dòng)，減小從自由空間到約束空間的沖擊，有效的實(shí)現(xiàn)腱驅(qū)動(dòng)機(jī)械手的操作控制，提高機(jī)械手的靈巧性。該方法可進(jìn)一步應(yīng)用于以腱為傳動(dòng)方式的多關(guān)節(jié)操作臂、末端執(zhí)行器等機(jī)構(gòu)中，因此該實(shí)現(xiàn)方法和裝置具有廣闊的應(yīng)用前景。
[0091]以上所述僅是本發(fā)明的優(yōu)選實(shí)施方式，應(yīng)當(dāng)指出，對(duì)于本【技術(shù)領(lǐng)域】的普通技術(shù)人員來(lái)說(shuō)，在不脫離本發(fā)明原理的前提下還可以作出若干改進(jìn)，這些改進(jìn)也應(yīng)視為本發(fā)明的保護(hù)范圍。
【權(quán)利要求】
1.一種腱驅(qū)動(dòng)機(jī)械手位置和腱張力的混合控制方法，其特征在于:包括如下步驟 步驟1，由路徑規(guī)劃模塊進(jìn)行機(jī)械手自由空間中的關(guān)節(jié)角位置路徑規(guī)劃，根據(jù)手指指尖當(dāng)前位置和物體的期望接觸點(diǎn)確定從初始點(diǎn)到接觸點(diǎn)的運(yùn)動(dòng)軌跡，并且設(shè)定期望關(guān)節(jié)角位置；根據(jù)操作對(duì)象特性設(shè)定期望接觸力，并且設(shè)置腱張力閾值； 步驟2，根據(jù)腱張力傳感器測(cè)量得到的腱張力值與腱張力閾值比較確定靈巧手和物體之間的接觸狀態(tài)，若腱張力值小于腱張力閾值說(shuō)明機(jī)械手位于自由空間，此時(shí)由位置比較模塊比較期望關(guān)節(jié)角位置和實(shí)際測(cè)量關(guān)節(jié)角位置之間的偏差，并且由位置解算模塊解算為腱的長(zhǎng)度偏差，由一個(gè)位置控制模塊通過(guò)合適的控制律得到腱位置偏差，把該偏差指令輸送給鍵驅(qū)動(dòng)器進(jìn)行控制； 步驟3，若腱張力值大于閾值說(shuō)明機(jī)械手位于約束空間，此時(shí)首先由接觸力到關(guān)節(jié)力矩轉(zhuǎn)換模塊通過(guò)力雅可比矩陣把期望的接觸力轉(zhuǎn)換為關(guān)節(jié)力矩；進(jìn)一步由腱張力分配模塊轉(zhuǎn)換為腱空間的力，同時(shí)進(jìn)行腱張力分配；然后由腱張力比較器比較期望腱張力和實(shí)際腱張力的差別，根據(jù)這個(gè)偏差由張力控制模塊基于合適的控制律把張力的偏差轉(zhuǎn)化為位置的偏差，把該偏差與位置控制通道的位置偏差求和后輸送給驅(qū)動(dòng)器進(jìn)行控制。
2.一種腱驅(qū)動(dòng)機(jī)械手位置和腱張力的混合控制裝置，其包括控制器(I)、與控制器(I)相連的腱驅(qū)動(dòng)器(2)、與腱驅(qū)動(dòng)器(2)相連的腱繩(3)、安裝于腱繩(3)上的腱張力傳感器(4)、腱驅(qū)動(dòng)機(jī)械手單指機(jī)構(gòu)(5)及關(guān)節(jié)角位置傳感器￠)，其特征在于:所述腱張力傳感器(4)和關(guān)節(jié)角位置傳感器(6)與控制器(I)相連進(jìn)而將采集的信息發(fā)送給控制器(1)，所述控制器(I)發(fā)送控制指令給腱驅(qū)動(dòng)器(2)，所述控制器(I)包括路徑規(guī)劃模塊、期望接觸力模塊、位置解算模塊、位置控制模塊、位置比較模塊、接觸力到關(guān)節(jié)力矩轉(zhuǎn)換模塊、腱張力分配模塊及張力控制模塊，路徑規(guī)劃模塊輸入手指指尖當(dāng)前位置和物體的期望接觸點(diǎn)，路徑規(guī)劃模塊輸出期望關(guān)節(jié)角位置到位置比較模塊和腱張力分配模塊，位置比較模塊的另一個(gè)輸入為實(shí)際測(cè)量關(guān)節(jié)角位置，位置比較模塊輸出位置偏差到位置結(jié)算模塊，位置解算模塊輸出到位置控制模塊，期望接觸力輸入到接觸力到關(guān)節(jié)力矩轉(zhuǎn)換模塊，轉(zhuǎn)換結(jié)果輸出到轉(zhuǎn)換腱張力分配模塊，腱張力模塊輸出的結(jié)果和腱張力傳感器測(cè)量的結(jié)果進(jìn)行比較，結(jié)果輸出到張力控制模塊，張力控制的輸出和位置控制的輸出求和然后輸出到腱驅(qū)動(dòng)控制器，控制機(jī)構(gòu)動(dòng)作。
【文檔編號(hào)】B25J13/00GK104191429SQ201410362296
【公開(kāi)日】2014年12月10日 申請(qǐng)日期:2014年7月28日 優(yōu)先權(quán)日:2014年7月28日
【發(fā)明者】王小濤, 王邢波, 馮敦超, 聶宏, 韓運(yùn)錚, 曹雯, 韓如雪 申請(qǐng)人:南京航空航天大學(xué)