CN107991872A - 欠驱动auv的虚拟锚泊水平面区域镇定控制方法及实现方法 - Google Patents
欠驱动auv的虚拟锚泊水平面区域镇定控制方法及实现方法 Download PDFInfo
- Publication number
- CN107991872A CN107991872A CN201711286715.8A CN201711286715A CN107991872A CN 107991872 A CN107991872 A CN 107991872A CN 201711286715 A CN201711286715 A CN 201711286715A CN 107991872 A CN107991872 A CN 107991872A
- Authority
- CN
- China
- Prior art keywords
- mrow
- auv
- msub
- mover
- centerdot
- 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
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Other Liquid Machine Or Engine Such As Wave Power Use (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供的是一种欠驱动AUV的虚拟锚泊水平面区域镇定控制方法及实现方法。包括：建立欠驱动AUV的水平面运动方程；建立还有环境影响下的欠驱动AUV的相对速度和相对加速度的方程；建立了虚拟锚泊的数学模型；通过改进的静态悬链线方程对水平分力进行表示，采用Traube改进抛物线法对水下机器人所受到的锚链张力进行求解；取锚链张力中的水平分力进行按照AUV的随艇坐标系进行分解，分为纵向力Fxm和横向力Fym以及转艏力矩Mm；将Fxm和Mm分配到推力器，得出主推进器的推力X和转艏力矩N。本发明方法不基于模型，不会因为载体而对本控制方法产生影响，可以使得欠驱动AUV在大干扰环境下实现区域镇定。
Description
技术领域
本发明涉及的是一种无人水下机器人运动控制方法，具体地说是一种欠驱动无人水下机器人(Underacutated Autonomous Underwater Vehicles)的虚拟锚泊水平面区域镇定控制方法。
背景技术
欠驱动水下机器人是指独立控制输入维数少于系统自由度的水下机器人，它是我国实施深海资源开发战略不可或缺的重大技术装备之一。相比于全驱动水下机器人，欠驱动水下机器人首先在成本方面有所降低，多推进装置的配置不仅代表了设备成本的提高，也使得对于水下机器人的能源消耗的大幅增加，同时也加重了机器人的重量。在水下机器人的航速不断增加的情况，全驱动水下机器人的侧向推进器和垂向推进器的作用会越来越弱，甚至无法让水下机器人转向。当全驱动水下机器人的某个推进器出现故障，无法正常工作时，水下机器人将由全驱动变为欠驱动，需要通过欠驱动控制算法对水下机器人进行有效控制，并且用较少的推进器同样达到期望的控制要求。因此，对于欠驱动水下机器人的控制研究具有重要理论意义和工程应用价值。
对于一些任务要求AUV保持在某个目标位置附近，例如在执行搜索任务时，AUV需要在所要求的节点位置进行图像数据的采集，然后将所搜集的数据储存。在这个过程中对于水下机器人的位置要求并不严格，不需要进入动力定位状态，由于海洋环境的影响，水下机器人在长时间的动力定位过程中会消耗大量的能源，这种情况下动力定位不是最佳的解决方式对于欠驱动水下机器人其本身不具备动力定位的能力，其横向运动与纵向运动完全耦合，对于上述动作实现起来更加困难。对于此类问题，一般采用镇定控制加以解决。
欠驱动水下机器人的镇定控制研究是欠驱动水下机器人的关键技术之一。对于欠驱动水下机器人点镇定控制问题研究都是基于系统运动学模型，采用非线性状态反馈的思想对其进行镇定，主要分为非连续时不变反馈镇定和连续时变反馈镇定，主要方法有σ变化法、反步法(back-stepping)等。但是这些研究都是基于系统无干扰且为无漂非完整约束系统，在环境干扰强烈的实际海洋环境中稳定性无法保证。目前国内外对非完整系统，特别是欠驱动机械系统、移动机器人的镇定的研究已经相当丰富，但对于欠驱动水下机器人的镇定研究还相对较少。
发明内容
本发明的目的在于提供一种能够使欠驱动水下机器人在海流环境下的区域镇定以及提高水下机器人的实用性的欠驱动AUV的虚拟锚泊水平面区域镇定控制方法。本发明的目的还在于提供一种欠驱动AUV的虚拟锚泊水平面区域镇定控制方法的实现方法。
本发明的欠驱动AUV的虚拟锚泊水平面区域镇定控制方法包括如下步骤：
步骤一、通过AUV的外部控制输入所要到达的目标位置的控制命令；
步骤二、通过AUV上的设备所测量到的位姿计算出当前位置与目标位置之间的间距和锚链角；
步骤三、判断是否符合：1.到达有效方围内；2.控制变量处于收敛状态，如果符合则退出；如果不符合则执行步骤四；
步骤四、启动虚拟锚泊水平面区域镇定控制，将锚链角θ和现在位置与目标位置的间距d，输入到悬链线方程中，计算出悬链曲线的辅助函数a的初值a1和a0，然后采用Traube改进抛物线法进行对a的精确求解，不断的对a进行迭代，直到求解出符合要求的|e|＜ε的a值；
步骤五、根据悬链线辅助方程计算出锚链张力的水平分力Fh；
步骤六、对水平分力Fh按照欠驱动AUV的随艇坐标进行分解，分为纵向力Fxm和横向力Fym以及转艏力矩Mm，其中，纵向力Fxm为AUV所要产生的纵向推力X，转艏力矩Mm为AUV所要产生的转艏力矩N；
步骤七、通过主推推力和垂直舵的不断工作对AUV不断的调整，使得其靠近目标点位置，寻找顶流的方向，实现区域镇定的控制目标。
本发明的欠驱动AUV的虚拟锚泊水平面区域镇定控制方法还可以包括：
1、计算出悬链曲线的辅助函数a的初值a1和a0的公式为：
x＝d，y＝h，h为锚孔至底土高度。
2、据悬链线辅助方程计算出锚链张力的水平分力Fh的公式为：
Fh＝a·q
q为悬链的线密度。
3、纵向力Fxm、横向力Fym以及转艏力矩Mm具体为：
Fxm＝Fhcos(ψ-θ)
Fym＝Fhsin(ψ-θ)
Mm＝Fhlmsin(ψ-θ)
其中，Fh为AUV受到锚链作用水平分力，ψ为AUV的艏向角，lm为锚泊水平分力到作用点的力臂。
本发明的欠驱动AUV的虚拟锚泊水平面区域镇定控制方法是通过如下手段实现的：
(1)建立欠驱动AUV的水平面运动方程:
建立欠驱动AUV在大地坐标系下的水平面三自由度运动方程：
其中M为质量矩阵，ν为AUV在随艇坐标系的速度向量，C(ν)为科氏向心力矩阵，D(ν)为流体阻尼矩阵，g(η)为重力和浮力产生的恢复力(力矩)矩阵，τ为执行机构产生的力(力矩)；
在水平面，执行机构产生的力/力矩向量表示为：
τ＝[X Y N]T
AUV水平面运动学方程表示为:
其中ν为AUV的横向速度；
AUV水平面动力学方程表示为:
(2)建立包括环境影响的欠驱动AUV的相对速度和相对加速度的方程：
假定水域为深广水域，只存在海流的影响，通过艇体坐标系下机器人的相对速度来补偿海流的影响，则相对速度：
ur＝u-UCcos(θ)cos(αC-ψ)
vr＝v-UCsin(αC-ψ)
其中，Uc为流速，αc为流向角，ψ为艏向角，θ为纵倾角，对相对速度式进行时间的微分后得到相对加速度：
(3)根据欠驱动水下机器人在虚拟锚泊状态下受力分析，建立虚拟锚泊的数学模型：
在一定深度的海中(定深情况)，AUV将锚链连接的锚抛入海水中，使其沉入水中并使其啮入土中，锚产生的抓力与水底固结起来，使得AUV可以固留在预定区域内。因此对于在水平面上处于虚拟锚泊状态的欠驱动AUV受锚链作用的受力包括：
Fxm＝Fhcos(ψ-θ)
Fym＝Fhsin(ψ-θ)
Mm＝Fhlmsin(ψ-θ)
其中，Fh为AUV受到锚链作用水平分力，ψ为AUV的艏向角，θ为锚链角，Fxm为Fh经过分解后的纵向力，Fym为Fh经过分解后的横向力，Mm为Fh经过分解后的转艏力矩，lm为锚泊水平分力到作用点的力臂；
(4)通过改进的静态悬链线方程对水平分力进行表示，
本发明对悬链线方程进行修改，在没有铺底锚链且根据a不同，悬链线方程将会沿着y轴上下移动的情况下，则悬链线方程为：
其中称为悬链曲线的辅助函数，q为悬链的线密度，Fh为悬链张力的水平分量；悬链曲线张力水平分力方程为：
Fh＝a·q；
(5)采用Traube改进抛物线法对水下机器人所受到的锚链张力进行求解：
由悬链线方程得：
代入x＝d，y＝h，d为AUV与目标点之间的水平距离，h为锚孔至底土高度，a视情况从某个数开始迭加，当首次f(an)·f(an+1)＜0求出初值a0和a1；采用Traube改进抛物线对a进行的精确求解：
1)γ＝(f(a1)-f(a0))/(a1-a0)，并求a2＝a1-f(a1)/γ；
2)通过下面算式对进行求解，
3)计算误差e＝a3-a2，若|e|＜ε(本发明中ε＝0.00001)结束，否则继续；
4)令
a0＝a1 f(a0)＝f(a1)
a1＝a2 f(a1)＝f(a2)
a2＝a1 f(a2)＝f(a1)
5)转2)，继续计算；
通过迭代方法求出a，利用悬链线辅助方程进而求得水平分力Fh；
(6)根据本发明的要求，取锚链张力中的水平分力进行按照AUV的随艇坐标系进行分解，分为纵向力Fxm和横向力Fym以及转艏力矩Mm：
在水平面控制中，将水平分力Fh按照AUV的随艇坐标系进行分解，分为纵向力Fxm和横向力Fym以及转艏力矩Mm；
(7)将Fxm和Mm分配到推力器，得出主推进器的推力X和转艏力矩N：
纵向力Fxm为AUV所要产生的纵向推力(主推推力)X，转艏力矩Mm为AUV所要产生的转艏力矩N,用AUV本身的推进器和垂直舵来模拟出锚链作用在AUV的效果。
本发明的基于虚拟锚泊的欠驱动水下机器人水平面区域镇定控制方法不基于模型，可以避免来自载体模型对控制效果产生的不良影响，并且可以抵抗海洋环境对欠驱动AUV的影响，能够欠驱动水下机器人在海流环境下的区域镇定以及提高水下机器人的实用性。该方法最终可用于欠驱动AUV在实际海洋环境中的镇定控制中。
附图说明
图1欠驱动AUV水平面运动控制系统图；
图2a-图2b欠驱动AUV处于锚泊转台的受力分析示意图；
图3欠驱动AUV虚拟锚泊控制系统流程示意图；
图4欠驱动AUV在定深、干扰环境下的初始状态示意图；
图5欠驱动AUV在定深、干扰环境下的转艏寻找顶流方向示意图；
图6欠驱动AUV在定深、干扰环境下的寻找到顶流方向，处于镇定状态示意图；
图7Traube改进抛物线计算流程示意图；
图8欠驱动AUV的虚拟锚泊水平面区域镇定控制控制流程示意图。
具体实施方式
下面举例对本发明做更详细的描述。
对在干扰环境下欠驱动AUV点镇定控制存在鲁棒性差，以及镇定控制严重依赖模型的问题，设计了一种基于虚拟锚泊的欠驱动水下机器人水平面区域镇定控制方法，包括：控制命令的输入模块、锚链角和间距计算模块、欠驱动AUV虚拟锚泊区域镇定控制器模块、欠驱动AUV水平面动力学方程、欠驱动AUV运动学方程。
本发明所设计基于虚拟锚泊的欠驱动水下机器人水平面区域镇定控制方法，其原理是，假定在目标位置处抛下一只锚，锚通过锚链与AUV连接。在水平面上，当AUV受到环境力作用离开目标位置时，锚链产生张力(本方法仅进行水平面控制，因此只取水平分力进行分析)，抵抗环境力的作用，从而使AUV保持在目标位置附近。虚拟锚链张力由AUV推力器提供，虚拟锚链最大张力即为AUV能够输出的最大控制力。
基于虚拟锚泊的欠驱动水下机器人水平面区域镇定控制方法的基本控制过程：AUV刚开始启动时速度为零(即使有主推推力作用下)，远小于海流的速度，导致AUV在海流的作用下被带离起始点，该阶段海流对AUV的作用占主导位置；当AUV的速度在主推推力的作用下不断增大，抵抗住了海流，在转艏力矩的作用下朝着目标点方向运动，同时搜索顶流的方向；当AUV的艏向角与海流的角度呈180deg差值时，表示AUV开始顶流运动(搜索方向完成)，转艏力矩、艏向角、主推推力将会收敛，转艏力矩将趋于零，只有主推推力在作用，使得AUV朝着目标点前进；在有效作用范围内以及顶流运动下，当AUV距离目标点一定距离时，主推推力的海流对AUV的作用力达到平衡，AUV将停止运动，如图4-6。
其中，当欠驱动AUV完成方向搜索(无转艏力矩作用)，朝着目标前进时，突然改变海流的流向，例如从220deg到180deg，AUV运动产生了变化，在本身推力和力矩再加上海流的影响下，偏离了目标点，后在虚拟锚链系统的调解下重新搜索方向，最后找到方向(顶流方向)，在有效作用半径内实现镇定。
整体的欠驱动AUV的虚拟锚泊水平面区域镇定控制的控制流程是这样实现的：欠驱动AUV的外部控制可输入所要到达的目标位置的控制命令，AUV上的设备(例如磁罗经、USBL、GPS、北斗等)所测量到的位姿计算出当前位置与目标位置之间的间距和锚链角，判断是否符合条件(1.到达有效方围内2.控制变量处于收敛状态)。如果不符合，将启动虚拟锚泊水平面区域镇定控制，根据输入的所输入的锚链角θ和现在位置与目标位置的间距d，输入到悬链线方程中，计算出a的初值a1和a0，然后采用Traube改进抛物线法进行对a的精确求解，不断的对a进行迭代，直到求解出符合要求(|e|＜ε)的a值，如图7。根据悬链线辅助方程计算出锚链张力的水平分力Fh，对水平分力Fh按照欠驱动AUV的随艇坐标进行分解，分为纵向力Fxm和横向力Fym以及转艏力矩Mm(见公式7)，其中，纵向力Fxm为AUV所要产生的纵向推力(主推推力)X，转艏力矩Mm为AUV所要产生的转艏力矩N，通过主推推力和垂直舵的不断工作对AUV不断的调整，使得其靠近目标点位置，寻找顶流的方向，实现区域镇定的控制目标。
欠驱动AUV虚拟锚泊区域镇定控制器的控制流程是这样实现的：根据所输入的锚链角θ和现在位置与目标位置的间距d，根据所设定的欠驱动AUV虚拟的锚孔至底土高度为h(控制器里面设定量)，输入到公式(10)中。公式(10)计算出a的初值a1和a0，然后采用Traube改进抛物线法进行对a的精确求解，不断的对a进行迭代，直到求解出符合要求(|e|＜ε)的a值，如图7。根据公式(9)计算出锚链张力的水平分力Fh，对水平分力Fh按照欠驱动AUV的随艇坐标进行分解，分为纵向力Fxm和横向力Fym以及转艏力矩Mm(见公式7)，其中，纵向力Fxm为AUV所要产生的纵向推力(主推推力)X，转艏力矩Mm为AUV所要产生的转艏力矩N，如图3。
整体基于虚拟锚泊的欠驱动水下机器人水平面区域镇定控制方法是这样实现的：欠驱动AUV的外部控制可输入所要到达的目标位置(ξd，ηd)的控制命令，欠驱动AUV根据自己现在所处的位姿，也就是实时的位置(ξ，η)和艏向角Ψ，计算出锚链角θ和现在位置与目标位置的间距d，根据所设定的欠驱动AUV虚拟的锚孔至底土高度为h，输入到公式(10)中。公式(10)计算出a的初值a1和a0，然后采用Traube改进抛物线法进行对a的精确求解，求解出符合要求(|e|＜ε)的a值，如图7。根据公式(9)计算出锚链张力的水平分力Fh，对水平分力Fh按照欠驱动AUV的随艇坐标进行分解，分为纵向力Fxm和横向力Fym以及转艏力矩Mm(见公式7)，其中，纵向力Fxm为AUV所要产生的纵向推力(主推推力)X，转艏力矩Mm为AUV所要产生的转艏力矩N,用AUV本身的推进器和垂直舵来模拟出锚链作用在AUV的效果，达到稳定的目的，如图3。将从虚拟锚泊区域镇定控制系统得到的X、N输入到欠驱动AUV的水平面动力学方程(公式(4))，并经过积分得到当前AUV的纵向速度u、横向速度v和回转角速度r，它们都是相对于干扰的速度，再将(u、v、r)输入到AUV的水平面运动学模型中(公式(3))，并经过积分得到当前AUV的位置(ξ，η)和艏向角Ψ，再与目标位置比较，计算出计算出锚链角θ和现在位置与目标位置的间距d，重复上述过程直到欠驱动AUV在目标位置的有效范围内(本发明采用的是d＝30为有效作用范围，该设定值在控制器中设定)找到顶流方向，并且处于镇定状态，控制算法将停止运算。
说明：a.在当d＞30时，目标点与水下AUV的实时位置的距离d＝30，避免因距离过大，导致产生的Fx过大，即对最大推力进行限定。本发明将d＝30作为虚拟锚链的有效作用半径，当AUV保持在该范围以内，并且运动参数已经收敛，则判断已经镇定。
b.计算初始值时，a值对于不同的距离d采用不同的计算精度，以保证计算初值时不会出错，提高算法的效率；本发明选用当d＞15，a的取值从1开始，每计算一次加1，直到符合的结果出现为止；当d＜＝15&&d＞5，a的取值从0.1开始，每计算一次加0.1，直到符合的结果出现为止；当d＜＝5，1的取值从0.001开始，每计算一次加0.01，直到符合的结果出现为止。
对于式(10)还存在两个问题：
a.当d＝0时，方程无解。此时，AUV位于目标点上，因此Fh＝0。
b.由于方程(10)属于超越方程，无法得到精确解，因此采用Traube改进抛物线法进行近似求解。
下面给出本发明的具体实施方式，并结合附图，具体步骤如下：
第一步，建立如图1欠驱动AUV的水平面运动方程:
建立欠驱动AUV在大地坐标系下的水平面三自由度运动方程：
其中M为质量矩阵，ν为AUV在随艇坐标系的速度向量，C(ν)为科氏向心力矩阵，D(ν)为流体阻尼矩阵，g(η)为重力和浮力产生的恢复力(力矩)矩阵，τ为执行机构产生的力(力矩)。
在水平面，执行机构产生的力/力矩向量可表示为：
τ＝[X Y N]T(2)
AUV水平面运动学方程如图1可以表示为:
其中ν为AUV的横向速度。
AUV水平面动力学方程如图1可表示为:
第二步，建立还有环境影响下的欠驱动AUV的相对速度和相对加速度的方程：
海洋环境中，AUV所受到的干扰力较为复杂以及多变，例如海流本身的影响，海水密度、盐度和温度的影响等。本发明假定水域为深广水域，只存在海流的影响。通过艇体坐标系下机器人的相对速度以此来补偿海流的影响，因此相对速度：
ur＝u-UCcos(θ)cos(αC-ψ)
vr＝v-UCsin(αC-ψ) (5)
其中，Uc为流速，αc为流向角，ψ为艏向角，θ为纵倾角，对相对速度式进行时间的微分后可得到相对加速度：
第三步，根据欠驱动水下机器人在虚拟锚泊状态下受力分析，建立了虚拟锚泊的数学模型：
在一定深度的海中(定深情况)，AUV将锚链连接的锚抛入海水中，使其沉入水中并使其啮入土中，锚产生的抓力与水底固结起来，使得AUV可以固留在预定区域内。因此对于在水平面上处于虚拟锚泊状态的欠驱动AUV受锚链作用的受力分析，如图2a-图2b：
其中，Fh为AUV受到锚链作用水平分力，ψ为AUV的艏向角，θ为锚链角，Fxm为Fh经过分解后的纵向力，Fym为Fh经过分解后的横向力，Mm为Fh经过分解后的转艏力矩，lm为锚泊水平分力到作用点的力臂。
第四步，通过改进的静态悬链线方程对水平分力进行表示，
本发明对悬链线方程进行修改，在没有铺底锚链且根据a不同，悬链线方程将会沿着y轴上下移动的情况下，则悬链线方程为：
其中称为悬链曲线的辅助函数，q为悬链的线密度，Fh为悬链张力的水平分量。悬链曲线张力水平分力方程为：
Fh＝a·q(9)
第五步，采用Traube改进抛物线法对水下机器人所受到的锚链张力进行求解：
由悬链线方程可知：
代入x＝d，y＝h(d为AUV与目标点之间的水平距离，h为锚孔至底土高度)，a视情况从某个数开始迭加，当首次f(an)·f(an+1)＜0求出初值a0和a1。采用Traube改进抛物线如图7对a进行的精确求解：
1)γ＝(f(a1)-f(a0))/(a1-a0)(11)
，并求a2＝a1-f(a1)/γ；
2)通过下面算式对a3进行求解，
3)计算误差e＝a3-a2，若|e|＜ε(本发明中ε＝0.00001)结束，否则继续；
4)令
5)转2)，继续计算。
通过迭代方法求出a，利用悬链线辅助方程进而求得水平分力Fh。
第六步，根据本发明的要求，取锚链张力中的水平分力进行按照AUV的随艇坐标系进行分解，分为纵向力Fxm和横向力Fym以及转艏力矩Mm：
在水平面控制中，将水平分力Fh按照AUV的随艇坐标系进行分解，分为纵向力Fxm和横向力Fym以及转艏力矩Mm。
第七步，将Fxm和Mm分配到推力器，得出主推进器的推力X和转艏力矩N：
纵向力Fxm为AUV所要产生的纵向推力(主推推力)X，转艏力矩Mm为AUV所要产生的转艏力矩N,用AUV本身的推进器和垂直舵来模拟出锚链作用在AUV的效果。
Claims (6)
1.一种欠驱动AUV的虚拟锚泊水平面区域镇定控制方法，其特征是：
步骤一、通过AUV的外部控制输入所要到达的目标位置的控制命令；
步骤二、通过AUV上的设备所测量到的位姿计算出当前位置与目标位置之间的间距和锚链角；
步骤三、判断是否符合：1.到达有效方围内；2.控制变量处于收敛状态，如果符合则退出；如果不符合则执行步骤四；
步骤四、启动虚拟锚泊水平面区域镇定控制，将锚链角θ和现在位置与目标位置的间距d，输入到悬链线方程中，计算出悬链曲线的辅助函数a的初值a1和a0，然后采用Traube改进抛物线法进行对a的精确求解，不断的对a进行迭代，直到求解出符合要求的|e|＜ε的a值；
步骤五、根据悬链线辅助方程计算出锚链张力的水平分力Fh；
步骤六、对水平分力Fh按照欠驱动AUV的随艇坐标进行分解，分为纵向力Fxm和横向力Fym以及转艏力矩Mm，其中，纵向力Fxm为AUV所要产生的纵向推力X，转艏力矩Mm为AUV所要产生的转艏力矩N；
步骤七、通过主推推力和垂直舵的不断工作对AUV不断的调整，使得其靠近目标点位置，寻找顶流的方向，实现区域镇定的控制目标。
2.根据权利要求1所述的欠驱动AUV的虚拟锚泊水平面区域镇定控制方法，其特征是：计算出悬链曲线的辅助函数a的初值a1和a0的公式为：
<mrow>
<mi>f</mi>
<mrow>
<mo>(</mo>
<mi>a</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>c</mi>
<mi>h</mi>
<mfrac>
<mi>x</mi>
<mi>a</mi>
</mfrac>
<mo>-</mo>
<mfrac>
<mi>y</mi>
<mi>a</mi>
</mfrac>
</mrow>
x＝d，y＝h，h为锚孔至底土高度。
3.根据权利要求1或2所述的欠驱动AUV的虚拟锚泊水平面区域镇定控制方法，其特征是：根据悬链线辅助方程计算出锚链张力的水平分力Fh的公式为：
Fh＝a·q
q为悬链的线密度。
4.根据权利要求1或2所述的欠驱动AUV的虚拟锚泊水平面区域镇定控制方法，其特征是纵向力Fxm、横向力Fym以及转艏力矩Mm具体为：
Fxm＝Fh cos(ψ-θ)
Fym＝Fh sin(ψ-θ)
Mm＝Fhlm sin(ψ-θ)
其中，Fh为AUV受到锚链作用水平分力，ψ为AUV的艏向角，lm为锚泊水平分力到作用点的力臂。
5.根据权利要求3所述的欠驱动AUV的虚拟锚泊水平面区域镇定控制方法，其特征是纵向力Fxm、横向力Fym以及转艏力矩Mm具体为：
Fxm＝Fh cos(ψ-θ)
Fym＝Fh sin(ψ-θ)
Mm＝Fhlm sin(ψ-θ)
其中，Fh为AUV受到锚链作用水平分力，ψ为AUV的艏向角，lm为锚泊水平分力到作用点的力臂。
6.一种欠驱动AUV的虚拟锚泊水平面区域镇定控制方法的实现方法，其特征是：
(1)建立欠驱动AUV的水平面运动方程:
建立欠驱动AUV在大地坐标系下的水平面三自由度运动方程：
<mrow>
<mi>H</mi>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>+</mo>
<mi>C</mi>
<mrow>
<mo>(</mo>
<mi>v</mi>
<mo>)</mo>
</mrow>
<mi>v</mi>
<mo>+</mo>
<mi>D</mi>
<mrow>
<mo>(</mo>
<mi>v</mi>
<mo>)</mo>
</mrow>
<mi>v</mi>
<mo>+</mo>
<mi>g</mi>
<mrow>
<mo>(</mo>
<mi>&eta;</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>&tau;</mi>
</mrow>
其中M为质量矩阵，ν为AUV在随艇坐标系的速度向量，C(ν)为科氏向心力矩阵，D(ν)为流体阻尼矩阵，g(η)为重力和浮力产生的恢复力矩阵，τ为执行机构产生的力；
在水平面，执行机构产生的力/力矩向量表示为：
τ＝[X Y N]T
AUV水平面运动学方程表示为:
<mrow>
<mover>
<mi>&xi;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mi>u</mi>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&psi;</mi>
<mo>-</mo>
<mi>v</mi>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&psi;</mi>
</mrow>
<mrow>
<mover>
<mi>&eta;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mi>u</mi>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&psi;</mi>
<mo>+</mo>
<mi>v</mi>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&psi;</mi>
</mrow>
<mrow>
<mover>
<mi>&psi;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mi>r</mi>
</mrow>
其中ν为AUV的横向速度；
AUV水平面动力学方程表示为:
<mrow>
<mo>(</mo>
<mi>m</mi>
<mo>-</mo>
<msub>
<mi>X</mi>
<mover>
<mi>u</mi>
<mo>&CenterDot;</mo>
</mover>
</msub>
<mo>)</mo>
<mover>
<mi>u</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mo>(</mo>
<mi>m</mi>
<mo>-</mo>
<msub>
<mi>Y</mi>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
</msub>
<mo>)</mo>
<mi>v</mi>
<mi>r</mi>
<mo>+</mo>
<mo>(</mo>
<msub>
<mi>X</mi>
<mi>u</mi>
</msub>
<mo>+</mo>
<msub>
<mi>X</mi>
<mrow>
<mi>u</mi>
<mo>|</mo>
<mi>u</mi>
<mo>|</mo>
</mrow>
</msub>
<mo>|</mo>
<mi>u</mi>
<mo>|</mo>
<mo>)</mo>
<mi>u</mi>
<mo>+</mo>
<mi>X</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>m</mi>
<mo>-</mo>
<msub>
<mi>Y</mi>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
</msub>
<mo>)</mo>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mo>-</mo>
<mo>(</mo>
<mi>m</mi>
<mo>-</mo>
<msub>
<mi>X</mi>
<mover>
<mi>u</mi>
<mo>&CenterDot;</mo>
</mover>
</msub>
<mo>)</mo>
<mi>u</mi>
<mi>r</mi>
<mo>+</mo>
<mo>(</mo>
<msub>
<mi>Y</mi>
<mi>v</mi>
</msub>
<mo>+</mo>
<msub>
<mi>Y</mi>
<mrow>
<mi>v</mi>
<mo>|</mo>
<mi>v</mi>
<mo>|</mo>
</mrow>
</msub>
<mo>|</mo>
<mi>v</mi>
<mo>|</mo>
<mo>)</mo>
<mi>v</mi>
</mrow>
<mrow>
<mo>(</mo>
<msub>
<mi>I</mi>
<mi>z</mi>
</msub>
<mo>-</mo>
<msub>
<mi>N</mi>
<mover>
<mi>r</mi>
<mo>&CenterDot;</mo>
</mover>
</msub>
<mo>)</mo>
<mover>
<mi>r</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mo>-</mo>
<mo>(</mo>
<msub>
<mi>X</mi>
<mover>
<mi>u</mi>
<mo>&CenterDot;</mo>
</mover>
</msub>
<mo>-</mo>
<msub>
<mi>Y</mi>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
</msub>
<mo>)</mo>
<mi>u</mi>
<mi>v</mi>
<mo>+</mo>
<mo>(</mo>
<msub>
<mi>N</mi>
<mi>r</mi>
</msub>
<mo>+</mo>
<msub>
<mi>N</mi>
<mrow>
<mi>r</mi>
<mo>|</mo>
<mi>r</mi>
<mo>|</mo>
</mrow>
</msub>
<mo>|</mo>
<mi>v</mi>
<mo>|</mo>
<mo>)</mo>
<mi>r</mi>
<mo>+</mo>
<mi>N</mi>
<mo>;</mo>
</mrow>
(2)建立包括环境影响的欠驱动AUV的相对速度和相对加速度的方程：
假定水域为深广水域，只存在海流的影响，通过艇体坐标系下机器人的相对速度来补偿海流的影响，则相对速度：
ur＝u-UC cos(θ)cos(αC-ψ)
vr＝v-UC sin(αC-ψ)
其中，Uc为流速，αc为流向角，ψ为艏向角，θ为纵倾角，对相对速度式进行时间的微分后得到相对加速度：
<mrow>
<msub>
<mover>
<mi>u</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>r</mi>
</msub>
<mo>=</mo>
<mover>
<mi>u</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>+</mo>
<msub>
<mi>U</mi>
<mi>c</mi>
</msub>
<mi>q</mi>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mi>&theta;</mi>
<mo>)</mo>
</mrow>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&alpha;</mi>
<mi>c</mi>
</msub>
<mo>-</mo>
<mi>&psi;</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>U</mi>
<mi>c</mi>
</msub>
<mi>r</mi>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mi>&theta;</mi>
<mo>)</mo>
</mrow>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&alpha;</mi>
<mi>c</mi>
</msub>
<mo>-</mo>
<mi>&psi;</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>r</mi>
</msub>
<mo>=</mo>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>+</mo>
<msub>
<mi>U</mi>
<mi>c</mi>
</msub>
<mi>r</mi>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&alpha;</mi>
<mi>c</mi>
</msub>
<mo>-</mo>
<mi>&psi;</mi>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
(3)根据欠驱动水下机器人在虚拟锚泊状态下受力分析，建立虚拟锚泊的数学模型：
在水平面上处于虚拟锚泊状态的欠驱动AUV受锚链作用的受力包括：
Fxm＝Fh cos(ψ-θ)
Fym＝Fh sin(ψ-θ)
Mm＝Fhlm sin(ψ-θ)
其中，Fh为AUV受到锚链作用水平分力，ψ为AUV的艏向角，θ为锚链角，Fxm为Fh经过分解后的纵向力，Fym为Fh经过分解后的横向力，Mm为Fh经过分解后的转艏力矩，lm为锚泊水平分力到作用点的力臂；
(4)通过改进的静态悬链线方程对水平分力进行表示，
在没有铺底锚链且根据a不同，悬链线方程将会沿着y轴上下移动的情况下，则悬链线方程为：
<mrow>
<mi>y</mi>
<mo>=</mo>
<mi>a</mi>
<mo>&CenterDot;</mo>
<mi>c</mi>
<mi>h</mi>
<mfrac>
<mi>x</mi>
<mi>a</mi>
</mfrac>
</mrow>
其中称为悬链曲线的辅助函数，q为悬链的线密度，Fh为悬链张力的水平分量；悬链曲线张力水平分力方程为：
Fh＝a·q；
(5)采用Traube改进抛物线法对水下机器人所受到的锚链张力进行求解：
由悬链线方程得：
<mrow>
<mi>f</mi>
<mrow>
<mo>(</mo>
<mi>a</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>c</mi>
<mi>h</mi>
<mfrac>
<mi>x</mi>
<mi>a</mi>
</mfrac>
<mo>-</mo>
<mfrac>
<mi>y</mi>
<mi>a</mi>
</mfrac>
</mrow>
代入x＝d，y＝h，d为AUV与目标点之间的水平距离，h为锚孔至底土高度，a视情况从某个数开始迭加，当首次f(an)·f(an+1)＜0求出初值a0和a1；采用Traube改进抛物线对a进行的精确求解：
1)γ＝(f(a1)-f(a0))/(a1-a0)，并求a2＝a1-f(a1)/γ；
2)通过下面算式对进行求解，
3)计算误差e＝a3-a2，若|e|＜ε结束，否则继续；
4)令
a0＝a1 f(a0)＝f(a1)
a1＝a2 f(a1)＝f(a2)
a2＝a1 f(a2)＝f(a1)
5)转2)，继续计算；
通过迭代方法求出a，利用悬链线辅助方程进而求得水平分力Fh；
(6)取锚链张力中的水平分力进行按照AUV的随艇坐标系进行分解，分为纵向力Fxm和横向力Fym以及转艏力矩Mm：
在水平面控制中，将水平分力Fh按照AUV的随艇坐标系进行分解，分为纵向力Fxm和横向力Fym以及转艏力矩Mm；
(7)将Fxm和Mm分配到推力器，得出主推进器的推力X和转艏力矩N：
纵向力Fxm为AUV所要产生的纵向推力X，转艏力矩Mm为AUV所要产生的转艏力矩N。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711286715.8A CN107991872B (zh) | 2017-12-07 | 2017-12-07 | 欠驱动auv的虚拟锚泊水平面区域镇定控制方法及实现方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711286715.8A CN107991872B (zh) | 2017-12-07 | 2017-12-07 | 欠驱动auv的虚拟锚泊水平面区域镇定控制方法及实现方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107991872A true CN107991872A (zh) | 2018-05-04 |
| CN107991872B CN107991872B (zh) | 2020-12-22 |
Family
ID=62036846
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711286715.8A Active CN107991872B (zh) | 2017-12-07 | 2017-12-07 | 欠驱动auv的虚拟锚泊水平面区域镇定控制方法及实现方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107991872B (zh) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108062091A (zh) * | 2017-11-14 | 2018-05-22 | 北京臻迪科技股份有限公司 | 浮游移动体的浮潜动力补偿方法、装置及浮游移动体 |
| CN109283842A (zh) * | 2018-08-02 | 2019-01-29 | 哈尔滨工程大学 | 一种无人艇航迹跟踪智能学习控制方法 |
| CN109470248A (zh) * | 2018-10-31 | 2019-03-15 | 青岛海洋科学与技术国家实验室发展中心 | 一种水下航行器导航系统及导航方法 |
| CN110618606A (zh) * | 2019-09-30 | 2019-12-27 | 哈尔滨工程大学 | 一种复合干扰下的欠驱动auv反步自适应模糊滑模控制方法 |
| CN110948485A (zh) * | 2019-11-21 | 2020-04-03 | 佛山科学技术学院 | 绳牵引并联机器人柔索悬链线模型精确数值解的计算方法 |
| CN115071926A (zh) * | 2022-05-16 | 2022-09-20 | 西北工业大学 | 一种基于Petri网的自主水下航行器锚泊卧底任务控制方法 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102298326A (zh) * | 2011-06-30 | 2011-12-28 | 哈尔滨工程大学 | 欠驱动auv自适应轨迹跟踪控制装置及控制方法 |
| KR101370649B1 (ko) * | 2012-09-04 | 2014-03-10 | 주식회사 한화 | 무인잠수정의 경로 제어방법 |
| CN105929842A (zh) * | 2016-04-20 | 2016-09-07 | 哈尔滨工程大学 | 一种基于动态速度调节的欠驱动uuv平面轨迹跟踪控制方法 |
-
2017
- 2017-12-07 CN CN201711286715.8A patent/CN107991872B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102298326A (zh) * | 2011-06-30 | 2011-12-28 | 哈尔滨工程大学 | 欠驱动auv自适应轨迹跟踪控制装置及控制方法 |
| KR101370649B1 (ko) * | 2012-09-04 | 2014-03-10 | 주식회사 한화 | 무인잠수정의 경로 제어방법 |
| CN105929842A (zh) * | 2016-04-20 | 2016-09-07 | 哈尔滨工程大学 | 一种基于动态速度调节的欠驱动uuv平面轨迹跟踪控制方法 |
Non-Patent Citations (4)
| Title |
|---|
| CHUNMENG JIANG.ETC: "Design of Novel s-plane controller of autonomous underwater vehicle established on sliding mode control", 《JOURNAL OF HARBIN INSTITUTE OF TECHNOLOGY(NEW SERIES)》 * |
| 万磊等: "非完全对称欠驱动无人艇全局渐近镇定控制", 《华中科技大学学报（自然科学版）》 * |
| 李晔: "欠驱动AUV 水平面运动的镇定控制设计", 《船舶工程》 * |
| 甘忠等: "基于抛物线法的迭代补偿算法", 《塑性工程学报》 * |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108062091A (zh) * | 2017-11-14 | 2018-05-22 | 北京臻迪科技股份有限公司 | 浮游移动体的浮潜动力补偿方法、装置及浮游移动体 |
| CN108062091B (zh) * | 2017-11-14 | 2020-12-11 | 北京臻迪科技股份有限公司 | 浮游移动体的浮潜动力补偿方法、装置及浮游移动体 |
| CN109283842A (zh) * | 2018-08-02 | 2019-01-29 | 哈尔滨工程大学 | 一种无人艇航迹跟踪智能学习控制方法 |
| CN109283842B (zh) * | 2018-08-02 | 2022-01-07 | 哈尔滨工程大学 | 一种无人艇航迹跟踪智能学习控制方法 |
| CN109470248A (zh) * | 2018-10-31 | 2019-03-15 | 青岛海洋科学与技术国家实验室发展中心 | 一种水下航行器导航系统及导航方法 |
| CN110618606A (zh) * | 2019-09-30 | 2019-12-27 | 哈尔滨工程大学 | 一种复合干扰下的欠驱动auv反步自适应模糊滑模控制方法 |
| CN110618606B (zh) * | 2019-09-30 | 2022-06-17 | 哈尔滨工程大学 | 一种复合干扰下的欠驱动auv反步自适应模糊滑模控制方法 |
| CN110948485A (zh) * | 2019-11-21 | 2020-04-03 | 佛山科学技术学院 | 绳牵引并联机器人柔索悬链线模型精确数值解的计算方法 |
| CN110948485B (zh) * | 2019-11-21 | 2022-11-08 | 佛山科学技术学院 | 绳牵引并联机器人柔索悬链线模型精确数值解的计算方法 |
| CN115071926A (zh) * | 2022-05-16 | 2022-09-20 | 西北工业大学 | 一种基于Petri网的自主水下航行器锚泊卧底任务控制方法 |
| CN115071926B (zh) * | 2022-05-16 | 2023-12-19 | 西北工业大学 | 一种基于Petri网的自主水下航行器锚泊卧底任务控制方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107991872B (zh) | 2020-12-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107991872A (zh) | 欠驱动auv的虚拟锚泊水平面区域镇定控制方法及实现方法 | |
| Wang et al. | Dynamic modeling and motion simulation for a winged hybrid-driven underwater glider | |
| CN102768539B (zh) | 基于迭代的自主水下航行器三维曲线路径跟踪控制方法 | |
| CN109814392B (zh) | 一种欠驱动水下机器人执行器故障鲁棒容错控制方法 | |
| CN112527018B (zh) | 一种欠驱动自主水下航行器的三维镇定控制方法 | |
| CN108829099A (zh) | 一种基于受限反步法控制的欠驱动无人船轨迹跟踪方法 | |
| CN108549394A (zh) | 一种基于领航者和虚拟领航者的多auv直线编队控制方法 | |
| CN110618606B (zh) | 一种复合干扰下的欠驱动auv反步自适应模糊滑模控制方法 | |
| CN113419428B (zh) | 基于3d映射制导的机/船协同路径跟踪控制器设计方法 | |
| CN105807789A (zh) | 基于t-s模糊观测器补偿的uuv控制方法 | |
| CN105643626A (zh) | 一种基于双重判据的水下机器人推力优化分配方法 | |
| Wang et al. | Path following control of the wave glider in waves and currents | |
| CN111857165B (zh) | 一种水下航行器的轨迹跟踪控制方法 | |
| CN105487386B (zh) | 一种在载荷布放强扰下的uuv自适应模糊滑模控制方法 | |
| CN106840143B (zh) | 一种判别水下机器人姿态稳定的方法 | |
| CN108363400B (zh) | 一种基于虚拟锚泊的欠驱动auv三维控制区域镇定控制方法 | |
| Zhang et al. | The application of self-tuning fuzzy PID control method to recovering AUV | |
| CN108572654A (zh) | 基于q学习的欠驱动auv虚拟锚泊三维镇定控制及实现方法 | |
| CN104155043A (zh) | 一种动力定位系统外界环境力测量方法 | |
| CN112925332B (zh) | 无人艇与水下无人潜器联合系统的协同交会对接控制方法 | |
| CN113147438A (zh) | 一种水下航行器无线充电系统及控制方法 | |
| Fu et al. | Research on thrust allocation of dynamic positioning ship with cycloidal propeller | |
| CN106094842B (zh) | 一种基于t-s模型和pdc的uuv垂直面运动h∞控制方法 | |
| Li et al. | Dynamics modeling and experiments of wave driven robot | |
| CN109814547B (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 |