CN112097774A - 基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法 - Google Patents

基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法 Download PDF

Info

Publication number
CN112097774A
CN112097774A CN202010972064.3A CN202010972064A CN112097774A CN 112097774 A CN112097774 A CN 112097774A CN 202010972064 A CN202010972064 A CN 202010972064A CN 112097774 A CN112097774 A CN 112097774A
Authority
CN
China
Prior art keywords
agent
information
ith
moment
matrix
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
CN202010972064.3A
Other languages
English (en)
Other versions
CN112097774B (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.)
Northeastern University Qinhuangdao Branch
Original Assignee
Northeastern University Qinhuangdao Branch
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 Northeastern University Qinhuangdao Branch filed Critical Northeastern University Qinhuangdao Branch
Priority to CN202010972064.3A priority Critical patent/CN112097774B/zh
Publication of CN112097774A publication Critical patent/CN112097774A/zh
Application granted granted Critical
Publication of CN112097774B publication Critical patent/CN112097774B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/20—Instruments for performing navigational calculations
    • 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
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Feedback Control In General (AREA)
  • Multi Processors (AREA)

Abstract

本发明提供基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法，涉及控制和信息技术领域。该方法首先构造多智能体系统的网络拓扑结构图，并确定其邻接矩阵；再根据智能体所使用的运动传感器和测量传感器分别建立各智能体的状态方程及观测方程；根据各智能体的状态方程及观测方程设计自适应卡尔曼滤波算法，获取各智能体的局部地图信息，并将其转换成信息向量和信息矩阵的形式，再利用平均跟踪算法设计实现分布式地图融合方法；设定多个智能体的初始值信息与路标点信息，以及过程噪声和观测噪声的统计量初值，运行分布式地图融合算法，并根据运行结果不断修正分布式地图融合算法，直至算法收敛，最终得到智能体探测区域的全局地图。

Description

基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法
技术领域
本发明涉及控制和信息技术领域，尤其涉及基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法。
背景技术
20世纪90年代末以来，关于单个智能体的同步定位与建图问题(SLAM)一直是人们关注的焦点。同步定位和建图(SLAM)指的是在没有非自主导航方法辅助下，如GPS、北斗等定位系统，智能体依靠实现自身位置估计的传感器，以及获取环境信息的传感器，在实现自身定位的同时绘制探测区域地图的过程。单个智能体进行SLAM过程时，如果系统模型中的噪声统计量未知，那么其准确的系统模型就不可知；同时，随着时间的推移，环境传感器会发生误差积累，导致绘制的地图偏差越来越大。
近年来，多智能体协同SLAM逐渐成为国内外机器人认知领域的研究热点。多智能体系统具有更快的操作速度、更高的执行效率，以及更可靠的系统稳定性。所以，在各种复杂的未知环境下，尤其是没有定位系统辅助的情况下进行救灾、资源勘查和空间探测等特殊任务，多智能体系统更有优势。
卡尔曼滤波已广泛应用于SLAM问题。但是，它需要精确已知的系统模型，这种条件只适用于仿真实验环境下。实际应用过程中，智能体所在的外部环境都是未知，这就导致各个机器人所使用的各种传感器会出现各种偏差。因此，各种噪声的统计量也就是未知且变化的，进而使得每个智能体的系统模型是未知的。
多智能体协同完成SLAM过程时，整个智能体网络结构可能是动态变化的，而且每个智能体的通信半径是有限的。所以，每个智能体需要自主决定在什么情况下和邻居智能体进行通信，从而提高整个算法的运行效率。而且，由于局部地图经常用状态向量和协方差矩阵来表示，这种表现形式下，很难求解多个智能体之间的互协方差。
发明内容
本发明要解决的技术问题是针对上述现有技术的不足，提供一种基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法，利用自适应卡尔曼滤波算法提高每个智能体的定位精度，并绘制出较为精确的局部地图，再通过平均跟踪算法将各个智能体的局部地图融合成全局地图同时实现全局定位。
为解决上述技术问题，本发明所采取的技术方案是：基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法，包括以下步骤：
步骤1：构造多智能体系统的网络拓扑结构图，每个节点代表一个智能体，每条边代表智能体间的信息交互；
所述构造的多智能体系统的网络拓扑结构图记为：
Figure BDA0002684448430000021
其中，
Figure BDA0002684448430000022
表示所有智能体的集合，ε表示所有智能体连边的集合，Nij＝{j|(i,j)∈ε；j≠i}表示由智能体i的邻居智能体所构成的集合，j表示智能体i的邻居智能体；
步骤2：确定所构造的多智能体系统的网络拓扑结构图的邻接矩阵；
多智能体系统的网络拓扑结构图
Figure BDA0002684448430000023
的邻接矩阵A，其形式为：如果智能体i和智能体j有边相连，则Aij＝a，a代表相邻智能体之间边的权重值；如果没有边相连，则为Aij＝0；
步骤3：根据智能体所使用的运动传感器和测量传感器分别建立多智能体系统中各智能体的状态方程及观测方程；
建立多智能体系统中各智能体的状态方程和观测方程，其形式如下：
Figure BDA0002684448430000024
其中，ri,k、ri,k-1分别为时刻k和k-1下第i个智能体的状态信息；ui,k为时刻k第i个智能体的控制输入；zi,k为时刻k下第i个智能体的观测信息；mi,k为时刻k下第i个智能体所观测到的路标点状态信息；wi,k-1为时刻k-1下第i个智能体的过程噪声；vi,k-1为时刻k-1下第i个智能体的观测噪声；wi,k-1和vi,k-1均符合(E～δ)的形式，E是噪声的均值，δ是噪声的协方差；f(ri,k-1,ui,k)是智能体的状态方程；h(ri,k,mi,k)是以相对距离为测量信息的观测方程；g(ri,k,zi,k)则是基于状态信息ri,k和观测信息zi,k求解路标点信息的方程；
步骤4：根据步骤3中各智能体的状态方程及观测方程设计自适应卡尔曼滤波算法，获取各智能体的局部地图信息；
首先设计自适应卡尔曼滤波算法的预测阶段，如下公式所示：
Figure BDA0002684448430000025
Pi,k|k-1＝Fi,kPi,k-1|k-1(Fi,k)T+Qi,k-1 (3)
其中，
Figure BDA0002684448430000026
是时刻k-1下第i个智能体的状态预测值；
Figure BDA0002684448430000027
是时刻k-1下第i个智能体的状态估计值；Fi,k是状态方程f(·)关于
Figure BDA0002684448430000028
的雅各比矩阵；ui,k是时刻k下第i个智能体的控制输入；Pi,k|k-1是时刻k-1下第i个智能体的状态协方差矩阵预测值；Pi,k-1|k-1是时刻k-1下第i个智能体的状态协方差矩阵估计值；Qi,k-1是时刻k-1下第i个智能体的状态噪声协方差矩阵；(·)T指的是矩阵的转置；
其次设计自适应卡尔曼滤波算法的更新阶段：
Figure BDA0002684448430000031
Figure BDA0002684448430000032
Figure BDA0002684448430000033
其中，l为噪声自适应过程的迭代步数；
Figure BDA0002684448430000034
为时刻k下第i个智能体经过l+1步噪声自适应处理的卡尔曼增益矩阵；
Figure BDA0002684448430000035
是为时刻k下第i个智能体经过l步噪声自适应处理后观测方程h(·)的雅各比矩阵；
Figure BDA0002684448430000036
是时刻k下第i个智能体的观测噪声协方差矩阵；
Figure BDA0002684448430000037
为时刻k下第i个智能体经过l+1步噪声自适应处理的状态估计值；
Figure BDA0002684448430000038
为时刻k下第i个智能体观测到的路标点信息估计值；
Figure BDA0002684448430000039
是时刻k下第i个智能体经过l+1步噪声自适应处理的状态协方差矩阵估计值；
Figure BDA00026844484300000310
是时刻k下第i个智能体经过l步噪声自适应处理的状态协方差矩阵预测值；
之后设计自适应卡尔曼滤波算法的噪声自适应迭代阶段：
Figure BDA00026844484300000311
Figure BDA00026844484300000312
最终，利用得到的状态信息
Figure BDA00026844484300000313
和观测信息zi,k，求解出每个智能体建立的局部地图信息，如下公式所示：
Figure BDA00026844484300000314
Figure BDA00026844484300000315
其中，Ei,k为时刻k下第i个智能体局部地图信息协方差矩阵；
Figure BDA00026844484300000316
为时刻k下第i个智能体局部地图信息估计值；mi,k为时刻k下第i个智能体局部地图信息实际值；
步骤5：将步骤4中获取到的每个智能体建立的局部地图信息转换成信息向量和信息矩阵的形式，再利用平均跟踪算法设计实现分布式地图融合方法；
步骤5.1：将每个智能体建立的局部地图以均值
Figure BDA0002684448430000041
和协方差Ei,k的常规表现形式转换成信息形式，使多智能体系统中每个智能体自主决定其自身与其他智能体之间的通信策略，并实现分布式控制，具体转换公式如下所示：
Figure BDA0002684448430000042
其中，ii,k为时刻k下第i个智能体的信息向量，Ii,k为时刻k下第i个智能体的信息矩阵；
步骤5.2：利用平均跟踪算法将每个智能体建立的局部地图进行融合，最终得到每个智能体探测区域的全局地图，具体方法为：
首先是测量更新阶段：
当k∈Ti,di,k＝di,k-1+1
Figure BDA0002684448430000043
Figure BDA0002684448430000044
当
Figure BDA0002684448430000045
di,k＝di,k-1
Figure BDA0002684448430000046
Figure BDA0002684448430000047
其中，Ti为第i个智能体进行通信的时刻集合；di,k为时刻k下第i个智能体进行通信的次数；
Figure BDA0002684448430000048
为时刻k下第i个智能体的信息矩阵预测值；
Figure BDA0002684448430000049
为时刻k下第i个智能体的信息向量预测值；
Figure BDA00026844484300000410
为时刻k下第i个智能体的信息矩阵估计值；
Figure BDA00026844484300000411
为时刻k下第i个智能体的信息向量估计值；
最后是空间更新阶段：
如果di,k＞0则：
Figure BDA00026844484300000412
Figure BDA00026844484300000413
其中，Ni,k为时刻k下第i个智能体的邻居智能体集合；
Figure BDA00026844484300000414
为时刻k下第j个智能体的信息矩阵预测值；
Figure BDA00026844484300000415
为时刻k下第j个智能体的信息向量预测值；Wij为邻居权重矩阵，其具体形式如下：
Figure BDA0002684448430000051
Figure BDA0002684448430000052
其中，dj,k为时刻k下第j个智能体进行通信的次数；
Figure BDA0002684448430000053
和
Figure BDA0002684448430000054
分别为第i个和第j个智能体及其邻居智能体的通信次数之和；
不断修正分布式地图融合算法，直至算法收敛，其收敛结果如下：
Figure BDA0002684448430000055
Figure BDA0002684448430000056
其中，
Figure BDA0002684448430000057
和
Figure BDA0002684448430000058
分别是信息矩阵和信息向量的全局形式；
Figure BDA0002684448430000059
其代表时刻k下所有智能体的通信次数之和，n为多智能体系统中智能体总数；
最终，每个智能体得到的全局形式的地图信息和全局形式的信息向量、信息矩阵之间的关系如下所示：
Figure BDA00026844484300000510
Figure BDA00026844484300000511
其中，
Figure BDA00026844484300000512
和
Figure BDA00026844484300000513
分别是地图信息协方差矩阵和地图信息估计值的全局形式；
步骤6：设定多个智能体的初始值信息与路标点信息，以及过程噪声和观测噪声的统计量初值，运行步骤5设计的分布式地图融合算法；
对每个智能体的初始状态ri,0，控制输入ui,k以及路标点信息进行初始化设置；同时，对分布式地图融合算法所用到的一些中间变量进行初始化设置；
步骤7：根据步骤6的运行结果不断修正分布式地图融合算法，直至算法收敛至式(23)，最终得到智能体探测区域的全局地图。
采用上述技术方案所产生的有益效果在于：本发明提供的基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法，可以实时地动态估计每个智能体获取局部地图过程中测量噪声和过程噪声统计量。对于多智能体系统的SLAM问题，现有的一些处理噪声的算法都是只能针对测量噪声或者过程噪声是未知的情况；另外，能同时处理两种噪声的算法只能先获取到所有的信息，然后离线进行优化过程，不能实时进行此过程。然而，本发明提出的方法既可以同时动态处理两种噪声问题，又能在机器人运行过程中实时进行优化。同时，该方法为分布式算法，并且每个智能体根据当前时刻所获取的局部地图信息是否有增加来自主决定是否和邻居智能体进行通信，这样对网络拓扑结构的要求简单，适用性强，并且计算量相对更小，大大提高了算法的运算效率。
附图说明
图1为本发明实施例提供的基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法的流程图；
图2为本发明实施例提供的3个两轮智能体构成的多智能体系统的网络拓扑结构图；
图3为本发明实施例提供的不同时刻各智能体对目标区域进行探测的结果，其中，(a)为k＝10时各个机器人对目标区域进行探测的结果，(b)为k＝20时各个机器人对目标区域进行探测的结果，(c)k＝30时各个机器人对目标区域进行探测的结果；
图4为本发明实施例提供的三个智能体的信息向量和信息矩阵图，其中，(a)为三个智能体的信息向量图，(b)为三个智能体的信息矩阵图；
图5为本发明实施例提供的多智能体系统中各个智能体对路标点(0，-4)的观测结果。
具体实施方式
下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。
本实施例以3个两轮智能体构成的多智能体系统为例，采用本发明的基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法得到智能体探测区域的全局地图。
本实施例中，基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法，如图1所示，包括以下步骤：
步骤1：构造3个两轮智能体所构成的多智能体的网络拓扑结构图，如图2所示，每个节点代表一个智能体，每条边代表智能体间的信息交互；
所述构造的多智能体系统的网络拓扑结构图记为：
Figure BDA0002684448430000061
其中，
Figure BDA0002684448430000062
表示所有智能体的集合，ε表示所有智能体连边的集合，Nij＝{j|(i,j)∈ε；j≠i}表示由智能体i的邻居智能体所构成的集合，j表示智能体i的邻居智能体；
步骤2：确定所构造的多智能体系统的网络拓扑结构图的邻接矩阵；
多智能体系统的网络拓扑结构图
Figure BDA0002684448430000063
的邻接矩阵A，其形式为：如果智能体i和智能体j有边相连，则Aij＝a，a代表相邻智能体之间边的权重值；如果没有边相连，则为Aij＝0；
本实施例中，多智能体系统的网络拓扑结构图
Figure BDA0002684448430000071
的邻接矩阵A，其形式为：
Figure BDA0002684448430000072
步骤3：根据智能体所采用的惯性导航方式以及激光雷达建立多智能体系统中各智能体的状态方程及观测方程；
首先，根据惯性导航方式，建立智能体的状态方程，具体形式如下，
Figure BDA0002684448430000073
其中，
Figure BDA0002684448430000074
Figure BDA0002684448430000075
为时刻k下第i个智能体状态信息的横坐标，
Figure BDA0002684448430000076
为时刻k下第i个智能体状态信息的纵坐标，
Figure BDA0002684448430000077
为时刻k下第i个智能体状态信息的偏航角；ΔT表示时间间隔；Vi表示第i个智能体的运行速度；γi,k表示时刻k下第i个智能体的舵角；Li表示第i个智能体两个轮子之间的宽度；
Figure BDA0002684448430000078
分别是时刻k下第i个智能体的状态信息的横坐标、纵坐标以及偏航角的噪声信息；f(ri,k-1,ui,k)是智能体的状态方程；
其次，根据激光雷达测距传感器的原理，建立智能体的观测方程，如下公式所示：
Figure BDA0002684448430000079
其中，zi,k为时刻k下第i个智能体的观测信息；Di,k为时刻k下第i个智能体与某个路标点的相对距离，θi,k为时刻k第i个智能体与某个路标点之间的角度；
Figure BDA00026844484300000710
分别是时刻k下第i个智能体观测信息中的相对距离以及智能体和路标点夹角的噪声信息；h(ri,k,mi,k)是以相对距离为测量信息的观测方程；mi,k为时刻k下第i个智能体所观测到的路标点状态信息，如下公式所示：
Figure BDA00026844484300000711
其中，g(ri,k,zi,k)则是基于状态信息ri,k和观测信息zi,k求解路标点信息的方程；
Figure BDA0002684448430000081
分别是第i个智能体所观测到的路标点状态信息的横坐标和纵坐标；
此外，定义每个智能体进行SLAM过程中所用变量的具体形式，如下所示，
Figure BDA0002684448430000082
其中，xi,k包含时刻k下第i个智能体自身的状态信息ri,k以及所观测到的路标点状态信息mi,k；Pi,k是时刻k下第i个智能体所有信息的协方差矩阵，其中，rrPi,k是时刻k下第i个智能体自身的协方差矩阵；rmPi,k和mrPi,k分别是时刻k下第i个智能体和其所观测到的路标点之间的互协方差矩阵；mmPi,k是时刻k下第i个智能体所观测到的路标点之间的协方差矩阵；
步骤4：根据步骤3中的状态方程和观测方程设计自适应卡尔曼算法；
首先设计自适应卡尔曼滤波的预测阶段：
Figure BDA0002684448430000083
Figure BDA0002684448430000084
rmPi,k|k-1＝Fi,k rrPi,k|k-1 (8)
mrPi,k|k-1＝(rmPi,k|k-1)T (9)
其中，
Figure BDA0002684448430000085
是时刻k-1下第i个智能体的状态预测值；
Figure BDA0002684448430000086
是时刻k-1下第i个智能体的状态估计值；rrPi,k|k-1是时刻k-1下第i个智能体自身状态的协方差矩阵预测值；rmPi,k|k-1和mrPi,k|k-1分别是时刻k-1下第i个智能体状态和其所观测到的路标点之间的互协方差矩阵预测值；mmPi,k|k-1是时刻k-1下第i个智能体所观测到的路标点信息之间的协方差矩阵预测值；Qi,k-1是时刻k-1下第i个智能体的过程噪声协方差矩阵；
Figure BDA0002684448430000087
是状态方程关于状态信息的雅各比矩阵，
Figure BDA0002684448430000088
是状态方程关于过程噪声的雅各比矩阵，其各自形式如下：
Figure BDA0002684448430000091
Figure BDA0002684448430000092
其次设计自适应卡尔曼滤波的更新阶段：
Figure BDA0002684448430000093
Figure BDA0002684448430000094
Figure BDA0002684448430000095
其中，l为噪声自适应过程的迭代步数；
Figure BDA0002684448430000096
为卡尔曼增益矩阵；
Figure BDA0002684448430000097
是时刻k下第i个智能体所有信息经过l步噪声自适应处理的协方差矩阵；
Figure BDA0002684448430000098
是时刻k下，第i个智能体的观测噪声经过l步噪声自适应处理的协方差矩阵；Hi,k是观测方程的雅各比矩阵，Hi,k＝[rHi,k mHi,k]；rHi,k为时刻k下，第i个智能体的观测方程对于状态信息的雅各比矩阵，mHi,k为时刻k下，第i个智能体的观测方程对于路标点信息的雅各比矩阵，如下公式所示：
Figure BDA0002684448430000099
Figure BDA00026844484300000910
之后设计自适应卡尔曼滤波的噪声自适应阶段：
Figure BDA00026844484300000911
Figure BDA00026844484300000912
最终，利用得到的状态信息
Figure BDA00026844484300000913
和观测信息zi,k，求解出每个智能体建立的局部地图信息，如下公式所示：
Figure BDA00026844484300000914
Figure BDA00026844484300000915
其中，rGi,k为时刻k下，第i个智能体的方程g(·)对于状态信息的雅各比矩阵；zGi,k为时刻k下，第i个智能体的方程g(·)对于路标点信息的雅各比矩阵；
步骤5：将步骤4中获取到的每个智能体建立的局部地图信息转换成信息向量和信息矩阵的形式，再利用平均跟踪算法设计实现分布式地图融合方法；
步骤5.1：将每个智能体建立的局部地图以均值
Figure BDA0002684448430000101
和协方差Ei,k的常规表现形式转换成信息形式，使多智能体系统中每个智能体自主决定其自身与其他智能体之间的通信策略，并实现分布式控制，具体转换公式如下所示：
Figure BDA0002684448430000102
其中，ii,k为时刻k下第i个智能体的信息向量，Ii,k为时刻k下第i个智能体的信息矩阵；
步骤5.2：利用平均跟踪算法将每个智能体建立的局部地图进行融合，最终得到每个智能体探测区域的全局地图，具体方法为：
首先是测量更新阶段：
当k∈Ti,di,k＝di,k-1+1
Figure BDA0002684448430000103
Figure BDA0002684448430000104
当
Figure BDA0002684448430000105
di,k＝di,k-1
Figure BDA0002684448430000106
Figure BDA0002684448430000107
其中，Ti为第i个智能体进行通信的时刻集合；di,k为时刻k下第i个智能体进行通信的次数；
Figure BDA0002684448430000108
为时刻k下第i个智能体的信息矩阵预测值；
Figure BDA0002684448430000109
为时刻k下第i个智能体的信息向量预测值；
Figure BDA00026844484300001010
为时刻k下第i个智能体的信息矩阵估计值；
Figure BDA00026844484300001011
为时刻k下第i个智能体的信息向量估计值；
最后是空间更新阶段：
如果di,k＞0则：
Figure BDA00026844484300001012
Figure BDA0002684448430000111
其中，Ni,k为时刻k下第i个智能体的邻居智能体集合；
Figure BDA0002684448430000112
为时刻k下第j个智能体的信息矩阵预测值；
Figure BDA0002684448430000113
为时刻k下第j个智能体的信息向量预测值；Wij为邻居权重矩阵，其具体形式如下：
Figure BDA0002684448430000114
Figure BDA0002684448430000115
其中，dj,k为时刻k下第j个智能体进行通信的次数；
Figure BDA0002684448430000116
和
Figure BDA0002684448430000117
分别为第i个和第j个智能体及其邻居智能体的通信次数之和；
不断修正分布式地图融合算法，直至算法收敛，其收敛结果如下：
Figure BDA0002684448430000118
Figure BDA0002684448430000119
其中，
Figure BDA00026844484300001110
和
Figure BDA00026844484300001111
分别是信息矩阵和信息向量的全局形式；
Figure BDA00026844484300001112
其代表时刻k下所有智能体的通信次数之和，n为多智能体系统中智能体总数；
最终，全局形式的地图信息和全局形式的信息向量、信息矩阵之间的关系如下所示：
Figure BDA00026844484300001113
Figure BDA00026844484300001114
其中，
Figure BDA00026844484300001115
和
Figure BDA00026844484300001116
分别是地图信息协方差矩阵和地图信息估计值的全局形式；
步骤6：设定多智能体的初始值信息与路标点信息，以及过程噪声和观测噪声的统计量初值，运行步骤5设计的分布式地图融合算法；
对每个智能体的初始状态ri,0，并设置每个智能体的恒定速度Vi以及每个智能体两个轮子之间的宽度的Li，以及对路标点信息进行初始化设置；同时，对分布式地图融合算法所用到的一些中间变量进行初始化设置，如：di,0＝0，
Figure BDA00026844484300001117
wi,0＝(wi,0～Qi,0)，vi,0＝(vi,0～Ri,0)，wi,0是过程噪声的均值，Qi,0是过程噪声的协方差矩阵，vi,0是观测噪声的均值，Ri,0是观测噪声的协方差矩阵；
本实施例运行步骤5设计的分布式地图融合算法，其运行过程的仿真效果如图3所示。在图3的(a)、(b)、(c)中，图中共有三个三角形，分别代表三个智能体，带‘*’的三角形代表智能体1，带‘+’的三角形代表智能体2，带‘o’的智能体代表智能体3，以上三种形状分别代表其自身位置的估计值；另外，普通的实线三角形代表各智能体的实际位置。其次，每个三角形内部的椭圆代表着各自的协方差信息。圆点表示路标点的实际值，各智能体对路标点的估计值的表现形式分别是：‘*’代表智能体1对路标点的估计值，‘+’代表智能体2对路标点的估计值，‘x’代表智能体3对路标点的估计值。以路标点估计值为中心的椭圆分别代表以下含义：实线椭圆代表智能体1对路标点估计值的协方差信息，点线椭圆代表智能体2对路标点估计值的协方差信息，虚线椭圆代表智能体3对路标点估计值的协方差信息。
步骤7：根据步骤6的运行结果不断修正分布式地图融合算法，直至算法收敛至式(30)和式(31)；其最终的收敛结果如图4所示，各智能体的信息向量和信息矩阵达到一致；同时，各智能体对路标点的估计值与对应的协方差信息达到一致，如图5所示。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明权利要求所限定的范围。

Claims (5)

1.一种基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法，其特征在于：包括以下步骤：
步骤1：构造多智能体系统的网络拓扑结构图，每个节点代表一个智能体，每条边代表智能体间的信息交互；
所述构造的多智能体系统的网络拓扑结构图记为：
Figure FDA0002684448420000011
其中，
Figure FDA0002684448420000014
表示所有智能体的集合，ε表示所有智能体连边的集合，Nij＝{j|(i,j)∈ε；j≠i}表示由智能体i的邻居智能体所构成的集合，j表示智能体i的邻居智能体；
步骤2：确定所构造的多智能体系统的网络拓扑结构图的邻接矩阵；
多智能体系统的网络拓扑结构图
Figure FDA0002684448420000012
的邻接矩阵A，其形式为：如果智能体i和智能体j有边相连，则Aij＝a，a代表相邻智能体之间边的权重值；如果没有边相连，则为Aij＝0；
步骤3：根据智能体所使用的运动传感器和测量传感器分别建立多智能体系统中各智能体的状态方程及观测方程；
步骤4：根据步骤3中各智能体的状态方程及观测方程设计自适应卡尔曼滤波算法，获取各智能体的局部地图信息；
步骤5：将步骤4中获取到的每个智能体建立的局部地图信息转换成信息向量和信息矩阵的形式，再利用平均跟踪算法设计实现分布式地图融合方法；
步骤6：设定多个智能体的初始值信息与路标点信息，以及过程噪声和观测噪声的统计量初值，运行步骤5设计的分布式地图融合算法；
步骤7：根据步骤6的运行结果不断修正分布式地图融合算法，直至算法收敛，最终得到智能体探测区域的全局地图。
2.根据权利要求1所述的基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法，其特征在于：所述步骤3建立的多智能体系统中各智能体的状态方程和观测方程，其形式如下：
Figure FDA0002684448420000013
其中，ri,k、ri,k-1分别为时刻k和k-1下第i个智能体的状态信息；ui,k为时刻k第i个智能体的控制输入；zi,k为时刻k下第i个智能体的观测信息；mi,k为时刻k下第i个智能体所观测到的路标点状态信息；wi,k-1为时刻k-1下第i个智能体的过程噪声；vi,k-1为时刻k-1下第i个智能体的观测噪声；wi,k-1和vi,k-1均符合(E～δ)的形式，E是噪声的均值，δ是噪声的协方差；f(ri,k-1,ui,k)是智能体的状态方程；h(ri,k,mi,k)是以相对距离为测量信息的观测方程；g(ri,k,zi,k)则是基于状态信息ri,k和观测信息zi,k求解路标点信息的方程。
3.根据权利要求2所述的基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法，其特征在于：所述步骤4的具体方法为：
首先设计自适应卡尔曼滤波算法的预测阶段，如下公式所示：
Figure FDA0002684448420000021
Pi,k|k-1＝Fi,kPi,k-1|k-1(Fi,k)T+Qi,k-1 (3)
其中，
Figure FDA0002684448420000022
是时刻k-1下第i个智能体的状态预测值；
Figure FDA0002684448420000023
是时刻k-1下第i个智能体的状态估计值；Fi,k是状态方程f(·)关于
Figure FDA0002684448420000024
的雅各比矩阵；ui,k是时刻k下第i个智能体的控制输入；Pi,k|k-1是时刻k-1下第i个智能体的状态协方差矩阵预测值；Pi,k-1|k-1是时刻k-1下第i个智能体的状态协方差矩阵估计值；Qi,k-1是时刻k-1下第i个智能体的状态噪声协方差矩阵；(·)T指的是矩阵的转置；
其次设计自适应卡尔曼滤波算法的更新阶段：
Figure FDA0002684448420000025
Figure FDA0002684448420000026
Figure FDA0002684448420000027
其中，l为噪声自适应过程的迭代步数；
Figure FDA0002684448420000028
为时刻k下第i个智能体经过l+1步噪声自适应处理的卡尔曼增益矩阵；
Figure FDA0002684448420000029
是为时刻k下第i个智能体经过l步噪声自适应处理后观测方程h(·)的雅各比矩阵；
Figure FDA00026844484200000210
是时刻k下第i个智能体的观测噪声协方差矩阵；
Figure FDA00026844484200000211
为时刻k下第i个智能体经过l+1步噪声自适应处理的状态估计值；
Figure FDA00026844484200000212
为时刻k下第i个智能体观测到的路标点信息估计值；
Figure FDA00026844484200000213
是时刻k下第i个智能体经过l+1步噪声自适应处理的状态协方差矩阵估计值；
Figure FDA00026844484200000214
是时刻k下第i个智能体经过l步噪声自适应处理的状态协方差矩阵预测值；
之后设计自适应卡尔曼滤波算法的噪声自适应迭代阶段：
Figure FDA00026844484200000215
Figure FDA0002684448420000031
最终，利用得到的状态信息
Figure FDA0002684448420000032
和观测信息zi,k，求解出每个智能体建立的局部地图信息，如下公式所示：
Figure FDA0002684448420000033
Figure FDA0002684448420000034
其中，Ei,k为时刻k下第i个智能体局部地图信息协方差矩阵；
Figure FDA0002684448420000035
为时刻k下第i个智能体局部地图信息估计值；mi,k为时刻k下第i个智能体局部地图信息实际值。
4.根据权利要求3所述的基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法，其特征在于：所述步骤5的具体方法为：
步骤5.1：将每个智能体建立的局部地图以均值
Figure FDA0002684448420000036
和协方差Ei,k的常规表现形式转换成信息形式，使多智能体系统中每个智能体自主决定其自身与其他智能体之间的通信策略，并实现分布式控制，具体转换公式如下所示：
Figure FDA0002684448420000037
其中，ii,k为时刻k下第i个智能体的信息向量，Ii,k为时刻k下第i个智能体的信息矩阵；
步骤5.2：利用平均跟踪算法将每个智能体建立的局部地图进行融合，最终得到每个智能体探测区域的全局地图，具体方法为：
首先是测量更新阶段：
当k∈Ti,di,k＝di,k-1+1
Figure FDA0002684448420000038
Figure FDA0002684448420000039
当
Figure FDA00026844484200000310
di,k＝di,k-1
Figure FDA00026844484200000311
Figure FDA00026844484200000312
其中，Ti为第i个智能体进行通信的时刻集合；di,k为时刻k下第i个智能体进行通信的次数；
Figure FDA00026844484200000313
为时刻k下第i个智能体的信息矩阵预测值；
Figure FDA00026844484200000314
为时刻k下第i个智能体的信息向量预测值；
Figure FDA0002684448420000041
为时刻k下第i个智能体的信息矩阵估计值；
Figure FDA0002684448420000042
为时刻k下第i个智能体的信息向量估计值；
最后是空间更新阶段：
如果di,k＞0则：
Figure FDA0002684448420000043
Figure FDA0002684448420000044
其中，Ni,k为时刻k下第i个智能体的邻居智能体集合；
Figure FDA0002684448420000045
为时刻k下第j个智能体的信息矩阵预测值；
Figure FDA0002684448420000046
为时刻k下第j个智能体的信息向量预测值；Wij为邻居权重矩阵，其具体形式如下：
Figure FDA0002684448420000047
Figure FDA0002684448420000048
其中，dj,k为时刻k下第j个智能体进行通信的次数；
Figure FDA0002684448420000049
和
Figure FDA00026844484200000410
分别为第i个和第j个智能体及其邻居智能体的通信次数之和；
不断修正分布式地图融合算法，直至算法收敛，其收敛结果如下：
Figure FDA00026844484200000411
Figure FDA00026844484200000412
其中，
Figure FDA00026844484200000413
和
Figure FDA00026844484200000414
分别是信息矩阵和信息向量的全局形式；
Figure FDA00026844484200000415
其代表时刻k下所有智能体的通信次数之和，n为多智能体系统中智能体总数；
最终，每个智能体得到的全局形式的地图信息和全局形式的信息向量、信息矩阵之间的关系如下所示：
Figure FDA00026844484200000416
Figure FDA00026844484200000417
其中，
Figure FDA0002684448420000051
和
Figure FDA0002684448420000052
分别是地图信息协方差矩阵和地图信息估计值的全局形式。
5.根据权利要求4所述的基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法，其特征在于：步骤5所述设定多个智能体的初始值信息与路标点信息，以及过程噪声和观测噪声的统计量初值，具体为：
对每个智能体的初始状态ri,0，控制输入ui,k以及路标点信息进行初始化设置；同时，对分布式地图融合算法所用到的一些中间变量进行初始化设置。
CN202010972064.3A 2020-09-16 2020-09-16 基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法 Active CN112097774B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010972064.3A CN112097774B (zh) 2020-09-16 2020-09-16 基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010972064.3A CN112097774B (zh) 2020-09-16 2020-09-16 基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法

Publications (2)

Publication Number Publication Date
CN112097774A true CN112097774A (zh) 2020-12-18
CN112097774B CN112097774B (zh) 2022-08-23

Family

ID=73759648

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010972064.3A Active CN112097774B (zh) 2020-09-16 2020-09-16 基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法

Country Status (1)

Country Link
CN (1) CN112097774B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113848703A (zh) * 2021-08-28 2021-12-28 同济大学 一种多智能体系统状态估计方法
CN114339595A (zh) * 2021-12-24 2022-04-12 北京理工大学重庆创新中心 一种基于多模型预测的超宽带动态反演定位方法
CN116389165A (zh) * 2023-05-26 2023-07-04 南京信息工程大学 非线性系统分布式安全状态估计方法、系统、装置及介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105737832A (zh) * 2016-03-22 2016-07-06 北京工业大学 基于全局最优数据融合的分布式slam方法
CN106991691A (zh) * 2017-02-24 2017-07-28 北京理工大学 一种适用于摄像机网络下的分布式目标跟踪方法
CN109813319A (zh) * 2019-03-07 2019-05-28 山东大学 一种基于slam建图的开环优化方法及系统
CN111089585A (zh) * 2019-12-30 2020-05-01 哈尔滨理工大学 一种基于传感器信息融合的建图及定位方法
CN111385155A (zh) * 2020-03-18 2020-07-07 东北大学秦皇岛分校 一种基于韧性协议的分布式平均跟踪方法
CN111414575A (zh) * 2020-03-18 2020-07-14 东北大学秦皇岛分校 基于符号函数的多智能体系统的分布式广义跟踪方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105737832A (zh) * 2016-03-22 2016-07-06 北京工业大学 基于全局最优数据融合的分布式slam方法
CN106991691A (zh) * 2017-02-24 2017-07-28 北京理工大学 一种适用于摄像机网络下的分布式目标跟踪方法
CN109813319A (zh) * 2019-03-07 2019-05-28 山东大学 一种基于slam建图的开环优化方法及系统
CN111089585A (zh) * 2019-12-30 2020-05-01 哈尔滨理工大学 一种基于传感器信息融合的建图及定位方法
CN111385155A (zh) * 2020-03-18 2020-07-07 东北大学秦皇岛分校 一种基于韧性协议的分布式平均跟踪方法
CN111414575A (zh) * 2020-03-18 2020-07-14 东北大学秦皇岛分校 基于符号函数的多智能体系统的分布式广义跟踪方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
殷鹏等: "基于多分辨率粒子滤波的全局协同定位方法", 《中国科学：技术科学》 *
王璐等: "多智能体协同视觉ＳＬＡＭ 技术研究进展", 《导航定位与授时》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113848703A (zh) * 2021-08-28 2021-12-28 同济大学 一种多智能体系统状态估计方法
CN113848703B (zh) * 2021-08-28 2023-12-08 同济大学 一种多智能体系统状态估计方法
CN114339595A (zh) * 2021-12-24 2022-04-12 北京理工大学重庆创新中心 一种基于多模型预测的超宽带动态反演定位方法
CN114339595B (zh) * 2021-12-24 2023-12-01 北京理工大学重庆创新中心 一种基于多模型预测的超宽带动态反演定位方法
CN116389165A (zh) * 2023-05-26 2023-07-04 南京信息工程大学 非线性系统分布式安全状态估计方法、系统、装置及介质
CN116389165B (zh) * 2023-05-26 2023-09-12 南京信息工程大学 非线性系统分布式安全状态估计方法、系统、装置及介质

Also Published As

Publication number Publication date
CN112097774B (zh) 2022-08-23

Similar Documents

Publication Publication Date Title
CN112097774B (zh) 基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法
WO2022205526A1 (zh) 一种用于水下无人机器人集群的动态定位信息融合方法
CN111240319B (zh) 室外多机器人协同作业系统及其方法
CN110244715B (zh) 一种基于超宽带技术的多移动机器人高精度协同跟踪方法
CN108896047B (zh) 分布式传感器网络协同融合与传感器位置修正方法
CN111522341A (zh) 网络异构机器人系统的多时变编队跟踪控制方法及系统
Shi et al. Adaptive informative sampling with environment partitioning for heterogeneous multi-robot systems
CN109947131A (zh) 一种基于强化学习的多水下机器人编队控制方法
Zhao et al. Review of slam techniques for autonomous underwater vehicles
CN116772867A (zh) 一种基于因子图的节点优选的多auv自适应协同定位方法及系统
CN117390498A (zh) 一种基于Transformer模型的固定翼集群无人机飞行能力评估方法
Gao et al. Localization of mobile robot based on multi-sensor fusion
CN114323034A (zh) 一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法
Song et al. Cooperative Positioning Algorithm Based on Manifold Gradient Filtering in UAV-WSN
Tao et al. Motion planning for slam based on frontier exploration
CN115755009A (zh) 一种多水下机器人分布式协同定位方法及系统
Tao et al. Cooperative simultaneous localization and mapping for multi-robot: Approach & experimental validation
Tong et al. Multi-robot cooperative map building in unknown environment considering estimation uncertainty
CN115145312A (zh) 仅方位角测量下基于双领导者模式的集群协同控制方法
CN113503891B (zh) 一种sinsdvl对准校正方法、系统、介质及设备
Zhao et al. 3D indoor map building with monte carlo localization in 2D map
Carney et al. Multi-agents path planning for a swarm of unmanned aerial vehicles
Huang et al. Multi-Robot Autonomous Exploration and Mapping Under Localization Uncertainty with Expectation-Maximization
CN114942646B (zh) 异构无人系统三维空间编队控制方法
CN118463996B (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