CN114880874B - 一种水面无人船参数自适应鲁棒估计方法与系统 - Google Patents
一种水面无人船参数自适应鲁棒估计方法与系统 Download PDFInfo
- Publication number
- CN114880874B CN114880874B CN202210633604.4A CN202210633604A CN114880874B CN 114880874 B CN114880874 B CN 114880874B CN 202210633604 A CN202210633604 A CN 202210633604A CN 114880874 B CN114880874 B CN 114880874B
- Authority
- CN
- China
- Prior art keywords
- unmanned ship
- water surface
- vector
- covariance matrix
- estimation
- 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 114
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 title claims abstract description 98
- 239000013598 vector Substances 0.000 claims abstract description 142
- 239000011159 matrix material Substances 0.000 claims abstract description 107
- 238000005259 measurement Methods 0.000 claims abstract description 52
- 230000008569 process Effects 0.000 claims abstract description 48
- 230000003044 adaptive effect Effects 0.000 claims abstract description 26
- 238000001914 filtration Methods 0.000 claims abstract description 22
- 230000003416 augmentation Effects 0.000 claims abstract description 19
- 230000003190 augmentative effect Effects 0.000 claims description 33
- 238000005457 optimization Methods 0.000 claims description 17
- 230000006870 function Effects 0.000 claims description 8
- 238000004590 computer program Methods 0.000 claims description 7
- 238000010276 construction Methods 0.000 claims description 7
- 238000013016 damping Methods 0.000 claims description 7
- 238000009826 distribution Methods 0.000 claims description 6
- 238000013507 mapping Methods 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 3
- 238000004088 simulation Methods 0.000 abstract description 7
- 230000000694 effects Effects 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000003860 storage Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000005312 nonlinear dynamic Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012797 qualification Methods 0.000 description 1
- 230000001172 regenerating effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- H—ELECTRICITY
- H03—ELECTRONIC CIRCUITRY
- H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
- H03H21/00—Adaptive networks
- H03H21/0012—Digital adaptive filters
- H03H21/0025—Particular filtering methods
- H03H21/0029—Particular filtering methods based on statistics
- H03H21/003—KALMAN filters
-
- 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
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Computer Hardware Design (AREA)
- Evolutionary Computation (AREA)
- Geometry (AREA)
- General Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Probability & Statistics with Applications (AREA)
- Feedback Control In General (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种水面无人船参数自适应鲁棒估计方法与系统，属于自动控制领域。首先构建考虑外部扰动的水面无人船增广状态估计问题，增广状态向量包括水面无人船状态向量、参数向量以及未知输入向量；然后依托系统实时输入向量和测量向量，采用设计的自适应鲁棒无迹卡尔曼滤波方法获得水面无人船模型参数；具体地，考虑到水面无人船系统状态可能受到离群值噪声的干扰，基于极大交叉熵准则与最小均方误差准则设计鲁棒卡尔曼滤波方法；考虑到水面无人船系统过程噪声统计特性未知，且系统状态维数较高的情况，引入具有过程噪声协方差矩阵约束的自适应律精准估计过程噪声协方差矩阵。仿真结果表明，本发明提高了水面无人船模型参数的估计精度。
Description
技术领域
本发明涉及一种水面无人船参数自适应鲁棒估计方法与系统，通过鲁棒最优准则设计鲁棒卡尔曼滤波以减弱离群值噪声对辨识结果的影响，通过自适应方法估计过程噪声的统计特性，从而提高水面无人船未知参数的估计精度与鲁棒性，属于自动控制领域。
背景技术
基于控制系统的输入向量和测量向量，估计出控制系统模型参数的具体数值的过程称为参数估计；自适应估计是指在估计的过程中自适应调整算法参数，而在卡尔曼滤波中，估计噪声统计特性，特别是噪声的协方差矩阵，是在自适应律设计时所考虑的一类主要问题；鲁棒估计则是指一类具有良好鲁棒性的估计算法。参数估计是自动控制领域的一项重要技术。在工程实践中，基于模型的控制器的设计及其参数的选取需要已知被控对象准确的系统模型和模型参数，提供精准的模型参数将有助于保障控制器发挥良好的控制效果。随着无人系统的发展，目前被控对象的运动学和动力学模型呈现出更高的复杂性，因此对参数估计的准确性提出了更高的要求。
随着无人系统的发展，水面无人船目前已经在军用和民用领域得到了广泛的关注，常被应用于环境监测、海洋搜救、资源勘探等场景。近年来，水面无人船的参数估计问题也得到了大量研究。例如，基于二次规划的水面无人船参数估计[Wang X,Zhao J,GengT.Nonlinear modeling and identification of vessel based on constrainedquadratic programming method[C].39th Chinese Control Conference(CCC).Shenyang,China；IEEE.2020:1028-33.]，该方法考虑了水面无人船模型参数估计问题中可能存在的约束条件，取得了较好的参数估计效果，但是没有考虑过程噪声的影响，仅仅针对测量噪声建立了最优化问题；例如，基于扩展卡尔曼滤波的水面无人船参数估计[PereraL P,Oliveira P,Guedes Soares C.System identification of nonlinear vesselsteering[J].Journal of Offshore Mechanics and Arctic Engineering,2015,137(3).]，该方法考虑了过程噪声和测量噪声，最优化问题的建立更加完备，但是扩展卡尔曼滤波应对具有强非线性动态的水面无人船模型时难以具有良好的精度，此外该方法也没有考虑离群值噪声和未知噪声特性的影响。
发明内容
发明目的：为解决现有技术存在的不足，本发明提供了一种水面无人船参数自适应鲁棒估计方法与系统，用于提高水面无人船模型参数估计的鲁棒性，并实现高维过程噪声协方差矩阵的高精度估计。
技术方案：为实现上述发明目的，本发明提供的一种水面无人船参数自适应鲁棒估计方法，包括如下步骤：
步骤1，构建考虑外部扰动的水面无人船增广状态估计问题，其中增广状态向量包括水面无人船状态向量、水面无人船模型参数向量以及代表水面无人船系统建模误差以及外界扰动的未知输入向量；
步骤2，依托水面无人船系统实时输入向量和测量向量，采用具有过程噪声协方差矩阵约束，基于极大交叉熵准则和最小均方误差准则结合的自适应鲁棒无迹卡尔曼滤波方法获得水面无人船模型参数；其中输入向量包括船载控制器的控制信号，测量向量包括GPS和IMU测量获得的数据；
过程噪声协方差矩阵估计的方法为：首先采用基于新息的自适应方法估计出噪声协方差矩阵然后根据已知结构引入协方差矩阵约束，将待估计的过程噪声协方差矩阵分为无结构约束的对角块和有结构约束的对角块；再采用KL散度构造并求解如下的最优化问题：
其中，k表示时刻，表示无结构约束的对角块，/>表示有结构约束的对角块的系数，/>是已知的结构，j1＝1,…,n1，j2＝1,…,n2，n1和n2分别表示无结构约束和有结构约束的对角块的个数；/>和/>分别表示协方差矩阵为/>和/>且服从零均值高斯分布的随机变量x的概率密度函数。
作为优选，基于新息的自适应方法估计噪声协方差矩阵表示为：
其中，k0表示计算协方差矩阵时采用的样本的初始时刻，N表示采用的样本的数量，分别是第i时刻的增广状态向量预测值和估计值。
作为优选，待估计的过程噪声协方差矩阵具有如下的形式：
如果实际问题中存在则将该对角块移除以保证/>正定，并在估计结果中直接给出/>
作为优选，采用KL散度构造的最优化问题的最优解为：
其中，是/>的维数，/>是/>在/>中对应的矩阵块，/>是/>在/>中对应的矩阵块。
作为优选，步骤1中构建的增广估计问题采用的系统方程为：
yk＝hξ(ξk,uk)+Edk+vk；
其中，xk和yk分别是第k时刻增广状态向量和测量向量，ξk-1和ξk分别是第k-1、k时刻水面无人船状态向量，uk-1和uk分别是第k-1、k时刻输入向量，dk-1和dk分别是第k-1、k时刻未知输入向量，ak-1是第k-1时刻参数向量，fξ表示不考虑噪声与未知输入向量时，k-1时刻状态向量ξk-1和输入向量uk-1到k时刻状态向量ξk的非线性映射，hξ表示不考虑噪声与未知输入向量时，k时刻状态向量ξk和输入向量uk到测量向量yk的非线性映射，wk-1是第k-1时刻零均值过程噪声，vk是第k时刻零均值测量噪声，在系统采样周期为Ts时，矩阵G和E具有如下的形式：
I2和I3分别是2×2和3×3的单位矩阵。
作为优选，步骤2中，具体包括：
步骤21，初始化自适应鲁棒无迹卡尔曼滤波，包括增广状态向量估计值及其误差的协方差矩阵/>
步骤22，获取水面无人船系统输入向量uk和测量向量yk；
步骤23，预测第k时刻的增广状态向量和增广状态向量预测误差的协方差矩阵/>
步骤24，预测第k时刻的测量向量测量向量预测误差的协方差矩阵/>和互协方差矩阵/>
步骤25，构造鲁棒状态估计所采用的最优化问题，包括：
采用无迹变换的结果，将水面无人船系统测量方程做如下的线性化：
其中，rk为线性化后的测量方程的噪声，其统计性质为：
测量方程线性化后的系统写作如下的形式：
其中I是单位矩阵，且向量ξk具有如下性质：
其中，和Tk分别是/>Ψk和/>的Cholesky因子；
进一步有如下等式：
该式改写为：
Yk＝Akxk+ek；
其中：
将极大交叉熵准则与最小均方误差准则结合，前者处理过程噪声，后者处理测量噪声，建立并求解如下最优化问题得到最优估计
其中，w是权重系数，n是增广状态向量维数，m是yk的维数，是ek的第i个元素，σ是带宽，Gσ(·)是高斯核函数；
步骤26，根据鲁棒无迹卡尔曼滤波迭代形式得到增广状态向量最优估计；
步骤27，采用具有协方差矩阵约束的自适应律估计过程噪声协方差矩阵；
步骤28，判断是否终止参数估计，如不终止返回步骤22。
作为优选，所述水面无人船状态向量中包括世界坐标系下水面无人船的质心位置x,y、偏航角ψ、体坐标系下水面无人船的质心速度u,v和偏航角速度r，所述水面无人船模型参数向量包括惯性相关参数、阻尼相关参数和驱动力相关参数。
基于相同发明构思，本发明提供的一种水面无人船参数自适应鲁棒估计系统，包括：
问题构建模块，用于构建考虑外部扰动的水面无人船增广状态估计问题，其中增广状态向量包括水面无人船状态向量、水面无人船模型参数向量以及代表水面无人船系统建模误差以及外界扰动的未知输入向量；
以及，自适应鲁棒估计模块，用于依托水面无人船系统实时输入向量和测量向量，采用具有过程噪声协方差矩阵约束，基于极大交叉熵准则和最小均方误差准则结合的自适应鲁棒无迹卡尔曼滤波方法获得水面无人船模型参数；其中输入向量包括船载控制器的控制信号，测量向量包括GPS和IMU测量获得的数据；过程噪声协方差矩阵估计的方法为：首先采用基于新息的自适应方法估计出噪声协方差矩阵然后根据已知结构引入协方差矩阵约束，将待估计的过程噪声协方差矩阵/>分为无结构约束的对角块和有结构约束的对角块；再采用KL散度构造并求解如下的最优化问题：
基于相同发明构思，本发明提供的一种计算机系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述计算机程序被加载至处理器时实现所述的水面无人船参数自适应鲁棒估计方法的步骤。
有益效果：本发明提出的水面无人船参数自适应鲁棒估计方法，首先将水面无人船模型参数估计问题转化为增广状态估计问题，然后依托系统实时输入向量和测量向量，采用改进的自适应鲁棒卡尔曼滤波方法估计水面无人船模型参数。采用本发明改进的结合极大交叉熵准则与最小均方误差准则的鲁棒估计方法，能够提高对环境离群值噪声的鲁棒性；采用本发明提出的具有协方差矩阵约束的自适应律，能够实现高维过程噪声协方差矩阵的高精度估计；仿真结果表明，将本发明能够实现水面无人船参数的精准估计。
附图说明
图1为本发明实施例方法的总体流程图。
图2为本发明实施例中自适应鲁棒无迹卡尔曼滤波方法流程图。
图3为本发明实施例估计出的双推进器差速水面无人船参数的误差曲线图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
由图1所示，本发明实施例公开的水面无人船参数自适应鲁棒估计方法，首先，构建考虑外部扰动的水面无人船增广状态估计问题，具体根据水面无人船运动学和动力学方程建立系统状态方程，根据实时差分定位和惯性测量单元建立系统测量方程，并引入系统未知输入，将系统参数和未知输入增广为状态，从而将水面无人船参数估计问题转化为增广状态估计问题；然后，依托水面无人船系统实时输入向量和测量向量，采用具有过程噪声协方差矩阵约束，基于极大交叉熵准则和最小均方误差准则结合的自适应鲁棒无迹卡尔曼滤波方法获得水面无人船模型参数。具体为：考虑到水面无人船系统状态可能受到离群值噪声的干扰，基于极大交叉熵准则与最小均方误差准则设计鲁棒卡尔曼滤波方法；考虑到水面无人船系统过程噪声统计特性未知，且系统状态维数较高的情况，引入具有过程噪声协方差矩阵约束的自适应律精准估计过程噪声协方差矩阵。
下面以双推进器差速水面无人船模型参数估计为例，说明本发明实施例的详细实施过程，包括以下具体步骤：
S1，考虑外部扰动的水面无人船增广状态估计问题构建；
在假设水流流速为零时，水面无人船模型可以分别用运动学和动力学方程表示：
其中，η＝[x,y,ψ]T，x,y表示世界坐标系下水面无人船的质心位置，ψ表示世界坐标系下水面无人船的偏航角，υ＝[u,v,r]T，u,v表示体坐标系下水面无人船的质心速度，r表示体坐标下水面无人船的偏航角速度，R表示旋转矩阵，M是惯性矩阵，C是科里奥利力矩阵，D是阻尼矩阵，τ是水面无人船所受的力，在不考虑环境扰动时，τ由驱动电机产生的前向推力τu和转矩τr组成，即τ＝[τu,0,τr]T：
旋转矩阵R、惯性矩阵M、科里奥利力矩阵C和阻尼矩阵D分别表示为：
其中，m是水面无人船的质量，Iz是水面无人船的转动惯量，xg是质心坐标，和/>是水动力导数；
科里奥利力矩阵C和阻尼矩阵D中c31、c32、d11、d22、d23、d32和d33分别为：
d11＝-Xu-X|u|u|u|；d22＝-Yv-Y|v|v|v|；d23＝-Yr-Y|r|r|r|；
d32＝-Nv-N|v|v|v|；d33＝-Nr-N|r|r|r|；
其中，Xu、X|u|u、Yv、Y|v|v、Yr、Y|r|r、Nv、N|v|v、Nr和N|r|r是不同类型的水动力导数；
对于双推进器差速水面无人船系统，其驱动力由两个(组)电机产生。在两个(组)电机转速与船载控制器的控制信号τ1和τ2成正比的假设下，τu和τr关于控制信号τ1和τ2有如下的关系式：
τu＝k1(|τ1|τ1+|τ2|τ2)-k2(|τ1|+|τ2|)u；
τr＝k1l(|τ1|τ1-|τ2|τ2)-k2l(|τ1|-|τ2|)u；
其中，k1和k2是系数，l是两个(组)电机分别产生的转矩的力臂。结合上述式子，对水面无人船运动学和动力学方程适当整理，可以得到双推进器差速水面无人船系统的状态方程：
其中，a1,…,a4是惯性相关参数，a5,…,a14是阻尼相关参数，a15,…,a18是驱动力相关参数。设向量a＝[a1,...,a18]T表示待辨识的参数向量。选取双推进器差速水面无人船状态向量为ξ＝[x,y,ψ,u,v,r]T，输入向量为u＝[τ1,τ2]T，则上式可以记作如下的离散化的状态方程：
ξk＝fξ(ξk-1,uk-1)；
假设水面无人船已安装全球定位系统(GPS)和惯性测量单元(IMU)。前者用于测量水面无人船的位置x,y和速度u,v，后者用于测量水面无人船偏航角ψ、偏航角速度r和加速度则系统的测量方程具有如下的形式：
另外，考虑水面无人船系统的建模误差以及外界扰动等未知输入对水面无人船动力学特性的影响，引入未知输入向量为将水面无人船参数估计问题转化为增广状态估计问题，选取如下的增广状态向量：
则增广状态估计问题采用的系统方程可以写为：
yk＝h(xk,uk)+vk＝hξ(ξk,uk)+Edk+vk；
其中，f表示不考虑噪声时，k-1时刻增广状态向量xk-1和输入向量uk-1到k时刻增广状态向量xk的非线性映射，h表示不考虑噪声时，k时刻增广状态向量xk和输入向量uk到测量向量yk的非线性映射，wk-1和vk分别是第k-1时刻和第k时刻，协方差矩阵为Qk-1和Rk的零均值过程噪声和零均值测量噪声，在系统采样周期为Ts时，矩阵G和E具有如下的形式：
S2，采用具有过程噪声协方差矩阵约束的自适应鲁棒无迹卡尔曼滤波获得水面无人船模型参数；
S21，初始化自适应鲁棒无迹卡尔曼滤波；
选定初始的增广状态向量估计值取值应当接近水面无人船的实际初始增广状态向量、参数和未知输入，同时给定初始的增广状态向量估计值误差的协方差矩阵/>
S22，获取水面无人船系统输入向量和测量向量；
水面无人船系统的输入向量uk可以直接从控制器中获得，而测量向量yk通过GPS和IMU的测量结果获得；
S23，预测增广状态向量和增广状态向量预测误差的协方差矩阵；
首先生成一组第k-1时刻的σ样本点
其中，是第k-1时刻估计出的增广状态向量，/>是该增广状态向量估计值的误差的协方差矩阵，n是增广状态向量维数，c是常数，/>表示/>的Cholesky因子的第i列组成的向量。然后通过水面无人船系统状态方程映射每个σ点的预测值/>
根据σ点的预测值预测第k时刻的增广状态向量/>
其中，W(i)为权重；
然后，计算增广状态向量预测值的误差的协方差矩阵
S24，预测测量向量、测量向量预测误差的协方差矩阵和互协方差矩阵；
重新生成σ点
计算每个σ点预测的测量向量/>
计算第k时刻的预测的测量向量
计算测量向量预测误差的协方差矩阵
计算和/>间的互协方差矩阵/>
S25，构造鲁棒状态估计所采用的最优化问题；
采用无迹变换的结果，可以将水面无人船系统测量方程做如下的线性化：
其中，rk为线性化后的测量方程的噪声，其统计性质为：
测量方程线性化后的系统可以写作如下的形式：
其中：
可以验证，ξk的期望E[ξk]是零均值的，其协方差矩阵可以变换为：
其中，和Tk分别是/>Ψk和/>的Cholesky因子；
进一步有如下等式：
该式改写为：
Yk＝Akxk+ek；
其中：
且可以验证向量ek均值为0，其协方差矩阵为单位阵。引入具有良好鲁棒性的极大交叉熵准则作为最优准则，与传统的卡尔曼滤波采用的最小均方误差准则结合，前者处理可能存在离群值的过程噪声，后者处理测量噪声，即前者针对状态方程建模，后者针对测量方程建模。建立并求解如下的最优化问题即可得到兼具鲁棒性与准确性的最优估计
其中，w是人为设定的权重系数，用于平衡两种最优准则，m是测量向量yk的维数。是ek的第i个元素，σ是带宽，Gσ(·)是高斯核函数：
S26，根据鲁棒无迹卡尔曼滤波迭代形式得到增广状态最优估计；
S25中的最优解满足如下的不动点方程，需要迭代求解：
其中，Im为m维单位矩阵，且：
经过矩阵求逆引理和对最优解的适当整理，可以得到如下的鲁棒无迹卡尔曼滤波器迭代式：
/>
其中，上标(t)表示第t次迭代，当如下条件满足时，令/>以及/>
其中，ε为阈值。之后，状态估计误差的协方差矩阵为：
S27，采用具有协方差矩阵约束的自适应律估计过程噪声协方差矩阵；
在噪声协方差矩阵缓慢变化的前提下，且为了保证噪声协方差矩阵的正定性，可以采用基于新息的自适应方法进行噪声协方差矩阵的初步估计：
其中，是基于新息的自适应方法估计出的协方差矩阵，k0表示计算协方差矩阵时采用的样本的初始时刻，N表示采用的样本的数量，/>是第i时刻的增广状态向量预测值，/>是第i时刻的增广状态向量估计值。水面无人船增广系统是一个高维系统，含有27维状态量。采用上式直接计算高维过程噪声协方差矩阵含有较大的误差，因此在已知过程噪声协方差矩阵结构的前提下，根据已知结构引入协方差矩阵约束可以减少协方差矩阵估计误差。假设待估计的过程噪声协方差矩阵具有如下的形式：
其中，j1＝1,…,n1代表无结构约束的对角块，需要求解，/>的个数为n1，表示有结构约束的对角块，即/>j2＝1,…,n2,/>是已知的结构，/>是需要求解的系数，/>的个数为n2。另外，如果实际问题中存在/>就将该对角块移除以保证/>正定，并在估计结果中直接给出/>基于使无约束的方法估计出的/>和考虑约束的/>构成的零均值高斯分布最相近的出发点，采用KL散度构造如下的最优化问题：
下面给出基于KL散度的协方差矩阵约束所采用的最优化问题：
其中，和/>分别表示协方差矩阵为/>和/>且服从零均值高斯分布的随机变量x的概率密度函数：
求解上述最优化问题可以得到最优解为：
其中，是/>的维数，/>是/>在/>中对应的矩阵块，/>是/>在/>中对应的矩阵块；
S28，判断是否终止参数辨识，如不终止返回S22。
本实施例中，采用MATLAB 2021b作为仿真软件，将本发明的水面无人船参数自适应鲁棒估计方法，与传统的无迹卡尔曼滤波方法进行比较。
仿真采用的双推进器差速水面无人船模型参数设置如表1所示：
表1：双推进器差速水面无人船模型参数设置
仿真采用的测量噪声为零均值高斯噪声，协方差矩阵为：
Rk＝diag(1,1,0.7615,4,4,76.15,100,100)×10-4；
水面无人船状态向量所受到的噪声由零均值高斯噪声和离群值组成，零均值高斯噪声的协方差矩阵为：
离群值每10秒产生一次，离群值也服从零均值高斯分布，其协方差矩阵为自适应鲁棒无迹卡尔曼和无迹卡尔曼滤波初始过程噪声的协方差为：
Q0＝diag(1,1,0.7615,25,25,3.046,01×18,10,10,10)×10-4；
系统输入设置如下：
系统所受的未知输入设置如下：
其中，rand表示0到1之间的均匀分布随机数。
待估计的过程噪声协方差矩阵有如下的已知结构：
其中，是2维单位矩阵，即/>是1维单位矩阵，即/> 是18×18维零矩阵，即/>
自适应鲁棒无迹卡尔曼滤波方法的参数设置为：σ＝12，ε＝10-6，w＝3.5×10-4，N＝200。采样周期Δt＝0.1。两种无迹卡尔曼滤波的增广状态向量初始估计相同，状态向量和未知输入初始估计设置为0，参数a1,…,a4,a15,…,a18设置为1，参数a5,…,a14设置为-1。仿真时长500秒，共进行20次仿真；
图3表示参数误差Δak的迭代曲线，Δak定义为：
其中是第t次仿真时，第i个参数在第k时刻的估计误差。
图3中标注为UKF的实线表示传统的无迹卡尔曼滤波估计出的参数的误差，标注为ARUKF的虚线为本发明提出的自适应鲁棒无迹卡尔曼滤波估计出的参数的误差。可以看到，本发明的水面无人船参数自适应鲁棒估计方法能够实现更好的估计效果。
基于相同发明构思，本发明实施例公开的一种水面无人船参数自适应鲁棒估计系统，包括：问题构建模块，用于构建考虑外部扰动的水面无人船增广状态估计问题，其中增广状态向量包括水面无人船状态向量、水面无人船模型参数向量以及代表水面无人船系统建模误差以及外界扰动的未知输入向量；以及，自适应鲁棒估计模块，用于依托水面无人船系统实时输入向量和测量向量，采用具有过程噪声协方差矩阵约束，基于极大交叉熵准则和最小均方误差准则结合的自适应鲁棒无迹卡尔曼滤波方法获得水面无人船模型参数。
上述描述的各模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。所述模块的划分仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个模块可以结合或者可以集成到另一个系统。
基于相同发明构思，本发明实施例公开的一种计算机系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述计算机程序被加载至处理器时实现所述的水面无人船参数自适应鲁棒估计方法的步骤。
本领域技术人员可以理解的是，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机系统(可以是个人计算机，服务器，或者网络设备等)执行本发明实施例所述方法的全部或部分步骤。存储介质包括：U盘、移动硬盘、只读存储器ROM、随机存取存储器RAM、磁碟或者光盘等各种可以存储计算机程序的介质。
以上仅为本发明创造的较佳实施例而已，并不用以限制本发明创造，凡在本发明创造的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明创造的保护范围之内。
Claims (10)
1.一种水面无人船参数自适应鲁棒估计方法，其特征在于，包括如下步骤：
步骤1，构建考虑外部扰动的水面无人船增广状态估计问题，其中增广状态向量包括水面无人船状态向量、水面无人船模型参数向量以及代表水面无人船系统建模误差以及外界扰动共同来确定的未知输入向量；
步骤2，依托水面无人船系统实时输入向量和测量向量，采用具有过程噪声协方差矩阵约束，基于极大交叉熵准则和最小均方误差准则结合的自适应鲁棒无迹卡尔曼滤波方法获得水面无人船模型参数；其中输入向量包括船载控制器的控制信号，测量向量包括GPS和IMU测量获得的数据；
过程噪声协方差矩阵估计的方法为：首先采用基于新息的自适应方法估计出噪声协方差矩阵然后根据已知结构引入协方差矩阵约束，将待估计的过程噪声协方差矩阵/>分为无结构约束的对角块和有结构约束的对角块；再采用KL散度构造并求解如下的最优化问题：
其中，k表示时刻，表示无结构约束的对角块，/>表示有结构约束的对角块/>的系数，/> 是已知的结构，j1＝1,…,n1，j2＝1,…,n2，n1和n2分别表示无结构约束和有结构约束的对角块的个数；/>和/>分别表示协方差矩阵为和/>且服从零均值高斯分布的随机变量x的概率密度函数。
2.根据权利要求1所述的水面无人船参数自适应鲁棒估计方法，其特征在于，基于新息的自适应方法估计噪声协方差矩阵表示为：
其中，k0表示计算协方差矩阵时采用的样本的初始时刻，N表示采用的样本的数量，分别是第i时刻的增广状态向量预测值和估计值。
3.根据权利要求1所述的水面无人船参数自适应鲁棒估计方法，其特征在于，待估计的过程噪声协方差矩阵具有如下的形式：
如果实际问题中存在则将该对角块移除以保证/>正定，并在估计结果中直接给出/>
4.根据权利要求1所述的水面无人船参数自适应鲁棒估计方法，其特征在于，采用KL散度构造的最优化问题的最优解为：
其中，mj2是的维数，/>是/>在/>中对应的矩阵块，/>是/>在/>中对应的矩阵块。
5.根据权利要求1所述的水面无人船参数自适应鲁棒估计方法，其特征在于，步骤1中构建的增广估计问题采用的系统方程为：
yk＝hξ(ξk,uk)+Edk+vk；
其中，xk和yk分别是第k时刻增广状态向量和测量向量，ξk-1和ξk分别是第k-1、k时刻水面无人船状态向量，uk-1和uk分别是第k-1、k时刻输入向量，dk-1和dk分别是第k-1、k时刻未知输入向量，ak-1是第k-1时刻参数向量，fξ表示不考虑噪声与未知输入向量时，k-1时刻状态向量ξk-1和输入向量uk-1到k时刻状态向量ξk的非线性映射，hξ表示不考虑噪声与未知输入向量时，k时刻状态向量ξk和输入向量uk到测量向量yk的非线性映射，wk-1是第k-1时刻零均值过程噪声，vk是第k时刻零均值测量噪声，在系统采样周期为Ts时，矩阵G和E具有如下的形式：
I2和I3分别是2×2和3×3的单位矩阵。
6.根据权利要求5所述的水面无人船参数自适应鲁棒估计方法，其特征在于，步骤2中，具体包括：
步骤21，初始化自适应鲁棒无迹卡尔曼滤波，包括增广状态向量估计值及其误差的协方差矩阵/>
步骤22，获取水面无人船系统输入向量uk和测量向量yk；
步骤23，预测第k时刻的增广状态向量和增广状态向量预测误差的协方差矩阵
步骤24，预测第k时刻的测量向量测量向量预测误差的协方差矩阵/>和互协方差矩阵/>
步骤25，构造鲁棒状态估计所采用的最优化问题，包括：
采用无迹变换的结果，将水面无人船系统测量方程做如下的线性化：
其中，rk为线性化后的测量方程的噪声，其统计性质为：
测量方程线性化后的系统写作如下的形式：
其中I是单位矩阵，且向量ξk具有如下性质：
其中，和Tk分别是/>Ψk和/>的Cholesky因子；
进一步有如下等式：
该式改写为：
Yk＝Akxk+ek；
其中：
将极大交叉熵准则与最小均方误差准则结合，前者处理过程噪声，后者处理测量噪声，建立并求解如下最优化问题得到最优估计
其中，w是权重系数，n是增广状态向量维数，m是yk的维数，是ek的第i个元素，σ是带宽，Gσ(·)是高斯核函数；
步骤26，根据鲁棒无迹卡尔曼滤波迭代形式得到增广状态向量最优估计；
步骤27，采用具有协方差矩阵约束的自适应律估计过程噪声协方差矩阵；
步骤28，判断是否终止参数估计，如不终止返回步骤22。
7.根据权利要求1所述的水面无人船参数自适应鲁棒估计方法，其特征在于，所述水面无人船状态向量中包括世界坐标系下水面无人船的质心位置x,y、偏航角ψ、体坐标系下水面无人船的质心速度u,v和偏航角速度r，所述水面无人船模型参数向量包括惯性相关参数、阻尼相关参数和驱动力相关参数。
8.根据权利要求7所述的水面无人船参数自适应鲁棒估计方法，其特征在于，对于双推进器差速水面无人船系统，结合运动学和动力学方程，得到其状态方程：
其中，a1,…,a4是惯性相关参数，a5,…,a14是阻尼相关参数，a15,…,a18是驱动力相关参数，τ1和τ2是用于控制两个/组电机转速的控制信号。
9.一种水面无人船参数自适应鲁棒估计系统，其特征在于，包括：
问题构建模块，用于构建考虑外部扰动的水面无人船增广状态估计问题，其中增广状态向量包括水面无人船状态向量、水面无人船模型参数向量以及代表水面无人船系统建模误差以及外界扰动共同来确定的未知输入向量；
以及，自适应鲁棒估计模块，用于依托水面无人船系统实时输入向量和测量向量，采用具有过程噪声协方差矩阵约束，基于极大交叉熵准则和最小均方误差准则结合的自适应鲁棒无迹卡尔曼滤波方法获得水面无人船模型参数；其中输入向量包括船载控制器的控制信号，测量向量包括GPS和IMU测量获得的数据；过程噪声协方差矩阵估计的方法为：首先采用基于新息的自适应方法估计出噪声协方差矩阵然后根据已知结构引入协方差矩阵约束，将待估计的过程噪声协方差矩阵/>分为无结构约束的对角块和有结构约束的对角块；再采用KL散度构造并求解如下的最优化问题：
其中，k表示时刻，表示无结构约束的对角块，/>表示有结构约束的对角块/>的系数，/> 是已知的结构，j1＝1,…,n1，j2＝1,…,n2，n1和n2分别表示无结构约束和有结构约束的对角块的个数；/>和/>分别表示协方差矩阵为和/>且服从零均值高斯分布的随机变量x的概率密度函数。
10.一种计算机系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述计算机程序被加载至处理器时实现根据权利要求1-8任一项所述的水面无人船参数自适应鲁棒估计方法的步骤。
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210633604.4A CN114880874B (zh) | 2022-06-07 | 2022-06-07 | 一种水面无人船参数自适应鲁棒估计方法与系统 |
| PCT/CN2022/100107 WO2023236247A1 (zh) | 2022-06-07 | 2022-06-21 | 一种水面无人船参数自适应鲁棒估计方法与系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210633604.4A CN114880874B (zh) | 2022-06-07 | 2022-06-07 | 一种水面无人船参数自适应鲁棒估计方法与系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114880874A CN114880874A (zh) | 2022-08-09 |
| CN114880874B true CN114880874B (zh) | 2024-03-12 |
Family
ID=82678820
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210633604.4A Active CN114880874B (zh) | 2022-06-07 | 2022-06-07 | 一种水面无人船参数自适应鲁棒估计方法与系统 |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN114880874B (zh) |
| WO (1) | WO2023236247A1 (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115859039B (zh) * | 2023-03-01 | 2023-05-23 | 南京信息工程大学 | 一种车辆状态估计方法 |
| CN117669396A (zh) * | 2024-02-02 | 2024-03-08 | 陕西欧卡电子智能科技有限公司 | 无人船运动检测方法、装置、无人船及存储介质 |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107179693A (zh) * | 2017-06-27 | 2017-09-19 | 哈尔滨工程大学 | 基于Huber估计的鲁棒自适应滤波和状态估计方法 |
| CN108520107A (zh) * | 2018-03-19 | 2018-09-11 | 山西大学 | 基于最大似然准则鲁棒卡尔曼滤波的系统状态估计方法 |
| CN109345875A (zh) * | 2018-09-28 | 2019-02-15 | 哈尔滨工程大学 | 一种提高船舶自动识别系统测量精度的估计方法 |
| WO2020050084A1 (ja) * | 2018-09-07 | 2020-03-12 | 国立研究開発法人宇宙航空研究開発機構 | 誘導制御プログラムを格納した記憶媒体 |
| CN112432644A (zh) * | 2020-11-11 | 2021-03-02 | 杭州电子科技大学 | 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法 |
| CN112511056A (zh) * | 2020-11-09 | 2021-03-16 | 广东电网有限责任公司广州供电局 | 一种基于相量测量的鲁棒发电机动态状态估计方法 |
| CN112525218A (zh) * | 2020-11-23 | 2021-03-19 | 哈尔滨工程大学 | 一种ins/dvl组合导航系统鲁棒智能协同校准方法 |
| CN112987758A (zh) * | 2021-04-29 | 2021-06-18 | 电子科技大学 | 一种多水面航行器协同跟踪编队控制方法 |
| CN113670314A (zh) * | 2021-08-20 | 2021-11-19 | 西南科技大学 | 基于pi自适应两级卡尔曼滤波的无人机姿态估计方法 |
| CN113792411A (zh) * | 2021-08-13 | 2021-12-14 | 中国人民解放军军事科学院国防科技创新研究院 | 一种基于中心误差熵准则无迹卡尔曼滤波的航天器姿态确定方法 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105785999B (zh) * | 2016-04-27 | 2018-11-02 | 大连海事大学 | 无人艇航向运动控制方法 |
| CN110487275A (zh) * | 2019-08-01 | 2019-11-22 | 西安工业大学 | 一种基于最小上限滤波的gps/ins组合导航定位方法 |
| CN110908395A (zh) * | 2019-11-26 | 2020-03-24 | 西北工业大学 | 改进的无人机航迹实时规划方法 |
-
2022
- 2022-06-07 CN CN202210633604.4A patent/CN114880874B/zh active Active
- 2022-06-21 WO PCT/CN2022/100107 patent/WO2023236247A1/zh unknown
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107179693A (zh) * | 2017-06-27 | 2017-09-19 | 哈尔滨工程大学 | 基于Huber估计的鲁棒自适应滤波和状态估计方法 |
| CN108520107A (zh) * | 2018-03-19 | 2018-09-11 | 山西大学 | 基于最大似然准则鲁棒卡尔曼滤波的系统状态估计方法 |
| WO2020050084A1 (ja) * | 2018-09-07 | 2020-03-12 | 国立研究開発法人宇宙航空研究開発機構 | 誘導制御プログラムを格納した記憶媒体 |
| CN109345875A (zh) * | 2018-09-28 | 2019-02-15 | 哈尔滨工程大学 | 一种提高船舶自动识别系统测量精度的估计方法 |
| CN112511056A (zh) * | 2020-11-09 | 2021-03-16 | 广东电网有限责任公司广州供电局 | 一种基于相量测量的鲁棒发电机动态状态估计方法 |
| CN112432644A (zh) * | 2020-11-11 | 2021-03-02 | 杭州电子科技大学 | 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法 |
| CN112525218A (zh) * | 2020-11-23 | 2021-03-19 | 哈尔滨工程大学 | 一种ins/dvl组合导航系统鲁棒智能协同校准方法 |
| CN112987758A (zh) * | 2021-04-29 | 2021-06-18 | 电子科技大学 | 一种多水面航行器协同跟踪编队控制方法 |
| CN113792411A (zh) * | 2021-08-13 | 2021-12-14 | 中国人民解放军军事科学院国防科技创新研究院 | 一种基于中心误差熵准则无迹卡尔曼滤波的航天器姿态确定方法 |
| CN113670314A (zh) * | 2021-08-20 | 2021-11-19 | 西南科技大学 | 基于pi自适应两级卡尔曼滤波的无人机姿态估计方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114880874A (zh) | 2022-08-09 |
| WO2023236247A1 (zh) | 2023-12-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114880874B (zh) | 一种水面无人船参数自适应鲁棒估计方法与系统 | |
| CN108803321B (zh) | 基于深度强化学习的自主水下航行器轨迹跟踪控制方法 | |
| CN111650948B (zh) | 一种可底栖式auv的水平面轨迹快速跟踪控制方法 | |
| Shariati et al. | Application of particle filter combined with extended Kalman filter in model identification of an autonomous underwater vehicle based on experimental data | |
| Wang et al. | Incorporating approximate dynamics into data-driven calibrator: A representative model for ship maneuvering prediction | |
| CN112182972B (zh) | 一种船舶操纵运动adam局部加权回归辨识建模方法 | |
| CN115016496A (zh) | 基于深度强化学习的水面无人艇路径跟踪方法 | |
| Lack et al. | Experimental Parameter Identification for an open-frame ROV: Comparison of towing tank tests and open water self-propelled tests | |
| Liu et al. | Deep reinforcement learning for vectored thruster autonomous underwater vehicle control | |
| CN111025909B (zh) | 船舶运动控制系统的Kalman三自由度解耦滤波方法 | |
| Kinjo et al. | Trajectory tracking of a fully-actuated surface vessel using nonlinear model predictive control | |
| CN113608541B (zh) | 一种水下航行器姿态控制方法和系统 | |
| CN114967714A (zh) | 一种自主式水下机器人抗扰运动控制方法及系统 | |
| Baier et al. | Hybrid physics and deep learning model for interpretable vehicle state prediction | |
| Wehbe et al. | A framework for on-line learning of underwater vehicles dynamic models | |
| CN113110512B (zh) | 一种减弱未知干扰与抖振影响的可底栖式auv自适应轨迹跟踪控制方法 | |
| Kinjo et al. | Trajectory tracking of a fully-actuated surface vessel using nonlinear model predictive control: Experimental results | |
| Linder et al. | Online Estimation of Ship's Mass and Center of Mass Using Inertial Measurements | |
| Hu et al. | Disturbance Observer-Based Model Predictive Control for an Unmanned Underwater Vehicle | |
| CN116774576A (zh) | 基于神经网络间接估计的水下航行器动力学黑盒建模方法 | |
| Wang et al. | Physics-informed Data-driven Approach for Ship Docking Prediction | |
| CN113608534A (zh) | 一种无人艇跟踪控制方法及系统 | |
| Sola et al. | Evaluation of a deep-reinforcement-learning-based controller for the control of an autonomous underwater vehicle | |
| Luo et al. | Modeling of ship manoeuvring motion using optimized support vector machines | |
| CN114035592B (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 |