[发明专利]一种基于假设卡尔曼滤波的人体目标跟随方法

权利要求书

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

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

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

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

所述步骤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)＝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)状态更新，计算

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个状态估计值；

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

所述步骤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)为控制输入。