实现系统自动引导的过程

测量前需对双经纬仪系统进行定向，定向过程需要手动完成。采用精确互瞄和光束平差两种方法完成定向^[5-7]，定向后通过人工瞄准精密转台末端平台上固定的靶标球实现引导跟踪模型的标定，从而确定相机视场和经纬仪视准轴方位的实时关系，驱动经纬仪激光点进入相机视场内^[8]。

3.1.1 精密转台坐标系参数解算

相机固定在精密转台的末端平台即俯仰旋转平台上，转台垂直俯仰机构固定在水平旋转机构上，垂直转轴和水平转轴异面垂直。设精密转台相对初始位置的水平旋转角为θ_z，俯仰旋转角为θ_x，水平垂直转轴间距为dL，如图2所示。设转台初始坐标系为xyz，经水平俯仰旋转后其末端平台坐标系为x^*y^*z^*，则转台初始坐标系与其末端平台坐标系的关系为

（xyz 1）′=（x^*y^*z^* 1）′ （1）

如图2所示，在转台末端平台上固定一球形靶标，借助靶标可标定出经纬仪坐标系和转台初始坐标系的关系。先后令转台水平和垂直旋转4个位置并用经纬仪系统测量各位置处靶标球坐标，利用最小二乘法分别拟合出转台的竖直旋转轴z轴参数和水平旋转轴x″轴参数，并进一步求解出x轴参数和y轴参数，从而得到转台初始坐标系xyz为双经纬仪坐标系间的转换矩阵T_dr0，即有

（x_dy_dz_d 1）′=T_dr0（x^*y^*z^*）′ （3）

在T_dr0中，（i_x，j_x，k_x）为水平轴矢量，（i_y，j_y，k_y）为垂直轴矢量，（x₀，y₀，z₀）为初始水平垂直轴间方位参数。

图2 精密二维转台水平垂直旋转示意图 Fig.2 The schematic diagram of the precision rotation stage

3.1.2 相机坐标系参数解算及实时跟踪的实现

转台末端平台坐标系与整体经纬仪坐标系的实时转换关系确定后，通过求解相机坐标系与末端平台坐标系的转换关系即可得到相机相对于整体坐标系的实时方位。系统采用一平面靶标完成相机和精密转台的标定过程，将靶标置于被测区域内固定，旋转精密转台使靶标位于相机视场中心，设此时精密转台相对原始位置绕垂直轴和水平轴的旋转角度分别为θ_z0与θ_x0，将θ_z0和θ_x0代入式（1）中得到转换矩阵T_r0，即有(https://www.xing528.com)

（xyz 1）′=T_r0（x₀^*y₀^*z₀^* 1）′ （5）

利用张正友标定法解算出相机内外参数，由相机外参得到相机坐标系和平面靶标坐标系的转换矩阵T_ct，用经纬仪人工瞄准平面靶标上4点，利用4个公共点在平面靶标坐标系和双经纬仪坐标系下的坐标通过最小二乘法求出两者转换矩阵T_td，得到此时相机坐标系与双经纬仪坐标系转换矩阵T_cd

（x_cy_cz_c 1）′=T_cd（x_dy_dz_d）′ （6）

T_cd=T_cth_td （7）

则相机坐标系与转台末端平台坐标系关系为

（x_cy_cz_c 1）′=T_cr（x₀^*y₀^*z₀^* 1）′ （8）

T_cr=T_cdT_dr0T_r0 （9）

当转台带动相机对任一被测区域进行扫描时（设转台分别绕垂直转轴和水平转轴旋转了θ_z与θ_x角度），相机实时坐标系与双经纬仪坐标系的实时关系为

（x_dy_dz_d 1）′=T_dr0T_rpinv（T_cr）（x_cy_cz_c 1）′ （10）

由式（10）可以得到相机坐标系原点O（x_cc0，y_cc0，z_cc0）及其主光轴方向矢量V（i_cc0，j_cc0，k_cc0）在经纬仪坐标系下的参数。设相机主光轴上任意点P（P_x，P_y，P_z）与相机坐标系原点距离为m_len，则有

（P_x，P_y，P_z）=（x_cc0，y_cc0，z_cc0）+m_len（i_cc0，j_cc0，k_cc0） （11）

跟踪引导实现过程中，设定相机与被测物距离的初始步长值为m_len，由m_len得到初始跟踪点坐标，进而解算出双经纬仪角度值并马达驱动使其对准跟踪点坐标。根据激光点是否在相机视场中进一步更改步长值，直到激光点位于相机视场为止。