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

申请日：2023-05-24

公开（公告）日：2023-08-01

公开（公告）号：CN116295538B

主分类号：G01C25/00

分类号：G01C25/00;H03H17/02;G06N3/006

优先权：

专利状态码：有效-授权

法律状态：2023.08.01#授权;2023.07.11#实质审查的生效;2023.06.23#公开

摘要：本发明公开了一种基于改进粒子滤波的USBL安装误差标定方法，属于水下机器人组合定位技术领域，该方法基于USBL输出的两个方位角信息和斜距信息，卫星定位系统提供的导航系下的位置信息；应答器在导航系下的绝对位置，首先构建系统状态空间模型；在构建的系统状态空间模型的基础上，开展基于改进粒子滤波算法的状态估计，获得最优状态估计值；在获得的最优状态估计值的基础上进行平滑变结构滤波处理。本方法实现对状态量的抗干扰鲁棒滤波估计，最终得到相对准确的状态向量。

主权项：1.一种基于改进粒子滤波的USBL安装误差标定方法，该方法基于的已知量有USBL输出信息：α，β，U，其中，α和β分别表示USBL输出的两个方位角信息，U表示USBL输出的斜距信息；卫星定位系统提供的导航系下的位置信息应答器在导航系下的绝对位置其特征在于，该方法具体包括如下步骤：S1.构建系统状态空间模型；S2.在步骤S1构建的系统状态空间模型的基础上，开展基于改进粒子滤波算法的状态估计，获得最优状态估计值；S3.在步骤S2获得的最优状态估计值的基础上进行平滑变结构滤波处理；步骤S1的具体方法是：首先，建立USBL安装误差标定系统的状态空间模型，包括状态方程和量测方程：Xk＝FXk-1+Wk-1Zk＝HXk+VkXk表示k时刻的状态向量，FXk-1表示状态转移函数，Wk-1表示系统噪声矩阵，Zk表示k时刻的量测矩阵，HXk表示量测转移函数，Vk表示k时刻的量测噪声矩阵；k时刻的状态向量Xk定义如下：Xk＝[AxAyAz]T其中，Ax、Ay、Az分别表示载体坐标系下USBL在x、y、z三个方向的安装误差角，上标T表示矩阵的转置；k时刻的量测矩阵Zk定义如下：zk＝[αβR]T考虑到安装误差角为常数，其微分为0，因此状态转移函数表示如下：FXk-1＝0量测转移函数HXk的表达式： 其中，[xayaza]T表示应答器在声学坐标系下的相对位置，角标a表示声学坐标系，具体表示如下： 其中，角标n表示导航坐标系，角标b表示载体坐标系，表示卫星定位系统提供的导航坐标系下的位置信息，是已知量；表示应答器在导航坐标下的绝对位置，是已知量；表示载体坐标系与导航坐标系的姿态转移矩阵；表示声学坐标系与载体系的安装误差姿态矩阵，即待标定参数，和状态向量Xk的关系表示如下： 步骤S2的具体方法如下：S21.初始化参数定义PXk表示状态向量的先验概率密度函数，初始化参数中，使用零时刻的状态向量先验概率密度函数PX0产生采样粒子群初始化粒子集个数为M＝120，所以i＝1，2，..，M；初始化各个粒子的权重为S22.重要性采样处理采用基于欧氏距离的方法计算各个粒子的权重，公式如下： 其中，表示k时刻第i个粒子的权重，R表示量测噪声协方差矩阵，Zk表示k时刻的量测矩阵，表示k时刻第i个粒子的状态估计值，为避免粒子权重值为0的情况，这里设置权重最小值为0.0000001；对基于欧氏距离的方法计算得到的各个粒子的权重值进行加权及归一化处理： 其中，表示加权及归一化处理后的k时刻第i个粒子的权重，η表示引入的权重调整因子，满足0＜η＜1；S23.重采样处理首先，保留前一时刻接近USBL测量值的高权重粒子；其次，权重小的粒子按照初始化的方式，以前一时刻的USBL测量值为均值，在测量误差范围内进行随机采样，最后，重新组建M＝120个粒子权重集合S24.计算最优状态估计值 其中，表示经过改进粒子滤波算法处理后的k时刻的状态估计值。

全文数据：

权利要求：

百度查询： 河海大学 一种基于改进粒子滤波的USBL安装误差标定方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD