CN104655133A - 一种车载升降桅杆平台姿态动态测量方法 - Google Patents

Abstract

本发明提出一种车载升降桅杆平台姿态动态测量方法，在传统以静态标定方法获取平台姿态变化量的基础上，引入了动态测量算法，即数据处理系统利用倾角传感器输出的桅杆平台姿态数据、车载惯性导航系统输出的车体姿态、地速和地理位置信息，稳定转塔输出的目标指向数据、测姿用高精度陀螺和稳定转塔伺服陀螺输出的角速度信息、桅杆控制箱输出的举升高度数据构建姿态更新矩阵，通过实时结算实现对桅杆平台姿态的动态测量。本发明能够实时精确测量桅杆举升后桅杆自身扭动、倾斜和弯曲等因素以及风力等外部扰动所带来的桅杆平台姿态变化量，从而有效减少桅杆姿态变化对车载侦察系统的目标定位精度的影响。

Description

一种车载升降桅杆平台姿态动态测量方法

技术领域

本发明属于光电侦察技术领域，主要涉及一种测量方法，尤其涉及一种用于测量车载升降桅杆平台姿态的动态测量方法。

背景技术

近些年来，车载侦察系统出现了应用升降桅杆的新方案，即光电侦察设备安置在桅杆的顶端平台上，工作时通过桅杆的升降功能，减少地物遮挡、地球曲率等因素的影响，达到增加系统可视作用距离的目的。

车载侦察系统的光电设备多以稳定转台形式安装在桅杆顶端平台上，一般含有测角、测距传感器，能够输出被测目标相对转台基座的方位、俯仰和距离值；同时一般会采用两轴转塔结构，其伺服系统采用一组陀螺来测量外部扰动，这一组陀螺(对两轴转塔需要两个陀螺)的典型温漂一般是0.2°/h左右。此时，车载侦察系统的定向测量精度仍然受桅杆举升后自身变形等不确定性因素影响，造成目标定位定向精度的下降，尤其是在车辆行进间工况下。

随着科学技术的进步和使用要求的提高，对车载侦察系统的目标定位定向精度提出了更高的要求，而桅杆姿态变化对其造成的影响日益突出。如何对车载侦察系统姿态进行准确、快速的测量，特别是对车载侦察系统在载车行进间进行姿态测量，成为急需解决的问题。

在实际应用中通常采用在光电侦察设备旁或下面安装惯性导航装置或寻北仪来避免桅杆姿态变化带来的误差问题，这种方式的成本较高，需要三个导航级的陀螺(一般精度需要达到0.02°/h，以达到约1mil的寻北精度)及配套的传感器、信号处理电路等。也可采用静态标定方法获取平台姿态变化量，该方法使用两轴倾角传感器，在载车驻车状态下分别测量桅杆未举升和举升至最高点的平台姿态数据，将两者的角度差值作为系统姿态变化量。该方法直接忽略了桅杆举升后自身姿态变化对侦察系统精度产生的影响，不适用于桅杆平台姿态的动态测量。中国期刊文章《基于模糊加权EKF的车载光电桅杆姿态估计算法》中提出了一种基于模糊加权的姿态估算算法，该算法将陀螺仪和倾角传感器输出的数据进行融合处理，通过构建姿态估计方程实现对车载桅杆式光电平台静动态情况下的姿态测量。国外期刊文章《Design of multi-sensordetermination systems》描述了一种多传感器的惯性导航系统，该系统由陀螺仪、加速度计和数字处理电路组成。但目前的技术都没有能够有效解决桅杆举升后桅杆自身扭动、倾斜和弯曲等因素以及风力等外部扰动所带来的姿态变化对车载侦察系统带来影响的问题。

发明内容

本发明要解决的技术问题是，针对现有技术存在的问题，为桅杆式车载升降桅杆平台提供一种姿态动态测量方法。

本发明的技术方案为：

所述一种车载升降桅杆平台姿态动态测量方法，其特征在于：该测量方法基于的装置包括测姿用高精度陀螺、伺服用陀螺、两轴倾角传感器；在桅杆平台上安装测姿用高精度陀螺和两轴倾角传感器，稳定转塔上安装有两个伺服用陀螺，稳定转塔安装在桅杆平台；测量方法包括以下步骤：

步骤1：查询车速是否为0，如果为假，等待；如果为真，执行步骤2；

步骤2：查询桅杆举升高度是否为0，如果为假，等待；如果为真，执行步骤3；

步骤3：同步采集倾角传感器输出的桅杆平台姿态数据(ψ_y,ψ_x)和车载惯性导航系统输出的车体姿态(y,p,r)并存入缓存，根据下式进行桅杆平台初始姿态(e_x,e_y,e_z)计算：

e_x＝y+ξ_z

e_y＝p-ψ_y+ξ_y

e_z＝r-ψ_x+ξ_x

其中(y,p,r)为车体在导航坐标系下的航向角、俯仰角和横滚角，(ψ_y,ψ_x)为桅杆平台在倾角传感坐标系下的俯仰角和横滚角，(ξ_x,ξ_y,ξ_z)为倾角传感器安装基准与车载惯性导航系统安装基准之间的角度偏差；并根据下式计算桅杆平台初始姿态矩阵Q₁：

$e=\sqrt{{e_x}^2+{e_y}^2+{e_z}^2}$

$Q_1=\frac{1}{e}\left[\begin{array}{c}{e}_x\sin \frac{e}{2}\\ e_y\sin \frac{e}{2}\\ e_z\sin \frac{e}{2}\\ e\cos \frac{e}{2}\end{array}\right];$

步骤4：查询桅杆平台初始姿态装订是否完成，如果为假，等待；如果为真，执行步骤5；

步骤5：同步采集桅杆举升高度数据l、稳定转塔输出的目标指向数据(α,β)、稳定转塔伺服用陀螺输出的角速度信息(ω₁,ω₂)、测姿用高精度陀螺输出的角速度信息ω₃和车载惯性导航系统输出的地速(V_e,V_n,V_u)、位置信息(L,B,H)并存入缓存，其中：(V_e,V_n,V_u)为惯性导航坐标系中的地球运动速度信息，(L,B,H)为车体所在位置的经度、纬度和高度数据；

步骤6：根据以下公式进行姿态解算，生成桅杆平台姿态

h＝H+l

ω_x＝ω₁cosβsinα+ω₂sinα

ω_y＝ω₂cosα+ω₁cosβcosα

ω_z＝ω₃

${\mathrm{\θ}}_x={\∫}_0^{\mathrm{\Δt}}{\mathrm{\ω}}_x\mathrm{dt}+{\∫}_0^{\mathrm{\Δt}}(-\frac{V_n}{M+h})$

${\mathrm{\θ}}_y={\∫}_0^{\mathrm{\Δt}}{\mathrm{\ω}}_y\mathrm{dt}+{\∫}_0^{\mathrm{\Δt}}(\frac{V_e}{N+h}+{\mathrm{\ω}}_x\cos B)$

${\mathrm{\θ}}_z={\∫}_0^{\mathrm{\Δt}}{\mathrm{\ω}}_z\mathrm{dt}+{\∫}_0^{\mathrm{\Δt}}(\frac{V_e\mathrm{tg\; B}}{N+h}+{\mathrm{\ω}}_x\sin B)$

其中，M为地球子午圈曲率半径，N为卯酉圈曲率半径；

|θ|²＝θ_x ²+θ_y ²+θ_y ²

$s=\frac{1}{\left|\mathrm{\θ}\right|}\sin \frac{\left|\mathrm{\θ}\right|}{2}=\frac{1}{2}-\frac{1}{48}{\left|\mathrm{\θ}\right|}^2+\frac{1}{3840}{\left|\mathrm{\θ}\right|}^4+...$

$c=\cos \frac{\left|\mathrm{\θ}\right|}{2}-1=-\frac{1}{8}{\left|\mathrm{\θ}\right|}^2+\frac{1}{384}{\left|\mathrm{\θ}\right|}^4+...$

取 $Q_{k+1}=\left[\begin{array}{c}{q}_2\\ q_3\\ q_4\\ q_1\end{array}\right],$ 将Q₁、s、c、θ_x、θ_y、θ_z代入下式解算：

$Q_{k+1}=Q_k+\left[\begin{array}{cccc}c& s{\mathrm{\θ}}_z& -s{\mathrm{\θ}}_y& s{\mathrm{\θ}}_x\\ -s{\mathrm{\θ}}_z& c& s{\mathrm{\θ}}_x& s{\mathrm{\θ}}_y\\ s{\mathrm{\θ}}_y& -s{\mathrm{\θ}}_x& c& s{\mathrm{\θ}}_z\\ s{\mathrm{\θ}}_x& s{\mathrm{\θ}}_y& s{\mathrm{\θ}}_z& c\end{array}\right]Q_k,k=1...n$

将解算出的q₁、q₂、q₃、q₄代入下式，生成桅杆平台姿态

$R=\left[\begin{array}{ccc}{q}_1^2+q_2^2-q_3^2-q_4^2& 2(q_2{q}_3-q_1{q}_4)& 2(q_2{q}_4+q_1{q}_3)\\ 2(q_2{q}_3+q_1{q}_4)& q_1^2-q_2^2+q_3^2-q_4^2& 2(q_3{q}_4-q_1{q}_2)\\ 2(q_2{q}_4-q_1{q}_3)& 2(q_1{q}_2+q_3{q}_4)& q_1^2-q_2^2-q_3^2+q_4^2\end{array}\right]$

$\mathrm{\φ}=\arctan [-\frac{R\left(\mathrm{1,2}\right)}{R\left(\mathrm{2,2}\right)}]$

$\mathrm{\γ}=\arctan [-\frac{R\left(\mathrm{3,1}\right)}{R(3,3)}]$

步骤7：第七步，按下式计算生成桅杆平台姿态矩阵：

步骤8：查询有无停机指令，如果为假，返回步骤5；如果为真，方法结束。

进一步的优选方案，所述一种车载升降桅杆平台姿态动态测量方法，其特征在于：测姿用高精度陀螺精度为0.02°/h，伺服用陀螺精度为0.2°/h。

有益效果

本发明在传统以静态标定方法获取平台姿态变化量的基础上，引入了动态测量算法，即数据处理系统利用倾角传感器输出的桅杆平台姿态数据、车载惯性导航系统输出的车体姿态、地速和地理位置信息，稳定转塔输出的目标指向数据、测姿用高精度陀螺和稳定转塔伺服陀螺输出的角速度信息、桅杆控制箱输出的举升高度数据构建姿态更新矩阵，通过实时结算实现对桅杆平台姿态的动态测量。本发明能够实时精确测量桅杆举升后桅杆自身扭动、倾斜和弯曲等因素以及风力等外部扰动所带来的桅杆平台姿态变化量，从而有效减少桅杆姿态变化对车载侦察系统的目标定位精度的影响。

附图说明

图1是本发明所提供的用于车载升降桅杆平台的动态测量装置的框架结构示意图。

其中：1-测姿用高精度陀螺、2-两轴倾角传感器、3-数据处理系统、4-稳定转塔、5-桅杆顶端平台、6-载车、7-惯性导航装置。

图2是本发明姿态动态测量方法的操作流程图。

具体实施方式

以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。

正如图1所示，车载桅杆平台姿态动态测量装置包括：测姿用高精度陀螺1、两轴倾角传感器2、数据处理系统3。其中，测姿用高精度陀螺1和两轴倾角传感器2安装在桅杆顶端平台5上，桅杆安装在载车6上。需要特别说明的是：载车一般为装甲车辆并安装有惯性导航装置7；稳定转塔4安装在桅杆顶端平台5；测姿用高精度陀螺精度为0.02°/h，稳定转塔上安装有两个精度0.2°/h的伺服用陀螺；测姿用高精度陀螺1和两轴倾角传感2、惯性导航装置7和两轴倾角传感2之间的安装位置偏差和基准角度偏差在动态测量装置装调时进行标定。测姿用高精度陀螺输出桅杆顶端平台角速度信息，倾角传感器输出桅杆平台初始姿态信息，惯性导航装置输出车体的位置、姿态、速度数据，稳定转塔输出目标指向数据和伺服陀螺角速度信息，上述四组数据同步传输至数据处理系统，由该系统实时解算桅杆平台姿态矩阵并存储在内部寄存器中。

不难看出，数据处理系统是实现本发明姿态动态测量方法的核心，数据处理系统中置有姿态解算软件包。当数据处理系统上电后，姿态解算软件包将按照图2所示的操作流程图，执行以下操作步骤：

步骤1：查询车速是否为0，如果为假，等待；如果为真，执行步骤2；

步骤2：查询桅杆举升高度是否为0，如果为假，等待；如果为真，执行步骤3；

步骤3：同步采集倾角传感器输出的桅杆平台姿态数据(ψ_y,ψ_x)和车载惯性导航系统输出的车体姿态(y,p,r)并存入缓存，根据下式进行桅杆平台初始姿态(e_x,e_y,e_z)计算：

e_x＝y+ξ_z

e_y＝p-ψ_y+ξ_y

e_z＝r-ψ_x+ξ_x

其中(y,p,r)为车体在导航坐标系下的航向角、俯仰角和横滚角，(ψ_y,ψ_x)为桅杆平台在倾角传感坐标系下的俯仰角和横滚角，(ξ_x,ξ_y,ξ_z)为倾角传感器安装基准与车载惯性导航系统安装基准之间的角度偏差；并根据下式计算桅杆平台初始姿态矩阵Q₁：

$e=\sqrt{{e_x}^2+{e_y}^2+{e_z}^2}$

$Q_1=\frac{1}{e}\left[\begin{array}{c}{e}_x\sin \frac{e}{2}\\ e_y\sin \frac{e}{2}\\ e_z\sin \frac{e}{2}\\ e\cos \frac{e}{2}\end{array}\right];$

步骤4：查询桅杆平台初始姿态装订是否完成，如果为假，等待；如果为真，执行步骤5；

步骤5：同步采集桅杆举升高度数据l、稳定转塔输出的目标指向数据(α,β)、稳定转塔伺服用陀螺输出的角速度信息(ω₁,ω₂)、测姿用高精度陀螺输出的角速度信息ω₃和车载惯性导航系统输出的地速(V_e,V_n,V_u)、位置信息(L,B,H)并存入缓存，其中：(V_e,V_n,V_u)为惯性导航坐标系中的地球运动速度信息，(L,B,H)为车体所在位置的经度、纬度和高度数据；

步骤6：根据以下公式进行姿态解算，生成桅杆平台姿态

h＝H+l

ω_x＝ω₁cosβsinα+ω₂sinα

ω_y＝ω₂cosα+ω₁cosβcosα

ω_z＝ω₃

${\mathrm{\θ}}_x={\∫}_0^{\mathrm{\Δt}}{\mathrm{\ω}}_x\mathrm{dt}+{\∫}_0^{\mathrm{\Δt}}(-\frac{V_n}{M+h})$

${\mathrm{\θ}}_y={\∫}_0^{\mathrm{\Δt}}{\mathrm{\ω}}_y\mathrm{dt}+{\∫}_0^{\mathrm{\Δt}}(\frac{V_e}{N+h}+{\mathrm{\ω}}_x\cos B)$

${\mathrm{\θ}}_z={\∫}_0^{\mathrm{\Δt}}{\mathrm{\ω}}_z\mathrm{dt}+{\∫}_0^{\mathrm{\Δt}}(\frac{V_e\mathrm{tg\; B}}{N+h}+{\mathrm{\ω}}_x\sin B)$

其中，M为地球子午圈曲率半径，N为卯酉圈曲率半径；

|θ|²＝θ_x ²+θ_y ²+θ_y ²

$s=\frac{1}{\left|\mathrm{\θ}\right|}\sin \frac{\left|\mathrm{\θ}\right|}{2}=\frac{1}{2}-\frac{1}{48}{\left|\mathrm{\θ}\right|}^2+\frac{1}{3840}{\left|\mathrm{\θ}\right|}^4+...$

$c=\cos \frac{\left|\mathrm{\θ}\right|}{2}-1=-\frac{1}{8}{\left|\mathrm{\θ}\right|}^2+\frac{1}{384}{\left|\mathrm{\θ}\right|}^4+...$

取 $Q_{k+1}=\left[\begin{array}{c}{q}_2\\ q_3\\ q_4\\ q_1\end{array}\right],$ 将Q₁、s、c、θ_x、θ_y、θ_z代入下式解算：

$Q_{k+1}=Q_k+\left[\begin{array}{cccc}c& s{\mathrm{\θ}}_z& -s{\mathrm{\θ}}_y& s{\mathrm{\θ}}_x\\ -s{\mathrm{\θ}}_z& c& s{\mathrm{\θ}}_x& s{\mathrm{\θ}}_y\\ s{\mathrm{\θ}}_y& -s{\mathrm{\θ}}_x& c& s{\mathrm{\θ}}_z\\ s{\mathrm{\θ}}_x& s{\mathrm{\θ}}_y& s{\mathrm{\θ}}_z& c\end{array}\right]Q_k,k=1...n$

将解算出的q₁、q₂、q₃、q₄代入下式，生成桅杆平台姿态

$R=\left[\begin{array}{ccc}{q}_1^2+q_2^2-q_3^2-q_4^2& 2(q_2{q}_3-q_1{q}_4)& 2(q_2{q}_4+q_1{q}_3)\\ 2(q_2{q}_3+q_1{q}_4)& q_1^2-q_2^2+q_3^2-q_4^2& 2(q_3{q}_4-q_1{q}_2)\\ 2(q_2{q}_4-q_1{q}_3)& 2(q_1{q}_2+q_3{q}_4)& q_1^2-q_2^2-q_3^2+q_4^2\end{array}\right]$

$\mathrm{\φ}=\arctan [-\frac{R\left(\mathrm{1,2}\right)}{R\left(\mathrm{2,2}\right)}]$

$\mathrm{\γ}=\arctan [-\frac{R(3,1)}{R(3,3)}]$

步骤7：第七步，按下式计算生成桅杆平台姿态矩阵：

步骤8：查询有无停机指令，如果为假，返回步骤5；如果为真，方法结束。

涉及到的坐标系的定义如下：

WGS84坐标系e定义为：原点O_t位于地球质心；Z_e指向(国际时间局)BIH1984.0定义的协议地球极(CTP)方向；X_e指向BIH1984.0的零度子午面和CTP赤道的交点，Y_e由右手规则确定，并与其他两轴构成右手地心地固(ECEF)直角坐标系。

导航坐标系n定义为：原点O_n为惯性导航装置几何中心；X_n沿参考椭球指向东，Y_n沿椭球子午圈方向并指向北；Z_n沿椭球外法线方向指向天顶。

倾角传感坐标系t：原点在倾角传感器几何中心；Z_t与倾角传感器安装面垂直指向外；Y_t与载车前进方向保持一致；X_t由右手规则确定。

速率陀螺坐标系b：原点为测姿用高精度陀螺和伺服陀螺敏感轴的交点；Z_b与测姿用高精度陀螺安装面垂直指向外；Y_b在稳定转塔处于方位零位时，与俯仰向伺服陀螺敏感轴方向保持一致并指向外；X_b由右手规则确定。

Claims (2)

1.一种车载升降桅杆平台姿态动态测量方法，其特征在于：该测量方法基于的装置包括测姿用高精度陀螺、伺服用陀螺、两轴倾角传感器；在桅杆平台上安装测姿用高精度陀螺和两轴倾角传感器，稳定转塔上安装有两个伺服用陀螺，稳定转塔安装在桅杆平台；测量方法包括以下步骤：

步骤1：查询车速是否为0，如果为假，等待；如果为真，执行步骤2；

步骤2：查询桅杆举升高度是否为0，如果为假，等待；如果为真，执行步骤3；

步骤3：同步采集倾角传感器输出的桅杆平台姿态数据(ψ_y,ψ_x)和车载惯性导航系统输出的车体姿态(y,p,r)并存入缓存，根据下式进行桅杆平台初始姿态(e_x,e_y,e_z)计算：

e_x＝y+ξ_z

e_y＝p-ψ_y+ξ_y

e_z＝r-ψ_x+ξ_x

其中(y,p,r)为车体在导航坐标系下的航向角、俯仰角和横滚角，(ψ_y,ψ_x)为桅杆平台在倾角传感坐标系下的俯仰角和横滚角，(ξ_x,ξ_y,ξ_z)为倾角传感器安装基准与车载惯性导航系统安装基准之间的角度偏差；并根据下式计算桅杆平台初始姿态矩阵Q₁：

$e=\sqrt{{e_x}^2+{e_y}^2+{e_z}^2}$

$Q_1=\frac{1}{e}\left[\begin{array}{c}{e}_x\sin \frac{e}{2}\\ e_y\sin \frac{e}{2}\\ e_z\sin \frac{e}{2}\\ e\cos \frac{e}{2}\end{array}\right];$

步骤4：查询桅杆平台初始姿态装订是否完成，如果为假，等待；如果为真，执行步骤5；

步骤5：同步采集桅杆举升高度数据l、稳定转塔输出的目标指向数据(α,β)、稳定转塔伺服用陀螺输出的角速度信息(ω₁,ω₂)、测姿用高精度陀螺输出的角速度信息ω₃和车载惯性导航系统输出的地速(V_e,V_n,V_u)、位置信息(L,B,H)并存入缓存，其中：(V_e,V_n,V_u)为惯性导航坐标系中的地球运动速度信息，(L,B,H)为车体所在位置的经度、纬度和高度数据；

步骤6：根据以下公式进行姿态解算，生成桅杆平台姿态

h＝H+l

ω_x＝ω₁cosβsinα+ω₂sinα

ω_y＝ω₂cosα+ω₁cosβcosα

ω_z＝ω₃

${\mathrm{\θ}}_x={\∫}_0^{\mathrm{\Δt}}{\mathrm{\ω}}_x\mathrm{dt}+{\∫}_0^{\mathrm{\Δt}}(-\frac{V_n}{M+h})$

${\mathrm{\θ}}_y={\∫}_0^{\mathrm{\Δt}}{\mathrm{\ω}}_y\mathrm{dt}+{\∫}_0^{\mathrm{\Δt}}(\frac{V_e}{N+h}+{\mathrm{\ω}}_x\cos B)$

${\mathrm{\θ}}_z={\∫}_0^{\mathrm{\Δt}}{\mathrm{\ω}}_z\mathrm{dt}+{\∫}_0^{\mathrm{\Δt}}(\frac{V_e\mathrm{tgB}}{N+h}+{\mathrm{\ω}}_x\sin B)$

其中，M为地球子午圈曲率半径，N为卯酉圈曲率半径；

|θ|²＝θ_x ²+θ_y ²+θ_y ²

$s=\frac{1}{\left|\mathrm{\θ}\right|}\sin \frac{\left|\mathrm{\θ}\right|}{2}=\frac{1}{2}-\frac{1}{48}{\left|\mathrm{\θ}\right|}^2+\frac{1}{3840}{\left|\mathrm{\θ}\right|}^4+...$

$c=\cos \frac{\left|\mathrm{\θ}\right|}{2}-1=-\frac{1}{8}{\left|\mathrm{\θ}\right|}^2+\frac{1}{384}{\left|\mathrm{\θ}\right|}^4+...$

取 $Q_{k+1}=\left[\begin{array}{c}{q}_2\\ q_3\\ q_4\\ q_1\end{array}\right],$ 将Q₁、s、c、θ_x、θ_y、θ_z代入下式解算：

$Q_{k+1}=Q_k+\left[\begin{array}{cccc}c& {\mathrm{s\θ}}_z& -{\mathrm{s\θ}}_y& {\mathrm{s\θ}}_x\\ -{\mathrm{s\θ}}_z& c& {\mathrm{s\θ}}_x& {\mathrm{s\θ}}_y\\ {\mathrm{s\θ}}_y& -{\mathrm{s\θ}}_x& c& {\mathrm{s\θ}}_z\\ {\mathrm{s\θ}}_x& {\mathrm{s\θ}}_y& {\mathrm{s\θ}}_z& c\end{array}\right]Q_k,k=1...n$

将解算出的q₁、q₂、q₃、q₄代入下式，生成桅杆平台姿态

$R=\left[\begin{array}{ccc}{q}_1^2+q_2^2-q_3^2-q_4^2& 2(q_2{q}_3-q_1{q}_4)& 2(q_2{q}_4+q_1{q}_3)\\ 2(q_2{q}_3+q_1{q}_4)& q_1^2-q_2^2+q_3^2-q_4^2& 2(q_3{q}_4-q_2{q}_2)\\ 2(q_2{q}_4-q_1{q}_3)& 2(q_1{q}_2+q_3{q}_4)& q_1^2-q_2^2-q_3^2+q_4^2\end{array}\right]$

$\mathrm{\φ}=\arctan [-\frac{R\left(\mathrm{1,2}\right)}{R\left(\mathrm{2,2}\right)}]$

$\mathrm{\γ}=\arctan [-\frac{R\left(\mathrm{3,1}\right)}{R\left(\mathrm{3,3}\right)}]$

步骤7：第七步，按下式计算生成桅杆平台姿态矩阵：

步骤8：查询有无停机指令，如果为假，返回步骤5；如果为真，方法结束。

2.根据权利要求1所述一种车载升降桅杆平台姿态动态测量方法，其特征在于：测姿用高精度陀螺精度为0.02°/h，伺服用陀螺精度为0.2°/h。