CN108896044B - 一种基于惯性导航和卫星导航的定位方法及装置 - Google Patents
一种基于惯性导航和卫星导航的定位方法及装置 Download PDFInfo
- Publication number
- CN108896044B CN108896044B CN201810811710.0A CN201810811710A CN108896044B CN 108896044 B CN108896044 B CN 108896044B CN 201810811710 A CN201810811710 A CN 201810811710A CN 108896044 B CN108896044 B CN 108896044B
- Authority
- CN
- China
- Prior art keywords
- coordinate
- positioning
- res
- flag
- length
- 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 57
- 239000011159 matrix material Substances 0.000 claims abstract description 72
- 238000013519 translation Methods 0.000 claims abstract description 58
- 238000005259 measurement Methods 0.000 claims abstract description 42
- 238000001914 filtration Methods 0.000 claims abstract description 37
- 125000000205 L-threonino group Chemical group [H]OC(=O)[C@@]([H])(N([H])[*])[C@](C([H])([H])[H])([H])O[H] 0.000 claims description 43
- 230000009191 jumping Effects 0.000 claims description 16
- 230000008569 process Effects 0.000 claims description 8
- 238000012545 processing Methods 0.000 claims description 6
- 230000000737 periodic effect Effects 0.000 claims description 2
- 238000009825 accumulation Methods 0.000 abstract description 5
- 239000000243 solution Substances 0.000 description 10
- 238000004364 calculation method Methods 0.000 description 9
- 238000003908 quality control method Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 5
- 230000001133 acceleration Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000004927 fusion Effects 0.000 description 3
- 238000012360 testing method Methods 0.000 description 3
- 238000012937 correction Methods 0.000 description 2
- 238000010790 dilution Methods 0.000 description 2
- 239000012895 dilution Substances 0.000 description 2
- 230000007774 longterm Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000003825 pressing Methods 0.000 description 2
- 238000013441 quality evaluation Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 239000000428 dust Substances 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000008092 positive effect Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000005215 recombination Methods 0.000 description 1
- 230000006798 recombination Effects 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
- 239000000779 smoke Substances 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
本发明提供一种基于惯性导航和卫星导航的定位方法及装置，包括：步骤(1)：计算第一夹角、横轴平移量、纵轴平移量；步骤(2)：利用人员在水平自由方位坐标系下的坐标的估计值和航向角的估计值构造系统状态矩阵，利用人员在水平自由方位坐标系下的步长、航向角的估计值构造系统参数矩阵，利用第一坐标、第一航向角构造测量值矩阵，从而构造扩展卡尔曼滤波的状态方程和量测方程；利用扩展卡尔曼滤波方法对人员坐标的估计值进行计算。本发明利用卫导和微惯导组合，利用卫导精度较高、误差稳定和不随时间累积的优点以及惯导的短时高精度来实现长时间复杂场景下的高精度定位，并可实现室内外精准定位的无缝衔接。
Description
技术领域
本发明涉及一种定位方法及装置，尤其涉及一种基于惯性导航和卫星导航的定位方法及装置。
背景技术
随着城市土地价值的提升，市内楼宇越建越高，户型的多样使得内部结构越来越复杂，当发生火灾等意外时，如何在第一时间将被困人员解救出来是至关重要的，这就对现场指挥调度提出了很高的要求，而要实现高效的指挥就要时刻知晓各个被救人员与营救人员的当前位置。但火灾现场烟尘大，照明设备失效，能见度低，靠自身辨别方位准确度大幅降低，因而单兵定位系统提供了很好的技术支持，借助该设备能够很快把被救人员解救出来，保障救援和被救人员的安全。
惯性导航系统(Micro-INS，Micro-Inertial-Navigation System)简称“微惯导”，是一种基于微机电系统(MEMS,Micro-Electro-Mechanical System)传感器技术的微型惯性导航系统,是不依赖于外部信息、也不向外部辐射能量的自主式导航系统。其工作环境不仅包括空中、地面，还可以在水下、地下。惯导的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息，还具有体积小、功耗低及环境适应能力好等优点，使其成为单兵导航系统的理想设备。但是其误差会随时间累积，长期定位精度会下降，无法满足长时间定位精度要求。
GNSS定位误差较稳定，不随时间累积、精度较高、成本也较低，但GNSS定位动态性能差、输出数据频率低、非自主以及不能对航行姿态测定等。与此同时，GNSS信号受环境影响较大，如在室内、隧道、森林或峡谷等地GNSS信号会因为受遮挡而变得微弱或消失，从而无法满足单兵导航系统自主、实时、稳定的定位要求。
发明内容
本发明要解决的问题是针对卫星信号质量较差时影响定位结果及GNSS信号受环境影响较大的问题，提供一种基于惯性导航和卫星导航的定位方法及装置。
为解决上述技术问题，一种基于惯性导航和卫星导航的定位方法，包括如下步骤：
步骤(1)：计算第一夹角Δθ、横轴平移量Δx、纵轴平移量Δy，所述第一夹角Δθ为第二轨迹到第一轨迹的旋转角度，所述第一轨迹为人员的第一坐标构成的轨迹，所述第二轨迹为人员的第二坐标构成的轨迹，所述横轴平移量Δx、纵轴平移量Δy分别为第一轨迹、第二轨迹的起点在横轴和纵轴上的平移量，所述人员的第一坐标为人员在水平自由方位坐标系下的卫星定位坐标，所述人员的第二坐标为人员在惯性坐标系中的惯性定位坐标；
步骤(2)：利用人员在水平自由方位坐标系下的坐标的估计值和航向角的估计值构造系统状态矩阵；利用人员在惯性坐标系中的步长、航向角的估计值构造系统参数矩阵；利用所述系统状态矩阵和系统参数矩阵构造卡尔曼滤波的状态方程；利用第一坐标、第一航向角构造测量值矩阵，所述第一航向角为利用第一坐标计算得到的人员在水平自由方位坐标系下的航向角；利用测量值矩阵和系统状态矩阵构造卡尔曼滤波的量测方程；利用卡尔曼滤波方法对人员在水平自由方位坐标系下的坐标的估计值进行计算。人员在惯性坐标系中的步长可利用第二坐标计算得到。
本发明中，利用卫导数据和惯导数据实现惯导坐标与卫导坐标的对齐，即将第一轨迹、第二轨迹进行对齐，即计算第一夹角Δθ、横轴平移量Δx、纵轴平移量Δy。构造系统状态矩阵、系统参数矩阵，利用卡尔曼滤波对人员在水平自由方位坐标系下的坐标的估计值进行计算。综合利用惯导和卫导数据进行融合定位求解，既能保证定位的短时精度，提高定位结果输出的频率，又能抑制长时间惯导数据的方向漂移和误差累积。
上述技术方案中，以固定周期采集人员的定位数据，一个周期的人员的定位数据包括该周期人员的第一坐标、该周期人员的第二坐标；所述步骤(1)之前还包括步骤(P1-1)和步骤(P1-2)；
步骤(P1-1)：创建存储队列，将第一标志Flag_length、第二标志Flag_res、第三标志Flag_cache均设置为false；
步骤(P1-2)：判断n≥length_N是否成立，若判断结果为是，则令第一标志Flag_length为True且执行步骤(1)，否则跳转到步骤(P2-3)，其中，n为存储队列的长度，length_N为第一预设长度；
所述步骤(1)和步骤(2)之间包括步骤(P2-1)-步骤(P2-4)；
步骤(P2-1)：判断Flag_res是否为true；
若判断第二标志Flag_res为true，则执行步骤(P2-2)；
若判断第二标志Flag_res为false，则判断confi_Leve＞Thres_confi_Leve是否成立；
若判断confi_Leve＞Thres_confi_Leve成立，则令Thres_res＝Set_Thres×res，且第二标志Flag_res、第三标志Flag_cache均设置为true，且跳转到步骤(P2-2)，confi_Leve为解算可信度，Thres_confi_Leve为第一阈值，Thres_res为残差阈值，Set_Thres为残差的预设阈值；
若判断confi_Leve＞Thres_confi_Leve不成立，则跳转到步骤(P2-3)；
优选地，confi_Leve＝max(leni)/LD，第一距离和
其中Δx、Δy分别为第一轨迹的起点平移到第二轨迹的起点的横轴平移量、纵轴平移量；
步骤(P2-2)：采集到一个周期的定位数据后，判断该周期人员的第一坐标是否可靠，将不可靠的第一坐标清零；
步骤(P2-3)：将步骤(P2-2)得到的该周期的定位数据保存在存储队列的队列末尾；执行步骤(P2-4)；
步骤(P2-4)：判断第一标志Flag_length是否为True，若判断结果为是，则跳转到步骤(2)，否则跳转到步骤(P1-2)；
优选地，所述步骤(1)中，利用下式计算第一夹角Δθ
其中，第一距离leni为存储队列中第i+1个第二坐标(XI,i+1,YI,i+1)与第i个第二坐标(XI,i,YI,i)之间的距离，Thetai为第i个第一航向角与第i个第二航向角的差值，所述第i个第一航向角为存储队列中第i+1个第一坐标(XG,i+1,YG,i+1)相对于第i个第一坐标(XG,i,YG,i)的航向角，所述第i个第二航向角为存储队列中第i+1个第二坐标(XI,i+1,YI,i+1)相对于第i个第二坐标(XI,i,YI,i)的航向角，i＝1,2,...,n-1。
优选地，所述步骤(1)中，利用最小二乘法计算横轴平移量Δx、纵轴平移量Δy；
更优选地，利用下式计算横轴平移量Δx、纵轴平移量Δy
其中，
(XG,i,YG,i)为存储队列中第i个第一坐标，(XI,i,YI,i)为存储队列中第i个第二坐标。
本发明中，对第一坐标是否可靠进行判断，从而对卫导数据进行质量控制和评估，剔除不满足要求的卫导定位点。
上述技术方案中，所述步骤(2)中，判断存储队列中的卫星失锁的定位数据和已清零的第一坐标的数量之和是否不小于N_Miss个，若判断结果为否，则利用卡尔曼滤波的状态方程和量测方程计算该周期的人员定位结果的估计值，否则，清空存储队列且将第二标志Flag_res、第三标志Flag_cache均设置为false，且利用卡尔曼滤波的状态方程计算该周期的人员定位结果的估计值。
本发明中，通过判断存储队列中的卫星失锁的定位数据和已清零的第一坐标的数量之和是否不小于N_Miss个，从而判断队列中的卫星定位数据是否可信，若队列中的卫星定位数据不可信，则清空存储队列，且仅利用卡尔曼滤波的状态方程计算该周期的人员定位结果的估计值，即仅利用惯性定位结果对人员定位结果进行估计。
上述技术方案中，所述步骤(P2-2)中，若(X′G,i,Y′G,i)与(XG,i,YG,i)之间的距离不大于Thres_A且残差值res与残差阈值Thres_res之差的绝对值不大于Thres_B，则判断该周期的第一坐标可靠，否则判断该周期的第一坐标不可靠。优选地，Thres_A＝Thres_B＝2米。(X′G,i,Y′G,i)为(XI,i,YI,i)转换到水平自由方位坐标系下的坐标。其中
上述技术方案中，所述步骤(P2-3)中，还包括：判断第三标志Flag_cache是否为false；若第三标志Flag_cache为false，则判断n＝length_N是否成立，若n＝length_N成立，则删除存储队列中第一个位置的定位数据且将该周期的定位数据保存到存储队列的最后一个位置，若n＝length_N不成立，则将定位数据保存到存储队列的最后一个位置；
若第三标志Flag_cache为True，则判断n＝length_M是否成立，若n＝length_M成立，则删除存储队列中第max_k个位置的定位数据且将定位数据保存到存储队列的最后一个位置，若n＝length_M不成立，则将定位数据保存到存储队列的最后一个位置；
其中，length_N＜length_M，length_M为第二预设长度，优选地，length_N≥15，更优选地，length_M≥20，若max(res_ki)/res＜0.2，则max_k＝1，若max(res_ki)/res≥0.2，则max_k＝maxIndex(res_ki)，maxIndex(res_ki)为res_ki矩阵中的最大值对应的i值，max(res_ki)为res_ki中的最大值，
上述技术方案中，所述步骤(2)中，卡尔曼滤波的状态方程为Xk＝Φk/k-1Xk-1+Wk，其中系统状态矩阵
系统参数矩阵
(xk,yk)、yawk分别为人员在水平自由方位坐标系下的坐标的估计值和航向角的估计值；
卡尔曼滤波的量测方程为：Zk＝JXk+Vk，其中
测量值矩阵
Wk、Vk分别为过程噪声矩阵、测量噪声矩阵；
优选地，x0、y0、yaw0利用下式计算
Δx、Δy、Δθ分别为第二标志Flag_res刚好被置为true时得到的值，(XI,YI)、I_yaw分别为第二标志Flag_res刚好被置为true时对应的第二坐标、第二航向角。。
上述技术方案中，所述步骤(P1-2)还包括，判断存储队列中卫星失锁的定位数据是否不小于N_Miss个，如判断结果为是，则清空存储队列，且将第二标志Flag_res、第三标志Flag_cache均设置为false。
上述技术方案中，所述步骤(P2-4)和步骤(2)之间还包括：
步骤(P2-5)：判断存储队列中卫星失锁的定位数据是否不小于N_Miss个，如判断结果为是，则清空存储队列，且将第二标志Flag_res、第三标志Flag_cache均设置为false，返回执行步骤(P1-2)。
上述技术方案中，将高度角低于20度的卫星测量得到的卫星定位数据清零和/或将水平精度衰减因子HDOP＜2的卫星测量得到的卫星定位数据清零。
本发明中，将高度角不满足要求和/或水平精度衰减因子不满足要求的卫星定位数据清零，从而避免因卫星定位数据不准确而造成的人员定位结果的估计不准确的问题。
本发明还提供一种基于惯性导航和卫星导航的定位方法的定位装置，其特征在于：包括：卫星定位模块，用于测量人员的第一坐标，所述人员的第一坐标为人员在水平自由方位坐标系下的卫星定位坐标；
惯性测量模块，用于测量人员的第二坐标，所述人员的第二坐标为人员在惯性坐标系中的惯性定位坐标；
信号处理单元：用于计算第一夹角Δθ、横轴平移量Δx、纵轴平移量Δy，所述第一夹角Δθ为第二轨迹到第一轨迹的旋转角度，所述第一轨迹为人员的第一坐标构成的轨迹，所述第二轨迹为人员的第二坐标构成的轨迹，所述横轴平移量Δx、纵轴平移量Δy分别为第一轨迹的起点平移到第二轨迹的起点在横轴和纵轴上的平移量，所述人员的第一坐标为人员在水平自由方位坐标系下的卫星定位坐标，所述人员的第二坐标为人员在惯性坐标系中的惯性定位坐标；利用人员在水平自由方位坐标系下的坐标的估计值和航向角的估计值构造系统状态矩阵，利用人员在惯性坐标系中的步长、所述航向角的估计值构造系统参数矩阵，利用所述系统状态矩阵和系统参数矩阵构造卡尔曼滤波的状态方程；
利用第一坐标、第一航向角构造测量值矩阵，所述第一航向角为利用第一坐标计算得到的航向角，利用测量值矩阵和系统状态矩阵构造卡尔曼滤波的量测方程；
利用卡尔曼滤波方法对人员在水平自由方位坐标系下的坐标的估计值进行计算；
所述卫星定位模块、惯性测量模块均与信号处理单元连接，
优选地，所述卫星定位模块、惯性测量模块固定设置在人员足部、腰部、肩部中任意的一个位置。
本发明具有的优点和积极效果是：
(1)基于微惯导技术的人员定位系统，提供相对定位坐标，其主要原因是器件本身的限制，微惯导无法敏感到地球自转，因此不能完成初始方位自主对准。本发明利用GNSS辅助微惯导，可以快速实现方位初始对准，并提供精准的绝对定位结果。
(2)在城市峡谷环境或者树下等场景，由于GNSS信号的遮挡，多路径误差的影响，GNSS的定位结果会出现较大误差。本发明利用采用先进的GNSS数据质量控制策略，智能识别算法隔离有较大误差的GNSS数据，实现GNSS/MEMS组合精准定位。
(3)现有GNSS/MEMS组合定位系统都是基于经典的组合定位方式，但是由于MEMS器件的精度比较低，导致该组合方式效果不理想。本发明利用微惯导能够短时间内提供精准相对定位的特点，首次提出了一种新的组合方式，实现GNSS/MEMS室内外无缝的精准的绝对定位。
由于基于INS技术的人员定位系统，提供都是相对定位，其主要原因是由于MEMS的精度太低，无法敏感到地球自转，因此不能完成初始方位自主对准。同时，在城市峡谷等环境下，GNSS信号受到遮挡，多路径误差的影响，导致GNSS定位结果会出现较大的误差，从而导致GNSS/MEMS组合定位精度低，鲁棒性差。本发明利用GNSS辅助INS，实现方位快速初始对准，并利用先进的数据质量控制策略,提高了系统的鲁棒性，及新的GNSS/MEMS组合定位方式，提供精准绝对定位结果。
微惯导/卫导全域人员定位系统经过大量测试，室外相对定位精度基本能够保持在1m以内，绝对定位精度保持在2m以内，室内定位精度误差控制在千分之三，即室内行走1000米，误差控制在3米以内。且能够完成室内外无缝定位，全过程都是自动完成，不需要人工干预。
GNSS和微惯导组合进行导航定位，则可以相互取长补短，利用GNSS精度较高、误差稳定和不随时间累积的优点以及微惯导的短时高精度来实现长时间复杂场景下的到高精度定位，并可实现室内外精准定位的无缝衔接。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本发明的实施例的基于惯性导航和卫星导航的定位方法的简要流程框图；
图2是本发明的实施例的基于惯性导航和卫星导航的定位方法的详细流程框图；
图3是本发明的实施例的缓存队列的一种存储形式示意图；
图4是本发明的实施例的缓存队列的又一种存储形式示意图；
图5是本发明的实施例的人员从室外到室内再到室外的行进示意图；
图6是本发明的实施例的基于ZUPT技术的MEMS定位轨迹；
图7本发明的实施例的基于惯性导航和卫星导航的定位方法的定位效果与GNSS定位的定位效果的对比示意图。图中，虚线为GNSS单点定位轨迹，实线为本发明的GNSS/INS组合定位轨迹。
具体实施方式
下面将结合本申请的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
如图1—图7所示，本发明提供一种基于惯性导航和卫星导航的定位方法及装置。
本发明提供的微惯导/卫导(INS/GNSS)全域人员定位系统是一种室内外无缝衔接的精准定位系统，本发明实现的主要包括：
(1)GNSS辅助下的MEMS快速精确初始对准问题，实现MEMS的初始航向精确求解；
(2)GNSS数据自动筛选和剔除问题，避免卫星定位跳变情况，实现微惯导/卫导高精度组合定位；
(3)室内外定位模式自动切换技术问题，实现室内外无缝精准定位。
现有卫导和惯导融合的系统中，惯导仅用于短时间内对卫导进行平滑，但惯导不长时间输出定位数据。本申请中，卫导、惯导都输出定位数据，将两个定位数据进行融合。现有技术中，可能几十秒或十几米就完全没有了精度，偏离了真实轨迹。但本申请在无卫导信号时，依然可以长时间保持定位精度。
INS/GNSS组合定位质量控制策略是一个非常复杂的过程，主要是保证INS/GNSS组合定位的准确性，提高INS/GNSS组合定位鲁棒性。主要思想为缓存一段时间内GNSS定位结果和INS定位结果，利用最小二乘法对卫导轨迹和惯导轨迹进行拟合，利用拟合的残差值判定组合定位结果的可靠性。
本发明中，惯性导航可采用微惯性导航。
存储队列的长度即对应存储的定位数据的周期数。存储队列的长度为n即表示存储了n个周期的定位数据。相同周期的第一坐标和第二坐标共同存储在存储队列的一个位置中。
惯性测量模块设置在足部(因为足部可以按步骤进行处理，可以用零速修正)，卫星接收模块设置在腰部、肩部等。
本发明中，首先在室外行走一段距离，基于行走路线上获取的卫导数据和惯导数据，利用序贯最小二乘法实现惯导坐标与卫导坐标的对齐。卫导数据局部定位误差可以达到5m甚至更大，因此定位误差大，位置跳动剧烈，因此，在利用卫导数据进行惯导坐标的坐标对齐之前，首先需要对卫导数据进行质量控制和评估，剔除不满足要求的卫导定位点；在完成坐标对齐后，在后续过程中，综合利用惯导和卫导数据进行融合定位求解，既能保证定位的短时精度，提高定位结果输出的频率，又能抑制长时间惯导数据的方向漂移和误差累积。综合定位的结果有反过来修正惯导的航向误差；当进入到树下、窄巷或者室内灯无法接收到卫导信号的场景中，则定位进入纯惯导解算模式，即惯导数据经过坐标对齐的结果作为最终的定位结果直接输出；长时间的室内行走，由于缺乏卫导数据的修正，其定位精度受惯导本身方向漂移的影响，最终定位精度可能会下降，并且方向出现一定程度的漂移。但当再次进入室外时，利用接收到的卫导信号，系统会修正融合定位存在的误差和方向漂移，达到高精度的综合定位效果。整个过程不需要人工参与，全部是系统自动模式切换。质量控制策略就是判断卫星数据是否可用。
MPOS数据为(XI,n,YI,n)，(卫导坐标)GPOS数据为(XG,i,YG,i)。惯性定位坐标为利用惯性定位装置采集的数据计算得到的水平自由方位坐标系下的人员的坐标，卫星定位坐标为利用卫星定位装置采集的数据计算得到的ENU坐标系下人员的坐标。
本发明主要包括：(1)利用GNSS辅助MEMS实现快速初始对准技术。(2)数据质量控制策略；(3)GNSS/INS新的组合定位方式。
本发明中，以固定周期采集人员的定位数据，一个周期的人员的定位数据包括该周期人员的第一坐标、该周期人员的第二坐标。
步骤(P1-1)：创建存储队列，用于存储GNSS定位、MEMS定位的本地坐标系的数据，将第一标志Flag_length、第二标志Flag_res、第三标志Flag_cache均设置为false，执行步骤(P1-2)；
步骤(P1-2)：判断n≥length_N是否成立，即判断存储队列长度是否大于等于length_N，若判断结果为是，则令第一标志Flag_length为True且执行步骤(1)，否则跳转到步骤(P2-2)，其中，n为存储队列的长度，length_N为第一预设长度，优选length_N的取值为15；
步骤(1)：计算第一夹角Δθ、横轴平移量Δx、纵轴平移量Δy，所述第一夹角Δθ为第二轨迹到第一轨迹的旋转角度，所述第一轨迹为人员的第一坐标构成的轨迹，所述第二轨迹为人员的第二坐标构成的轨迹，所述横轴平移量Δx、纵轴平移量Δy分别为第一轨迹、第二轨迹的起点在横轴和纵轴上的平移量。
利用下式计算第一夹角Δθ
其中，第一距离leni为存储队列中第i+1个第二坐标(XI,i+1,YI,i+1)与第i个第二坐标(XI,i,YI,i)之间的距离，Thetai为第i个第一航向角与第i个第二航向角的差值，所述第i个第一航向角为存储队列中第i+1个第一坐标(XG,i+1,YG,i+1)相对于第i个第一坐标(XG,i,YG,i)的航向角，所述第i个第二航向角为存储队列中第i+1个第二坐标(XI,i+1,YI,i+1)相对于第i个第二坐标(XI,i,YI,i)的航向角，i＝1,2,...,n-1。
(XG,i,YG,i)为第i个第一坐标。(XI,i,YI,i)为第i个第二坐标。leni为人员在惯性坐标系中的步长，即为相邻两个第二坐标之间的距离。G_yawi为第一航向角，即利用第一坐标计算的航向角。I_yawi为第二航向角，即利用第二坐标计算的航向角。
利用最小二乘法计算第一轨迹的起点平移到第二轨迹的起点的平移量(Δx,Δy)。Δθ通过加权的方式进行计算，使得计算更为准确。
更优选地，利用下式计算横轴平移量Δx、纵轴平移量Δy，矩阵形式可以表示为：L＝HA
其中，
(XG,i,YG,i)为存储队列中第i个第一坐标，(XI,i,YI,i)为存储队列中第i个第二坐标，i＝1,2,...,N。
假定缓存了n(n>15)秒的定位数据，包括MPOS(水平自由方位坐标系下的INS坐标)数据和GPOS(ENU坐标系下的GPS坐标)数据。由于MPOS数据和GPOS数据的起点相同，且在不考虑定位误差的情况下，都为同一轨迹数据在不同水平坐标系下的投影。则两组数据理论上只是存在一个旋转关系，旋转角为常值Δθ。但是由于误差积累，INS航向角会发生缓慢的漂移，且GPOS定位结果只能达到米级。则在求解Δθ时需要采用一定的策略进行优化求解。
对缓存数据进行最小二乘求取两曲线之间的夹角Δθ，两曲线起点的平移量Δx,Δy，残差值res，残差最大值点max_k及解算可信度confi_Leve。
实际中采用零速修正对每一步进行修正，但输出还是按固定周期(例如1s)输出惯导数据、卫星数据。
在考虑定位误差的情况下，设两曲线之间的夹角Δθ，两曲线起点的平移量Δx,Δy，残差值res，残差最大值点max_k及解算可信度confi_Leve。
算法模型为：
leni＝sqrt((YI,i+1-YI,i)2+(XI,i+1-XI,i)2)
G_yawi＝atan2(YG,i+1-YG,i,XG,i+1-XG,i)
I_yawi＝atan2(YI,i+1-YI,i,XI,i+1-XI,i)
Thetai＝G_yawi-I_yawi
其中leni为两个惯导定位点(两个第二坐标)之间的距离，G_yawi为利用卫导坐标(第一坐标)计算的航向角，I_yawi为利用惯导坐标(第二坐标)计算的航向角。根据距第一个缓存点的距离加权求取Δθ
confi_Leve＝max(leni)/LD
利用最小二乘求取平移量Δx,Δy，残差值res。函数模型为
第1个点到第n个点的残差值res_ki
再求取残差值res
如果残差最大值点索引值max_k
其中maxIndex(res_ki)为求res_ki矩阵中最大值的索引(res_ki中最大值对应的i值)，max(res_ki)为求res_ki中的最大值。以上的计算主要起到粗差探测的作用。
leni＝sqrt((YI,i+1-YI,i)2+(XI,i+1-XI,i)2)，G_yawi＝atan2(YG,i+1-YG,i,XG,i+1-XG,i)，I_yawi＝atan2(YI,i+1-YI,i,XI,i+1-XI,i)，Thetai＝G_yawi-I_yawi。
执行完步骤(1)之后执行步骤(P2-1)。
步骤(P2-1)：判断Flag_res是否为true；
若判断第二标志Flag_res为true，则执行步骤(P2-2)；
若判断第二标志Flag_res为false，则判断confi_Leve＞Thres_confi_Leve是否成立；
若判断confi_Leve＞Thres_confi_Leve成立，则令Thres_res＝Set_Thres×res，且第二标志Flag_res、第三标志Flag_cache均设置为true，且跳转到步骤(P2-2)；
若判断confi_Leve＞Thres_confi_Leve不成立，则跳转到步骤(P2-3)；
优选地，解算可信度confi_Leve＝max(leni)/LD，第一距离和
Thres_confi_Leve为第一阈值，Thres_res为残差阈值，Set_Thres为残差的预设阈值。
执行完步骤(P2-1)之后执行步骤(P2-2)。
可信度阈值的取值范围为0＜Thres_confi_Leve＜1，残差的预设阈值Set_Thres的取值范围为2m＜Set_Thres＜3m。优选地，残差的预设阈值为残差的倍数值。
步骤(P2-2)：采集到一个周期的定位数据后，判断该周期人员的第一坐标是否可靠，将不可靠的第一坐标清零，执行步骤(P2-3)。
步骤(P2-3)：将步骤(P2-2)得到的该周期的定位数据保存在存储队列的队列末尾；执行步骤(P2-4)。本发明中，步骤(2-2)、(2-3)的执行顺序可调换，即可先判断数据可靠性再存入存储队列，或先将数据存储在存储队列中再判断数据的可靠性。若(X′G,i,Y′G,i)与(XG,i,YG,i)之间的距离不大于Thres_A且残差值res与残差阈值Thres_res之差的绝对值不大于Thres_B，则判断该周期的第一坐标可靠，否则判断该周期的第一坐标不可靠。优选地，Thres_A＝Thres_B＝2米。
利用步骤(1)求取Δx,Δy,Δθ，获取当前位置的估计值MPOS_E，再利用MPOS_E与GNSS定位数据进行比较，且利用残差值res与残差阈值Thres_res进行比较来判断GNSS定位结果是否可靠。如果GNSS定位结果不可靠，GNSS定位清零。MPOS_E利用INS(惯导)定位结果的坐标转换获取的。MPOS_E即为根据下式通过(XI,i,YI,i)计算得到的(X′G,i,Y′G,i)。
根据经验值，MPOS_E与GNSS定位数据之差大于2m或残差值res与残差阈值之差大于2m，则认为GNSS定位结果不可靠。若(X′G,i,Y′G,i)与(XG,i,YG,i)之间的距离大于Thres_A，或残差值res与残差阈值Thres_res之差的绝对值大于Thres_B，则判断GNSS定位结果不可靠。
利用坐标变换公式，利用Δx,Δy,Δθ将惯导的定位数据(XI,i,YI,i)变换为GPS坐标下的GPOS数据(X′G,i,Y′G,i)，将转换后的(X′G,i,Y′G,i)和GPOS数据(XG,i,YG,i)进行对比(因为理论上二者应该在同一个点或相差不大)，且将残差值res与残差阈值进行比较，判断卫导数据是否可信。
若卫导数据不可靠，则该GNSS定位数据(XG,i,YG,i)清零，但仍保留对应的该周期惯导数据(第二坐标)。清零的GNSS数据仅对EKF中的量测模型有影响。
执行完步骤(P2-3)之后执行步骤(P2-4)。
所述步骤(P2-3)中，还包括：判断第三标志Flag_cache是否为false。
若第三标志Flag_cache为false，则判断n＝length_N是否成立，若n＝length_N成立，则删除存储队列中第一个位置的定位数据且将该周期的定位数据保存到存储队列的最后一个位置，若n＝length_N不成立，则将定位数据保存到存储队列的最后一个位置。
若第三标志Flag_cache为True，则判断n＝length_M是否成立，若n＝length_M成立，则删除存储队列中第max_k个位置的定位数据且将定位数据保存到存储队列的最后一个位置，若n＝length_M不成立，则将定位数据保存到存储队列的最后一个位置。其中，length_N＜length_M，length_M为第二预设长度，优选地，length_N≥15，更优选地，length_M≥20，若max(res_ki)/res＜0.2，则max_k＝1，若max(res_ki)/res≥0.2，则max_k＝maxIndex(res_ki)，maxIndex(res_ki)为res_ki矩阵中的最大值对应的i值，max(res_ki)为res_ki中的最大值，
如果cache_flag为false，将当前GNSS定位、MEMS定位的本地坐标系的数据存入存储队列，如果此时队列长度为n，则存入存储队列是删掉缓存的第一个数据，将当前数据压入存储队列末尾。如果cache_flag为true，则将当前GNSS定位、MEMS定位的本地坐标系的数据存入存储队列，如果此时队列长度为m(经验值为20)，则删掉存储队列的第max_k个数据，将当前数据压入存储队列末尾。执行步骤(P2-4)。本申请先存15个计算，再到20个，更好的求取两个轨迹的之间的夹角，提高系统的稳健性。如图3，存储队列中保存15对数据(每对数据中包括一个周期的第一坐标和第二坐标)，开始计算夹角Δθ、横轴平移量Δx、纵轴平移量Δy。图4是利用20对数据进行计算的示意图。
程序执行过程中，依然向队列存数据，直到存够20对数据。若队列长度仍然为15对，即程序执行过程并未存入新的数据对，则继续存入数据，即将第1对数据向左移动一位(不是删除第1对)，最右面进入新来的数据对。若队列长度为20对，即完全存满了，则找出偏差最大的值删除，仍然令队列容量为20。存够15对数据后，即可计算夹角Δθ、平移量Δx,Δy，则后续的系统模型公式中，即每一对新来的数据，都重新更新。
步骤(P2-4)：判断第一标志Flag_length是否为True，若判断结果为是，则跳转到步骤(2)，否则跳转到步骤(P1-2)；执行完步骤(P2-4)之后执行步骤(2)。
步骤(P2-5)：判断存储队列中卫星失锁的定位数据是否不小于N_Miss个，如判断结果为是，则清空存储队列，且将第二标志Flag_res、第三标志Flag_cache均设置为false，返回执行步骤(P1-2)。优选地，N_Miss＝5。
判断卫星是否失锁，如失锁达到p秒，则清空存储队列。返回执行步骤(P1-2)。若采集第一坐标、第二坐标的周期为1秒，则优选p为5。
步骤(2)：卡尔曼滤波
本发明中，可采用扩展卡尔曼滤波或其他卡尔曼滤波形式。
利用人员在水平自由方位坐标系下的坐标的估计值和航向角的估计值构造系统状态矩阵，利用人员在水平自由方位坐标系下的步长、航向角的估计值构造系统参数矩阵，利用所述系统状态矩阵和系统参数矩阵构造扩展卡尔曼滤波的状态方程。
利用第一坐标、第一航向角构造测量值矩阵，所述第一航向角为利用第一坐标计算得到的人员在水平自由方位坐标系下的航向角，利用测量值矩阵和系统状态矩阵构造扩展卡尔曼滤波的量测方程；利用扩展卡尔曼滤波方法对人员在水平自由方位坐标系下的坐标的估计值进行计算。
扩展卡尔曼滤波的系统模型为
Xk＝Xk-1+len*cos(yawk-1)+wxk
Yk＝Yk-1+len*sin(yawk-1)+wyk
yawk＝yawk-1+wyawk
量测模型为
由于系统模型为非线性模型，则采用EKF。
系统模型的矩阵形式可以表示为：Xk＝Φk/k-1Xk-1+Wk，其中系统状态矩阵
系统参数矩阵
(xk,yk)、yawk分别为人员在水平自由方位坐标系下的坐标的估计值和航向角的估计值。
量测模型的矩阵形式表示为：Zk＝JXk+Vk式中，
测量值矩阵
其中(XG,k,YG,k)为第一坐标，G_yawk为第一航向角。
给出系统模型、量测模型，本领域人员即可理解如何进行计算。本领域人员也可采用其他形式的卡尔曼滤波方程进行计算。
EKF的初始值：
其中，XI、YI为惯导数据(第二坐标)，I_yaw为惯导的航向角(第二航向角)。
其中Δx、Δy、Δθ在定位质量控制策略算法中求得，取第二标志Flag_res刚好被置为true时的Δx、Δy、Δθ值。Δx、Δy、Δθ分别为第二标志Flag_res刚好被置为true时得到的值。(XI,YI)、I_yaw分别为第二标志Flag_res刚好被置为true时对应的第二坐标(XI,i,YI,i)、第二航向角I_yawi。
状态量的初始方差、系统噪声、观测噪声都是根据大量测试的确定的经验值。状态量的初始方差为kalman滤波中状态量的初始方差。Wk、Vk分别为系统噪声、观测噪声。Wk、Vk均为均值为0的白噪声，且均为根据大量测试的确定的经验值。
本方法中，GNSS单点定位质量控制策略：由于低高度角卫星的观测数据受多路径、大气延迟误差影响更加严重，因此加入截止高度角限制(本方案中取20度)，以剔除低高度角卫星参与解算。同时，精度衰减因子DOP(Dilution of Precision)反应了卫星的几何分布对定位误差的影响，是衡量导航系统性能的重要指标。HDOP是指水平精度衰减因子，根据HDOP值滤除不稳定的信号，本方案实验中(HDOP值的阈值设置为2)。
每个卫星都有高度角，所有卫星一起有1个HDOP。若判断高度角或HDOP不满足条件，则令该卫导定位数据为0。
如图5所示，虚线范围S为室内，箭头为人员行进方向，人员从B点开始进入室内。在室内卫星信号变弱，若失锁达到p秒，即缓存的队列中有p个卫星定位数据(第一坐标)不可信，则清空存储队列；然后从失锁p秒后，则仅利用惯导定位，即仅利用式子(1)的系统模型进行迭代定位，而不用式子(2)的量测模型。人员从C处开始进入室外，又开始收到卫星信号，则又在存储队列中存储卫星定位数据(第一坐标)，直到存储队列中有20对数据，再重新结合第一坐标(卫导定位数据)、第二坐标(惯导定位数据)进行定位。
惯导解算可以参考文献[Aboelmagd Noureldin,Tashfeen B.Karamat，etc.Fundamentals of Inertial Navigation,Satellite-based Positioning and theirIntegration[M].Springer-Verlag Berlin Heidelberg 2013]。此处不赘述。
图6是仅利用惯导数据的定位结果。图7中虚线为仅利用卫星数据的定位结果。图7中实线为本发明的基于惯性导航和卫星导航的定位方法的定位结果。从图6可以看出，基于ZUPT技术的MEMS定位轨迹是相对定位，并且随着行走距离的增加，航向发生了偏移。通过图7可以看出，GNSS单点定位轨迹在某些地方出现了偏差，而GNSS/INS组合定位能够很好的抑制这些偏差，并且定位轨迹是绝对位置，航向也没有发生偏移。结合图6、图7，可以看出GNSS/INS组合定位是最优的。
本发明还提供一种基于惯性导航和卫星导航的定位方法的定位装置，包括：
卫星定位模块，用于测量人员的第一坐标，所述人员的第一坐标为人员在水平自由方位坐标系下的卫星定位坐标；
惯性测量模块，用于测量人员的第二坐标，所述人员的第二坐标为人员在惯性坐标系中的惯性定位坐标；
信号处理单元：用于计算第一夹角Δθ、横轴平移量Δx、纵轴平移量Δy，所述第一夹角Δθ为第二轨迹到第一轨迹的旋转角度，所述第一轨迹为人员的第一坐标构成的轨迹，所述第二轨迹为人员的第二坐标构成的轨迹，所述横轴平移量Δx、纵轴平移量Δy分别为第一轨迹的起点平移到第二轨迹的起点在横轴和纵轴上的平移量，所述人员的第一坐标为人员在水平自由方位坐标系下的卫星定位坐标，所述人员的第二坐标为人员在惯性坐标系中的惯性定位坐标；利用人员在水平自由方位坐标系下的坐标的估计值和航向角的估计值构造系统状态矩阵，利用人员在惯性坐标系中的步长、所述航向角的估计值构造系统参数矩阵，利用所述系统状态矩阵和系统参数矩阵构造卡尔曼滤波的状态方程；
利用第一坐标、第一航向角构造测量值矩阵，所述第一航向角为利用第一坐标计算得到的航向角，利用测量值矩阵和系统状态矩阵构造卡尔曼滤波的量测方程；
利用卡尔曼滤波方法对人员在水平自由方位坐标系下的坐标的估计值进行计算；
所述卫星定位模块、惯性测量模块均与信号处理单元连接，
优选地，所述卫星定位模块、惯性测量模块固定设置在人员足部、腰部、肩部中任意的一个位置。
需要说明的是，本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。
以上对本发明的实施例进行了详细说明，但所述内容仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明范围所作的均等变化与改进等，均应仍归属于本专利涵盖范围之内。在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落入本申请所附权利要求所限定的范围。在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
Claims (12)
1.一种基于惯性导航和卫星导航的定位方法，其特征在于，包括如下步骤：
步骤(P1-1)：创建存储队列，将第一标志Flag_length、第二标志Flag_res、第三标志Flag_cache均设置为false；
步骤(P1-2)：判断n≥length_N是否成立，若判断结果为是，则令第一标志Flag_length为True且执行步骤(1)，否则跳转到步骤(P2-2)，其中，n为存储队列的长度，length_N为第一预设长度；
步骤(1)：计算第一夹角Δθ、横轴平移量Δx、纵轴平移量Δy，所述第一夹角Δθ为第二轨迹到第一轨迹的旋转角度，所述第一轨迹为人员的第一坐标构成的轨迹，所述第二轨迹为人员的第二坐标构成的轨迹，所述横轴平移量Δx、纵轴平移量Δy分别为第一轨迹的起点平移到第二轨迹的起点的横轴平移量、纵轴平移量，所述人员的第一坐标为人员在水平自由方位坐标系下的卫星定位坐标，所述人员的第二坐标为人员在惯性坐标系中的惯性定位坐标；
利用下式计算第一夹角Δθ
其中，第一距离leni为存储队列中第i+1个第二坐标(XI,i+1,YI,i+1)与第i个第二坐标(XI,i,YI,i)之间的距离，Thetai为第i个第一航向角与第i个第二航向角的差值，所述第i个第一航向角为存储队列中第i+1个第一坐标(XG,i+1,YG,i+1)相对于第i个第一坐标(XG,i,YG,i)的航向角，所述第i个第二航向角为存储队列中第i+1个第二坐标(XI,i+1,YI,i+1)相对于第i个第二坐标(XI,i,YI,i)的航向角，i＝1,2,...,n-1；
利用最小二乘法计算横轴平移量Δx、纵轴平移量Δy，利用下式计算横轴平移量Δx、纵轴平移量Δy
其中，
步骤(2)：利用人员在水平自由方位坐标系下的坐标的估计值和航向角的估计值构造系统状态矩阵，利用人员在惯性坐标系中的步长、所述航向角的估计值构造系统参数矩阵，利用所述系统状态矩阵和系统参数矩阵构造卡尔曼滤波的状态方程；
利用第一坐标、第一航向角构造测量值矩阵，所述第一航向角为利用第一坐标计算得到的航向角，利用测量值矩阵和系统状态矩阵构造卡尔曼滤波的量测方程；
利用卡尔曼滤波方法对人员在水平自由方位坐标系下的坐标的估计值进行计算；
卡尔曼滤波的状态方程为Xk＝Φk/k-1Xk-1+Wk，其中系统状态矩阵
系统参数矩阵
(xk,yk)、yawk分别为人员在水平自由方位坐标系下的坐标的估计值和航向角的估计值；
卡尔曼滤波的量测方程为：Zk＝JXk+Vk，其中
测量值矩阵
Wk、Vk分别为过程噪声矩阵、测量噪声矩阵，G_yawk为第一航向角；
x0、y0、yaw0利用下式计算
Δx、Δy、Δθ分别为第二标志Flag_res刚好被置为true时得到的值，(XI,YI)、I_yaw分别为第二标志Flag_res刚好被置为true时对应的第二坐标、第二航向角。
2.根据权利要求1所述的定位方法，其特征在于：
所述步骤(1)和步骤(2)之间包括步骤(P2-1)-步骤(P2-5)，
步骤(P2-1)：判断Flag_res是否为true，
若判断第二标志Flag_res为true，则执行步骤(P2-2)，
若判断第二标志Flag_res为false，则判断confi_Leve＞Thres_confi_Leve是否成立，
若判断confi_Leve＞Thres_confi_Leve成立，则令Thres_res＝Set_Thres×res，且第二标志Flag_res、第三标志Flag_cache均设置为true，且跳转到步骤(P2-2)，confi_Leve为解算可信度，Thres_confi_Leve为第一阈值，Thres_res为残差阈值，Set_Thres为残差的预设阈值，res为残差值；
若判断confi_Leve＞Thres_confi_Leve不成立，则跳转到步骤(P2-3)；
步骤(P2-2)：采集到一个周期人员的定位数据后，判断该周期人员的第一坐标是否可靠，将不可靠的第一坐标清零，其中一个周期的人员的定位数据包括该周期人员的第一坐标、该周期人员的第二坐标；
步骤(P2-3)：将步骤(P2-2)得到的该周期人员的定位数据保存在存储队列的队列末尾；执行步骤(P2-4)；
步骤(P2-4)：判断第一标志Flag_length是否为True，若判断结果为是，则跳转到步骤(P2-5)，否则跳转到步骤(P1-2)；
步骤(P2-5)：判断存储队列中卫星失锁的定位数据是否不小于N_Miss个，如判断结果为是，则清空存储队列，且将第二标志Flag_res、第三标志Flag_cache均设置为false，返回执行步骤(P1-2)。
3.根据权利要求2所述的定位方法，其特征在于：confi_Leve＝max(leni)/LD，第一距离和
4.根据权利要求2所述的定位方法，其特征在于：所述步骤(2)中，判断存储队列中的卫星失锁的定位数据和已清零的第一坐标的数量之和是否不小于N_Miss个，若判断结果为否，则利用卡尔曼滤波的状态方程和量测方程计算该周期的人员定位结果的估计值，否则，清空存储队列且将第二标志Flag_res、第三标志Flag_cache均设置为false，且利用卡尔曼滤波的状态方程计算该周期的人员定位结果的估计值。
5.根据权利要求4所述的定位方法，其特征在于：所述步骤(P2-2)中，若(X'G,i,YG',i)与(XG,i,YG,i)之间的距离不大于Thres_A且残差值res与残差阈值Thres_res之差的绝对值不大于Thres_B，则判断该周期的第一坐标可靠，否则判断该周期的第一坐标不可靠，
6.根据权利要求2所述的定位方法，其特征在于：所述步骤(P2-3)中，还包括：判断第三标志Flag_cache是否为false；
若第三标志Flag_cache为false，则判断n＝length_N是否成立，若n＝length_N成立，则删除存储队列中第一个位置的定位数据且将该周期的定位数据保存到存储队列的最后一个位置，若n＝length_N不成立，则将定位数据保存到存储队列的最后一个位置；
若第三标志Flag_cache为True，则判断n＝length_M是否成立，若n＝length_M成立，则删除存储队列中第max_k个位置的定位数据且将定位数据保存到存储队列的最后一个位置，若n＝length_M不成立，则将定位数据保存到存储队列的最后一个位置，
其中，length_N＜length_M，length_M为第二预设长度，若max(res_ki)/res＜0.2，则max_k＝1，若max(res_ki)/res≥0.2，则max_k＝maxIndex(res_ki)，maxIndex(res_ki)为res_ki矩阵中的最大值对应的i值，max(res_ki)为res_ki中的最大值，
7.根据权利要求6所述的定位方法，其特征在于：length_N≥15。
8.根据权利要求7所述的定位方法，其特征在于：length_M≥20。
9.根据权利要求4所述的定位方法，其特征在于：所述步骤(P1-2)还包括，判断存储队列中卫星失锁的定位数据是否不小于N_Miss个，如判断结果为是，则清空存储队列，且将第二标志Flag_res、第三标志Flag_cache均设置为false。
10.根据权利要求1-9中任一项所述的定位方法，其特征在于：将高度角低于20度的卫星测量得到的卫星定位数据清零和/或将水平精度衰减因子HDOP＜2的卫星测量得到的卫星定位数据清零。
11.一种基于惯性导航和卫星导航的定位方法的定位装置，其特征在于：包括：
卫星定位模块，用于测量人员的第一坐标，所述人员的第一坐标为人员在水平自由方位坐标系下的卫星定位坐标；
惯性测量模块，用于测量人员的第二坐标，所述人员的第二坐标为人员在惯性坐标系中的惯性定位坐标；
信号处理单元：用于执行如下步骤
步骤(P1-1)：创建存储队列，将第一标志Flag_length、第二标志Flag_res、第三标志Flag_cache均设置为false；
步骤(P1-2)：判断n≥length_N是否成立，若判断结果为是，则令第一标志Flag_length为True且执行步骤(1)，否则跳转到步骤(P2-2)，其中，n为存储队列的长度，length_N为第一预设长度；
步骤(1)：计算第一夹角Δθ、横轴平移量Δx、纵轴平移量Δy，所述第一夹角Δθ为第二轨迹到第一轨迹的旋转角度，所述第一轨迹为人员的第一坐标构成的轨迹，所述第二轨迹为人员的第二坐标构成的轨迹，所述横轴平移量Δx、纵轴平移量Δy分别为第一轨迹的起点平移到第二轨迹的起点在横轴和纵轴上的平移量，所述人员的第一坐标为人员在水平自由方位坐标系下的卫星定位坐标，所述人员的第二坐标为人员在惯性坐标系中的惯性定位坐标；
利用下式计算下式计算第一夹角Δθ
其中，第一距离leni为存储队列中第i+1个第二坐标(XI,i+1,YI,i+1)与第i个第二坐标(XI,i,YI,i)之间的距离，Thetai为第i个第一航向角与第i个第二航向角的差值，所述第i个第一航向角为存储队列中第i+1个第一坐标(XG,i+1,YG,i+1)相对于第i个第一坐标(XG,i,YG,i)的航向角，所述第i个第二航向角为存储队列中第i+1个第二坐标(XI,i+1,YI,i+1)相对于第i个第二坐标(XI,i,YI,i)的航向角，i＝1,2,...,n-1；
利用最小二乘法计算横轴平移量Δx、纵轴平移量Δy，利用下式计算横轴平移量Δx、纵轴平移量Δy
其中，
步骤(2)：利用人员在水平自由方位坐标系下的坐标的估计值和航向角的估计值构造系统状态矩阵，利用人员在惯性坐标系中的步长、所述航向角的估计值构造系统参数矩阵，利用所述系统状态矩阵和系统参数矩阵构造卡尔曼滤波的状态方程；
利用第一坐标、第一航向角构造测量值矩阵，所述第一航向角为利用第一坐标计算得到的航向角，利用测量值矩阵和系统状态矩阵构造卡尔曼滤波的量测方程；
利用卡尔曼滤波方法对人员在水平自由方位坐标系下的坐标的估计值进行计算；
卡尔曼滤波的状态方程为Xk＝Φk/k-1Xk-1+Wk，其中系统状态矩阵
系统参数矩阵
(xk,yk)、yawk分别为人员在水平自由方位坐标系下的坐标的估计值和航向角的估计值，lenk为第一距离；
卡尔曼滤波的量测方程为：Zk＝JXk+Vk，其中
测量值矩阵
Wk、Vk分别为过程噪声矩阵、测量噪声矩阵，G_yawk为第一航向角；
x0、y0、yaw0利用下式计算
Δx、Δy、Δθ分别为第二标志Flag_res刚好被置为true时得到的值，(XI,YI)、I_yaw分别为第二标志Flag_res刚好被置为true时对应的第二坐标、第二航向角；
所述卫星定位模块、惯性测量模块均与信号处理单元连接。
12.根据权利要求11所述的定位装置，其特征在于：所述卫星定位模块、惯性测量模块固定设置在人员足部、腰部、肩部中任意的一个位置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810811710.0A CN108896044B (zh) | 2018-07-23 | 2018-07-23 | 一种基于惯性导航和卫星导航的定位方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810811710.0A CN108896044B (zh) | 2018-07-23 | 2018-07-23 | 一种基于惯性导航和卫星导航的定位方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108896044A CN108896044A (zh) | 2018-11-27 |
| CN108896044B true CN108896044B (zh) | 2021-09-10 |
Family
ID=64352567
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810811710.0A Active CN108896044B (zh) | 2018-07-23 | 2018-07-23 | 一种基于惯性导航和卫星导航的定位方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108896044B (zh) |
Families Citing this family (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109541571B (zh) * | 2018-12-29 | 2021-05-07 | 北京智行者科技有限公司 | Eps零偏和多线激光雷达的联合标定方法 |
| CN109798890B (zh) * | 2019-02-01 | 2021-09-17 | 上海戴世智能科技有限公司 | 用于车用组合惯性导航系统在无观测值条件下的启动方法、车用组合惯性导航系统和车辆 |
| CN110702106B (zh) * | 2019-10-15 | 2021-04-09 | 深圳市元征科技股份有限公司 | 一种无人机及其航向对准方法、装置和存储介质 |
| CN111342856A (zh) * | 2020-02-28 | 2020-06-26 | Oppo广东移动通信有限公司 | 一种通信装置和电子设备 |
| CN111399022A (zh) * | 2020-04-08 | 2020-07-10 | 湖南格艾德电子科技有限公司 | 一种卫星导航与惯性导航超紧组合仿真定位的方法及终端 |
| CN111580139B (zh) * | 2020-05-27 | 2022-12-20 | 阿波罗智联(北京)科技有限公司 | 卫星导航数据的有效性判断方法及装置、电子设备 |
| CN111811512B (zh) * | 2020-06-02 | 2023-08-01 | 北京航空航天大学 | 基于联邦平滑的mpos离线组合估计方法和装置 |
| CN112415558B (zh) * | 2021-01-25 | 2021-04-16 | 腾讯科技（深圳）有限公司 | 行进轨迹的处理方法及相关设备 |
| CN115908476A (zh) * | 2021-09-30 | 2023-04-04 | 华为技术有限公司 | 轨迹估计模型训练方法、轨迹估计方法及设备 |
| CN114323018A (zh) * | 2021-11-26 | 2022-04-12 | 中国航空无线电电子研究所 | 一种验证航空航迹融合算法软件的方法 |
| CN114485678B (zh) * | 2021-12-31 | 2023-09-12 | 上海航天控制技术研究所 | 天地一体月面着陆导航方法 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6697736B2 (en) * | 2002-02-06 | 2004-02-24 | American Gnc Corporation | Positioning and navigation method and system thereof |
| EP2378248A2 (en) * | 2010-04-19 | 2011-10-19 | Honeywell International Inc. | Systems and methods for determining inertial navigation system faults |
| CN106940184A (zh) * | 2017-04-24 | 2017-07-11 | 湖南华诺星空电子技术有限公司 | 一种基于差分gps的惯导定位方法及系统 |
| CN107289941A (zh) * | 2017-06-14 | 2017-10-24 | 湖南格纳微信息科技有限公司 | 一种基于惯导的室内定位方法与装置 |
| CN107390246A (zh) * | 2017-07-06 | 2017-11-24 | 电子科技大学 | 一种基于遗传神经网络的gps/ins组合导航方法 |
| CN107525503A (zh) * | 2017-08-23 | 2017-12-29 | 王伟 | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 |
| CN107656300A (zh) * | 2017-08-15 | 2018-02-02 | 东南大学 | 基于北斗/gps双模软件接收机的卫星/惯性超紧组合系统及方法 |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103645489A (zh) * | 2013-11-22 | 2014-03-19 | 航天恒星科技有限公司 | 一种航天器gnss单天线定姿方法 |
| US9476989B2 (en) * | 2014-04-16 | 2016-10-25 | Honeywell International Inc. | Vector tracking loop operability through oscillator micro-jump event |
| IL234691A (en) * | 2014-09-16 | 2017-12-31 | Boyarski Shmuel | A method and system for inertial navigation using a world-wide fastening system |
| US9593962B2 (en) * | 2014-10-08 | 2017-03-14 | Honeywell International Inc. | Systems and methods for attitude fault detection based on integrated GNSS/inertial hybrid filter residuals |
| CN106376007B (zh) * | 2015-07-20 | 2020-01-17 | 中国移动通信集团四川有限公司 | 一种基站覆盖性能定位方法及系统 |
-
2018
- 2018-07-23 CN CN201810811710.0A patent/CN108896044B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6697736B2 (en) * | 2002-02-06 | 2004-02-24 | American Gnc Corporation | Positioning and navigation method and system thereof |
| EP2378248A2 (en) * | 2010-04-19 | 2011-10-19 | Honeywell International Inc. | Systems and methods for determining inertial navigation system faults |
| CN106940184A (zh) * | 2017-04-24 | 2017-07-11 | 湖南华诺星空电子技术有限公司 | 一种基于差分gps的惯导定位方法及系统 |
| CN107289941A (zh) * | 2017-06-14 | 2017-10-24 | 湖南格纳微信息科技有限公司 | 一种基于惯导的室内定位方法与装置 |
| CN107390246A (zh) * | 2017-07-06 | 2017-11-24 | 电子科技大学 | 一种基于遗传神经网络的gps/ins组合导航方法 |
| CN107656300A (zh) * | 2017-08-15 | 2018-02-02 | 东南大学 | 基于北斗/gps双模软件接收机的卫星/惯性超紧组合系统及方法 |
| CN107525503A (zh) * | 2017-08-23 | 2017-12-29 | 王伟 | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 |
Non-Patent Citations (5)
| Title |
|---|
| "A hybrid error modeling for MEMS IMU in integrated GPS/INS navigation system";Maged Ismail等;《The Journal of Global Positioning Systems Cover Image》;20180419;第16卷(第6期);1-12 * |
| "Development and evaluation of GNSS/INS data processing software for position and orientation systems";Xiaoji Niu等;《Survey Review》;20150331;第47卷(第341期);87-98 * |
| "一种惯导辅助卫星导航三阶载波PLL算法";吕鹏等;《计算机仿真》;20131231;第30卷(第12期);13-16，124 * |
| "采用卫星与惯性导航对空投精确控制研究";尹剑等;《计算机仿真》;20161231;第33卷(第12期);69-72,184 * |
| "高动态GNSS/INS标量深组合跟踪技术研究";班亚龙;《中国博士学位论文全文数据库 信息科技辑》;20170615(第6期);I136-47 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108896044A (zh) | 2018-11-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108896044B (zh) | 一种基于惯性导航和卫星导航的定位方法及装置 | |
| CN109099912B (zh) | 室外精确定位导航方法、装置、电子设备及存储介质 | |
| US9488480B2 (en) | Method and apparatus for improved navigation of a moving platform | |
| CN105607104B (zh) | 一种基于gnss与ins的自适应导航定位系统及方法 | |
| CN107830858B (zh) | 一种基于重力辅助的手机航向估计方法 | |
| KR101473653B1 (ko) | 보행자 모션 인식 기반 보행자 위치 추정 장치, 및 그 방법 | |
| CN104121905A (zh) | 一种基于惯性传感器的航向角获取方法 | |
| US10999709B2 (en) | Method and system for determining a direction of movement of an object | |
| CN109059909A (zh) | 基于神经网络辅助的卫星/惯导列车定位方法与系统 | |
| Woyano et al. | Evaluation and comparison of performance analysis of indoor inertial navigation system based on foot mounted IMU | |
| CN114615740A (zh) | 一种基于蓝牙、pdr和地图匹配融合的室内人员定位方法 | |
| Atia et al. | An enhanced 3D multi-sensor integrated navigation system for land-vehicles | |
| US10697776B2 (en) | Method and system for tracking and determining a position of an object | |
| CN117232506A (zh) | 一种复杂战场环境下的军用移动装备定位系统 | |
| US20210306811A1 (en) | Method and system for determining a direction of movement of an object | |
| CN112179356B (zh) | 一种基于智能航迹推算的弱网环境导航的方法及系统 | |
| Chen et al. | Modeling and Assessment on The Tightly Coupled Integration of TWTOA-Based UWB and INS | |
| GB2567889A (en) | Method and system for determining a direction of movement of an object | |
| Jouybari et al. | Impact of GNSS Signal outage on EOPs using forward Kalman filter and smoothing algorithm | |
| FI127640B (en) | Method and system for determining the direction of movement of an object | |
| GB2567887A (en) | Method and system for tracking and determining a position of an object | |
| FI127639B (en) | Method and arrangement for tracking and determining position of an object | |
| Kuusniemi et al. | Evaluation of Bayesian approaches for multi-sensor multi-network seamless positioning | |
| Sarvrood et al. | Un-Tethered Positioning and Navigation Solution for IoT Applications and Land Vehicles | |
| CN117203553A (zh) | 一种用于估计人的位置和速度的方法和设备 |
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 | ||
| CP03 | Change of name, title or address | ||
| CP03 | Change of name, title or address |
Address after: 410205 1705, building 1b, Hunan military civilian integration science and Technology Innovation Industrial Park, No. 699, Qingshan Road, Changsha high tech Development Zone, Changsha, Hunan Patentee after: Hunan yunjiangna micro Information Technology Co.,Ltd. Address before: 410205 No. 5, 4th Floor, Building F4, Yuyuan, Lugu, 27 Wenxuan Road, Changsha High-tech Development Zone, Hunan Province Patentee before: HUNAN GLONAVIN INFORMATION TECHNOLOGY CO.,LTD. |