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

摘要

本发明公开了一种基于MAPSOPF算法的多机器人协作定位控制方法，包括步骤：将实际运行环境抽象成一张平面图，根据路标的位置和机器人的路径建立环境地图；初始化机器人相关信息、黑板信息；采用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，直到每个机器人都到达终点，协作定位任务结束。