一種光學式運動捕捉系統下的物體運動數據捕捉方法研究

蔡嘉暉，王紅偉，王漪夢

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

0 引言

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

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

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

1 物體位姿捕捉

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

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

圖1 標記點模型及其零位坐標系

(1)

(2)

由于標記點粘貼位置不確定，標記點所構向量的模長存在過大或過小問題，不利于后續的旋轉矩陣求解計算，對計算結果可能造成較大的誤差影響，因此需要對以上向量進行標準化處理，得到其單位向量。

(3)

記初始狀態下的標準化后的標記點單位向量集合為A，運動狀態下的標記點單位向量集合為B，即:

A=[αnorm1αnorm2αnorm3]

B=[βnorm1βnorm2βnorm3]

(4)

設在運動狀態下物體的標記點坐標系相對與零點坐標系的旋轉矩陣為R，根據旋轉后向量β等于旋轉前向量α乘旋轉矩陣R，可得：

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

(5)

根據式(5)可得：

(6)

即：

R=BA-1

(7)

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

(8)

設在運動狀態下物體的相對于初始狀態的位置坐標為P，根據當前標記點位置何物體的零位坐標PZ可得：

(9)

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

(10)

2 物體的運動參數計算

2.1 速度、加速度計算

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

圖2 X軸物體運動示意圖

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

(11)

(12)

(13)

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

(14)

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

(15)

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

2.2 角速度、角加速度計算

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

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

(16)

根據式(16)的物體旋轉矩陣公式，可以求出物體在該旋轉矩陣下對應姿態的歐拉角φ、θ、ψ，旋轉矩陣到歐拉角的計算公式如式(17)所示：

(17)

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

圖3 物體章動角轉動示意圖

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

(18)

(19)

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

(20)

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

(21)

物體自轉角θ的角速度和角加速度為：

(22)

3 卡爾曼濾波

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

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

(24)

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

(25)

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

(26)

同理物體的姿態角預測值為：

(27)

最后，通過把運動捕捉系統捕捉的物體位姿測量值和式(26)和式(27)獲得的位置和姿態角預測值代入標準卡爾曼濾波方程中，可以得到濾波后的物體的運動數據曲線。

4 實驗測試與分析

4.1 運動數據捕捉模型測試系統

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

圖4 測試系統組成框圖

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

4.2 實驗測試過程

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

圖6 物體運動捕捉標記點模型

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

表1 路徑點位姿數據對比

4.3 測試數據分析

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

圖7 機器人末端運動數據的卡爾曼濾波

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

圖8 濾波數據局部放大圖

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

圖9 機器人末端速度和角速度

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

圖10 機器人末端加速度和角加速度

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

5 結束語

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