潜水器运动轨迹修复方法及系统

1.本发明涉及潜水器操纵与控制领域，具体地，涉及潜水器运动轨迹修复方法及系统，更为具体地，涉及基于阿基米德螺旋线和三次样条曲线插值加密的潜水器运动轨迹修复策略。


背景技术：

2.当前，潜水器在海洋勘测、海事救援、军事侦察等领域发挥着重要的作用，良好的操纵性是潜水器安全航行的重要基础，所以准确地预报潜水器的操纵性至关重要。近年来，许多学者应用系统辨识法对潜水器的操纵性进行了预报，包括传统方法例如最小二乘法，卡尔曼滤波法等；也包括基于近年来新兴的神经网络和支持向量机的辨识方法。随着研究的不断深入，在实际试验中发现，姿态传感器采集到的潜水器状态数据往往是稀疏而非光滑连续的，经过数值仿真发现此现象会降低潜水器的预报精度。因此，准确修复潜水器的航行轨迹数据对于提升潜水器的操纵性预报精度、加强潜水器航行安全保障等具有重要意义。为此，本发明专利提出使用阿基米德螺旋线和三次样条曲线对轨迹进行插值加密的修复策略，以此来提供一种准确预报潜水器操纵性的新解决思路。
3.专利文献cn109726355a(申请号：201910008797.2)公开了一种基于向量插值的船舶轨迹修复方法，其修复船舶轨迹的方法首先是采用双向迭代的方法对船舶地理坐标的高斯投影处理后的数据进行插值，求取出两组船舶坐标预测值，然后选择权重对两组坐标预测值进行加权平均，最后再根据高斯反投影计算船舶的地理坐标，从而实现对船舶轨迹的修复。而本发明主要是使用阿基米德螺旋线和三次样条曲线进行插值加密的操纵轨迹修复，以便实现更高精度的潜水器模型系统辨识，这跟前述发明采用的技术创新方法与应用思路完全不同。
4.专利文献cn111858814a(申请号：202010707304.7)公开了一种运动轨迹的修复方法、装置、设备和存储介质，包括：基于在用户运动过程中获取的定位点信息生成初始运动轨迹；定位点信息包括经纬度、速度及时间戳；对初始运动轨迹进行分析，获得问题定位点；对问题定位点进行修正；根据修正后的问题定位点对初始运动轨迹进行修复，获得目标运动轨迹。


技术实现要素：

5.针对现有技术中的缺陷，本发明的目的是提供一种潜水器运动轨迹修复方法及系统。
6.根据本发明提供的一种潜水器运动轨迹修复方法，包括：
7.步骤s1：利用阿基米德螺旋线和三次样条曲线分别对稀疏的试验数据进行插值加密，得到修复后的试验数据；
8.步骤s2：修复后的试验数据基于潜水器一阶非线性操纵响应模型，使用最小二乘法进行潜水器操控系统辨识。
9.优选地，所述步骤s1中利用阿基米德螺旋线对稀疏的试验数据进行插值加密采用：
10.阿基米德螺旋线的极坐标公式表示为：
11.r＝aθ
ꢀꢀ
(1)
12.其中，r,θ分别表示极径和极角，a表示正常数；
13.使用ψi代表给定的第i个艏向角ψ的数据点，ψi和ψ
i 1
间用阿基米德螺旋线连接，表示为ψiψ
i 1
，在此之间对ψi的方向进行规定，ψ
i 1
处仅规定位置而不规定方向；为了保证轨迹的曲率连续，其间设置等时间间隔δt的离散化取值，取前一段螺旋线中最后一个插值点表示为则确定ψiψ
i 1
的初始方向为由此得到ψiψ
i 1
：
[0014][0015]
其中，σ表示初始角度；ρ表示阿基米德螺旋线螺旋的方向；(ti,ψi)表示两个稀疏点间阿基米德螺旋线插值加密后的数据点。
[0016]
优选地，所述步骤s1中利用三次样条曲线对稀疏的试验数据进行插值加密采用：
[0017]
设置时间间隔为δt，令重构后的时间序列用t
′
表示，当有n 1个数据节点，那么节点区间数为n，此时有n个关于艏向角ψ三次样条曲线插值多项式：
[0018]
ψi(t
′
)＝ai bi(t
′‑
ti) ci(t
′‑
ti)2 di(t
′‑
ti)3ꢀꢀ
(3)
[0019]
其中，ai,bi,ci和di为三次样条曲线插值的多项式系数，(ti,ψi)表示两个稀疏点间三次样条曲线插值加密后的数据点。
[0020]
优选地，所述步骤s2中潜水器一阶非线性操纵响应模型采用：
[0021][0022]
其中，t和k表示操纵性指数；α表示非线性项系数；为潜水器艏向角速度；ψ为潜水器艏向角；δ为潜水器舵角；表示潜水器艏向角速度关于时间的一阶导数。
[0023]
优选地，所述步骤s2采用：
[0024]
将方程潜水器一阶非线性操纵响应模型转化为ψ与δ的关系，代入得：
[0025][0026]
其中，和分别表示艏向角关于时间的一阶与二阶导数；
[0027]
取采样时间间隔δt，基于差分近似处理和可得：
[0028][0029]
其中，ψ(t)表示t时刻的潜水器艏向角；ψ(t-1)表示t-1时刻的潜水器艏向角；ψ(t-2)表示t-2时刻的潜水器艏向角；δ(t-2)表示t-2时刻的潜水器舵角；
[0030]
将其写成最小二乘格式y(t)＝θx(t)：
[0031]
输入矩阵x(t)：
[0032][0033]
输出标量y(t)：
[0034]
y(t)＝ψ(t)-2ψ(t-1) ψ(t-2)
ꢀꢀ
(8)
[0035]
待辨识的参数矩阵θ：
[0036][0037]
根据本发明提供的一种潜水器运动轨迹修复系统，包括：
[0038]
模块m1：利用阿基米德螺旋线和三次样条曲线分别对稀疏的试验数据进行插值加密，得到修复后的试验数据；
[0039]
模块m2：修复后的试验数据基于潜水器一阶非线性操纵响应模型，使用最小二乘法进行潜水器操控系统辨识。
[0040]
优选地，所述模块m1中利用阿基米德螺旋线对稀疏的试验数据进行插值加密采用：
[0041]
阿基米德螺旋线的极坐标公式表示为：
[0042]
r＝aθ
ꢀꢀ
(1)
[0043]
其中，r,θ分别表示极径和极角，a表示正常数；
[0044]
使用ψi代表给定的第i个艏向角ψ的数据点，ψi和ψ
i 1
间用阿基米德螺旋线连接，表示为ψiψ
i 1
，在此之间对ψi的方向进行规定，ψ
i 1
处仅规定位置而不规定方向；为了保证轨迹的曲率连续，其间设置等时间间隔δt的离散化取值，取前一段螺旋线中最后一个插值点表示为则确定ψiψ
i 1
的初始方向为由此得到ψiψ
i 1
：
[0045][0046]
其中，σ表示初始角度；ρ表示阿基米德螺旋线螺旋的方向；(ti,ψi)表示两个稀疏点间阿基米德螺旋线插值加密后的数据点。
[0047]
优选地，所述模块m1中利用三次样条曲线对稀疏的试验数据进行插值加密采用：
[0048]
设置时间间隔为δt，令重构后的时间序列用t
′
表示，当有n 1个数据节点，那么节点区间数为n，此时有n个关于艏向角ψ三次样条曲线插值多项式：
[0049]
ψi(t
′
)＝ai bi(t
′‑
ti) ci(t
′‑
ti)2 di(t
′‑
ti)3ꢀꢀ
(3)
[0050]
其中，ai,bi,ci和di为三次样条曲线插值的多项式系数，(ti,ψi)表示两个稀疏点间三次样条曲线插值加密后的数据点。
[0051]
优选地，所述模块m2中潜水器一阶非线性操纵响应模型采用：
[0052][0053]
其中，t和k表示操纵性指数；α表示非线性项系数；为潜水器艏向角速度；ψ
为潜水器艏向角；δ为潜水器舵角；表示潜水器艏向角速度关于时间的一阶导数。
[0054]
优选地，所述模块m2采用：
[0055]
将方程潜水器一阶非线性操纵响应模型转化为ψ与δ的关系，代入得：
[0056][0057]
其中，和分别表示艏向角关于时间的一阶与二阶导数；
[0058]
取采样时间间隔δt，基于差分近似处理和可得：
[0059][0060]
其中，ψ(t)表示t时刻的潜水器艏向角；ψ(t-1)表示t-1时刻的潜水器艏向角；ψ(t-2)表示t-2时刻的潜水器艏向角；δ(t-2)表示t-2时刻的潜水器舵角；
[0061]
将其写成最小二乘格式y(t)＝θx(t)：
[0062]
输入矩阵x(t)：
[0063][0064]
输出标量y(t)：
[0065]
y(t)＝ψ(t)-2ψ(t-1) ψ(t-2)
ꢀꢀ
(8)
[0066]
待辨识的参数矩阵θ：
[0067][0068]
与现有技术相比，本发明具有如下的有益效果：本发明针对潜水器实际试验中采集的运动轨迹具有稀疏性这一现象，提出了基于阿基米德螺旋线和三次样条曲线插值加密的潜水器运动轨迹修复策略，可以较精确的还原再现出潜水器的原始运动轨迹，且本策略修复后的轨迹曲率连续、曲线光滑，符合潜水器实际运动的规律，为精确预报潜水器操纵性奠定基础。该方法对潜水器的运动轨迹修复，进而提升操纵性预报精度具有独特的优越性。
附图说明
[0069]
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
[0070]
图1为本发明的潜水器插值加密的运动轨迹修复策略示意图。
[0071]
图2为本发明的基于最小二乘法对阿基米德螺旋线插值加密的潜水器运动轨迹的辨识结果。
[0072]
图3为本发明的基于最小二乘法对三次样条曲线插值加密的潜水器运动轨迹的辨识结果。
具体实施方式
[0073]
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
[0074]
实施例1
[0075]
本发明提供了一种基于阿基米德螺旋线和三次样条曲线插值加密的潜水器轨迹修复策略，针对潜水器稀疏的试验数据会降低其操纵性预报精度这一问题，提出了基于阿基米德螺旋线和三次样条曲线对潜水器缺失轨迹进行插值加密的修复策略。
[0076]
首先，阐述了阿基米德螺旋线和三次样条曲线插值加密策略的原理和具体实现方法；接着，将稀疏的试验数据带入到阿基米德螺旋线和三次样条曲线的公式中进行处理，求取出插值点坐标，进而得到插值加密后的潜水器试验数据，从而实现对潜水器缺失轨迹的修复；最后，使用最小二乘法对实验数据进行辨识，并引入均方根误差来评判精度。如图1所示，本发明从理论上严谨地阐述了基于阿基米德螺旋线和三次样条曲线插值加密技术的潜水器运动轨迹修复策略，对稀疏的试验数据进行了插值加密的修复处理，为提高潜水器操纵性预报精度提供了一种新思路。
[0077]
根据本发明提出基于阿基米德螺旋线和三次样条插值加密的潜水器运动轨迹修复方法，包括：
[0078]
使用阿基米德螺旋线插值加密的轨迹修复步骤：阐述阿基米德螺旋线插值原理，对稀疏的试验数据进行插值加密；
[0079]
使用三次样条曲线插值加密的轨迹修复步骤：阐述三次样条曲线插值原理，对稀疏的试验数据进行插值加密；
[0080]
试验数据修复后使用最小二乘法的系统辨识步骤：阐述最小二乘法系统辨识原理，基于潜水器一阶非线性操纵响应模型，使用最小二乘法对修复后的试验数据进行系统辨识；
[0081]
所述潜水器一阶非线性操纵响应模型是通过将舵角作为输入，艏向角作为输出的黑箱建模思想，实现了艏向角对舵角响应的准确刻画。
[0082]
所述使用阿基米德螺旋线插值加密的轨迹修复步骤中，插值加密步骤如下：
[0083]
阿基米德螺旋线的极坐标公式可表示为：
[0084]
r＝aθ
[0085]
其中，r,θ分别代表极径和极角，a为一正常数。使用ψi代表给定的第i个艏向角ψ的数据点，ψi和ψ
i 1
间用阿基米德螺旋线连接，表示为ψiψ
i 1
，在此之间对ψi的方向进行规定，ψ
i 1
处仅规定位置而不规定方向。为了保证轨迹的曲率连续，其间设置等时间间隔δt的离散化取值，所取前一段螺旋线中最后一个插值点表示为则确定ψiψ
i 1
的初始方向为由此得到ψiψ
i 1
：
[0086][0087]
式中，σ为初始角度：
[0088]
σ＝atan2(υ
iψ
,υ
it
)
[0089][0090]
ρ为阿基米德螺旋线螺旋的方向，表示为：
[0091]
ρ＝-sign(t
i 1
·
ψi ψ
i 1
·
ti)
[0092]
ρ＝1时，表示螺旋路径从极坐标的原点按顺时针方向传播，反之则为逆时针方向。
[0093]
阿基米德螺旋线段的具体比例取决于正常数a，其值可通过终止点ψ
i 1
的限制条件解得，进而可求得两个稀疏点间阿基米德螺旋线插值加密后的数据点(ti,ψi)。
[0094]
所述使用三次样条曲线插值加密的轨迹修复步骤中，插值加密步骤如下：
[0095]
设置时间间隔为δt，令重构后的时间序列用t
′
表示，当有n 1个数据节点，那么节点区间数为n，此时有n个关于艏向角ψ三次样条曲线插值多项式：
[0096]
ψi(t
′
)＝ai bi(t
′‑
ti) ci(t
′‑
ti)2 di(t
′‑
ti)3[0097]
其中i＝0,1
…
n，对ψ导可得：
[0098][0099][0100]
设代入上式后，根据二阶微分连续性可计算出各节点区间的系数：
[0101][0102]
此时未知量m的数量为n 1，方程的数量是n-1，于是选择自然边界条件，则有m0＝0，mn＝0，根据一阶微分连续性可得：
[0103][0104]
将bi,ci和di代入上式可得：
[0105][0106]
将其表示为矩阵的形式：
[0107][0108]
到此即可求得各个节点区间的插值多项式函数ψi(t
′
)，进而可求得三次样条曲线插值加密后的数据点(ti,ψi)。
[0109]
所述插值加密修复后使用最小二乘方法进行系统辨识的步骤中，所述系统辨识的步骤如下：
[0110]
潜水器的一阶非线性操纵响应模型可以表示为：
[0111][0112]
式中，t和k是操纵性指数，α为非线性项系数；为潜水器艏向角速度；ψ为潜水器艏向角；δ为潜水器舵角。
[0113]
在试验中，实际采集的潜水器模型状态数据为离散数据，所以要对操纵响应模型进行离散化处理：
[0114]
首先将方程转化为ψ与δ的关系，代入得：
[0115][0116]
取采样时间间隔δt，基于差分近似处理和可得：
[0117][0118]
将其写成最小二乘格式y(t)＝θx(t)：
[0119]
输入矩阵x(t)：
[0120][0121]
输出标量y(t)：
[0122]
y(t)＝ψ(t)-2ψ(t-1) ψ(t-2)
[0123]
待辨识的参数矩阵θ：
[0124][0125]
综上，即可基于修复后的加密数据通过最小二乘法辨识出参数矩阵θ。
[0126]
将辨识得到的操纵性指数代入到潜水器操纵响应方程中，并对艏向角数据进行30
°
z形试验仿真预报。其中，基于阿基米德螺旋线(as)插值修复数据的最小二乘(ls)辨识
预报结果如图2所示；基于三次样条曲线(spline)插值修复数据的ls辨识预报结果如图3所示；预报结果较原数据的均方根误差(rmse)比较见下表。
[0127]
表1均方根误差比较
[0128][0129]
根据本发明提出基于阿基米德螺旋线和三次样条插值加密的潜水器运动轨迹修复系统，包括：
[0130]
使用阿基米德螺旋线插值加密的轨迹修复模块：阐述阿基米德螺旋线插值原理，对稀疏的试验数据进行插值加密；
[0131]
使用三次样条曲线插值加密的轨迹修复模块：阐述三次样条曲线插值原理，对稀疏的试验数据进行插值加密；
[0132]
试验数据修复后使用最小二乘法的系统辨识模块：阐述最小二乘法系统辨识原理，基于潜水器一阶非线性操纵响应模型，使用最小二乘法对修复后的试验数据进行系统辨识；
[0133]
所述潜水器一阶非线性操纵响应模型是通过将舵角作为输入，艏向角作为输出的黑箱建模思想，实现了艏向角对舵角响应的准确刻画。
[0134]
所述使用阿基米德螺旋线插值加密的轨迹修复模块中，插值加密如下：
[0135]
阿基米德螺旋线的极坐标公式可表示为：
[0136]
r＝aθ
[0137]
其中，r,θ分别代表极径和极角，a为一正常数。使用ψi代表给定的第i个艏向角ψ的数据点，ψi和ψ
i 1
间用阿基米德螺旋线连接，表示为ψiψ
i 1
，在此之间对ψi的方向进行规定，ψ
i 1
处仅规定位置而不规定方向。为了保证轨迹的曲率连续，其间设置等时间间隔δt的离散化取值，所取前一段螺旋线中最后一个插值点表示为则确定ψiψ
i 1
的初始方向为由此得到ψiψ
i 1
：
[0138][0139]
式中，σ为初始角度：
[0140]
σ＝atan2(υ
iψ
,υ
it
)
[0141][0142]
其中，ψ
i-1ilast
表示
[0143]
ρ为阿基米德螺旋线螺旋的方向，表示为：
[0144]
ρ＝-sign(t
i 1
·
ψi ψ
i 1
·
ti)
[0145]
ρ＝1时，表示螺旋路径从极坐标的原点按顺时针方向传播，反之则为逆时针方向。
[0146]
阿基米德螺旋线段的具体比例取决于正常数a，其值可通过终止点ψ
i 1
的限制条件解得，进而可求得两个稀疏点间阿基米德螺旋线插值加密后的数据点(ti,ψi)。
[0147]
所述使用三次样条曲线插值加密的轨迹修复模块中，插值加密如下：
[0148]
设置时间间隔为δt，令重构后的时间序列用t
′
表示，当有n 1个数据节点，那么节点区间数为n，此时有n个关于艏向角ψ三次样条曲线插值多项式：
[0149]
ψi(t
′
)＝ai bi(t
′‑
ti) ci(t
′‑
ti)2 di(t
′‑
ti)3[0150]
其中i＝0,1
…
n，对ψ导可得：
[0151][0152][0153]
设代入上式后，根据二阶微分连续性可计算出各节点区间的系数：
[0154][0155]
此时未知量m的数量为n 1，方程的数量是n-1，于是选择自然边界条件，则有m0＝0，mn＝0，根据一阶微分连续性可得：
[0156][0157]
将bi,ci和di代入上式可得：
[0158][0159]
将其表示为矩阵的形式：
[0160][0161]
到此即可求得各个节点区间的插值多项式函数ψi(t
′
)，进而可求得三次样条曲线插值加密后的数据点(ti,ψi)。
[0162]
所述插值加密修复后使用最小二乘方法进行系统辨识的模块中，所述系统辨识如下：
[0163]
潜水器的一阶非线性操纵响应模型可以表示为：
[0164]
[0165]
式中，t和k是操纵性指数，α为非线性项系数；为潜水器艏向角速度；ψ为潜水器艏向角；δ为潜水器舵角。
[0166]
在试验中，实际采集的潜水器模型状态数据为离散数据，所以要对操纵响应模型进行离散化处理：
[0167]
首先将方程转化为ψ与δ的关系，代入得：
[0168][0169]
取采样时间间隔δt，基于差分近似处理和可得：
[0170][0171]
将其写成最小二乘格式y(t)＝θx(t)：
[0172]
输入矩阵x(t)：
[0173][0174]
输出标量y(t)：
[0175]
y(t)＝ψ(t)-2ψ(t-1) ψ(t-2)
[0176]
待辨识的参数矩阵θ：
[0177][0178]
综上，即可基于修复后的加密数据通过最小二乘法辨识出参数矩阵θ。
[0179]
将辨识得到的操纵性指数代入到潜水器操纵响应方程中，并对艏向角数据进行30
°
z形试验仿真预报。其中，基于阿基米德螺旋线(as)插值修复数据的最小二乘(ls)辨识预报结果如图2所示；基于三次样条曲线(spline)插值修复数据的ls辨识预报结果如图3所示；预报结果较原数据的均方根误差(rmse)比较见下表。
[0180]
表1均方根误差比较
[0181][0182]
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
[0183]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述
特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本技术的实施例和实施例中的特征可以任意相互组合。