息和路徑文件的數(shù)據(jù)庫(kù)；對(duì)機(jī)器人編隊(duì)信 息、路徑信息進(jìn)行輸入，并顯示機(jī)器人移動(dòng)控制信息的人機(jī)交互界面；對(duì)機(jī)器人行進(jìn)中的位 姿信息進(jìn)行計(jì)算得到位姿誤差的位姿誤差算法模塊；向機(jī)器人發(fā)送控制信息的無(wú)線傳感網(wǎng) 絡(luò)Sink節(jié)點(diǎn)。
[0059] 群機(jī)器人單元，由多個(gè)單體機(jī)器人組成，每個(gè)單體機(jī)器人接收并解析來(lái)自所述上 位機(jī)單元的控制信息，并調(diào)整行走策略，完成路徑軌跡的跟隨控制及群體調(diào)度任務(wù)。
[0060] 群機(jī)器人單元中的單體機(jī)器人包括：為機(jī)器人提供動(dòng)力的供電模塊；解析控制信 息并控制機(jī)器人行進(jìn)的主控模塊，該主控模塊采用AVRAtmega32芯片；與上位機(jī)單元進(jìn)行 通信的無(wú)線通訊模塊，該無(wú)線通訊模塊采用Zigbee的通訊方式。
[0061] 如圖2所示，本發(fā)明實(shí)施例的基于視覺(jué)定位的群機(jī)器人控制方法用于實(shí)現(xiàn)本發(fā)明 實(shí)施例的基于視覺(jué)定位的群機(jī)器人控制系統(tǒng)，包括以下步驟：
[0062] S1、視頻采集定位單元采集群機(jī)器人單元的運(yùn)動(dòng)范圍內(nèi)的圖像，根據(jù)該運(yùn)動(dòng)范圍 建立坐標(biāo)系，識(shí)別群機(jī)器人單元中每個(gè)機(jī)器人的位姿信息，包括機(jī)器人的坐標(biāo)信息和姿態(tài) 角信息，并通過(guò)網(wǎng)絡(luò)接口將該位姿信息發(fā)送給上位機(jī)單元；
[0063] S11、對(duì)視頻采集定位模塊覆蓋的區(qū)域進(jìn)行坐標(biāo)標(biāo)定，通過(guò)灰度檢測(cè)算法對(duì)機(jī)器人 的標(biāo)簽進(jìn)行識(shí)別，以此確定每個(gè)機(jī)器人的坐標(biāo)信息；
[0064] S12、對(duì)機(jī)器人的輪廓進(jìn)行標(biāo)定，通過(guò)ICP匹配算法對(duì)移動(dòng)的機(jī)器人目標(biāo)進(jìn)行匹 配，通過(guò)動(dòng)態(tài)匹配輪廓算法計(jì)算機(jī)器人的姿態(tài)角信息；
[0065] S2、上位機(jī)單元根據(jù)位姿誤差算法生成控制信息，具體步驟為：
[0066] S21、上位機(jī)單元讀取路徑文件，按照機(jī)器人編號(hào)將每個(gè)機(jī)器人的路徑軌跡存入鏈 表數(shù)據(jù)結(jié)構(gòu)中，該路徑文件包括機(jī)器人的路徑軌跡、目標(biāo)點(diǎn)坐標(biāo)和路徑階段信息；
[0067]S22、根據(jù)每個(gè)機(jī)器人當(dāng)前的位姿信息和S21中的路徑軌跡，實(shí)時(shí)計(jì)算機(jī)器人姿態(tài) 角與路徑軌跡方向的偏差，記為姿態(tài)誤差；并實(shí)時(shí)計(jì)算當(dāng)前坐標(biāo)信息與目標(biāo)點(diǎn)坐標(biāo)的距離， 記作位置誤差；
[0068] S23、將姿態(tài)誤差和位置誤差作為模糊控制算法的輸入，生成每個(gè)機(jī)器人運(yùn)動(dòng)狀態(tài) 調(diào)整的控制信息；對(duì)機(jī)器人的模糊控制方法具體為：
[0069] 控制機(jī)器人兩輪速度差進(jìn)行控制，在調(diào)整大角度誤差時(shí)采取大速率調(diào)整，在調(diào)整 小角度誤差時(shí)采取小速率調(diào)整；
[0070] 角度誤差調(diào)整進(jìn)入緩沖區(qū)域時(shí)，進(jìn)一步的減小調(diào)整速率，對(duì)速率較大的車(chē)輪施加 輕微剎車(chē)；
[0071] 角度誤差調(diào)整接近零時(shí)，將兩輪的速率調(diào)整至同一數(shù)值；
[0072] 對(duì)機(jī)器人運(yùn)動(dòng)狀態(tài)進(jìn)行控制的控制方程為：
[0073]
[0074]其中，[xp(k+l)yp(k+l)Θp(k+l)]為反饋調(diào)整狀態(tài)變化，[xp(k)yp(k)Θp(k)] 為調(diào)整前移動(dòng)機(jī)器人位姿狀態(tài)，[vp(k) wp(k)]為移動(dòng)機(jī)器人的速度變化向量，ΔΤ為時(shí)間 間隔。
[0075]S24、判斷每個(gè)機(jī)器人是否完成當(dāng)前路徑階段，若未完成，則持續(xù)進(jìn)行反饋修正；若 已完成，則進(jìn)入下一路徑階段；
[0076]S3、對(duì)群機(jī)器人中的所有編號(hào)機(jī)器人的參數(shù)設(shè)置和目標(biāo)點(diǎn)坐標(biāo)進(jìn)行遍歷引導(dǎo)，完 成對(duì)群機(jī)器人的控制協(xié)作。
[0077] 在本發(fā)明的另一個(gè)實(shí)施例中，如圖3所示，攝像頭首先采集實(shí)時(shí)視頻信息，通過(guò)識(shí) 別個(gè)體機(jī)器人標(biāo)簽進(jìn)行位姿定位，上位機(jī)獲取個(gè)體機(jī)器人實(shí)時(shí)位姿信息以及個(gè)體機(jī)器人的 路徑編隊(duì)信息后，根據(jù)位姿誤差算法生成決策指令。通訊模塊使多個(gè)機(jī)器人組成無(wú)線傳感 網(wǎng)絡(luò)，接收上位機(jī)點(diǎn)對(duì)點(diǎn)傳輸?shù)闹噶钚畔ⅲㄟ^(guò)微控制器解析實(shí)現(xiàn)個(gè)體機(jī)器人實(shí)時(shí)控制。系 統(tǒng)采用集中式計(jì)算布局，實(shí)現(xiàn)單個(gè)機(jī)器人的啟動(dòng)、停止、加減速以及路徑跟隨可靠性較好， 在執(zhí)行多個(gè)機(jī)器人路徑規(guī)劃控制精確度高。
[0078] 進(jìn)一步的，視頻識(shí)別定位模塊可采用若干高清攝像頭覆蓋群機(jī)器人運(yùn)動(dòng)范圍，通 過(guò)攝像頭標(biāo)定機(jī)器人運(yùn)動(dòng)范圍坐標(biāo)系。攝像頭模塊通過(guò)識(shí)別機(jī)器人標(biāo)簽確定其位姿參數(shù)， 具體通過(guò)攝像頭感知標(biāo)簽及環(huán)境灰度值差異以及個(gè)體機(jī)器人本身輪廓進(jìn)行識(shí)別定位，得到 相應(yīng)移動(dòng)機(jī)器人的位姿信息并通過(guò)網(wǎng)口輸入控制計(jì)算機(jī)數(shù)據(jù)庫(kù)中。
[0079] 如圖4所示，視頻模塊感知個(gè)體機(jī)器人位姿具體流程為：對(duì)攝像頭覆蓋的區(qū)域進(jìn) 行坐標(biāo)標(biāo)定，以確定實(shí)際區(qū)域與攝像頭圖像映射關(guān)系，處理視頻數(shù)據(jù)流，通過(guò)灰度檢測(cè)算法 對(duì)機(jī)器人個(gè)體標(biāo)簽進(jìn)行識(shí)別以此確定單個(gè)機(jī)器人的坐標(biāo)位置。識(shí)別個(gè)體機(jī)器人姿態(tài)角需要 對(duì)個(gè)體機(jī)器人輪廓進(jìn)行標(biāo)定，并通過(guò)ICP匹配算法對(duì)移動(dòng)的機(jī)器人目標(biāo)進(jìn)行匹配，通過(guò)動(dòng) 態(tài)匹配輪廓計(jì)算個(gè)體機(jī)器人姿態(tài)角，最終將位姿信息x、y、Θ寫(xiě)入上位機(jī)數(shù)據(jù)庫(kù)中。
[0080] 上位機(jī)用于讀取群機(jī)器人路徑文件，并按照個(gè)體機(jī)器人編號(hào)將分解路徑存入鏈表 數(shù)據(jù)結(jié)構(gòu)，通過(guò)攝像頭模塊數(shù)據(jù)庫(kù)接口獲取相應(yīng)機(jī)器人的位姿信息。依據(jù)控制計(jì)算機(jī)位姿 誤差算法，實(shí)時(shí)姿態(tài)與行進(jìn)方位角偏差為姿態(tài)誤差；目標(biāo)點(diǎn)與當(dāng)前坐標(biāo)距離為位置誤差。依 據(jù)移動(dòng)機(jī)器人模糊控制策略，采取不同角度偏差調(diào)整策略，主要通過(guò)控制移動(dòng)機(jī)器人兩輪 速度差進(jìn)行控制，為了避免角度偏差過(guò)大，在調(diào)整大角度誤差時(shí)應(yīng)米取大速率調(diào)整；當(dāng)角度 偏差減小到一個(gè)較小數(shù)值時(shí)，在調(diào)整小角度誤差時(shí)應(yīng)采取小速率調(diào)整；角度偏差調(diào)整進(jìn)入 緩沖區(qū)域時(shí)，調(diào)整速率應(yīng)當(dāng)進(jìn)一步減小，對(duì)差速中較大的驅(qū)動(dòng)輪施加輕微剎車(chē)；角度偏差幾 乎要調(diào)整到極小值時(shí)，此時(shí)應(yīng)當(dāng)調(diào)整兩輪速度至同一值，依靠慣性進(jìn)行調(diào)整。同時(shí)在機(jī)器人 逼近目標(biāo)點(diǎn)過(guò)程中適當(dāng)減速。其串口與無(wú)線傳感網(wǎng)絡(luò)Sink節(jié)點(diǎn)連接，以點(diǎn)對(duì)點(diǎn)的傳輸方式 將指令發(fā)送至個(gè)體移動(dòng)機(jī)器人。
[0081] 如圖5所示，移動(dòng)機(jī)器人包括外殼、供電模塊、主控模塊、無(wú)線通訊模塊、驅(qū)動(dòng)模塊 以及LED指示燈。供電模塊由一組可重復(fù)充電鋰電池組以及穩(wěn)壓電路構(gòu)成。由個(gè)體移動(dòng)機(jī) 器人主控芯片采用AVRAtmega32芯片。ATmega32擁有先進(jìn)的RISC架構(gòu)，可執(zhí)行多達(dá)131條 工作指令，提供8路10位ADC，四通道PWM與32個(gè)可編程I/O接口，在保證低功耗的同時(shí)， 具有強(qiáng)大的運(yùn)算性能，在執(zhí)行行進(jìn)策略過(guò)程中，主控器控制I/O口高低電平輸出控制LED指 示燈點(diǎn)亮或者媳滅，表明此時(shí)個(gè)體移動(dòng)機(jī)器人的行進(jìn)狀態(tài)。
[0082] 如圖6所示，本發(fā)明實(shí)施例的無(wú)線通信模塊采用Zigbee通信方式，所選型號(hào)為 DRF1607!LZigBee技術(shù)是一種近距離、低復(fù)雜度、低功耗、低數(shù)據(jù)速率、低成本的雙向無(wú)線通 信技術(shù)，主要適合于自動(dòng)控制和遠(yuǎn)程控制領(lǐng)域，可以嵌入各種設(shè)備中，同時(shí)支持地理定位功 能。相對(duì)于現(xiàn)有的各種無(wú)線通信技術(shù)，ZigBee技術(shù)最低功耗和成本的技術(shù)，這些特點(diǎn)決定 了在本專(zhuān)利中中采用ZigBee組網(wǎng)是非常合適的。ZigBee在室內(nèi)通常能達(dá)到30-50米的作 用距離，完全滿(mǎn)足要求，Zigbee模塊與主控芯片的串口針腳相連。
[0083] 驅(qū)動(dòng)電機(jī)采用步進(jìn)電機(jī)，型號(hào)為35BY48L061B-03。步進(jìn)電機(jī)作為一種專(zhuān)門(mén)用于位 置和速度精確控制的特種電機(jī)，最大特點(diǎn)是其"數(shù)字性"，對(duì)于控制器發(fā)過(guò)來(lái)的每一個(gè)脈沖 信號(hào)，步進(jìn)電機(jī)在其驅(qū)動(dòng)器的推動(dòng)下運(yùn)轉(zhuǎn)一個(gè)固定角度。如接收到一串脈沖步進(jìn)電機(jī)將連 續(xù)運(yùn)轉(zhuǎn)一段相應(yīng)距離。同時(shí)可通過(guò)控制脈沖頻率，直接對(duì)電機(jī)轉(zhuǎn)速進(jìn)行控制。由于步進(jìn)電 機(jī)工作原理易學(xué)易用，成本低（相對(duì)于伺服）、電機(jī)和驅(qū)動(dòng)器不易損壞，適合于本實(shí)施例的 個(gè)體機(jī)器人驅(qū)動(dòng)器控制。
[0084] 本實(shí)施例中的步進(jìn)電機(jī)采用BA6845FS驅(qū)動(dòng)芯片進(jìn)行驅(qū)動(dòng)。BA6845FS驅(qū)動(dòng)芯片最 大輸出電流達(dá)到1A，擁有三種輸出模式：前進(jìn)，后退，節(jié)能。該集成電路擁有低的飽和電壓， 能以較低電壓驅(qū)動(dòng)電機(jī)。
[0085] 本實(shí)施例的控制方法具體步驟為：視覺(jué)定位通過(guò)標(biāo)簽識(shí)別獲取個(gè)體機(jī)器人位姿； 對(duì)視覺(jué)定位坐標(biāo)系與機(jī)器人運(yùn)動(dòng)坐標(biāo)系進(jìn)行修正；將個(gè)體機(jī)器人路徑軌跡按照個(gè)體機(jī)器人 編號(hào)存儲(chǔ)路徑鏈表中；依據(jù)當(dāng)前個(gè)體機(jī)器人位姿與路徑軌跡計(jì)算機(jī)器人偏角誤差及位置變 化；以位姿誤差作為模糊控制器輸入，模糊控制規(guī)則解模糊輸出個(gè)體機(jī)器人運(yùn)動(dòng)調(diào)整策略； 判斷個(gè)體機(jī)器人是否完成指定段路徑跟隨，是則進(jìn)入下一段路徑軌跡，否則持續(xù)進(jìn)行反饋 修正；通過(guò)持續(xù)的模糊控制策略反饋，使得個(gè)體機(jī)器人逐漸逼近既定路徑軌跡，從而使得機(jī) 器人運(yùn)動(dòng)軌跡與所給目標(biāo)軌跡趨于一致，最終達(dá)到路徑跟隨的效果；通過(guò)主控計(jì)算機(jī)對(duì)路 徑列表編號(hào)機(jī)器人的參數(shù)設(shè)置以及目標(biāo)節(jié)點(diǎn)遍歷引導(dǎo)，完成對(duì)多個(gè)機(jī)器人的控制協(xié)作。
[0086] 其中，位置連續(xù)變化狀態(tài)方程為：
[0087]
[0088]其中，[xp(k+l) yp(k+l) Θ p(k+l)]為反饋調(diào)整狀態(tài)變化，[xp(k) yp(k) Θ p(k)] 為調(diào)整前移動(dòng)機(jī)器人位姿狀態(tài)，[vp(k) wp(k)]為移動(dòng)機(jī)器人的速度變化向量，ΔΤ為時(shí)間 間隔。
[0089] 得到的位姿誤差輸入模糊控制器，并得到相應(yīng)的控制策略輸出：
[0090] 模糊控制器首先判斷朝向偏差是否過(guò)大，若是，則需要按照偏差角調(diào)整變向，否則 直行，以距離偏差的1/2為單次行程向目標(biāo)點(diǎn)逼近。角