[发明专利]一种基于开关卡尔曼滤波器的运动目标跟踪方法

权利要求书

1.一种基于开关卡尔曼滤波器的运动目标跟踪方法，其特征在于，包括如下步骤：

S1，视觉系统采集图像信息，经过模板匹配和相机投影变换算法获得目标在机器人坐标系下的位置；

S2，初始化卡尔曼滤波器的状态转移矩阵、测量矩阵、误差协方差矩阵、状态迭代误差协方差矩阵；

S3，使用运动监视器监测目标的运动速度是否产生突变，若否，则使用卡尔曼滤波器估计得到目标的运动速度；若是，则先重置卡尔曼滤波器的目标状态向量，再使用重置后的卡尔曼滤波器估计得到目标的运动速度；

S4，根据期望图像特征与当前图像特征的偏差计算得到控制偏差并输入到视觉伺服控制器中，获得机器人的运动速度，并与卡尔曼滤波器估计得到的目标的运动速度相加作为机器人最终的运动速度，实现实时动态跟踪；

所述步骤S2包括：

考虑对平面目标进行跟踪，在参考坐标系为机器人基坐标系下，保持目标位置和机器人位置不变，使用相机测量目标重心在机器人基坐标系下的位置，总共采集N个数据，

求误差协方差矩阵：

R_k即为卡尔曼滤波器的误差协方差矩阵；

P_1,0即为卡尔曼滤波器的状态迭代误差协方差矩阵；

状态转移矩阵A与测量矩阵H为：

其中t为视觉系统采集和处理一张图片所需的时间；

所述步骤S3中，所述使用运动监视器监测目标的运动速度是否产生突变包括：

在k时刻，运动预测检测器的判别条件为：

|Δx_p(k)-μ(k)|＜δ(k)

若条件为真，则表示检测到了不光滑运动，即目标的运动产生突变，此时重置卡尔曼滤波器；

其中Δx_p(k)是卡尔曼滤波器的m步预测误差：

z(k)为实际的测量值，是由k-m时刻对k时刻的m步预测值δ(k)为误差带宽，μ(k)为误差带中心；

所述误差带宽通过最近n次预测误差的标准偏差来定义：

E{Δx_p}为最近n次预测误差的均值；

则误差带宽定义为标准偏差的倍数：

δ(k)＝λσ_n(k)

其中λ为一个常数；

所述误差带中心定义为最近n次预测误差的均值：

μ(k)＝E{Δx_p}。