CN117308999A - 一种基于群仿射性质的旋转调制惯导***快速对准方法 - Google Patents

Abstract

本发明涉及惯性导航技术领域，具体涉及一种基于群仿射性质的旋转调制惯导***的快速对准方法，适用于飞机、无人飞行器、火箭、船舶等运载器中惯导***的快速对准及惯性导航；该方法在旋转调制惯导多位置对准的基础上，抛弃传统的“粗对准+精对准”的对准思路，基于李群理论设计了满足群仿射性质的对准状态模型，该状态模型结合卡尔曼滤波可以实现任意大初始失准角初始对准，并能在短时间内快速收敛。

Description

一种基于群仿射性质的旋转调制惯导***快速对准方法

技术领域

本发明涉及惯性导航技术领域，具体涉及一种基于群仿射性质的旋转调制惯导***的快速对准方法，适用于飞机、无人飞行器、火箭、船舶等运载器中惯导***的快速对准及惯性导航。

背景技术

惯导***是一种完全依靠自身惯性器件完成导航任务的自主式导航***，可测量载体相对于惯性空间的角速度与加速度并计算载体实时的姿态、速度和位置信息。飞机、无人飞行器、火箭、船舶等海陆空各式运载装备通常都配置有惯导***以提供自主的导航信息。长航时飞行器的应用中，可靠的导航信息是保证飞行器能精确执行相关任务的必要条件。针对GNSS拒止环境应用场景，组合导航等利用外部传感器进行惯导误差抑制的导航手段不再可行，纯惯性导航性能受到考验，能够持续提供长航时高精度导航信息的旋转调制惯导***是最佳的选择。

旋转调制惯导***的初始对准过程在整个导航过程中至关重要，直接影响后续导航精度，初始对准一直是惯导领域的重点研究问题。在旋转调制惯导***的某些应用场景中，对初始对准的速度有较高要求，以满足快速响应能力，然而对准的速度与精度两方面相矛盾。此外，在传统“粗对准+精对准”方案中，还存在以下问题：1)粗对准时间与精对准时间分配问题：有限时间内精对准时间如果分配较短，卡尔曼滤波估计值收敛越差；粗对准如果分配较短，受基座晃动干扰较大，也将影响精对准收敛；2)参数设置问题：长时间对准中卡尔曼滤波的初始方差阵设置即使不准确，也可逐渐收敛，但短时快速对准中初始方差阵的设置将显著影响对准精度，在基座晃动干扰不同情况下粗对准精度不同导致初始方差阵的设置没有普适性。

发明内容

为了解决旋转调制惯导***初始对准的对准时间分配问题以及参数设置问题，并使初始对准短时间内可达到高精度，本发明提出一种基于群仿射性质的旋转调制惯导***快速对准方法。该方法在旋转调制惯导多位置对准的基础上，抛弃传统的“粗对准+精对准”的对准思路，基于李群理论设计了满足群仿射性质的对准状态模型，该状态模型结合卡尔曼滤波可以实现任意大初始失准角初始对准，并能在短时间内快速收敛。

本发明采用的技术方案如下：一种基于群仿射性质的旋转调制惯导***的快速对准方法，分为以下步骤：

S1：定义坐标系，惯导***上电开机，惯导***的转位机构执行特定转位次序，惯性测量单元(Inertial Measurement Unit，IMU)采集陀螺与加速度计数据并实时传输给导航计算机。

S1.1定义坐标系：

定义地心惯性坐标系为惯性参考坐标系，记为i系；地心地固坐标系(ECEF)记为e系；定义“东-北-天”地理坐标系为导航坐标系，记为n系；定义载体坐标系的x-y-z三个轴分别指向载体的右-前-上方向，载体坐标系记为b系，定义IMU坐标系的x-y-z三个轴分别对应x-y-z三个陀螺的敏感轴，IMU坐标系记为s系；

S1.2惯导***开机，设置初始对准时间T_a，启动导航计算机计时器，初始化时刻k＝0；

S1.3IMU采集k时刻陀螺输出角速度和加速度计输出比力/>并将和/>实时传输给导航计算机。

“～”表示陀螺输出角速度和加速度计输出比力包含测量误差。

S1.4开机后转位机构同步执行双位置转位次序：

1)转位机构静止T_a/2-180/ω-0.5×ω/a秒；

2)绕天向轴转位机构以ω的角速度旋转180°；

3)转位机构静止T_a/2-180/ω-0.5×ω/a秒；

其中，T_a为初始对准时间，单位为秒，工程应用中通常设置为300秒或600秒；ω为转位机构转动的角速度，单位为°/s；a为转位机构转动的角加速度，单位为°/s²。

S2：导航计算机实时接收到陀螺输出角速度和加速度计输出比力/>后，执行惯性导航解算：

S2.1初始化导航参数，具体如下：

S2.1.1定义k时刻的导航参数：IMU的姿态矩阵为速度矢量为v^e(k)，位置矢量为p^e(k)，辅助速度矢量为/>

上标“e”表示此参数为在地心地固坐标系(e系)中的参数，以下同。

S2.1.2将k＝0时刻的姿态矩阵进行初始化：

S2.1.3将k＝0时刻的速度矢量v^e(0)进行初始化：

v^e(0)＝[0 0 0]^T (2)

上标“T”表示矩阵转置。

S2.1.4将k＝0时刻的位置矢量p^e(0)进行初始化：

p^e(0)＝[L₀ λ₀ h₀]^T (3)

式中，L₀表示惯导***在初始位置的纬度，λ₀表示惯导***在初始位置的经度，h₀表示惯导***在初始位置的海拔。

S2.1.5将k＝0时刻的辅助速度矢量进行初始化：

为e系相对于i系的旋转角速度在e系中的投影，/>地球自转速率ω_ie＝15°/h；/>表示计算矢量/>的反对称矩阵，以下同；

S2.2导航计算机计时器时间更新：k＝k+1；

S2.3执行地心地固坐标系下的惯性导航解算，具体如下：

S2.3.1进行姿态更新：

式中I_3×3为3×3维的单位矩阵；φ(k)为k时刻的等效旋转矢量，等效旋转矢量由陀螺输出角速度计算得到，计算过程可参考文献[捷联惯导算法与组合导航原理/严恭敏，翁浚编著—西安：西北工业大学出版社，2019.8]；φ(k)为φ(k)的模，即φ(k)＝|φ(k)|；(φ(k)×)表示计算矢量φ(k)的反对称矩阵。

S2.3.2进行速度更新：

式中为加速度计输出的比力值；T_s为IMU数据的采样时间间隔；/>为k时刻的万有引力矢量；k-1时刻的辅助速度矢量/>与k-1时刻的速度矢量v^e(k-1)间的关系、k-1时刻的万有引力矢量/>与k-1时刻的重力矢量g^e(k-1)间的关系分别为

S2.3.3进行位置更新：

S3：构建满足群仿射性质的旋转调制惯导***快速对准状态模型

基于李群理论，如果将一个动态***的状态量定义在李群空间，且满足全局状态量独立的条件，则其对应的状态模型满足群仿射性质。基于群仿射性质，李群空间中的线性状态模型能够准确地反映非线性状态模型的传播过程。这就是说，可以建立满足群仿射性质的线性状态模型来实现非线性状态模型的求解。李群理论及群仿射性质的具体可参考文献[Barrau A.Non-linear state error based extended Kalman filters withapplications to navigation[D].Mines Paristech,2015.]。本发明的核心思想是抛弃传统“粗对准+精对准”方案中的粗对准过程，以充分利用对准数据并缩短对准时间。然而这会引起初始失准角偏大，对准模型将由线性状态模型衰减成为非线性状态模型，导致卡尔曼滤波失效。通过构建满足群仿射性质的旋转调制惯导***快速对准的状态模型，将有效解决非线性模型导致卡尔曼滤波失效问题。具体如下：

S3.1构建满足群仿射性质的旋转调制惯导***快速对准的状态模型

式中，X_k为k时刻的18维状态向量，Z_k为k时刻的3维量测向量；Φ_k|k-1为从k-1时刻到k时刻的状态一步转移矩阵；G_k|k-1为从k-1时刻到k时刻的***噪声分配矩阵，H_k为k时刻的量测矩阵；w_k-1表示k-1时刻的***噪声矩阵；v_k表示k时刻的观测噪声矩阵，噪声矩阵w_k-1的设定可参考文献[捷联惯导算法与组合导航原理/严恭敏，翁浚编著—西安：西北工业大学出版社，2019.8]；

S3.1.1构建18维状态向量X_k：

式中，表示k时刻IMU的姿态误差，下标“lg”表示此误差的类型为满足群仿射性质的左不变误差；/>表示k时刻IMU的速度误差；/>表示k时刻IMU的位置误差；ε^s(k)表示k时刻IMU的陀螺漂移，/>表示k时刻IMU的加速度计零偏，L^s(k)表示k时刻IMU的内杆臂误差；左不变误差的定义可参考文献[Barrau A.Non-linear state errorbased extended Kalman filters with applications to navigation[D].MinesParistech,2015.]。

S3.1.2构建3维量测向量Z_k：

式中，v^e(k)表示k时刻的速度矢量：

为量测速度，因为旋转调制惯导***的对准是运载体停留在原地进行，所以/>

S3.1.3构建从k-1时刻到k时刻的状态一步转移矩阵Φ_k|k-1：

式中I_18×18表示18×18的单位矩阵，I_3×3表示3×3的单位矩阵，O_3×3表示3×3的零矩阵。

S3.1.4构建从k-1时刻到k时刻的***噪声分配矩阵G_k|k-1：

S3.1.5构建k时刻的量测矩阵H_k：

S4：基于S3构建的旋转调制惯导***快速对准状态模型进行状态量估计

S4.1计算从k-1时刻至k时刻的状态向量后验估计

式中，表示k-1时刻输出的18维状态向量X_k-1的后验估计；/>的初始值 表示状态向量X_k从k-1时刻至k时刻的一步预测，即k时刻的先验估计。

S4.2计算从k-1时刻到k时刻的状态一步预测均方误差矩阵：

式中，P_k-1表示k-1时刻的状态后验估计均方误差矩阵，P_k|k-1表示从k-1时刻到k时刻的状态一步预测均方误差矩阵；

其中，P_k-1的初始值 表示n系投影下的均方误差矩阵初值，T表示均方误差坐标系转换矩阵：

S4.3计算k时刻滤波增益K_k：

式中，量测噪声协方差矩阵R为：

S4.4计算k时刻状态后验估计

S4.5计算k时刻各导航参数的误差估计值：

姿态误差估计值为/>的第1维至第3维，表示为/>即：

辅助速度误差估计值为/>的第4维至第6维，表示为/>即：

位置误差估计值为/>的第7维至第9维，表示为/>即：

S4.6计算k时刻状态后验估计均方误差阵P_k：

P_k＝P_kk-1-K_kH_kP_kk-1 (28)

S5:利用S4.5得到误差估计值修正S2的导航解算结果，具体如下：

S5.1修正姿态矩阵得到修正后的姿态矩阵/>

式中，表示对/>进行矩阵指数运算，其计算公式为：

S5.2修正辅助速度矢量得到修正后的辅助速度矢量/>

S5.3修正位置矢量p^e(k)，得到修正后的位置矢量

S6：判断对准是否继续进行，当k＜T_a时，执行S6.1；当k＝T_a时，执行S6.2，输出修正后的高精度姿态矩阵结束对准；具体如下：

S6.1当k＜T_a时，执行以下步骤：

S6.1.1将修正后的导航参数和/>覆盖导航计算机的内存中未修正的/>p^e(k)，即：

S6.1.2将p^e(k)、/>P_k存储在导航计算机的内存中；然后返回至S2.2，继续往下执行至S6，进行下一次S6的条件判断；

S6.2当k＝T_a时，输出修正后的姿态矩阵完成初始对准。

与现有技术相比，本发明的有益效果在于：

1.本发明抛弃了“粗对准+精对准”的传统对准框架，无需进行粗对准，无需考虑粗对准与精对准间的时间分配；

2.本发明不需要根据粗对准方案设置合适的均方误差矩阵初始值，其通用性强；

3.本发明节省了粗对准时间，使对准完成更加快速，比传统方法收敛更好，在有限的对准时间内，对准精度更高。

附图说明

图1为本发明流程图；

图2为导航纬度误差；

图3为导航经度误差；

图4为导航径向误差。

具体实施方式

为详细说明本发明公开的技术方案，下面结合具体实施例做进一步的阐述，此处描述的具体实施例仅用以解释本申请，并不用于限定本申请。

实施例1

对所提方法在实时***上的对准性能进行验证，试验中惯导***的性能参数如表1所示：

表1IMU性能参数

在转台4个方位的航向基准如表2所示。

表2航向基准

基于图1的流程，初始对准时间T_a＝300秒，进行了两组在4个方位的快速对准试验。

对比试验中，采用如下4种对准方案：

Scheme1(惯性系粗对准100秒+200秒KF精对准，记为IF+KF)、Scheme2(罗经法粗对准100秒+200秒KF精对准，记为GC+KF)、Scheme3(优化对准100秒+200秒KF精对准，记为OBA+KF)、以及本发明所提方案Scheme4(LSEGAKF对准300秒)。

在快速对准中，初始均方误差矩阵选取将影响对准结果。对于水平姿态，设置为0.01°；对于航向角对应的初始均方误差矩阵参数，其可观性相对较差，且直接影响定位发散程度，因此每种对准方案进行了3种不同的参数选择，结果1和误差1对应的参数选择P_{_ψ}＝(0.1°)²，结果2和误差2对应P_{_ψ}＝(1°)²，结果3和误差3对应P_{_ψ}＝(0.01°)²。

对准结束后，将S6.2输出的姿态矩阵转换为欧拉角，利用航向对准结果考核对准精度。试验结果如表3所示：

表3航向对准结果与误差 单位：(°)

航向误差标准差如表4所示：

表4航向误差标准差

可见，本发明(Scheme4)对准精度误差最小。

实施例2

进行了车载导航试验以进一步验证所提对准算法的对准精度与导航性能。

利用实施例1中的4种对准方法进行300秒快速对准试验，对准结束后进行导航误差对比。Scheme1-Scheme3三种方法的时间分配为粗对准100秒，精对准200秒。试验结果如图2～图4所示。

另外还进行了不同对准时间分配下的快速对准试验，导航误差对比如表5所示：

表5不同对准时间分配的导航误差对比

表中，“C30+F270”表示粗对准时间30秒+精对准时间270秒。

可见，本发明(LSEGAKF)在2小时内的导航误差最小，从侧面反映本发明的对准精度最高。

Claims (2)

1.一种基于群仿射性质的旋转调制惯导***的快速对准方法，其特征在于，该方法分为以下步骤：

S1：定义坐标系，惯导***上电开机，惯导***的转位机构执行特定转位次序，惯性测量单元采集陀螺与加速度计数据并实时传输给导航计算机；

S1.1定义坐标系：

定义地心惯性坐标系为惯性参考坐标系，记为i系；地心地固坐标系记为e系；定义“东-北-天”地理坐标系为导航坐标系，记为n系；定义载体坐标系的x-y-z三个轴分别指向载体的右-前-上方向，载体坐标系记为b系，定义IMU坐标系的x-y-z三个轴分别对应x-y-z三个陀螺的敏感轴，IMU坐标系记为s系；

S1.2惯导***开机，设置初始对准时间T_a，启动导航计算机计时器，初始化时刻k＝0；

S1.3 IMU采集k时刻陀螺输出角速度和加速度计输出比力/>并将/>和实时传输给导航计算机；

“～”表示陀螺输出角速度和加速度计输出比力包含测量误差；

S1.4开机后转位机构同步执行双位置转位次序：

1)转位机构静止T_a/2-180/ω-0.5×ω/a秒；

2)绕天向轴转位机构以ω的角速度旋转180°；

3)转位机构静止T_a/2-180/ω-0.5×ω/a秒；

其中，T_a为初始对准时间，单位为秒，ω为转位机构转动的角速度，单位为°/s；a为转位机构转动的角加速度，单位为°/s²；

S2：导航计算机实时接收到陀螺输出角速度和加速度计输出比力/>后，执行惯性导航解算：

S2.1初始化导航参数，具体如下：

S2.1.1定义k时刻的导航参数：IMU的姿态矩阵为速度矢量为v^e(k)，位置矢量为p^e(k)，辅助速度矢量为/>

上标“e”表示此参数为在地心地固坐标系中的参数，以下同；

S2.1.2将k＝0时刻的姿态矩阵进行初始化：

S2.1.3将k＝0时刻的速度矢量v^e(0)进行初始化：

v^e(0)＝[0 0 0]^T (2)

上标“T”表示矩阵转置；

S2.1.4将k＝0时刻的位置矢量p^e(0)进行初始化：

p^e(0)＝[L₀ λ₀ h₀]^T (3)

式中，L₀表示惯导***在初始位置的纬度，λ₀表示惯导***在初始位置的经度，h₀表示惯导***在初始位置的海拔；

S2.1.5将k＝0时刻的辅助速度矢量进行初始化：

为e系相对于i系的旋转角速度在e系中的投影，/>地球自转速率ω_ie＝15°/h；/>表示计算矢量/>的反对称矩阵，以下同；

S2.2导航计算机计时器时间更新：k＝k+1；

S2.3执行地心地固坐标系下的惯性导航解算，具体如下：

S2.3.1进行姿态更新：

式中I_3×3为3×3维的单位矩阵；φ(k)为k时刻的等效旋转矢量，等效旋转矢量由陀螺输出角速度计算得到，φ(k)为φ(k)的模，即φ(k)＝|φ(k)|；

S2.3.2进行速度更新：

式中为加速度计输出的比力值；T_s为IMU数据的采样时间间隔；/>为k时刻的万有引力矢量；k-1时刻的辅助速度矢量/>与k-1时刻的速度矢量v^e(k-1)间的关系、k-1时刻的万有引力矢量/>与k-1时刻的重力矢量g^e(k-1)间的关系分别为

S2.3.3进行位置更新：

S3：构建满足群仿射性质的旋转调制惯导***快速对准状态模型

通过构建满足群仿射性质的旋转调制惯导***快速对准的状态模型，将有效解决非线性模型导致卡尔曼滤波失效问题；具体如下：

S3.1构建满足群仿射性质的旋转调制惯导***快速对准的状态模型

式中，X_k为k时刻的18维状态向量，Z_k为k时刻的3维量测向量；Φ_k|k-1为从k-1时刻到k时刻的状态一步转移矩阵；G_k|k-1为从k-1时刻到k时刻的***噪声分配矩阵，H_k为k时刻的量测矩阵；w_k-1表示k-1时刻的***噪声矩阵；v_k表示k时刻的观测噪声矩阵；

S3.1.1构建18维状态向量X_k：

式中，表示k时刻IMU的姿态误差，下标“lg”表示此误差的类型为满足群仿射性质的左不变误差；/>表示k时刻IMU的速度误差；/>表示k时刻IMU的位置误差；ε^s(k)表示k时刻IMU的陀螺漂移，/>表示k时刻IMU的加速度计零偏，L^s(k)表示k时刻IMU的内杆臂误差；

S3.1.2构建3维量测向量Z_k：

式中，v^e(k)表示k时刻的速度矢量：

为量测速度，因为旋转调制惯导***的对准是运载体停留在原地进行，所以

S3.1.3构建从k-1时刻到k时刻的状态一步转移矩阵Φ_k|k-1：

式中I_18×18表示18×18的单位矩阵，I_3×3表示3×3的单位矩阵，0_3×3表示3×3的零矩阵；

S3.1.4构建从k-1时刻到k时刻的***噪声分配矩阵G_k|k-1：

S3.1.5构建k时刻的量测矩阵H_k：

S4：基于S3构建的旋转调制惯导***快速对准状态模型进行状态量估计

S4.1计算从k-1时刻至k时刻的状态向量后验估计

式中，表示k-1时刻输出的18维状态向量X_k-1的后验估计；/>的初始值/> 表示状态向量X_k从k-1时刻至k时刻的一步预测，即k时刻的先验估计；

S4.2计算从k-1时刻到k时刻的状态一步预测均方误差矩阵：

式中，P_k-1表示k-1时刻的状态后验估计均方误差矩阵，P_kk-1表示从k-1时刻到k时刻的状态一步预测均方误差矩阵；

其中，P_k-1的初始值 表示n系投影下的均方误差矩阵初值，T表示均方误差坐标系转换矩阵：

S4.3计算k时刻滤波增益K_k：

式中，量测噪声协方差矩阵R为：

S4.4计算k时刻状态后验估计

S4.5计算k时刻各导航参数的误差估计值：

姿态误差估计值为/>的第1维至第3维，表示为/>即：

辅助速度误差估计值为/>的第4维至第6维，表示为/>即：

位置误差估计值为/>的第7维至第9维，表示为/>即：

S4.6计算k时刻状态后验估计均方误差阵P_k：

P_k＝P_k|k-1-K_kH_kP_k|k-1 (28)

S5:利用S4.5得到误差估计值修正S2的导航解算结果，具体如下：

S5.1修正姿态矩阵得到修正后的姿态矩阵/>

式中，表示对/>进行矩阵指数运算，其计算公式为：

S5.2修正辅助速度矢量得到修正后的辅助速度矢量/>

S5.3修正位置矢量p^e(k)，得到修正后的位置矢量

S6：判断对准是否继续进行，当k＜T_a时，执行S6.1；当k＝T_a时，执行S6.2，输出修正后的高精度姿态矩阵结束对准；具体如下：

S6.1当k＜T_a时，执行以下步骤：

S6.1.1将修正后的导航参数和/>覆盖导航计算机的内存中未修正的/>p^e(k)，即：

S6.1.2将p^e(k)、/>P_k存储在导航计算机的内存中；然后返回至S2.2，继续往下执行至S6，进行下一次S6的条件判断；

S6.2当k＝T_a时，输出修正后的姿态矩阵完成初始对准。

2.一种根据权利要求1所述基于群仿射性质的旋转调制惯导***的快速对准方法，其特征在于：S1.4中，初始对准时间T_a设置为300秒或600秒。