[发明专利]一种基于陀螺全站仪－激光标靶的盾构机自动导向系统的在线标定方法

权利要求书

1、一种基于陀螺全站仪-激光标靶的盾构机自动导向系统的在线标定方法，其特征在于包括下列步骤：

(1)捷联式陀螺寻北仪进行初始对准，确定捷联式陀螺寻北仪的初始水平方位角初始俯仰角θ₀和初始横滚角γ₀；

(2)具有自动锁定目标功能的激光全站仪发射激光到激光标靶下部的反射棱镜，然后自动锁定激光标靶下部的反射棱镜，并连续地测量捷联式陀螺寻北仪的水平方位角测量结果用符号表示；

(3)捷联式陀螺寻北仪进入捷联姿态解算状态，利用步骤(1)计算得到的初始水平方位角初始俯仰角θ₀和初始横滚角γ₀初始化捷联姿态解算状态，然后根据陀螺寻北仪中陀螺的输出实时连续解算捷联式陀螺寻北仪的水平方位角俯仰角θ和横滚角γ；

(4)采用捷联式陀螺寻北仪中的两个加速度的输出计算出俯仰角θ和横滚角γ，计算结果用θ_a和γ_a表示；

(5)将步骤(3)计算得到的θ和γ分别与步骤(2)计算得到的步骤(4)计算得到的θ_a和γ_a相减，然后将θ-θ_a和γ-γ_a作为观测量，采用卡尔曼滤波器在线估计陀螺漂移，并在捷联式陀螺寻北仪中进行补偿。

2、根据权利要求1所述的基于陀螺全站仪-激光标靶的盾构机自动导向系统的在线标定方法，其特征在于：步骤(3)所述的捷联姿态解算状态的具体步骤为：

(1)根据捷联式陀螺寻北仪初始对准得到的捷联式陀螺寻北仪的初始水平方位角初始俯仰角θ₀和初始横滚角γ₀计算初始姿态矩阵和四元数q(0)，计算公式如下：

令Cbn=T11T12T13T21T22T23T31T32T33]]>

则有：

q0(0)=±121+T11+T22-T33]]>

q1(0)=±121+T11-T22-T33]]>

q2(0)=±121-T11+T22-T33]]>

q3(0)=±121-T11-T22+T33]]>

(2)利用θ₀和γ₀计算捷联式陀螺寻北仪的z轴角速度增量Δθ_z：

(3)更新四元数和姿态矩阵

q(n+1)={(1-(Δθ0)28+(Δθ0)4384)I+(12-(Δθ0)248)(Δθ)}q(n)]]>

其中，

Δθ=0-Δθx-Δθy-ΔθzΔθx0Δθz-ΔθyΔθy-Δθz0ΔθxΔθzΔθy-Δθx0]]>

Δθ0=Δθx2+Δθy2+Δθz2]]>

姿态矩阵的更新公式如下：

Cbn=T11T12T13T21T22T23T31T32T33=q02+q12-q22-q322(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q02-q12+q22-q322(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)q02-q12-q22+q32;]]>

(4)计算捷联式陀螺寻北仪的水平方位角

主值判断如下表

(5)计算捷联式陀螺寻北仪的俯仰角θ：

θ＝sin^-1(T₂₃)；

(6)计算捷联式陀螺寻北仪的横滚角γ：

γ=tg-1(-T31T33)]]>

主值判断如下表

 

T₃₃T₃₁γ真值γ所在象限+-γ第一象限--γ+π第二象限-+γ+π第三象限++γ第四象限

捷联姿态解算完毕。