專利名稱：一種多機器人跟蹤移動目標(biāo)算法的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及多機器人對移動目標(biāo)跟蹤領(lǐng)域，具體來說是根據(jù)移動目標(biāo) 的預(yù)測位置來動態(tài)調(diào)整移動機器人的運動模型，以使得移動目標(biāo)始終處于 各個機器人的可視范圍。
背景技術(shù)：
目標(biāo)跟蹤技術(shù)已經(jīng)被廣泛應(yīng)用于監(jiān)控、自動防衛(wèi)系統(tǒng)以及機器人領(lǐng) 域。隨著機器人在人類日常生活中的普及，機器人必須實現(xiàn)諸如編隊、對 環(huán)境的感知、避障等任務(wù)。移動目標(biāo)跟蹤也是機器人領(lǐng)域的一個重要，而 且富有挑戰(zhàn)性的研究方向，尤其是當(dāng)移動目標(biāo)的運動路徑、運動速度等參 數(shù)未知的情況下。使用多機器人對隨機運動目標(biāo)進行跟蹤時必須考慮如下
兩個問題， 一是如何確保移動目標(biāo)始終處于各個機器人的可視范圍內(nèi)；二 是如何使用各個機器人對移動目標(biāo)的測量信息融合得到最終的移動目標(biāo) 位置。

發(fā)明內(nèi)容
本發(fā)明提供了 一種新的多機器人跟蹤移動目標(biāo)的算法，使用移動目標(biāo) 的歷史位置信息構(gòu)造移動目標(biāo)的位置預(yù)測模型，使用移動目標(biāo)的預(yù)測位置 構(gòu)造動態(tài)機器人運動控制模型，并使用融合算法對移動目標(biāo)位置進行更新 得到更加精確的移動目標(biāo)位置。
本發(fā)明算法首先根據(jù)移動目標(biāo)的歷史信息動態(tài)構(gòu)造移動目標(biāo)的運動 預(yù)測模型，并使用此模型對移動目標(biāo)的位置進行預(yù)測，然后使用移動目標(biāo) 的預(yù)測位置對機器人的運動控制模型進行動態(tài)調(diào)整，最后使用各個機器人 對移動目標(biāo)的檢測信息來更新移動目標(biāo)的位置。
一種新的多機器人跟蹤移動目標(biāo)的算法，包括以下步驟
1)使用移動目標(biāo)的當(dāng)前位置信息和前一時刻的位置信息構(gòu)造移動目 標(biāo)的運動預(yù)測模型，構(gòu)造移動目標(biāo)的運動預(yù)測模型時分兩種情況進行考慮 一是在保證移動目標(biāo)相鄰時刻間位置角度差保持一致的條件下，當(dāng)預(yù) 測到的移動目標(biāo)位置和當(dāng)前時刻移動目標(biāo)位置之間的笛卡爾距離小于等 于之前相鄰時刻間移動目標(biāo)位置間的笛卡爾距離時，預(yù)測的移動目標(biāo)位置 和當(dāng)前時刻移動目標(biāo)位置之間的歐氏距離差和角度差同當(dāng)前時刻移動目 標(biāo)位置和前一時刻移動目標(biāo)位置的差都保持一致的移動目標(biāo)運動預(yù)測模 型，簡稱為等距離等角度的移動目標(biāo)運動預(yù)測模型，二是在保證移動目標(biāo) 相鄰時刻間位置角度差保持一致的條件下，當(dāng)預(yù)測到的移動目標(biāo)位置和當(dāng) 前時刻移動目標(biāo)位置之間的笛卡爾距離大于之前相鄰時刻間移動目標(biāo)位 置間的笛卡爾距離時，預(yù)測的移動目標(biāo)位置和當(dāng)前時刻移動目標(biāo)位置之間 的歐氏距離差同當(dāng)前時刻移動目標(biāo)位置和前一時刻移動目標(biāo)位置的差都 保持一致的移動目標(biāo)運動預(yù)測模型，簡稱為等距離的移動目標(biāo)運動預(yù)測模 型；
2)使用移動目標(biāo)運動預(yù)測模型的移動目標(biāo)預(yù)測位置構(gòu)造動態(tài)的機器 人運動控制模型，從而使得移動目標(biāo)始終處于移動機器人的可視范圍內(nèi)；
3 )使用移動機器人對移動目標(biāo)的測量信息更新移動目標(biāo)的位置信息， 同時使用協(xié)方差插值方法融合各個機器人對移動目標(biāo)位置的估計，以得到 最終的移動目標(biāo)位置。
所述的等距離等角度的移動目標(biāo)運動預(yù)測模型如式(3)所示。其中
o"e"(A:+l)—x和。^'e"(A:+i;Ly表示移動目標(biāo)在壯l時刻的笛卡爾坐標(biāo)，
。6X0—;c和ofy'e"(A:)j表示移動目標(biāo)在A:時刻的笛卡爾坐標(biāo)。
1)—x和表示機器人在A+l時刻的笛卡爾坐標(biāo)。
<formula>formula see original document page 6</formula>
其中^—5為式(2)所示。
<formula>formula see original document page 6</formula>V((o&'ert(A+l)—jc)2 +(o^erfOt+l)—力2) "V((o《erfOt)一:c)2 +(o^erf0t)_y)2) *cos ^3=arccos-
V(咖rf0t+l)一少-咖c/(/:)一力2+(o&'erf(A:+l)一"-o嶺"(A:)一;02
(6)
W…arctan—(7)
oty'e"(A: + l) —x
所述的等距離的移動目標(biāo)運動預(yù)測;漠型如式(8)所示。
〖o(jì)fy'e"(A: + 2) — ;c — / rW" = 2 x oty.ec"A: +1) — x — — x ,。、
S — 一 — 一
—六"(A + 2) _ _y 一 /w"W" = 2 x +1) _ _y — —y
所述的機器人運動控制模型如式(9)所示，其中ra6o^+2)—;c和 ra6o《A:+2)j表示機器人在"2時刻的位置坐標(biāo)，6>_/ (6 + 2)表示機器人在 /H"2時刻的方位角，7 0t + l)表示機器人在/H"l時刻的方位角。 c^ec《t+2)一x^re^'c^和ofy'e"(A:+2)_yj^e&c/表示才幾器人在時刻的移 動目標(biāo)預(yù)測位置。
o6o"A + 2) _ jc = /"o6o/(A: +1) — x
-"oA(0/fJt + " t、2 ++ 2、 i; wed'V /—/y)Ao"Jt +1、 v、2 1
cos(6>_/ 0t + 2))
(otyec/(/: + 2) _ :c — pm^'" - ro6o/(A +1) _ jc)2 + (otyec,(A + 2) — y _ - ro6o"A +1)—力2
(9)
、-^(。tyec/(A +1) — jc - ro6o/(* +1) 一 x) + (otye"(A +1) — y - "6o"A: +1) 一》 roi>of(fc + 2) — y = ro&o/(A: +1) — :f
V(otyec/(A: + 2)一x一 — ro6o/(A + l)_;t)2 +(o~e"(A + 2) —y —P"^'" - "o6o/(A +1) —_v)2
、-V(。 e"(Jt +1) 一 ;c - ro6o"A +1) _ + +1) j - ,oAo/(A +1) 一力2
^_ + 2) = arctan 。一"2)丄—'c,-由(hl) —y
-肌加。一^+')-"。6。^+" j+一 一" otye"(A +1) _ x - /*o6o/(A +1) _ jc /"o6o"A: +1) _ x
本發(fā)明方法使用移動目標(biāo)當(dāng)前時刻的位置和前一時刻的位置構(gòu)造移 動目標(biāo)的運動預(yù)測模型，使用移動目標(biāo)的預(yù)測位置對移動機器人的控制模 型進行動態(tài)調(diào)整，同時使用協(xié)方差插值融合方法對移動目標(biāo)位置進行更 新。本發(fā)明方法能夠保證移動目標(biāo)始終處于多機器人的可視范圍內(nèi)，可以 保證對移動目標(biāo)位置估計的精確性。


圖1為本發(fā)明算法的具體流程圖2為移動目標(biāo)位置預(yù)測模型構(gòu)造描述；
圖3為等距離等角度的移動目標(biāo)運動預(yù)測模型;
圖4為等距離的移動目標(biāo)運動預(yù)測模型；圖5為才幾器人的動態(tài)控制才莫型；
圖6為兩個機器人對移動目標(biāo)位置預(yù)測測試的仿真環(huán)境； 圖7為機器人Rl對移動目標(biāo)位置跟蹤后的機器人路徑信息； 圖8為機器人R2對移動目標(biāo)位置跟蹤后的機器人路徑信息； 圖9為兩個機器人對移動目標(biāo)位置跟蹤后的移動目標(biāo)位置信息； 圖10為兩個機器人對移動目標(biāo)位置跟蹤得到的移動目標(biāo)位置信息和 實際移動目標(biāo)位置信息的比較。
具體實施例方式
一種新的多機器人跟蹤移動目標(biāo)算法，算法流程圖如圖l所示，首先 造移動目標(biāo)的運動預(yù)測模型，構(gòu)造移動目標(biāo)的運動預(yù)測模型時分兩種情況 進行考慮 一是在保證移動目標(biāo)相鄰時刻間位置角度差保持一致的條件 下，當(dāng)預(yù)測到的移動目標(biāo)位置和當(dāng)前時刻移動目標(biāo)位置之間的笛卡爾距離 小于等于之前相鄰時刻間移動目標(biāo)位置間的笛卡爾距離，二是在保證移動 目標(biāo)相鄰時刻間位置角度差保持一致的條件下，當(dāng)預(yù)測到的移動目標(biāo)位置 和當(dāng)前時刻移動目標(biāo)位置之間的笛卡爾距離大于之前相鄰時刻間移動目 標(biāo)位置間的笛卡爾距離。然后使用移動目標(biāo)運動預(yù)測模型的移動目標(biāo)預(yù)測 位置構(gòu)造機器人的運動控制模型，并使用運動控制模型對機器人的位姿進 行估計，以保證移動目標(biāo)始終處于移動機器人的可視范圍內(nèi)。最后使用移 動機器人對移動目標(biāo)的測量信息更新移動目標(biāo)的位置信息，同時使用協(xié)方 差插值方法融合各個機器人對移動目標(biāo)位置的估計，得到最終的移動目標(biāo) 位置。
首先構(gòu)造移動目標(biāo)的運動預(yù)測模型，圖2表示了移動目標(biāo)的運動預(yù)測 模型構(gòu)造方法。a表示移動目標(biāo)在時刻^+l和A:的位置坐標(biāo)之間的角度差， 卩表示在保證"2時刻移動目標(biāo)的位置和A+l時刻移動目標(biāo)位置之間的歐 式距離和壯l時刻與A:時刻之間歐式距離相等的條件下，和壯l時刻目標(biāo) 位置之間最大的角度差。對移動目標(biāo)的運動預(yù)測模型構(gòu)造分為兩種情況進 行考慮
一是當(dāng)P大于a時的移動目標(biāo)運動預(yù)測模型構(gòu)造，此時構(gòu)造方法如圖 3所示，此移動目標(biāo)運動預(yù)測模型稱為等距離等角度的移動目標(biāo)運動預(yù)測 模型。二是當(dāng)卩小于a時的移動目標(biāo)運動預(yù)測模型構(gòu)造，此時構(gòu)造方法如圖 4所示，此移動目標(biāo)運動預(yù)測模型稱為等距離的移動目標(biāo)運動預(yù)測模型。
由圖2可知，a的計算方法為式(l), P的計算方法為式(2)。其中 c^/e"(A:+l)_jc和oty'e"(A+l)j表示移動目標(biāo)在壯l時刻的笛卡爾坐標(biāo)， otye"(A:)—x和oZy'ecf(A;)j表示移動目標(biāo)在&時刻的笛卡爾坐標(biāo)。
(1)
(2)
a = arctan ~^~^-— arctan ~^——、
"J(o6X丸+1) 一 x - oty'e"(A:) — + (oZy'e"(A +1)—>" _ ofy'e"(A) 一 >>) 〃 =arcsin "^^"^-一, — /\ — -一 ,
由圖3可知，當(dāng)卩大于a時，等距離等角度的移動目標(biāo)運動預(yù)測模型 為式(3)所示。 —+ 2) — jc — / mi/" = +1) — x
+V(^fye"(A +1) — x — o辦/e"(A:) 一 x)2 + (oty^"(A: +1)—少一 —力2 cos(夕—5) (〗)
oZ X允+ 2) — y — j mZ/" = +1) j
+V(^^/e"(A +1) — x 一 一 jc)2 + +1) — V — c ^/e"(A:) 一 j/)2 sin(P 一 5)
式(3)中0_5表示圖3中移動目標(biāo)fc+2時刻的預(yù)測4立置和x坐標(biāo)系之間 的角度差，如式(4)所示。
P —5 = 2x;r — 61 —P2 — ^3 —64 (4)
式(4)中的m， 02， M和04的值如式(5)，(6)和(7)所示。
<formula>formula see original document page 9</formula>
(6)
<formula>formula see original document page 9</formula>
由圖4可知，當(dāng)(3小于a時，等距離的移動目標(biāo)運動預(yù)測模型為式(8) 所示。
<formula>formula see original document page 9</formula>其次是根據(jù)圖5獲得機器人的動態(tài)控制模型，為了使得移動目標(biāo)處于 機器人的可視范圍內(nèi)，機器人在/t+2時刻的方位角必須滿足式(9)所示的關(guān) 系。其中ra6o《A+l)—jc和ra6o/("lLy表示機器人在A+l時刻的笛卡爾坐 標(biāo)位置，及(A: + 1)表示機器人在A+l時刻的方位角，ra6W(A:+2)—;c和 rak 《A:+2)j表示機器人在A+2時刻的笛卡爾坐標(biāo)位置，0—及(Jt+2)表示機 器人在;t+2時刻的方位角。
arctan~^~^-^-^——^^一6>_雖+ 1)
+1) — x - ；-o&o"t +1) — x _ (9)
oZv'ec"A + 2) y 戸血卜z-o6o"A: +1) >> ^ ,、
=arctan^——^-'一'—尸-^-^=^~一0 —及(A + 2)
oty'ec,(A: + 2) — x — / rW" — ra6o/(A +1) — x —
由圖5和式(9)可得，時刻A:+2時機器人的動態(tài)控制模型如為式(10)所 示。其中rak ^:+2)—x和ra6o《&+2)_j表示機器人在fc+2時刻的位置坐標(biāo)， i (A: + 2)表示才幾器人在A+2時刻的方位角，6 —及(A: + 1)表示沖幾器人在A:+l 時刻的方位角。o6XA:+2)—;c和—'e"(A:+2)j表示機器人 在t+2時刻的移動目標(biāo)預(yù)測位置。
'o6o/(A + 2) _ jc = ro—fc +1) — x
cos((9 —W(A + 2))
otyec'(fc +1) _ x - a^of(;c +1) — ;c)2 + (otyec/(A +1) — ^ - ro&o"/t +1) _力2 ro6o/(A + 2) _少=ro6o"A: +1) — _V
-2) 一 x 一 prec/!c/ - roi>o"A +1) — x)2 + (otye"(A + 2) — y 一 / recto - ro6of+1) 一力2 (1 0)
+1) _ jc - ro +1) — x)2 + (咖c/(A: +1) 一 - n 6碌+1) _力2 J
P 一柳+ 2)=她n 。一"2)丄—c卜由("l)一y — oty"/(t + 2) — _ / redf'c/ — /v 6o/(A +1) — x
-肌加 一闊—"一"i)j+肌t肌一(""
otyec,(A +1) — jc - ro6o/(A +1) _ :c ro辦o/(A +1) — ;c
再次是當(dāng)各個機器人檢測到移動路標(biāo)時，使用擴展卡爾曼濾波器對移 動目標(biāo)位置進行更新。假設(shè)機器人Rl對移動目標(biāo)的位置均值和協(xié)方差估 計依次為ofy'ec/joo礎(chǔ)ow—i /， oty'ec/j os脅"-W"ce加,"/^/， 機器人R2對
移動目標(biāo)的位置均值和協(xié)方差估計 Oty'e"J70W力'0"—及2 和
oty'e"j90W'"o"一w"ceWm'"/)^。 <吏用協(xié)方差4翁j直方法^"才幾器人Rl和才幾器人 R2的信息進行融合后的移動目標(biāo)位置估計協(xié)方差為式(ll)所示。
+(1 - w) x (o映"^po礎(chǔ)o打一imcertof叫2) 1 使用協(xié)方差插值方法對機器人R1和機器人R2的信息進行融合后的移動目標(biāo)位置估計均值為式(12)所示。w x ^j o礎(chǔ)ow—tmce加i"/y — 1) 1 x oty'W — — 1 (12)x — 一 一 一、+(1 - w) x ^ay/,/o"一wwcerto'剛—2) x— / os7Y/ow — 2乂w的取值如式(13)所示。式(13)中d表示^poszY/o"—w"ceWfl/"(v/ 矩陣的跡,表示o^/ecf j9as/"'0"一w"certo/"(v/矩P車的跡。w = (13)圖6描述的是機器人R1和機器人R2對移動目標(biāo)進行跟蹤的測試環(huán)境。 圖7、圖8、圖9以及圖10為使用本發(fā)明算法得到的測試結(jié)果。圖7為機 器人Rl對移動目標(biāo)進行跟蹤后機器人的運動軌跡，圖8為機器人R2對 移動目標(biāo)進行跟蹤后機器人的運動軌跡，圖9為機器人Rl和R2估計得 到的移動目標(biāo)的運動軌跡，圖10為移動目標(biāo)的估計運動軌跡和實際運動 軌跡之間的誤差?？梢钥闯?，使用本發(fā)明得到的移動目標(biāo)估計軌跡和實際 軌跡的誤差不大，因此，本發(fā)明可以很好的實現(xiàn)多機器人對移動目標(biāo)的跟 蹤。
權(quán)利要求
1.一種新的多機器人跟蹤移動目標(biāo)算法，包括以下步驟1)使用移動目標(biāo)的當(dāng)前位置信息和前一時刻的位置信息構(gòu)造移動目標(biāo)的運動預(yù)測模型；2)使用移動目標(biāo)的運動預(yù)測模型的移動目標(biāo)預(yù)測位置構(gòu)造動態(tài)的機器人運動控制模型；3)使用移動機器人對移動目標(biāo)的測量信息更新移動目標(biāo)的位置信息，同時使用協(xié)方差插值方法融合各個機器人對移動目標(biāo)位置的估計，以得到最終的移動目標(biāo)位置。
2. 如權(quán)利要求1所述的方法，其特征在于在保證移動目標(biāo)相鄰時 刻間位置角度差保持一致的條件下，當(dāng)預(yù)測到的移動目標(biāo)位置和當(dāng)前時刻 移動目標(biāo)位置之間的笛卡爾距離小于等于之前相鄰時刻間移動目標(biāo)位置 間的笛卡爾距離時，移動目標(biāo)的運動預(yù)測模型為式(3)所示，否則，移動目 標(biāo)的運動預(yù)測模型為式(4)所示<formula>formula see original document page 2</formula>0"-arctan一闊J (8) o&.e"(A: +1) — x其中:c和oZ /e"(A:+l)j表示移動目標(biāo)在A:+l時刻的笛卡 爾坐標(biāo)；oty'e"(&)—:c和ofy'e"(^Ly表示移動目標(biāo)在A時刻的笛卡爾坐標(biāo)； ra6o《A:+l)—;c和ro6o《A:+l)j表示機器人在A+l時刻的笛卡爾坐標(biāo)； ra6o《A:+2)—jc和raZ o《A:+2)j表示機器人在壯2時刻的笛卡爾坐標(biāo)。 P — i (/t + 2)表示機器人在"2時刻的方位角，e — i (A: + l)表示機器人在A:+l 時刻的方4立角；ofy'ec《A:+2)—x—j rai/"和oZy'ec《A:+2)_^_/^e<^C表示才幾器人 在A:+2時刻的移動目標(biāo)預(yù)測位置。
3.如權(quán)利要求1所述的方法，其特征在于根據(jù)移動目標(biāo)的運動預(yù) 測模型式(3)或式(4)對移動目標(biāo)位置進行預(yù)測后，機器人的運動控制模型 如式(9)所示ro6oZ(A + 2) _ jc = ro6o"A +1) _ x■o6of" + l、 jc、 +"/>,ec/" + 2、 v mW/c/-raAo〃"l、 v、2 )(otyec/(A + 2) _ x _ pm/'" - ro6o"A: +1) — x)2 + (otyec/(々+ 2) _少—pre力c/ - ro6o"A +1) _力2oty'ec,(A +1) _ x - ro6o"A +1) — x)2 + (otyec"it +1) _ jv - ro6o"A +1) _ "o6o,(A + 2) _ y = n 6o/(A +1) _ _v^ ^ (9)si，—柳+ 2))+ 1) — :c - ro6o/(A: +1) — jc)2 + (otyec"A +1) — - roAo"A: +1) 一 "20 —尺0 + 2)=肌加+ 2)丄—"-"W(/: +1) 一 y — oZyec,(A: + 2) — ;c — ; re^" - ro6o/(A +1) _ x—肌加。映"("1)一"一"I)一;; + arctan —j otyec/(A +1) — x - /*o6o,(& +1) _ x ro6o/(A +1) — x
4.如權(quán)利要求1所述的方法，其特征在于使用協(xié)方差插值方法進 行目標(biāo)位置協(xié)方差信息融合的方法如式(10)所示，目標(biāo)位置均值信息融合 的方法如式(ll)所示(。fy.erfjms7'".。M—wwcwto'w/y) 1 =wx(。ty'e"^as訴。"一wwcer加'剛l) 1+(1-w)x(。Zy'ec/_pos7'"'ow_wmwto>2/>"2) 1(11)、+(1-w)x(oty.e"wwce加/"(y — 2) 1 xo 'e" —pWz'。w —2,其中o映"_pos7'"'o"_/ _/和o映c/j: o礎(chǔ)ow—W2ce加/"(y7為機器人Rl 對移動目標(biāo)的位置均值和協(xié)方差估計，oty'ecf^os/"o"一及2和 c^'ec/j os衍ow—wzceWm'"0^為才幾器人R2對移動目才示的4立置均值和妨、方差 估計，w的值如式(12)所示。w = ~^~ (12)其中 /1 表示 _poszYz'o"—柳certoz'"^^ 矩陣的跡，f2 表示o映"jmsWo"一wwce加/"/^矩P車的跡。
全文摘要
本發(fā)明公開了一種新的多機器人跟蹤移動目標(biāo)算法，包括以下步驟1)使用移動目標(biāo)的當(dāng)前位置信息和前一時刻的位置信息構(gòu)造移動目標(biāo)的運動預(yù)測模型；2)使用移動目標(biāo)的運動預(yù)測模型的移動目標(biāo)預(yù)測位置構(gòu)造動態(tài)的機器人運動控制模型；3)使用移動機器人對移動目標(biāo)的測量信息更新移動目標(biāo)的位置信息，同時使用協(xié)方差插值方法融合各個機器人對移動目標(biāo)位置的估計，以得到最終的移動目標(biāo)位置。本發(fā)明方法能夠保證移動目標(biāo)始終處于多機器人的可視范圍內(nèi)，可以保證對移動目標(biāo)位置估計的精確性。
文檔編號G06T7/20GK101630413SQ20091010160
公開日2010年1月20日 申請日期2009年8月14日 優(yōu)先權(quán)日2009年8月14日
發(fā)明者亮 張, 蔣榮欣, 陳耀武 申請人:浙江大學(xué)