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

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

Info

Publication number
CN107084714A
CN107084714A CN201710300372.XA CN201710300372A CN107084714A CN 107084714 A CN107084714 A CN 107084714A CN 201710300372 A CN201710300372 A CN 201710300372A CN 107084714 A CN107084714 A CN 107084714A
Authority
CN
China
Prior art keywords
mrow
msub
msubsup
robot
mtd
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201710300372.XA
Other languages
English (en)
Other versions
CN107084714B (zh
Inventor
喻梅
胡晓凯
王建荣
于健
高洁
徐天
徐天一
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
NINGBO ZHIWEI RUICHI INFORMATION TECHNOLOGY Co.,Ltd.
Original Assignee
Tianjin University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tianjin University filed Critical Tianjin University
Priority to CN201710300372.XA priority Critical patent/CN107084714B/zh
Publication of CN107084714A publication Critical patent/CN107084714A/zh
Application granted granted Critical
Publication of CN107084714B publication Critical patent/CN107084714B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

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

Description

一种基于RoboCup3D的多机器人协作目标定位方法
技术领域
本发明涉及一种足球定位方法。特别是涉及一种采用扩展卡尔曼滤波和粒子滤波来对多机器人定位进行优化的基于RoboCup3D的多机器人协作目标定位方法。
背景技术
目前在机器人定位的相关技术中，多机器人协作系统通过系统中各个机器人间的合作能够提高机器人完成各种高级任务的能力及效率。与单机器人相比，多机器人系统在各个方面都表现出了极大的优越性，具有容错性好、效率高、可扩展性好、鲁棒性强等优点。
在多机器人协作系统的众多研究问题中，最基本的一个问题就是定位问题，机器人在环境中的定位能力是完成各种高级任务的基础。因此，多机器人协作定位越来越受到重视和研究。
多机器人协作定位是指多个机器人在共同的工作环境内相互协作，同时寻求确定各自在所处工作环境中的位置和姿态。机器人通过协作，可以进行实时探测信息交换，共享周围的环境信息，一定程度拓展了个体的观测视野，通过相互间的观测修正和融合不同机器人的观测信息，可以有效抑制个体机器人观测噪声，消除累积误差，提高整体定位精度。
RoboCup3D采用三维的仿真环境模拟机器人的实际运动状态，而足球定位是RoboCup3D中的基本问题，实质是机器人的目标识别，定位和跟踪。单机器人定位结果容易受视觉噪声，机器性能影响，误差较大，而多机器人协作定位能有效解决这一问题。
发明内容
本发明所要解决的技术问题是，提供一种通过多个机器人视觉传感器对目标物的相对观察量进行有效融合，进而完成协作定位的基于RoboCup3D的多机器人协作目标定位方法。
本发明所采用的技术方案是：一种基于RoboCup3D的多机器人协作目标定位方法，包括如下步骤：
1)根据扩展的卡尔曼滤波算法，计算当前全局机器人位置信息，来进行全局机器人位置预测，卡尔曼滤波算法中卡尔曼滤波器的状态表示如下：
·在时刻K全局机器人位置预测；
·Pk|k,后验预测误差协方差矩阵，表示预测的精确程度；
2)根据扩展卡尔曼滤波算法建立视觉观测模型，通过传播和更新，递归的计算全局机器人位置和协方差矩阵；
3)根据粒子滤波算法，通过一系列随机抽取的状态向量样本来表示后验概率，用所抽取的一系列样本的分布来代表全局机器人位置分布，采用重采样方法来有效解决序贯重要性采样中的退化问题。
步骤1)包括：
(1)卡尔曼滤波器根据上一时刻全局机器人的位置，对下一时刻全局机器人的位置作出估计，预测所述估计的协方差矩阵，其中。
预测状态
预测估计协方差矩阵
式中，Fk是作用在Xk-1上的状态变换模型，Bk是作用在控制器向量Uk上的输入,Wk是过程噪声，并假定其符合均值为零，协方差矩阵为Qk的多元正态分布；
(2)计算测量余量
式中，Zk为带高斯噪声的线性函数，表示真实状态Xk的一个测量值，Hk表示观测模型；
(3)计算最优卡尔曼增益
(4)根据当前观测到的全局机器人位置信息来优化第(1)步所述的协方差矩阵，得到优化的协方差矩阵，
更新的状态估计
更新的协方差矩阵Pk|k＝(I-KkHk)Pk|k-1 (6)
(5)用第(1)步得到的协方差矩阵和第(4)步得到的优化的协方差矩阵来更新滤波器变量，得到更加准确的全局机器人位置的估计值。
步骤2)包括：
(1)建立全局机器人彼此之间的观察模型H，设各机器人相对位姿的参数为其中zp表示机器人与机器人之间的相对距离，表示机器人与机器人之间的相对转角，zα表示机器人与机器人之间运动方向之间的相对角度；
全局机器人之间的相对位姿的估计表示为：
式中，dij表示机器人i与机器人j之间的距离，npij(k)表示零均值高斯噪声，xj和xi表示运动方向向量，yj和yi表示运动方向的法向量，θj和θi表示机器人运动方向与水平方向的角度
在任意两个机器人之间的观测模型用雅克比矩阵表示为：
其中npij(k)表示零均值高斯噪声；
(2)根据扩展卡尔曼滤波算法将全局机器人位置向量，用可微函数表示为非线性函数表达式如下：
Xk＝f(Xk-1,Uk-1,Wk-1) (10)
Zk＝h(Xk,Vk) (11)
式中，Wk是过程噪声，Vk是观测噪声，均服从正态分布；
(3)将步骤1)中的当前全局机器人位置信息代入第(2)步的可微函数表达式，得到线性化的表达式，
(4)用第(3)步中得到的线性表达式来更新观察模型H，得到：
Pk|k-1＝(I-KkHk)Pk|k-1 (17)。
步骤3)包括：
(1)初始化k＝0时刻，从全局机器人中选取N个机器人，彼此独立，抽取的范围为待估计的后验分布p(x0:k|y0: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全局机器人位置预测；
·Pk|k,后验预测误差协方差矩阵，表示预测的精确程度；
该步骤具体包括：
(1)卡尔曼滤波器根据上一时刻全局机器人的位置，对下一时刻全局机器人的位置作出估计，预测所述估计的协方差矩阵，其中。
预测状态
预测估计协方差矩阵
式中，Fk是作用在Xk-1上的状态变换模型，Bk是作用在控制器向量Uk上的输入,Wk是过程噪声，并假定其符合均值为零，协方差矩阵为Qk的多元正态分布；
(2)计算测量余量
式中，Zk为带高斯噪声的线性函数，表示真实状态Xk的一个测量值，Hk表示观测模型；
(3)计算最优卡尔曼增益
(4)根据当前观测到的全局机器人位置信息来优化第(1)步所述的协方差矩阵，得到优化的协方差矩阵，
更新的状态估计
更新的协方差矩阵Pk|k＝(I-KkHk)Pk|k-1 (6)
(5)用第(1)步得到的协方差矩阵和第(4)步得到的优化的协方差矩阵来更新滤波器变量，得到更加准确的全局机器人位置的估计值。
2)根据扩展卡尔曼滤波算法建立视觉观测模型，通过传播和更新，递归的计算全局机器人位置和协方差矩阵；包括：
(1)建立全局机器人彼此之间的观察模型H，设各机器人相对位姿的参数为其中zp表示机器人与机器人之间的相对距离，表示机器人与机器人之间的相对转角，zα表示机器人与机器人之间运动方向之间的相对角度，如图1所示；
全局机器人之间的相对位姿的估计表示为：
式中，dij表示机器人i与机器人j之间的距离，npij(k)表示零均值高斯噪声，xj和xi表示运动方向向量，yj和yi表示运动方向的法向量，θj和θi表示机器人运动方向与水平方向的角度。
任意两个机器人之间的观测模型用雅克比矩阵表示为：
其中npij(k)表示零均值高斯噪声；
(2)根据扩展卡尔曼滤波算法将全局机器人位置向量，用可微函数表示为非线性函数表达式如下：
Xk＝f(Xk-1,Uj-1,Wk-1) (10)
Zk＝h(Xk,Vk) (11)
式中，Wk是过程噪声，Vk是观测噪声，均服从正态分布；
(3)将步骤1)中的当前全局机器人位置信息代入第(2)步的可微函数表达式，得到线性化的表达式，
(4)用第(3)步中得到的线性表达式来更新观察模型H，得到：
Pk|k-1＝(I-KkHk)Pk|k-1 (17)。
3)根据粒子滤波算法，通过一系列随机抽取的状态向量样本来表示后验概率，用所抽取的一系列样本的分布来代表全局机器人位置分布，采用重采样方法来有效解决序贯重要性采样中的退化问题，包括：
(1)初始化k＝0时刻,从全局机器人中选取N个机器人，彼此独立，抽取的范围为待估计的后验分布p(x0:k|y0: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所示。将两者的长处相结合，在提高精度的同时，减少计算时间，达到实时定位。

Claims (4)

1.一种基于RoboCup3D的多机器人协作目标定位方法，其特征在于，包括如下步骤：
1)根据扩展的卡尔曼滤波算法，计算当前全局机器人位置信息，来进行全局机器人位置预测，卡尔曼滤波算法中卡尔曼滤波器的状态表示如下：
在时刻K全局机器人位置预测；
·Pk|k,后验预测误差协方差矩阵，表示预测的精确程度；
2)根据扩展卡尔曼滤波算法建立视觉观测模型，通过传播和更新，递归的计算全局机器人位置和协方差矩阵；
3)根据粒子滤波算法，通过一系列随机抽取的状态向量样本来表示后验概率，用所抽取的一系列样本的分布来代表全局机器人位置分布，采用重采样方法来有效解决序贯重要性采样中的退化问题。
2.根据权利要求1所述的一种基于RoboCup3D的多机器人协作目标定位方法，其特征在于，步骤1)包括：
(1)卡尔曼滤波器根据上一时刻全局机器人的位置，对下一时刻全局机器人的位置作出估计，预测所述估计的协方差矩阵，其中。
预测状态
<mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>F</mi> <mi>k</mi> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>B</mi> <mi>k</mi> </msub> <msub> <mi>U</mi> <mi>k</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>
预测估计协方差矩阵
<mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>F</mi> <mi>k</mi> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>F</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>Q</mi> <mi>k</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>
式中，Fk是作用在Xk-1上的状态变换模型，Bk是作用在控制器向量Uk上的输入,Wk是过程噪声，并假定其符合均值为零，协方差矩阵为Qk的多元正态分布；
(2)计算测量余量
式中，Zk为带高斯噪声的线性函数，表示真实状态Xk的一个测量值，Hk表示观测模型；
(3)计算最优卡尔曼增益
<mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msubsup> <mi>S</mi> <mi>k</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>
(4)根据当前观测到的全局机器人位置信息来优化第(1)步所述的协方差矩阵，得到优化的协方差矩阵，
更新的状态估计
更新的协方差矩阵Pk|k＝(I-KkHk)Pk|k-1 (6)
(5)用第(1)步得到的协方差矩阵和第(4)步得到的优化的协方差矩阵来更新滤波器变量，得到更加准确的全局机器人位置的估计值。
3.根据权利要求1所述的一种基于RoboCup3D的多机器人协作目标定位方法，其特征在于，步骤2)包括：
(1)建立全局机器人彼此之间的观察模型H，设各机器人相对位姿的参数为其中zp表示机器人与机器人之间的相对距离，表示机器人与机器人之间的相对转角，zα表示机器人与机器人之间运动方向之间的相对角度；
全局机器人之间的相对位姿的估计表示为：
式中，dij表示机器人i与机器人j之间的距离，npij(k)表示零均值高斯噪声，xj和xi表示运动方向向量，yj和yi表示运动方向的法向量，θj和θi表示机器人运动方向与水平方向的角度
在任意两个机器人之间的观测模型用雅克比矩阵表示为：
<mrow> <msub> <mi>H</mi> <mi>i</mi> </msub> <mo>=</mo> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <mfrac> <mrow> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>-</mo> <msub> <mi>x</mi> <mi>j</mi> </msub> </mrow> <msub> <mi>d</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> </msub> </mfrac> </mtd> <mtd> <mfrac> <mrow> <msub> <mi>y</mi> <mi>i</mi> </msub> <mo>-</mo> <msub> <mi>y</mi> <mi>j</mi> </msub> </mrow> <msub> <mi>d</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> </msub> </mfrac> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <msub> <mi>y</mi> <mi>j</mi> </msub> <mo>-</mo> <msub> <mi>y</mi> <mi>i</mi> </msub> </mrow> <msubsup> <mi>d</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> <mn>2</mn> </msubsup> </mfrac> </mtd> <mtd> <mfrac> <mrow> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>-</mo> <msub> <mi>x</mi> <mi>j</mi> </msub> </mrow> <msubsup> <mi>d</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> <mn>2</mn> </msubsup> </mfrac> </mtd> <mtd> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>H</mi> <mi>j</mi> </msub> <mo>=</mo> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <mfrac> <mrow> <msub> <mi>x</mi> <mi>j</mi> </msub> <mo>-</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> </mrow> <msub> <mi>d</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> </msub> </mfrac> </mtd> <mtd> <mfrac> <mrow> <msub> <mi>y</mi> <mi>j</mi> </msub> <mo>-</mo> <msub> <mi>y</mi> <mi>i</mi> </msub> </mrow> <msub> <mi>d</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> </msub> </mfrac> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <msub> <mi>y</mi> <mi>i</mi> </msub> <mo>-</mo> <msub> <mi>y</mi> <mi>j</mi> </msub> </mrow> <msubsup> <mi>d</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> <mn>2</mn> </msubsup> </mfrac> </mtd> <mtd> <mfrac> <mrow> <msub> <mi>x</mi> <mi>j</mi> </msub> <mo>-</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> </mrow> <msubsup> <mi>d</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> <mn>2</mn> </msubsup> </mfrac> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>
其中npij(k)表示零均值高斯噪声；
(2)根据扩展卡尔曼滤波算法将全局机器人位置向量，用可微函数表示为非线性函数表达式如下：
Xk＝f(Xk-1,Uk-1,Wk-1) (10)
Zk＝h(Xk,Vk) (11)
式中，Wk是过程噪声，Vk是观测噪声，均服从正态分布；
(3)将步骤1)中的当前全局机器人位置信息代入第(2)步的可微函数表达式，得到线性化的表达式，
<mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>U</mi> <mi>k</mi> </msub> <mo>,</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>F</mi> <mi>k</mi> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>F</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>Q</mi> <mi>k</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>
(4)用第(3)步中得到的线性表达式来更新观察模型H，得到：
<mrow> <msub> <mi>F</mi> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mrow> <mo>&amp;part;</mo> <mi>f</mi> </mrow> <mrow> <mo>&amp;part;</mo> <mi>x</mi> </mrow> </mfrac> <mo>|</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>u</mi> <mi>k</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>H</mi> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mrow> <mo>&amp;part;</mo> <mi>h</mi> </mrow> <mrow> <mo>&amp;part;</mo> <mi>x</mi> </mrow> </mfrac> <mo>|</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mover> <mi>y</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow>
Pk|k-1＝(I-KkHk)Pk|k-1 (17)。
4.根据权利要求1所述的一种基于RoboCup3D的多机器人协作目标定位方法，其特征在于，步骤3)包括：
(1)初始化k＝0时刻，从全局机器人中选取N个机器人，彼此独立，抽取的范围为待估计的后验分布p(x0:k|y0:k)，得到全局机器人中N个机器人的位置信息采样点i＝1,2,…N；
(2)对第(1)步得到的N个机器人的位置信息采样点，计算重要性权值，如下：
<mrow> <msubsup> <mi>w</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>=</mo> <msubsup> <mi>w</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mfrac> <mrow> <mi>P</mi> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>|</mo> <msubsup> <mi>x</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mi>P</mi> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>|</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> </mrow> <mrow> <mi>q</mi> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>|</mo> <msubsup> <mi>x</mi> <mrow> <mn>0</mn> <mo>:</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>,</mo> <msub> <mi>Z</mi> <mrow> <mn>0</mn> <mo>:</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mi>N</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow>
式中，表示k时刻机器人i的重要性权值，表示在已知k-1时刻位置信息的条件下，k时刻的后验分布概率，
归一化重要性权值，如下：
<mrow> <msubsup> <mover> <mi>w</mi> <mo>~</mo> </mover> <mi>k</mi> <mi>i</mi> </msubsup> <mo>=</mo> <msubsup> <mi>w</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>/</mo> <msubsup> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </msubsup> <msubsup> <mi>w</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>
(3)在第(2)步已计算完的N个机器人的位置信息采样点中，按照重要性权值大小，从i＝1,2,…N的机器人集合中去除权值低于阈值的机器人，得到新的N个机器人的位置信息采样点i＝1,2,…N，并把机器人位置信息的权值重新进行分配
<mrow> <msubsup> <mi>x</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>w</mi> <mo>~</mo> </mover> <mi>k</mi> <mi>i</mi> </msubsup> <mo>=</mo> <mn>1</mn> <mo>/</mo> <mi>N</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow>
(4)重复第(3)步操作，直到全局机器人的位置信息样本趋于稳定，输出对位置信息的预测：
<mrow> <msub> <mover> <mi>x</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msubsup> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </msubsup> <msubsup> <mi>w</mi> <mi>k</mi> <mi>i</mi> </msubsup> <msubsup> <mover> <mi>x</mi> <mo>~</mo> </mover> <mi>k</mi> <mi>i</mi> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>21</mn> <mo>)</mo> </mrow> <mo>.</mo> </mrow> 3
CN201710300372.XA 2017-04-29 2017-04-29 一种基于RoboCup3D的多机器人协作目标定位方法 Active CN107084714B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710300372.XA CN107084714B (zh) 2017-04-29 2017-04-29 一种基于RoboCup3D的多机器人协作目标定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710300372.XA CN107084714B (zh) 2017-04-29 2017-04-29 一种基于RoboCup3D的多机器人协作目标定位方法

Publications (2)

Publication Number Publication Date
CN107084714A true CN107084714A (zh) 2017-08-22
CN107084714B CN107084714B (zh) 2019-10-22

Family

ID=59611388

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710300372.XA Active CN107084714B (zh) 2017-04-29 2017-04-29 一种基于RoboCup3D的多机器人协作目标定位方法

Country Status (1)

Country Link
CN (1) CN107084714B (zh)

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108563112A (zh) * 2018-03-30 2018-09-21 南京邮电大学 用于仿真足球机器人控球的控制方法
CN108563220A (zh) * 2018-01-29 2018-09-21 南京邮电大学 仿人足球机器人的运动规划
CN109397294A (zh) * 2018-12-05 2019-03-01 南京邮电大学 一种基于ba-abc融合通信算法的机器人协作定位方法
CN109959381A (zh) * 2017-12-22 2019-07-02 深圳市优必选科技有限公司 一种定位方法、装置、机器人及计算机可读存储介质
CN110220513A (zh) * 2019-04-30 2019-09-10 中国科学院深圳先进技术研究院 一种目标定位的方法、系统、无人机及存储介质
CN110543919A (zh) * 2019-09-09 2019-12-06 厦门理工学院 一种机器人定位控制方法、终端设备及存储介质
CN110807799A (zh) * 2019-09-29 2020-02-18 哈尔滨工程大学 一种结合深度图推断的线特征视觉里程计方法
CN111595354A (zh) * 2020-05-27 2020-08-28 东南大学 一种自适应动态观测域的ekf-slam算法
CN113721188A (zh) * 2021-08-06 2021-11-30 北京理工大学 拒止环境下的多无人机自身定位与目标定位方法
CN113984073A (zh) * 2021-09-29 2022-01-28 杭州电子科技大学 一种基于方位的移动机器人协同校正算法
CN114577199A (zh) * 2022-02-17 2022-06-03 广州大学 一种基于Gmapping算法的垃圾分类机器人二维栅格地图构建系统
CN115077529A (zh) * 2022-06-16 2022-09-20 哈尔滨工业大学（威海） 一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102818567A (zh) * 2012-08-08 2012-12-12 浙江大学 集合卡尔曼滤波－粒子滤波相结合的auv组合导航方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102818567A (zh) * 2012-08-08 2012-12-12 浙江大学 集合卡尔曼滤波－粒子滤波相结合的auv组合导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
曹晓丽等: "改进粒子滤波算法分析研究", 《吉林省教育学院学报》 *
李良群等: "迭代扩展卡尔曼粒子滤波器", 《西安电子科技大学学报·自然科学版》 *
蓝冬英等: "基于仿人足球机器人协作定位技术研究", 《机器人技术与应用》 *

Cited By (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109959381B (zh) * 2017-12-22 2021-06-04 深圳市优必选科技有限公司 一种定位方法、装置、机器人及计算机可读存储介质
CN109959381A (zh) * 2017-12-22 2019-07-02 深圳市优必选科技有限公司 一种定位方法、装置、机器人及计算机可读存储介质
CN108563220A (zh) * 2018-01-29 2018-09-21 南京邮电大学 仿人足球机器人的运动规划
CN108563112A (zh) * 2018-03-30 2018-09-21 南京邮电大学 用于仿真足球机器人控球的控制方法
CN109397294A (zh) * 2018-12-05 2019-03-01 南京邮电大学 一种基于ba-abc融合通信算法的机器人协作定位方法
CN110220513A (zh) * 2019-04-30 2019-09-10 中国科学院深圳先进技术研究院 一种目标定位的方法、系统、无人机及存储介质
CN110220513B (zh) * 2019-04-30 2022-10-04 中国科学院深圳先进技术研究院 一种目标定位的方法、系统、无人机及存储介质
CN110543919A (zh) * 2019-09-09 2019-12-06 厦门理工学院 一种机器人定位控制方法、终端设备及存储介质
CN110543919B (zh) * 2019-09-09 2021-10-15 厦门理工学院 一种机器人定位控制方法、终端设备及存储介质
CN110807799A (zh) * 2019-09-29 2020-02-18 哈尔滨工程大学 一种结合深度图推断的线特征视觉里程计方法
CN110807799B (zh) * 2019-09-29 2023-05-30 哈尔滨工程大学 一种结合深度图推断的线特征视觉里程计方法
CN111595354A (zh) * 2020-05-27 2020-08-28 东南大学 一种自适应动态观测域的ekf-slam算法
CN113721188A (zh) * 2021-08-06 2021-11-30 北京理工大学 拒止环境下的多无人机自身定位与目标定位方法
CN113984073A (zh) * 2021-09-29 2022-01-28 杭州电子科技大学 一种基于方位的移动机器人协同校正算法
CN113984073B (zh) * 2021-09-29 2024-05-28 杭州电子科技大学 一种基于方位的移动机器人协同校正算法
CN114577199A (zh) * 2022-02-17 2022-06-03 广州大学 一种基于Gmapping算法的垃圾分类机器人二维栅格地图构建系统
CN115077529A (zh) * 2022-06-16 2022-09-20 哈尔滨工业大学（威海） 一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统
CN115077529B (zh) * 2022-06-16 2024-04-26 哈尔滨工业大学（威海） 一种基于最优加权改进粒子滤波的多机器人协同定位方法及系统

Also Published As

Publication number Publication date
CN107084714B (zh) 2019-10-22

Similar Documents

Publication Publication Date Title
CN107084714B (zh) 一种基于RoboCup3D的多机器人协作目标定位方法
CN105737832B (zh) 基于全局最优数据融合的分布式slam方法
CN109597864B (zh) 椭球边界卡尔曼滤波的即时定位与地图构建方法及系统
CN106772524B (zh) 一种基于秩滤波的农业机器人组合导航信息融合方法
CN111798491B (zh) 一种基于Elman神经网络的机动目标跟踪方法
CN103776453B (zh) 一种多模型水下航行器组合导航滤波方法
CN108682023A (zh) 基于Elman神经网络的紧耦合无迹卡尔曼跟踪滤波算法
KR100816269B1 (ko) 언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도작성 방법
CN104155998B (zh) 一种基于势场法的航迹规划方法
CN103973263B (zh) 一种逼近滤波方法
CN104330083A (zh) 基于平方根无迹卡尔曼滤波的多机器人协同定位算法
CN106403953B (zh) 一种用于水下自主导航与定位的方法
CN103247057A (zh) 目标-回波-道路网三元数据关联的道路目标多假设跟踪算法
CN104048676A (zh) 基于改进粒子滤波的mems陀螺随机误差补偿方法
Leung et al. An improved weighting strategy for rao-blackwellized probability hypothesis density simultaneous localization and mapping
CN107843259B (zh) 一种基于vckf的多机器人协同导航定位方法
CN108051001A (zh) 一种机器人移动控制方法、系统及惯性传感控制装置
CN113029173A (zh) 车辆导航方法及装置
Choi et al. An enhanced CSLAM for multi-robot based on unscented Kalman filter
Ryan Information-theoretic tracking control based on particle filter estimate
CN115950414A (zh) 一种不同传感器数据的自适应多重融合slam方法
CN104268597A (zh) 基于ahcif的集中式测量值扩维融合方法
CN115328168A (zh) 基于自适应强跟踪的移动机器人同步定位与建图方法及系统
CN104320108A (zh) 基于ahcif的集中式测量值加权融合方法
CN109115228A (zh) 一种基于加权最小二乘容积卡尔曼滤波的目标定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right

Effective date of registration: 20210701

Address after: Room 819, block C, Hebang building, 899 Tiantong North Road, Zhonghe street, Yinzhou District, Ningbo City, Zhejiang Province, 315100

Patentee after: NINGBO ZHIWEI RUICHI INFORMATION TECHNOLOGY Co.,Ltd.

Address before: 300192 92 Weijin Road, Nankai District, Tianjin

Patentee before: Tianjin University

TR01 Transfer of patent right
PE01 Entry into force of the registration of the contract for pledge of patent right
PE01 Entry into force of the registration of the contract for pledge of patent right

Denomination of invention: A multi robot cooperative target location method based on robocup3d

Effective date of registration: 20211124

Granted publication date: 20191022

Pledgee: Ningbo Tianjin Enterprise Service Co.,Ltd.

Pledgor: NINGBO ZHIWEI RUICHI INFORMATION TECHNOLOGY Co.,Ltd.

Registration number: Y2021330002348

PC01 Cancellation of the registration of the contract for pledge of patent right
PC01 Cancellation of the registration of the contract for pledge of patent right

Date of cancellation: 20221111

Granted publication date: 20191022

Pledgee: Ningbo Tianjin Enterprise Service Co.,Ltd.

Pledgor: NINGBO ZHIWEI RUICHI INFORMATION TECHNOLOGY Co.,Ltd.

Registration number: Y2021330002348

PE01 Entry into force of the registration of the contract for pledge of patent right
PE01 Entry into force of the registration of the contract for pledge of patent right

Denomination of invention: A Multi robot Cooperative Target Location Method Based on RoboCup3D

Effective date of registration: 20221114

Granted publication date: 20191022

Pledgee: Ningbo Tianjin Enterprise Service Co.,Ltd.

Pledgor: NINGBO ZHIWEI RUICHI INFORMATION TECHNOLOGY Co.,Ltd.

Registration number: Y2022980021792