CN110763234B - 一种水下机器人海底地形匹配导航路径规划方法 - Google Patents
一种水下机器人海底地形匹配导航路径规划方法 Download PDFInfo
- Publication number
- CN110763234B CN110763234B CN201910975977.8A CN201910975977A CN110763234B CN 110763234 B CN110763234 B CN 110763234B CN 201910975977 A CN201910975977 A CN 201910975977A CN 110763234 B CN110763234 B CN 110763234B
- Authority
- CN
- China
- Prior art keywords
- underwater robot
- terrain
- particle
- calculating
- path planning
- 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/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
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/30—Assessment of water resources
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明属于水下机器人的水下导航领域,具体涉及一种水下机器人海底地形匹配导航路径规划方法。本发明将地形起伏层度考虑为水下机器人状态转移概率的影响因子,可以避免传统海底地形匹配导航路径规划算法由于追求路径总地形起伏最大化导致的路径会经过某些平坦区域的问题,保证了地形匹配算法在规划得到的整条上均能得到较高的匹配精度。本发明通过粒子滤波器实时对水下机器人状态进行跟踪,并根据状态跟踪结果选择最优动作,实现了对水下机器人运动不确定性的考虑。本发明可以使水下机器人应用地形匹配导航的可靠性大大提高。
Description
技术领域
本发明属于水下机器人的水下导航领域,具体涉及一种水下机器人海底地形匹配导航路径规划方法。
背景技术
随着多波束测深技术的发展,高精度海底地形测绘成为可能,因而以多波束声纳测绘得到的海底地形为信息源的海底地形匹配导航技术已经得到了世界各国的广泛重视。然而由于地形匹配精度受海底地形起伏程度影响严重,因而需要对装备海底地形匹配导航系统的水下机器人进行路径规划,使其实现对地形过于平坦区域的避让。同时,由于状态转移噪声等导致的水下机器人运动不确定性,水下机器人在执行任务过程中需要实时对自身位置进行估计并在线规划航线。
目前现有的海底地形匹配导航的路径规划方法多将地形起伏程度作为奖励函数或代价函数考虑。虽然地形起伏程度越高匹配效果越好,但两者之间的关系并非线性的,因而大多数算法所规划的最大化总地形起伏程度的路径并不能获得最好的地形匹配效果,反而会因为前往特定地形起伏极端剧烈区域导致水下机器人不得不经过某些地形平坦区域,并导致地形匹配算法失效。如公开日为2018年11月23日、公开号为CN108871351A、发明名称为“一种AUV海底地形匹配的动态路径规划方法”的专利申请,该方法将路径规划划分为了全局路径规划和局部规划两个阶段,实现了动态避障等过程。但该发明没有考虑水下机器人运动的不确定性,无法实现对水下机器人运动状态的跟踪估计,一旦水下机器人偏离航线很难及时进行补救。
发明内容
本发明的目的在于提供解决海底地形匹配导航的最优路径规划问题的一种水下机器人海底地形匹配导航路径规划方法。
本发明的目的通过如下技术方案来实现:
具体包括以下步骤:
步骤1:将水下机器人作业区域先验海底地形图进行栅格划分;
步骤2:计算每个栅格的值函数;
步骤3:粒子初始化;
步骤4:针对每个粒子,根据水下机器人的控制输入数据和先验地图进行状态更新;
步骤5:为粒子选择最优动作;
步骤6:根据多波束测深数据进行观测更新;
步骤7:计算每个粒子的权重;
步骤8:根据所有粒子对应的权重和最优动作计算水下机器人的控制输入;
步骤9:重采样更新粒子;
步骤10:判断水下机器人是否到达终点;若水下机器人到达终点,则完成路径规划;若水下机器人未到达终点。则执行控制输入后返回步骤4。
本发明还可以包括:
所述的步骤2中计算每个栅格值函数的具体方法为:
步骤2.1:若当前栅格为终点,令其值函数V(x0)为1000,否则针对当前栅格周围的八个栅格,分别计算其地形匹配过程协方差Ψ,计算公式为:
步骤2.2:针对当前栅格周围的八个栅格,分别计算其参考导航协方差Σ,计算公式如下:
其中,σeast为参考导航系统的东向标准差;σnorth为参考导航系统的北向标准差;σeast,north为参考导航系统的东向-北向标准差;
步骤2.3:计算给定控制输入u后水下机器人由当前栅格x0转移到周边其他八个栅格xi(i=1,2,...,8)的状态转移概率p(xi|x0,u)i=1,2,...,8,计算公式为:
步骤2.5:计算当前栅格的值函数V(x0),并记录值函数所对应的水下机器人动作u;所述的当前栅格的值函数V(x0)的计算公式为:
所述的步骤4中针对每个粒子,根据水下机器人的控制输入数据和先验地图进行状态更新具体包括:针对粒子k,其中k=1,2,...,K,利用参考导航系统给出的控制输入u对其位置进行更新,得到xk′=xk+u+Q,其中Q为水下机器人状态转移噪声的协方差。
所述的步骤5中为粒子选择最优动作具体为:针对粒子k在状态转移之后的位置xk′,根据马尔科夫决策过程,以使当前栅格的值函数V(x0)最大的运动输入uk作为其对应最优控制输入。
所述的步骤6中根据多波束测深数据进行观测更新具体为:针对每个粒子,根据步骤4中的状态更新值,提取其实测地图在先验地图中对应的预测结果,并计算当前预测地形的地形粗糙度rough,对于大小为M×N的实测地图,粗糙度rough的计算公式为:
其中rx和ry分别为地形粗糙度在x和y方向的分量,计算公式如下:
其中h(i,j)为先验地图中(i,j)处的地形高程。
所述的步骤7中计算每个粒子的权重的方法具体为:针对每个粒子,根据其预测地形以及多波束声纳获得的测深数据z′(i,j),其中i∈[1,M],j∈[1,N],并计算新息量Δz:
则第k个粒子的权重wk为:
wk=Δzk·roughk
其中Δzk和roughk为该粒子对应的新息量和预测地形粗糙度。
所述的步骤8中根据所有粒子对应的权重和最优动作计算水下机器人的控制输入u的具体计算公式为:
其中K为粒子个数。
本发明的有益效果在于:
本发明将地形起伏层度考虑为水下机器人状态转移概率的影响因子,可以避免传统海底地形匹配导航路径规划算法由于追求路径总地形起伏最大化导致的路径会经过某些平坦区域的问题,保证了地形匹配算法在规划得到的整条上均能得到较高的匹配精度。本发明通过粒子滤波器实时对水下机器人状态进行跟踪,并根据状态跟踪结果选择最优动作,实现了对水下机器人运动不确定性的考虑。本发明可以使水下机器人应用地形匹配导航的可靠性大大提高。
附图说明
图1是本发明的总流程图。
图2是本发明中惩罚函数选择示意图。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明的目的在于提供能够一种水下机器人海底地形匹配导航路径规划方法,该方法通过计算水下机器人的状态转移概率函数对海底地形起伏层度进行评估,利用马尔可夫决策过程选取水下机器人在各个栅格的最优动作,并通过粒子滤波实现对水下机器人状态的跟踪估计和控制输入计算,从而解决海底地形匹配导航的最优路径规划问题。
本发明将影响水下机器人地形匹配导航效果的地形起伏程度作为水下机器人状态转移概率函数的影响因子,利用马尔科夫决策过程进行最优规划;通过粒子滤波实现对水下机器人运动状态的跟踪估计,根据估计结果对路线进行在线规划。本发明将地形起伏程度作为水下机器人状态转移概率函数的影响因子,事实上是搜索了所有节点地形起伏均较为剧烈的路径,从而保证了水下机器人在整条路径上的匹配导航精度。
结合图一,本发明的具体流程如下:
步骤一、将水下机器人作业区域先验海底地形图划分为等大小的栅格。
步骤二、根据海底地形起伏程度和参考导航系统参数,计算各个栅格的值函数并执行步骤三。
步骤2.1、若当前栅格为终点,令其值函数V(x0)为1000,否则针对当前栅格周围的八个栅格,分别计算其地形匹配过程协方差Ψ,计算公式如下
步骤2.2、针对当前栅格周围的八个栅格,分别计算其参考导航协方差Σ,计算公式如下:
其中参考导航系统的东向标准差σeast,北向标准差σnorth,东向-北向标准差σeast,north由参考导航系统参数给出。
步骤2.3、在通过步骤2.1和步骤2.2计算得到栅格xi(i=1,2,...,8)对应的地形匹配过程协方差Ψi和参考导航协方差Σi后,计算给定控制输入u后水下机器人由当前栅格x0转移到周边其他八个栅格xi(i=1,2,...,8)的状态转移概率p(xi|x0,u)(i=1,2,...,8),计算公式为:
步骤2.4、考虑水下机器人经过地形不明显区域的惩罚,结合图2可以看到,当匹配过程协方差Ψi和参考导航协方差Σi之和越大,对应概率分布越扁平,概率小于0.05的区域越大,因而令
步骤2.5、计算当前栅格的值函数,方程为:
并记录值函数所对应的水下机器人动作u。
步骤三、进行粒子初始化,执行步骤四。
步骤四、针对每个粒子,根据水下机器人的参考导航数据进行状态更新,并利用马尔可夫决策过程求解当前粒子的最优动作,执行步骤五。
步骤4.1、针对粒子k(k=1,2,...,K),利用参考导航系统给出的控制输入u对其位置进行更新,得到xk′=xk+u+Q,其中Q为水下机器人状态转移噪声的协方差。
步骤4.2、针对粒子k在状态转移之后的位置xk′,根据马尔科夫决策过程,以在步骤2.5中给出的其值函数最大的运动输入uk作为其对应最优控制输入。
步骤五、利用多波束声纳观测值进行观测更新,根据每个粒子当前状态的先验地形和多波束声纳观测值计算各个粒子权重,执行步骤六。
步骤5.1、针对每个粒子,根据步骤4.2给出的状态更新值x′,提取其实测地图在先验地图中对应的预测结果,并计算当前预测地形的地形粗糙度rough,对于大小为M×N的实测地图,其计算公式为:
其中rx和ry分别为地形粗糙度在x和y方向的分量,计算公式如下:
其中h(i,j)为先验地图中(i,j)处的地形高程。
步骤5.2、针对每个粒子,根据其预测地形以及多波束声纳获得的测深数据z′(i,j)(i∈[1,M],j∈[1,N]),并计算新息量Δz:
步骤5.3、针对第k个粒子(k=1,2,...,K),计算其权重wk:
wk=Δzk·roughk
其中Δzk和roughk为该粒子对应的新息量和预测地形粗糙度。
步骤六、根据所有粒子对应的权重和最优动作计算水下机器人的控制输入u:
其中K为粒子个数。
步骤七、重采样更新粒子。
步骤八、判断水下机器人是否到达终点,是则算法结束;否则水下机器人执行控制输入后算法执行步骤四。
本发明的有益效果在于提出的海底地形匹配导航路径规划方法将地形起伏层度考虑为水下机器人状态转移概率的影响因子,可以避免传统海底地形匹配导航路径规划算法由于追求路径总地形起伏最大化导致的路径会经过某些平坦区域的问题,保证了地形匹配算法在规划得到的整条上均能得到较高的匹配精度;同时,本发明通过粒子滤波器实时对水下机器人状态进行跟踪,并根据状态跟踪结果选择最优动作,实现了对水下机器人运动不确定性的考虑。以上两点有益效果可使水下机器人应用地形匹配导航的可靠性大大提高。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (6)
1.一种水下机器人海底地形匹配导航路径规划方法,其特征在于,具体包括以下步骤:
步骤1:将水下机器人作业区域先验海底地形图进行栅格划分;
步骤2:计算每个栅格的值函数;
步骤2.1:若当前栅格为终点,令其值函数V(x0)为1000,否则针对当前栅格周围的八个栅格,分别计算其地形匹配过程协方差Ψ,计算公式为:
步骤2.2:针对当前栅格周围的八个栅格,分别计算其参考导航协方差∑,计算公式如下:
其中,σeast为参考导航系统的东向标准差;σnorth为参考导航系统的北向标准差;σeast,north为参考导航系统的东向-北向标准差;
步骤2.3:计算给定控制输入u后水下机器人由当前栅格x0转移到周边其他八个栅格xi(i=1,2,...,8)的状态转移概率p(xi|x0,u)i=1,2,...,8,计算公式为:
步骤2.5:计算当前栅格的值函数V(x0),并记录值函数所对应的水下机器人动作u;所述的当前栅格的值函数V(x0)的计算公式为:
步骤3:粒子初始化;
步骤4:针对每个粒子,根据水下机器人的控制输入数据和先验地图进行状态更新;
步骤5:为粒子选择最优动作;
步骤6:根据多波束测深数据进行观测更新;
步骤7:计算每个粒子的权重;
步骤8:根据所有粒子对应的权重和最优动作计算水下机器人的控制输入;
步骤9:重采样更新粒子;
步骤10:判断水下机器人是否到达终点;若水下机器人到达终点,则完成路径规划;若水下机器人未到达终点,则执行控制输入后返回步骤4。
2.根据权利要求1所述的一种水下机器人海底地形匹配导航路径规划方法,其特征在于:所述的步骤4中针对每个粒子,根据水下机器人的控制输入数据和先验地图进行状态更新具体包括:针对粒子k,其中k=1,2,...,K,利用参考导航系统给出的控制输入u对其位置进行更新,得到xk′=xk+u+Q,其中Q为水下机器人状态转移噪声的协方差。
3.根据权利要求2所述的一种水下机器人海底地形匹配导航路径规划方法,其特征在于:所述的步骤5中为粒子选择最优动作具体为:针对粒子k在状态转移之后的位置xk′,根据马尔科夫决策过程,以使当前栅格的值函数V(x0)最大的运动输入uk作为其对应最优控制输入。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910975977.8A CN110763234B (zh) | 2019-10-15 | 2019-10-15 | 一种水下机器人海底地形匹配导航路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910975977.8A CN110763234B (zh) | 2019-10-15 | 2019-10-15 | 一种水下机器人海底地形匹配导航路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110763234A CN110763234A (zh) | 2020-02-07 |
CN110763234B true CN110763234B (zh) | 2022-10-28 |
Family
ID=69331142
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910975977.8A Active CN110763234B (zh) | 2019-10-15 | 2019-10-15 | 一种水下机器人海底地形匹配导航路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110763234B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113537628B (zh) * | 2021-08-04 | 2023-08-22 | 郭宏亮 | 一种基于分布强化学习的通用可靠最短路方法 |
CN115182747B (zh) * | 2022-09-13 | 2023-02-03 | 湖南大学 | 隧道裂缝自动修复方法、装置、系统及可读存储介质 |
Family Cites Families (27)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2011163143A1 (en) * | 2010-06-21 | 2011-12-29 | Optimal Ranging, Inc. | Uav power line position and load parameter estimation |
CN103090861B (zh) * | 2013-01-14 | 2015-08-12 | 哈尔滨工程大学 | 水下机器人的多线地形匹配导航方法 |
CN104075715B (zh) * | 2014-07-07 | 2017-09-01 | 东南大学 | 一种结合地形和环境特征的水下导航定位方法 |
GB2541220A (en) * | 2015-08-12 | 2017-02-15 | Atlantic Inertial Systems Ltd | Inertial sensor |
CN105115492B (zh) * | 2015-08-14 | 2017-10-24 | 武汉大学 | 基于声学多普勒计程仪的水下地形匹配导航系统 |
CN105424036B (zh) * | 2015-11-09 | 2018-02-13 | 东南大学 | 一种低成本水下潜器地形辅助惯性组合导航定位方法 |
JP6761216B2 (ja) * | 2015-12-09 | 2020-09-23 | 国立研究開発法人 海上・港湾・航空技術研究所 | 水中航走体の経路設定方法、それを用いた水中航走体の最適制御方法及び水中航走体並びに移動体の経路設定方法 |
CN106123850B (zh) * | 2016-06-28 | 2018-07-06 | 哈尔滨工程大学 | Auv配载多波束声呐水下地形测绘修正方法 |
CN106403953B (zh) * | 2016-09-05 | 2019-07-16 | 江苏科技大学 | 一种用于水下自主导航与定位的方法 |
CN106767834B (zh) * | 2017-01-24 | 2019-11-08 | 哈尔滨工程大学 | 一种基于模糊熵值的auv水下地形匹配适配区划分方法 |
CN106767836B (zh) * | 2017-02-17 | 2019-12-06 | 哈尔滨工程大学 | 一种auv地形匹配导航滤波方法 |
CN106885576B (zh) * | 2017-02-22 | 2020-02-14 | 哈尔滨工程大学 | 一种基于多点地形匹配定位的auv航迹偏差估计方法 |
CN107314768B (zh) * | 2017-07-06 | 2020-06-09 | 上海海洋大学 | 水下地形匹配辅助惯性导航定位方法及其定位系统 |
CN107589748A (zh) * | 2017-08-21 | 2018-01-16 | 江苏科技大学 | 基于UnscentedFastSLAM算法的AUV自主导航方法 |
CN107643082B (zh) * | 2017-09-05 | 2020-03-31 | 东南大学 | 基于多波束的多路径并行iccp水下地形匹配方法 |
CN108562287A (zh) * | 2018-01-08 | 2018-09-21 | 哈尔滨工程大学 | 一种基于自适应采样粒子滤波的水下地形辅助导航方法 |
US10809064B2 (en) * | 2018-02-08 | 2020-10-20 | Raytheon Company | Image geo-registration for absolute navigation aiding using uncertainy information from the on-board navigation system |
CN108489491A (zh) * | 2018-02-09 | 2018-09-04 | 上海交通大学 | 一种水下自主航行器的三维航迹智能规划方法 |
CN108592916B (zh) * | 2018-04-20 | 2020-08-07 | 杭州电子科技大学 | 悬浮式水下自主航行器的多航次地图定位和导航方法 |
CN108803313B (zh) * | 2018-06-08 | 2022-07-12 | 哈尔滨工程大学 | 一种基于海流预测模型的路径规划方法 |
CN109000656B (zh) * | 2018-06-15 | 2020-08-04 | 淮海工学院 | 基于空间聚类的水下地形匹配导航适配区选择方法 |
CN108871351B (zh) * | 2018-08-02 | 2020-12-08 | 哈尔滨工程大学 | 一种auv海底地形匹配的动态路径规划方法 |
CN109186610B (zh) * | 2018-10-15 | 2020-12-08 | 哈尔滨工程大学 | 一种auv地形匹配导航的鲁棒bslam方法 |
CN110220510B (zh) * | 2019-06-03 | 2023-03-31 | 哈尔滨工程大学 | 一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法 |
CN110244759B (zh) * | 2019-06-05 | 2022-06-21 | 哈尔滨工程大学 | 一种基于区间优化的水下机器人时间最优路径规划方法 |
CN110207721B (zh) * | 2019-06-06 | 2022-06-21 | 哈尔滨工程大学 | 一种考虑残差分布的无效地形匹配结果识别方法 |
CN110320520B (zh) * | 2019-06-26 | 2022-08-02 | 哈尔滨工程大学 | 一种测深信息同步定位与建图的鲁棒后端图优化方法 |
-
2019
- 2019-10-15 CN CN201910975977.8A patent/CN110763234B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN110763234A (zh) | 2020-02-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108871351B (zh) | 一种auv海底地形匹配的动态路径规划方法 | |
CN108759833B (zh) | 一种基于先验地图的智能车辆定位方法 | |
CN107356254B (zh) | 适用于地磁辅助导航航迹规划的粒子群优化方法 | |
CN109597864B (zh) | 椭球边界卡尔曼滤波的即时定位与地图构建方法及系统 | |
CN106885576B (zh) | 一种基于多点地形匹配定位的auv航迹偏差估计方法 | |
CN110726415B (zh) | 一种自适应的水下多波束同步定位与建图方法 | |
CN103776453A (zh) | 一种多模型水下航行器组合导航滤波方法 | |
CN110763234B (zh) | 一种水下机器人海底地形匹配导航路径规划方法 | |
Lu et al. | Lane marking based vehicle localization using particle filter and multi-kernel estimation | |
CN110487276B (zh) | 一种基于相关分析的采样矢量匹配定位方法 | |
CN110954132A (zh) | Grnn辅助自适应卡尔曼滤波进行导航故障识别的方法 | |
Guivant | Efficient simultaneous localization and mapping in large environments | |
CN110132281B (zh) | 一种基于询问应答模式的水下高速目标高精度自主声学导航方法 | |
Wang et al. | Matching error of the iterative closest contour point algorithm for terrain-aided navigation | |
CN110220510A (zh) | 一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法 | |
Das et al. | An experimental study on relative and absolute pose graph fusion for vehicle localization | |
Bai et al. | A Robust Generalized $ t $ Distribution-Based Kalman Filter | |
CN110207721B (zh) | 一种考虑残差分布的无效地形匹配结果识别方法 | |
CN109656136B (zh) | 基于声学测量网络的水下多auv协同定位编队拓扑结构优化方法 | |
CN114088098A (zh) | 一种用于极区水下航行器数据库辅助导航路径规划方法 | |
CN113325867A (zh) | 一种无人航行器搜寻的路径规划方法、装置和无人航行器 | |
CN113008235B (zh) | 基于矩阵k-l散度的多源导航信息融合方法 | |
Gade et al. | Probability distribution for association of maneuvering vehicles | |
Ma et al. | A robust fusion terrain-aided navigation method with a single Beam Echo Sounder | |
Dong et al. | Indoor robot localization combining feature clustering with wireless sensor network |
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 |