一种基于动态虚拟小船制导算法的欠驱动船舶路径跟踪规划方法技术

本发明专利技术公开了一种基于动态虚拟小船制导算法的欠驱动船舶路径跟踪规划方法，包括以下步骤：S1：建立导引虚拟小船数学模型，根据导引虚拟小船数学模型采用制导算法规划出船舶跟踪控制参考路径，获取导引虚拟小船产生参考路径的命令时间序列信号；S2：进入路径跟踪控制状态：S3：根据实际船舶与导引虚拟小船的相对位置动态配置动态虚拟小船的船舶位置和运动姿态；S4：根据鲁棒自适应控制策略设计控制律，即实船的主机转速nc和舵角命令信号δc，控制实船主机和舵机执行装置，驱动控制实船跟踪动态虚拟小船，最终实现船舶在航海实践中的路径跟踪控制任务；S5：测量实际船舶位置判断是否到达目的地，如果“是”，结束船舶航行；如果“否”，时间更新t＝t+1并进入S2。

【技术实现步骤摘要】
一种基于动态虚拟小船制导算法的欠驱动船舶路径跟踪规划方法
本专利技术涉及船舶控制工程与船舶自动化航行领域，尤其涉及一种基于动态虚拟小船制导算法的欠驱动船舶路径跟踪规划方法。
技术介绍
目前已有的制导算法是从导弹射击制导研究领域引入到船舶导航领域中，即我们通常所说的Lineofsight(LOS)算法。它解决了间接航迹保持控制中利用航迹偏差和航向偏差计算船舶引导航向的问题，也是目前船载自动操舵仪中较为广泛采用的算法，下面我们对制导算法做简单的解释和说明。可变的LOS半径和航路点切换边界环是LOS算法中两个重要的概念或变量。LOS算法的目的是演绎出航迹保持控制中完成控制任务需求的航向参考命令，即ψlos(方位角)。利用现有的船舶航向保持控制器驱动转舵执行装置，使船舶航向跟踪参考命令ψlos，减小航迹偏差，最终实现船舶沿着计划航线自动航行。图1给出了LOS算法的基本制导原理图。在航海实践中，计划航线通常由驾驶员通过设置航路点进行设计，如图1中的(xi-1,yi-1),(xi,yi)和(xi+1,yi+1)。以当前船位(x,y)为中心，以LOS半径R画弧，在LOS半径选取恰当的前提下能够交计划航线Pi-1Pi于点AB和AF。起始于当前船位(x,y)终止于点AF的线段定义为LOS线，所对应的方位角即为ψlos，可由式(1)计算且ψlos∈<-π,π>。通常情况下R有3种取法：1)传统LOS算法：R＝αLpp，Lpp为船舶的两柱间长。这种情况下必须要求α＞1，否则船舶将绕着计划航线频繁转圈；2)改进LOS算法：R＝d+Lpp，d为当前船位到计划航向的投影距离，即航迹偏差。这样选择能够保证R＞d恒成立，保证LOS线始终存在；3)指数收敛LOS算法，如式(2)所示。其中，lambertw是函数f(x)＝xex的反函数，选取最小LOS半径Rmin＝1.7Lpp，系数b为指数收敛因子，b＝0.05。LOS制导算法能够实现直线航线对船舶的导航引导作用，最终实现航迹保持控制任务。但船舶驶向航路点附近时，即图1中Pi位置处，需要进行航路点转换。这就涉及到了航路点切换边界环的概念，即以(xi,yi)为中心以δi为半径的圆形区域。但船舶进入到该区域内，参考计划航线就由航线段{(xi-1,yi-1)，(xi,yi)}切换到航线段{(xi,yi)，(xi+1,yi+1)}，切换条件为式(3)。在航海实践中，参考路径(即计划航线)是由驾驶员通过设置航路点来确定，以引导船舶在公海上按照预设航线航行。参考路径不仅包括直线段，还包括转向部分的曲线段。已有技术LOS制导算法主要思想是通过航迹偏差计算出参考航向，通过航向保持控制引导船舶沿着计算出的参考航向航行，间接引导船舶沿着计划参考路径航行，实现航迹保持控制。由于常规水面船舶运动具有大惯性、欠驱动等特点，时间常数为几十秒甚至可达到几百秒，近年来，国内外学者在欠驱动船舶路径跟踪控制方面做了大量研究工作。这些成果都基于一个前提假设“任何参考路径可以由虚拟小船产生，包括直线和光滑曲线”，所解决的问题集中于欠驱动船舶能够有效跟踪虚拟小船产生的运动轨迹，从而实现路径跟踪控制。然而，关于如何解决虚拟小船和航路点之间的路径规划问题在当前研究成果中少有提及。基于以上分析，LOS制导算法应用于欠驱动船舶路径跟踪规划方法主要存在以下2点缺陷：1、LOS算法只解决了直线段的航迹保持控制问题，航路点位置附近采用自动转向，并没有进行有效的航迹保持控制。2、LOS算法不能直接应用于已有的路径跟踪控制研究成果，解决航海实践中的船舶航迹保持控制问题(包括直线和光滑曲线)。尽管欠驱动船舶路径跟踪控制研究在一定假设基础上解决了船舶对虚拟小船运动轨迹的跟踪，但虚拟小船与参考路径之间的路径规划问题没有得到有效解决。
技术实现思路
根据现有技术存在的问题，本专利技术公开了一种基于动态虚拟小船制导算法的欠驱动船舶路径跟踪规划方法，包括以下步骤：S1：设定航路点信息W1,W2,…，Wn，建立导引虚拟小船数学模型，设定航行速度ur，根据导引虚拟小船数学模型采用制导算法规划出船舶跟踪控制参考路径，获取导引虚拟小船产生参考路径的命令时间序列信号；S2：进入路径跟踪控制状态：根据S1中获取的导引虚拟小船产生参考路径的命令时间序列信号，利用导引虚拟小船数学模型实时获取导引虚拟小船的船舶位置和运动姿态；S3：根据实际船舶与导引虚拟小船的相对位置动态配置动态虚拟小船的船舶位置和运动姿态；S4：根据鲁棒自适应控制策略设计控制律即实船的主机转速nc和舵角命令信号δc，控制实船主机和舵机执行装置，驱动控制实船跟踪动态虚拟小船，最终实现船舶在航海实践中的路径跟踪控制任务；S5：测量实际船舶位置判断是否到达目的地，如果“是”，结束船舶航行；如果“否”，时间更新t＝t+1并进入S2。进一步的，S1中：导引虚拟小船数学模型采用如下公式(4)表示：其中：Wi为计划航路点、对应的坐标值用(xiyi)表示；(xryr)和ψr分别为导引虚拟小船的位置坐标和艏向角，ur和rr分别为导引虚拟小船前进速度命令信号和转首角速率命令信号；当参考路径为直线时：ur＝constant,rrLi＝0,trLi＝distance/ur，(i＝1,2,…n-1)，distance代表直线段航线的长度，其中：rrLi和trLi为参考路径为直线时导引虚拟小船的命令信号及对应的时间序列；当参考路径为曲线时：将转向点部分过渡曲线作为圆弧计算，rrCi为非零常量，利用公式(5)求每一段直线航线的方位角φi-1,i和φi,i+1，转向偏差Δφi＝φi,i+1-φi-1,i其中：]Δφi∈(0,π/2]转向半径Ri通过内插求取，Ri∈[Rmin,Rmax]当Δφi＞π/2，Ri＝Rmin；则当参考路径为光滑曲线时，ur＝constant,rrCi＝ur/Ri,trCi＝Δφi/rrCi,(i＝2,3,…n-1)；其中：φi-1,i为直线航线Wi-1Wi的方位角，rrCi和trCi为参考路径为曲线时导引虚拟小船的命令信号及对应的时间序列。进一步的，S3中：动态配置动态虚拟小船的船舶位置和运动姿态具体采用如下方式：设实际船舶与导引虚拟小船之间的距离为Ze，实船与动态虚拟小船之间的距离为ldb；当Ze≥ldbset，ldbset为设定的参数阈值，动态虚拟小船首向角ψd和船舶位置(xdyd)分别采用如下公式(6)和公式(7)计算：xd＝x+ldbsetcos(ψd),yd＝y+ldbsetsin(ψd)(7)当Ze＜ldbset时，动态虚拟小船位置与导引虚拟小船位置重合，即：xd＝xr,yd＝yr，艏向角ψd仍采用公式(6)计算。进一步的，S4中设计控制律时：动态虚拟小船相关微分变量通过公式(8)所示的参考模型解决：其中：(xy)和ψ为实际船舶的位置和艏向角，分别为xds和yds对应变量的导数，τ为时间常数。由于采用了上述技术方案，本专利技术提供的基于动态虚拟小船制导算法的欠驱动船舶路径跟踪规划方法，采用制导算法解决了实际船舶对虚拟小船运动轨迹的跟踪，在跟踪过程中解决了虚拟小船与参考路径之间的规划问题。本专利技术还具有以下有益效果：利用本专利技术公开的制导算法解决了“LOS算法只能用于直线段航迹保持控制问题”的缺本文档来自技高网...

【技术保护点】
一种基于动态虚拟小船制导算法的欠驱动船舶路径跟踪规划方法，其特征在于包括以下步骤：S1：设定航路点信息W1,W2,…，Wn，建立导引虚拟小船数学模型，设定航行速度ur，根据导引虚拟小船数学模型采用制导算法规划出船舶跟踪控制参考路径，获取导引虚拟小船产生参考路径的命令时间序列信号；S2：进入路径跟踪控制状态：根据S1中获取的导引虚拟小船产生参考路径的命令时间序列信号，利用导引虚拟小船数学模型实时获取导引虚拟小船的船舶位置和运动姿态；S3：根据实际船舶与导引虚拟小船的相对位置动态配置动态虚拟小船的船舶位置和运动姿态；S4：根据鲁棒自适应控制策略设计控制律即实船的主机转速nc和舵角命令信号δc，控制实船主机和舵机执行装置，驱动控制实船跟踪动态虚拟小船，最终实现船舶在航海实践中的路径跟踪控制任务；S5：测量实际船舶位置判断是否到达目的地，如果“是”，结束船舶航行；如果“否”，时间更新t＝t+1并进入S2。

【技术特征摘要】
1.一种基于动态虚拟小船制导算法的欠驱动船舶路径跟踪规划方法，其特征在于包括以下步骤：S1：设定航路点信息W1,W2,…，Wn，建立导引虚拟小船数学模型，设定航行速度ur，根据导引虚拟小船数学模型采用制导算法规划出船舶跟踪控制参考路径，获取导引虚拟小船产生参考路径的命令时间序列信号；S2：进入路径跟踪控制状态：根据S1中获取的导引虚拟小船产生参考路径的命令时间序列信号，利用导引虚拟小船数学模型实时获取导引虚拟小船的船舶位置和运动姿态；S3：根据实际船舶与导引虚拟小船的相对位置动态配置动态虚拟小船的船舶位置和运动姿态；S4：根据鲁棒自适应控制策略设计控制律即实船的主机转速nc和舵角命令信号δc，控制实船主机和舵机执行装置，驱动控制实船跟踪动态虚拟小船，最终实现船舶在航海实践中的路径跟踪控制任务；S5：测量实际船舶位置判断是否到达目的地，如果“是”，结束船舶航行；如果“否”，时间更新t＝t+1并进入S2。2.根据权利要求1所述的一种基于动态虚拟小船制导算法的欠驱动船舶路径跟踪规划方法，其特征还在于：S1中：导引虚拟小船数学模型采用如下公式(4)表示：其中：Wi为计划航路点、对应的坐标值用(xiyi)表示；(xryr)和ψr分别为导引虚拟小船的位置坐标和艏向角，ur和rr分别为导引虚拟小船前进速度命令信号和转首角速率命令信号；当参考路径为直线时：ur＝constant,rrLi＝0,trLi＝distance/ur，(i＝1,2,…n-1)，distance代表直线段航线的长度，其中：rrLi和trLi为参考路径为直线时导引虚拟小船的命令信号及对应的时间序列；当参考路径为曲线时：将转向点部分过渡曲线作为圆弧计算，rrCi为非零常量，利用如下公式(5)求每一段直线航线的方位角φi-1,i和φi,i+1，转向偏差Δ...

【专利技术属性】
技术研发人员：张国庆，张显库，尹勇，金一丞，庞洪帅，
申请(专利权)人：大连海事大学，
类型：发明
国别省市：辽宁;21