CN105182975B - 一种基于改进的人工协调场的气垫船避障方法 - Google Patents
一种基于改进的人工协调场的气垫船避障方法 Download PDFInfo
- Publication number
- CN105182975B CN105182975B CN201510593659.7A CN201510593659A CN105182975B CN 105182975 B CN105182975 B CN 105182975B CN 201510593659 A CN201510593659 A CN 201510593659A CN 105182975 B CN105182975 B CN 105182975B
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- aircushion vehicle
- mtd
- repulsion
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 10
- 230000002153 concerted effect Effects 0.000 claims abstract description 15
- 230000001133 acceleration Effects 0.000 claims abstract description 5
- 238000004364 calculation method Methods 0.000 claims abstract description 3
- 230000004888 barrier function Effects 0.000 claims description 6
- 230000000694 effects Effects 0.000 claims description 2
- 239000011159 matrix material Substances 0.000 claims description 2
- 230000002265 prevention Effects 0.000 abstract description 6
- 230000007423 decrease Effects 0.000 abstract 1
- 230000006872 improvement Effects 0.000 description 2
- 206010044565 Tremor Diseases 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000001149 cognitive effect Effects 0.000 description 1
- 238000012938 design process Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Landscapes
- Traffic Control Systems (AREA)
Abstract
本发明提供的是一种基于改进的人工协调场的气垫船避障方法。(1)获取状态信息、目标点位置以及障碍物位置；(2)设计PID控制律，代替人工协调场中的吸引力；(3)计算风险度，若风险度大于设定的风险度则进入步骤(4)，否则进入步骤(6)；(4)设计斥力场，并利用最大加速度约束设计斥力场的场强参数，进而计算出斥力；(5)根据斥力计算出协调力；(6)算出大地坐标下的合力，将合力的方向作为期望艏向设计艏向PID控制律，将合力的大小作为纵向控制力对气垫船进行控制。本发明减小了传统吸引力只与位置有关而造成的气垫船在目标点附近的抖动，实现了有约束的避碰，能够在气垫船的性能范围内实现安全避障，具有很强的实用价值。
Description
技术领域
本发明涉及的是一种气垫船避障方法，特别是基于改进的人工协调场的气垫船避障方法。
背景技术
气垫船在海上航行过程中不可避免的会遇到障碍物，由于气垫船的速度较大，因此发生碰撞的概率增大。针对气垫船手操模式，驾驶人员要想控制船成功避开障碍物需要经历信息采集、信息分析、路径规划以及控船等几个步骤，由于气垫船速度很大留给操作人员的决策时间有限，这给驾驶人员很大的工作压力。
国内外关于水面船的避碰控制研究大多集中在专家避碰辅助系统，利用专家的经验对当前的避碰局面给出指导，但是专家系统存在以下缺点：第一，海上情况复杂多变，对于每只船(即便是完全相同的船)都存在各自特殊的问题，需要适合的避碰方案来解决问题，所以对于经验总结的方案来说是无法做到完全适合的；第二，由于每个驾驶员对于海上航行的知识认知不同，无法与总结经验的科学家的认知完全一样，所以这些方案不但无法对驾驶员产生帮助，反而可能会对其产生误导。另外还有一些学者利用电子海图提前对路径规划实现避障。但由于路径规划需要进行在线寻优，算法复杂，并不适合对实时性要求比较高的气垫船。
人工协调场作为一种改进的人工势场，应用于船舶避碰具有计算简单，实时性好，避免了局部极小值，但容易在目标点附近振荡，并且没有考虑到气垫船的运动约束。
发明内容
本发明的目的在于提供一种在考虑气垫船运动约束下实现避障，同时减小气垫船的抖动，提高操气垫船航行安全性的基于改进的人工协调场的气垫船避障方法。
本发明的目的是这样实现的：
(1)获取气垫船状态信息、目标点位置以及障碍物位置；
(2)根据气垫船状态和目标点位置设计PID控制律，代替人工协调场中的吸引力；
(3)根据气垫船状态信息和障碍物位置计算风险度，若风险度大于设定的风险度则进入步骤(4)，否则进入步骤(6)；
(4)设计斥力场，并利用气垫船的最大加速度约束设计斥力场的场强参数，进而计算出斥力；
(5)根据斥力计算出协调力；
(6)将吸引力、排斥力以及协调力合成，算出大地坐标下的合力，然后将所述合力的方向作为气垫船的期望艏向设计艏向PID控制律，将所述合力的大小作为气垫船的纵向控制力对气垫船进行控制。
人工协调场作为一种改进的人工势场，应用于船舶避碰具有计算简单，实时性好，避免了局部极小值，但容易在目标点附近振荡，并且没有考虑到气垫船的运动约束。本发明提出了一种基于改进的人工协调场的气垫船避障方法则很好的解决了这些问题，目的在于在考虑气垫船运动约束下实现避障，同时减小气垫船的抖动，提高操气垫船航行安全性。
本发明相对现有技术具有如下的优点及效果：
人工协调场中的吸引力通过PID控制律产生，由于该控制律将位置偏差、速度以及偏差积分考虑在内，克服了以往吸引力只依赖于位置信息造成的气垫船在目标点附近的抖动。排斥力中的场强参数是根据气垫船的最大加速度设计的，因此协调场的设计考虑了气垫船的运动约束。最后通过转换坐标将水平面协调场的合力转换为气垫船的纵向力和回转力矩，解决了气垫船横向欠驱动的问题。
本发明通过PID控制率来改进人工协调场中的吸引力，减小了传统吸引力只与位置有关而造成的气垫船在目标点附近的抖动，将排斥力与气垫船的运动约束结合在一起实现了有约束的避碰。另外，协调力只根据斥力的大小和方向确定，不需要再设计场强参数，简化了设计过程。本发明能够在气垫船的性能范围内实现安全避障，具有很强的实用价值。
附图说明
图1改进的人工协调场示意图。
图2气垫船避障算法流程图。
图3气垫船避让静态障碍物过程。
图4气垫船与障碍物距离变化曲线。
具体实施方式
下面结合附图对本发明作进一步的详细描述。
结合图2，气垫船避障方法的实现包括如下步骤：
1.读取当前时刻气垫船的位置(x,y)、速度(u,v)、艏向ψ以及回转率r，目标点的位置(xg,yg)，障碍物的位置(xo,yo)；
2.根据气垫船状态和目标点位置设计如下PID控制律计算吸引力Fa：
其中，Pg＝[xg,yg]T，P＝[x,y]T， U＝[u,v]T，Kp,Ki,Kd为2×2的增益矩阵；
3.计算碰撞风险度risk，并与设定的r_d比较：
其中d为设定的障碍物与气垫船安全距离，ε为设定的安全余量。若risk＞r_d，则进行下一步计算，否则转至步骤6；
4.设计斥力场并计算斥力Fr；
Fr＝kr(P-Po)
其中，Po＝[xo,yo]T；
场强参数kr如下计算：
其中，m≥1，pos(x)＝max(x,0)，Uor为气垫船相对于障碍物的相对速度，因为这里考虑的是静态障碍物，因此有Uor＝U，e(x)表示向量x的单位向量，amax表示气垫船的最大加速度；
5.根据斥力计算协调力Fn：
Fn＝kn*λ*Fr
其中，kn为标量的可调参数，λ的主要作用就是产生垂直于斥力的协调力，避免局部极小值的出现。
6.计算气垫船在大地坐标下的合力F，并转换成气垫船的纵向力和回转力矩：
设二维合力F＝[f1,f2]T，f1,f2分别为合力在大地坐标下的北向分力和东向分力，则气垫船的期望艏向ψd：
气垫船的回转控制力矩τn可根据PID计算为
τn＝kp(ψd-ψ)+ki∫(ψd-ψ)dt+kd(rd-r)
其中，kp,ki,kd为标量的可调PID参数，ψd为期望的艏向，为期望的艏向回转率。
气垫船的纵向控制力τx为
这样就把大地坐标下计算的合力转换为气垫船的纵向控制力和回转控制力矩。
Claims (1)
1.一种基于改进的人工协调场的气垫船避障方法，其特征是：
(1)获取气垫船状态信息、目标点位置以及障碍物位置；
所述气垫船状态信息包括当前时刻位置(x,y)、速度(u,v)、艏向ψ以及回转率r，目标点位置用(xg,yg)表示，障碍物位置用(xo,yo)表示；
(2)根据气垫船状态和目标点位置设计PID控制律，代替人工协调场中的吸引力；
吸引力Fa为：
<mrow>
<msub>
<mi>F</mi>
<mi>a</mi>
</msub>
<mo>=</mo>
<msub>
<mi>K</mi>
<mi>p</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>P</mi>
<mi>g</mi>
</msub>
<mo>-</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>i</mi>
</msub>
<mo>&Integral;</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>P</mi>
<mi>g</mi>
</msub>
<mo>-</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
<mi>d</mi>
<mi>t</mi>
<mo>-</mo>
<msub>
<mi>K</mi>
<mi>d</mi>
</msub>
<mover>
<mi>P</mi>
<mo>&CenterDot;</mo>
</mover>
</mrow>
其中，Pg＝[xg,yg]T，P＝[x,y]T，U＝[u,v]T，Kp,Ki,Kd为2×2的增益矩阵；
(3)根据气垫船状态信息和障碍物位置计算风险度，若风险度大于设定的风险度则进入步骤(4)，否则进入步骤(6)；
风险度risk为，
<mrow>
<mi>r</mi>
<mi>i</mi>
<mi>s</mi>
<mi>k</mi>
<mo>=</mo>
<mfrac>
<mrow>
<mi>d</mi>
<mo>+</mo>
<mi>&epsiv;</mi>
</mrow>
<mrow>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>P</mi>
<mi>o</mi>
</msub>
<mo>-</mo>
<mi>P</mi>
<mo>|</mo>
<mo>|</mo>
</mrow>
</mfrac>
</mrow>
其中d为设定的障碍物与气垫船安全距离，ε为设定的安全余量；
(4)设计斥力场，并利用气垫船的最大加速度约束设计斥力场的场强参数，进而计算出斥力；
斥力Fr为：
Fr＝kr(P-Po)
其中，Po＝[xo,yo]T；
场强参数kr如下计算：
<mrow>
<msub>
<mi>k</mi>
<mi>r</mi>
</msub>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<msup>
<mrow>
<mo>(</mo>
<mi>p</mi>
<mi>o</mi>
<mi>s</mi>
<mo>(</mo>
<mrow>
<mo>|</mo>
<mo>|</mo>
<mi>P</mi>
<mo>-</mo>
<msub>
<mi>P</mi>
<mi>o</mi>
</msub>
<mo>|</mo>
<mo>|</mo>
<mo>-</mo>
<mi>&epsiv;</mi>
<mo>-</mo>
<msup>
<mrow>
<mo>(</mo>
<mrow>
<mi>p</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>U</mi>
<mrow>
<mi>o</mi>
<mi>r</mi>
</mrow>
</msub>
<mo>&CenterDot;</mo>
<mi>e</mi>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>P</mi>
<mi>o</mi>
</msub>
<mo>-</mo>
<mi>P</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>/</mo>
<mn>2</mn>
<msub>
<mi>a</mi>
<mrow>
<mi>m</mi>
<mi>a</mi>
<mi>x</mi>
</mrow>
</msub>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mi>m</mi>
</msup>
</mfrac>
</mrow>
其中，m≥1，pos(x)＝max(x,0)，Uor为气垫船相对于障碍物的相对速度，因为这里考虑的是静态障碍物，因此有Uor＝U，e(x)表示向量x的单位向量，amax表示气垫船的最大加速度；
(5)根据斥力计算出协调力；
协调力Fn为：
Fn＝kn*λ*Fr
其中，kn为标量的可调参数，λ的作用就是产生垂直于斥力的协调力，避免局部极小值的出现；
(6)将吸引力、排斥力以及协调力合成，算出大地坐标下的合力，然后将所述合力的方向作为气垫船的期望艏向设计艏向PID控制律，将所述合力的大小作为气垫船的纵向控制力对气垫船进行控制；
大地坐标下的合力为F为：
<mrow>
<mi>F</mi>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>F</mi>
<mi>a</mi>
</msub>
<mo>+</mo>
<msub>
<mi>F</mi>
<mi>r</mi>
</msub>
<mo>+</mo>
<msub>
<mi>F</mi>
<mi>n</mi>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mtable>
<mtr>
<mtd>
<mrow>
<mi>i</mi>
<mi>f</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>r</mi>
<mi>i</mi>
<mi>s</mi>
<mi>k</mi>
<mo>></mo>
<mi>r</mi>
<mo>_</mo>
<mi>d</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>F</mi>
<mi>a</mi>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>e</mi>
<mi>l</mi>
<mi>s</mi>
<mi>e</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
设二维合力F＝[f1,f2]T，f1,f2分别为合力在大地坐标下的北向分力和东向分力，则气垫船的期望艏向ψd：
<mrow>
<msub>
<mi>&psi;</mi>
<mi>d</mi>
</msub>
<mo>=</mo>
<mi>a</mi>
<mi>r</mi>
<mi>c</mi>
<mi>t</mi>
<mi>a</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>f</mi>
<mn>2</mn>
</mrow>
<mrow>
<mi>f</mi>
<mn>1</mn>
</mrow>
</mfrac>
<mo>)</mo>
</mrow>
</mrow>
气垫船的回转控制力矩τn根据PID计算为
τn＝kp(ψd-ψ)+ki∫(ψd-ψ)dt+kd(rd-r)
其中，kp,ki,kd为标量的可调PID参数，ψd为期望的艏向，为期望的艏向回转率；
气垫船的纵向控制力τx为
<mrow>
<msub>
<mi>&tau;</mi>
<mi>x</mi>
</msub>
<mo>=</mo>
<msqrt>
<mrow>
<mi>f</mi>
<mn>1</mn>
<mo>&times;</mo>
<mi>f</mi>
<mn>1</mn>
<mo>+</mo>
<mi>f</mi>
<mn>2</mn>
<mo>&times;</mo>
<mi>f</mi>
<mn>2</mn>
</mrow>
</msqrt>
<mo>.</mo>
</mrow>
2
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510593659.7A CN105182975B (zh) | 2015-09-17 | 2015-09-17 | 一种基于改进的人工协调场的气垫船避障方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510593659.7A CN105182975B (zh) | 2015-09-17 | 2015-09-17 | 一种基于改进的人工协调场的气垫船避障方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105182975A CN105182975A (zh) | 2015-12-23 |
| CN105182975B true CN105182975B (zh) | 2017-10-03 |
Family
ID=54905115
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510593659.7A Expired - Fee Related CN105182975B (zh) | 2015-09-17 | 2015-09-17 | 一种基于改进的人工协调场的气垫船避障方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105182975B (zh) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110415561B (zh) * | 2019-06-14 | 2021-07-02 | 青岛科技大学 | 一种非冲突会遇局面下船舶集群态势分析方法 |
| CN110415562B (zh) * | 2019-06-14 | 2021-03-02 | 青岛科技大学 | 一种受限水域冲突会遇船舶集群态势分析方法 |
| CN110444046B (zh) * | 2019-06-14 | 2021-03-02 | 青岛科技大学 | 一种受限水域非冲突会遇船舶集群态势分析方法 |
| TWI756647B (zh) | 2020-03-18 | 2022-03-01 | 財團法人船舶暨海洋產業研發中心 | 基於人工勢場法的船舶避碰方法及系統 |
| CN115407785B (zh) * | 2022-11-01 | 2023-04-25 | 中国船舶集团有限公司第七〇七研究所 | 一种船舶避碰控制方法、装置、设备及存储介质 |
| CN116859960B (zh) * | 2023-07-28 | 2024-02-20 | 哈尔滨理工大学 | 一种多uuv队形切换与编队避障的自适应方法 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB2022040B (en) * | 1978-06-06 | 1982-06-09 | Hovermarine Transport Ltd | Controlling marine vehicles |
| CN103513655B (zh) * | 2013-09-11 | 2017-06-20 | 哈尔滨工程大学 | 一种气垫船自动驾控系统专用控制装置 |
| CN104760588B (zh) * | 2014-11-27 | 2017-06-20 | 哈尔滨工程大学 | 一种全垫升气垫船的安全限界控制方法以及基于安全限界的应急工况控制方法 |
-
2015
- 2015-09-17 CN CN201510593659.7A patent/CN105182975B/zh not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| CN105182975A (zh) | 2015-12-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105182975B (zh) | 一种基于改进的人工协调场的气垫船避障方法 | |
| CN110377039B (zh) | 一种车辆避障轨迹规划与跟踪控制方法 | |
| CN111681452B (zh) | 一种基于Frenet坐标系下的无人驾驶汽车动态换道轨迹规划方法 | |
| CN107856737B (zh) | 一种基于危险程度变权重的人机协同转向控制方法 | |
| Bae et al. | Path generation and tracking based on a Bezier curve for a steering rate controller of autonomous vehicles | |
| CN109050835B (zh) | 全驱动自主水下机器人结构及回收三维路径跟踪方法 | |
| CN106444794A (zh) | 一种无参数欠驱动uuv垂直面路径跟踪滑模控制方法 | |
| CN108829099A (zh) | 一种基于受限反步法控制的欠驱动无人船轨迹跟踪方法 | |
| WO2006013922A1 (ja) | 車両の制御装置 | |
| Fan et al. | Study on path following control method for automatic parking system based on LQR | |
| CN107792062A (zh) | 一种自动泊车控制系统 | |
| Yoshihara et al. | Autonomous predictive driving for blind intersections | |
| CN107065567A (zh) | 一种基于自适应神经网络约束控制的自动停车控制系统 | |
| CN105843224A (zh) | 基于神经动态模型反步法的auv水平面路径跟踪控制方法 | |
| CN106403957A (zh) | 一种全垫升气垫船航路点导引控制方法 | |
| CN107323457B (zh) | 一种人机协同的共享转向控制方法 | |
| CN115258073B (zh) | 环境干扰下的船舶拖曳系统轨迹跟踪方法 | |
| CN112527018A (zh) | 一种欠驱动自主水下航行器的三维镇定控制方法 | |
| KR20230009949A (ko) | 경로 제어 모듈, 연관된 경로 제어 디바이스 및 연관된 방법 | |
| CN107991872A (zh) | 欠驱动auv的虚拟锚泊水平面区域镇定控制方法及实现方法 | |
| Zwaan et al. | Haptic shared steering control with an adaptive level of authority based on time-to-line crossing | |
| CN205396080U (zh) | 一种汽车主动避撞系统 | |
| Li et al. | Multi-UUV formation coordination control based on combination of virtual structure and leader | |
| Sun et al. | Ribbon model based path tracking method for autonomous land vehicle | |
| CN114564015B (zh) | 一种拒止环境下的欠驱动无人艇分布式编队控制方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20171003 |