CN108710295A - 一种基于渐进容积信息滤波的机器人跟随方法 - Google Patents

一种基于渐进容积信息滤波的机器人跟随方法 Download PDF

Info

Publication number
CN108710295A
CN108710295A CN201810365081.3A CN201810365081A CN108710295A CN 108710295 A CN108710295 A CN 108710295A CN 201810365081 A CN201810365081 A CN 201810365081A CN 108710295 A CN108710295 A CN 108710295A
Authority
CN
China
Prior art keywords
covariance
moment
state
information
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
Application number
CN201810365081.3A
Other languages
English (en)
Other versions
CN108710295B (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 CN201810365081.3A priority Critical patent/CN108710295B/zh
Publication of CN108710295A publication Critical patent/CN108710295A/zh
Application granted granted Critical
Publication of CN108710295B publication Critical patent/CN108710295B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/048—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators using a predictor
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/0088—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot characterized by the autonomous decision making process, e.g. artificial intelligence, predefined behaviours
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/12—Target-seeking control

Abstract

一种基于渐进容积信息滤波的机器人跟随方法，该方法将量测更新过程分解为若干个子过程，利于多个不同传感器信息的融合；其次，在量测更新过程中渐近地引入量测信息，使其得到充分利用，提高了人体目标跟踪精度；另外，通过增大观测噪声协方差，使得渐进过程中协方差矩阵保持正定性，从而保证滤波器的稳定性；本发明尤其适用于人机协作的智能机器人跟随方法，采用渐进容积信息滤波算法融合收集到的机器人传感器量测信息，得到人物的状态估计，从而实现人物跟随的目的，相比现有的机器人跟随方法，本发明能显著提高人物跟踪的精度和稳定性。

Description

一种基于渐进容积信息滤波的机器人跟随方法
技术领域
本发明属于服务机器人领域，尤其是一种适用于人机协作的智能机器人跟随方法。
背景技术
随着人工智能、机器人技术的融合与发展，机器人变得更加智能化并逐渐渗透到健康医疗、家居服务等行业。与传统的工业机器人不同，服务机器人的工作环境更为复杂，并且需要与人们互动以提供有效的服务，表现出良好的沟通技巧。由于目前家庭服务和医疗保健的需求日益增长，人们对服务机器人安全性、功能性和灵活性的要求越来越高。而人机协作能力将成为下一代智能机器人的必要属性之一，有着广泛而良好的应用前景。
人体目标识别与跟踪是服务机器人实现人机协作的前提和基础，而设计性能良好的人体位姿估计器是其重要工作之一。考虑到人体目标跟踪过程涉及的非线性滤波问题，扩展卡尔曼滤波方法是一种应用广泛的非线性滤波方法。特别的，通过引入其信息形式，即可得扩展信息滤波方法，这样不仅可避免获取精确的初始状态信息，而且在量测不相关的前提下，其量测更新过程可分解为若干个子过程累加的形式，滤波结构更加灵活，利于融合不同传感器的量测数据。然而，与扩展卡尔曼滤波方法类似，扩展信息滤波方法同样需要计算雅可比矩阵且存在一阶截断误差，特别是当系统具有强非线性时，会引入较大的线性化误差，破坏滤波器的稳定性甚至发散，从而导致机器人无法完成人机协作任务。另一方面，无迹卡尔曼滤波等方法通过估计状态不确定区域内的一系列回归点来线性化系统状态和量测模型，这样，无需计算非线性函数的雅克比矩阵。相比于扩展卡尔曼滤波，无迹卡尔曼方法理论上拟合精度可达二阶以上，具有更高的滤波精度。然而，无迹卡尔曼滤波的方法性能易受参数取值的影响，尤其是当系统具有高维数时协方差矩阵易失去正定性，导致滤波性能不佳甚至出现滤波发散的现象。而通过径向容积准则求解数值积分来得到系统的状态估计，可一定程度减少协方差不正定情形的发生。然而，由于线性化误差、数值计算误差等因素影响，还是不可避免地引起滤波器的不稳定。目前尚未有人体目标跟踪方法可以克服上述因素的影响，保证滤波器的稳定性，使得机器人可以安全、灵活的实现人机协作。
发明内容
为了克服现有服务机器人跟随方法的精度较低、稳定性较差的不足，本发明提供了一种基于渐进容积信息滤波的机器人跟随方法，在跟踪精度得到提高的前提下，增强机器人跟随系统的稳定性。
为实现上述目的，本发明所采用的技术方案是：
一种基于渐进容积信息滤波的机器人跟随方法，所述方法包括以下步骤：
步骤1，初始化参数，包括k＝0时刻跟随系统的状态向量状态协方差P0|0以及过程噪声协方差Qk，并将量测噪声协方差Rk放大N倍；
步骤2，机器人获取k时刻传感器的量测信息，根据球面径向规则，由k-1时刻估计状态和协方差Pk-1|k-1构造Sigma点σi,k-1|k-1,i＝1,2,…,L，从而获得k时刻跟随系统的预测状态及协方差Pk|k-1；
步骤3，引入信息矩阵Yk|k-1及信息状态向量对量测更新过程进行迭代，根据k时刻预测状态及协方差Pk|k-1得迭代初始状态和，已知迭代过程中状态和协方差构造Sigma点从而获得k时刻的量测预测值及其互协方差
步骤4，根据步骤3获得的量测预测值与互协方差结合k时刻机器人获取的传感器量测信息zk计算得第n次迭代更新后的信息矩阵及信息状态向量
步骤5，根据步骤4获得的信息矩阵及信息状态向量得第n次迭代更新后跟随系统的状态估计与协方差
步骤6，判断n≤N是否成立，若满足，执行步骤3-步骤6；否则，进入下一时刻跟随系统的状态估计，执行步骤2-步骤6。
进一步，所述步骤1中，定义k时刻跟随系统状态向量其中，为k时刻移动机器人的状态向量，且 和分别为k时刻移动机器人在x和y轴上的位置，和为其在x和y轴上的速度；为k时刻跟随对象的状态向量，且 和分别为k时刻跟随对象在x和y轴上的位置，和为其在x和y轴上的速度，上标T表示矩阵转置。
更进一步，所述步骤1中，根据经验初始化k＝0时刻跟随系统的状态向量状态协方差P0|0；根据跟随系统的状态模型初始化过程噪声协方差Qk；根据传感器的量测精度初始化量测噪声协方差Rk。
所述步骤2中，所述传感器量测信息为机器人相对跟随对象的距离及方位，且L＝2d，d为跟随系统状态向量xk的维数。
本发明的技术构思为：首先，该方法将量测更新过程分解为若干个子过程，利于多个不同传感器信息的融合；其次，在量测更新过程中渐近地引入量测信息，使其得到充分利用，提高了人体目标跟踪精度；另外，通过增大观测噪声协方差，使得渐进过程中协方差矩阵保持正定性，从而保证滤波器的稳定性。
本发明的有益效果主要表现在：相比现有的机器人跟随方法，该方法充分考虑了线性化误差及外部因素的影响，在保证计算复杂度的前提下，提高了机器人跟随的精度和鲁棒性。
附图说明
图1是服务机器人跟随系统示意图，其中，1为跟踪目标，2为跟随机器人，3为激光传感器，4为超声波传感器。
图2是本发明方法流程图。
具体实施方式
为使本发明的技术方案和优点更加清晰，下面结合附图对本发明作进一步描述。应当理解，此处所描述的具体实施案例仅仅用以解释本发明，并不用于限定本发明。
参照图1和图2，一种基于渐近容积信息滤波的机器人跟随方法，如图1所示，服务机器人通过激光传感器以及超声波传感器获得跟随对象的位置以及移动信息。跟随系统的状态空间模型如公式(1)所示，传感器量测模型如公式(2)：
其中，为k时刻的跟随系统状态，k＝1,2...。和分别为k时刻移动机器人在x和y轴上的位置，和为其在x和y轴上的速度；和分别为k时刻跟随对象在x和y轴上的位置，和为其在x和y轴上的速度。Ω为角速度，ΔT为采样周期，上标T表示矩阵转置。wk为零均值且协方差为Qk的过程噪声。zk为k时刻跟随机器人所得的传感器量测值，vk为零均值、方差为Rk的量测噪声，且与过程噪声wk不相关。
结合附图2，一种基于渐近容积信息滤波的移动机器人跟随方法，包括以下步骤：
步骤1，根据经验初始化k＝0时刻跟随系统的状态向量状态协方差P0|0；根据跟随系统的状态模型初始化过程噪声协方差Qk；根据传感器的量测精度初始化量测噪声协方差Rk并将其放大N倍；
步骤2，机器人获取k时刻传感器的量测信息，根据球面径向规则，由k-1时刻估计状态和协方差Pk-1|k-1构造Sigma点σi,k-1|k-1,i＝1,2,…,L，构造方式如公式(4)所示，从而获得k时刻跟随系统的预测状态及协方差Pk|k-1，获得方式如公式(6)、公式(7)所示：
Sk-1|k-1＝chol(Pk-1|k-1) (3)
公式(3)中chol(·)表示矩阵的乔洛斯基分解，公式(4)中ξi表示矩阵ξ的第i列，矩阵ξ的定义如公式(5)所示，其中d＝8，Id为d维的单位矩阵，公式(6)、公式(7)中L＝16，fk(·)表示跟随系统的状态转移函数；
步骤3，引入信息矩阵Yk|k-1及信息状态向量对量测更新过程进行迭代，引入方式如公式(8)、公式(9)所示，根据第n次迭代过程中状态和协方差构造Sigma点构造方式如公式(11)所示，结合k时刻机器人获取的传感器量测信息zk从而获得第n次迭代过程中量测预测值及其互协方差获得方式如公式(12)、公式(13)所示：
其中，hk(·)表示传感器量测函数，迭代初始状态和分别如公式(14)、公式(15)所示：
步骤4，根据步骤3获得的量测预测值与互协方差结合k时刻的传感器量测信息zk计算得n次更新后的信息矩阵及信息状态向量计算方式如公式(16)、公式(17)：
其中，
步骤5，根据步骤4获得的信息矩阵及信息状态向量得到迭代更新后的目标后验状态向量与协方差
步骤6，判断n≤N是否成立，若满足，执行步骤3-步骤6；否则，进入下一时刻跟随系统的状态估计，执行步骤2-步骤6。

Claims (4)

1.一种基于渐进容积信息滤波的机器人跟随方法，其特征在于，所述方法包括以下步骤：
步骤1，初始化参数，包括k＝0时刻跟随系统的状态向量状态协方差P0|0以及过程噪声协方差Qk，并将量测噪声协方差Rk放大N倍；
步骤2，机器人获取k时刻传感器的量测信息，根据球面径向规则，由k-1时刻估计状态和协方差Pk-1|k-1构造Sigma点σi,k-1|k-1,i＝1,2,…,L，从而获得k时刻跟随系统的预测状态及协方差Pk|k-1；
步骤3，引入信息矩阵Yk|k-1及信息状态向量对量测更新过程进行迭代，根据k时刻预测状态及协方差Pk|k-1得迭代初始状态和已知迭代过程中状态和协方差构造Sigma点从而获得k时刻的量测预测值及其互协方差
步骤4，根据步骤3获得的量测预测值与互协方差结合k时刻机器人获取的传感器量测信息zk计算得第n次迭代更新后的信息矩阵及信息状态向量
步骤5，根据步骤4获得的信息矩阵及信息状态向量得第n次迭代更新后跟随系统的状态估计与协方差
步骤6，判断n≤N是否成立，若满足，执行步骤3-步骤6；否则，进入下一时刻跟随系统的状态估计，执行步骤2-步骤6。
2.如权利要求1所述的一种基于渐进容积信息滤波的机器人跟随方法，其特征在于：所述步骤1中，k时刻跟随系统状态向量其中，为k时刻移动机器人的状态向量，且 和分别为k时刻移动机器人在x和y轴上的位置，和为其在x和y轴上的速度；为k时刻跟随对象的状态向量，且 和分别为k时刻跟随对象在x和y轴上的位置，和为其在x和y轴上的速度，上标T表示矩阵转置。
3.如权利要求1或2所述的一种基于渐进容积信息滤波的机器人跟随方法，其特征在于：在所述步骤1中，根据经验初始化k＝0时刻跟随系统的状态向量状态协方差P0|0；根据跟随系统的状态模型初始化过程噪声协方差Qk；根据传感器的量测精度初始化量测噪声协方差Rk。
4.如权利要求1或2所述的一种基于渐进容积信息滤波的机器人跟随方法，其特征在于：所述步骤2中，传感器量测信息为机器人相对跟随对象的距离及方位，且L＝2d，d为跟随系统状态向量xk的维数。
CN201810365081.3A 2018-04-20 2018-04-20 一种基于渐进容积信息滤波的机器人跟随方法 Active CN108710295B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810365081.3A CN108710295B (zh) 2018-04-20 2018-04-20 一种基于渐进容积信息滤波的机器人跟随方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810365081.3A CN108710295B (zh) 2018-04-20 2018-04-20 一种基于渐进容积信息滤波的机器人跟随方法

Publications (2)

Publication Number Publication Date
CN108710295A true CN108710295A (zh) 2018-10-26
CN108710295B CN108710295B (zh) 2021-06-18

Family

ID=63867406

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810365081.3A Active CN108710295B (zh) 2018-04-20 2018-04-20 一种基于渐进容积信息滤波的机器人跟随方法

Country Status (1)

Country Link
CN (1) CN108710295B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110174907A (zh) * 2019-04-02 2019-08-27 诺力智能装备股份有限公司 一种基于自适应卡尔曼滤波的人体目标跟随方法
CN114131604A (zh) * 2020-08-26 2022-03-04 北京市商汤科技开发有限公司 机器人的状态确定方法及装置、机器人及存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB2181240A (en) * 1985-10-05 1987-04-15 Plessey Co Plc A method of detecting sound impulses
CN102411371A (zh) * 2011-11-18 2012-04-11 浙江大学 一种基于多传感器服务机器人跟随系统和方法
CN103268403A (zh) * 2013-04-25 2013-08-28 杭州电子科技大学 一种基于容积强跟踪信息滤波器的目标跟踪方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB2181240A (en) * 1985-10-05 1987-04-15 Plessey Co Plc A method of detecting sound impulses
CN102411371A (zh) * 2011-11-18 2012-04-11 浙江大学 一种基于多传感器服务机器人跟随系统和方法
CN103268403A (zh) * 2013-04-25 2013-08-28 杭州电子科技大学 一种基于容积强跟踪信息滤波器的目标跟踪方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
CHRISTOF CHLEBEK.ETAL: "Progressive Gaussian Filter Using Importance Sampling and Particle Flow", 《 19TH INTERNATIONAL CONFERENCE ON INFORMATION FUSION (FUSION)》 *
YULONG HUANG.ETAL: "Gaussian approximate filter with progressive measurement update", 《2015 IEEE 54TH ANNUAL CONFERENCE ON DECISION AND CONTROL (CDC)》 *
谢会来: "高斯混合交互式多模型容积信息滤波算法", 《传感器与微系统》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110174907A (zh) * 2019-04-02 2019-08-27 诺力智能装备股份有限公司 一种基于自适应卡尔曼滤波的人体目标跟随方法
CN114131604A (zh) * 2020-08-26 2022-03-04 北京市商汤科技开发有限公司 机器人的状态确定方法及装置、机器人及存储介质
CN114131604B (zh) * 2020-08-26 2023-11-03 北京市商汤科技开发有限公司 机器人的状态确定方法及装置、机器人及存储介质

Also Published As

Publication number Publication date
CN108710295B (zh) 2021-06-18

Similar Documents

Publication Publication Date Title
CN105549049B (zh) 一种应用于gps导航的自适应卡尔曼滤波算法
CN102402225B (zh) 一种实现移动机器人同时定位与地图构建的方法
CN110530365B (zh) 一种基于自适应卡尔曼滤波的人体姿态估计方法
CN107704432A (zh) 一种转移概率自适应的交互多模型目标跟踪方法
CN108762309A (zh) 一种基于假设卡尔曼滤波的人体目标跟随方法
CN104182609B (zh) 基于去相关的无偏转换量测的三维目标跟踪方法
CN100585622C (zh) 基于高斯混合模型的人体跟踪方法
CN108710295A (zh) 一种基于渐进容积信息滤波的机器人跟随方法
CN106772351B (zh) 基于有限步记忆的卡尔曼滤波方法
CN106323291A (zh) 一种基于平方根容积卡尔曼滤波的多机器人协同定位算法
CN113763549B (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
CN111291471B (zh) 一种基于l1正则无迹变换的约束多模型滤波方法
CN108061887A (zh) 一种基于模糊交互式多模型算法的临近空间目标跟踪方法
CN110517286A (zh) 基于多智能体控制的单目标动态跟踪与围捕方法
CN112328959B (zh) 一种基于自适应扩展卡尔曼概率假设密度滤波器的多目标跟踪方法
WO2023168849A1 (zh) 机械臂动作捕捉方法、介质、电子设备及系统
CN110287178A (zh) 一种基于数据差值的桥梁渐进式漂移数据清洗方法
CN112379667A (zh) 领导跟随二阶多智能体系统有限时间一致性跟踪控制方法
CN108347694B (zh) 一种基于边界条件的节点定位方法及系统
CN110702093B (zh) 基于粒子滤波的定位方法、装置、存储介质及机器人
CN108680162A (zh) 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法
CN110099443B (zh) 一种无线传感器网络中节点追踪的负载均衡方法
CN117621060A (zh) 一种环境感知的足式机器人落足控制方法及系统
CN108169722A (zh) 一种未知干扰影响下传感器的系统偏差配准方法
CN111339494A (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