使用粒子滤波器跟踪一个车一样的机器人

打开生活的脚本

粒子滤波是一种基于采样的递归贝叶斯估计算法stateEstimatorPF对象。在这个例子中，一个遥控的类似汽车的机器人在户外环境中被跟踪。机器人的位姿测量是由车载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]';carbot = ExampleHelperCarBot(initialPose, dt);

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

设置粒子过滤器

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

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

主循环

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

使用固定速率支持以20hz运行循环20秒。

r = rateControl (1 / dt);

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

重置(r);simulationTime = 0;而simulationTime < 20%如果时间没有到生成要发送给机器人的运动命令。注意在命令的动作和%机器人实际执行的运动。uCmd(1) = 0.7*abs(sin(simulationTime)) + 0.1;%线速度uCmd (2) = 0.08 * cos (simulationTime);%角速度驱动器(carbot, uCmd);根据运动模型预测碳位[statePred, covPred] = predict(pf, dt, uCmd);%获取GPS读数测量= exampleHelperCarBotGetGPSReading (carbot);%如果测量可用，则称正确，否则仅使用%的预测结果如果~isempty(measurement) [statecorrecated, covCorrected] = correct(pf, measurement');其他的stateCorrected = statePred;covCorrected = covPred;结束lastBestGuess = stateCorrected (1:3);%更新图如果~ isempty (get(大的,“CurrentFigure”）)%，如果数字没有被过早杀死updatePlot(carbot, pf, lastBestGuess, simulationTime);其他的打破结束等待(r);%更新模拟时间模拟时间=模拟时间+ dt;结束

图CarBot包含一个axes对象。标题为t = 19.95的axes对象包含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)²*randn(l,1);Wh = w + (sd2)²*randn(l,1);γ= (sd3) ^ 2 * randn (l, 1);

%增加一个小的数字，防止div/0错误wh(abs(wh)<1e-19) = 1e-19;

将速度样本转换为pose样本predictParticles(:，1) = prevParticles(:，1) - (vh./wh).*sin(thetas) + (vh./wh)。* sinθ+ wh * dT);predictParticles (:, 2) = prevParticles (:, 2) + (vh. / wh)。* cos(θ)- (vh. / wh)。* cosθ+ wh * dT);predictParticles(:，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为N × 6矩阵(N为粒子数)，且测量是一个1 x 3的向量。

函数似然= exampleHelperCarBotMeasurementLikelihood(pf, predictParticles, measurement)

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

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

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

将误差规范转换为似然度量。评估多元正态分布似然的PDF = 1/√((2*pi)。^3 * det(measurementNoise)) * exp(-0.5 * measurementErrorNorm);

结束

参考

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