一種基於雷射跟蹤儀的地面二維碼標定方法及裝置與流程
2024-02-11 02:09:15
本發明涉及坐標標定技術，具體涉及一種基於雷射跟蹤儀的地面二維碼標定方法及裝置。
背景技術：
AGV(Automatic Guided Vehicle，自動導航小車)是一種無人操縱的自動化運輸設備，能承載一定的重量在出發地和目的地之間自主運行，是自動物流系統和柔性製造系統的重要組成設備，具有良好的市場前景和應用價值。導航系統是AGV的核心控制部分，定位則是導航系統中極其重要的功能組成部分。
隨著二維碼技術的發展，二維碼的應用領域也深入到了工業領域，AGV採用二維碼進行位姿估計時，二維碼粘貼過程會導致粘貼的二維碼位姿信息與二維碼自帶的預設位姿信息存在位姿偏差，這一偏差對AGV位姿估計產生一定的影響，為滿足AGV路徑引導系統定位精度的要求，地面二維碼的位姿測量及誤差計算成為二維碼定位技術中的一個關鍵問題。
技術實現要素：
為解決上述技術問題，本發明提供一種基於雷射跟蹤儀的地面二維碼標定方法及裝置，實現了對二維碼的位姿及位姿誤差的準確測量，提高二維碼的位姿及位姿誤差的測量精度。
為實現上述目的，本發明提供以下技術方案：
一方面，本發明提供了一種基於雷射跟蹤儀的地面二維碼標定方法，包括：
S101：建立雷射跟蹤儀坐標系A、二維碼坐標系B、攝像機坐標系C、靶球坐標系D和世界坐標系H，其中，二維碼預設位姿的坐標係為世界坐標系H；
S102：計算：
雷射跟蹤儀坐標系A和靶球坐標系D之間的變換矩陣
靶球坐標系D和攝像機坐標系C之間的變換矩陣
世界坐標系H和雷射跟蹤儀坐標系A之間的變換矩陣
攝像機坐標系C和二維碼坐標系B之間的變換矩陣
S103：根據變換矩陣變換矩陣變換矩陣和變換矩陣計算世界坐標系H和二維碼坐標系B之間的變換矩陣
S104：通過變換矩陣計算二維碼的位姿及位姿誤差。
進一步的，設有4個在同一平面的靶球P0、靶球P1、靶球P2和靶球P3，靶球P1、靶球P2和靶球P3分布在靶球P0的周圍；所述靶球坐標系D以靶球P0為原點，向量的方向為靶球坐標系D的x軸方向，向量的方向為靶球坐標系D的y軸方向，同時垂直於x軸方向和y軸方向為z軸方向。
進一步的，所述建立雷射跟蹤儀坐標系A、二維碼坐標系B、攝像機坐標系C、靶球坐標系D和世界坐標系H的步驟包括：
雷射跟蹤儀坐標系A由雷射跟蹤儀自行設定；
二維碼坐標系B以二維碼中心為原點，平行於相鄰兩條邊線的方向分別為x軸與y軸方向，垂直於二維碼向上為z軸方向；
攝像機坐標系C以鏡頭中心為原點，垂直於鏡頭中軸為z軸方向，平行於鏡頭為x軸方向，同時垂直於x軸方向和z軸方向為y軸方向。
進一步的，計算雷射跟蹤儀坐標系A和靶球坐標系D之間的變換矩陣的步驟包括：
雷射跟蹤儀測量靶球P0、靶球P1、靶球P2和靶球P3在雷射跟蹤儀坐標系A內的坐標AP0，AP1,AP2,AP3，根據公式DPk＝APk-AP0計算靶球P1、靶球P2和靶球P3在靶球坐標系D中的坐標DP1,DP2,DP3；其中，k＝1,2,3；
根據坐標AP0，AP1,AP2,AP3和坐標DP1,DP2,DP3計算雷射跟蹤儀坐標系A和靶球坐標系D之間的變換矩陣
進一步的，計算靶球坐標系D和攝像機坐標系C之間的變換矩陣的步驟包括：
通過用攝像機標定棋盤格位姿，記錄攝像機坐標系C與棋盤格坐標系E之間的變換矩陣
計算雷射跟蹤儀坐標系A和靶球坐標系D之間的變換矩陣
移動攝像機後重新計算變換矩陣和變換矩陣
分別計算攝像機坐標系C和靶球坐標系D的變換矩陣和自旋轉軸
重新計算並獲取另一組變換矩陣和自旋轉軸
通過變換矩陣和自旋轉軸計算靶球坐標系D和攝像機坐標系C之間的變換矩陣
進一步的，計算世界坐標系H和雷射跟蹤儀坐標系A之間的變換矩陣的步驟包括：
選取世界坐標系H中的3個已知坐標點HP1,HP2,HP3，利用雷射跟蹤儀測量3個坐標點在雷射跟蹤儀坐標系A內坐標AP1,AP2,AP3；
根據坐標點HP1,HP2,HP3和坐標AP1,AP2,AP3計算世界坐標系H和雷射跟蹤儀坐標系A之間的變換矩陣
進一步的，計算攝像機坐標系C和二維碼坐標系B之間的變換矩陣的步驟包括：
移動攝像機至二維碼正上方，攝像機標定二維碼並記錄攝像機坐標系C與二維碼坐標系B之間的變換矩陣
進一步的，所述根據變換矩陣變換矩陣變換矩陣和變換矩陣計算世界坐標系H和二維碼坐標系B之間的變換矩陣的步驟包括：
採用下述公式計算變換矩陣
進一步的，所述通過變換矩陣計算二維碼的位姿及位姿誤差的步驟包括：
通過二維碼坐標系B中二維碼中心位置坐標BP0＝[0 0 0 1]T計算世界坐標系H中二維碼中心位置坐標HP0；
通過坐標HP0獲取二維碼實際坐標值(xc,yc,z0)；
二維碼的位姿誤差為二維碼的預設位姿的坐標(xt,yt,z0)與二維碼實際坐標值(xc,yc,z0)的差值。
另一方面，本發明提供了一種基於雷射跟蹤儀的地面二維碼標定裝置，包括：
雷射跟蹤儀、靶球、攝像機、靶球-攝像機固定架和計算機；
所述靶球和所述攝像機設置在所述靶球-攝像機固定架上，所述攝像機的鏡頭向下，在所述攝像機鏡頭的正上方的靶球-攝像機固定架上設有4個靶球；所述攝像機與所述計算機相連接，所述雷射跟蹤儀與所述計算機相連接。
由上述技術方案可知，本發明所述的一種基於雷射跟蹤儀的地面二維碼標定方法及裝置，提升了二維碼位姿測量精度，雷射跟蹤儀的自動捕捉功能與數據批量處理過程簡化了操作。機器人手眼標定算法計算靶球坐標系與攝像機坐標系間的變換關係減小了機械加工和安裝造成的尺寸誤差，攝像機標定二維碼位姿減小了人工標定二維碼位姿的隨機誤差；並且雷射跟蹤儀設置完成後可以自動跟蹤靶球移動，避免了測量過程中手動找尋靶球位置的繁瑣工作。
附圖說明
為了更清楚地說明本發明實施例或現有技術中的技術方案，下面將對實施例或現有技術描述中所需要使用的附圖作簡單地介紹，顯而易見地，下面描述中的附圖是本發明的一些實施例，對於本領域普通技術人員來講，在不付出創造性勞動的前提下，還可以根據這些附圖獲得其他的附圖。
圖1是本發明實施例提供的一種基於雷射跟蹤儀的地面二維碼標定方法的流程示意圖；
圖2是本發明實施例提供的一種基於雷射跟蹤儀的地面二維碼標定方法中靶球坐標系的示意圖；
圖3是本發明實施例提供的一種基於雷射跟蹤儀的地面二維碼標定方法中雷射跟蹤儀坐標系的示意圖；
圖4是本發明實施例提供的一種基於雷射跟蹤儀的地面二維碼標定方法中二維碼坐標系的示意圖；
圖5是本發明實施例提供的一種基於雷射跟蹤儀的地面二維碼標定方法中攝像機坐標系的示意圖；
圖6是本發明實施例提供的一種基於雷射跟蹤儀的地面二維碼標定方法中靶球坐標系和攝像機坐標繫結合的示意圖；
圖7是本發明實施例提供的一種基於雷射跟蹤儀的地面二維碼標定裝置的結構示意圖。
具體實施方式
為使本發明實施例的目的、技術方案和優點更加清楚，下面將結合本發明實施例中的附圖，對本發明實施例中的技術方案進行清楚、完整的描述，顯然，所描述的實施例是本發明一部分實施例，而不是全部的實施例。基於本發明中的實施例，本領域普通技術人員在沒有作出創造性勞動前提下所獲得的所有其他實施例，都屬於本發明保護的範圍。
隨著二維碼技術的發展，二維碼也應用到了工業領域，二維碼在AGV路徑引導系統中的定位修正時，二維碼粘貼過程會導致粘貼的二維碼位姿信息與二維碼自帶的預設論位姿信息存在位姿偏差，為滿足AGV路徑引導系統定位精度的要求，地面二維碼的位姿測量及誤差計算成為二維碼定位技術中的一個關鍵問題。為解決上述技術問題，本發明實施例提供一種基於雷射跟蹤儀的地面二維碼標定方法及裝置。
本發明實施例一提供一種基於雷射跟蹤儀的地面二維碼標定方法，參見圖1，該方法包括：
S101：建立雷射跟蹤儀坐標系A、二維碼坐標系B、攝像機坐標系C、靶球坐標系D和世界坐標系H，其中，二維碼預設位姿的坐標係為世界坐標系H；
在本步驟中，設有4個在同一平面的靶球P0、靶球P1、靶球P2和靶球P3，靶球P1、靶球P2和靶球P3分布在靶球P0的周圍；參見圖2，所述靶球坐標系D以靶球P0為原點，向量的方向為靶球坐標系D的x軸方向，向量的方向為靶球坐標系D的y軸方向，同時垂直於x軸方向和y軸方向為z軸方向。參見圖3，雷射跟蹤儀坐標系A由雷射跟蹤儀自行設定；參見圖4，二維碼坐標系B以二維碼中心為原點，平行於相鄰兩條邊線的方向分別為x軸與y軸方向，垂直於二維碼向上為z軸方向；參見圖5和圖6，攝像機坐標系C以鏡頭中心為原點，垂直於鏡頭中軸為z軸方向，平行於鏡頭為x軸方向，同時垂直於x軸方向和z軸方向為y軸方向。
S102：計算：
雷射跟蹤儀坐標系A和靶球坐標系D之間的變換矩陣
靶球坐標系D和攝像機坐標系C之間的變換矩陣
世界坐標系H和雷射跟蹤儀坐標系A之間的變換矩陣
攝像機坐標系C和二維碼坐標系B之間的變換矩陣
在本步驟中，通過雷射跟蹤儀擺正校準，組裝靶球-攝像機固定架與攝像機；用攝像機標定棋盤格位姿，記錄攝像機坐標系C與棋盤格坐標系E之間的變換矩陣為旋轉矩陣，為平移矩陣；
依次將4個靶球放置在靶球-攝像機固定架上的卡槽P0,P1,P2,P3內，用雷射跟蹤儀測量4個靶球在雷射跟蹤儀坐標系內的位置坐標AP0，AP1,AP2,AP3；通過下式計算靶球P1,P2,P3在靶球坐標系中的坐標DP1,DP2,DP3：
DPk＝APk-AP0(k＝1,2,3)
此處，各坐標表示為3×1矩陣形式，即[x,y,z]T；
由APk＝R2-1·DPk+P2-1(k＝1,2,3)可得
令可得
所以，
移動靶球-攝像機固定架；重新記錄攝像機坐標系C與棋盤格坐標系E之間變換矩陣重新計算雷射跟蹤儀坐標系A與靶球坐標系D之間的變換矩陣
計算靶球-攝像機固定架移動前後，攝像機坐標系C、靶球坐標系D分別的變換矩陣：計算各自旋轉軸與(3×1矩陣)；再次移動靶球-攝像機固定架得到一組新的變換矩陣與和旋轉軸與旋轉軸計算公式如下：
由得，
由R·n＝n及式(1)，得同理，所以：
由式(2)得所以
以二維碼預設坐標所在坐標係為世界坐標系H，根據實際情況選取世界坐標系H內具有代表性的3個點，且已知世界坐標系H內坐標，記為HP1,HP2,HP3；
用雷射跟蹤儀分別測量3個點的坐標，記為AP1,AP2,AP3；
由可得
令可得
所以，
移動靶球-攝像機固定架至二維碼正上方，攝像機標定二維碼，記錄攝像機坐標系與二維碼坐標系間的變換矩陣
用雷射跟蹤儀測量此時3個靶球在雷射測距儀坐標系A內的坐標AP1,AP2,AP3並記錄，此時
由可得
令可得
所以，
S103：根據變換矩陣變換矩陣變換矩陣和變換矩陣計算世界坐標系H和二維碼坐標系B之間的變換矩陣
在本步驟中，採用公式計算世界坐標系H與二維碼坐標系B之間的變換矩陣
S104：通過變換矩陣計算二維碼的位姿及位姿誤差。
在本步驟中，二維碼坐標系B中二維碼中心位置坐標表示為4×1矩陣，即BP0＝[0 0 0 1]T，計算二維碼中心坐標：此時，HP0＝[x0 y0 z0 1]T，取(x0,y0)；
二維碼實際坐標值記為(xc,yc)，即預設值記為(xt,yt)，則誤差值為Δx＝xc-xt,Δy＝yc-yt；
計算二維碼角度誤差：測量在水平地面進行，繞x軸、y軸旋轉角度忽略不計，繞z軸旋轉角度記為θ，則：
上述是測量一個二維碼坐標位姿及位姿誤差的步驟，若測量多個二維碼坐標的位姿及位姿誤差，需重複進行測量與數據記錄工作，計算過程可在測量全部結束後應用MATLAB讀取excel表格功能進行數據批量處理，簡化工作。
從上述描述可知，本發明提供的一種基於雷射跟蹤儀的地面二維碼標定方法，實現了雷射跟蹤儀的自動捕捉功能與數據批量處理過程簡化了操作；攝像機標定二維碼位姿減小了人工標定二維碼位姿的隨機誤差，提高了二維碼位姿測量精度。
本發明實施例二提供一種基於雷射跟蹤儀的地面二維碼標定裝置，參見圖7，該裝置包括：雷射跟蹤儀、靶球、攝像機、靶球-攝像機固定架和計算機；
所述靶球和所述攝像機設置在所述靶球-攝像機固定架上，所述攝像機的鏡頭向下，在所述攝像機鏡頭的正上方的靶球-攝像機固定架上設有4個靶球；所述攝像機與所述計算機相連接，所述雷射跟蹤儀與所述計算機相連接。
從上述描述可知，本發明提供的一種基於雷射跟蹤儀的地面二維碼標定裝置，結構簡單，減小了機械加工和安裝造成的尺寸誤差；並且雷射跟蹤儀設置完成後可以自動跟蹤靶球移動，避免了測量過程中手動找尋靶球位置的繁瑣工作。
以上實施例僅用於說明本發明的技術方案，而非對其限制；儘管參照前述實施例對本發明進行了詳細的說明，本領域的普通技術人員應當理解：其依然可以對前述各實施例所記載的技術方案進行修改，或者對其中部分技術特徵進行等同替換；而這些修改或替換，並不使相應技術方案的本質脫離本發明各實施例技術方案的精神和範圍。