CN116753961B - 基于状态观测的动力定位船舶高速循迹导航方法 - Google Patents
基于状态观测的动力定位船舶高速循迹导航方法 Download PDFInfo
- Publication number
- CN116753961B CN116753961B CN202311027264.1A CN202311027264A CN116753961B CN 116753961 B CN116753961 B CN 116753961B CN 202311027264 A CN202311027264 A CN 202311027264A CN 116753961 B CN116753961 B CN 116753961B
- Authority
- CN
- China
- Prior art keywords
- ship
- track
- heading
- formula
- turning
- 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 34
- 239000013598 vector Substances 0.000 claims abstract description 50
- 238000013178 mathematical model Methods 0.000 claims abstract description 43
- 238000001914 filtration Methods 0.000 claims abstract description 12
- 239000011159 matrix material Substances 0.000 claims description 34
- 230000009191 jumping Effects 0.000 claims description 18
- 238000005259 measurement Methods 0.000 claims description 17
- 230000007613 environmental effect Effects 0.000 claims description 13
- 238000013016 damping Methods 0.000 claims description 8
- 238000012546 transfer Methods 0.000 claims description 7
- 230000008569 process Effects 0.000 claims description 4
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 6
- 230000001133 acceleration Effects 0.000 description 5
- 230000010355 oscillation Effects 0.000 description 4
- 230000010354 integration Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 208000024891 symptom Diseases 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- 238000004378 air conditioning Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003137 locomotive effect Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000002194 synthesizing effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Abstract
本发明涉及船舶导航技术领域，尤其涉及基于状态观测的动力定位船舶高速循迹导航方法，包括如下流程：S1:建立动力定位船舶水平面三自由度运动数学模型；S2:将噪声信息滤除并分离出高、低频运动信息，对状态向量进行估计，得到状态向量的估计值；S3:根据状态向量的估计值，结合用户输入航迹信息，计算出动力定位船舶期望艏向，并根据期望艏向及进行循迹导航。本发明提供的方法可以实现较高的循迹精度，保证船舶按照预先设定的航迹航行。
Description
技术领域
本发明涉及船舶导航技术领域，尤其涉及基于状态观测的动力定位船舶高速循迹导航方法。
背景技术
高速循迹导航是动力定位船舶的一种重要的功能模式，可以有效辅助船舶进行某些特定作业任务。高速循迹导航作业需要保证循迹精度，即航迹偏差越小越好。高速循迹并非单纯的直线段循迹，它还包含了每两段直线之间的圆弧段循迹。增加圆弧段对循迹过程进行过渡的原因在于，船舶在高速航行时推进器横向能力大打折扣，使得船舶成为欠驱动状态，因此进行航段间切换时需要平滑过渡。一般地，船舶都配备了自动舵操控装置，可以很好地保证船舶高速直航时的艏向精度。但是，循迹控制的直接目标为航迹偏差而非艏向偏差。通常，将航迹偏差通过引导方法转换为艏向目标，从而将航迹控制问题转化为艏向控制问题，如何设计合理有效的引导方法成为此控制问题的关键环节。而现有技术只考虑了航迹偏差对艏向引导的影响，未考虑到航迹偏差变化趋势及环境力的影响，可能造成的后果有航迹偏差控制超调、震荡及航迹偏差存在稳态误差等。
发明内容
本发明所要解决的技术问题是提供基于状态观测的动力定位船舶高速循迹导航方法，通过建立船舶运动数学模型式，并利用扩展卡尔曼滤波将噪声信息滤除并分离出高、低频运动信息，对状态向量进行估计，然后再利用状态向量/>的估计值，结合用户输入航迹信息，计算出动力定位船舶期望艏向，并根据期望艏向进行分段循迹导航，可以实现较高的循迹精度，保证船舶按照预先设定的航迹航行。
本发明是通过以下技术方案予以实现：
基于状态观测的动力定位船舶高速循迹导航方法，其包括如下步骤：
J1：建立动力定位船舶水平面三自由度运动数学模型为式（1）和式（2）；
（1）；
（2）；
式中：为状态向量，/>为非线性状态转移函数，/>为描述推进器配置的控制矩阵，/>为控制输入，/>为噪声系数矩阵，/>为三自由度运动数学模型的零均值高斯白噪声向量，/>为系统测量模型，/>为观测矩阵，/>为零均值高斯白噪声三维向量；
J2：将动力定位船舶水平面三自由度运动数学模型的噪声信息滤除并分离出高、低频运动信息，对动力定位船舶水平面三自由度运动数学模型中的状态向量进行估计，得到状态向量/>的估计值；
J3：根据状态向量的估计值，结合用户输入航迹信息，计算出动力定位船舶期望艏向，并根据期望艏向进行循迹导航。
进一步，步骤J1中建立动力定位船舶水平面三自由度运动数学模型的方法如下：
首先建立船舶的低频运动数学模型如式（3）所示：
（3）；
式中：为船舶惯性矩阵，/>为在船体坐标系下船舶的运动速度及角速度状态向量，D为船舶阻尼矩阵，/>为坐标变换阵，T代表矩阵转置，/>为纵荡、横荡和艏摇三个自由度上的未知环境力，/>为过程噪声的幅值，/>为低频运动数学模型零均值高斯白噪声向量；
其次，建立船舶的高频运动数学模型式（4），并将式（4）表示为状态空间形式为式（5）；
（4）；
（5）；
其中：表示模型的传递函数，/>表示/>自由度波浪强度；/>表示虚变量，/>表示/>自由度相对阻尼系数，/>表示/>自由度波浪主导频率，/>=1,2,3分别表示纵荡、横荡及艏摇三个自由度；/>为船舶高频状态向量；/>为高频运动纵荡、横荡位置和艏向角度三维向量，/>、/>、/>均为系数矩阵，/>为高频运动数学模型零均值高斯白噪声向量；
然后建立系统测量模型式（6）：
（6）；
其中：为在大地坐标系下船舶的运动位置及艏向状态向量；
最后综合低频运动数学模型、高频运动数学模型及系统测量模型，得到动力定位船舶非线性数学模型式（7），并将式（7）表示为状态空间形式得到动力定位船舶水平面三自由度运动数学模型：
（7）；
式中：为包含时间常数的三维对角阵；/>为三维对角阵，表示未知环境力的幅值；/>为未知环境力模型的零均值高斯白噪声向量。
进一步，步骤J3中计算动力定位船舶期望艏向的具体步骤如下：
L1：根据用户输入航迹判断动力定位船舶航行在用户输入航迹的直线段还是圆弧段，确定分段引导策略；
首先根据式（8）及式（9）分别计算用户输入航迹拐弯之前航段的航迹方向角及拐弯之后航段的航迹方向角/>，然后根据计算出的/>及/>判断船舶左转还是右转，若则为右转，若/>则为左转；
（8）；
（9）；
其中：、/>、/>分别为用户输入航迹上的三个点的坐标，为动力定位船舶当前观测位置坐标；
然后，根据式（10）计算圆弧段圆心的坐标；
（10）；
其中：为圆弧段圆心坐标，/>为航迹拐点处的转弯半径；
再根据式（11）计算出圆心至当前观测船位的方位角：
（11），
再根据几何关系得到右转时航迹拐点至转弯圆心连线的方位角为式（12），左转时航迹拐点至转弯圆心连线的方向角/>为式（13）：而无论是左转还是右转时，转弯圆心至航迹拐点连线的方位角/>均为式（14）：
（12）；
（13）；
（14）；
并由此得出转弯圆心至转弯圆弧起点连线的方向角及转弯圆心至转弯圆弧终点连线的方向角/>分别为式（15）、式（16）
(15)；
(16)；
其中，=1为航迹右转，/>=-1为航迹左转；
最后航迹右转时，若，则判断船舶航行在转弯之前航段，则跳转至步骤L2；若/>，则判断船舶航行在圆弧航段，则跳转至步骤L3；若/>则判断船舶已通过圆弧航段，航行在转弯之后航段，则跳转至步骤L4；
左转时，若则判断航行在转弯之前航段，则跳转至步骤L2；若则判断航行在圆弧航段，则跳转至步骤L3；若/>则判断已通过圆弧航段，航行在转弯之后航段，则跳转至步骤L4；
L2：根据式（17）计算出当前观测位置到转弯之前航段的航迹偏差，根据式（18）计算出垂向速度/>，根据式（19）计算出期望艏向：
（17）；
（18）；
（19）；
其中垂向速度为当前船舶观测合速度/>垂直于拐弯之前航段的分量，垂向速度正方向为/>顺时针旋转/>，/>、/>分别为航迹偏差增益系数及垂向速度增益系数，/>为环境力引起的漂角观测值，/>为纵荡速度，/>为横荡速度，/>为艏向；
再用当前观测艏向减去期望艏向，若差值大于零，则向左修正艏向使船舶达到期望艏向靠近航迹，若差值小于零，则向右修正艏向使船舶达到期望艏向靠近航迹；
L3：首先根据式（20）计算出当前观测船位与圆心的距离，然后根据式（21）计算出圆弧段航迹偏差/>：
（20）；
(21)；
再根据式（22）垂向速度，根据式（23）计算出期望艏向；
(22)；
(23)；
其中：为转弯圆心至船舶当前位置的连线与圆弧交点在圆弧上的切线方向角，，
再用当前观测艏向减去期望艏向，若差值大于零，则向左修正艏向使船舶达到期望艏向靠近航迹，若差值小于零，则向右修正艏向使船舶达到期望艏向靠近航迹；
L4：根据式（24）计算出观测船位至直线段航迹BC的航迹偏差，根据式（25）计算垂向速度/>，再根据式（26）计算出期望艏向；
(24)
（25）
（26），
再用当前观测艏向减去期望艏向，若差值大于零，则向左修正艏向使船舶达到期望艏向靠近航迹，若差值小于零，则向右修正艏向使船舶达到期望艏向靠近航迹。
发明的有益效果：
1、将状态观测与循迹导航方法相结合，通过船舶运动状态最佳估计的方法获取船舶运动信息，作为高速循迹导航方法的输入，避免了测量噪声干扰；
2、设计了分段引导逻辑及流程，可快速判断船舶当前航向在直线航段还是圆弧航段，并在不同航段采用不同算法计算期望艏向；
3、在循迹导航算法中引入垂向速度概念及计算方法，且考虑直线航段航行时对环境力干扰的补偿，从而减少航迹偏差控制超调及震荡、提高循迹精度。
附图说明
图1是本发明流程图。
图2是直线航段引导策略原理图。
图3是圆弧航段航迹右转引导策略原理图。
图4是圆弧航段航迹左转引导策略原理图。
具体实施方式
基于状态观测的动力定位船舶高速循迹导航方法，其流程图如附图1所示：具体包括如下步骤：
J1： 建立动力定位船舶水平面三自由度运动数学模型；
首先建立船舶的低频运动数学模型为式（3）：
（3）；
其中，/>，，/>，
为船舶惯性矩阵，/>为控制输入；/>为描述推进器配置的控制矩阵；/>为三维对角阵，表示过程噪声的幅值；/>为低频运动数学模型零均值高斯白噪声向量；/>为船舶质量，/>为船舶转动惯量，/>为船舶质心纵向坐标，/>为纵向水动力加速度导数，/>为横向水动力加速度导数，/>为艏向对横向的耦合水动力加速度导数，/>为横向对艏向的耦合水动力加速度导数，/>为艏向水动力加速度导数；D为船舶阻尼矩阵，/>为纵向水动力速度导数，/>为横向水动力速度导数，/>为艏向对横向的耦合水动力速度导数，/>为横向对艏向的耦合水动力速度导数，/>为艏向水动力速度导数；/>为纵荡、横荡和艏摇三个自由度上的未知环境力，/>，/>为包含时间常数的三维对角阵；/>为三维对角阵，表示未知环境力的幅值；/>为零均值高斯白噪声向量，/>为在船体坐标系下，船舶的运动速度及角速度状态向量，/>，/>为纵荡速度，/>为横荡速度，/>为艏摇角速度，T代表矩阵转置，/>为艏向，/>为坐标变换阵；
注：本发明中所定义的大地坐标系为北东地系，即正北方向为横轴，正东方向为纵轴；船体坐标系为前右下系，即船艏方向为横轴，右舷方向为纵轴。
由于船舶的高频运动是船舶对一阶波浪力的响应，在位置和艏向上视为附加了阻尼项的二阶谐波振荡器，由此建立船舶的高频运动数学模型为式（4）：
（4）；
其中：表示模型的传递函数，/>表示/>自由度波浪强度；/>=1,2,3分别表示纵荡、横荡及艏摇三个自由度；/>表示/>自由度相对阻尼系数；/>表示/>自由度波浪主导频率，/>表示模型的传递函数，/>表示虚变量，将式（4）表示为状态空间形式为式（5）：
（5）；
式中，为船舶高频状态向量；/>表示高频运动纵荡，/>表示高频横荡位置，/>表示高频艏向角度，/>表示/>的积分，/>表示/>的积分，表示/>的积分，/>为高频运动数学模型零均值高斯白噪声向量；/>为三维向量，分别表示高频运动纵荡、横荡位置和艏向角度，/>、/>、/>分别为系数矩阵；
，
其中I为单位阵。
动力定位船舶测量传感器提供带有测量噪声的高、低频运动位置及艏向信息，由此建立系统测量模型式（6）：
（6）；
其中，表示系统测量模型，/>为零均值高斯白噪声三维向量，/>为在大地坐标系下，船舶的运动位置及艏向状态向量；/>；
综合低频运动数学模型、高频运动数学模型及系统测量模型，得到动力定位船舶非线性数学模型式（7）：
（7）；
将式（7）表示为状态空间形式得到动力定位船舶水平面三自由度运动数学模型式（1）、式（2）：
（1）；
（2）；
式中：状态向量为15维状态向量；/>为非线性状态转移函数，/>为三自由度运动数学模型的零均值高斯白噪声向量，/>为零均值高斯白噪声三维向量；
；
E为噪声系数矩阵：，
H为观测矩阵：；
J2： 将动力定位船舶水平面三自由度运动数学模型的噪声信息滤除并分离出高、低频运动信息，对动力定位船舶水平面三自由度运动数学模型中的状态向量进行估计，得到状态向量/>的估计值；
扩展卡尔曼滤波方法为现有技术，本发明中的船舶水平面三自由度运动数学模型是一个非线性连续系统，扩展卡尔曼滤波算法将非线性函数围绕滤波值展开成泰勒级数，略去二次以上的项，得到非线性系统的线性化模型，再应用卡尔曼滤波基本方程，得到滤波问题的解。
具体操作时可按如下方法进行：
将J1中的式（1）、式（2）用非线性连续系统表示为式（27）、（28）：
（27）；
（28）；
其中，为t时刻系统的状态向量；/>为t时刻系统测量值；/>及分别表示系统模型、测量模型的函数关系；/>和/>分别为t时刻过程噪声向量及测量噪声向量；
将式（27）离散化，得到式（29）：
(29)；
其中：离散周期为，/>表示离散的第k时刻的状态向量，表示离散的第k+1时刻的状态向量，一阶偏导，/>表示离散化误差与动态系统不确定性的总和，为零均值高斯白噪声；其协方差阵/>,/>为系统噪声矩阵，/>为克罗尼克函数，满足/>；
将式（28）离散化，得到式（30）：
（30）；
其中：表示离散的第k时刻的传感器测量值；测量噪声/>为零均值高斯白噪声，其协方差阵/>；其中/>为测量噪声矩阵，/>为克罗尼克函数，满足/>；
利用扩展卡尔曼滤波方法，将式（30）围绕滤波值线性化，并舍去二次以上的项，得到系统状态一步预测值式（31）：
（31）；
预测误差方差阵为式（32）：
（32）；
其中：，I为单位阵，/>。
滤波增益矩阵为式（33）：
（33）；/>
式中：，
状态观测更新值为式（34）：
（34）；
滤波误差方差阵为式（35）：
（35）；
对于动力定位船舶水平面三自由度运动数学模型式（1）和式（2），给定0时刻滤波初值及滤波误差方差阵初值/>，通过（31）、（32）、（33）、（34）、（35）进行循环迭代，即得到动力定位船舶运动模型中状态向量/>的估计值。
J3：根据状态向量的估计值，结合用户输入航迹信息，计算出动力定位船舶期望艏向，并根据期望艏向进行循迹导航。
计算动力定位船舶期望艏向的具体步骤如下：
L1：取用户输入航迹上的三点，其坐标分别为A、B/>、C/>，动力定位船舶当前观测位置坐标为S/>，由环境力引起的漂角观测值/>为船舶观测合速度/>的方向角/>与观测艏向/>的夹角，/>；
根据用户输入航迹判断动力定位船舶航行在用户输入航迹的直线段还是圆弧段，确定分段引导策略；
其中、/>、/>均属于状态向量/>的估计值中的量值，高速循迹时/>，则/>时/>为正，/>时/>为负。
用户输入航迹拐弯之前航段AB段航迹方向角为， 具体直线航段引导策略原理图如附图2所示，拐弯之后航段BC段航迹方向角/>，圆弧航段航迹右转引导策略原理图如附图3所示，圆弧航段航迹左转引导策略原理图如附图4所示，根据式（8）计算出/>，根据式（9）计算出/>：/>
（8）；
（9）；
若则航迹拐点B点处圆弧段航迹为右转，若则航迹拐点B点处圆弧段航迹为左转；
根据式（10）计算圆弧段圆心O的坐标；
（10）；
其中：为圆弧段圆心坐标，/>为航迹拐点处的转弯半径；圆弧与AB、BC段的切点分别为F、G；
根据式（11）计算出圆心O至当前观测船位S的方位角：/>
（11）；
根据几何关系可得右转时航迹拐点至转弯圆心连线即BO的方向角为式（12），左转时航迹拐点至转弯圆心连线BO的方向角/>为式（13）：而无论是左转还是右转时，转弯圆心至航迹拐点连线OB的方位角/>均为式（14）：
（12）；
（13）；
（14）；
由此可得转弯圆心至转弯圆弧起点连线OF的方向角及转弯圆心至转弯圆弧终点连线OG的方向角/>分别为式（15）、式（16）：
(15)
(16)，
其中，=1为航迹右转，/>=-1为航迹左转；
航迹右转时，若，则判断船舶航行在转弯之前航段，即AB航段，则跳转至步骤L2；若/>，则判断船舶航行在圆弧航段，则跳转至步骤L3；若/>则判断船舶已通过B处圆弧航段，航行在转弯之后航段,即BC航段，则跳转至步骤L4；/>
左转时，若则判断航行在转弯之前航段，即AB航段，则跳转至步骤L2；若则判断航行在圆弧航段，则跳转至步骤L3；若/>则判断已通过B处圆弧航段，航行在转弯之后航段，即BC航段，则跳转至步骤L4；
L2：根据式（17）计算出当前观测位置S到直线段航迹AB的航迹偏差d1，根据式（18）计算出垂向速度，根据式（19）计算出期望艏向：
（17）；
（18）；
（19）；
其中垂向速度为当前船舶观测合速度/>垂直于拐弯之前航段AB的分量，垂向速度正方向为/>顺时针旋转/>，/>、/>分别为航迹偏差增益系数及垂向速度增益系数，/>为环境力引起的漂角观测值，即为船舶观测合速度/>的方向角/>与观测艏向的夹角，/>；其中/>、/>、/>均属于状态向量/>的估计值中的量值，高速循迹时/>，则/>时/>为正，/>时/>为负。
再用当前观测艏向减去期望艏向，若差值大于零，则向左修正艏向使船舶达到期望艏向靠近航迹，若差值小于零，则向右修正艏向使船舶达到期望艏向靠近航迹；
L3： 首先根据式（20）计算出当前观测船位S与圆心O的距离，然后根据式（21）计算出圆弧段航迹偏差/>：
（20）；
(21)；
再根据式（22）垂向速度，根据式（23）计算出期望艏向；
(22)；
(23)；
其中：为转弯圆心至船舶当前位置的连线与圆弧交点在圆弧上的切线方向角，/>；
再用当前观测艏向减去期望艏向，若差值大于零，则向左修正艏向使船舶达到期望艏向靠近航迹，若差值小于零，则向右修正艏向使船舶达到期望艏向靠近航迹；
L4：根据式（24）计算出观测船位S至直线段航迹BC的航迹偏差，根据式（25）计算垂向速度/>，再根据式（26）计算出期望艏向；
(24)；
（25）；
（26）；
再用当前观测艏向减去期望艏向，若差值大于零，则向左修正艏向使船舶达到期望艏向靠近航迹，若差值小于零，则向右修正艏向使船舶达到期望艏向靠近航迹。
由于在无论船舶航行在直线段还是圆弧段，其期望艏向均与航迹偏差及垂向速度/>相关，其中航迹偏差/>可以用来判断当前观测船位在航迹的左侧还是右侧，航迹偏差/>包括/>、/>、/>，当/>＜0时，当前观测船位在航迹左侧，当/>＞0时，当前观测船位在航迹右侧。而垂向速度/>项起阻尼作用，使得船舶逼近或远离航迹的速度不要过快从而避免航迹偏差超调甚至震荡；而直线段漂角/>补偿项作用为消除航迹偏差的稳态误差。
本发明提供的基于状态观测的动力定位船舶高速循迹导航方法，将状态观测与循迹导航方法相结合，通过船舶运动状态最佳估计的方法获取船舶运动信息，作为高速循迹导航方法的输入，避免了测量噪声干扰；通过设计分段引导逻辑及流程，可快速判断船舶当前航向在直线航段还是圆弧航段，并在不同航段采用不同算法计算期望艏向；在循迹引导算法中引入垂向速度概念及计算方法，且考虑直线航段航行时对环境力干扰的补偿，从而减少航迹偏差控制超调及震荡、提高循迹精度。
综上所述，本发明提出的基于状态观测的动力定位船舶高速循迹导航方法，通过建立船舶运动数学模型式，并对状态向量进行估计，然后再利用状态向量/>的估计值，结合用户输入航迹信息，计算出动力定位船舶期望艏向，并根据期望艏向进行分段循迹导航，可以实现较高的循迹精度，保证船舶按照预先设定的航迹航行。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (2)
1.基于状态观测的动力定位船舶高速循迹导航方法，其特征在于：包括如下步骤：
J1：建立动力定位船舶水平面三自由度运动数学模型为式（1）和式（2）；
（1）；
（2）；
式中：为状态向量，/>为非线性状态转移函数，/>为描述推进器配置的控制矩阵，为控制输入，/>为噪声系数矩阵，/>为三自由度运动数学模型的零均值高斯白噪声向量，为系统测量模型，/>为观测矩阵，/>为零均值高斯白噪声三维向量；
J2：将动力定位船舶水平面三自由度运动数学模型的噪声信息滤除并分离出高、低频运动信息，对动力定位船舶水平面三自由度运动数学模型中的状态向量进行估计，得到状态向量/>的估计值；
J3：根据状态向量的估计值，结合用户输入航迹信息，计算出动力定位船舶期望艏向，并根据期望艏向进行循迹导航；
计算动力定位船舶期望艏向的具体步骤如下：
L1：根据用户输入航迹判断动力定位船舶航行在用户输入航迹的直线段还是圆弧段，确定分段引导策略；
首先根据式（8）及式（9）分别计算用户输入航迹拐弯之前航段的航迹方向角及拐弯之后航段的航迹方向角/>，然后根据计算出的/>及/>判断船舶左转还是右转，若则为右转，若/>则为左转；
（8）；
（9）；
其中：、/>、/>分别为用户输入航迹上的三个点的坐标，/>为动力定位船舶当前观测位置坐标；
然后，根据式（10）计算圆弧段圆心的坐标；
（10）；其中：/>为圆弧段圆心坐标，/>为航迹拐点处的转弯半径；
再根据式（11）计算出圆心至当前观测船位的方位角：
（11）；
再根据几何关系得到右转时航迹拐点至转弯圆心连线的方位角为式（12），左转时航迹拐点至转弯圆心连线的方向角/>为式（13），而无论是左转还是右转时，转弯圆心至航迹拐点连线的方位角/>均为式（14）：
（12）；
（13）；
（14）；
并由此得出转弯圆心至转弯圆弧起点连线的方向角及转弯圆心至转弯圆弧终点连线的方向角/>分别为式（15）、式（16）：
(15)；
(16)；
其中，=1为航迹右转，/>=-1为航迹左转；
最后航迹右转时，若，则判断船舶航行在转弯之前航段，则跳转至步骤L2；若，则判断船舶航行在圆弧航段，则跳转至步骤L3；若/>则判断船舶已通过圆弧航段，航行在转弯之后航段，则跳转至步骤L4；
左转时，若则判断航行在转弯之前航段，则跳转至步骤L2；若/>则判断航行在圆弧航段，则跳转至步骤L3；若/>则判断已通过圆弧航段，航行在转弯之后航段，则跳转至步骤L4；
L2：根据式（17）计算出当前观测位置到转弯之前航段的航迹偏差，根据式（18）计算出垂向速度/>，根据式（19）计算出期望艏向：
（17）；
（18）；
（19）；
其中垂向速度为当前船舶观测合速度/>垂直于拐弯之前航段的分量，垂向速度正方向为/>顺时针旋转/>，/>、/>分别为航迹偏差增益系数及垂向速度增益系数，/>为环境力引起的漂角观测值，/>为纵荡速度，/>为横荡速度，/>为艏向；
再用当前观测艏向减去期望艏向，若差值大于零，则向左修正艏向使船舶达到期望艏向靠近航迹，若差值小于零，则向右修正艏向使船舶达到期望艏向靠近航迹；
L3：首先根据式（20）计算出当前观测船位与圆心的距离，然后根据式（21）计算出圆弧段航迹偏差/>：
（20）；
(21)；
再根据式（22）垂向速度，根据式（23）计算出期望艏向；
(22)；
(23)；
其中：为转弯圆心至船舶当前位置的连线与圆弧交点在圆弧上的切线方向角，，
再用当前观测艏向减去期望艏向，若差值大于零，则向左修正艏向使船舶达到期望艏向靠近航迹，若差值小于零，则向右修正艏向使船舶达到期望艏向靠近航迹；
L4：根据式（24）计算出观测船位至直线段航迹BC的航迹偏差，根据式（25）计算垂向速度/>，再根据式（26）计算出期望艏向；
(24)
（25）
（26），
再用当前观测艏向减去期望艏向，若差值大于零，则向左修正艏向使船舶达到期望艏向靠近航迹，若差值小于零，则向右修正艏向使船舶达到期望艏向靠近航迹。
2.根据权利要求1所述的基于状态观测的动力定位船舶高速循迹导航方法，其特征在于：步骤J1中建立动力定位船舶水平面三自由度运动数学模型的方法如下：
首先建立船舶的低频运动数学模型如式（3）所示：
（3）；
式中：为船舶惯性矩阵，/>为在船体坐标系下船舶的运动速度及角速度状态向量，D为船舶阻尼矩阵，/>为坐标变换阵，T代表矩阵转置，/>为纵荡、横荡和艏摇三个自由度上的未知环境力，/>为过程噪声的幅值，/>为低频运动数学模型零均值高斯白噪声向量；
其次，建立船舶的高频运动数学模型式（4），并将式（4）表示为状态空间形式为式（5）；
（4）；
（5）；
其中：表示模型的传递函数，/>表示/>自由度波浪强度；/>表示虚变量，/>表示/>自由度相对阻尼系数，/>表示/>自由度波浪主导频率，/>=1,2,3分别表示纵荡、横荡及艏摇三个自由度；/>为船舶高频状态向量；/>为高频运动纵荡、横荡位置和艏向角度三维向量，/>、/>、/>均为系数矩阵，/>为高频运动数学模型零均值高斯白噪声向量；
然后建立系统测量模型式（6）：
（6）；
其中：为在大地坐标系下船舶的运动位置及艏向状态向量；
最后综合低频运动数学模型、高频运动数学模型及系统测量模型，得到动力定位船舶非线性数学模型式（7），并将式（7）表示为状态空间形式得到动力定位船舶水平面三自由度运动数学模型：
（7）；
式中：为包含时间常数的三维对角阵；/>为三维对角阵，表示未知环境力的幅值；/>为未知环境力模型的零均值高斯白噪声向量。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311027264.1A CN116753961B (zh) | 2023-08-16 | 2023-08-16 | 基于状态观测的动力定位船舶高速循迹导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311027264.1A CN116753961B (zh) | 2023-08-16 | 2023-08-16 | 基于状态观测的动力定位船舶高速循迹导航方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116753961A CN116753961A (zh) | 2023-09-15 |
| CN116753961B true CN116753961B (zh) | 2023-10-31 |
Family
ID=87959432
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311027264.1A Active CN116753961B (zh) | 2023-08-16 | 2023-08-16 | 基于状态观测的动力定位船舶高速循迹导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116753961B (zh) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2005308428A (ja) * | 2004-04-19 | 2005-11-04 | Mitsubishi Electric Corp | 位置推定装置 |
| CN101804862A (zh) * | 2010-04-07 | 2010-08-18 | 南京航空航天大学 | 一种无人机推力变向装置及其控制方法 |
| CN103344245A (zh) * | 2013-07-09 | 2013-10-09 | 北京航空航天大学 | 火星进入段imu和甚高频无线电组合导航的ud-skf方法 |
| CN103576555A (zh) * | 2013-11-14 | 2014-02-12 | 哈尔滨工程大学 | 一种动力定位船舶循迹导引控制方法 |
| CN104635773A (zh) * | 2015-01-13 | 2015-05-20 | 哈尔滨工程大学 | 一种基于改进强跟踪滤波状态观测器的船舶动力定位方法 |
| CN109884901A (zh) * | 2019-04-04 | 2019-06-14 | 中国人民解放军海军工程大学 | 一种船舶综合操控系统的滑模滤波器设计方法 |
| CN110908395A (zh) * | 2019-11-26 | 2020-03-24 | 西北工业大学 | 改进的无人机航迹实时规划方法 |
| CN111538339A (zh) * | 2020-05-29 | 2020-08-14 | 智慧航海（青岛）科技有限公司 | 船舶航迹控制方法及装置 |
| CN115865390A (zh) * | 2022-07-19 | 2023-03-28 | 浙江树人学院 | 一种fdi攻击下非线性信息物理系统滤波方法及系统 |
| WO2023050636A1 (zh) * | 2021-09-29 | 2023-04-06 | 哈尔滨工程大学 | 一种全垫升气垫船路径跟踪方法 |
-
2023
- 2023-08-16 CN CN202311027264.1A patent/CN116753961B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2005308428A (ja) * | 2004-04-19 | 2005-11-04 | Mitsubishi Electric Corp | 位置推定装置 |
| CN101804862A (zh) * | 2010-04-07 | 2010-08-18 | 南京航空航天大学 | 一种无人机推力变向装置及其控制方法 |
| CN103344245A (zh) * | 2013-07-09 | 2013-10-09 | 北京航空航天大学 | 火星进入段imu和甚高频无线电组合导航的ud-skf方法 |
| CN103576555A (zh) * | 2013-11-14 | 2014-02-12 | 哈尔滨工程大学 | 一种动力定位船舶循迹导引控制方法 |
| CN104635773A (zh) * | 2015-01-13 | 2015-05-20 | 哈尔滨工程大学 | 一种基于改进强跟踪滤波状态观测器的船舶动力定位方法 |
| CN109884901A (zh) * | 2019-04-04 | 2019-06-14 | 中国人民解放军海军工程大学 | 一种船舶综合操控系统的滑模滤波器设计方法 |
| CN110908395A (zh) * | 2019-11-26 | 2020-03-24 | 西北工业大学 | 改进的无人机航迹实时规划方法 |
| CN111538339A (zh) * | 2020-05-29 | 2020-08-14 | 智慧航海（青岛）科技有限公司 | 船舶航迹控制方法及装置 |
| WO2023050636A1 (zh) * | 2021-09-29 | 2023-04-06 | 哈尔滨工程大学 | 一种全垫升气垫船路径跟踪方法 |
| CN115865390A (zh) * | 2022-07-19 | 2023-03-28 | 浙江树人学院 | 一种fdi攻击下非线性信息物理系统滤波方法及系统 |
Non-Patent Citations (2)
| Title |
|---|
| 基于滤波新息指导的降维动态模型库建立算法;武青海;曲朝阳;;电光与控制(第12期);全文 * |
| 基于载机速度变化分析的SAR运动补偿;徐凯 等;计算机工程与应用;第208-210页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116753961A (zh) | 2023-09-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111580523B (zh) | 一种基于侧滑角补偿的无人艇路径跟踪自抗扰控制方法 | |
| CN105785999B (zh) | 无人艇航向运动控制方法 | |
| CN106950974B (zh) | 欠驱动自主水下航行器的对三维路径进行理解及跟踪控制方法 | |
| Morgado et al. | Embedded vehicle dynamics aiding for USBL/INS underwater navigation system | |
| Karmozdi et al. | INS-DVL navigation improvement using rotational motion dynamic model of AUV | |
| CN111562742B (zh) | 一种欠驱动船舶的在线建模与自适应控制方法 | |
| CN110308719B (zh) | 一种水面无人艇路径跟踪控制方法 | |
| RU2442718C1 (ru) | Способ определения гидродинамических параметров математической модели движения судна | |
| CN108460210B (zh) | 一种基于噪残差和协方差匹配的动力定位系统噪声特性实时估计方法 | |
| CN102411657A (zh) | 自由航行状态下耙吸挖泥船动力定位的滤波器设计方法 | |
| CN111798702B (zh) | 无人艇路径跟踪控制方法、系统、存储介质及终端 | |
| CN112015086B (zh) | 一种欠驱动水面船有限时间路径跟踪输出反馈控制方法 | |
| Chu et al. | Improved super-twisting sliding mode control for ship heading with sideslip angle compensation | |
| CN116753961B (zh) | 基于状态观测的动力定位船舶高速循迹导航方法 | |
| CN113467231A (zh) | 基于侧滑补偿ilos制导律的无人艇路径跟踪方法 | |
| Liu | Pre-filtered backstepping control for underactuated ship path following | |
| JP6487264B2 (ja) | ウェイポイント生成装置 | |
| CN111506090A (zh) | 一种水下机器人深度区间控制方法和系统 | |
| RU2223197C1 (ru) | Аппаратура автоматического управления движением судна | |
| CN106919172B (zh) | 一种动力定位船舶跟踪的导引控制方法 | |
| Xu et al. | Path following control for large inland ships in a restricted waterway using the nonlinear terminal sliding mode method | |
| CN115421483A (zh) | 一种无人船操纵运动预报方法 | |
| Chen et al. | Improved line-of-sight nonlinear trajectory tracking control of autonomous underwater vehicle exposed to high variable speed ocean currents | |
| Papoulias et al. | Hopf bifurcations and nonlinear studies of gain margins in path control of marine vehicles | |
| LYU et al. | A SR-UKF-based method to identify submarine hydrodynamic coefficients |
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 |