[發(fā)明專利]一種多機(jī)器人系統(tǒng)合作定位危險氣味源方法

有效

• 申請?zhí)枺?/td>: 201010609631.5
• 申請日: 2010-12-28
• 公開（公告）號: CN102034030A
• 公開（公告）日: 2011-04-27
• 發(fā)明（設(shè)計(jì)）人: 呂強(qiáng);謝小高;羅平
• 申請（專利權(quán)）人: 杭州電子科技大學(xué)
• 主分類號: G06F19/00
• 分類號: G06F19/00
• 代理公司: 杭州求是專利事務(wù)所有限公司 33200
• 代理人: 杜軍

1.一種多機(jī)器人系統(tǒng)合作定位危險氣味源方法，其特征在于該方法包括以下步驟：

(1)利用氣味分子的運(yùn)動學(xué)模型，建立氣味源位置的觀測模型，具體方法是：

A.建立單個氣味分子團(tuán)的運(yùn)動模型:????????????????????????????????????????????????；其中指氣味分子團(tuán)在時刻的位置,指氣味分子團(tuán)在時刻位置的微分,是在時刻的均值風(fēng)速度,表示一個隨機(jī)過程，該隨機(jī)過程服從均值為零，方差為的高斯分布；

對氣味分子團(tuán)的運(yùn)動模型在時間段[,]內(nèi)進(jìn)行積分，獲得氣味分子團(tuán)在時刻被氣味源釋放，在當(dāng)前時刻時的位置：

????????????

其中[,]指積分的時間段，指氣味源釋放氣味分子團(tuán)的時間，指當(dāng)前時間,指的是在當(dāng)前時間氣味分子團(tuán)的位置,是在時刻氣味源的位置；

B.通過定義和，將運(yùn)動模型離散化；其中離散時刻是氣味源釋放氣味分子團(tuán)時間的整數(shù)值，離散時刻是當(dāng)前時間的整數(shù)值，是指時間的微小變化，氣味分子團(tuán)的離散模型為：

?????????????

其中表示氣味源在時刻釋放氣味分子團(tuán)，在時刻的位置；表示在時間段[,]內(nèi)風(fēng)推動氣味分子團(tuán)的移動距離；表示隨機(jī)過程，該隨機(jī)過程服從均值為零，方差的正態(tài)分布；

????令，

則氣味分子團(tuán)的離散運(yùn)動模型進(jìn)一步表示如下：

?????????????????????

其中為時刻氣味分子團(tuán)的位置,為時刻氣味源的位置,為時刻風(fēng)推動氣味分子團(tuán)移動的距離,是測量噪聲并且服從零均值，方差的正態(tài)分布；

????C.通過定義，得到氣味源位置的觀測模型：

????????????????????????

其中，是第個機(jī)器人在時刻對氣味源位置的測量值；

(2)基于最大熵粒子濾波原理，獲得氣味源位置的先驗(yàn)概率分布；如果在時刻，檢測到氣味，使用測量值修正氣味源位置的先驗(yàn)概率分布，得到氣味源位置的后驗(yàn)概率分布，通過采樣氣味源位置的后驗(yàn)概率分布產(chǎn)生氣味源位置新的估計(jì)值；然后，獲得第個機(jī)器人的下一步的位置；如果在時刻，沒有檢測到氣味，則采樣氣味源位置的先驗(yàn)概率分布產(chǎn)生氣味源位置新的估計(jì)值，獲得第個機(jī)器人的下一步的位置，具體方法是：

a.建立關(guān)于氣味源位置的背景分布；

????????????????

其中,表示氣味源位置滿足正態(tài)分布的隨機(jī)變量,是氣味源位置的經(jīng)驗(yàn)估計(jì)值,是關(guān)于氣味源位置的經(jīng)驗(yàn)估計(jì)協(xié)方差矩陣,表示正態(tài)分布；

????根據(jù)各個機(jī)器人對氣味源位置的估計(jì)值，求解下式：

???????????????

???????????????

其中表示機(jī)器人還沒有獲得測量值時的時刻,是第個機(jī)器人對氣味源位置的先驗(yàn)估計(jì)值,表示的是測量預(yù)測值的先驗(yàn)平均值，并由第個機(jī)器人獲得；表示測量預(yù)測值的先驗(yàn)矩陣,是機(jī)器人的個數(shù)；

b.基于最大熵原理，采用共軛梯度法求解下式：

??????

其中：???

????????????

?????????

?????????

通過求解上式，獲得拉格朗日乘子和，從而得到氣味源位置的先驗(yàn)概率分布：

???????

其中表示氣味源位置滿足正態(tài)分布的隨機(jī)變量，是氣味源位置的均值，是關(guān)于氣味源位置的協(xié)方差矩陣，表示正態(tài)分布；??????????????

c.根據(jù)第個機(jī)器人是否檢測到氣味，執(zhí)行不同的步驟；如果檢測到氣味，則執(zhí)行c-1，否則執(zhí)行c-2；

c-1.?檢測到氣味，執(zhí)行以下兩個步驟:

①使用測量值更新拉格朗日乘子和，得：

其中表示機(jī)器人獲得測量值時的時刻，是測量噪聲協(xié)方差矩陣，????這樣就獲得了氣味源位置的后驗(yàn)概率分布：

???????

采樣氣味源位置的后驗(yàn)概率分布，得到氣味源位置的最新估計(jì)值；

②基于氣味源位置的最新估計(jì)值，則機(jī)器人下一步的位置為氣味源位置的最新估計(jì)值，即，其中是時刻第個機(jī)器人的位置；

c-2.沒有檢測到氣味，則執(zhí)行以下兩個步驟：

③采樣氣味源位置的先驗(yàn)概率分布，得到氣味源位置的最新估計(jì)值；

④基于氣味源位置的最新估計(jì)值和沒有檢測到氣味的周期數(shù)，計(jì)算機(jī)器人下一步的位置，如果在兩個采用周期內(nèi)沒有檢測到氣味，執(zhí)行步驟ⅰ，如果在兩個采用周期及兩個采用周期以上沒有檢測到氣味，則執(zhí)行ⅱ；

ⅰ機(jī)器人的下一步的位置為：

其中和分別是風(fēng)在軸和軸方向的速度,是第個機(jī)器人在時刻對于氣味源位置最新估計(jì)值在軸方向的坐標(biāo),第個機(jī)器人在時刻位置在軸方向的坐標(biāo),和第個機(jī)器人在時刻位置在軸和軸方向的坐標(biāo)；

ⅱ機(jī)器人下一步位置是基于氣味源位置最新估計(jì)值產(chǎn)生,具體方法是：

???????????????

???????????????

其中和分別是控制軸和軸方向搜索范圍控制參數(shù)，在范圍內(nèi)產(chǎn)生一個均勻分布的隨機(jī)數(shù),是機(jī)器人的個數(shù)；

d.當(dāng)?shù)趥€機(jī)器人到達(dá)新的位置后，重復(fù)步驟a至步驟c，直到群體機(jī)器人中的某一個機(jī)器人檢測到的氣味濃度達(dá)到預(yù)先設(shè)定值，或通過視覺傳感器判斷出氣味源，然后通過無線網(wǎng)絡(luò)通知第個機(jī)器人，則第個機(jī)器人停止搜索，并提供氣味源的估計(jì)位置。