一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
阅读说明：本技术 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法 (Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm ) 是由 赵玉新 李帅阳 奔粤阳 吴磊 李倩 周广涛 臧新乐 魏廷枭 温官昊 于 2020-11-16 设计创作，主要内容包括：本发明提供一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法,完成初始对准后,利用实时采集三个轴上陀螺的输出信号和加速度计的输出信号进行捷联惯性导航解算；将GPS给出的位置信息作为量测,利用间接法卡尔曼滤波对实时解算的导航参数进行校正；利用校正的导航参数对运载体机动引起的线加速度和哥氏加速度进行补偿；利用互补滤波获得的陀螺积分误差作为量测量,再次利用卡尔曼滤波对已经校正过的捷联惯性导航解算的水平姿态信息进行校正,获得更高的测量精度。该方法有效利用加速度计的低频高精度特性,对陀螺仪进行不断修正,使水平姿态保持较高精度输出,保证系统在不同运动状态下具有较高的姿态测量精度,有一定的工程应用价值。(The invention provides a water surface vessel horizontal attitude measurement method based on a cascading Kalman filtering algorithm, which comprises the steps of collecting output signals of gyros on three axes and output signals of accelerometers in real time to carry out strapdown inertial navigation resolving after initial alignment is completed; taking the position information given by the GPS as measurement, and correcting the navigation parameters resolved in real time by using indirect Kalman filtering; compensating for linear acceleration and coriolis acceleration caused by vehicle maneuvers using the corrected navigation parameters; and (3) taking the gyro integral error obtained by complementary filtering as the measurement quantity, and correcting the corrected horizontal attitude information resolved by the strapdown inertial navigation by using Kalman filtering again to obtain higher measurement precision. The method effectively utilizes the low-frequency high-precision characteristic of the accelerometer to continuously correct the gyroscope, so that the horizontal attitude keeps higher-precision output, the system is ensured to have higher attitude measurement precision in different motion states, and the method has certain engineering application value.)
技术领域
本发明涉及以微机电惯性测量单元为核心器件的姿态测量系统，提供的是水面舰船的基于级联卡尔曼滤波算法的水平姿态测量的方法，属于导航制导与控制领域。
背景技术
随着微机电系统技术的发展，低成本MEMS IMU在导航领域有着越来越多的应用，通过利用基于微机电系统的惯性传感器进行运动参数测量，可以检测船舶在海中复杂的运动状态，实时输出载体的角运动参数和线运动参数，从而实现用户对水面舰船的运动数据采集。
微机电陀螺仪具有随机漂移特性，其积分误差随时间累积，加速度计不存在累积误差，但是易受到载体震动影响。常用的将二者数据融合的算法是卡尔曼滤波和互补滤波，例如在专利申请号为201811070907.X，名称为“基于机动状态判断的MEMS惯性导航系统水平姿态自修正方法”的专利文件中，通过比较加速度计输出和当地重力加速度幅值，将载体运动分为低，中，高动态。在低和中动态时，实时调整量测噪声矩阵，在高动态时只进行时间更新。但是若载体较长时间处于高动态下，则姿态误差会越来越大。又如在专利申请号为201911277173.7，名称为“一种高精度无人机系统及智能控制方法”的专利文件中，基于GPS模块接受卫星信号的精度因子和速度信息对互补滤波器的截止频率建立了自适应函数来满足不同运动状态下的解算精度。本发明结合互补滤波和级联卡尔曼滤波器，通过一级卡尔曼滤波得到的导航参数借助比力方程补偿载体线加速度和哥氏加速度得到理论加速度计输出，再与实际加速度计输出比较得出陀螺漂移修正量，将其作为二级卡尔曼滤波的量测，最后得出最优导航参数。优点是通过二级卡尔曼滤波弥补了互补滤波增益固定且不一定为最优的缺点，以及在不同运动状态下都能保持水平姿态较高精度输出，有效的提高了系统姿态测量精度，具有一定的工程应用价值。
发明内容
本发明的目的在于提高水面舰船在机动场景下的姿态测量精度，提供一种基于级联卡尔曼滤波算法的水平姿态测量的方法，为水面舰船提供准确的水平姿态信息。
本发明的目的是这样实现的：捷联惯性导航系统完成初始对准之后，利用实时采集三个轴上陀螺的输出信号和加速度计的输出信号fb进行捷联惯性导航解算；将GPS给出的位置信息作为量测，利用间接法卡尔曼滤波对实时解算的导航参数进行校正；借助于比力方程，利用校正的导航参数对运载体机动引起的线加速度和哥氏加速度进行补偿；利用互补滤波获得的陀螺积分误差作为量测量，再次利用卡尔曼滤波对已经校正过的捷联惯性导航解算的水平姿态信息进行校正，获得更高的测量精度。具体步骤如下：
步骤1、对捷联惯性导航系统的惯性测量元件进行充分预热，并完成初始对准，使之进入导航工作状态；
步骤2、将GPS给出的位置信息作为量测，利用间接法卡尔曼滤波器对实时解算的导航参数进行校正，获得校正后的捷联惯性导航系统的捷联矩阵速度V1 n和位置P1；
步骤3、利用步骤2中校正后的捷联惯性导航系统的捷联矩阵速度V1 n和位置P1，借助于比力方程对运载体机动引起的线加速度和哥氏加速度进行补偿，得到加速度计在导航系(n系，本发明中导航坐标系选取地理坐标系)的理论输出f1 n；
步骤4、将步骤3中求得的f1 n投影到载体系(b系)，借助加速度计在载体系的实际输出fb，利用互补滤波获得的陀螺积分后的姿态误差进而获得陀螺积分误差Δωb；
步骤5、将步骤4中得到的陀螺积分误差Δωb作为量测量，再次利用卡尔曼滤波对已经校正过的捷联惯性导航解算的捷联矩阵进行校正，获得校正后的捷联姿态矩阵
本发明还包括这样一些结构特征：
1.所述步骤3加速度计在导航系的理论输出f1 n的求取：
其中，V1 n为经卡尔曼滤波校正后的速度，为相邻两个姿态解算时刻的速度变化率，为校正后的地球自转角速度在导航系的投影，为运载体的水平速度引起导航系相当于地球坐标系(e系)的旋转角速度，gn为校正后的地球重力矢量在导航系的投影。
2.所述步骤4陀螺积分后的姿态误差的计算：
其中，f1 n为加速度计在导航系的理论输出，fb为加速度计载体系的实际输出，为经卡尔曼滤波校正后的捷联姿态矩阵，T表示对矩阵转置，||为相应矢量模值。
3.所述步骤4中量测量的选取，将陀螺积分误差Δωb作为捷联惯性导航进行卡尔曼滤波时的量测值Z2(t)，量测阵Z2(t)和对应量测阵H2(t)分别为：
与现有技术相比，本发明的有益效果是：本发明综合利用微机电惯性测量单元输出的加速度和角速度信息，实现了系统在不同运动状态下高精度的水平姿态测量。通过对运载体机动引起的线加速度和哥氏加速度进行补偿，有效利用加速度计的低频高精度特性，对陀螺仪进行不断修正，使水平姿态保持较高精度输出，即使系统有运动加速度时，依然保持失准角的最优计算，保证系统在不同运动状态下均具有较高的姿态测量精度，有效的提高了系统姿态测量精度，具有一定的工程应用价值。
本发明对运载体机动引起的线加速度和哥氏加速度进行补偿，有效利用加速度计的低频高精度特性，对陀螺仪进行不断修正，使水平姿态保持较高精度输出，即使系统有运动加速度时，依然保持失准角的最优计算，保证系统在不同运动状态下均具有较高的姿态测量精度，具有一定的工程应用价值。
附图说明
图1为本发明的实现流程图。
图2为本发明的算法实现流程图。
具体实施方式
为了更好地了解本发明的技术内容，下面结合说明书附图加以解释和说明，该方法的主要步骤为：
步骤1、对捷联惯性导航系统的惯性测量元件进行充分预热；
步骤2、初始化捷联惯性导航系统的导航参数，实时采集加速度计输出的比力fb和陀螺仪输出的角速度
步骤3、对捷联惯性导航系统进行初始对准，得到载体系(b系)到导航坐标系(n系，本发明中导航坐标系选取地理坐标系)的初始捷联姿态矩阵之后开始进入导航工作状态；
步骤4、根据步骤3中得到的陀螺捷联惯性导航系统的初始捷联姿态矩阵和步骤2中实时采集到角速度递推更新得到姿态四元数并对其进行归一化计算，归一化后的姿态四元数q具体表达式为：
q＝[q1 q2 q3 q4]T (5)
其中，q1、q2、q3和q4是归一化后四元数q的元素；
步骤5、根据步骤4中得到的归一化后的姿态四元数q计算得到新的捷联姿态矩阵
步骤6、利用步骤5得到的捷联姿态矩阵将步骤2采集到的捷联惯性导航系统的加速度计输出的比力fb转换到导航坐标系下：
其中，fb表示加速度计输出比力在导航坐标系的投影；
步骤7、根据步骤6中系统的加速度计输出比力在导航坐标系的投影fb去除有害加速度后得到系统的加速度，进一步更新计算得到速度，记为Vn(t)：
其中，为系统解算的东向速度、为其北向速度、为其天向速度，t表示当前时刻；
步骤8、利用步骤7更新计算后的速度Vn(t)更新计算捷联惯性导航系统的位置P(t)：
其中，为纬度,λ(t)为经度，h(t)为高度，t表示当前时刻；
步骤9、选取位置误差(纬度误差经度误差δλ和高度误差δh)、东北天三个方向的速度误差δV＝[δVE δVN δVU]T、平台失准角误差φ＝[φx φy φz]T、载体系三轴的加速度计的零位偏移ΔA＝[ΔAbx ΔAby ΔAbz]T和陀螺仪的常值漂移ε＝[εbxεby εbz]T为卡尔曼滤波的状态估计量X，即
X＝[δr δV φ ΔA ε]T (10)
步骤10、选取卡尔曼滤波的系统噪声向量WB为：
W＝[wax way waz wgx wgy wgz]T (11)
其中，wax、way和waz为捷联惯性导航系统在载体系中三轴加速度计的随机噪声，wgx、wgy和wgz为捷联惯性导航系统在载体系中三轴陀螺仪的随机噪声，均为高斯白噪声；
步骤11、利用GPS提供的纬度和经度λA(t)作为捷联惯性导航系统进行卡尔曼滤波的外部辅助信息，与捷联惯性导航系统导航更新计算的位置信息(纬度和经度λ(t))分别作差，将所得到的位置差值作为捷联惯性导航进行卡尔曼滤波时的量测值Z1(t)，量测值Z1(t)和对应量测阵H1(t)分别为：
步骤12、基于卡尔曼滤波算法对捷联惯性导航系统的误差进行实时估计，得到捷联惯性导航系统的位置误差速度误差和平台失准角的状态估计值
步骤13、利用步骤12中估计出来的位置误差速度误差来校正捷联惯性导航系统解算的位置和速度信息；
其中，V1 n为校正后的惯导速度、P1为校正后的惯导位置；
步骤14、利用步骤12实时估计出来的平台失准角来构造补偿四元数对捷联惯性导航系统解算的当前时刻捷联姿态矩阵对应的姿态四元数q＝[q1 q2 q3 q4]T进行补偿得到修正后的四元数q′＝[q′1 q′2 q′3 q′4]T：
其中，修正后的四元数q′的元素为：
步骤19、将步骤18中得到的修正后的四元数q′进行归一化处理：
得到归一化后的四元数为：
其中，q′2、和是归一化后四元数的元素；
步骤20、根据归一化后的姿态四元数计算得到校正后的捷联姿态矩阵
至此，完成卡尔曼滤波器1对捷联惯性导航系统的导航参数校正的过程；
步骤21、利用步骤17至步骤20中校正的捷联惯性导航系统的导航参数，借助于比力方程对运载体机动引起的线加速度和哥氏加速度进行补偿，得到加速度计在导航系的理论输出f1 n：
其中，V1 n为经卡尔曼滤波校正后的速度，为相邻两个姿态解算时刻的速度变化率，为校正后的地球自转角速度在导航系的投影，为运载体的水平速度引起导航系相当于地球坐标系的旋转角速度，gn为校正后的地球重力矢量在导航系的投影；
步骤22、对步骤21中求得的f1 n在b系的投影和加速度计的实际输出两矢量进行叉乘运算得陀螺积分后的姿态误差
其中，为经卡尔曼滤波校正后的捷联姿态矩阵，T表示对矩阵转置，| |为相应矢量模值；
步骤23、利用步骤22得到的姿态误差求取陀螺积分误差Δωb：
其中，kp，ki为比例系数，dt为解算周期；
步骤24、将步骤23中求得的陀螺积分误差Δωb作为捷联惯性导航进行卡尔曼滤波时的量测值Z2(t)，量测值Z2(t)和对应量测阵H2(t)分别为：
步骤24、基于卡尔曼滤波算法对捷联惯性导航系统的误差进行实时估计，得到捷联惯性导航系统的位置误差速度误差和平台失准角的状态估计值
步骤25、利用步骤24实时估计出来的平台失准角来构造补偿四元数对捷联惯性导航系统解算的当前时刻捷联姿态矩阵对应的姿态四元数进行补偿得到修正后的四元数q″＝[q″1 q″2q″3q″4]T：
其中，修正后的四元数q″的元素为：
步骤26、将步骤25中得到的修正后的四元数q″进行归一化处理：
得到归一化后的四元数为：
其中，和是归一化后四元数的元素；
步骤27、根据归一化后的姿态四元数计算得到校正后的捷联姿态矩阵
至此，卡尔曼滤波器2对捷联惯性导航系统的导航参数校正的过程；
至此就完成了基于级联卡尔曼滤波算法的水面舰船水平姿态更新和修正。
综上，本发明涉及以微机电惯性测量单元为核心器件的姿态测量系统，提供的是一种基于级联式卡尔曼滤波的水面舰船水平姿态测量方法。捷联惯性导航系统完成初始对准之后，利用实时采集三个轴上陀螺的输出信号和加速度计的输出信号fb进行捷联惯性导航解算；将GPS给出的位置信息作为量测，利用间接法卡尔曼滤波对实时解算的导航参数进行校正；借助于比力方程，利用校正的导航参数对运载体机动引起的线加速度和哥氏加速度进行补偿；利用互补滤波获得的陀螺积分误差作为量测量，再次利用卡尔曼滤波对已经校正过的捷联惯性导航解算的水平姿态信息进行校正，获得更高的测量精度。该方法有效利用加速度计的低频高精度特性，对陀螺仪进行不断修正，使水平姿态保持较高精度输出，即使系统有运动加速度时，依然保持失准角的最优计算，保证系统在不同运动状态下均具有较高的姿态测量精度，具有一定的工程应用价值。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：路径规划方法、装置、无人设备及存储介质