CN112666972B - 一种基于距离和角度加权的自主水下航行器避障方法 - Google Patents
一种基于距离和角度加权的自主水下航行器避障方法 Download PDFInfo
- Publication number
- CN112666972B CN112666972B CN202011480961.9A CN202011480961A CN112666972B CN 112666972 B CN112666972 B CN 112666972B CN 202011480961 A CN202011480961 A CN 202011480961A CN 112666972 B CN112666972 B CN 112666972B
- Authority
- CN
- China
- Prior art keywords
- underwater vehicle
- autonomous underwater
- angle
- obstacle
- distance
- 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
Abstract
本发明涉及一种基于距离和角度加权的自主水下航行器避障方法，将直线跟踪转化为对航路点的跟踪，并对航路过程中遇到的障碍物进行躲避行为，保证自主水下航行器能安全可靠地到达目标点。通过对加权函数的增益因子进行优化，使得自主水下航行器以指定角度避开障碍物，避免过早的开始回避行为，并且不会以急剧变化的角度返回路径跟踪。通过对加权函数的设置，简化计算复杂性，大大提高控制性能。
Description
技术领域
本发明涉及一种自主水下航行器的控制方法，更具体地说是在路径跟踪时躲避障碍物的方法。
背景技术
在过去的几十年里，机器人技术在水下环境中的应用迅速发展，他们可以在水下进行搜索、探测和侦查等任务，为了确保能安全可靠地完成任务，所用自主式水下航行器在行驶过程中必须能够避开障碍物，这就是我们针对的避障问题。
避障问题是自主水下航行器研究过程中的关键课题，由于被控对象的非线性、耦合性和复杂环境干扰等特性，自主水下航行器避障问题的复杂度大大提升。目前为止，水下航行器的避障方法主要有：人工势场法、栅格法和快速扩展随机树法等。人工势场法存在着局部极小值点的问题；栅格法的缺点在于，栅格越小生成的路径越精细但是计算量大大增加，栅格越大则导致生成的路径不平滑，甚至可能导致无可行路径；快速扩展随机树法虽然可以解决人工势场法存在的局部极小值问题，但是算法收敛速度很慢，使得效率大幅下降。
模糊逻辑算法是仿照驾驶员的思维建立关于躲避障碍物的规则，通过传感器获取实时环境信息后，将人类思维转换成控制信号，对于障碍物的信息精确度要求不高，适合水下航行器的环境特性，但是在自主水下航行器避障方面的应用并不多。这种方法既不需要处理势场法产生的局部极小值问题，也无需考虑算法收敛性，并且在本发明中简化了控制规则的设定，计算量大大减少。
发明内容
本发明解决的技术问题是：基于模糊逻辑的加权算法求得加权因子，实现自主水下航行器在路径跟踪过程中对障碍物的躲避。
本发明的技术方案是：一种基于距离和角度加权的自主水下航行器避障方法，包括以下步骤：
步骤1：通过自主水下航行器搭载的传感器，获得当前自主水下航行器的位置坐标(X,Y)和偏航角ψcur，通过声呐获取障碍物的位置(Xobs，Yobs)；
步骤2：根据步骤1得到的自主水下航行器的当前位置与能探测到的每个障碍物的位置计算自主水下航行器和障碍物之间的距离Dis(i,c)和二者之间的角度Angle(i,c):
步骤3：通过设置若干个航路点(Xway,Yway)，计算跟踪角ψtrack：
其中(Xway,Yway)为下一个经过的航路点，(Xpre.way,Ypre.way)为上一个经过的航路点；
步骤4：模糊逻辑思想，对步骤2中的角度和距离分别设置了不同的权重计算规则，最终得到避障行为的增益因子表达式如下：
ω(i,c)＝ω1(i,c)×ω2(i,c)
i是表示每一时刻，c表示障碍物；a表示自主水下航行器距离障碍物最小的安全距离，b表示自主水下航行器遇到障碍物后开始做出反应的距离；
步骤5:根据步骤4得到的避障行为增益因子的公式，得到自主水下航行器为了避障要转的角度ψoa：
其中，ψmax＝π/2，是自主水下航行器的最大偏航角；
步骤6；计算出指令方向与实际航向之间的差值ψerror并作用于舵：
ψerror(i)＝ψtrack(i)+ψoa(i)-ψcur(i)
其中，ψtrack(i)+ψoa(i)为指令方向，ψcur(i)为当前时刻的实际航向；
步骤7：计算自主水下航行器当前位置与当前要抵达的航路点之间的距离S：
若S小于设定的监视半径WR，则表示完成当前的路径跟踪任务，此时可停止航行或者切换下一段期望轨迹；否则返回步骤2继续循环执行。
本发明进一步的技术方案是：所述步骤4中的角度的权重计算规则为ω1(i,c)，具体表达式为：
本发明进一步的技术方案是：所述步骤4中的距离权重计算规则为：
发明效果
本发明的技术效果在于：过本发明所构思的以上技术方案与现有技术相比，主要具备以下的技术优点如下：
1.本发明将直线跟踪转化为对航路点的跟踪(步骤3对ψtrack的计算)，并对航路过程中遇到的障碍物进行躲避行为，保证自主水下航行器能安全可靠地到达目标点。
2.通过对加权函数的增益因子进行优化(步骤4中ω(i,c)的计算)，使得自主水下航行器以指定角度避开障碍物，避免过早的开始回避行为，并且不会以急剧变化的角度返回路径跟踪。
3.通过对加权函数的设置(步骤4中ω1(i,c)和ω2(i,c)函数的设置)，简化计算复杂性，大大提高控制性能。
附图说明
图1避障系统流程图；
图2角度的权重ω1函数；
图3距离的权重ω2函数。其中8表示自主水下航行器距离障碍物最小的安全距离，25表示自主水下航行器遇到障碍物25米后需要开始做出反应；
图4自主水下航行器路径示意图；
图5自主水下航行器避障结果。
具体实施方式
在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“顺时针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
本发明的目的在于提出一种基于距离和角度的加权的水下航行器避障方法，旨在提高轨迹跟踪的控制性能，增强避障精准性，减少计算复杂度。
本发明的实现步骤如下：
步骤1.初始化，通过自主水下航行器搭载的传感器，获得当前自主水下航行器的位置坐标和偏航角等信息，通过声呐获取障碍物的位置；
步骤2.通过计算自主水下航行器当前航路点与上一个航路点的坐标差值，求得跟踪角ψtrack；
步骤3.通过自主水下航行器当前位置与障碍物之间的距离和角度，计算获得一个权重值，进一步计算得到为了避障要转的角度ψoa；
步骤4.基于以上步骤，计算出指令方向与实际航向之间的差值ψerror；
步骤5.根据所用自主水下航行器特性推导出水下航行器的舵的控制信号；
步骤6.计算自主水下航行器当前位置与当前航路点之间的距离，若小于设定的监视半径WR，则表示完成当前的路径跟踪任务，则可停止航行或者切换下一段期望轨迹，否则返回步骤2继续循环执行。
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合流程图(附图1)和实例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实例仅用以解释本发明，并不限定用于本发明。
步骤1中，通过传感器获得自主水下航行器当前的位置坐标(X,Y)和偏航角ψcur。
根据设置的航路点(Xway,Yway)计算跟踪角ψtrack，具体形式：
根据自主水下航行器当前位置与能探测到的每个障碍物的信息计算距离Dis(i,c)和角度Angle(i,c):
依照模糊逻辑思想，对角度和距离分别设置了不同的权重计算规则，如附图2和附图3，表示为公式4和5，计算得到一个权重值ω：
ω(i,c)＝ω1(i,c)×ω2(i,c) (6)
在ω2中，a表示距离障碍物最小的安全距离，b表示开始避障反应的距离，在本实例中，a＝8米，b＝25米。
进一步计算得到为了避障要转的角度ψoa：
其中，ψmax＝π/2是自主水下航行器的最大偏航角。
基于以上步骤，计算出指令方向与实际航向之间的差值ψerror并作用于舵：
ψerror(i)＝ψtrack(i)+ψoa(i)-ψcur(i) (8)
计算自主水下航行器当前位置与当前要抵达的航路点之间的距离S：
若S小于设定的监视半径WR，则表示完成当前的路径跟踪任务，则可停止航行或者切换下一段期望轨迹，否则继续循环执行。
仿真分析
如图4所示，设置了要跟踪的路径，即在竖直方向上，从0米到280米的直线；图5是仿真结果图，圆圈是障碍物信息，圆心位置分别是(80，0)(150,0)和(220,0)，半径分别为5.5米，2米和3.5米，曲线条代表自主水下航行器实际的路线，可以看出自主水下航行器能从0米起点到达280米的终点位置，且避开了途中经过的三个大小不一的障碍物，说明本方法的可行性和正确性。
Claims (1)
1.一种基于距离和角度加权的自主水下航行器避障方法，其特征在于，包括以下步骤：
步骤1：通过自主水下航行器搭载的传感器，获得当前自主水下航行器的位置坐标(X,Y)和偏航角ψcur，通过声呐获取障碍物的位置(Xobs，Yobs)；
步骤2：根据步骤1得到的自主水下航行器的当前位置与能探测到的每个障碍物的位置计算自主水下航行器和障碍物之间的距离Dis(i,c)和二者之间的角度Angle(i,c):
步骤3：通过设置若干个航路点(Xway,Yway)，计算跟踪角ψtrack：
其中(Xway,Yway)为下一个经过的航路点，(Xpre.way,Ypre.way)为上一个经过的航路点；
步骤4：模糊逻辑思想，对步骤2中的角度和距离分别设置了不同的权重计算规则，最终得到避障行为的增益因子表达式如下：
ω(i,c)＝ω1(i,c)×ω2(i,c)
i是表示每一时刻，c表示障碍物；a表示自主水下航行器距离障碍物最小的安全距离，b表示自主水下航行器遇到障碍物后开始做出反应的距离；
步骤5:根据步骤4得到的避障行为增益因子的公式，得到自主水下航行器为了避障要转的角度ψoa：
其中，ψmax＝π/2，是自主水下航行器的最大偏航角；
步骤6；计算出指令方向与实际航向之间的差值ψerror并作用于舵：
ψerror(i)＝ψtrack(i)+ψoa(i)-ψcur(i)
其中，ψtrack(i)+ψoa(i)为指令方向，ψcur(i)为当前时刻的实际航向；
步骤7：计算自主水下航行器当前位置与当前要抵达的航路点之间的距离S：
若S小于设定的监视半径WR，则表示完成当前的路径跟踪任务，此时可停止航行或者切换下一段期望轨迹；否则返回步骤2继续循环执行；
所述步骤4中的角度的权重计算规则为ω1(i,c)，具体表达式为：
所述步骤4中的距离权重计算规则为：
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011480961.9A CN112666972B (zh) | 2020-12-15 | 2020-12-15 | 一种基于距离和角度加权的自主水下航行器避障方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011480961.9A CN112666972B (zh) | 2020-12-15 | 2020-12-15 | 一种基于距离和角度加权的自主水下航行器避障方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112666972A CN112666972A (zh) | 2021-04-16 |
| CN112666972B true CN112666972B (zh) | 2022-07-05 |
Family
ID=75404951
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011480961.9A Active CN112666972B (zh) | 2020-12-15 | 2020-12-15 | 一种基于距离和角度加权的自主水下航行器避障方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112666972B (zh) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102621988A (zh) * | 2012-04-10 | 2012-08-01 | 西北工业大学 | 一种水下航行器的避障方法 |
| KR20120129002A (ko) * | 2011-05-18 | 2012-11-28 | 부산대학교 산학협력단 | 수중로봇과 그 제어방법 |
| CN104571128A (zh) * | 2014-12-26 | 2015-04-29 | 燕山大学 | 基于距离与视差信息的水下机器人避障方法 |
| CN105807769A (zh) * | 2016-03-09 | 2016-07-27 | 哈尔滨工程大学 | 无人水下航行器ivfh避碰方法 |
| CN107239075A (zh) * | 2017-04-19 | 2017-10-10 | 哈尔滨工程大学 | 自主水下航行器平面直线航线跟踪的智能s面制导控制方法 |
| JP2018181166A (ja) * | 2017-04-20 | 2018-11-15 | 株式会社Ihi | 移動体の制御方法および移動体制御システム |
| CN108829134A (zh) * | 2018-07-03 | 2018-11-16 | 中国船舶重工集团公司第七〇九研究所 | 一种深海机器人实时自主避障方法 |
| CN109725650A (zh) * | 2019-03-08 | 2019-05-07 | 哈尔滨工程大学 | 一种密集障碍物环境下的auv避障方法 |
| CN109782807A (zh) * | 2019-03-08 | 2019-05-21 | 哈尔滨工程大学 | 一种回形障碍物环境下的auv避障方法 |
-
2020
- 2020-12-15 CN CN202011480961.9A patent/CN112666972B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20120129002A (ko) * | 2011-05-18 | 2012-11-28 | 부산대학교 산학협력단 | 수중로봇과 그 제어방법 |
| CN102621988A (zh) * | 2012-04-10 | 2012-08-01 | 西北工业大学 | 一种水下航行器的避障方法 |
| CN104571128A (zh) * | 2014-12-26 | 2015-04-29 | 燕山大学 | 基于距离与视差信息的水下机器人避障方法 |
| CN105807769A (zh) * | 2016-03-09 | 2016-07-27 | 哈尔滨工程大学 | 无人水下航行器ivfh避碰方法 |
| CN107239075A (zh) * | 2017-04-19 | 2017-10-10 | 哈尔滨工程大学 | 自主水下航行器平面直线航线跟踪的智能s面制导控制方法 |
| JP2018181166A (ja) * | 2017-04-20 | 2018-11-15 | 株式会社Ihi | 移動体の制御方法および移動体制御システム |
| CN108829134A (zh) * | 2018-07-03 | 2018-11-16 | 中国船舶重工集团公司第七〇九研究所 | 一种深海机器人实时自主避障方法 |
| CN109725650A (zh) * | 2019-03-08 | 2019-05-07 | 哈尔滨工程大学 | 一种密集障碍物环境下的auv避障方法 |
| CN109782807A (zh) * | 2019-03-08 | 2019-05-21 | 哈尔滨工程大学 | 一种回形障碍物环境下的auv避障方法 |
Non-Patent Citations (3)
| Title |
|---|
| Farman Ali，等.Type-2 fuzzy ontology-based semantic knowledge for collision avoidance of autonomous underwater vehicles.《Information Sciences》.2015,第295卷第441-464页. * |
| 郭银景，等.AUV实时避障算法研究进展.《水下无人系统》.2020,第28卷(第4期),第351-358页. * |
| 高剑，等.基于行为的自主水下航行器无碰路径跟踪控制.《大连海事大学学报》.2012,第38卷(第4期),第30-34页. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112666972A (zh) | 2021-04-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108762264B (zh) | 基于人工势场与滚动窗口的机器人的动态避障方法 | |
| CN109241552B (zh) | 一种基于多约束目标的水下机器人运动规划方法 | |
| JP4467534B2 (ja) | 障害物のある環境下で自律移動する移動ロボットおよび移動ロボットの制御方法。 | |
| Lacaze et al. | Path planning for autonomous vehicles driving over rough terrain | |
| CN107037809A (zh) | 一种基于改进蚁群算法的无人艇避碰方法 | |
| CN108628326B (zh) | 一种智能水下机器人运动重规划策略 | |
| CN109947123A (zh) | 一种基于视线导引律的无人机路径跟踪与自主避障方法 | |
| KR101133037B1 (ko) | 자율이동차량용 충돌회피를 위한 경로갱신방법 및 그 장치 | |
| KR20090061298A (ko) | 이동 로봇의 경로 계획 방법 및 장치 | |
| CN112141091B (zh) | 解决车位偏移和定位偏移的二次泊车方法、系统及车辆 | |
| CN114200945B (zh) | 一种移动机器人的安全控制方法 | |
| CN104571128A (zh) | 基于距离与视差信息的水下机器人避障方法 | |
| CN113093742B (zh) | 一种自动规避多障碍物的无人船路径跟踪系统 | |
| CN113010958A (zh) | 自航船舶的模拟系统及其运作方法 | |
| CN113625702B (zh) | 基于二次规划的无人车同时路径跟踪与避障方法 | |
| JP4467533B2 (ja) | 折線追従移動ロボットおよび折線追従移動ロボットの制御方法 | |
| CN115167440A (zh) | 一种基于虚拟领航-领航-跟随的多机器人编队控制方法 | |
| CN108801262B (zh) | 一种船舶自动航行控制器航路规划与纠偏修正方法 | |
| Oetiker et al. | A navigation-field-based semi-autonomous nonholonomic vehicle-parking assistant | |
| CN112666972B (zh) | 一种基于距离和角度加权的自主水下航行器避障方法 | |
| Nakamura et al. | Path Planning for Mobile Robot Considering Turnabouts on Narrow Road by Deep Q-Network | |
| CN113341999A (zh) | 一种基于优化d*算法的叉车路径规划方法及装置 | |
| CN115993089B (zh) | 基于pl-icp的在线四舵轮agv内外参标定方法 | |
| CN116009558A (zh) | 一种结合运动学约束的移动机器人路径规划方法 | |
| CN111694367A (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 |