车载全站仪位姿估计方法研究

吴维军,朱洪涛,曹娟华,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)通过车载全站仪的位姿估计实现全站仪免置平全自动设站，对提高设备测量效率和测量智能化与自动化具有重要意义。