利用粒子滤波跟踪汽车机器人

打开生活的脚本

粒子滤波器是一种基于采样的递归贝叶斯估计算法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]';= examplehelpercart (initialPose, dt);

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

设置粒子过滤器

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

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

主循环

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

使用固定速率支持，以20hz的频率运行20秒。金宝app

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);%如果测量可用，则调用correct，否则仅使用%的预测结果如果~isempty(measurement) [statecorreced, covCorrected] = correct(pf, measurement'); / /测量其他的stateCorrected = statePred;covCorrected = covPred;结束lastBestGuess = stateCorrected (1:3);%更新图如果~ isempty (get(大的,“CurrentFigure”)）%如果没有过早的死亡updatePlot(carbot, pf, lastBestGuess, simulationTime);其他的打破结束等待(r);%更新模拟时间simulationTime = simulationTime + dt;结束

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

结果图的详细信息

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

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

状态转移函数

基于采样的状态转移函数根据规定的运动模型演化粒子，使粒子形成提案分布的表示。下面是一个基于类独轮车机器人速度运动模型的状态转移函数的例子。关于这个运动模型的更多细节，请参阅第五章［1］．减少sd1，sd2和sd3查看跟踪性能如何下降。在这里sd1表示线速度的不确定性，sd2表示角速度的不确定性。sd3是对方向的额外扰动。

function 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);Wh = w + (sd2)^2*randn(l,1);γ= (sd3) ^ 2 * randn (l, 1);

wh(abs(wh)<1e-19) = 1e-19;

预测粒子(:，1)= prevParticles(:，1) - (vh./wh).*sin(theta) + (vh./wh)* sinθ+ wh * dT);predictParticles (:, 2) = prevParticles (:, 2) + (vh. / wh)。* cos(θ)- (vh. / wh)。* cosθ+ wh * dT);pretparticles (:，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 × 3向量。

function likelihood = exampleHelperCarBotMeasurementLikelihood(pf, predictParticles, measurement)

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

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

%测量的正态分布噪声%假设三个姿态分量的测量有相同的误差分布

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

结束

参考

[1] S. Thrun, W. Burgard, D. Fox，概率机器人，MIT出版社，2006