[發(fā)明專利]移動機(jī)器人視覺系統(tǒng)防抖裝置與防抖補(bǔ)償控制方法

有效

• 申請?zhí)枺?/td>: 200910072585.7
• 申請日: 2009-07-24
• 公開（公告）號: CN101612735A
• 公開（公告）日: 2009-12-30
• 發(fā)明（設(shè)計(jì)）人: 吳偉國;邵益凱
• 申請（專利權(quán)）人: 哈爾濱工業(yè)大學(xué)
• 主分類號: B25J19/00
• 分類號: B25J19/00;B25J5/00
• 代理公司: 哈爾濱市松花江專利商標(biāo)事務(wù)所
• 代理人: 楊立超

1.一種移動機(jī)器人視覺系統(tǒng)防抖裝置，所述視覺系統(tǒng)防抖裝置由防抖機(jī) 構(gòu)、傳感器系統(tǒng)和防抖控制系統(tǒng)三部分構(gòu)成；其特征在于：所述防抖機(jī)構(gòu)由左 右移動單元、上下移動單元、俯仰單元、偏擺單元和基座(22)構(gòu)成，上下移 動單元、左右移動單元、俯仰單元和偏擺單元均設(shè)置在基座(22)上；所述傳 感器系統(tǒng)包括傾角傳感器(26)、第一振動傳感器(24)和第二振動傳感器(25)， 傾角傳感器(26)、第一振動傳感器(24)和第二振動傳感器(25)均安裝在 基座(22)上；所述防抖控制系統(tǒng)主要包括控制系統(tǒng)硬件和控制系統(tǒng)軟件，用 于視覺系統(tǒng)的防抖；

上下移動單元由上下驅(qū)動電機(jī)(1)、上下移動單元傳動機(jī)構(gòu)、上下運(yùn)動導(dǎo) 向軸(7)、固定塊(8)、滑塊(6)和上下運(yùn)動同步帶(28)構(gòu)成；上下移動 單元傳動機(jī)構(gòu)包括上下運(yùn)動繩輪(2)、上下運(yùn)動換向輪(3)、第一上下運(yùn)動同 步帶輪(4)和第二上下運(yùn)動同步帶輪(5)；上下驅(qū)動電機(jī)(1)通過上下移動 單元傳動機(jī)構(gòu)驅(qū)動與上下運(yùn)動同步帶(28)固連的固定塊(8)及滑塊(6)一 起沿上下運(yùn)動導(dǎo)向軸(7)上下運(yùn)動；上下移動單元設(shè)置在安裝有兩個(gè)視覺傳 感器(27)的架體(50)的下方，滑塊(6)與架體(50)連接以實(shí)現(xiàn)兩個(gè)視 覺傳感器(27)相對基座(22)同時(shí)上下移動；

左右移動單元由左右驅(qū)動電機(jī)(18)、左右移動單元傳動機(jī)構(gòu)和左右運(yùn)動 導(dǎo)向軸(21)構(gòu)成；左右移動單元傳動機(jī)構(gòu)包括第一左右同步帶輪(19)、第 二左右同步帶輪(20)和左右運(yùn)動同步帶(29)；左右驅(qū)動電機(jī)(18)通過左 右移動單元傳動機(jī)構(gòu)驅(qū)動與左右運(yùn)動同步帶(29)固連的滑塊(6)一起沿著 導(dǎo)向軸(21)左右運(yùn)動；左右移動單元設(shè)置在安裝有兩個(gè)視覺傳感器(27)的 架體(50)的下方，滑塊(6)與架體(50)連接以實(shí)現(xiàn)兩個(gè)視覺傳感器(27) 相對基座(22)同時(shí)左右移動；

俯仰單元由俯仰驅(qū)動電機(jī)(14)、俯仰單元傳動機(jī)構(gòu)和支架(17)構(gòu)成， 俯仰單元傳動機(jī)構(gòu)包括第一俯仰繩輪(15)和第二俯仰繩輪(16)；俯仰驅(qū)動 電機(jī)(14)通過俯仰單元傳動機(jī)構(gòu)驅(qū)動支架(17)做俯仰運(yùn)動，固裝在支架(17) 內(nèi)的兩個(gè)視覺傳感器(27)相對基座(22)同時(shí)做俯仰運(yùn)動，固裝在支架(17) 內(nèi)的兩個(gè)視覺傳感器(27)連同支架(17)安裝在架體(50)內(nèi)；

偏擺單元由兩個(gè)偏擺驅(qū)動電機(jī)(9)和兩套偏擺單元傳動機(jī)構(gòu)構(gòu)成；每套 偏擺單元傳動機(jī)構(gòu)包括第一偏擺繩輪(10)、偏擺換向輪(11)、第一偏擺同步 帶輪(12)和第二偏擺同步帶輪(13)；兩個(gè)偏擺驅(qū)動電機(jī)(9)、兩套偏擺單 元傳動機(jī)構(gòu)左右對稱設(shè)置，每個(gè)偏擺驅(qū)動電機(jī)(9)通過相應(yīng)的偏擺單元傳動 機(jī)構(gòu)實(shí)現(xiàn)每個(gè)視覺傳感器(27)相對基座(22)偏擺運(yùn)動；

所述控制系統(tǒng)硬件由主控器(51)和五個(gè)電機(jī)驅(qū)動與控制器(52)組成； 五個(gè)電機(jī)驅(qū)動與控制器(52)分別控制上下驅(qū)動電機(jī)(1)、左右驅(qū)動電機(jī)(18)、 俯仰驅(qū)動電機(jī)(14)以及兩個(gè)偏擺驅(qū)動電機(jī)(9)的運(yùn)動；控制系統(tǒng)軟件是基 于移動機(jī)器人視覺系統(tǒng)防抖裝置的防抖補(bǔ)償控制方法利用相應(yīng)程序語言編寫 的專業(yè)軟件；傳感器系統(tǒng)將感知到的移動機(jī)器人視覺系統(tǒng)抖動信息輸出給主控 器(51)進(jìn)行處理，主控器(51)內(nèi)的控制系統(tǒng)軟件進(jìn)行補(bǔ)償計(jì)算得到相應(yīng)補(bǔ) 償量后輸出相應(yīng)的命令信息給五個(gè)電機(jī)驅(qū)動與控制器(52)，五個(gè)電機(jī)驅(qū)動與 控制器(52)控制、驅(qū)動防抖機(jī)構(gòu)中相應(yīng)的電機(jī)運(yùn)轉(zhuǎn)進(jìn)行抖動補(bǔ)償，以控制兩 個(gè)視覺傳感器(27)相對地面保持平穩(wěn)，從而實(shí)現(xiàn)移動機(jī)器人視覺系統(tǒng)的防抖 目的。

2.一種權(quán)利要求1所述移動機(jī)器人視覺系統(tǒng)防抖裝置的防抖補(bǔ)償控制方 法，其特征在于：所述方法按照以下步驟實(shí)現(xiàn)的：

步驟一、由第一振動傳感器(24)、第二振動傳感器(25)和傾角傳感器 (26)測出來的抖動參數(shù)h₁、h₂、及相應(yīng)的差分量確定測得抖動量變化 的速度和加速度量；

h₁表示基座(22)沿Z軸方向的移動；

h₂表示基座(22)沿Y軸方向的移動；

表示基座(22)相對X軸方向轉(zhuǎn)角；

表示基座(22)相對Y軸方向轉(zhuǎn)角；

其中，X軸、Y軸所在坐標(biāo)系的坐標(biāo)原點(diǎn)位于基座的幾何中心處；

步驟二、根據(jù)傳感器系統(tǒng)在采樣時(shí)間T內(nèi)測量的情況，判斷移動機(jī)器人視 覺系統(tǒng)是否有抖動；如果有抖動需進(jìn)行抖動補(bǔ)償，執(zhí)行步驟三、四、五；否則 則不需補(bǔ)償；

步驟三、計(jì)算補(bǔ)償量：任意構(gòu)形{a₁，a₂，θ₃，θ₄，θ₅}下兩個(gè)視覺傳感器(27) 相對于裝置基座(22)的位姿矩陣分別為⁰T_CCD4＝⁰A₁¹A₂²A₃³A₄、 ⁰T_CCD5＝⁰A₁¹A₂²A₃³A₅；當(dāng)前構(gòu)形下兩個(gè)視覺傳感器(27)相對于 裝置基座(22)的位姿矩陣分別為由 第一振動傳感器(24)、第二振動傳感器(25)、傾角傳感器(26)測出來 的抖動參數(shù)h₁、h₂、確定視覺傳感器(27)相對于裝置基座(22) 抖動的位姿矩陣為則有如下關(guān)系 式：

上述式中的X、Y表示X軸、Y軸，X軸、Y軸所在坐標(biāo)系的坐標(biāo)原點(diǎn)位 于基座的幾何中心處；

由上述兩個(gè)式子在檢測得到抖動參數(shù)h₁、h₂、的情況下可推算出 防抖機(jī)構(gòu)在當(dāng)前構(gòu)形過度到抖動補(bǔ)償構(gòu)形{a₁，a₂，θ₃，θ₄，θ₅}情況 下須補(bǔ)償?shù)母麝P(guān)節(jié)抖動補(bǔ)償矢量(a1-a^1,a2-a^2,θ3-θ^3,θ4-θ^4),(a1-a^1,]]>a2-a^2,θ3-θ^3,θ5-θ^5),]]>即兩個(gè)視覺傳感器(27)在理論上的抖動補(bǔ)償矢 量分別為Δq=(Δq1,Δq2,Δq3,Δq4)=(a1-a^1,a2-a^2,θ3-θ^3,θ4-θ^4),]]>Δq=(Δq1,Δq2,Δq3,Δq4)=(a1-a^1,a2-a^2,θ3-θ^3,θ5-θ^5),]]>進(jìn)行微分后可得則防抖裝置各關(guān)節(jié)補(bǔ)償后位置矢量為：

a₁表示兩個(gè)視覺傳感器(27)的左右移動矢量；

a₂表示兩個(gè)CCD的上下移動矢量；

θ₃表示兩個(gè)CCD的俯仰角；

θ₄表示CCD1的偏擺角；

θ₅表示CCD2的偏擺角；

⁰A₁表示左右移動單元相對于基座(22)的位姿矩陣；

¹A₂表示上下移動單元相對于左右移動單元的位姿矩陣；

²A₃表示俯仰單元相對于上下移動單元的位姿矩陣；

³A₄表示偏擺單元中CCD1相對于俯仰單元的位姿矩陣；

³A₅表示偏擺單元中CCD2相對于俯仰單元的位姿矩陣；

步驟四、規(guī)劃補(bǔ)償軌跡：Δq_i(t)，t∈(0，T)，求得關(guān)節(jié)抖動補(bǔ)償矢量Δq后， 為了保證在采樣時(shí)間T內(nèi)能完成補(bǔ)償過程，即關(guān)節(jié)抖動補(bǔ)償矢量從0到Δq， 而且在T時(shí)刻時(shí)各關(guān)節(jié)的速度和加減速度都為0，必須在采樣時(shí)間T內(nèi)對補(bǔ) 償?shù)倪^程進(jìn)行4-3-4關(guān)節(jié)軌跡插值規(guī)劃；

步驟五、基于PD反饋和逆動力學(xué)計(jì)算的抖動補(bǔ)償控制：以 q=q(qd1,qd2,qd3,qd4)=q(a^1+Δq1,a^2+Δq2,θ^3+Δq3,(θ4+Δq4)]]>或(θ₅+Δq₅))及該位 置下速度q·=q·(q·d1,q·d2,q·d3,q·d4),]]>加速度q··=q··(q··d1,q··d2,q··d3,q··d4)]]>為期望的控制輸 入，采用PD反饋和逆動力學(xué)計(jì)算的方法對防抖裝置的各自由度運(yùn)動單元上 的電機(jī)進(jìn)行控制；反饋量為來自于各電機(jī)上的光電編碼器位置和速度量經(jīng) 機(jī)械傳動裝置折算到關(guān)節(jié)側(cè)的量。