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

龙图腾网获悉东南大学申请的专利一种基于KLD重采样的抗差自适应UFastSLAM自主导航方法获国家发明授权专利权，本发明授权专利权由国家知识产权局授予，授权公告号为：CN115291523B 。

龙图腾网通过国家知识产权局官网在2025-07-18发布的发明授权授权公告中获悉：该发明授权的专利申请号/专利号为：202211061382.X，技术领域涉及：G05B13/04；该发明授权一种基于KLD重采样的抗差自适应UFastSLAM自主导航方法是由王立辉;翟鸿启;蔡体菁;应泽华;孟骞设计研发完成，并于2022-08-31向国家知识产权局提交的专利申请。

本一种基于KLD重采样的抗差自适应UFastSLAM自主导航方法在说明书摘要公布了：一种基于KLD重采样的抗差自适应UFastSLAM自主导航方法，包括步骤：1、在载体位姿估计阶段，融入抗差自适应因子，利用抗差自适应无迹粒子滤波算法RAUPF对载体位姿进行估计；2、在特征状态估计阶段，利用抗差自适应无迹滤波算法RAUKF对环境特征的位置信息进行估计更新；3、在粒子重采样阶段，采用基于KL散度的自适应粒子重采样方法在线实时调整所需的粒子数，在保证精度的情况下提高算法的实时性。该方法可以适用于量测噪声时变且量测信息中有异常干扰时的环境，改善因异常扰动和噪声统计特性不准确等因素而引起的载体定位精度下降及实时性不高的问题。

本发明授权一种基于KLD重采样的抗差自适应UFastSLAM自主导航方法在权利要求书中公布了：1.一种基于KLD重采样的抗差自适应UFastSLAM自主导航方法，其特征在于：包括如下步骤： （1）在载体位姿估计阶段，融入抗差自适应因子，利用抗差自适应无迹粒子滤波算法对载体位姿进行估计； 步骤（1）的具体方法如下： （1-1）载体位姿预测阶段： 在载体位姿预测阶段，粒子扩充后的状态向量和协方差矩阵分别表示为： ； ； 其中，和分别是时刻的状态向量和协方差矩阵，是控制噪声协方差； 将进行Cholesky分解，则分解后的协方差矩阵表示为： ； 其中，表示Cholesky分解； 根据Sigma点对称采样策略对机器人位姿状态提取2L+1个Sigma点，L是增广状态向量的维数，则Sigma点集为： ； 其中，表示矩阵的第列，，是一个很小的数，用来避免对强非线性系统采样时所发生的非局部效应，称作Sigma缩放参数，这里取默认值； 然后，将采样的点通过非线性运动函数进行转换； ； 其中，和分别是增广状态向量中的载体状态分量和控制分量； 通过对进行线性加权处理，便获得系统的状态预测向量和状态预测协方差矩阵为： ； ； 其中，和分别为均值和协方差的权值，由下式计算得到： ； 式中，为结合后验概率分布高阶矩信息项的参数，理想取值为2； （1-2）载体位姿更新阶段： 在载体位姿更新阶段，当某个特征被载体再次观测到时，由量测方程生成测量值Sigma点集为： ； 其中，表示第个特征在时刻的位置估计值； 根据测量Sigma点集计算预测测量值的均值为： ； 由此可得，新息协方差和载体状态与观测量间的互协方差表示为： ； ； 其中，自适应抗差因子由下式计算得到： ； 其中，为常数，一般情况下，由下式计算得到： ； 其中，为矩阵求逆算子； 故，新息协方差矩阵表示为： ； 根据带有的新息协方差和互协方差矩阵计算卡尔曼增益矩阵为： ； 因此，载体位姿状态后验估计均值和协方差矩阵分别表示为： ； ； 当同一时刻观测到多个特征点时，重复计算，得到最终的载体位姿状态估计均值和协方差； 最后，由上式的结果作为粒子建议分布的统计量，同时生成新一代的粒子： ； （1-3）重要性权值计算： RAUFastSLAM算法的重要性权值也是高斯分布的形式，通过下式计算得到： ； 其中，： （2）在特征状态估计阶段，利用抗差自适应无迹滤波算法对环境特征的位置信息进行估计更新； 步骤（2）的具体方法如下： （2-1）新特征状态初始化： 在新特征状态初始化阶段，若水下环境中每个特征点的位置信息采用的列向量来表示，即特征状态的维数为，则根据对称性采样原则，需要对每个特征采样个Sigma点，因此，由当前观测量和量测噪声协方差得Sigma点集为： ； 对构造的Sigma点进行非线性变换： ； 则新特征的均值和协方差分别表示为： ； ； （3-2）已有特征的位置更新； 当观测到的特征与状态量中已有的特征数据关联时，需要对该特征的位置信息进行更新，首先，在采样该特征的Sigma点之前对特征的状态向量和协方差进行扩充处理，则扩充后的状态向量和协方差分别表示为： ； ； 同时，构建个Sigma点集为： ； 其中，是增广矩阵的维数，，这里取； 由载体的当前位姿信息和非线性量测模型对Sigma点集进行变换，可得： ； 因此，预测量测值和对应的协方差矩阵分别表示为： ； ； 同样地，为抑制环境不确定干扰对特征估计精度的影响，在量测新息协方差矩阵中引入自适应抗差因子，其中，自适应抗差因子由下式计算得到： ； 其中，为常数，通常为，而由下式计算得到： ； 则量测新息协方差矩阵和互协方差矩阵分别表示为： ； ； 由此可得，其滤波增益为： ； 故，特征的均值和协方差矩阵分别表示为： ； ； （3）在粒子重采样阶段，采用基于KL散度的自适应粒子重采样方法在线实时调整所需的粒子数，在保证精度的情况下提高算法的实时性； 步骤（3）的具体方法如下： 为了避免粒子退化，当粒子集中的有效粒子个数小于某一阈值，所述阈值选取粒子总数的75%，需要对其执行重采样操作，其中，有效粒子数通过下式求得： ； 其中，是总的粒子个数，是第个粒子的归一化权重； 载体位姿状态的真实后验概率分布与其基于粒子的近似分布之间的KL散度定义为： ；每当有新的粒子被重采样时，为了使KL散度以概率小于预先给定的误差，所需的最少粒子个数由下式计算： ； 其中，表示非空子空间的个数，表示标准正态分布的上分位点。

如需购买、转让、实施、许可或投资类似专利技术，可联系本专利的申请人或专利权人东南大学，其通讯地址为：210096 江苏省南京市玄武区四牌楼2号；或者联系龙图腾网官方客服，联系龙图腾网可拨打电话0551-65771310或微信搜索“龙图腾网”。