CN103970135A - 一种基于mapso优化粒子滤波的多移动机器人协作定位方法 - Google Patents

Abstract

本发明公开了一种基于MAPSOPF算法的多机器人协作定位控制方法，包括步骤：将实际运行环境抽象成一张平面图，根据路标的位置和机器人的路径建立环境地图；初始化机器人相关信息、黑板信息；采用MAPSOPF算法并根据适应度值预测机器人下一步的位置，更新预测位置、黑板信息，完成精确定位。通过调整权重系数和学习因子，引入竞争机制，以提高算法的优化效果，最终提高多机器人的协作定位精度。

Description

一种基于MAPSO优化粒子滤波的多移动机器人协作定位方法

技术领域

本发明涉及一种多移动机器人的协作定位方法，尤其涉及一种基于多Agent粒子群优化(MAPSO)的粒子滤波算法的多移动机器人协作定位方法。

背景技术

多移动机器人的协作定位是一个具有典型性和通用性的多机器人问题，是多机器人同时定位与建图的重要组成部分。多机器人协作定位技术已广泛应用于航天、工农业、柔性制造业等领域。目前解决多移动机器人的协作定位问题的算法很多，主要有遗传算法、卡尔曼滤波算法、粒子滤波算法等。遗传算法容易陷入局部极小值，且该算法对参数的选取有很强的依赖性；卡尔曼滤波算法仅限于解决线性、高斯问题，且雅克比矩阵的计算比较复杂；粒子滤波算法容易出现粒子贫化问题，使得粒子聚集在错误的解周围，降低定位精度。

粒子群优化算法(PSO)需要调整的参数较少，易于实现，且可进行并行计算和无梯度计算，有较好的全局搜索能力。将粒子群优化算法应用于多机器人协作定位控制，调整提议分布，提高定位精度。在较大规模的多机器人系统中，要优化的行为控制参数很多，即优化目标维数高，目标函数极值多，采用标准的粒子群优化算法及后来改进的粒子滤波算法，很难准确收敛到全局最优值。

本发明提出了一种基于多Agent优化的PSOPF算法的多机器人协作定位控制方法。Agent具有自治性、交互性、反应性、推理性、规划能力、学习能力等特点。本发明将多Agent系统(MAS)与PSOPF算法进行结合，提出一种改进的粒子群优化粒子滤波算法(MAPSOPF)，核心是提高算法的收敛性能和执行效率，优化机器人对路标的观测来提高定位与建图的精度。

发明内容

针对以上现有技术中的不足，本发明的目的在于提供一种提高算法的收敛性能和执行效率、提高定位与建图的精度的多移动机器人协作定位方法，本发明的技术方案如下：一种基于MAPSO优化粒子滤波的多移动机器人协作定位方法，所述机器人自身设置有用于获取仿真环境全局地图信息的探测器、用于计算运动路程信息的里程计、罗盘及激光扫描仪，其包括以下步骤：

101、将机器人的实际运行环境抽象成一张平面图，然后在所述平面图中加上路标的位置和机器人的路径形成一张环境地图，用ID号标记路标；

102、根据步骤101得到的环境地图，初始化仿真环境、机器人的位置信息和黑板信息，其中黑板信息包括各机器人的身份及位置坐标、观测路标位置以及最大适应度值σ；在仿真环境的地图中任意设置若干个路标位置点，同时设置每个机器人的路径，将每个机器人的位置作为一个粒子，若干个粒子构成粒子群，将粒子群初始化为3*N的矩阵，其中3表示待优化的三个参数，即机器人的X坐标值、Y坐标值和角度值，N表示粒子个数，机器人通过自身的探测器获取仿真环境的全局地图信息；

103、每个机器人根据自身携带的里程计信息和罗盘信息预测下一时刻的位置，采用自身携带的激光扫描仪对路标进行观测，得到机器人的运动模型(1)和观测模型如下：

$\left\{\begin{array}{c}{x}_r\left(t\right)=x_r(t-1)+v\·\mathrm{\ΔT}\·\cos [\mathrm{\φ}(t-1)+\mathrm{\ω}(t-1)\·\mathrm{\ΔT}]+{\mathrm{\β}}_x\\ y_r\left(t\right)=y_r(t-1)+v\·\mathrm{\ΔT}\·\sin [\mathrm{\φ}(t-1)+\mathrm{\ω}(t-1)\·\mathrm{\ΔT}]+{\mathrm{\β}}_y\\ \mathrm{\φ}\left(t\right)=\mathrm{\φ}(t-1)+\mathrm{\ω}(t-1)\·\mathrm{\ΔT}+{\mathrm{\β}}_{\mathrm{\φ}}\end{array}\right\}---\left(1\right)$

其中，x_r(t)，y_r(t)，θ(t)是机器人在时刻t的位姿，β_x,β_y和β_φ是运动噪声，ΔT是内部传感器采样时间间隔，v是机器人的运行速度，d_rl是传感器观测到的路标与机器人之间的距离，表示传感器观测到的路标与机器人前进方向的夹角，α_R和α_θ是观测噪声，是机器人航向角；

104、根据步骤103中得到的运动模型和观测模型，采用MAPSOPF算法对步骤103所得预测位置以及观测信息进行更新并计算粒子适应度值f_fitness，其中f_fitness＝exp{-sqrt[(z_t-z_tPre)R^-1(z_t-z_tPre)^T/c₃]}，其中，R是观测噪声的协方差矩阵，z_tPre是在时刻t机器人对路标的预测值，c₃为一定值，z_t表示路标观测值，z_t为z_t根据和m计算得到：其中m是路标位置坐标的集合，是粒子的预测位姿；如果该信息的适应度函数值f_fitness小于步骤102中黑板信息中的最大适应度值σ，则根据黑板中的信息对机器人的定位信息进行调整；否则就采用步骤103中得到观测模型的信息调整定位信息，直到迭代次数达到最大值，或算法中的适应度值大于设定的阀值δ时，转到步骤105；

105、根据公式w_t+1＝w_t*exp(-0.5*(z_t-z_tpre)²)计算粒子的权值，并根据权重的大小按比例抽取粒子加入到新的粒子集中；

106、更新黑板信息，将最优的观测信息及路标的坐标信息记录在黑板中，并返回步骤103，直到每个机器人都到达终点，协作定位任务结束。

进一步的，步骤102中设置8个路标位置点。

本发明的优点及有益效果如下：

本发明提出了一种基于多Agent优化的PSOPF算法的多机器人协作定位控制方法。Agent具有自治性、交互性、反应性、推理性、规划能力、学习能力等特点。本发明将多Agent系统(MAS)与PSOPF算法进行结合，提出一种改进的粒子群优化粒子滤波算法(MAPSOPF)，核心是提高算法的收敛性能和执行效率，优化机器人对路标的观测来提高定位与建图的精度。

附图说明

图1为基于MAPSOPF算法的多机器人协作定位控制流程图；

具体实施方式

下面结合附图给出一个非限定性的实施例对本发明作进一步的阐述。

参照图1所示，本发明运用完全型网络结构与黑板机制相结合的方法，实现静态环境中多机器人的协作定位任务，具体实现步骤如下：

步骤1：将实际运行环境抽象成一张平面图，根据路标位置和机器人的路径建立环境地图。用ID号标记路标，以便机器人探测路标信息。

步骤2：根据步骤1得到的环境信息，初始化仿真环境、机器人的位置信息和黑板信息。在地图中任意设置8个路标位置点，同时设置每个机器人的路径。将粒子群初始化为3*N的矩阵，其中3表示需要优化的三个参数，即机器人的X坐标值、Y坐标值和角度值，N表示粒子个数。机器人通过自身的探测器获取全局地图信息，初始化黑板的信息。包括各机器人的身份及位置坐标，观测精度较高的路标位置，以及最大的适应度值。

步骤3：每个机器人根据里程计信息和罗盘信息预测下一时刻的位置，采用激光扫描仪对路标进行观测。机器人的运动模型和感知模型如下：

$\left\{\begin{array}{c}{x}_r\left(t\right)=x_r(t-1)+v\·\mathrm{\ΔT}\·\cos [\mathrm{\φ}(t-1)+\mathrm{\ω}(t-1)\·\mathrm{\ΔT}]+{\mathrm{\β}}_x\\ y_r\left(t\right)=y_r(t-1)+v\·\mathrm{\ΔT}\·\sin [\mathrm{\φ}(t-1)+\mathrm{\ω}(t-1)\·\mathrm{\ΔT}]+{\mathrm{\β}}_y\\ \mathrm{\φ}\left(t\right)=\mathrm{\φ}(t-1)+\mathrm{\ω}(t-1)\·\mathrm{\ΔT}+{\mathrm{\β}}_{\mathrm{\φ}}\end{array}\right\}---\left(1\right)$

其中，x_r(t)，y_r(t)，θ(t)是机器人在时刻t的位姿，β_x,β_y和β_φ是运动噪声，ΔT是内部传感器采样时间间隔，d_rl和分别是传感器观测到的信标与机器人之间的距离及与机器人前进方向的夹角。α_R和α_θ是观测噪声。是机器人航向。

步骤4：每个机器人采用基于MAPSOPF算法对步骤3所得预测位置以及观测信息进行更新并计算适应度值，如果该信息的适应度函数值小于黑板信息中的适应度值，则根据黑板中的信息对机器人的定位信息进行调整；否则就采用步骤3所得的观测信息调整定位信息。直到迭代次数达到最大值，或算法中的适应度值大于设定的阀值(δ)δ(本实验取值为0.5)时，转到步骤5。

为了增加算法的多样性，同时调整相关系数，对PSOPF算法引入竞争机制。每个粒子选取与之相邻的两个粒子作为邻域粒子，每个粒子计算出个体适应度后再与邻域粒子的个体适应度相比较，选取个体适应度更优的粒子代替原来的粒子，作为粒子群中新的粒子。开始给予算法一个较大的自学习因子，目的在于增加算法的局部搜索能力，随着迭代次数的增加，社会学习因子逐渐增大，目的在于增加算法向全局最优的收敛速度。

适应度函数是MAPSOPF算法的重要部分，可以用来评价粒子的优化程度。它的表达形式如下：

f_fitness＝exp{-sqrt[(z_t-z_tPre)R^-1(z_t-z_tPre)^T/c₃]} (3)

其中，R是观测噪声的协方差矩阵，z_tPre是在时刻t对路标的预测值，c₃根据经验选取，(此处取为3,z_t可根据和m共同计算得到：

$z_t=f(x_t^{*},m)---\left(4\right)$

其中m是路标的集合，是粒子的预测位姿。

采用MAPSOPF算法对粒子群体进行优化，当迭代次数达到最大时，或者算法的最优适应度值大于阀值(σ)时，完成更新阶段的寻优任务，并计算最优的适应度值，记录最优的观测路标。

步骤5：由公式w_t+1＝w_t*exp(-0.5*(z_t-z_tpre)²)计算每个粒子的权值并排序，选取80％权值较大的粒子，加入到新的粒子集中，其余20％的粒子通过复制权值较大的粒子得到。

步骤6：更新黑板信息，将最优的观测信息及路标的坐标信息记录在黑板中，并返回步骤3，直到每个机器人都到达终点，协作定位任务结束。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明方法权利要求所限定的范围。

Claims (2)

1.一种基于MAPSO优化粒子滤波的多移动机器人协作定位方法，所述机器人自身设置有用于获取仿真环境全局地图信息的探测器、用于计算运动路程信息的里程计、罗盘及激光扫描仪，其特征在于包括以下步骤：

101、将机器人的实际运行环境抽象成一张平面图，然后在所述平面图中加上路标的位置和机器人的路径形成一张环境地图，用ID号标记路标；

102、根据步骤101得到的环境地图，初始化仿真环境、机器人的位置信息和黑板信息，其中黑板信息包括各机器人的身份及位置坐标、观测路标位置以及最大适应度值σ；在仿真环境的地图中任意设置若干个路标位置点，同时设置每个机器人的路径，将每个机器人的位置作为一个粒子，若干个粒子构成粒子群，将粒子群初始化为3*N的矩阵，其中3表示待优化的三个参数，即机器人的X坐标值、Y坐标值和角度值，N表示粒子个数，机器人通过自身的探测器获取仿真环境的全局地图信息；

103、每个机器人根据自身携带的里程计信息和罗盘信息预测下一时刻的位置，采用自身携带的激光扫描仪对路标进行观测，得到机器人的运动模型(1)和观测模型如下：

$\left\{\begin{array}{c}{x}_r\left(t\right)=x_r(t-1)+v\·\mathrm{\ΔT}\·\cos [\mathrm{\φ}(t-1)+\mathrm{\ω}(t-1)\·\mathrm{\ΔT}]+{\mathrm{\β}}_x\\ y_r\left(t\right)=y_r(t-1)+v\·\mathrm{\ΔT}\·\sin [\mathrm{\φ}(t-1)+\mathrm{\ω}(t-1)\·\mathrm{\ΔT}]+{\mathrm{\β}}_y\\ \mathrm{\φ}\left(t\right)=\mathrm{\φ}(t-1)+\mathrm{\ω}(t-1)\·\mathrm{\ΔT}+{\mathrm{\β}}_{\mathrm{\φ}}\end{array}\right\}---\left(1\right)$

其中，x_r(t)，y_r(t)，θ(t)是机器人在时刻t的位姿，β_x,β_y和β_φ是运动噪声，ΔT是内部传感器采样时间间隔，v是机器人的运行速度，d_rl是传感器观测到的路标与机器人之间的距离，表示传感器观测到的路标与机器人前进方向的夹角，α_R和α_θ是观测噪声，是机器人航向角；

104、根据步骤103中得到的运动模型和观测模型，采用MAPSOPF算法对步骤103所得预测位置以及观测信息进行更新并计算粒子适应度值f_fitness，其中f_fitness＝exp{-sqrt[(z_t-z_tPre)R^-1(z_t-z_tPre)^T/c₃]}，其中，R是观测噪声的协方差矩阵，z_tPre是在时刻t机器人对路标的预测值，c₃为一定值，z_t表示路标观测值，z_t为z_t根据和m计算得到：其中m是路标位置坐标的集合，是粒子的预测位姿；

如果该信息的适应度函数值f_fitness小于步骤102中黑板信息中的最大适应度值σ，则根据黑板中的信息对机器人的定位信息进行调整；否则就采用步骤103中得到观测模型的信息调整定位信息，直到迭代次数达到最大值，或算法中的适应度值大于设定的阀值δ时，转到步骤105；

105、根据公式w_t+1＝w_t*exp(-0.5*(z_t-z_tpre)²)计算粒子的权值，并根据权重的大小按比例抽取粒子加入到新的粒子集中；

106、更新黑板信息，将最优的观测信息及路标的坐标信息记录在黑板中，并返回步骤103，直到每个机器人都到达终点，协作定位任务结束。

2.根据权利要求1所述的基于MAPSO优化粒子滤波的多移动机器人协作定位方法，其特征在于：步骤102中设置8个路标位置点。