基于粒子滤波的定位算法 ——原理、理解与仿真

基于粒子滤波的定位算法 ——原理、理解与仿真_第1张图片

1 算法原理

1.1 机器人定位问题

关于机器人定位,有三大问题,它们分别是:

(1)“全局定位”:指初始位置未知,机器人靠自身运动确定自己在地图中的位姿。

(2)“位姿跟踪”:指已知自身位姿或者已经通过“全局定位”得到了一个较好的位姿估计,在后续运动时补偿精度较差的运动控制误差;

(3)“绑架劫持”:指机器人在已知自身位姿的情况下,得到了一个错误的位姿信息或者外界将其放到另外一个位姿,而里程计信息给出了错误的信息甚至没有给出控制信息。

1.2 粒子滤波步骤(可结合2中例题)

(1)初始状态:用大量粒子模拟运动状态,使粒子在空间内均匀分布;

(2)预测阶段:根据状态转移方程,将每一个粒子带入,得到一个预测粒子;

(3)校正阶段:对预测粒子进行评价(计算权重),越接近于真实状态的粒子,其权重越大;

(4)重采样:根据粒子权重对粒子进行筛选,筛选过程中,既要大量保留权重大的粒子,又要有一小部分权重小的粒子;

(5)滤波:将重采样后的粒子带入状态转移方程得到新的预测粒子,即步骤(2)。

2 一个二维定位的例子 (基于粒子滤波的定位算法)

2.1 问题

在二维空间,假设运动小车的初始位置、状态方程(运动预测方程)、传感器测量数据,用粒子滤波方法进行对其进行定位。

2.2 预设参数

(1)粒子总数N=200;

(2)运动时间T=10秒,且假设每秒进行一步动作;

(3)运动场地大小WorldSizeWorldSize=100100平方米;

(4)控制小车运动的方程,小车沿着某曲线运动(但实际的运动情况肯定和给的控制有些差别,这理解为叠加在理想运动方程上的控制误差,控制误差假设为5米);

(5)传感器测量的小车位置数据同样也和实际情况不一致,这也可以理解为叠加在真实位置上的测量误差,测量误差假设为5米;

(6)小车初始位置已知,假设处在100*100场地中的(50,20)位置;

2.3 步骤与理解分析

(1)初始化粒子群

在整个场地内,将总粒子数N进行均匀分布,得到如图所示结果。
基于粒子滤波的定位算法 ——原理、理解与仿真_第2张图片

(2)小车开始运动(同时每运动一步进行一次测量)

小车按照控制给定的叠加了控制噪声的运动方程进行运动,运动达到下一位置后,传感器对当前位置进行测量,并得到一个测量的位置(不准确,含有测量噪声)

(3)粒子群更新(预测步骤)

把粒子群中的全部粒子逐个带入小车的运动方程中,得到粒子群的下一步位置。同时,计算此时每个粒子的位置和测量得到的小车位置 这两个位置之间的几何距离,按照距离的不同给每个粒子添加一个权重,用于重采样。显然,距离越近,权重越大(权重和距离关系的函数,这里采用高斯分布钟形曲线的右侧,即随距离增大,权重单调递减)。下一步,得到全部粒子的权重后,将它们进行归一化。

(4)重采样

对于全部M个粒子,它们的归一化权重集合为 \omega ,第 i 个粒子的权重为 \omega[i] 。则重采样过程可理解为旋转轮盘抽奖。如下图。
基于粒子滤波的定位算法 ——原理、理解与仿真_第3张图片

由于不同粒子的归一化权重不同,它们占轮盘的面积也不同,因此权重大的粒子更容易被抽中。

现在,我们需要重采样得到新的M个粒子组成的粒子群。因此,我们按照上述轮盘,抽M次,抽到某个权重 \omega[i] ,则把该权重对应的粒子放入新的M个粒子组成的粒子群中。这样,那些权重大的粒子可能被反复抽到,从而重复出现在新的粒子群中,而那些权重小的粒子可能会在新的粒子群中被丢弃。

某一次运动过程结束后,可画出小车实际位置、重采样后粒子群位置、以及粒子群的几何中心位置,如图所示。
基于粒子滤波的定位算法 ——原理、理解与仿真_第4张图片

(5)重复步骤(2)~(4),直到结束。

最后,我们可以得到整个运动过程中,小车实际路径、测量得到的路径、粒子群中心位置构成的路径,如图所示。

基于粒子滤波的定位算法 ——原理、理解与仿真_第5张图片
另外,还可以得到测量位置与真实位置间的误差,以及粒子群中心位置与真实位置间的误差,如图所示。
基于粒子滤波的定位算法 ——原理、理解与仿真_第6张图片

简单分析可知,综合控制和测量,利用粒子滤波进行定位,较单纯相信测量而言,更加精确。

2.4 本例的MATLAB源代码(附有详细注释)
%粒子滤波(定位运动轨迹)
%在二维空间,假设运动物体的一组(非线性)运动位置、速度、加速度数据,用粒子滤波方法进行处理
clc,clear,close all
%% 参数设置
N = 200; %粒子总数
Q = 5; %过程噪声(控制误差)  状态转移方程中使用
R = 5; %测量噪声  由真实位置叠加测量噪声得到测量位置
T = 10; %测量时间(总步数)
theta = pi/T; %旋转角度
distance = 80/T; %每次走的距离(步长)
WorldSize = 100; %世界大小
X = zeros(2, T); %存储系统状态(每列存储二维位置坐标(x,y),共T个位置)
Z = zeros(2, T); %存储系统的观测状态(每列存储二维位置坐标(x,y),共T次测量)
P = zeros(2, N); %建立粒子群(每列存储当前粒子的二维位置坐标,共N个粒子)
PCenter = zeros(2, T); %所有粒子的中心位置
w = zeros(N, 1); %每个粒子的权重
err = zeros(2,T); %误差(第一行为粒子与真实路径误差  第二行为测量与真实路径误差)
X(:, 1) = [50; 20]; %初始系统状态 即初始位置在坐标(50,20)
Z(:, 1) = X(:,1) + wgn(2,1,10*log10(R)); %初始系统的观测状态(为真实位姿叠加高斯噪声)
                                             %y = wgn(m,n,p) 产生一个m行n列的高斯白噪声的矩阵,p以dBW为单位指定输出噪声的强度
%% 初始化粒子群
for i = 1 : N
    P(:, i) = [WorldSize*rand; WorldSize*rand];%随机产生第i个粒子的坐标(rand为产生[0,1]之间均匀分布)
    dist = norm(P(:, i)-Z(:, 1)); %与测量位置相差的距离
    %求权重 (权重与距离的关系 为 均值是0,方差是sqrt(R)的高斯分布曲线)  因为均值为0且距离大于0 因此权重随着距离增加沿高斯曲线右侧递减
    w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R); 
end
PCenter(:, 1) = sum(P, 2) / N;%t=1时刻(初始时刻)所有粒子的几何中心位置
% 初始状态(t=1)画图
err(1,1) = norm(X(:, 1) - PCenter(:, 1));%粒子群几何中心与系统真实状态的误差
err(2,1) = wgn(1, 1, 10*log10(R));
figure(1);
hold on
set(0,'defaultfigurecolor','w')
plot(X(1, 1), X(2, 1), 'r.', 'markersize',30) %真实的初始状态位置(红点表示)
%grid on
axis([0 100 0 100]);
set(gca,'XTick',0:10:100) %改变x轴坐标间隔显示 这里间隔为10
set(gca,'YTick',0:10:100) %改变y轴坐标间隔显示 这里间隔为10
plot(P(1, :), P(2, :), 'k.', 'markersize',5); %各个粒子位置(N个黑点)
plot(PCenter(1, 1), PCenter(2, 1), 'b.', 'markersize',25); %所有粒子的中心位置(蓝点表示)
legend('真实位置', '粒子群', '粒子群的几何中心');
title('初始状态');
hold off
%% 开始运动
for k = 2 : T %从t=2到T
    %模拟一个弧线运动的状态
    X(:, k) = X(:, k-1) + distance * [(-cos(k * theta)); sin(k * theta)] + wgn(2, 1, 10*log10(Q)); %状态方程
    Z(:, k) = X(:, k) + wgn(2, 1, 10*log10(R)); %观测方程(状态上叠加测量的高斯噪声) 
    %粒子滤波
    %预测
    for i = 1 : N
        P(:, i) = P(:, i) + distance * [-cos(k * theta); sin(k * theta)] + wgn(2, 1, 10*log10(Q));%粒子群带入状态方程
        dist = norm(P(:, i)-Z(:, k)); %粒子群中各粒子 与 测量位置 的距离
        w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R); %求权重(距离近权重大)
    end
    %归一化权重
    wsum = sum(w);
    for i = 1 : N
        w(i) = w(i) / wsum;
    end
    %重采样(更新)
    for i = 1 : N
        wmax = 2 * max(w) * rand; %另一种重采样规则
        index = randi(N, 1);%生成一个在[1(默认值),N]之间均匀分布的伪随机整数
        while(wmax > w(index))
            wmax = wmax - w(index);
            index = index + 1;
            if index > N
                index = 1;
            end 
        end
        Pnext(:, i) = P(:, index); %得到新粒子放入临时集Pnext
    end
    P=Pnext;%用临时集Pnext更新粒子集P
    PCenter(:, k) = sum(P, 2) / N; %重采样后所有粒子的中心位置
    %计算误差
    err(1,k) = norm(X(:, k) - PCenter(:, k)); %粒子几何中心与系统真实状态的误差
    err(2,k) = norm(X(:, k) - Z(:, k));
    %画图
    figure(2);
    set(0,'defaultfigurecolor','w')
    clf;%清空figure(2)中的图像 以便循环重新画
    hold on
    plot(X(1, k), X(2, k), 'r.', 'markersize',30); %系统状态位置
    plot(P(1, :), P(2, :), 'k.', 'markersize',5); %各个粒子位置
    plot(PCenter(1, k), PCenter(2, k), 'b.', 'markersize',25); %所有粒子的中心位置
    axis([0 100 0 100]);
    title('运动过程');
    legend('真实状态', '粒子群', '粒子群的几何中心');
    hold off
    pause(0.1);%停0.1s开始下次迭代
end
%% 绘制轨迹
figure(3);
set(0,'defaultfigurecolor','w')
plot(X(1,:), X(2,:), 'r.-', Z(1,:), Z(2,:), 'g.-', PCenter(1,:), PCenter(2,:), 'b.-');
axis([0 100 0 100]);
set(gca,'XTick',0:10:100) %改变x轴坐标间隔显示 这里间隔为10
set(gca,'YTick',0:10:100) %改变y轴坐标间隔显示 这里间隔为10
legend('真实轨迹', '测量轨迹', '粒子群几何中心轨迹');
xlabel('横坐标 x'); ylabel('纵坐标 y');
%% 绘制误差
figure(4);
set(0,'defaultfigurecolor','w')
%set(gca,'FontSize',12);%设置图标字体大小
plot(err(1,:),'b.-');%err1为各时刻 真实位置与粒子群中心的几何距离
hold on
plot(err(2,:),'r.-');%err2为各时刻 真实位置与测量位置的几何距离
hold off
xlabel('步数 t');
legend('粒子群误差', '测量误差');
title('真实位置与粒子群中心的集合距离');

3 更多

3.1 失效恢复问题

蒙特卡罗定位以目前的形式能够解决全局定位问题,但是不能从机器人绑架中或全局定位失效中恢复。定位过程中,获取位置的同时,不在最可能位置处的粒子会逐渐消失,在某个时候,只有单一位置的粒子能够“幸存”,如果这个位姿碰巧是错误的,算法不能恢复。实际上,任何随机算法(如蒙特卡罗定位算法),在重采样步骤中可能意外地丢弃所有正确位置附近的粒子,特别是当粒子数较少,且粒子扩散到较大空间时,这个问题就显得很重要了。

  • 解决办法:
    通过简单的探索算法可以解决这个问题,探索算法的思想是:增加随机粒子到粒子集合。通过假设机器人可能以小概率遭到绑架,注入一些随机粒子,从而在运动模型上产生一些随机状态,即使机器人不被绑架,随机粒子也能提升额外的鲁棒性级别。

  • 这引起两个问题:
    (1)在每次算法迭代中,应该增加多少粒子;(2)从哪种分布产生这些粒子。

  • 解答:
    (1)一种简单的方法是每次迭代增加固定数目的随机粒子;另一种更好的想法是基于某些定位性能的评估增加粒子。其中,实现第二种想法的一个方法是监控传感器测量的概率分布,在粒子滤波里,重要性权重是这个概率分布的随机估计,其均值(即增加的粒子数目)为:
    在这里插入图片描述

(2)一种简单的方法是根据均匀分布在位置空间产生粒子,并用当前观测值加权这些粒子;另一种更好的想法是根据测量分布直接产生粒子,根据观测似然分布,附加的粒子能够直接放置在相应的位置上。

你可能感兴趣的:(SLAM)