基于三維視覺的動(dòng)態(tài)目標(biāo)軌跡還原方案設(shè)計(jì)

王 任，管紅根，王茂森，戴勁松，袁志龍

(南京理工大學(xué) 機(jī)械工程學(xué)院，南京 210094)

1 引言

動(dòng)態(tài)目標(biāo)的運(yùn)動(dòng)軌跡蘊(yùn)含著速度與空間位置等重要信息，在多種場(chǎng)景中有著重要的應(yīng)用前景。如在醫(yī)學(xué)領(lǐng)域需要測(cè)量病人的肢體運(yùn)動(dòng)軌跡以用于病理分析，在無人機(jī)導(dǎo)航問題中需要實(shí)時(shí)獲取動(dòng)態(tài)障礙物的運(yùn)動(dòng)軌跡?，F(xiàn)有的軌跡方法追蹤往往局限于指定單類目標(biāo)的追蹤，限制了其應(yīng)用的場(chǎng)景。文獻(xiàn)[2]中基于卡爾曼濾波算法對(duì)行人進(jìn)行跟蹤，得到了二維圖像坐標(biāo)系下的行人運(yùn)動(dòng)軌跡。但該方法得到的是行人像素點(diǎn)的運(yùn)動(dòng)軌跡，不具有行人實(shí)際的運(yùn)動(dòng)距離尺度信息。文獻(xiàn)[3]通過背景差分、閾值分割的方法追蹤魚類，并通過單目攝像頭還原魚兒的運(yùn)動(dòng)軌跡，由于單目相機(jī)本身存在缺乏和實(shí)際軌跡對(duì)應(yīng)的尺度信息，因此該方案也無法直接得到追蹤目標(biāo)物的具有客觀運(yùn)動(dòng)尺度大小的軌跡。此外，通過背景差分和閾值分割追蹤的目標(biāo)穩(wěn)定性較差，在光照變化或者有其他動(dòng)態(tài)目標(biāo)干擾時(shí)容易失效。文獻(xiàn)[4-5]利用激光雷達(dá)產(chǎn)生的點(diǎn)云進(jìn)行動(dòng)態(tài)目標(biāo)的軌跡檢測(cè)追蹤，但激光雷達(dá)價(jià)格昂貴，而且計(jì)算量較大，無法在計(jì)算性能有限的移動(dòng)設(shè)備上實(shí)時(shí)運(yùn)行。文獻(xiàn)[6]通過MEMS傳感器對(duì)室內(nèi)的消防員定位，借助人的運(yùn)動(dòng)特征改善了傳感器本身的漂移缺陷。文獻(xiàn)[7]使用單一GPS傳感器對(duì)進(jìn)行室內(nèi)外的機(jī)器人定位與軌跡追蹤，但在室內(nèi)環(huán)境，GPS信號(hào)往往很差，存在很大定位誤差。文獻(xiàn)[8]通過YOLO、SSD、Faster RCNN等3種深度學(xué)習(xí)目標(biāo)檢測(cè)算法對(duì)網(wǎng)球進(jìn)行軌跡追蹤，但前提是網(wǎng)球起點(diǎn)位置已知并且擊打到指定高度，從而獲得網(wǎng)球的實(shí)際運(yùn)動(dòng)軌跡尺度大小信息。為了避免傳統(tǒng)基于人工特征的目標(biāo)檢測(cè)算法的普適性和準(zhǔn)確性缺陷，本文使用基于卷積神經(jīng)網(wǎng)絡(luò)的目標(biāo)檢測(cè)識(shí)別算法，對(duì)目標(biāo)物進(jìn)行分離并在二維圖像中定位?？梢宰⒁獾剑诋?dāng)前眾多的軌跡還原方案中，主要存在目標(biāo)檢測(cè)種類單一、準(zhǔn)確性低以及不能得到目標(biāo)物實(shí)際運(yùn)動(dòng)尺度大小等缺點(diǎn)。

本文主要針對(duì)動(dòng)態(tài)單個(gè)目標(biāo)的三維軌跡還原問題，提出基于視覺的解決方案。采用視覺神經(jīng)網(wǎng)絡(luò)算法對(duì)目標(biāo)進(jìn)行檢測(cè)識(shí)別，設(shè)計(jì)坐標(biāo)系轉(zhuǎn)換接口將目標(biāo)物在二維像素中的位置轉(zhuǎn)換到世界參考坐標(biāo)系中。在計(jì)算能力有限的移動(dòng)設(shè)備上部署，程序運(yùn)行采用有序多線程，充分利用設(shè)備資源以保證實(shí)時(shí)性。并以籃球?yàn)槔M(jìn)行實(shí)驗(yàn)驗(yàn)證，具有良好的實(shí)時(shí)性與準(zhǔn)確性。對(duì)無人機(jī)導(dǎo)航以及肢體軌跡追蹤等場(chǎng)景具有工程實(shí)際應(yīng)用借鑒意義。

2 硬件平臺(tái)

本方案基于Ubuntu系統(tǒng)，有關(guān)硬件如圖1所示。核心計(jì)算板卡采用盈馳的微型主機(jī)，主控芯片為主頻2.3 GHz的酷睿I5系列CPU。具有四核心八線程，無獨(dú)立顯卡。傳感器包含Intel的D435I實(shí)感相機(jī)和小覓智能的MYNTEYES1030相機(jī)。D435I用于檢測(cè)捕捉目標(biāo)物在相機(jī)視野下的軌跡。D435I相機(jī)，最遠(yuǎn)捕捉距離為10 m，在戶外陽光下也可以使用。D435I可以輸出像素對(duì)齊的彩色圖和深度圖，且具有圖像畸變校正功能。分辨率為640×480，幀率最高可達(dá)90 FPS。另一個(gè)相機(jī)為小覓智能的MYNTEYE相機(jī)，用于運(yùn)行視覺慣性里程計(jì)定位算法，可以將相機(jī)視野下的軌跡關(guān)聯(lián)到世界坐標(biāo)系。該相機(jī)內(nèi)置六軸IMU傳感器，與雙目相機(jī)圖像保持硬件時(shí)間同步，2個(gè)攝像頭的分辨率均為750×480，幀率為30 FPS，IMU頻率高達(dá)1 000 Hz，滿足SLAM算法對(duì)穩(wěn)定定位時(shí)的圖像和IMU條件要求。

圖1 系統(tǒng)硬件圖

3 系統(tǒng)軟件處理設(shè)計(jì)

本方案采用ROS框架，總體流程如圖2所示。主要由目標(biāo)檢測(cè)，還原目標(biāo)物在相機(jī)坐標(biāo)系下的軌跡，視覺慣性里程計(jì)將相機(jī)坐標(biāo)系和世界坐標(biāo)系關(guān)聯(lián)，軌跡濾波等4個(gè)部分組成。

圖2 軟件總體流程框圖

為了實(shí)現(xiàn)在計(jì)算資源有限的嵌入式平臺(tái)實(shí)現(xiàn)動(dòng)態(tài)目標(biāo)軌跡還原，綜合考慮性能和精度，本方案主要利用了2種方法：基于YOLO的目標(biāo)檢測(cè)方法和基于視覺慣性里程計(jì)的方法。

3.1 目標(biāo)檢測(cè)

還原目標(biāo)物在相機(jī)參考坐標(biāo)系下的軌跡需先獲取目標(biāo)物在彩色圖像中的位置信息，通過對(duì)象檢測(cè)的方法實(shí)現(xiàn)。對(duì)象檢測(cè)需要確定目標(biāo)在圖像中的位置和類別，目前主要有2種方法：第1種以HOG+SVM為代表的傳統(tǒng)檢測(cè)方法；第2種為基于深度學(xué)習(xí)的方法。第2種方法又可以細(xì)分為以RCNN為代表的2階段檢測(cè)器和以YOLO為代表的基于回歸問題的單級(jí)檢測(cè)器。盡管RCNN算法準(zhǔn)確，但比較復(fù)雜、速度很慢，不能滿足在計(jì)算能力有限的平臺(tái)上實(shí)時(shí)工作。相比2階段檢測(cè)法，YOLO系列算法不需要候選區(qū)域，通過端到端的方法對(duì)目標(biāo)分類識(shí)別，速度更快。因此本方案采用速度更快的YOLO方法。

由于要部署在微型主機(jī)上，沒有獨(dú)立GPU計(jì)算單元，因而考慮采用NCNN推理框架。NCNN是騰訊優(yōu)圖于2017年推出的高性能神經(jīng)網(wǎng)絡(luò)前向計(jì)算框架，可不依賴GPU運(yùn)行。該推理框架為實(shí)現(xiàn)在移動(dòng)端部署做了專門的優(yōu)化，支持多核并行計(jì)算加速，優(yōu)化內(nèi)存，直接內(nèi)存零拷貝引用加載網(wǎng)絡(luò)模型。NCNN支持YOLO網(wǎng)絡(luò)模型。YOLO智能卷積神經(jīng)網(wǎng)絡(luò)誕生至今已有多個(gè)版本的迭代，本方案采用YOLO-Fastest版本的網(wǎng)絡(luò)模型算法。該算法對(duì)單目標(biāo)檢測(cè)做了針對(duì)性優(yōu)化，適合部署在計(jì)算能力有限的平臺(tái)。NCNN推理YOLO-Fastest網(wǎng)絡(luò)模型得到的目標(biāo)邊界框由5個(gè)預(yù)測(cè)要素組成，分別是邊框左邊界、邊框右邊界、邊框上邊界、邊框下邊界和置信度。彩色圖的目標(biāo)邊框信息和深度圖如圖3所示。我們?nèi)【匦芜吙蛑行狞c(diǎn)作為軌跡還原的中心點(diǎn)。該中心點(diǎn)像素坐標(biāo)為(,)，其計(jì)算公式為：

(1)

由圖3可以看到，計(jì)算出的(,)目標(biāo)檢測(cè)框的中心坐標(biāo)可以直接對(duì)齊到深度圖上。

圖3 彩色圖的目標(biāo)邊框信息和深度圖像素直接對(duì)齊圖

3.2 還原目標(biāo)物在相機(jī)視野下的軌跡

目標(biāo)物在相機(jī)坐標(biāo)下的軌跡點(diǎn)通過D435I相機(jī)的射影逆變換計(jì)算得到。相機(jī)模型的成像過程如圖4所示。

圖4 針孔成像模型過程示意圖

設(shè)為---相機(jī)坐標(biāo)系，坐標(biāo)系原點(diǎn)為相機(jī)光心。相機(jī)坐標(biāo)系中軸正向指向相機(jī)正前方，軸正向垂直于軸豎直向下，軸正向指向相機(jī)右端。設(shè)空間點(diǎn)為[,,]，其中為空間點(diǎn)到相機(jī)光心的距離也即深度。點(diǎn)穿過相機(jī)光心并在物理成像平面上形成點(diǎn)′為[′,′,′]。和′之間的坐標(biāo)變換根據(jù)圖5中的相似三角形關(guān)系可得。

(2)

式(2)中：為焦距，是光心到物理成像平面的距離；為了表示倒立成像，公式添加了負(fù)號(hào)。

圖5 投影相似三角形示意圖

式(2)反映的是空間點(diǎn)和其成像點(diǎn)′之間的坐標(biāo)關(guān)系，但本方案的實(shí)際操作都是先以像素為單位進(jìn)行的。需要將物理成像平面上的點(diǎn)轉(zhuǎn)換到像素平面上。像素坐標(biāo)系定義為--，為圖片左上角原點(diǎn)，和軸分別與軸和軸方向相同。因此需要將物理成像平面上的點(diǎn)進(jìn)行平移縮放，才能得到--中對(duì)應(yīng)的點(diǎn)。設(shè)為軸的縮放系數(shù)，為軸的縮放系數(shù)，[,]為原點(diǎn)平移量。則坐標(biāo)系的旋轉(zhuǎn)縮放可以表示為：

(3)

式(3)中，和的單位為像素m，表示單位距離上的像素個(gè)數(shù)。將相機(jī)的焦距轉(zhuǎn)化為在，方向上的像素度量表示。則可令=，=。代入式(3)可得：

(4)

式(4)中，的單位為m，和的單位為像素。將式(4)寫成齊次坐標(biāo)的形式為：

(5)

式(5)中,矩陣稱為內(nèi)參矩陣。通過該內(nèi)參矩陣可以將物理世界中的點(diǎn)投影到相機(jī)坐標(biāo)系。

在本方案系統(tǒng)中，[,,1]為從公式得到的已知量，空間點(diǎn)為目標(biāo)量，對(duì)式(5)變換得到:

(6)

通過式(6)即可得到相機(jī)參考坐標(biāo)系下的目標(biāo)物軌跡點(diǎn)。

3.3 坐標(biāo)系轉(zhuǎn)換接口設(shè)計(jì)

經(jīng)過3.1和3.2節(jié)，可以得到相機(jī)固定不動(dòng)時(shí)目標(biāo)物在相機(jī)視野下的軌跡。當(dāng)目標(biāo)物運(yùn)動(dòng)范圍過大，超出了D435I相機(jī)固定時(shí)的視野范圍時(shí)，我們需要相機(jī)跟隨目標(biāo)物運(yùn)動(dòng)，以保持目標(biāo)物始終在相機(jī)視野范圍內(nèi)，形成對(duì)目標(biāo)物軌跡的完整捕捉。為了得到相機(jī)運(yùn)動(dòng)時(shí)目標(biāo)物在世界參考坐標(biāo)系下的軌跡，考慮對(duì)相機(jī)本身的運(yùn)動(dòng)軌跡進(jìn)行定位。本方案采用視覺慣性里程計(jì)解決相機(jī)本身的軌跡定位問題。現(xiàn)有的視覺里程計(jì)主要分為2類：第1類以O(shè)RB-SLAM和VINS-MONO為代表的基于優(yōu)化的方法;第2類以MSCKF為代表的基于濾波器的方法。前者通常精度更高但對(duì)設(shè)備的計(jì)算性能要求也更高。本方案最終要在微型移動(dòng)設(shè)備平臺(tái)落地，因此主要考慮基于濾波器的S-MSCKF方法。

為了將相機(jī)坐標(biāo)系下的目標(biāo)物軌跡還原到世界坐標(biāo)系，我們需要對(duì)相機(jī)坐標(biāo)系和里程計(jì)坐標(biāo)系固連，相機(jī)坐標(biāo)系和S-MSCKF里程計(jì)坐標(biāo)系之間的固連通過旋轉(zhuǎn)平移實(shí)現(xiàn)。旋轉(zhuǎn)平移矩陣可以反映這層變換關(guān)系。數(shù)學(xué)上用3×3的方陣表示旋轉(zhuǎn)，3×1的向量表示平移。將二者合并，可以寫成4×4的齊次方陣，即：

(7)

設(shè)備運(yùn)行時(shí)坐標(biāo)關(guān)系如圖6所示。在進(jìn)行坐標(biāo)系變換之前需要先明確本系統(tǒng)所涉及的3個(gè)坐標(biāo)系：① 以設(shè)備運(yùn)行時(shí)的起始出發(fā)點(diǎn)作為坐標(biāo)原點(diǎn)的全局坐標(biāo)系，在圖6中記為world；② S-MSCKF追蹤系統(tǒng)設(shè)備本身運(yùn)動(dòng)的里程計(jì)參考坐標(biāo)系，在圖6中記為odom；③ 和里程計(jì)坐標(biāo)系之間存在固定的旋轉(zhuǎn)平移關(guān)系的相機(jī)參考坐標(biāo)系，在圖6中記yolo_traj_d435。

圖6 設(shè)備運(yùn)行時(shí)坐標(biāo)關(guān)系圖

ROS自身有static_transform_publisher工具，用以發(fā)布2個(gè)參考坐標(biāo)系之間的靜態(tài)變換，但在本系統(tǒng)方案中，該工具并不適用，因?yàn)槠洳粌H會(huì)對(duì)當(dāng)前時(shí)刻軌跡點(diǎn)做一個(gè)坐標(biāo)變換，而且也會(huì)對(duì)歷史時(shí)刻的軌跡點(diǎn)做坐標(biāo)變換，導(dǎo)致在世界坐標(biāo)系下目標(biāo)物軌跡還原方向錯(cuò)誤，因而需要自己重新設(shè)計(jì)坐標(biāo)轉(zhuǎn)換接口。

令里程計(jì)坐標(biāo)系用來表示，固定參考系用表示，相機(jī)參考系用表示，則從相機(jī)參考系下的點(diǎn)轉(zhuǎn)變到世界參考坐標(biāo)系下的點(diǎn)公式依據(jù)為：

=

(8)

坐標(biāo)轉(zhuǎn)換接口程序流程如圖7所示，對(duì)應(yīng)的具體步驟如下：

圖7 坐標(biāo)系轉(zhuǎn)換接口程序流程界面

1在S-MSCKF的msckf_vio.cpp源文件中，我們用Eigen::Isometry3d表示式(7)的旋轉(zhuǎn)平移矩陣。式(8)中表示從D435I相機(jī)坐標(biāo)系到里程計(jì)坐標(biāo)系的變換。程序中用T_i_yolo表示。

2式(8)中表示從里程計(jì)坐標(biāo)系到全局參考坐標(biāo)系的旋轉(zhuǎn)平移變換，通過S-MSCKF的T_i_w獲得。T_i_w*T_i_yolo得到D435I相機(jī)坐標(biāo)在世界參考系下的表示。

3在createRosIO()中設(shè)置personOdomCallback()話題回調(diào)函數(shù)，接收來自yolodect進(jìn)程發(fā)布的相機(jī)視野下的目標(biāo)點(diǎn)位置話題/odom_traj，對(duì)應(yīng)式(8)中。

4T_yolo2w*d435point_translation得到式(8)中，也即全局參考坐標(biāo)系下的目標(biāo)物軌跡點(diǎn)。

3.4 軌跡濾波

實(shí)際測(cè)試中發(fā)現(xiàn)，設(shè)備還原的軌跡存在少量的無效點(diǎn)。這些無效點(diǎn)，視覺上體現(xiàn)為軌跡的位移突變。必須剔除這些無用的軌跡點(diǎn)。

經(jīng)過多次實(shí)際運(yùn)行發(fā)現(xiàn)，這些異常點(diǎn)通常只有當(dāng)目標(biāo)物處于相機(jī)視野邊緣且運(yùn)動(dòng)速度較快時(shí)候出現(xiàn)。而系統(tǒng)正常還原軌跡的前提是保證NCNN推理出的目標(biāo)物邊框中心點(diǎn)對(duì)齊到深度圖像的像素點(diǎn)正確。當(dāng)目標(biāo)物處于相機(jī)視野邊緣，由于目標(biāo)物的不完整以及運(yùn)動(dòng)速度較快不利于實(shí)時(shí)解算，因此很可能發(fā)生目標(biāo)物中心點(diǎn)計(jì)算錯(cuò)誤或者計(jì)算卡頓，導(dǎo)致NCNN計(jì)算出的邊框中心點(diǎn)比深度圖像中對(duì)應(yīng)的像素點(diǎn)位置滯后，從而導(dǎo)致如圖8所示的異常值點(diǎn)。

圖8 包含異常點(diǎn)軌跡圖

我們用=[,,]表示時(shí)刻的點(diǎn)位置，相鄰時(shí)刻的軌跡點(diǎn)間位移可表示為：

(9)

根據(jù)實(shí)際測(cè)試經(jīng)驗(yàn)，在設(shè)備穩(wěn)定還原估計(jì)速度范圍內(nèi)，目標(biāo)物單次處于相機(jī)圖像邊緣位置時(shí)的異常點(diǎn)不超過3個(gè)。為了應(yīng)對(duì)異常軌跡點(diǎn)更多的極端不利情況，我們維護(hù)一個(gè)從時(shí)刻開始的元素為Δ的5元數(shù)組{Δ,Δ+1,Δ+2,Δ+3,Δ+4}。

當(dāng)Δ≥，為目標(biāo)物外接長方體中最短棱長，此處對(duì)應(yīng)為籃球直徑，將Δ存入數(shù)組，依次計(jì)算之后的4個(gè)Δ。當(dāng)異常軌跡點(diǎn)數(shù)量越多，中的元素大于的就越多，最不利情況下，中所有元素都大于，因此只要判定+5時(shí)刻Δ+5≤是否成立，其中為尺寸系數(shù)，需要根據(jù)實(shí)際目標(biāo)物尺寸測(cè)試調(diào)整，對(duì)于籃球，取=0.6。當(dāng)Δ+5≤成立，軌跡在正常速度捕捉范圍內(nèi)，便可以剔除中Δ≥的點(diǎn)。當(dāng)Δ+5>時(shí)，則目標(biāo)物運(yùn)動(dòng)狀態(tài)超出本設(shè)備速度捕捉能力范圍。試驗(yàn)者手持籃球并走出一個(gè)近似字母q的軌跡形狀，在ROS中顯示如圖8所示，可以看到有幾處偏離實(shí)際軌跡的異常點(diǎn)。實(shí)際濾波效果后的軌跡圖如圖9所示。

圖9 軌跡濾波剔除異常值點(diǎn)圖

3.5 數(shù)據(jù)傳輸任務(wù)多線程設(shè)計(jì)

動(dòng)態(tài)目標(biāo)軌跡還原的實(shí)時(shí)性是本方案力求保證的一個(gè)重要指標(biāo)。保證這一指標(biāo)的核心是協(xié)同處理好系統(tǒng)的數(shù)據(jù)傳輸。如何合理配置讀取攝像頭數(shù)據(jù)并處理的任務(wù)是本文研究的重點(diǎn)。

本方案設(shè)計(jì)yolodetect進(jìn)程處理D435I數(shù)據(jù)，并通過ROS話題與S-MSCKF進(jìn)程實(shí)現(xiàn)數(shù)據(jù)交互。對(duì)于簡(jiǎn)單的數(shù)據(jù)通信，通常只需要一個(gè)話題發(fā)布者和一個(gè)話題接收回調(diào)函數(shù)。此種單線程的方式，各個(gè)回調(diào)函數(shù)以串行的方式進(jìn)行，若某個(gè)圖像回調(diào)函數(shù)的處理時(shí)間過長，會(huì)導(dǎo)致其他的回調(diào)函數(shù)被阻塞，極易造成數(shù)據(jù)來不及處理而被新數(shù)據(jù)覆蓋丟失問題。多任務(wù)未經(jīng)設(shè)計(jì)安排的系統(tǒng)資源使用情況如圖10所示，其中yolodetect進(jìn)程CPU使用率為36%。

圖10 改進(jìn)前系統(tǒng)資源使用界面

針對(duì)yolodect進(jìn)程的性能表現(xiàn)，作出如下改進(jìn)：

1) 分別為彩色、深度圖像處理以及接收坐標(biāo)轉(zhuǎn)換后的軌跡點(diǎn)數(shù)據(jù)顯示開辟任務(wù)回調(diào)線程imageCallback()、depthCallback()、yolo_trajCallback()；

2) 將NCNN解算目標(biāo)物程序段位置從main()函數(shù)的while()循環(huán)處理改為在imageCallback()線程中，形成一幀彩色圖像到來銜接一次解算，避免了NCNN的無效或重復(fù)解算；

3) 當(dāng)彩色圖像幀的目標(biāo)物位置未解算完成，而新一幀的深度圖像到來時(shí)，depthCallback()會(huì)被調(diào)用。為了保證depthCallback()線程調(diào)用時(shí)，對(duì)應(yīng)時(shí)刻的目標(biāo)物位置已經(jīng)解算出來，我們采用unordered_map容器數(shù)據(jù)結(jié)構(gòu)關(guān)聯(lián)標(biāo)記每一幀彩色圖像時(shí)間戳對(duì)應(yīng)的目標(biāo)物位置是否解算完成，若未解算完成，則退出depthCallback()，避免做無用功。

由圖11可知，由于采用了有序的多線程，yolodetect進(jìn)程的總CPU使用率降低了24%，獲得了顯著的性能提升。內(nèi)存則從30 MiB升至55.4 MiB。這是因?yàn)閥olodetect進(jìn)程中并行有序處理著彩色圖像和深度圖像，內(nèi)存稍有上升。其中第一個(gè)nodelet進(jìn)程由于增加了3.3節(jié)的坐標(biāo)系轉(zhuǎn)換接口任務(wù)，CPU使用率相比S-MSCKF源程序有所增加。大幅度性能提升將更有利于本方案系統(tǒng)部署在無人機(jī)、移動(dòng)機(jī)器人等計(jì)算資源有限的實(shí)際工程應(yīng)用中。

圖11 改進(jìn)后系統(tǒng)資源使用界面

4 實(shí)驗(yàn)驗(yàn)證與結(jié)果分析

為了驗(yàn)證前述動(dòng)態(tài)目標(biāo)追蹤的效果，進(jìn)行如下實(shí)驗(yàn)。本次實(shí)驗(yàn)在南京理工大學(xué)319幢一樓的走廊和樓道之間實(shí)施。實(shí)驗(yàn)場(chǎng)地如圖12所示?；@球沿著紅色真值點(diǎn)線路放置，共18個(gè)放置點(diǎn)。本方案系統(tǒng)的雙目相機(jī)MYNT-EYE用于運(yùn)行S-MSCKF并對(duì)設(shè)備本身定位，深度相機(jī)D435I用于捕捉籃球的離散化軌跡。為了保持籃球運(yùn)動(dòng)軌跡的估計(jì)值和實(shí)際位置的時(shí)間戳一致性，將籃球置于每個(gè)路徑點(diǎn)較長時(shí)間。利用ROS系統(tǒng)的錄制包功能，記錄下設(shè)備對(duì)籃球的放置軌跡點(diǎn)還原數(shù)據(jù)。

圖12 實(shí)驗(yàn)場(chǎng)地示意圖

4.1 標(biāo)定

軌跡還原中的射影變換需要獲取相機(jī)內(nèi)參，相機(jī)內(nèi)參通過Kalibr工具標(biāo)定獲得。標(biāo)定完成后獲得參數(shù)如表1、表2所示。

表1 D435I相機(jī)標(biāo)定參數(shù)Table 1 D435I Camera calibration parameters

表2 MYNTEYE相機(jī)標(biāo)定參數(shù)Table 2 MYNTEYE Camera calibration parameters

4.2 神經(jīng)網(wǎng)絡(luò)模型訓(xùn)練

為了識(shí)別籃球目標(biāo)，需預(yù)先對(duì)籃球圖片進(jìn)行訓(xùn)練，訓(xùn)練基于Darknet框架。當(dāng)損失函數(shù)值趨于平穩(wěn)不變時(shí)，則認(rèn)為網(wǎng)絡(luò)模型收斂。訓(xùn)練過程如圖13所示。

圖13 網(wǎng)絡(luò)訓(xùn)練過程曲線

4.3 實(shí)驗(yàn)結(jié)果分析

軌跡還原誤差評(píng)定參數(shù)如表3所示。

從表3計(jì)算結(jié)果參數(shù)來看，對(duì)于籃球在真值總長8.1 m路徑上的軌跡點(diǎn)還原，最小誤差為0.073 m。平均誤差為0.156 m，為籃球直徑的63.4%，最大誤差為0.305 m，結(jié)合圖14三維估計(jì)點(diǎn)的分布情況可知，在轉(zhuǎn)彎處由于S-MSCKF產(chǎn)生了漂移，導(dǎo)致誤差較大，以至后面的軌跡點(diǎn)整體誤差也變大。在紋理更為豐富的場(chǎng)景中，視覺慣性里程計(jì)的漂移會(huì)有所減少。

表3 軌跡還原誤差評(píng)定參數(shù)(m)Table 3 Evaluation parameters of trajectory restoration error

圖14 軌跡點(diǎn)還原三維曲線

5 結(jié)論

針對(duì)動(dòng)態(tài)目標(biāo)的三維軌跡還原問題，設(shè)計(jì)了基于三維視覺和深度學(xué)習(xí)并能部署在計(jì)算資源有限的移動(dòng)平臺(tái)上的方案。在坐標(biāo)系轉(zhuǎn)換方面，設(shè)計(jì)了坐標(biāo)轉(zhuǎn)換接口，克服了ROS自帶的坐標(biāo)轉(zhuǎn)換工具的缺陷。引入NCNN和視覺慣性里程計(jì)實(shí)現(xiàn)無需相機(jī)固定且具有實(shí)際客觀尺度信息的動(dòng)態(tài)目標(biāo)物軌跡還原功能。對(duì)于軌跡還原過程中的異常軌跡點(diǎn)進(jìn)行判定剔除，從而得到更符合目標(biāo)物實(shí)際運(yùn)動(dòng)的軌跡。設(shè)計(jì)了有序的多線程任務(wù)，降低了方案的CPU資源使用率，方便部署在計(jì)算資源有限的平臺(tái)上。實(shí)驗(yàn)表明，本文設(shè)計(jì)的動(dòng)態(tài)目標(biāo)軌跡還原系統(tǒng)可以對(duì)經(jīng)過神經(jīng)網(wǎng)絡(luò)訓(xùn)練的目標(biāo)物進(jìn)行軌跡還原，可以達(dá)到30 FPS的實(shí)時(shí)效果，并在ROS中直觀可視化顯示其軌跡，實(shí)現(xiàn)了在計(jì)算能力有限的平臺(tái)實(shí)時(shí)捕捉三維軌跡的應(yīng)用需求。