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

权利要求书

1.一种基于人工蜂群的蒙特卡洛定位方法，其特征在于，在传统蒙特卡洛定位方法中加入模拟蜜蜂采蜜的行为以实现定位，具体步骤如下：

采用N个带权值的离散粒子模拟被估状态的后验概率密度函数，Bel(x)＝{xi,ωi}i＝1…N

xi代表一个粒子，ω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 ) ]]>

归一化权值

在通过样本的加权平均，进而计算出机器人位姿。

2.根据权利要求1所述的一种基于人工蜂群的蒙特卡洛定位方法，其特征在于，步骤5)中，在搜索过程中若一个蜜源经过连续有限次循环没有改进，而当前位置也不是全局最优，表明该位置陷入局部最优，则放弃该蜜源位置，通过侦查蜂按公式(1)随机搜索的新蜜源位置代替当前蜜源位置。