CN113175942B - 动态环境下的杆臂速度补偿方法、系统及介质 - Google Patents

Abstract

本发明提供了一种动态环境下的杆臂速度补偿方法、系统及介质，包括：步骤1：根据主惯导速度、转塔姿态角、转塔角速度和主惯导杆臂，计算转塔方位转动中心相对于地理坐标系的速度；步骤2：根据发射架起竖角、起竖角速度和子惯导杆臂，计算子惯导相对于转塔坐标系的速度；步骤3：根据转塔方位转动中心速度、转塔姿态角、转塔角速度、发射架起竖角、发射架杆臂、子惯导杆臂和子惯导相对于转塔坐标系的速度，计算子惯导相对于地理坐标系的速度，并作为导弹动基座对准的输入以及惯导解算的速度初值。本发明将速度误差进行反馈校正，获得高精度动态惯导系统对准结果，从而提高子惯导系统对准的快速性和准确性。

Description

动态环境下的杆臂速度补偿方法、系统及介质

技术领域

本发明涉及速度补偿技术领域，具体地，涉及一种动态环境下的杆臂速度补偿方法、系统及介质。

背景技术

动基座传递对准过程采用卡尔曼滤波器，根据所用量测信息的不同可分为传递对准不同的匹配方案，如位置、速度、姿态、角速度和加速度匹配对准方案，其中，速度匹配是最广泛运用的对准方案。同时，动基座对准完成后，惯导解算的速度初值也是影响导航误差的重要因素。因此，主子惯导之间的杆臂速度必须进行精确的补偿。

杆臂速度补偿时，主子惯导之间一般为刚性连接，使用常规的杆臂速度补偿方法即可。但当主子惯导之间存在相对运动时，需要根据相对运动的不同，采用特定的杆臂速度补偿方法。

专利文献CN105865488A(申请号：CN201610334839.8)公开了一种基于自主量测信息的静基座动态快速精确对准方法，包括如下技术步骤设计：子惯导初始化、动态起竖子惯导、自主获取位置和速度量测信息、子惯导进行精对准；基于子惯导捷联解算结果和获取的位置量测信息与速度量测信息利用卡尔曼滤波方法估计子惯导随机误差，通过反馈校正减小航向和水平姿态误差，获取高精度的子惯导对准结果。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种动态环境下的杆臂速度补偿方法、系统及介质。

根据本发明提供的动态环境下的杆臂速度补偿方法，包括：

步骤1：根据主惯导速度、转塔姿态角、转塔角速度和主惯导杆臂，计算转塔方位转动中心相对于地理坐标系的速度；

步骤2：根据发射架起竖角、起竖角速度和子惯导杆臂，计算子惯导相对于转塔坐标系的速度；

步骤3：根据转塔方位转动中心速度、转塔姿态角、转塔角速度、发射架起竖角、发射架杆臂、子惯导杆臂和子惯导相对于转塔坐标系的速度，计算子惯导相对于地理坐标系的速度，并作为导弹动基座对准的输入以及惯导解算的速度初值。

优选的，转塔坐标系到地理坐标系的姿态矩阵为：

式中，ψ_z为转塔偏航角，θ_z为转塔俯仰角，φ_z为转塔滚转角，相对于当地地理坐标系，转塔偏航角ψ_z北偏东为正，定义域[0°，360°)；

则转塔方位转动中心o_z相对于地理坐标系的速度为：

式中：

为主惯导在当地地理坐标系下的速度；

为主惯导中心相对于转塔转动中心的杆臂在转塔坐标系下的投影；

为主惯导中心相对于转塔转动中心的主惯导速度在转塔坐标系下的投影；

为转塔坐标系相对于惯性坐标系的转动角速度在转塔坐标系下的投影；

为

的转置；ω_ie为地球自转速率；L为纬度；R_M为子午圆曲率半径；R_N为卯酉圆曲率半径；t为转动时间；g为重力加速度。

优选的，子惯导相对于转塔坐标系的速度为：

式中，

为发射架俯仰角速度；

为子惯导相对于发射架高低旋转中心的杆臂在发射架坐标系下的投影；θ_j发射架起竖角。

优选的，子惯导速度为：

式中，

为发射架杆臂在转塔坐标系下的投影。

根据本发明提供的动态环境下的杆臂速度补偿系统，包括：

模块M1：根据主惯导速度、转塔姿态角、转塔角速度和主惯导杆臂，计算转塔方位转动中心相对于地理坐标系的速度；

模块M2：根据发射架起竖角、起竖角速度和子惯导杆臂，计算子惯导相对于转塔坐标系的速度；

模块M3：根据转塔方位转动中心速度、转塔姿态角、转塔角速度、发射架起竖角、发射架杆臂、子惯导杆臂和子惯导相对于转塔坐标系的速度，计算子惯导相对于地理坐标系的速度，并作为导弹动基座对准的输入以及惯导解算的速度初值。

优选的，转塔坐标系到地理坐标系的姿态矩阵为：

式中，ψ_z为转塔偏航角，θ_z为转塔俯仰角，φ_z为转塔滚转角，相对于当地地理坐标系，转塔偏航角ψ_z北偏东为正，定义域[0°，360°)；

则转塔方位转动中心o_z相对于地理坐标系的速度为：

式中：

为主惯导在当地地理坐标系下的速度；

为主惯导中心相对于转塔转动中心的杆臂在转塔坐标系下的投影；

为主惯导中心相对于转塔转动中心的主惯导速度在转塔坐标系下的投影；

为转塔坐标系相对于惯性坐标系的转动角速度在转塔坐标系下的投影；

为

的转置；ω_ie为地球自转速率；L为纬度；R_M为子午圆曲率半径；R_N为卯酉圆曲率半径；t为转动时间；g为重力加速度。

优选的，子惯导相对于转塔坐标系的速度为：

式中，

为发射架俯仰角速度；

为子惯导相对于发射架高低旋转中心的杆臂在发射架坐标系下的投影；θ_j发射架起竖角。

优选的，子惯导速度为：

式中，

为发射架杆臂在转塔坐标系下的投影。

根据本发明提供的一种存储有计算机程序的计算机可读存储介质，所述计算机程序被处理器执行时实现上述的方法的步骤。

与现有技术相比，本发明具有如下的有益效果：

(1)本发明在机动起竖对准过程中，通过静基座起竖环境下动态杆臂补偿方法自主获取位置和速度量测信息，将位置、速度以及姿态误差进行反馈校正，逐步减小航向及水平姿态误差，获得高精度动态惯导系统对准结果，从而提高子惯导系统对准的快速性和准确性；

(2)本方法能够在飞行器高动态飞行过程中实现对惯性导航系统中杆臂效应误差的在线标定及校正，有效提高高超声速飞行器惯性导航系统性能，适合于工程应用。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为子惯导速度计算流程图；

图2为本发明实施例的结构示意图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

实施例：

如图2，本发明应用环境为转塔安装在运动的战车上，可以绕方位转动中心进行调转，主惯导固定安装在转塔上，发射架可绕着旋转轴在转塔上起竖(旋转轴固定安装在转塔上)，位于导弹内部的子惯导固定安装在发射架上。首先，由主惯导速度，转塔姿态角，转塔角速度，主惯导杆臂计算转塔方位转动中心相对于地理坐标系的速度；然后由发射架起竖角，起竖角速度，子惯导杆臂，计算子惯导相对于转塔坐标系的速度；最后，由转塔方位转动中心速度，转塔姿态角，转塔角速度，发射架起竖角，发射架杆臂，子惯导杆臂，子惯导相对于转塔坐标系的速度，计算子惯导相对于地理坐标系的速度。获得的子惯导速度，作为导弹动基座对准的输入以及惯导解算的速度初值。

以下将结合图1对复杂动态环境下的杆臂速度补偿技术作进一步的详细描述。

相关坐标系定义如下：

惯性参考坐标系(ox_iy_iz_i)：空间中静止或匀速直线运动的参考坐标系。

地球坐标系(ox_ey_ez_e)：原点o取在地心；oz_e轴沿极轴方向；ox_e轴在赤道平面与本初子午面的交线上；oy_e轴由右手定则确定。

地理坐标系(ox_ty_tz_t)：原点o取在导弹质心；ox_t轴指向北；oy_t轴指向天；oz_t轴指向东。

转塔坐标系(o_zx_zy_zz_z)：原点o_z取在转塔转动中心；o_zx_z轴指向发射装置前向方向(和发射架在发射装置平台上投影方向一致)；o_zy_z垂直于发射装置平面，向上为正；o_zz_z垂直于x_zo_zy_z平面，且与o_zx_z轴和o_zy_z轴满足右手准则。

发射架坐标系o_jx_jy_jz_j：原点o_j为发射架高低旋转中心；o_jx_j为沿发射架高低指向方向，指向弹头为正；o_jy_j在发射集装箱纵对称平面内，垂直于o_jx_j，向上为正；o_jz_j垂直于x_jo_jy_j平面，且与o_jx_j轴和o_jy_j轴满足右手准则。

步骤一、由主惯导速度，转塔姿态角，转塔角速度，主惯导杆臂计算转塔方位转动中心相对于地理坐标系的速度。

转塔坐标系到地理坐标系的姿态矩阵为：

式中，ψ_z为转塔偏航角、θ_z为转塔俯仰角、φ_z为转塔滚转角(相对于当地地理坐标系，其中转塔偏航角ψ_z北偏东为正，定义域[0°，360°))；

则转塔方位转动中心o_z相对于地理坐标系的速度为：

式中：

为主惯导在当地地理坐标系下的速度；

为主惯导中心相对于转塔转动中心的杆臂在转塔坐标系下的投影；

为转塔坐标系相对于惯性坐标系的转动角速度在转塔坐标系下的投影；

为

的转置；ω_ie＝7.2915E-5rad/s为地球自转速率；L为纬度；R_M＝R(1-2e+3esin²L)+h为子午圆曲率半径；R_N＝R(1+esin²L)+h为卯酉圆曲率半径；R＝6378137m为地球长轴半径；e＝3.353E-3为地球椭圆度；

步骤二、由发射架起竖角，起竖角速度，子惯导杆臂，计算子惯导相对于转塔坐标系的速度。

子惯导相对于转塔坐标系的速度为：

式中，

为发射架俯仰角速度(发射架坐标系下的投影)，

为子惯导相对于发射架高低旋转中心的杆臂在发射架坐标系下的投影。

步骤三、由转塔方位转动中心速度，转塔姿态角，转塔角速度，发射架起竖角，发射架杆臂，子惯导杆臂，子惯导相对于转塔坐标系的速度，计算子惯导相对于地理坐标系的速度。

子惯导速度为：

在本申请的描述中，需要理解的是，术语“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (3)

1.一种动态环境下的杆臂速度补偿方法，其特征在于，包括：

步骤1：根据主惯导速度、转塔姿态角、转塔角速度和主惯导杆臂，计算转塔方位转动中心相对于地理坐标系的速度；

步骤2：根据发射架起竖角、起竖角速度和子惯导杆臂，计算子惯导相对于转塔坐标系的速度；

步骤3：根据转塔方位转动中心速度、转塔姿态角、转塔角速度、发射架起竖角、发射架杆臂、子惯导杆臂和子惯导相对于转塔坐标系的速度，计算子惯导相对于地理坐标系的速度，并作为导弹动基座对准的输入以及惯导解算的速度初值；

转塔坐标系到地理坐标系的姿态矩阵为：

式中，ψ_z为转塔偏航角，θ_z为转塔俯仰角，φ_z为转塔滚转角，转塔偏航角ψ_z相对于当地地理坐标系，北偏东为正，定义域[0°，360°)；

则转塔方位转动中心o_z相对于地理坐标系的速度为：

式中：

为主惯导在当地地理坐标系下的速度；

为主惯导中心相对于转塔转动中心的杆臂在转塔坐标系下的投影；

为主惯导中心相对于转塔转动中心的主惯导速度在转塔坐标系下的投影；

为转塔坐标系相对于惯性坐标系的转动角速度在转塔坐标系下的投影；

为

的转置；ω_ie为地球自转速率；L为纬度；R_M为子午圆曲率半径；R_N为卯酉圆曲率半径；t为转动时间；g为重力加速度；

子惯导相对于转塔坐标系的速度为：

式中，

为发射架俯仰角速度；

为子惯导相对于发射架高低旋转中心的杆臂在发射架坐标系下的投影；θ_j发射架起竖角；

子惯导速度为：

式中，

为发射架杆臂在转塔坐标系下的投影。

2.一种动态环境下的杆臂速度补偿系统，其特征在于，包括：

模块M1：根据主惯导速度、转塔姿态角、转塔角速度和主惯导杆臂，计算转塔方位转动中心相对于地理坐标系的速度；

模块M2：根据发射架起竖角、起竖角速度和子惯导杆臂，计算子惯导相对于转塔坐标系的速度；

模块M3：根据转塔方位转动中心速度、转塔姿态角、转塔角速度、发射架起竖角、发射架杆臂、子惯导杆臂和子惯导相对于转塔坐标系的速度，计算子惯导相对于地理坐标系的速度，并作为导弹动基座对准的输入以及惯导解算的速度初值；

转塔坐标系到地理坐标系的姿态矩阵为：

式中，ψ_z为转塔偏航角，θ_z为转塔俯仰角，φ_z为转塔滚转角，转塔偏航角ψ_z相对于当地地理坐标系，北偏东为正，定义域[0°，360°)；

则转塔方位转动中心o_z相对于地理坐标系的速度为：

式中：

为主惯导在当地地理坐标系下的速度；

为主惯导中心相对于转塔转动中心的杆臂在转塔坐标系下的投影；

为主惯导中心相对于转塔转动中心的主惯导速度在转塔坐标系下的投影；

为转塔坐标系相对于惯性坐标系的转动角速度在转塔坐标系下的投影；

为

的转置；ω_ie为地球自转速率；L为纬度；R_M为子午圆曲率半径；R_N为卯酉圆曲率半径；t为转动时间；g为重力加速度；

子惯导相对于转塔坐标系的速度为：

式中，

为发射架俯仰角速度；

为子惯导相对于发射架高低旋转中心的杆臂在发射架坐标系下的投影；θ_j发射架起竖角；

子惯导速度为：

式中，

为发射架杆臂在转塔坐标系下的投影。

3.一种存储有计算机程序的计算机可读存储介质，其特征在于，所述计算机程序被处理器执行时实现权利要求1所述的方法的步骤。

Images