本發(fā)明屬于復(fù)雜場景安全管控和人機(jī)交互的多目標(biāo)跟蹤，特別是涉及一種基于多目標(biāo)跟蹤的機(jī)器人人員跟隨方法。

背景技術(shù)：

1、人們對能夠跟隨人類的服務(wù)機(jī)器人的需求不斷增長，此類機(jī)器人用于協(xié)助人們完成引導(dǎo)、看守和老人護(hù)理等日常任務(wù)。當(dāng)機(jī)器人執(zhí)行協(xié)作任務(wù)時，它們必須穩(wěn)健地跟蹤目標(biāo)位置，在接下來的過程中，當(dāng)物體遮擋或離開相機(jī)視野(filed?ofview，fov)時，就會出現(xiàn)問題。為了區(qū)分視覺跟蹤目標(biāo)，多目標(biāo)跟蹤(multi-object?tracking,mot)至關(guān)重要，它區(qū)分不同的個體，并保留跟蹤目標(biāo)的位置信息，以便重新匹配目標(biāo)。

2、現(xiàn)有的跟蹤算法都是基于oab跟蹤框架，即在第一幀中選擇跟蹤目標(biāo)，后續(xù)幀基于提取的跟蹤目標(biāo)目標(biāo)特征以區(qū)分跟蹤目標(biāo)。該框架不適合更復(fù)雜的行人跟蹤場景。我們的工作提出了一種不同于這些傳統(tǒng)框架的新穎框架，在多對象跟蹤框架內(nèi)呈現(xiàn)了機(jī)器人人跟蹤系統(tǒng)。該系統(tǒng)采用了目標(biāo)定位技術(shù)，可以準(zhǔn)確識別多個跟蹤對象并在多個跟蹤對象之間動態(tài)切換跟隨id，因此，在方法中利用多目標(biāo)跟蹤來感知機(jī)器人視覺跟蹤，并通過關(guān)聯(lián)算法和狀態(tài)估計(jì)有效地管理多個目標(biāo)。

3、現(xiàn)有的多目標(biāo)跟蹤算法在對象頻繁交互和相互遮擋的復(fù)雜場景中面臨著挑戰(zhàn)，在這種情況下，檢測器檢測到的多個目標(biāo)和跟蹤預(yù)測的多個狀態(tài)位置具有很高的iou重疊。這使得單獨(dú)使用iou作為關(guān)聯(lián)成本矩陣很難正確匹配目標(biāo)。為了解決這個問題，一些算法將外觀特征集成到關(guān)聯(lián)成本中。然而，被遮擋目標(biāo)的特征通常不可靠，這可能會削弱這些方法的有效性，因此本發(fā)明設(shè)計(jì)了一種基于多目標(biāo)跟蹤的機(jī)器人人員跟隨方法。

技術(shù)實(shí)現(xiàn)思路

1、為了實(shí)現(xiàn)上述目的，本發(fā)明方法引入了一個基于多目標(biāo)跟蹤框架的機(jī)器人人跟蹤框架。該框架最初采用rgb-d相機(jī)來同時并行捕獲彩色圖像和深度信息。通過目標(biāo)檢測和多目標(biāo)跟蹤算法對彩色圖像進(jìn)行處理，實(shí)現(xiàn)目標(biāo)的實(shí)時視覺跟蹤。為了解決人口稠密場景中物體頻繁遮擋的問題，我們設(shè)計(jì)了深度成本矩陣來增強(qiáng)跟蹤性能。此外，我們設(shè)計(jì)了兩種目標(biāo)定位方法來識別和動態(tài)切換多目標(biāo)跟蹤中的跟隨id。隨后，根據(jù)定位數(shù)據(jù)在空間坐標(biāo)系內(nèi)規(guī)劃機(jī)器人和跟蹤目標(biāo)的路徑。最終，提出了全向輪機(jī)器人的橫向和縱向控制模型，旨在實(shí)現(xiàn)動態(tài)目標(biāo)的有效實(shí)時跟蹤。

2、本發(fā)明所采用的技術(shù)方案包括以下步驟：

3、s1圖像采集：機(jī)器人通過astro?pro相機(jī)獲取每幀彩色圖像和深度圖像。此步驟確保了后續(xù)處理中的高精度輸入數(shù)據(jù)，其中深度圖像為后續(xù)定位與路徑規(guī)劃提供了關(guān)鍵的三維信息。

4、s2目標(biāo)檢測：將s1獲取的彩色圖像輸入至yolox模型中進(jìn)行目標(biāo)檢測。yolox模型是一種高效的實(shí)時目標(biāo)檢測算法，能夠在復(fù)雜場景中快速識別目標(biāo)。輸出的視頻幀圖像中包括所有檢測目標(biāo)的區(qū)域坐標(biāo)及對應(yīng)的置信度，從而為多目標(biāo)跟蹤提供基礎(chǔ)數(shù)據(jù)。

5、s3多目標(biāo)跟蹤：將s2輸出的檢測框集合輸入到多目標(biāo)跟蹤方法中進(jìn)行多個目標(biāo)的跟蹤。多目標(biāo)跟蹤算法不僅能跟蹤每個目標(biāo)的位置，還能為每個目標(biāo)分配唯一的id，以確保目標(biāo)在不同幀之間的一致性。輸出包括每個目標(biāo)對應(yīng)的id和各目標(biāo)的定位框，確保機(jī)器人能夠正確識別和區(qū)分跟隨目標(biāo)。

6、s4目標(biāo)定位：將s3獲取的所有跟蹤目標(biāo)，依據(jù)預(yù)定的定位策略，進(jìn)行動態(tài)單目標(biāo)定位。輸出包括單個跟隨目標(biāo)的位置信息，以確定機(jī)器人當(dāng)前要跟隨的目標(biāo)。

7、s5路徑規(guī)劃：首先，將s4獲取的目標(biāo)相對于圖像的位置信息與深度圖像結(jié)合，獲取機(jī)器人相對于目標(biāo)的距離，并將其轉(zhuǎn)換為世界坐標(biāo)系下的位置信息。然后，使用dubins曲線規(guī)劃機(jī)器人與跟蹤目標(biāo)之間的路徑軌跡。dubins曲線是一種考慮最小轉(zhuǎn)彎半徑的路徑規(guī)劃方法，適用于機(jī)器人在有限的運(yùn)動空間內(nèi)進(jìn)行路徑規(guī)劃。

8、s6運(yùn)動控制：根據(jù)s5獲取的跟蹤軌跡，將軌跡輸入至橫縱向控制算法?？刂扑惴▽⒏鶕?jù)目標(biāo)位置的變化動態(tài)調(diào)整機(jī)器人的速度和方向，以確保平滑且穩(wěn)定的跟蹤過程。最終輸出機(jī)器人的三軸運(yùn)動速度，包括前進(jìn)速度、轉(zhuǎn)彎速度和垂直方向速度，以實(shí)現(xiàn)精確的目標(biāo)跟蹤。

9、s7結(jié)束當(dāng)前幀的跟蹤，進(jìn)入下一幀：在完成s6步驟后，機(jī)器人將繼續(xù)處理下一幀的圖像數(shù)據(jù)，重復(fù)s2至s6的步驟。通過這樣的迭代循環(huán)，機(jī)器人能夠持續(xù)跟蹤動態(tài)目標(biāo)，實(shí)現(xiàn)對目標(biāo)的實(shí)時跟隨。

10、進(jìn)一步地，步驟s2中，首先為了將檢測模型部署于jetson?xavier?nx嵌入式平臺，采用了yolox-nano輕量型模型，該模型采用了coco預(yù)訓(xùn)練權(quán)重，最終輸出當(dāng)前幀t的檢測集合zt＝[u,v,s,r]，u,v,s和r分別表示檢測框中心點(diǎn)位置坐標(biāo)，面積和長寬比。為了提升該模型在聚類人群場景中的性能了，我們選用了特點(diǎn)各異的crowdhuman、cityperson和ethz數(shù)據(jù)集。crowdhuman數(shù)據(jù)集以其高密度人群場景中的行人圖像而聞名，這使得算法能夠更好地識別和跟蹤擁擠環(huán)境中的個體。cityperson數(shù)據(jù)集包含了多樣的城市街道場景，強(qiáng)化了模型在交通繁忙背景下的應(yīng)用能力。ethz數(shù)據(jù)集則提供了多角度、不同時間段的行人圖像，加深了模型對行人動態(tài)變化的理解。這些數(shù)據(jù)集的集成旨在提升yolox-nano模型利用coco預(yù)訓(xùn)練權(quán)重后在實(shí)際應(yīng)用中的泛化能力，確保其在現(xiàn)實(shí)場景下的準(zhǔn)確性和魯棒性。

11、進(jìn)一步地，步驟s3中，多目標(biāo)跟蹤的運(yùn)動預(yù)測模型基于卡爾曼濾波器(kf)，該濾波器會更新被跟蹤物體的狀態(tài)。狀態(tài)向量定義為代表位置(u,v)、面積(s)、長寬比(r)以及它們各自的速度分量。在同一幀，yolox檢測器檢測到的位置zt＝[u,v,s,r]被用作觀測向量。

12、假設(shè)物體的運(yùn)動遵循勻速模式，則可以使用下面的卡爾曼預(yù)測方程來估計(jì)幀t的狀態(tài)向量

13、

14、這里，ft代表狀態(tài)轉(zhuǎn)換矩陣，wt是過程噪聲，pt|t-1是預(yù)測誤差協(xié)方差，qt是過程噪聲協(xié)方差矩陣。隨后，yolox探測器根據(jù)跟蹤目標(biāo)zt在幀t的觀測結(jié)果修正先驗(yàn)估計(jì)，并更新狀態(tài)向量

15、

16、這里，ht是觀測矩陣，rt是觀測噪聲協(xié)方差矩陣，kt是卡爾曼增益矩陣。當(dāng)跟蹤的軌跡被遮擋而又沒有觀測更新zt時，狀態(tài)誤差可能會隨著時間的推移而累積，導(dǎo)致kf參數(shù)偏離正確路徑，從而增加預(yù)測誤差。為了減少這種累積誤差，當(dāng)物體被遮擋后被重新檢測到時，我們會回溯其在遮擋期間的狀態(tài)，創(chuàng)建一個虛擬軌跡，并更新kf參數(shù)。假設(shè)物體被遮擋時的最后一次觀測值為而物體被重新檢測到時的第一次觀測值為那么在t1和t2之間的虛擬觀測狀態(tài)zt可以描述為：

17、

18、在這里，沿著虛擬軌跡zt(t1<t<t2)，用于重新計(jì)算卡爾曼濾波器的預(yù)測和更新步驟。

19、在數(shù)據(jù)關(guān)聯(lián)部分，在幀t時，yolox探測器會生成一組檢測框而卡爾曼濾波器會生成一組預(yù)測跟蹤框?qū)τ谌魏谓o定的檢測框和預(yù)測框都可以計(jì)算出一個iou值，作為成本矩陣ciou中的相似性度量：

20、

21、iou成本矩陣是通過計(jì)算兩個方框的交叉面積與結(jié)合面積之比得出的。這種方法因其簡單有效而具有優(yōu)勢，特別是在測量方框之間的重疊時。然而，iou在涉及重疊或遮擋的情況下可能并不有效，尤其是當(dāng)多個物體沿同一直線緊密排列時，因?yàn)樗鼰o法準(zhǔn)確區(qū)分單個物體之間的實(shí)際空間關(guān)系。為了解決這個問題，本節(jié)在iou成本矩陣的基礎(chǔ)上引入了深度成本矩陣cdcm：

22、

23、函數(shù)f可獲取方框底邊的垂直坐標(biāo)，從而更好地比較同一地平面上不同物體的相對深度。在有遮擋或物體聚集的情況下，底邊的深度信息可以作為更穩(wěn)定的特征，因?yàn)榈走呁ǔＴ诓糠终趽鯐r仍然可見，而且遮擋前后的深度值保持一致。

24、隨后，將iou成本矩陣ciou與cdcm結(jié)合起來，可以得到加權(quán)數(shù)據(jù)關(guān)聯(lián)成本矩陣cd：

25、cd＝ciou(i,j)+ωdcm·cdcm(i,j)

26、這里，ωdcm是cdcm矩陣系數(shù)，匈牙利算法通過從成本矩陣cd中選擇最小匹配對來解決分配問題，目標(biāo)是最小化：

27、

28、表示從當(dāng)前幀軌跡集到檢測框zt完美匹配的所有排列組合σ的集合。輸出結(jié)果分別對應(yīng)匹配的激活軌跡、未匹配的軌跡和未匹配的高分方框。對已匹配的軌跡使用卡爾曼濾波器對軌跡參數(shù)進(jìn)行更新并作為結(jié)果輸出，對存在未匹配的軌跡進(jìn)行保留，若存在一定數(shù)量未匹配將其刪除，未匹配的檢測框用于創(chuàng)建新的軌跡并分配一個id號用于下一幀進(jìn)行匹配。

29、進(jìn)一步地，在步驟s4目標(biāo)定位策略，在傳統(tǒng)的單目標(biāo)跟蹤中，通常是在初始幀內(nèi)手動選擇一個跟蹤id。相比之下，采用多目標(biāo)跟蹤技術(shù)的機(jī)器人人員跟蹤的目標(biāo)選擇過程更為靈活。在簡單的跟蹤任務(wù)中，多目標(biāo)跟蹤系統(tǒng)可以自動識別并選擇位于初始幀圖像中心的跟蹤id作為主要跟蹤目標(biāo)。這一選擇過程通過以下公式實(shí)現(xiàn)：

30、

31、這里，z0代表第一幀中的檢測結(jié)果，和是檢測框左右邊界的水平坐標(biāo)，w是圖像的寬度。方框中心(xc,yc)與圖像中心最接近的個體將被識別為跟蹤目標(biāo)。利用rgb-d攝像機(jī)的深度值d，可計(jì)算出跟蹤目標(biāo)相對于機(jī)器人的三維空間坐標(biāo)如下：

32、

33、其中，φ表示攝像機(jī)的水平視場。此外，本節(jié)還采用了mobilenet進(jìn)行手勢檢測，該網(wǎng)絡(luò)已在hagrid數(shù)據(jù)集上進(jìn)行了預(yù)訓(xùn)練，以檢測特定的手勢，具體過程為：機(jī)器人啟動后，機(jī)器人進(jìn)入停止?fàn)顟B(tài)，等待1秒后過渡到搜索狀態(tài)，檢測到人時觸發(fā)α事件，促使機(jī)器人進(jìn)入候選狀態(tài)進(jìn)行行人選擇。如果行人做出β手勢，機(jī)器人就會轉(zhuǎn)入跟隨狀態(tài)，以確定跟隨id并傳輸位置信息。如果當(dāng)前幀中不存在η手勢，就會觸發(fā)β事件，機(jī)器人就會恢復(fù)到搜索狀態(tài)，以確定下一幀的位置。在跟隨狀態(tài)下，如果行人做出τ手勢，或者跟隨目標(biāo)離開攝像機(jī)視角多幀，分別觸發(fā)τ或γ事件，機(jī)器人就會返回搜索狀態(tài)，尋找下一幀的位置。

34、進(jìn)一步地，在步驟s5路徑規(guī)劃求解過程：接受到s4步驟后的跟蹤目標(biāo)的位置信息(x,y,θ)(以自身坐標(biāo)系為參照)后，負(fù)責(zé)生成離散軌跡?？紤]到攝像機(jī)視場和非全局最短路徑問題的限制，本節(jié)采用dubins進(jìn)行路徑規(guī)劃。杜賓斯曲線提供六種基本路徑類型組合(lsl、lsr、rsl、rsr、lrl、rlr)。在對規(guī)劃的曲線進(jìn)行離散化之前，有必要確定最短的路徑組合。為了便于計(jì)算，需要進(jìn)行坐標(biāo)變換，將機(jī)器人的起點(diǎn)設(shè)置為s(0,0,φ)，跟蹤目標(biāo)的終點(diǎn)設(shè)置為g(d,0,ψ)。具體變換公式如下：

35、

36、這里φ、d、ψ和r分別表示機(jī)器人的朝向角度、機(jī)器人到目標(biāo)的歸一化距離、目標(biāo)的朝向角度和機(jī)器人的轉(zhuǎn)彎半徑。例如，在lsl路徑類型中，子路徑長度計(jì)算如下：

37、

38、通過計(jì)算起始旋轉(zhuǎn)角度t、路徑長度p和最終旋轉(zhuǎn)角度q，可以確定lsl路徑組合的長度。然后在六個基本組合中選擇最短的路徑進(jìn)行離散化。離散化是通過插值法完成的，將連續(xù)的杜賓斯路徑分解為一系列離散點(diǎn)。在給定離散步長l和起始插值狀態(tài)(xo,yo,θo)的情況下，每個路徑段的插值公式是：

39、

40、其中為轉(zhuǎn)彎半徑，路徑長度在計(jì)算過程中已歸一化。迭代插值生成一組完整的離散路徑點(diǎn)，直到達(dá)到計(jì)劃的路徑長度。最后，這組點(diǎn)被傳送到路徑跟蹤模塊進(jìn)行進(jìn)一步處理。

41、進(jìn)一步地，在步驟s6機(jī)器人運(yùn)動步驟求解流程：機(jī)器人運(yùn)動控制是基于全向輪式機(jī)器人模型設(shè)計(jì)的，其中橫向和縱向控制模型旨在補(bǔ)償計(jì)劃跟蹤軌跡中的誤差。橫向控制器根據(jù)機(jī)器人當(dāng)前位置與計(jì)劃路徑之間的偏差，使用lqr線性控制器計(jì)算機(jī)器人的角速度ω和橫向速度vy。相反，縱向控制器則根據(jù)機(jī)器人與跟蹤目標(biāo)之間的距離，使用pid控制器計(jì)算機(jī)器人的縱向速度vx。

42、其中，機(jī)器人運(yùn)動控制是基于全向輪式機(jī)器人模型設(shè)計(jì)的，其中橫向和縱向控制模型旨在補(bǔ)償計(jì)劃跟蹤軌跡中的誤差。橫向控制器根據(jù)機(jī)器人當(dāng)前位置與計(jì)劃路徑之間的偏差，使用lqr線性控制器計(jì)算機(jī)器人的角速度ω和橫向速度vy。相反，縱向控制器則根據(jù)機(jī)器人與跟蹤目標(biāo)之間的距離，使用pid控制器計(jì)算機(jī)器人的縱向速度vx。

43、橫向控制系統(tǒng)采用lqr線性控制器來調(diào)整機(jī)器人相對于軌跡的橫向誤差dp和航向誤差θp。這些偏差是通過比較機(jī)器人的實(shí)際位置和它在預(yù)定路徑上的投影來確定的。橫向控制誤差定義為而控制輸入為u＝[vy,ω]t。

44、首先建立了機(jī)器人實(shí)際位置與其在預(yù)定路徑上投影點(diǎn)之間差異的數(shù)學(xué)表示，該模型依賴于大地坐標(biāo)系與規(guī)劃軌跡的幾何關(guān)系。從圖中不難看出航向誤差eθ和其變化率

45、eθ＝θ-θr

46、

47、接著定義了橫向誤差ed作為機(jī)器人當(dāng)前位置向量與規(guī)劃軌跡匹配點(diǎn)位置向量在法線方向上的投影差：

48、

49、隨后，通過對機(jī)器人的位置變化矢量和規(guī)劃軌跡匹配點(diǎn)的位置變化矢量的差值在方向上的投影，以及隨時間的變化率的影響，導(dǎo)出了橫向誤差變化速度

50、

51、這里，機(jī)器人的位置變化矢量被表達(dá)為車身坐標(biāo)系中的速度分量和的線性組合，而規(guī)劃軌跡匹配點(diǎn)的位置變化矢量則考慮了沿著軌跡的速度

52、

53、考慮規(guī)劃曲線的曲率k對法向量變化率的影響，根據(jù)frenet公式有

54、

55、帶入公式，展開并化簡可得:

56、

57、假設(shè)θ-θr為極小值，則橫向誤差變化率為：

58、

59、接著根據(jù)線性狀態(tài)空間模型有橫向控制誤差將橫向控制問題框架化，引入狀態(tài)轉(zhuǎn)移矩陣與控制矩陣并進(jìn)行離散化以形成下一時刻橫向控制誤差et+1的預(yù)測。該模型通過如下矩陣方程表達(dá)：

60、

61、其中橫向控制誤差橫向控制變量u＝[ay,α]t，帶入公式則有

62、

63、因此，狀態(tài)轉(zhuǎn)換矩陣和控制矩陣可以通過幾何關(guān)系確定。通過定義成本函數(shù)j并最小化加權(quán)長期累積誤差與控制努力之和，我們提出了一個離散lqr問題：

64、

65、引入了權(quán)重矩陣q和r，并在給定的動態(tài)約束下，通過應(yīng)用拉格朗日乘數(shù)法最終推到出riccati方程：

66、

67、通過不斷迭代求解p矩陣，接著求解反饋增益矩陣k：

68、

69、最終求解控制矩陣uk:

70、ut＝-ket

71、在整個過程中，橫向控制系統(tǒng)被模擬為一個線性系統(tǒng)，通過調(diào)整控制輸入u來減少誤差e。

72、縱向控制系統(tǒng)使用pid控制器來保持機(jī)器人與跟蹤目標(biāo)之間的預(yù)定距離d。機(jī)器人的縱向速度vx與當(dāng)前跟蹤目標(biāo)深度和設(shè)定距離之間的誤差成正比。縱向速度與誤差項(xiàng)之間的關(guān)系詳見下式：

73、vx＝kp*(d-d)+ki*∫(d-d)dt

74、其中，kp和ki為控制增益，d-d為目標(biāo)當(dāng)前深度與預(yù)定距離之間的誤差，dt為連續(xù)幀之間的時間間隔。