視覺定位方法和視覺定位裝置與流程
本發(fā)明總的來說涉及圖像處理領(lǐng)域，具體來說涉及用于基于識別對象的幾何約束進行視覺定位的視覺定位方法和視覺定位裝置。
背景技術(shù)：
在自動化設(shè)備領(lǐng)域中，越來越依靠使用機器視覺的視覺定位技術(shù)。根據(jù)目標的不同視覺定位技術(shù)按照目標種類進行區(qū)分，可以分為基于合作目標的定位技術(shù)和基于非合作目標的定位技術(shù)。
合作目標可以分為自然特征和人工標識物特征。自然特征主要包括目標自身的夾角、直線、圓、矩形、三角形等規(guī)則的幾何特征。人工標識物又可分為點特征標識物、線特征標識物以及高級幾何特征標識物。
基于標識點的視覺定位技術(shù)又可以稱為PnP問題，即通過點特征數(shù)據(jù)獲取目標的姿態(tài)信息。點特征數(shù)據(jù)一般由一系列點對組成，點對包括物方空間標識點和其對應(yīng)的投影點。對PnP問題的各種解決方案，例如EPnP算法、DLS算法、RPnP算法、ASPnP算法和LHM算法等等均可以快速、高精度解決PnP問題。但是，上述算法僅適用于幾十個甚至上百個點對的情景，且其測量精度隨著點對數(shù)量的減少而迅速下降。
因此，需要改進的視覺定位方法和視覺定位裝置。
技術(shù)實現(xiàn)要素：
本發(fā)明的目的在于針對上述現(xiàn)有技術(shù)中的缺陷和不足，提供能夠基于識別對象的幾何約束進行識別對象的姿態(tài)的實時和高精度定位的新穎的和改進的視覺定位方法和視覺定位裝置。
根據(jù)本發(fā)明的一方面，提供了一種視覺定位方法，包括：在識別對象上設(shè)置多個標識點；根據(jù)所述多個標識點之間的位置關(guān)系，對所述多個標識點進行拓撲區(qū)分；根據(jù)所述多個標識點之間的拓撲關(guān)系，建立標識點三維坐標的線性方程組，并求解所述多個標識點的三維坐標；根據(jù)標識點的三維坐標，建立標識點坐標系；和，求解所述標識點坐標系的旋轉(zhuǎn)矩陣，以得到所述識別對象的姿態(tài)信息。
在上述視覺定位方法中，所述識別對象是視覺定位眼鏡，且所述多個標識點是設(shè)置在所述視覺定位眼鏡的框架上的五個標識點。
在上述視覺定位方法中，所述多個標識點之間的拓撲關(guān)系包括所述多個標識點之間的平行和線段比例關(guān)系。
在上述視覺定位方法中，所述對多個標識點進行拓撲區(qū)分的步驟具體包括：
利用以下公式(1)判斷三個點P1、P2、P3是否位于同一直線上：
其中，是由P1、P2、P3組成的兩個矢量，且當(dāng)L小于預(yù)定閾值時，確定三個點P1、P2、P3位于同一直線上。
在上述視覺定位方法中，所述建立求解標識點三維坐標的線性方程組的步驟具體包括：
令標識點在世界坐標系下的坐標為Pwi(xwi,ywi,zwi)，在相機坐標系下的坐標為Pci(xci,yci,zci)，且在成像面上的像素坐標為Iui(xui,yui,1)，則Pci(xci,yci,zci)與Iui(xui,yui,1)的關(guān)系表示為以下公式(2)：
其中，(dx,dy)為像元尺寸，sx為不垂直因子，f為焦距，(u0,v0)為圖像中心；
令5個標識點中Pw2Pw3//Pw0Pw4，設(shè)2號點到3號點的距離|Pw2Pw3|與0號點到4號點的距離|Pw0Pw4|比為k，得到以下公式(3)：
令R、T為標識點從世界坐標系到相機坐標系的變換，得到以下公式(4)：
Pci＝R·Pwi+T (4)
將公式(4)帶入公式(3)，得到公式(5)：
Pc2＝R·Pw2+T
＝R·(k(Pw4-Pw0)+Pw3)+T
＝R·(k(Pw4-Pw0+T-T)+Pw3)+T
＝k(RPw4-RPw0+T-T)+RPw3+T
＝k(Pc4-Pc0)+Pc3 (5)
因此得到公式(6)：
Pc2＝[Pc0,Pc3,Pc4][-k,1,k]T (6)
令Pc＝[Pc0,Pc3,Pc4]，根據(jù)公式(2)和公式(6)得到公式(7)：
Pc＝[Pc0,Pc3,Pc4]＝K-1diag(λ0,λ3,λ4)[Iu0,Iu3,Iu4] (7)
將公式(7)兩邊同時去逆并乘以Pc2得到公式(8)：
Pc-1Pc2＝diag(1/λ0,1/λ3,1/λ4)[Iu0,Iu3,Iu4]-1K(λ2K-1Iu2)
＝diag(λ2/λ0,λ2/λ3,λ2/λ4)[Iu0,Iu3,Iu4]-1Iu2 (8)
令[η0,η3,η4]＝[Iu0,Iu3,Iu4]-1Iu2，帶入公式(8)得到公式(9)：
Pc-1Pc2＝diag(λ2/λ0,λ2/λ3,λ2/λ4)[η0,η3,η4] (9)
根據(jù)公式(6)得到公式(10)：
Pc-1Pc2＝[-k,1,k]T (10)
聯(lián)立公式(9)和公式(10)求解出λi，如以下公式(11)表示：
根據(jù)以下公式(12)由λi表示標識點在相機坐標系下的坐標為：
在上述視覺定位方法中，所述建立標識點坐標系的步驟具體包括：
根據(jù)Pc0、Pc2、Pc3、Pc4四個標識點建立如以下公式(13)表示的標識點坐標系M，坐標系原點為點Pc4：
式中為x軸方向向量，為z軸方向向量，為y軸方向向量。
在上述視覺定位方法中，所述求解標識點坐標系的旋轉(zhuǎn)矩陣以得到識別對象的姿態(tài)信息的步驟具體包括：
標識點群在t時刻的坐標系Mt與t+1時刻的坐標系Mt+1之間的關(guān)系由公式(14)表示：
Mt+1＝R·Mt+T (14)
由向量的平移不變性得到以下公式(15)：
以公式(16)將公式(15)得到的旋轉(zhuǎn)矩陣歐拉角化：
其中，角度α、β和γ分別為所述識別對象在所述相機坐標系中的方位角、俯仰角和橫滾角。
在上述視覺定位方法中，所述五個標識點是安裝在所述視覺識別眼鏡的框架上的五個沉降孔中的五個紅外LED燈。
根據(jù)本發(fā)明的另一方面，提供了一種視覺定位裝置，包括：設(shè)置單元，用于在識別對象上設(shè)置多個標識點；拓撲單元，用于根據(jù)所述多個標識點之間的位置關(guān)系，對所述多個標識點進行拓撲區(qū)分；坐標獲取單元，用于根據(jù)所述多個標識點之間的拓撲關(guān)系，建立標識點三維坐標的線性方程組，并求解該多個標識點的三維坐標；坐標系建立單元，用于根據(jù)標識點的三維坐標，建立標識點坐標系；和，信息獲取單元，用于求解所述標識點坐標系的旋轉(zhuǎn)矩陣，以得到所述識別對象的姿態(tài)信息。
在上述視覺定位裝置中，所述識別對象是視覺定位眼鏡，且所述多個標識點是設(shè)置在所述視覺定位眼鏡的框架上的五個標識點。
在上述視覺定位裝置中，所述多個標識點之間的拓撲關(guān)系包括所述多個標識點之間的平行和線段比例關(guān)系。
在上述視覺定位裝置中，所述拓撲單元具體用于：
利用以下公式(1)判斷三個點P1、P2、P3是否位于同一直線上：
其中，是由P1、P2、P3組成的兩個矢量，且當(dāng)L小于預(yù)定閾值時，確定三個點P1、P2、P3位于同一直線上。
在上述視覺定位裝置中，所述方程組建立單元具體用于：
令標識點在世界坐標系下的坐標為Pwi(xwi,ywi,zwi)，在相機坐標系下的坐標為Pci(xci,yci,zci)，且在成像面上的像素坐標為Iui(xui,yui,1)，則Pci(xci,yci,zci)與Iui(xui,yui,1)的關(guān)系表示為以下公式(2)：
其中，(dx,dy)為像元尺寸，sx為不垂直因子，f為焦距，(u0,v0)為圖像中心；
令5個標識點中Pw2Pw3//Pw0Pw4，設(shè)2號點到3號點的距離|Pw2Pw3|與0號點到4號點的距離|Pw0Pw4|比為k，得到以下公式(3)：
令R、T為標識點從世界坐標系到相機坐標系的變換，得到以下公式(4)：
Pci＝R·Pwi+T (4)
將公式(4)帶入公式(3)，得到公式(5)：
Pc2＝R·Pw2+T
＝R·(k(Pw4-Pw0)+Pw3)+T
＝R·(k(Pw4-Pw0+T-T)+Pw3)+T
＝k(RPw4-RPw0+T-T)+RPw3+T
＝k(Pc4-Pc0)+Pc3 (5)
因此得到公式(6)：
Pc2＝[Pc0,Pc3,Pc4][-k,1,k]T (6)
令Pc＝[Pc0,Pc3,Pc4]，根據(jù)公式(2)和公式(6)得到公式(7)：
Pc＝[Pc0,Pc3,Pc4]＝K-1diag(λ0,λ3,λ4)[Iu0,Iu3,Iu4] (7)
將公式(7)兩邊同時去逆并乘以Pc2得到公式(8)：
Pc-1Pc2＝diag(1/λ0,1/λ3,1/λ4)[Iu0,Iu3,Iu4]-1K(λ2K-1Iu2)
＝diag(λ2/λ0,λ2/λ3,λ2/λ4)[Iu0,Iu3,Iu4]-1Iu2 (8)
令[η0,η3,η4]＝[Iu0,Iu3,Iu4]-1Iu2，帶入公式(8)得到公式(9)：
Pc-1Pc2＝diag(λ2/λ0,λ2/λ3,λ2/λ4)[η0,η3,η4] (9)
根據(jù)公式(6)得到公式(10)：
Pc-1Pc2＝[-k,1,k]T (10)
聯(lián)立公式(9)和公式(10)求解出λi，如以下公式(11)表示：
根據(jù)以下公式(12)由λi表示標識點在相機坐標系下的坐標為：
在上述視覺定位裝置中，所述坐標系建立單元具體用于：
根據(jù)Pc0、Pc2、Pc3、Pc4四個標識點建立如以下公式(13)表示的標識點坐標系M，坐標系原點為點Pc4：
式中為x軸方向向量，為z軸方向向量，為y軸方向向量。
在上述視覺定位裝置中，所述信息獲取單元具體用于：
標識點群在t時刻的坐標系Mt與t+1時刻的坐標系Mt+1之間的關(guān)系由公式(14)表示：
Mt+1＝R·Mt+T (14)
由向量的平移不變性得到以下公式(15)：
以公式(16)將公式(15)得到的旋轉(zhuǎn)矩陣歐拉角化：
其中，角度α、β和γ分別為所述識別對象在所述相機坐標系中的方位角、俯仰角和橫滾角。
在上述視覺定位裝置中，所述五個標識點是安裝在所述視覺識別眼鏡的框架上的五個沉降孔中的五個紅外LED燈。
通過根據(jù)本發(fā)明的視覺定位方法和視覺定位裝置，可以實現(xiàn)不具有顯著的自然特征的識別對象的姿態(tài)的實時和高精度定位。
在根據(jù)本發(fā)明的視覺定位方法和視覺定位裝置中，通過在識別對象上設(shè)置標識點來快速實現(xiàn)識別對象的姿態(tài)參數(shù)的測量。當(dāng)應(yīng)用于人體時，根據(jù)本發(fā)明的視覺定位方法和視覺定位裝置適用于室內(nèi)或封閉空間內(nèi)，快速實現(xiàn)人體頭部姿態(tài)參數(shù)的測量，具有非接觸、不受電磁干擾、工作時間長、穩(wěn)定性高等優(yōu)點。
附圖說明
圖1是根據(jù)本發(fā)明實施例的視覺定位方法的示意性流程圖；
圖2是示出根據(jù)本發(fā)明實施例的視覺定位眼鏡上多個標識點之間的拓撲關(guān)系的示意圖；
圖3是示出三點共線性的示意圖；
圖4是點透視投影成像模型的示意圖；
圖5是根據(jù)本發(fā)明實施例的視覺定位裝置的示意性框圖。
具體實施方式
以下描述用于公開本發(fā)明以使本領(lǐng)域技術(shù)人員能夠?qū)崿F(xiàn)本發(fā)明。以下描述中的優(yōu)選實施例只作為舉例，本領(lǐng)域技術(shù)人員可以想到其他顯而易見的變型。在以下描述中界定的本發(fā)明的基本原理可以應(yīng)用于其他實施方案、變形方案、改進方案、等同方案以及沒有背離本發(fā)明的精神和范圍的其他技術(shù)方案。
可以理解的是，術(shù)語“一”應(yīng)理解為“至少一”或“一個或多個”，即在一個實施例中，一個元件的數(shù)量可以為一個，而在另外的實施例中，該元件的數(shù)量可以為多個，術(shù)語“一”不能理解為對數(shù)量的限制。
以下說明書和權(quán)利要求中使用的術(shù)語和詞不限于字面的含義，而是僅由本發(fā)明人使用以使得能夠清楚和一致地理解本發(fā)明。因此，對本領(lǐng)域技術(shù)人員很明顯僅為了說明的目的而不是為了如所附權(quán)利要求和它們的等效物所定義的限制本發(fā)明的目的而提供本發(fā)明的各種實施例的以下描述。
雖然比如“第一”、“第二”等的序數(shù)將用于描述各種組件，但是在這里不限制那些組件。該術(shù)語僅用于區(qū)分一個組件與另一組件。例如，第一組件可以被稱為第二組件，且同樣地，第二組件也可以被稱為第一組件，而不脫離發(fā)明構(gòu)思的教導(dǎo)。在此使用的術(shù)語“和/或”包括一個或多個關(guān)聯(lián)的列出的項目的任何和全部組合。
在這里使用的術(shù)語僅用于描述各種實施例的目的且不意在限制。如在此使用的，單數(shù)形式意在也包括復(fù)數(shù)形式，除非上下文清楚地指示例外。另外將理解術(shù)語“包括”和/或“具有”當(dāng)在該說明書中使用時指定所述的特征、數(shù)目、步驟、操作、組件、元件或其組合的存在，而不排除一個或多個其它特征、數(shù)目、步驟、操作、組件、元件或其組的存在或者附加。
包括技術(shù)和科學(xué)術(shù)語的在這里使用的術(shù)語具有與本領(lǐng)域技術(shù)人員通常理解的術(shù)語相同的含義，只要不是不同地限定該術(shù)語。應(yīng)當(dāng)理解在通常使用的詞典中限定的術(shù)語具有與現(xiàn)有技術(shù)中的術(shù)語的含義一致的含義。
下面結(jié)合附圖和具體實施方式對本發(fā)明作進一步詳細的說明：
如上所述，對于不具有顯著的自然特征的識別對象，比如人的頭部，常常借助于識別人的頭部上佩戴的其他物體，比如眼鏡的方式來進行識別。
由于人體工程學(xué)原理，眼鏡多為不規(guī)則的弧線組成，其提供的自然特征也相對有限。因此，在本發(fā)明的實施例中，使用了基于合作目標的定位技術(shù)，即在眼鏡框上布局人工標識物。并且，考慮到眼鏡框有限的寬度以及標識物的特征提取，使用了點特征標識物。
并且，考慮到視覺定位眼鏡僅能提供有限數(shù)量的標識點，本發(fā)明不能采用上述PnP問題的各種算法。因此，P3P、P4P、P5P問題是本發(fā)明研究的重點。由于P3P問題使用標識點數(shù)量較少，其通常含有4組疑似解。通過增加一個標識點，任意三個標識點組成標識點集合，求取標識點集合解的交集即為正確解。P4P問題和P5P問題的解決方案一般使用多個標識點之間的幾何約束，即平行、夾角、距離、異面等等，直接或間接解算標識點的三維坐標。Liu.M.L.等人充分利用標識點的幾何構(gòu)型約束，包括夾角、混合積和距離約束等等，使用迭代的方法得到了標識點的精確坐標。Z.Y.Hu等人使用數(shù)學(xué)方法分析了標識點的幾何構(gòu)型約束問題，量化了姿態(tài)解算方程可能解的個數(shù)和幾何構(gòu)型的數(shù)量之間的關(guān)系。Wu PC等人提出了一個解析運動模型用于排除可能解。Yang Guo通過研究透視投影和仿射投影的轉(zhuǎn)換及仿射變換不變形，提出并驗證了共面P4P問題的解的上限2個。同時提出了應(yīng)用于共面P4P問題的奇異值分解算法，但沒有進行實驗驗證。Long Li等人引入了Frobenius范數(shù)替代奇異值分解，但是該方法僅提高了算法的運行速度，未能提高算法的精度和抗噪聲能力。
因此，根據(jù)本發(fā)明實施例的一方面，提供了一種視覺定位方法，包括：在識別對象上設(shè)置多個標識點；根據(jù)該多個標識點之間的位置關(guān)系，對該多個標識點進行拓撲區(qū)分；根據(jù)該多個標識點之間的拓撲關(guān)系，建立標識點三維坐標的線性方程組，并求解該多個標識點的三維坐標；根據(jù)標識點的三維坐標，建立標識點坐標系；和，求解該標識點坐標系的旋轉(zhuǎn)矩陣，以得到該識別對象的姿態(tài)信息。
圖1是根據(jù)本發(fā)明實施例的視覺定位方法的示意性流程圖。如圖1所示，根據(jù)本發(fā)明實施例的視覺定位方法包括：S1，在識別對象上設(shè)置多個標識點；S2，根據(jù)該多個標識點之間的位置關(guān)系，對該多個標識點進行拓撲區(qū)分；S3，根據(jù)該多個標識點之間的拓撲關(guān)系，建立標識點三維坐標的線性方程組，并求解該多個標識點的三維坐標；S4，根據(jù)標識點的三維坐標，建立標識點坐標系；和S5，求解該標識點坐標系的旋轉(zhuǎn)矩陣，以得到該識別對象的姿態(tài)信息。
在上述視覺定位方法中，該識別對象是視覺定位眼鏡，且該多個標識點是設(shè)置在該視覺定位眼鏡的框架上的五個標識點。
這里，本領(lǐng)域技術(shù)人員可以理解，應(yīng)用根據(jù)本發(fā)明實施例的視覺定位方法的原理，可以對各種識別對象進行視覺定位。即使對于人的頭部，也不限于使用視覺定位眼鏡來輔助進行定位，還可以使用例如頭盔、或者頭戴顯示器這樣的可穿戴設(shè)備來輔助進行定位。以下，將以視覺定位眼鏡為例對本發(fā)明的實施例進行詳細描述。
根據(jù)眼鏡結(jié)構(gòu)，可以在鏡框上布局人工標識點。根據(jù)標識點數(shù)量與定位算法解算復(fù)雜程度與精度的關(guān)系，在根據(jù)本發(fā)明實施例的視覺定位方法中，提出了基于5個共面標識點的幾何約束姿態(tài)解算方法，并設(shè)計了相應(yīng)的視覺定位眼鏡。
在上述視覺定位方法中，該多個標識點之間的拓撲關(guān)系包括該多個標識點之間的平行和線段比例關(guān)系。
圖2是示出根據(jù)本發(fā)明實施例的視覺定位眼鏡上多個標識點之間的拓撲關(guān)系的示意圖。5個共面標識點的位置關(guān)系如圖3所示，標識點編號為0-4。根據(jù)投影不變性對5個標識點進行區(qū)分。根據(jù)共線性關(guān)系可以得到兩組具有特殊位置關(guān)系的標識點：0號、4號標識點位于直線L2上，1號、2號、3號標識點位于直線L1上。由于點的共線性不受透視投影變換的影響，因此使用正弦函數(shù)量化表示1號、2號、3號標識點的共線性。圖3是示出三點共線性的示意圖。如圖4所示，利用以下公式(1)判斷三個點P1、P2、P3是否位于同一直線上：
其中，是由P1、P2、P3組成的兩個矢量。L的取值范圍在0和1之間，L越接近1表示兩條直線的共線性越低，越接近0則表示兩條直線的共線性越高。在視覺測量系統(tǒng)中，共線性會受到圖像處理偏差和鏡頭畸變誤差的影響。因此在使用公式(1)判斷共線性時，需要添加一個閾值(例如：sin3°)作為判斷的準則。
也就是說，在上述視覺定位方法中，對多個標識點進行拓撲區(qū)分的步驟具體包括：
利用以下公式(1)判斷三個點P1、P2、P3是否位于同一直線上：
其中，是由P1、P2、P3組成的兩個矢量，且當(dāng)L小于預(yù)定閾值時，確定三個點P1、P2、P3位于同一直線上。
在共線點組P1、P2、P3中，根據(jù)其對應(yīng)圖像點x坐標的不同，從右到左可依次區(qū)分；同理可以區(qū)分共線點組P0、P4。至此完成標識點陣列的編號。當(dāng)然，本領(lǐng)域技術(shù)人員可以理解，在上面該中，利用機器視覺完成點的共線性的判斷，實際上也可以采用諸如人工標識之類的方法，僅需要對多個標識點完成拓撲區(qū)分即可。
圖4是點透視投影成像模型的示意圖。5個標識點的透視投影成像模型如圖5所示。則在根據(jù)本發(fā)明實施例的視覺定位方法中，建立求解標識點三維坐標的線性方程組的步驟具體包括：
令標識點在世界坐標系下的坐標為Pwi(xwi,ywi,zwi)，在相機坐標系下的坐標為Pci(xci,yci,zci)，且在成像面上的像素坐標為Iui(xui,yui,1)，則Pci(xci,yci,zci)與Iui(xui,yui,1)的關(guān)系表示為以下公式(2)：
其中，(dx,dy)為像元尺寸，其是相機的制造參數(shù)，sx為不垂直因子，f為焦距，(u0,v0)為圖像中心，均為相機內(nèi)參數(shù)，可通過對相機標定獲得。
令5個標識點中Pw2Pw3//Pw0Pw4，設(shè)2號點到3號點的距離|Pw2Pw3|與0號點到4號點的距離|Pw0Pw4|比為k，得到以下公式(3)：
令R、T為標識點從世界坐標系到相機坐標系的變換，得到以下公式(4)：
Pci＝R·Pwi+T (4)
將公式(4)帶入公式(3)，得到公式(5)：
Pc2＝R·Pw2+T
＝R·(k(Pw4-Pw0)+Pw3)+T
＝R·(k(Pw4-Pw0+T-T)+Pw3)+T
＝k(RPw4-RPw0+T-T)+RPw3+T
＝k(Pc4-Pc0)+Pc3 (5)
因此得到公式(6)：
Pc2＝[Pc0,Pc3,Pc4][-k,1,k]T (6)
令Pc＝[Pc0,Pc3,Pc4]，根據(jù)公式(2)和公式(6)得到公式(7)：
Pc＝[Pc0,Pc3,Pc4]＝K-1diag(λ0,λ3,λ4)[Iu0,Iu3,Iu4] (7)
將公式(7)兩邊同時去逆并乘以Pc2得到公式(8)：
Pc-1Pc2＝diag(1/λ0,1/λ3,1/λ4)[Iu0,Iu3,Iu4]-1K(λ2K-1Iu2)
＝diag(λ2/λ0,λ2/λ3,λ2/λ4)[Iu0,Iu3,Iu4]-1Iu2 (8)
令[η0,η3,η4]＝[Iu0,Iu3,Iu4]-1Iu2，帶入公式(8)得到公式(9)：
Pc-1Pc2＝diag(λ2/λ0,λ2/λ3,λ2/λ4)[η0,η3,η4] (9)
根據(jù)公式(6)得到公式(10)：
Pc-1Pc2＝[-k,1,k]T (10)
聯(lián)立公式(9)和公式(10)求解出λi，如以下公式(11)表示：
根據(jù)以下公式(12)由λi表示標識點在相機坐標系下的坐標為：
在根據(jù)本發(fā)明實施例的視覺定位方法中，在獲得標識點在相機坐標系下的坐標之后，建立標識點坐標系的步驟具體包括：
根據(jù)Pc0、Pc2、Pc3、Pc4四個標識點建立如以下公式(13)表示的標識點坐標系M，坐標系原點為點Pc4：
式中為x軸方向向量，為z軸方向向量，為y軸方向向量。
在根據(jù)本發(fā)明實施例的視覺定位方法中，求解標識點坐標系的旋轉(zhuǎn)矩陣以得到識別對象的姿態(tài)信息的步驟具體包括：
標識點群在t時刻的坐標系Mt與t+1時刻的坐標系Mt+1之間的關(guān)系由公式(14)表示：
Mt+1＝R·Mt+T (14)
由向量的平移不變性得到以下公式(15)：
以公式(16)將公式(15)得到的旋轉(zhuǎn)矩陣歐拉角化：
其中，角度α、β和γ分別為該識別對象在該相機坐標系中的方位角、俯仰角和橫滾角。這樣，就得到了識別對象在相機系統(tǒng)中的轉(zhuǎn)動參數(shù)，從而實現(xiàn)了識別對象的視覺定位。
在根據(jù)本發(fā)明實施例的LED視覺定位眼鏡中，將5個紅外LED燈分別固定安裝在特殊設(shè)計的眼鏡框上的5個沉降孔中，并連接LED燈供電電路。在根據(jù)本發(fā)明實施例的測量過程中，被測目標帶上視覺定位眼鏡，點亮LED燈，即可在紅外攝像機視場范圍內(nèi)轉(zhuǎn)動。攝像機拍攝視覺定位眼鏡圖像，并將圖像傳輸至運算處理單元；經(jīng)過圖像處理提取標識點圖像坐標，將標識點陣列編號；最后將標識點圖像坐標和世界坐標輸入標識點三維坐標解算算法，獲取標識點在攝像機坐標系下的坐標，由該坐標計算被測目標的姿態(tài)信息。
也就是說，在根據(jù)本發(fā)明實施例的視覺定位方法中，該五個標識點是安裝在該視覺識別眼鏡的框架上的五個沉降孔中的五個紅外LED燈。
根據(jù)本發(fā)明實施例的另一方面，提供了一種視覺定位裝置，包括：設(shè)置單元，用于在識別對象上設(shè)置多個標識點；拓撲單元，用于根據(jù)該多個標識點之間的位置關(guān)系，對該多個標識點進行拓撲區(qū)分；坐標獲取單元，用于根據(jù)該多個標識點之間的拓撲關(guān)系，建立標識點三維坐標的線性方程組，并求解該多個標識點的三維坐標；坐標系建立單元，用于根據(jù)標識點的三維坐標，建立標識點坐標系；和，信息獲取單元，用于求解該標識點坐標系的旋轉(zhuǎn)矩陣，以得到該識別對象的姿態(tài)信息。
圖5是根據(jù)本發(fā)明實施例的視覺定位裝置的示意性框圖。如圖5所示，根據(jù)本發(fā)明實施例的視覺定位裝置100包括：設(shè)置單元101，用于在識別對象上設(shè)置多個標識點；拓撲單元102，用于根據(jù)設(shè)置單元101所設(shè)置的多個標識點之間的位置關(guān)系，對該多個標識點進行拓撲區(qū)分；坐標獲取單元103，用于根據(jù)拓撲單元102獲得的多個標識點之間的拓撲關(guān)系，建立標識點三維坐標的線性方程組，并求解該多個標識點的三維坐標；坐標系建立單元104，用于根據(jù)坐標獲取單元103所獲取的標識點的三維坐標，建立標識點坐標系；和，信息獲取單元105，用于求解坐標系建立單元104所建立的標識點坐標系的旋轉(zhuǎn)矩陣，以得到該識別對象的姿態(tài)信息。
在上述視覺定位裝置中，該識別對象是視覺定位眼鏡，且該多個標識點是設(shè)置在該視覺定位眼鏡的框架上的五個標識點。
在上述視覺定位裝置中，該多個標識點之間的拓撲關(guān)系包括該多個標識點之間的平行和線段比例關(guān)系。
在上述視覺定位裝置中，該拓撲單元具體用于：
利用以下公式(1)判斷三個點P1、P2、P3是否位于同一直線上：
其中，是由P1、P2、P3組成的兩個矢量，且當(dāng)L小于預(yù)定閾值時，確定三個點P1、P2、P3位于同一直線上。
在上述視覺定位裝置中，該方程組建立單元具體用于：
令標識點在世界坐標系下的坐標為Pwi(xwi,ywi,zwi)，在相機坐標系下的坐標為Pci(xci,yci,zci)，且在成像面上的像素坐標為Iui(xui,yui,1)，則Pci(xci,yci,zci)與Iui(xui,yui,1)的關(guān)系表示為以下公式(2)：
其中，(dx,dy)為像元尺寸，sx為不垂直因子，f為焦距，(u0,v0)為圖像中心；
令5個標識點中Pw2Pw3//Pw0Pw4，設(shè)2號點到3號點的距離|Pw2Pw3|與0號點到4號點的距離|Pw0Pw4|比為k，得到以下公式(3)：
令R、T為標識點從世界坐標系到相機坐標系的變換，得到以下公式(4)：
Pci＝R·Pwi+T (4)
將公式(4)帶入公式(3)，得到公式(5)：
Pc2＝R·Pw2+T
＝R·(k(Pw4-Pw0)+Pw3)+T
＝R·(k(Pw4-Pw0+T-T)+Pw3)+T
＝k(RPw4-RPw0+T-T)+RPw3+T
＝k(Pc4-Pc0)+Pc3 (5)
因此得到公式(6)：
Pc2＝[Pc0,Pc3,Pc4][-k,1,k]T (6)
令Pc＝[Pc0,Pc3,Pc4]，根據(jù)公式(2)和公式(6)得到公式(7)：
Pc＝[Pc0,Pc3,Pc4]＝K-1diag(λ0,λ3,λ4)[Iu0,Iu3,Iu4] (7)
將公式(7)兩邊同時去逆并乘以Pc2得到公式(8)：
Pc-1Pc2＝diag(1/λ0,1/λ3,1/λ4)[Iu0,Iu3,Iu4]-1K(λ2K-1Iu2)
＝diag(λ2/λ0,λ2/λ3,λ2/λ4)[Iu0,Iu3,Iu4]-1Iu2 (8)
令[η0,η3,η4]＝[Iu0,Iu3,Iu4]-1Iu2，帶入公式(8)得到公式(9)：
Pc-1Pc2＝diag(λ2/λ0,λ2/λ3,λ2/λ4)[η0,η3,η4] (9)
根據(jù)公式(6)得到公式(10)：
Pc-1Pc2＝[-k,1,k]T (10)
聯(lián)立公式(9)和公式(10)求解出λi，如以下公式(11)表示：
根據(jù)以下公式(12)由λi表示標識點在相機坐標系下的坐標為：
在上述視覺定位裝置中，該坐標系建立單元具體用于：
根據(jù)Pc0、Pc2、Pc3、Pc4四個標識點建立如以下公式(13)表示的標識點坐標系M，坐標系原點為點Pc4：
式中為x軸方向向量，為z軸方向向量，為y軸方向向量。
在上述視覺定位裝置中，該信息獲取單元具體用于：
標識點群在t時刻的坐標系Mt與t+1時刻的坐標系Mt+1之間的關(guān)系由公式(14)表示：
Mt+1＝R·Mt+T (14)
由向量的平移不變性得到以下公式(15)：
以公式(16)將公式(15)得到的旋轉(zhuǎn)矩陣歐拉角化：
其中，角度α、β和γ分別為該識別對象在該相機坐標系中的方位角、俯仰角和橫滾角。
在上述視覺定位裝置中，該五個標識點是安裝在該視覺識別眼鏡的框架上的五個沉降孔中的五個紅外LED燈。
通過根據(jù)本發(fā)明實施例的視覺定位方法和視覺定位裝置，可以實現(xiàn)不具有顯著的自然特征的識別對象的姿態(tài)的實時和高精度定位。
在根據(jù)本發(fā)明實施例的視覺定位方法和視覺定位裝置中，通過在識別對象上設(shè)置標識點來快速實現(xiàn)識別對象的姿態(tài)參數(shù)的測量，從而實現(xiàn)對象在相機系統(tǒng)中的視覺定位。當(dāng)應(yīng)用于人體時，根據(jù)本發(fā)明的視覺定位方法和視覺定位裝置適用于室內(nèi)或封閉空間內(nèi)，快速實現(xiàn)人體頭部姿態(tài)參數(shù)的測量，具有非接觸、不受電磁干擾、工作時間長、穩(wěn)定性高等優(yōu)點。
當(dāng)然，本發(fā)明還可有其它多種實施例，在不背離本發(fā)明的精神及其實質(zhì)的情況下，熟悉本領(lǐng)域的技術(shù)人員當(dāng)可根據(jù)本發(fā)明做出各種相應(yīng)的改變和變形，但這些相應(yīng)的改變和變形都應(yīng)屬于本發(fā)明所附的權(quán)利要求的保護范圍。
- 還沒有人留言評論。精彩留言會獲得點贊！