專(zhuān)利名稱(chēng)：移動(dòng)機(jī)器人視覺(jué)系統(tǒng)防抖裝置與防抖補(bǔ)償控制方法
技術(shù)領(lǐng)域：
本發(fā)明涉及機(jī)器人用視覺(jué)系統(tǒng)防抖裝置與防抖補(bǔ)償控制方法，屬于機(jī)器人 應(yīng)用領(lǐng)域。
背景技術(shù)：
機(jī)器人視覺(jué)是一門(mén)新興的發(fā)展迅速的學(xué)科，八十年代以來(lái)，機(jī)器人視覺(jué)的 研究已經(jīng)歷了從實(shí)驗(yàn)室走向?qū)嶋H應(yīng)用的發(fā)展階段。但與此同時(shí)，機(jī)器人視覺(jué)系 統(tǒng)也出現(xiàn)了不少問(wèn)題。機(jī)器人的移動(dòng)過(guò)程中也經(jīng)常會(huì)遇到路面不平等抖動(dòng)現(xiàn) 象，其視覺(jué)系統(tǒng)中所拍攝到的圖像亦無(wú)法避免運(yùn)動(dòng)模糊現(xiàn)象。
針對(duì)運(yùn)動(dòng)模糊問(wèn)題，通常采用較高的快門(mén)速度可以減輕這種震動(dòng)所導(dǎo)致的 照片模糊，但在曝光時(shí)間較長(zhǎng)的拍攝中，比如在陰天或者傍晚時(shí)，拍的畫(huà)面都 比較虛，這是由于光線較弱的環(huán)境下而要比較長(zhǎng)的快門(mén)時(shí)間，在這種情況下， 震動(dòng)所帶來(lái)的圖像模糊問(wèn)題幾乎不可避免。為了緩解這個(gè)問(wèn)題，使攝影者輕松 拍出清晰得照片，相機(jī)生產(chǎn)廠家紛紛開(kāi)發(fā)了減輕防抖得技術(shù)，稱(chēng)為"防抖"
(Anti-Shake簡(jiǎn)稱(chēng)AS)或圖像穩(wěn)定(Image Stabilization簡(jiǎn)稱(chēng)IS)技術(shù)。
目前，已經(jīng)有一些具備防抖功能的攝像機(jī)已經(jīng)被研制出來(lái)，包括以佳能、 松下為首的光學(xué)防抖技術(shù)，以?shī)W林巴斯、尼康為首的電子防抖技術(shù)，還有柯尼 卡美能達(dá)獨(dú)家的機(jī)身防抖技術(shù)，但它們都不是針對(duì)移動(dòng)機(jī)器人用的，也不是用 機(jī)械的方法去實(shí)施達(dá)到的。經(jīng)文獻(xiàn)檢索發(fā)現(xiàn)，專(zhuān)利公告號(hào)為1096622，專(zhuān)利號(hào) 為94101806，名稱(chēng)為防止圖象抖動(dòng)的攝象機(jī)。該攝象機(jī)包含檢測(cè)攝象機(jī)抖動(dòng)的抖動(dòng)檢測(cè)裝置，獲得校正量的計(jì)算裝置和校正攝象機(jī)體的抖動(dòng)的校正裝置。 但是該攝象機(jī)只能檢測(cè)極其微小的抖動(dòng)，校正裝置也是通過(guò)鏡頭變焦來(lái)實(shí)現(xiàn) 的，其抖動(dòng)環(huán)境與移動(dòng)機(jī)器人不一樣，校正方式也無(wú)法適用于移動(dòng)機(jī)器人。專(zhuān)
利公告號(hào)為1610387，專(zhuān)利號(hào)為200410087084，名稱(chēng)為圖像拾取設(shè)備及其照相 機(jī)抖動(dòng)校正方法。該發(fā)明涉及圖像拾取設(shè)備及其照相機(jī)抖動(dòng)校正方法，是通過(guò) 圖像拾取裝置拾取一個(gè)對(duì)象的圖像，以此來(lái)確定照相機(jī)抖動(dòng)分量，從而來(lái)校正 相機(jī)的抖動(dòng)。該發(fā)明的不足之處是在抖動(dòng)圖像出來(lái)之后才開(kāi)始校正的，不能達(dá) 到一次性輸出清晰圖像的效果。另外。專(zhuān)利公告號(hào)為1812501，專(zhuān)利號(hào)為 200610001684，名稱(chēng)為攝像裝置及其攝像方法。該攝像裝置是根據(jù)快門(mén)速度改 變搖攝控制，從而提供適于拍攝條件的防抖動(dòng)效果。該裝置的適用條件是相對(duì) 穩(wěn)定的搖攝條件，很難適用于移動(dòng)機(jī)器人所面對(duì)的復(fù)雜的無(wú)規(guī)則的抖動(dòng)環(huán)境。 民用攝像機(jī)和照相機(jī)內(nèi)部抖動(dòng)系統(tǒng)僅能補(bǔ)償?shù)皖l和小抖動(dòng)量情況下的抖動(dòng)，而 面向輪式、履帶式、腿式等移動(dòng)機(jī)器人的視覺(jué)系統(tǒng)均屬于工業(yè)視覺(jué)攝像裝置， 其裝置內(nèi)、外無(wú)抖動(dòng)措施，而帶有工業(yè)視覺(jué)系統(tǒng)的移動(dòng)機(jī)器人在不平整地面或 野外移動(dòng)時(shí)因震動(dòng)導(dǎo)致視覺(jué)裝置抖動(dòng)成像不清晰，來(lái)自地面的抖動(dòng)可能是中低 頻，也有高頻情況。而且抖動(dòng)幅度有小有大，因此，上述專(zhuān)利中提及的防抖方 法均不能解決移動(dòng)機(jī)器人在不平整路面或野外環(huán)境下的復(fù)雜無(wú)規(guī)則的抖動(dòng)主 動(dòng)補(bǔ)償與控制問(wèn)題。

發(fā)明內(nèi)容
本發(fā)明的目的是針對(duì)移動(dòng)機(jī)器人的抖動(dòng)情況，通過(guò)機(jī)械和控制手段從根本 上實(shí)時(shí)地消除機(jī)器人視覺(jué)系統(tǒng)的抖動(dòng)，以達(dá)到視覺(jué)系統(tǒng)運(yùn)動(dòng)的平穩(wěn)， 一次性輸 出較清晰圖像的效果；進(jìn)而提供了一種移動(dòng)機(jī)器人視覺(jué)系統(tǒng)防抖裝置與防抖補(bǔ)償控制方法。
本發(fā)明為達(dá)到上述發(fā)明目的所采用的技術(shù)方案包括視覺(jué)系統(tǒng)防抖裝置和 防抖補(bǔ)償控制方法兩部分
' 本發(fā)明所述視覺(jué)系統(tǒng)防抖裝置由防抖機(jī)構(gòu)、傳感器系統(tǒng)和防抖控制系統(tǒng)三 部分構(gòu)成；所述防抖機(jī)構(gòu)由左右移動(dòng)單元、上下移動(dòng)單元、俯仰單元、偏擺單 元和基座構(gòu)成，上下移動(dòng)單元、左右移動(dòng)單元、俯仰單元和偏擺單元均設(shè)置在
基座上;所述傳感器系統(tǒng)包括傾角傳感器、第一振動(dòng)傳感器和第二振動(dòng)傳感器， 傾角傳感器、第一振動(dòng)傳感器和第二振動(dòng)傳感器均安裝在基座上；所述防抖控 制系統(tǒng)主要包括控制系統(tǒng)硬件和控制系統(tǒng)軟件，用來(lái)實(shí)現(xiàn)視覺(jué)系統(tǒng)的防抖；
上下移動(dòng)單元由上下驅(qū)動(dòng)電機(jī)、上下移動(dòng)單元傳動(dòng)機(jī)構(gòu)、上下運(yùn)動(dòng)導(dǎo)向軸、 固定塊、滑塊和上下運(yùn)動(dòng)同步帶構(gòu)成；上下移動(dòng)單元傳動(dòng)機(jī)構(gòu)包括上下運(yùn)動(dòng)繩 輪、上下運(yùn)動(dòng)換向輪、第一上下運(yùn)動(dòng)同步帶輪和第二上下運(yùn)動(dòng)同步帶輪；驅(qū)動(dòng) 電機(jī)通過(guò)上下移動(dòng)單元傳動(dòng)機(jī)構(gòu)驅(qū)動(dòng)與同步帶固連的固定塊及滑塊一起沿導(dǎo) 向軸上下運(yùn)動(dòng)；上下移動(dòng)單元設(shè)置在安裝有兩個(gè)視覺(jué)傳感器的架體的下方，滑 塊與架體連接以實(shí)現(xiàn)兩個(gè)視覺(jué)傳感器相對(duì)基座同時(shí)上下移動(dòng)；
左右移動(dòng)單元由左右驅(qū)動(dòng)電機(jī)、左右移動(dòng)單元傳動(dòng)機(jī)構(gòu)和左右運(yùn)動(dòng)導(dǎo)向軸 構(gòu)成；左右移動(dòng)單元傳動(dòng)機(jī)構(gòu)包括第一左右同步帶輪、第二左右同步帶輪和左 右運(yùn)動(dòng)同步帶；左右驅(qū)動(dòng)電機(jī)通過(guò)左右移動(dòng)單元傳動(dòng)機(jī)構(gòu)驅(qū)動(dòng)與左右運(yùn)動(dòng)同步 帶固連的滑塊一起沿著導(dǎo)向軸左右運(yùn)動(dòng)；左右移動(dòng)單元設(shè)置在安裝有兩個(gè)視覺(jué) 傳感器的架體的下方，滑塊與架體連接以實(shí)現(xiàn)兩個(gè)視覺(jué)傳感器相對(duì)基座同時(shí)左 右移動(dòng)；
俯仰單元由俯仰驅(qū)動(dòng)電機(jī)、俯仰單元傳動(dòng)機(jī)構(gòu)和支架構(gòu)成，俯仰單元傳動(dòng)機(jī)構(gòu)包括第一俯仰繩輪和第二俯仰繩輪；俯仰驅(qū)動(dòng)電機(jī)通過(guò)俯仰單元傳動(dòng)機(jī)構(gòu) 驅(qū)動(dòng)支架做俯仰運(yùn)動(dòng)，固裝在支架內(nèi)的兩個(gè)視覺(jué)傳感器相對(duì)基座同時(shí)做俯仰運(yùn) 動(dòng)；
偏擺單元由兩個(gè)偏擺驅(qū)動(dòng)電機(jī)和兩套偏擺單元傳動(dòng)機(jī)構(gòu)構(gòu)成；每套偏擺單 元傳動(dòng)機(jī)構(gòu)包括每一偏擺繩輪、偏擺換向輪、第一偏擺同步帶輪和第二偏擺同 步帶輪；兩個(gè)偏擺驅(qū)動(dòng)電機(jī)、兩套偏擺單元傳動(dòng)機(jī)構(gòu)左右對(duì)稱(chēng)設(shè)置，每個(gè)偏擺 驅(qū)動(dòng)電機(jī)通過(guò)相應(yīng)的偏擺單元傳動(dòng)機(jī)構(gòu)實(shí)現(xiàn)每個(gè)視覺(jué)傳感器相對(duì)基座偏擺運(yùn) 動(dòng)；
所述控制系統(tǒng)硬件由主控器和五個(gè)電機(jī)驅(qū)動(dòng)與控制器組成；五個(gè)電機(jī)驅(qū)動(dòng) 與控制器分別控制上下驅(qū)動(dòng)電機(jī)、左右驅(qū)動(dòng)電機(jī)、俯仰驅(qū)動(dòng)電機(jī)以及兩個(gè)偏擺 驅(qū)動(dòng)電機(jī)的運(yùn)動(dòng)；控制系統(tǒng)軟件是基于移動(dòng)機(jī)器人視覺(jué)系統(tǒng)防抖裝置的防抖補(bǔ) 償控制方法利用相應(yīng)程序語(yǔ)言編寫(xiě)的專(zhuān)業(yè)軟件；傳感器系統(tǒng)將感知到的移動(dòng)機(jī) 器人視覺(jué)系統(tǒng)抖動(dòng)信息輸出給主控器進(jìn)行處理，主控器內(nèi)的控制系統(tǒng)軟件進(jìn)行 補(bǔ)償計(jì)算得到相應(yīng)補(bǔ)償量后輸出相應(yīng)的命令信息給五個(gè)電機(jī)驅(qū)動(dòng)與控制器，五 個(gè)電機(jī)驅(qū)動(dòng)與控制器控制、驅(qū)動(dòng)防抖機(jī)構(gòu)中相應(yīng)的電機(jī)運(yùn)轉(zhuǎn)進(jìn)行抖動(dòng)補(bǔ)償，以 控制兩個(gè)視覺(jué)傳感器相對(duì)地面保持平穩(wěn)，從而實(shí)現(xiàn)移動(dòng)機(jī)器人視覺(jué)系統(tǒng)的防抖 目的。
本發(fā)明所述移動(dòng)機(jī)器人視覺(jué)系統(tǒng)防抖裝置的防抖補(bǔ)償控制方法，所述方法 按照以下步驟實(shí)現(xiàn)的
步驟一、由第一振動(dòng)傳感器、第二振動(dòng)傳感器和傾角傳感器測(cè)出來(lái)的抖動(dòng) 參數(shù)&、 /22、 ^、灼及相應(yīng)的差分量確定測(cè)得抖動(dòng)量變化的速度和加速度量；
步驟二、根據(jù)傳感器系統(tǒng)在采樣時(shí)間r內(nèi)測(cè)量的情況，判斷移動(dòng)機(jī)器人視覺(jué)系統(tǒng)是否有抖動(dòng)；如果有抖動(dòng)需進(jìn)行抖動(dòng)補(bǔ)償，執(zhí)行步驟三；否則則不需補(bǔ) 償；
步驟三、計(jì)算補(bǔ)償量任意構(gòu)形k"2,《,《,^下兩個(gè)CCD27相對(duì)于裝 置基座22的位姿矩陣分別為°7^。4=°4' 4、 °rCCD5=W4H;當(dāng)前構(gòu) 形k^^，4,^下兩個(gè)視覺(jué)傳感器相對(duì)于裝置基座的位姿矩陣分別為
乂o)f。^V^W4、 °:fCCD5=°^V42V4;由第一振動(dòng)傳感器、第二振動(dòng)傳感器、 傾角傳感器測(cè)出來(lái)的抖動(dòng)參數(shù)h、 &、 ^、伊2 (進(jìn)行差分后可得^、 4、 A、
& );確定視覺(jué)傳感器相對(duì)于裝置基座抖動(dòng)的位姿矩陣為
0rs = Tra"(X, &)7V朋(Y, & )i W(X,仍)i W(Y,伊2),則有如下關(guān)系式
0rccw -吁ccd4 =0rs =>0為i為、3 44 -。i!2i3 3i4 = !Tra"(X, & )7Va"(Y, /z2)i o"X， ^ )i o"Y,
YCCD5 -0:fCCD54 U2 24 34 -。i, 4 2i3 3i5 = r謂(x' & )r，(y, & )i 。"x, )及。/(y, %)
由上述兩個(gè)式子在檢測(cè)得到抖動(dòng)參數(shù)&、 &、 ^、 ^的情況下可推算出防 抖機(jī)構(gòu)在當(dāng)前構(gòu)形^,^4，么，^過(guò)度到抖動(dòng)補(bǔ)償構(gòu)形^,^《A，W情況下須補(bǔ) 償?shù)母麝P(guān)節(jié)運(yùn)動(dòng)量矢量(^-4， a-a2，《-4，《-么)、(""，a2，《-^， 《-4 )，即兩個(gè)視覺(jué)傳感器在理論上的抖動(dòng)補(bǔ)償量矢量分別為
Aq-(勿,，勿2,勿3,勿4)= ( a! —^ ， "2_^，《—》3， & —《)、Aq-(勿"勿2,勿3,勿4)= (fl,-4， fl2-a2，《-4，《-4),進(jìn)行微分后可得A^、 Aii;則防抖裝置各關(guān)
節(jié)位置矢量為q = q(仏,《2,《3,《4) = q(么+」仏,<52 +勿2,爻+勿3,(《+勿4)或的+勿5));
步驟四、規(guī)劃補(bǔ)償軌跡 'W，"(Q，『)，求得關(guān)節(jié)補(bǔ)償角矢量Aq后， 為了保證在采樣時(shí)間r內(nèi)能完成補(bǔ)償過(guò)程，即關(guān)節(jié)補(bǔ)償矢量Aq從O到Aq，而且在r時(shí)刻時(shí)關(guān)節(jié)角的速度和加減速度都為o,必須在采樣時(shí)間r內(nèi)對(duì)補(bǔ)
償?shù)倪^(guò)程進(jìn)行4-3-4關(guān)節(jié)軌跡插值規(guī)劃；
步驟五、基于PD反饋和逆動(dòng)力學(xué)計(jì)算的抖動(dòng)補(bǔ)償控制以
q = q(《rfl, &2, &3,《")=q(乓+勿,，a2 +勿2' 4 +勿3,(《+ )或(《+ 》及該位
置下速度4 = 4(^, ^2,^/3,^4)、加速度4-4d,^,^3，^4)為期望的控制輸
入，采用PD反饋和逆動(dòng)力學(xué)計(jì)算的方法對(duì)防抖裝置的各自由度運(yùn)動(dòng)單元上 .的電機(jī)進(jìn)行控制；反饋量為來(lái)自于各電機(jī)上的光電編碼器位置和速度量經(jīng)
機(jī)械傳動(dòng)裝置折算到關(guān)節(jié)側(cè)的量。
本發(fā)明的技術(shù)效果
本發(fā)明所述防抖裝置設(shè)置左右移動(dòng)單元、上下移動(dòng)單元、俯仰單元、偏擺
單元，具有5個(gè)自由度，實(shí)現(xiàn)對(duì)兩個(gè)視覺(jué)傳感器較大范圍內(nèi)的運(yùn)動(dòng)控制并對(duì)兩
個(gè)視覺(jué)傳感器產(chǎn)生的抖動(dòng)進(jìn)行防控。
本發(fā)明方法可實(shí)現(xiàn)在復(fù)雜的無(wú)規(guī)則的抖動(dòng)環(huán)境中有效控制兩個(gè)視覺(jué)傳感 器相對(duì)地面保持平穩(wěn)，從而實(shí)現(xiàn)移動(dòng)機(jī)器人視覺(jué)系統(tǒng)的防抖目的，進(jìn)而一次性
輸出較清晰圖像。控制系統(tǒng)仿真實(shí)驗(yàn)結(jié)果表明(參見(jiàn)圖9a 圖9d):在隨機(jī)抖 動(dòng)檢測(cè)情況下，采用本發(fā)明所述的防抖裝置及其抖動(dòng)補(bǔ)償控制方法可以大幅度 提高視覺(jué)傳感器相對(duì)于地面的位姿精度；采用本發(fā)明所述的防抖補(bǔ)償控制方法 后的兩個(gè)視覺(jué)傳感器的各項(xiàng)位姿參數(shù)曲線圖9d能夠較為精確地接近于圖9a所 示的期望位姿參數(shù)曲線。


圖la是本發(fā)明的視覺(jué)系統(tǒng)防抖裝置的三維立體圖，圖lb是為上下移動(dòng)單 元的立體圖，圖lc是偏擺單元的立體圖，圖ld是俯仰單元的主視圖，圖le是左右移動(dòng)單元的立體圖，圖If是本發(fā)明的視覺(jué)系統(tǒng)防抖裝置的控制系統(tǒng)框
圖；圖2是本發(fā)明的視覺(jué)系統(tǒng)防抖裝置的機(jī)構(gòu)簡(jiǎn)圖；圖3是本發(fā)明的視覺(jué)系統(tǒng) 防抖裝置的D-H坐標(biāo)圖；圖4為在一個(gè)采樣周期內(nèi)關(guān)節(jié)補(bǔ)償量的4-3-4關(guān)節(jié)插 值軌跡規(guī)劃曲線；圖5是本發(fā)明的視覺(jué)系統(tǒng)防抖裝置的動(dòng)力學(xué)簡(jiǎn)化模型圖；圖 6為基于PD反饋和逆動(dòng)力學(xué)前饋的控制器框圖；圖7是本發(fā)明的視覺(jué)系統(tǒng)防 抖裝置在仿人機(jī)器人的應(yīng)用示意圖；圖8是本發(fā)明的視覺(jué)系統(tǒng)防抖裝置在輪式 移動(dòng)機(jī)器人的應(yīng)用示意圖；圖9a為無(wú)抖動(dòng)時(shí)CCD相對(duì)于地面的位姿曲線圖， 圖9b抖動(dòng)頻率戶20時(shí)隨機(jī)抖動(dòng)參數(shù)&、 /^曲線圖，圖9c為有抖動(dòng)不補(bǔ)償時(shí) CCD相對(duì)于地面的位姿曲線圖，圖9d進(jìn)行抖動(dòng)補(bǔ)償時(shí)CCD相對(duì)于地面的位 姿曲線圖。
具體實(shí)施例方式
具體實(shí)施方式
一、下面結(jié)合圖la lf、圖2說(shuō)明本實(shí)施方式本實(shí)施方 式所述的移動(dòng)機(jī)器人視覺(jué)系統(tǒng)防抖裝置由防抖機(jī)構(gòu)、傳感器系統(tǒng)和防抖控制系 統(tǒng)三部分構(gòu)成；所述防抖機(jī)構(gòu)由左右移動(dòng)單元、上下移動(dòng)單元、俯仰單元、偏 擺單元和基座22構(gòu)成，上下移動(dòng)單元、左右移動(dòng)單元、俯仰單元和偏擺單元 均設(shè)置在基座22上；所述傳感器系統(tǒng)包括傾角傳感器26、第一振動(dòng)傳感器24 和第二振動(dòng)傳感器25，傾角傳感器26、第一振動(dòng)傳感器24和第二振動(dòng)傳感器 25均安裝在基座22上，傾角傳感器26分別測(cè)量基座22相對(duì)X軸方向和Y軸 方向的轉(zhuǎn)角；第一振動(dòng)傳感器24、第二振動(dòng)傳感器25分別測(cè)量基座22沿Z 軸方向和Y軸方向的移動(dòng)；所述防抖控制系統(tǒng)主要包括控制系統(tǒng)硬件和控制系 統(tǒng)軟件，用來(lái)實(shí)現(xiàn)視覺(jué)系統(tǒng)的防抖；
上下移動(dòng)單元由上下驅(qū)動(dòng)電機(jī)l、上下移動(dòng)單元傳動(dòng)機(jī)構(gòu)、上下運(yùn)動(dòng)導(dǎo)向軸7、固定塊8、滑塊6和上下運(yùn)動(dòng)同步帶28構(gòu)成；上下移動(dòng)單元傳動(dòng)機(jī)構(gòu)包 括上下運(yùn)動(dòng)繩輪2、上下運(yùn)動(dòng)換向輪3、第一上下運(yùn)動(dòng)同步帶輪4和第二上下 運(yùn)動(dòng)同步帶輪5;驅(qū)動(dòng)電機(jī)1通過(guò)上下移動(dòng)單元傳動(dòng)機(jī)構(gòu)驅(qū)動(dòng)與同步帶28固 連的固定塊8及滑塊6 —起沿導(dǎo)向軸7上下運(yùn)動(dòng)；上下移動(dòng)單元設(shè)置在安裝有 兩個(gè)視覺(jué)傳感器27的架體50的下方，滑塊6與架體50連接以實(shí)現(xiàn)兩個(gè)視覺(jué) 傳感器27相對(duì)基座22同時(shí)上下移動(dòng)；
左右移動(dòng)單元由左右驅(qū)動(dòng)電機(jī)18、左右移動(dòng)單元傳動(dòng)機(jī)構(gòu)和左右運(yùn)動(dòng)導(dǎo) 向軸21構(gòu)成；左右移動(dòng)單元傳動(dòng)機(jī)構(gòu)包括第一左右同步帶輪19、第二左右同 步帶輪20和左右運(yùn)動(dòng)同步帶29;左右驅(qū)動(dòng)電機(jī)18通過(guò)左右移動(dòng)單元傳動(dòng)機(jī) 構(gòu)驅(qū)動(dòng)與左右運(yùn)動(dòng)同步帶29固連的滑塊6 —起沿著導(dǎo)向軸21左右運(yùn)動(dòng)；左右 移動(dòng)單元設(shè)置在安裝有兩個(gè)視覺(jué)傳感器27的架體50的下方，滑塊6與架體 50連接以實(shí)現(xiàn)兩個(gè)視覺(jué)傳感器27相對(duì)基座22同時(shí)左右移動(dòng)；
俯仰單元由俯仰驅(qū)動(dòng)電機(jī)14、俯仰單元傳動(dòng)機(jī)構(gòu)和支架17構(gòu)成，俯仰單 元傳動(dòng)機(jī)構(gòu)包括第一俯仰繩輪15和第二俯仰繩輪16;俯仰驅(qū)動(dòng)電機(jī)14通過(guò) 俯仰單元傳動(dòng)機(jī)構(gòu)驅(qū)動(dòng)支架17做俯仰運(yùn)動(dòng)，固裝在支架17內(nèi)的兩個(gè)視覺(jué)傳感 器27相對(duì)基座22同時(shí)做俯仰運(yùn)動(dòng)(俯仰驅(qū)動(dòng)電機(jī)14通過(guò)第一俯仰繩輪15 帶動(dòng)第二俯仰繩輪16及與第二俯仰繩輪16固接的支架17與兩個(gè)視覺(jué)傳感器 27 —起實(shí)現(xiàn)俯仰運(yùn)動(dòng))；
偏擺單元由兩個(gè)偏擺驅(qū)動(dòng)電機(jī)9和兩套偏擺單元傳動(dòng)機(jī)構(gòu)構(gòu)成；每套偏擺 單元傳動(dòng)機(jī)構(gòu)包括每一偏擺繩輪10、偏擺換向輪11、第一偏擺同步帶輪12 和第二偏擺同步帶輪13;兩個(gè)偏擺驅(qū)動(dòng)電機(jī)9、兩套偏擺單元傳動(dòng)機(jī)構(gòu)左右對(duì) 稱(chēng)設(shè)置，每個(gè)偏擺驅(qū)動(dòng)電機(jī)9通過(guò)相應(yīng)的偏擺單元傳動(dòng)機(jī)構(gòu)實(shí)現(xiàn)每個(gè)視覺(jué)傳感器27相對(duì)基座22偏擺運(yùn)動(dòng)；偏擺單元除具有實(shí)現(xiàn)裝置的防抖功能外還實(shí)現(xiàn)了 機(jī)器人的雙目視覺(jué)；
傾角傳感器26、第一振動(dòng)傳感器24和第二振動(dòng)傳感器25均安裝在基座 22上，傾角傳感器26分別測(cè)量基座22相對(duì)Z軸方向和Y軸方向的轉(zhuǎn)角；第 一振動(dòng)傳感器24、第二振動(dòng)傳感器25分別測(cè)量基座22沿Z軸方向和Y軸方 向的移動(dòng)；
所述控制系統(tǒng)硬件由主控器51和五個(gè)電機(jī)驅(qū)動(dòng)與控制器52組成；五個(gè)電 機(jī)驅(qū)動(dòng)與控制器52分別控制上下驅(qū)動(dòng)電機(jī)1、左右驅(qū)動(dòng)電機(jī)18、俯仰驅(qū)動(dòng)電 機(jī)14以及兩個(gè)偏擺驅(qū)動(dòng)電機(jī)9的運(yùn)動(dòng)；控制系統(tǒng)軟件是基于移動(dòng)機(jī)器人視覺(jué) 系統(tǒng)防抖裝置的防抖補(bǔ)償控制方法利用相應(yīng)程序語(yǔ)言編寫(xiě)的專(zhuān)業(yè)軟件；傳感器 系統(tǒng)將感知到的移動(dòng)機(jī)器人視覺(jué)系統(tǒng)抖動(dòng)信息輸出給主控器51進(jìn)行處理，主 控器51內(nèi)的控制系統(tǒng)軟件進(jìn)行補(bǔ)償計(jì)算得到相應(yīng)補(bǔ)償量后輸出相應(yīng)的命令信 息給五個(gè)電機(jī)驅(qū)動(dòng)與控制器52，五個(gè)電機(jī)驅(qū)動(dòng)與控制器52控制、驅(qū)動(dòng)防抖機(jī) 構(gòu)中相應(yīng)的電機(jī)運(yùn)轉(zhuǎn)進(jìn)行抖動(dòng)補(bǔ)償，以控制兩個(gè)視覺(jué)傳感器27 (CCD27)相 對(duì)地面保持平穩(wěn)，從而實(shí)現(xiàn)移動(dòng)機(jī)器人視覺(jué)系統(tǒng)的防抖目的。
傾角傳感器26用來(lái)測(cè)量基座22分別相對(duì)X軸方向和Y軸方向的轉(zhuǎn)角^ 和灼(X軸、Y軸所在坐標(biāo)系的坐標(biāo)原點(diǎn)位于基座的幾何中心處)；該傳感器 為長(zhǎng)沙平川公司提供的SCA100T-D01型雙軸傾角傳感器，測(cè)量范圍為 0.5g(土30度)，單極5V供電，比例電壓輸出，高分辨率，數(shù)字SPI或模擬輸 出，內(nèi)置溫度傳感器，長(zhǎng)期穩(wěn)定性非常好，高分辨率，低噪聲，工作溫度范圍 寬。第一振動(dòng)傳感器24、第二振動(dòng)傳感器25分別用來(lái)測(cè)量基座22沿Z軸方 向和Y軸方向的移動(dòng)&、 /22;該傳感器為長(zhǎng)沙平川公司提供的N1000060(SCA60C)振動(dòng)傳感器，該傳感器為直流響應(yīng)，供電電壓+5V，電流損耗低(典 型植2mA)，與供電電壓成比例輸出，負(fù)載驅(qū)動(dòng)能力強(qiáng)。本發(fā)明所述的被控對(duì) 象——兩個(gè)視覺(jué)傳感器(CCD) 27為VS-902型微型高清晰工業(yè)CCD攝像機(jī)，尺 寸為38(W)X38(H)X50(D)，重量約250g，結(jié)構(gòu)緊湊，外型小巧、功耗低。防 抖控制系統(tǒng)23根據(jù)第一振動(dòng)傳感器24、第二振動(dòng)傳感器25、傾角傳感器26 分別測(cè)出來(lái)的抖動(dòng)參數(shù)^、 /22、 ^、 ^進(jìn)行抖動(dòng)補(bǔ)償計(jì)算，控制并驅(qū)動(dòng)防抖機(jī) 構(gòu)上的兩個(gè)視覺(jué)CCD傳感器(兩個(gè)視覺(jué)傳感器27)相對(duì)地面保持平穩(wěn)， 一次 性輸出較清晰圖像。本發(fā)明的防抖補(bǔ)償控制方法通過(guò)對(duì)裝置的動(dòng)力學(xué)分析進(jìn)行 動(dòng)態(tài)控制，采用前饋及PD反饋的控制方法實(shí)現(xiàn)裝置的防抖目的。
本實(shí)施方式所述的移動(dòng)機(jī)器人視覺(jué)系統(tǒng)防抖裝置具有5個(gè)自由度分別為 CCD 27的兩個(gè)獨(dú)立的擺動(dòng)、共同的俯仰自由度、整個(gè)裝置的上下移動(dòng)和左右 移動(dòng)。上下驅(qū)動(dòng)電機(jī)l、左右驅(qū)動(dòng)電機(jī)18、俯仰驅(qū)動(dòng)電機(jī)14和一個(gè)偏擺驅(qū)動(dòng) 電機(jī)9控制一個(gè)視覺(jué)傳感器27的位姿；上下驅(qū)動(dòng)電機(jī)1、左右驅(qū)動(dòng)電機(jī)18、 俯仰驅(qū)動(dòng)電機(jī)14和另一個(gè)偏擺驅(qū)動(dòng)電機(jī)9控制另一個(gè)視覺(jué)傳感器27的位姿。 其中擺動(dòng)和俯仰的軸線相交于一點(diǎn)類(lèi)似人眼的轉(zhuǎn)動(dòng)，目的是為了獲得機(jī)器人更 大的視野范圍。視覺(jué)系統(tǒng)防抖裝置中上下移動(dòng)單元至于左右移動(dòng)單元之上，目 的是為減小上下驅(qū)動(dòng)電機(jī)1的負(fù)載。視覺(jué)系統(tǒng)防抖裝置的寬為138mm，高為 190mm，深為112mm，總質(zhì)量為1. 42kg。
'具體實(shí)施方式
二、結(jié)合圖3、圖4、圖5、圖6說(shuō)明本實(shí)施方式，本實(shí)施 方式所述的移動(dòng)機(jī)器人視覺(jué)系統(tǒng)防抖裝置的防抖補(bǔ)償控制方法為
步驟一、由第一振動(dòng)傳感器24、第二振動(dòng)傳感器25和傾角傳感器26測(cè)
出來(lái)的抖動(dòng)參數(shù)&、 &、 ^、伊2及相應(yīng)的差分量確定測(cè)得抖動(dòng)量變化的速度和加速度量；
步驟二、根據(jù)傳感器系統(tǒng)在采樣時(shí)間7內(nèi)測(cè)量的情況，判斷移動(dòng)機(jī)器人視 覺(jué)系統(tǒng)是否有抖動(dòng)；如果有抖動(dòng)需進(jìn)行抖動(dòng)補(bǔ)償，執(zhí)行步驟三；否則則不需補(bǔ) 償；
步驟三、計(jì)算補(bǔ)償量建立各運(yùn)動(dòng)關(guān)節(jié)的D-H坐標(biāo)系描述相鄰桿件間 .平移q、 fl2和轉(zhuǎn)動(dòng)^、 &、 ^的關(guān)系；通過(guò)D-H參數(shù)和其次變換矩陣法推導(dǎo) 出各坐標(biāo)系相對(duì)0坐標(biāo)系的齊次變換矩陣，任意構(gòu)形k，^《A,《)下兩個(gè) CCD 27相對(duì)于裝置基座22的位姿矩陣分別為、CD,。4142434、 °reeD5=^"243J5;當(dāng)前構(gòu)形",^，4,4，^下兩個(gè)CCD 27相對(duì)于裝置基座 22的位姿矩陣分別為吁ccD,。i,Wi4、 afccD^i^AA;由第一振動(dòng)傳感 器24、第二振動(dòng)傳感器25、傾角傳感器26測(cè)出來(lái)的抖動(dòng)參數(shù)^、 &、伊!、 化(進(jìn)行差分后可得^、 4、 A、 確定CCD 27相對(duì)于裝置基座22抖
.動(dòng)的位姿矩陣為。7; =7>""(X，/07>a"(Y,&)i^(X,^)i W(Y,p2)，則有如下關(guān)系式
。rCCD4 -吁，=0rs 乂 24 乂 -。i, 'i2 2i3 3i4 = r訓(xùn)(x' )r腦(y, & )及w(x,伊,)t 。"y,化) 0rCCD5 -吁訓(xùn)=07^% M2 24 、5 -。i, 'i2 2i3 3i5 = r，(x, h)7v""(Y' ;^)及0仏^)i 。"Y,
由上述兩個(gè)式子在檢測(cè)得到抖動(dòng)參數(shù)h、 &、 ^、灼的情況下可推算出防 抖機(jī)構(gòu)在當(dāng)前構(gòu)形b,^，4,^^J過(guò)度到抖動(dòng)補(bǔ)償構(gòu)形fe，^《A，^情況下須補(bǔ) 償?shù)母麝P(guān)節(jié)運(yùn)動(dòng)量矢量U -^ ， "2 -52，《-4，《-么)、(a, -4 ， "2，《-4，
《-《)，即兩個(gè)CCD27在理論上的抖動(dòng)補(bǔ)償量矢量分別為 Aq-(勿p勿2，勿3,勿4)= ( q _^ ， "2_52， <93_4， 6>4-》4)、 Aq = (z^，勿2，匈3,勿4U,-a,， "2-a2，《-4，《-4)，進(jìn)行微分后可得A4、 則防抖裝置各關(guān) 節(jié)位置矢量為q = 9(仏'《2,《3'《4) = 9(4十勿p^ +勿2,4 +勿3,(《+^4)或(《+勿5))。
步驟四、規(guī)劃補(bǔ)償軌跡 '(f)，"(0，:O，求得關(guān)節(jié)補(bǔ)償角矢量Aq后， 為了保證在采樣時(shí)間T內(nèi)能完成補(bǔ)償過(guò)程，即關(guān)節(jié)補(bǔ)償矢量Aq從0到Aq ，
而且在r時(shí)刻時(shí)關(guān)節(jié)角的速度和加減速度都為o，必須在采樣時(shí)間r內(nèi)對(duì)補(bǔ)
償?shù)倪^(guò)程進(jìn)行4-3-4關(guān)節(jié)軌跡插值規(guī)劃；
步驟五、基于PD反饋和逆動(dòng)力學(xué)計(jì)算的抖動(dòng)補(bǔ)償控制以
q = q(^/i,^2'^^ 4) = q(4 +^2力3 +勿3'(& +勿4)或(《及該位
置下速度4 =4(^,^/2,^，^)、加速度4 = 4(^,^2,^,^4)為期望的控制輸
入，采用PD反饋和逆動(dòng)力學(xué)計(jì)算的方法對(duì)防抖裝置的各自由度運(yùn)動(dòng)單元上 的電機(jī)進(jìn)行控制；反饋量為來(lái)自于各電機(jī)上的光電編碼器位置和速度量經(jīng)
機(jī)械傳動(dòng)裝置折算到關(guān)節(jié)側(cè)的量。
圖4為補(bǔ)償量在第/個(gè)采樣周期r內(nèi)的4-3-4關(guān)節(jié)插值軌跡規(guī)劃法得
到的關(guān)節(jié)補(bǔ)償量勿,與時(shí)間的關(guān)系曲線。求得關(guān)節(jié)補(bǔ)償量Aq后，為了保證在 采樣時(shí)間r內(nèi)能完成補(bǔ)償過(guò)程，即關(guān)節(jié)補(bǔ)償量Aq從O到Aq，而且在7時(shí)刻
時(shí)關(guān)節(jié)角的速度和減速度都為o，必須對(duì)補(bǔ)償?shù)倪^(guò)程進(jìn)行軌跡規(guī)劃得到
Aq(f)。軌跡規(guī)劃出來(lái)的Aq(O經(jīng)過(guò)逆動(dòng)力學(xué)計(jì)算得到要達(dá)到該關(guān)節(jié)角所應(yīng)提 供的力矩T,，直流電機(jī)將力矩r,傳遞給防抖裝置，通過(guò)關(guān)節(jié)的轉(zhuǎn)動(dòng)和移動(dòng)來(lái)
進(jìn)行抖動(dòng)補(bǔ)償。
圖5為視覺(jué)系統(tǒng)防抖裝置的動(dòng)力學(xué)簡(jiǎn)化模型(該圖含有各桿件質(zhì)心、
.質(zhì)量參數(shù)，是動(dòng)力學(xué)模型必須表示的)。通過(guò)視覺(jué)系統(tǒng)防抖裝置的動(dòng)力學(xué)簡(jiǎn) 化模型，分析影響系統(tǒng)性能指標(biāo)的各種因素，設(shè)計(jì)出合理的系統(tǒng)控制方案。
圖5中的結(jié)構(gòu)模型各參數(shù)的含義如表1所示。同時(shí)取(裝置)基座22所在的平面為零勢(shì)能面。
表1模型中各參數(shù)含義
裝置的運(yùn)動(dòng)方式質(zhì) 旦 里關(guān)節(jié) 角動(dòng) 能勢(shì) 臺(tái).匕 目匕力 矩
左右移動(dòng)附tA
左右移動(dòng)、上下移動(dòng)附2a2r2
左右移動(dòng)、上下移動(dòng)、俯仰附3&r3
左右移動(dòng)、上下移動(dòng)、俯仰、擺動(dòng)附4《r4
引入拉格朗日方程法建立機(jī)器人的動(dòng)力學(xué)方程-
=1!—!,Z = l,2,3，4,5;經(jīng)測(cè)量和參數(shù)識(shí)別實(shí)驗(yàn)確定質(zhì)心位置、質(zhì)量、 A械 3仏
轉(zhuǎn)動(dòng)慣量以及粘性摩擦和動(dòng)摩擦項(xiàng)系數(shù)矩陣等，得到較為精確的動(dòng)力學(xué)方程
式矢量表示形式T(q,4，a),q)a + ￡(q,4) + §4 + 5(4) + I(q)(其中， q = ,&'&'《4) = q(4 +」a'^ + '4 + '(《+勿4)或的+ d《5》)用于進(jìn)，亍基于 逆動(dòng)力學(xué)計(jì)算的前饋，以減小抖動(dòng)補(bǔ)償控制的誤差。其中，變量上方的" "
號(hào)表示經(jīng)參數(shù)識(shí)別后的量，如&(q)、 ￡(q,4)、 §4、 6(4)、欲q)分別表示經(jīng)參數(shù)識(shí) 別實(shí)驗(yàn)和參數(shù)識(shí)別算法計(jì)算后得到的慣性力項(xiàng)的系數(shù)矩陣、離心力與科氏力 項(xiàng)、粘性摩擦項(xiàng)系數(shù)矩陣、動(dòng)摩擦項(xiàng)、重力項(xiàng)。
圖6為基于pd反饋和逆動(dòng)力學(xué)前饋的控制器框圖?？刂戚斎霝榉蓝堆b 置各關(guān)節(jié)期望得到補(bǔ)償?shù)年P(guān)節(jié)位置
qrf = qrf (&i' ' w") = q(A +勿！, 52 +勿2' 4 + d《3'(《+ A4 )或(《+」《5))及該位置 下速度、加速度，這里下標(biāo)表示期望得到補(bǔ)償。此外，變量上方的"A" 號(hào)表示進(jìn)行抖動(dòng)補(bǔ)償之前的關(guān)節(jié)當(dāng)前位置量，如A表示關(guān)節(jié)4當(dāng)前位置量。
具體實(shí)施方式
三、下面結(jié)合圖7說(shuō)明本實(shí)施方式，本發(fā)明所述視覺(jué)系統(tǒng) 防抖裝置可應(yīng)用在仿人機(jī)器人30上。視覺(jué)系統(tǒng)防抖裝置可作為仿人機(jī)器人的頭部，通過(guò)裝置基座22與仿人機(jī)器人30上的頸部31上的接口固連。視覺(jué)系 .統(tǒng)防抖裝置中的防抖控制系統(tǒng)23作為防抖控制的底層控制系統(tǒng)，通過(guò)控制 線、數(shù)據(jù)線與仿人機(jī)器人30胸部?jī)?nèi)的上位控制機(jī)相連。
具體實(shí)施方式
四結(jié)合圖8說(shuō)明本實(shí)施方式，本發(fā)明所述視覺(jué)系統(tǒng)防抖 裝置可作為仿人機(jī)器人的頭部，通過(guò)裝置基座22與輪式移動(dòng)機(jī)器人32上的 頸部33上的接口固連。視覺(jué)系統(tǒng)防抖裝置中的防抖控制系統(tǒng)23作為防抖控 制的底層控制系統(tǒng)，通過(guò)控制線、數(shù)據(jù)線與輪式移動(dòng)機(jī)器人32胸部?jī)?nèi)的上位 控制機(jī)相連。
本發(fā)明所述視覺(jué)系統(tǒng)防抖裝置在移動(dòng)機(jī)器人中應(yīng)用不僅限制在以上兩種 移動(dòng)機(jī)器人上，還包括其他類(lèi)型的移動(dòng)機(jī)器人如履帶式移動(dòng)機(jī)器人等。
具體實(shí)施方式
五結(jié)合圖9說(shuō)明本實(shí)施方式。采用PD控制+逆動(dòng)力學(xué)計(jì) 算法進(jìn)行抖動(dòng)補(bǔ)償控制仿真。用fee，(p,、，^zj表示兩個(gè)視覺(jué)傳感器27相對(duì) 于地面的位姿參數(shù)。圖9a為無(wú)抖動(dòng)時(shí)即期望的CCD相對(duì)于地面的位姿曲線； 在圖9b所示抖動(dòng)頻率戶20時(shí)隨機(jī)抖動(dòng)參數(shù)&、 &曲線(縱軸表示抖動(dòng)位移 量，單位為m，抖動(dòng)位移幅度為ilmm;橫軸表示時(shí)間，單位為s)、抖動(dòng)偏擺 幅度A、 "2皆為±5°、傳感器的響應(yīng)頻率A 20Hz (即采樣時(shí)間為0.05s)的情 況下，利用Matlab軟件進(jìn)行有抖動(dòng)不補(bǔ)償控制和抖動(dòng)補(bǔ)償控制，控制仿真結(jié) 果如圖9c和9d所示。9c為有抖動(dòng)不補(bǔ)償時(shí)CCD相對(duì)于地面的位姿曲線； 圖9d為進(jìn)行抖動(dòng)補(bǔ)償時(shí)CCD相對(duì)于地面的位姿曲線。由圖9c和圖9a對(duì)比 可以明顯看出未經(jīng)補(bǔ)償?shù)膱D9c所示的位姿曲線表明抖動(dòng)給視覺(jué)傳感器27 帶來(lái)了較大的位姿誤差，其結(jié)果將導(dǎo)致圖像模糊和較大的視覺(jué)定位誤差；而 由圖9d和圖9a對(duì)比可以明顯看出采用抖動(dòng)補(bǔ)償后的27各項(xiàng)位姿參數(shù)曲線 圖9d能夠較為精確地接近于圖9a所示的期望位姿參數(shù)曲線。
19
權(quán)利要求
1、一種移動(dòng)機(jī)器人視覺(jué)系統(tǒng)防抖裝置，所述視覺(jué)系統(tǒng)防抖裝置由防抖機(jī)構(gòu)、傳感器系統(tǒng)和防抖控制系統(tǒng)三部分構(gòu)成；其特征在于所述防抖機(jī)構(gòu)由左右移動(dòng)單元、上下移動(dòng)單元、俯仰單元、偏擺單元和基座(22)構(gòu)成，上下移動(dòng)單元、左右移動(dòng)單元、俯仰單元和偏擺單元均設(shè)置在基座(22)上；所述傳感器系統(tǒng)包括傾角傳感器(26)、第一振動(dòng)傳感器(24)和第二振動(dòng)傳感器(25)，傾角傳感器(26)、第一振動(dòng)傳感器(24)和第二振動(dòng)傳感器(25)均安裝在基座(22)上；所述防抖控制系統(tǒng)主要包括控制系統(tǒng)硬件和控制系統(tǒng)軟件，用于視覺(jué)系統(tǒng)的防抖；上下移動(dòng)單元由上下驅(qū)動(dòng)電機(jī)(1)、上下移動(dòng)單元傳動(dòng)機(jī)構(gòu)、上下運(yùn)動(dòng)導(dǎo)向軸(7)、固定塊(8)、滑塊(6)和上下運(yùn)動(dòng)同步帶(28)構(gòu)成；上下移動(dòng)單元傳動(dòng)機(jī)構(gòu)包括上下運(yùn)動(dòng)繩輪(2)、上下運(yùn)動(dòng)換向輪(3)、第一上下運(yùn)動(dòng)同步帶輪(4)和第二上下運(yùn)動(dòng)同步帶輪(5)；驅(qū)動(dòng)電機(jī)(1)通過(guò)上下移動(dòng)單元傳動(dòng)機(jī)構(gòu)驅(qū)動(dòng)與同步帶(28)固連的固定塊(8)及滑塊(6)一起沿導(dǎo)向軸(7)上下運(yùn)動(dòng)；上下移動(dòng)單元設(shè)置在安裝有兩個(gè)視覺(jué)傳感器(27)的架體(50)的下方，滑塊(6)與架體(50)連接以實(shí)現(xiàn)兩個(gè)視覺(jué)傳感器(27)相對(duì)基座(22)同時(shí)上下移動(dòng)；左右移動(dòng)單元由左右驅(qū)動(dòng)電機(jī)(18)、左右移動(dòng)單元傳動(dòng)機(jī)構(gòu)和左右運(yùn)動(dòng)導(dǎo)向軸(21)構(gòu)成；左右移動(dòng)單元傳動(dòng)機(jī)構(gòu)包括第一左右同步帶輪(19)、第二左右同步帶輪(20)和左右運(yùn)動(dòng)同步帶(29)；左右驅(qū)動(dòng)電機(jī)(18)通過(guò)左右移動(dòng)單元傳動(dòng)機(jī)構(gòu)驅(qū)動(dòng)與左右運(yùn)動(dòng)同步帶(29)固連的滑塊(6)一起沿著導(dǎo)向軸(21)左右運(yùn)動(dòng)；左右移動(dòng)單元設(shè)置在安裝有兩個(gè)視覺(jué)傳感器(27)的架體(50)的下方，滑塊(6)與架體(50)連接以實(shí)現(xiàn)兩個(gè)視覺(jué)傳感器(27)相對(duì)基座(22)同時(shí)左右移動(dòng)；俯仰單元由俯仰驅(qū)動(dòng)電機(jī)(14)、俯仰單元傳動(dòng)機(jī)構(gòu)和支架(17)構(gòu)成，俯仰單元傳動(dòng)機(jī)構(gòu)包括第一俯仰繩輪(15)和第二俯仰繩輪(16)；俯仰驅(qū)動(dòng)電機(jī)(14)通過(guò)俯仰單元傳動(dòng)機(jī)構(gòu)驅(qū)動(dòng)支架(17)做俯仰運(yùn)動(dòng)，固裝在支架(17)內(nèi)的兩個(gè)視覺(jué)傳感器(27)相對(duì)基座(22)同時(shí)做俯仰運(yùn)動(dòng)；偏擺單元由兩個(gè)偏擺驅(qū)動(dòng)電機(jī)(9)和兩套偏擺單元傳動(dòng)機(jī)構(gòu)構(gòu)成；每套偏擺單元傳動(dòng)機(jī)構(gòu)包括每一偏擺繩輪(10)、偏擺換向輪(11)、第一偏擺同步帶輪(12)和第二偏擺同步帶輪(13)；兩個(gè)偏擺驅(qū)動(dòng)電機(jī)(9)、兩套偏擺單元傳動(dòng)機(jī)構(gòu)左右對(duì)稱(chēng)設(shè)置，每個(gè)偏擺驅(qū)動(dòng)電機(jī)(9)通過(guò)相應(yīng)的偏擺單元傳動(dòng)機(jī)構(gòu)實(shí)現(xiàn)每個(gè)視覺(jué)傳感器(27)相對(duì)基座(22)偏擺運(yùn)動(dòng)；所述控制系統(tǒng)硬件由主控器(51)和五個(gè)電機(jī)驅(qū)動(dòng)與控制器(52)組成；五個(gè)電機(jī)驅(qū)動(dòng)與控制器(52)分別控制上下驅(qū)動(dòng)電機(jī)(1)、左右驅(qū)動(dòng)電機(jī)(18)、俯仰驅(qū)動(dòng)電機(jī)(14)以及兩個(gè)偏擺驅(qū)動(dòng)電機(jī)(9)的運(yùn)動(dòng)；控制系統(tǒng)軟件是基于移動(dòng)機(jī)器人視覺(jué)系統(tǒng)防抖裝置的防抖補(bǔ)償控制方法利用相應(yīng)程序語(yǔ)言編寫(xiě)的專(zhuān)業(yè)軟件；傳感器系統(tǒng)將感知到的移動(dòng)機(jī)器人視覺(jué)系統(tǒng)抖動(dòng)信息輸出給主控器(51)進(jìn)行處理，主控器(51)內(nèi)的控制系統(tǒng)軟件進(jìn)行補(bǔ)償計(jì)算得到相應(yīng)補(bǔ)償量后輸出相應(yīng)的命令信息給五個(gè)電機(jī)驅(qū)動(dòng)與控制器(52)，五個(gè)電機(jī)驅(qū)動(dòng)與控制器(52)控制、驅(qū)動(dòng)防抖機(jī)構(gòu)中相應(yīng)的電機(jī)運(yùn)轉(zhuǎn)進(jìn)行抖動(dòng)補(bǔ)償，以控制兩個(gè)視覺(jué)傳感器(27)相對(duì)地面保持平穩(wěn)，從而實(shí)現(xiàn)移動(dòng)機(jī)器人視覺(jué)系統(tǒng)的防抖目的。
2、 一種權(quán)利要求1所述移動(dòng)機(jī)器人視覺(jué)系統(tǒng)防抖裝置的防抖補(bǔ)償控制方法，其特征在于所述方法按照以下步驟實(shí)現(xiàn)的步驟一、由第一振動(dòng)傳感器(24)、第二振動(dòng)傳感器(25)和傾角傳感器 (26)測(cè)出來(lái)的抖動(dòng)參數(shù)&、 /22、 ^、灼及相應(yīng)的差分量確定測(cè)得抖動(dòng)量變化 的速度和加速度量；步驟二、根據(jù)傳感器系統(tǒng)在采樣時(shí)間7內(nèi)測(cè)量的情況，判斷移動(dòng)機(jī)器人視覺(jué)系統(tǒng)是否有抖動(dòng)；如果有抖動(dòng)需進(jìn)行抖動(dòng)補(bǔ)償，執(zhí)行步驟三；否則則不需補(bǔ) 償；步驟三、計(jì)算補(bǔ)償量任意構(gòu)形h，^,《，^,《l下兩個(gè)視覺(jué)傳感器(27)相對(duì)于裝置基座(22 )的位姿矩陣分別為e^D^、'42434 、°rccd5=° 434;當(dāng)前構(gòu)形fe,^,4A,^下兩個(gè)視覺(jué)傳感器(27)相對(duì)于裝置基座(22)的位姿矩陣分別為吁③Zi,i2、3i,、 Acd^^^AA;由 第一振動(dòng)傳感器(24)、第二振動(dòng)傳感器(25)、傾角傳感器(26)測(cè)出來(lái).的抖動(dòng)參數(shù)m、 /22、 ^、 ^ (進(jìn)行差分后可得^、 &、 a、 確定視覺(jué)傳感器(27 )相對(duì)于裝置基座(22 )抖動(dòng)的位姿矩陣為。rs =rraw(X,/Orra"(Y,/i2)i o/(X,A)i o"Y,^2)，則有如下關(guān)系式，ccd4 =。& =>。4 " H 34 — A1A 24 3足=2>朋",)7>W7(Y, / 2 )及W(X, ^ )及。"Y, % )。rCCD5 -0:fCCD5=0rs=^04 、 24 34—。4142 A3 4 = rm"(x, & )rra"(y' & )i 。"x,仍)i w (y,伊2)由上述兩個(gè)式子在檢測(cè)得到抖動(dòng)參數(shù)h、 &、 w、 w的情況下可推算出防 抖機(jī)構(gòu)在當(dāng)前構(gòu)形^,^^夂,^過(guò)度到抖動(dòng)補(bǔ)償構(gòu)形h,^《A,^情況下須補(bǔ)償?shù)母麝P(guān)節(jié)運(yùn)動(dòng)量矢量(a -a, ， a2 -a2 ， 6>3 -4 ， a - A )、 ( a -^ ， a2 -52 ，《-《，即兩個(gè)視覺(jué)傳感器27在理論上的抖動(dòng)補(bǔ)償量矢量分別為 Aq-(勿"勿2，勿3,勿4)= ( a, — ^ ， fl2—4，《—^3， & —么)、Aq-(匈!，匈2,勿3,勿4)= U,-^，a2，《-4，《-4)，進(jìn)行微分后可得A(j、 則防抖裝置各關(guān) 節(jié)位置矢量為q = q(^,《2,《3,《4) = q(4 +勿2,4 +勿3'(《+匈4)或(《+勿5》；步驟四、規(guī)劃補(bǔ)償軌跡勿'W，^^，n，求得關(guān)節(jié)補(bǔ)償角矢量Aq后， 為了保證在采樣時(shí)間r內(nèi)能完成補(bǔ)償過(guò)程，即關(guān)節(jié)補(bǔ)償矢量Aq從O到Aq，.而且在r時(shí)刻時(shí)關(guān)節(jié)角的速度和加減速度都為o，必須在采樣時(shí)間r內(nèi)對(duì)補(bǔ)償?shù)倪^(guò)程進(jìn)行4-3-4關(guān)節(jié)軌跡插值規(guī)劃；步驟五、基于PD反饋和逆動(dòng)力學(xué)計(jì)算的抖動(dòng)補(bǔ)償控制以q = q("i'^2'^/3'^4) = q(4 +勿2,夂+ ^3,(& +勿4)或(《+^《5))及該位置下速度4 = 4(^,^2,^3,^4)、加速度4 = 4(^^2，^3,^4)為期望的控制輸入，采用PD反饋和逆動(dòng)力學(xué)計(jì)算的方法對(duì)防抖裝置的各自由度運(yùn)動(dòng)單元上 的電機(jī)進(jìn)行控制；反饋量為來(lái)自于各電機(jī)上的光電編碼器位置和速度量經(jīng) .機(jī)械傳動(dòng)裝置折算到關(guān)節(jié)側(cè)的量。
全文摘要
移動(dòng)機(jī)器人視覺(jué)系統(tǒng)防抖裝置與防抖補(bǔ)償控制方法，它涉及移動(dòng)機(jī)器人用視覺(jué)系統(tǒng)防抖裝置與防抖補(bǔ)償控制方法。針對(duì)移動(dòng)機(jī)器人的抖動(dòng)情況，通過(guò)機(jī)械和控制手段從根本上實(shí)時(shí)地消除機(jī)器人視覺(jué)系統(tǒng)的抖動(dòng)，以達(dá)到視覺(jué)系統(tǒng)運(yùn)動(dòng)的平穩(wěn)。視覺(jué)系統(tǒng)防抖裝置由防抖機(jī)構(gòu)、傳感器系統(tǒng)、防抖控制系統(tǒng)構(gòu)成；防抖控制系統(tǒng)用來(lái)實(shí)現(xiàn)視覺(jué)系統(tǒng)的防抖。本方法步驟為抖動(dòng)參數(shù)的測(cè)量、判斷移動(dòng)機(jī)器人視覺(jué)系統(tǒng)是否有抖動(dòng)、計(jì)算補(bǔ)償量、規(guī)劃補(bǔ)償軌跡和基于PD反饋和逆動(dòng)力學(xué)計(jì)算的抖動(dòng)補(bǔ)償控制。防抖裝置實(shí)現(xiàn)對(duì)兩個(gè)視覺(jué)傳感器較大范圍內(nèi)的運(yùn)動(dòng)控制并對(duì)兩個(gè)視覺(jué)傳感器產(chǎn)生的抖動(dòng)進(jìn)行防控。采用防抖補(bǔ)償控制方法后的兩個(gè)視覺(jué)傳感器的各項(xiàng)位姿參數(shù)曲線能夠較為精確地接近于期望位姿參數(shù)曲線。
文檔編號(hào)B25J19/00GK101612735SQ20091007258
公開(kāi)日2009年12月30日 申請(qǐng)日期2009年7月24日 優(yōu)先權(quán)日2009年7月24日
發(fā)明者吳偉國(guó), 邵益凱 申請(qǐng)人:哈爾濱工業(yè)大學(xué)