多機(jī)器人系統(tǒng)的三維機(jī)器人定位研究

李伯平

(江西省撫州市東鄉(xiāng)區(qū)技工學(xué)校, 江西 撫州 331800)

機(jī)器人通常在復(fù)雜、動(dòng)態(tài)環(huán)境下工作[1]，它們依據(jù)自身的傳感器獲取外界環(huán)境信息，執(zhí)行任務(wù)。在復(fù)雜環(huán)境中常部署多個(gè)機(jī)器人協(xié)同作業(yè)[2]，共同完成任務(wù)。當(dāng)多機(jī)器人協(xié)同作業(yè)時(shí)，機(jī)器人需要知曉鄰近機(jī)器人的位置，這涉及到機(jī)器人定位問題。

針對(duì)機(jī)器人定位問題，研究人員進(jìn)行了大量研究[2]。He等[3]針對(duì)大型傳感網(wǎng)絡(luò)，提出非測(cè)距定位算法APTI。類似地，Li和Nagpal也分別提出非測(cè)距定位算法[4-5]。但是，這些定位算法是基于一個(gè)條件：系統(tǒng)內(nèi)需部署已知位置的機(jī)器人。這類似于傳感網(wǎng)絡(luò)的錨節(jié)點(diǎn)，類似地，將已知位置的機(jī)器人稱為錨-機(jī)器人。

然而，如何部署錨-機(jī)器人是關(guān)鍵問題。在實(shí)際應(yīng)用環(huán)境中，機(jī)器人是移動(dòng)的，預(yù)先獲取的機(jī)器人位置也是徒勞的。由于實(shí)際應(yīng)用場景常是多機(jī)器人協(xié)同作業(yè)，多機(jī)器人可形成一個(gè)自組織網(wǎng)絡(luò)，相互通信。因此，可充分利用這個(gè)特性實(shí)現(xiàn)定位。

此外，在實(shí)際應(yīng)用環(huán)境中，機(jī)器人部署于三維空間，它的位置也是三維的。但是，多數(shù)機(jī)器人定位算法所估計(jì)的機(jī)器人位置是二維的。例如，Zarzhitsky等[6]利用分布式定位算法估計(jì)了機(jī)器人的二維位置。

為此，針對(duì)多機(jī)器人定位問題，提出簡易的機(jī)器人三維定位算法(Simple Robot Three-Dimensional Localization,SRDL)算法。SRDL算法無需部署錨-機(jī)器人。最初，系統(tǒng)內(nèi)的所有機(jī)器人的位置未知。機(jī)器人借助于內(nèi)嵌的通信設(shè)備，與鄰近機(jī)器人通信。并通過不斷地通信，實(shí)現(xiàn)自主定位。仿真結(jié)果表明，提出的SRDL算法能夠?qū)崿F(xiàn)自主定位，能夠估計(jì)自己的三維位置。

1 3D定位算法

多機(jī)器人場景如圖1所示。假定場景內(nèi)有m個(gè)機(jī)器人。每個(gè)機(jī)器人能與鄰近機(jī)器人進(jìn)行通信，且機(jī)器人的通信半徑為R。令Xi=(xi,yi,zi)表示第i個(gè)機(jī)器人的真實(shí)位置，且1≤i≤m。

(1)

式(1)中，||Xi-Xj||表示節(jié)點(diǎn)i與節(jié)點(diǎn)j間歐式距離。

(2)

2 SRDL定位算法

SRDL定位算法先隨機(jī)估計(jì)自己的位置，然后通過迭代校正位置。因此，SRDL定位算法可分為初值階段和校正階段[7]。在初值階段，節(jié)點(diǎn)最初隨機(jī)估計(jì)自己的位置，并廣播自己的位置；在校正階段，節(jié)點(diǎn)通過鄰近節(jié)點(diǎn)通信，包括一跳鄰居節(jié)點(diǎn)和二跳鄰居節(jié)點(diǎn)，校正位置。

在初值階段，節(jié)點(diǎn)先隨機(jī)的估計(jì)自己位置，再向鄰居節(jié)點(diǎn)廣播Hello消息，其包含了自己的位置估計(jì)值。

在校正階段，節(jié)點(diǎn)利用與一跳鄰居的通信信息，并引用引力進(jìn)行修正，再利用與二跳鄰居的通信信息，并引用斥力進(jìn)行修正。接下來，重點(diǎn)分析校正階段。

2.1 校正階段

每個(gè)迭代可細(xì)分為信息采集和位置更新兩個(gè)階段，如圖2所示。在迭代開始，節(jié)點(diǎn)先采集一跳、二跳鄰居節(jié)點(diǎn)信息，再依據(jù)這些信息進(jìn)行位置更新。最終，通過不斷的迭代，實(shí)現(xiàn)自主定位。

圖2 不同階段的位置估計(jì)值

2.1.1信息采集

圖3 Hello消息的傳遞

2.1.2位置更新

在信息采集階段，節(jié)點(diǎn)能夠獲取一跳、二跳的位置估計(jì)值[9]。因此，節(jié)點(diǎn)利用這些信息更新自己的位置。具體而言，節(jié)點(diǎn)先利用一跳鄰居信息，并用“吸引力”修正位置偏遠(yuǎn)的節(jié)點(diǎn)；然后，節(jié)點(diǎn)利用二跳鄰居節(jié)點(diǎn)信息，并借助于“排斥力”修正距離偏小的節(jié)點(diǎn)。

1) 吸引力

(3)

式(3)中，ωa表示吸引力增益系數(shù)。

圖4 吸引力示例

(4)

(5)

(6)

2) 排斥力

節(jié)點(diǎn)i不僅擁有一跳鄰居節(jié)點(diǎn)估計(jì)的位置，也擁有兩跳鄰居節(jié)點(diǎn)估計(jì)的位置[11]。因此，節(jié)點(diǎn)i充分利用兩跳鄰居節(jié)點(diǎn)信息，對(duì)自己位置進(jìn)一步修正。

(7)

(8)

式(8)中，ωr表示“排斥力”增益系數(shù)。

如圖5所示，節(jié)點(diǎn)h、j分別為節(jié)點(diǎn)i的二跳、一跳鄰居節(jié)點(diǎn)。節(jié)點(diǎn)h與節(jié)點(diǎn)i間的距離理應(yīng)滿足式(7)，若估計(jì)的距離不滿足，則需進(jìn)行修正，用排斥力拉開距離。

圖5 排斥力示例

(8)

(9)

(10)

3) 協(xié)調(diào)力

利用吸引力和排斥力能夠修正節(jié)點(diǎn)位置，但若只利用這兩力更新節(jié)點(diǎn)位置，難以避免局部最小問題。因此，引入?yún)f(xié)調(diào)力。以小的概率將協(xié)調(diào)力替代排斥力。具體而言，本文以0.1概率替換排斥力。

令ωf表示協(xié)調(diào)力的增益系數(shù)，并通過協(xié)調(diào)力修正位置，有：

(11)

2.1.3位置更新流程

在定位過程中，節(jié)點(diǎn)先獲取一跳、二跳鄰居節(jié)點(diǎn)信息，然后再分別依式(6)、式(10)和式(11)計(jì)算吸引力、排斥力和協(xié)調(diào)力。同時(shí)，產(chǎn)生一個(gè)0至1的隨機(jī)數(shù)ξ。如果ξ小于0.9，則利用吸引力和排斥力修正位置，有：

(12)

反之，若ξ大于0.9，則利用吸引力和協(xié)調(diào)辦修正位置，有：

(13)

節(jié)點(diǎn)就根據(jù)自己產(chǎn)生的隨機(jī)數(shù)，依據(jù)式(12)或式(13)迭代更新自己的位置，直到滿足定位精度E或達(dá)到預(yù)置的迭代次數(shù)。

2.2 定位誤差E

式(12)或式(13)所更新的位置只是相對(duì)位置，并非絕對(duì)位置，這就存在相對(duì)位置誤差、相對(duì)角度誤差。假定圖6(a)為節(jié)點(diǎn)的真實(shí)位置，圖6(b)為節(jié)點(diǎn)估計(jì)的位置。此估計(jì)的位置肯定存在相對(duì)位置誤差和相對(duì)角度誤差。

圖6 相對(duì)誤差

(13)

(14)

(15)

式(15)中：

(16)

式(16)中，V所部署區(qū)域的空間體積。從式(16)可知，E值越低，定位越準(zhǔn)確。

3 性能仿真

利用網(wǎng)絡(luò)仿真軟件NS-2.34構(gòu)建仿真平臺(tái)，分析SRDL算法的定位性能。在以下實(shí)驗(yàn)中，吸引力、排斥力和協(xié)調(diào)力的增益系數(shù)分別設(shè)置為ωa=1、ωr=1和ωf=5。認(rèn)為吸引力、排斥力在修正位置的作用相同。但協(xié)調(diào)力是以起到協(xié)調(diào)力，避免局部最小問題。同時(shí)，它出現(xiàn)的概率低(0.1)，因此，將它的增益系數(shù)設(shè)為5。一旦使用協(xié)調(diào)力修正，就大幅度運(yùn)用協(xié)調(diào)力修正。

3.1 實(shí)驗(yàn)一

在103m3的三維網(wǎng)絡(luò)內(nèi)隨機(jī)部署27個(gè)節(jié)點(diǎn)(機(jī)器人)。并在仿真開始，給所有節(jié)點(diǎn)分配一個(gè)隨機(jī)位置估計(jì)值。節(jié)點(diǎn)的通信半徑為6.0 m。

經(jīng)過755次迭代，SRDL算法估計(jì)了27個(gè)節(jié)點(diǎn)位置。如圖7所示，藍(lán)色小圓圈表示估計(jì)位置，紅色加號(hào)表示節(jié)點(diǎn)位置。從圖7可知，SRDL算法基本能準(zhǔn)確地估計(jì)節(jié)點(diǎn)位置。

圖7 27個(gè)節(jié)點(diǎn)的位置估計(jì)值

3.2 實(shí)驗(yàn)二

本次實(shí)驗(yàn)考查通信半徑R和節(jié)點(diǎn)數(shù)m對(duì)定位精度E的影響。且R從1 m變化至15 m，步長為1 m。節(jié)點(diǎn)數(shù)m取10、30、50和100。這些節(jié)點(diǎn)最初隨機(jī)分布于103m3的三維網(wǎng)絡(luò)內(nèi)。本次實(shí)驗(yàn)迭代10 000次，取平均值作為最終仿真數(shù)據(jù)。

圖8顯示了節(jié)點(diǎn)數(shù)m和通信半徑R對(duì)定位精度E的影響。在R從1到15的變化期間，節(jié)點(diǎn)數(shù)m在10、30、50和100時(shí)定位誤差E的平均值分別為0.85，0.73，0.70，0.68。這些數(shù)據(jù)表明：節(jié)點(diǎn)數(shù)越多，定位誤差越低。原因在于：節(jié)點(diǎn)數(shù)越多，節(jié)點(diǎn)密度越高。節(jié)點(diǎn)獲取的一跳鄰居節(jié)點(diǎn)、兩跳鄰居節(jié)點(diǎn)的位置信息越多，越能修正節(jié)點(diǎn)的位置信息。

此外，觀察圖8不難發(fā)現(xiàn)，四條定位誤差曲線隨R從1～15的變化過程呈凹陷狀。當(dāng)R=7 m時(shí)，定位誤差最低。這也說明，并非R越大，定位誤差越小。

圖8 定位誤差與通信半徑的關(guān)系

4 結(jié)論

針對(duì)多機(jī)器人系統(tǒng)的機(jī)器人自主定位問題，提出簡易的三維定位算法SRDL。SRDL算法先通過內(nèi)嵌的通信設(shè)備與鄰近機(jī)器人通信，并獲取鄰近機(jī)器人的位置信息，然后，再引用修正力修正位置。通過多次迭代修正，最終實(shí)現(xiàn)自主定位。最后，利用仿真實(shí)驗(yàn)分析了通信半徑和機(jī)器人密度對(duì)定位精度的影響。

本文并沒有通過實(shí)驗(yàn)分析吸引力、排斥力和協(xié)調(diào)力的增益系數(shù)對(duì)定位精度的影響，僅從理論上推導(dǎo)并假定了這三個(gè)增益系數(shù)值。后期，將通過實(shí)驗(yàn)進(jìn)一步分析，尋求最優(yōu)的參數(shù)值。