误差参数标定方法及装置与流程

本发明涉及飞行器导航控制技术，尤其涉及一种误差参数标定方法及装置。

背景技术：

惯性仪表是捷联惯导系统的心脏，其误差的大小将直接影响空间飞行器的入轨精度和落点偏差的大小。对于执行深空探测的一些航天器，在飞到一定高度后，不能再使用组合导航进行修正；或者重要的打击武器，在飞出本土后，为了提高它的抗干扰能力，能够正确执行打击任务也会关闭卫星组合导航功能。此时，如果能够进行一次惯性系统制导工具误差系数在轨标定，将是提高空间飞行器导航精度的一个重要手段，而误差系数分离工作的核心是参数估计方法的研究。

目前，惯性系统误差参数地面标定的方法已经比较成熟，但是惯性系统实际应用到飞行任务中时，由于受到飞行器震动、空间环境的变化，地面标定的误差参数无法满足飞行器、航天器长期在空间飞行精确地定姿定位的要求。

技术实现要素：

本发明解决的技术问题是：相比于现有技术，提供了一种误差参数标定方法及装置，实现了实时、在轨标定出制导工具的误差参数的目的。

本发明的上述目的通过以下技术方案予以实现：

第一方面，本发明提供了一种误差参数标定方法，包括：

建立飞行器导航系统的状态方程和量测方程，所述状态方程和量测方程中均包含误差参数向量，所述误差参数向量由多个误差参数构成；

判别每个所述误差参数的可观测性；

当存在可观测的误差参数时，以预设时长为滤波周期，利用卡尔曼滤波或自适应滤波，标定出所述可观测的误差参数。

进一步地，所述误差参数包括平台失准角误差、速度误差、位置误差、陀螺的标度因数误差、陀螺漂移误差、加速度计的标度因数误差、以及加速度计的漂移误差；每个所述误差参数均为矢量，且包含三个方向分量。

进一步地，所述状态方程为：

式(1)中，a表示状态矩阵；为飞行器本体系到发射惯性系的转换矩阵；x表示误差参数向量，φ表示平台失准角误差，δv表示速度误差，δr表示位置误差，δkg表示陀螺的标度因数误差，b1表示陀螺漂移误差，δka表示加速度计的标度因数误差，表示加速度计的漂移误差，t表示转置运算；ηg表示陀螺测量的白噪声，εa为加速度计测量的白噪声；为误差参数向量x的一阶导数向量；所述状态矩阵a为：

式(2)中，为陀螺测得的飞行器本体系相对于发射惯性系的角速度在飞行器本体系的投影，表示以为主对角线元素的矩阵；f^b为加速度计测得的比力在飞行器本体系的投影，表示求解的反对称矩阵；i3×3表示3×3的单位矩阵；g表示牛顿引力常数，m表示地球质量，x,y,z表示飞行器在发射惯性系下的坐标，r表示飞行器到发射惯性系原点的距离；为飞行器本体系到发射惯性系的转换矩阵；

所述量测方程为：

式(3)中，z(t)表示制导差值矩阵；φ^i″为星敏感器测量的姿态角和惯导解算姿态角之差；zv(t)为gps测量的飞行器速度和惯导解算的速度之差；zr(t)为gps测量的飞行器位置和惯导解算的位置之差；h9×21表示量测矩阵；x表示所述误差参数向量；v9×1表示白噪声向量；所述量测矩阵h9×21为：

式(4)中，i3×3表示3×3的单位矩阵；所述白噪声向量v9×1为：

式(5)中，和为星敏感器的姿态量测白噪声，δmx、δmy和δmz为gps的量测速度白噪声，δxg、δxg和δzg为gps的量测位置白噪声；t表示转置运算。

进一步地，判别每个所述误差参数的可观测性，包括：

利用所述状态矩阵，计算转移矩阵，所述转移矩阵的计算公式为：

式(6)中，φk,k-1表示状态k-1时刻到状态k时刻的转移矩阵，i21×21表示21×21的单位矩阵；t1表示预设时长；ak表示第k个预设时长的状态矩阵；m为正整数且m≥2；

根据所述转移矩阵和所述量测矩阵，计算待奇异值分解矩阵，所述待奇异值分解矩阵的计算公式为：

式(7)中，q表示待奇异值分解矩阵，h9×21表示量测矩阵，φ1,0表示状态0时刻到状态1时刻的转移矩阵，φm-1,m-2表示状态m-2时刻到状态m-1时刻的转移矩阵，t表示转置运算；

将所述待奇异值分解矩阵进行奇异值分解，以获取第一奇异值向量、第二奇异值向量、以及奇异值矩阵，所述奇异值分解的公式为：

q＝u∑v^t(8)

式(8)中，q表示待奇异值分解矩阵，∑表示奇异值矩阵，∑的主对角线元素为σi,i∈[1,m]；u表示第一奇异值向量，u＝[ui]＝[u1u2...um]；v表示第二奇异值向量，v＝[vi]＝[v1v2...vm]；t表示转置运算；

利用所述σi、ui、vi、以及所述制导差值矩阵z(t)，计算可观测性判别向量y21×1，所述可观测性判别向量y的计算公式为：

式(9)中，t表示转置运算；

将所述可观测性判别向量y21×1的每个元素与设定阈值δe进行比较；

当所述可观测性判别向量y21×1的第l,l∈[1,21]行第1列位置处的元素大于设定阈值δe时，判定误差参数向量x中第l,l∈[1,21]行第1列位置处的所述误差参数可观测。

第二方面，本发明还提供了一种误差参数标定装置，该装置包括：

构建模块，用于建立飞行器导航系统的状态方程和量测方程，所述状态方程和量测方程中均包含误差参数向量，所述误差参数向量由多个误差参数构成；

判别模块，用于判别每个所述误差参数的可观测性；

标定模块，用于当存在可观测的误差参数时，以预设时长为滤波周期，利用卡尔曼滤波或自适应滤波，标定出所述可观测的误差参数。

进一步地，所述误差参数包括平台失准角误差、速度误差、位置误差、陀螺的标度因数误差、陀螺漂移误差、加速度计的标度因数误差、以及加速度计的漂移误差；每个所述误差参数均为矢量，且包含三个方向分量。

进一步地，所述状态方程为：

式(1)中，a表示状态矩阵；cbⁱ为飞行器本体系到发射惯性系的转换矩阵；x表示误差参数向量，φ表示平台失准角误差，δv表示速度误差，δr表示位置误差，δkg表示陀螺的标度因数误差，b1表示陀螺漂移误差，δka表示加速度计的标度因数误差，表示加速度计的漂移误差，t表示转置运算；ηg表示陀螺测量的白噪声，εa为加速度计测量的白噪声；为误差参数向量x的一阶导数向量；所述状态矩阵a为：

式(2)中，为陀螺测得的飞行器本体系相对于发射惯性系的角速度在飞行器本体系的投影，表示以为主对角线元素的矩阵；f^b为加速度计测得的比力在飞行器本体系的投影，表示求解的反对称矩阵；i3×3表示3×3的单位矩阵；g表示牛顿引力常数，m表示地球质量，x,y,z表示飞行器在发射惯性系下的坐标，r表示飞行器到发射惯性系原点的距离；为飞行器本体系到发射惯性系的转换矩阵；

所述量测方程为：

式(3)中，z(t)表示制导差值矩阵；φ^i″为星敏感器测量的姿态角和惯导解算姿态角之差；zv(t)为gps测量的飞行器速度和惯导解算的速度之差；zr(t)为gps测量的飞行器位置和惯导解算的位置之差；h9×21表示量测矩阵；x表示所述误差参数向量；v9×1表示白噪声向量；所述量测矩阵h9×21为：

式(4)中，i3×3表示3×3的单位矩阵；所述白噪声向量v9×1为：

式(5)中，和为星敏感器的姿态量测白噪声，δmx、δmy和δmz为gps的量测速度白噪声，δxg、δxg和δzg为gps的量测位置白噪声；t表示转置运算。

进一步地，所述判别模块包括：

第一计算单元，用于利用所述状态矩阵，计算转移矩阵，所述转移矩阵的计算公式为：

式(6)中，φk,k-1表示状态k-1时刻到状态k时刻的转移矩阵，i21×21表示21×21的单位矩阵；t1表示预设时长；ak表示第k个预设时长的状态矩阵；m为正整数且m≥2；

第二计算单元，用于根据所述转移矩阵和所述量测矩阵，计算待奇异值分解矩阵，所述待奇异值分解矩阵的计算公式为：

式(7)中，q表示待奇异值分解矩阵，h9×21表示量测矩阵，φ1,0表示状态0时刻到状态1时刻的转移矩阵，φm-1,m-2表示状态m-2时刻到状态m-1时刻的转移矩阵，t表示转置运算；

分解单元，用于将所述待奇异值分解矩阵进行奇异值分解，以获取第一奇异值向量、第二奇异值向量、以及奇异值矩阵，所述奇异值分解的公式为：

q＝u∑v^t(8)

式(8)中，q表示待奇异值分解矩阵，∑表示奇异值矩阵，∑的主对角线元素为σi,i∈[1,m]；u表示第一奇异值向量，u＝[ui]＝[u1u2...um]；v表示第二奇异值向量，v＝[vi]＝[v1v2...vm]；t表示转置运算；

第三计算单元，用于利用所述σi、ui、vi、以及所述制导差值矩阵z(t)，计算可观测性判别向量y21×1，所述可观测性判别向量y的计算公式为：

式(9)中，t表示转置运算；

比较单元，用于将所述可观测性判别向量y21×1的每个元素与设定阈值δe进行比较；

判定单元，用于当所述可观测性判别向量y21×1的第l,l∈[1,21]行第1列位置处的元素大于设定阈值δe时，判定误差参数向量x中第l,l∈[1,21]行第1列位置处的所述误差参数可观测。

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

(1)、本发明通过建立飞行器导航系统的状态方程和量测方程；判别每个误差参数的可观测性；当存在可观测的误差参数时，利用卡尔曼滤波或自适应滤波，标定出所述可观测的误差参数，能够克服现有制导工具误差分离技术“天地不一致”的不足，能够实时、在轨标定出制导工具的误差系数，算法简单，便于工程化。

(2)、本发明能够降低惯性器件维护保障费用，由于具有在轨标定的能力，从而能够减少惯性系统地面定期标定的次数，节约人力物力。

(3)、本发明能够提高飞行器制导精度，减少对卫星导航的依赖，扩展空间飞行器执行任务的范围和能力。

附图说明

图1是本发明实施例一中的一种误差参数标定方法的流程图；

图2是本发明实施例二中的一种误差参数标定装置的结构图。

具体实施方式

下面结合附图和实施例对本发明作进一步详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。

实施例一

图1是本发明实施例一中的一种误差参数标定方法的流程图，本实施例可适用于需要对制导工具的误差参数进行在轨标定的情况，该方法可以由误差参数标定装置来执行，其中该装置可以由软件和/或硬件实现。参考图1，本实施例提供的误差参数标定方法具体可以包括如下步骤：

s110、建立飞行器导航系统的状态方程和量测方程，所述状态方程和量测方程中均包含误差参数向量，所述误差参数向量由多个误差参数构成。

具体的，所述误差参数包括平台失准角误差、速度误差、位置误差、陀螺的标度因数误差、陀螺漂移误差、加速度计的标度因数误差、以及加速度计的漂移误差；每个所述误差参数均为矢量，且包含三个方向分量。所述每个所述误差参数均为矢量，且包含三个方向分量，即每个误差参数均为矢量，在各自坐标系均包含三个方向向量。例如，陀螺漂移误差在其坐标系包含三个方向分量：x轴向分量、y轴向分量、以及z轴向分量。又例如，加速度计的标度因数误差在其坐标系包含三个方向分量：x轴向分量、y轴向分量、以及z轴向分量。

具体的，所述状态方程为：

式(1)中，a表示状态矩阵；为飞行器本体系到发射惯性系的转换矩阵；x表示误差参数向量，φ表示平台失准角误差，δv表示速度误差，δr表示位置误差，δkg表示陀螺的标度因数误差，b1表示陀螺漂移误差，δka表示加速度计的标度因数误差，表示加速度计的漂移误差，t表示转置运算；ηg表示陀螺测量的白噪声，εa为加速度计测量的白噪声；为误差参数向量x的一阶导数向量；所述状态矩阵a为：

式(2)中，为陀螺测得的飞行器本体系相对于发射惯性系的角速度在飞行器本体系的投影，表示以为主对角线元素的矩阵；f^b为加速度计测得的比力在飞行器本体系的投影，表示求解的反对称矩阵；i3×3表示3×3的单位矩阵；g表示牛顿引力常数，m表示地球质量，x,y,z表示飞行器在发射惯性系下的坐标，r表示飞行器到发射惯性系原点的距离；为飞行器本体系到发射惯性系的转换矩阵；

所述量测方程为：

式(3)中，z(t)表示制导差值矩阵；φⁱ”为星敏感器测量的姿态角和惯导解算姿态角之差；zv(t)为gps测量的飞行器速度和惯导解算的速度之差；zr(t)为gps测量的飞行器位置和惯导解算的位置之差；h9×21表示量测矩阵；x表示所述误差参数向量；v9×1表示白噪声向量；所述量测矩阵h9×21为：

式(4)中，i3×3表示3×3的单位矩阵；所述白噪声向量v9×1为：

式(5)中，和为星敏感器的姿态量测白噪声，δmx、δmy和δmz为gps的量测速度白噪声，δxg、δxg和δzg为gps的量测位置白噪声；t表示转置运算。

s120、判别每个所述误差参数的可观测性。

可选的，判别每个所述误差参数的可观测性，包括：

利用所述状态矩阵，计算转移矩阵，所述转移矩阵的计算公式为：

式(6)中，φk,k-1表示状态k-1时刻到状态k时刻的转移矩阵，i21×21表示21×21的单位矩阵；t1表示预设时长；ak表示第k个预设时长的状态矩阵；m为正整数且m≥2。

具体的，t1表示预设时长，所述预设时长通常为200ms的整数倍，最大为1s。例如，若预设时长t1＝200ms，ak即表示第k个200ms时段的状态矩阵。

根据所述转移矩阵和所述量测矩阵，计算待奇异值分解矩阵，所述待奇异值分解矩阵的计算公式为：

式(7)中，q表示待奇异值分解矩阵，h9×21表示量测矩阵，φ1,0表示状态0时刻到状态1时刻的转移矩阵，φm-1,m-2表示状态m-2时刻到状态m-1时刻的转移矩阵，t表示转置运算。

将所述待奇异值分解矩阵进行奇异值分解，以获取第一奇异值向量、第二奇异值向量、以及奇异值矩阵，所述奇异值分解的公式为：

q＝u∑v^t(8)

式(8)中，q表示待奇异值分解矩阵，∑表示奇异值矩阵，∑的主对角线元素为σi,i∈[1,m]；u表示第一奇异值向量，u＝[ui]＝[u1u2...um]；v表示第二奇异值向量，v＝[vi]＝[v1v2...vm]；t表示转置运算。

利用所述σi、ui、vi、以及所述制导差值矩阵z(t)，计算可观测性判别向量y21×1，所述可观测性判别向量y的计算公式为：

式(9)中，t表示转置运算。

将所述可观测性判别向量y21×1的每个元素与设定阈值δe进行比较。

具体的，由于可观测性判别向量y21×1中每个元素的范围都在0～1之间，因而，所述设定阈值δe可选为0.1。本实施例中，所述设定阈值δe＝0.1，即将所述可观测性判别向量y21×1的每个元素与设定阈值0.1进行比较。

当所述可观测性判别向量y21×1的第l,l∈[1,21]行第1列位置处的元素大于设定阈值δe时，判定误差参数向量x中第l,l∈[1,21]行第1列位置处的所述误差参数可观测。

具体的，本实施例中，x表示误差参数向量，φ表示平台失准角误差，δv表示速度误差，δr表示位置误差，δkg表示陀螺的标度因数误差，b1表示陀螺漂移误差，δka表示加速度计的标度因数误差，表示加速度计的漂移误差；且每个误差参数在各自坐标系均包含三个方向向量。例如，当所述可观测性判别向量y21×1的第10行第1列位置处的元素大于设定阈值δe＝0.1时，判定误差参数向量x中第10行第1列位置处的所述误差参数可观测，即陀螺的标度因数误差δkg可观测。又例如，当所述可观测性判别向量y21×1的第19行第1列位置处的元素大于设定阈值δe＝0.1时，判定误差参数向量x中第19行第1列位置处的所述误差参数可观测，即加速度计的漂移误差可观测。

s130、当存在可观测的误差参数时，以预设时长为滤波周期，利用卡尔曼滤波或自适应滤波，标定出所述可观测的误差参数。

具体的，本实施例中，当存在可观测的误差参数时，以步骤s120中的所述预设时长t1为滤波周期，所述滤波周期可取为200ms的整数倍，最大为1s。利用卡尔曼滤波或自适应滤波，标定出所述可观测的误差参数。

本实施例的技术方案通过建立飞行器导航系统的状态方程和量测方程；判别每个误差参数的可观测性；当存在可观测的误差参数时，利用卡尔曼滤波或自适应滤波，标定出所述可观测的误差参数，能够克服现有制导工具误差分离技术“天地不一致”的不足，能够实时、在轨标定出制导工具的误差系数，算法简单，便于工程化；能够降低惯性器件维护保障费用，由于具有在轨标定的能力，从而能够减少惯性系统地面定期标定的次数，节约人力物力；能够提高飞行器制导精度，减少对卫星导航的依赖，扩展空间飞行器执行任务的范围和能力。

实施例二

图2是本发明实施例二中的一种误差参数标定装置的结构图，本实施例可适用于需要对制导工具的误差参数进行在轨标定的情况。参考图2，本实施例提供的误差参数标定装置具体可以如下：

构建模块210，用于建立飞行器导航系统的状态方程和量测方程，所述状态方程和量测方程中均包含误差参数向量，所述误差参数向量由多个误差参数构成；

判别模块220，用于判别每个所述误差参数的可观测性；

标定模块230，用于当存在可观测的误差参数时，以预设时长为滤波周期，利用卡尔曼滤波或自适应滤波，标定出所述可观测的误差参数。

可选的，所述误差参数包括平台失准角误差、速度误差、位置误差、陀螺的标度因数误差、陀螺漂移误差、加速度计的标度因数误差、以及加速度计的漂移误差；每个所述误差参数均为矢量，且包含三个方向分量。

可选的，所述状态方程为：

式(1)中，a表示状态矩阵；为飞行器本体系到发射惯性系的转换矩阵；x表示误差参数向量，φ表示平台失准角误差，δv表示速度误差，δr表示位置误差，δkg表示陀螺的标度因数误差，b1表示陀螺漂移误差，δka表示加速度计的标度因数误差，表示加速度计的漂移误差，t表示转置运算；ηg表示陀螺测量的白噪声，εa为加速度计测量的白噪声；为误差参数向量x的一阶导数向量；所述状态矩阵a为：

式(2)中，为陀螺测得的飞行器本体系相对于发射惯性系的角速度在飞行器本体系的投影，表示以为主对角线元素的矩阵；f^b为加速度计测得的比力在飞行器本体系的投影，表示求解的反对称矩阵；i3×3表示3×3的单位矩阵；g表示牛顿引力常数，m表示地球质量，x,y,z表示飞行器在发射惯性系下的坐标，r表示飞行器到发射惯性系原点的距离；为飞行器本体系到发射惯性系的转换矩阵；

所述量测方程为：

式(3)中，z(t)表示制导差值矩阵；φⁱ”为星敏感器测量的姿态角和惯导解算姿态角之差；zv(t)为gps测量的飞行器速度和惯导解算的速度之差；zr(t)为gps测量的飞行器位置和惯导解算的位置之差；h9×21表示量测矩阵；x表示所述误差参数向量；v9×1表示白噪声向量；所述量测矩阵h9×21为：

式(4)中，i3×3表示3×3的单位矩阵；所述白噪声向量v9×1为：

式(5)中，和为星敏感器的姿态量测白噪声，δmx、δmy和δmz为gps的量测速度白噪声，δxg、δxg和δzg为gps的量测位置白噪声；t表示转置运算。

可选的，所述判别模块包括：

第一计算单元，用于利用所述状态矩阵，计算转移矩阵，所述转移矩阵的计算公式为：

式(6)中，φk,k-1表示状态k-1时刻到状态k时刻的转移矩阵，i21×21表示21×21的单位矩阵；t1表示预设时长；ak表示第k个预设时长的状态矩阵；m为正整数且m≥2；

第二计算单元，用于根据所述转移矩阵和所述量测矩阵，计算待奇异值分解矩阵，所述待奇异值分解矩阵的计算公式为：

式(7)中，q表示待奇异值分解矩阵，h9×21表示量测矩阵，φ1,0表示状态0时刻到状态1时刻的转移矩阵，φm-1,m-2表示状态m-2时刻到状态m-1时刻的转移矩阵，t表示转置运算；

分解单元，用于将所述待奇异值分解矩阵进行奇异值分解，以获取第一奇异值向量、第二奇异值向量、以及奇异值矩阵，所述奇异值分解的公式为：

q＝u∑v^t(8)

式(8)中，q表示待奇异值分解矩阵，∑表示奇异值矩阵，∑的主对角线元素为σi,i∈[1,m]；u表示第一奇异值向量，u＝[ui]＝[u1u2...um]；v表示第二奇异值向量，v＝[vi]＝[v1v2...vm]；t表示转置运算；

第三计算单元，用于利用所述σi、ui、vi、以及所述制导差值矩阵z(t)，计算可观测性判别向量y21×1，所述可观测性判别向量y的计算公式为：

式(9)中，t表示转置运算；

比较单元，用于将所述可观测性判别向量y21×1的每个元素与设定阈值δe进行比较；

判定单元，用于当所述可观测性判别向量y21×1的第l,l∈[1,21]行第1列位置处的元素大于设定阈值δe时，判定误差参数向量x中第l,l∈[1,21]行第1列位置处的所述误差参数可观测。

本实施例提供的误差参数标定装置，与本发明任意实施例所提供的误差参数标定方法属于同一发明构思，可执行本发明任意实施例所提供的误差参数标定方法，具备执行误差参数标定方法相应的功能模块和有益效果。未在本实施例中详尽描述的技术细节，可参见本发明任意实施例提供的误差参数标定方法。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。