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

(南京理工大學 機械工程學院，南京 210094)

1 引言

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

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

2 硬件平臺

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

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

3 系統(tǒng)軟件處理設計

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

圖2 軟件總體流程框圖

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

3.1 目標檢測

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

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

(1)

由圖3可以看到，計算出的(,)目標檢測框的中心坐標可以直接對齊到深度圖上。

圖3 彩色圖的目標邊框信息和深度圖像素直接對齊圖

3.2 還原目標物在相機視野下的軌跡

目標物在相機坐標下的軌跡點通過D435I相機的射影逆變換計算得到。相機模型的成像過程如圖4所示。

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

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

(2)

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

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

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

(3)

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

(4)

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

(5)

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

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

(6)

通過式(6)即可得到相機參考坐標系下的目標物軌跡點。

3.3 坐標系轉(zhuǎn)換接口設計

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

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

(7)

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

圖6 設備運行時坐標關系圖

ROS自身有static_transform_publisher工具，用以發(fā)布2個參考坐標系之間的靜態(tài)變換，但在本系統(tǒng)方案中，該工具并不適用，因為其不僅會對當前時刻軌跡點做一個坐標變換，而且也會對歷史時刻的軌跡點做坐標變換，導致在世界坐標系下目標物軌跡還原方向錯誤，因而需要自己重新設計坐標轉(zhuǎn)換接口。

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

=

(8)

坐標轉(zhuǎn)換接口程序流程如圖7所示，對應的具體步驟如下：

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

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

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

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

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

3.4 軌跡濾波

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

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

圖8 包含異常點軌跡圖

我們用=[,,]表示時刻的點位置，相鄰時刻的軌跡點間位移可表示為：

(9)

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

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

圖9 軌跡濾波剔除異常值點圖

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

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

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

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

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

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

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

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

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

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

4 實驗驗證與結果分析

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

圖12 實驗場地示意圖

4.1 標定

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

表1 D435I相機標定參數(shù)Table 1 D435I Camera calibration parameters

表2 MYNTEYE相機標定參數(shù)Table 2 MYNTEYE Camera calibration parameters

4.2 神經(jīng)網(wǎng)絡模型訓練

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

圖13 網(wǎng)絡訓練過程曲線

4.3 實驗結果分析

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

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

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

圖14 軌跡點還原三維曲線

5 結論

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