[发明专利]一种移动站车载全站仪位姿估计方法

权利要求书

1.一种移动站车载全站仪位姿估计方法，其特征是按如下步骤：

(1)测量并计算车载全站仪和轨检仪的初始姿态

建立轨检仪独立坐标系O'-X'Y'Z'，其以全站仪站点(X_s,Y_s,Z_s)为原点，以X'轴正方向为轨道前进方向，Z'轴垂直于轨道所在平面，向上为正方向，Y'轴在轨道所在平面内，与X'轴和Z'轴正交，符合空间直角坐标系左手定则；轨检仪坐标系的正北方向角θ'、俯仰角α'通过轨道设计线形计算得到，而水平倾斜角β'通过水平传感器测量得到；根据空间坐标转换关系，得到轨检仪坐标系O'-X'Y'Z'与大地坐标系O-XYZ的坐标转换关系：

其中，R′为轨检仪姿态矩阵，X、Y、Z为大地坐标系下的坐标，X′、Y′、Z′为轨检仪坐标系下的坐标，X_s、Y_s、Z_s为全站仪站点在大地坐标系下的坐标；

建立全站仪坐标系与大地坐标系之间的坐标转换关系：

其中，R为全站仪姿态矩阵，x、y、z为全站仪坐标系下的坐标；

(2)基于全站仪位姿模型的位姿估计

通过全站仪姿态矩阵R和轨检仪姿态矩阵R′，求解全站仪坐标系到轨检仪坐标系的坐标旋转矩阵R_s：

R_s＝(R')^-1R (3)

当轨检仪推至新的轨道位置时，根据里程增量△l，得到全站仪当前点里程l，再由此查询轨道设计线形中坡度表和曲线表，得到当前轨道坡度α_i和轨道方向角θ_i，再结合轨检仪水平传感器测量值得到当前水平角β_i，求解当前轨检仪坐标系与大地坐标系的坐标旋转矩阵R'_i：

测量过程中坐标旋转矩阵R_s保持不变，得到全站仪当前的姿态矩阵：

R_i＝R'_iR_s (5)

控制全站仪对所跟踪的CPIII控制点进行测量，得到全站仪坐标系下该CPIII控制点的坐标(x,y,z)，根据公式(6)计算出全站仪当前站点坐标(X_si,Y_si,Z_si)：