具体实施方式

为使本发明的目的、内容和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

本实施例提出一种基于捷联惯导动基座对准原理的肩射筒式武器简易火控运动目标自动装表方法，该方法在简易火控中安装一套惯性测量单元(IMU)，流程如图1所示，具体包括如下步骤：

S1、IMU启动，采集IMU的加速度计和陀螺仪数据

S2、根据加速度计和陀螺仪数据，计算瞄准装置倾斜角

①定义坐标系

地面坐标系Axyz：与地面固连，原点A在发射点；Ax轴在水平面内，与瞄准跟踪开始时刻简易火控瞄准纵轴在水平面内的投影平行，向前为正；Ay轴垂直于水平面，向上为正；Az轴与Ax轴和Ay轴形成右手系，简记为“A系”。肩射筒式武器弹药飞行时间短、距离近，地球转动角速度对打击精度的影响可以忽略，可将A系作为惯性坐标系。计算当地重力加速度，建立地面坐标系，确定简易火控相对于水平面的倾斜角，输出给简易火控的显示组件，在目镜中显示倾斜角度，提示射手将火控保持在水平状态；

瞄准线坐标系Ox_cy_cz_c：与简易火控固联，原点O在简易火控瞄准轴线中心；Oz_c轴与瞄准“+”分划的横轴重合；Oy_c轴与“+”分划的竖轴重合；Ox_c轴为瞄准线，向前为正，与Oy_c轴和Oz_c轴形成右手系，简记为“c系”。

IMU坐标系Ox_my_mz_m：与简易火控固联，原点O在IMU中心。理想情况下，Ox_m轴、Oy_m轴、Oz_m轴分别平行于Ox_c轴、Oy_c轴、Oz_c轴，且方向一致，简记为“m系”。由于存在安装误差，m系与c系存在不平行误差。

临时平台坐标系原点O在简易火控瞄准轴线中心；与瞄准跟踪开始时刻的瞄准线坐标系(c系)重合。理想情况下，临时平台坐标系稳定在惯性空间，简记为“系”。由于测量误差、计算误差等因素影响，系各轴的方向与其理想情况相比可能存在误差。

字母上方有“～”符号表示该字母代表测量值，字母上方有“︿”符号表示该字母代表计算值。矢量及矢量分量的字母其上角标自左向右依次表示矢量投影的坐标系、时间节点的序号，下角标自左向右依次表示参考坐标系、运动坐标系、投影坐标轴。如表示：第k个时间节点处，c系相对于A系的角速度投影在c系Oy_c轴上分量的测量值。变换矩阵用字母C表示，其上角标表示坐标变换的目的坐标系，下角标自左向右依次表示坐标变换的出发坐标系、时间节点的序号。如表示：第k个时间节点处从c系到系的变换矩阵。矢量和矩阵的上下角标可以有一个或多个，也可以没有，无论有几个都依照上述次序确定含义。

②根据公式(1)，计算瞄准装置倾斜角

其中，为平均比例矢量(重力加速度)在z方向分量的模，为在y方向分量的模。

S3、检测是否收到跟踪信号，若检测到跟踪信号，则进行下一步；否则一直查询是否收到跟踪信号

S4、定义转换矩阵初值

在t₀时刻按照c系建立系，即t₀时刻的系与c系重合。c系相对于系的捷联矩阵用表示，在t₀时刻为根据系的定义，是单位矩阵。

S5、采集加速度计数据，提取比力矢量采集陀螺仪数据，提取角速度矢量

由于可将A系作为惯性坐标系，m系与c系固联，则在t_k时刻，IMU中加速度计提取比力矢量陀螺仪提取角速度矢量

S6、捷联矩阵四元数即时修正

随着瞄准跟踪过程的进行，简易火控姿态不断变化，瞄准线坐标系(c系)不断转动，随之改变，使用四元数法计算各时刻的设c系相对于系的转动四元数为 其中i_c、j_c、k_c是c系三个坐标轴上的单位矢量，Q的即时修正通过公式(2)实现：

根据给出初始值q₀＝1、q₁＝0、q₂＝0、q₃＝0；

求解方程(2)，获得t_k时刻的四元数，计算得到

S7、进行加速度比力坐标转换若瞄准跟踪未结束，则返回S5，否则进入下一步

S8、计算平均比力矢量(即重力加速度)

记t0时刻的在瞄准跟踪结束时刻t_n，将全部的(k＝0,1,2......,n)做算术平均，根据公式(4)，在系中得到平均比力矢量

S9、计算瞄准线初始高低角和方向角

由于经过训练后的射手，在瞄准跟踪过程能够控制身体晃动，稳定跟踪目标运动，射手自身扰动加速度度很小。因此，可以将作为发射点处重力加速度g的辨识结果，则在A系中的分解即为重力加速度在A系中的分解。

将表示为：

的模为则在A系中的分解为：

根据A系和系的定义，其原点都在发射点，x轴都指向t₀时刻的目标，其中Ax轴平行于水平面指向目标，轴直接指向目标。因此，系相对于A系存在高低角(目标高低角导致)和倾斜角(简易火控横向不水平导致)，方向角根据坐标转换关系，系与A系的变换矩阵为：

将进行转置，得到A系到系的变换矩阵

则与之间的表达式为：

展开形式为：

分项列出如下：

可以得到：

所以，瞄准线初始高低角和方向角的计算如公式(13)：

S10、计算转换矩阵

将公式(12)的计算结果代入式(7)得到转换矩阵根据公式(14)，利用和t_n时刻的计算瞄准跟踪结束时刻的c系到A系的转换矩阵

的表达式为：

其中，θ_n、ψ_n、γ_n是跟踪结束t_n时刻，c系相对于A系的姿态角，称θ_n为瞄准线高低角，ψ_n为瞄准线方向角，对γ_n并不关心。

S11、计算结束时刻瞄准线高低角和方向角

从公式(15)中可解出：

括号中的数字表示中元素的行列位置。同理，还可以根据计算各时间节点t_k处的瞄准线高低角和方向角。

S12、瞄准跟踪过程的主要任务已完成后，计算瞄准线转动角速度并输出结果

(1)跟踪开始t₀时刻，瞄准线在地面坐标系的初始角位置由确定，明显因此，瞄准线在地面坐标系的初始高低角根据公式(13)计算，初始方向角ψ_c0＝0；

(2)跟踪结束t_n时刻，瞄准线在地面坐标系的角位置由确定，瞄准线在地面坐标系的高低角和方向角ψ_cn由式(16)计算。

因此，即可计算瞄准线的转动角速度，高低角速度方向角速度的计算公式为：

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。