CN110260859B - 一种考虑数据关联间耦合关系的位姿图修正方法 - Google Patents
一种考虑数据关联间耦合关系的位姿图修正方法 Download PDFInfo
- Publication number
- CN110260859B CN110260859B CN201910492175.1A CN201910492175A CN110260859B CN 110260859 B CN110260859 B CN 110260859B CN 201910492175 A CN201910492175 A CN 201910492175A CN 110260859 B CN110260859 B CN 110260859B
- Authority
- CN
- China
- Prior art keywords
- data association
- confidence
- data
- score
- 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
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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
- G01C21/32—Structuring or formatting of map data
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Theoretical Computer Science (AREA)
- Automation & Control Theory (AREA)
- Data Mining & Analysis (AREA)
- General Engineering & Computer Science (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- Mathematical Analysis (AREA)
- Pure & Applied Mathematics (AREA)
- Computational Mathematics (AREA)
- Algebra (AREA)
- Mathematical Optimization (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Complex Calculations (AREA)
Abstract
本发明涉及一种考虑数据关联间耦合关系的位姿图修正方法,属于水下机器人多波束同步定位与建图领域。包括令N为当前位姿图内由地形匹配得到的数据关联个数,i等于1、j等于1;判断j、i、N的大小关系,计算数据关联j给出的数据关联i的置信度Scorei,j,计算数据关联i总置信度Scorei;判断i与N大小关系,根据当前位姿图内所有数据关联的置信度计算置信度阈值Score_thre;置信程度最小的数据关联其置信度若大于置信度阈值,则输出当前位姿图并停止算法,否则将该数据关联判定为无效数据关联,将其在当前位姿图中移除。本方法不需要依赖于辅助设备,算法执行过程中不需要进行图优化过程,计算效率高且可保证算法在线实时运行,应用前景广阔。
Description
技术领域
本发明涉及一种考虑数据关联间耦合关系的位姿图修正方法,属于水下机器人多波束同步定位与建图领域。
背景技术
在应用多波束同步定位与建图技术为水下机器人提供精确的导航定位信息时,由于海底地形趋于平缓,且多波束声纳作为一种声学测绘仪器,其测绘效果会受到海洋环境和水下机器人运动的干扰,导致多波束同步定位与建图技术构建得到的位姿图中存在大量的无效数据关联,因而需要对无效数据关联进行识别与剔除,从而实现对位姿图的修正。目前现有位姿图修正方法依赖于后续的图优化阶段,导致计算耗时较长,无法满足水下机器人多波束同步定位与建图的在线运行。
公开日为2017年9月5日、公开号为CN107132521A、发明名称为“一种BSLAM中地形匹配结果正确性判别方法”的专利申请,该方法需要不断循环进行位姿图优化以达到剔除无效数据关联并修正位姿图的目的,由于位姿图优化算法通常由高斯牛顿法等寻优方法实现,计算复杂度较高,导致该方法需要耗费大量时间。
公开日为2015年8月19日、公开号为CN104850615A、发明名称为“一种基于g2o的SLAM后端优化算法方法”的专利申请,该方法通过在位姿图优化中增加开关变量实现对位姿图的修正,导致位姿图优化中求解的变量空间增大,大大增加。
发明内容
本发明的目的是为了对多波束同步定位与建图技术构建得到的位姿图中的无效数据关联进行识别与剔除,实现对位姿图的修正而提供一种考虑数据关联间耦合关系的位姿图修正方法。
本发明的目的是这样实现的,一种考虑数据关联间耦合关系的位姿图修正方法,具体包括以下步骤:
步骤1、令N为当前位姿图内由地形匹配得到的数据关联个数,令i等于1;
步骤2、令j等于1;
步骤3、判断j是否等于i,若j等于i,令j=j+1,并继续执行步骤3,否则判断j是否大于N,若j不大于N,则计算数据关联j给出的数据关联i的置信度Scorei,j,并令j=j+1再次执行步骤3,否则计算数据关联i总置信度Scorei并执行步骤4;
步骤4、判断i是否等于N,若是,根据当前位姿图内所有数据关联的置信度计算置信度阈值Score_thre并执行步骤5,否则令i=i+1并执行步骤2;
步骤5、判断置信程度最小的数据关联其置信度是否大于置信度阈值Score_thre,若大于置信度阈值,输出当前位姿图并停止算法,若小于或等于置信度阈值,则将该数据关联判定为无效数据关联,将其在当前位姿图中移除并执行步骤1。
本发明还包括这样一些结构特征:
一种考虑数据关联间耦合关系的位姿图修正方法,所述步骤3具体包括以下步骤:
步骤3.1、判断j是否等于i,若j等于i,令j=j+1,并执行步骤3.1,否则执行步骤3.2;
步骤3.2、判断j是否大于N,若j不大于N,则执行步骤3.3,否则执行步骤3.8;
步骤3.3、对于数据关联i(i=1,2,...,N)与数据关联j(j=1,2,...,i-1,i+1,...,N),将两个数据关联对应的四个时刻按时间顺序排列表示为0,1,2,3四个时刻;
步骤3.4、若数据关联i对应的时刻为0和1,则两个数据关联属于平行关系,执行步骤3.5;若数据关联i对应的时刻为2和3,则两个数据关联属于平行关系,执行步骤3.5;若数据关联i对应的时刻为0和3,则两个数据关联属于包含关系,执行步骤3.6;若数据关联i对应的时刻为1和2,则两个数据关联属于包含关系,执行步骤3.6;若数据关联i对应的时刻为0和2,则两个数据关联属于交叉关系,执行步骤3.7;若数据关联i对应的时刻为1和3,则两个数据关联属于交叉关系,执行步骤3.7;
步骤3.5、数据关联j提供的数据关联i的置信度Scorei,j表示为:
式中表示连接k到l时刻的数据关联的大小,根据数据关联结果给出,Skl为连接k到l时刻的数据关联的协方差,表示连接k到l时刻的里程计约束的大小,根据惯性导航系统给出,Okl为连接k到l时刻的里程计约束的协方差,由于水下多波束同步定位与建图中数据关联由地形匹配给出,置信度高,所以数据关联的协方差S01和S23均取值为0.0001,而Okl则以k到l时刻以秒为单位的时间间隔表示;
步骤3.6、数据关联j提供的数据关联i的置信度Scorei,j表示为:
步骤3.7、数据关联j提供的数据关联i的置信度Scorei,j表示为:
步骤3.8、计算数据关联i总置信度Scorei并执行步骤4,其中Scorei计算方法为:
一种考虑数据关联间耦合关系的位姿图修正方法,所述步骤4具体包括以下步骤:
步骤4.1、判断i是否等于N,若i等于N,则执行步骤4.2,否则令i=i+1并执行步骤2;
步骤4.2、置信度阈值Score_thre计算方法为:
Score_thre=Scoreave+Scorecov
式中Scoreave为Scorei(i=1,2,...,N)的均值,Scorecov为Scorei(i=1,2,...,N)的标准差。
与现有技术相比,本发明的有益效果是:本发明提出了一种考虑数据关联间耦合关系的位姿图修正方法,该方法只需要输入里程计信息和数据关联,不依赖于其他辅助设备,通过对两两数据关联间耦合关系的计算,不断迭代剔除无效数据关联,实现对水下机器人多波束同步定位与建图中位姿图的准确修正计算,且修正过程独立于后续的图优化阶段,计算效率高,能保证算法实时在线运行。
附图说明
图1是本发明的流程图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述。
本发明公开的是一种考虑数据关联间耦合关系的位姿图修正方法,主要是一种对水下机器人的多波束同步定位与建图的位姿图中数据关联的有效性进行判定的方法。
如附图1所示,是本发明的流程图。主要包括以下步骤:
步骤一、令N为当前位姿图内由地形匹配得到的数据关联个数,令i等于1,执行步骤二。
步骤二、令j等于1,执行步骤三。
步骤三、若j等于i,令j=j+1并执行步骤三,否则判断j是否大于N,若j不大于N,则计算数据关联j给出的数据关联i的置信度Scorei,j,并令j=j+1再次执行步骤三,否则计算数据关联i总置信度Scorei并执行步骤四。步骤三具体为:
步骤3.1、若j等于i,令j=j+1并再次执行步骤3.1,否则执行步骤3.2。
步骤3.2、若j不大于N则执行步骤3.3,否则执行步骤3.8。
步骤3.3、对于数据关联i(i=1,2,...,N)与数据关联j(j=1,2,...,i-1,i+1,...,N),将两个数据关联对应的四个时刻按时间顺序排列分别表示为0,1,2,3四个时刻。
步骤3.4、若数据关联i对应的时刻为0和1或2和3,两个数据关联属于平行关系,执行步骤3.5;若数据关联i对应的时刻为0和3或1和2,两个数据关联属于包含关系,执行步骤3.6;若数据关联i对应的时刻为0和2或1和3,两个数据关联属于交叉关系,执行步骤3.7。步骤3.5、数据关联j提供的数据关联i的置信度Scorei,j表示为:
其中表示连接k到l时刻的数据关联的大小,根据数据关联结果给出,Skl为连接k到l时刻的数据关联的协方差,表示连接k到l时刻的里程计约束的大小,根据惯性导航系统给出,Okl为连接k到l时刻的里程计约束的协方差。由于水下多波束同步定位与建图中数据关联由地形匹配给出,置信度较高,数据关联的协方差S01和S23均取值为0.0001,而Okl则以k到l时刻以秒为单位的时间间隔表示。
步骤3.6、数据关联j提供的数据关联i的置信度Scorei,j表示为:
步骤3.7、数据关联j提供的数据关联i的置信度Scorei,j表示为:
步骤3.8、计算数据关联i总置信度Scorei并执行步骤四,其中Scorei计算方法如下所示。
步骤四、判断i是否等于N,是的话根据当前位姿图内所有数据关联的置信度计算置信度阈值Score_thre并执行步骤五,否则令i=i+1并执行步骤二。步骤四具体为:
步骤4.1、若i等于N,执行步骤4.2,否则令i=i+1并执行步骤二。
步骤4.2、置信度阈值Score_thre计算方法如下所示。
Score_thre=Scoreave+Scorecov
其中Scoreave为Scorei(i=1,2,...,N)的均值,Scorecov为Scorei(i=1,2,...,N)的标准差。
步骤五、判断置信程度最小的数据关联其置信度是否大于置信度阈值Score_thre,若大于置信度阈值,输出当前位姿图并停止算法,若小于或等于阈值,则将该数据关联判定为无效数据关联,将其在当前位姿图中移除并执行步骤一。
综上,本发明公开的是一种考虑数据关联间耦合关系的位姿图修正方法,主要是一种对水下机器人的多波束同步定位与建图的位姿图中数据关联的有效性进行判定的方法。主要步骤包括:步骤一是令N为当前位姿图内由地形匹配得到的数据关联个数,令i等于1,执行步骤二;步骤二是令j等于1,执行步骤三;步骤三是若j等于i,令j=j+1并执行步骤三,否则判断j是否大于N,若j不大于N,则计算数据关联j给出的数据关联i的置信度Scorei,j,并令j=j+1再次执行步骤三,否则计算数据关联i总置信度Scorei并执行步骤四;步骤四是判断i是否等于N,是的话根据当前位姿图内所有数据关联的置信度计算置信度阈值Score_thre并执行步骤五,否则令i=i+1并执行步骤二;步骤五是判断置信程度最小的数据关联其置信度是否大于置信度阈值Score_thre,若大于置信度阈值,输出当前位姿图并停止算法,若小于或等于阈值,则将该数据关联判定为无效数据关联,将其在当前位姿图中移除并执行步骤一。
Claims (1)
1.一种考虑数据关联间耦合关系的位姿图修正方法,其特征在于,具体包括以下步骤:
步骤1、令N为当前位姿图内由地形匹配得到的数据关联个数,令i等于1;
步骤2、令j等于1;
步骤3、判断j是否等于i,若j等于i,令j=j+1,并执行步骤3,否则判断j是否大于N,若j不大于N,则计算数据关联j给出的数据关联i的置信度Scorei,j,并令j=j+1再次执行步骤3,否则计算数据关联i总置信度Scorei并执行步骤4;
步骤3.1、判断j是否等于i,若j等于i,令j=j+1,并执行步骤3.1,否则执行步骤3.2;
步骤3.2、判断j是否大于N,若j不大于N,则执行步骤3.3,否则执行步骤3.8;
步骤3.3、对于数据关联i,i=1、2、...、N,与数据关联j,j=1、2、...、i-1、i+1、...,N,将两个数据关联对应的四个时刻按时间顺序排列表示为0,1,2,3四个时刻;
步骤3.4、若数据关联i对应的时刻为0和1,则两个数据关联属于平行关系,执行步骤3.5;若数据关联i对应的时刻为2和3,则两个数据关联属于平行关系,执行步骤3.5;若数据关联i对应的时刻为0和3,则两个数据关联属于包含关系,执行步骤3.6;若数据关联i对应的时刻为1和2,则两个数据关联属于包含关系,执行步骤3.6;若数据关联i对应的时刻为0和2,则两个数据关联属于交叉关系,执行步骤3.7;若数据关联i对应的时刻为1和3,则两个数据关联属于交叉关系,执行步骤3.7;
步骤3.5、数据关联j提供的数据关联i的置信度Scorei,j表示为:
式中表示连接k到l时刻的数据关联的大小,根据数据关联结果给出,Skl为连接k到l时刻的数据关联的协方差,表示连接k到l时刻的里程计约束的大小,根据惯性导航系统给出,Okl为连接k到l时刻的里程计约束的协方差,由于水下多波束同步定位与建图中数据关联由地形匹配给出,置信度高,所以数据关联的协方差S01和S23均取值为0.0001,而Okl则以k到l时刻以秒为单位的时间间隔表示;
步骤3.6、数据关联j提供的数据关联i的置信度Scorei,j表示为:
步骤3.7、数据关联j提供的数据关联i的置信度Scorei,j表示为:
步骤3.8、计算数据关联i总置信度Scorei并执行步骤4,其中Scorei计算方法为:
步骤4、判断i是否等于N,若是,根据当前位姿图内所有数据关联的置信度计算置信度阈值Score_thre并执行步骤5,否则令i=i+1并执行步骤2;
步骤4.1、判断i是否等于N,若i等于N,则执行步骤4.2,否则令i=i+1并执行步骤2;
步骤4.2、置信度阈值Score_thre计算方法为:
Score_thre=Scoreave+Scorecov
式中Scoreave为Scorei(i=1,2,...,N)的均值,Scorecov为Scorei(i=1,2,...,N)的标准差;
步骤5、判断置信程度最小的数据关联的置信度是否大于置信度阈值Score_thre,若大于置信度阈值,则输出当前位姿图并停止算法,若小于置信度阈值,则将该数据关联判定为无效数据关联,将其在当前位姿图中移除并执行步骤1,若等于置信度阈值,则将该数据关联判定为无效数据关联,将其在当前位姿图中移除并执行步骤1。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910492175.1A CN110260859B (zh) | 2019-06-06 | 2019-06-06 | 一种考虑数据关联间耦合关系的位姿图修正方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910492175.1A CN110260859B (zh) | 2019-06-06 | 2019-06-06 | 一种考虑数据关联间耦合关系的位姿图修正方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110260859A CN110260859A (zh) | 2019-09-20 |
CN110260859B true CN110260859B (zh) | 2022-12-13 |
Family
ID=67917106
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910492175.1A Active CN110260859B (zh) | 2019-06-06 | 2019-06-06 | 一种考虑数据关联间耦合关系的位姿图修正方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110260859B (zh) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
TW201721095A (zh) * | 2015-12-07 | 2017-06-16 | Univ Nat Taiwan Normal | 改良型同時定位及建圖之演算法 |
CN107103355A (zh) * | 2017-04-21 | 2017-08-29 | 北京工业大学 | 一种基于改进人工鱼群算法的智能车slam数据关联方法 |
GB201718507D0 (en) * | 2017-07-31 | 2017-12-27 | Univ Oxford Innovation Ltd | A method of constructing a model of the motion of a mobile device and related systems |
CN109141437A (zh) * | 2018-09-30 | 2019-01-04 | 中国科学院合肥物质科学研究院 | 一种机器人全局重定位方法 |
CN109186610A (zh) * | 2018-10-15 | 2019-01-11 | 哈尔滨工程大学 | 一种auv地形匹配导航的鲁棒bslam方法 |
-
2019
- 2019-06-06 CN CN201910492175.1A patent/CN110260859B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
TW201721095A (zh) * | 2015-12-07 | 2017-06-16 | Univ Nat Taiwan Normal | 改良型同時定位及建圖之演算法 |
CN107103355A (zh) * | 2017-04-21 | 2017-08-29 | 北京工业大学 | 一种基于改进人工鱼群算法的智能车slam数据关联方法 |
GB201718507D0 (en) * | 2017-07-31 | 2017-12-27 | Univ Oxford Innovation Ltd | A method of constructing a model of the motion of a mobile device and related systems |
CN109141437A (zh) * | 2018-09-30 | 2019-01-04 | 中国科学院合肥物质科学研究院 | 一种机器人全局重定位方法 |
CN109186610A (zh) * | 2018-10-15 | 2019-01-11 | 哈尔滨工程大学 | 一种auv地形匹配导航的鲁棒bslam方法 |
Non-Patent Citations (5)
Title |
---|
AUV-SLAM的特征提取与匹配算法研究;郑艳梅;《中国优秀硕士学位论文全文数据库信息科技辑》;20160715;全文 * |
Robust Optimization with Credibilitu Factor for Graph-based SLAM;CHEN,L;《IEEE International Conference on Robotics and Biomimetics(ROBIO)》;20160922;全文 * |
优化的多假设FastSLAM数据关联方法;徐君等;《计算机工程》;20131015(第10期);全文 * |
基于改进RBPF算法的移动机器人SLAM方法研究;陈国军等;《自动化仪表》;20190520(第05期);全文 * |
基于最小生成集和位姿估计的深度图像目标识别;范旭明;《信息通信》;20190415(第04期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN110260859A (zh) | 2019-09-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110349103A (zh) | 一种基于深度神经网络和跳跃连接的无干净标签图像去噪方法 | |
CN112861427A (zh) | 一种基于改进灰狼优化算法的永磁同步电机参数辨识方法 | |
CN110060286B (zh) | 一种单目深度估计方法 | |
CN103258533B (zh) | 远距离语音识别中的模型域补偿新方法 | |
CN112597610B (zh) | 机械臂结构轻量化设计的优化方法、装置及设备 | |
Idestam-Almquist | Generalization under implication by recursive anti-unification | |
CN110260859B (zh) | 一种考虑数据关联间耦合关系的位姿图修正方法 | |
KR20210098395A (ko) | 노이즈 제거 알고리즘 디버깅 방법, 장치 및 전자기기 | |
CN112597993A (zh) | 基于补丁检测的对抗防御模型训练方法 | |
CN114638384A (zh) | 一种基于机器学习的故障诊断方法及系统 | |
CN114708479A (zh) | 一种基于图结构和特征的自适应防御方法 | |
CN116243610B (zh) | 一种数据驱动车辆队列容错跟踪控制追踪方法及系统 | |
CN103440226A (zh) | 基于hmm校正与神经网络延拓的emd端点效应抑制方法 | |
Li et al. | Dynamics of almost periodic mutualism model with time delays | |
US20080147576A1 (en) | Data processing apparatus, data processing method data processing program and computer readable medium | |
CN110190831B (zh) | 一种混合范数非负自适应滤波器 | |
CN116631438A (zh) | 一种基于最小p范数的宽度学习及二次相关的声源定位方法 | |
CN112257484A (zh) | 一种基于深度学习的多声源测向方法及系统 | |
CN114171043B (zh) | 回声的确定方法、装置、设备以及存储介质 | |
CN104835176A (zh) | 一种激光视觉图像的阈值分割方法 | |
CN114841338A (zh) | 训练模型参数的方法、决策确定方法、装置及电子设备 | |
CN110308655B (zh) | 一种基于a3c算法的伺服系统补偿方法 | |
CN109669348B (zh) | 基于迭代反馈调整的工业机器人及其轨迹跟踪控制方法 | |
CN113592013A (zh) | 一种基于图注意力网络的三维点云分类方法 | |
CN114091343A (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 |