CN116817927B - 双滤波器组合导航定位与测姿方法、电子设备及介质 - Google Patents
双滤波器组合导航定位与测姿方法、电子设备及介质 Download PDFInfo
- Publication number
- CN116817927B CN116817927B CN202311069076.5A CN202311069076A CN116817927B CN 116817927 B CN116817927 B CN 116817927B CN 202311069076 A CN202311069076 A CN 202311069076A CN 116817927 B CN116817927 B CN 116817927B
- Authority
- CN
- China
- Prior art keywords
- filter
- mimu
- matrix
- speed
- time
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 28
- 238000005259 measurement Methods 0.000 claims abstract description 21
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 73
- 230000009977 dual effect Effects 0.000 claims description 13
- 238000001514 detection method Methods 0.000 claims description 11
- 238000001914 filtration Methods 0.000 claims description 11
- 238000012937 correction Methods 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 4
- 230000017105 transposition Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 abstract description 12
- 230000006870 function Effects 0.000 abstract description 3
- 238000000691 measurement method Methods 0.000 description 5
- 230000008901 benefit Effects 0.000 description 4
- 230000000694 effects Effects 0.000 description 4
- 238000013461 design Methods 0.000 description 3
- 238000006073 displacement reaction Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Landscapes
- Navigation (AREA)
Abstract
本申请公开了一种双滤波器组合导航定位与测姿方法、电子设备及介质。该方法可以包括：构建主滤波器与子滤波器，分别确定两个滤波器的状态方程和观测方程；系统上电后，对MIMU初始化，GNSS上电后捕捉卫星信号；MIMU初始化完成后，实时计算MIMU的速度、位置和姿态角，进而进行航位推算；当GNSS有效且更新时，依次启动主滤波器与子滤波器进行解算；根据解算结果修正MIMU的姿态、速度和位置，重复进行航位推算，最终输出补偿后的姿态、速度和位置。本发明利用航位推算误差，采用一种双滤波器的组合导航算法，实现了低动态应用场景下MIMU/GNSS单天线组合导航高精度定位和测姿功能。
Description
技术领域
本发明涉及惯性导航技术领域，更具体地，涉及一种双滤波器组合导航定位与测姿方法、电子设备及介质。
背景技术
在很多应用场景，车辆行驶速度较慢，迫切需要解决低动态条件下高精度位置测量和姿态测量的问题，如智慧港口、智能农业、智能泊车等领域。MIMU/GNSS组合导航产品，可以为载体提供高精度的位置、速度、姿态角等信息。为节约成本，MIMU（MEMS IMU，即MEMS惯性测量单元）一般精度较低，GNSS（全球定位系统）也选择使用单天线方式，而MIMU精度越低，误差随着时间发散越快，一般需要与GNSS组合导航，利用GNSS的位置速度信息修正MIMU的误差。但是GNSS单天线方式在低动态环境下（速度在5m/s以下），航向角误差很大，达到几度甚至几十度，采用传统的位置/速度组合模式，无法修正MIMU航向角误差，在低动态条件下，航向角误差会随着时间慢慢发散。
现有技术中，一般采用MIMU与GNSS双天线组合导航模式。GNSS双天线可以为MIMU提供高精度的位置、速度和航向角，使MIMU/GNSS组合导航的状态变量全部可观，从而使MIMU的位置误差和航向角误差得到实时修正，保持较高的精度。但是双天线会带来成本的增加，目前双天线方式逐渐被很多应用领域淘汰。
因此，有必要开发一种双滤波器组合导航定位与测姿方法、电子设备及介质。
公开于本发明背景技术部分的信息仅仅旨在加深对本发明的一般背景技术的理解，而不应当被视为承认或以任何形式暗示该信息构成已为本领域技术人员所公知的现有技术。
发明内容
本发明提出了一种双滤波器组合导航定位与测姿方法、电子设备及介质，其能够利用航位推算误差，采用一种双滤波器的组合导航算法，实现了低动态应用场景下MIMU/GNSS单天线组合导航高精度定位和测姿功能。
第一方面，本公开实施例提供了一种双滤波器组合导航定位与测姿方法，包括：
构建主滤波器与子滤波器，分别确定两个滤波器的状态方程和观测方程；
系统上电后，对MIMU初始化，GNSS上电后捕捉卫星信号；
MIMU初始化完成后，实时计算MIMU的速度、位置和姿态角，进而进行航位推算；
当GNSS有效且更新时，依次启动所述主滤波器与所述子滤波器进行解算；
根据解算结果修正MIMU的姿态、速度和位置，重复进行航位推算，最终输出补偿后的姿态、速度和位置。
优选地，所述状态方程为：
Xi,k/k-1=Φi,k/k-1Xi,k+Bi,kUi,k+Γi,kωk
其中，i=1或2，1表示主滤波器，2表示子滤波器，k表示时刻，Xi,k/k-1为滤波器i在k时刻的状态一步预测结果，Xi,k是由滤波器i在k时刻的最优状态估计，Φi,k/k-1是滤波器i在k时刻的状态的一步转移矩阵，Γi,k是滤波器i在k时刻的系统噪声矩阵，ωk是k时刻的状态方程白噪声序列，Ui,k为滤波器i在k时刻的已知的确定性输入，Bi,k为滤波器i在k时刻的输入系数矩阵。
优选地，所述观测方程为：
Zi,k=Hi,kXi,k+vk
其中，Zi,k是滤波器i在k时刻的量测值，Hi,k是滤波器量测方程系数矩阵，Xi,k是由滤波器量测值观测到的系统状态，vk是观测方程白噪声序列。
优选地，航位推算包括：
，
其中，Vdre、Vdrn、Ldr、λdr分别为航位推算所得的东向速度、北向速度、纬度、经度，为Ldr的导数，/>为λdr的导数，V为MIMU计算的东向速度和北向速度的合成速度，ψ为MIMU解算的航向角，Re为地球半径。
优选地，所述主滤波器与所述子滤波器采用卡尔曼滤波器进行解算。
优选地，所述卡尔曼滤波器为：
，
其中，Xk/k-1为k时刻的状态一步预测结果，Xk-1为k-1时刻的最优状态估计，Xk为k时刻的最优状态估计，Pk-1为k-1时刻的状态估计均方差阵，Pk为k时刻的状态估计均方差阵，Pk/k-1为k时刻的状态一步预测均方差阵，Kk为k时刻的卡尔曼滤波增益，Zk为k时刻的量测值，Hk为k时刻的量测方程系数矩阵，Uk为k时刻的已知的确定性输入，Bk为在k时刻的输入系数矩阵，Qk-1为系统噪声序列的方差矩阵，Rk为量程噪声序列的方差矩阵，I为单位矩阵，T表示矩阵转置。
优选地，所述主滤波器采用位置/速度组合模式，通过闭环卡尔曼滤波算法，计算MIMU的俯仰失准角、横滚失准角、东向速度误差、北向速度误差、经度误差、纬度误差以及X轴和Y轴陀螺零偏；
MIMU的速度和位置减去误差值；
根据失准角斜对称矩阵修正姿态矩阵，并重置姿态四元数；
X、Y轴陀螺零偏估计值累加后作为X、Y轴陀螺角速度的补偿值进行修正。
优选地，所述子滤波器计算航向失准角和Z轴陀螺零偏值；
采用失准角斜对称矩阵修正姿态矩阵，并重置姿态四元数；
Z轴陀螺零偏估计值累加后作为Z轴陀螺角速度的补偿值进行修正。
第二方面，本公开实施例还提供了一种电子设备，该电子设备包括：
存储器，存储有可执行指令；
处理器，所述处理器运行所述存储器中的所述可执行指令，以实现所述的双滤波器组合导航定位与测姿方法。
第三方面，本公开实施例还提供了一种计算机可读存储介质，该计算机可读存储介质存储有计算机程序，该计算机程序被处理器执行时实现所述的双滤波器组合导航定位与测姿方法。
本发明的方法和装置具有其它的特性和优点，这些特性和优点从并入本文中的附图和随后的具体实施方式中将是显而易见的，或者将在并入本文中的附图和随后的具体实施方式中进行详细陈述，这些附图和具体实施方式共同用于解释本发明的特定原理。
附图说明
通过结合附图对本发明示例性实施例进行更详细的描述，本发明的上述以及其它目的、特征和优势将变得更加明显，其中，在本发明示例性实施例中，相同的参考标号通常代表相同部件。
图1示出了根据本发明的双滤波器组合导航定位与测姿方法的步骤的流程图。
图2示出了根据本发明的一个实施例的双滤波器组合导航定位与测姿方法的步骤的流程图。
图3示出了根据本发明的一个实施例的位置曲线的示意图。
图4示出了根据本发明的一个实施例的航向角曲线的示意图。
具体实施方式
下面将更详细地描述本发明的优选实施方式。虽然以下描述了本发明的优选实施方式，然而应该理解，可以以各种形式实现本发明而不应被这里阐述的实施方式所限制。
本发明提供一种双滤波器组合导航定位与测姿方法，包括：
构建主滤波器与子滤波器，分别确定两个滤波器的状态方程和观测方程；
系统上电后，对MIMU初始化，GNSS上电后捕捉卫星信号；
MIMU初始化完成后，实时计算MIMU的速度、位置和姿态角，进而进行航位推算；
当GNSS有效且更新时，依次启动主滤波器与子滤波器进行解算；
根据解算结果修正MIMU的姿态、速度和位置，重复进行航位推算，最终输出补偿后的姿态、速度和位置。
在一个示例中，状态方程为：
Xi,k/k-1=Φi,k/k-1Xi,k+Bi,kUi,k+Γi,kωk
其中，i=1或2，1表示主滤波器，2表示子滤波器，k表示时刻，Xi,k/k-1为滤波器i在k时刻的状态一步预测结果，Xi,k是由滤波器i在k时刻的最优状态估计，Φi,k/k-1是滤波器i在k时刻的状态的一步转移矩阵，Γi,k是滤波器i在k时刻的系统噪声矩阵，ωk是k时刻的状态方程白噪声序列，Ui,k为滤波器i在k时刻的已知的确定性输入，Bi,k为滤波器i在k时刻的输入系数矩阵。
在一个示例中，观测方程为：
Zi,k=Hi,kXi,k+vk
其中，Zi,k是滤波器i在k时刻的量测值，Hi,k是滤波器量测方程系数矩阵，Xi,k是由滤波器量测值观测到的系统状态，vk是观测方程白噪声序列。
在一个示例中，航位推算包括：
，
其中，Vdre、Vdrn、Ldr、λdr分别为航位推算所得的东向速度、北向速度、纬度、经度，为Ldr的导数，/>为λdr的导数，V为MIMU计算的东向速度和北向速度的合成速度，ψ为MIMU解算的航向角，Re为地球半径。
在一个示例中，主滤波器与子滤波器采用卡尔曼滤波器进行解算。
在一个示例中，卡尔曼滤波器为：
，
其中，Xk/k-1为k时刻的状态一步预测结果，Xk-1为k-1时刻的最优状态估计，Xk为k时刻的最优状态估计，Pk-1为k-1时刻的状态估计均方差阵，Pk为k时刻的状态估计均方差阵，Pk/k-1为k时刻的状态一步预测均方差阵，Kk为k时刻的卡尔曼滤波增益，Zk为k时刻的量测值，Hk为k时刻的量测方程系数矩阵，Uk为k时刻的已知的确定性输入，Bk为在k时刻的输入系数矩阵，Qk-1为系统噪声序列的方差矩阵，Rk为量程噪声序列的方差矩阵，I为单位矩阵，T表示矩阵转置。
在一个示例中，主滤波器采用位置/速度组合模式，通过闭环卡尔曼滤波算法，计算MIMU的俯仰失准角、横滚失准角、东向速度误差、北向速度误差、经度误差、纬度误差以及X轴和Y轴陀螺零偏；
MIMU的速度和位置减去误差值；
根据失准角斜对称矩阵修正姿态矩阵，并重置姿态四元数；
X、Y轴陀螺零偏估计值累加后作为X、Y轴陀螺角速度的补偿值进行修正。
在一个示例中，子滤波器计算航向失准角和Z轴陀螺零偏值；
采用失准角斜对称矩阵修正姿态矩阵，并重置姿态四元数；
Z轴陀螺零偏估计值累加后作为Z轴陀螺角速度的补偿值进行修正。
具体地，MIMU/GNSS单天线组合导航系统中，一般采用位置/速度或位置组合模式，即利用MIMU解算的位置和速度与GNSS位置和速度作差，建立误差状态方程，从而估计MIMU的误差并实时补偿。但是由于状态方程中的航向误差可观性比较弱，航向角的估计效果比较差，随着时间推移，航向角有发散的趋势。本方法在位置/速度组合导航模式基础上，再利用MIMU解算的航向角，结合组合导航的速度和位置进行航位推算，将航位推算的位置、MIMU的航向角误差、陀螺零偏作为状态变量，建立误差状态方程，以GNSS的位置作为观测量，从而实时修正MIMU的航向角误差。
本方法采用双滤波器方式，建立两种状态方程和观测方程，分别计算MIMU的不同状态变量。MIMU上电初始化后，进行INS惯性解算，实时计算MIMU的姿态角、速度和位置，同时进行航位推算。GNSS有效后进行卡尔曼滤波解算，估计出MIMU的姿态失准角、速度误差和位置误差，并进行闭环修正。
本方法采用两个滤波器进行解算，则两个滤波器的状态方程和观测方程分别为：
Xi,k/k-1=Φi,k/k-1Xi,k+Bi,kUi,k+Γi,kωk
Zi,k=Hi,kXi,k+vk。
主滤波器忽略X、Y、Z轴加速度计零偏▽bx、▽by、▽bz，以及天向速度误差dVu、高度误差dh，MIMU/GNSS单天线组合导航系统的误差状态变量选取为10维，主滤波器的状态向量X1为：
X1=[φe,φn,φu1,dVe,dVn,dλ,dL,εbx,εby,εbz1]T
其中，φe为俯仰失准角，φn为横滚失准角，φu1为主滤波器的航向失准角，dVe为东向速度误差，dVn为北向速度误差，dλ为经度误差，dL为纬度误差，εbx为X轴陀螺零偏，εby为Y轴陀螺零偏，εbz1为主滤波器的Z轴陀螺零偏。
子滤波器以MIMU航位推算的经度λdr、纬度Ldr、航向失准角φu2和子滤波器的Z轴陀螺零偏εbz2作为状态变量，则子滤波器的状态向量X2为：
X2=[λdr,Ldr,φu2,εbz2]T。
对于主滤波器，其状态一步转移矩阵Φ1,k/k-1和量测方程系数矩阵H1,k与一般的位置/速度组合模式一致，B1,kU1,k取零，量测值为MIMU解算的速度、位置与GNSS速度、位置的差值，即：
，
其中，VIe、VIn、λI、LI为MIMU解算的东向速度、北向速度、经度、纬度，VGe、VGn、λG、LG为GNSS的东向速度、北向速度、经度、纬度。
而子滤波器的状态一步转移矩阵Φ2,k/k-1定义如下：
，
子滤波器的，
其中，Re为地球半径，C31、C32、C33为MIMU导航解算中的姿态矩阵第三行分量，εbx、εby为主滤波器估计的X轴陀螺零偏和Y轴陀螺零偏，Vdre、Vdrn为航位推算的东向速度、北向速度，Ldr为航位推算的纬度。
子滤波器的量测值为GNSS的经度和纬度，即：
，
对应的量测方程系数矩阵为：
，
至此，两个滤波器的状态方程和观测方程已经定义完成。
主滤波器中的φu1和εbz1可观性比较弱，导致航向角有发散趋势；子滤波器利用航位推算的经度、纬度，在状态方程中耦合航向失准角，使φu2和εbz2变得可观，从而估计出准确的航向角。
系统上电后，对MIMU初始化，GNSS上电后捕捉卫星信号；MIMU初始化完成后，实时计算MIMU的速度、位置和姿态角，进而进行航位推算。
惯性解算是MIMU在初始对准后，利用采集的角速度和加速度实时计算速度、位置和姿态角，姿态角的计算一般采用四元数法，这里不再赘述。
航位推算是利用载体运动的位移为基础，根据当前计算的航向角，对前向速度在东向和北向的分量进行积分，从而得到东向和北向的位移。比较积分得到的位移和GNSS给出的位置，建立卡尔曼滤波状态方程，可以得到航向角误差的估计。航位推算的相关公式为：
，
其中，Vdre、Vdrn、Ldr、λdr分别为航位推算所得的东向速度、北向速度、纬度、经度，V为MIMU计算的东向速度和北向速度的合成速度，ψ为MIMU解算的航向角。
当GNSS有效且更新时，依次启动主滤波器与子滤波器进行解算；本方法中的滤波器计算采用卡尔曼滤波算法，卡尔曼滤波器的形式为：
，
其中，Xk/k-1为k时刻的状态一步预测结果，Xk-1为k-1时刻的最优状态估计，Xk为k时刻的最优状态估计，Pk-1为k-1时刻的状态估计均方差阵，Pk为k时刻的状态估计均方差阵，Pk/k-1为k时刻的状态一步预测均方差阵，Kk为k时刻的卡尔曼滤波增益，Zk为k时刻的量测值，Hk为k时刻的量测方程系数矩阵，Uk为k时刻的已知的确定性输入，Bk为在k时刻的输入系数矩阵，Qk-1为系统噪声序列的方差矩阵，Rk为量程噪声序列的方差矩阵，I为单位矩阵，T表示矩阵转置。
主滤波器采用位置/速度组合模式，进行卡尔曼滤波解算，频率为1Hz，采用闭环卡尔曼滤波方法，估计出MIMU的俯仰失准角、横滚失准角、东向速度误差、北向速度误差、经度误差、纬度误差，以及X轴和Y轴陀螺零偏，滤波完成后对MIMU解算结果进行闭环修正。修正方法为：MIMU的速度和位置直接减去误差估计值；根据失准角斜对称矩阵修正姿态矩阵，并重置姿态四元数，其中，修正的姿态矩阵为：
，
其中，为实时解算的姿态矩阵，/>为修正后的姿态矩阵，/>为失准角斜对称矩阵，公式如下：
，
其中，φu1为航向失准角；X、Y轴陀螺零偏估计值累加后作为X、Y轴陀螺角速度的补偿值进行修正。
子滤波器估计出航向失准角和Z轴陀螺零偏值，同样采用失准角斜对称矩阵修正姿态矩阵，并重置姿态四元数；Z轴陀螺零偏估计值累加后作为Z轴陀螺角速度的补偿值进行修正。
根据解算结果修正MIMU的姿态、速度和位置，重复进行航位推算，最终输出补偿后的姿态、速度和位置，得到高精度的导航结果。
在低动态环境下，单天线GNSS航向角误差很大，无法直接利用GNSS航向角来进行组合导航，而传统的位置/速度组合模式虽然能够保持位置、速度和水平姿态精度，但是航向角误差仍会发散，所以在MIMU/GNSS单天线组合导航条件下，如何同时保持位置、速度和姿态精度，是本方法的关键。
本方法的关键点是将设计两个滤波器，一个滤波器采用传统的位置/速度组合模式，另外一个滤波器利用航位推算的位置与航向角误差的耦合关系建立误差方程，第一个滤波器有效估计位置、速度和水平姿态误差，第二个滤波器有效估计航向角误差，从而实现MIMU的位置、速度、姿态的全维补偿。
本方法设计简单，适应性强，通用性强，容易实现，成本也比较低。
本发明还提供一种电子设备，电子设备包括：存储器，存储有可执行指令；处理器，处理器运行存储器中的可执行指令，以实现上述的双滤波器组合导航定位与测姿方法。
本发明还提供一种计算机可读存储介质，该计算机可读存储介质存储有计算机程序，该计算机程序被处理器执行时实现上述的双滤波器组合导航定位与测姿方法。
为便于理解本发明实施例的方案及其效果，以下给出三个具体应用示例。本领域技术人员应理解，该示例仅为了便于理解本发明，其任何具体细节并非意在以任何方式限制本发明。
实施例1
图1示出了根据本发明的一个实施例的双滤波器组合导航定位与测姿方法的步骤的流程图。
如图1所示，该双滤波器组合导航定位与测姿方法包括：步骤101，构建主滤波器与子滤波器，分别确定两个滤波器的状态方程和观测方程；步骤102，系统上电后，对MIMU初始化，GNSS上电后捕捉卫星信号；步骤103，MIMU初始化完成后，实时计算MIMU的速度、位置和姿态角，进而进行航位推算；步骤104，当GNSS有效且更新时，依次启动主滤波器与子滤波器进行解算；步骤105，根据解算结果修正MIMU的姿态、速度和位置，重复进行航位推算，最终输出补偿后的姿态、速度和位置。
本方法的实施流程如图2所示。系统上电后，先对MIMU初始化，配置好测量范围、采样率、带宽等。GNSS上电后也开始捕捉卫星信号。
MIMU初始化完成后，将进入纯惯性导航解算过程，实时计算MIMU的速度、位置和姿态角，并根据速度和航向角进行航位推算。
当GNSS有效且更新时，启动主滤波器解算，估计出MIMU的速度误差、位置误差、俯仰失准角和横滚失准角，以及X轴和Y轴的陀螺零偏。主滤波器计算完成后，将解算的X轴和Y轴陀螺零偏送入子滤波器，再估算方位失准角和Z轴陀螺零偏。
以上误差估计完成后，即可修正MIMU的姿态、速度和位置，进行下一步解算，最终将补偿后的姿态、速度和位置输出，得到高精度的导航结果。
为验证本发明对应的MIMU产品能够适应低动态高精度的设计需求，对该产品设计试验进行验证。选择星况较好的路面比较平整的开阔路段，惯导水平固定在车体内，惯导外接GNSS双天线，基线长度大约1m，惯导纵向坐标与双天线基线方向尽量一致。其中GNSS双天线的航向角精度大约为0.2°，不参与计算。以双天线航向角作为基准，验证MINU/GNSS单天线组合导航时的航向角精度。上电后，先在平整的水平面静态保持1分钟，等待GNSS有效，然后开始行驶，速度低于5m/s。为验证一般情况下的组合导航效果，行驶轨迹可作S型变道行驶。
图3中INS为经主滤波器修正后的位置，DR为航位推算的位置，GNSS为卫导测量的位置。从位置曲线图可以看出，INS与GNSS拟合度较好，说明主滤波器组合导航精度较高。航位推算位置误差较大，是由于惯导航向角误差发散导致的，需要经过子滤波器来估计航向角误差。
图4中GNSS为双天线测量的航向角，基线长度1m，对应精度0.2°，可作为惯导航向基准进行比较。主滤波器的航向误差显然是发散的，经过子滤波器修正后，MIMU航向角与双天线航向角稳态误差在1°左右，视为MIMU与双天线基线方向的安装误差角，可见经过子滤波器后，航向角精度得到显著提高。
实施例2
本公开提供一种电子设备包括，该电子设备包括：存储器，存储有可执行指令；处理器，处理器运行存储器中的可执行指令，以实现上述双滤波器组合导航定位与测姿方法。
根据本公开实施例的电子设备包括存储器和处理器。
该存储器用于存储非暂时性计算机可读指令。具体地，存储器可以包括一个或多个计算机程序产品，该计算机程序产品可以包括各种形式的计算机可读存储介质，例如易失性存储器和/或非易失性存储器。该易失性存储器例如可以包括随机存取存储器（RAM）和/或高速缓冲存储器（cache）等。该非易失性存储器例如可以包括只读存储器（ROM）、硬盘、闪存等。
该处理器可以是中央处理单元（CPU）或者具有数据处理能力和/或指令执行能力的其它形式的处理单元，并且可以控制电子设备中的其它组件以执行期望的功能。在本公开的一个实施例中，该处理器用于运行该存储器中存储的该计算机可读指令。
本领域技术人员应能理解，为了解决如何获得良好用户体验效果的技术问题，本实施例中也可以包括诸如通信总线、接口等公知的结构，这些公知的结构也应包含在本公开的保护范围之内。
有关本实施例的详细说明可以参考前述各实施例中的相应说明，在此不再赘述。
实施例3
本公开实施例提供一种计算机可读存储介质，该计算机可读存储介质存储有计算机程序，该计算机程序被处理器执行时实现所述的双滤波器组合导航定位与测姿方法。
根据本公开实施例的计算机可读存储介质，其上存储有非暂时性计算机可读指令。当该非暂时性计算机可读指令由处理器运行时，执行前述的本公开各实施例方法的全部或部分步骤。
上述计算机可读存储介质包括但不限于：光存储介质（例如：CD－ROM和DVD）、磁光存储介质（例如：MO）、磁存储介质（例如：磁带或移动硬盘）、具有内置的可重写非易失性存储器的媒体（例如：存储卡）和具有内置ROM的媒体（例如：ROM盒）。
本领域技术人员应理解，上面对本发明的实施例的描述的目的仅为了示例性地说明本发明的实施例的有益效果，并不意在将本发明的实施例限制于所给出的任何示例。
以上已经描述了本发明的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。
Claims (8)
1.一种双滤波器组合导航定位与测姿方法，其特征在于，包括：
构建主滤波器与子滤波器，分别确定两个滤波器的状态方程和观测方程；
系统上电后，对MIMU初始化，GNSS上电后捕捉卫星信号；
MIMU初始化完成后，实时计算MIMU的速度、位置和姿态角，进而进行航位推算；
当GNSS有效且更新时，依次启动所述主滤波器与所述子滤波器进行解算；
根据解算结果修正MIMU的姿态、速度和位置，根据时间步进重复进行航位推算，最终输出对应时间补偿后的姿态、速度和位置；
其中，所述主滤波器采用位置/速度组合模式，通过闭环卡尔曼滤波算法，计算MIMU的俯仰失准角、横滚失准角、东向速度误差、北向速度误差、经度误差、纬度误差以及X轴和Y轴陀螺零偏；
MIMU的速度和位置减去误差值；
根据失准角斜对称矩阵修正姿态矩阵，并重置姿态四元数；
X、Y轴陀螺零偏估计值累加后作为X、Y轴陀螺角速度的补偿值进行修正；
其中，所述子滤波器计算航向失准角和Z轴陀螺零偏值；
采用失准角斜对称矩阵修正姿态矩阵，并重置姿态四元数；
Z轴陀螺零偏估计值累加后作为Z轴陀螺角速度的补偿值进行修正。
2.根据权利要求1所述的双滤波器组合导航定位与测姿方法，其中，所述状态方程为：
Xi,k/k-1=Φi,k/k-1Xi,k+Bi,kUi,k+Γi,kωk
其中，i=1或2，1表示主滤波器，2表示子滤波器，k表示时刻，Xi,k/k-1为滤波器i在k时刻的状态一步预测结果，Xi,k是由滤波器i在k时刻的最优状态估计，Φi,k/k-1是滤波器i在k时刻的状态的一步转移矩阵，Γi,k是滤波器i在k时刻的系统噪声矩阵，ωk是k时刻的状态方程白噪声序列，Ui,k为滤波器i在k时刻的已知的确定性输入，Bi,k为滤波器i在k时刻的输入系数矩阵。
3.根据权利要求2所述的双滤波器组合导航定位与测姿方法，其中，所述观测方程为：
Zi,k=Hi,kXi,k+vk
其中，Zi,k是滤波器i在k时刻的量测值，Hi,k是滤波器量测方程系数矩阵，Xi,k是由滤波器量测值观测到的系统状态，vk是观测方程白噪声序列。
4.根据权利要求3所述的双滤波器组合导航定位与测姿方法，其中，航位推算包括：
其中，Vdre、Vdrn、Ldr、λdr分别为航位推算所得的东向速度、北向速度、纬度、经度，为Ldr的导数，/>为λdr的导数，V为MIMU计算的东向速度和北向速度的合成速度，ψ为MIMU解算的航向角，Re为地球半径。
5.根据权利要求4所述的双滤波器组合导航定位与测姿方法，其中，所述主滤波器与所述子滤波器采用卡尔曼滤波器进行解算。
6.根据权利要求5所述的双滤波器组合导航定位与测姿方法，其中，所述卡尔曼滤波器为：
其中，Xk/k-1为k时刻的状态一步预测结果，Xk-1为k-1时刻的最优状态估计，Xk为k时刻的最优状态估计，Pk-1为k-1时刻的状态估计均方差阵，Pk为k时刻的状态估计均方差阵，Pk/k-1为k时刻的状态一步预测均方差阵，Kk为k时刻的卡尔曼滤波增益，Zk为k时刻的量测值，Hk为k时刻的量测方程系数矩阵，Uk为k时刻的已知的确定性输入，Bk为在k时刻的输入系数矩阵，Qk-1为系统噪声序列的方差矩阵，Rk为量程噪声序列的方差矩阵，I为单位矩阵，T表示矩阵转置。
7.一种电子设备，其特征在于，所述电子设备包括：
存储器，存储有可执行指令；
处理器，所述处理器运行所述存储器中的所述可执行指令，以实现权利要求1-6中任一项所述的双滤波器组合导航定位与测姿方法。
8.一种计算机可读存储介质，其特征在于，该计算机可读存储介质存储有计算机程序，该计算机程序被处理器执行时实现权利要求1-6中任一项所述的双滤波器组合导航定位与测姿方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311069076.5A CN116817927B (zh) | 2023-08-24 | 2023-08-24 | 双滤波器组合导航定位与测姿方法、电子设备及介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311069076.5A CN116817927B (zh) | 2023-08-24 | 2023-08-24 | 双滤波器组合导航定位与测姿方法、电子设备及介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116817927A CN116817927A (zh) | 2023-09-29 |
| CN116817927B true CN116817927B (zh) | 2023-12-22 |
Family
ID=88139478
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311069076.5A Active CN116817927B (zh) | 2023-08-24 | 2023-08-24 | 双滤波器组合导航定位与测姿方法、电子设备及介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116817927B (zh) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE19919249A1 (de) * | 1999-04-28 | 2000-11-02 | Bodenseewerk Geraetetech | Koppelnavigationssystem |
| KR20070021872A (ko) * | 2005-08-19 | 2007-02-23 | 한국전자통신연구원 | 추측 항법과 지피에스를 이용한 복합 항법 장치 및 그 방법 |
| CN101949703A (zh) * | 2010-09-08 | 2011-01-19 | 北京航空航天大学 | 一种捷联惯性/卫星组合导航滤波方法 |
| CN102506857A (zh) * | 2011-11-28 | 2012-06-20 | 北京航空航天大学 | 一种基于双imu/dgps组合的相对姿态测量实时动态滤波方法 |
| CN103454665A (zh) * | 2013-08-26 | 2013-12-18 | 哈尔滨工程大学 | 一种双差gps/sins组合导航姿态测量方法 |
| CN105180968A (zh) * | 2015-09-02 | 2015-12-23 | 北京天航华创科技股份有限公司 | 一种imu/磁强计安装失准角在线滤波标定方法 |
| WO2023045357A1 (zh) * | 2021-09-23 | 2023-03-30 | 哈尔滨工程大学 | 一种基于统计相似度量的组合导航鲁棒滤波方法 |
-
2023
- 2023-08-24 CN CN202311069076.5A patent/CN116817927B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE19919249A1 (de) * | 1999-04-28 | 2000-11-02 | Bodenseewerk Geraetetech | Koppelnavigationssystem |
| KR20070021872A (ko) * | 2005-08-19 | 2007-02-23 | 한국전자통신연구원 | 추측 항법과 지피에스를 이용한 복합 항법 장치 및 그 방법 |
| CN101949703A (zh) * | 2010-09-08 | 2011-01-19 | 北京航空航天大学 | 一种捷联惯性/卫星组合导航滤波方法 |
| CN102506857A (zh) * | 2011-11-28 | 2012-06-20 | 北京航空航天大学 | 一种基于双imu/dgps组合的相对姿态测量实时动态滤波方法 |
| CN103454665A (zh) * | 2013-08-26 | 2013-12-18 | 哈尔滨工程大学 | 一种双差gps/sins组合导航姿态测量方法 |
| CN105180968A (zh) * | 2015-09-02 | 2015-12-23 | 北京天航华创科技股份有限公司 | 一种imu/磁强计安装失准角在线滤波标定方法 |
| WO2023045357A1 (zh) * | 2021-09-23 | 2023-03-30 | 哈尔滨工程大学 | 一种基于统计相似度量的组合导航鲁棒滤波方法 |
Non-Patent Citations (2)
| Title |
|---|
| 基于SIMS/GPS的汽车运动状态组合测量系统;张小龙;冯能莲;宋健;王继先;;农业机械学报(10);全文 * |
| 基于UKF的低成本SINS/GPS组合导航系统滤波算法;沈忠;俞文伯;房建成;;系统工程与电子技术(第03期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116817927A (zh) | 2023-09-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CA3003298C (en) | Gnss and inertial navigation system utilizing relative yaw as an observable for an ins filter | |
| US7509216B2 (en) | Inertial navigation system error correction | |
| Stančić et al. | The integration of strap-down INS and GPS based on adaptive error damping | |
| US6860023B2 (en) | Methods and apparatus for automatic magnetic compensation | |
| US8086405B2 (en) | Compensation for mounting misalignment of a navigation device | |
| CN113203418B (zh) | 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统 | |
| CN109596144B (zh) | Gnss位置辅助sins行进间初始对准方法 | |
| CN103822633A (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
| CN112432642B (zh) | 一种重力灯塔与惯性导航融合定位方法及系统 | |
| KR100443550B1 (ko) | 오차보정시스템을 구비하는 관성측정유닛-지피에스통합시스템과 미지정수 검색범위 축소방법 및 사이클 슬립검출방법, 및 그를 이용한 항체 위치, 속도,자세측정방법 | |
| CN113253325B (zh) | 惯性卫星序贯紧组合李群滤波方法 | |
| CN110954102A (zh) | 用于机器人定位的磁力计辅助惯性导航系统及方法 | |
| Cui et al. | In-motion alignment for low-cost SINS/GPS under random misalignment angles | |
| CN108627152A (zh) | 一种微型无人机基于多传感器数据融合的导航方法 | |
| CN103278165A (zh) | 基于剩磁标定的磁测及星光备份的自主导航方法 | |
| CN105910623B (zh) | 利用磁强计辅助gnss/mins紧组合系统进行航向校正的方法 | |
| Li et al. | Common frame based unscented quaternion estimator for inertial-integrated navigation | |
| CN109489661A (zh) | 一种卫星初始入轨时陀螺组合常值漂移估计方法 | |
| CN116222551A (zh) | 一种融合多种数据的水下导航方法及装置 | |
| CN114964222A (zh) | 一种车载imu姿态初始化方法、安装角估计方法及装置 | |
| CN111912427A (zh) | 一种多普勒雷达辅助捷联惯导运动基座对准方法及系统 | |
| CN110779514A (zh) | 面向仿生偏振导航辅助定姿的分级卡尔曼融合方法及装置 | |
| Han et al. | A fast SINS initial alignment method based on RTS forward and backward resolution | |
| CN116817927B (zh) | 双滤波器组合导航定位与测姿方法、电子设备及介质 | |
| Wang et al. | An adaptive cascaded Kalman filter for two-antenna GPS/MEMS-IMU integration |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |