CN118111434A - 基于自适应互补卡尔曼滤波的偏振光/mems组合航姿测量方法 - Google Patents
基于自适应互补卡尔曼滤波的偏振光/mems组合航姿测量方法 Download PDFInfo
- Publication number
- CN118111434A CN118111434A CN202410036652.4A CN202410036652A CN118111434A CN 118111434 A CN118111434 A CN 118111434A CN 202410036652 A CN202410036652 A CN 202410036652A CN 118111434 A CN118111434 A CN 118111434A
- Authority
- CN
- China
- Prior art keywords
- polarized light
- measurement
- attitude
- kalman filtering
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 43
- 238000001914 filtration Methods 0.000 title claims abstract description 39
- 230000000295 complement effect Effects 0.000 title claims abstract description 29
- 230000005484 gravity Effects 0.000 claims abstract description 20
- 238000000691 measurement method Methods 0.000 claims abstract description 8
- 238000005259 measurement Methods 0.000 claims description 45
- 239000013598 vector Substances 0.000 claims description 37
- 239000011159 matrix material Substances 0.000 claims description 28
- 230000008569 process Effects 0.000 claims description 12
- 230000003044 adaptive effect Effects 0.000 claims description 11
- 238000012937 correction Methods 0.000 claims description 6
- 238000004422 calculation algorithm Methods 0.000 claims description 5
- 238000005070 sampling Methods 0.000 claims description 3
- 238000012216 screening Methods 0.000 claims description 3
- 230000010287 polarization Effects 0.000 abstract description 15
- 238000012360 testing method Methods 0.000 abstract description 15
- 238000011160 research Methods 0.000 abstract description 7
- 230000017105 transposition Effects 0.000 abstract description 2
- 239000011664 nicotinic acid Substances 0.000 description 8
- 230000004927 fusion Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 239000013307 optical fiber Substances 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 230000007246 mechanism Effects 0.000 description 3
- 229910000838 Al alloy Inorganic materials 0.000 description 2
- WHXSMMKQMYFTQS-UHFFFAOYSA-N Lithium Chemical compound [Li] WHXSMMKQMYFTQS-UHFFFAOYSA-N 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 229910052744 lithium Inorganic materials 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 108091092878 Microsatellite Proteins 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 210000001557 animal structure Anatomy 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 238000005452 bending Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 210000004556 brain Anatomy 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Landscapes
- Navigation (AREA)
Abstract
基于自适应互补卡尔曼滤波的偏振光/MEMS组合航姿测量方法，它涉及一种偏振光/MEMS组合航姿测量方法。本发明为了解决针对图像式偏振传感器/MIMU组合导航***在倾斜、遮挡等复杂应用场景下的研究仍然较少，并且存在一定的局限性的问题。本发明通过偏振光传感器和加速度计消除惯性导航***的累积误差，本发明根据实测重力与局部理论重力的相关性，以最大相关熵构建自适应互补因子，进一步提高姿态和航向精度；本发明搭建了基于自适应互补卡尔曼滤波的偏振光/MEMS组合航姿测量装置，并且通过室外动态转位试验和车载试验对本专利方法进行验证。本发明属于无人平台导航技术领域。
Description
技术领域
本发明涉及一种偏振光/MEMS组合航姿测量方法，属于无人平台导航技术领域。
背景技术
随着自主无人***的快速发展，GNSS拒止环境下的自主性强、可靠性高的导航***成为无人平台执行任务的重要保障。目前常见的导航方法有惯性导航、卫星导航、地磁导航、大气偏振光导航等，这些导航技术由于其不同的特点而被应用于不同的领域。其中大气偏振导航***利用自然光在大气中的稳定的随时空变化的散射模式来进行定向，具有误差不随时间累计、高自主性以及抗电磁干扰与抗无线电欺骗的定向能力，因此最近几年被广泛研究。
学者们通过对动物器官感知自然环境形成导航信息的机理和其大脑内导航细胞处理信息的机制研究，将自然界的偏振光信息源转化为载体运动的航向信息，分别研制了基于点源式和图像式的仿生偏振光传感器获取载体航向。并且研究了在沙漠、水下、城市等复杂环境下的抗干扰仿生偏振光航向测量技术。然而通过单一传感器获得导航信息的方式存在很多缺陷。惯性导航***(INS)误差会随时间的推移不断累积。偏振光罗盘的环境适应性较差，极易受到云层、树木或建筑物遮挡，导致测量精度大幅下降。
INS和偏振光罗盘相结合的方法可以获得更好的性能。一方面，INS航向解算的累积误差可以利用偏振光罗盘进行校正。另一方面，INS可以弥补偏振光罗盘短时内的随机误差，为遮挡情况下的导航提供必不可少的航姿信息。因此学者们对偏振光辅助的组合导航技术进行了大量研究。
Xian将点源式偏振光传感器输出的航向作为新的约束节点加入到视觉惯性里程计中，提高了整体的位姿求解精度。He搭建了基于MEME惯性测量单元(MIMU)/图像式偏振光传感器/Global Navigation Satellite System(GNSS)的组合导航***，并使用卡尔曼滤波方法实现导航信息融合，其组合航向精度优于1°。Huang研究了应用于微卫星姿态测量的点源式偏振光/地磁的组合导航***，并进行了相关的仿真试验。Du[16,17]利用点源式偏振光传感器辅助捷联惯性导航***(SINS)进行初始对准，并且对卡尔曼滤波器的可观测性进行了分析。Zhang利用偏振度(DOP)来筛选可靠的E矢量，从而提高航向测量的鲁棒性，并仿真了与星敏感器和INS融合的组合导航***。Xie研究了基于图优化的多源融合导航***，并利用偏振光传感器对航向进行约束，从而减少GNSS拒止环境下的航向漂移。Zhao研究了基于图像式偏振光传感器和IMU的无缝组合导航方法，利用long s hort-term memory(LSTM)神经网络学***角变化状态下的航向误差。Shen提出了一种新的鲁棒滤波方法实现了MIMU/GPS/偏振罗盘的导航信息融合，提高了载体定位精度。Yang研究了不连续观测下的偏振光罗盘辅助惯性导航***，复杂场景下的航向误差小于1.1894°。Liu构建了基于点源式偏振传感器和IMU的紧组合航姿参考***，提高其自适应能力。Yang研究了基于偏振光辅助的INS/星敏感器夜间姿态测量***，为复杂夜间环境导航提供了参考。
综上所述，虽然图像式偏振光传感器通过探测全天空偏振模式来获得相对点源式偏振传感器更鲁棒的导航信息，但是针对图像式偏振传感器/MIMU组合导航***在倾斜、遮挡等复杂应用场景下的研究仍然较少，并且存在一定的局限性：(1)未充分利用加速度计测量的重力矢量信息对俯仰角和横滚角的累积误差进行修正；(2)对载体倾斜状态下的运动适应性不足；(3)在重力矢量信息不可用状态下，没有鲁棒的信息融合机制。
发明内容
本发明为解决针对图像式偏振传感器/MIMU组合导航***在倾斜、遮挡等复杂应用场景下的研究仍然较少，并且存在一定的局限性的问题，进而提出一种基于自适应互补卡尔曼滤波的偏振光/MEMS组合航姿测量方法。
本发明为解决上述问题采取的技术方案是：本发明的步骤包括：
步骤1、设定陀螺仪和加速度计的三轴输出分别为θ＝[θx,θy,θz]T和a＝[ax,ay,az]T，载体当前姿态下的旋转矩阵为首先使用旋转矩阵将导航系下的重力矢量[0,0,-1]T投影到载体系下
步骤2、然后将载体系下的重力矢量值gb与加速度计输出值的单位化测量值进行交叉向量积运算
步骤3、将交叉向量积的结果Eg输入到PI调节器中，从而获得陀螺仪的漂移补偿量Δθ
Δθ＝Eg(kp+ki·Δt) (21)
公式(21)中，Δt表示IMU的采样间隔，ki通常取1，kp由加速度计的合力|a|和理论重力值|glocal|之间的偏差确定；
步骤4、基于最大相关熵因子kp的自适应互补卡尔曼滤波方法其中kp表示为
公式(22)中，σ表示高斯核函数的宽度，从而可以获得修正后的陀螺仪测量值θ％＝θ-Δθ；
步骤5、将修正后的陀螺仪测量值输入到IMU的姿态更新方程中，即可实现基于加速度计修正的陀螺仪姿态更新方法，降低陀螺仪的累积误差；
步骤6、然后构建偏振光传感器和MEMS卡尔曼滤波的状态方程和量测方程，将平台失准角、零偏漂移误差向量和陀螺比例因子误差向量作为状态向量，偏振光传感器测量的航向作为观测向量；最后进行时间更新和量测更新，即可实时输出载体的航姿状态。
进一步的，将步骤4中获得陀螺仪测量值输入到IMU的姿态更新方程中，即可实现基于加速度计修正的陀螺仪姿态更新方法，降低陀螺仪的累积误差；将状态方程的时间更新，将量测方程的量测更新，设定为状态向量X的最优估计值，则第k时刻的自适应互补卡尔曼滤波航姿测量算法具体步骤为：
步骤401、状态一步预测
步骤402、状态一步预测均方误差阵
步骤403、量测判别
根据偏振光传感器航向测量值yawpolar与IMU的航向更新值yawimu的差异，对观测量进行筛选；当abs(yawpolar-yawimu)＜ε时，认为Zk有效，进行卡尔曼滤波，逐步完成状态更新和量测更新；若abs(yawpolar-yawimu)＞ε，认为Zk无效，跳过卡尔曼滤波的量测更新步骤，直接将基于互补滤波的IMU姿态更新结果作为航姿参考***的输出值；
步骤404、滤波增益
步骤405、状态估计
步骤406、状态估计均方误差阵
Pk＝(I-KkHk)Pk/k-1(26)；
上述过程中ε取经验值2°，卡尔曼滤波预测更新过程的初值协方差矩阵P(0)选择单位矩阵，在这个过程中假设***噪声和观测噪声都是高斯白噪声，它们的协方差矩阵分别为Q0和R0。
本发明的有益效果是：
1、本发明提出了一种基于自适应互补卡尔曼滤波的姿态航向测量新方法，通过偏振光传感器和加速度计消除惯性导航***的累积误差；
2、本发明根据实测重力与局部理论重力的相关性，以最大相关熵构建自适应互补因子，进一步提高姿态和航向精度；
3、本发明搭建了基于自适应互补卡尔曼滤波的偏振光/MEMS组合航姿测量装置，并且通过室外动态转位试验和车载试验对本专利方法进行验证。
附图说明
图1是仿生偏振视觉传感器倾斜模式下的探测过程示意图；
图2是自适应互补卡尔曼滤波流程图；
图3是转位试验设备图；
图4是转位试验航姿测量结果示意图；
图5是无人平台车载试验设备及运行轨迹示意图；
图6是无人平台车载试验航姿测量结果示意图。
实施例
结合图1至图6说明本实施例
在MEMS惯性测量单元(IMU)中，陀螺仪短时间内可以提供高动态的姿态数据，但因漂移的影响，长时间后精度会降低。在载体静止或匀速状态下，加速度计测量的重力矢量误差不会随时间累计，但其短期内的精度差。利用两者在频域上的互补特性，采用互补滤波器进行数据融合，提高姿态测量的精度。
在IMU姿态更新算法，选取“东–北–天(E–N–U)”地理坐标系作为捷联惯导***的导航参考坐标系，后面记为n系，则以n系作为参考系的姿态微分方程为
公式(1)中，矩阵表示载体系(b系)相对于导航坐标系(n系)的姿态阵。陀螺输出的是b系相对于惯性系(i系)的角速度/>而角速度信息/>不能直接测量获得，所以通常不会对上述微分方程进行直接求解，而是结合绝对惯性坐标系i实现姿态更新。
根据矩阵链乘规则，有
公式(2)中，角标括号中的符号m表示tm时刻；由于i系是绝对不动的惯性参考坐标系，它与时间无关，不需标注时刻；而n系和b系相对于i系都是动坐标系，均跟时间有关，需标注时刻；矩阵表示以i系作为参考基准，b系从tm-1时刻到tm时刻的旋转变化，/>可由陀螺角速度/>确定；/>表示以i系作为参考基准，n系从tm时刻到tm-1时刻的旋转变化，包含两部分：地球自转引起的导航系旋转，以及惯导***在地球表面附近移动因地球表面弯曲而引起的n系旋转；但是b MEMS的输出惯性信息的随机噪声通常会将地球自转角速率淹没，所以该项不予考虑。
和/>分别表示tm-1和tm时刻的捷联姿态矩阵；若陀螺在时间段[tm-1,tm]内(T＝tm-tm-1)进行了两次等间隔采样，角增量分别为Δθm和Δθm-1，采用“单子样+前一周期”圆锥误差补偿算法，有
公式(3)和(4)中，记为圆锥误差补偿后的陀螺输出三维角增量/>构成的反对称矩阵；然而，由于平台失准角φ＝[φE,φN,φU]T的存在，当前时刻的等效旋转矩阵会存在一定的偏差，可以表示为：
为修正后的载体等效旋转矩阵。
然后可以根据惯性器件的姿态误差传播方程构建***状态方程；***的状态向量可以表示为：
X＝[φE,φN,φU,Gbx,Gby,Gbz,Gsx,Gsy,Gsz]T (6)
公式(6)中，Gb＝[Gbx,Gby,Gby]T表示载体系下的零偏漂移误差向量；Gs＝[Gsx,Gsy,Gsz]T表示陀螺的比例因子误差向量；***的状态方程可以表示为
w中的每一项表示对应状态变量的高斯噪声项，定义为：
因此，***的状态矩阵可以表示为
公式(9)中，A可以表示为
选取仿生偏振光传感器解算的航向角作为观测值；其观测方程为
Z＝HX+V (11)
公式(11)中，H＝[0 0 1 01×6]，V＝[wpolar]。并且定义Q和R分别为***噪声和观测噪声的协方差矩阵；
然后进行偏振光传感器水平角误差补偿；仿生偏振光传感器测得的偏振角AOP为入射光经过鱼眼镜头和偏振片产生的偏振角度，该角度是相对于相机坐标系的参考轴的角度；但是偏振光传感器的水平姿态角会影响传感器观测到的天空区域，进而影响AOP的测量值；因此需要找到图像中像素与入射光的对应关系，从而实现仿生偏振光传感器水平角误差补偿；如图1所示，代表任意像素点对应的观测方向，在相机坐标系下，γc是/>相对Zc轴的离轴角，αc是/>在OXcYc平面的投影矢量与Xc轴的夹角，则/>可以表示为：
公式(12)中，γl、αl为水平参考坐标系下观测点的天顶角、方位角；在水平参考坐标系下的表达式为
其中
公式(14)中，roll和pitch表示相机的横滚和俯仰角，为向量/>的元素；从入射光坐标系到水平参考坐标系的转换矩阵可以计算为
公式(15)中，由此可以获得水平角补偿后的水平参考坐标系下的天顶角和方位角；进一步地，入射光坐标系下的偏振角φ与偏振光传感器测得的偏振角ψ的关系为：
进一步地，为了利用瑞丽散射模型，需要计算偏振矢量在入射光坐标系中的表达式
公式(17)中，φ为入射光坐标系下的偏振角，和/>分别为入射光坐标系y轴和x轴的坐标基向量；可以分别表示为：
即可求出倾斜状态下的偏振视觉航向；
尽管传统的卡尔曼滤波方法可以利用偏振航向来抑制MIMU的航向漂移；然而，载体在长时间运行时同样存在姿态漂移；组合***的姿态误差将进一步影响偏振航向的输出值，降低组合***的航向精度；因此，使用加速度计测量的绝对重力矢量和陀螺仪角速率进行互补滤波，在修正姿态的基础上，提高航向解算精度；
设定陀螺仪和加速度计的三轴输出分别为θ＝[θx,θy,θz]T和a＝[ax,ay,az]T，载体当前姿态下的旋转矩阵为首先使用旋转矩阵将导航系下的重力矢量[0,0,-1]T投影到载体系下
然后将载体系下的重力矢量值gb与加速度计输出值的单位化测量值进行交叉向量积运算
将交叉向量积的结果Eg输入到PI调节器中，从而获得陀螺仪的漂移补偿量Δθ
Δθ＝Eg(kp+ki·Δt)
式中Δt表示IMU的采样间隔，ki通常取1，kp由加速度计的合力|a|和理论重力值|glocal|之间的偏差确定；传统的互补滤波方法使用分段函数来取kp的值，这很难选择合适的区间和权重；根据最大相关熵准则，在载体运动过程中，当地的理论重力glocal和测量重力|a|之间的相关性可以用基于高斯核的相关熵来表示；因此，本文提出了一种基于最大相关熵因子kp的自适应互补卡尔曼滤波方法，其中kp表示为
公式(21)中，σ表示高斯核函数的宽度。从而可以获得修正后的陀螺仪测量值θ％＝θ-Δθ，将其输入到IMU的姿态更新方程中，即可实现基于加速度计修正的陀螺仪姿态更新方法，降低陀螺仪的累积误差；然后状态方程(7)和量测方程(11)的时间更新和量测更新即可通过卡尔曼滤波实现；设定为状态向量X的最优估计值，则第k时刻的自适应互补卡尔曼滤波航姿测量算法具体步骤为：
Step1:状态一步预测
Step2:状态一步预测均方误差阵
Step3:量测判别
根据偏振光传感器航向测量值yawpolar与IMU的航向更新值yawimu的差异，对观测量进行筛选；当abs(yawpolar-yawimu)＜ε时，认为Zk有效，进行卡尔曼滤波，逐步完成状态更新和量测更新；若abs(yawpolar-yawimu)＞ε，认为Zk无效，跳过卡尔曼滤波的量测更新步骤，直接将基于互补滤波的IMU姿态更新结果作为航姿参考***的输出值；
Step4:滤波增益
Step5:状态估计
Step6:状态估计均方误差阵
Pk＝(I-KkHk)Pk/k-1 (26)
上述过程中ε取经验值2°，卡尔曼滤波预测更新过程的初值协方差矩阵P(0)选择单位矩阵，在这个过程中假设***噪声和观测噪声都是高斯白噪声，它们的协方差矩阵分别为Q0和R0，与IMU和偏振传感器的器件误差有关。
自适应互补卡尔曼滤波流程如图2所示。
试验例
试验在2022年12月16日下午进行，试验的地点为哈尔滨工业大学航天博物馆附近(经度126.6258、纬度45.7278、海拔148.74m)。偏振光传感器、MEMS惯性测量单元、光纤惯导***公共固定在铝合金支架上，从而避免实验中传感器与测量基准之间的相对位移。锂电池模块为各个传感器供电，大型锂电池模块为计算机供电。然后将铝合金支架固定在电动旋转平台上，***上电后，首先进行预热和初始对准后，再通过遥控器控制转台周期旋转，并在此过程中采集偏振光图像、MEMS陀螺仪和加速度计输出数据、和光纤惯导***的位姿信息。最后使用计算机进行航姿解算，其CPU型号Laptop i7-11800。
室外转位试验的水平角测量结果如图4所示，其中蓝色线表示只使用MEMS进行姿态解算的结果，红色线为互补滤波后的结果，黑色线为俯仰角和横滚角的真值。可以看出，直接使用MEMS进行姿态更新时，由于陀螺存在累积误差，其测量值相对真实值先不断漂移。当载体绕某一方向转动时，水平角也同样在进行微小的变化，但是当转动方向改变，IMU的零偏误差的方向同样发生了改变，由此导致水平角发散的方向发生改变，从而呈现出不断波动的趋势。此外，传统的KF方法也没有充分利用陀螺仪和加速度计的互补特性，姿态解的结果呈现漂移趋势。然而，所提出的ACKF方法可以有效地降低MIMU的姿态累积误差。
为了进一步验证仿生偏振光/IMU组合航姿测量方法的有效性，开展无人平台车载试验中。试验设备如图5所示。试验过程中仿生偏振光传感器、MEMS惯性测量单元、光纤惯性导航***、GNSS接收机、GNSS天线和供电模块都被固定在无人平台上。其中偏振光传感器与MEMS构成航姿测量***，光纤惯性导航***与GNSS组合作为航姿参考基准。在航天博物馆附近的车载轨迹如图5所示，绿色标记为起点，红色标记为终点。
不同方法的俯仰角和滚转角结果如图6所示由于振动、转弯和路况的影响，纵摇角和横摇角的随机误差明显增加。然而，与纯MIMU解和KF相比，该方法的姿态精度仍有显著提高。相对于KF，ACKF的俯仰角和滚转角均方根误差分别降低了89.3％和93.2％。此外，ACKF使用自适应因子来描述重力矢量与陀螺仪输出角速率的相关性，以实现偏振传感器的姿态补偿。ACKF的航向测量误差相对于KF降低9.6％。
以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明，任何熟悉本专业的技术人员，在不脱离本发明技术方案范围内，当可利用上述揭示的技术内容做出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案内容，依据本发明的技术实质，在本发明的精神和原则之内，对以上实施例所作的任何简单的修改、等同替换与改进等，均仍属于本发明技术方案的保护范围之内。
Claims (2)
1.基于自适应互补卡尔曼滤波的偏振光/MEMS组合航姿测量方法，其特征在于：所述基于自适应互补卡尔曼滤波的偏振光/MEMS组合航姿测量方法的步骤包括：
步骤1、设定陀螺仪和加速度计的三轴输出分别为θ＝[θx,θy,θz]T和a＝[ax,ay,az]T，载体当前姿态下的旋转矩阵为首先使用旋转矩阵将导航系下的重力矢量[0,0,-1]T投影到载体系下
步骤2、然后将载体系下的重力矢量值gb与加速度计输出值的单位化测量值进行交叉向量积运算
步骤3、将交叉向量积的结果Eg输入到PI调节器中，从而获得陀螺仪的漂移补偿量Δθ
Δθ＝Eg(kp+ki·Δt) (21)
公式(21)中，Δt表示IMU的采样间隔，ki通常取1，kp由加速度计的合力|a|和理论重力值|glocal|之间的偏差确定；
步骤4、基于最大相关熵因子kp的自适应互补卡尔曼滤波方法其中kp表示为
公式(22)中，σ表示高斯核函数的宽度，从而可以获得修正后的陀螺仪测量值θ％＝θ-Δθ；
步骤5、将修正后的陀螺仪测量值输入到IMU的姿态更新方程中，即可实现基于加速度计修正的陀螺仪姿态更新方法，降低陀螺仪的累积误差；
步骤6、然后构建偏振光传感器和MEMS卡尔曼滤波的状态方程和量测方程，将平台失准角、零偏漂移误差向量和陀螺比例因子误差向量作为状态向量，偏振光传感器测量的航向作为观测向量；最后进行时间更新和量测更新，即可实时输出载体的航姿状态。
2.根据权利要求1所述的基于自适应互补卡尔曼滤波的偏振光/MEMS组合航姿测量方法，其特征在于：将步骤4中获得陀螺仪测量值输入到IMU的姿态更新方程中，即可实现基于加速度计修正的陀螺仪姿态更新方法，降低陀螺仪的累积误差；将状态方程的时间更新，将量测方程的量测更新，设定为状态向量X的最优估计值，则第k时刻的自适应互补卡尔曼滤波航姿测量算法具体步骤为：
步骤401、状态一步预测
步骤402、状态一步预测均方误差阵
步骤403、量测判别
根据偏振光传感器航向测量值yawpolar与IMU的航向更新值yawimu的差异，对观测量进行筛选；当abs(yawpolar-yawimu)＜ε时，认为Zk有效，进行卡尔曼滤波，逐步完成状态更新和量测更新；若abs(yawpolar-yawimu)＞ε，认为Zk无效，跳过卡尔曼滤波的量测更新步骤，直接将基于互补滤波的IMU姿态更新结果作为航姿参考***的输出值；
步骤404、滤波增益
步骤405、状态估计
步骤406、状态估计均方误差阵
Pk＝(I-KkHk)Pk/k-1 (26)；
上述过程中ε取经验值2°，卡尔曼滤波预测更新过程的初值协方差矩阵P(0)选择单位矩阵，在这个过程中假设***噪声和观测噪声都是高斯白噪声，它们的协方差矩阵分别为Q0和R0。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410036652.4A CN118111434A (zh) | 2024-01-10 | 2024-01-10 | 基于自适应互补卡尔曼滤波的偏振光/mems组合航姿测量方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410036652.4A CN118111434A (zh) | 2024-01-10 | 2024-01-10 | 基于自适应互补卡尔曼滤波的偏振光/mems组合航姿测量方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN118111434A true CN118111434A (zh) | 2024-05-31 |
Family
ID=91209769
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410036652.4A Pending CN118111434A (zh) | 2024-01-10 | 2024-01-10 | 基于自适应互补卡尔曼滤波的偏振光/mems组合航姿测量方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN118111434A (zh) |
-
2024
- 2024-01-10 CN CN202410036652.4A patent/CN118111434A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106780699B (zh) | 一种基于sins/gps和里程计辅助的视觉slam方法 | |
| CN107270893B (zh) | 面向不动产测量的杆臂、时间不同步误差估计与补偿方法 | |
| CN110926468B (zh) | 基于传递对准的动中通天线多平台航姿确定方法 | |
| CN111504312B (zh) | 一种基于视觉惯性偏振光融合的无人机位姿估计方法 | |
| Fakharian et al. | Adaptive Kalman filtering based navigation: An IMU/GPS integration approach | |
| CN109556631B (zh) | 一种基于最小二乘的ins/gnss/偏振/地磁组合导航***对准方法 | |
| US10788324B2 (en) | Method and apparatus for calculation of angular velocity using acceleration sensor and geomagnetic sensor | |
| CN112097763A (zh) | 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 | |
| CN111102993A (zh) | 一种旋转调制型捷联惯导***晃动基座初始对准方法 | |
| CN110954102B (zh) | 用于机器人定位的磁力计辅助惯性导航***及方法 | |
| CN109506660B (zh) | 一种用于仿生导航的姿态最优化解算方法 | |
| US11408735B2 (en) | Positioning system and positioning method | |
| CN112562077B (zh) | 一种融合pdr和先验地图的行人室内定位方法 | |
| CN110017837A (zh) | 一种姿态抗磁干扰的组合导航方法 | |
| CN113720330B (zh) | 一种亚角秒级的遥感卫星高精度姿态确定设计与实现方法 | |
| CN111189474A (zh) | 基于mems的marg传感器的自主校准方法 | |
| CN110824453A (zh) | 一种基于图像跟踪与激光测距的无人机目标运动估计方法 | |
| He et al. | A combination orientation compass based on the information of polarized skylight/geomagnetic/MIMU | |
| CN110736457A (zh) | 一种基于北斗、gps和sins的组合导航方法 | |
| CN111024091A (zh) | 视觉辅助微型无人机室内飞行三维姿态算法 | |
| CN113566850B (zh) | 惯性测量单元的安装角度标定方法、装置和计算机设备 | |
| CN112197765B (zh) | 一种实现水下机器人精细导航的方法 | |
| WO2024109002A1 (zh) | 仿生视觉多源信息智能感知无人平台 | |
| CN113110556A (zh) | 一种基于视觉传感器的无人机位置估计***及估计方法 | |
| CN110375773B (zh) | Mems惯导***姿态初始化方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination |