CN104635773B - 一种基于改进强跟踪滤波状态观测器的船舶动力定位方法 - Google Patents
一种基于改进强跟踪滤波状态观测器的船舶动力定位方法 Download PDFInfo
- Publication number
- CN104635773B CN104635773B CN201510016643.XA CN201510016643A CN104635773B CN 104635773 B CN104635773 B CN 104635773B CN 201510016643 A CN201510016643 A CN 201510016643A CN 104635773 B CN104635773 B CN 104635773B
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- msubsup
- mover
- mtd
- 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
Landscapes
- Navigation (AREA)
- Feedback Control In General (AREA)
Abstract
本发明公开了一种基于改进强跟踪滤波状态观测器的船舶动力定位方法，包括以下步骤：采集船舶的位置和艏向；利用改进强跟踪滤波状态观测器滤除掉船舶位置和艏向中噪声部分，得到满足精度要求的船舶位置和艏向估计值发送给PID控制器；PID控制器根据船舶位置和艏向期望值和接收到的信息，输出控制力和力矩，控制船舶运动。本发明在外界干扰发生突变的情况下跟踪滤波效果更好，本发明能够提高船舶的稳定性。
Description
技术领域
本发明属于动力定位船舶控制领域，尤其涉及一种针对外界环境突变情况下的，一种基于改进强跟踪滤波状态观测器的船舶动力定位方法。
背景技术
对海洋进行充分地探索和研究，进行合理而有效的资源开采，对人类社会的发展起着不可估量的作用。随着海洋资源开采不断向深海推进，现如今的一些海上作业平台，比如浮式生产储油轮、海洋钻井平台等，虽然仍可以完成油气开采、铺管铺缆、海洋地质勘探等，但综合考虑效果均不理想。在此背景下，船舶动力定位***(Dynamic PositioningSystem，简称DPS)应运而生。近年来，动力定位***对获取海洋资源的主导权起着关键性的作用。船舶动力定位***是一种通过推进***产生推力以抵抗外界的环境干扰(风、波浪和海流)，从而动态控制船舶定位在某确定的位置或按照一定的预设航线航行的技术。
在海上运动的船舶，会受到风、浪、流和各种噪声的干扰。这些干扰中，风、低频二阶波浪、流会使船舶发生的缓慢的漂移，可以通过动力定位的推进器进行补偿抵消；高频一阶波浪和噪声干扰，不会引起船舶位置的改变，但会引起不规则的振荡运动，加大动力定位***的推进器的磨损，从而减少推进器的使用寿命。设计的状态观测器，正是要将高频一阶波浪和噪声滤除，使推进器只对低频的干扰做出响应。本专利中所采用的改进强跟踪滤波算法相对于其他的滤波算法来说，在外界干扰发生突变的情况下跟踪滤波效果更好。
发明内容
本发明的目的是提供一种能够提高船舶稳定性的，一种基于改进强跟踪滤波状态观测器的船舶动力定位方法。
本发明是通过以下技术方案实现的：
一种基于改进强跟踪滤波状态观测器的船舶动力定位方法，包括以下几个步骤：
步骤一：采集船舶的位置和艏向；
步骤二：利用改进强跟踪滤波状态观测器滤除掉船舶位置和艏向中噪声部分，得到满足精度要求的船舶位置和艏向估计值发送给PID控制器；
步骤三：PID控制器根据船舶位置和艏向期望值和接收到的信息，输出控制力和力矩，控制船舶运动。
本发明一种基于改进强跟踪滤波状态观测器的船舶动力定位方法，还可以包括：
1、利用改进强跟踪滤波状态观测器滤除掉船舶位置和艏向中噪声部分的方法为：
步骤一：建立船舶运动数学模型的状态空间方程：
z＝h(x，v)
其中，f是船舶***的非线性***状态函数矩阵，h是非线性***量测函数，x＝[ηTνT]T是选定的状态向量，η＝[x y ψ]T表示在北东坐标系下的船舶的位置和姿态角，ν＝[u vr]T表示在船体坐标系下的船舶的线速度和角速度，u＝[X Y N]T是输入向量表示在船体坐标系下的船舶所受的力和力矩，z为输出向量表示在北东坐标系下的包含噪声部分的船舶位置和艏向，w＝wv和v＝wz为互不相关的零均值高斯白噪声，
将状态空间方程的离散化表示为：
步骤二：将包含噪声部分的船舶位置和艏向zk作为改进强跟踪滤波状态观测器的输入，得到满足精度要求的船舶位置和艏向估计值
2、改进强跟踪滤波状态观测器通过以下设计步骤实现：
a、确定状态初始值、误差协方差初值p0、遗忘因子ρ、弱化因子β和常数αi(i),i＝1,2,…,n；
b、计算残差：
其中，Hk为函数h进行泰勒展开所得到的雅格比矩阵，
c、计算多重次优渐消因子矩阵和矩阵
确定残差协方差阵Vk：
其中，ρ为预先设定的遗忘因子；
求得多重次优渐消因子矩阵λk：
其中αi为常数，系数
其中，Qk和Rk分别为环境干扰高频噪声和测量噪声的方差，β为预先设定的弱化因子，pk为误差估计协方差，Φk和Γk为函数f进行泰勒展开所得的雅格比矩阵，
根据多重次优渐消因子矩阵求得矩阵
d、根据矩阵计算状态预报误差协方差pk|k-1：
e、根据状态预报误差协方差pk|k-1计算滤波增益kk
f、更新状态估计
g、更新误差估计协方差pk
保存滤波数据结果将中的作为满足要求的船舶位置和艏向估计值传送给PID控制器，重复步骤b到步骤h，直到数据处理结束。
3、PID控制器为：
τ＝[τX τY τN]T表示船舶推进器输出推力和力矩，KP，KI和KD为PID算法中的比例系数；ηe为船舶期望目标位置姿态ηd和实际位置姿态η之间的偏差，即ηe＝ηd-η，η＝[x,y,ψ]T分别对应船舶实际北东位置和艏向角；ηd＝[xd,yd,ψd]T分别对应船舶期望北东位置和艏向角。有益效果：
本专利中所采用的改进强跟踪滤波算法相对于其他的滤波算法来说，在外界干扰发生突变的情况下跟踪滤波效果更好。扩展卡尔曼滤波(Extended Kalman Filter，简称EKF)在模型不确定时，鲁棒性差；且当滤波达到稳定状态时，EKF的增益矩阵Kk为极小值，若***发生突变，就使得EKF不能跟踪***的状态，丧失了对突变状态的跟踪能力。强跟踪滤波(Strong Tracking Filter，简称STF)算法基于正交原理，在预报误差协方差中引入时变渐消因子λk，来减弱旧观测量对当前滤波估计值的影响，对突变状态具有很强的跟踪能力。
当用多重次优渐消因子的强跟踪滤波时，λk是一个对角矩阵，对角元素λi绝大多数情况下是不完全相等的，所以λi乘以正定阵所得到的不一定是正定阵，那么最终的状态预报误差协方差就不是正定阵了，严重情况下有可能导致滤波发散。针对这一现象，引用改进强跟踪滤波算法(Improved Strong Track Filter，ISTF)，运用Cholesky的三角化分解思想，改变多重时变渐消因子在状态预报误差协方差公式中的作用方式，使得本发明具有更好的稳定性。
附图说明
图1为本发明方法流程图；
图2为本发明***结构方框图。
具体实施方式
下面将结合附图对本发明做进一步详细说明。
附图1是采用基于改进强跟踪滤波方法的状态观测器的船舶的运动控制流程图，描述了船舶运动控制器的处理过程。
本发明的目的按如下步骤实现：
步骤1：在有环境干扰的情况下，通过传感器***来采集动力定位船舶的位置和艏向信息。通过这些位置和艏向信息就可以抽象出船舶运动的动力学和运动学过程，其中动力定位船舶的运动学数学模型为：
z＝η+ωz
其中，η＝[x y ψ]T表示在北东坐标系下的船舶的位置和姿态角；ν＝[u v r]T表示在船体坐标系下的船舶的线速度和角速度；u＝[X Y N]T是输入向量，表示在船体坐标系下的船舶所受的力和力矩；τ＝[X Y N]T表示在船体坐标系下的船舶所受的力和力矩；J(η)为坐标转换矩阵，实现船体坐标系的线速度和角速度与北东坐标系的位置导数和姿态角导数的相互转化；M为***惯性矩阵，C(ν)为科里奥利向心力矩阵，D(ν)为阻尼系数矩阵；ωv和ωz均是零均值高斯白噪声，代表测量误差，Eν是ωv相应的系数矩阵。
为方便步骤2中状态观测器的设计，将其化为状态空间方程的标准形式，如下式所示：
z＝h(x,v)
上式中，f是***的非线性***状态函数矩阵，h是非线性***量测函数。x＝[ηTνT]T是选定的状态向量，z为输出向量，对应的在北东坐标系下的船舶的位置和艏向(包含环境干扰和测量噪声所带来的噪声部分)；w和v为互不相关的零均值高斯白噪声，分别代表环境干扰和量测误差。
对上述状态空间方程进行离散化处理，可以得到下式：
xk＝fk-1(xk-1,uk-1,wk-1)
zk＝hk(xk,vk)
其中wk和vk的统计特性如下：
其中，δkl表达式为：
那么，zk就是对应的在北东坐标系下的船舶的位置和艏向(包含环境干扰和测量噪声所带来的噪声部分)，作为步骤2中滤波输入。
步骤2：利用基于改进强跟踪滤波方法的状态观测器滤除掉波浪干扰中的高频部分和测量传感器在测量船舶位置和艏向过程中产生的测量噪声，将估计出的满足精度要求的船舶的位置和艏向信息发送给状态反馈控制器。基于改进强跟踪滤波算法的状态观测器通过以下设计步骤实现：
①定初始值
确定状态初始值误差协方差初值p0，遗忘因子ρ，弱化因子β，和经验常数αi(i)(i＝1,2,…,n)。
②计算残差
残差计算公式如下
其中，Hk为函数h进行泰勒展开所得到的雅格比矩阵，和Hk的具体计算公式如下：
③计算多重次优渐消因子矩阵λk和
1)确定残差协方差阵Vk，公式如下
其中，ρ为预先确定的遗忘因子。
2)先计算出Mk和Nk，从而计算系数ck。
其中，Qk,Rk为环境干扰高频噪声和测量噪声的方差，β为预先确定的弱化因子，pk为误差估计协方差，具体公式见⑦；Φk和Γk的为函数f进行泰勒展开所得的雅格比矩阵，具体计算公式如下：
3)根据预先确定的经验常数αi，求得多重次优渐消因子矩阵λk。
4)根据λk求出
④计算状态预报误差协方差pk|k-1。
⑤计算滤波增益Kk。
⑥更新状态估计
⑦更新误差估计协方差pk。
⑧保存滤波数据结果因为选取的状态变量x＝[ηT νT]T，所以中的就是我们所需要的满足要求的船舶的运动位置和艏向信息将其反馈给状态反馈控制器。
循环上述步骤②—⑦，不断更新状态估计和误差估计协方差pk，就可以实现对动力定位船的位置和姿态信息中的噪声的滤除。
步骤3：设计PID控制器，调节相应的参数，并得出相应的控制力，控制船舶的运动。
本发明中采用的是非常基础和非常实用的PID控制器，如图2所示。PID控制器鲁棒性很强，对被控对象的特性变化不太敏感。在PID控制中，比例环节，积分环节和微分环节的控制是相互影响的，其参数选取的好坏直接影响PID控制器的控制效果。各环节的作用如下：
比例环节(Kp)的调整，控制器成比例的产生控制作用，达到减小偏差的效果；
积分环节(Ki)的调整，主要作用为消除静态误差，积分作用不能调节的太强，会导致***不稳定；
微分环节(Kd)的调整，影响***的动态特性，等于是提前引入修正信号，加快***调整速度。
根据PID控制器中的三个环节对***的不同影响及相互之间的影响合理的选取PID的参数。
PID控制器由式表示：
式中，τ＝[τX τY τN]T表示船舶推进器输出推力和力矩，KP，KI和KD为PID算法中的比例系数；ηe为船舶期望目标位置姿态ηd和实际位置姿态η之间的偏差，即ηe＝ηd-η。其中，η＝[x,y,ψ]T，分别对应船舶实际北东位置和艏向角；ηd＝[xd,yd,ψd]T，分别对应船舶期望北东位置和艏向角。
Claims (2)
1.一种基于改进强跟踪滤波状态观测器的船舶动力定位方法，其特征在于，包括以下几个步骤：
步骤一：采集船舶的位置和艏向；
步骤二：利用改进强跟踪滤波状态观测器滤除掉船舶位置和艏向中噪声部分，得到满足精度要求的船舶位置和艏向估计值发送给PID控制器；
步骤三：PID控制器根据船舶位置和艏向期望值和接收到的信息，输出控制力和力矩，控制船舶运动；
所述的利用改进强跟踪滤波状态观测器滤除掉船舶位置和艏向中噪声部分的方法为：
步骤一：建立船舶运动数学模型的状态空间方程：
<mrow>
<mover>
<mi>x</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mi>f</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>,</mo>
<mi>u</mi>
<mo>,</mo>
<mi>w</mi>
<mo>)</mo>
</mrow>
</mrow>
z＝h(x,v)
其中，f是船舶***的非线性***状态函数矩阵，h是非线性***量测函数，x＝[ηT ν'T]T是选定的状态向量，η＝[ηx ηy ηψ]T表示在北东坐标系下的船舶的位置和姿态角，ν'＝[UV R]T表示在船体坐标系下的船舶的线速度和角速度，u＝[X Y N]T是输入向量表示在船体坐标系下的船舶所受的力和力矩，z为输出向量表示在北东坐标系下的包含噪声部分的船舶位置和艏向，w＝wv和v＝wz为互不相关的零均值高斯白噪声，
将状态空间方程的离散化表示为：
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>x</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mi>f</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>w</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>z</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mi>h</mi>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>x</mi>
<mi>k</mi>
</msub>
<mo>,</mo>
<msub>
<mi>v</mi>
<mi>k</mi>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>;</mo>
</mrow>
步骤二：将包含噪声部分的船舶位置和艏向zk作为改进强跟踪滤波状态观测器的输入，得到满足精度要求的船舶位置和艏向估计值
所述的改进强跟踪滤波状态观测器通过以下设计步骤实现：
a、确定状态初始值误差协方差初值p0、遗忘因子ρ、弱化因子β和常数αi(i),i＝1,2,…,n；
b、计算残差：
<mrow>
<msub>
<mi>S</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mi>z</mi>
<mi>k</mi>
</msub>
<mo>-</mo>
<msub>
<mi>H</mi>
<mi>k</mi>
</msub>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
其中，Hk为函数h进行泰勒展开所得到的雅格比矩阵，
<mrow>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<mi>f</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>H</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mo>&part;</mo>
<msub>
<mi>h</mi>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mi>k</mi>
</msub>
<mo>,</mo>
<msub>
<mi>v</mi>
<mi>k</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo>&part;</mo>
<msub>
<mi>x</mi>
<mi>k</mi>
</msub>
</mrow>
</mfrac>
<msub>
<mo>|</mo>
<mrow>
<msub>
<mi>x</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</msub>
<mo>;</mo>
</mrow>
c、计算多重次优渐消因子矩阵和矩阵
确定残差协方差阵Vk：
<mrow>
<msub>
<mi>V</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&epsiv;</mi>
<mn>1</mn>
</msub>
<msubsup>
<mi>&epsiv;</mi>
<mn>1</mn>
<mi>T</mi>
</msubsup>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>k</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mfrac>
<mrow>
<msub>
<mi>&rho;V</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>&epsiv;</mi>
<mi>k</mi>
</msub>
<msubsup>
<mi>&epsiv;</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
</mrow>
<mrow>
<mn>1</mn>
<mo>+</mo>
<mi>&rho;</mi>
</mrow>
</mfrac>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>k</mi>
<mo>&GreaterEqual;</mo>
<mn>2</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中，ρ为预先设定的遗忘因子；
求得多重次优渐消因子矩阵λk：
<mrow>
<msubsup>
<mi>&lambda;</mi>
<mi>k</mi>
<mi>i</mi>
</msubsup>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c</mi>
<mi>k</mi>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c</mi>
<mi>k</mi>
</msub>
<mo>&GreaterEqual;</mo>
<mn>1</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>1</mn>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c</mi>
<mi>k</mi>
</msub>
<mo><</mo>
<mn>1</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
其中αi为常数，系数
<mrow>
<mover>
<mi>A</mi>
<mo>&OverBar;</mo>
</mover>
<mo>=</mo>
<mi>d</mi>
<mi>i</mi>
<mi>a</mi>
<mi>g</mi>
<mo>&lsqb;</mo>
<msqrt>
<msub>
<mi>&alpha;</mi>
<mn>1</mn>
</msub>
</msqrt>
<mo>,</mo>
<msqrt>
<msub>
<mi>&alpha;</mi>
<mn>2</mn>
</msub>
</msqrt>
<mo>,</mo>
<mo>...</mo>
<mo>,</mo>
<msqrt>
<msub>
<mi>&alpha;</mi>
<mi>n</mi>
</msub>
</msqrt>
<mo>&rsqb;</mo>
<mo>,</mo>
</mrow>
<mrow>
<msub>
<mi>N</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mi>V</mi>
<mi>k</mi>
</msub>
<mo>-</mo>
<msub>
<mi>H</mi>
<mi>k</mi>
</msub>
<msub>
<mi>&Gamma;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>Q</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>&Gamma;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
<msubsup>
<mi>H</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<mo>-</mo>
<msub>
<mi>&beta;R</mi>
<mi>k</mi>
</msub>
</mrow>
<mrow>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<mover>
<mi>A</mi>
<mo>&OverBar;</mo>
</mover>
<msub>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
<msubsup>
<mi>H</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<msub>
<mi>H</mi>
<mi>k</mi>
</msub>
</mrow>
其中，Qk和Rk分别为环境干扰高频噪声和测量噪声的方差，β为预先设定的弱化因子，pk为误差估计协方差，Φk和Γk为函数f进行泰勒展开所得的雅格比矩阵，
<mrow>
<msub>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mo>&part;</mo>
<msub>
<mi>f</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>w</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo>&part;</mo>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mfrac>
<msub>
<mo>|</mo>
<mrow>
<msub>
<mi>x</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</msub>
</mrow>
<mrow>
<msub>
<mi>&Gamma;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mo>&part;</mo>
<msub>
<mi>f</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>w</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo>&part;</mo>
<msub>
<mi>w</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mfrac>
<msub>
<mo>|</mo>
<mrow>
<msub>
<mi>x</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</msub>
<mo>,</mo>
</mrow>
根据多重次优渐消因子矩阵求得矩阵
<mrow>
<msub>
<mover>
<mi>&Lambda;</mi>
<mo>&OverBar;</mo>
</mover>
<mi>k</mi>
</msub>
<mo>=</mo>
<mi>d</mi>
<mi>i</mi>
<mi>a</mi>
<mi>g</mi>
<mo>&lsqb;</mo>
<msqrt>
<msubsup>
<mi>&lambda;</mi>
<mi>k</mi>
<mn>1</mn>
</msubsup>
</msqrt>
<mo>,</mo>
<msqrt>
<msubsup>
<mi>&lambda;</mi>
<mi>k</mi>
<mn>2</mn>
</msubsup>
</msqrt>
<mo>,</mo>
<mo>...</mo>
<mo>,</mo>
<msqrt>
<msubsup>
<mi>&lambda;</mi>
<mi>k</mi>
<mi>n</mi>
</msubsup>
</msqrt>
<mo>&rsqb;</mo>
<mo>;</mo>
</mrow>
d、根据矩阵计算状态预报误差协方差pkk-1：
<mrow>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mover>
<mi>&Lambda;</mi>
<mo>&OverBar;</mo>
</mover>
<mi>k</mi>
</msub>
<msub>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>&Phi;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
<msubsup>
<mover>
<mi>&Lambda;</mi>
<mo>&OverBar;</mo>
</mover>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<mo>+</mo>
<msub>
<mi>&Gamma;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>Q</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>&Gamma;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
<mo>;</mo>
</mrow>
e、根据状态预报误差协方差pkk-1计算滤波增益kk
<mrow>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>H</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>H</mi>
<mi>k</mi>
</msub>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>H</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<mo>+</mo>
<msub>
<mi>R</mi>
<mi>k</mi>
</msub>
<mo>)</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
<mo>;</mo>
</mrow>
f、更新状态估计
<mrow>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>z</mi>
<mi>k</mi>
</msub>
<mo>-</mo>
<msub>
<mi>H</mi>
<mi>k</mi>
</msub>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
g、更新误差估计协方差pk
<mrow>
<msub>
<mi>P</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<mrow>
<mo>(</mo>
<mi>I</mi>
<mo>-</mo>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<msub>
<mi>H</mi>
<mi>k</mi>
</msub>
<mo>)</mo>
</mrow>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msup>
<mrow>
<mo>(</mo>
<mi>I</mi>
<mo>-</mo>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<msub>
<mi>H</mi>
<mi>k</mi>
</msub>
<mo>)</mo>
</mrow>
<mi>T</mi>
</msup>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<msub>
<mi>R</mi>
<mi>k</mi>
</msub>
<msubsup>
<mi>K</mi>
<mi>k</mi>
<mi>T</mi>
</msubsup>
<mo>;</mo>
</mrow>
h、保存滤波数据结果将中的作为满足要求的船舶位置和艏向估计值传送给PID控制器，重复步骤b到步骤h，直到数据处理结束。
2.根据权利要求1所述的一种基于改进强跟踪滤波状态观测器的船舶动力定位方法，其特征在于：所述的PID控制器为：
<mrow>
<mi>&tau;</mi>
<mo>=</mo>
<msub>
<mi>K</mi>
<mi>P</mi>
</msub>
<msub>
<mi>&eta;</mi>
<mi>e</mi>
</msub>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>I</mi>
</msub>
<mo>&Integral;</mo>
<msub>
<mi>&eta;</mi>
<mi>e</mi>
</msub>
<mi>d</mi>
<mi>t</mi>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>D</mi>
</msub>
<mfrac>
<mrow>
<msub>
<mi>d&eta;</mi>
<mi>e</mi>
</msub>
</mrow>
<mrow>
<mi>d</mi>
<mi>t</mi>
</mrow>
</mfrac>
</mrow>
τ＝[τX τY τN]T表示船舶推进器输出推力和力矩，KP，KI和KD为PID算法中的比例系数；ηe为船舶期望目标位置姿态ηd和实际位置姿态η之间的偏差，即ηe＝ηd-η，η＝[ηX,ηY,ηψ]T分别对应东北坐标系下的船舶的位置和姿态角；ηd＝[ηXd,ηYd,ηψd]T分别对应船舶期望北东位置和姿态角。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510016643.XA CN104635773B (zh) | 2015-01-13 | 2015-01-13 | 一种基于改进强跟踪滤波状态观测器的船舶动力定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510016643.XA CN104635773B (zh) | 2015-01-13 | 2015-01-13 | 一种基于改进强跟踪滤波状态观测器的船舶动力定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN104635773A CN104635773A (zh) | 2015-05-20 |
| CN104635773B true CN104635773B (zh) | 2018-04-17 |
Family
ID=53214639
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510016643.XA Active CN104635773B (zh) | 2015-01-13 | 2015-01-13 | 一种基于改进强跟踪滤波状态观测器的船舶动力定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN104635773B (zh) |
Families Citing this family (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106338919B (zh) * | 2016-11-02 | 2019-04-19 | 哈尔滨工程大学 | 基于增强学习型智能算法的无人艇航迹跟踪控制方法 |
| CN108195400A (zh) * | 2017-12-22 | 2018-06-22 | 清华大学 | 捷联式微机电惯性导航***的动基座对准方法 |
| CN108388738A (zh) * | 2018-03-01 | 2018-08-10 | 青岛科技大学 | 一种船舶动力定位***噪声和状态实时估计自适应滤波方法 |
| CN109143850A (zh) * | 2018-07-05 | 2019-01-04 | 江苏理工学院 | 基于强跟踪滤波算法的轨道车辆悬吊***参数估计方法 |
| CN108983774B (zh) * | 2018-07-16 | 2021-06-01 | 哈尔滨工程大学 | 一种基于模糊状态观测器的单喷泵推进无人水面艇自适应航向控制方法 |
| CN110456631B (zh) * | 2019-08-19 | 2022-12-09 | 哈尔滨工业大学 | 行星探测捕获制动与器器分离物理跟踪装置与方法 |
| CN111469999A (zh) * | 2020-04-29 | 2020-07-31 | 上海交通大学 | 基于波浪前馈的动力定位模型试验控制方法及*** |
| CN113296396B (zh) * | 2021-05-26 | 2022-06-03 | 广东电网有限责任公司 | 一种高频噪声功率增益的自动跟踪***及方法 |
| CN116753961B (zh) * | 2023-08-16 | 2023-10-31 | 中国船舶集团有限公司第七〇七研究所 | 基于状态观测的动力定位船舶高速循迹导航方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102411657A (zh) * | 2011-10-31 | 2012-04-11 | 江苏科技大学 | 自由航行状态下耙吸挖泥船动力定位的滤波器设计方法 |
| CN103605886A (zh) * | 2013-11-12 | 2014-02-26 | 中交天津航道局有限公司 | 一种船舶动力定位***多模型自适应融合滤波方法 |
| KR20140056887A (ko) * | 2012-11-02 | 2014-05-12 | 대우조선해양 주식회사 | 흘수 변화를 고려한 동적 위치 유지 시스템 및 그의 동적 위치 유지 방법 |
| CN103970021A (zh) * | 2014-05-21 | 2014-08-06 | 哈尔滨工程大学 | 一种基于模型预测控制的松弛动力定位控制*** |
| CN104199464A (zh) * | 2014-08-19 | 2014-12-10 | 哈尔滨工程大学 | 一种基于环境估计的实时环境最优艏向船舶动力定位控制方法 |
-
2015
- 2015-01-13 CN CN201510016643.XA patent/CN104635773B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102411657A (zh) * | 2011-10-31 | 2012-04-11 | 江苏科技大学 | 自由航行状态下耙吸挖泥船动力定位的滤波器设计方法 |
| KR20140056887A (ko) * | 2012-11-02 | 2014-05-12 | 대우조선해양 주식회사 | 흘수 변화를 고려한 동적 위치 유지 시스템 및 그의 동적 위치 유지 방법 |
| CN103605886A (zh) * | 2013-11-12 | 2014-02-26 | 中交天津航道局有限公司 | 一种船舶动力定位***多模型自适应融合滤波方法 |
| CN103970021A (zh) * | 2014-05-21 | 2014-08-06 | 哈尔滨工程大学 | 一种基于模型预测控制的松弛动力定位控制*** |
| CN104199464A (zh) * | 2014-08-19 | 2014-12-10 | 哈尔滨工程大学 | 一种基于环境估计的实时环境最优艏向船舶动力定位控制方法 |
Non-Patent Citations (1)
| Title |
|---|
| 船舶动力定位***多模型自适应融合滤波;杨金节 等;《舰船科学技术》;20140831;第36卷(第8期);第39页第2节第1段、第41页第3节末段 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN104635773A (zh) | 2015-05-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104635773B (zh) | 一种基于改进强跟踪滤波状态观测器的船舶动力定位方法 | |
| CN107168312B (zh) | 一种补偿uuv运动学和动力学干扰的空间轨迹跟踪控制方法 | |
| CN109507885B (zh) | 基于自抗扰的无模型自适应auv控制方法 | |
| Do | Global robust and adaptive output feedback dynamic positioning of surface ships | |
| CN103955218B (zh) | 一种基于非线性控制理论的无人艇轨迹跟踪控制装置及方法 | |
| CN109032136B (zh) | 基于主从分布式模型预测控制的欠驱动多无人船编队跟踪方法 | |
| CN104793625B (zh) | 一种自主水下航行器动态抗饱和俯仰角控制方法 | |
| Zhang et al. | NNFFC-adaptive output feedback trajectory tracking control for a surface ship at high speed | |
| Li et al. | Robust adaptive motion control for underwater remotely operated vehicles with velocity constraints | |
| CN108803321A (zh) | 基于深度强化学习的自主水下航行器轨迹跟踪控制方法 | |
| CN112965371B (zh) | 基于固定时间观测器的水面无人艇轨迹快速跟踪控制方法 | |
| CN102385316A (zh) | 一种基于神经网络反步法的欠驱动自主水下航行器变深控制方法 | |
| CN109189103B (zh) | 一种具有暂态性能约束的欠驱动auv轨迹跟踪控制方法 | |
| CN107065569B (zh) | 基于rbf神经网络补偿的船舶动力定位滑模控制***及方法 | |
| JP2007317165A (ja) | 自律移動ロボットの動作計画方法、自律移動ロボットの動作計画方法を利用した自律移動ロボットの制御方法、自律移動ロボットの動作計画装置、自律移動ロボットの動作計画プログラム及びその記録媒体、自律移動ロボットの制御プログラム | |
| CN104181815B (zh) | 一种基于环境估计的船舶运动补偿控制方法 | |
| CN114442640B (zh) | 一种水面无人艇轨迹跟踪控制方法 | |
| WO2016003292A1 (en) | Method and system for dynamic positioning of instrumented cable towed in water | |
| CN110083057A (zh) | 基于水翼运动姿态的pid控制方法 | |
| CN107315348B (zh) | 一种基于惩罚式小波网络的无人潜航器位姿控制方法 | |
| CN109885096A (zh) | 一种基于Lyapunov-MPC技术的自主水下机器人路径跟踪闭环控制方法 | |
| Ngongi et al. | Linear fuzzy controller design for dynamic positioning system of surface ships | |
| CN114967714A (zh) | 一种自主式水下机器人抗扰运动控制方法及*** | |
| CN113110512B (zh) | 一种减弱未知干扰与抖振影响的可底栖式auv自适应轨迹跟踪控制方法 | |
| de Cerqueira Gava et al. | AUV cruising auto pilot for a long straight confined underwater tunnel |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |