CN108680162A - 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法 - Google Patents
一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法 Download PDFInfo
- Publication number
- CN108680162A CN108680162A CN201810359381.0A CN201810359381A CN108680162A CN 108680162 A CN108680162 A CN 108680162A CN 201810359381 A CN201810359381 A CN 201810359381A CN 108680162 A CN108680162 A CN 108680162A
- Authority
- CN
- China
- Prior art keywords
- human body
- covariance
- body target
- axis
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 230000000750 progressive effect Effects 0.000 title claims abstract description 16
- 238000001914 filtration Methods 0.000 title claims abstract description 15
- 238000005259 measurement Methods 0.000 claims abstract description 25
- 238000005530 etching Methods 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 230000000977 initiatory effect Effects 0.000 claims description 3
- 238000012804 iterative process Methods 0.000 claims description 3
- 235000013399 edible fruits Nutrition 0.000 claims description 2
- 230000003044 adaptive effect Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 239000002245 particle Substances 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000012466 permeate Substances 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
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时刻系统的状态预测值及其对应协方差Pk|k-1；
步骤4，对系统的量测更新过程进行迭代。根据迭代过程中系统的似然函数以及采样粒子的权重自适应地调整迭代步长Δ；同时，令γn＝γn-1+Δ，判断γn是否大于等于1，结果为是，则令Δ＝1-γn-1，结果为否，则不做修改；然后，将量测噪声Rk放大Δ倍，并计算第γn次更新后的量测预测值及其协方差判断第γn次更新后的状态估计误差是否在均方意义下有界，结果为是，则执行步骤5；结果为否，则k时刻系统的后验状态向量及其协方差Pk为第γn-1次目标后验状态向量与协方差并令k＝k+1，执行步骤3；
步骤5，根据所得的量测预测值及其协方差以及互协方差得到第γn次量测更新后系统的后验状态向量与协方差
步骤6，判断γn是否等于1，结果为是，则k时刻目标后验状态向量及其协方差Pk为第γn次目标后验状态向量与协方差并令k＝k+1，执行步骤3；结果为否，则令n＝n+1，执行步骤4。
进一步，在所述步骤1中，建立跟随机器人在未知动态环境中的运动学模型，同时选取系统的状态向量为其中，为k时刻人体目标的状态向量，和分别为人体目标在x轴和y轴上的位置，和为人体目标在x轴和y轴上的速度，为跟随机器人的状态向量， 为机器人在x轴和y轴上的位置，为机器人在x轴和y轴上的速度。
再进一步，所述步骤1中，机器人与人体目标间的位置关系通过视觉、激光等传感器得到，进而根据二者之间的量测信息建立系统的量测模型。
更进一步，在所述步骤4中，由判定条件判断系统的状态估计误差是否在均方意义下有界，其中，为第γn次量测更新后的新息；当不满足判定条件时，说明没有必要对量测更新过程再进行迭代。
本发明的有益效果主要体现在：本发明提出了一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法。针对线性化误差过大引起的系统跟踪性能下降的问题，该方法在量测更新过程中，通过渐进地引入当前量测信息以减小线性化误差，从而有效地提高人体目标的跟踪精度。其次，在量测渐进过程中引入判定条件，进一步地补偿系统中存在的线性化误差。同时，给出了渐进步长的自适应调整策略，有效地降低系统的计算复杂度。相比于现有的目标跟踪方法，该方法充分考虑线性化误差对系统的影响，在保证计算复杂度的前提下，提高了对人体目标的跟踪精度。
附图说明
图1是跟随机器人与人体目标的位置示意图，其中，1为激光，2为跟随机器人，3为跟踪人物。
图2是机器人跟随系统的坐标转换图，其中，4为跟踪人物坐标系，5为机器人坐标系。
图3是本方法的算法流程图。
具体实施方式
下面结合附图对本发明做进一步说明。
参照图1～图3，一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法，由图1所示，跟随机器人采用激光传感器对人体目标进行探测，获得服务机器人与人体目标间的距离与角度；如图2所示，根据激光传感器返回的量测信息，通过坐标转换得到在全局坐标下机器人与人体目标间的位置关系，跟随系统的状态空间模型如公式(1)所示，传感器量测模型如公式(2)所示：
其中，系统的状态向量为 和分别为k时刻人体目标在x轴和y轴上的位置，和为人体目标在x轴和y轴上的速度。为机器人在x轴和y轴上的位置，为机器人在x轴和y轴上的速度。ω为角速度，采样时间为Δtk＝tk-tk-1。过程噪声wk为零均值协方差为Qk的高斯噪声。量测噪声vk是与过程噪声wk无关的，均值为零且协方差Rk的高斯噪声。
参照图3，一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法，包括以下步骤：
步骤1，建立跟随系统在未知动态环境中的状态模型，并根据机器人与跟踪人体目标间的位置关系建立相应的量测模型；
步骤2，初始化参数，确定机器人跟随系统的状态向量以及对应的噪声协方差Qk；
步骤3，对系统状态进行更新，结合UT变换得到k时刻的状态预测值及其对应协方差Pk|k-1；
其中，为系统的sigma点，为对应的权值。
步骤4，对量测更新过程进行迭代，根据迭代过程中系统的似然函数以及采样粒子的权重自适应地调整迭代步Δ
其中，
令γn＝γn-1+Δ，判断γn是否大于等于1，结果为是，则令Δ＝1-γn-1，结果为否，则不做修改；然后，将量测噪声Rk放大Δ倍，并计算第γk次更新后的量测预测值协方差与互协方差
然后，判断第n次更新后的新息是否满足其中，为第γn次迭代后的量测新息；结果为是，则执行步骤5；结果为否，则k时刻目标后验状态向量及其协方差Pk为第γn-1次目标后验状态向量与协方差并令k＝k+1，执行步骤3；
步骤5，根据所得的量测预测值及其协方差以及互协方差得到第γn次更新后的目标后验状态向量与协方差
其中，系统的权值为
步骤6，判断γn是否等于1，结果为是，则k时刻目标后验状态向量及其协方差Pk为第γn次目标后验状态向量与协方差并令k＝k+1，执行步骤3；结果为否，则令n＝n+1，执行步骤4。
Claims (4)
1.一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法，其特征在于，包括如下步骤：
步骤1，建立跟随系统的状态模型，以及服务对象的量测模型；
步骤2，初始化参数，确定机器人跟随系统的状态向量以及对应的噪声协方差；
步骤3，对系统状态进行更新，得到k时刻系统的状态预测值及其对应协方差Pk|k-1；
步骤4，对系统的量测更新过程进行迭代，根据迭代过程中系统的似然函数以及采样粒子的权重自适应地调整迭代步长Δ，同时，令γn＝γn-1+Δ，判断γn是否大于等于1，如果结果为是，则令Δ＝1-γn-1，结果为否，则不做修改；然后，将量测噪声Rk放大Δ倍，并计算第γn次更新后的量测预测值及其协方差判断第γn次更新后的状态估计误差是否在均方意义下有界，结果为是，则执行步骤5；结果为否，则k时刻系统的后验状态向量及其协方差Pk为第γn-1次目标后验状态向量与协方差并令k＝k+1，执行步骤3；
步骤5，根据所得的量测预测值及其协方差以及互协方差得到第γn次量测更新后系统的后验状态向量与协方差
步骤6，判断γn是否等于1，结果为是，则k时刻目标后验状态向量及其协方差Pk为第γn次目标后验状态向量与协方差并令k＝k+1，执行步骤3；结果为否，则令n＝n+1，执行步骤4。
2.根据权利要求1所述一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法，其特征在于，在所述步骤1中，建立跟随机器人在未知动态环境中的运动学模型，同时选取系统的状态向量为其中，为k时刻人体目标的状态向量，和分别为人体目标在x轴和y轴上的位置，和为人体目标在x轴和y轴上的速度，为跟随机器人的状态向量，为机器人在x轴和y轴上的位置，为机器人在x轴和y轴上的速度。
3.根据权利要求1或2所述一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法，其特征在于，所述步骤1中，机器人与人体目标间的位置关系通过视觉、激光等传感器得到，进而根据二者之间的量测信息建立系统的量测模型。
4.根据权利要求1或2所述一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法，其特征在于，在所述步骤4中，由判定条件判断系统的状态估计误差是否在均方意义下有界，其中，为第γn次量测更新后的新息，，当不满足判定条件时，说明没有必要对量测更新过程再进行迭代。
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 true CN108680162A (zh) | 2018-10-19 |
| CN108680162B 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) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110530365A (zh) * | 2019-08-05 | 2019-12-03 | 浙江工业大学 | 一种基于自适应卡尔曼滤波的人体姿态估计方法 |
| CN111242982A (zh) * | 2020-01-02 | 2020-06-05 | 浙江工业大学 | 一种基于渐进卡尔曼滤波的人体目标跟踪方法 |
| CN112346340A (zh) * | 2020-10-26 | 2021-02-09 | 海丰通航科技有限公司 | 非线性数据处理方法、装置、设备和介质 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20110081423A (ko) * | 2010-01-08 | 2011-07-14 | 동명대학교산학협력단 | 측정거리의 바이어스 모델링을 통한 삼각측량 방법, 시스템 및 기억매체 |
| CN103383261A (zh) * | 2013-07-02 | 2013-11-06 | 河海大学 | 一种改进型无损卡尔曼滤波室内动目标定位方法 |
| CN107783944A (zh) * | 2017-09-20 | 2018-03-09 | 北京航空航天大学 | 一种多模型自校准无迹卡尔曼滤波方法 |
-
2018
- 2018-04-20 CN CN201810359381.0A patent/CN108680162B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20110081423A (ko) * | 2010-01-08 | 2011-07-14 | 동명대학교산학협력단 | 측정거리의 바이어스 모델링을 통한 삼각측량 방법, 시스템 및 기억매체 |
| CN103383261A (zh) * | 2013-07-02 | 2013-11-06 | 河海大学 | 一种改进型无损卡尔曼滤波室内动目标定位方法 |
| CN107783944A (zh) * | 2017-09-20 | 2018-03-09 | 北京航空航天大学 | 一种多模型自校准无迹卡尔曼滤波方法 |
Non-Patent Citations (2)
| Title |
|---|
| K. XIONG; H.Y. ZHANG; C.W. CHAN;: "Performance evaluation of UKF-based nonlinear filtering", 《AUTOMATICA》 * |
| RONGHUI ZHAN;JIANWEI WAN: "Iterated Unscented Kalman Filter for Passive Target Tracking", 《 IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS》 * |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110530365A (zh) * | 2019-08-05 | 2019-12-03 | 浙江工业大学 | 一种基于自适应卡尔曼滤波的人体姿态估计方法 |
| CN110530365B (zh) * | 2019-08-05 | 2021-05-18 | 浙江工业大学 | 一种基于自适应卡尔曼滤波的人体姿态估计方法 |
| CN111242982A (zh) * | 2020-01-02 | 2020-06-05 | 浙江工业大学 | 一种基于渐进卡尔曼滤波的人体目标跟踪方法 |
| CN112346340A (zh) * | 2020-10-26 | 2021-02-09 | 海丰通航科技有限公司 | 非线性数据处理方法、装置、设备和介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108680162B (zh) | 2021-02-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20190195631A1 (en) | Positioning method, positioning device, and robot | |
| CN104809326B (zh) | 一种异步传感器空间配准算法 | |
| CN108680162A (zh) | 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法 | |
| CN107728138A (zh) | 一种基于当前统计模型的机动目标跟踪方法 | |
| CN111521195B (zh) | 一种智能机器人 | |
| CN103047982B (zh) | 基于角度信息的目标自适应跟踪方法 | |
| CN106199580B (zh) | 一种基于模糊推理系统的Singer模型改进算法 | |
| CN112432644B (zh) | 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法 | |
| CN110530365A (zh) | 一种基于自适应卡尔曼滤波的人体姿态估计方法 | |
| CN106772351B (zh) | 基于有限步记忆的卡尔曼滤波方法 | |
| CN110209180B (zh) | 一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法 | |
| CN110231620B (zh) | 一种噪声相关系统跟踪滤波方法 | |
| CN105955027A (zh) | 一种基于多阶运动信息估计的前馈控制方法 | |
| CN111982102B (zh) | 一种复杂环境下基于bp-ekf的uwb-imu定位方法 | |
| CN110174907A (zh) | 一种基于自适应卡尔曼滤波的人体目标跟随方法 | |
| CN110702093B (zh) | 基于粒子滤波的定位方法、装置、存储介质及机器人 | |
| CN115900695A (zh) | 一种应用于车辆的智能泊车车辆定位的方法 | |
| CN113779497B (zh) | 一种解决量测信息存在随机时延和丢包的目标跟踪方法 | |
| CN107966697B (zh) | 一种基于渐进无迹卡尔曼的移动目标跟踪方法 | |
| CN108710295A (zh) | 一种基于渐进容积信息滤波的机器人跟随方法 | |
| CN114895298B (zh) | 一种雷达慢速弱机动目标量测修正伯努利滤波方法及装置 | |
| CN114302359B (zh) | 一种基于WiFi-PDR融合的高精度室内定位方法 | |
| CN105388471B (zh) | 一种自适应电磁场时延估计方法及装置 | |
| CN110944293B (zh) | 基于路径衰减和卡尔曼滤波融合的无线电搜索定位方法 | |
| Li et al. | Tracking an underwater maneuvering target using an adaptive Kalman filter |
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 |