專利名稱：機器人擬人手指自適應指尖力跟蹤控制方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種器人擬人手指控制方法。
背景技術(shù)：
進入20世紀90年代，具有一般功能的傳統(tǒng)工業(yè)機器人的應用趨向飽和，而隨著機器人技術(shù)的進一步發(fā)展以及人工智能技術(shù)的出現(xiàn)，機器人的應用逐漸從工業(yè)領(lǐng)域，延伸到農(nóng)業(yè)、醫(yī)療康復、服務(wù)娛樂業(yè)等領(lǐng)域當中，其操作環(huán)境往往是動態(tài)非良好的，操作對象也變得更加的多樣化和復雜化，這就要求機器人配置功能更加強大，適應性和靈活性更加好的末端執(zhí)行器，從而促使國內(nèi)外學者相繼展開新一代機器人末端執(zhí)行器的研究工作。目前，研究者們普遍采用的方案是以人手為原型，模擬人手的功能和結(jié)構(gòu)設(shè)計具有多關(guān)節(jié)、多手指的多自由度機器人末端執(zhí)行器，稱之為機器人多指靈巧手。目前存在的機器人多指靈巧手， 可以分為兩大類采用電機驅(qū)動的剛性靈巧手和采用氣動柔性驅(qū)動器(如PMA、FPA等)驅(qū)動的柔性靈巧手，如專利號為200810122003. 7的發(fā)明專利就是采用氣動柔性驅(qū)動器FPA驅(qū)動。機器人多指靈巧手能夠像人手一樣進行靈活抓取和操作，其控制系統(tǒng)及控制策略的研究顯得更加重要。多指靈巧手指尖輸出力的精確程度，直接決定了靈巧手能否對目標物體實現(xiàn)穩(wěn)定抓取。對目標物體實現(xiàn)穩(wěn)定抓取，可以看作是靈巧手多個手指及手掌與目標物體之間存在一組力約束的多機器人系統(tǒng)，故在進行抓取和操作時具有多閉環(huán)特征，存在力控制的不唯一性、力對抗與合作等問題。然而，多指靈巧手的抓取過程往往是一個與環(huán)境接觸動態(tài)非良好的過程，多數(shù)情況下靈巧手或者操作者對環(huán)境位置(指尖與目標物體接觸初始位置)的估計不夠精確，即靈巧手在未知環(huán)境中抓取或操作目標物體。人們更多的是期望在未知環(huán)境中，手指同時滿足位置與力的某種理想的動態(tài)關(guān)系，同時希望手指與環(huán)境的接觸力保持恒定的期望值。由于氣動柔性驅(qū)動器(如PMA、FPA等)主要構(gòu)成材料是橡膠，同時采用氣動驅(qū)動， 其精確的數(shù)學模型難以建立，故采用氣動柔性驅(qū)動器驅(qū)動的柔性靈巧手指尖力難以精確控制。

發(fā)明內(nèi)容
為了克服已有現(xiàn)有的機器人擬人手指指尖力的控制精度較差、控制效果較差的不足，本發(fā)明提供一種控制精度較高、控制效果良好的機器人擬人手指自適應指尖力跟蹤控制方法。本發(fā)明解決其技術(shù)問題所采用的技術(shù)方案是—種機器人擬人手指自適應指尖力跟蹤控制方法，包括以下步驟1)設(shè)定機器人擬人手指具有η個關(guān)節(jié)和η個自由度，η為自然數(shù)，且η >2，所述手指包括1個側(cè)擺關(guān)節(jié)和η-1個彎曲關(guān)節(jié)，建立η自由度手指的D-H連桿坐標系，其中，坐標系WL0為基座標系，即定坐標系；X1Y1Z1, X2Y2Z2, . . . X1J1A^B對于基座標系建立，都是動坐標系，XnYnZn為指尖坐標系；2)在手指指尖安裝指尖五維力傳感器，通過指尖五維力傳感器測量擬人手指與環(huán)境的實際接觸力，所述實際接觸力是相對于指尖坐標系的，通過以下坐標變換nJTnF = JtF (4)其中nJ表示相對于指尖坐標系的雅克比矩陣；τ =[τιτ2τ3... τη]Τ表示用于抵消手指接觸力nF，手指各關(guān)節(jié)的輸出力矩，其中τ工表示側(cè)擺關(guān)節(jié)的輸出力矩；τ 2表示彎曲關(guān)節(jié)1的輸出力矩；τ 3表示彎曲關(guān)節(jié)2的輸出力矩；...τη表示彎曲關(guān)節(jié)η-1的輸出力矩J表示手指相對于基座標系的雅克比矩陣；F= [fx fy fz mx my mj表示指尖與環(huán)境相對于基座標系的接觸力，其中fx表示指尖接觸力？在^坐標軸的分力；fy表示指尖接觸力F在yQ坐標軸的分力；fz表示指尖接觸力F在^坐標軸的分力；mx表示指尖接觸力F繞 X0坐標軸的分力矩;my表示指尖接觸力F繞yQ坐標軸的分力矩;mz表示指尖接觸力F繞^ 坐標軸的分力矩；3)經(jīng)過坐標變換后的手指與目標物體的實際接觸力，與期望的接觸力&相減，得到接觸力誤差Fe ；其中期望的參考力Fr相對于手指基座標系而言；如果接觸力誤差Fe小于預設(shè)的控制閾值，結(jié)束；否則進入步驟4)；4)接觸力誤差F6乘以比例因子η，與指尖實際位置X相加得到指尖下一控制周期的參考位置Xp其中比例因子η是通過模糊自整定控制器在線調(diào)節(jié)，模糊自整定控制器的輸入量為接觸力誤差Fe及接觸力誤差變化率Fe。；X= [Px py ρζ]，Px表示指尖相對于基座標系的χ坐標值；Py表示指尖相對于基座標系的y坐標值；Pz表示指尖相對于基座標系的ζ 坐標值；5)由步驟4)得到的新的參考位置\經(jīng)過手指逆運動學L—1 (Xr)求解得到期望參考關(guān)節(jié)角度θ 2r. . . θ m]，其中θ&表示側(cè)擺關(guān)節(jié)的期望參考側(cè)擺角度；θ&表示彎曲關(guān)節(jié)1的期望參考彎曲角度；...表示彎曲關(guān)節(jié)n-Ι的期望參考彎曲角度；6)期望參考關(guān)節(jié)角度Θ^與手指各個關(guān)節(jié)實際角度Θ相減，其中關(guān)節(jié)實際角度由安裝在關(guān)節(jié)上角度傳感器檢測得到，二者的差值作為手指位置控制器的輸入，手指位置控制器輸出結(jié)果經(jīng)過手指正運動學L(Θ)求解得到指尖實際位置。本發(fā)明的技術(shù)構(gòu)思為針對一類采用柔性驅(qū)動器(PMA、FMA等)的多指靈巧手輸出力動態(tài)跟蹤。基于手指指尖力傳感器反饋信息，發(fā)明了一種模糊自適應指尖力跟蹤控制算法，實現(xiàn)在未知環(huán)境下手指指尖力精確跟蹤的同時，盡量減少指尖對環(huán)境的沖擊作用，以免造成目標物體的不可恢復的損傷。手指與環(huán)境接觸的情況，接觸引起的環(huán)境局部微小變形由矢量&表示。手指指尖與環(huán)境的接觸力可由彈性力來模擬F = K6Xe = Ke(X-Xe) (1)其中Xe——環(huán)境位置，讓；X——手指實際位置，匪；Ke——環(huán)境剛度系數(shù)，N/Wi。根據(jù)式(1)，手指與環(huán)境接觸系統(tǒng)可簡化為一個“質(zhì)量-彈簧”系統(tǒng)，假設(shè)精確知道環(huán)境位置，及環(huán)境剛度系數(shù)，指尖實際位置可以通過關(guān)節(jié)位置傳感器得到，從而可以精確計算得到手指與環(huán)境實際接觸力。但在實際操作過程中，由于各種不可預知的因素，靈巧手及操作者很難精確了解環(huán)境剛度Ke及環(huán)境表面位置這導致了手指力控制往往存在誤差， 無法在要求高精密輸出力的場合使用。自適應指尖力跟蹤算法具體過程機器人多指靈巧手對目標物體實施抓取或操作時，可以從兩個運動空間考慮自由運動空間和接觸空間(環(huán)境約束空間)。自由運動空間 手指從初始位置運動至與環(huán)境接觸，整個過程運動過程，手指與環(huán)境的接觸力F = 0。接觸空間指尖從與環(huán)境接觸開始運動至相對靜止狀態(tài)，整個過程手指與環(huán)境的接觸力F從小變大。本發(fā)明提出的在未知環(huán)境下，自適應指尖力跟蹤控制算法的核心思想是根據(jù)指尖力傳感器的反饋信息，自適應調(diào)整指尖參考位置Xp從而逐步間接逼近期望的參考力&。 具體實施過程如下首先，以環(huán)境位置人為目標參考位置，手指從指尖初始位置運動至與環(huán)境位置接觸；接著，指尖繼續(xù)運動，與環(huán)境接觸力逐漸增大，根據(jù)指尖力傳感器的力反饋信息，手指不斷調(diào)整目標參考位置，逐步逼近期望指尖力。在未知環(huán)境中，很難精確知道環(huán)境位置X6，假設(shè)環(huán)境位置估計X6'與環(huán)境實際位置X6之間存在偏差ζ = Xe-X' ε，用環(huán)境位置估計V作為算法初始目標參考位置，同時算法中不涉及環(huán)境剛度，因此，可以消除未知環(huán)境帶來的影響。由于指尖力F在各力的分量是可以解耦的，為了表達清楚，可以只考慮某一維操作空間情況，用&、xe‘、&、 ；代替) 、χ^ ‘、XpFp指尖參考位置自適應調(diào)整算法如下Xr (t) = χ (t-τ) + δ (t) (2)δ (t) = η (t) · (fr-f(t-T)) (3)式中，xr (t) — t時刻指尖目標參考位置，mm，t = 0時刻指尖目標參考位置為環(huán)境位置估計；T——算法控制周期，s ；x(t-T)——t-T時刻指尖實際位置，mm ；δ (t)——t時刻指尖目標參考位置補償算子，mm ；fr——期望目標參考力，N ；f (t-T)——t-T時刻手指與環(huán)境實際接觸力，由指尖力傳感器檢測通過坐標轉(zhuǎn)換得到，N;η (f)——比例因子，謹/N。本發(fā)明的有益效果主要表現(xiàn)在本發(fā)明提出的自適應指尖力跟蹤算法，采用模糊控制對指尖目標參考位置補償算子δ (t)進行整定，實際上是對比例因子η (t)實施在線模糊自整定。根據(jù)輸入量，經(jīng)過模糊控制器實時調(diào)節(jié)比例因子n(t)的大小，進而改變目標參考位置，使得指尖與環(huán)境的接觸力能夠快速、平滑、零超調(diào)跟蹤期望參考力。


圖1是柔性手指與環(huán)境接觸模型。圖2是手指的連桿坐標系。圖3是未知環(huán)境下指尖力模糊自適應跟蹤控制器。
5
圖4是手指自適應指尖力跟蹤控制實驗方案。圖5是未知環(huán)境下手指自適應指尖力動態(tài)跟蹤實驗結(jié)果。
具體實施例方式下面結(jié)合附圖對本發(fā)明作進一步描述。參照圖1 圖4，一種機器人擬人手指自適應指尖力跟蹤控制方法，包括以下步驟1)設(shè)定機器人擬人手指具有η個關(guān)節(jié)和η個自由度，η為自然數(shù)，且η >2，所述手指包括1個側(cè)擺關(guān)節(jié)和η-1個彎曲關(guān)節(jié)，建立η自由度手指的D-H連桿坐標系，其中，坐標系Kyc^o為基座標系，即定坐標系；X1Y1Z1, X2Y2Z2, . . . X1J1A^B對于基座標系建立，都是動坐標系，XnYnZn為指尖坐標系；2)在手指指尖安裝指尖五維力傳感器，通過指尖五維力傳感器測量擬人手指與環(huán)境的實際接觸力，所述實際接觸力是相對于指尖坐標系的，通過以下坐標變換nJTnF = JtF (4)其中3表示相對于指尖坐標系的雅克比矩陣；τ = [ τ i τ 2 τ 3. . . τ η]Τ表示用于抵消手指接觸力nF，手指各關(guān)節(jié)的輸出力矩，其中T1表示側(cè)擺關(guān)節(jié)的輸出力矩；12表示彎曲關(guān)節(jié)1的輸出力矩；τ 3表示彎曲關(guān)節(jié)2的輸出力矩；...Tn表示彎曲關(guān)節(jié)η-1的輸出力矩J表示手指相對于基座標系的雅克比矩陣；F = [fx fy fz mx my mj表示指尖與環(huán)境相對于基座標系的接觸力，其中fx表示指尖接觸力？在^坐標軸的分力；fy表示指尖接觸力F在yQ坐標軸的分力；fz表示指尖接觸力F在^坐標軸的分力；mx表示指尖接觸力F 繞^坐標軸的分力矩;my表示指尖接觸力F繞yQ坐標軸的分力矩；mz表示指尖接觸力F繞 Z0坐標軸的分力矩；3)經(jīng)過坐標變換后的手指與目標物體的實際接觸力，與期望的接觸力&相減，得到接觸力誤差Fe ；其中期望的參考力Fr相對于手指基座標系而言；如果接觸力誤差Fe小于預設(shè)的控制閾值，結(jié)束；否則進入步驟4)；4)接觸力誤差Fe乘以比例因子η，與指尖實際位置X相加得到指尖下一控制周期的參考位置Xp其中比例因子η是通過模糊自整定控制器在線調(diào)節(jié)，模糊自整定控制器的輸入量為接觸力誤差Fe及接觸力誤差變化率Fe。；X= [Px py ρζ]，Px表示指尖相對于基座標系的χ坐標值；Py表示指尖相對于基座標系的y坐標值；Pz表示指尖相對于基座標系的ζ 坐標值；5)由步驟4)得到的新的參考位置&經(jīng)過手指逆運動學廠1 (Xr)求解得到期望參考關(guān)節(jié)角度θ 2r. . . θ m]，其中θ&表示側(cè)擺關(guān)節(jié)的期望參考側(cè)擺角度；θ&表示彎曲關(guān)節(jié)1的期望參考彎曲角度；...表示彎曲關(guān)節(jié)n-Ι的期望參考彎曲角度；6)期望參考關(guān)節(jié)角度Θ^與手指各個關(guān)節(jié)實際角度Θ相減，其中關(guān)節(jié)實際角度由安裝在關(guān)節(jié)上角度傳感器檢測得到，二者的差值作為手指位置控制器的輸入，手指位置控制器輸出結(jié)果經(jīng)過手指正運動學L(Θ)求解得到指尖實際位置。參照圖1，手指1與環(huán)境2接觸的情況，接觸引起的環(huán)境2局部微小變形由矢量& 表示。手指1指尖與環(huán)境2的接觸力可由彈性力來模擬。手指1與環(huán)境2接觸系統(tǒng)可簡化為一個“質(zhì)量-彈簧”系統(tǒng)，假設(shè)精確知道環(huán)境位置，及環(huán)境剛度系數(shù)，指尖實際位置可以通
權(quán)利要求
1. 一種機器人擬人手指自適應指尖力跟蹤控制方法，其特征在于所述跟蹤控制方法包括以下步驟1)設(shè)定機器人擬人手指具有η個關(guān)節(jié)和η個自由度，η為自然數(shù)，且η> 2，所述手指包括1個側(cè)擺關(guān)節(jié)和η-1個彎曲關(guān)節(jié)，建立η自由度手指的D-H連桿坐標系，其中，坐標系 x0y0z0為基座標系，即定坐標系；X1Y1Z1, X2Y2Z2,…xny#n相對于基座標系建立，都是動坐標系，XnYnZn為指尖坐標系；2)在手指指尖安裝指尖五維力傳感器，通過指尖五維力傳感器測量擬人手指與環(huán)境的實際接觸力，所述實際接觸力是相對于指尖坐標系的，通過以下坐標變換nJtoF = JtF (4)其中nJ表示相對于指尖坐標系的雅克比矩陣；τ =[τι τ2 ^口 τη]Τ表示用于抵消手指接觸力nF，手指各關(guān)節(jié)的輸出力矩，其中τ 表示側(cè)擺關(guān)節(jié)的輸出力矩；τ 2表示彎曲關(guān)節(jié)1的輸出力矩；τ3表示彎曲關(guān)節(jié)2的輸出力矩；…τη表示彎曲關(guān)節(jié)η-1的輸出力矩J表示手指相對于基座標系的雅克比矩陣；F = [fx fy fz mx my mj表示指尖與環(huán)境相對于基座標系的接觸力，其中fx表示指尖接觸力？在^坐標軸的分力；fy表示指尖接觸力 F在yQ坐標軸的分力；fz表示指尖接觸力F在^坐標軸的分力；mx表示指尖接觸力F繞^ 坐標軸的分力矩;my表示指尖接觸力F繞yQ坐標軸的分力矩;mz表示指尖接觸力F繞^坐標軸的分力矩；3)經(jīng)過坐標變換后的手指與目標物體的實際接觸力，與期望的接觸力&相減，得到接觸力誤差Fe ；其中期望的參考力Fr相對于手指基座標系而言；如果接觸力誤差Fe小于預設(shè)的控制閾值，結(jié)束；否則進入步驟4)；4)接觸力誤差F6乘以比例因子η，與指尖實際位置X相加得到指尖下一控制周期的參考位置I，其中比例因子η是通過模糊自整定控制器在線調(diào)節(jié)，模糊自整定控制器的輸入量為接觸力誤差Fe及接觸力誤差變化率Fe。;X= [px py ρζ]，Ρχ表示指尖相對于基座標系的χ坐標值；py表示指尖相對于基座標系的1坐標值；Pz表示指尖相對于基座標系的ζ 坐標值；5)由步驟4)得到的新的參考位置&經(jīng)過手指逆運動學L4(I)求解得到期望參考關(guān)節(jié)角度Θ^=，其中θ &表示側(cè)擺關(guān)節(jié)的期望參考側(cè)擺角度；表示彎曲關(guān)節(jié)1的期望參考彎曲角度；…表示彎曲關(guān)節(jié)n-Ι的期望參考彎曲角度；6)期望參考關(guān)節(jié)角度Θ^與手指各個關(guān)節(jié)實際角度Θ相減，其中關(guān)節(jié)實際角度由安裝在關(guān)節(jié)上角度傳感器檢測得到，二者的差值作為手指位置控制器的輸入，手指位置控制器輸出結(jié)果經(jīng)過手指正運動學L(Θ)求解得到指尖實際位置。
全文摘要
一種機器人擬人手指自適應指尖力跟蹤控制方法，包括以下步驟1)設(shè)定機器人擬人手指具有n個關(guān)節(jié)和n個自由度；2)通過指尖五維力傳感器測量擬人手指與環(huán)境的實際接觸力；3)實際接觸力與期望的接觸力Fr相減，得到接觸力誤差Fe；4)接觸力誤差Fe乘以比例因子η，與指尖實際位置X相加得到指尖下一控制周期的參考位置Xr；5)參考位置Xr經(jīng)過手指逆運動學L-1(Xr)求解得到期望參考關(guān)節(jié)角度Θr；6)期望參考關(guān)節(jié)角度Θr與手指各個關(guān)節(jié)實際角度Θ相減，差值作為手指位置控制器的輸入，手指位置控制器輸出結(jié)果經(jīng)過手指正運動學L(Θ)求解得到指尖實際位置。本發(fā)明控制精度較高、控制效果良好。
文檔編號B25J15/08GK102363301SQ201110318758
公開日2012年2月29日 申請日期2011年10月19日 優(yōu)先權(quán)日2011年10月19日
發(fā)明者張立彬, 楊慶華, 王志恒, 鮑官軍 申請人:浙江工業(yè)大學