本發(fā)明涉及三維空間目標定位、位置追蹤、目標姿態(tài)測定技術(shù)領(lǐng)域，尤其涉及虛擬現(xiàn)實應(yīng)用中的空間定位與追蹤、運動捕捉、運動分析、康復(fù)醫(yī)療中的步態(tài)分析和目標姿態(tài)測定等方面的一種三維空間目標精準定位和追蹤方法。

背景技術(shù)：

空間目標三維位置和姿態(tài)測定常用的技術(shù)從原理上說可分為機械式、聲學(xué)式、電磁式、慣性式、光學(xué)式。各種方式優(yōu)缺點對比如下：

隨著技術(shù)的發(fā)展，目前最常用是慣性式和光學(xué)式。本發(fā)明在慣性式中引入單臺攝像機，利用一臺攝像機輔助慣性傳感器進行空間移動目標位置的精準測定，有效地解決了慣性式方法中位置數(shù)據(jù)測定中的積累誤差越來越大的缺點。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的是：在慣性式中引入單臺攝像機，利用一臺攝像機輔助慣性傳感器進行空間移動目標位置的精準測定，有效解決慣性式方法中位置數(shù)據(jù)測定中的積累誤差越來越大的缺點。

本發(fā)明提供了一種三維空間目標精準定位和追蹤方法，本方法在被測目標上設(shè)置兩個標識點：第一標識點和第二標識點、慣性傳感器、一臺攝像機；所述第一標識點和第二標識點之間的距離L已知；所述慣性傳感器通過三軸角加速度，獲得三軸角度或能夠描述三軸角度的四元數(shù)。

通過公式：以下計算的前提條件是，將攝像機置于慣性傳感器相同的坐標系下；

和

得到第一標識點(4)的坐標(X₁，Y₁，Z₁)和第二標識點(5)的坐標(X₂，Y₂，Z₂)；

上式中，

n＝1或2；A,B,C是由慣性傳感器所確定的三軸角度推算出的平面的法相量；∝_x，∝_y，u₀，v₀為攝像機內(nèi)參數(shù)，是已知量，滿足

優(yōu)選的，慣性傳感器所確定的三軸角度得到的平面的法相量(A,B,C)的過程如下：

平面方程如下AX+BY+CZ+d＝0；

用于描述3軸角度的四元數(shù)的表達式為：

q＝q₀+i*q_x+j*q_y+k*q_z；

設(shè)初始位置時q＝1，對應(yīng)的平面法矢量為

其四元數(shù)表示為r₀＝0+i*0+j*0+k*1；

任意位置時p＝p₀+i*p_x+j*p_y+k*p_z，

對應(yīng)的平面發(fā)矢量為

其四元數(shù)表示為r＝0+i*A+j*B+k*C；

則r＝pr₀p^*，

即(A,B,C)可由pr₀p^*算出；

其中，p^*＝p₀-i*p_x-j*p_y-k*p_z為p^*的共軛。

優(yōu)選的，所述第一標識點和第二標識點是反光球、反光貼片、特殊的圖案或具有一定特征性的圖像。

優(yōu)選的，所述攝像機是彩色攝像機、紅外攝像機、深度掃描設(shè)備等能夠識別目標上的標識點或特征的光學(xué)設(shè)備或從獲得的光學(xué)圖像上識別標識點的位置的設(shè)備。

有益效果：本方法僅使用慣性傳感器提供的三軸角度信息，不使用位置信息，避免了慣性傳感器位置數(shù)據(jù)測定中累積誤差越來越大的缺點。由于只使用一臺攝像機，安裝和移動方便靈活，而且不增加太多的成本。

附圖說明

圖1為本發(fā)明實施例提供的三維空間目標精準定位和追蹤方法原理圖。

圖2為本發(fā)明實施例中統(tǒng)一坐標系的說明圖。

具體實施方式

為使本發(fā)明解決的技術(shù)問題、采用的技術(shù)方案和達到的技術(shù)效果更加清楚，下面結(jié)合附圖和實施例對本發(fā)明作進一步的詳細說明?？梢岳斫獾氖?，此處所描述的具體實施例僅僅用于解釋本發(fā)明，而非對本發(fā)明的限定。另外還需要說明的是，為了便于描述，附圖中僅示出了與本發(fā)明相關(guān)的部分而非全部內(nèi)容。

請參照圖1，本方法的系統(tǒng)包括：被測目標1上的2個標識點(第一標識點4和第二標識點5)、慣性傳感器2、一臺攝像機3。

被測目標1：取一長方體作為被測目標1。

標識點：表面特殊材質(zhì)，反射紅外線的小球。

被測目標表面的2個標識點：固定于長方體表面的兩個標識點。實測其距離為L。

慣性傳感器2：可以提供自身的三軸旋轉(zhuǎn)角,進而提供四元數(shù)，計算出自身平面的法相量。

攝像機3：紅外攝像機，可以檢測出其圖像上兩個標識點的平面坐標。

環(huán)境要求及準備：

無陽光直射的室內(nèi)，增加紅外線照明作為光源，這樣紅外線攝像機只能拍攝到標識點反射的紅外光線，可以很容易的識別到圖像上標識點的位置。

在長方體表面上固定2個標識點，并實測其間距為L。

統(tǒng)一坐標系：

將慣性傳感器放置在攝像機正上方，使傳感器的坐標系和相機坐標系重合，設(shè)置慣性傳感器數(shù)據(jù)歸零，則完成慣性傳感器和攝像機的坐標系的統(tǒng)一。(見圖2，其中3代表相機，2代表傳感器)

使用方法：

統(tǒng)一坐標系后，將慣性傳感器固定在長方體的表面，則可以根據(jù)慣性傳感器的數(shù)據(jù)和紅外攝像機捕捉到的標識點坐標利用本發(fā)明的計算方法，實時計算出長方體在空間的位置和姿態(tài)。完成對被測目標長方體的運動捕捉。

理論分析：

將攝像機置于慣性傳感器相同的坐標系下(統(tǒng)一坐標系的方法有很多，這里不詳述)

相機機的內(nèi)參數(shù)為

設(shè)空間中任一標識點的三維坐標為(X,Y,Z)，此標識點被相機捕捉到后，在攝像機圖像上的坐標為(u,v)，S為標識點到相機的距離，則有式

式(1)

展開后得到

即

式(2)

平面方程如下AX+BY+CZ+d＝0，其中(A,B,C)是由目標上的慣性傳感器所確定的三軸角度推算出的平面的法相量。

具體過程如下：慣性傳感器所確定的三軸角度得到的平面的法相量(A,B,C)的過程如下：

平面方程如下AX+BY+CZ+d＝0；

用于描述3軸角度的四元數(shù)的表達式為：

q＝q₀+i*q_x+j*q_y+k*q_z；

設(shè)初始位置時q＝1，對應(yīng)的平面法矢量為

其四元數(shù)表示為r₀＝0+i*0+j*0+k*1；

任意位置時p＝p₀+i*p_x+j*p_y+k*p_z，

對應(yīng)的平面發(fā)矢量為

其四元數(shù)表示為r＝0+i*A+j*B+k*C；

則r＝pr₀p^*，

即(A,B,C)可由pr₀p^*算出；

其中，p^*＝p₀-i*p_x-j*p_y-k*p_z為p^*的共軛。

與式(2)聯(lián)立得

式(3)

解方程得

式(4)X＝-dm_x,Y＝-dm_y,Z＝-dm_z

其中

對于目標表面上的兩個標識點(X₁,Y₁,Z₁)和(X₂,Y₂,Z₂)

X₁＝-dm_x1,Y₁＝-dm_y1,Z₁＝-dm_z1

X₂＝-dm_x2,Y₂＝-dm_y2,Z₂＝-dm_z2

已知兩個標識點的距離為L，則有

即得式(5)

式(5)代入式(4)得

和

得到兩個標識點(X₁,Y₁,Z₁)和(X₂,Y₂,Z₂)后，即可分析被測目標1的其他位置點信息。

在虛擬現(xiàn)實應(yīng)用中，可以把VR眼鏡作為實施例中的長方體，這樣可以實時測得VR眼鏡的位置和姿態(tài)，用來控制VR眼鏡內(nèi)現(xiàn)實的內(nèi)容。

在康復(fù)醫(yī)療的步態(tài)分析中，可以把大腿或小腿作為實施例中的長方體，可以檢測出大腿或小腿的運動信息，用來判斷疾病及康復(fù)情況。

在高爾夫球動作分析中，可以把髖關(guān)節(jié)作為實施例中的長方體，可以檢測髖關(guān)節(jié)的位移和旋轉(zhuǎn)，用來分析擊球動作。

最后應(yīng)說明的是：以上各實施例僅用以說明本發(fā)明的技術(shù)方案，而非對其限制；盡管參照前述各實施例對本發(fā)明進行了詳細的說明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解：其對前述各實施例所記載的技術(shù)方案進行修改，或者對其中部分或者全部技術(shù)特征進行等同替換，并不使相應(yīng)技術(shù)方案的本質(zhì)脫離本發(fā)明各實施例技術(shù)方案的范圍。