車載全站儀位姿估計方法研究

吳維軍,朱洪濤,曹娟華,2,熊麗娟,殷 華,趙書琦

(1.南昌大學機電工程學院,南昌 330031； 2.江西制造職業技術學院,南昌 330095； 3.江西農業大學軟件學院,南昌 330045； 4.江西日月明測控科技股份有限公司,南昌 330029)

基于現有“相對+絕對”軌道精測模式，采用少量的軌道測量儀定位數據對軌道檢查儀慣性軌跡進行坐標約束，有利于更好地控制軌道絕對位置和軌道平順性[1，2]。但在該模式中，軌道測量儀和軌道檢查儀的數據采集是獨立的，設備成本、人力成本和時間成本較大。因此，為降低成本、提升效率，將全站儀固定于軌道檢查儀上，以CPⅢ控制網為基準進行全站儀免置平自由設站，可得到高精度的全站儀站點坐標，并以其為小車特征點(結合小車姿態測量成果)測量線路中線坐標。最后，以此對陀螺儀慣性軌跡進行約束，可得到全線軌道的內、外部幾何狀態參數，實現一套設備、一套人馬、一次測量的高效數據采集模式。

為了保證測量精度，全站儀在設站過程中，通常需要對6～8個CPⅢ控制點進行多測回的冗余觀測[3，4]。根據全站儀免置平設站原理，在全站儀自身位置和姿態未知的情況下，至少需要進行4個 CPⅢ控制點的人工照準和測量[5]。完成4個CPⅢ控制點的測量后，才能粗略計算出全站儀的位置和姿態(粗解)，實現自動設站，即以站點位姿粗解為基礎，計算待測CPⅢ控制點相對于全站儀的坐標和角度，實現程序控制下的自動旋轉、自動照準和自動測量，以提高后續冗余基準點及冗余測回的測量效率。其困難在于，車載全站儀安裝于軌道檢查儀上，其視準軸僅高出軌道面約30 cm，人工照準CPⅢ控制點的操作是一件痛苦而費時的工作，車載全站儀每次設站都需要對前4個CPⅢ控制點進行人工照準和測量，這種非完全的自動設站依然存在勞動強度大、系統綜合測量效率低的顯著問題。

開展車載全站儀位姿估計方法研究，是期望能在車載全站儀起始站點設站后，軌道檢查儀推行至后續站點時，能夠利用軌道平縱斷面設計線形及交點坐標等線路設計參數[6]、軌道檢查儀水平及慣性軌跡等相對測量數據[7]、移動過程中全站儀對某一特定點的跟蹤測量等信息，對全站儀的實時位置和姿態進行估計作為站點位姿粗解，實現該站點全部待測控制點多測回測量完全自動設站。該方法使得后續設站點完全擺脫人工操作，可極大地提高車載全站儀軌道精測方法的測量效率和自動化程度。

1 車載全站儀位姿模型

車載全站儀軌道精測系統如圖1所示，全站儀固定安裝于軌道檢查儀上，全站儀與軌檢儀之間的位置關系保持不變。當軌道檢查儀在線路上推行，通過軌向陀螺儀和高低陀螺儀及水平、軌距等輔助傳感器采集線路相對數據，得到線路平面和高程兩個方向的相對軌跡；當推行一定距離后，進行全站儀免置平設站，并以全站儀站點坐標為基礎計算軌道定位數據，用于糾正和約束陀螺儀的相對軌跡，實現軌道絕對位置和相對平順性的綜合測量。

圖1 車載全站儀位姿模型示意

當推行一定距離后，打開剎車，小車處于靜止狀態，進行全站儀免置平設站，即：全站儀在免置平狀態下測量多個(通常為8個)CPⅢ控制點，根據CPⅢ控制點測量數據和大地坐標系下已知的設計數據，基于平差方法計算出高精度的站點信息。站點信息包括全站儀位置信息和姿態信息，是聯系全站儀坐標系與大地坐標系的關鍵參數，根據公式(1)：當全站儀測量任意一個未知點，測量坐標(x，y，z)，根據全站儀站點信息可計算該未知點在大地坐標系下的坐標(X，Y，Z)。

(1)

矩陣R是坐標旋轉矩陣，是全站儀的姿態的數學表示。其姿態信息主要包括3個參數，方向角φ為全站儀坐標系x軸與大地正北方向的夾角，傾斜角η為全站儀坐標系x軸與水平面的夾角，傾斜角ω為全站儀坐標系y軸與水平面的夾角，根據空間坐標變換原理[8]，全站儀姿態矩陣R中元素都是關于這3個角度的三角函數

(2)

通過免置平設站，獲得全站儀的姿態信息和位置信息(站點坐標(Xs，Ys，Zs))，在車載全站儀軌道精測方法中，以其站點作為小車的特征點，進行軌道中線坐標的測量[9]。如圖1所示：軌檢儀坐標系O′-X′Y′Z′以全站儀站點(Xs，Ys，Zs)為原點，以X′軸正方向為軌道前進方向，Z′軸垂直于軌道所在平面，向上為正方向，Y′軸在軌道所在平面內，與X′軸和Z′軸正交，符合空間直角坐標系左手定則。全站儀安裝于軌檢儀上，全站儀站點與左軌的橫向距離為D，與軌道面垂向距離為H；軌距傳感器位于小車橫梁另一側，軌距測量值為g，在軌檢儀坐標系下，軌道中線坐標為(0，-D+g/2，-H)，根據公式(3)，得到軌道中線坐標(Xc，Yc，Zc)

(3)

其中

(4)

2 車載全站儀位姿估計

全站儀按預先規定的方向安裝于小車底座，減小了全站儀位置的各向互換性偏差。軌檢儀坐標系原點和全站儀坐標系原點都位于全站儀站點位置，兩坐標系之間的坐標轉換僅存在坐標旋轉

(5)

將全站儀坐標系下坐標(x，y，z)轉換為軌檢儀坐標系下的坐標(X′，Y′，Z′)后，通過軌檢儀坐標系與大地坐標系之間的轉換關系(公式(3))，得到大地坐標系下的坐標(X，Y，Z)

(6)

聯立式(1)和式(6)，得到以下關系式

(7)

(8)

完成第1次軌道定位測量后，鎖定其中最為容易跟蹤的CPⅢ控制點，將全站儀設為跟蹤模式。推動軌檢儀前進，進行軌道內部幾何狀態(軌向、高低、軌距、超高等)的采集[10]，推行約40 m，再次將軌檢儀停穩，進行第i個軌道定位點的測量。根據全站儀初始設站后得到的里程L0和軌檢儀里程增量l，可得到全站儀當前點里程[11]

L=L0+l

(9)

(10)

利用測量過程中全站儀與小車之間的位置關系不變，即坐標旋轉矩陣Rs不變

(11)

(12)

(13)

根據全站儀角度與坐標之間的轉換關系，計算目標棱鏡的水平角Ah和垂直角Av[13]

(14)

(15)

調用控制子程序將全站儀快速地旋轉到指定位置，照準棱鏡，完成該目標棱鏡的自動測量。是否能實現全站儀免置平設站中棱鏡點的自動照準和測量，核心在于全站儀的位置和姿態的精確估計。

3 目標棱鏡的旋轉照準誤差分析

目標棱鏡水平角和垂直角計算誤差主要來自于全站儀位置和姿態的估計誤差，而全站儀位置和姿態的估計誤差來源于前期測量和計算，將相關量的計算代入公式(13)并進行整理，得到CPⅢ控制點在全站儀坐標系下的坐標

(16)

不考慮CPⅢ網誤差的情況下，其誤差主要來源于第1次定位測量中全站儀免置平設站的旋轉矩陣誤差和軌檢儀坐標系旋轉矩陣誤差、第i次定位測量中軌檢儀坐標系旋轉矩陣誤差和第i次設站測量中第1個CPⅢ控制點的坐標測量誤差。

R=Rz(θ)Ry(β)Rx(α)

(17)

其中，Rx(α)為坐標系繞x軸旋轉α角；Ry(β)為坐標系繞y軸旋轉β角；Rz(θ)為坐標系繞z軸旋轉θ角。

根據空間坐標變換原理，矩陣R-1是矩陣R的逆運算，坐標左乘R-1物理意義是該點所在坐標系依次逆序和反向繞軸旋轉，即

R-1=Rx(-α)Ry(-β)Rz(-θ)

(18)

(19)

(20)

軌道橫向偏差和垂向偏差的鄰點偏差記為Δd，軌枕間距記為Δl，按公式(21)可計算出軌道方向角和軌道坡度角的近似誤差Δα′或Δθ′[15]

(21)

軌檢儀超高測量項目的示值誤差為Δh，可計算出軌檢儀傾角測量誤差Δβ′[16]

(22)

根據軌檢儀超高測量項目示值誤差小于0.3 mm可知[17]，傾角測量誤差Δβ′≤0.012°。

2×0.134+2×0.157+2×0.012=0.606°

(23)

對車載全站儀的位姿進行估計，其實質是以軌道檢查儀的傾角傳感器輸出、線路的設計參數來獲取的全站儀的姿態，其姿態角度的誤差大小與線路的質量關系密切，但其與全站儀ATR窗口搜索范圍(缺省值為4°)相比，目標棱鏡的旋轉照準精度已經足夠，有助于免置平自動設站快速、順利地進行。

4 線路試驗

根據車載全站儀位姿估計方法進行程序設計，程序流程如圖2所示。

圖2 車載全站儀位姿估計程序流程

儀器：基于車載全站儀軌道精測系統樣機(含Leica TS60 全站儀1臺)。

場地：帶CPⅢ的無砟軌道線路800 m；溫度25～29 ℃；氣壓1 001～1 007 hPa；濕度53%～78%；

實驗方法如下。

(1)軌道設計線形、CPⅢ設計坐標等資料的準備和數據導入。

(2)新建測量文件，進行第1次全站儀免置平手動設站和軌道中線坐標計算，求出全站儀坐標系與軌檢儀坐標系間的坐標旋轉矩陣RS。

(4)通過全站儀位置和姿態的估計，計算CPⅢ控制點在全站儀坐標系下水平角和垂直角，控制全站儀在免置平狀態下自動照準棱鏡，完成免置平自動設站，記錄各CPⅢ控制點的水平角誤差和垂直角誤差。

(5)重復步驟(3)和(4)，進行第3～10站的全站儀免置平自動設站，并記錄每次設站中每個CPⅢ控制點的水平角誤差和垂直角誤差。

(24)

對測量數據進行分析，以測站點為單位，采用白賽爾公式對CPⅢ控制點水平角和垂直角測量中誤差進行計算[20]，同時記錄每測站中水平角和垂直角的最大誤差，記錄于表1中。

表1 線路實驗數據 (°)

在全站儀后續的9次設站中，通過計算得到的水平角和垂直角，順利實現了全站儀對每個CPⅢ控制點的照準和測量，說明車載全站儀位姿估計精度可以滿足全站儀免置平自動設站的需要。同時，由表1可知：水平角誤差中誤差和垂直角中誤差均小于0.25，遠小于全站儀ATR窗口搜索范圍的缺省值4，即便從極值考慮，水平角誤差和垂直角誤差最大值僅為0.35和0.43，能夠保證全站儀在自動旋轉后，快速精確地照準目標棱鏡。

5 結論

(1)根據空間坐標變換原理和方法建立車載全站儀和軌道檢查儀的姿態模型，以全站儀坐標系、軌檢儀坐標系和大地坐標系三者之間的位姿關系為基礎，構建移動站車載全站儀的位姿估計方法，原理清晰，意義明確。

(2)對車載全站儀位姿進行準確估計是實現車載全站儀免置平自動設站的關鍵，理論分析可知，全站儀坐標系下目標棱鏡的水平角和垂直角計算誤差小于各坐標旋轉角誤差的代數累加和0.606，全站儀能在ATR窗口范圍內搜索到目標棱鏡，完成自動照準工作。

(3)在線路試驗中，全站儀能實現CPⅢ控制點的自動照準和測量，說明車載全站儀位姿估計精度可以滿足要求。CPⅢ控制點水平角和垂直角照準中誤差均小于0.25，有利于全站儀在設站過程中快速、精確地照準和測量。

(4)通過車載全站儀的位姿估計實現全站儀免置平全自動設站，對提高設備測量效率和測量智能化與自動化具有重要意義。