使用粒子过滤器跟踪一个汽车机器人

打开实时脚本

粒子滤波是一种基于采样的递归贝叶斯估计算法stateEstimatorPF对象。在本例中，在室外环境中跟踪遥控车形机器人。机器人姿势测量由车载GPS提供，该GPS存在噪音。有已知的运动命令发送给机器人，但由于机械松弛或模型不准确，机器人无法执行准确的指令运动。本例我将演示如何使用stateEstimatorPF为了减少测量数据中噪声的影响，得到更准确的机器人姿态估计。用非线性系统描述仿车机器人的运动学模型。粒子滤波器非常适合于估计这类系统的状态，因为它可以处理固有的非线性。

$\begin{array}{cl} x_{}^{˙} & ＝ v 因为（ θ ） \\ y_{}^{˙} & ＝ v 罪（ θ ） \\ θ_{}^{˙} & ＝ \frac{v}{l} 棕褐色 ϕ \\ ϕ_{}^{˙} & ＝ ω \end{array}$

场景：类似汽车的机器人持续驱动并改变其速度和转向角。机器人的姿势由一些嘈杂的外部系统测量，例如GPS或Vicon系统。沿着路径，它将穿过无法测量的屋顶区域。

输入：

机器人部分姿态噪声测量( $x$ ， $y$ ， $θ$ ）.请注意这不是一个完整的状态度量。没有前轮方向的测量( $ϕ$ )以及所有的速度( $x_{}^{˙}$ ， $y_{}^{˙}$ ， $θ_{}^{˙}$ ， $ϕ_{}^{˙}$ ）.
发送给机器人的线速度和角速度命令( $v_{c}$ ， $ω_{c}$ ）.请注意机器人的指令动作和实际动作之间会有一些不同。

目标:估计部分姿势( $x$ ， $y$ ， $θ$ )类似汽车的机器人。请注意再次强调，车轮的方向( $ϕ$ )并不包括在估计之内。从观察者的角度来看，汽车的完整状态只有[ $x$ ， $y$ ， $θ$ ， $x_{}^{˙}$ ， $y_{}^{˙}$ ， $θ_{}^{˙}$ ］．

接近：使用stateEstimatorPF处理两个噪声输入(两个输入本身都不可靠)，并对电流(部分)位姿做出最佳估计。

在预测阶段，我们用一个简化的、类似独轮车的机器人模型更新粒子的状态，如下所示。注意，用于状态估计的系统模型并不是实际系统的精确表示。这是可以接受的，只要模型差异在系统噪声(如粒子群)中被很好地捕获。有关详细信息，请参见预测．

$\begin{array}{cl} x_{}^{˙} & ＝ v 因为（ θ ） \\ y_{}^{˙} & ＝ v 罪（ θ ） \\ θ_{}^{˙} & ＝ ω \end{array}$

在正确的在此阶段，粒子的重要性权重（可能性）由当前测量的误差范数确定( $\sqrt{（ Δ x ）^{2} + （ Δ y ）^{2} + （ Δ θ ）^{2}}$ )，因为我们只对这三个组件进行了测量。有关详细信息，请参见正确的．

初始化一个类车机器人

rng(“默认”）;%表示可重复结果dt = 0.05;%时间步initialPose = [0 0 0 0]';/ / / / / / / /

图cbot包含一个轴对象。标题为t = 0的axis对象包含11个类型为矩形、散点、直线、文本的对象。这些物体代表实际的姿势，估计的姿势。

设置粒子过滤器

本节使用5000个粒子配置粒子过滤器。最初，所有粒子都是从正态分布中随机选取的，具有初始状态下的平均值和单位协方差。每个粒子包含6个状态变量( $x$ ， $y$ ， $θ$ ， $x_{}^{˙}$ ， $y_{}^{˙}$ ， $θ_{}^{˙}$ ）.注意，第三个变量被标记为Circular，因为它是汽车方向。指定两个回调函数也非常重要StateTransitionFcn和MeasurementLikelihoodFcn．这两个函数直接决定了粒子滤波器的性能。这两个函数的详细信息可以在本示例的最后两节中找到。

pf = stateEstimatorPF;初始化(pf, 5000， [initialPose(1:3)'， 0,0,0]， eye(6)，“循环变量”，[0 0 1 0 0]);pf.StateEstimationMethod =“的意思是”；pf.ResamplingMethod =“系统”；% StateTransitionFcn定义了粒子如何在没有测量的情况下进化pf.StateTransitionFcn = @exampleHelperCarBotStateTransition;% measurementlikelihood dfcn定义了测量如何影响我们的估计pf.MeasurementLikelihoodFcn = @exampleHelperCarBotMeasurementLikelihood;%x，y和θ的最后最佳估计lastBestGuess = [0 0 0];

主循环

注意，在这个例子中，命令机器人的线速度和角速度是任意选择的时间相关函数。另外，注意循环的固定速率定时是通过rateControl．

使用固定速率支持，以20 Hz的频率运行环路20秒。万博1manbetx

r=速率控制（1/dt）；

重置固定速率对象，重启定时器。在运行时间相关的代码之前重置计时器。

重置(r);simulationTime = 0;而simulationTime < 20%如果时间还没到%生成要发送给机器人的运动命令%注意在命令动作和命令动作之间会有一些差异%机器人实际执行的动作。uCmd(1) = 0.7*abs(sin(simulationTime)) + 0.1;%线速度uCmd（2）=0.08*cos（模拟时间）；%角速度驱动器（carbot、uCmd）；%基于运动模型的车体姿态预测[statpred, covPred] = predict(pf, dt, uCmd);获得GPS读数测量= exampleHelperCarBotGetGPSReading (carbot);%如果测量是可用的，那么调用正确，否则就使用%的预测结果如果~isempty(measurement) [stateCorrected, covCorrected] = correct(pf, measurement');其他的stateCorrected = statePred;covCorrected = covPred;结束lastBestGuess=状态修正（1:3）；%更新图如果~ isempty (get(大的,“CurrentFigure”））%如果数字没有被过早杀死updatePlot(carbot, pf, lastBestGuess, simulationTime);其他的打破结束等待(r);%更新模拟时间simulationTime = simulationTime + dt;结束

图cbot包含一个轴对象。标题为t = 19.95的axis对象包含11个类型为矩形、散点、行、文本的对象。这些物体代表实际的姿势，估计的姿势。

结果数字的细节

三幅图展示了粒子滤波器的跟踪性能。

在第一个图中，粒子滤波器在汽车离开初始姿态时跟踪良好。
在第二幅图中，机器人驶入有屋顶的区域，无法进行测量，粒子只是根据预测模型(用橙色标记)进化。你可以看到粒子逐渐形成一个马蹄形的前端，估计的姿态逐渐偏离实际的姿态。
在第三幅图中，机器人已经驶出了屋顶区域。通过新的测量，估计的姿势逐渐收敛到实际的姿势。

状态转移函数

基于采样的状态转换函数根据指定的运动模型演化粒子，从而使粒子形成建议分布的表示。下面是一个基于单轮机器人速度运动模型的状态转换函数示例。有关此运动模型的更多详细信息，请参阅翼点5英寸[1]. 减少sd1，sd2和sd3看看跟踪性能是如何恶化的。在这里sd1表示线速度的不确定性，sd2表示角速度的不确定性。sd3是对方向的额外扰动。

函数predictParticles = exampleHelperCarBotStateTransition(pf, prevParticles, dT, u)

θ= prevParticles (: 3);

w = u (2);v = u (1);

l =长度(prevParticles);

生成速度样本sd1 = 0.3;sd2 = 1.5;sd3 = 0.02;Vh = v + (sd1)^2*randn(l,1);w + (sd2)^2*randn(l,1);γ= (sd3) ^ 2 * randn (l, 1);

% Add a small number to prevent div/0 error wh(abs(wh)<1e-19) = 1e-19;

prevParticles(:，1) = prevParticles(:，1) - (vh./wh).*sin(theta) + (vh./wh)。* sinθ+ wh * dT);predictParticles (:, 2) = prevParticles (:, 2) + (vh. / wh)。* cos(θ)- (vh. / wh)。* cosθ+ wh * dT);prevParticles(:，3) = prevParticles(:，3) + wh*dT + gamma*dT;predictParticles (:, 4) = (- (vh. / wh)。*罪(θ)+ (vh. / wh)。* sin(θ+ wh * dT)) / dT;predictParticles (:, 5) = ((vh. / wh)。* cos(θ)- (vh. / wh)。* cosθ+ wh * dT)) / dT;predictParticles(:，6) = wh + gamma;

结束

测量似然函数

测量似然函数根据粒子与测量值之间的误差范数计算每个预测粒子的似然值。每个粒子的重要性权重将根据计算的似然值进行分配。在这个例子中，predictParticles是一个nx6矩阵(N是粒子的数量)，并且测量是一个1x3的向量。

函数似然性=示例帮助者碳测量似然性（pf、预测粒子、测量）

%测量包含所有的状态变量predictMeasurement = predictParticles;

注:在本例中，我们没有完整的状态观测，但只有当前位姿的测量，因此measurementErrorNorm %仅基于位姿误差。measurementError = bsxfun(@minus, predictMeasurement(:，1:3)， measurement);measurementErrorNorm =√(measurementError求和。^ 2, 2));

%测量的正态分布噪声%假设所有三个位姿分量的测量具有相同的误差分布测量噪声=眼(3);

%将误差范数转换为似然度量。%评估多元正态分布可能性的PDF=1/sqrt（（2*pi）。^3*det（measurementNoise））*exp（-0.5*measurementornorm）；

结束

参考

[1] S. Thrun, W. Burgard, D. Fox，概率机器人，麻省理工学院出版社，2006