車載全站儀位姿估計(jì)方法研究

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

(1.南昌大學(xué)機(jī)電工程學(xué)院,南昌 330031； 2.江西制造職業(yè)技術(shù)學(xué)院,南昌 330095； 3.江西農(nóng)業(yè)大學(xué)軟件學(xué)院,南昌 330045； 4.江西日月明測(cè)控科技股份有限公司,南昌 330029)

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

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

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

1 車載全站儀位姿模型

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

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

當(dāng)推行一定距離后，打開剎車，小車處于靜止?fàn)顟B(tài)，進(jìn)行全站儀免置平設(shè)站，即：全站儀在免置平狀態(tài)下測(cè)量多個(gè)(通常為8個(gè))CPⅢ控制點(diǎn)，根據(jù)CPⅢ控制點(diǎn)測(cè)量數(shù)據(jù)和大地坐標(biāo)系下已知的設(shè)計(jì)數(shù)據(jù)，基于平差方法計(jì)算出高精度的站點(diǎn)信息。站點(diǎn)信息包括全站儀位置信息和姿態(tài)信息，是聯(lián)系全站儀坐標(biāo)系與大地坐標(biāo)系的關(guān)鍵參數(shù)，根據(jù)公式(1)：當(dāng)全站儀測(cè)量任意一個(gè)未知點(diǎn)，測(cè)量坐標(biāo)(x，y，z)，根據(jù)全站儀站點(diǎn)信息可計(jì)算該未知點(diǎn)在大地坐標(biāo)系下的坐標(biāo)(X，Y，Z)。

(1)

矩陣R是坐標(biāo)旋轉(zhuǎn)矩陣，是全站儀的姿態(tài)的數(shù)學(xué)表示。其姿態(tài)信息主要包括3個(gè)參數(shù)，方向角φ為全站儀坐標(biāo)系x軸與大地正北方向的夾角，傾斜角η為全站儀坐標(biāo)系x軸與水平面的夾角，傾斜角ω為全站儀坐標(biāo)系y軸與水平面的夾角，根據(jù)空間坐標(biāo)變換原理[8]，全站儀姿態(tài)矩陣R中元素都是關(guān)于這3個(gè)角度的三角函數(shù)

(2)

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

(3)

其中

(4)

2 車載全站儀位姿估計(jì)

全站儀按預(yù)先規(guī)定的方向安裝于小車底座，減小了全站儀位置的各向互換性偏差。軌檢儀坐標(biāo)系原點(diǎn)和全站儀坐標(biāo)系原點(diǎn)都位于全站儀站點(diǎn)位置，兩坐標(biāo)系之間的坐標(biāo)轉(zhuǎn)換僅存在坐標(biāo)旋轉(zhuǎn)

(5)

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

(6)

聯(lián)立式(1)和式(6)，得到以下關(guān)系式

(7)

(8)

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

L=L0+l

(9)

(10)

利用測(cè)量過程中全站儀與小車之間的位置關(guān)系不變，即坐標(biāo)旋轉(zhuǎn)矩陣Rs不變

(11)

(12)

(13)

根據(jù)全站儀角度與坐標(biāo)之間的轉(zhuǎn)換關(guān)系，計(jì)算目標(biāo)棱鏡的水平角Ah和垂直角Av[13]

(14)

(15)

調(diào)用控制子程序?qū)⑷緝x快速地旋轉(zhuǎn)到指定位置，照準(zhǔn)棱鏡，完成該目標(biāo)棱鏡的自動(dòng)測(cè)量。是否能實(shí)現(xiàn)全站儀免置平設(shè)站中棱鏡點(diǎn)的自動(dòng)照準(zhǔn)和測(cè)量，核心在于全站儀的位置和姿態(tài)的精確估計(jì)。

3 目標(biāo)棱鏡的旋轉(zhuǎn)照準(zhǔn)誤差分析

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

(16)

不考慮CPⅢ網(wǎng)誤差的情況下，其誤差主要來源于第1次定位測(cè)量中全站儀免置平設(shè)站的旋轉(zhuǎn)矩陣誤差和軌檢儀坐標(biāo)系旋轉(zhuǎn)矩陣誤差、第i次定位測(cè)量中軌檢儀坐標(biāo)系旋轉(zhuǎn)矩陣誤差和第i次設(shè)站測(cè)量中第1個(gè)CPⅢ控制點(diǎn)的坐標(biāo)測(cè)量誤差。

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

(17)

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

根據(jù)空間坐標(biāo)變換原理，矩陣R-1是矩陣R的逆運(yùn)算，坐標(biāo)左乘R-1物理意義是該點(diǎn)所在坐標(biāo)系依次逆序和反向繞軸旋轉(zhuǎn)，即

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

(18)

(19)

(20)

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

(21)

軌檢儀超高測(cè)量項(xiàng)目的示值誤差為Δh，可計(jì)算出軌檢儀傾角測(cè)量誤差Δβ′[16]

(22)

根據(jù)軌檢儀超高測(cè)量項(xiàng)目示值誤差小于0.3 mm可知[17]，傾角測(cè)量誤差Δβ′≤0.012°。

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

(23)

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

4 線路試驗(yàn)

根據(jù)車載全站儀位姿估計(jì)方法進(jìn)行程序設(shè)計(jì)，程序流程如圖2所示。

圖2 車載全站儀位姿估計(jì)程序流程

儀器：基于車載全站儀軌道精測(cè)系統(tǒng)樣機(jī)(含Leica TS60 全站儀1臺(tái))。

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

實(shí)驗(yàn)方法如下。

(1)軌道設(shè)計(jì)線形、CPⅢ設(shè)計(jì)坐標(biāo)等資料的準(zhǔn)備和數(shù)據(jù)導(dǎo)入。

(2)新建測(cè)量文件，進(jìn)行第1次全站儀免置平手動(dòng)設(shè)站和軌道中線坐標(biāo)計(jì)算，求出全站儀坐標(biāo)系與軌檢儀坐標(biāo)系間的坐標(biāo)旋轉(zhuǎn)矩陣RS。

(4)通過全站儀位置和姿態(tài)的估計(jì)，計(jì)算CPⅢ控制點(diǎn)在全站儀坐標(biāo)系下水平角和垂直角，控制全站儀在免置平狀態(tài)下自動(dòng)照準(zhǔn)棱鏡，完成免置平自動(dòng)設(shè)站，記錄各CPⅢ控制點(diǎn)的水平角誤差和垂直角誤差。

(5)重復(fù)步驟(3)和(4)，進(jìn)行第3～10站的全站儀免置平自動(dòng)設(shè)站，并記錄每次設(shè)站中每個(gè)CPⅢ控制點(diǎn)的水平角誤差和垂直角誤差。

(24)

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

表1 線路實(shí)驗(yàn)數(shù)據(jù) (°)

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

5 結(jié)論

(1)根據(jù)空間坐標(biāo)變換原理和方法建立車載全站儀和軌道檢查儀的姿態(tài)模型，以全站儀坐標(biāo)系、軌檢儀坐標(biāo)系和大地坐標(biāo)系三者之間的位姿關(guān)系為基礎(chǔ)，構(gòu)建移動(dòng)站車載全站儀的位姿估計(jì)方法，原理清晰，意義明確。

(2)對(duì)車載全站儀位姿進(jìn)行準(zhǔn)確估計(jì)是實(shí)現(xiàn)車載全站儀免置平自動(dòng)設(shè)站的關(guān)鍵，理論分析可知，全站儀坐標(biāo)系下目標(biāo)棱鏡的水平角和垂直角計(jì)算誤差小于各坐標(biāo)旋轉(zhuǎn)角誤差的代數(shù)累加和0.606，全站儀能在ATR窗口范圍內(nèi)搜索到目標(biāo)棱鏡，完成自動(dòng)照準(zhǔn)工作。

(3)在線路試驗(yàn)中，全站儀能實(shí)現(xiàn)CPⅢ控制點(diǎn)的自動(dòng)照準(zhǔn)和測(cè)量，說明車載全站儀位姿估計(jì)精度可以滿足要求。CPⅢ控制點(diǎn)水平角和垂直角照準(zhǔn)中誤差均小于0.25，有利于全站儀在設(shè)站過程中快速、精確地照準(zhǔn)和測(cè)量。

(4)通過車載全站儀的位姿估計(jì)實(shí)現(xiàn)全站儀免置平全自動(dòng)設(shè)站，對(duì)提高設(shè)備測(cè)量效率和測(cè)量智能化與自動(dòng)化具有重要意義。