蔡嘉暉，王紅偉，王漪夢

(1.北方工業(yè)大學(xué) 機(jī)械與材料工程學(xué)院，北京 100041； 2.北京航天測控技術(shù)有限公司，北京 100041)

0 引言

運動捕捉技術(shù)是通過傳感器獲取運動物體的運動學(xué)參數(shù)信息，近年來，運動捕捉技術(shù)取得了突飛猛進(jìn)的發(fā)展，逐漸成為社會的研究熱點，商業(yè)化的運動捕捉設(shè)備相繼被推向市場，其已被廣泛應(yīng)用于數(shù)字化保護(hù)、游戲、動畫、人體工程學(xué)研究、模擬訓(xùn)練、虛擬現(xiàn)實等研究領(lǐng)域[1]。運動捕捉技術(shù)分為機(jī)械式運動捕捉、聲學(xué)式運動捕捉、電磁式運動捕捉、光學(xué)式運動捕捉和基于視頻的運動捕捉，其中光學(xué)式運動捕捉系統(tǒng)具有使用方便，運動受限較小，采樣速率高等優(yōu)點[2-3]，能滿足多數(shù)的高速運動捕捉需求。

光學(xué)式運動捕捉系統(tǒng)的原理是通過紅外高速攝像機(jī)確定被測量對象上布置的被動反光或主動發(fā)光的標(biāo)記點在整個運動過程中的瞬時位置，從而進(jìn)行下一步的運動分析[4-5]。光學(xué)式運動捕捉技術(shù)應(yīng)用廣泛，如人體運動分析[6-7]、飛行器的設(shè)計的控制技術(shù)[8-9]和機(jī)器人誤差補(bǔ)償和控制技術(shù)[10]等。在相關(guān)研究過程中，研究者需要根據(jù)研究對象設(shè)計標(biāo)記點的布置方法及其計算公式從而獲得物體的運動數(shù)據(jù)，然而，根據(jù)特定對象設(shè)計的運動數(shù)據(jù)捕捉方法不具有通用性，無法應(yīng)用于其他研究對象的數(shù)據(jù)捕捉過程中。因此，在基于光學(xué)運動捕捉系統(tǒng)下的相關(guān)研究中，研究者在其前期的標(biāo)記點布置和運動數(shù)據(jù)解算過程中花費了大量的時間和精力，不利于后續(xù)的研究進(jìn)展，降低了研究效率。

針以上情況，本文設(shè)計了一種光學(xué)式運動捕捉系統(tǒng)下的物體通用運動數(shù)據(jù)捕捉方法，能有效捕捉到不同物體的運動數(shù)據(jù)，簡化了物體運動分析研究中的數(shù)據(jù)獲取步驟，為后續(xù)的運動分析和控制研究提供了數(shù)據(jù)基礎(chǔ)。本文設(shè)計的運動數(shù)據(jù)捕捉方法包括標(biāo)記點的布置模型和運動數(shù)據(jù)的計算公式兩部分。首先標(biāo)記點模型由3個標(biāo)記點組成，在物體運動過程中能有效捕捉到標(biāo)記點的位置信息，然后設(shè)計了一種物體的位姿計算公式，根據(jù)標(biāo)記坐標(biāo)計算出物體對應(yīng)的位置和姿態(tài)。同時由于測量系統(tǒng)的測量誤差及測量環(huán)境中的噪音會對測量結(jié)果產(chǎn)生一定的影響，因此設(shè)計了一種卡爾曼濾波方法消除了誤差的影響，得到較為平滑的物體運動曲線。最后再根據(jù)濾波后的運動數(shù)據(jù)計算出物體在運動過程中對應(yīng)的速度、角速度、角速度和角加速度。最后根據(jù)本題設(shè)計的物體運動數(shù)據(jù)捕捉方法進(jìn)行了機(jī)器人的末端運動捕捉實驗，從而驗證本文提出物體的運動數(shù)據(jù)捕捉方法的正確性。

1 物體位姿捕捉

光學(xué)式運動捕捉系統(tǒng)基于計算機(jī)視覺原理，由多個高速攝像機(jī)從不同角度對同一標(biāo)記點的拍攝圖像來確定這一點在三維空間的位置信息。攝像機(jī)組以足夠高的速率連續(xù)拍攝運動物體，得到圖像序列中標(biāo)記點的運動軌跡，通過目標(biāo)物體上標(biāo)記點的監(jiān)視和跟蹤來完成運動捕捉任務(wù)。

由于運動捕捉系統(tǒng)只能獲得標(biāo)記點的空間坐標(biāo)，因此運動物體的位姿數(shù)據(jù)需要通過捕捉特定排列的標(biāo)記點后分析計算獲得。由此本文提出了一種針對剛性物體的通用位姿測量方法，其標(biāo)記點粘貼模型由3個不共線的標(biāo)記點組成，為了方便計算，同時提高測量精度，標(biāo)記點排列采用如圖1所示的直角三角形方式。

圖1 標(biāo)記點模型及其零位坐標(biāo)系

(1)

(2)

由于標(biāo)記點粘貼位置不確定，標(biāo)記點所構(gòu)向量的模長存在過大或過小問題，不利于后續(xù)的旋轉(zhuǎn)矩陣求解計算，對計算結(jié)果可能造成較大的誤差影響，因此需要對以上向量進(jìn)行標(biāo)準(zhǔn)化處理，得到其單位向量。

(3)

記初始狀態(tài)下的標(biāo)準(zhǔn)化后的標(biāo)記點單位向量集合為A，運動狀態(tài)下的標(biāo)記點單位向量集合為B，即:

A=[αnorm1αnorm2αnorm3]

B=[βnorm1βnorm2βnorm3]

(4)

設(shè)在運動狀態(tài)下物體的標(biāo)記點坐標(biāo)系相對與零點坐標(biāo)系的旋轉(zhuǎn)矩陣為R，根據(jù)旋轉(zhuǎn)后向量β等于旋轉(zhuǎn)前向量α乘旋轉(zhuǎn)矩陣R，可得：

βi=Rαi,i=1,2,3

(5)

根據(jù)式(5)可得：

(6)

即：

R=BA-1

(7)

物體在運動捕捉過程中的空間位置可看作為3個標(biāo)記點的幾何中心，由此可得到在初始狀態(tài)下的物體位置坐標(biāo)PZ為：

(8)

設(shè)在運動狀態(tài)下物體的相對于初始狀態(tài)的位置坐標(biāo)為P，根據(jù)當(dāng)前標(biāo)記點位置何物體的零位坐標(biāo)PZ可得：

(9)

通過式(1)可求得的運動狀態(tài)下物體坐標(biāo)系相對與初始狀態(tài)下零點坐標(biāo)系的相對坐標(biāo)為P，通過式(2)可求得運動狀態(tài)下物體坐標(biāo)系相對與初始狀態(tài)下零點坐標(biāo)系的旋轉(zhuǎn)矩陣R，由此可得到物體運動過程中的運動坐標(biāo)系相對于零點坐標(biāo)系的齊次坐標(biāo)為：

(10)

2 物體的運動參數(shù)計算

2.1 速度、加速度計算

光學(xué)式運動捕捉系統(tǒng)通過高速攝像機(jī)捕物體的運動信息，其捕捉頻率與相機(jī)的幀數(shù)有關(guān)，幀數(shù)越高對應(yīng)的捕捉數(shù)據(jù)越多，通常情況下一幀對應(yīng)一組物體標(biāo)記點坐標(biāo)數(shù)據(jù)。設(shè)相機(jī)的捕捉幀數(shù)為F，可知每組數(shù)據(jù)之間采集時間間隔t=1/F，由于高速攝像機(jī)的拍攝幀數(shù)通常大于一百甚至幾百幀，其采集時間間隔極短，一般為幾毫秒，因此在每個時間間隔中物體的運動可近似看成勻加速運動。

圖2 X軸物體運動示意圖

物體在X軸的運動如圖2所示，3個數(shù)據(jù)采集點的時間間隔相等，根據(jù)勻加速運動物體的位移公式可知：

(11)

(12)

(13)

同理，把物體的Y軸和Z軸運動數(shù)據(jù)代入到上述公式中，可求得物體的Y軸向速度和加速度為：

(14)

物體的Z軸向速度和加速度為：

(15)

設(shè)物體在各軸的初始速度v0=0，根據(jù)式(12)～(15)可逐個遞推出物體在整個運動過程中各采集點的速度v和加速度a(速度v和加速度a均為矢量)。

2.2 角速度、角加速度計算

物體的姿態(tài)描述一般用歐拉角表示，歐拉角是用來確定定點轉(zhuǎn)動剛體位置的一組獨立角參量，由章動角ψ、旋進(jìn)角ψ、自轉(zhuǎn)角θ組成。歐拉角的定義很多，在工業(yè)標(biāo)準(zhǔn)中一般采用Z-X-Y轉(zhuǎn)角系統(tǒng)，設(shè)物體運動坐標(biāo)系{B}與初始狀態(tài)下的零位坐標(biāo)系為{A}重合，現(xiàn)將物體運動坐標(biāo)系{B}先繞ZA旋轉(zhuǎn)φ，然后繞YA軸旋轉(zhuǎn)ψ，最后繞XA軸旋轉(zhuǎn)θ，可得到運動坐標(biāo)系{B}相對于零位坐標(biāo)系{A}的旋轉(zhuǎn)矩陣為：

R(φ,θ,ψ)=Rz(φ)Ry(θ)Rx(ψ)=

(16)

根據(jù)式(16)的物體旋轉(zhuǎn)矩陣公式，可以求出物體在該旋轉(zhuǎn)矩陣下對應(yīng)姿態(tài)的歐拉角φ、θ、ψ，旋轉(zhuǎn)矩陣到歐拉角的計算公式如式(17)所示：

(17)

同理，由于物理運動過程中相鄰兩個采集點的時間間隔極短，因此在每個采集點之間的物體轉(zhuǎn)動可看成勻加速轉(zhuǎn)動。在物體的第i采集點到i+1個采集點的繞X軸旋的轉(zhuǎn)示意圖如圖3所示。

圖3 物體章動角轉(zhuǎn)動示意圖

根據(jù)物體的旋轉(zhuǎn)運動示意圖，可知物體第i個采集點到第i+1個采集點的旋轉(zhuǎn)角度的計算公式為：

(18)

(19)

已知勻加速轉(zhuǎn)動物體的初始角速度和角加速度，可知物體在下一時刻的角速度為：

(20)

同理，把物體的繞Y軸的旋進(jìn)角ψ和繞Z軸的自轉(zhuǎn)角θ轉(zhuǎn)動數(shù)據(jù)代入到上述公式中，可求得物體旋進(jìn)角ψ的角速度和角加速度為：

(21)

物體自轉(zhuǎn)角θ的角速度和角加速度為：

(22)

3 卡爾曼濾波

由于運動捕捉系統(tǒng)自身存在一定的測量誤差，同時由于環(huán)境光的強(qiáng)弱和場景中的鏡面反射都會對測量結(jié)果造成一定的誤差影響?？柭鼮V波是一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀察數(shù)據(jù)，對系統(tǒng)狀態(tài)進(jìn)行最有估計的算法，因此，本文捕捉設(shè)計了一種針對物體運動捕捉的卡爾曼濾波方法，能有效消除數(shù)據(jù)捕捉過程中的噪音和干擾的影響，得到相對平滑的物體運動捕捉數(shù)據(jù)曲線。

卡爾曼濾波分為預(yù)測和更新兩個階段，其預(yù)測階段的時間更新方程為：

(24)

其更新階段的測量更新方程為：

(25)

在本題中，需要對物體捕捉獲得的位置坐標(biāo)和姿態(tài)角分別進(jìn)行卡爾曼濾波，因為各捕捉點之間的時間間隔為幾到幾十毫秒，因此在每個捕捉點之間物體可看作做勻速運動，因此，可以得到物體的位置預(yù)測值為：

(26)

同理物體的姿態(tài)角預(yù)測值為：

(27)

最后，通過把運動捕捉系統(tǒng)捕捉的物體位姿測量值和式(26)和式(27)獲得的位置和姿態(tài)角預(yù)測值代入標(biāo)準(zhǔn)卡爾曼濾波方程中，可以得到濾波后的物體的運動數(shù)據(jù)曲線。

4 實驗測試與分析

4.1 運動數(shù)據(jù)捕捉模型測試系統(tǒng)

為了測試所設(shè)計的物體運動數(shù)據(jù)捕捉模型的正確性，進(jìn)行了物體運動捕捉測試系統(tǒng)的搭建。測試系統(tǒng)組成如圖4所示。本題采用Vicon公司的多相機(jī)運動捕捉系統(tǒng)，其拍攝幀數(shù)為100幀，空間定位誤差為0.1 mm。通過把模型粘貼在運動中的協(xié)作機(jī)器人的末端關(guān)節(jié)上獲取的標(biāo)記點的坐標(biāo)信息，然后根據(jù)采集的標(biāo)記點坐標(biāo)數(shù)據(jù)計算出機(jī)器人末端關(guān)節(jié)的運動參數(shù)，對比機(jī)器人自身的運動參數(shù)從而驗證模型算法的正確性，實驗測試系統(tǒng)如圖5所示。

圖4 測試系統(tǒng)組成框圖

圖5 物體運動捕捉實驗平臺

4.2 實驗測試過程

在機(jī)器人末端關(guān)節(jié)上粘貼如圖6所示的標(biāo)記點。在機(jī)器人示教器上設(shè)定5個運動路徑點，控制機(jī)器人依次經(jīng)過并在每個路徑點停留1 s， 然后通過多相機(jī)運動捕捉系統(tǒng)捕捉機(jī)器人上標(biāo)記點的坐標(biāo)信息，代入到上述的運動數(shù)據(jù)求解算法中，計算出機(jī)器人末端關(guān)節(jié)的運動位姿變化軌跡，及其運動過程中的速度、加速度、角速度、角加速度。

圖6 物體運動捕捉標(biāo)記點模型

在機(jī)器人運動過程中，記錄下經(jīng)過5個路徑點時機(jī)器人示教器上的空間位置和姿態(tài)，并通過運動捕捉獲取的標(biāo)記點坐標(biāo)計算出機(jī)器人末端位姿，與示教器下機(jī)器人末端位姿進(jìn)行對比，其結(jié)果如表1所示，其中各路徑點數(shù)據(jù)中上行為示教器數(shù)據(jù)，下行為運動捕捉計算數(shù)據(jù)。從表可以看出，在同一路徑點，通過標(biāo)記點坐標(biāo)計算出的機(jī)器人末端位姿與機(jī)器人示教器上的末端位姿一致，其誤差小于1%，驗證了上述本題設(shè)計的機(jī)器人位姿解算算法的正確性。

表1 路徑點位姿數(shù)據(jù)對比

4.3 測試數(shù)據(jù)分析

通過物體的運動捕捉實驗可以獲得機(jī)器人末端標(biāo)記點的運動軌跡，同時，為了消除運動捕捉系統(tǒng)誤差和外界的噪音影響，對捕捉的運動數(shù)據(jù)進(jìn)行卡爾曼濾波處理得到濾波后的標(biāo)記點坐標(biāo)數(shù)據(jù)，最后代入位姿計算公式中從而得到機(jī)器人的末端位姿運動軌跡，其結(jié)果如圖7所示。

圖7 機(jī)器人末端運動數(shù)據(jù)的卡爾曼濾波

從圖7中可以看出，機(jī)器人在啟動和停止階段的運動數(shù)據(jù)存在較大的波動，其不利于后續(xù)的數(shù)據(jù)計算與分析。而通過卡爾曼濾波處理可以消除噪音與系統(tǒng)誤差的影響，在卡爾曼濾波結(jié)果的局部放大圖8中，可以看出通過卡爾曼濾波處理后，有效的消除了數(shù)據(jù)的波動范圍，平滑了物體運動數(shù)據(jù)曲線，對后續(xù)的數(shù)據(jù)分析提供了良好的數(shù)據(jù)基礎(chǔ)。

圖8 濾波數(shù)據(jù)局部放大圖

把通過濾波后的物體運動數(shù)據(jù)代入上面的速度和角速度計算公式中，求解出物體在運動過程中X、Y、Z軸的速度和角速度分量，其結(jié)果如圖9所示，可以看出在機(jī)器人啟動和停止階段速度在零位附近有來回波動，這與機(jī)器人啟動時發(fā)生的顫動一致。

圖9 機(jī)器人末端速度和角速度

最后根據(jù)計算出的物體各軸向的速度和角速度曲線從而求解出物體的加速度和角加速度，其結(jié)果如圖10所示。

圖10 機(jī)器人末端加速度和角加速度

通過上面的實驗數(shù)據(jù)分析結(jié)果，可以看出本題設(shè)計的物體運動數(shù)據(jù)捕捉模型能有效的獲得物體在運動過程中的運動信息，包括物體的位置姿態(tài)、速度加速度和角速度角加速度的時間變化曲線。同時，在機(jī)器人的設(shè)定路徑點處，捕捉數(shù)據(jù)與機(jī)器人的真實位姿誤差小于1%，通過實驗捕捉計算得到的機(jī)器人各運動數(shù)據(jù)曲線與機(jī)器人自身的運動數(shù)據(jù)曲線一致，從而驗證了本文所提出的物體通用運動數(shù)據(jù)捕捉方法的正確性。

5 結(jié)束語

本文基于光學(xué)式運動捕捉系統(tǒng)設(shè)計了一種物體的通用運動數(shù)據(jù)捕捉方法，能有效捕捉不同被測物體的運動數(shù)據(jù)。首先由設(shè)計的標(biāo)記點模型計算出物體的位置與姿態(tài)數(shù)據(jù)，再通過卡爾曼濾波處理消除誤差的影響，從而獲得較為平滑的物體運動曲線。然后根據(jù)濾波后的數(shù)據(jù)計算出各采集點對應(yīng)的物體速度和角速度及其加速度和角加速度。最后，本題基于AUBO-I5型協(xié)作機(jī)器人進(jìn)行運動捕捉實驗，實驗結(jié)果表明，本文設(shè)計的物體運動數(shù)據(jù)捕捉方法能有效獲取機(jī)器人末端的運動信息，其獲得的機(jī)器人運動數(shù)據(jù)在路徑點處與真實機(jī)器人的位姿誤差小于1%， 其位置、姿態(tài)、速度、加速度運動曲線與真實機(jī)器人運動曲線一致。本文設(shè)計的物體通用運動數(shù)據(jù)捕捉方法為基于光學(xué)式運動捕捉技術(shù)的研究提供了前提基礎(chǔ)，降低了數(shù)據(jù)獲取步驟的研究難度，縮短了研究時間，提高了相關(guān)的研究效率。能為后面研究者對于物體的運動數(shù)據(jù)捕捉方法研究提供一定的方法借鑒。