[发明专利]一种基于双贝叶斯估计的水下运动体运动状态估计方法

摘要

本发明属于运动状态估计领域，特别是一种基于双贝叶斯估计的水下运动体状态估计方法。建立水下运动体的运动状态估计模型：利用观测量解算出前一时刻水下运动体的速度大小及方向：利用下一时刻的观测信息更新贝叶斯估计参数：利用自适应贝叶斯运动估计方法，估计出水下运动体当前时刻的速度大小及方向；利用水下运动体当前时刻位置和自适应贝叶斯的估计结果，通过无迹贝叶斯运动估计方法，得到下一时刻的水下运动体位置。针对水下运动体运动情况复杂的特点，利用双贝叶斯估计的方法，该方法贴合水下运动体的运动模型，能够精确把握水下运动体运动状态。算法的估计精度高于单级运动状态估计方法。

主权项

一种基于双贝叶斯估计的水下运动体运动状态估计方法，其特征在于，包括以下几个步骤：(1)建立水下运动体的运动状态估计模型：(1.1)建立水下运动体的观测模型：$\left\{\begin{array}{c}\mathrm{\ξ}=R\cos E\cos A+x\\ \mathrm{\η}=R\sin E+y\\ \mathrm{\ζ}=R\cos E\sin A+z\end{array}\right.$(1.2)建立水下运动体的运动模型$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\ξ}}=vcos\mathrm{\φ}cos\mathrm{\θ}\\ \stackrel{\·}{\mathrm{\η}}=vsin\mathrm{\φ}cos\mathrm{\θ}\\ \stackrel{\·}{\mathrm{\ζ}}=vsin\mathrm{\θ}\end{array}\right.$(1.3)建立对水下运动体速度大小和方向进行估计的自适应贝叶斯估计器系统模型：$\left(\begin{array}{c}{\mathrm{\φ}}_k\\ {\stackrel{\·}{\mathrm{\φ}}}_k\\ {\mathrm{\θ}}_k\\ {\stackrel{\·}{\mathrm{\θ}}}_k\\ v_k\\ {\stackrel{\·}{v}}_k\end{array}\right)=\left(\begin{array}{cccccc}1& T& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& T& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& T\\ 0& 0& 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}{\mathrm{\φ}}_{k+1}\\ {\stackrel{\·}{\mathrm{\φ}}}_{k+1}\\ {\mathrm{\θ}}_{k+1}\\ {\stackrel{\·}{\mathrm{\θ}}}_{k+1}\\ v_{k+1}\\ {\stackrel{\·}{v}}_{k+1}\end{array}\right)+\left(\begin{array}{ccc}0& 0& 0\\ 1& 0& 0\\ 0& 0& 0\\ 0& 1& 0\\ 0& 0& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{w}_{\stackrel{\·}{\mathrm{\φ}}k}\\ w_{\stackrel{\·}{\mathrm{\θ}}k}\\ w_{\stackrel{\·}{v}k}\end{array}\right)$$\left(\begin{array}{c}{\mathrm{\φ}}_k\\ {\mathrm{\θ}}_k\\ v_k\end{array}\right)=\left(\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\end{array}\right)\left(\begin{array}{c}{\mathrm{\φ}}_{k+1}\\ {\stackrel{\·}{\mathrm{\φ}}}_{k+1}\\ {\mathrm{\θ}}_{k+1}\\ {\stackrel{\·}{\mathrm{\θ}}}_{k+1}\\ v_{k+1}\\ {\stackrel{\·}{v}}_{k+1}\end{array}\right)+V_k$(1.4)建立对水下运动体位置进行估计的无迹贝叶斯估计器系统模型：X_k+1＝f(X_k,φ,θ,v)+W_ky_k＝HX_k+V_k(2)利用观测量解算出前一时刻水下运动体的速度大小及方向：通过观测量解算出前一时刻水下运动体的速度及其方向；(3)利用下一时刻的观测信息更新贝叶斯估计参数：在获得下一时刻的观测信息后，对贝叶斯估计的参数进行更新；分别进行自适应估计器和无迹估计器的相关参数更新；(3.1)更新自适应估计器的参数，将水下运动体的角加速度和加速度的变化量视作系统噪声，使用噪声估计器进行估计；(3.2)利用观测信息更新无迹估计器的参数；(4)利用自适应贝叶斯运动估计方法，估计出水下运动体当前时刻的速度大小及方向；通过自适应贝叶斯运动估计方法中的估计方程，得到下一时刻水下运动体的速度大小和方向的估计值；(5)利用水下运动体当前时刻位置和自适应贝叶斯的估计结果，通过无迹贝叶斯运动估计方法，得到下一时刻的水下运动体位置。