使用粒子滤波跟踪轮式机器人

打开生活的脚本

粒子滤波是一种sampling-based递推贝叶斯估计的算法,实现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处理两个吵闹的输入(输入都不是可靠的本身)和充分评估当前的(部分)构成。

在预测阶段,我们更新的一个简化的粒子,unicycle-like机器人模型,如下所示。注意,系统模型用于状态估计不是一个实际系统的精确表示。这是可以接受的,只要系统中模型的区别是书评噪声(如由粒子群)。更多细节,请参阅预测。

$\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包含一个坐标轴对象。坐标轴对象与标题t = 0时,包含x (m), ylabel y (m)包含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 0));pf.StateEstimationMethod =“的意思是”;pf.ResamplingMethod =“系统”;% StateTransitionFcn定义粒子如何发展,而不需要测量pf.StateTransitionFcn = @exampleHelperCarBotStateTransition;% MeasurementLikelihoodFcn定义测量如何影响我们的估计pf.MeasurementLikelihoodFcn = @exampleHelperCarBotMeasurementLikelihood;去年最佳估计% x, y和θlastBestGuess = (0 0 0);

主循环

注意在这个例子中,机器人的吩咐线速度和角速度arbitrarily-picked时间函数。同时,注意循环实现的固定时间rateControl。

运行循环使用固定利率支持20 Hz 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);%预测carbot提出基于运动模型[statePred, covPred] =预测(pf, dt, uCmd);%得到GPS阅读测量= exampleHelperCarBotGetGPSReading (carbot);%如果测量可用,然后调用正确的,否则就用%的预测结果如果~ isempty(测量)[stateCorrected covCorrected] =正确(pf,测量);其他的stateCorrected = statePred;covCorrected = covPred;结束lastBestGuess = stateCorrected (1:3);%更新图如果~ isempty (get(大的,“CurrentFigure”))%如果图不是过早死亡updatePlot (pf, carbot lastBestGuess simulationTime);其他的打破结束等待(r);%更新仿真时间simulationTime = simulationTime + dt;结束

图CarBot包含一个坐标轴对象。坐标轴对象标题t = 19.95,包含x (m), ylabel y (m)包含11个类型的对象的矩形,散射线,文本。一个或多个行显示的值只使用这些对象标记代表实际的姿势,估计姿势。

结果数据的细节

这三个数据显示粒子滤波器的跟踪性能。

在第一个图中,粒子滤波跟踪汽车以及它开车离开初始姿势。
在第二个图,机器人驱动器到屋顶面积,没有测量,基于预测模型和粒子进化(标有橙色)。你可以看到粒子逐步形成horseshoe-like战线,和估计逐渐偏离实际。
在第三个图,机器人已经赶出了屋顶。新的测量,估计姿势逐渐回归实际的姿势。

状态转移函数

sampling-based状态转换函数的发展基于规定的运动模型的粒子,粒子将形成一个代表提案的分布。下面是一个状态转换函数的例子基于速度unicycle-like机器人的运动模型。关于这个运动模型的更多细节,请参见第五章[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);wh = w + (sd2) ^ 2 * randn (l, 1);γ= (sd3) ^ 2 * randn (l, 1);

%添加少量防止div / 0错误wh (abs (wh) < 1 e-19) = 1 e-19;

%转换速度样本构成样本predictParticles (: 1) = prevParticles (: 1)——(vh. / wh)。*罪(θ)+ (vh. / wh)。* sinθ+ wh * dT);predictParticles (:, 2) = prevParticles (:, 2) + (vh. / wh)。* cos(θ)- (vh. / wh)。* cosθ+ wh * dT);predictParticles (:, 3) = prevParticles (:, 3) + wh * dT +γ* 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 +γ;

结束

测量似然函数

测量似然函数计算每个粒子根据预测的可能性之间的误差标准粒子和测量。每个粒子的重要性权重分配方案的基础上计算的可能性。在这个特定的例子中,predictParticles是一个N x 6矩阵(N是粒子数),然后呢测量是一个1 x 3向量。

函数= exampleHelperCarBotMeasurementLikelihood可能性(pf、predictParticles测量)

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

%计算观测误差预测和实测%注意在这个例子中,我们没有完整的状态观察,但只有%当前姿态的测量,因此measurementErrorNorm %只是基于姿势错误。measurementError = bsxfun (@minus predictMeasurement(:, 1:3),测量);measurementErrorNorm =√(measurementError求和。^ 2,2));

% Normal-distributed噪音测量%假设在所有三个构成部分有相同的测量误差分布measurementNoise =眼(3);

%误差准则转化为测量的可能性。%的PDF评估多元正态分布概率= 1 /√(2 *π)。^ 3 *侦破(measurementNoise)) * exp (-0.5 * measurementErrorNorm);

结束

参考

[1]杜伦,w . Burgard d·福克斯,概率机器人,麻省理工学院出版社,2006年