蔡嘉暉，王紅偉，王漪夢(mèng)

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

0 引言

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

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

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

1 物體位姿捕捉

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

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

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

(1)

(2)

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

(3)

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

A=[αnorm1αnorm2αnorm3]

B=[βnorm1βnorm2βnorm3]

(4)

設(shè)在運(yùn)動(dòng)狀態(tài)下物體的標(biāo)記點(diǎn)坐標(biāo)系相對(duì)與零點(diǎn)坐標(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)

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

(8)

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

(9)

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

(10)

2 物體的運(yùn)動(dòng)參數(shù)計(jì)算

2.1 速度、加速度計(jì)算

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

圖2 X軸物體運(yùn)動(dòng)示意圖

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

(11)

(12)

(13)

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

(14)

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

(15)

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

2.2 角速度、角加速度計(jì)算

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

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

(16)

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

(17)

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

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

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

(18)

(19)

已知?jiǎng)蚣铀俎D(zhuǎn)動(dòng)物體的初始角速度和角加速度，可知物體在下一時(shí)刻的角速度為：

(20)

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

(21)

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

(22)

3 卡爾曼濾波

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

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

(24)

其更新階段的測(cè)量更新方程為：

(25)

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

(26)

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

(27)

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

4 實(shí)驗(yàn)測(cè)試與分析

4.1 運(yùn)動(dòng)數(shù)據(jù)捕捉模型測(cè)試系統(tǒng)

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

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

圖5 物體運(yùn)動(dòng)捕捉實(shí)驗(yàn)平臺(tái)

4.2 實(shí)驗(yàn)測(cè)試過程

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

圖6 物體運(yùn)動(dòng)捕捉標(biāo)記點(diǎn)模型

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

表1 路徑點(diǎn)位姿數(shù)據(jù)對(duì)比

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

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

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

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

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

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

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

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

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

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

5 結(jié)束語(yǔ)

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