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

说明书

本发明公开了一种基于开关卡尔曼滤波器的运动目标跟踪方法，包括步骤：S1，视觉系统采集图像信息，获得目标在机器人坐标系下的位置；S2，初始化卡尔曼滤波器的状态转移矩阵、测量矩阵、误差协方差矩阵、状态迭代误差协方差矩阵；S3，使用运动监视器监测目标的运动速度是否产生突变，若否，则使用卡尔曼滤波器估计得到目标的运动速度；若是，则先重置卡尔曼滤波器的目标状态向量，再使用重置的卡尔曼滤波器估计得到目标的运动速度；S4，把估计得到的目标运动速度前馈到机器人视觉伺服控制器中，实现实时动态跟踪。本发明在目标运动状态产生突变的情况下，能够抑制因目标运动状态产生突变导致的跟踪误差，具有稳定的跟踪效果和较好的鲁棒性。

技术领域

本发明属于机器人控制领域，主要是基于视觉的运动目标跟踪，具体涉及一种基于开关卡尔曼滤波器的运动目标跟踪方法。

背景技术

运动目标实时跟踪技术在机器人生产线、服务机器人领域有着广泛的应用需求。但由于运动目标的运动状态未知，运动状态可能产生突变等原因，跟踪误差较大，难得达到实际应用的要求，因此开发一种鲁棒性、高效性的、能够抑制因为目标的运动状态产生突变导致跟踪误差扩大的跟踪算法具有重大意义。

发明内容

本发明的目的在于克服现有技术的不足，满足现有的需求，提供一种基于开关卡尔曼滤波器的运动目标跟踪方法。该方法利用卡尔曼滤波器对目标的运动状态进行估计，再结合运动监视器对目标运动状态是否产生突变进行监控，在快速运动情况和运动状态产生突变的情况下对目标进行跟踪。

为了实现上述目的，本发明提供一种基于开关卡尔曼滤波器的运动目标跟踪方法，包括如下步骤：

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

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

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

S4，把步骤S3中估计得到的目标运动速度前馈到机器人视觉伺服控制器中，实现实时动态跟踪。

进一步地，所述步骤S2包括：

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

求误差协方差矩阵：

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

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

所述状态转移矩阵与测量矩阵为：

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

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

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

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

其中是卡尔曼滤波器的m步预测误差：

为实际的测量值，是由k-m 时刻对时刻的m步预测值；

为误差带宽，为误差带中心；