一种基于RoboCup3D的多机器人协作目标定位方法

摘要

一种基于RoboCup3D的多机器人协作目标定位方法，包括：根据扩展的卡尔曼滤波算法，计算当前全局机器人位置信息，来进行全局机器人位置预测；根据扩展卡尔曼滤波算法建立视觉观测模型，通过传播和更新，递归的计算全局机器人位置和协方差矩阵；根据粒子滤波算法，通过一系列随机抽取的状态向量样本来表示后验概率，用所抽取的一系列样本的分布来代表全局机器人位置分布，采用重采样方法来有效解决序贯重要性采样中的退化问题。本发明将两种方法的长处相结合，在提高精度的同时，减少计算时间，达到实时定位。即通过多个机器人视觉传感器对目标物的相对观察量进行有效融合，进而完成协作定位。

说明书

技术领域

本发明涉及一种足球定位方法。特别是涉及一种采用扩展卡尔曼滤波和粒子滤波来对多机器人定位进行优化的基于RoboCup3D的多机器人协作目标定位方法。

背景技术

目前在机器人定位的相关技术中，多机器人协作系统通过系统中各个机器人间的合作能够提高机器人完成各种高级任务的能力及效率。与单机器人相比，多机器人系统在各个方面都表现出了极大的优越性，具有容错性好、效率高、可扩展性好、鲁棒性强等优点。

在多机器人协作系统的众多研究问题中，最基本的一个问题就是定位问题，机器人在环境中的定位能力是完成各种高级任务的基础。因此，多机器人协作定位越来越受到重视和研究。

多机器人协作定位是指多个机器人在共同的工作环境内相互协作，同时寻求确定各自在所处工作环境中的位置和姿态。机器人通过协作，可以进行实时探测信息交换，共享周围的环境信息，一定程度拓展了个体的观测视野，通过相互间的观测修正和融合不同机器人的观测信息，可以有效抑制个体机器人观测噪声，消除累积误差，提高整体定位精度。

RoboCup3D采用三维的仿真环境模拟机器人的实际运动状态，而足球定位是RoboCup3D中的基本问题，实质是机器人的目标识别，定位和跟踪。单机器人定位结果容易受视觉噪声，机器性能影响，误差较大，而多机器人协作定位能有效解决这一问题。

发明内容

本发明所要解决的技术问题是，提供一种通过多个机器人视觉传感器对目标物的相对观察量进行有效融合，进而完成协作定位的基于RoboCup3D的多机器人协作目标定位方法。

本发明所采用的技术方案是：一种基于RoboCup3D的多机器人协作目标定位方法，包括如下步骤：

1)根据扩展的卡尔曼滤波算法，计算当前全局机器人位置信息，来进行全局机器人位置预测，卡尔曼滤波算法中卡尔曼滤波器的状态表示如下：

·在时刻K全局机器人位置预测；

·P_k|k,后验预测误差协方差矩阵，表示预测的精确程度；

2)根据扩展卡尔曼滤波算法建立视觉观测模型，通过传播和更新，递归的计算全局机器人位置和协方差矩阵；

3)根据粒子滤波算法，通过一系列随机抽取的状态向量样本来表示后验概率，用所抽取的一系列样本的分布来代表全局机器人位置分布，采用重采样方法来有效解决序贯重要性采样中的退化问题。

步骤1)包括：

(1)卡尔曼滤波器根据上一时刻全局机器人的位置，对下一时刻全局机器人的位置作出估计，预测所述估计的协方差矩阵，其中。

预测状态

预测估计协方差矩阵

式中，F_k是作用在X_k-1上的状态变换模型，B_k是作用在控制器向量U_k上的输入,W_k是过程噪声，并假定其符合均值为零，协方差矩阵为Q_k的多元正态分布；

(2)计算测量余量

式中，Z_k为带高斯噪声的线性函数，表示真实状态X_k的一个测量值，H_k表示观测模型；

(3)计算最优卡尔曼增益

(4)根据当前观测到的全局机器人位置信息来优化第(1)步所述的协方差矩阵，得到优化的协方差矩阵，

更新的状态估计

更新的协方差矩阵P_k|k＝(I-K_kH_k)P_k|k-1(6)

(5)用第(1)步得到的协方差矩阵和第(4)步得到的优化的协方差矩阵来更新滤波器变量，得到更加准确的全局机器人位置的估计值。

步骤2)包括：

(1)建立全局机器人彼此之间的观察模型H，设各机器人相对位姿的参数为其中z_p表示机器人与机器人之间的相对距离，表示机器人与机器人之间的相对转角，z_α表示机器人与机器人之间运动方向之间的相对角度；

全局机器人之间的相对位姿的估计表示为：

式中，d_ij表示机器人i与机器人j之间的距离，n_pij(k)表示零均值高斯噪声，x_j和x_i表示运动方向向量，y_j和y_i表示运动方向的法向量，θ_j和θ_i表示机器人运动方向与水平方向的角度

在任意两个机器人之间的观测模型用雅克比矩阵表示为：

其中n_pij(k)表示零均值高斯噪声；

(2)根据扩展卡尔曼滤波算法将全局机器人位置向量，用可微函数表示为非线性函数表达式如下：

X_k＝f(X_k-1,U_k-1,W_k-1)(10)

Z_k＝h(X_k,V_k)(11)

式中，W_k是过程噪声，V_k是观测噪声，均服从正态分布；

(3)将步骤1)中的当前全局机器人位置信息代入第(2)步的可微函数表达式，得到线性化的表达式，

(4)用第(3)步中得到的线性表达式来更新观察模型H，得到：

P_k|k-1＝(I-K_kH_k)P_k|k-1(17)。

步骤3)包括：

(1)初始化k＝0时刻，从全局机器人中选取N个机器人，彼此独立，抽取的范围为待估计的后验分布p(x_0:k|y_0:k)，得到全局机器人中N个机器人的位置信息采样点

(2)对第(1)步得到的N个机器人的位置信息采样点，计算重要性权值，如下：

式中，表示k时刻机器人i的重要性权值，表示在已知k-1时刻位置信息的条件下，k时刻的后验分布概率，

归一化重要性权值，如下：

(3)在第(2)步已计算完的N个机器人的位置信息采样点中，按照重要性权值大小，从的机器人集合中去除权值低于阈值的机器人，得到新的N个机器人的位置信息采样点并把机器人位置信息的权值重新进行分配

(4)重复第(3)步操作，直到全局机器人的位置信息样本趋于稳定，输出对位置信息的预测：

本发明的一种基于RoboCup3D的多机器人协作目标定位方法，通过仿真平台分别比较基于扩展卡尔曼滤波和粒子滤波的定位算法，并用均方根误差算法RMSE来对误差进行评测，过程噪声和观测噪声都为零均值高斯噪声，且Q＝10，R＝1，进行50次采样，采样间隔为1秒。通过误差分析来对两种滤波方法的定位精度进行分析。本发明将两种方法的长处相结合，在提高精度的同时，减少计算时间，达到实时定位。即通过多个机器人视觉传感器对目标物的相对观察量进行有效融合，进而完成协作定位。

附图说明

图1是机器人与机器人之间运动方向之间的相对角度示意图；

图2是EKF改进后的定位轨迹图；

图3是EKF与PF的均方误差图。

具体实施方式

下面结合实施例和附图对本发明的一种基于RoboCup3D的多机器人协作目标定位方法做出详细说明。

本发明的一种基于RoboCup3D的多机器人协作目标定位方法，使用扩展卡尔曼滤波，粒子滤波等信息融合方法,通过多个机器人视觉传感器对目标的相对观察量进行有效融合，进而完成协作定位的任务。本发明利用扩展卡尔曼滤波方法，融合运动信息及相对观测信息协同定位的滤波方程，详细分析了其定位特点。并通过仿真实验验证了EKF定位算法的准确性与有效性。对初始条件未知的多机器人协作定位问题，研究与分析粒子滤波与扩展卡尔曼滤波相结合的协作定位算法，在仿真实验上对其测试并分析结果。

本发明的一种基于RoboCup3D的多机器人协作目标定位方法，包括如下步骤：

1)根据扩展的卡尔曼滤波算法，计算当前全局机器人位置信息，来进行全局机器人位置预测，其中，卡尔曼滤波算法中卡尔曼滤波器的状态表示如下：

·在时刻K全局机器人位置预测；

·P_k|k,后验预测误差协方差矩阵，表示预测的精确程度；

该步骤具体包括：

(1)卡尔曼滤波器根据上一时刻全局机器人的位置，对下一时刻全局机器人的位置作出估计，预测所述估计的协方差矩阵，其中。

预测状态

预测估计协方差矩阵

式中，F_k是作用在X_k-1上的状态变换模型，B_k是作用在控制器向量U_k上的输入,W_k是过程噪声，并假定其符合均值为零，协方差矩阵为Q_k的多元正态分布；

(2)计算测量余量

式中，Z_k为带高斯噪声的线性函数，表示真实状态X_k的一个测量值，H_k表示观测模型；

(3)计算最优卡尔曼增益

(4)根据当前观测到的全局机器人位置信息来优化第(1)步所述的协方差矩阵，得到优化的协方差矩阵，

更新的状态估计

更新的协方差矩阵P_k|k＝(I-K_kH_k)P_k|k-1(6)

(5)用第(1)步得到的协方差矩阵和第(4)步得到的优化的协方差矩阵来更新滤波器变量，得到更加准确的全局机器人位置的估计值。

2)根据扩展卡尔曼滤波算法建立视觉观测模型，通过传播和更新，递归的计算全局机器人位置和协方差矩阵；包括：

(1)建立全局机器人彼此之间的观察模型H，设各机器人相对位姿的参数为其中z_p表示机器人与机器人之间的相对距离，表示机器人与机器人之间的相对转角，z_α表示机器人与机器人之间运动方向之间的相对角度，如图1所示；

全局机器人之间的相对位姿的估计表示为：

式中，d_ij表示机器人i与机器人j之间的距离，n_pij(k)表示零均值高斯噪声，x_j和x_i表示运动方向向量，y_j和y_i表示运动方向的法向量，θ_j和θ_i表示机器人运动方向与水平方向的角度。

任意两个机器人之间的观测模型用雅克比矩阵表示为：

其中n_pij(k)表示零均值高斯噪声；

(2)根据扩展卡尔曼滤波算法将全局机器人位置向量，用可微函数表示为非线性函数表达式如下：

X_k＝f(X_k-1,U_j-1,W_k-1)(10)

Z_k＝h(X_k,V_k)(11)

式中，W_k是过程噪声，V_k是观测噪声，均服从正态分布；

(3)将步骤1)中的当前全局机器人位置信息代入第(2)步的可微函数表达式，得到线性化的表达式，

(4)用第(3)步中得到的线性表达式来更新观察模型H，得到：

P_k|k-1＝(I-K_kH_k)P_k|k-1(17)。

3)根据粒子滤波算法，通过一系列随机抽取的状态向量样本来表示后验概率，用所抽取的一系列样本的分布来代表全局机器人位置分布，采用重采样方法来有效解决序贯重要性采样中的退化问题，包括：

(1)初始化k＝0时刻,从全局机器人中选取N个机器人，彼此独立，抽取的范围为待估计的后验分布p(x_0:k|y_0:k)，得到全局机器人中N个机器人的位置信息采样点

(2)对第(1)步得到的N个机器人的位置信息采样点，计算重要性权值，如下：

式中，表示k时刻机器人i的重要性权值，表示在已知k-1时刻位置信息的条件下，k时刻的后验分布概率。

归一化重要性权值，如下：

(3)在第(2)步已计算完的N个机器人的位置信息采样点中，按照重要性权值大小，从的机器人集合中去除权值低于阈值的机器人，得到新的N个机器人的位置信息采样点并把机器人位置信息的权值重新进行分配

(4)重复第(3)步操作，直到全局机器人的位置信息样本趋于稳定，输出对位置信息的预测：

通过实验可知，对于单独的EKF算法，在初期可能会出现滤波发散的情况，但是随着采样次数增加，预测值逐渐收敛到真实值。对PF算法，虽然计算量大，所花费时间长，但能在复杂的非线性非高斯环境中，预测值接近位置信息，如图2所示。将上述两种滤波方法相结合，在初始化时完成EKF与PF的参数设置，然后滤波去噪。使用PF算法进行收敛知道满足EKF的误差范围，再使用EKF进行状态估计和更新，如图3所示。将两者的长处相结合，在提高精度的同时，减少计算时间，达到实时定位。