CN113296505B - 一种基于速变los的无人船多模式路径跟踪控制方法 - Google Patents
一种基于速变los的无人船多模式路径跟踪控制方法 Download PDFInfo
- Publication number
- CN113296505B CN113296505B CN202110548443.4A CN202110548443A CN113296505B CN 113296505 B CN113296505 B CN 113296505B CN 202110548443 A CN202110548443 A CN 202110548443A CN 113296505 B CN113296505 B CN 113296505B
- Authority
- CN
- China
- Prior art keywords
- path
- unmanned ship
- psi
- tracking
- mode
- 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
- 238000000034 method Methods 0.000 title claims abstract description 35
- 230000008859 change Effects 0.000 title claims abstract description 31
- 230000008569 process Effects 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 30
- 230000004044 response Effects 0.000 claims description 18
- 230000035772 mutation Effects 0.000 claims description 11
- 238000004422 calculation algorithm Methods 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 claims description 7
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 7
- 239000004243 E-number Substances 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 3
- 238000013016 damping Methods 0.000 claims description 3
- 230000005611 electricity Effects 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
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
技术领域
本发明涉及无人船航行控制领域,具体为一种基于速变LOS的无人船多模式路径跟踪控制方法。
背景技术
目前,无人船在海洋测绘、水质采样、环境监测、水文探测、海上搜救等诸多领域发挥了越来越大的作用,其具有广阔的发展空间与应用前景,涵盖的技术领域非常广泛。具备自动化功能的无人船为相关团队的实验提供了便利,但在执行任务过程中,作为实验平台的无人船有着更多功能和模式的需求。很多现有的无人船跟踪方法通常只能一直正向跟踪路径,对路径参数更新具有严格的要求,不能在跟踪过程中实时干预并调整已经设定好的跟踪方向和路径信息来实现反向跟踪和改变路径形状。
中国专利CN 11506086 A公开了一种改进LOS引导律结合模糊PID的无人船路径跟踪控制方法,该方法通过增加变速控制律到LOS引导律以及采用模糊PID设计控制器,在一定程度上降低了航速对无人船路径跟踪控制的影响。该方法采取了有限路径点来进行路径的跟踪,通过判断与下一个点的距离来跟新跟踪的路径点,这种方法没有考虑到切换路径点数目过少时切换路径点会给控制效果带来不好的影响,并且没有严格跟踪设置好的路径,即不能有效的跟踪第一个路径点。
在传统的LOS路径跟踪方法中,由于设定期望纵向速度为定值且没有充分利用到路径信息。这使得在无人船经过路径切向角突变的路径点时,原有的纵向速度太大导致控制算法不能及时调整姿态来很好的跟踪路径,在路径切向角突变的路径点附近产生了过冲现象,导致跟踪效果不良。
发明内容
为了解决现有技术的上述问题,本发明目的在于提供一种基于速变LOS的无人船多模式路径跟踪控制方法,能够在跟踪过程中实时干预并调整跟踪模式、应对严格跟踪需求和克服路径信息有限并且在路径切向角突变点处有效减轻过冲现象的路径跟踪控制方法。
本发明的目的至少通过如下技术方案之一实现。
一种基于速变LOS的无人船多模式路径跟踪控制方法,包括以下步骤:
S1、建立惯性坐标系下固定双桨推进无人船的三自由度运动学和动力学模型;
S2、构建无人船多模式路径跟踪控制***;
S3、实时对无人船多模式路径跟踪控制***中的多模式决策模块下达指令,实现无人船多模式路径跟踪控制。
进一步地,步骤S1中,运动学与动力学模型的建立过程如下:
建立惯性坐标系下固定双桨推进无人船的三自由度运动学和动力学模型,其中,Oexeye是建立在地球上的惯性坐标系,船体坐标系是以船体中心为原点,纵向指向船艏并平行水面,横向垂直于船艏并平行于水面,在地球上的惯性坐标系下无人船的运动学和动力学模型为:
其中,ψ是无人船当前的艏摇角,u,v,r分别表示船体坐标系中的纵向速度、横向速度和艏摇角速度;τu和τr分别表示无人船动力装置产生的纵向推力和横向力矩,τ1,τ2,τ3分别表示无人船在纵向、横向、艏向上受到的干扰力,mii和dii分别为实验无人船的给定惯性系数和阻尼系数;由于不考虑外界干扰,τ1,τ2,τ3设置为0;为无人船当前的位置(x,y)的对应导数,和为对应的导数;
其中τu=f1+f2,τr=(f1-f2)·B/2;B表示无人船双桨的横向距离,f1和f2分别表示两个螺旋桨的推力;
由于固定双桨推进无人船以两个直流电机为推进***,两个螺旋桨分别对应两个直流电机,螺旋桨推力和电调输出电压有如下线性关系:f=kV;
结合上述方程最终得到实际的控制输入:左电机控制电压VL和右电机控制电压VR分别为:
VL=(2·τr/B+τu)/2k,VR=(-2·τr/B+τu)/2k;
则动力学方程进一步改写为:
进一步地,步骤S3中所述无人船多模式路径跟踪控制***包括路径拓展模块、严格路径选择模块和多模式决策模块;
路径拓展模块根据拓展后的路径点数目和输入路径点数目计算得到插值步长,接收多模式决策模块传递的信号,对输入路径信息分别进行分段线性插值、球面插值得到拓展后的路径信息;
严格路径选择模块根据拓展后路径信息设置第一虚拟任务点,根据拓展后的路径信息和第一虚拟任务点通过路径参数更新律、艏摇角制导律和纵向速度制导律得到当前跟踪路径参数、路径切向角矩阵、期望艏摇角和纵向速度;
多模式决策模块根据第一虚拟任务点、期望艏摇角和纵向速度,通过第一PID控制器、第二PID控制器控制无人船的运动;
无人船在跟踪路径时,多模式决策模块实时接收指令使无人船多模式路径跟踪控制***实现不同的功能。
进一步地,路径拓展模块中,给定拓展后的路径点数目Ne,路径信息输入点的个数Nn,设计插值步长:
其中,(xi,yi)是pathin插值前的第i个路径点坐标,(xθ,yθ)为pathin经过插值后的第θ个路径点的坐标,θ表示一个与时间无关的路径参数,拓展后的路径信息为最终无人船的跟踪路径信息。
进一步地,严格路径选择模块中,第一虚拟任务点设置为:
进一步地,路径参数更新律和艏摇角制导律具体如下:
给定一条期望路径的路径信息和第一虚拟任务点,通过位置误差计算得到路径参数更新律和三种期望艏摇角;
给定拓展后的路径信息矩阵pathinterp和无人船当前的位置(x,y),由此得出xe轴路径信息差分矩阵xdiff(θ)和ye轴路径信息差分矩阵ydiff(θ)为:
得到路径切向角矩阵ψd(θ)为:
ψd(θ)=atan2(ydiff(θ),xdiff(θ));
定义横向跟踪误差矩阵xe(θ)和纵向跟踪误差矩阵ye(θ)为:
设计跟踪误差矩阵derr(θ)为:
derr(θ)=xe(θ)·xe(θ)+ye(θ)·ye(θ);
则当前跟踪路径参数θnow更新律具体如下:
艏摇角制导律如下:
其中ψr1为正向跟踪期望艏摇角,ψr2为反向跟踪期望艏摇角,ψr3为第一虚拟任务点期望艏摇角,n×l为无人船的可视距离,n为整数,l为无人船的长度,ψr1中θnow取argminderr(θ)+1,ψr2中θnow取argminderr(θ)-1。
进一步地,纵向速度制导律具体如下:
由路径切向角计算得到路径切向角导数,选取路径切向角导数大于定值的点为路径切向角突变点;当无人船进入路径切向角突变点一定范围内时,根据相对路径切向角突变点的距离设置期望纵向速度,越接近路径切向角突变点期望纵向速度越低;
给定当前的跟踪路径参数θnow、路径切向角差分矩阵ψdiff(θ)和初始设定的期望纵向速度up;由|ψdiff(θ)|>ψchange得到切向角突变参数矩阵θchange,其中ψchange为设定的突变角;
纵向速度响应距离为:
dres=min|θchange-θnow|;
根据纵向速度响应距离得到纵向速度制导律为:
其中,umin为设定好的最低期望纵向速度,dmax为响应限制距离且有其中kres为响应限制参数,dmid为响应中间距离,ures为路径响应后的期望纵向速度,最后由第二PID控制器根据误差ures-u得到输出τu。
进一步地,多模式决策模块通过接收指令使无人船多模式路径跟踪控制***进行严格路径选择、反向跟踪返航或改变路径拓展模式功能;
多模式决策模块接收严格路径选择模块、艏摇角制导律传递过来的第一虚拟任务点(x0,y0)Τ以及三种期望艏摇角ψr1、ψr2和ψr3,根据接收指令选择是否使能严格路径选择功能、反向跟踪返航功能以及改变路径拓展模式功能,即从ψr1、ψr2和ψr3中选择传递到第一PID控制器的多模式决策后的期望艏摇角ψr以及传递信号给路径拓展模块选择插值算法得到不同的pathinterp,最后由第一PID控制器根据误差ψr-ψ得到输出ψr。
进一步地,严格路径选择功能如下:
当无人船执行严格路径选择功能时,无人船跟踪受到第一虚拟任务点(x0,y0)Τ的约束,跟踪目标锁定第一虚拟任务点,此时ψr=ψr3;当无人船经过第一虚拟任务点一定范围内时再从ψr1、ψr2中进行选择;
其中反向跟踪返航功能如下:
无人船正常正向跟踪时,选择ψr=ψr1;
当无人船执行反向跟踪返航功能时,选择ψr=ψr2。
其中改变路径拓展模式功能如下:
无人船在路径跟踪过程中可响应指令并传递信号给路径拓展模块,曲线跟踪时路径拓展模块切换为球面插值算法,直线跟踪时路径拓展模块切换为分段线性插值算法对路径信息pathin进行实时处理,得到不同的拓展后的路径信息pathinterp。
进一步地,通过LOS制导算出的期望艏摇角和真实艏摇角的取值区间不同,会引发误差计算错误的问题,为了传递正确的艏摇角误差ψr-ψ给第一PID控制器,设置一个误差纠正模块,具体如下:
若-π<ψr-ψ或ψr-ψ<=π成立,则传递ψr-ψ给第一PID控制器;
若-π<ψr-ψ或ψr-ψ<=π不成立,且ψr-ψ=-π成立,则传递ψr-ψ=π给第一PID控制器;
若-π<ψr-ψ、ψr-ψ<=π或ψr-ψ=-π不成立,且-π>ψr-ψ成立,则传递以下参数给第一PID控制器:
若-π<ψr-ψ、ψr-ψ<=π、ψr-ψ=-π或-π>ψr-ψ不成立,则传递以下参数给第一PID控制器:
本发明与现有技术比,其优点如下:
1)本发明采用的路径拓展模块克服了路径信息有限的不良条件,并可以平滑路径信息;
2)本发明采用的严格路径选择模块能够应对严格跟踪任务需求,在跟踪过程中严格跟踪初始路径点;
3)本发明采用的纵向速度制导律有效利用了路径信息并抑制了无人船在路径切向角突变处的过冲现象;
4)本发明采用的多模式决策模块可以对路径信息和跟踪顺序进行实时干预,实现了多模式路径跟踪控制。
附图说明
图1为本发明实施例坐标系示意图;
图2为本发明实施例方法流程图;
图3为本发明实施例多模式决策流程图;
图4和图5均为本发明实施例中的仿真对比图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白,下面结合附图并举实施例,对本发明的具体实施进行详细说明。
实施例:
一种基于速变LOS的无人船多模式路径跟踪控制方法,包括以下步骤:
S1、建立惯性坐标系下固定双桨推进无人船的三自由度运动学和动力学模型;
如图1所示,运动学与动力学模型的建立过程如下:
建立惯性坐标系下固定双桨推进无人船的三自由度运动学和动力学模型,其中,Oexeye是建立在地球上的惯性坐标系,船体坐标系是以船体中心为原点,纵向指向船艏并平行水面,横向垂直于船艏并平行于水面,在地球上的惯性坐标系下无人船的运动学和动力学模型为:
上述方程中,ψ是无人船当前的艏摇角,u,v,r分别表示船体坐标系中纵向、横向速度和艏摇角速度;τu和τr分别表示无人船动力装置产生的纵向推力和横向力矩,τ1,τ2,τ3分别表示无人船在纵向、横向、艏向上受到的干扰力,mii和dii分别为实验无人船的给定惯性系数和阻尼系数;由于不考虑外界干扰,τ1,τ2,τ3设置为0;为无人船当前的位置(x,y)的对应导数,和为对应的导数;本实施例中,m11=25.8、m22=33.8、m33=6.2、d11=12、d22=17、d33=0.5
其中τu=f1+f2,τr=(f1-f2)·B/2;B表示无人船双桨的横向距离,f1和f2分别表示两个螺旋桨的推力;
由于固定双桨推进无人船以两个直流电机为推进***,两个螺旋桨分别对应两个直流电机,螺旋桨推力和电调输出电压有如下线性关系:f=kV;
结合上述方程最终得到实际的控制输入:左电机控制电压VL和右电机控制电压VR分别为:
VL=(2·τr/B+τu)/2k,VR=(-2·τr/B+τu)/2k;
则动力学方程可以进一步改写为:
S2、构建无人船多模式路径跟踪控制***;
所述无人船多模式路径跟踪控制***包括路径拓展模块、严格路径选择模块和多模式决策模块;
路径拓展模块根据拓展后的路径点数目和输入路径点数目计算得到插值步长,接收多模式决策模块传递的信号,对输入路径信息分别进行分段线性插值、球面插值得到拓展后的路径信息;
严格路径选择模块根据拓展后路径信息设置第一虚拟任务点,根据拓展后的路径信息和第一虚拟任务点通过路径参数更新律、艏摇角制导律和纵向速度制导律得到当前跟踪路径参数、路径切向角矩阵、期望艏摇角和纵向速度;
多模式决策模块根据第一虚拟任务点、期望艏摇角和纵向速度,通过第一PID控制器、第二PID控制器控制无人船的运动
无人船在跟踪路径时,多模式决策模块实时接收指令使无人船多模式路径跟踪控制***实现不同的功能。
路径拓展模块中,给定拓展后的路径点数目Ne,路径信息输入点的个数Nn,设计插值步长:
其中,(xi,yi)是pathin插值前的第i个路径点坐标,(xθ,yθ)为pathin经过插值后的第θ个路径点的坐标,θ表示一个与时间无关的路径参数,拓展后的路径信息为最终无人船的跟踪路径信息。
严格路径选择模块中,第一虚拟任务点设置为:
路径参数更新律和艏摇角制导律具体如下:
给定一条期望路径的路径信息和第一虚拟任务点,通过位置误差计算得到路径参数更新律和三种期望艏摇角;
给定拓展后的路径信息矩阵pathinterp和无人船当前的位置(x,y),由此得出xe轴路径信息差分矩阵xdiff(θ)和ye轴路径信息差分矩阵ydiff(θ)为:
得到路径切向角矩阵ψd(θ)为:
ψd(θ)=atan2(ydiff(θ),xdiff(θ));
定义横向跟踪误差矩阵xe(θ)和纵向跟踪误差矩阵ye(θ)为:
设计跟踪误差矩阵derr(θ)为:
derr(θ)=xe(θ)·xe(θ)+ye(θ)·ye(θ)
则当前跟踪路径参数θnow更新律具体如下:
艏摇角制导律如下:
其中ψr1为正向跟踪期望艏摇角,ψr2为反向跟踪期望艏摇角,ψr3为第一虚拟任务点期望艏摇角,n×l为无人船的可视距离,n为整数,l为无人船的长度,ψr1中θnow取argminderr(θ)+1,ψr2中θnow取argminderr(θ)-1。
如图1所示为无人船与pathinterp上某一路径点的坐标示意图,图中具体标注了路径参数为的θ时无人船与该点的纵向误差ye(θ)、横向误差xe(θ),无人船的可视距离n×l,θ=θnow成立时的正向跟踪期望艏摇角ψr1、路径参数为θ处的路径切向角ψd(θ)。
纵向速度制导律具体如下:
由路径切向角计算得到路径切向角导数,选取路径切向角导数大于定值的点为路径切向角突变点;当无人船进入路径切向角突变点一定范围内时,根据相对路径切向角突变点的距离设置期望纵向速度,越接近路径切向角突变点期望纵向速度越低;
给定当前的跟踪路径参数θnow、路径切向角差分矩阵ψdiff(θ)和初始设定的期望纵向速度up;由|ψdiff(θ)|>ψchange得到切向角突变参数矩阵θchange,其中ψchange为设定的突变角;本实施例中,ψchange设置为π/6,即θchange得到了切向角差值大于π/6的路径参数信息。
纵向速度响应距离为:
dres=min|θchange-θnow|;
根据纵向速度响应距离得到纵向速度制导律为:
其中,umin为设定好的最低期望纵向速度,dmax为响应限制距离且有其中kres为响应限制参数,dmid为响应中间距离,ures为路径响应后的期望纵向速度,最后由第二PID控制器根据误差ures-u得到输出τu。本实施例中,kres设置为0.2,dmid设置为
多模式决策模块通过接收指令使无人船多模式路径跟踪控制***进行严格路径选择、反向跟踪返航或改变路径拓展模式功能;
多模式决策模块接收严格路径选择模块、艏摇角制导律传递过来的第一虚拟任务点(x0,y0)Τ以及三种期望艏摇角ψr1、ψr2和ψr3,根据接收指令选择是否使能严格路径选择功能、反向跟踪返航功能以及改变路径拓展模式功能,即从ψr1、ψr2和ψr3中选择传递到第一PID控制器的多模式决策后的期望艏摇角ψr以及传递信号给路径拓展模块选择插值算法得到不同的pathinterp,最后由第一PID控制器根据误差ψr-ψ得到输出ψr。
严格路径选择功能如下:
当无人船执行严格路径选择功能时,无人船跟踪受到第一虚拟任务点(x0,y0)Τ的约束,跟踪目标锁定第一虚拟任务点,此时ψr=ψr3;本实施例中,当无人船经过第一虚拟任务点4m圆形范围内时再从ψr1、ψr2中进行选择;
其中反向跟踪返航功能如下:
无人船正常正向跟踪时,选择ψr=ψr1;
当无人船执行反向跟踪返航功能时,选择ψr=ψr2。
其中改变路径拓展模式功能设计如下:
无人船在路径跟踪过程中可响应指令并传递信号给路径拓展模块,曲线跟踪时路径拓展模块切换为球面插值算法,直线跟踪时路径拓展模块切换为分段线性插值算法对路径信息pathin进行实时处理,得到不同的拓展后的路径信息pathinterp。
通过LOS制导算出的期望艏摇角和真实艏摇角的取值区间不同,会引发误差计算错误的问题,为了传递正确的艏摇角误差ψr-ψ给第一PID控制器,设置一个误差纠正模块,具体如下:
若-π<ψr-ψ或ψr-ψ<=π成立,则传递ψr-ψ给第一PID控制器;
若-π<ψr-ψ或ψr-ψ<=π不成立,且ψr-ψ=-π成立,则传递ψr-ψ=π给第一PID控制器。
若-π<ψr-ψ、ψr-ψ<=π或ψr-ψ=-π不成立,且-π>ψr-ψ成立,则传递以下参数给第一PID控制器:
若-π<ψr-ψ、ψr-ψ<=π、ψr-ψ=-π或-π>ψr-ψ不成立,则传递以下参数给第一PID控制器:
S3、实时对无人船多模式路径跟踪控制***中的多模式决策模块下达指令,实现无人船多模式路径跟踪控制。
根据图4、图5可以看出,本发明在跟踪效果上优于传统LOS:
1、本发明设置的第一虚拟任务点可以使无人船在跟踪过程中严格跟踪初始路径点(图4、图5中初始路径点均为(20,0))。
2、本发明采用的路径拓展模块和纵向速度制导律可以平滑路径信息,有效利用了路径信息并抑制了无人船在路径切向角突变点处的过冲现象(如图4中的(20,20)、(40,20)、(40,40),图5中(20,20)、(30,30)、(30,10))。
3、此外本发明采用的多模式决策模块可以对路径信息和跟踪顺序进行实时干预,实现了多模式路径跟踪控制,满足不同任务需求。
Claims (9)
1.一种基于速变LOS的无人船多模式路径跟踪控制方法,其特征在于,包括以下步骤:
S1、建立惯性坐标系下固定双桨推进无人船的三自由度运动学和动力学模型;
S2、构建无人船多模式路径跟踪控制***;
所述无人船多模式路径跟踪控制***包括路径拓展模块、严格路径选择模块和多模式决策模块;
路径拓展模块根据拓展后的路径点数目和输入路径点数目计算得到插值步长,接收多模式决策模块传递的信号,对输入路径信息分别进行分段线性插值、球面插值得到拓展后的路径信息;
严格路径选择模块根据拓展后路径信息设置第一虚拟任务点,根据拓展后的路径信息和第一虚拟任务点通过路径参数更新律、艏摇角制导律和纵向速度制导律得到当前跟踪路径参数、路径切向角矩阵、期望艏摇角和纵向速度;
多模式决策模块根据第一虚拟任务点、期望艏摇角和纵向速度,通过第一PID控制器、第二PID控制器控制无人船的运动;
无人船在跟踪路径时,多模式决策模块实时接收指令使无人船多模式路径跟踪控制***实现不同的功能;
S3、实时对无人船多模式路径跟踪控制***中的多模式决策模块下达指令,实现无人船多模式路径跟踪控制。
2.根据权利要求1所述的一种基于速变LOS的无人船多模式路径跟踪控制方法,其特征在于,步骤S1中,运动学与动力学模型的建立过程如下:
建立惯性坐标系下固定双桨推进无人船的三自由度运动学和动力学模型,其中,Oexeye是建立在地球上的惯性坐标系,船体坐标系是以船体中心为原点,纵向指向船艏并平行水面,横向垂直于船艏并平行于水面,在地球上的惯性坐标系下无人船的运动学和动力学模型为:
其中,ψ是无人船当前的艏摇角,u,v,r分别表示船体坐标系中的纵向速度、横向速度和艏摇角速度;τu和τr分别表示无人船动力装置产生的纵向推力和横向力矩,τ1,τ2,τ3分别表示无人船在纵向、横向、艏向上受到的干扰力,mii和dii分别为实验无人船的给定惯性系数和阻尼系数;由于不考虑外界干扰,τ1,τ2,τ3设置为0;为无人船当前的位置(x,y)的对应导数,和为对应的导数;
其中τu=f1+f2,τr=(f1-f2)·B/2;B表示无人船双桨的横向距离,f1和f2分别表示两个螺旋桨的推力;
由于固定双桨推进无人船以两个直流电机为推进***,两个螺旋桨分别对应两个直流电机,螺旋桨推力和电调输出电压有如下线性关系:f=kV;
结合上述方程最终得到实际的控制输入:左电机控制电压VL和右电机控制电压VR分别为:
VL=(2·τr/B+τu)/2k,VR=(-2·τr/B+τu)/2k;
则动力学方程进一步改写为:
5.根据权利要求4所述的一种基于速变LOS的无人船多模式路径跟踪控制方法,其特征在于,路径参数更新律和艏摇角制导律具体如下:
给定一条期望路径的路径信息和第一虚拟任务点,通过位置误差计算得到路径参数更新律和三种期望艏摇角;
给定拓展后的路径信息矩阵pathinterp和无人船当前的位置(x,y),由此得出xe轴路径信息差分矩阵xdiff(θ)和ye轴路径信息差分矩阵ydiff(θ)为:
得到路径切向角矩阵ψd(θ)为:
ψd(θ)=atan2(ydiff(θ),xdiff(θ));
定义横向跟踪误差矩阵xe(θ)和纵向跟踪误差矩阵ye(θ)为:
xe(θ)=cosψd(θ)·(x-xθ)+sinψd(θ)·(y-yθ)
ye(θ)=-sinψd(θ)·(x-xθ)+cosψd(θ)·(y-yθ);
设计跟踪误差矩阵derr(θ)为:
derr(θ)=xe(θ)·xe(θ)+ye(θ)·ye(θ);
则当前跟踪路径参数θnow更新律具体如下:
艏摇角制导律如下:
其中ψr1为正向跟踪期望艏摇角,ψr2为反向跟踪期望艏摇角,ψr3为第一虚拟任务点期望艏摇角,n×l为无人船的可视距离,n为整数,l为无人船的长度,ψr1中θnow取argminderr(θ)+1,ψr2中θnow取argminderr(θ)-1。
6.根据权利要求5所述的一种基于速变LOS的无人船多模式路径跟踪控制方法,其特征在于,纵向速度制导律具体如下:
由路径切向角计算得到路径切向角导数,选取路径切向角导数大于定值的点为路径切向角突变点;当无人船进入路径切向角突变点一定范围内时,根据相对路径切向角突变点的距离设置期望纵向速度,越接近路径切向角突变点期望纵向速度越低;
给定当前的跟踪路径参数θnow、路径切向角差分矩阵ψdiff(θ)和初始设定的期望纵向速度up;由|ψdiff(θ)|>ψchange得到切向角突变参数矩阵θchange,其中ψchange为设定的突变角;
纵向速度响应距离为:
dres=min|θchange-θnow|;
根据纵向速度响应距离得到纵向速度制导律为:
7.根据权利要求6所述的一种基于速变LOS的无人船多模式路径跟踪控制方法,其特征在于,多模式决策模块通过接收指令使无人船多模式路径跟踪控制***进行严格路径选择、反向跟踪返航或改变路径拓展模式功能;
多模式决策模块接收严格路径选择模块、艏摇角制导律传递过来的第一虚拟任务点(x0,y0)T以及三种期望艏摇角ψr1、ψr2和ψr3,根据接收指令选择是否使能严格路径选择功能、反向跟踪返航功能以及改变路径拓展模式功能,即从ψr1、ψr2和ψr3中选择传递到第一PID控制器的多模式决策后的期望艏摇角ψr以及传递信号给路径拓展模块选择插值算法得到不同的pathinterp,最后由第一PID控制器根据误差ψr-ψ得到输出ψr。
8.根据权利要求7所述的一种基于速变LOS的无人船多模式路径跟踪控制方法,其特征在于,严格路径选择功能如下:
当无人船执行严格路径选择功能时,无人船跟踪受到第一虚拟任务点(x0,y0)T的约束,跟踪目标锁定第一虚拟任务点,此时ψr=ψr3;当无人船经过第一虚拟任务点一定范围内时再从ψr1、ψr2中进行选择;
其中反向跟踪返航功能如下:
无人船正常正向跟踪时,选择ψr=ψr1;
当无人船执行反向跟踪返航功能时,选择ψr=ψr2;
其中改变路径拓展模式功能如下:
无人船在路径跟踪过程中可响应指令并传递信号给路径拓展模块,曲线跟踪时路径拓展模块切换为球面插值算法,直线跟踪时路径拓展模块切换为分段线性插值算法对路径信息pathin进行实时处理,得到不同的拓展后的路径信息pathinterp。
9.根据权利要求7所述的一种基于速变LOS的无人船多模式路径跟踪控制方法,其特征在于,通过LOS制导算出的期望艏摇角和真实艏摇角的取值区间不同,会引发误差计算错误的问题,为了传递正确的艏摇角误差ψr-ψ给第一PID控制器,设置一个误差纠正模块,具体如下:
若-π<ψr-ψ或ψr-ψ<=π成立,则传递ψr-ψ给第一PID控制器;
若-π<ψr-ψ或ψr-ψ<=π不成立,且ψr-ψ=-π成立,则传递ψr-ψ=π给第一PID控制器;
若-π<ψr-ψ、ψr-ψ<=π或ψr-ψ=-π不成立,且-π>ψr-ψ成立,则传递以下参数给第一PID控制器:
若-π<ψr-ψ、ψr-ψ<=π、ψr-ψ=-π或-π>ψr-ψ不成立,则传递以下参数给第一PID控制器:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110548443.4A CN113296505B (zh) | 2021-05-19 | 2021-05-19 | 一种基于速变los的无人船多模式路径跟踪控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110548443.4A CN113296505B (zh) | 2021-05-19 | 2021-05-19 | 一种基于速变los的无人船多模式路径跟踪控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113296505A CN113296505A (zh) | 2021-08-24 |
CN113296505B true CN113296505B (zh) | 2022-10-25 |
Family
ID=77322871
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110548443.4A Active CN113296505B (zh) | 2021-05-19 | 2021-05-19 | 一种基于速变los的无人船多模式路径跟踪控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113296505B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114489091B (zh) * | 2022-01-21 | 2023-01-13 | 湖南大学无锡智能控制研究院 | 一种自主水下机器人路径跟踪的制导律控制方法、设备及*** |
CN116520834B (zh) * | 2023-04-24 | 2024-05-28 | 安徽建筑大学 | 一种低能耗的无人船巡航方法 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
AU2003221083A1 (en) * | 2003-03-25 | 2004-10-18 | Rorze Corporation | Robot simulation device, and robot simulation program |
CN109358499A (zh) * | 2018-09-25 | 2019-02-19 | 大连海事大学 | 基于速度与航向双重制导的无人船精确路径跟踪控制方法 |
CN109883427B (zh) * | 2019-03-25 | 2023-04-07 | 江苏科技大学 | 一种基于Dubins路径和概率威胁地图的无人船的避障方法 |
CN109901598A (zh) * | 2019-04-08 | 2019-06-18 | 哈尔滨工程大学 | 基于随机模型预测控制技术的自主水下机器人路径跟踪方法 |
CN111142527B (zh) * | 2019-12-31 | 2023-08-11 | 陕西欧卡电子智能科技有限公司 | 无人船任意路径的追踪控制方法 |
CN111487966B (zh) * | 2020-04-13 | 2022-09-09 | 哈尔滨工程大学 | 一种基于航路点的水面无人艇自适应路径跟踪控制方法 |
-
2021
- 2021-05-19 CN CN202110548443.4A patent/CN113296505B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN113296505A (zh) | 2021-08-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Xiang et al. | Smooth transition of AUV motion control: From fully-actuated to under-actuated configuration | |
CN102768539B (zh) | 基于迭代的自主水下航行器三维曲线路径跟踪控制方法 | |
CN111487966B (zh) | 一种基于航路点的水面无人艇自适应路径跟踪控制方法 | |
Borhaug et al. | Cross-track control for underactuated autonomous vehicles | |
CN109050835B (zh) | 全驱动自主水下机器人结构及回收三维路径跟踪方法 | |
Cao et al. | Toward optimal rendezvous of multiple underwater gliders: 3D path planning with combined sawtooth and spiral motion | |
CN113419428B (zh) | 基于3d映射制导的机/船协同路径跟踪控制器设计方法 | |
CN113296505B (zh) | 一种基于速变los的无人船多模式路径跟踪控制方法 | |
CN112034865B (zh) | 基于优化算法的全驱动水下航行器航迹跟踪控制方法 | |
CN111857165B (zh) | 一种水下航行器的轨迹跟踪控制方法 | |
CN113093804B (zh) | 基于反演滑模控制的无人艇编队控制方法和控制*** | |
CN109901402A (zh) | 一种基于航向平滑技术的自主水下机器人路径跟踪方法 | |
CN113741433B (zh) | 一种水面无人船的分布式编队方法 | |
Yang et al. | A hybrid autonomous recovery scheme for AUV based dubins path and non-singular terminal sliding mode control method | |
Raimondi et al. | Hierarchical fuzzy/lyapunov control for horizontal plane trajectory tracking of underactuated AUV | |
Dai et al. | Dual closed loop AUV trajectory tracking control based on finite time and state observer | |
CN108363400B (zh) | 一种基于虚拟锚泊的欠驱动auv三维控制区域镇定控制方法 | |
CN113359737A (zh) | 一种考虑队形伸缩的船舶编队自适应事件触发控制方法 | |
CN113960994A (zh) | 一种多无人艇协同航行的s面自适应控制算法 | |
CN116679693A (zh) | 融合螺旋桨水动力的无人艇自抗扰控制方法 | |
CN114564015B (zh) | 一种拒止环境下的欠驱动无人艇分布式编队控制方法 | |
Song et al. | Autonomous berthing of unmanned surface vehicles based on improved dubins-RRT algorithm and non-singular terminal sliding mode control | |
Miotto et al. | UUV on-board path planning in a dynamic environment for the Manta test vehicle | |
Ma et al. | An improved adaptive dynamic window algorithm for target tracking of unmanned surface vehicles | |
CN117519136B (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 |