CN108762309A - 一种基于假设卡尔曼滤波的人体目标跟随方法 - Google Patents

Abstract

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

Description

一种基于假设卡尔曼滤波的人体目标跟随方法

技术领域

本发明应用于移动机器人目标跟随领域，涉及到移动机器人人体目标跟随问题，尤其是人体目标实时的位姿估计，消除激光雷达引起的传感器测量误差，实现了一种有效的实时人体目标跟随方法。

背景技术

随着科技技术的发展，机器人机器在安全、护理、家务劳动、救援和人机交互等方面的应用也越来越广泛。而在人机交互方面，尤其是服务机器人，人体的跟随也是其中重要的环节，因为有很多可能的情况下机器人需要跟随一个人，比如搬运大型货物、跟随行李箱和医疗协作等，在工业领域和民事领域均体现出极高的研究和应用价值。

本发明主要考虑移动机器人如何有效地进行人体目标的跟随并避免激光传感器带来的量测噪声。主要人体目标跟随使用的方法包括基于视觉的跟随方法、基于激光雷达的跟随方法和基于多传感器融合的跟随方法。基于视觉的跟随方法在没有干扰的情况下可以进行有效的跟随，但是视觉目标跟随经常受到光照、外观改变和背景等因素的影响，基于多传感器融合的跟随方法模型复杂，成本较高，而基于激光雷达的目标跟随方法能够很好的应对以上问题。

在进行人体目标位姿估计时，由于传感器测量误差带来的影响，通常借助卡尔曼滤波算法来进行估计，此类方法处理随机干扰时具有数据储存量小，算法容易实现，且实施成本低等优点，而由于人体目标运动模型的不确定性，采用普通的卡尔曼滤波算法往往会影响人体位姿的估计效果。

发明内容

为了克服现有移动机器人人体目标跟随方法的鲁棒性较差、人体位姿的估计效果较差、识别率较低的不足，本发明提供一种基于假设卡尔曼滤波的人体目标跟随方法，该方法在保证系统鲁棒性的前提下对人体位姿进行准确的估计，能够满足移动机器人有效地跟随人体的应用需求。

本发明为解决上述技术问题提供了如下解决方案：

一种基于假设卡尔曼滤波的人体目标跟随方法，包括以下步骤：

1)通过移动机器与人的相对位置关系建立机器人跟随模型；

2)利用激光雷达获取目标的二维信息，提取出人体的腿部的几何特征，并用支持向量机方法来训练和识别；

3)应用假设卡尔曼滤波方法对机器人进行运动滤波，使移动机器人能够平稳运行；

4)移动机器人控制器的设计。

进一步，步骤1)中，通过移动机器与人的相对位置关系建立机器人跟随模型的方法主要包括以下步骤：

(1.1)考虑两轮差速轮式移动机器人，则系统的状态空间模型可描述为：

x(k)＝Ax(k-1)+Bu(k-1)+w(k-1) (1)

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，x_k为k时刻的跟随状态，x_k＝[θ_k,d_k]^T，θ_k,d_k分别为移动机器人的跟随偏角和跟随距离。w(k-1)是均值和方差阵分别为q(k-1)和Q(k-1)的状态噪声，u(k-1)＝[u₁(k-1)u₂(k-1)]^T为控制输入，A为状态转移矩阵，是将输入转换为状态的矩阵，其中Δt为系统运行周期，d_r为两驱动轮间距。

(1.2)系统的观测方程为：

z(k)＝Hx(k)+v(k) (2)

其中，其中，k表示当前离散化时刻，k-1表示上一离散化时刻，系统状态观测z(k)＝[θ_k d_k]^T，H为观测矩阵,v(k)是零均值且协方差阵为R(k)的高斯噪声且与过程噪声w(k)不相关。

进一步，步骤2)中利用激光雷达获取目标的二维信息，提取出人体的腿部的几何特征，并用支持向量机方法来训练和识别，包括以下步骤：

(2.1)激光扫描出来的人腿形状为类圆弧形，考虑数据集由于人腿的特殊性，激光扫描出来的人腿形状有一些几何学上的特征，通过提取人腿形状的一些有效的特征来识别人腿，通过激光扫描可以得到人体腿部的聚类数据，我们用几个符号来表示聚类数据的特征，W表示激光聚类数据的宽度(第一个与最后一个点之间的距离)，G代表聚类数据的周长(依次连接每个点得到的总的距离)，D代表激光聚类数据的深度，θ表示最近的点和第一个点连接的直线以及和最远点连接的直线形成的角度，另外本文取G/W作为第五个特征。每一个输入x_i表示腿部聚类数据的特征输入，且都对应一个方式标签y_i∈{-1,+1}，SVM能够找到一个超平面来分离两类数据。非线性支持向量机是求以下对偶问题：

其中，a_i,a_j为拉格朗日乘子，K(x_i,x_j)为核函数，y_i，y_j∈{-1,+1}。K(x_i,x_j)＝exp(-||x_i-x_j||²/2σ²)为高斯核函数，N为数据的总数。

(2.2)使用带高斯内核的支持向量机算法对训练集进行训练并在测试集上测试训练模型；计算精确率(Precision)、召回率(Recall)和F1-score，得到最优的特征选择。

(2.3)应用支持向量机预测模型对运动区域中的聚类进行预测，得到对应的标签输出y_i；判定相应的聚类是否为人体腿部数据并找到相应的腿部聚类，计算聚类中心。

进一步，步骤3)应用假设卡尔曼滤波方法对机器人进行运动滤波，使移动机器人能够平稳运行，包括以下步骤：

(3.1)由于人的运动模型是未知的，如果使用匀速模型或者加速度模型会存在较大误差。因此在每一时刻的估计位置附近选取多个假设的预测位姿，我们选取多个假设的预测位姿，记为：

x(k|k-1)＝{xⁱ(k|k-1),xⁱ⁺¹(k|k-1),xⁱ⁺²(k|k-1)...} (4)

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i，i+1，i+2...为相应估计器的索引，x(k|k-1)为状态预测值，xⁱ(k|k-1),xⁱ⁺¹(k|k-1),xⁱ⁺²(k|k-1)...为k时刻的预测跟随状态。

状态初始化，获取机器人与人之间位置的初始状态P(0|0)。为初始状态，而P(0|0)为协方差矩阵。

(3.2)状态预测，计算状态以及P(k|k-1)。

P(k|k-1)＝P(k-1|k-1)+Q(k-1) (6)

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，为k时刻的预测跟随状态。Q(k-1)的状态噪声方差阵，uⁱ(k-1)＝[u₁(k-1)u₂(k-1)]^T为控制输入，A为二维单位阵，为状态转移矩阵，B是将输入转换为状态的矩阵，P(k|k-1)为k-1时刻的协方差预测值，P(k-1|k-1)为k-1时刻的估计值。

(3.3)量测更新，计算Γ(k)

Γ(k)＝H(k)P(k|k-1)H^T(k) (8)

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，为k时刻的预测观测，为k时刻的预测跟随状态。H为二维单位阵，P(k|k-1)为k-1时刻的协方差预测值，P(k-1|k-1)为k-1时刻的估计值，Γ(k)为中间变量。

(3.4)状态更新，计算P(k|k)

K(k)＝P(k|k-1)H^T(k)(Γ(k)+R(k))^-1 (9)

P(k|k)＝P(k|k-1)-K(k)H(k)P(k|k-1) (11)

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，z(k)为实际的观测，为k时刻的预测观测，为k时刻的预测跟随状态，为当前时刻估计的状态，H为二维单位阵，R(k)为状态噪声协方差矩阵，P(k|k)为k时刻的协方差值，P(k|k-1)为k时刻的预测值，Γ(k)为中间变量。

(3.5)由于人在运动过程中的方位每一时刻都可能发生变化(突然左右转弯等)，仅使用基本的Kalman滤波算法往往得不到理想的效果。为此，设计一种假设Kalman滤波器方法，在时刻k，由上一时刻的状态，来假设多个目标的可能状态，并通过马氏距离来找到最优的估计值。其中马氏距离由下式表示：

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，为当前时刻估计的状态，为当前时刻状态的预测值，Σ_X为协方差矩阵。

(3.6)对于x(k|k-1)＝{xⁱ(k|k-1),xⁱ⁺¹(k|k-1),xⁱ⁺²(k|k-1)...}通过式(5)，(6)，(7)，(8)，(9)，(10)，(11)，(12)计算M^j(k)，通过min{M^j(k)}得到对应的j值，从而得到最优的估计：

其中，k表示当前离散化时刻，j为假设的个数，k-1表示上一离散化时刻，i，i+1，i+2...为相应估计器的索引，x(k|k-1)为状态预测值，xⁱ(k|k-1),xⁱ⁺¹(k|k-1),xⁱ⁺²(k|k-1)...为k时刻的预测跟随状态，min{M^j(k)}为计算出的所有马氏距离中最小的那一个，为当前时刻估计的状态，为第j个状态估计值。

进一步，步骤4)移动机器人控制器设计，控制器的设计包括以下步骤：

(4.1)在移动机器人人体跟随系统中，移动机器人的运动控制采用PID控制：

其中，⊙为Hadamard积，k表示当前离散化时刻x(k)＝[θ(k)d(k)]^T，u(k)＝[u₁(k)u₂(k)]^T，u₁(k)，u₂(k)为控制输入θ_k、d_k为k时刻机器人的跟随偏角和跟随距离。K_P(k)＝[K_P1(k)K_P2(k)]，T_I(k)＝[T_I1(k)T_I2(k)]，K_p，T_I分别为比例系数参数和积分系数参数，k＝1,2...为时间序列。

(4.2)则移动机器人的左右轮速度为：

其中，k表示当前离散化时刻，v_L(k)为左轮速度，v_R(k)为右轮速度，u₁(k)，u₂(k)为控制输入。

本发明设计了一种基于假设卡尔曼滤波的人体目标跟随方法，其工作原理如下：首先通过支持向量机检测和识别人体；其次，采用假设卡尔曼滤波估计移动机器人与人的位置关系；最后，通过PID控制器使移动机器人与人能够保持恒定的距离和角度，从而达到稳定的跟随效果。通过假设卡尔曼滤波的方法，实时估计移动机器人与人之间的角度和距离关系，实现有效的人体目标跟随。

本发明的有益效果是：1、针对移动机器人运动模型建立了系统状态方程和观测方程，并考虑由于人的运动模型是未知的，因此在每一时刻的估计位置附近选取多个假设的预测位姿，使用假设卡尔曼滤波器，2、运用支持向量机方法极大提高了人体的识别率，保证了移动机器人能够很好的跟随人体。估计结果可以满足实际应用的精度与实时性要求。

附图说明

图1是本发明方法的坐标变换图。

图2是本发明方法的实时位姿估计效果图。

图3是本发明方法的实时估计误差图，表示X方向的估计误差。

图4是本发明方法的实时估计误差图，表示Y方向的估计误差。

具体实施方式

为使本发明的目的、技术方案和优点更加清晰，下面结合附图和仿真数据对本发明的技术方案作进一步描述。

参照图1～图4，一种基于假设卡尔曼滤波的人体目标跟随方法，首先通过支持向量机检测和识别人体；其次，采用假设卡尔曼滤波估计移动机器人与人的位置关系；最后，通过PID控制器使移动机器人与人能够保持恒定的距离和角度，从而达到稳定的跟随效果。

基于假设卡尔曼滤波的人体目标跟随方法包括以下步骤：

1)通过移动机器与人的相对位置关系建立机器人跟随模型；

2)利用激光雷达获取目标的二维信息，提取出人体的腿部的几何特征，并用支持向量机方法来训练和识别；

3)应用假设卡尔曼滤波方法对机器人进行运动滤波，使移动机器人能够平稳运行；

4)移动机器人控制器的设计。

进一步，步骤1)中，通过移动机器与人的相对位置关系建立机器人跟随模型的方法包括以下步骤：

(1.1)考虑两轮差速轮式移动机器人，则系统的状态空间模型可描述为：

x(k)＝Ax(k-1)+Bu(k-1)+w(k-1)

(17)

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，x_k为k时刻的跟随状态，x_k＝[θ_k,d_k]^T，θ_k,d_k分别为移动机器人的跟随偏角和跟随距离。w(k-1)为系统噪声，方差为u(k-1)＝[u₁(k-1) u₂(k-1)]^T为控制输入，为状态转移矩阵，是将输入转换为状态的矩阵，其中Δt为系统运行周期，d_r＝30cm为两驱动轮间距。

(1.2)系统的观测方程为：

z(k)＝Hx(k)+v(k)

(18)

其中，其中，k表示当前离散化时刻，k-1表示上一离散化时刻，系统状态观测z(k)＝[θ_k d_k]^T，观测矩阵v(k)是零均值且协方差阵为1的高斯噪声且与过程噪声w(k)不相关。

进一步，步骤2)中利用激光雷达获取目标的二维信息，提取出人体的腿部的几何特征，并用支持向量机方法来训练和识别，包括以下步骤：

(2.1)激光扫描出来的人腿形状为类圆弧形，考虑数据集由于人腿的特殊性，激光扫描出来的人腿形状有一些几何学上的特征，通过提取人腿形状的一些有效的特征来识别人腿，通过激光扫描可以得到人体腿部的聚类数据，我们用几个符号来表示聚类数据的特征，W表示激光聚类数据的宽度(第一个与最后一个点之间的距离)，G代表聚类数据的周长(依次连接每个点得到的总的距离)，D代表激光聚类数据的深度，θ表示最近的点和第一个点连接的直线以及和最远点连接的直线形成的角度，另外本文取G/W作为第五个特征。每一个输入x_i表示腿部聚类数据的特征输入，且都对应一个方式标签y_i∈{-1,+1}，SVM能够找到一个超平面来分离两类数据。非线性支持向量机是求以下对偶问题：

a_i≥0,i＝1,2,...,N

(19)

其中，a_i,a_j为拉格朗日乘子，K(x_i,x_j)为核函数，y_i，y_j∈{-1,+1}。K(x_i,x_j)＝exp(-||x_i-x_j||²/2σ²)为高斯核函数，N为数据的总数。

(2.2)使用带高斯内核的支持向量机算法对训练集进行训练并在测试集上测试训练模型；计算精确率(Precision)、召回率(Recall)和F1-score，得到最优的特征选择。

(2.3)应用支持向量机预测模型对运动区域中的聚类进行预测，得到对应的标签输出y_i；判定相应的聚类是否为人体腿部数据并找到相应的腿部聚类，计算聚类中心。

进一步，步骤3)应用假设卡尔曼滤波方法对机器人进行运动滤波，使移动机器人能够平稳运行，包括以下步骤：

(3.1)由于人的运动模型是未知的，如果使用匀速模型或者加速度模型会存在较大误差。因此在每一时刻的估计位置附近选取多个假设的预测位姿，我们选取多个假设的预测位姿，记为：

x(k|k-1)＝{xⁱ(k|k-1),xⁱ⁺¹(k|k-1),xⁱ⁺²(k|k-1)...}

(20)

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i，i+1，i+2...为相应估计器的索引，x(k|k-1)为状态预测值，xⁱ(k|k-1),xⁱ⁺¹(k|k-1),xⁱ⁺²(k|k-1)...为k时刻的预测跟随状态。

状态初始化，获取机器人与人之间位置的初始状态P(0|0)。为初始状态，而P(0|0)＝100为协方差矩阵。

(3.2)状态预测，计算状态以及P(k|k-1)。

P(k|k-1)＝P(k-1|k-1)+Q(k-1)

(22)

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，为k时刻的预测跟随状态。Q(k-1)的状态噪声方差阵，uⁱ(k-1)＝[u₁(k-1)u₂(k-1)]^T为控制输入，为状态转移矩阵，是将输入转换为状态的矩阵，其中Δt为系统运行周期，d_r＝30cm为两驱动轮间距。，P(k|k-1)为k-1时刻的协方差预测值，P(k-1|k-1)为k-1时刻的估计值。

(3.3)量测更新，计算Γ(k)

Γ(k)＝H(k)P(k|k-1)H^T(k)

(24)

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，为k时刻的预测观测，为k时刻的预测跟随状态。观测矩阵P(k|k-1)为k-1时刻的协方差预测值，P(k-1|k-1)为k-1时刻的估计值，Γ(k)为中间变量。

(3.4)状态更新，计算P(k|k)

K(k)＝P(k|k-1)H^T(k)(Γ(k)+R(k))^-1

(25)

P(k|k)＝P(k|k-1)-K(k)H(k)P(k|k-1)(27)

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，z(k)为实际的观测，为k时刻的预测观测，为k时刻的预测跟随状态，为当前时刻估计的状态，为状态噪声协方差矩阵，P(k|k)为k时刻的协方差值，P(k|k-1)为k时刻的预测值，Γ(k)为中间变量。

(3.5)由于人在运动过程中的方位每一时刻都可能发生变化(突然左右转弯等)，仅使用基本的Kalman滤波算法往往得不到理想的效果。为此，设计一种假设Kalman滤波器方法，在时刻k，由上一时刻的状态，来假设多个目标的可能状态，并通过马氏距离来找到最优的估计值。其中马氏距离由下式表示：

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，为当前时刻估计的状态，为当前时刻状态的预测值，Σ_X为协方差矩阵。

(3.6)对于x(k|k-1)＝{xⁱ(k|k-1),xⁱ⁺¹(k|k-1),xⁱ⁺²(k|k-1)...}通过式(5)，(6)，(7)，(8)，(9)，(10)，(11)，(12)计算M^j(k)，通过min{M^j(k)}得到对应的j值，从而得到最优的估计：

其中，k表示当前离散化时刻，j为假设的个数，k-1表示上一离散化时刻，i，i+1，i+2...为相应估计器的索引，x(k|k-1)为状态预测值，xⁱ(k|k-1),xⁱ⁺¹(k|k-1),xⁱ⁺²(k|k-1)...为k时刻的预测跟随状态，min{M^j(k)}为计算出的所有马氏距离中最小的那一个，为当前时刻估计的状态，为第j个状态估计值。

进一步，步骤4)移动机器人控制器设计，控制器的设计包括以下步骤：

(4.1)在移动机器人人体跟随系统中，移动机器人的运动控制采用PID控制：

其中，⊙为Hadamard积，k表示当前离散化时刻，x(k)＝[θ(k) d(k)]^T，u(k)＝[u₁(k) u₂(k)]^T，u₁(k)，u₂(k)为控制输入θ_k、d_k为k时刻机器人的跟随偏角和跟随距离。K_P(k)＝[K_P1(k) K_P2(k)]＝[45 8]，T_I(k)＝[T_I1(k) T_I2(k)]＝[0.5 0.1]，K_p，T_I分别为比例系数参数和积分系数参数，k＝1,2...为时间序列。

(4.2)则移动机器人的左右轮速度为：

其中，k表示当前离散化时刻，v_L(k)为左轮速度，v_R(k)为右轮速度，u₁(k)，u₂(k)为控制输入。

以上结合附图详细说明和陈述了本发明的实施方式，但并不局限于上述方式。在本领域的技术人员所具备的知识范围内，只要以本发明的构思为基础，还可以做出多种变化和改进。

Claims (5)

1.一种基于假设卡尔曼滤波的人体目标跟随方法，其特征在于，所述方法包括以下步骤：

1)通过移动机器与人的相对位置关系建立机器人跟随模型；

2)利用激光雷达获取目标的二维信息，提取出人体的腿部的几何特征，并用支持向量机方法来训练和识别；

3)应用假设卡尔曼滤波方法对机器人进行运动滤波，使移动机器人能够平稳运行；

4)移动机器人控制器的设计。

2.如权利要求1所述的一种基于假设卡尔曼滤波的人体目标跟随方法，其特征在于，所述步骤1)中，通过移动机器与人的相对位置关系建立机器人跟随模型的方法包括以下步骤：

(1.1)考虑两轮差速轮式移动机器人，则系统的状态空间模型可描述为：

x(k)＝Ax(k-1)+Bu(k-1)+w(k-1) (1)

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，x_k为k时刻的跟随状态，x_k＝[θ_k,d_k]^T，θ_k,d_k分别为移动机器人的跟随偏角和跟随距离，w(k-1)是均值和方差阵分别为q(k-1)和Q(k-1)的状态噪声，u(k-1)＝[u₁(k-1) u₂(k-1)]^T为控制输入，A为状态转移矩阵，是将输入转换为状态的矩阵，其中Δt为系统运行周期，d_r为两驱动轮间距；

(1.2)系统的观测方程为：

z(k)＝Hx(k)+v(k) (2)

其中，其中，k表示当前离散化时刻，k-1表示上一离散化时刻，系统状态观测z(k)＝[θ_kd_k]^T，H为观测矩阵,v(k)是零均值且协方差阵为R(k)的高斯噪声且与过程噪声w(k)不相关。

3.如权利要求1或2所述的一种基于假设卡尔曼滤波的人体目标跟随方法，其特征在于，所述步骤2)包括以下步骤：

(2.1)激光扫描出来的人腿形状为类圆弧形，考虑数据集W表示激光聚类数据的宽度，即第一个与最后一个点之间的距离，G代表聚类数据的周长，即依次连接每个点得到的总的距离，D代表激光聚类数据的深度，θ表示最近的点和第一个点连接的直线以及和最远点连接的直线形成的角度，取G/W作为第五个特征；每一个输入x_i表示腿部聚类数据的特征输入，且都对应一个方式标签y_i∈{-1,+1}，SVM能够找到一个超平面来分离两类数据，非线性支持向量机是求以下对偶问题：

其中，a_i,a_j为拉格朗日乘子，K(x_i,x_j)为核函数，y_i，y_j∈{-1,+1}，K(x_i,x_j)＝exp(-||x_i-x_j||²/2σ²)为高斯核函数，N为数据的总数；

(2.2)使用带高斯内核的支持向量机算法对训练集进行训练并在测试集上测试训练模型；计算精确率、召回率和F1-score，得到最优的特征选择；

(2.3)应用支持向量机预测模型对运动区域中的聚类进行预测，得到对应的标签输出y_i；判定相应的聚类是否为人体腿部数据并找到相应的腿部聚类，计算聚类中心。

4.如权利要求1或2所述的一种基于假设卡尔曼滤波的人体目标跟随方法，其特征在于，所述步骤3)中，运动滤波包括以下步骤：

(3.1)在每一时刻的估计位置附近选取多个假设的预测位姿，选取多个假设的预测位姿，记为：

x(k|k-1)＝{xⁱ(k|k-1),xⁱ⁺¹(k|k-1),xⁱ⁺²(k|k-1)...} (4)

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i，i+1，i+2...为相应估计器的索引，x(k|k-1)为状态预测值，xⁱ(k|k-1),xⁱ⁺¹(k|k-1),xⁱ⁺²(k|k-1)...为k时刻的预测跟随状态；

状态初始化，获取机器人与人之间位置的初始状态P(0|0)，为初始状态，而P(0|0)为协方差矩阵；

(3.2)状态预测，计算状态以及P(k|k-1)：

P(k|k-1)＝P(k-1|k-1)+Q(k-1) (6)

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，为k时刻的预测跟随状态，Q(k-1)的状态噪声方差阵，uⁱ(k-1)＝[u₁(k-1)u₂(k-1)]^T为控制输入，A为二维单位阵，为状态转移矩阵，B是将输入转换为状态的矩阵，P(k|k-1)为k-1时刻的协方差预测值，P(k-1|k-1)为k-1时刻的估计值；

(3.3)量测更新，计算Γ(k)

Γ(k)＝H(k)P(k|k-1)H^T(k) (8)

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，为k时刻的预测观测，为k时刻的预测跟随状态；H为二维单位阵，P(k|k-1)为k-1时刻的协方差预测值，P(k-1|k-1)为k-1时刻的估计值，Γ(k)为中间变量；

(3.4)状态更新，计算P(k|k)

K(k)＝P(k|k-1)H^T(k)(Γ(k)+R(k))^-1 (9)

P(k|k)＝P(k|k-1)-K(k)H(k)P(k|k-1) (11)

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，i为第i个估计器，z(k)为实际的观测，为k时刻的预测观测，为k时刻的预测跟随状态，为当前时刻估计的状态，H为二维单位阵，R(k)为状态噪声协方差矩阵，P(k|k)为k时刻的协方差值，P(k|k-1)为k时刻的预测值，Γ(k)为中间变量；

(3.5)设计一种假设Kalman滤波器方法，在时刻k，由上一时刻的状态，来假设多个目标的可能状态，并通过马氏距离来找到最优的估计值，其中马氏距离由下式表示：

其中，k表示当前离散化时刻，k-1表示上一离散化时刻，为当前时刻估计的状态，为当前时刻状态的预测值，Σ_X为协方差矩阵；

(3.6)对于x(k|k-1)＝{xⁱ(k|k-1),xⁱ⁺¹(k|k-1),xⁱ⁺²(k|k-1)...}通过式(5)，(6)，(7)，(8)，(9)，(10)，(11)，(12)计算M^j(k)，通过min{M^j(k)}得到对应的j值，从而得到最优的估计：

其中，k表示当前离散化时刻，j为假设的个数，k-1表示上一离散化时刻，i，i+1，i+2...为相应估计器的索引，x(k|k-1)为状态预测值，xⁱ(k|k-1),xⁱ⁺¹(k|k-1),xⁱ⁺²(k|k-1)...为k时刻的预测跟随状态，min{M^j(k)}为计算出的所有马氏距离中最小的那一个，为当前时刻估计的状态，为第j个状态估计值。

5.如权利要求1或2所述的一种基于假设卡尔曼滤波的人体目标跟随方法，其特征在于，所述步骤4)中，控制器的设计包括以下步骤：

(4.1)在移动机器人人体跟随系统中，移动机器人的运动控制采用PID控制：

其中，⊙为Hadamard积，k表示当前离散化时刻x(k)＝[θ(k)d(k)]^T，u(k)＝[u₁(k)u₂(k)]^T，u₁(k)，u₂(k)为控制输入θ_k、d_k为k时刻机器人的跟随偏角和跟随距离；K_P(k)＝[K_P1(k)K_P2(k)]，T_I(k)＝[T_I1(k) T_I2(k)]，K_p，T_I分别为比例系数参数和积分系数参数，k＝1,2...为时间序列；

(4.2)则移动机器人的左右轮速度为：

其中，k表示当前离散化时刻，v_L(k)为左轮速度，v_R(k)为右轮速度，u₁(k)，u₂(k)为控制输入。