申请/专利权人：浙江工业大学

申请日：2018-05-03

公开（公告）日：2021-05-18

公开（公告）号：CN108762309B

主分类号：G05D1/12(20060101)

分类号：G05D1/12(20060101)

优先权：

专利状态码：有效-授权

法律状态：2021.05.18#授权;2018.11.30#实质审查的生效;2018.11.06#公开

摘要：一种基于假设卡尔曼滤波的人体目标跟随方法，包括如下步骤：1通过移动机器人与人的相对位置关系建立机器人跟随模型；2利用激光雷达获取目标的二维信息，提取出人体的腿部的几何特征，并用支持向量机方法来训练和识别；3应用假设卡尔曼滤波方法对机器人进行运动滤波，使移动机器人能够平稳运行；4移动机器人控制器的设计。针对移动机器人运动模型建立了系统状态方程和观测方程，并考虑由于人的运动模型是未知的，因此在每一时刻的估计位置附近选取多个假设的预测位姿，使用假设卡尔曼滤波器，运用支持向量机方法极大提高了人体的识别率，保证了移动机器人能够很好的跟随人体，估计结果可以满足实际应用的精度与实时性要求。

主权项：1.一种基于假设卡尔曼滤波的人体目标跟随方法，其特征在于，所述方法包括以下步骤：1通过移动机器与人的相对位置关系建立机器人跟随模型；2利用激光雷达获取目标的二维信息，提取出人体的腿部的几何特征，并用支持向量机方法来训练和识别；3应用假设卡尔曼滤波方法对机器人进行运动滤波，使移动机器人能够平稳运行；所述步骤3中，运动滤波包括以下步骤：3.1在每一时刻的估计位置附近选取多个假设的预测位姿，选取多个假设的预测位姿，记为：xk|k-1＝{xik|k-1,xi+1k|k-1,xi+2k|k-1...}4其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i，i+1，i+2...为相应估计器的索引，xk|k-1为状态预测值，xik|k-1,xi+1k|k-1,xi+2k|k-1...为k时刻的预测跟随状态；状态初始化，获取机器人与人之间位置的初始状态P0|0，为初始状态，而P0|0为协方差矩阵；3.2状态预测，计算状态以及Pk|k-1： Pk|k-1＝Pk-1|k-1+Qk-16其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，为k时刻的预测跟随状态，Qk-1的状态噪声方差阵，uik-1＝[u1k-1u2k-1]T为控制输入，A为二维单位阵，为状态转移矩阵，B是将输入转换为状态的矩阵，Pk|k-1为k-1时刻的协方差预测值，Pk-1|k-1为k-1时刻的估计值；3.3量测更新，计算 Γk＝HkPk|k-1HTk8其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，为k时刻的预测观测，为k时刻的预测跟随状态；H为二维单位阵，Pk|k-1为k-1时刻的协方差预测值，Pk-1|k-1为k-1时刻的估计值，Γk为中间变量；3.4状态更新，计算Kk＝Pk|k-1HTkΓk+Rk-19 Pk|k＝Pk|k-1-KkHkPk|k-111其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，zk为实际的观测，为k时刻的预测观测，为k时刻的预测跟随状态，为当前时刻估计的状态，H为二维单位阵，Rk为状态噪声协方差矩阵，Pk|k为k时刻的协方差值，Pk|k-1为k时刻的预测值，Γk为中间变量；3.5设计一种假设Kalman滤波器方法，在时刻k，由上一时刻的状态，来假设多个目标的可能状态，并通过马氏距离来找到最优的估计值，其中马氏距离由下式表示： 其中，k表示当前离散化时刻，k-1表示上一离散化时刻，为当前时刻估计的状态，为当前时刻状态的预测值，ΣX为协方差矩阵；3.6对于xk|k-1＝{xik|k-1,xi+1k|k-1,xi+2k|k-1...}通过式5，6，7，8，9，10，11，12计算Mjk，通过min{Mjk}得到对应的j值，从而得到最优的估计： 其中，k表示当前离散化时刻，j为假设的个数，k-1表示上一离散化时刻，i，i+1，i+2...为相应估计器的索引，xk|k-1为状态预测值，xik|k-1,xi+1k|k-1,xi+2k|k-1...为k时刻的预测跟随状态，min{Mjk}为计算出的所有马氏距离中最小的那一个，为当前时刻估计的状态，为第j个状态估计值；4移动机器人控制器的设计；所述步骤4中，控制器的设计包括以下步骤：4.1在移动机器人人体跟随系统中，移动机器人的运动控制采用PID控制： 其中，⊙为Hadamard积，k表示当前离散化时刻xk＝[θkdk]T，uk＝[u1ku2k]T，u1k，u2k为控制输入θk、dk为k时刻机器人的跟随偏角和跟随距离；KPk＝[KP1kKP2k]，TIk＝[TI1kTI2k]，Kp，TI分别为比例系数参数和积分系数参数，k＝1,2...为时间序列；4.2则移动机器人的左右轮速度为： 其中，k表示当前离散化时刻，vLk为左轮速度，vRk为右轮速度，u1k，u2k为控制输入。

全文数据：一种基于假设卡尔曼滤波的人体目标跟随方法技术领域[0001]本发明应用于移动机器人目标跟随领域，涉及到移动机器人人体目标跟随问题，尤其是人体目标实时的位姿估计，消除激光雷达引起的传感器测量误差，实现了一种有效的实时人体目标跟随方法。背景技术[0002]随着科技技术的发展，机器人机器在安全、护理、家务劳动、救援和人机交互等方面的应用也越来越广泛。而在人机交互方面，尤其是服务机器人，人体的跟随也是其中重要的环节，因为有很多可能的情况下机器人需要跟随一个人，比如搬运大型货物、跟随行李箱和医疗协作等，在工业领域和民事领域均体现出极高的研究和应用价值。[0003]本发明主要考虑移动机器人如何有效地进行人体目标的跟随并避免激光传感器带来的量测噪声。主要人体目标跟随使用的方法包括基于视觉的跟随方法、基于激光雷达的跟随方法和基于多传感器融合的跟随方法。基于视觉的跟随方法在没有干扰的情况下可以进行有效的跟随，但是视觉目标跟随经常受到光照、外观改变和背景等因素的影响，基于多传感器融合的跟随方法模型复杂，成本较高，而基于激光雷达的目标跟随方法能够很好的应对以上问题。[0004]在进行人体目标位姿估计时，由于传感器测量误差带来的影响，通常借助卡尔曼滤波算法来进行估计，此类方法处理随机干扰时具有数据储存量小，算法容易实现，且实施成本低等优点，而由于人体目标运动模型的不确定性，采用普通的卡尔曼滤波算法往往会影响人体位姿的估计效果。发明内容[0005]为了克服现有移动机器人人体目标跟随方法的鲁棒性较差、人体位姿的估计效果较差、识别率较低的不足，本发明提供一种基于假设卡尔曼滤波的人体目标跟随方法，该方法在保证系统鲁棒性的前提下对人体位姿进行准确的估计，能够满足移动机器人有效地跟随人体的应用需求。[0006]本发明为解决上述技术问题提供了如下解决方案：[0007]—种基于假设卡尔曼滤波的人体目标跟随方法，包括以下步骤：[0008]1通过移动机器与人的相对位置关系建立机器人跟随模型；[0009]2利用激光雷达获取目标的二维信息，提取出人体的腿部的几何特征，并用支持向量机方法来训练和识别；[0010]3应用假设卡尔曼滤波方法对机器人进行运动滤波，使移动机器人能够平稳运行；[0011]4移动机器人控制器的设计。[0012]进一步，步骤1中，通过移动机器与人的相对位置关系建立机器人跟随模型的方法主要包括以下步骤：[0013]1.1考虑两轮差速轮式移动机器人，则系统的状态空间模型可描述为：[0014]Xk=Axk_l+Buk_l+wk_lI[0015]其中，k表示当前离散化时刻，k-1表示上一离散化时刻，XkSk时刻的跟随状态，Xk=[0k，dk]T，0k，dk分别为移动机器人的跟随偏角和跟随距离。wk-l是均值和方差阵分别为qk-Ι和Qk-Ι的状态噪声，uk-1=[mk-1U2k-1]τ为控制输入，A为状态转移矩阵，是将输入转换为状态的矩阵，其中△t为系统运行周期，dr为两驱动轮间距。[0016]1.2系统的观测方程为：[0017]zk=Hxk+Vk⑵[0018]其中，其中，k表示当前离散化时刻，k-ι表示上一离散化时刻，系统状态观测zk=[0kdk]T，H为观测矩阵，Vk是零均值且协方差阵为Rk的高斯噪声且与过程噪声wk不相关。[0019]进一步，步骤2中利用激光雷达获取目标的二维信息，提取出人体的腿部的几何特征，并用支持向量机方法来训练和识别，包括以下步骤：[0020]2.1激光扫描出来的人腿形状为类圆弧形，考虑数据集U,.y,C1，由于人腿的特殊性，激光扫描出来的人腿形状有一些几何学上的特征，通过提取人腿形状的一些有效的特征来识别人腿，通过激光扫描可以得到人体腿部的聚类数据，我们用几个符号来表示聚类数据的特征，W表示激光聚类数据的宽度第一个与最后一个点之间的距离），G代表聚类数据的周长依次连接每个点得到的总的距离），D代表激光聚类数据的深度，Θ表示最近的点和第一个点连接的直线以及和最远点连接的直线形成的角度，另外本文取GW作为第五个特征。每一个输入^表示腿部聚类数据的特征输入，且都对应一个方式标签{-1，+1}，SVM能够找到一个超平面来分离两类数据。非线性支持向量机是求以下对偶问题：[0021]3[0022]其中，ai，aj为拉格朗日乘子，Kxi，xj为核函数#，7卢{-1，+1}。1匕山）=6叉口-|Xl-Xj|122〇2为高斯核函数，N为数据的总数〇[0023]2.2使用带高斯内核的支持向量机算法对训练集进行训练并在测试集上测试训练模型;计算精确率Precision、召回率^Recall和Fl-score，得到最优的特征选择。[0024]2.3应用支持向量机预测模型对运动区域中的聚类进行预测，得到对应的标签输出y1;判定相应的聚类是否为人体腿部数据并找到相应的腿部聚类，计算聚类中心。[0025]进一步，步骤3应用假设卡尔曼滤波方法对机器人进行运动滤波，使移动机器人能够平稳运行，包括以下步骤：[0026]3.1由于人的运动模型是未知的，如果使用匀速模型或者加速度模型会存在较大误差。因此在每一时刻的估计位置附近选取多个假设的预测位姿，我们选取多个假设的预测位姿，记为：[0027]XkIk_l={x1kIk_l，x1+1kIk_l，x1+2kIk_l···}⑷[0028]其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i，i+1，i+2...为相应估计器的索引，Xk|k-1为状态预测值，X1IiIk-I，x1+1k|k-l，x1+2k|k-l...为k时刻的预测跟随状态。[0029]状态初始化，获取机器人与人之间位置的初始状态10，P010。科:〇:.1〇为初始状态，而p〇I〇为协方差矩阵。[0030]3.2状态预测，计算状态#到々-1以及？〇^-1。[0031]μ-I=Ax{k-]\V-\+Biik~\5[0032]PkIk-1=Pk-11k-1+Qk-16[0033]其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，f灸μ-1为k时刻的预测跟随状态。Qk-Ι的状态噪声方差阵，U1k-1=[U1k-1U2k-1]7为控制输入，A为二维单位阵，为状态转移矩阵，B是将输入转换为状态的矩阵，PkIk-1为k-Ι时刻的协方差预测值，Pk-11k-Ι为k-Ι时刻的估计值。[0034]3·3量测更新，计算？I-1，Fk[0035]£i：i|i-l=iKikik-l7[0036]Γk=HkPkIk-1Htk⑶[0037]其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，1为k时刻的预测观测，:e;fI「-1为k时刻的预测跟随状态。H为二维单位阵，Pkk-1为k-1时刻的协方差预测值，Pk-11k-1为k-1时刻的估计值，Γk为中间变量。[0038]3·4状态更新，计算.η*μ,PkIk[0039]Kk=PkIk-1HtkΓk+RkΓ19[0040]26[0108]PkIk=PkIk-1-KkHkPkIk-127[0109]其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，zk为实际的观测，打*丨（-1为k时刻的预测观测，|;太μ-1为k时刻的预测跟随状态，E⑷衫为当前时刻估计的状态，为状态噪声协方差矩阵，Pk|k为k时刻的协方差值，PkIk-ι为k时刻的预测值，Γk为中间变量。[0110]3.5由于人在运动过程中的方位每一时刻都可能发生变化突然左右转弯等），仅使用基本的Kalman滤波算法往往得不到理想的效果。为此，设计一种假设Kalman滤波器方法，在时刻k，由上一时刻的状态，来假设多个目标的可能状态，并通过马氏距离来找到最优的估计值。其中马氏距离由下式表示：[0111]28[0112]其中，k表示当前离散化时刻，k-i表示上一离散化时刻，为当前时刻估计的状态，耶μ-ι为当前时刻状态的预测值，ςχ为协方差矩阵。[0113]3.6对于XkIk-1={x1kIk-1，x1+1kIk-1，x1+2kIk-Ι…}通过式5，（6，7，（8，（9，（10，（11，（12计算Mjk，通过min{Mjk}得到对应的j值，从而得到最优的估计：[0114]xc|r=.r'f|〇29[0115]其中，k表示当前离散化时刻，j为假设的个数，k-1表示上一离散化时刻，i，i+1，i+2...为相应估计器的索弓丨，XkIk-1为状态预测值，X1kIk-1，X1+1kIk-1，X1+2kIk-1...为k时刻的预测跟随状态，min{M\k}为计算出的所有马氏距离中最小的那一个，fcμ为当前时刻估计的状态，ΐνμ为第j个状态估计值。[0116]进一步，步骤4移动机器人控制器设计，控制器的设计包括以下步骤：[0117]4.1在移动机器人人体跟随系统中，移动机器人的运动控制采用PID控制：[0118]30[0119]其中，〇为Hadamard积，k表示当前离散化时刻，Xk=[Θkdk]T，uk=[uiku2k]T，mk，u2k为控制输入0k、dk为k时刻机器人的跟随偏角和跟随距离。Kpk=[KP1kKP2k]=[45T12k]=[0.50.1]，Kp，Τώ、别为比例系数参数和积分系数参数，k=1，2...为时间序列。[0120]4.2则移动机器人的左右轮速度为：[0123]其中，k表示当前离散化时刻，VLk为左轮速度，VRk为右轮速度，mk，U2k为控制输入。[0124]以上结合附图详细说明和陈述了本发明的实施方式，但并不局限于上述方式。在本领域的技术人员所具备的知识范围内，只要以本发明的构思为基础，还可以做出多种变化和改进。

权利要求：1.一种基于假设卡尔曼滤波的人体目标跟随方法，其特征在于，所述方法包括以下步骤：1通过移动机器与人的相对位置关系建立机器人跟随模型；2利用激光雷达获取目标的二维信息，提取出人体的腿部的几何特征，并用支持向量机方法来训练和识别；3应用假设卡尔曼滤波方法对机器人进行运动滤波，使移动机器人能够平稳运行；4移动机器人控制器的设计。2.如权利要求1所述的一种基于假设卡尔曼滤波的人体目标跟随方法，其特征在于，所述步骤1中，通过移动机器与人的相对位置关系建立机器人跟随模型的方法包括以下步骤：1.1考虑两轮差速轮式移动机器人，则系统的状态空间模型可描述为：其中，k表示当前离散化时刻，k-1表示上一离散化时刻，》为k时刻的跟随状态，Xk=[01{，11^，01{，11{分别为移动机器人的跟随偏角和跟随距离^卜1是均值和方差阵分别为1k-Ι和Qk-Ι的状态噪声，uk-1=[mk-1u2k-1]τ为控制输入，A为状态转移矩阵，是将输入转换为状态的矩阵，其中At为系统运行周期，dr为两驱动轮间距；1.2系统的观测方程为：zk=Hxk+Vk2其中，其中，k表示当前离散化时刻，k-i表示上一离散化时刻，系统状态观测Z⑹=[ekdk]T，H为观测矩阵，V⑹是零均值且协方差阵为R⑹的高斯噪声且与过程噪声W⑹不相关。3.如权利要求1或2所述的一种基于假设卡尔曼滤波的人体目标跟随方法，其特征在于，所述步骤2包括以下步骤：2.1激光扫描出来的人腿形状为类圆弧形，考虑数据集!vy,匕，W表示激光聚类数据的宽度，即第一个与最后一个点之间的距离，G代表聚类数据的周长，即依次连接每个点得到的总的距离，D代表激光聚类数据的深度，Θ表示最近的点和第一个点连接的直线以及和最远点连接的直线形成的角度，取GW作为第五个特征;每一个输入X1表示腿部聚类数据的特征输入，且都对应一个方式标签{-1，+1}，SVM能够找到一个超平面来分离两类数据，非线性支持向量机是求以下对偶问题：其中，ai，aj为拉格朗日乘子，KXi，Xj为核函数，为高斯核函数，N为数据的总数；2.2使用带高斯内核的支持向量机算法对训练集进行训练并在测试集上测试训练模型;计算精确率、召回率和Fl-score，得到最优的特征选择；2.3应用支持向量机预测模型对运动区域中的聚类进行预测，得到对应的标签输出y1;判定相应的聚类是否为人体腿部数据并找到相应的腿部聚类，计算聚类中心。4.如权利要求1或2所述的一种基于假设卡尔曼滤波的人体目标跟随方法，其特征在于，所述步骤3中，运动滤波包括以下步骤：3.1在每一时刻的估计位置附近选取多个假设的预测位姿，选取多个假设的预测位姿，记为：其中，k表示当前离散化时刻，k-Ι表示上一离散化时刻，i，i+1，i+2...为相应估计器的索引，XkIk-Ι为状态预测值...为k时刻的预测跟随状态；状态初始化，获取机器人与人之间位置的初始状态训1〇，P〇I〇，训I〇为初始状态，而P〇I〇为协方差矩阵；3.2状态预测，计算状态μ-1以及PkIk-i:其中，k表示当前离散化时刻，k-Ι表示上一离散化时刻，i为第i个估计器，α叫-1为k时刻的预测跟随状态，Qk-l的状态噪声方差阵，Uik-1=[mk-lu2k-1]τ为控制输入，A为二维单位阵，为状态转移矩阵，B是将输入转换为状态的矩阵，PkIk-1为k-1时刻的协方差预测值，Pk-11k-Ι为k-Ι时刻的估计值；3.3量测更新，计算其中，k表示当前离散化时刻，k-Ι表示上一离散化时刻，i为第i个估计器为k时刻的预测观测，为k时刻的预测跟随状态;H为二维单位阵，Pk|k-Ι为k-1时刻的协方差预测值，Pk-11k-1为k-1时刻的估计值，Γ⑹为中间变量；3·4状态更新，计算IPkIk其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，zk为实际的观测，为k时刻的预测观测，％1^时刻的预测跟随状态，为当前时刻估计的状态，H为二维单位阵，R⑹为状态噪声协方差矩阵，PkIk为k时刻的协方差值，PkIk-Ι为k时刻的预测值，Γ⑹为中间变量；3.5设计一种假设Kalman滤波器方法，在时刻k，由上一时刻的状态，来假设多个目标的可能状态，并通过马氏距离来找到最优的估计值，其中马氏距离由下式表示：其中，k表示当前离散化时刻，k-1表示上一离散化时刻，辦iIi:为当前时刻估计的状态，1-1为当前时刻状态的预测值，ΣX为协方差矩阵；3.6对于通过式5，（6，（7，8，（9，（10，（11，（12计算，⑹，通过minM⑹}得到对应的j值，从而得到最优的估计：其中，k表示当前离散化时刻，j为假设的个数，k-Ι表示上一离散化时刻，i，i+1，i+2...为相应估计器的索引，XkIk-1为状态预测值，...为k时刻的预测跟随状态，min{MWk}为计算出的所有马氏距离中最小的那一个，.ΐμ为当前时刻估计的状态，I〃为第j个状态估计值。5.如权利要求1或2所述的一种基于假设卡尔曼滤波的人体目标跟随方法，其特征在于，所述步骤4中，控制器的设计包括以下步骤：4.1在移动机器人人体跟随系统中，移动机器人的运动控制采用PID控制：其中，θ为Hadamard积，k表示当前离散化时刻Xk=[Θkdk]T，uk=[uikU2⑹]T，m⑹，U2⑹为控制输入0k、dk为k时刻机器人的跟随偏角和跟随距离;Kpk=[KP1kKP2⑹]，T1⑹=[T11⑹T12⑹]，ΚΡ，Τών别为比例系数参数和积分系数参数，k=l，2...为时间序列；4.2则移动机器人的左右轮速度为：其中，k表示当前离散化时刻，VL⑹为左轮速度，VR⑹为右轮速度，Ul⑹，U2⑹为控制输入。

百度查询： 浙江工业大学 一种基于假设卡尔曼滤波的人体目标跟随方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD