买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：河海大学

摘要：本发明公开了基于UKF和滚动时域估计的水下机器人组合定位方法，本发明整个过程主要步骤包括：系统建模与目标函数描述、针对异常量测值剔除、基于滚动时域的自适应UKF算法以及基于鸽群算法的最优值求解。本发明可以解决水下机器人SINSUSBL组合导航系统中，因系统的约束条件存在以及机器人受外界干扰从而降低MHE算法的精度的问题，本发明引入改进的自适应UKF算法，针对过程噪声Q易被外界干扰影响的问题提供了一种解决方案。相比于传统SINSUSBL组合导航组合定位方法，本方法能有效减小水下机器人运动过程中外界干扰对状态估计的影响，进一步提高对非线性系统的组合定位能力。

主权项：1.一种基于UKF和滚动时域估计的水下机器人组合定位方法，该方法基于如下已知量：SINS陀螺仪输出的设备坐标系下的三维角速度信息Ωgyro＝[ΩxΩyΩz]T，其中Ωx、Ωy、Ωz分别表示SINS陀螺仪输出的设备坐标系下x方向、y方向、z方向的角速度，上标T表示矩阵的转置；SINS加速度计输出的设备坐标系下的三维角加速度信息αacc＝[axayaz]T，其中ax、ay、az分别表示SINS加速度计输出的设备坐标系下x方向、y方向、z方向的三维角加速度；USBL输出的设备坐标系下的三维位置信息Mk＝[mx，kmy，kmz，k]T，其中mx，k、my，k、mz，k分别表示USBL输出的设备坐标系下x方向、y方向、z方向的位置；其特征在于，该方法具体包括如下步骤：步骤一：对非线性系统进行建模，同时采用MHE算法得出待解决问题的代价函数，作为后续步骤的前提；步骤二：采用基于MHE算法针对异常测量值进行剔除处理，通过对滑动窗内的每个测量值进行假设，并且分别计算并比较当前假设下的代价函数，得到最终的代价函数；步骤三：基于滚动时域的自适应UKF算法来计算先验协方差矩阵；步骤四：基于鸽群算法计算最终代价函数的最优初值；步骤一的具体方法如下：对于非线性系统： 其中，xk为k时刻系统状态，xk+1为k+1时刻系统状态，f是非线性状态方程函数，h是非线性观测方程函数，wk，vk分别为状态噪声和量测噪声；设置滑动窗N＝10，当k≤10时使用全信息滚动时域算法进行状态估计，k时刻优化问题的代价函数Jk为： 其中，为目标最优解，即最优初值；Rk为量测噪声方差阵；Qk为过程噪声方差阵；上标T表示转置；x0为系统状态初值；为系统初值的先验估计值；P0为的协方差矩阵，vj为量测噪声，wj为状态噪声；上述代价函数的目标最优解由步骤四求得，求解后k时刻的状态估计值由最优初值表示；当k＞10时，计算时域t划分为{0≤t≤k-N-1}，{k-N≤t≤k-1}，其中{0≤t≤k-N-1}部分通过到达代价函数描述，{k-N≤t≤k-1}部分使用近似滚动时域算法进行状态估计，其k时刻优化问题的代价函数为： 其最优解为滑动窗内的最优“初值”由步骤四求得，求解后k时刻的状态估计值可以由最优解表示；步骤二的具体方法如下：针对滑动窗MHE内有一个测量值被干扰从而变为异常值的情况，假设被影响的是滑动窗内第l个测量值，则k时刻滑动窗内第l个测量值的代价函数可以描述为： 其中，σ的选择标准为A为非线性系统线性化后的状态转移矩阵，δ的取值与非线性系统线性化后的状态转移矩阵和量测矩阵有关，通过l不同取值时系统的结构参数取得；xk-N为序列为k-N的系统状态值；为xk-N的先验估计值；则在滑动窗范围内算出每一个找出其中的最小值为此时在针对测量异常值剔除问题时，与等价，由步骤三、步骤四求解；步骤三具体包括如下子步骤：子步骤3.1：采样点选择对于滑动窗之前的所有状态值xk-N和协方差矩阵Pk-N，按照对称采样原则选取2n+1个Sigma点，n为系统状态维数，则： λ＝α2n+k-n，α＝0.5；β＝2；k＝3-n其中，为选取的Sigma点集；λ为算法参数；α，k，β为Sigma采点参数；分别为状态预测量均值权重和其协方差权重；为状态向量的后验估计；分别为i＝0时的状态预测量均值权重和其协方差权重；子步骤3.2：自适应UKF算法过程首先计算预测状态的均值与方差，方法如下： 其中，为预测状态的加权均值；为的方差阵，为k-N+1时刻的Sigma点集；Qk-N+1为k-N+1时刻的过程噪声方差阵；接下来计算量测预测值及其协方差矩阵和互协方差阵： 其中，C为非线性系统线性化后的量测矩阵；为k-N+1时刻Sigma点的量测预测值；为量测预测均值；Tk-N+1分别为的协方差矩阵和互协方差阵；Rk-N+1为k-N+1时刻的量测噪声方差阵；最后进行状态更新，根据当前时刻的状态预测值和下一时刻的测量值得出下一时刻的状态估计值，方法如下： 其中，Kk-N+1表示滤波增益；Pk-N+1为k-N+1时刻的协方差阵；yk-N+1为k-N+1时刻的量测值；子步骤3.3：Qk自适应优化 其中，ξk-N+1为k-N+1时刻估计状态值与预测值之差；为初始时刻到k-N+1时刻偏差平均值；ΔQk-N+1为Qk-N+1的校正项；Pk-N+1|k-N为基于k-N时刻预测的k-N+1时刻的协方差矩阵；Qk-1为k-1时刻的过程噪声方差阵；Pk-N为k-N时刻的协方差阵；Qk-N为k-N时刻的过程噪声方差阵；步骤四具体包括如下子步骤：子步骤4.1：地图指南针算子阶段设置迭代次数为G＝200，G1为地图指南针算子阶段迭代次数阈值，G1＝100；初始化鸽群算法种群数量为H＝100，维度为n，当G＜G1时： 其中，表示鸽群算法中第t个个体随机初始化的速度在G次迭代后的值，t＝1，…，H；F为地图和指南针算子，F∈0，1；rand表示一个随机数，取值在0和1之间；xbest表示种群在G次迭代过程之前全局最优位置；e为常数；为鸽群算法中第t个个体随机初始化的位置在G次迭代后的值；子步骤4.2：地标算子导航阶段当200≥G≥G1时，进入地标算子导航阶段： 其中，代表第G-1次迭代时鸽群的中心位置；表示将作为最优初值带入代价函数时相应的值；当迭代次数超过200，即200＜G时结束迭代，且： 由上式求出步骤一中代价函数的最优初值，进而可以用该值表示出k-N时刻的状态估计值。

全文数据：

权利要求：

百度查询： 河海大学 基于UKF和滚动时域估计的水下机器人组合定位方法