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

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


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