專利名稱：基于單目攝像頭和led的機(jī)器人對(duì)接定位和導(dǎo)航策略的制作方法
技術(shù)領(lǐng)域：
本發(fā)明屬于群體機(jī)器人領(lǐng)域，更具體地說(shuō)，是涉及一種群體機(jī)器人自組裝對(duì)接過(guò)程中的定位和導(dǎo)航策略。
背景技術(shù)：
隨著技術(shù)的發(fā)展和社會(huì)的進(jìn)步，人們對(duì)機(jī)器人的功能需求不斷增加，在一些非結(jié)構(gòu)化的復(fù)雜環(huán)境中往往需要機(jī)器人具有良好的環(huán)境適應(yīng)性和靈活性，群體機(jī)器人自組裝和自重構(gòu)特點(diǎn)能夠滿足這種要求同時(shí)也是群體機(jī)器人研究的關(guān)鍵方面。目前，國(guó)內(nèi)外一些高校研究的群體機(jī)器人在自組裝和自重構(gòu)方面的能力都還比較有限，尤其是針對(duì)自組裝過(guò)程中機(jī)器人的定位和導(dǎo)航策略這一難點(diǎn)，主要的方案都是基于紅外傳感器、觸控開(kāi)關(guān)等局部有限傳感導(dǎo)航和機(jī)械外形配合、磁鐵等對(duì)齊引導(dǎo)方式，所提供的定位和導(dǎo)航信息不足，具有使用價(jià)值的有效方案和策略較少，限制了群體機(jī)器人自組裝的效率。

發(fā)明內(nèi)容
現(xiàn)有基于紅外傳感器、觸控開(kāi)關(guān)、機(jī)械外形配合或磁鐵的技術(shù)方案存在對(duì)接引導(dǎo)信息不足，效率較低等問(wèn)題，為了解決現(xiàn)有技術(shù)的不足，本發(fā)明提出了一種能夠提供相對(duì)豐富的定位和導(dǎo)航信息，引導(dǎo)機(jī)器人快速組裝對(duì)接的基于單目攝像頭和LED的對(duì)接定位和導(dǎo)航方案。本發(fā)明突破了現(xiàn)有常見(jiàn)技術(shù)方案的思維，采用攝像頭觀察RGB的LED的顏色和位置，為定位和導(dǎo)航提供豐富全面的信息，提供了一種更加有效的對(duì)接定位和導(dǎo)航策略。所述的對(duì)接定位和導(dǎo)航策略通過(guò)如下步驟實(shí)現(xiàn)步驟1 機(jī)器人A原地旋轉(zhuǎn)調(diào)整，隨機(jī)移動(dòng)，直到發(fā)現(xiàn)紅色頂部LED ；步驟2 機(jī)器人A根據(jù)觀察到的紅色頂部LED成像位置調(diào)整自身姿態(tài)，使紅色頂部 LED在攝像頭中的成像位于圖像水平方向的中央；步驟3 計(jì)算左右兩個(gè)底部LED成像到頂部LED成像的水平距離；步驟4 根據(jù)觀察到的步驟3中的LED成像距離關(guān)系，機(jī)器人A調(diào)整自身的位姿；所述的LED成像距離關(guān)系是指在攝像頭的成像平面上，底部LED的成像點(diǎn)到頂部 LED的成像點(diǎn)的水平間距的比值ratio，具體的位姿調(diào)整基于ratio的值來(lái)確定如果ratio > 1，機(jī)器人A順時(shí)針轉(zhuǎn)動(dòng)一個(gè)角度，然后向左平移，直到紅色的頂部 LED4的成像再次位于圖像水平方向中央位置，返回步驟3 ；如果ratio < 1，機(jī)器人A逆時(shí)針轉(zhuǎn)動(dòng)一個(gè)角度，然后向右平移，直到紅色的頂部 LED4的成像再次位于圖像水平方向中央位置，返回步驟3 ；如果ratio = 1，機(jī)器人A已經(jīng)與機(jī)器人B的當(dāng)前面實(shí)現(xiàn)對(duì)正，機(jī)器人A通過(guò)觀察當(dāng)前面上三個(gè)LED的顏色組合狀態(tài)，采取下一步動(dòng)作，導(dǎo)航或者對(duì)接，執(zhí)行步驟5 ；步驟5 觀察當(dāng)前面上三個(gè)LED的顏色組合狀態(tài)，根據(jù)觀察到的LED顏色組合狀態(tài)，獲得導(dǎo)航信息；如果得到當(dāng)前面是目標(biāo)對(duì)接面則執(zhí)行步驟7，否則執(zhí)行步驟6 ；步驟6 根據(jù)步驟5中的導(dǎo)航信息，機(jī)器人A導(dǎo)航移動(dòng)的到下一個(gè)對(duì)接面，然后執(zhí)行步驟1 ；步驟7 執(zhí)行對(duì)接過(guò)程。本發(fā)明的對(duì)接定位和導(dǎo)航策略的硬件系統(tǒng)構(gòu)成主要由攝像頭部分和RGB三色一體LED部分構(gòu)成，通過(guò)對(duì)攝像頭中觀察到的LED成像信息提取，來(lái)實(shí)現(xiàn)對(duì)接導(dǎo)航和相對(duì)定位的目的。所述的攝像頭部分主要是由一個(gè)以CMOS的圖像傳感器芯片為核心的攝像頭組成，攝像頭固定安裝在機(jī)器人主動(dòng)對(duì)接面上，采用240X320像素點(diǎn)通過(guò)VGA時(shí)序以RGB565 格式輸出圖像數(shù)據(jù)。所述的LED部分采用的是RGB三色一體的LED，每3個(gè)LED為一組以特定的方式布置在機(jī)器人的所有側(cè)面上，通過(guò)不同的顏色組合來(lái)表示不同的導(dǎo)航信息；通過(guò)特定的布置方式使得同一面上的3個(gè)LED在攝像頭中成像的位置關(guān)系和機(jī)器人之間的相對(duì)位姿有確定的函數(shù)關(guān)系，從而實(shí)現(xiàn)準(zhǔn)確的相對(duì)定位。所述的對(duì)接定位策略是通過(guò)計(jì)算LED在攝像頭中成像的距離關(guān)系，利用同一面上底部?jī)蓚€(gè)LED成像到頂部LED成像的水平距離的比值結(jié)合相關(guān)的幾何關(guān)系解算出兩個(gè)機(jī)器人之間的相對(duì)位姿，并根據(jù)當(dāng)前位姿不斷調(diào)整，直到機(jī)器人之間相互對(duì)正。所述的對(duì)接導(dǎo)航策略是通過(guò)攝像頭觀察當(dāng)前對(duì)接面上的LED成像的顏色信息來(lái)實(shí)現(xiàn)的，當(dāng)前對(duì)接面通過(guò)設(shè)置自身的LED顏色組合引導(dǎo)機(jī)器人盡快導(dǎo)航到目標(biāo)對(duì)接面，實(shí)現(xiàn)對(duì)接。每個(gè)面上LED顏色的組合有33 = 27種，可以充分滿足信息表達(dá)需要。所述的定位和導(dǎo)航策略的硬件系統(tǒng)還包括在機(jī)器人的每個(gè)側(cè)面上都布置有兩個(gè)模擬紅外收發(fā)傳感器，主要在兩個(gè)對(duì)接面相互貼近LED移出攝像頭視角之后，用來(lái)輔助機(jī)器人之間的相對(duì)定位以及障礙的規(guī)避。本發(fā)明的優(yōu)點(diǎn)是(1)對(duì)接機(jī)器人之間的相對(duì)定位主要采用一個(gè)單目攝像頭和LED配合實(shí)現(xiàn)，設(shè)計(jì)簡(jiǎn)單，成本低廉；攝像頭數(shù)據(jù)輸出采用RGB565格式，包含豐富的顏色空間信息，可以根據(jù)需要轉(zhuǎn)換到多種顏色空間中，方便進(jìn)行顏色識(shí)別。(2)采用LED顏色組合表示機(jī)器人的狀態(tài)，每個(gè)面上LED顏色組合有33 = 27種， 能夠?yàn)闄C(jī)器人導(dǎo)航提供豐富的信息。(3)每個(gè)對(duì)接面上的3個(gè)LED采用了一種特殊的布置方案，機(jī)器人不需要反復(fù)的多余移動(dòng)，通過(guò)一次觀察LED在攝像頭中的成像位置關(guān)系即可確定兩個(gè)機(jī)器人之間的相對(duì)位姿及目標(biāo)對(duì)接面所在的相對(duì)方位。(4)本發(fā)明的策略設(shè)計(jì)簡(jiǎn)單，通過(guò)攝像頭和LED的特殊布置及顏色組合即可以實(shí)現(xiàn)相關(guān)的定位和導(dǎo)航，有效提高了機(jī)器人對(duì)接組裝的效率。


圖1是本發(fā)明基于單目攝像頭和LED的機(jī)器人對(duì)接定位和導(dǎo)航策略的機(jī)器人結(jié)構(gòu)示意圖；圖2是主動(dòng)對(duì)接面上攝像頭布置示意圖；圖3是機(jī)器人定位幾何關(guān)系原理分析圖；圖4是基于單目攝像頭和LED的機(jī)器人對(duì)接定位和導(dǎo)航策略方法的流程圖中1.主動(dòng)對(duì)接面；2.被動(dòng)對(duì)接面；3.攝像頭；4.頂部LED ；5.底部LED ；6.模擬紅外收發(fā)傳感器；7.對(duì)接卡槽；8.對(duì)接卡扣；9. CMOS成像傳感器；10.攝像
頭^頭。
具體實(shí)施例方式下面結(jié)合附圖對(duì)本發(fā)明做進(jìn)一步的詳細(xì)說(shuō)明。請(qǐng)參閱圖1 2所示，本發(fā)明中的定位和導(dǎo)航策略通過(guò)CMOS單目攝像頭觀察LED 實(shí)現(xiàn)機(jī)器人之間的定位和導(dǎo)航。圖1展示了兩個(gè)機(jī)器人之間定位和導(dǎo)航的一個(gè)基本策略， 所述的機(jī)器人包括至少兩個(gè)側(cè)面，其中一個(gè)為主動(dòng)對(duì)接面1，其余為被動(dòng)對(duì)接面2，所述的主動(dòng)對(duì)接面1和被動(dòng)對(duì)接面2上分別設(shè)置有一組LED(包括兩個(gè)底部LED5和一個(gè)頂部 LED4)、兩個(gè)模擬紅外收發(fā)傳感器6和一對(duì)對(duì)接卡槽7，所述的主動(dòng)對(duì)接面1上還設(shè)置有一對(duì)對(duì)接卡扣8和CMOS單目攝像頭3 (簡(jiǎn)稱攝像頭)。通過(guò)機(jī)器人A的主動(dòng)對(duì)接面1上的CMOS 單目攝像頭3觀察機(jī)器人B上LED組的顏色，結(jié)合機(jī)器人A自身的主動(dòng)對(duì)接面1上兩個(gè)模擬紅外收發(fā)傳感器6的信息，使得帶有主動(dòng)對(duì)接面1的機(jī)器人A迅速導(dǎo)航定位，與機(jī)器人B 實(shí)現(xiàn)對(duì)接，多個(gè)機(jī)器人的對(duì)接形成群體機(jī)器人。將需要對(duì)接的主動(dòng)對(duì)接面或者被動(dòng)對(duì)接面統(tǒng)稱為目標(biāo)對(duì)接面，目標(biāo)對(duì)接面具有與其余主動(dòng)對(duì)接面或者被動(dòng)對(duì)接面2不同的LED組合信號(hào)。本發(fā)明中，機(jī)器人的對(duì)接是通過(guò)每個(gè)機(jī)器人的主動(dòng)對(duì)接面1尋找目標(biāo)對(duì)接面，并將主動(dòng)對(duì)接面1與目標(biāo)對(duì)接面連接，實(shí)現(xiàn)兩個(gè)機(jī)器人之間連接，其中主動(dòng)對(duì)接面1上的攝像頭3觀察目標(biāo)對(duì)接面上LED顏色和成像位置信息，采取定位和導(dǎo)航?jīng)Q策。實(shí)現(xiàn)定位和導(dǎo)航的過(guò)程對(duì)硬件的要求比較簡(jiǎn)單，每個(gè)機(jī)器人上的相關(guān)的設(shè)備主要包括一個(gè)攝像頭3、兩個(gè)模擬紅外收發(fā)傳感器6、一個(gè)頂部LED 4和兩個(gè)底部LED 5，所述的LED均為RGB三色LED。通過(guò)以上基本的硬件配置，結(jié)合本發(fā)明的策略算法即可以為機(jī)器人自組裝過(guò)程提供有效的對(duì)接定位和導(dǎo)航功能。請(qǐng)參閱圖2 3所示，本發(fā)明的定位和導(dǎo)航策略所使用的攝像頭3和LED的安裝和位置關(guān)系都進(jìn)行了特殊的設(shè)計(jì)，所述的攝像頭3位置如圖2所示，從主動(dòng)對(duì)接面1的正前方觀察，攝像頭3恰好位于頂部LED 4和底部LED 5組成的三角形的幾何中心位置；所述的 LED的位置如圖1 2所示，兩個(gè)底部LED 5以一定的間距布置在機(jī)器人的主動(dòng)對(duì)接面1和被動(dòng)對(duì)接面2上，頂部LED 4布置在靠近機(jī)體內(nèi)側(cè)的位置，高度比兩個(gè)底部LED5高，并且與主動(dòng)對(duì)接面1或被動(dòng)對(duì)接面2不共面；以上布置可以保證攝像頭3在不同的相對(duì)位姿上觀察到的LED成像關(guān)系不同且有明確的函數(shù)對(duì)應(yīng)關(guān)系。請(qǐng)參閱圖3所示，為本發(fā)明的定位原理圖，所述的明確的函數(shù)對(duì)應(yīng)關(guān)系是由機(jī)器人本身的結(jié)構(gòu)參數(shù)和相關(guān)的幾何光學(xué)原理確定的，圖中的角度α，L和f為機(jī)器人本身的安裝和結(jié)構(gòu)參數(shù)，其中，角α為底部LED的俯視圖投影與頂部LED的俯視圖投影形成的等腰三角形的頂角的一半，L為頂部LED的俯視圖投影與底部LED的俯視圖投影之間的距離，兩個(gè)底部LED與頂部LED之間的距離相等，即三個(gè)LED組成等腰三角形，f為攝像頭3的CMOS 成像傳感器9所在平面與攝像頭3上鏡頭10的垂直距離；角度β和距離d為需要對(duì)接的兩個(gè)機(jī)器人(本發(fā)明中為機(jī)器人A和機(jī)器人B)的相對(duì)位姿參數(shù)，其中，β為需要對(duì)接的兩個(gè)機(jī)器人之間的偏角(主動(dòng)對(duì)接面與目標(biāo)對(duì)接面對(duì)正狀態(tài)下此角應(yīng)為0度)，d為主動(dòng)對(duì)接面與目標(biāo)對(duì)接面之間的距離，本發(fā)明中選取的是主動(dòng)對(duì)接面與目標(biāo)對(duì)接面上頂部LED之間的距離；Ll和L2分別為兩個(gè)底部LED5在攝像頭成像平面方向到頂部LED 4的實(shí)際水平間距，Ll'和L2'分別為兩個(gè)底部LED5在攝像頭成像平面上的成像點(diǎn)到頂部LED 4的成像點(diǎn)的水平間距，利用攝像頭圖像，通過(guò)測(cè)量Ll'和L2'并得到Ll'和L2'的比值ratio，就可以計(jì)算出兩個(gè)機(jī)器人之間的相對(duì)位姿關(guān)系參數(shù)——角度β和距離d。根據(jù)圖3中的幾何關(guān)系，有
權(quán)利要求
1.基于單目攝像頭和LED的機(jī)器人對(duì)接定位和導(dǎo)航策略，其特征在于，具體包括如下步驟步驟1 機(jī)器人A原地旋轉(zhuǎn)調(diào)整，隨機(jī)移動(dòng)，直到發(fā)現(xiàn)紅色頂部LED ； 步驟2 機(jī)器人A根據(jù)觀察到的紅色頂部LED成像位置調(diào)整自身姿態(tài)，使紅色頂部LED 在攝像頭中的成像位于圖像水平方向的中央；步驟3 計(jì)算左右兩個(gè)底部LED成像到頂部LED成像的水平距離； 步驟4 根據(jù)觀察到的步驟3中的LED成像距離關(guān)系，機(jī)器人A調(diào)整自身的位姿； 所述的LED成像距離關(guān)系是指在攝像頭的成像平面上，底部LED的成像點(diǎn)到頂部LED 的成像點(diǎn)的水平間距的比值ratio，具體的位姿調(diào)整基于ratio的值來(lái)確定如果ratio > 1，機(jī)器人A順時(shí)針轉(zhuǎn)動(dòng)一個(gè)角度，然后向左平移，直到紅色的頂部LED的成像再次位于圖像水平方向中央位置，返回步驟3 ；如果ratio < 1，機(jī)器人A逆時(shí)針轉(zhuǎn)動(dòng)一個(gè)角度，然后向右平移，直到紅色的頂部LED的成像再次位于圖像水平方向中央位置，返回步驟3 ；如果ratio = 1，機(jī)器人A已經(jīng)與機(jī)器人B的當(dāng)前面實(shí)現(xiàn)對(duì)正，機(jī)器人A通過(guò)觀察當(dāng)前面上三個(gè)LED的顏色組合狀態(tài)，采取下一步動(dòng)作，導(dǎo)航或者對(duì)接，執(zhí)行步驟5 ；步驟5 觀察當(dāng)前面上三個(gè)LED的顏色組合狀態(tài)，根據(jù)觀察到的LED顏色組合狀態(tài)，獲得導(dǎo)航信息；如果得到當(dāng)前面是目標(biāo)對(duì)接面則執(zhí)行步驟7，否則執(zhí)行步驟6 ；步驟6 根據(jù)步驟5中的導(dǎo)航信息，機(jī)器人A導(dǎo)航移動(dòng)的到下一個(gè)對(duì)接面，然后執(zhí)行步驟1 ；步驟7 執(zhí)行對(duì)接過(guò)程。
2.根據(jù)權(quán)利要求1所述的基于單目攝像頭和LED的機(jī)器人對(duì)接定位和導(dǎo)航策略，其特征在于步驟5中所述的三個(gè)LED的顏色組合狀態(tài)，對(duì)應(yīng)的完整的狀態(tài)空間有33 = 27個(gè)組合方式。
3.根據(jù)權(quán)利要求1所述的基于單目攝像頭和LED的機(jī)器人對(duì)接定位和導(dǎo)航策略，其特征在于步驟5中所述的三個(gè)LED的顏色組合狀態(tài)，具體是指如下幾種三個(gè)LED均亮紅色該機(jī)器人B為已對(duì)接機(jī)器人，當(dāng)前面為目標(biāo)對(duì)接面，機(jī)器人A應(yīng)該和該目標(biāo)對(duì)接面進(jìn)行對(duì)接；頂部LED為紅色，底部LED均為綠色該機(jī)器人B為已對(duì)接機(jī)器人，當(dāng)前面不是目標(biāo)對(duì)接面，目標(biāo)對(duì)接面在當(dāng)前面的右側(cè)，機(jī)器人A應(yīng)該向右側(cè)導(dǎo)航移動(dòng)；頂部LED為紅色，底部LED均為藍(lán)色該機(jī)器人B為已對(duì)接機(jī)器人，當(dāng)前面不是目標(biāo)對(duì)接面，目標(biāo)對(duì)接面在當(dāng)前面的左側(cè)，機(jī)器人A應(yīng)該向左側(cè)導(dǎo)航移動(dòng)；三個(gè)LED均亮藍(lán)色該機(jī)器人B為未對(duì)接機(jī)器人，且處于漫游狀態(tài)，還沒(méi)有找到一個(gè)已對(duì)接機(jī)器人；三個(gè)LED均亮綠色該機(jī)器人B為未對(duì)接機(jī)器人，且處于導(dǎo)航狀態(tài)，已經(jīng)找到已對(duì)接機(jī)器人，但是還沒(méi)有找到目標(biāo)對(duì)接面。
4.根據(jù)權(quán)利要求1所述的基于單目攝像頭和LED的機(jī)器人對(duì)接定位和導(dǎo)航策略，其特征在于步驟7中所述的對(duì)接過(guò)程開(kāi)始于機(jī)器人A與目標(biāo)對(duì)接面對(duì)正之后，機(jī)器人A繼續(xù)前進(jìn)并不斷調(diào)整保持對(duì)正狀態(tài)，當(dāng)機(jī)器人A主動(dòng)對(duì)接面和機(jī)器人B的目標(biāo)對(duì)接面接近時(shí)，LED 會(huì)移出攝像頭的視角，此時(shí)機(jī)器人A利用自身模擬紅外收發(fā)傳感器測(cè)距來(lái)保證最后一段距離的對(duì)正狀態(tài)，直到主動(dòng)對(duì)接面上的接觸開(kāi)關(guān)碰到目標(biāo)對(duì)接面，實(shí)現(xiàn)對(duì)接。
5.根據(jù)權(quán)利要求1所述的基于單目攝像頭和LED的機(jī)器人對(duì)接定位和導(dǎo)航策略，其特征在于所述的機(jī)器人包括至少兩個(gè)側(cè)面，其中一個(gè)為主動(dòng)對(duì)接面，其余為被動(dòng)對(duì)接面，所述的主動(dòng)對(duì)接面和被動(dòng)對(duì)接面上分別設(shè)置有一組LED、兩個(gè)模擬紅外收發(fā)傳感器和一對(duì)對(duì)接卡槽，所述的主動(dòng)對(duì)接面上還設(shè)置有一對(duì)對(duì)接卡扣和攝像頭。
6.根據(jù)權(quán)利要求5所述的基于單目攝像頭和LED的機(jī)器人對(duì)接定位和導(dǎo)航策略，其特征在于所述的LED均為RGB三色LED。
7.根據(jù)權(quán)利要求5所述的基于單目攝像頭和LED的機(jī)器人對(duì)接定位和導(dǎo)航策略，其特征在于所述的攝像頭位置，從主動(dòng)對(duì)接面的正前方觀察，攝像頭恰好位于頂部LED和底部 LED組成的三角形的幾何中心位置；所述的LED的位置，兩個(gè)底部LED布置在機(jī)器人的主動(dòng)對(duì)接面和被動(dòng)對(duì)接面上，頂部LED布置在靠近機(jī)體內(nèi)側(cè)的位置，高度比兩個(gè)底部LED高，并且與主動(dòng)對(duì)接面或被動(dòng)對(duì)接面不共面。
全文摘要
本發(fā)明公開(kāi)了一種基于單目攝像頭和LED的機(jī)器人對(duì)接定位和導(dǎo)航策略，主要通過(guò)設(shè)置在機(jī)器人上的CMOS單目攝像頭和3個(gè)為1組的RGB三色一體的LED實(shí)現(xiàn)。攝像頭固定安裝在機(jī)器人的主動(dòng)對(duì)接面上，用以觀察目標(biāo)機(jī)器人LED成像的位置和顏色關(guān)系，LED在機(jī)器人的所有主動(dòng)對(duì)接面和被動(dòng)對(duì)接面上都布置1組。通過(guò)LED在攝像頭中成像的尺寸關(guān)系計(jì)算出攝像頭所在的主動(dòng)對(duì)接面和LED所在的對(duì)接面之間的相對(duì)位姿關(guān)系進(jìn)行相對(duì)定位，通過(guò)攝像頭辨別當(dāng)前面上的LED不同的顏色組合來(lái)識(shí)別當(dāng)前面的狀態(tài)進(jìn)行導(dǎo)航。本發(fā)明的策略通過(guò)LED的成像狀態(tài)能夠提供豐富的參考信息，實(shí)現(xiàn)機(jī)器人對(duì)接過(guò)程中的相對(duì)定位、目標(biāo)尋找和快速導(dǎo)航，有效提高了群體機(jī)器人對(duì)接的效率。
文檔編號(hào)G01C21/00GK102436261SQ20111039989
公開(kāi)日2012年5月2日 申請(qǐng)日期2011年12月5日 優(yōu)先權(quán)日2011年12月5日
發(fā)明者李寧, 鄭隨兵, 陳友東, 魏洪興 申請(qǐng)人:北京航空航天大學(xué)