CN109084752B - 一种基于全连通约束的地磁导航定位方法 - Google Patents
一种基于全连通约束的地磁导航定位方法 Download PDFInfo
- Publication number
- CN109084752B CN109084752B CN201811218108.2A CN201811218108A CN109084752B CN 109084752 B CN109084752 B CN 109084752B CN 201811218108 A CN201811218108 A CN 201811218108A CN 109084752 B CN109084752 B CN 109084752B
- Authority
- CN
- China
- Prior art keywords
- geomagnetic
- total
- current position
- carrier
- navigation
- 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
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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明提出一种基于全连通约束的地磁导航定位方法,首先建立导航区域内的地磁数据库;根据设定的运载体起始位置和终止位置,得到起始位置和终止位置的磁场总强,并确定运载体的起始航向角;在每个计算周期内,采集当前位置地磁总强,判断当前位置地磁总强与地磁数据库是否唯一对应,如果唯一对应,则由地磁数据库获取当前位置,否则基于全连通的四点定位方法实现在磁图中唯一确定与目标值匹配的位置作为当前位置;确定当前位置后,判断当前位置与终止位置的距离是否小于一个运动步长,若是则结束导航,否则计算当前时刻的航向角并继续下一计算周期。本发明通过四点定位以保证地磁数据与坐标位置的唯一对应关系,从而提高地磁导航定位的准确性。
Description
技术领域
本发明涉及一种基于全连通约束的地磁导航定位方法,属于地磁导航技术领域。
背景技术
地磁导航是利用地磁图来实现的一种新型的导航定位技术,地磁匹配算法是地磁导航的核心。地磁匹配算法的本质是一种搜索的导航算法,其基本原理是:在某一个区域内建立地磁数据库,通过固定在运载体上的地磁传感器测量地磁值,并将该地磁信息与存储在计算机中的地磁数据库相匹配,从而获得运载体的具***置。该技术能够有效地改善传统导航***技术的缺陷,既能避免惯性导航***随时间变化存在累积误差从而使得导航精度下降的不足,又能克服卫星导航技术中受环境干扰严重等问题。
地磁匹配算法作为一种重要的地磁导航算法,能够获得相对精度较高的位置信息,而且在一定程度上可以弥补实测数据精度不足的缺陷。可以将二次插值技术应用到地磁匹配算法中,但是通过匹配得到的位置往往由于传感器测得的地磁数据与地磁数据库中的位置不能唯一对应,容易产生较高的误匹配率,从而无法实现准确的导航定位。
发明内容
本发明针对地磁匹配导航过程中,仅使用磁场总强作为唯一的地磁参量时,由于等磁线的存在,会出现地磁值与坐标位置存在不一一对应的问题,基于图论的相关理论,提出了一种基于全连通约束的地磁导航定位方法,通过四点定位以保证地磁数据与坐标位置的唯一对应关系,从而提高地磁导航定位的准确性。
本发明的技术方案为:
所述一种基于全连通约束的地磁导航定位方法,其特征在于:包括以下步骤:
步骤1:在导航区域内,根据若干已知点的磁场总强,利用插值方法,建立该导航区域内的地磁数据库;
步骤2:在地磁坐标系二维平面内,建立运载体的运动方程为:
其中L=V×ΔT,表示运载体的运动步长;V表示载体在ΔT时间内做匀速运动的运动速度,ΔT为采样周期;k表示载体运动的某一时刻,θk为k时刻的航向角,(xk-1,yk-1)为k-1时刻运载体的位置坐标,(xk,yk)为k时刻运载体的位置坐标;
步骤3:根据设定的运载体起始位置(x0,y0)和终止位置(xt,yt),由步骤1中的地磁数据库得到起始位置和终止位置的磁场总强B0和Bt,并确定运载体的起始航向角运载体从起始位置(x0,y0)按照起始航向角θ0前进一个运动步长;
步骤4:采集当前位置地磁总强,判断当前位置地磁总强与地磁数据库中的地磁总强是否唯一对应,如果唯一对应,则根据地磁数据库获取当前位置坐标,否则通过以下过程确定当前位置坐标:
步骤4.1:运载体从当前位置开始沿平行于地磁坐标系的坐标轴方向,行走一个边长为的L的正方形ABCD,并得到正方形ABCD四个顶点的地磁总强:B0,B1,B2,B3;
步骤4.2:在地磁数据库中查找所有与B0,B1,B2,B3匹配的地磁总强,分别记为其中i=1,2,...,m;j=1,2,...,n;k=1,2,...,p;l=1,2,...,q,m,n,p,q分别为与B0,B1,B2,B3相同的地磁总强的个数,将每个地磁总强对应的具***置记为:
步骤4.3:通过遍历计算步骤4.2得到的具***置中,每相邻两个点之间的距离:
并选择得到满足相邻两点之间的距离与正方形边长L相等的所有位置;
步骤4.4:在步骤4.3得到的所有位置中,通过多点连通性,由四点约束的条件唯一确定出一个正方形,并以该正方形的起点位置作为当前位置;
步骤5:确定当前位置后,判断当前位置与终止位置的距离是否小于一个运动步长,若是,则结束导航,若不是,则进入步骤6;
步骤6:根据当前位置坐标和设定的终止位置(xt,yt),计算当前时刻的航向角,并以该航向角前进一个运动步长后,返回步骤4。
有益效果
本发明提出一种基于全连通约束的地磁导航定位方法,通过四点定位以保证地磁数据与坐标位置的唯一对应关系,从而提高地磁导航定位的准确性。
本发明的附加方面和优点将在下面的描述中部分给出,部分将从下面的描述中变得明显,或通过本发明的实践了解到。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解,其中:
图1全连通约束的地磁导航定位算法流程图;
图2运载体的移动路线及地磁信息与位置信息匹配;
图3利用三轮移动机器人搭建的运载体实物图;
图4某一导航区域内地磁背景环境三维图及二维等值线图;地磁总强单位为:μT;
图5地磁多值点分布;
图6基于全连通约束唯一确定的四点定位结果;
图7基于全连通约束的地磁导航结果。
具体实施方式
下面详细描述本发明的实施例,所述实施例是示例性的,旨在用于解释本发明,而不能理解为对本发明的限制。
本发明针对地磁匹配导航过程中,仅使用磁场总强作为唯一的地磁参量时,由于等磁线的存在,会出现地磁值与坐标位置存在不一一对应的问题,基于图论的相关理论,提出了一种基于全连通约束的地磁导航定位方法,通过四点定位以保证地磁数据与坐标位置的唯一对应关系,从而提高地磁导航定位的准确性。
该方法包括以下步骤:
步骤1:在导航区域内,根据若干已知点的磁场总强,利用插值方法,建立该导航区域内的地磁数据库;地磁背景环境三维图及二维等值线图如附图4所示。
步骤2:在地磁坐标系二维平面内,根据运载体与目标位置之间的几何关系确定载体的运动学方程,
式中,k表示载体运动的某一时刻,vk为载体在k时刻的运动速度,θk为k时刻的航向角,ΔT为采样周期,(xk-1,yk-1)为k-1时刻运载体的位置坐标,(xk,yk)为k时刻运载体的位置坐标。当载体在ΔT时间内做匀速运动,即vk可以用一个常量V表示,则式(1)可以简化如下:
式中,L=V×ΔT,表示载体的运动步长。
步骤3:初始化:根据设定的运载体起始位置(x0,y0)和终止位置(xt,yt),由步骤1中的地磁数据库得到起始位置和终止位置的磁场总强B0=56.5μT和Bt=62.9μT,并确定运载体的起始航向角运载体从起始位置(x0,y0)按照起始航向角θ0前进一个运动步长。
步骤4:采集当前位置地磁总强,判断当前位置地磁总强与地磁数据库中的地磁总强是否唯一对应,如果唯一对应,则根据地磁数据库获取当前位置坐标,否则,如附图5所示,通过以下基于全连通的四点定位方法实现在磁图中唯一确定一个与目标值匹配的位置作为当前位置:
步骤4.1:运载体从当前位置开始沿平行于地磁坐标系的坐标轴方向,如图2所示行走一个边长为的L的正方形ABCD,并得到正方形ABCD四个顶点的地磁总强:B0,B1,B2,B3;
步骤4.2:在地磁数据库中查找所有与B0,B1,B2,B3匹配的地磁总强,分别记为其中i=1,2,...,m;j=1,2,...,n;k=1,2,...,p;l=1,2,...,q,m,n,p,q分别为与B0,B1,B2,B3相同的地磁总强的个数,将每个地磁总强对应的具***置记为:
步骤4.3:通过遍历计算步骤4.2得到的具***置中,每相邻两个点之间的距离:
并选择得到满足相邻两点之间的距离与正方形边长L相等的所有位置;
步骤4.4:为了保证定位点的唯一性,在步骤4.3得到的所有位置中,通过多点连通性,由四点约束的条件,即在地磁坐标系中,xA=xB,xC=xD,yA=yD,yB=yc,唯一确定出一个正方形,并以该正方形的起点位置作为当前位置。如附图6所示。
步骤5:确定当前位置后,判断当前位置与终止位置的距离是否小于一个运动步长,若是,则结束导航,若不是,则进入步骤6;
步骤6:根据当前位置坐标和设定的终止位置(xt,yt),计算当前时刻的航向角,并以该航向角前进一个运动步长后,返回步骤4。
由导航定位的路径轨迹附图7可以看出,该算法可以解决由于地磁值与位置坐标的多值问题导致在地磁导航过程中位置无法唯一确定的缺陷,通过全连通约束的四点定位地磁导航定位可以实现导航目的,从而验证了该发明的有效性。
尽管上面已经示出和描述了本发明的实施例,可以理解的是,上述实施例是示例性的,不能理解为对本发明的限制,本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
Claims (1)
1.一种基于全连通约束的地磁导航定位方法,其特征在于:包括以下步骤:
步骤1:在导航区域内,根据若干已知点的磁场总强,利用插值方法,建立该导航区域内的地磁数据库;
步骤2:在地磁坐标系二维平面内,建立运载体的运动方程为:
其中L=V×ΔT,表示运载体的运动步长;V表示载体在ΔT时间内做匀速运动的运动速度,ΔT为采样周期;k表示载体运动的某一时刻,θk为k时刻的航向角,(xk-1,yk-1)为k-1时刻运载体的位置坐标,(xk,yk)为k时刻运载体的位置坐标;
步骤3:根据设定的运载体起始位置(x0,y0)和终止位置(xt,yt),由步骤1中的地磁数据库得到起始位置和终止位置的磁场总强B0和Bt,并确定运载体的起始航向角运载体从起始位置(x0,y0)按照起始航向角θ0前进一个运动步长;
步骤4:采集当前位置地磁总强,判断当前位置地磁总强与地磁数据库中的地磁总强是否唯一对应,如果唯一对应,则根据地磁数据库获取当前位置坐标,否则通过以下过程确定当前位置坐标:
步骤4.1:运载体从当前位置开始沿平行于地磁坐标系的坐标轴方向,行走一个边长为的L的正方形ABCD,并得到正方形ABCD四个顶点的地磁总强:B0,B1,B2,B3;
步骤4.2:在地磁数据库中查找所有与B0,B1,B2,B3匹配的地磁总强,分别记为其中i=1,2,...,m;j=1,2,...,n;k=1,2,...,p;l=1,2,...,q,m,n,p,q分别为与B0,B1,B2,B3相同的地磁总强的个数,将每个地磁总强对应的具***置记为:
步骤4.3:通过遍历计算步骤4.2得到的具***置中,每相邻两个点之间的距离:
并选择得到满足相邻两点之间的距离与正方形边长L相等的所有位置;
步骤4.4:在步骤4.3得到的所有位置中,通过多点连通性,由四点约束的条件唯一确定出一个正方形,并以该正方形的起点位置作为当前位置;
步骤5:确定当前位置后,判断当前位置与终止位置的距离是否小于一个运动步长,若是,则结束导航,若不是,则进入步骤6;
步骤6:根据当前位置坐标和设定的终止位置(xt,yt),计算当前时刻的航向角,并以该航向角前进一个运动步长后,返回步骤4。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811218108.2A CN109084752B (zh) | 2018-10-18 | 2018-10-18 | 一种基于全连通约束的地磁导航定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811218108.2A CN109084752B (zh) | 2018-10-18 | 2018-10-18 | 一种基于全连通约束的地磁导航定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109084752A CN109084752A (zh) | 2018-12-25 |
CN109084752B true CN109084752B (zh) | 2022-04-08 |
Family
ID=64843837
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811218108.2A Active CN109084752B (zh) | 2018-10-18 | 2018-10-18 | 一种基于全连通约束的地磁导航定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109084752B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110196045A (zh) * | 2019-06-23 | 2019-09-03 | 西北工业大学 | 一种基于栅格特征的梯度下降地磁导航方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101354253A (zh) * | 2008-09-19 | 2009-01-28 | 北京航空航天大学 | 一种基于匹配度的地磁辅助导航方法 |
CN101520328A (zh) * | 2009-04-01 | 2009-09-02 | 西北工业大学 | 地磁场线图匹配自主导航方法 |
CN102809376A (zh) * | 2012-08-06 | 2012-12-05 | 哈尔滨工程大学 | 一种基于等值线的辅助导航定位方法 |
CN106017473A (zh) * | 2016-05-19 | 2016-10-12 | 中国地质大学(武汉) | 一种室内社交导航*** |
CN107421535A (zh) * | 2017-05-22 | 2017-12-01 | 上海交通大学 | 一种基于磁场特征和加速度信息计步的室内行人定位*** |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP4466705B2 (ja) * | 2007-09-21 | 2010-05-26 | ヤマハ株式会社 | ナビゲーション装置 |
CN105783917B (zh) * | 2014-12-18 | 2019-05-07 | 阿里巴巴集团控股有限公司 | 基于地磁的移动终端定位方法及其装置 |
-
2018
- 2018-10-18 CN CN201811218108.2A patent/CN109084752B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101354253A (zh) * | 2008-09-19 | 2009-01-28 | 北京航空航天大学 | 一种基于匹配度的地磁辅助导航方法 |
CN101520328A (zh) * | 2009-04-01 | 2009-09-02 | 西北工业大学 | 地磁场线图匹配自主导航方法 |
CN102809376A (zh) * | 2012-08-06 | 2012-12-05 | 哈尔滨工程大学 | 一种基于等值线的辅助导航定位方法 |
CN106017473A (zh) * | 2016-05-19 | 2016-10-12 | 中国地质大学(武汉) | 一种室内社交导航*** |
CN107421535A (zh) * | 2017-05-22 | 2017-12-01 | 上海交通大学 | 一种基于磁场特征和加速度信息计步的室内行人定位*** |
Non-Patent Citations (2)
Title |
---|
Geomagnetic matching algorithm based on closest contour point and relative position constraint;Han Daoda,et.al;《2017 36th Chinese Control Conference (CCC)》;20170930;第5932-5936页 * |
基于RSSI和地磁场特征融合的室内定位算法设计与研究;李宗骍;《中国优秀硕士学位论文全文数据库 (信息科技辑)》;20160115;I136-286 * |
Also Published As
Publication number | Publication date |
---|---|
CN109084752A (zh) | 2018-12-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Wang et al. | Map-based localization method for autonomous vehicles using 3D-LIDAR | |
Zhao et al. | A robust laser-inertial odometry and mapping method for large-scale highway environments | |
CN113781582B (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
CN106123908B (zh) | 车辆导航方法和*** | |
Wang et al. | A simple and parallel algorithm for real-time robot localization by fusing monocular vision and odometry/AHRS sensors | |
CN109166140B (zh) | 一种基于多线激光雷达的车辆运动轨迹估计方法及*** | |
CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及*** | |
CN111982099B (zh) | 机器人混合定位方法、装置、设备及计算机可读介质 | |
CN104281148A (zh) | 基于双目立体视觉的移动机器人自主导航方法 | |
CN113447949B (zh) | 一种基于激光雷达和先验地图的实时定位***及方法 | |
KR101957750B1 (ko) | 3차원 자기장 지도에 기초하여 생성된 입자 필터를 이용하여 위치를 추정하는 이동 단말 및 그 장치를 이용한 방법 | |
CN106352879B (zh) | 一种基于图优化的uwb定位与编码器融合的位姿估计方法 | |
Tang et al. | LE-VINS: A robust solid-state-LiDAR-enhanced visual-inertial navigation system for low-speed robots | |
US20170108338A1 (en) | Method for geolocating a carrier based on its environment | |
CN115962773A (zh) | 一种移动机器人同步定位与地图构建方法、装置和设备 | |
CN115540850A (zh) | 一种激光雷达与加速度传感器结合的无人车建图方法 | |
CN109084752B (zh) | 一种基于全连通约束的地磁导航定位方法 | |
CN114593739A (zh) | 基于视觉检测与参考线匹配的车辆全局定位方法及装置 | |
CN102707268A (zh) | 机动雷达组网批处理式误差配准器 | |
CN107990893B (zh) | 二维激光雷达slam中探测环境发生突变的检测方法 | |
CN113554705B (zh) | 一种变化场景下的激光雷达鲁棒定位方法 | |
CN108508894A (zh) | 一种基于二维激光的机器人定位方法 | |
CN111239761B (zh) | 一种用于室内实时建立二维地图的方法 | |
CN105242262A (zh) | 一种基于天线周期扫描时间差无源定位方法 | |
Kim et al. | Mobile robot localization by matching 2D image features to 3D point cloud |
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 |