[发明专利]一种基于人工蜂群的蒙特卡洛定位方法

摘要

一种基于人工蜂群的蒙特卡洛定位方法，在传统蒙特卡洛定位方法中加入模拟蜜蜂采蜜的行为以实现定位，首先在给定的空间初始化一定数量粒子的初始粒子样本集S1，其次建立机器人运动模型，并将初始粒子样本集S1中所有粒子根据运动模型形成一次新粒子样本集S2，再次建立观测模型，并将观测模型作为人工蜂群算法的适应度函数，而后将一次新粒子样本集S2作为人工蜂群的初始蜜源位置，模拟蜜蜂采蜜行为进行全局寻优，最后更新粒子权值，计算出机器人位姿；有效提高粒子的探索能力，以避免粒子在复杂或者结构相似的环境中，陷入局部最优而导致定位失败；同时使在保证粒子多样性的同时，快速收敛到机器人的真实位置。

主权项

1.一种基于人工蜂群的蒙特卡洛定位方法，其特征在于，在传统蒙特卡洛定位方法中加入模拟蜜蜂采蜜的行为以实现定位，具体步骤如下：采用N个带权值的离散粒子模拟被估状态的后验概率密度函数，Bel(x)＝{xi,ωi}i＝1…Nxi代表一个粒子，ωi代表该粒子的权重；1)在给定的空间初始化一定数量的粒子形成初始粒子样本集S1选取机器人在地图上的坐标值及与x轴方向夹角作为状态量(x，y，θ)，每个粒子为三维，按公式(1)随机生成N个粒子，形成初始粒子样本集S1； x i j = x min j + r a n d ( 0 , 1 ) ( x m a x j - x min j ) - - - ( 1 ) ]]>其中，是第j维上的上限和下限，rand(0,1)为[0，1]间的随机数，为第i个粒子第j维的值；2)建立机器人运动模型，步骤1)中生成的所有粒子根据运动模型形成一次新粒子样本集S2假设上一时刻移动机器人位姿为xt-1，控制命令为ut-1＝(Vt-1,αt-1)，高斯噪声为vt，则机器人的运动模型为： x t y t θ t = x t - 1 + Δ t · V t - 1 c o s ( θ t - 1 + α t - 1 Δ t ) + ϵ x x t - 1 + Δ t · V t - 1 sin ( θ t - 1 + α t - 1 Δ t ) + ϵ y θ t - 1 + Δ t · α t - 1 + ϵ θ - - - ( 2 ) ]]>其中，xt-1,yt-1,θt-1表示t-1时刻机器人的坐标值和方向角，V,α表示机器人的线速度和角速度，根据运动模型采样形成一次新粒子样本集S2；3)建立观测模型，并将观测模型作为人工蜂群算法的适应度函数机器人的观测模型p(y|x)表示机器人位姿x时激光传感器感知观测量y的概率，即环境信息与地图的匹配程度，室内环境中机器人靠激光传感器感知周围环境，激光传感器对周围环境进行扫描，得到N个距离数据，按如下公式(3)计算出最终概率值，并将其作为人工蜂群优化过程中的适应度函数； p ( y | x ) = Σ i = 1 N 1 2 π σ exp - ( y i - y i * ) 2 2 σ 2 - - - ( 3 ) ]]>公式(3)中，yi,分别为粒子在x处时激光第i条射线的实际测量值和期望测量值，σ为高斯误差；4)将步骤2)中的一次新粒子样本集S2作为人工蜂群的初始蜜源位置，模拟蜜蜂采蜜行为进行全局寻优①根据公式(3)计算一次新粒子样本集S2的适应度值，并选取较好的N/2个作为人工蜂群的引领蜂，剩下的N/2个作为跟随蜂，第i个引领蜂按如下公式(4)在蜜源附近领域搜索：x'ij＝xij+φ(xij-xkj) (4)公式(4)中，k∈{1,2,...N/2}，j是随机选择的下标，φ是[-1，1]上的均匀随机分布值，引领蜂按贪婪原则，比较记忆中最优位置和领域搜索解，当搜索解优于记忆中最优位置，替换记忆中最优位置，反之保持不变；②待引领蜂完成搜索过程后，将蜜源信息通过舞蹈与跟随蜂共享，在根据公式(3)计算每个引领蜂的适应度，并根据公式(5)计算引领蜂被选择的概率； p i = fit i Σ n = 1 N fit n - - - ( 5 ) ]]>其中，fiti为第i个引领蜂的适应度函数值i＝1,2…N/2；③跟随蜂根据概率pi选择蜜源位置，并根据公式(4)进行一次局部搜索，按贪婪原则从x'ij和xij中选择较好的搜索解作为下一代候选解；随着迭代次数的增加，(xij-xkj)之间距离缩小，搜索空间也缩小，经过若干次搜索后，样本集比较集中，形成二次新粒子样本集S3；5)更新粒子权值，计算机器人姿态根据传感器测量数据，计算二次新粒子样本集S3的每个粒子权值，保证与真实位置越接近的粒子权值越大； ω k t = ω k t - 1 * p ( y k t | x k t ) , k ∈ { 1 , 2... N } - - - ( 6 ) ]]>归一化权值在通过样本的加权平均，进而计算出机器人位姿。