[发明专利]机器人跟踪方法、装置、设备及计算机可读存储介质

权利要求书

1.一种机器人跟踪方法，其特征在于，所述方法包括：

在跟踪的每个时刻，获取至少两个超声波阵列对机器人的观测数据，包括：

在k时刻，获取至少两个超声波阵列对机器人的观测数据其中k，n均为大于0的整数，均为所述至少两个超声波阵列测得的机器人角度、距离数据的向量；

利用预设扩维IMM-EKF算法估计所述每个时刻机器人运动状态，通过与k时刻m种运动状态对应的m种运动模型相匹配的m个扩维EKF滤波器，分别获取k时刻每种运动模型下机器人对应的状态估计，获得m个状态，对所述m个状态进行加权计算得到k时刻机器人状态估计结果，包括：

机器人跟踪系统建立步骤：建立所述机器人跟踪系统，所述机器人跟踪系统包括机器人的运动方程和观测方程，如下表示：

运动方程：

观测方程：

C_ij＝P(M_k＝M^j|M_k-1＝Mⁱ)；M_k表示k时刻的运动模型，M^j表示第j个运动模型，M_k-1表示k-1时刻的运动模型，Mⁱ表示第i个运动模型；

其中i，j＝1，2...m表示模型数量，n＝1，2......l表示超声波阵列数量，m、l均为大于等于1的整数，k∈N表示时刻，C_ij表示k-1时刻目标从模型i转移到k时刻的模型j的概率，表示k时刻第i个模型状态转移矩阵，表示k时刻第i个运动模型下的目标状态，表示k时刻第n个阵列的观测矩阵，表示k时刻第n个阵列接收到的目标状态观测；表示模型i的过程噪声，表示为第n个阵列的观测噪声，两种噪声均假设为零均值、协方差分别为的高斯白噪声；

模型输入交互步骤：设为k-1时刻扩维EKF滤波器i的状态估计，为相应的协方差矩阵估计，为k-1时刻模型i的概率，交互式计算后，扩维EKF滤波器j在k时刻的输入计算公式如下：

其中

子模型滤波步骤：在各个扩维EKF滤波器计算得到相应的输入利用获得的量测进行各个模型下对应的状态估计更新；

模型概率更新步骤：对各个模型i＝1，2，...m计算模型概率，计算公式如下：

其中，

估计融合输出步骤：根据各个模型的更新概率和状态估计以及协方差矩阵估计，计算出当前时刻目标的状态估计和协方差矩阵估计，计算公式如下：

x_k|k表示k时刻目标状态估计，P_k|k表示k时刻目标状态协方差矩阵估计，

其中所述每个时刻用k时刻表示，k，m均为大于0的整数。

2.根据权利要求1所述的方法，其特征在于，所述子模型滤波步骤包括：

状态预测子步骤：对于各个模型i＝1，2...m，分别计算相应的预测状态和预测协方差矩阵，计算公式如下：

数据融合子步骤：利用扩维算法进行数据融合，各个相应变量公式如下：

对应于模型i＝1，2...m，各自的量测预测残差和量测协方差计算公式如下：

同时计算与模型i对应的似然函数，在假定服从高斯分布的条件下，似然函数如下：

滤波更新子步骤：对应于模型i＝1，2，...m，分别计算各自的滤波增益，状态估计更新，以及误差协方差矩阵，计算公式如下：