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

摘要

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

主权项

一种车载升降桅杆平台姿态动态测量方法，其特征在于：该测量方法基于的装置包括测姿用高精度陀螺、伺服用陀螺、两轴倾角传感器；在桅杆平台上安装测姿用高精度陀螺和两轴倾角传感器，稳定转塔上安装有两个伺服用陀螺，稳定转塔安装在桅杆平台；测量方法包括以下步骤：步骤1：查询车速是否为0，如果为假，等待；如果为真，执行步骤2；步骤2：查询桅杆举升高度是否为0，如果为假，等待；如果为真，执行步骤3；步骤3：同步采集倾角传感器输出的桅杆平台姿态数据(ψ_y,ψ_x)和车载惯性导航系统输出的车体姿态(y,p,r)并存入缓存，根据下式进行桅杆平台初始姿态(e_x,e_y,e_z)计算：e_x＝y+ξ_ze_y＝p‑ψ_y+ξ_ye_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＝ω₃${\theta }_x={\int }_0^{\mathrm{\Delta t}}{\omega }_x\mathrm{dt}+{\int }_0^{\mathrm{\Delta t}}(-\frac{V_n}{M+h})$${\theta }_y={\int }_0^{\mathrm{\Delta t}}{\omega }_y\mathrm{dt}+{\int }_0^{\mathrm{\Delta t}}(\frac{V_e}{N+h}+{\omega }_x\cos B)$${\theta }_z={\int }_0^{\mathrm{\Delta t}}{\omega }_z\mathrm{dt}+{\int }_0^{\mathrm{\Delta t}}(\frac{V_e\mathrm{tgB}}{N+h}+{\omega }_x\sin B)$其中，M为地球子午圈曲率半径，N为卯酉圈曲率半径；|θ|²＝θ_x²+θ_y²+θ_y²$s=\frac{1}{\left|\theta \right|}\sin \frac{\left|\theta \right|}{2}=\frac{1}{2}-\frac{1}{48}{\left|\theta \right|}^2+\frac{1}{3840}{\left|\theta \right|}^4+...$$c=\cos \frac{\left|\theta \right|}{2}-1=-\frac{1}{8}{\left|\theta \right|}^2+\frac{1}{384}{\left|\theta \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\theta }}_z& -{\mathrm{s\theta }}_y& {\mathrm{s\theta }}_x\\ -{\mathrm{s\theta }}_z& c& {\mathrm{s\theta }}_x& {\mathrm{s\theta }}_y\\ {\mathrm{s\theta }}_y& -{\mathrm{s\theta }}_x& c& {\mathrm{s\theta }}_z\\ {\mathrm{s\theta }}_x& {\mathrm{s\theta }}_y& {\mathrm{s\theta }}_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]$$\phi =\arctan [-\frac{R\left(\mathrm{1,2}\right)}{R\left(\mathrm{2,2}\right)}]$$\gamma =\arctan [-\frac{R\left(\mathrm{3,1}\right)}{R\left(\mathrm{3,3}\right)}]$步骤7：第七步，按下式计算生成桅杆平台姿态矩阵：步骤8：查询有无停机指令，如果为假，返回步骤5；如果为真，方法结束。