本发明涉及的是一种自主水下航行器的控制方法,具体地说是一种欠驱动自主水下航行器对平面直线航线跟踪的智能s面制导控制方法。
背景技术:
水平面直线航线跟踪控制具有很强的应用背景,是自主水下航行器完成调查型任务以及远程航行的重要控制手段。依据文献《fine-scaleseafloorsurveyinruggeddeep-oceanterrainwithanautonomousrobot》(ieeeinternationalconferenceonroboticsandautomation,2000)及网站http://waittinstitute.org/wi-archive/cat-1-reefs/所述,美国的伍兹霍尔研究所利用不同的载体进行了区域搜索式的作业,实现了对不同海区的水下目标探测、海底地形数据采集等工作。在上述应用中,区域搜索式作业中水下航行器行进的路径形式均为梳状路径。
根据水下航行器操纵性知识,关于纵中剖面对称的水下航行器在无操纵力作用下的运动具有直线稳定性,无方向稳定性和航线稳定性。因此需要包含推进器及操纵面等执行机构的控制系统介入,才能保证水下航行器的直线航线跟踪从而进行可靠地水下航行作业。《intelligentpidguidancecontrolforauvpathtracking》(j.cent.southuniv.2015,第22卷)描述了一种智能pid制导控制方法,控制水下航行器进行水平面直线航线跟踪控制,通过目标航线更替完成梳状路径形式的区域搜索。控制方法经过了充分的试验验证,并结合探测装置成功应用于海中的目标标定。与以往虚拟目标点形式的输出不同(《具有pid反馈增益的自主水下航行器三维直线路径跟踪控制方法》,中国发明专利,2012),这种制导控制方法采用参考艏向角作为控制输出,避免了虚拟目标点的“不可达”、海流作用下存在稳态偏差问题,且参考艏向角更容易被执行层的状态控制器理解。
技术实现要素:
本发明的目的在于提供一种可以有效抵抗海流干扰,稳定、精确地跟踪,适用于欠驱动自主水下航行器进行远程航行及调查类任务的自主水下航行器平面直线航线跟踪的智能s面制导控制方法。
本发明的目的是这样实现的:
步骤一:依据航行器当前位置信息判断是否更新跟踪目标;
步骤二:获取决定当前目标直线航线的两个关键路径点的信息;
步骤三:依据航行器自身位置以及目标航线信息,计算位置的横向偏差;
步骤四:依据偏差的历史变化计算偏差变化率;
步骤五:依据步骤三至四获得的横向偏差及其变化率智能调整s面控制算法的控制参数;
步骤六:以步骤三至四获得的横向偏差及其变化率为输入,采用步骤五获得的控制参数,经非线性的s面控制率计算参考艏向角,得到控制输出。
本发明的技术要点主要体现在:
1、水下航行器的跟踪目标,具体为当前时刻航行器只有一个目标,且所跟踪的目标是平面的两个关键点确定的平面直线,描述为关键点t0(ξ0,η0)和t1(ξ1,η1),以及直线
2、决定当前目标直线的两个关键点t0和t1,具体为t0和t1为组成航行器路径的关键点序列p′={p′0,p′1,p′2,…,p′q}中的相邻两个。
3、水下航行器所跟踪的目标动态特性,具体为依据航行器ps(ξs,ηs)与t0(ξ0,η0)和t1(ξ1,η1)的相对位置关系更新关键点,从而实现目标航线的更替,定义判据
如果κ≤0,则更新关键点。
4、计算载体与目标直线段之间横向偏差pe,包含以下步骤:
步骤一:计算横向距离,即偏差绝对值,公式如下:
步骤二:计算正负,规定航行器位于方向线段t0t1的左侧时pe>0:
sgn(pe)=sgn[(ξ1-ξ0)·(η1-ηs)-(η1-η0)·(η1-ηs)]
步骤三:计算横向偏差;
pe=sgn(pe)·|pe|。
5、计算横向偏差变化率
式为偏差变化率的离散形式,采用移动窗口内增量均值的方法计算,md为移动窗口的宽度md∈z,以控制节拍为单位,itt与n同为控制节拍,不同的是itt在目标更新时清零。
6、制导控制器的算法为参数智能调整的s面控制方法,算法以偏差及偏差变化率为控制输入,以参考艏向角为控制输出,表达如下:
ke、kv、ki为控制参数,mi为积分窗口宽度,ψ′为归一化的参考艏向角。
7、制导控制器的参数采用智能调整策略,具体为模糊自适应的调整方式:
本发明为自主水下航行器提供了一种平面直航线跟踪的无偏差跟踪的制导控制器,以消除航行器与航线之间的横向偏差。可以有效抵抗海流干扰,稳定地、精确地跟踪,适用于欠驱动自主水下航行器进行远程航行及调查类任务。
本发明所公开的制导控制器需要同状态控制器(艏向角控制器)组合形成分层控制系统,才可进行直航线跟踪。该技术方案所述的智能s面制导控制方法是在此基础上提出的,考虑到s函数的有介性、连续性、对称性、中间密集两侧疏松的优良特性,以及水下航行器状态控制的良好效果,将其作为智能体三层理性行为模型中规划层的制导控制器是同样可行的和实用的。
本发明具有以下优点及有益效果:
1.具有模块化特点。不依赖载体的数学模型及载体其它结构因素,输入离散点形式表示的平面路径,输出的是以角度为度量单位的操纵面转动幅度。这种优良特性方便载体间的移植及工程化应用。
2.控制系统的鲁棒性好。由于控制算法不依赖水下航行器的动力学模型,且具有自适应调整特性,经多个异构水下航行器平台,于水池、湖泊、海上试验等不同环境中实现了航行作业,均达到了极好的控制效果。
3.抗海流干扰能力强,稳定性好,精度高。在所设计的直线段跟踪控制器作用下,水下航行器能够感受到海流给横向偏差带来的影响,在制导控制算法的作用下能够连续地调整参考艏向角输出,经状态控制器调整操纵面角度实现航行器转艏,从而消除横向偏差。以某180kg级水下航行器为例,在海上强海流的干扰下直航线跟踪偏差≤2.0m(95%),且无稳态偏差。
附图说明
图1为180kg级欠驱动自主水下航行器外形图执行器布置;
图2为分层的平面直线段跟踪控制器结构图;
图3为欠驱动水下航行器进行直线航线跟踪的示意图;
图4为海试试验数据之直航线路径与航迹对比图;
图5为海试试验数据之水平面直航线跟踪偏差;
图6为海试实验数据之水平面直航线踪部分偏差;
图7为海试试验数据之水平面直航线跟踪偏差的数学统计;
图8为s面控制参数的模糊调整之对偏差项参数的调整表1;
图9为s面控制参数的模糊调整之对偏差微分项参数的调整表2;
图10为s面控制参数的模糊调整之对干扰调整项参数的调整表3;
图11是本发明的流程图。
具体实施方式
本发明的技术方案主要包括如下步骤:
步骤一:判断是否更新目标,依据航行器ps(ξs,ηs)与t0(ξ0,η0)和t1(ξ1,η1)的相对位置关系更新关键点,从而实现目标航线的更替,定义判据
步骤二:获取目标直线航线的两个关键路径点,依据步骤一,获取当前关键点的索引值index,读取路径文件,得到相邻的2个路径点,并且赋值给决定当前目标直航线的唯一关键点对t0(ξ0,η0)和t1(ξ1,η1)。
步骤三:计算横向偏差,已知航行器位置ps(ξs,ηs)与关键点t0(ξ0,η0)和t1(ξ1,η1),依据几何关系计算横向偏差
步骤四:计算偏差变化率,采用移动窗口内增量均值的方法计算
md为移动窗口的宽度。
步骤五:s面控制参数的智能调整,制导控制器的参数采用智能调整策略,具体为模糊自适应的调整方式,输入为步骤三、四得到的横向偏差pe及变化率
步骤六:制导控制的计算,以步骤3-4获得的横向偏差及其变化率为输入,采用步骤5获得的控制参数,经非线性的s面控制率计算参考艏向角:
下面结合附图举例对本发明作进一步描述:
本实施例的实施平台是一台微小型自主水下航行器如图1。控制系统的执行机构采用欠驱动的布置方案,在中心线的艉部布置一套推进器1,由直流无刷电机和螺旋桨组成;艉部布置“十”字型操纵面,由步进电机产生动力,左右舷对称布置升降舵3,上下对称布置转艏舵2。平台搭载的运动传感器包括gps、磁罗经、加速度计、dvl、水压力传感器,水面运动状态依赖gps、磁罗经、dvl感知,水下运动状态依赖磁罗经、加速度计、dvl、水压力传感器感知并且进行推算导航。
本发明所述的适用于欠驱动自主水下航行器平面直线航线跟踪的智能s面制导控制方法,如图2所示,处于整个直航线跟踪控制系统的上层,与下层的状态控制共同实现跟踪控制的功能。制导控制器实现由位置偏差向艏向角偏差的转变如图3,状态控制器实现欠驱动水下航行器的艏向角控制。参考艏向角由两部分组成:ψref=ψo+ψc,其中ψo为目标航线的方向,ψc为航行器与目标航线之间横向偏差决定的局部偏航角:
ψc=f2(pe)
f2包含本发明所述制导控制器核心算法。
设当前目标直线段为由点t0(ξ0,η0)和t1(ξ1,η1)所确定的方向线段(t0≠t1),航行器的位置ps(ξs,ηs),航向为ψs,依据航行器与目标直线段的相对位置关系进行目标直线段的更替,具体判据为:
计算向量
这里t0(ξ0,η0)和t1(ξ1,η1)始终为组成路径的关键点序列p′={p′0,p′1,p′2,…,p′q}中相邻的两个点。p′以如下格式保存在mypathpt.dat文件中:
“index(long)latitude(double)longitude(double)depth(double)height(double)ispassed(bool)”。
所决定的直线的表达式为:
依据航行器当前位置及直线方程,计算横向偏差。
按照以下公式计算航行器距离目标直线段的距离:
其正负利用向量
sgn(pe)=sgn[(ξ1-ξ0)·(η1-ηs)-(η1-η0)·(η1-ηs)]
横向偏差即pe=sgn(pe)·|pe|;
计算偏差变化率。采用移动窗口内增量均值的方法计算:
上式为偏差变化率的离散形式,采用移动窗口内增量均值的方法计算,md为移动窗口的宽度md∈z,以控制节拍为单位,itt与n同为控制节拍,不同的是itt在目标航线更新时清零。这里偏差变化率的计算方式具有一定的灵活性,也可以采用线性权值的方式计算:
s面控制参数的自适应调整。采用模糊控制的方式,依据位置偏差pe以及变化率
模糊规则表如附图8-10的表1-3所示。这里选择输入、输出模糊语言集t(i,o):t(i,o)={零(0),小(1),中(2),大(3),极(4)}
隶属度函数为:
z:u0(d)=0(d=0)
d为符号形式的输入;解模糊采用面积中心法(coa)计算权重值。设:μi是距离输入d对每一个模糊子集的隶属度值,则coa方法表示为:
设计制导控制率。在研究欠驱动自主水下航行器直航线跟踪机理的基础上,依据李雅普诺夫稳定性理论设计渐进控制率。横向偏差pe为制导器的状态变量,依据lyapunov稳定性定理设计控制率,取lyapunov函数
ke,kv,ki分别为s面函数的控制参数,它们的更新采用上述自适应调整的方式计算。
海上试验验证与分析:
将本发明所述的制导控制算法集成到某180kg公斤级的自主水下航行器,在海上进行了直线航线的跟踪试验,航行器的巡航速度为1.3m/s,路径的垂向坐标为深度1.5m。
图5为整个航行过程中的跟踪误差,在进行参考点更迭时跟踪误差较大一致达到30.4m,这是因路径的粗略表达而未考虑auv运动能力以及海流干扰所致;在对长边的跟踪过程中,跟踪误差会从航线更迭引起的较大值(15~30m)逐渐收敛至较小范围(2.0m)并且在较大海流干扰后仍能返回稳定区域;
图6为图5中虚线框内曲线部分的放大,虚线表示ε=2.0m的参考线;
图7为这段误差的统计,可以得出结论:auv在较大海流干扰下,对于直线航线的跟踪过程中,跟踪误差≤2.37m的概率为95%。
本发明适用于欠驱动及全驱动自主水下航行器执行远程航行及调查作业任务,具有较强的抗海流干扰能力,不具有模型依赖性,能够使得水下航行器在水平面实现对直线航线的无偏跟踪,从而满足如区域搜索、水下对接等对水平面跟踪精度要求较高的应用需求。
海试结果表明,本发明对三维路径的理解及跟踪控制方法具有全局渐进稳定特性,并且能够保证无偏差的跟踪,适用于欠驱动自主水下航行器在复杂海况下执行远程航行及调查型任务。