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

背景技術：

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

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

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

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

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

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

和

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

上式中，

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

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

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

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

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

設初始位置時q＝1，對應的平面法矢量為

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

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

對應的平面發(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ù)據(jù)測定中累積誤差越來越大的缺點。由于只使用一臺攝像機，安裝和移動方便靈活，而且不增加太多的成本。

附圖說明

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

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

具體實施方式

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

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

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

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

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

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

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

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

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

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

統(tǒng)一坐標系：

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

使用方法：

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

理論分析：

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

相機機的內參數(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；

設初始位置時q＝1，對應的平面法矢量為

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

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

對應的平面發(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)實應用中，可以把VR眼鏡作為實施例中的長方體，這樣可以實時測得VR眼鏡的位置和姿態(tài)，用來控制VR眼鏡內現(xiàn)實的內容。

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

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

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