CN110174907B - 一种基于自适应卡尔曼滤波的人体目标跟随方法 - Google Patents

一种基于自适应卡尔曼滤波的人体目标跟随方法 Download PDF

Info

Publication number
CN110174907B
CN110174907B CN201910260986.9A CN201910260986A CN110174907B CN 110174907 B CN110174907 B CN 110174907B CN 201910260986 A CN201910260986 A CN 201910260986A CN 110174907 B CN110174907 B CN 110174907B
Authority
CN
China
Prior art keywords
human body
covariance
kalman filtering
calculating
noise
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
CN201910260986.9A
Other languages
English (en)
Other versions
CN110174907A (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.)
Noblelift Intelligent Equipment Co Ltd
Original Assignee
Noblelift Intelligent Equipment Co Ltd
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 Noblelift Intelligent Equipment Co Ltd filed Critical Noblelift Intelligent Equipment Co Ltd
Priority to CN201910260986.9A priority Critical patent/CN110174907B/zh
Publication of CN110174907A publication Critical patent/CN110174907A/zh
Application granted granted Critical
Publication of CN110174907B publication Critical patent/CN110174907B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/12—Target-seeking control

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

一种基于自适应卡尔曼滤波的人体目标跟随方法，该方法考虑到卡尔曼滤波的局限性,提出自适应卡尔曼滤波算法使其更好地处理量测过程中的不确定性。本方法首先设计了一个随机线性非高斯系统，通过引入自适应因子对其包含的高斯噪声、不确定性扰动噪声以及野值进行补偿和剔除，提高了人体目标跟随的精度和鲁棒性。相比现有的人体目标跟随方法，该方法充分考虑了野值和不确定性扰动噪声的影响，提高了人体目标跟随的精度和鲁棒性。

Description

一种基于自适应卡尔曼滤波的人体目标跟随方法
技术领域
本发明属于人体目标跟随领域，具体涉及一种基于自适应卡尔曼滤波的人体目标跟随方法。
背景技术
人体目标跟随是生产和生活领域中的一个息息相关的问题，在仓储管理与搬运、超市导购车、自动跟随行李箱等方面发挥着重要的作用。同时，随着微电子技术的发展，视觉传感器在人体目标跟随与定位中的应用得到了学术界和工业界的广泛关注。视觉传感器利用图像对移动目标进行感知，并提供丰富的环境信息以及准确的定位服务。
在实际检测环境下，人体目标跟随通常会涉及量测信息的优化问题，例如激光传感器的目标测距系统与视觉传感器的目标定位系统等等。卡尔曼滤波算法作为一种有效的线性估计方法被广泛地应用于人体目标跟随系统中。
然而，传统的卡尔曼滤波算法受限于其高斯假设前提，致使量测中由野值和不确定性扰动等产生的噪声无法得到有效减弱。
为了避免此类问题，引入自适应因子，有效地不确定性扰动噪声以及野值进行补偿和剔除，因此自适应卡尔曼滤波算法被广泛使用。
发明内容
本发明的目的是提供一种基于自适应卡尔曼滤波的人体目标跟随方法，以克服传统卡尔曼滤波算法受限于高斯噪声的假设前提，设计了一个随机线性非高斯系统，通过引入自适应因子对其包含的高斯噪声、不确定性扰动噪声以及野值进行补偿和剔除，提高了人体目标跟随精度和鲁棒性。
本发明的上述技术目的是通过以下技术方案得以实现的：一种基于自适应卡尔曼滤波的人体目标跟随方法，包括以下步骤：
步骤1，初始化参数，确定系统的状态和量测方程及其对应的噪声协方差；
步骤2，在假设系统无未知干扰和野值的条件下，计算人体目标的先验状态向量
Figure SMS_1
及其协方差/>
Figure SMS_2
与先验残差/>
Figure SMS_3
及其协方差/>
Figure SMS_4
；
步骤3，通过先验残差测试来计算马氏距离的平方
Figure SMS_5
及自由度为/>
Figure SMS_6
的卡方；然后比较大小，如果/>
Figure SMS_7
，执行步骤,5，否则执行步骤4；
步骤4，通过后验残差测试来计算马氏距离的平方
Figure SMS_8
及自由度为/>
Figure SMS_9
的卡方；然后比较大小，如果/>
Figure SMS_10
，计算自适应因子/>
Figure SMS_11
及更新后的先验残差/>
Figure SMS_12
；
步骤5，在假设系统无未知干扰和野值的条件下，计算卡尔曼滤波增益与后验状态向量
Figure SMS_13
及其协方差/>
Figure SMS_14
。
作为本发明的优选，在所述步骤1中，
Figure SMS_15
时刻的人体目标状态向量为
Figure SMS_16
。/>
Figure SMS_17
，/>
Figure SMS_18
和/>
Figure SMS_19
分别为人体目标的位置、速度和加速度。/>
作为本发明的优选，在所述步骤3中，先验残差测试计算马氏距离的平方
Figure SMS_21
及自信度为/>
Figure SMS_27
的/>
Figure SMS_30
；然后比较大小，如果满足如下/>
Figure SMS_22
，直接计算卡尔曼滤波增益与后验状态向量/>
Figure SMS_25
及其协方差/>
Figure SMS_29
，否则通过后验残差测试计算马氏距离的平方/>
Figure SMS_31
及自信度为/>
Figure SMS_20
的卡方；然后比较大小，如果/>
Figure SMS_24
，计算自适应因子/>
Figure SMS_28
及更新后的先验残差/>
Figure SMS_32
，最后计算卡尔曼滤波增益与后验状态向量/>
Figure SMS_23
及其协方差/>
Figure SMS_26
。
综上所述，本发明具有如下有益效果：
本发明提出了一种基于自适应卡尔曼的人体目标跟随方法。该方法通过引入了卡方检测方法，对先验的每个更新时刻的状态进行置信域判断，从而剔除野值，有效地解决了由数值量测误差过大引起的系统跟踪性能下降的问题。同时，为了减弱未知扰动对系统的影响，在后验估计中引入了一个自适应因子，不断更新先验残差，从而提高对人体目标跟随精度和鲁棒性。
附图说明：
图1是本发明方法的人体目标跟随示意图。
图2是本发明方法流程图。
图3是本发明方法与传统卡尔曼滤波算法的对比加速度误差图；
图4是本发明方法与传统卡尔曼滤波算法的对比速度误差图；
图5是本发明方法与传统卡尔曼滤波算法的对比位置误差图。
具体实施方式
下面结合附图对本发明做进一步说明。
本具体实施例仅仅是对本发明的解释，其并不是对本发明的限制，本领域技术人员在阅读完本说明书后可以根据需要对本实施例做出没有创造性贡献的修改，但只要在本实用新型的权利要求范围内都受到专利法的保护。
实施例1：参照图1~图3，一种基于自适应卡尔曼的人体目标跟随方法，如图1所示，模拟在实际路况中对人体进行跟踪。移动机器人的人体目标跟随系统的状态空间模型如式(1)和(2)所示：
Figure SMS_33
(1)
Figure SMS_34
(2)
其中，人体目标状态向量
Figure SMS_42
。/>
Figure SMS_36
，/>
Figure SMS_47
和/>
Figure SMS_44
分别为/>
Figure SMS_51
时刻人体目标的位置、速度和加速度，/>
Figure SMS_43
，/>
Figure SMS_52
和/>
Figure SMS_38
分别为/>
Figure SMS_49
时刻人体目标的位置、速度和加速度，
Figure SMS_35
为采样时间。/>
Figure SMS_45
为零均值协方差为/>
Figure SMS_37
的加速度过程噪声。/>
Figure SMS_46
，/>
Figure SMS_39
分别为两个加速度计的量测值，/>
Figure SMS_48
是均值为零协方差为/>
Figure SMS_40
高斯噪声，/>
Figure SMS_50
是未知情况的量测噪声（比如图1中的小坡等），/>
Figure SMS_41
表示野值造成的量测噪声。
结合附图2，所述基于自适应卡尔曼的人体目标跟随方法包括以下步骤：
步骤1，初始化参数，确定系统的状态和量测方程及其对应的噪声协方差；
Figure SMS_53
(3)
Figure SMS_54
(4)
步骤2，在假设系统无未知干扰和野值的条件下，计算人体目标的先验状态向量
Figure SMS_55
及其协方差/>
Figure SMS_56
与先验残差/>
Figure SMS_57
及其协方差/>
Figure SMS_58
：
Figure SMS_59
(5)
Figure SMS_60
(6)
Figure SMS_61
(7)
Figure SMS_62
(8)
步骤3，通过先验残差测试来计算马氏距离的平方
Figure SMS_63
及自由度为/>
Figure SMS_64
的/>
Figure SMS_65
；然后比较大小，如果满足如下/>
Figure SMS_66
，执行步骤,5，否则执行步骤4：
Figure SMS_67
(9)
Figure SMS_68
(10)
步骤4，通过后验残差测试来计算马氏距离的平方
Figure SMS_69
及自由度为/>
Figure SMS_70
的卡方；然后比较大小，如果/>
Figure SMS_71
，计算自适应因子/>
Figure SMS_72
及更新后的先验残差/>
Figure SMS_73
：
Figure SMS_74
(11)
其中，
Figure SMS_75
(12)
则，
Figure SMS_76
(13)
Figure SMS_77
(14)
Figure SMS_78
(15)
把(12)和(13)代入上式，得
Figure SMS_79
(16)
Figure SMS_80
(17)
因此，
Figure SMS_81
(18)
其中，
Figure SMS_82
，如果/>
Figure SMS_83
，则计算
Figure SMS_84
(19)
然后更新先验残差
Figure SMS_85
(20)
步骤5，在假设系统无未知干扰和野值的条件下，计算卡尔曼滤波增益与后验状态向量
Figure SMS_86
及其协方差/>
Figure SMS_87
：
Figure SMS_88
(21)
Figure SMS_89
(22)
Figure SMS_90
(23)
结合附图3、附图4和附图5，本发明方法与传统卡尔曼滤波算法的对比误差图，三幅附图分别表示加速度误差，速度误差，位置误差。不难发现，人体目标的加速度、速度以及位置的估计精度通过本发明方法得到了显著提升。

Claims (1)

1.一种基于自适应卡尔曼滤波的人体目标跟随方法，其特征在于，包括以下步骤： 移动机器人的人体目标跟随系统的状态空间模型公式为：
Figure QLYQS_4
， 公式中，/>
Figure QLYQS_10
为人体目标状态相量，/>
Figure QLYQS_12
，
Figure QLYQS_3
和/>
Figure QLYQS_7
分别为/>
Figure QLYQS_9
时刻人体目标的位置、速度和加速度，/>
Figure QLYQS_11
，/>
Figure QLYQS_1
和/>
Figure QLYQS_5
分别为/>
Figure QLYQS_8
时刻人体目标的位置、速度和加速度，/>
Figure QLYQS_13
为采样时间， />
Figure QLYQS_2
为零均值且协方差为/>
Figure QLYQS_6
的加速度过程噪声，
Figure QLYQS_14
，
Figure QLYQS_15
，/>
Figure QLYQS_16
分别为两个加速度计的量测值，/>
Figure QLYQS_17
是均值为零且协方差为/>
Figure QLYQS_18
高斯噪声，
Figure QLYQS_19
是未知情况的量测噪声，/>
Figure QLYQS_20
表示野值造成的量测噪声；
步骤1，初始化参数，确定系统由位置、速度、加速度组成的状态和对应的量测方程及其对应的噪声协方差，具体公式为：
Figure QLYQS_21
，/>
Figure QLYQS_22
，
步骤2，在假设系统无未知干扰和野值的条件下，根据卡尔曼滤波公式计算人体目标的先验状态向量
Figure QLYQS_23
及其协方差/>
Figure QLYQS_24
与先验残差/>
Figure QLYQS_25
及其协方差/>
Figure QLYQS_26
，具体计算方式为：/>
Figure QLYQS_27
；
步骤3，通过先验残差测试来计算马氏距离的平方
Figure QLYQS_28
及自由度为/>
Figure QLYQS_29
的卡方/>
Figure QLYQS_30
；然后比较大小，如果/>
Figure QLYQS_31
，执行步骤5，否则执行步骤4，
Figure QLYQS_32
；
步骤4，通过后验残差测试来计算马氏距离的平方
Figure QLYQS_35
及自由度为/>
Figure QLYQS_37
的卡方/>
Figure QLYQS_41
；然后比较大小，如果/>
Figure QLYQS_34
，计算自适应因子/>
Figure QLYQS_36
及更新后的先验残差/>
Figure QLYQS_39
，/>
Figure QLYQS_40
； 其中，/>
Figure QLYQS_33
，
Figure QLYQS_38
Figure QLYQS_42
Figure QLYQS_43
，
Figure QLYQS_44
Figure QLYQS_45
， 因此，
Figure QLYQS_46
，然后更新先验残差
Figure QLYQS_47
；
步骤5，在假设系统无未知干扰和野值的条件下，计算卡尔曼滤波增益与后验状态向量
Figure QLYQS_48
及其协方差/>
Figure QLYQS_49
，/>
Figure QLYQS_50
。/>
CN201910260986.9A 2019-04-02 2019-04-02 一种基于自适应卡尔曼滤波的人体目标跟随方法 Active CN110174907B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910260986.9A CN110174907B (zh) 2019-04-02 2019-04-02 一种基于自适应卡尔曼滤波的人体目标跟随方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910260986.9A CN110174907B (zh) 2019-04-02 2019-04-02 一种基于自适应卡尔曼滤波的人体目标跟随方法

Publications (2)

Publication Number Publication Date
CN110174907A CN110174907A (zh) 2019-08-27
CN110174907B true CN110174907B (zh) 2023-03-31

Family

ID=67689011

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910260986.9A Active CN110174907B (zh) 2019-04-02 2019-04-02 一种基于自适应卡尔曼滤波的人体目标跟随方法

Country Status (1)

Country Link
CN (1) CN110174907B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111222437A (zh) * 2019-12-31 2020-06-02 浙江工业大学 一种基于多深度图像特征融合的人体姿态估计方法
CN111273673A (zh) * 2020-03-09 2020-06-12 新石器慧通（北京）科技有限公司 一种无人车的自动驾驶跟随方法、系统及无人车
CN111523208B (zh) * 2020-04-09 2023-08-01 淮阴工学院 一种对人体行走足底地面反应力的卡尔曼滤波方法
CN113422593B (zh) * 2021-07-05 2024-04-26 北京信息科技大学 滤波方法、滤波器、计算机可读存储介质、处理器和fpga

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106529678A (zh) * 2016-10-18 2017-03-22 南京航空航天大学 一种基于最大最小蚂蚁系统优化的slam数据关联方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106500695B (zh) * 2017-01-05 2019-02-01 大连理工大学 一种基于自适应扩展卡尔曼滤波的人体姿态识别方法
CN107966697B (zh) * 2017-11-07 2020-10-30 浙江工业大学 一种基于渐进无迹卡尔曼的移动目标跟踪方法
CN108710295B (zh) * 2018-04-20 2021-06-18 浙江工业大学 一种基于渐进容积信息滤波的机器人跟随方法
CN108762309B (zh) * 2018-05-03 2021-05-18 浙江工业大学 一种基于假设卡尔曼滤波的人体目标跟随方法
CN110530365B (zh) * 2019-08-05 2021-05-18 浙江工业大学 一种基于自适应卡尔曼滤波的人体姿态估计方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106529678A (zh) * 2016-10-18 2017-03-22 南京航空航天大学 一种基于最大最小蚂蚁系统优化的slam数据关联方法

Also Published As

Publication number Publication date
CN110174907A (zh) 2019-08-27

Similar Documents

Publication Publication Date Title
CN110174907B (zh) 一种基于自适应卡尔曼滤波的人体目标跟随方法
CN110988894B (zh) 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法
CN111025250B (zh) 一种车载毫米波雷达在线标定方法
CN108120438B (zh) 一种基于imu和rfid信息融合的室内目标快速跟踪方法
CN113074739A (zh) 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法
JP2006194856A (ja) 移動物体の方向検出方法及びそのシステム
CN111256727B (zh) 一种基于Augmented EKF的提高里程计精度的方法
CN107462911A (zh) 汽车上下高架的检测方法及其装置
CN113119980A (zh) 一种用于电动车的道路坡度估计方法、系统和设备
CN110702093B (zh) 基于粒子滤波的定位方法、装置、存储介质及机器人
CN108680162B (zh) 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法
Xu et al. Indoor Vision/INS Integrated Mobile Robot Navigation Using Multimodel‐Based Multifrequency Kalman Filter
Jiang et al. A novel SLAM algorithm with Adaptive Kalman filter
CN107966697B (zh) 一种基于渐进无迹卡尔曼的移动目标跟踪方法
CN111766573A (zh) 卡尔曼滤波的提高阵列光栅定位空间分辨率的方法及系统
CN107990893B (zh) 二维激光雷达slam中探测环境发生突变的检测方法
KR101907611B1 (ko) 자율 주행체의 위치 정보를 획득하는 방법
CN111090281B (zh) 基于改进粒子滤波算法估算机器人方位的方法和装置
CN112161634B (zh) 基于rtb的线控底盘地图构建与同时定位方法
CN112346479B (zh) 一种基于集中式卡尔曼滤波的无人航行器状态估计方法
Yuan et al. Robot position realization based on multi-sensor information fusion algorithm
Madhloom et al. A modified Kalman filter-based mobile robot position measurement using an accelerometer and wheels encoder
CN112904393B (zh) 一种导航路径几何约束辅助的农业机械自主导航方法
CN113534131B (zh) 基于gm-phd的运动参数估计组合平滑滤波方法
CN111708982B (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