專利名稱：基于Zigbee通信的多機(jī)器人隊(duì)形控制系統(tǒng)及方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種多機(jī)器人隊(duì)形控制技術(shù)，尤其涉及一種基于Zigbee通信的多機(jī)器人隊(duì)形控制系統(tǒng)及方法。
背景技術(shù)：
隨著機(jī)器人技術(shù)的發(fā)展，機(jī)器人的功能越來越多，應(yīng)用也越來越廣泛。同時(shí)，依賴機(jī)器人所需要完成的任務(wù)越來越精密和復(fù)雜，單個(gè)機(jī)器人逐漸難以完成人們制定的目標(biāo)， 此時(shí)，多個(gè)機(jī)器人組成機(jī)器人系統(tǒng)，通過協(xié)調(diào)和合作來完成任務(wù)，逐漸成為機(jī)器人領(lǐng)域發(fā)展的重要趨勢。同時(shí)，很多事例表明，使用多個(gè)機(jī)器人能夠比增加單個(gè)機(jī)器人的能力和復(fù)雜度更加節(jié)省成本，也更能出色地完成任務(wù)。
多機(jī)器人的隊(duì)形控制問題是近來控制領(lǐng)域的熱點(diǎn)問題，其主要任務(wù)是使多機(jī)器人的隊(duì)形控制更加的準(zhǔn)確。所謂的多機(jī)器人的隊(duì)形控制是指多個(gè)機(jī)器人在達(dá)到目標(biāo)的過程中，保持預(yù)設(shè)定的某種隊(duì)形，同時(shí)又要克服周圍環(huán)境的干擾和影響(例如存在障礙物或者空間的物理限制)的一種控制技術(shù)。多機(jī)器人保持定的隊(duì)形有著以下的好處一是能夠充分的獲取周圍的環(huán)境信息；二是能夠保持系統(tǒng)的穩(wěn)定性和安全性；三是能夠提高整體的工作效率；四是能夠提高系統(tǒng)的魯棒性。
當(dāng)前，多機(jī)器人的隊(duì)形控制方法有所發(fā)展，但仍然存在很多不足，如方法實(shí)現(xiàn)的成本過高，可擴(kuò)展性不強(qiáng)，穩(wěn)定性較差。這些劣勢也抑制了多機(jī)器人的隊(duì)形控制在實(shí)際工業(yè)應(yīng)用領(lǐng)域的發(fā)展。發(fā)明內(nèi)容
本發(fā)明的目的在于針對現(xiàn)有技術(shù)的不足，提供一種基于Zigbee通信的多機(jī)器人隊(duì)形控制系統(tǒng)及方法。
本發(fā)明解決其技術(shù)問題采用的技術(shù)方案是一種基于Zigbee通信的多機(jī)器人隊(duì)形控制系統(tǒng)，該系統(tǒng)主要由攝像頭、通信模塊、上位機(jī)和多個(gè)機(jī)器人組成；所述攝像頭通過網(wǎng)線與上位機(jī)相連，所有機(jī)器人通過通信模塊與上位機(jī)進(jìn)行Zigbee通信。
—種應(yīng)用上述系統(tǒng)的基于Zigbee通信的多機(jī)器人隊(duì)形控制方法,該方法包括以下步驟
(I)攝像頭采集多個(gè)機(jī)器人運(yùn)動的圖像并傳送至上位機(jī)；
(2)上位機(jī)對攝像頭傳送來的圖像進(jìn)行處理，得到各個(gè)機(jī)器人的位置和方向；
(3)上位機(jī)根據(jù)步驟2得到的各個(gè)機(jī)器人的位置(Xi，Yi)和方向Qi得到每個(gè)機(jī)器人下一采樣周期內(nèi)的兩輪的轉(zhuǎn)速(《u，ωΜ)，其中《匸為機(jī)器人左輪的轉(zhuǎn)速，《Ki為機(jī)器人右輪的轉(zhuǎn)速；
(4)將步驟3中求出的機(jī)器人兩輪的轉(zhuǎn)速(ω& ωΕ )寫入通信數(shù)據(jù)包，并通過通信模塊發(fā)送至各個(gè)機(jī)器人；
(5)每個(gè)機(jī)器人收到上位機(jī)發(fā)送的通信數(shù)據(jù)包之后即解析數(shù)據(jù)包，并根據(jù)數(shù)據(jù)包4內(nèi)容進(jìn)行運(yùn)動，從而實(shí)現(xiàn)多機(jī)器人的隊(duì)形控制。
相對于現(xiàn)有技術(shù)，本發(fā)明具有的有益效果是本發(fā)明為一種基于Zigbee通信的多機(jī)器人隊(duì)形控制系統(tǒng)及方法，能為多機(jī)器人控制與Zigbee通信研究者提供一個(gè)方便實(shí)用的實(shí)現(xiàn)方法，來驗(yàn)證理論研究算法的可行性，并可用于改進(jìn)相關(guān)的理論研究。同時(shí)，可以檢驗(yàn)通信存在丟包或者時(shí)延情況下的控制性能，為控制算法的改進(jìn)提供數(shù)據(jù)支持。本發(fā)明中上位機(jī)與各個(gè)機(jī)器人之間的通信基于Zigbee通信，Zigbee通信具有近距離、自組織、低功耗、低復(fù)雜度、低成本等特點(diǎn)。本發(fā)明成本低，精度高，穩(wěn)定性好，適用于各種多機(jī)器人隊(duì)形的研究領(lǐng)域，具有良好的發(fā)展?jié)摿Α?br>

圖I是本發(fā)明的整體結(jié)構(gòu)框圖2是本發(fā)明的工作流程圖3是通信數(shù)據(jù)包的格式。
具體實(shí)施方式
下面結(jié)合附圖詳細(xì)描述本發(fā)明，本發(fā)明的目的和效果將變得更加明顯。
如圖I所示，描述了本發(fā)明的整體結(jié)構(gòu)組成。
實(shí)現(xiàn)多機(jī)器人隊(duì)形控制方法的系統(tǒng)主要由攝像頭、通信模塊、上位機(jī)和多個(gè)機(jī)器人組成。攝像頭通過網(wǎng)線與上位機(jī)相連，所有機(jī)器人通過通信模塊與上位機(jī)進(jìn)行Zigbee通 目。
攝像頭每隔一個(gè)固定的采樣周期即采集一幀多個(gè)機(jī)器人運(yùn)動的圖像，并通過網(wǎng)線上傳至上位機(jī)，上位機(jī)調(diào)用OpenCV的函數(shù)對圖像進(jìn)行處理，即可得到各個(gè)機(jī)器人的位置和方向，然后運(yùn)行控制算法，得到每個(gè)機(jī)器人下下一采樣周期內(nèi)的運(yùn)動速度，寫入數(shù)據(jù)包并通過Zigbee通信發(fā)送至各個(gè)機(jī)器人，機(jī)器人收到相應(yīng)的數(shù)據(jù)包之后即將運(yùn)動速度從數(shù)據(jù)包中解析出來，并執(zhí)行。
本發(fā)明可以使用德國Basler公司的pia2400_17gc攝像頭。該攝像頭具有500萬象素的分辨率和千兆網(wǎng)數(shù)據(jù)接口，用支架將攝像頭固定在實(shí)驗(yàn)平臺的上方，保證攝像頭的視野能完全覆蓋機(jī)器人的運(yùn)動區(qū)域。將攝像頭與電源相連，并用網(wǎng)線將攝像頭與上位機(jī)連接，用于攝像頭采集的圖像傳送。
本發(fā)明所使用的機(jī)器人可以為瑞士聯(lián)邦理工大學(xué)微處理器和接口實(shí)驗(yàn)室的研究團(tuán)隊(duì)開發(fā)的Khepera II雙輪移動機(jī)器人，主要部件包括兩個(gè)電機(jī)，內(nèi)置可充電電池，控制器，用于通信擴(kuò)展的串口。Khepera II雙輪移動機(jī)器人最初設(shè)計(jì)作為研究和教學(xué)工具結(jié)構(gòu)提供給瑞士研究程序。它允許對現(xiàn)實(shí)世界中的軌跡執(zhí)行、避障、傳感信息處理、對行為的處理假設(shè)模擬的算法改進(jìn)開發(fā)。Khepera II雙輪移動機(jī)器人作為機(jī)器人實(shí)驗(yàn)和應(yīng)用平臺現(xiàn)在被廣泛應(yīng)用于世界各地。Khepera II雙輪移動機(jī)器人擁有擴(kuò)展功能，完全符合原來的設(shè)計(jì)目標(biāo)，可以使用Khepera II雙輪移動機(jī)器人的任何附件。這是一個(gè)簡單易用，功能強(qiáng)勁的機(jī)器人應(yīng)用的標(biāo)準(zhǔn)平臺。使用前，先用給定的充電器給Khepera II雙輪移動機(jī)器人的內(nèi)置電池充電，當(dāng)充電器上的指示燈由紅色變?yōu)榫G色時(shí)，即表示電池已經(jīng)充滿電，然后打開機(jī)器人側(cè)面的電源開關(guān)，此時(shí)機(jī)器人即由內(nèi)部電池供電。并選擇機(jī)器人的運(yùn)行模式1，即串口設(shè)置為9600波特率，共8位，I個(gè)起始位，2個(gè)停止位，無校驗(yàn)位。
本發(fā)明中的通信模塊采用的是TI公司的eZ430_RF2500套件。eZ430-RF2500套件結(jié)合了超低功耗MSP430微控制器與無線通信功能，從而進(jìn)一步簡化了低功耗無線系統(tǒng)的開發(fā)工作。利用eZ430-RF2500套件可以很方便地實(shí)現(xiàn)Zigbee通信。eZ430_RF2500套件通過串口與Kh印era II雙輪移動機(jī)器人相連，并由Kh印era II的內(nèi)置電池供電。
本發(fā)明中的上位機(jī)可以由普通的臺式電腦或筆記本電腦來實(shí)現(xiàn)。用網(wǎng)線將攝像頭與上位機(jī)相連，并在上位機(jī)上安裝攝像頭的驅(qū)動程序。上位機(jī)的USB端口插有一個(gè)通信模塊，用于上位機(jī)與各個(gè)機(jī)器人之間進(jìn)行Zigbee通信。上位機(jī)的功能是接收攝像頭傳送來的圖像，對圖像進(jìn)行處理，執(zhí)行控制算法，發(fā)送數(shù)據(jù)包給各個(gè)機(jī)器人，并顯示機(jī)器人的運(yùn)動圖像與各種運(yùn)動曲線。
本發(fā)明基于Zigbee通信的多機(jī)器人隊(duì)形控制方法，包括以下步驟
I、攝像頭采集多個(gè)機(jī)器人運(yùn)動的圖像并傳送至上位機(jī)
設(shè)定攝像頭的采樣周期為O. 5s，攝像頭每隔一個(gè)采樣周期即拍攝一幀多個(gè)機(jī)器人運(yùn)動的圖像，保存為bmp格式，并通過網(wǎng)線傳送至上位機(jī)。
2、上位機(jī)對攝像頭傳送來的圖像進(jìn)行處理，得到各個(gè)機(jī)器人的位置和方向
上位機(jī)的攝像頭圖像處理部分用于處理攝像頭采集的圖像從而得出各個(gè)機(jī)器人的位置和方向，該部分程序采用C++語言編寫，調(diào)用了部分?jǐn)z像頭廠商提供的API函數(shù)。在對圖像處理之前，首先要完成攝像頭的畸變校正。調(diào)用OpenCV提供的一個(gè)可以直接使用的校正算法，即輸入攝像頭傳送來的圖像由cvCalibrateCamera2 O函數(shù)得到畸變系數(shù)并生成校正后的圖像。
對攝像頭傳送來的圖像進(jìn)行處理的具體實(shí)現(xiàn)如下首先將圖像旋轉(zhuǎn)，然后再將圖像的原點(diǎn)定位到實(shí)際環(huán)境的原點(diǎn)中，再創(chuàng)建一個(gè)灰度圖空間，調(diào)用cvCvtColorO函數(shù)將采集的RGB圖像轉(zhuǎn)換為灰度圖，再調(diào)用cvSmoothO函數(shù)對灰度圖進(jìn)行高斯平滑，然后用 cvHoughCircles O函數(shù)檢測圓形,遍歷所有的圓后,識別出所有圓的圓心坐標(biāo)和半徑大小， 然后根據(jù)圓的半徑大小，判斷對應(yīng)哪個(gè)機(jī)器人，同時(shí)得到的圓心坐標(biāo)即為各個(gè)機(jī)器人的位置坐標(biāo)(XiJi), i = 1，2，...，N，N為機(jī)器人的個(gè)數(shù)。又由于采樣周期很短，將機(jī)器人上一采樣時(shí)刻位置與當(dāng)前位置連線的方向作為機(jī)器人當(dāng)前的運(yùn)動方向Θ i，i = 1，2，. . .，N。
3、上位機(jī)根據(jù)步驟2得到的各個(gè)機(jī)器人的位置(Xi，Yi)和方向Qi得到每個(gè)機(jī)器人下一采樣周期內(nèi)的兩輪的轉(zhuǎn)速(《u，ωΜ)，其中《匸為機(jī)器人左輪的轉(zhuǎn)速，《Ki為機(jī)器人右輪的轉(zhuǎn)速
根據(jù)機(jī)器人需要跟蹤的參考軌跡和機(jī)器人隊(duì)形之間的幾何關(guān)系，可以計(jì)算出各個(gè)機(jī)器人在采樣時(shí)刻的參考位置(xdi，ydi)和參考方向Qdi，以及參考速度Vdi和參考角速度ω di °
然后根據(jù)公式求出各個(gè)機(jī)器人的跟蹤誤差( , yei, Θ J，其中Xei為機(jī)器人的參考位置與實(shí)際位置之差在機(jī)器人運(yùn)動方向上的投影值，yei為機(jī)器人的參考位置與實(shí)際位置之差在垂直于機(jī)器人運(yùn)動方向上的投影值，θ 機(jī)器人的參考方向與實(shí)際方向之差，計(jì)算公式如下
權(quán)利要求
1.一種基于Zigbee通信的多機(jī)器人隊(duì)形控制系統(tǒng)，其特征在于，該系統(tǒng)主要由攝像頭、通信模塊、上位機(jī)和多個(gè)機(jī)器人組成；所述攝像頭通過網(wǎng)線與上位機(jī)相連，所有機(jī)器人通過通信模塊與上位機(jī)進(jìn)行Zigbee通信。
2.一種應(yīng)用權(quán)利要求I所述系統(tǒng)的基于Zigbee通信的多機(jī)器人隊(duì)形控制方法，其特征在于，該方法包括以下步驟(1)攝像頭采集多個(gè)機(jī)器人運(yùn)動的圖像并傳送至上位機(jī)；(2)上位機(jī)對攝像頭傳送來的圖像進(jìn)行處理，得到各個(gè)機(jī)器人的位置和方向；(3)上位機(jī)根據(jù)步驟2得到的各個(gè)機(jī)器人的位置(Xi，Yi)和方向Qi得到每個(gè)機(jī)器人下一采樣周期內(nèi)的兩輪的轉(zhuǎn)速(《u，ωΜ)，其中03匸為機(jī)器人左輪的轉(zhuǎn)速，為機(jī)器人右輪的轉(zhuǎn)速；(4)將步驟3中求出的機(jī)器人兩輪的轉(zhuǎn)速(cou，ωΕ )寫入通信數(shù)據(jù)包，并通過通信模塊發(fā)送至各個(gè)機(jī)器人；(5)每個(gè)機(jī)器人收到上位機(jī)發(fā)送的通信數(shù)據(jù)包之后即解析數(shù)據(jù)包，并根據(jù)數(shù)據(jù)包內(nèi)容進(jìn)行運(yùn)動，從而實(shí)現(xiàn)多機(jī)器人的隊(duì)形控制。
3.根據(jù)權(quán)利要求2所述的方法，其特征在于，所述步驟(I)具體為設(shè)定攝像頭的采樣周期為O. 5s，攝像頭每隔一個(gè)采樣周期即拍攝一幀多個(gè)機(jī)器人運(yùn)動的圖像，保存為bmp格式，并通過網(wǎng)線傳送至上位機(jī)。
4.根據(jù)權(quán)利要求2所述的方法，其特征在于，所述步驟(2)具體為首先將圖像旋轉(zhuǎn)，然后再將圖像的原點(diǎn)定位到實(shí)際環(huán)境的原點(diǎn)中，再創(chuàng)建一個(gè)灰度圖空間，調(diào)用cvCvtColorO 函數(shù)將采集的RGB圖像轉(zhuǎn)換為灰度圖，再調(diào)用cvSmoothO函數(shù)對灰度圖進(jìn)行高斯平滑，然后用cvHoughCircles O函數(shù)檢測圓形,遍歷所有的圓后,識別出所有圓的圓心坐標(biāo)和半徑大小，然后根據(jù)圓的半徑大小，判斷對應(yīng)哪個(gè)機(jī)器人，同時(shí)得到的圓心坐標(biāo)即為各個(gè)機(jī)器人的位置坐標(biāo)(Xi，Yi), i = I, 2,..., N, N為機(jī)器人的個(gè)數(shù)；將機(jī)器人上一采樣時(shí)刻位置與當(dāng)前位置連線的方向作為機(jī)器人當(dāng)前的運(yùn)動方向Θ P i = 1，2，. . .，N。
5.根據(jù)權(quán)利要求2所述的方法，其特征在于，所述步驟(3)具體為根據(jù)機(jī)器人需要跟蹤的參考軌跡和機(jī)器人隊(duì)形之間的幾何關(guān)系，可以計(jì)算出各個(gè)機(jī)器人在采樣時(shí)刻的參考位置(xdi，ydi)和參考方向0 di,以及參考速度vdi和參考角速度codi ；然后根據(jù)公式求出各個(gè)機(jī)器人的跟蹤誤差(xei，yei, Θ J，其中xei為機(jī)器人的參考位置與實(shí)際位置之差在機(jī)器人運(yùn)動方向上的投影值，yei為機(jī)器人的參考位置與實(shí)際位置之差在垂直于機(jī)器人運(yùn)動方向上的投影值，θ 機(jī)器人的參考方向與實(shí)際方向之差，計(jì)算公式如下
6.根據(jù)權(quán)利要求2所述的方法，其特征在于，所述步驟(5)具體為每個(gè)機(jī)器人上所插的通信模塊都會接收到上位機(jī)發(fā)送的通信數(shù)據(jù)包，然后將數(shù)據(jù)包中第一個(gè)字節(jié)中的ID解析出來，進(jìn)行判斷如果數(shù)據(jù)包中的ID和機(jī)器人自身的ID相同，則此時(shí)通信模塊上的綠色 LED燈亮，表示數(shù)據(jù)包是上位機(jī)發(fā)給該機(jī)器人的，然后繼續(xù)對該數(shù)據(jù)包按照定義的格式進(jìn)行解析，得到相應(yīng)的機(jī)器人兩輪的轉(zhuǎn)速ωΜ)，并將(cou，ωΕ )通過串口傳送給該機(jī)器人的控制器，控制該機(jī)器人的電機(jī)在下一個(gè)采樣周期內(nèi)按照轉(zhuǎn)速(《u，ωΕ )運(yùn)動；如果數(shù)據(jù)包中的ID和機(jī)器人自身的ID不相同，則此時(shí)通信模塊上的紅色LED燈亮，表示數(shù)據(jù)包不是上位機(jī)發(fā)給該機(jī)器人的，然后將該數(shù)據(jù)包丟棄，機(jī)器人的運(yùn)動速度保持不變。全文摘要
本發(fā)明涉及一種基于Zigbee通信的多機(jī)器人隊(duì)形控制系統(tǒng)及方法；攝像頭以一定的采樣頻率采集多個(gè)機(jī)器人的運(yùn)動圖像并傳送至上位機(jī)，在上位機(jī)上對圖像進(jìn)行處理，從而得到各個(gè)機(jī)器人的位置和方向；上位機(jī)計(jì)算出各個(gè)機(jī)器人下一個(gè)采樣周期內(nèi)的運(yùn)動速度，最后上位機(jī)通過通信模塊將機(jī)器人的運(yùn)動速度寫入數(shù)據(jù)包并發(fā)送給機(jī)器人；機(jī)器人通過通信模塊收到數(shù)據(jù)包后解析出運(yùn)動速度并執(zhí)行；如此反復(fù)，直至達(dá)到相應(yīng)的隊(duì)形控制目標(biāo)；本發(fā)明能實(shí)現(xiàn)多機(jī)器人的隊(duì)形控制，驗(yàn)證各種控制算法，并分析不同的通信條件對多機(jī)器人隊(duì)形控制的影響，為多機(jī)器人隊(duì)形控制的研究提供一種經(jīng)濟(jì)高效的方法。
文檔編號G05D1/02GK102981504SQ20121047914
公開日2013年3月20日 申請日期2012年11月21日 優(yōu)先權(quán)日2012年11月21日
發(fā)明者陳積明, 倪利平, 曹向輝, 孫優(yōu)賢 申請人:浙江大學(xué)