一种基于CKF-SLAM改进的无人水下航行器动态目标跟踪的方法技术

本发明专利技术涉及一种基于CKF

【技术实现步骤摘要】
一种基于CKF
‑
SLAM改进的无人水下航行器动态目标跟踪的方法


[0001]本专利技术涉及水下动态目标跟踪
，尤其涉及一种基于CKF
‑
SLAM改进的无人水下航行器动态目标跟踪的方法。

技术介绍

[0002]目前在动态目标跟踪(Object Tracking,OT)的问题上，主要集中在基于卡尔曼滤波类方法。扩展卡尔曼滤波(Extended Kalman Filter,EKF)SLAM算法，是一种最小均方差估计方法，将非线性系统利用泰勒级数一阶展开线性化，再进行线性卡尔曼滤波；无迹卡尔曼滤波(Unscented Kalman Filter,UKF)SLAM算法，该算法利用高斯随机变量来近似表达状态分布，用一组确定性采样得到的Sigma点来逼近，提高变量的平均值和方差，精度逼近二阶泰勒；容积卡尔曼滤波(Cubature Kalman Filter,CKF)SLAM算法，该算法是在三阶球面
‑
容积变换规则下，提取容积样本点集，用来描述后验状态变量的统计特征，并由先验状态估计平均值和方差。
[0003]EKF
‑
SLAM算法简单易于实现并且在未知环境特征点较少时，UUV实际轨迹可以逼近理想的轨迹，但当未知环境中的特征点增加时，UUV实际轨迹和理想轨迹偏差较大，因此该算法只适合应用在环境特征点少的小区域环境；UKF
‑
SLAM算法在精度上有所提高，计算复杂度上因无须计算雅克比矩阵，因此更容易实现，但同样不适用于庞大复杂的环境；CKF
‑
SLAM算法的容积样本点集的权重比非负，保证在计算高斯积分时滤波性能和数值稳定，但该算法存在实时性不高、计算量大以及在面对动态目标物跟踪精度较低的问题。

技术实现思路

[0004](一)要解决的技术问题
[0005]本专利技术要解决针对容积卡尔曼滤波同步定位与建图算法在水下动态目标跟踪的应用中，存在算法实时性不高、计算复杂以及对动态目标物跟踪精度较低的技术问题。
[0006](二)技术方案
[0007]为解决上述技术问题，本专利技术提供了一种基于CKF
‑
SLAM改进的无人水下航行器动态目标跟踪的方法，包括步骤：
[0008]步骤一：在未知环境中，运动状态下的UUV识别到目标物并进行跟踪，将UUV自身位姿、环境地图特征的估计和目标位姿三者建立对应关系，并建立UUV和目标物的运动方程和观测方程，将SLAM技术和对动态目标跟踪相融合；
[0009]步骤二：对CKF算法利用三阶容积准则选取m个相同权值的容积点进行近似计算，再利用数值积分法计算非线性方程模型的后验状态估计平均值和方差，并对协方差矩阵的平方根因子进行更新；
[0010]步骤三：通过所述运动方程预测系统的状态，所述观测方程根据地图特征点观测信息和目标物观测信息对系统状态进行更新。
[0011]进一步地，所述运动方程为：
[0012][0013]式中表示噪声，表示UUV运动模型；表示目标物运动模型，表示噪声，表示目标状态转移信息矩阵。
[0014]进一步地，所述观测方程为：
[0015]Z
k
＝H(X
k
)+δ
k
；
[0016]式中，表示UUV观测模型，表示目标物观测模型，表示环境地图特征点的观测值，表示目标的观测值，且表示目标的观测值，且中包含多个差异地图环境特征信息，存在动态目标的伪观测值，观测噪声矩阵δ
k
服从N(0,R
k
)的正态分布。
[0017]进一步地，用概率估计来描述同步定位地图构建与动态目标跟踪问题为：
[0018][0019][0020]已知控制信息矩阵μ
k
和观测信息矩阵Z
k
的条件，求解对应时刻地图特征向量M
k
、动态目标物状态向量和UUV状态向量的条件概率。式中，μ
k
＝[μ
1 μ2ꢀ…ꢀ
μ
k
]为UUV全部控制信息矩阵，μ
k
表示控制向量,由环境地图特征信息构成的向量，其中表示为检测到的第i个环境地图特征点的位置。
[0021]进一步地，利用三阶容积准则计算高斯加权积分：
[0022][0023][0024]算法分为预测阶段和更新阶段。
[0025]进一步地，步骤三具体包括：
[0026]预测阶段将误差协方差矩阵分解，计算容积点信息矩阵和先验概率估计，并对当前时刻系统状态和误差协方差矩阵的平方根因子预测，推算UUV和目标的位姿信息；
[0027]数据关联包含三种关系：外部传感器单元获取的信息和UUV自身传感器单元获取数据之间关系；UUV自身位姿状态和环境地图特征之间关系；环境地图特征数据之间的关系；数据关联阶段分别计算出UUV和目标的观测值，并和其实际观测值比较；筛选和剔除可能存在的伪观测值；
[0028]更新阶段是在观测信息通过数据关联环节后计算观测向量后验概率和更新系统状态信息、误差协方差平方根因子。
[0029]进一步地，步骤三包括步骤：
[0030]S1:前一时刻到当前时刻，乔列斯基分解误差协方差阵P
k
‑1得
[0031][0032]S2:求解容积点矩阵和其经过状态方程的容积点、再预测系统状态：
[0033][0034]式中，(i＝1,2,
…
,m,m＝2n)；
[0035][0036][0037]S3:UUV位姿预测和误差协方差矩阵的平方根因子预测：
[0038][0039][0040]S4:在数据关联部分采用传统的χ2检验法进行数据关联，当k时刻环境地图特征观测数据和目标观测数据分别满足以下条件时，将观测数据的最小值用于系统的更新：
[0041][0042][0043]式中，m
i
表示探测到的环境特征；R
m
和R
t
表示误差信息矩阵；和Z
t
(k|k
‑
1)表示观测数据值；和C(Z
t
(k|k
‑
1),Z
t
(k|k
‑
1))表示观测数据值对应的误差信息矩阵；查χ2表获取γ；
[0044]S5:进行观测更新：
[0045][0046][0047]S6:计算卡尔曼增益矩阵W
k
：
[0048][0049][0050][0051][0052][0053]S7:更新系统状态信息矩阵、误差协方差平方根因子：
[0054][0055][0056](三)有益效果
[0057]本专利技术的上述技术方案具有如下优点：
[0058]1、对CKF
‑
SLAM算法进行改进，提出基于平方根容积卡尔曼滤波SLAM的无人水下本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于CKF
‑
SLAM改进的无人水下航行器动态目标跟踪的方法，其特征在于，包括步骤：步骤一：在未知环境中，运动状态下的UUV识别到目标物并进行跟踪，将UUV自身位姿、环境地图特征的估计和目标位姿三者建立对应关系，并建立UUV和目标物的运动方程和观测方程，将SLAM技术和对动态目标跟踪相融合；步骤二：对CKF算法利用三阶容积准则选取m个相同权值的容积点进行近似计算，再利用数值积分法计算非线性方程模型的后验状态估计平均值和方差，并对协方差矩阵的平方根因子进行更新；步骤三：通过所述运动方程预测系统的状态，所述观测方程根据地图特征点观测信息和目标物观测信息对系统状态进行更新。2.根据权利要求1所述的一种基于CKF
‑
SLAM改进的无人水下航行器动态目标跟踪的方法，其特征在于，所述运动方程为：式中表示噪声，表示UUV运动模型；表示目标物运动模型，表示噪声，表示目标状态转移信息矩阵。3.根据权利要求2所述的一种基于CKF
‑
SLAM改进的无人水下航行器动态目标跟踪的方法，其特征在于，所述观测方程为：Z
k
＝H(X
k
)+δ
k
；式中，表示UUV观测模型，表示目标物观测模型，表示环境地图特征点的观测值，表示目标的观测值，且表示目标的观测值，且中包含多个差异地图环境特征信息，存在动态目标的伪观测值，观测噪声矩阵δ
k
服从N(0,R
k
)的正态分布。4.根据权利要求3所述的一种基于CKF
‑
SLAM改进的无人水下航行器动态目标跟踪的方法，其特征在于，用概率估计来描述同步定位地图构建与动态目标跟踪问题为：法，其特征在于，用概率估计来描述同步定位地图构建与动态目标跟踪问题为：已知控制信息矩阵μ
k
和观测信息矩阵Z
k
的条件，求解对应时刻地图特征向量M
k
、动态目标物状态向量和UUV状态向量的条件概率。式中，μ
k
＝[μ1μ2…
μ
k
]为UUV全部控制信息矩阵，μ
k
表示控制向量,由环境地图特征信息构成的向量，其中表示为检测到的第i个环境地图特征点的位置。5.根据权利要求1所述的一种基于C...

【专利技术属性】
技术研发人员：邢传玺，都立立，
申请(专利权)人：云南民族大学，
类型：发明
国别省市：