CN108680162B - 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法 - Google Patents

一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法 Download PDF

Info

Publication number
CN108680162B
CN108680162B CN201810359381.0A CN201810359381A CN108680162B CN 108680162 B CN108680162 B CN 108680162B CN 201810359381 A CN201810359381 A CN 201810359381A CN 108680162 B CN108680162 B CN 108680162B
Authority
CN
China
Prior art keywords
covariance
target
measurement
human body
robot
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
CN201810359381.0A
Other languages
English (en)
Other versions
CN108680162A (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.)
Zhejiang University of Technology ZJUT
Original Assignee
Zhejiang University of Technology ZJUT
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 Zhejiang University of Technology ZJUT filed Critical Zhejiang University of Technology ZJUT
Priority to CN201810359381.0A priority Critical patent/CN108680162B/zh
Publication of CN108680162A publication Critical patent/CN108680162A/zh
Application granted granted Critical
Publication of CN108680162B publication Critical patent/CN108680162B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/66—Tracking systems using electromagnetic waves other than radio waves

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Manipulator (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Feedback Control In General (AREA)

Abstract

一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法，该方法针对线性化误差过大引起的系统跟踪性能下降问题，在量测更新过程中，通过渐进地引入当前量测信息以减小线性化误差，从而有效地提高人体目标的跟踪精度。其次，在量测渐进过程中引入判定条件，进一步地补偿系统中存在的线性化误差。相比于现有的目标跟踪方法，该方法充分考虑线性化误差对系统的影响，在保证计算复杂度的前提下，提高了对人体目标的跟踪精度。

Description

一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法
技术领域
本发明属于移动机器人领域，尤其是一种用于跟随机器人的人体目标跟踪方法。
背景技术
随着社会生产和生活要求的日益提高，移动机器人产业发展非常迅速并逐步渗透于社会服务、医疗健康以及资源探测等领域。在未知复杂的动态环境中，实现对服务对象的识别与跟踪是提高移动机器人环境适应能力的主要途径之一，同时也是其通过人机协作完成复杂指定任务的前提和基础。
为实现移动机器人对人体目标的有效检测与跟踪，需要设计一个稳定的、性能良好的人体目标跟踪估计器。考虑到人体目标跟踪过程中存在的非线性滤波问题，一些常用的非线性滤波方法有扩展卡尔曼滤波方法、无迹卡尔曼滤波方法以及容积卡尔曼滤波方法等。然而，上述非线性估计器的拟合精度十分有限，当系统具有强非线性时，会引入较大的线性化误差，从而破坏目标跟踪估计器的稳定性与收敛性。特别地，受环境变化、传感器性能等因素的影响，在人体目标跟踪过程中可能会存在目标遮挡、丢失等情况。这将引起某一采样时刻的量测信息缺失，使得系统的线性化误差增大，从而导致机器人无法准确地跟踪服务对象。为了减小线性化误差对系统的影响，可采用迭代卡尔曼滤波方法或高斯渐进滤波方法对系统状态进行估计。此类方法通过对其中的量测更新过程进行多次迭代运算，可一定程度地提高系统对人体目标的跟踪精度。然而，现有的迭代方法没有充分考虑到线性化误差的补偿问题，易导致量测迭代次数过多，进而产生过自信的估计结果。目前，尚未有人体目标跟踪方法能够克服上述因素的影响，使得跟随机器人能可靠、稳定地实现人体跟随功能。
发明内容
为了克服现有人体目标跟踪方法的精度较低、稳定性较差的不足，本发明提供了一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法，该方法在保证计算复杂度的前提下，考虑线性化误差对系统的影响，有效地提高人体目标的跟踪精度。
本发明解决其技术问题所采用的技术方案是：
一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法，所述方法包括以下步骤：
步骤1，建立跟随系统的状态模型，以及相关服务对象的量测模型；
步骤2，初始化参数，确定机器人跟随系统的状态向量以及对应的噪声协方差；
步骤3，对系统状态进行更新，得到k时刻系统的状态预测值
Figure GDA0002811812800000021
及其对应协方差Pk|k-1；
步骤4，对系统的量测更新过程进行迭代。根据迭代过程中系统的似然函数以及采样粒子的权重自适应地调整迭代步长Δ；同时，令γn＝γn-1+Δ，判断γn是否大于等于1，结果为是，则令Δ＝1-γn-1，结果为否，则不做修改；然后，将量测噪声vk放大Δ倍，并计算第n次更新后的量测预测值
Figure GDA0002811812800000031
及其协方差
Figure GDA0002811812800000032
判断第n次更新后的状态估计误差是否在均方意义下有界，结果为是，则执行步骤5；结果为否，则k时刻系统的后验状态向量
Figure GDA0002811812800000033
及其协方差Pk为第n次目标后验状态向量
Figure GDA0002811812800000034
与协方差
Figure GDA0002811812800000035
并令k＝k+1，执行步骤3；
步骤5，根据所得的量测预测值
Figure GDA0002811812800000036
及其协方差
Figure GDA0002811812800000037
以及互协方差
Figure GDA0002811812800000038
得到第n次量测更新后系统的后验状态向量
Figure GDA0002811812800000039
与协方差
Figure GDA00028118128000000310
步骤6，判断γn是否等于1，结果为是，则k时刻目标后验状态向量
Figure GDA00028118128000000311
及其协方差Pk为第n次目标后验状态向量
Figure GDA00028118128000000312
与协方差
Figure GDA00028118128000000313
并令k＝k+1，执行步骤3；结果为否，则令n＝n+1，执行步骤4。
进一步，在所述步骤1中，建立跟随机器人在未知动态环境中的运动学模型，同时选取系统的状态向量为
Figure GDA00028118128000000314
其中，
Figure GDA00028118128000000315
为k时刻人体目标的状态向量，
Figure GDA00028118128000000316
和
Figure GDA00028118128000000317
分别为人体目标在x轴和y轴上的位置，
Figure GDA00028118128000000318
和
Figure GDA00028118128000000319
为人体目标在x轴和y轴上的速度，
Figure GDA00028118128000000320
为跟随机器人的状态向量，
Figure GDA00028118128000000321
Figure GDA00028118128000000322
为机器人在x轴和y轴上的位置，
Figure GDA00028118128000000323
为机器人在x轴和y轴上的速度。
再进一步，所述步骤1中，机器人与人体目标间的位置关系通过视觉、激光等传感器得到，进而根据二者之间的量测信息建立系统的量测模型。
更进一步，在所述步骤4中，由判定条件
Figure GDA00028118128000000324
判断系统的状态估计误差是否在均方意义下有界，其中，
Figure GDA0002811812800000041
为第n次量测更新后的新息；当不满足判定条件时，说明没有必要对量测更新过程再进行迭代。
本发明的有益效果主要体现在：本发明提出了一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法。针对线性化误差过大引起的系统跟踪性能下降的问题，该方法在量测更新过程中，通过渐进地引入当前量测信息以减小线性化误差，从而有效地提高人体目标的跟踪精度。其次，在量测渐进过程中引入判定条件，进一步地补偿系统中存在的线性化误差。同时，给出了渐进步长的自适应调整策略，有效地降低系统的计算复杂度。相比于现有的目标跟踪方法，该方法充分考虑线性化误差对系统的影响，在保证计算复杂度的前提下，提高了对人体目标的跟踪精度。
附图说明
图1是跟随机器人与人体目标的位置示意图，其中，1为激光，2为跟随机器人，3为跟踪人物。
图2是机器人跟随系统的坐标转换图，其中，4为跟踪人物坐标系，5为机器人坐标系。
图3是本方法的算法流程图。
具体实施方式
下面结合附图对本发明做进一步说明。
参照图1～图3，一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法，由图1所示，跟随机器人采用激光传感器对人体目标进行探测，获得服务机器人与人体目标间的距离与角度；如图2所示，根据激光传感器返回的量测信息，通过坐标转换得到在全局坐标下机器人与人体目标间的位置关系，跟随系统的状态空间模型如公式(1)所示，传感器量测模型如公式(2)所示：
Figure GDA0002811812800000051
Figure GDA0002811812800000052
Figure GDA0002811812800000053
Figure GDA0002811812800000054
其中，系统的状态向量为
Figure GDA0002811812800000055
Figure GDA0002811812800000056
Figure GDA0002811812800000057
和
Figure GDA0002811812800000058
分别为k时刻人体目标在x轴和y轴上的位置，
Figure GDA0002811812800000059
和
Figure GDA00028118128000000510
为人体目标在x轴和y轴上的速度。
Figure GDA00028118128000000511
为机器人在x轴和y轴上的位置，
Figure GDA00028118128000000512
为机器人在x轴和y轴上的速度。ω为角速度，采样时间为Δtk＝tk-tk-1。过程噪声wk为零均值协方差为Qk的高斯噪声。量测噪声vk是与过程噪声wk无关的，均值为零且协方差Rk的高斯噪声。
参照图3，一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法，包括以下步骤：
步骤1，建立跟随系统在未知动态环境中的状态模型，并根据机器人与跟踪人体目标间的位置关系建立相应的量测模型；
步骤2，初始化参数，确定机器人跟随系统的状态向量
Figure GDA0002811812800000061
以及对应的噪声协方差Qk；
步骤3，对系统状态进行更新，结合UT变换得到k时刻的状态预测值
Figure GDA0002811812800000062
及其对应协方差
Figure GDA0002811812800000063
Figure GDA0002811812800000064
Figure GDA0002811812800000065
其中，
Figure GDA0002811812800000066
为系统的sigma点，
Figure GDA0002811812800000067
为对应的权值。
步骤4，对量测更新过程进行迭代，根据迭代过程中系统的似然函数以及采样粒子的权重自适应地调整迭代步长Δ
Figure GDA0002811812800000068
其中，
Figure GDA0002811812800000069
Figure GDA00028118128000000610
Figure GDA00028118128000000611
令γn＝γn-1+Δ，判断γn是否大于等于1，结果为是，则令Δ＝1-γn-1，结果为否，则不做修改；然后，将量测噪声vk放大Δ倍，并计算第n次更新后的量测预测值
Figure GDA00028118128000000612
协方差
Figure GDA00028118128000000613
与互协方差
Figure GDA00028118128000000614
Figure GDA0002811812800000071
Figure GDA0002811812800000072
Figure GDA0002811812800000073
然后，判断第n次更新后的新息是否满足
Figure GDA0002811812800000074
其中，
Figure GDA0002811812800000075
为第n次迭代后的量测新息；结果为是，则执行步骤5；结果为否，则k时刻目标后验状态向量
Figure GDA0002811812800000076
及其协方差Pk为第n次目标后验状态向量
Figure GDA0002811812800000077
与协方差
Figure GDA0002811812800000078
并令k＝k+1，执行步骤3；
步骤5，根据所得的量测预测值
Figure GDA0002811812800000079
及其协方差
Figure GDA00028118128000000710
以及互协方差
Figure GDA00028118128000000711
得到第n次更新后的目标后验状态向量
Figure GDA00028118128000000712
与协方差
Figure GDA00028118128000000713
Figure GDA00028118128000000714
Figure GDA00028118128000000715
其中，系统的权值为
Figure GDA00028118128000000716
步骤6，判断γn是否等于1，结果为是，则k时刻目标后验状态向量
Figure GDA00028118128000000717
及其协方差Pk为第n次目标后验状态向量
Figure GDA00028118128000000718
与协方差
Figure GDA00028118128000000719
并令k＝k+1，执行步骤3；结果为否，则令n＝n+1，执行步骤4。

Claims (4)

1.一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法，其特征在于，包括如下步骤：
步骤1，建立跟随系统的状态模型，以及服务对象的量测模型；
步骤2，初始化参数，确定机器人跟随系统的状态向量以及对应的噪声协方差；
步骤3，对系统状态进行更新，得到k时刻系统的状态预测值
Figure FDA0002737015030000011
及其对应协方差Pk|k-1；
步骤4，对系统的量测更新过程进行迭代，根据迭代过程中系统的似然函数以及采样粒子的权重自适应地调整迭代步长Δ，
Figure FDA0002737015030000012
其中，
Figure FDA0002737015030000013
Figure FDA0002737015030000014
Figure FDA0002737015030000015
同时，令γn＝γn-1+Δ，判断γn是否大于等于1，如果结果为是，则令Δ＝1-γn-1，结果为否，则不做修改；然后，将量测噪声vk放大Δ倍，并计算第n次更新后的量测预测值
Figure FDA0002737015030000016
及其协方差
Figure FDA0002737015030000017
判断第n次更新后的状态估计误差是否在均方意义下有界,由判定条件
Figure FDA0002737015030000018
判断系统的状态估计误差是否在均方意义下有界，其中，
Figure FDA0002737015030000019
为第n次量测更新后的新息，结果为是，则执行步骤5；结果为否，则k时刻系统的后验状态向量
Figure FDA00027370150300000110
及其协方差Pk为第n-1次目标后验状态向量
Figure FDA0002737015030000021
与协方差
Figure FDA0002737015030000022
并令k＝k+1，执行步骤3；
步骤5，根据所得的量测预测值
Figure FDA0002737015030000023
及其协方差
Figure FDA0002737015030000024
以及互协方差
Figure FDA0002737015030000025
得到第n次量测更新后系统的后验状态向量
Figure FDA0002737015030000026
与协方差
Figure FDA0002737015030000027
步骤6，判断γn是否等于1，结果为是，则k时刻目标后验状态向量
Figure FDA0002737015030000028
及其协方差Pk为第n次目标后验状态向量
Figure FDA0002737015030000029
与协方差
Figure FDA00027370150300000210
并令k＝k+1，执行步骤3；结果为否，则令n＝n+1，执行步骤4。
2.根据权利要求1所述一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法，其特征在于，在所述步骤1中，建立跟随机器人在未知动态环境中的运动学模型，同时选取系统的状态向量为
Figure FDA00027370150300000211
其中，
Figure FDA00027370150300000212
为k时刻人体目标的状态向量，
Figure FDA00027370150300000213
和
Figure FDA00027370150300000214
分别为人体目标在x轴和y轴上的位置，
Figure FDA00027370150300000215
和
Figure FDA00027370150300000216
为人体目标在x轴和y轴上的速度，
Figure FDA00027370150300000217
为跟随机器人的状态向量，
Figure FDA00027370150300000218
为机器人在x轴和y轴上的位置，
Figure FDA00027370150300000219
为机器人在x轴和y轴上的速度。
3.根据权利要求1或2所述一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法，其特征在于，所述步骤1中，机器人与人体目标间的位置关系通过视觉、激光传感器得到，进而根据二者之间的量测信息建立系统的量测模型。
4.根据权利要求1或2所述一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法，其特征在于，在所述步骤4中，由判定条件
Figure FDA00027370150300000220
判断系统的状态估计误差是否在均方意义下有界，其中，
Figure FDA0002737015030000031
为第n次量测更新后的新息，当不满足判定条件时，说明没有必要对量测更新过程再进行迭代。
CN201810359381.0A 2018-04-20 2018-04-20 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法 Active CN108680162B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810359381.0A CN108680162B (zh) 2018-04-20 2018-04-20 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810359381.0A CN108680162B (zh) 2018-04-20 2018-04-20 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法

Publications (2)

Publication Number Publication Date
CN108680162A CN108680162A (zh) 2018-10-19
CN108680162B true CN108680162B (zh) 2021-02-26

Family

ID=63802093

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810359381.0A Active CN108680162B (zh) 2018-04-20 2018-04-20 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法

Country Status (1)

Country Link
CN (1) CN108680162B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110530365B (zh) * 2019-08-05 2021-05-18 浙江工业大学 一种基于自适应卡尔曼滤波的人体姿态估计方法
CN111242982A (zh) * 2020-01-02 2020-06-05 浙江工业大学 一种基于渐进卡尔曼滤波的人体目标跟踪方法
CN112346340B (zh) * 2020-10-26 2022-10-18 海丰通航科技有限公司 非线性数据处理方法、装置、设备和介质

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101162585B1 (ko) * 2010-01-08 2012-07-04 동명대학교산학협력단 측정거리의 바이어스 모델링을 통한 삼각측량 방법, 시스템 및 기억매체
CN103383261B (zh) * 2013-07-02 2015-11-18 河海大学 一种改进型无损卡尔曼滤波室内动目标定位方法
CN107783944A (zh) * 2017-09-20 2018-03-09 北京航空航天大学 一种多模型自校准无迹卡尔曼滤波方法

Also Published As

Publication number Publication date
CN108680162A (zh) 2018-10-19

Similar Documents

Publication Publication Date Title
CN109597864B (zh) 椭球边界卡尔曼滤波的即时定位与地图构建方法及系统
US8374624B2 (en) Location measurement method based on predictive filter
CN108680162B (zh) 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法
CN108332758B (zh) 一种移动机器人的走廊识别方法及装置
CN106772351B (zh) 基于有限步记忆的卡尔曼滤波方法
CN105447574B (zh) 一种辅助截断粒子滤波方法、装置及目标跟踪方法及装置
CN110209180B (zh) 一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法
CN112986977B (zh) 一种克服雷达扩展卡尔曼航迹滤波发散的方法
CN110988894A (zh) 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法
CN110231620B (zh) 一种噪声相关系统跟踪滤波方法
CN108303095B (zh) 适用于非高斯系统的鲁棒容积目标协同定位方法
CN109520503A (zh) 一种平方根容积模糊自适应卡尔曼滤波slam方法
CN102706345A (zh) 一种基于衰减记忆序贯检测器的机动目标跟踪方法
CN110174907A (zh) 一种基于自适应卡尔曼滤波的人体目标跟随方法
CN111189454A (zh) 基于秩卡尔曼滤波的无人车slam导航方法
CN111025229B (zh) 一种水下机器人纯方位目标估计方法
CN110702093B (zh) 基于粒子滤波的定位方法、装置、存储介质及机器人
CN107218917B (zh) 一种移动机器人航向角估计方法
CN117392215A (zh) 一种基于改进amcl和pl-icp点云匹配的移动机器人位姿校正方法
CN115457510A (zh) 一种基于变分贝叶斯的自适应协同目标跟踪方法
CN104331087B (zh) 一种鲁棒的水下传感器网络目标跟踪方法
CN113884098A (zh) 一种基于具体化模型的可迭代的卡尔曼滤波定位方法
CN107966697B (zh) 一种基于渐进无迹卡尔曼的移动目标跟踪方法
CN110007298B (zh) 一种目标超前预测跟踪方法
CN105513091A (zh) 一种基于双贝叶斯估计的水下运动体运动状态估计方法

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