CN113093742A - 一种自动规避多障碍物的无人船路径跟踪系统 - Google Patents

一种自动规避多障碍物的无人船路径跟踪系统 Download PDF

Info

Publication number
CN113093742A
CN113093742A CN202110341065.2A CN202110341065A CN113093742A CN 113093742 A CN113093742 A CN 113093742A CN 202110341065 A CN202110341065 A CN 202110341065A CN 113093742 A CN113093742 A CN 113093742A
Authority
CN
China
Prior art keywords
unmanned ship
unit
speed
value
estimate
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.)
Granted
Application number
CN202110341065.2A
Other languages
English (en)
Other versions
CN113093742B (zh
Inventor
刘陆
黄自鹏
李铁山
王丹
彭周华
王浩亮
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Dalian Maritime University
Original Assignee
Dalian Maritime University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Dalian Maritime University filed Critical Dalian Maritime University
Priority to CN202110341065.2A priority Critical patent/CN113093742B/zh
Publication of CN113093742A publication Critical patent/CN113093742A/zh
Application granted granted Critical
Publication of CN113093742B publication Critical patent/CN113093742B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种自动规避多障碍物的无人船路径跟踪系统，包括，避碰控制模块和路径跟踪模块，避碰控制模块用于根据无人船和环境障碍物的位置和角度计算避碰控制角速度；路径跟踪模块包括LOS制导单元、非线性微分控制器单元Ⅱ、观测器单元以及动力学控制器单元，其根据避碰控制角速度计算无人船的纵向方向的控制力矩和艏摇方向的控制力矩进而控制无人船运动。本发明公开的系统实现了根据地球坐标系下无人船的位置坐标和艏摇角信息、环境障碍物的位置坐标，在实现路径跟踪的同时避开环境障碍物，实现无碰撞风险的无人船路径跟踪，具有系统更为方便简洁、能够适用多个障碍物的复杂海洋环境以及节省了成本等优点。

Description

一种自动规避多障碍物的无人船路径跟踪系统
技术领域
本发明涉及多无人船控制技术领域，尤其是一种自动规避多障碍物的无人船路径跟踪系统。
背景技术
无人船路径跟踪在近海巡逻、海洋探测、远洋搜救等领域具有重要的应用价值，因而得到了研究学者的广泛关注。针对无人船路径跟踪控制问题，国内外学者已经提出很多控制方法，如比例积分控制、滑模控制、模糊预测控制、神经网络控制等。此外无人船工作在复杂的海洋环境中时，周围环境不仅存在已知的障碍物，可能也存在诸如礁石、移动浮标、航行路线上驶过的船只等未知的障碍物，这些都可能造成无人船执行任务的失败甚至威胁无人船的安全。这时就需要将路径规划和反应式避碰有机结合，因此，如何研究一种能够实时避障的无人船路径跟踪方法十分重要。
无人船避障包含“碰撞检测”和“碰撞解决”两个基本过程，基于这两个过程，国内外学者做了大量的研究工作，并提出了许多避障避碰方法。Venanzio Cichella等人提出基于视线角度的避碰方法，但是该方法仅引入了视线角度(LOS)来完成“碰撞解决”过程，没有考虑“碰撞检测”问题。Thiago Marinho等人提出了基于视线角度(LOS)和碰撞时间(TTC)的避碰方法，该方法虽然不依赖于移动障碍物位置信息，但是该方法需要采用专业设备获取视线角度(LOS)和碰撞时间(TTC)，大大增加了运营成本。Soltan等人提出了基于有限循环(LCM)的避碰方法，其缺点是仅适用于静态障碍物。Wiig等人提出了视觉锥方法来处理速度恒定的运动障碍物，其缺点是不能用于多障碍物场景。
因此亟需一种无人船路径跟踪系统以解决上述问题。
发明内容
本发明提出了一种自动规避多障碍物的无人船路径跟踪系统，可以解决无人船无法有效地与移动障碍物进行避让的问题。
本发明采用的技术手段如下：
一种自动规避多障碍物的无人船路径跟踪系统，包括，
避碰控制模块，用于获取无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ以及第i个环境障碍物在地球坐标系下的x轴位置xic、y轴位置yic以计算避碰控制角速度rc；
LOS制导单元，用于获取所述避碰控制角速度rc、无人船的纵向速度估计值
Figure BDA0002999613310000021
和横漂速度估计值
Figure BDA0002999613310000022
以及无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ以计算前向速度给定值αu和艏摇角速度给定值αr；
非线性微分控制器单元Ⅱ，用于获取所述前向速度给定值αu和艏摇角速度给定值αr以计算前向速度给定值αu的估计值uf、前向速度给定值的导数
Figure BDA0002999613310000023
的估计值
Figure BDA0002999613310000024
艏摇角速度给定值αr的估计值rf以及艏摇角速度给定值的导数
Figure BDA0002999613310000025
的估计值
Figure BDA0002999613310000026
观测器单元，用于获取无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ、无人船的纵向方向的控制力矩τu和艏摇方向的控制力矩τr以计算无人船的纵向速度估计值
Figure BDA0002999613310000027
横漂速度估计值
Figure BDA0002999613310000028
艏摇角速度估计值
Figure BDA0002999613310000029
纵向扰动估计值
Figure BDA00029996133100000210
以及艏摇方向扰动估计值
Figure BDA00029996133100000211
以及，
动力学控制器单元，用于获取所述前向速度给定值αu的估计值uf、前向速度给定值的导数
Figure BDA00029996133100000212
的估计值
Figure BDA00029996133100000213
艏摇角速度给定值αr的估计值rf、艏摇角速度给定值的导数
Figure BDA00029996133100000214
的估计值
Figure BDA00029996133100000215
纵向速度估计值
Figure BDA00029996133100000216
艏摇角速度估计值
Figure BDA00029996133100000217
纵向扰动估计值
Figure BDA00029996133100000218
以及艏摇方向扰动估计值
Figure BDA00029996133100000219
以计算所述纵向方向的控制力矩τu和艏摇方向的控制力矩τr并将所述纵向方向的控制力矩τu和艏摇方向的控制力矩τr输入至无人船。
进一步地，所述避碰控制模块包括，
计算单元，用于获取无人船在大地坐标系下的x轴位置x、y轴位置y以及第i个环境障碍物在地球坐标系下的x轴位置xic、y轴位置yic以计算无人船与第i个环境障碍物之间的距离ρi和无人船和第i个环境障碍物之间的视线角度χi；
非线性微分控制器单元Ⅰ，用于获取所述无人船与第i个环境障碍物之间的距离ρi以计算无人船和第i个环境障碍物之间的距离导数的估计值
Figure BDA00029996133100000220
比例单元Ⅰ，用于获取所述无人船和第i个环境障碍物之间的距离导数的估计值
Figure BDA00029996133100000221
和无人船与第i个环境障碍物之间的距离ρi以计算碰撞时间Tic；
比较单元，用于获取所述碰撞时间Tic以计算参数变量ξi，
加法器单元，用于获取所述无人船和第i个环境障碍物之间的视线角度χi和无人船在大地坐标系下的艏摇角ψ以计算无人船在船体坐标系下的纵向速度u和第i个环境障碍物的速度uic之间的夹角δi；以及，
比例单元Ⅱ，用于获取所述无人船在船体坐标系下的纵向速度u和第i个环境障碍物的速度uic之间的夹角δi与参数变量ξi以计算避碰控制角速度rc。
进一步地，所述无人船与第i个环境障碍物之间的距离ρi通过公式(1)进行计算：
Figure BDA0002999613310000031
所述无人船和第i个环境障碍物之间的视线角度χi通过公式(2)进行计算：
Figure BDA0002999613310000032
所述无人船和第i个环境障碍物之间的距离导数的估计值
Figure BDA0002999613310000033
通过公式(3)进行计算：
Figure BDA0002999613310000034
其中：γiρ为设计的比例参数；ρif是ρi的估计值；
Figure BDA0002999613310000035
是
Figure BDA0002999613310000036
的估计值；
Figure BDA0002999613310000037
为无人船与第i个环境障碍物之间的距离ρi的导数；
所述碰撞时间Tic通过公式(4)进行计算：
Figure BDA0002999613310000038
所述参数变量ξi通过公式(5)进行计算：
Figure BDA0002999613310000039
所述无人船在船体坐标系下的纵向速度u和第i个环境障碍物的速度uic之间的夹角δi通过公式(6)进行计算：
δi＝χi-ψ+π/2 (6)
所述避碰控制角速度rc通过公式(7)进行计算：
Figure BDA00029996133100000310
其中：kic和ki为控制增益，n为环境障碍物的个数。
进一步地，所述LOS制导单元包括制导律设计单元和参数化路径单元；
所述参数化路径单元，用于将无人船的给定参数化路径输入至制导律设计单元；
所述制导律设计单元，用于获取所述无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ、所述避碰控制角速度rc、无人船的纵向速度估计值
Figure BDA00029996133100000311
横漂速度估计值
Figure BDA00029996133100000312
以及无人船的给定参数化路径以计算所述前向速度给定值αu和艏摇角速度给定值αr。
进一步地，计算所述前向速度给定值αu和艏摇角速度给定值αr包括以下步骤：
通过公式(8)计算期望路径切向角ψd：
Figure BDA0002999613310000041
通过公式(9)计算横向跟踪误差xe和纵向跟踪误差ye：
Figure BDA0002999613310000042
通过公式(10)和公式(11)计算前向速度给定值αu、艏摇角速度给定值αr以及参考制导信号αψ：
Figure BDA0002999613310000043
Figure BDA0002999613310000044
其中：
Figure BDA0002999613310000045
υs是参考速度；ψe＝ψ-αψ为航向跟踪误差；
Figure BDA0002999613310000046
是侧滑角；Δ为前向距离；
Figure BDA0002999613310000047
Figure BDA0002999613310000048
Δx和Δψ为设定参数；
Figure BDA0002999613310000049
为中间变量；ku和kr是设计参数。
进一步地，所述前向速度给定值αu的估计值uf、前向速度给定值的导数
Figure BDA00029996133100000410
的估计值
Figure BDA00029996133100000411
艏摇角速度给定值αr的估计值rf以及艏摇角速度给定值的导数
Figure BDA00029996133100000412
的估计值
Figure BDA00029996133100000413
通过公式(12)和公式(13)进行计算：
Figure BDA00029996133100000414
Figure BDA00029996133100000415
其中：γu和γr为设计的比例参数；uf,
Figure BDA00029996133100000416
rf,
Figure BDA00029996133100000417
分别是αu,
Figure BDA00029996133100000418
αr,
Figure BDA00029996133100000419
的估计值。
进一步地，所述无人船的纵向速度估计值
Figure BDA00029996133100000420
横漂速度的估计值
Figure BDA00029996133100000421
纵向扰动估计值
Figure BDA0002999613310000051
以及艏摇方向扰动估计值
Figure BDA0002999613310000052
通过公式(14)进行计算：
Figure BDA0002999613310000053
其中：η＝[x,y,ψ]是无人船的位置和角度；
Figure BDA0002999613310000054
是无人船位置和角度的观测值；
Figure BDA0002999613310000055
为观测器单元的输出信号；K1,K2,K3为三维观测器增益矩阵；R(ψ)为坐标变换矩阵；M为无人船惯性矩阵。
进一步地，所述无人船在纵向方向的控制力矩τu和艏摇方向的控制力矩τr通过公式(15)进行计算：
Figure BDA0002999613310000056
其中：
Figure BDA0002999613310000057
ku,kr为动力学控制增益；mu为无人船在纵向方向上的惯性系数；mr为无人船艏摇方向上的惯性系数；zu,zr为误差变量。
与现有技术比较，本发明公开的自动规避多障碍物的无人船路径跟踪系统具有以下有益效果：第一、与现有无人船避碰方法相比，本发明提出的避碰方法通过添加碰撞时间Tc，将碰撞检测功能嵌入到避碰控制定律中，使得防撞系统始终处于运行之中，较以往的提前比较实际距离ρi与安全距离ρsf来判断是否避碰更为方便简洁。第二、现有的无人船避碰方法大多只能避开单个障碍物，本发明提出的新型避碰方法可以避开多个障碍物，能够应用于存在多个障碍物的复杂海洋环境。同时，本发明提出的新型避碰方法不仅可以避开静止的障碍物还能够躲避开动态的障碍物。第三，现有的无人船避碰控制方法与其他控制方法结合时大多不够灵活，只能单独使用，不能作为一个整体运用，本发明提出的新型避碰方法可与路径跟踪、编队控制等典型的控制方法相结合。
附图说明
图1是一种自动规避多障碍物的无人船路径跟踪方法结构示意图；
图2是无人船规避移动障碍物的运动轨迹；
图3是无人船和移动障碍物之间的距离；
图4是无人船规避多个障碍物时的运动轨迹；
图5是无人船规避多个障碍物时的跟踪误差；
图6是无人船规避多个障碍物时的三向速度及其观测值；
图7是无人船规避多个障碍物时的三向扰动及其观测值。
具体实施方式
如图1所示，本发明公开自动规避多障碍物的无人船路径跟踪系统包括避碰控制环节和路径跟踪环节组成。传感器输出的环境障碍物位置信息和无人船输出的位置坐标及艏摇角信息被送到避碰控制环节，具体经过其中的计算单元、非线性微分控制器Ⅰ、比例单元Ⅰ、加法器、比较单元、比例单元Ⅱ最终得到避碰控制角速度；所述的路径跟踪环节包括非线性微分控制器Ⅱ、制导律设计单元、参数化路径单元、观测器单元、动力学控制器单元。制导律设计单元的输入端分别与避碰控制环节中的比例单元Ⅱ、无人船、观测器单元和参数化路径的输出端相连，输出端与非线性微分控制器Ⅱ相连；非线性微分控制器Ⅱ的输出端与动力学控制器相连；动力学控制器的另一个输入端与观测器单元相连，两个输出端分别与观测器单元和无人船相连；观测器单元的另一个输入端与无人船相连。
在本发明中，无人船运动学和动力学模型如下：
Figure BDA0002999613310000061
Figure BDA0002999613310000062
式中：x,y,ψ分别代表为无人船在地球坐标系下的x轴、y轴上的位置以及艏摇角；u,υ,r分别代表无人船在船体坐标系下的纵向速度、横漂速度和艏摇角速度；mu,mv,mr是无人船在船体坐标系下的各向惯性系数；fu(·),fv(·),fr(·)是无人船各向的不确定性；τu,τr分别是无人船在纵向和艏摇两个方向的控制力矩；τwu,τwv,τwr分别为无人船前向、横向以及艏摇方向的干扰力矩，其是由海洋环境中的风、浪和海流引起的外界干扰。
障碍物运动学模型定义如下
Figure BDA0002999613310000063
式中：xic,yic分别代表第i个环境障碍物在地球坐标系下的x轴、y轴的位置；uic是障碍物的速度；ψic是障碍物的航向角。
本发明公开的自动规避多障碍物的无人船路径跟踪系统，包括，
避碰控制模块，用于获取无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ以及第i个环境障碍物在地球坐标系下的x轴位置xic、y轴位置yic以计算避碰控制角速度rc；
LOS制导单元，用于获取所述避碰控制角速度rc、无人船的纵向速度估计值
Figure BDA0002999613310000071
和横漂速度估计值
Figure BDA0002999613310000072
以及无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ以计算前向速度给定值αu和艏摇角速度给定值αr；
非线性微分控制器单元Ⅱ，用于获取所述前向速度给定值αu和艏摇角速度给定值αr以计算前向速度给定值αu的估计值uf、前向速度给定值的导数
Figure BDA0002999613310000073
的估计值
Figure BDA0002999613310000074
艏摇角速度给定值αr的估计值rf以及艏摇角速度给定值的导数
Figure BDA0002999613310000075
的估计值
Figure BDA0002999613310000076
观测器单元，用于获取无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ、无人船的纵向方向的控制力矩τu和艏摇方向的控制力矩τr以计算无人船的纵向速度估计值
Figure BDA0002999613310000077
横漂速度估计值
Figure BDA0002999613310000078
艏摇角速度估计值
Figure BDA0002999613310000079
纵向扰动估计值
Figure BDA00029996133100000710
以及艏摇方向扰动估计值
Figure BDA00029996133100000711
以及，
动力学控制器单元，用于获取所述前向速度给定值αu的估计值uf、前向速度给定值的导数
Figure BDA00029996133100000712
的估计值
Figure BDA00029996133100000713
艏摇角速度给定值αr的估计值rf、艏摇角速度给定值的导数
Figure BDA00029996133100000714
的估计值
Figure BDA00029996133100000715
纵向速度估计值
Figure BDA00029996133100000716
艏摇角速度估计值
Figure BDA00029996133100000717
纵向扰动估计值
Figure BDA00029996133100000718
以及艏摇方向扰动估计值
Figure BDA00029996133100000719
以计算所述纵向方向的控制力矩τu和艏摇方向的控制力矩τr并将所述纵向方向的控制力矩τu和艏摇方向的控制力矩τr输入至无人船。
本发明中主要包括避碰控制环节和路径跟踪环节，其中避碰控制环节由避碰控制模块实现，所述避碰控制模块包括，
计算单元，用于获取无人船在大地坐标系下的x轴位置x、y轴位置y以及第i个环境障碍物在地球坐标系下的x轴位置xic、y轴位置yic以计算无人船与第i个环境障碍物之间的距离ρi和无人船和第i个环境障碍物之间的视线角度χi；具体地，计算单元的输入信号有无人船的位置信号x,y、传感器单元输出的第i个环境障碍物位置坐标xic,yic；计算单元的输出信号包括无人船与第i个环境障碍物之间的距离ρi和无人船和第i个环境障碍物之间的视线角度χi经过下列公式计算得出：
所述无人船与第i个环境障碍物之间的距离ρi通过公式(1)进行计算：
Figure BDA00029996133100000720
所述无人船和第i个环境障碍物之间的视线角度χi通过公式(2)进行计算：
Figure BDA0002999613310000081
非线性微分控制器单元Ⅰ，用于获取所述无人船与第i个环境障碍物之间的距离ρi以计算无人船和第i个环境障碍物之间的距离导数的估计值
Figure BDA0002999613310000082
具体地，计算单元输出的无人船和第i个环境障碍物之间的距离ρi送到非线性微分控制器Ⅰ，得到距离的导数
Figure BDA0002999613310000083
的估计值
Figure BDA0002999613310000084
非线性微分控制器Ⅰ通过公式(3)计算所述无人船和第i个环境障碍物之间的距离导数的估计值
Figure BDA0002999613310000085
Figure BDA0002999613310000086
其中：γiρ为设计的比例参数；ρif是ρi的估计值；
Figure BDA0002999613310000087
是
Figure BDA0002999613310000088
的估计值；
Figure BDA0002999613310000089
为无人船与第i个环境障碍物之间的距离ρi的导数；
比例单元Ⅰ，用于获取所述无人船和第i个环境障碍物之间的距离导数的估计值
Figure BDA00029996133100000810
和无人船与第i个环境障碍物之间的距离ρi以计算碰撞时间Tic；具体地，比例单元Ⅰ的输入信号包括来自计算单元的无人船与第i个环境障碍物之间的距离ρi以及来自非线性微分控制器Ⅰ的距离导数的估计值
Figure BDA00029996133100000811
比例单元Ⅰ的输出信号Tic通过公式(4)进行计算：
Figure BDA00029996133100000812
其中：Tic定义为碰撞时间(TTC)；当无人船与障碍物之间的距离ρi越来越大时，说明
Figure BDA00029996133100000813
此时Tic>0；反之，当无人船与障碍物之间的距离ρi慢慢变小时，说明
Figure BDA00029996133100000814
此时Tic<0。
比较单元，用于获取所述碰撞时间Tic以计算参数变量ξi；所述参数变量ξi通过公式(5)进行计算：
Figure BDA00029996133100000815
其中ξi为参数变量；当Tic>0时，ξi→+∞；当Tic<0时，ξi＝-Tic；
加法器单元，用于获取所述无人船和第i个环境障碍物之间的视线角度χi和无人船在大地坐标系下的艏摇角ψ以计算无人船在船体坐标系下的纵向速度u和第i个环境障碍物的速度uic之间的夹角δi，具体地，加法器单元的输入信号包括无人船的艏摇角ψ和来自计算单元的无人船和第i个环境障碍物之间的视线角度χi；加法器单元通过公式(6)进行计算无人船在船体坐标系下的纵向速度u和第i个环境障碍物的速度uic之间的夹角δi：
δi＝χi-ψ+π/2 (6)
比例单元Ⅱ，用于获取所述无人船在船体坐标系下的纵向速度u和第i个环境障碍物的速度uic之间的夹角δi与参数变量ξi以计算避碰控制角速度rc，具体地，加法器输出的无人船在船体坐标系下的纵向速度u和第i个环境障碍物的速度uic之间的夹角δi和比较单元输出的参数变量ξi同时输入到比例单元Ⅱ，得到最终的避碰控制角速度rc；比例单元Ⅱ的输出的避碰控制角速度rc通过公式(7)进行计算：
Figure BDA0002999613310000091
其中：其中rc为无人船的避碰控制角速度；kic和ki为控制增益。当无人船与环境障碍物之间距离增大时，说明Tic>0,ξi→+∞，此时不需要采取避碰措施，因此rc→0；相反，当无人船与障碍物之间距离减小时，说明Tic<0,ξi＝-Tic，此时rc不为零。
路径跟踪环节由以下LOS制导单元、非线性微分控制器单元Ⅱ、观测器单元以及动力学控制器单元组成，具体地，所述LOS制导单元包括制导律设计单元和参数化路径单元；
所述参数化路径单元，用于将无人船的给定参数化路径(xd(θ),yd(θ))输入至制导律设计单元；
所述制导律设计单元，用于获取所述无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ、所述避碰控制角速度rc、无人船的纵向速度估计值
Figure BDA0002999613310000092
横漂速度估计值
Figure BDA0002999613310000093
以及无人船的给定参数化路径(xd(θ),yd(θ))以计算所述前向速度给定值αu和艏摇角速度给定值αr，具体地，制导律设计单元的输入量有：无人船的大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ、来自观测器单元的无人船纵向速度、横漂速度的估计
Figure BDA0002999613310000094
来自比例单元Ⅱ的避碰控制角速度rc以及无人船的给定参数化路径(xd(θ),yd(θ))；以上输入量经过制导律设计单元得到前向速度给定值αu和艏摇角速度给定值αr，计算所述前向速度给定值αu和艏摇角速度给定值αr具体包括以下步骤：
通过公式(8)计算期望路径切向角ψd：
Figure BDA0002999613310000095
其中：(xd(θ),yd(θ))为给定的参数化路径；θ是一个与时间无关的路径参数；
对于一艘位置为(x,y)的无人船，通过公式(9)计算横向跟踪误差xe和纵向跟踪误差ye：
Figure BDA0002999613310000101
通过公式(10)和公式(11)计算前向速度给定值αu、艏摇角速度给定值αr以及参考制导信号αψ：
Figure BDA0002999613310000102
Figure BDA0002999613310000103
其中：αu为前向速度给定值，αr为艏摇角速度给定值，αψ为参考制导信号；
Figure BDA0002999613310000104
υs是参考速度；ψe＝ψ-αψ为航向跟踪误差；
Figure BDA0002999613310000105
是侧滑角；Δ为前向距离；
Figure BDA0002999613310000106
Δx和Δψ为设定参数；
Figure BDA0002999613310000107
为中间变量；ku和kr是设计参数；rc为避碰控制角速度。
观测器单元的输入量包括无人船的位置信息x,y和艏摇角ψ、来自动力学控制器的无人船控制力矩τ＝[τu,τr]，以上输入量经过观测器单元得到所述无人船的纵向速度估计值
Figure BDA0002999613310000108
横漂速度的估计值
Figure BDA0002999613310000109
纵向扰动估计值
Figure BDA00029996133100001010
以及艏摇方向扰动估计值
Figure BDA00029996133100001011
具体是通过公式(14)进行计算：
Figure BDA00029996133100001012
其中：η＝[x,y,ψ]是无人船的位置和角度；
Figure BDA00029996133100001013
是无人船位置和角度的观测值；
Figure BDA00029996133100001014
为观测器单元的输出信号；K1,K2,K3为三维观测器增益矩阵；R(ψ)为坐标变换矩阵；M为无人船惯性矩阵。
非线性微分控制器Ⅱ的输入信号为来自制导律设计单元的前向速度给定值αu和艏摇角速度给定值αr；所述前向速度给定值αu的估计值uf、前向速度给定值的导数
Figure BDA00029996133100001015
的估计值
Figure BDA00029996133100001016
艏摇角速度给定值αr的估计值rf以及艏摇角速度给定值的导数
Figure BDA00029996133100001017
的估计值
Figure BDA00029996133100001018
通过公式(12)和公式(13)进行计算：
Figure BDA0002999613310000111
Figure BDA0002999613310000112
其中：γu和γr为设计的比例参数；uf,
Figure BDA0002999613310000113
rf,
Figure BDA0002999613310000114
分别是αu,
Figure BDA0002999613310000115
αr,
Figure BDA0002999613310000116
的估计值。
动力学控制器的输入信号包括来自观测器单元的速度估计
Figure BDA0002999613310000117
和扰动估计
Figure BDA0002999613310000118
以及来自非线性微分控制器Ⅱ的输出信号uf,
Figure BDA0002999613310000119
和rf,
Figure BDA00029996133100001110
动力学控制器的输出信号τ＝[τu,τr]包括所述无人船在纵向方向的控制力矩τu和艏摇方向的控制力矩τr，具体是通过公式(15)进行计算：
Figure BDA00029996133100001111
其中：
Figure BDA00029996133100001112
ku,kr为动力学控制增益；mu为无人船在纵向方向上的惯性系数；mr为无人船艏摇方向上的惯性系数；zu,zr为误差变量。
下面结合附图1对本发明进行进一步说明。本发明设计的一种自动规避多障碍物的无人船路径跟踪控制器结构及设计方法如图1所示。首先，无人船的位置信号x,y和艏摇角ψ以及传感器单元输出的环境障碍物位置信息xic,yic通过避碰控制环节输出无人船的避碰控制角速度rc。之后参数化路径单元输出参考路径的位置信息[xd(θ),yd(θ)]、无人船位置信息和艏摇角信息[x,y,ψ]、观测器单元输出的无人船估计速度
Figure BDA00029996133100001113
比例单元Ⅱ输出的避碰控制角速度rc，四者通过制导律设计单元输出前向速度给定值αu、艏摇角速度给定值αr到非线性微分控制器Ⅱ中。非线性微分控制器Ⅱ输出的信号
Figure BDA00029996133100001114
和观测器单元输出的估计值
Figure BDA00029996133100001115
共同输入到动力学控制器中得到无人船控制力矩τu,τr。无人船的位置信息和艏摇角信息[x,y,ψ]与动力学控制器的输出信号τu,τr输入到观测器单元得到估计值
Figure BDA00029996133100001116
综合上所述构成了自动规避多障碍物的无人船路径跟踪控制器。
仿真结果如图2-7所示。图2是无人船在规避移动障碍物时的运动轨迹，图中的点线是给定的期望路径，实线是无人船的实际运动轨迹，虚线代表移动障碍物，点划线为同一时刻无人船和移动障碍物两者位置连线。图3是无人船和移动障碍物之间的距离图，从图中可以看出在任何时刻无人船和移动障碍物之间的距离都不为零。图4是无人船规避多个障碍物时的运动轨迹，图中的点线是给定的期望路径，实线是无人船的实际运动轨迹，三个圆代表环境障碍物。从图中可以看出无人船跟踪上了给定的参数化路径，同时绕开了环境障碍物。图5是无人船规避多个障碍物时的跟踪误差，图中可以看出，排除需要避障的时间，无人船跟踪误差可以收敛到零并保持。图6无人船的三向速度及其观测值，图中点线为无人船三相速度的实际值，虚线为其估计值。从图中可以看出无人船的速度在一定范围内且观测器观测的速度准确。图7是无人船的三向扰动及其观测值，其中点线为不确定性和扰动的实际值，虚线为观测器的估计值，图中可以看出观测器对不确定性和扰动的观测效果良好。
基于以上分析，本专利提出的自动多规避障碍物的无人船路径跟踪方法，在实现路径跟踪的同时能够避免碰撞。根据地球坐标系下无人船的位置坐标和艏摇角信息、环境障碍物的位置坐标，在实现路径跟踪的同时避开环境障碍物，实现无碰撞风险的无人船路径跟踪。
实际应用中可以通过船载全球定位模块获得无人船位置信息，姿态测量模块获得无人船的艏摇角，障碍物的位置信息可以通过船载激光测距仪得到，该测距仪价格合理且易携带。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (8)

1.一种自动规避多障碍物的无人船路径跟踪系统，其特征在于：包括，
避碰控制模块，用于获取无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ以及第i个环境障碍物在地球坐标系下的x轴位置xic、y轴位置yic以计算避碰控制角速度rc；
LOS制导单元，用于获取所述避碰控制角速度rc、无人船的纵向速度估计值
Figure FDA0002999613300000011
和横漂速度估计值
Figure FDA0002999613300000012
以及无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ以计算前向速度给定值αu和艏摇角速度给定值αr；
非线性微分控制器单元Ⅱ，用于获取所述前向速度给定值αu和艏摇角速度给定值αr以计算前向速度给定值αu的估计值uf、前向速度给定值的导数
Figure FDA0002999613300000013
的估计值
Figure FDA0002999613300000014
艏摇角速度给定值αr的估计值rf以及艏摇角速度给定值的导数
Figure FDA0002999613300000015
的估计值
Figure FDA0002999613300000016
观测器单元，用于获取无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ、无人船的纵向方向的控制力矩τu和艏摇方向的控制力矩τr以计算无人船的纵向速度估计值
Figure FDA0002999613300000017
横漂速度估计值
Figure FDA0002999613300000018
艏摇角速度估计值
Figure FDA0002999613300000019
纵向扰动估计值
Figure FDA00029996133000000110
以及艏摇方向扰动估计值
Figure FDA00029996133000000111
以及，
动力学控制器单元，用于获取所述前向速度给定值αu的估计值uf、前向速度给定值的导数
Figure FDA00029996133000000112
的估计值
Figure FDA00029996133000000113
艏摇角速度给定值αr的估计值rf、艏摇角速度给定值的导数
Figure FDA00029996133000000114
的估计值
Figure FDA00029996133000000115
纵向速度估计值
Figure FDA00029996133000000116
艏摇角速度估计值
Figure FDA00029996133000000117
纵向扰动估计值
Figure FDA00029996133000000118
以及艏摇方向扰动估计值
Figure FDA00029996133000000119
以计算所述纵向方向的控制力矩τu和艏摇方向的控制力矩τr并将所述纵向方向的控制力矩τu和艏摇方向的控制力矩τr输入至无人船。
2.根据权利要求1所述的自动规避多障碍物的无人船路径跟踪系统，其特征在于：所述避碰控制模块包括，
计算单元，用于获取无人船在大地坐标系下的x轴位置x、y轴位置y以及第i个环境障碍物在地球坐标系下的x轴位置xic、y轴位置yic以计算无人船与第i个环境障碍物之间的距离ρi和无人船和第i个环境障碍物之间的视线角度χi；
非线性微分控制器单元Ⅰ，用于获取所述无人船与第i个环境障碍物之间的距离ρi以计算无人船和第i个环境障碍物之间的距离导数的估计值
Figure FDA00029996133000000120
比例单元Ⅰ，用于获取所述无人船和第i个环境障碍物之间的距离导数的估计值
Figure FDA00029996133000000121
和无人船与第i个环境障碍物之间的距离ρi以计算碰撞时间Tic；
比较单元，用于获取所述碰撞时间Tic以计算参数变量ξi，
加法器单元，用于获取所述无人船和第i个环境障碍物之间的视线角度χi和无人船在大地坐标系下的艏摇角ψ以计算无人船在船体坐标系下的纵向速度u和第i个环境障碍物的速度uic之间的夹角δi；以及，
比例单元Ⅱ，用于获取所述无人船在船体坐标系下的纵向速度u和第i个环境障碍物的速度uic之间的夹角δi与参数变量ξi以计算避碰控制角速度rc。
3.根据权利要求2所述的自动规避多障碍物的无人船路径跟踪系统，其特征在于：所述无人船与第i个环境障碍物之间的距离ρi通过公式(1)进行计算：
Figure FDA0002999613300000021
所述无人船和第i个环境障碍物之间的视线角度χi通过公式(2)进行计算：
Figure FDA0002999613300000022
所述无人船和第i个环境障碍物之间的距离导数的估计值
Figure FDA0002999613300000023
通过公式(3)进行计算：
Figure FDA0002999613300000024
其中：γiρ为设计的比例参数；ρif是ρi的估计值；
Figure FDA0002999613300000025
是
Figure FDA0002999613300000026
的估计值；
Figure FDA0002999613300000027
为无人船与第i个环境障碍物之间的距离ρi的导数；
所述碰撞时间Tic通过公式(4)进行计算：
Figure FDA0002999613300000028
所述参数变量ξi通过公式(5)进行计算：
Figure FDA0002999613300000029
所述无人船在船体坐标系下的纵向速度u和第i个环境障碍物的速度uic之间的夹角δi通过公式(6)进行计算：
δi＝χi-ψ+π/2 (6)
所述避碰控制角速度rc通过公式(7)进行计算：
Figure FDA00029996133000000210
其中：kic和ki为控制增益，n为环境障碍物的个数。
4.根据权利要求3所述的自动规避多障碍物的无人船路径跟踪系统，其特征在于：所述LOS制导单元包括制导律设计单元和参数化路径单元；
所述参数化路径单元，用于将无人船的给定参数化路径输入至制导律设计单元；
所述制导律设计单元，用于获取所述无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ、所述避碰控制角速度rc、无人船的纵向速度估计值
Figure FDA0002999613300000031
横漂速度估计值
Figure FDA0002999613300000032
以及无人船的给定参数化路径以计算所述前向速度给定值αu和艏摇角速度给定值αr。
5.根据权利要求4所述的自动规避多障碍物的无人船路径跟踪系统，其特征在于：计算所述前向速度给定值αu和艏摇角速度给定值αr包括以下步骤：
通过公式(8)计算期望路径切向角ψd：
Figure FDA0002999613300000033
通过公式(9)计算横向跟踪误差xe和纵向跟踪误差ye：
Figure FDA0002999613300000034
通过公式(10)和公式(11)计算前向速度给定值αu、艏摇角速度给定值αr以及参考制导信号αψ：
Figure FDA0002999613300000035
Figure FDA0002999613300000036
其中：
Figure FDA0002999613300000037
υs是参考速度；ψe＝ψ-αψ为航向跟踪误差；
Figure FDA0002999613300000038
是侧滑角；Δ为前向距离；
Figure FDA0002999613300000039
Figure FDA00029996133000000310
Δx和Δψ为设定参数；
Figure FDA00029996133000000311
为中间变量；ku和kr是设计参数。
6.根据权利要求5所述的自动规避多障碍物的无人船路径跟踪系统，其特征在于：所述前向速度给定值αu的估计值uf、前向速度给定值的导数
Figure FDA00029996133000000312
的估计值
Figure FDA00029996133000000313
艏摇角速度给定值αr的估计值rf以及艏摇角速度给定值的导数
Figure FDA00029996133000000314
的估计值
Figure FDA00029996133000000315
通过公式(12)和公式(13)进行计算：
Figure FDA00029996133000000316
Figure FDA0002999613300000041
其中：γu和γr为设计的比例参数；uf,
Figure FDA0002999613300000042
rf,
Figure FDA0002999613300000043
分别是αu,
Figure FDA0002999613300000044
αr,
Figure FDA0002999613300000045
的估计值。
7.根据权利要求6所述的自动规避多障碍物的无人船路径跟踪系统，其特征在于：所述无人船的纵向速度估计值
Figure FDA0002999613300000046
横漂速度的估计值
Figure FDA0002999613300000047
纵向扰动估计值
Figure FDA0002999613300000048
以及艏摇方向扰动估计值
Figure FDA0002999613300000049
通过公式(14)进行计算：
Figure FDA00029996133000000410
其中：η＝[x,y,ψ]是无人船的位置和角度；
Figure FDA00029996133000000411
是无人船位置和角度的观测值；
Figure FDA00029996133000000412
为观测器单元的输出信号；K1,K2,K3为三维观测器增益矩阵；R(ψ)为坐标变换矩阵；M为无人船惯性矩阵。
8.根据权利要求7所述的自动规避多障碍物的无人船路径跟踪系统，其特征在于：所述无人船在纵向方向的控制力矩τu和艏摇方向的控制力矩τr通过公式(15)进行计算：
Figure FDA00029996133000000413
其中：
Figure FDA00029996133000000414
ku,kr为动力学控制增益；mu为无人船在纵向方向上的惯性系数；mr为无人船艏摇方向上的惯性系数；zu,zr为误差变量。
CN202110341065.2A 2021-03-30 2021-03-30 一种自动规避多障碍物的无人船路径跟踪系统 Active CN113093742B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110341065.2A CN113093742B (zh) 2021-03-30 2021-03-30 一种自动规避多障碍物的无人船路径跟踪系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110341065.2A CN113093742B (zh) 2021-03-30 2021-03-30 一种自动规避多障碍物的无人船路径跟踪系统

Publications (2)

Publication Number Publication Date
CN113093742A true CN113093742A (zh) 2021-07-09
CN113093742B CN113093742B (zh) 2023-05-26

Family

ID=76670999

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110341065.2A Active CN113093742B (zh) 2021-03-30 2021-03-30 一种自动规避多障碍物的无人船路径跟踪系统

Country Status (1)

Country Link
CN (1) CN113093742B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114217603A (zh) * 2021-11-05 2022-03-22 大连海事大学 一种多无人船编队安全优化控制系统
CN114408122A (zh) * 2022-01-27 2022-04-29 大连海事大学 一种船舶防碰撞控制系统及其设计方法
CN115494848A (zh) * 2022-09-30 2022-12-20 大连海事大学 一种规避动静态障碍物的无人船路径跟踪视距制导方法及系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7957858B1 (en) * 2007-05-07 2011-06-07 The United States Of America As Represented By The Secretary Of The Navy Method for determining projected obstacle areas for moving obstacles
US20170193830A1 (en) * 2016-01-05 2017-07-06 California Institute Of Technology Controlling unmanned aerial vehicles to avoid obstacle collision
CN107168335A (zh) * 2017-06-28 2017-09-15 大连海事大学 一种考虑混合多目标避障的水面无人艇路径跟踪制导方法
CN110703765A (zh) * 2019-11-07 2020-01-17 大连海事大学 一种无人船的碰撞自规避方法及系统

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7957858B1 (en) * 2007-05-07 2011-06-07 The United States Of America As Represented By The Secretary Of The Navy Method for determining projected obstacle areas for moving obstacles
US20170193830A1 (en) * 2016-01-05 2017-07-06 California Institute Of Technology Controlling unmanned aerial vehicles to avoid obstacle collision
CN107168335A (zh) * 2017-06-28 2017-09-15 大连海事大学 一种考虑混合多目标避障的水面无人艇路径跟踪制导方法
CN110703765A (zh) * 2019-11-07 2020-01-17 大连海事大学 一种无人船的碰撞自规避方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
范云生;孙晓界;王国峰;郭晨;: "一种无人水面艇自主动态避碰跟踪控制方法", 系统仿真学报 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114217603A (zh) * 2021-11-05 2022-03-22 大连海事大学 一种多无人船编队安全优化控制系统
CN114217603B (zh) * 2021-11-05 2024-04-30 大连海事大学 一种多无人船编队安全优化控制系统
CN114408122A (zh) * 2022-01-27 2022-04-29 大连海事大学 一种船舶防碰撞控制系统及其设计方法
CN115494848A (zh) * 2022-09-30 2022-12-20 大连海事大学 一种规避动静态障碍物的无人船路径跟踪视距制导方法及系统
CN115494848B (zh) * 2022-09-30 2024-05-03 大连海事大学 一种规避动静态障碍物的无人船路径跟踪视距制导方法及系统

Also Published As

Publication number Publication date
CN113093742B (zh) 2023-05-26

Similar Documents

Publication Publication Date Title
Wang et al. Dynamics-level finite-time fuzzy monocular visual servo of an unmanned surface vehicle
CN113093742A (zh) 一种自动规避多障碍物的无人船路径跟踪系统
CN109765892B (zh) 一种无人船集群的碰撞自规避编队控制器结构及设计方法
CN107168312B (zh) 一种补偿uuv运动学和动力学干扰的空间轨迹跟踪控制方法
CN105807789B (zh) 基于t-s模糊观测器补偿的uuv控制方法
CN106774400B (zh) 一种基于逆动力学的无人机三维轨迹制导方法
CN106708068B (zh) 仿生波动鳍推进水下航行器路径跟踪控制方法
CN112650231B (zh) 一种实现避碰和避障的欠驱船编队控制系统
CN111857165B (zh) 一种水下航行器的轨迹跟踪控制方法
CN113093804B (zh) 基于反演滑模控制的无人艇编队控制方法和控制系统
CN113467231A (zh) 基于侧滑补偿ilos制导律的无人艇路径跟踪方法
He et al. Robust orientation-sensitive trajectory tracking of underactuated autonomous underwater vehicles
CN115903800A (zh) 一种基于领导者协同的多无人船编队的严格安全控制方法
CN112015086A (zh) 一种欠驱动水面船有限时间路径跟踪输出反馈控制方法
CN114995409A (zh) 一种无人船自主巡航控制系统及巡航方法
Zhu et al. Finite time PAILOS based path following control of underactuated marine surface vessel with input saturation
Sun et al. An innovative distributed self-organizing control of unmanned surface vehicle swarm with collision avoidance
Rigatos et al. Input–output linearizing control of the underactuated hovercraft using the derivative-free nonlinear kalman filter
Salgado-Jiménez et al. Control of ROVs using a model-free 2nd-order sliding mode approach
Silpa-Anan et al. Preliminary experiments in visual servo control for autonomous underwater vehicle
Mei et al. A hybrid artificial potential field method for autonomous surface vessel path planning in dynamic riverine environment
CN108459614B (zh) 一种基于cw-rnn网络的uuv实时避碰规划方法
Mousazadeh et al. Experimental evaluation of a new developed algorithm for an autonomous surface vehicle and comparison with simulink results
CN115480580A (zh) 一种基于nmpc的水下机器人路径跟踪与避障的控制方法
Tian et al. Development of a guidance and control system for an underwater plume exploring AUV

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