基于UKF和滚动时域估计的水下机器人组合定位方法

专利主权项内容

1.一种基于UKF和滚动时域估计的水下机器人组合定位方法，该方法基于如下已知量：SINS陀螺仪输出的设备坐标系下的三维角速度信息Ω＝[ΩΩΩ]，其中Ω、Ω、Ω分别表示SINS陀螺仪输出的设备坐标系下x方向、y方向、z方向的角速度，上标T表示矩阵的转置；gyroxyzTxyzSINS加速度计输出的设备坐标系下的三维角加速度信息α＝[aaa]，其中a、a、a分别表示SINS加速度计输出的设备坐标系下x方向、y方向、z方向的三维角加速度；accxyzTxyzUSBL输出的设备坐标系下的三维位置信息M＝[mmm]，其中m、m、m分别表示USBL输出的设备坐标系下x方向、y方向、z方向的位置；kx, ky, kz, kTx, ky, kz, k其特征在于，该方法具体包括如下步骤：步骤一：对非线性系统进行建模，同时采用MHE算法得出待解决问题的代价函数，作为后续步骤的前提；步骤二：采用基于MHE算法针对异常测量值进行剔除处理，通过对滑动窗内的每个测量值进行假设，并且分别计算并比较当前假设下的代价函数，得到最终的代价函数；步骤三：基于滚动时域的自适应UKF算法来计算先验协方差矩阵；步骤四：基于鸽群算法计算最终代价函数的最优初值。