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

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

Info

Publication number
CN113093742B
CN113093742B CN202110341065.2A CN202110341065A CN113093742B CN 113093742 B CN113093742 B CN 113093742B CN 202110341065 A CN202110341065 A CN 202110341065A CN 113093742 B CN113093742 B CN 113093742B
Authority
CN
China
Prior art keywords
unmanned ship
unit
estimate
speed
formula
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
Application number
CN202110341065.2A
Other languages
English (en)
Other versions
CN113093742A (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 (6)

1.一种自动规避多障碍物的无人船路径跟踪系统，其特征在于：包括，
避碰控制模块，用于获取无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ以及第i个环境障碍物在地球坐标系下的x轴位置xic、y轴位置yic以计算避碰控制角速度rc；
所述避碰控制模块包括，
计算单元，用于获取无人船在大地坐标系下的x轴位置x、y轴位置y以及第i个环境障碍物在地球坐标系下的x轴位置xic、y轴位置yic以计算无人船与第i个环境障碍物之间的距离ρi和无人船和第i个环境障碍物之间的视线角度χi；
非线性微分控制器单元Ⅰ，用于获取所述无人船与第i个环境障碍物之间的距离ρi以计算无人船和第i个环境障碍物之间的距离导数的估计值
Figure FDA0004168075200000011
比例单元Ⅰ，用于获取所述无人船和第i个环境障碍物之间的距离导数的估计值
Figure FDA0004168075200000012
和无人船与第i个环境障碍物之间的距离ρi以计算碰撞时间Tic；
比较单元，用于获取所述碰撞时间Tic以计算参数变量ξi，
加法器单元，用于获取所述无人船和第i个环境障碍物之间的视线角度χi和无人船在大地坐标系下的艏摇角ψ以计算无人船在船体坐标系下的纵向速度u和第i个环境障碍物的速度uic之间的夹角δi；以及，
比例单元Ⅱ，用于获取所述无人船在船体坐标系下的纵向速度u和第i个环境障碍物的速度uic之间的夹角δi与参数变量ξi以计算避碰控制角速度rc；
LOS制导单元，用于获取所述避碰控制角速度rc、无人船的纵向速度估计值
Figure FDA0004168075200000013
和横漂速度估计值/>
Figure FDA0004168075200000014
以及无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ以计算前向速度给定值αu和艏摇角速度给定值αr；
所述LOS制导单元包括制导律设计单元和参数化路径单元；
所述参数化路径单元，用于将无人船的给定参数化路径输入至制导律设计单元；
所述制导律设计单元，用于获取所述无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ、所述避碰控制角速度rc、无人船的纵向速度估计值
Figure FDA0004168075200000015
横漂速度估计值/>
Figure FDA0004168075200000016
以及无人船的给定参数化路径以计算所述前向速度给定值αu和艏摇角速度给定值αr；
非线性微分控制器单元Ⅱ，用于获取所述前向速度给定值αu和艏摇角速度给定值αr以计算前向速度给定值αu的估计值uf、前向速度给定值的导数
Figure FDA0004168075200000017
的估计值/>
Figure FDA0004168075200000018
艏摇角速度给定值αr的估计值rf以及艏摇角速度给定值的导数/>
Figure FDA0004168075200000019
的估计值/>
Figure FDA00041680752000000110
观测器单元，用于获取无人船在大地坐标系下的x轴位置x、y轴位置y和艏摇角ψ、无人船的纵向方向的控制力矩τu和艏摇方向的控制力矩τr以计算无人船的纵向速度估计值
Figure FDA0004168075200000021
横漂速度估计值/>
Figure FDA0004168075200000022
艏摇角速度估计值/>
Figure FDA0004168075200000023
纵向扰动估计值/>
Figure FDA0004168075200000024
以及艏摇方向扰动估计值/>
Figure FDA0004168075200000025
以及，
动力学控制器单元，用于获取所述前向速度给定值αu的估计值uf、前向速度给定值的导数
Figure FDA0004168075200000026
的估计值/>
Figure FDA0004168075200000027
艏摇角速度给定值αr的估计值rf、艏摇角速度给定值的导数/>
Figure FDA0004168075200000028
的估计值
Figure FDA0004168075200000029
纵向速度估计值/>
Figure FDA00041680752000000210
艏摇角速度估计值/>
Figure FDA00041680752000000211
纵向扰动估计值/>
Figure FDA00041680752000000212
以及艏摇方向扰动估计值/>
Figure FDA00041680752000000213
以计算所述纵向方向的控制力矩τu和艏摇方向的控制力矩τr并将所述纵向方向的控制力矩τu和艏摇方向的控制力矩τr输入至无人船。
2.根据权利要求1所述的自动规避多障碍物的无人船路径跟踪系统，其特征在于：所述无人船与第i个环境障碍物之间的距离ρi通过公式(1)进行计算：
Figure FDA00041680752000000214
所述无人船和第i个环境障碍物之间的视线角度χi通过公式(2)进行计算：
Figure FDA00041680752000000215
所述无人船和第i个环境障碍物之间的距离导数的估计值
Figure FDA00041680752000000216
通过公式(3)进行计算：
Figure FDA00041680752000000217
其中：γiρ为设计的比例参数；ρif是ρi的估计值；
Figure FDA00041680752000000218
是/>
Figure FDA00041680752000000219
的估计值；/>
Figure FDA00041680752000000220
为无人船与第i个环境障碍物之间的距离ρi的导数；
所述碰撞时间Tic通过公式(4)进行计算：
Figure FDA00041680752000000221
所述参数变量ξi通过公式(5)进行计算：
Figure FDA00041680752000000222
所述无人船在船体坐标系下的纵向速度u和第i个环境障碍物的速度uic之间的夹角δi通过公式(6)进行计算：
δi＝χi-ψ+π/2 (6)
所述避碰控制角速度rc通过公式(7)进行计算：
Figure FDA0004168075200000031
其中：kic和ki为控制增益，n为环境障碍物的个数。
3.根据权利要求2所述的自动规避多障碍物的无人船路径跟踪系统，其特征在于：计算所述前向速度给定值αu和艏摇角速度给定值αr包括以下步骤：
通过公式(8)计算期望路径切向角ψd：
Figure FDA0004168075200000032
通过公式(9)计算横向跟踪误差xe和纵向跟踪误差ye：
Figure FDA0004168075200000033
通过公式(10)和公式(11)计算前向速度给定值αu、艏摇角速度给定值αr以及参考制导信号αψ：
Figure FDA0004168075200000034
Figure FDA0004168075200000035
其中：
Figure FDA0004168075200000036
υs是参考速度；ψe＝ψ-αψ为航向跟踪误差；/>
Figure FDA0004168075200000037
是侧滑角；Δ为前向距离；/>
Figure FDA0004168075200000038
Figure FDA0004168075200000039
Δx和Δψ为设定参数；/>
Figure FDA00041680752000000310
为中间变量；ku和kr是设计参数。
4.根据权利要求3所述的自动规避多障碍物的无人船路径跟踪系统，其特征在于：所述前向速度给定值αu的估计值uf、前向速度给定值的导数
Figure FDA00041680752000000311
的估计值/>
Figure FDA00041680752000000312
艏摇角速度给定值αr的估计值rf以及艏摇角速度给定值的导数/>
Figure FDA00041680752000000313
的估计值/>
Figure FDA00041680752000000314
通过公式(12)和公式(13)进行计算：
Figure FDA00041680752000000315
Figure FDA0004168075200000041
其中：γu和γr为设计的比例参数；
Figure FDA0004168075200000042
分别是/>
Figure FDA0004168075200000043
的估计值。
5.根据权利要求4所述的自动规避多障碍物的无人船路径跟踪系统，其特征在于：所述无人船的纵向速度估计值
Figure FDA0004168075200000044
横漂速度的估计值/>
Figure FDA0004168075200000045
纵向扰动估计值/>
Figure FDA0004168075200000046
以及艏摇方向扰动估计值/>
Figure FDA0004168075200000047
通过公式(14)进行计算：
Figure FDA0004168075200000048
/>
其中：η＝[x,y,ψ]是无人船的位置和角度；
Figure FDA0004168075200000049
是无人船位置和角度的观测值；
Figure FDA00041680752000000410
为观测器单元的输出信号；K1,K2,K3为三维观测器增益矩阵；R(ψ)为坐标变换矩阵；M为无人船惯性矩阵。
6.根据权利要求5所述的自动规避多障碍物的无人船路径跟踪系统，其特征在于：所述无人船在纵向方向的控制力矩τu和艏摇方向的控制力矩τr通过公式(15)进行计算：
Figure FDA00041680752000000411
其中：
Figure FDA00041680752000000412
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 CN113093742A (zh) 2021-07-09
CN113093742B true 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)

Families Citing this family (3)

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

Citations (3)

* 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
CN107168335A (zh) * 2017-06-28 2017-09-15 大连海事大学 一种考虑混合多目标避障的水面无人艇路径跟踪制导方法
CN110703765A (zh) * 2019-11-07 2020-01-17 大连海事大学 一种无人船的碰撞自规避方法及系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10665115B2 (en) * 2016-01-05 2020-05-26 California Institute Of Technology Controlling unmanned aerial vehicles to avoid obstacle collision

Patent Citations (3)

* 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
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
一种无人水面艇自主动态避碰跟踪控制方法;范云生;孙晓界;王国峰;郭晨;;系统仿真学报(第10期);全文 *

Also Published As

Publication number Publication date
CN113093742A (zh) 2021-07-09

Similar Documents

Publication Publication Date Title
CN113093742B (zh) 一种自动规避多障碍物的无人船路径跟踪系统
CN105955268B (zh) 一种考虑局部避碰的uuv动目标滑模跟踪控制方法
CN109765892B (zh) 一种无人船集群的碰撞自规避编队控制器结构及设计方法
WO2020253028A1 (zh) 一种基于航迹重规划的水面无人艇动态避碰方法
Choi et al. Enhanced SLAM for a mobile robot using extended Kalman filter and neural networks
CN107329477A (zh) 一种无人船导航和自动驾驶设备及其方法
CN106094606A (zh) 一种水面无人艇导航与控制遥操作平台
CN109583144B (zh) 一种无人海洋航行器的动力学优化控制器结构及设计方法
CN112650231B (zh) 一种实现避碰和避障的欠驱船编队控制系统
CN111026135B (zh) 一种无人艇高性能航行前馈控制系统及其控制方法
CN109933074B (zh) 一种有领导者的多无人船群集运动控制器结构的设计方法
Pêtrès et al. Reactive path planning for autonomous sailboat
CN111679669A (zh) 一种无人船自主精准停靠方法及系统
He et al. Robust orientation-sensitive trajectory tracking of underactuated autonomous underwater vehicles
CN113093804B (zh) 基于反演滑模控制的无人艇编队控制方法和控制系统
CN114061565A (zh) 一种无人船舶slam及其应用方法
Zhang et al. Research on the maneuverability and path following control of the wave glider with a propeller-rudder system
Krupinski et al. Pool testing of auv visual servoing for autonomous inspection
CN115903800A (zh) 一种基于领导者协同的多无人船编队的严格安全控制方法
Mei et al. A hybrid artificial potential field method for autonomous surface vessel path planning in dynamic riverine environment
Mishra et al. Cooperative relative pose estimation to aid landing of an unmanned aerial vehicle on a moving platform
Mousazadeh et al. Experimental evaluation of a new developed algorithm for an autonomous surface vehicle and comparison with simulink results
CN113110458B (zh) 一种无人船虚拟目标跟踪控制系统
CN115480580A (zh) 一种基于nmpc的水下机器人路径跟踪与避障的控制方法
CN112799414B (zh) 一种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