CN110823212A - 一种基于粒子滤波的sins/dr组合导航系统位置跟踪确定方法 - Google Patents

一种基于粒子滤波的sins/dr组合导航系统位置跟踪确定方法 Download PDF

Info

Publication number
CN110823212A
CN110823212A CN201810920933.0A CN201810920933A CN110823212A CN 110823212 A CN110823212 A CN 110823212A CN 201810920933 A CN201810920933 A CN 201810920933A CN 110823212 A CN110823212 A CN 110823212A
Authority
CN
China
Prior art keywords
weight
particle
sins
navigation system
method based
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.)
Granted
Application number
CN201810920933.0A
Other languages
English (en)
Other versions
CN110823212B (zh
Inventor
王万征
邓亮
裴兴凯
陈静
庄广琛
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Automation Control Equipment Institute BACEI
Original Assignee
Beijing Automation Control Equipment Institute BACEI
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Automation Control Equipment Institute BACEI filed Critical Beijing Automation Control Equipment Institute BACEI
Priority to CN201810920933.0A priority Critical patent/CN110823212B/zh
Publication of CN110823212A publication Critical patent/CN110823212A/zh
Application granted granted Critical
Publication of CN110823212B publication Critical patent/CN110823212B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar Systems Or Details Thereof (AREA)
  • Navigation (AREA)

Abstract

本发明属于捷联导航控制技术，具体公开了一种基于粒子滤波的SINS/DR组合导航系统位置跟踪确定方法，首先根据目标的初始状态量及可能出现的误差半径对粒子进行随机分布，然后进行粒子状态预测，计算粒子权值，确定各个粒子与真实位置的近似程度和所有粒子的状态融合成的估计值，当某粒子的权值小于某一阈值时，进行重采样。本方法以粒子滤波理论为指导，以目标位置为特征实现了SINS/DR组合导航系统中的位置跟踪算法，可以避免里程计输出异常时引起的位置跳变，且误差不随时间发散，提高了位置和速度的实时输出精度。

Description

一种基于粒子滤波的SINS/DR组合导航系统位置跟踪确定 方法
技术领域
本发明属于捷联导航控制技术，具体涉及一种导航系统位置跟踪确定方法。
背景技术
在SINS/DR组合导航系统中，通常以里程计进行航位推算所得的位置、速度作为量测量与捷联惯导系统进行组合导航，通过卡尔曼滤波对捷联惯导系统的姿态误差进行估计，从而保持惯导姿态精度。在载体行进过程中，如果里程计测量发生异常情况，如打滑、侧移、离地等，会导致航位推算结果发生异常。在实时处理过程中，无法通过滤波器相关量对航位推算结果进行修正，使得里程计在发生异常情况时，航位推算结果会发生跳变，影响精度。
发明内容
本发明的目的在于提供一种基于粒子滤波的SINS/DR组合导航系统位置跟踪确定方法，能够避免里程计输出异常时引起的位置跳变，且误差不随时间发散。
本发明的技术方案如下：
一种基于粒子滤波的SINS/DR组合导航系统位置跟踪确定方法，该方法包括如下步骤：
1)粒子初始化，确定目标的初始状态量及可能出现的误差半径，然后根据式(1)对粒子进行随机分布
其中，S(i)t为第i个粒子的航程，ΔS为根据运动速度预估的误差，ΔV为目标预估的速度，random为(-1,1)内的随机数，V(i)t为第i个粒子的速度，weight(i)为第i个粒子的权重，N为粒子数；
2)粒子状态预测
通过状态转移方程来计算每个粒子在该周期的状态预测值，用来描述目标运动的状态转移方程如式(2)所示：
Figure BDA0001764213450000021
其中，Vodo为里程计速度；
3)粒子权值计算，确定各个粒子与真实位置的近似程度
Figure BDA0001764213450000022
其中，weight(i)为每个粒子的权重，S为参考位置；
利用下式确定每个粒子权值占整个累计权值的比重
其中，sum_weight为权值累加值；
4)跟踪估计，确定所有粒子的状态融合成的估计值
Figure BDA0001764213450000024
5)设置一个权值阈值weight_min，当某粒子的权值小于该阈值时，进行重采样
Figure BDA0001764213450000025
所述的步骤3)中的参考位置S是根据SINS解算结果和航位推算结果综合计算的，其计算方法如式(4)所示：
S＝0.995×(S+VSINS×t)+0.005×SDR (4)
其中，VSINS为组合导航系统解算的速度，t为计算周期，SDR为航位推算位置。
所述的权值阈值weight_min为
Figure BDA0001764213450000031
本发明的显著效果在于：
本方法以粒子滤波理论为指导，以目标位置为特征实现了SINS/DR组合导航系统中的位置跟踪算法，可以避免里程计输出异常时引起的位置跳变，且误差不随时间发散，提高了位置和速度的实时输出精度。
具体实施方式
下面结合具体实施例对本发明作进一步详细说明。
首先要确定粒子模型及其状态转移模型。粒子模型包含两个状态变量：航程St和前向速度Vt。
位置跟踪步骤如下。
1)粒子初始化
粒子初始化是通过确定目标的初始状态量及可能出现的误差半径，然后根据式(1)对粒子进行随机分布：
Figure BDA0001764213450000032
其中，S(i)t为第i个粒子的航程，ΔS为根据运动速度预估的误差，ΔV为目标预估的速度，random为(-1,1)内的随机数，V(i)t为第i个粒子的速度，weight(i)为第i个粒子的权重，N为粒子数。
粒子初始化只在开始时执行一次，后续的步骤每个计算周期都执行一次。
2)粒子状态预测
粒子状态预测是通过状态转移方程来计算每个粒子在该周期的状态预测值，用来描述目标运动的状态转移方程如式(2)所示：
Figure BDA0001764213450000041
其中，Vodo为里程计速度。
3)粒子权值计算
粒子通过状态预测得到了目标可能的位置，权值计算是用来计算各个粒子与真实位置的近似程度。权值计算方法如式(3)所示：
Figure BDA0001764213450000042
其中，weight(i)为每个粒子的权重，S为参考位置。
参考位置S是根据SINS解算结果和航位推算结果综合计算的，其计算方法如式(4)所示：
S＝0.995×(S+VSINS×t)+0.005×SDR (4)
其中，VSINS为组合导航系统解算的速度，t为计算周期，SDR为航位推算位置。
计算目标运动状态的估计值时，需要用归一化的加权粒子集来进行计算。权值归一化即求出每个粒子权值占整个累计权值的比重，权值归一化公式如式(5)所示：
Figure BDA0001764213450000043
其中，sum_weight为权值累加值。
4)跟踪估计
归一化后的权值即可反映出该粒子在所有粒子中能代表跟踪目标运动状态的比重，权值越大则说明该粒子越接近目标真实状态。通过权值即可将所有粒子的状态融合成一个估计值，计算方法如式(6)所示：
Figure BDA0001764213450000051
5)重采样
随着迭代次数的增多，一些粒子的权值会变得很小，使得这些粒子对于估计出目标运动状态难以起到作用，出现粒子退化现象。在迭代计算中，设置一个权值阈值weight_min，本实施例中设为
Figure BDA0001764213450000052
当某粒子的权值小于该阈值时，进行重采样，方法如式(7)所示：
通过以上流程对载体的位置进行持续跟踪计算，即可得到航迹。正常情况下，里程计未发生异常情况时，里程计输出的位置增量无误差，SINS系统虽然可以通过航位推算的速度对其速度误差和姿态误差进行修正，但加表零偏引起在一个滤波周期内的速度误差仍会导致位置误差随时间发散。利用本发明中基于粒子滤波的位置跟踪算法，位置误差仍然存在，但在一定时间收敛后，误差不再随着时间发散。
当里程计发生异常时，位置误差发生跳变，在实时处理中会影响该时刻测量精度。利用本方法可以使位置输出在长时间情况下跟踪里程计位置而不发生跳变，且不会随时间发散。
里程计正常情况下速度输出为1m/s，在里程计打滑时，里程计输出的速度跳变为零，通过本发明方法跟踪到的速度在该异常时刻变化值约为0.05m/s，误差减小了一个数量级以上。

Claims (3)

1.一种基于粒子滤波的SINS/DR组合导航系统位置跟踪确定方法，其特征在于，该方法包括如下步骤：
1)粒子初始化，确定目标的初始状态量及可能出现的误差半径，然后根据式(1)对粒子进行随机分布
其中，S(i)t为第i个粒子的航程，ΔS为根据运动速度预估的误差，ΔV为目标预估的速度，random为(-1,1)内的随机数，V(i)t为第i个粒子的速度，weight(i)为第i个粒子的权重，N为粒子数；
2)粒子状态预测
通过状态转移方程来计算每个粒子在该周期的状态预测值，用来描述目标运动的状态转移方程如式(2)所示：
Figure FDA0001764213440000012
其中，Vodo为里程计速度；
3)粒子权值计算，确定各个粒子与真实位置的近似程度
Figure FDA0001764213440000013
其中，weight(i)为每个粒子的权重，S为参考位置；
利用下式确定每个粒子权值占整个累计权值的比重
Figure FDA0001764213440000014
其中，sum_weight为权值累加值；
4)跟踪估计，确定所有粒子的状态融合成的估计值
Figure FDA0001764213440000021
5)设置一个权值阈值weight_min，当某粒子的权值小于该阈值时，进行重采样
2.如权利要求1所述的一种基于粒子滤波的SINS/DR组合导航系统位置跟踪确定方法，其特征在于：
所述的步骤3)中的参考位置S是根据SINS解算结果和航位推算结果综合计算的，其计算方法如式(4)所示：
S＝0.995×(S+VSINS×t)+0.005×SDR (4)
其中，VSINS为组合导航系统解算的速度，t为计算周期，SDR为航位推算位置。
3.如权利要求1所述的一种基于粒子滤波的SINS/DR组合导航系统位置跟踪确定方法，其特征在于：所述的权值阈值weight_min为
Figure FDA0001764213440000023
CN201810920933.0A 2018-08-14 2018-08-14 一种基于粒子滤波的sins/dr组合导航系统位置跟踪确定方法 Active CN110823212B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810920933.0A CN110823212B (zh) 2018-08-14 2018-08-14 一种基于粒子滤波的sins/dr组合导航系统位置跟踪确定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810920933.0A CN110823212B (zh) 2018-08-14 2018-08-14 一种基于粒子滤波的sins/dr组合导航系统位置跟踪确定方法

Publications (2)

Publication Number Publication Date
CN110823212A true CN110823212A (zh) 2020-02-21
CN110823212B CN110823212B (zh) 2023-04-07

Family

ID=69547109

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810920933.0A Active CN110823212B (zh) 2018-08-14 2018-08-14 一种基于粒子滤波的sins/dr组合导航系统位置跟踪确定方法

Country Status (1)

Country Link
CN (1) CN110823212B (zh)

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
BR7606899A (pt) * 1975-10-15 1977-08-30 Hahn C Gmbh Aparelho para receber uma peca de trabalho cilindrica
US20040119640A1 (en) * 2002-12-23 2004-06-24 Menegozzi Lionel N. Method for determining azimuth and elevation angles using a single axis direction finding system
US20100109950A1 (en) * 2008-11-06 2010-05-06 Texas Instruments Incorporated Tightly-coupled gnss/imu integration filter having speed scale-factor and heading bias calibration
CN104819716A (zh) * 2015-04-21 2015-08-05 北京工业大学 一种基于mems的ins/gps组合的室内外个人导航算法
CN105263149A (zh) * 2015-09-29 2016-01-20 上海交通大学 移动无线传感器网络中节点自适应分布式重聚类方法
CN105424041A (zh) * 2016-01-18 2016-03-23 重庆邮电大学 一种基于bd/ins紧耦合的行人定位算法
CN106017486A (zh) * 2016-05-16 2016-10-12 浙江大学 一种面向无人车导航的基于轨迹拐点滤波的地图定位方法
CN106840211A (zh) * 2017-03-24 2017-06-13 东南大学 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法
CN107092264A (zh) * 2017-06-21 2017-08-25 北京理工大学 面向银行厅堂环境的服务机器人自主导航与自动充电方法
CN107270898A (zh) * 2017-06-28 2017-10-20 东南大学 基于mems传感器和vlc定位融合的双粒子滤波导航装置和方法
CN107782307A (zh) * 2016-08-26 2018-03-09 北京自动化控制设备研究所 一种sins/dr组合导航系统里程计异常数据后处理方法
CN107941211A (zh) * 2017-11-13 2018-04-20 北京邮电大学 基于二阶级联的多元素融合定位方法、装置及电子设备
CN108168552A (zh) * 2016-12-07 2018-06-15 广州映博智能科技有限公司 一种看护机器人路标定位系统

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
BR7606899A (pt) * 1975-10-15 1977-08-30 Hahn C Gmbh Aparelho para receber uma peca de trabalho cilindrica
US20040119640A1 (en) * 2002-12-23 2004-06-24 Menegozzi Lionel N. Method for determining azimuth and elevation angles using a single axis direction finding system
US20100109950A1 (en) * 2008-11-06 2010-05-06 Texas Instruments Incorporated Tightly-coupled gnss/imu integration filter having speed scale-factor and heading bias calibration
CN104819716A (zh) * 2015-04-21 2015-08-05 北京工业大学 一种基于mems的ins/gps组合的室内外个人导航算法
CN105263149A (zh) * 2015-09-29 2016-01-20 上海交通大学 移动无线传感器网络中节点自适应分布式重聚类方法
CN105424041A (zh) * 2016-01-18 2016-03-23 重庆邮电大学 一种基于bd/ins紧耦合的行人定位算法
CN106017486A (zh) * 2016-05-16 2016-10-12 浙江大学 一种面向无人车导航的基于轨迹拐点滤波的地图定位方法
CN107782307A (zh) * 2016-08-26 2018-03-09 北京自动化控制设备研究所 一种sins/dr组合导航系统里程计异常数据后处理方法
CN108168552A (zh) * 2016-12-07 2018-06-15 广州映博智能科技有限公司 一种看护机器人路标定位系统
CN106840211A (zh) * 2017-03-24 2017-06-13 东南大学 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法
CN107092264A (zh) * 2017-06-21 2017-08-25 北京理工大学 面向银行厅堂环境的服务机器人自主导航与自动充电方法
CN107270898A (zh) * 2017-06-28 2017-10-20 东南大学 基于mems传感器和vlc定位融合的双粒子滤波导航装置和方法
CN107941211A (zh) * 2017-11-13 2018-04-20 北京邮电大学 基于二阶级联的多元素融合定位方法、装置及电子设备

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
王宏健等: "基于方差缩减粒子滤波的无人水下航行器航位推算", 《信息与控制》, no. 02, 15 April 2013 (2013-04-15) *
蒋治宇等: "无迹卡尔曼滤波和无迹粒子滤波在惯导初始对准中的应用对比", 《科学技术与工程》, no. 07, 8 March 2018 (2018-03-08) *
郭玉胜，等: "惯性/卫星/里程计多信息融合方法及在铁路测绘中的应用", 《中国惯性技术学报》, vol. 25, no. 3, 30 June 2017 (2017-06-30) *

Also Published As

Publication number Publication date
CN110823212B (zh) 2023-04-07

Similar Documents

Publication Publication Date Title
CN108614426B (zh) 一种基于干扰观测器的多移动机器人编队鲁棒控制方法
CN108573271B (zh) 多传感器目标信息融合的优化方法及装置、计算机设备和记录介质
Semeniuk et al. Bridging GPS outages using neural network estimates of INS position and velocity errors
WO2015155833A1 (ja) 衝突防止装置
CN110031019A (zh) 一种用于自动驾驶车辆的打滑检测处理方法
US11465782B2 (en) Systems and methods for autonomous deorbiting of a spacecraft
KR20140060349A (ko) 차량용 타이어 파라미터를 추정하는 방법
CN107807069B (zh) 一种海面溢油的自适应跟踪控制方法及其系统
CN105270445B (zh) 自动列车运行装置以及自动列车运行方法
CN112254727B (zh) 基于teb的路径规划方法及装置
CN112987061B (zh) 一种基于gps和激光雷达模糊融合定位方法
CN110597262A (zh) 一种基于漂角修正的欠驱动船舶路径跟踪自适应控制方法
CN109213203A (zh) 一种基于预见控制的舰载机自动着舰控制方法
CN110637209B (zh) 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质
JP2022513511A (ja) 完全性範囲を特定するための方法
CN111666672A (zh) 一种针对小推力发动机推力下降故障的能力评估方法
US20200334837A1 (en) Method for predicting a motion of an object, method for calibrating a motion model, method for deriving a predefined quantity and method for generating a virtual reality view
CN112986977B (zh) 一种克服雷达扩展卡尔曼航迹滤波发散的方法
CN109582026B (zh) 基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法
CN110702093A (zh) 基于粒子滤波的定位方法、装置、存储介质及机器人
CN113252060B (zh) 基于神经网络模型的车辆轨迹推算方法及装置
CN115123233A (zh) 规划车辆轨迹的方法和装置
CN110823212B (zh) 一种基于粒子滤波的sins/dr组合导航系统位置跟踪确定方法
Michałek et al. Vector-field-orientation tracking control for a mobile vehicle disturbed by the skid-slip phenomena
CN111412919A (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