利索能及
我要发布
收藏
专利号: 2024117049470
申请人: 江苏科技大学
专利类型:发明专利
专利状态:已下证
更新日期:2026-03-02
缴费截止日期: 暂无
联系人

摘要:

权利要求书:

1.一种包含野值检测和多重回溯滤波的SINS鲁棒行进间对准方法,其特征在于:包括以下步骤:步骤1:实时获取并存储SINS和GNSS的输出信息,并进行姿态更新;

步骤2:检测当前时刻GNSS采集到的速度信息是否被野值信息污染,得到检测结果;

步骤3:依据检测结果和参考矢量,重构观测矢量;

步骤4:基于参考矢量和重构的观测矢量,采用多重回溯滤波方法,实现对准;

其中,步骤1中,所述的进行姿态更新,具体操作包括:

根据SINS输出信息,对以下真实载体坐标系姿态变换矩阵微分方程进行实时解算:其中,b(t)表示连续时间t下的真实载体坐标系,ib表示初始时刻固定在惯性空间的载体坐标系, 表示连续时间t下从b系旋转到ib系的姿态变换矩阵, 表示连续时间t下的载体系姿态变换矩阵的微分形式, 表示载体坐标系相对于惯性坐标系的旋转角速度在载体坐标系下的投影,(·)×表示(·)的反对称矩阵;

将连续时间t离散化,离散化后的时间用k表示,对离散化的真实载体系姿态变换矩阵进行迭代计算,实现姿态更新:其中,k为当前时刻,k‑1表示上一时刻, 通过下式求得:其中,E3表示3×3的单位矩阵,Δθ1和Δθ2表示陀螺仪输出的两个连续时刻的角速度增量, 表示载体坐标系从k‑1时刻到k时刻的旋转矢量;

根据GNSS输出信息,对以下导航坐标系姿态变换矩阵微分方程进行实时解算:其中,n(t)表示连续时间t下的导航坐标系,in表示初始时刻固定在惯性坐标空间的导航坐标系, 表示连续时间t下从n系旋转到in系的姿态变换矩阵, 表示连续时间t下导航坐标系姿态变换矩阵的微分形式, 表示导航坐标系相对于惯性坐标系的旋转角速度在导航坐标系下的投影;

对离散化的导航系姿态变换矩阵进行迭代计算,实现姿态更新:其中 通过下式计算:

其中, 为导航系从k‑1时刻到k时刻的旋转矢量,Ts为更新周期;

其中,步骤2中,所述的检测当前时刻GNSS采集到的速度信息是否被野值污染,得到检测结果,具体操作包括:S2_1:构造如下相关梯度值:

式中, 表示从in坐标系旋转到ib坐标系的常值姿态矩阵, 表示包含野值信息的观测矢量,表示为: Ξv表示野值信息,βv,S表示观测矢量, 表示导航坐标系n旋转到in系的姿态变换矩阵;αv,s表示参考矢量;

S2_2:若相关梯度值超过预设的阈值时,说明当前时刻GNSS采集到的速度信息被野值信息污染,否则说明当前时刻GNSS采集到的速度信息未被野值信息污染;

其中,步骤3中,所述的依据检测结果和参考矢量,重构观测矢量,具体操作包括:S3_1:设计如下权重函数γ(θk):

式中,θk表示相关梯度值,ρ为预设的阈值;

S3_2:根据预设的阈值ρ以及权重函数γ(θk)对包含野值信息的观测矢量 进行重构,得到重构后的观测矢量 表示为:其中,步骤4中,所述的基于参考矢量和重构的观测矢量,采用多重回溯滤波方法,实现对准,具体操作包括:S4_1:考虑SINS中陀螺仪量测误差的情况下,对陀螺仪输出的角速度信息进行重新建模,表示为:其中, 表示实际陀螺仪输出的角速度信息, 表示载体坐标系相对于惯性坐标系的旋转角速度在载体坐标系下的投影, 表示旋转角速度误差,bg表示陀螺仪偏置,wg表示随机噪声;

对应的计算载体系姿态变换矩阵表示为:

其中,表示通过计算得到的载体坐标系,称为计算载体系, 表示计算载体系姿态变换矩阵的微分形式, 表示从 系旋转到ib系的姿态变换矩阵;

S4_2:真实载体系姿态变换矩阵 与计算载体系姿态变换矩阵 之间存在误差,表示为:其中, 表示从b系旋转到 系的姿态变换矩阵, 表示从计算载体系 到真实载体系b的旋转矢量;

结合上式,计算载体系 到真实载体系b的旋转矢量微分方程表示如下:S4_3:选取引起计算载体坐标系 与真实载体坐标系b误差的旋转矢量 以及引起陀螺仪常值误差的陀螺仪偏置bg作为系统状态变量,表示为:对重构后的观测矢量 进行整理拆分,表示为:

式中,b(tm)表示积分下限时间tm时的真实载体系, 表示从b(t)旋转到b(tm)的姿态b变换矩阵, 表示从b(tm)旋转到ib的姿态变换矩阵;f 表示加速度计测量的比力信息;

定义 根据链式法则,展开 得:

用系统状态变量表示重构后的观测矢量 表示为:

S4_4:构造如下系统量测方程:

Zk=HkXk+Vk

其中:

S4_5:利用SINS和GNSS输出数据和当前时刻的姿态变换矩阵进行解算,估计得到当前姿态数据,对当前姿态数据进行正向滤波,估计当前时刻的系统状态变量并在下一时刻进行补偿,完成正向滤波;

S4_6:正向滤波完成后,对SINS和GNSS输出数据进行处理,利用反向滤波的姿态变换矩阵方程和速度更新方程,进行解算,估计得到当前姿态数据,对当前姿态数据进行反向滤波,估计当前时刻的系统状态变量并在下一时刻进行补偿,完成反向滤波;

S4_7:通过不断进行正向滤波与反向滤波,实现对准。

2.根据权利要求1所述的一种包含野值检测和多重回溯滤波的SINS鲁棒行进间对准方法,其特征在于:在执行步骤3之前,还包括以下步骤:在连续时间t下,导航坐标系下的比力方程表示为:

n

其中,V表示导航坐标系下载体的速度矢量, 表示导航坐标系下载体的速度矢量的b微分, 表示连续时间t下的姿态矩阵,f表示加速度计测量的比力信息,e表示地球坐标系, 表示地球自转角速度在导航坐标系下的投影, 表示导航坐标系相对地球坐标系n的角速度在导航系下的投影,g表示重力加速度在导航坐标系下的投影;

对上述导航坐标系下的比力方程在[0,t]时间段内进行积分,整理如下:其中,αv为参考矢量,βv为观测矢量,表示如下:

对上述构造的参考矢量和观测矢量采用滑动窗口方法,将积分区间改为[tm,t],tm为可变的积分下限时间,改写如下:

3.根据权利要求2所述的一种包含野值检测和多重回溯滤波的SINS鲁棒行进间对准方法,其特征在于:所述的反向滤波的姿态变换矩阵方程是根据离散化的正向姿态矩阵更新方程逆推得到的,表示为:其中,表示反向过程, 表示反向过程中k‑1时刻从b系旋转到n系的姿态变换矩阵, 表示反向过程中k时刻从b系旋转到n系的姿态变换矩阵, 表示反向过程中k时刻陀螺仪输出的实际角速度信息。

4.根据权利要求3所述的一种包含野值检测和多重回溯滤波的SINS鲁棒行进间对准方法,其特征在于:反向滤波的速度更新方程是根据离散化的正向速度更新方程逆推得到的,表示为:其中,表示反向过程, 表示反向过程中k时刻导航坐标系下载体的速度矢量,表示反向过程中k时刻的加速度计测量的比力信息, 表示反向过程中k时刻的地球自转角速度在导航坐标系下的投影, 表示反向过程中k时刻的导航坐标系相对地球坐标系的角速度在导航系下的投影。

5.根据权利要求1所述的一种包含野值检测和多重回溯滤波的SINS鲁棒行进间对准方法,其特征在于:所述的正向滤波完成后,对SINS和GNSS输出数据进行处理,具体操作包括:每一次正向滤波与反向滤波的切换均对相应的数据进行处理,将反向过程的数据与正向过程的数据联合表示为:n

其中, 表示反向过程中k时刻的速度信息,V (N‑k)表示正向过程中N‑k时刻的速b度信息, 表示反向过程中k时刻的比力信息,f (N‑k)表示正向过程中N‑k时刻的比力信息, 表示反向过程中k时刻的角速度信息, 表示正向过程中N‑k时刻的角速度信息。

6.根据权利要求1所述的一种包含野值检测和多重回溯滤波的SINS鲁棒行进间对准方法,其特征在于:反向滤波的初始值,设置为:反向过程中的常值姿态矩阵等于正向过程中最后时刻的从b系旋转到n系的姿态变换矩阵,反向过程中的初始速度等于正向过程中最后时刻速度的相反数。

7.根据权利要求1所述的一种包含野值检测和多重回溯滤波的SINS鲁棒行进间对准方法,其特征在于:所述的估计当前时刻的系统状态变量并在下一时刻进行补偿,具体包括:将陀螺仪偏置bg,k补偿回溯过程的角速度 更新真实载体坐标系下姿态变换矩阵更新:式中, 表示反向过程中k时刻的载体坐标系相对于惯性坐标系的旋转角速度在载体坐标系下的投影, 表示正向过程中N‑k时刻的陀螺仪输出的实际角速度信息。

8.根据权利要求1所述的一种包含野值检测和多重回溯滤波的SINS鲁棒行进间对准方法,其特征在于:反向滤波过程中,系统状态变量表示为: