羅豪龍，李建勝，鄒丹平，楊子迪，李廣云

（1 戰(zhàn)略支援部隊(duì)信息工程大學(xué) 地理空間信息學(xué)院，鄭州 450001）

（2 上海交通大學(xué) 上海市北斗導(dǎo)航與位置服務(wù)重點(diǎn)實(shí)驗(yàn)室，上海 200240）

0 引言

光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)是一種用于跟蹤和捕獲目標(biāo)動(dòng)作，并實(shí)時(shí)獲取目標(biāo)6 自由度運(yùn)動(dòng)信息的設(shè)備。隨著室內(nèi)高精度導(dǎo)航定位等技術(shù)領(lǐng)域的飛速發(fā)展，光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)在醫(yī)療康復(fù)、運(yùn)動(dòng)檢測(cè)、目標(biāo)跟蹤、場(chǎng)景動(dòng)畫、虛擬現(xiàn)實(shí)等方面發(fā)揮著越來越重要的作用［1-4］。目前主流的光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)如Vicon 運(yùn)動(dòng)捕捉系統(tǒng)、Optitrack 運(yùn)動(dòng)捕捉系統(tǒng)等標(biāo)稱精度能達(dá)到毫米甚至亞毫米級(jí)，在室內(nèi)導(dǎo)航領(lǐng)域?yàn)闊o人機(jī)、無人車導(dǎo)航感知提供真實(shí)的位置和姿態(tài)信息［5-7］。然而在實(shí)際使用中，由于場(chǎng)地震動(dòng)、運(yùn)動(dòng)捕捉系統(tǒng)的相機(jī)數(shù)量、運(yùn)動(dòng)捕捉系統(tǒng)的空間分布和噪聲等因素的影響，光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的實(shí)際定位精度往往遠(yuǎn)低于廠家的標(biāo)稱精度。尤其是大場(chǎng)景下，受到實(shí)際場(chǎng)地和空間布局的限制，光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的覆蓋范圍大，往往存在相機(jī)分布分散、共視區(qū)域的相機(jī)數(shù)量少、相機(jī)與目標(biāo)距離遠(yuǎn)等諸多實(shí)際問題，導(dǎo)致光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度嚴(yán)重降低。如果光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的實(shí)際定位精度與標(biāo)稱精度相差過大，運(yùn)動(dòng)捕捉系統(tǒng)的性能將大打折扣，難以發(fā)揮運(yùn)動(dòng)捕捉系統(tǒng)的實(shí)際作用。因此，研究如何準(zhǔn)確標(biāo)定實(shí)際場(chǎng)景尤其是大場(chǎng)景下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度，對(duì)于保證光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)高效、準(zhǔn)確、穩(wěn)定的運(yùn)行，根據(jù)實(shí)際場(chǎng)景選擇配套方案的光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)和拓展光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的應(yīng)用場(chǎng)景具有重要價(jià)值和研究意義。

國(guó)外，針對(duì)光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)定位精度的研究主要集中在醫(yī)學(xué)生物領(lǐng)域。AURAND A M 長(zhǎng)時(shí)間多次測(cè)量同一個(gè)點(diǎn)，利用重復(fù)定位精度評(píng)估光學(xué)運(yùn)動(dòng)定位系統(tǒng)的重復(fù)定位精度［8］；VAN DER KRUK E 對(duì)包括運(yùn)動(dòng)捕捉系統(tǒng)、實(shí)時(shí)動(dòng)態(tài)（Real-Time Kinematic，RTK）載波相位差分定位系統(tǒng)、射頻識(shí)別（Radio Frequency Identification，RFID）定位系統(tǒng)等17 種定位系統(tǒng)的研究進(jìn)展進(jìn)行了梳理總結(jié)［9］；WINDOLF M 在180 mm×180 mm×150 mm 小空間下對(duì)Vicon-460 的系統(tǒng)精度進(jìn)行了檢測(cè)，缺點(diǎn)在于檢測(cè)范圍過小，難以反映實(shí)際場(chǎng)景中系統(tǒng)的定位精度［10］。國(guó)內(nèi)，楊治鐸利用地面網(wǎng)格作為真實(shí)值對(duì)光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度開展研究，但是地面網(wǎng)格的精度難以保證［11］；譚菁華利用千分尺通過距離測(cè)量的方式檢測(cè)光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度，存在的問題在于千分尺的測(cè)量范圍太小，不能用于實(shí)際場(chǎng)景中光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的精度檢測(cè)［12］。此外，國(guó)內(nèi)學(xué)者更多是將光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位結(jié)果作為真實(shí)值對(duì)其他定位系統(tǒng)進(jìn)行標(biāo)定［13-14］。如劉智偉利用光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)實(shí)現(xiàn)了超寬帶（Ultra Wide Band，UWB）系統(tǒng)的精度標(biāo)定，但是對(duì)光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)本身的定位精度檢測(cè)缺乏研究［15］。

目前學(xué)者主要在小空間內(nèi)檢測(cè)光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度，且往往采用重復(fù)軌跡精度的方式評(píng)估系統(tǒng)的定位精度，不能真實(shí)反映實(shí)際場(chǎng)景尤其是大場(chǎng)景下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的實(shí)際定位精度［16］。因此，為了獲取實(shí)際場(chǎng)景尤其是大場(chǎng)景下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的實(shí)際定位精度，本文提出一種大場(chǎng)景下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的標(biāo)定方法。利用全站儀和光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)進(jìn)行公共點(diǎn)測(cè)量獲取公共點(diǎn)的真實(shí)值和測(cè)量值，基于羅德里格矩陣的抗差最小二乘迭代方法進(jìn)行公共點(diǎn)轉(zhuǎn)換，實(shí)現(xiàn)光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的精度標(biāo)定。同時(shí)，基于該方法實(shí)現(xiàn)了不同光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)間的外參標(biāo)定。

1 光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)定位原理

1.1 系統(tǒng)構(gòu)成

光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)由多個(gè)發(fā)射紅外光的高速攝像機(jī)組成，光線經(jīng)反光標(biāo)記（marker）球反射紅外光并獲取其二維圖像，利用三維重建原理計(jì)算marker 球的三維空間坐標(biāo)數(shù)據(jù)。多個(gè)marker 球與跟蹤目標(biāo)固連建立剛體，可以實(shí)時(shí)獲取剛體的6 自由度位置和姿態(tài)。本文所用的光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)為英國(guó)Oxford Metrics Limited公司生產(chǎn)的Vicon 光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)，Vicon 光學(xué)運(yùn)動(dòng)捕捉是目前主流的光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)之一，具有低延遲、采樣精度高等優(yōu)點(diǎn)。Vicon 系統(tǒng)包括光學(xué)攝像機(jī)、Vicon Tracker 運(yùn)動(dòng)捕捉軟件、數(shù)據(jù)采集器、信息轉(zhuǎn)換盒、T 型校正架、L 型標(biāo)定直角尺和專用連接線等。鏡頭間采用并聯(lián)連接方式保證單個(gè)鏡頭獨(dú)立工作，確保單鏡頭出現(xiàn)故障時(shí)不影響系統(tǒng)正常工作。某高校大型測(cè)試場(chǎng)布設(shè)的光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)相機(jī)配置如表1 所示。

表1 Vicon 系統(tǒng)相機(jī)配置Table 1 Vicon system camera configuration

1.2 定位原理

Vicon 運(yùn)動(dòng)捕捉系統(tǒng)由多個(gè)相機(jī)組成的定位系統(tǒng)，系統(tǒng)定位原理為立體視覺的三維空間定位原理。圖1所示為世界坐標(biāo)系Ow-XwYwZw、相機(jī)坐標(biāo)系Oc-XcYcZc、圖像坐標(biāo)系Oi-xy和像素坐標(biāo)系O-uv的對(duì)應(yīng)關(guān)系，其中Ow、Oc、Oi和O分別為三個(gè)坐標(biāo)系的原點(diǎn)。

圖1 坐標(biāo)系對(duì)應(yīng)關(guān)系Fig.1 Coordinate system correspondence

圖2 所示為雙目立體視覺的三維空間定位原理。假設(shè)空間三維點(diǎn)Pw（xw，yw，zw）在左右目相機(jī)的像素坐標(biāo)系下坐標(biāo)分別為（u1，v1）和（ur，vr）。根據(jù)坐標(biāo)系間的轉(zhuǎn)換關(guān)系［17］，有

圖2 雙目立體視覺的三維空間定位原理Fig.2 Three-dimensional spatial positioning principle based on binocular stereo vision

式中，Κl，Kr為左右目相機(jī)內(nèi)參，Tl，Tr為世界坐標(biāo)系下左右目相機(jī)系的位姿，zl，zr表示點(diǎn)在左右目相機(jī)坐標(biāo)系下的Z軸方向坐標(biāo)。根據(jù)式（1）、（2），可以解算空間點(diǎn)Pw的三維位置坐標(biāo)。

2 光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)標(biāo)定原理

本文利用全站儀進(jìn)行光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的精度標(biāo)定。測(cè)量原理為全站儀和光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)測(cè)量若干公共點(diǎn)，全站儀的測(cè)量結(jié)果作為真實(shí)值，光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的測(cè)量結(jié)果作為測(cè)量值，通過公共點(diǎn)轉(zhuǎn)換的方式實(shí)現(xiàn)二者系統(tǒng)坐標(biāo)系的統(tǒng)一，并計(jì)算光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的實(shí)際定位精度。光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度標(biāo)定原理如圖3 所示。

圖3 光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)標(biāo)定原理Fig.3 Calibration principle of optical motion capture system

2.1 全站儀測(cè)量原理

全站儀是一種由電子經(jīng)緯儀、光電測(cè)距儀和數(shù)據(jù)處理系統(tǒng)所組成的高精度測(cè)量?jī)x器，它將光學(xué)、機(jī)械和電子技術(shù)相結(jié)合，集成水平角、豎直角、距離和高差等多個(gè)測(cè)量功能于一身。全站儀采用光學(xué)測(cè)量原理，利用激光束對(duì)目標(biāo)點(diǎn)進(jìn)行測(cè)量，通過接收反射回來的信號(hào)，測(cè)量目標(biāo)點(diǎn)與全站儀之間的水平角度、垂直角度和距離等信息，并借助內(nèi)部軟件計(jì)算出目標(biāo)點(diǎn)的三維坐標(biāo)。目前，全站儀廣泛應(yīng)用于地形測(cè)量、礦山勘探、變形監(jiān)測(cè)等多個(gè)領(lǐng)域。本文所用的全站儀為L(zhǎng)eica MS60 全站儀，Leica MS60 具有測(cè)量、掃描和自動(dòng)照準(zhǔn)的功能，技術(shù)參數(shù)如表2 所示。

表2 全站儀技術(shù)參數(shù)Table 2 Technical parameters of total station

全站儀的測(cè)量原理如圖4 所示。全站儀記錄水平角和垂直角和測(cè)站點(diǎn)到目標(biāo)點(diǎn)的距離S，根據(jù)測(cè)量的水平角α和垂直角θ，計(jì)算得到目標(biāo)點(diǎn)的三維坐標(biāo)為

圖4 全站儀測(cè)量原理Fig.4 Measuring principle of total station

2.2 公共點(diǎn)測(cè)量原理

光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的測(cè)量區(qū)域均勻選取若干測(cè)量點(diǎn)作為公共點(diǎn)，全站儀和光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)分別對(duì)公共點(diǎn)進(jìn)行觀測(cè)，光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)測(cè)量的公共點(diǎn)坐標(biāo)作為觀測(cè)值，全站儀測(cè)量的公共點(diǎn)坐標(biāo)作為真實(shí)值，通過坐標(biāo)系統(tǒng)一并計(jì)算光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的實(shí)際定位精度。然而，由于光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)測(cè)量的為maker 球球心，全站儀測(cè)量的是棱鏡幾何中心，二者測(cè)量的點(diǎn)非共心，即非公共點(diǎn)。因此，如何選取合適的測(cè)量裝置，實(shí)現(xiàn)兩個(gè)系統(tǒng)的公共點(diǎn)統(tǒng)一是測(cè)量的難點(diǎn)。為解決測(cè)量點(diǎn)不一致的情況，利用Leica 系列半徑為19.05 mm的CCR1.5 靶球和同尺寸半球形攝影測(cè)量反光工件分別替代全站儀的常規(guī)棱鏡和光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的marker 球。由于攝影測(cè)量反光工件和全站儀靶球半徑相同，且攝影測(cè)量反光工件球心和全站儀靶球球心重合，因此全站儀測(cè)量的靶球球心和光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)測(cè)量的攝影測(cè)量反光工件球心是相同測(cè)量點(diǎn)，達(dá)到了兩套系統(tǒng)測(cè)量公共點(diǎn)的目的。公共點(diǎn)測(cè)量裝置如圖5 所示，圖5（a）為光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的常用的反光marker 球，圖5（b）為全站儀棱鏡，圖5（c）為半球形攝影測(cè)量反光工件，圖5（d）為全站儀靶球。

圖5 公共點(diǎn)測(cè)量裝置Fig.5 Device for measuring common points

圖6 所示為公共點(diǎn)布設(shè)方案，根據(jù)光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的相機(jī)空間分布在四個(gè)分區(qū)域分別均勻布設(shè)若干公共點(diǎn)。公共點(diǎn)的測(cè)量過程為：1）全站儀靶球放置在公共點(diǎn)A位置處，全站儀測(cè)量公共點(diǎn)A的位置坐標(biāo)；2）基座保持不動(dòng)，半球形攝影測(cè)量反光工件替代全站儀靶球，光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)測(cè)量公共點(diǎn)A的位置坐標(biāo)；3）移動(dòng)靶球到公共點(diǎn)B處，全站儀與光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)依次測(cè)量公共點(diǎn)B的位置坐標(biāo)；4）每個(gè)公共點(diǎn)測(cè)量三次取平均值，循環(huán)依次測(cè)量所有公共點(diǎn)空間位置。公共點(diǎn)測(cè)量原理如圖7 所示。

圖6 公共點(diǎn)布設(shè)方案Fig.6 Common point layout scheme

圖7 公共點(diǎn)測(cè)量原理Fig.7 Principle of measuring common points

2.3 坐標(biāo)系轉(zhuǎn)換原理

由于全站儀和光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的測(cè)量結(jié)果分別在全站儀坐標(biāo)系和運(yùn)動(dòng)捕捉系統(tǒng)坐標(biāo)系，二者坐標(biāo)系并不統(tǒng)一，無法將全站儀的測(cè)量結(jié)果與運(yùn)動(dòng)捕捉系統(tǒng)的測(cè)量結(jié)果直接計(jì)算。因此，需要實(shí)現(xiàn)全站儀坐標(biāo)系和運(yùn)動(dòng)捕捉系統(tǒng)坐標(biāo)系的統(tǒng)一，即確定運(yùn)動(dòng)捕捉系統(tǒng)坐標(biāo)系到全站儀坐標(biāo)系的位姿變換矩陣。

羅德里格矩陣是由三個(gè)獨(dú)立的元素組成一個(gè)矩陣，避免了象限判斷的問題，利用羅德里格矩陣求解坐標(biāo)系轉(zhuǎn)換矩陣具有計(jì)算簡(jiǎn)單和精度高等優(yōu)點(diǎn)［18-19］。此外，引入抗差的方法盡可能減少測(cè)量粗差對(duì)計(jì)算結(jié)果的影響，提高坐標(biāo)系轉(zhuǎn)換精度［20-21］。因此采用基于羅德里格矩陣的抗差最小二乘迭代方法求解坐標(biāo)系轉(zhuǎn)換矩陣，即運(yùn)動(dòng)捕捉系統(tǒng)坐標(biāo)系到全站儀坐標(biāo)系的位姿變換矩陣。羅德里格矩陣的定義為

1）引入反對(duì)稱矩陣S，其中元素a，b，c相互獨(dú)立。

2）定義I為三階單位陣，則R=(I+S)(I-S)-1正交矩陣。將R展開后得到

假設(shè)某一公共點(diǎn)在全站儀坐標(biāo)系和運(yùn)動(dòng)捕捉系統(tǒng)坐標(biāo)下的坐標(biāo)分別為(xi，yi，zi)T和(ui，vi，wi)T，建立二者的坐標(biāo)系轉(zhuǎn)換關(guān)系為

式中，R為由羅德里格矩陣表示的旋轉(zhuǎn)矩陣，T為平移矩陣。

公共點(diǎn)作差得到

式（7）同時(shí)左乘(I-S)化簡(jiǎn)得到

令kij=ki-kj，式（8）化簡(jiǎn)為

將I、S代入，經(jīng)整理得到

對(duì)于Ax=B的形式中，由于A為奇異陣，因此至少2 個(gè)方程組才能解算出a，b，c。因此，當(dāng)有n個(gè)公共點(diǎn)時(shí)，可以得到一個(gè)3(n-1)×3 維的方程組

通過最小二乘原理計(jì)算未知數(shù)初值a0，b0，c0，根據(jù)式（5）、（6）得到旋轉(zhuǎn)矩陣的初始值R0和平移矩陣的初始值T0。

由式（5）、（6）得到

線性化得到

由此得到誤差方程為

其中

為避免測(cè)量粗差對(duì)參數(shù)估計(jì)的影響，引入基于IGG3 的抗差估計(jì)進(jìn)行最小二乘迭代計(jì)算［22］?；贗GG3的抗差估計(jì)方法可以處理非線性、異方差等復(fù)雜情況的數(shù)據(jù)，并具有良好的魯棒性和計(jì)算效率。該方法通過對(duì)每次觀測(cè)賦予權(quán)重，并在迭代過程不斷調(diào)整權(quán)重和估計(jì)參數(shù)，以減少異常值對(duì)結(jié)果的影響，達(dá)到抗差的目的。

由式（14）得到

Pi為每次觀測(cè)的權(quán)因子系數(shù)，取為1。根據(jù)IGG3 中的定義，wi的形式為

式中，k0一般取1.0～1.5，k1一般取3.0～4.5，v為殘差量vi與標(biāo)準(zhǔn)化殘差的比值σvi。其中，標(biāo)準(zhǔn)化殘差的計(jì)算方程為

式中，σv0為單位權(quán)中誤差，qvi為協(xié)方差矩陣對(duì)角線上的第i個(gè)元素。單位權(quán)方差的計(jì)算公式為

式中，V表示誤差項(xiàng)，P表示權(quán)重矩陣，m表示方程組的行數(shù)3(n-1)，t表示待估計(jì)的參數(shù)個(gè)數(shù)，取為6。

利用抗差最小二乘迭代算法計(jì)算旋轉(zhuǎn)矩陣參數(shù)a，b，c和平移矩陣參數(shù)Tx，，Ty，Tz，解算得到光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)坐標(biāo)系到全站儀坐標(biāo)系的旋轉(zhuǎn)矩陣R和平移矩陣T。

2.4 光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)精度檢測(cè)

根據(jù)2.3 中的方法得到光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)坐標(biāo)系到全站儀坐標(biāo)系的位姿變換矩陣和，利用式（19）將光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的測(cè)量值PV轉(zhuǎn)換到全站儀坐標(biāo)系下得到Pl，通過式（20）與全站儀測(cè)量的真實(shí)值PL作差得到光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)在每個(gè)公共點(diǎn)X、Y、Z三軸方向上的測(cè)量誤差，根據(jù)式（21）計(jì)算光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的點(diǎn)位均方根誤差。

3 動(dòng)捕系統(tǒng)外參標(biāo)定方法

Vicon 運(yùn)動(dòng)捕捉系統(tǒng)和度量運(yùn)動(dòng)捕捉系統(tǒng)（NOKOV）是出自不同廠家且相互獨(dú)立的光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)。兩個(gè)系統(tǒng)的空間布局如圖8 所示，兩個(gè)系統(tǒng)的相機(jī)非共視。

圖8 Vicon/度量系統(tǒng)空間分布Fig.8 Spatial distribution of Vicon system and NOKOV system

由于室內(nèi)無人機(jī)導(dǎo)航感知的任務(wù)要求，需要獲取兩套系統(tǒng)的外參，即實(shí)現(xiàn)兩套運(yùn)動(dòng)捕捉坐標(biāo)系的坐標(biāo)系統(tǒng)一。由于度量系統(tǒng)和Vicon 系統(tǒng)的測(cè)量區(qū)域非共視，因此無法直接測(cè)量公共點(diǎn)計(jì)算兩套系統(tǒng)的外參。采取的方式為利用全站儀建立統(tǒng)一坐標(biāo)系的方法，即全站儀固定不動(dòng)，利用全站儀分別在度量運(yùn)動(dòng)捕捉系統(tǒng)與Vicon 運(yùn)動(dòng)捕捉系統(tǒng)的觀測(cè)區(qū)域測(cè)量若干公共點(diǎn)，利用2.3 中的坐標(biāo)系轉(zhuǎn)換方法分別計(jì)算Vicon 運(yùn)動(dòng)捕捉系統(tǒng)到全站儀坐標(biāo)系的位姿變換矩陣和度量運(yùn)動(dòng)捕捉系統(tǒng)的位姿變換矩陣，進(jìn)而計(jì)算Vicon 動(dòng)捕系統(tǒng)和度量動(dòng)捕系統(tǒng)的位姿變換矩陣，實(shí)現(xiàn)兩套系統(tǒng)的外參標(biāo)定。

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

圖9 所示為某高校大型測(cè)試場(chǎng)，測(cè)試場(chǎng)的空間約為12 m×40 m×8 m。測(cè)試場(chǎng)布設(shè)一套由34 個(gè)相機(jī)鏡頭組成的Vicon 光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)，鏡頭均勻分布在測(cè)試場(chǎng)的四個(gè)平行區(qū)域，相鄰區(qū)域間包含若干個(gè)具有共視區(qū)域的相機(jī)，整個(gè)測(cè)試場(chǎng)組成一套完整的Vicon 運(yùn)動(dòng)捕捉系統(tǒng)。Vicon 系統(tǒng)軟件中的俯視示意圖如圖10所示。在室內(nèi)無人機(jī)避障導(dǎo)航、車機(jī)協(xié)同定位、多機(jī)協(xié)同作業(yè)等實(shí)際任務(wù)中，要求Vicon 光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)為目標(biāo)提供可靠的高精度定位坐標(biāo)。一方面Vicon 光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)為目標(biāo)提供高精度定位坐標(biāo)輔助目標(biāo)執(zhí)行導(dǎo)航避障等任務(wù)；另一方面Vicon 光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)為目標(biāo)提供高精度定位真值以驗(yàn)證定位導(dǎo)航的算法精度。然而，大場(chǎng)景下受Vicon 光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)相機(jī)分布、相機(jī)數(shù)量以及場(chǎng)地振動(dòng)等因素的影響，Vicon系統(tǒng)的實(shí)際定位精度與廠家標(biāo)稱的定位精度往往不符。因此，需要檢驗(yàn)大場(chǎng)景下Vicon 運(yùn)動(dòng)捕捉系統(tǒng)的實(shí)際定位精度。為標(biāo)定光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度，采用分區(qū)域標(biāo)定和整體區(qū)域標(biāo)定的方案，即分別計(jì)算四個(gè)分區(qū)域下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度和整體區(qū)域下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度。

圖9 Vicon 系統(tǒng)測(cè)試場(chǎng)Fig.9 Vicon system test field

圖10 Vicon 系統(tǒng)俯視圖Fig.10 Vertical view of vicon system

根據(jù)Vicon 光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的分布，在Vicon 系統(tǒng)布設(shè)的四個(gè)區(qū)域分別均勻選取10 個(gè)空間點(diǎn)，Leica MS60 和Vicon 系統(tǒng)分別測(cè)量各區(qū)域10 個(gè)公共點(diǎn)的真實(shí)值和測(cè)量值，利用分區(qū)域公共點(diǎn)計(jì)算各分區(qū)域下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度，利用所有公共點(diǎn)計(jì)算得到整體區(qū)域下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度。圖11 表示Vicon 系統(tǒng)標(biāo)定實(shí)驗(yàn)，右側(cè)上下插圖分別表示全站儀靶球?qū)嶒?yàn)裝置和半球形攝影測(cè)量反光工件實(shí)驗(yàn)裝置，圖12～16 表示系統(tǒng)定位精度結(jié)果。

圖11 Vicon 系統(tǒng)標(biāo)定試驗(yàn)Fig.11 Process of TCP calibration

圖12 區(qū)域一VICON 系統(tǒng)定位精度Fig.12 Vicon positioning accuracy in the first area

圖13 區(qū)域二Vicon 系統(tǒng)定位精度Fig.13 Vicon positioning accuracy in the second area

圖14 區(qū)域三Vicon 系統(tǒng)定位精度Fig.14 Vicon positioning accuracy in the third area

圖15 區(qū)域四Vicon 系統(tǒng)定位精度Fig.15 Vicon positioning accuracy in the fourth area

圖16 整體區(qū)域Vicon 系統(tǒng)定位精度Fig.16 Vicon positioning accuracy in the whole area

從圖12～16 中可以看出

1）各分區(qū)域下系統(tǒng)在X、Y、Z軸方向的定位精度多數(shù)在2 mm 以內(nèi)，表明各分區(qū)域下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度較高。

2）整體區(qū)域下系統(tǒng)在X軸方向和Z軸方向的定位精度多數(shù)在5 mm 以內(nèi)；Y軸方向的定位精度較差，部分點(diǎn)在Y軸方向的誤差超過10 mm。結(jié)合圖6 和圖10 分析，原因在于全站儀架站位置位于測(cè)試場(chǎng)中心附近，且測(cè)試場(chǎng)的長(zhǎng)邊與全站儀的Y軸近似平行。而整體區(qū)域是由沿Y軸方向的四個(gè)分區(qū)域組成，且靠近測(cè)試場(chǎng)中間區(qū)域共視相機(jī)的數(shù)量多，測(cè)試場(chǎng)邊緣共視相機(jī)的數(shù)量少。因此，Y軸誤差較大且呈現(xiàn)沿測(cè)試場(chǎng)中心向邊緣逐漸增大的趨勢(shì)。

3）整體區(qū)域下動(dòng)捕系統(tǒng)的定位精度明顯低于各區(qū)域下動(dòng)捕系統(tǒng)的定位精度。原因在于整體區(qū)域由四個(gè)分區(qū)域的相機(jī)組成，區(qū)域間具有共視區(qū)域的相機(jī)數(shù)量少，相機(jī)與目標(biāo)的距離遠(yuǎn)，以及場(chǎng)景振動(dòng)等因素的影響，導(dǎo)致整體區(qū)域下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的實(shí)際定位精度嚴(yán)重降低。

經(jīng)過計(jì)算得到，四個(gè)分區(qū)域和整體區(qū)域下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位均方根誤差（Root Mean Square Error，RMSE）如表3 所示。

表3 系統(tǒng)定位精度Table 3 Target position

從表3 中可以看出：

1）區(qū)域一、區(qū)域二、區(qū)域三、區(qū)域四的點(diǎn)位RMSE 分別為2.385 mm、0.877 mm、1.787 mm 和2.890 mm。經(jīng)分析區(qū)域一和區(qū)域四位于測(cè)試場(chǎng)邊緣，區(qū)域二和區(qū)域三靠近測(cè)試場(chǎng)中心，因此區(qū)域二和區(qū)域三可觀測(cè)到的相機(jī)數(shù)量多于區(qū)域一和區(qū)域四，因此定位精度稍優(yōu)于區(qū)域一和區(qū)域四。

2）整體區(qū)域的點(diǎn)位RMSE 為8.126 mm，相較于各分區(qū)域下的定位精度明顯降低。原因在于整體區(qū)域由四個(gè)分區(qū)域組成，且相鄰分區(qū)域的共視相機(jī)數(shù)量較少，因此，當(dāng)所有相機(jī)組成一個(gè)整體的視覺運(yùn)動(dòng)捕捉系統(tǒng)時(shí)，受相機(jī)數(shù)量和相機(jī)分布等因素的影響，整體區(qū)域下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度明顯降低。

實(shí)驗(yàn)二是Vicon 光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)和度量運(yùn)動(dòng)捕捉系統(tǒng)的外參標(biāo)定。由于兩個(gè)系統(tǒng)之間的相機(jī)非共視，因此如何實(shí)現(xiàn)系統(tǒng)間的外參標(biāo)定是面臨的難題。基于Vicon 系統(tǒng)精度標(biāo)定的思路，利用全站儀分別進(jìn)行Vicon 區(qū)域和度量區(qū)域下的公共點(diǎn)測(cè)量，分別計(jì)算Vicon 坐標(biāo)系到全站儀坐標(biāo)系的坐標(biāo)系矩陣和度量坐標(biāo)系全站儀坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣，最后計(jì)算度量系統(tǒng)到Vicon 系統(tǒng)的坐標(biāo)系轉(zhuǎn)換矩陣，實(shí)現(xiàn)Vicon 系統(tǒng)和度量系統(tǒng)的外參標(biāo)定。圖17 所示為度量系統(tǒng)的相機(jī)分布，Vicon 系統(tǒng)的相機(jī)分布如圖9 所示，圖18 為度量/Vicon系統(tǒng)外參標(biāo)定實(shí)驗(yàn)。

圖18 Vicon/度量系統(tǒng)外參標(biāo)定實(shí)驗(yàn)Fig.18 External parameter calibration experiment between Vicon system and NOKOV system

實(shí)驗(yàn)過程中，利用全站儀在Vicon 系統(tǒng)的測(cè)量區(qū)域采集和度量系統(tǒng)的測(cè)量區(qū)域分別采集10 個(gè)公共點(diǎn)和15 個(gè)公共點(diǎn)。經(jīng)過計(jì)算，得到Vicon 系統(tǒng)與全站儀坐標(biāo)系的坐標(biāo)變換矩陣為

度量系統(tǒng)與全站儀坐標(biāo)系的坐標(biāo)變換矩陣為

最終計(jì)算得到度量系統(tǒng)到Vicon 系統(tǒng)的坐標(biāo)系轉(zhuǎn)換矩陣為

由此，度量系統(tǒng)與Vicon 系統(tǒng)的外參標(biāo)定完成。

5 結(jié)論

提出大場(chǎng)景下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度標(biāo)定方法。利用同尺寸同心靶球解決全站儀和運(yùn)動(dòng)捕捉系統(tǒng)測(cè)量點(diǎn)不一致的問題，基于羅德里格矩陣的抗差最小二乘迭代計(jì)算坐標(biāo)系轉(zhuǎn)換矩陣，針對(duì)大場(chǎng)景下Vicon 系統(tǒng)的相機(jī)空間分布提出分別計(jì)算分區(qū)域和整體區(qū)域下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度方法。實(shí)驗(yàn)結(jié)果表明，大場(chǎng)景下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的定位精度明顯降低，系統(tǒng)在X軸、Y軸、Z軸和點(diǎn)位精度的RMSE 分別為1.393 mm、7.606 mm、2.496 mm 和8.126 mm。說明大場(chǎng)景下受相機(jī)分布、相機(jī)數(shù)量、場(chǎng)景振動(dòng)等因素的影響，光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的實(shí)際定位精度可能嚴(yán)重降低。并利用該方法實(shí)現(xiàn)了Vicon 系統(tǒng)和度量系統(tǒng)的外參標(biāo)定。驗(yàn)證了提出的大場(chǎng)景光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)精度標(biāo)定方法的有效性和可行性。隨著對(duì)光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的應(yīng)用需求和定位精度要求不斷提高，提出的方法對(duì)未來大場(chǎng)景、復(fù)雜場(chǎng)景下光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)的標(biāo)定工作具有一定的啟發(fā)和參考意義。