CN109003292B - 一种基于开关卡尔曼滤波器的运动目标跟踪方法 - Google Patents

一种基于开关卡尔曼滤波器的运动目标跟踪方法 Download PDF

Info

Publication number
CN109003292B
CN109003292B CN201810661518.8A CN201810661518A CN109003292B CN 109003292 B CN109003292 B CN 109003292B CN 201810661518 A CN201810661518 A CN 201810661518A CN 109003292 B CN109003292 B CN 109003292B
Authority
CN
China
Prior art keywords
target
kalman filter
error
robot
motion
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201810661518.8A
Other languages
English (en)
Other versions
CN109003292A (zh
Inventor
李琳
林燕龙
邹焱飚
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN201810661518.8A priority Critical patent/CN109003292B/zh
Publication of CN109003292A publication Critical patent/CN109003292A/zh
Application granted granted Critical
Publication of CN109003292B publication Critical patent/CN109003292B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters

Landscapes

  • Engineering & Computer Science (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Image Analysis (AREA)
  • Feedback Control In General (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

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

Description

一种基于开关卡尔曼滤波器的运动目标跟踪方法
技术领域
本发明属于机器人控制领域，主要是基于视觉的运动目标跟踪，具体涉及一种基于开关卡尔曼滤波器的运动目标跟踪方法。
背景技术
运动目标实时跟踪技术在机器人生产线、服务机器人领域有着广泛的应用需求。但由于运动目标的运动状态未知，运动状态可能产生突变等原因，跟踪误差较大，难得达到实际应用的要求，因此开发一种鲁棒性、高效性的、能够抑制因为目标的运动状态产生突变导致跟踪误差扩大的跟踪算法具有重大意义。
发明内容
本发明的目的在于克服现有技术的不足，满足现有的需求，提供一种基于开关卡尔曼滤波器的运动目标跟踪方法。该方法利用卡尔曼滤波器对目标的运动状态进行估计，再结合运动监视器对目标运动状态是否产生突变进行监控，在快速运动情况和运动状态产生突变的情况下对目标进行跟踪。
为了实现上述目的，本发明提供一种基于开关卡尔曼滤波器的运动目标跟踪方法，包括如下步骤：
S1，视觉系统采集图像信息，经过模板匹配和相机投影变换算法获得目标在机器人坐标系下的位置；
S2，初始化卡尔曼滤波器的状态转移矩阵、测量矩阵、误差协方差矩阵、状态迭代误差协方差矩阵；
S3，使用运动监视器监测目标的运动速度是否产生突变，若否，则使用卡尔曼滤波器估计得到目标的运动速度；若是，则先重置卡尔曼滤波器的目标状态向量，再使用重置后的卡尔曼滤波器估计得到目标的运动速度；
S4，把步骤S3中估计得到的目标运动速度前馈到机器人视觉伺服控制器中，实现实时动态跟踪。
进一步地，所述步骤S2包括：
考虑对平面目标进行跟踪，在参考坐标系为机器人基坐标系下，保持目标位置和机器人位置不变，使用相机测量目标重心在机器人基坐标系下的位置，总共采集N个数据，
求误差协方差矩阵：
Figure 63257DEST_PATH_IMAGE001
Figure 799132DEST_PATH_IMAGE002
即为卡尔曼滤波器的误差协方差矩阵；
Figure 325928DEST_PATH_IMAGE003
Figure 181758DEST_PATH_IMAGE004
即为卡尔曼滤波器的状态迭代误差协方差矩阵；
所述状态转移矩阵
Figure 909542DEST_PATH_IMAGE005
与测量矩阵
Figure 991768DEST_PATH_IMAGE006
为：
Figure 181309DEST_PATH_IMAGE007
其中t为视觉系统采集和处理一张图片所需的时间。
进一步地，所述步骤S3中，所述使用运动监视器监测目标的运动速度是否产生突变包括：
在
Figure 173536DEST_PATH_IMAGE008
时刻，运动预测检测器的判别条件为：
Figure 931277DEST_PATH_IMAGE009
若条件为真，则表示检测到了不光滑运动，即目标的运动产生突变，此时重置卡尔曼滤波器；
其中
Figure 359853DEST_PATH_IMAGE010
是卡尔曼滤波器的m步预测误差：
Figure 103818DEST_PATH_IMAGE011
Figure 199819DEST_PATH_IMAGE012
为实际的测量值，
Figure 128461DEST_PATH_IMAGE013
是由k-m 时刻对
Figure 591989DEST_PATH_IMAGE008
时刻的m步预测值；
Figure 417862DEST_PATH_IMAGE015
为误差带宽，
Figure 127192DEST_PATH_IMAGE016
为误差带中心；
所述误差带宽通过最近n次预测误差的标准偏差来定义：
Figure 84957DEST_PATH_IMAGE017
Figure 295358DEST_PATH_IMAGE018
为最近n次预测误差的均值；
则误差带宽定义为标准差的倍数：
Figure 851104DEST_PATH_IMAGE019
其中
Figure 246183DEST_PATH_IMAGE021
为一个常数；
所述误差带中心定义为最近n次预测误差的均值：
Figure 277592DEST_PATH_IMAGE022
。
进一步地，所述步骤S3中，
系统状态迭代方程为：
Figure 901472DEST_PATH_IMAGE023
测量过程为：
Figure 826571DEST_PATH_IMAGE024
随机变量
Figure 877704DEST_PATH_IMAGE025
分别为状态迭代误差与测量误差，并且相互独立，服从零均值高斯分布：
Figure 396410DEST_PATH_IMAGE026
Figure 73248DEST_PATH_IMAGE027
矩阵
Figure 603586DEST_PATH_IMAGE028
为在存在控制向量和迭代误差的情况下联系状态
Figure 481413DEST_PATH_IMAGE029
和下一个状态
Figure 346469DEST_PATH_IMAGE030
的状态转移矩阵，向量
Figure 312151DEST_PATH_IMAGE031
为控制输入变量，
Figure 821630DEST_PATH_IMAGE032
的矩阵
Figure 994991DEST_PATH_IMAGE033
为控制矩阵，
Figure 98076DEST_PATH_IMAGE034
矩阵
Figure 992083DEST_PATH_IMAGE035
为测量矩阵；
卡尔曼滤波器的方程可以分成两部分，一部分为时间更新，另一部分为测量校正，
时间更新方程：
Figure 746281DEST_PATH_IMAGE036
测量校正过程：
Figure 575697DEST_PATH_IMAGE037
设在机器人基坐标系下，目标的重心坐标为
Figure 290712DEST_PATH_IMAGE038
，在跟踪过程中，Z保持不变；假设采样周期足够短，目标状态在采样周期内的加速度看做是恒定不变的，其状态可设为：
Figure 113044DEST_PATH_IMAGE039
状态空间方程为：
Figure 472481DEST_PATH_IMAGE040
其中，
Figure 597432DEST_PATH_IMAGE041
是直接影响系统状态的随机噪声，并且假设
Figure 658797DEST_PATH_IMAGE041
的元素具有高斯分布
Figure 301131DEST_PATH_IMAGE042
的协方差矩阵，
Figure 639709DEST_PATH_IMAGE041
可以在前期测量计算得到。
Figure 60195DEST_PATH_IMAGE043
观测值为目标的笛卡尔坐标：
Figure 359589DEST_PATH_IMAGE044
其中
Figure 399089DEST_PATH_IMAGE045
为测量误差，并且假设
Figure 982386DEST_PATH_IMAGE046
的元素具有高斯分布
Figure 324506DEST_PATH_IMAGE047
，
Figure 360464DEST_PATH_IMAGE048
为
Figure 203655DEST_PATH_IMAGE049
的协方差矩阵；
测量矩阵为：
Figure 392191DEST_PATH_IMAGE050
。
进一步地，所述步骤S4中，
考虑对一个运动目标进行跟踪，并假设期望的特征向量为一个常向量（图像平面上），此时误差的时间导数为：
Figure 154479DEST_PATH_IMAGE051
其中，
Figure 818679DEST_PATH_IMAGE052
为图像雅各比矩阵，
Figure 75348DEST_PATH_IMAGE053
为相机速度矢量，
Figure 367658DEST_PATH_IMAGE054
表示由目标运动引起的误差导数项；
设计一个控制率使得误差以指数方式收敛到0：
Figure 176214DEST_PATH_IMAGE055
所以：
Figure 937496DEST_PATH_IMAGE056
其中
Figure 247124DEST_PATH_IMAGE057
为
Figure 269306DEST_PATH_IMAGE058
的估计，这一项的引入是为了补偿目标运动产生的误差；
其中
Figure 389709DEST_PATH_IMAGE059
为笛卡尔空间中目标的运动速度，因此可以直接在笛卡尔空间对目标的运动状态进行预测，将预测得到的目标速度
Figure 621976DEST_PATH_IMAGE059
作为一个前馈量对控制器进行补偿。
相比现有技术，本发明在目标运动状态产生突变的情况下，能够抑制因目标运动状态产生突变导致的跟踪误差，具有稳定的跟踪效果，具有较好的鲁棒性。
附图说明
图1是基于开关卡尔曼滤波器的运动目标跟踪算法框图。
图2是机器人运动目标跟踪流程图。
图3是跟踪误差曲线图，其中图3a)为运动目标X方向轨迹变化图，3b)为运动目标Y方向轨迹变化图，3c)为机器人末端X方向跟踪误差曲线，3d)为机器人末端Y方向跟踪误差曲线。
具体实施方式
为了更好地理解本发明，下面结合附图对本发明作进一步地描述：
本发明基于开关卡尔曼滤波器的运动目标跟踪方法，图1是本发明的算法框图,具体实施步骤如下：
步骤1，视觉系统采集图像信息，经过模板匹配和相机投影变换算法获得目标在机器人坐标系下的位置；
步骤2，初始化卡尔曼滤波器的状态转移矩阵、测量矩阵、误差协方差矩阵、状态迭代误差协方差矩阵；
步骤3，卡尔曼滤波器对目标的运动速度进行估计，若运动监视器检测到目标的运动状态产生突变，向卡尔曼滤波器发出开关信号，重置卡尔曼滤波器，否则，卡尔曼滤波器把估计得到的速度前馈到视觉伺服控制器。
步骤4，根据期望图像特征与当前图像特征的偏差计算得到控制偏差并输入到视觉伺服控制器中，获得机器人的运动速度，并与卡尔曼滤波器估计得到的目标的运动速度相加作为机器人最终的运动速度，实现实时动态跟踪。
负反馈调节用于镇定系统，使跟踪误差收敛。
图2为采用本发明的完整的机器人运动目标跟踪算法的流程图，具体实施步骤如下：
步骤1：视觉系统采集图像信息，经过模板匹配和相机投影变换算法获得目标在机器人坐标系下的位置。
步骤2：卡尔曼滤波器对目标的运动速度进行估计，若运动监视器检测到目标的运动状态产生突变，像卡尔曼滤波器发出开关信号，重置卡尔曼滤波器，否则，卡尔曼滤波器把估计得到的速度前馈到视觉伺服控制器。
步骤3，根据期望图像特征与当前图像特征的偏差计算得到控制偏差。输入到视觉伺服控制器中，获得机器人的运动速度，与卡尔曼滤波器估计得到的速度相加作为机器人最终的运动速度。
步骤4，积分得到机器人需要运动的矢量距离，与当前机器人末端的位置相加得到机器人运动的下一个笛卡尔空间点。
步骤5，根据机器人逆运动学得到机器人各个关节位置，在机器人关节空间做三次样图插补，得到电机在机器人每一个插补周期的输入值。
图3a)-3d)为算法跟踪误差曲线。使用基于运动监视器的开关卡尔曼滤波器的运动目标跟踪算法对目标进行跟踪，在目标运动没有产生突变的情况下，运动监视器没有发出开关信号，跟踪误差先增加后减少到0.5mm以内。在目标突然静止时，由于目标的突然静止，先前假设的运动模型失效，状态迭代会产生一个误差很大的值，这会使得卡尔曼滤波器估计得到的目标运动状态具有很大的误差。在目标突然静止之后的短时间内，机器人定位误差会迅速扩大，同时，卡尔曼滤波器的两步预测值与当前状态的测量值的误差很大，运动监视器会监测到目标运动状态的突变，产生开关信号重置卡尔曼滤波器，在这之后，跟踪误差又快速以指数方式收敛到0.03mm内。
本发明是一种实时有效的运动目标跟踪方法，通过卡尔曼滤波器对目标状态进行估计得到目标的运动速度，前馈到视觉伺服控制器中，减少跟踪过程的误差；运动监视器根据卡尔曼滤波器的预测偏差来判断目标的运动状态是否产生突变，若目标运动状态产生突变，发出开关信号重置卡尔曼滤波器，抑制因为目标运动状态产生突变导致的跟踪误差。
本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (1)

1.一种基于开关卡尔曼滤波器的运动目标跟踪方法，其特征在于，包括如下步骤：
S1，视觉系统采集图像信息，经过模板匹配和相机投影变换算法获得目标在机器人坐标系下的位置；
S2，初始化卡尔曼滤波器的状态转移矩阵、测量矩阵、误差协方差矩阵、状态迭代误差协方差矩阵；
S3，使用运动监视器监测目标的运动速度是否产生突变，若否，则使用卡尔曼滤波器估计得到目标的运动速度；若是，则先重置卡尔曼滤波器的目标状态向量，再使用重置后的卡尔曼滤波器估计得到目标的运动速度；
S4，根据期望图像特征与当前图像特征的偏差计算得到控制偏差并输入到视觉伺服控制器中，获得机器人的运动速度，并与卡尔曼滤波器估计得到的目标的运动速度相加作为机器人最终的运动速度，实现实时动态跟踪；
所述步骤S2包括：
考虑对平面目标进行跟踪，在参考坐标系为机器人基坐标系下，保持目标位置和机器人位置不变，使用相机测量目标重心在机器人基坐标系下的位置，总共采集N个数据，
求误差协方差矩阵：
Figure FDA0003335195380000011
Rk即为卡尔曼滤波器的误差协方差矩阵；
Figure FDA0003335195380000012
P1,0即为卡尔曼滤波器的状态迭代误差协方差矩阵；
状态转移矩阵A与测量矩阵H为：
Figure FDA0003335195380000021
Figure FDA0003335195380000022
其中t为视觉系统采集和处理一张图片所需的时间；
所述步骤S3中，所述使用运动监视器监测目标的运动速度是否产生突变包括：
在k时刻，运动预测检测器的判别条件为：
|Δxp(k)-μ(k)|＜δ(k)
若条件为真，则表示检测到了不光滑运动，即目标的运动产生突变，此时重置卡尔曼滤波器；
其中Δxp(k)是卡尔曼滤波器的m步预测误差：
Figure FDA0003335195380000023
z(k)为实际的测量值，
Figure FDA0003335195380000024
是由k-m时刻对k时刻的m步预测值δ(k)为误差带宽，μ(k)为误差带中心；
所述误差带宽通过最近n次预测误差的标准偏差来定义：
Figure FDA0003335195380000025
E{Δxp}为最近n次预测误差的均值；
则误差带宽定义为标准偏差的倍数：
δ(k)＝λσn(k)
其中λ为一个常数；
所述误差带中心定义为最近n次预测误差的均值：
μ(k)＝E{Δxp}。
CN201810661518.8A 2018-06-25 2018-06-25 一种基于开关卡尔曼滤波器的运动目标跟踪方法 Active CN109003292B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810661518.8A CN109003292B (zh) 2018-06-25 2018-06-25 一种基于开关卡尔曼滤波器的运动目标跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810661518.8A CN109003292B (zh) 2018-06-25 2018-06-25 一种基于开关卡尔曼滤波器的运动目标跟踪方法

Publications (2)

Publication Number Publication Date
CN109003292A CN109003292A (zh) 2018-12-14
CN109003292B true CN109003292B (zh) 2022-01-18

Family

ID=64601714

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810661518.8A Active CN109003292B (zh) 2018-06-25 2018-06-25 一种基于开关卡尔曼滤波器的运动目标跟踪方法

Country Status (1)

Country Link
CN (1) CN109003292B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109886323B (zh) * 2019-01-31 2020-10-30 中国地质大学（武汉） 一种用于测试运动轨迹滤波算法的装置
CN109623162B (zh) * 2019-01-31 2020-02-14 中国地质大学（武汉） 一种基于视觉伺服与卡尔曼滤波补偿的二维激光雕刻机
CN110876275A (zh) * 2019-04-30 2020-03-10 深圳市大疆创新科技有限公司 一种瞄准控制方法、移动机器人及计算机可读存储介质
CN110007601B (zh) * 2019-05-06 2022-03-25 广东工业大学 一种双边遥操作系统的控制装置和设备
CN110792430B (zh) * 2019-11-20 2021-08-17 中国地质大学（北京） 一种基于多传感器数据融合的随钻测斜方法及装置
CN111289967A (zh) * 2020-03-31 2020-06-16 四川长虹电器股份有限公司 基于毫米波雷达的人员检测跟踪与计数算法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102721951A (zh) * 2012-05-04 2012-10-10 西安电子科技大学 一种高机动目标跟踪方法
CN103034249A (zh) * 2012-12-27 2013-04-10 中国电子科技集团公司第二十六研究所 一种基于模糊决策的双轴光电转台复合控制系统和控制方法
CN108038868A (zh) * 2017-10-17 2018-05-15 国网河南省电力公司郑州供电公司 基于三维数字模型的变电站复杂环境跨视场目标跟踪方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102721951A (zh) * 2012-05-04 2012-10-10 西安电子科技大学 一种高机动目标跟踪方法
CN103034249A (zh) * 2012-12-27 2013-04-10 中国电子科技集团公司第二十六研究所 一种基于模糊决策的双轴光电转台复合控制系统和控制方法
CN108038868A (zh) * 2017-10-17 2018-05-15 国网河南省电力公司郑州供电公司 基于三维数字模型的变电站复杂环境跨视场目标跟踪方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
《Visual servo control II: Advanced approaches》;Francois Chaumette; Seth Hutchinson;《IEEE Robotics & Automation Magazine,2007》;20070402;第14卷(第1期);第109-118页 *
《光学测试转台若干控制算法研究》;杨天锡;《中国优秀硕士学位论文全文数据库 信息科技辑,2009年第03期,I140-501》;20090315;第50-51页 *
《基于双目视觉的智能车辆路径识别方法研究》;季聪;《中国优秀硕士学位论文全文数据库 信息科技辑,2014年第02期,I138-755》;20140215;第18,22页 *

Also Published As

Publication number Publication date
CN109003292A (zh) 2018-12-14

Similar Documents

Publication Publication Date Title
CN109003292B (zh) 一种基于开关卡尔曼滤波器的运动目标跟踪方法
CN110675435B (zh) 基于卡尔曼滤波和χ2检测平滑处理的车辆轨迹跟踪方法
CN107627303B (zh) 一种基于眼在手上结构的视觉伺服系统的pd-smc控制方法
JP4967062B2 (ja) オプティカルフロー、運動学及び深さ情報を使用して、物体の適切な運動を推定する方法
CN111178385B (zh) 一种鲁棒在线多传感器融合的目标跟踪方法
CN108255791B (zh) 基于分布式传感器一致性的机动目标跟踪方法
Assa et al. A robust vision-based sensor fusion approach for real-time pose estimation
CN110948504B (zh) 机器人加工作业法向恒力跟踪方法和装置
CN107229037B (zh) 移动平台传感器量测数据扩维空间配准方法
CN109352655B (zh) 一种基于多输出高斯过程回归的机器人变形补偿方法
Shen et al. Leader-follower formation control without leader’s velocity information
CN111890373A (zh) 车载机械臂的感知定位方法
CN107945166B (zh) 基于双目视觉的待测物体三维振动轨迹的测量方法
CN113442171A (zh) 一种机器人动态稳定判别方法和动态自适应姿态控制方法
Jayakumar et al. Risk-sensitive filters for recursive estimation of motion from images
US20100246893A1 (en) Method and Apparatus for Nonlinear Dynamic Estimation of Feature Depth Using Calibrated Moving Cameras
CN112034713A (zh) 非理想网络环境下移动目标的最优状态估计方法及系统
CN115446836B (zh) 一种基于多种图像特征信息混合的视觉伺服方法
Norizuki et al. Contact prediction control for a teleoperation system with time delay
CN110614636A (zh) 一种视觉伺服机器人测量时滞补偿方法
CN114034290B (zh) 放样机器人系统的放样方法
KR102406240B1 (ko) 강인한 스테레오 비주얼 관성 항법 장치 및 방법
CN113534164B (zh) 一种基于主被动联合声纳阵列的目标路径跟踪方法
CN110649911B (zh) 一种基于α散度的分布式非线性卡尔曼滤波方法
Martínez-Rey et al. Smart laser scanner for event-based state estimation applied to indoor positioning

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant