基于CKF-SLAM改进的无人水下航行器动态目标跟踪算法研究.pdf

云南民族大学学报（自然科学版），（）：收稿日期：基金项目：国家自然基金（）；云南省基础研究专项面上项目（）；云南省高校信息与通信安全灾备重点实验室（）作者简介：都立立（），女，硕士研究生 主要从事水下定位研究通信作者：邢传玺（），男，博士，教授 主要从事水声信号处理研究基于 改进的无人水下航行器动态目标跟踪算法研究都立立，邢传玺，万志良，李聪颖（云南民族大学 电气信息工程学院，云南 昆明 ）摘要：针对容积卡尔曼滤波（，）同步定位与建图（，）算法在动态目标跟踪（，）的应用中，存在算法实时性不高、计算复杂以及对动态目标物跟踪精度较低的问题，提出基于平方根容积卡尔曼滤波 的无人水下航行器（，）目标跟踪算法（）该算法将 中复杂的计算部分，利用 阶容积准则选取一组相同权值的容积点来近似计算，再利用数值积分法计算非线性方程模型的后验状态估计平均值和方差，并对协方差矩阵的平方根因子进行更新 仿真结果表明：算法简化了计算量和改善了数值精度，提高了 在未知水下环境中自身定位的精度和动态目标物跟踪的能力关键词：动态目标跟踪；容积卡尔曼滤波；同步定位与建图；平方根容积卡尔曼滤波；无人水下航行器中图分类号：文献标志码：文章编号：（）在水下侦察领域，对动态目标进行跟踪是至关重要的研究课题 传统动态目标跟踪技术需要将实时数据与已知先验数据进行匹配才能实现，但在实际匹配过程中容易导致大幅偏差 文中可在无先验信息 的情况下，即在未知水下环境中，动态目标跟踪在完成对自身位置定位与周围环境建图 的同时，探测估计动态目标的轨迹与位姿，整个运动过程中一直对动态目标进行跟踪 前者为 问题，后者为目标跟踪问题 目前在动态目标跟踪的问题上，主要集中在基于卡尔曼滤波类 方法 扩展卡尔曼滤波（，）算法，是一种最小均方差估计方法，将非线性系统利用泰勒级数一阶展开线性化，再进行线性卡尔曼滤波 算法简单易于实现并且在未知环境特征点较少时，实际轨迹可以逼近理想的轨迹，但当未知环境中的特征点增加时，实际轨迹和理想轨迹偏差较大，因此该算法只适合应用在环境特征点少的小区域环境 无迹卡尔曼滤波（，）算法，该算法利用高斯随机变量来近似表达状态分布，用一组确定性采样得到的 点来逼近，提高变量的平均值和方差，精度逼近二阶泰勒 算法在精度上有所提高，计算复杂度上因无须计算雅克比矩阵，因此更容易实现，但同样不适用于庞大复杂的环境 容积卡尔曼滤波（，）算法，该算法是在三阶球面 容积变换规则下，提取容积样本点集，用来描述后验状态变量的统计特征，并由先验状态估计平均值和方差 算法的容积样本点集的权重比非负，保证在计算高斯积分时滤波性能和数值稳定，但也存在算法实时性不高、计算量大以及在面对动态目标物跟踪精度较低的问题基于以上内容，提出平方根容积卡尔曼滤波 的 动态目标跟踪算法 该算法将 自身位姿、特征地图的估计和目标位姿三者相互耦合构成一个整体，确保了各个状态之间的关联性，并且随机函数的均值和方差采用容积准则下数值积分法计算，平方根因子对系统进行更新 结果表明，提高了 在未知水下环境对自身定位与动态目标跟踪能力 同步定位地图构建和动态目标跟踪问题描述 问题的概率描述 在水下环境实现同步定位与建图需要明确 个问题：首先是机器人在什么地方，即定位；其次是机器人周围环境是什么，即建图 两者的完成需要提取并组合侧扫声呐获取的未知环境信息和 内部的里程计和 单元获取的自身位姿信息，在移动的同时完成环境地图的构建并且持续的对自身的位置的预测、优化和更新 图 为 系统结构框图F/、IMU30DFF5F+XkH+1kp(X2Tmnkm=X(Xkvkk=vk-1v)Z=k(X)kvk|Xvk,)1mmm图 系统结构框图 运动模型、观测模型为（，）（）（）（）则 数学描述为 （，：，：）（）式中，：表示控制指令；：表示观测数据；表示地图特征向量；表示 的状态向量 接下来求解状态最优估计，分为预测阶段和更新阶段 预测阶段由 的位姿模型和前一时刻的后验概率得到后一时刻的先验概率分布为（，：，：）（，）（，：，：）（）式中，（，）表示为位姿模型 更新阶段根据观测模型得到观测数值 为（，：，：）（，）（，：，：）（）同步定位地图构建和动态目标跟踪问题描述在未知环境中，运动状态下的 识别到目标物并进行跟踪，整个过程需要将 自身位姿、环境地图特征的估计和目标位姿 者建立对应关系 因此要建立 和目标物各自的运动方程和观测方程，运动模型负责预测系统的状态，观测模型则根据地图特征点观测信息和目标物观测信息对系统状态进行更新图 为 动态目标跟踪的系统框图该系统包含运动模型和观测模型 系统运动模型为（，）（）第 期都立立，邢传玺，万志良，等：基于 改进的无人水下航行器动态目标跟踪算法研究式中 表示噪声，（，）表示 运动模型；（，）表示目标物运动模型，表示噪声，表示目标状态转移信息矩阵 系统观测模型为 （）（）式中，（，）表示 观测模型，（，）表示目标物观测模型，表示环境地图特征点的观测值，表示目标的观测值，且 （中包含多个差异地图环境特征信息，存在动态目标的伪观测值），观测噪声矩阵 服从 （，）的正态分布图 动态目标跟踪系统框图因此用概率估计来描述同步定位地图构建与动态目标跟踪问题为（）（，）（）已知控制信息矩阵 和观测信息矩阵 的条件，求解对应时刻地图特征向量、动态目标物状态向量 和 状态向量 的条件概率 式中，为 全部控制信息矩阵，表示控制向量，由环境地图特征信息构成的向量，其中 表示为检测到的第 个环境地图特征点的位置 无人水下航行器动态目标跟踪算法 平方根容积卡尔曼滤波 算法该算法对 算法中计算复杂的部分进行改进，利用 阶容积准则选取 个相同权值的容积点 来近似计算，再利用数值积分法计算非线性方程模型的后验状态估计平均值和方差，并对协方差矩阵的平方根因子进行更新 ，减少了计算量，提高了系统的运行效率和计算结果的精度利用三阶容积准则计算高斯加权积分 （）（；，）（）（）槡 ，（）整个算法分为预测阶段和更新阶段，图 为 算法框图 基于平方根容积卡尔曼滤波 的无人水下航行器动态目标跟踪算法该算法流程图如图 所示，处理过程分为 步：预测、数据关联和更新 预测阶段将误差协方差矩阵分解，计算容积点信息矩阵和先验概率估计，并对当前时刻系统状态和误差协方差矩阵的平方根因子预测 ，最终推算 和目标的位姿信息 数据关联包含 种关系：外部传感器单元获取的信息和 自身传感器单元获取数据 之间关系；自身位姿状态和环境地图特征之间关系；环境地图特征数据之间的关系 同时数据关联阶段解决两方面问题：分别计算出 和目标的观测值，并和其实际观测值比较；筛选云南民族大学学报（自然科学版）第 卷和剔除可能存在的伪观测值 更新阶段是在观测信息通过数据关联环节后计算观测向量后验概率和更新系统状态信息、误差协方差平方根因子 D1111kkaakkkeC 0su0 Q=SC111|11(),1,2,2()jaakkjkjjkkksufjnn=+=+?xCSxx2()1|1|121*|1|1|1|1|1*|1|112()1,2,sunnjkkkkjsuNk kk kk kk kk kTTk kk knnNQR +=+=?sxQ RCCR1()irkkkh=+zsV1|1|1,|11()ikk kjk ki ji jk kkh=+=DCsZD2,|1|1121*,|1|1|1|1|1,*|1121,2,snii jk kk kjsiNiik kk kk kk kk ksiTk knnQR=?zZZZzZzQ RZR*,|1|1|1,1|1|1|1|1|1()()xzTk kk kk kxzzz Tzzkk kk kk kiikk kkkk k=+PCZWPPPssWzzCxsxsMK K 图 算法框图!7=,(MK K 5K DED图 目标跟踪算法流程图根据上述分析可得该算法的简要步骤为：前一时刻到当前时刻为例，乔列斯基分解误差协方差阵 得 （）求解容积点矩阵 和其经过状态方程的容积点、再预测系统状态 （）式中，（，）（）（）（）位姿预测和误差协方差矩阵的平方根因子预测 槡 （）第 期都立立，邢传玺，万志良，等：基于 改进的无人水下航行器动态目标跟踪算法研究 （，）（）在数据关联部分采用传统的 检验法进行数据关联，当 时刻环境地图特征观测数据 ，和目标观测数据 分别满足以下条件时，将观测数据的最小值用于系统的更新 （，（）（），（）（，（）（）（，（）（），（）（，（）（）式中，表示探测到的环境特征；和 表示误差信息矩阵；（）和（）表示观测数据值；（），（）和 （），（）表示观测数据值对应的误差信息矩阵；查 表获取 再进行观测更新 （），（），（）计算卡尔曼增益矩阵 （，），（）槡 ，（），（）槡 ，（）（，）（）更新系统状态信息矩阵、误差协方差平方根因子 （），（）（）（）仿真实验及分析 仿真环境及参数设置仿真环境 使用 处理器，主频，的 计算机，目标初始位置设为 ，循环次数 ，蒙特卡洛次数 ，滤波器个数为 ，系统初始状态的方差和均值为 （，），量测模型设置为 （），（），这里仅说明了在二维平面的参数 速度和控制速度分别为 、，转角误差为 ，轮间距为 仿真环境 使用 处理器，主频，的 计算机，在开源仿真平台上，建立 目标跟踪仿真实验空间，目标从坐标（，）开始，沿确定的轨迹运行，参数设置和仿真环境一相同，在整个仿真实验过程中 在完成同步定位与地图构建的同时对目标保持追踪状态对改进的 算法在以上 种仿真平台中仿真，完成自身定位和对环境信息建图的同时，对动态目标一直保持跟踪状态，同时也和 、这 种算法做了对比，比较这 种算法在定位精度和目标跟踪能力的差异程度 仿真结果及性能分析在 种仿真环境中，应用改进的 算法实现对动态目标跟踪实验，与目标实际路径进行比较、计算均方根误差进一步分析算法的性能，算法仿真结果如图 、所示仿真结果表明，在降低计算复杂度和优化改进得到的 算法对目标物的跟踪轨迹基本吻合，反映出该算法实时性高和对目标物轨迹估计精度很高，但随着维度升高特征点采样数增多，算法估计路径和动态目标实际轨迹偏差会增大云南民族大学学报（自然科学版）第 卷50100150200t/s0100200300400500600700RMSE/m,CSRCKF-SLAM-OTm3 0002 5002 0001 5001 0005000-500-1 5004 5004 000m3 5003 0002 0001 5001 00050002 500,CSRCKF-SLAM-OTm1042.521.515 000m4 0003 0002 0001 000104m0.5 11.52（）路径跟踪（二维）（）路径跟踪（三维）（）位置分析（二维）5010015020002468101214RMSE/(m/s)020406080100t/s400450500550600650700750800850900RMSE/m020406080100510152025303540RMSE(m/s)t/st/s（）位置分析（三维）（）速率分析（二维）（）速率分析（二维）图 仿真环境 跟踪轨迹和均方根误差0100200300400500600700X/m0.511.5Y/m,CSRCKF-SLAM-OT0100200300400500600700X/m12345Y/m,CSRCKF-SLAM-OT0100200300400500600700-0.200.20.40.60.811.2（）跟踪轨迹（）均方根误差分析图 仿真环境 跟踪轨迹和均方根误差 种算法对比分析在仿真平台中，对 、和 种算法分别在二维平面和三维空间中仿真，得到动态目标真实路径和 种算法目标估计路径，如图 、所示由图可以看到实直线轨迹、虚线轨迹、双画线轨迹和点画线轨迹分别表示目标物真实轨迹、和 算法目标估计轨迹 无论在二维平面还是三维空间中，种算法的轨迹从设置的初始位置开始，均朝目标物运动方向进行跟踪，的动态目标跟踪轨迹和实际轨迹的偏差 最大，其次是 的动态目标跟踪轨迹的偏差，而 算法动态目标跟踪轨迹与目标真实轨迹基本吻合，表明了该算法提高了 对动态目标物的跟踪能力（）三种算法跟踪结果（）局部放大图图 种算法动态目标跟踪结果（二维）第 期都立立，邢传玺，万志良，等：基于 改进的无人水下航行器动态目标跟踪算法研究（）三种算法三维跟踪轨迹（）局部放大图图 种算法动态目标跟踪结果（三维）（）定位与跟踪误差分析在仿真平台中，对 、和 种算法分别在二维平面和三维空间中进行目标在 和 方向的误差对比分析，如图 所示（）目标在 方向误差对比（二维）（）目标在 方向误差对比（二维）（）目标在 方向误差对比（三维）（）目标在 方向误差对比（三维）图 在 、方向误差对比由图可以看到无论在二维平面还是三维空间中，在 方向和 方向上均有 算法误差最小，其次是 算法误差，算法误差 最大 表明了采用改进的 算法，提高了 自身定位精度和对动态目标跟踪能力（）均方根误差分析在仿真平台中，对 、和 种算法分别在二维平面和三维空间中，计算位置均方根和速度均方根来评价三种算法的估计效果，如图 所示 （）（）（）槡，（）云南民族大学学报（自然科学版）第 卷 （）（）（）槡，（）式中，（，）为目标物真实位置，（，）为 估计位置；（，）为目标物速度，（，）为 速度，为时间 这里仅表示出在二维平面的公式，三维空间的公式原理同二维由图可以看出无论在二维平面还是三维空间中，位置均方根误差和速度均方根误差 均有 算法均方根误差最小，其次是 算法均方根误差，算法均方根误差最大 表明了采用改进的 算法明显提升了对动态目标估计的精度（）位置均方根误差分析（二维）（）速度均方根误差分析（二维）（）速度均方根误差分析（三维）（）位置均方根误差分析（三维）图 位置、速度的均方根误差对比 结语为解决容积卡尔曼滤波 算法存在的算法实时性不高、计算量大以及对动态目标物跟踪精度较低的问题，提出 的无人水下航行器动态目标跟踪算法 该算法将 、环境地图特征和动态目标物三者构成一个整体，更具体的是对 位姿的估计、环境地图特征的估计和目标位姿的估计这三者互相耦合 建立 与动态目标各自的运动模型和观测模型，得到的信息经过预测阶段、数据关联阶段和更新阶段，并在更新阶段传递系统协方差矩阵的平方根因子 在 种算法跟踪轨迹仿真、定位与跟踪误差分析和均方根误差分析结果中，一致表明采用 算法，减小了计算复杂度，提高了 自身定位精度和动态目标跟踪能力，在水下侦察领域应用意义巨大改进的 算法在随着系统维度的升高，对目标估计轨迹与目标实际轨迹偏差增大的问题，将是今后研究的重点参考文献：张杰，李婧华，胡超 基于容积卡尔曼滤波的卫星导航定位解算方法 中国科学院大学学报，（）：徐魏超，王冠凌，陈孟元 无人机协助下基于 的无线传感器网络节点定位研究 智能系统学报，（）：张敬艳，董凯，孙顺 基于 的多普勒雷达目标跟踪算法 舰船电子工程，（）：第 期都立立，邢传玺，万志良，等：基于 改进的无人水下航行器动态目标跟踪算法研究 刘清慧，高江，邓南明 基于方差平方根 算法在鱼雷跟踪中的应用 鱼雷技术，（）：王宏健，傅桂霞，李娟，等 基于强跟踪 的无人水下航行器 仪器仪表学报，（）：欧阳广帅，周晶 基于卡尔曼滤波的高精度弹道滤波算法研究 电子测量技术，（）：陶明，凌有铸，陈孟元，等 迭代的平方根容积卡尔曼滤波 算法 计算机工程，（）：高伟，张亚，孙骞，等 基于迭代平方根 的 算法 哈尔滨工业大学学报，（）：陈孟元 基于自适应 的序贯式 目标跟踪算法 传感技术学报，（）：李朕阳，郎朗，陈孟元 基于 的移动机器人动态目标跟踪算法 电子测量与仪器学报，（）：，（）：，（）：，（），（）：，（）：，（）：，（），（）：，（）：赵璇，何波，吉德志，等 基于粒子滤波的机器人定位及动态目标跟踪 系统仿真学报，（）：许红，谢文冲，袁华东，等 基于自适应的增广状态 交互式多模型的机动目标跟踪算法 电子与信息学报，（）：张云武，申琼，罗松 卡尔曼滤波在水下载体被动弹道跟踪系统中的应用 舰船科学技术，（）：，（，）：（）（）（），（）（），：；（责任编辑梁志茂）云南民族大学学报（自然科学版）第 卷