基于粒子滤波的自动装配定位方法

高 巍，邵晓东，刘焕玲

（西安电子科技大学 电子装备结构设计教育部重点实验室，陕西 西安 710071）

0 引言

借助虚拟装配技术，用户可以模拟零件的实际装配过程，分析产品的装配性能。但是，由于空间方向的模糊性和人对零件运动控制的不确定性，操作者很难对虚拟零件进行精确定位［1］。因此，如何高效、准确地对零件进行定位并对装配过程进行仿真是虚拟装配的一个技术难题。目前学者对该问题已经进行了大量研究，并提出了一些可行的解决方案。

美国华盛顿州立大学Jayaram 等开发的虚拟装配环境VADE［1］，通过捕获计算机辅助设计（Computer Aided Design，CAD）系统中的几何约束信息和最终装配位置，实现对零件的精确引导和定位；英国Heriot-Watt大学Richard G.Dewar［2］等提出利用近似捕捉和碰撞捕捉等方法，自动将与基体件发生碰撞干涉后的装配件直接调整到目标装配位置；浙江大学CAD&CG 国家重点实验室开发的虚拟装配原型系统VIRDAS［3-4］，通过语义识别捕捉交互意图，按照预先定义的装配语义引导零件装配运动；文献［5-6］提出利用过程框架、特征框架与场景匹配引擎构成的框架系统捕获操作者的装配意图，由匹配成功的过程框架引导装配运动，完成装配件的精确定位；文献［7-12］通过动态识别零件间的几何约束关系，限制零件运动的自由度，沿受约束方向引导零件运动到预定装配位置。

然而，上述方法都是根据零件间预先定义好的受约束运动规则，将满足约束条件的装配件直接定位到目标位置和姿态，忽略了过程信息。在实际装配中，零件间的约束建立和装配定位是通过零件间的相互碰撞和用户驱动完成的。此外，上述方法只适用于人机交互装配，耗时多，效率低。

本文提出一种基于粒子滤波的装配定位方法，利用一种概率方法（Monte Carlo方法）模拟装配过程中零件的位姿分布，根据碰撞响应引导装配运动，仿真实际装配过程。该方法的基本思想是利用随机样本描述零件位姿的概率分布，在每一个仿真时刻，利用重要性函数对装配件进行位姿采样，通过调节各采样粒子权值的大小对发生干涉的零件进行位姿重采样，模拟实际装配中零件位姿的概率分布，以样本的加权计算结果估计零件下一时刻的位姿，直到零件与目标位姿满足装配要求。与现有方法相比，该方法利用随机样本描述零件位姿的概率分布，反映了装配过程中人对零件操控的模糊性和零件定位的不确定性；无需进行特征匹配和约束识别，使装配引导更具普遍性，解决了对于复杂的装配体来说，自动约束识别出来的约束与用户意图不一定相符的问题［13］；在虚拟环境中，装配过程由计算机自动执行，省去了大量重复的人机交互工作，提高了装配效率和仿真速度。

1 粒子滤波算法描述

1.1 Monte Carlo方法

粒子滤波的思想基于 Monte Carlo 方法。Monte Carlo方法的核心思想是利用一些服从后验概率分布的随机采样粒子之和来近似估计连续概率密度函数的值，即

经过随机采样后，可通过式（3）计算经验估计，

1.2 重要性采样方法

由式（1）可知，有限个离散粒子之和可以用来近似表示后验概率分布，但有时直接按照后验概率分布来抽样会很困难。为了解决这一问题，可利用重要性采样方法，选择一个已知的、容易抽样并且覆盖真实分布p（x）的建议分布q（x）来抽样，通过对服从建议分布的采样粒子进行加权来近似目标概率分布。重要性采样方法实际上是对函数f（xk）的数学期望值表达式进行了变形，即

式中q（x0：k|z1：k）为重要性函数。令wk（x0：k）=，称为重要性采样的权函数。可以采用按照概率分布q（x0：k|z1：k）抽样得到的粒子近似表示E（f（x0：k）），即

重要性采样方法存在的一个严重问题是退化现象（degeneracy problem）［14］，即随着时间的推移，粒子权重的方差会增大，导致样本集无法实现所要表达的后验概率密度，严重降低了算法精确度。重采样方法［15-16］可以有效解决这一问题，其思想是去除权值小的样本，复制权值大的样本。常用的重采样方法有随机采样、残差重采样和系统重采样等［17］。然而，重采样方法会导致在采样结果中存在大量重复粒子，降低粒子多样性，甚至导致粒子枯竭。增加采样粒子数可以改善粒子枯竭的问题，但会影响计算的实时性，降低算法效率。为了解决这个问题，需要对每个粒子引入马尔科夫链蒙特卡洛（Markov Chain Monte Carlo，MCMC）方法。

1.3 MCMC方法

MCMC方法［17］可以有效解决粒子枯竭问题，通过引入一个MCMC 步骤来增加粒子的多样性。其基本思想是：如果粒子服从后验概率，则在k时刻，实施核为的Markov链变换后，在满足等式恒成立的前提下，仍然可以得到一组满足后验概率分布p（x0：k|z1：k）的粒子群，且新的粒子群更加逼近真实的目标分布。Markov变换不会影响粒子群的概率分布，利用变换后的粒子群近似后验概率密度函数，近似误差不会大于变 换 前［18］。MCMC重采样主要有Gibbs抽样和MH（Metropolis Hastings）抽样两种实现方法［19-20］，本文采用MH 算法，其步骤如下：

（1）按照均匀分布抽样u，即u～U［0，1］。

2 装配引导方法

虚拟环境中，零件的空间姿态用矩阵A=（AX，AY，AZ）表示，其中AX，AY和AZ分别为零件局部坐标系X，Y，Z 轴在世界坐标系中的方向矢量。零件的空间位置用矩阵T=（x，y，z）表示，（x，y，z）为零件局部坐标系原点在世界坐标系中的坐标值。

本文方法流程如图1所示。首先，在CAD 系统中根据设计者的装配意图对零件进行预装配，得到装配体中零件间的相对位姿关系。在虚拟环境中，根据基体件的位姿和零件间的相对位姿关系计算装配件的目标位姿PE（AE，TE）。然后，随机选取或用户交互输入装配件的初始位姿，开始装配仿真。在任意时刻t，根据装配件在t-1 时刻位置Tt-1=（xt-1，yt-1，zt-1）和目标位置TE=（xE，yE，zE），分别沿X，Y 和Z 轴进行坐标采样，计算t时刻的估计位置。若在＾Tt装配件不与基体件发生碰撞，则将作为其t 时刻的位置Tt；否则返回Tt-1进行姿态变换，当姿态满足装配要求时进行MH 抽样，再次计算。重复以上操作，直到零件与目标位姿满足装配要求。

2.1 位置变换

本文利用Monte Carlo方法［21］对零件坐标进行采样并赋予重要性权值，根据采样粒子的加权和对零件位置进行估计。在t-1时刻，沿世界坐标系X，Y 和Z 轴分别选取n 个采样点，记做分别服从正态 分 布Nx，t-1（xE，σx，t-12），Ny，t-1（yE，σy，t-12），Nz，t-1（zE，σz，t-12）。为了确保装配件逐渐接近目标位置，且对于任意随机变量x～N（μ，σ2），有

位置变换分为两步：①坐标初始化，按照初始状态概率分布函数P（x0）沿X，Y 和Z 轴分别随机抽取n 个粒子，得到，加权平均求得装配件初始位姿；②对于t=1，2，…，循环操作以下三个步骤。

（1）位置抽样 根据装配件t-1时刻位置Tt-1和目标位置TE，计算σx，t-1，σy，t-1，σz，t-1，得到正态分布概率密度函数Nx，t-1，Ny，t-1，Nz，t-1，按照重要性函数q（Xt|X0：t-1，z1：t）=p（Xt|Xt-1）抽样［22］，得到t时刻位置估计后的粒子集合n｝。

式中ξi1，ξi2，ξi3，ξi4，ξi5和ξi6均为区间［0，1］上服从均匀分布的随机数。

2.2 姿态变换

假设t-1时刻装配件姿态矩阵为At-1，姿态变换矩阵为Rt-1，t时刻的姿态矩阵为At，有

式中：Rt-1=R（Z，φt-1）R（Y，θt-1）R（X，αt-1）；R（X，αt-1），R（Y，θt-1）和R（Z，φt-1）分别表示绕世界坐标系X，Y，Z 轴转动αt-1，θt-1，φt-1角的旋转矩阵。因此，确定了转角αt-1，θt-1和φt-1即可求出装配件从t-1到t时刻的姿态变换矩阵。下面给出αt-1的求解过程，θt-1，φt-1的求解方法与αt-1类似。

（1）将矩阵At-1=（AXt-1，AYt-1，AZt-1），AE=（AXE，AYE，AZE）各方向矢量在世界坐标系YOZ平面上进行投影，得到矩阵A′t-1=（A′Xt-1，A′Yt-1，A′Zt-1）和A′E=（A′XE，A′YE，A′ZE），如图2所示。

（2）计算矩阵A′t-1和A′E各方向矢量间的夹角αX，αY和αZ（如图2）。若零件转动方向满足右手定则，则夹角为正。

式中Xe，Ye，Ze分别为沿世界坐标系X，Y，Z 轴正方向的单位向量。

（3）计算转角αt-1的极限值，令

（4）生成随机数Rnd，计算转角αt-1=2·Rnd·α0。若α0为正，则αt-1在区间［0，2α0］上服从均匀分布；若α0为负，则αt-1在区间［2α0，0］上服从均匀分布。

同理得到转角θt-1，φt-1，计算Rt-1并代入式（6），得到At。

3 实例

本文提出的基于粒子滤波的装配定位方法在自主开发的虚拟装配系统中得到了应用，系统界面如图3所示。下面以电机（基体件）与端盖（装配件）的装配为例，对上述提出的算法进行验证，采样粒子数为20。通过预装配得到端盖的目标位姿。随机选取或操作者指定端盖的初始位姿，进行位姿变换。具体过程如下：①对端盖进行位置变换。实时获取端盖位置信息，根据重要性函数进行坐标采样，利用采样粒子的加权和估计端盖下一时刻的位置坐标。如果在估计位置处未与电机机体发生碰撞，则将估计位置作为端盖的当前位置。重复以上操作，直到与电机机体发生碰撞，如图4a所示。②对端盖进行姿态变换。实时计算端盖绕世界坐标系各坐标轴的转角极限值，并在转动区间内按照均匀分布抽样，得到姿态变换矩阵，进行姿态变换。当端盖与目标姿态满足装配要求时，姿态变换完成（如图4b）。③再次进行位置变换，端盖放置到位（如图4c）。

若装配件位置（重）采样后不与基体件发生碰撞，则将该次位置变换称为有效位置变换。图4a和图4c给出了端盖装配过程中有效位置变换的路径。由图4可以看出，与基于约束的装配引导方法［7-12］和基于语义识别的装配引导方法［3-4，13］相比，本文方法利用随机概率事件模拟零件运动的不确定性，直观反映了真实装配过程；通过碰撞响应进行装配定位，不需要预定义装配约束并进行约束识别，使该方法适用于复杂装配体的装配仿真；装配过程由程序自动执行，大大提高了装配效率。

将装配过程划分为多个时间段，统计每个时间段内零件运动的有效位姿变换次数和总体位姿变换次数，绘制曲线如图5所示。结合实际装配经验，由图5可以看出：位姿变换的有效次数占总体次数的70%～90%，说明装配的部分时间用于寻找可行装配路径，排除发生干涉位置；1.5s～2s内进行姿态变换，且姿态变换速度远大于位置变换速度；在装配末段，位姿变换的有效次数占总体次数比重大大提高，说明在目标位置附近，装配效率提高且定位更加准确。

4 讨论

4.1 方法性能分析

为了对方法性能进行分析，定义有效位姿变换率

式中：Na为有效位姿变换次数，Np为位姿变换总次数。Pa高，说明装配性能好，装配操作的有效率高，零件设计合理，容易装配。将Pa，Np和平均每步位姿变换时间（t0）作为方法性能分析的指标。由方法基本思想可知，影响方法性能的主要因素有采样粒子数（Ne）和零件外形的复杂程度，下面详细讨论这些因素对方法性能的影响。

4.1.1 采样粒子数对方法性能的影响

以端盖装配（如图3）为例，在Xeon CPU 3.60 GHz的PC机上，分别选择10，30，50，100，150个采样粒子，各进行20次装配实验，每次装配件均从同一初始位置开始运动，记录Na，Np和t0，计算Pa，取平均值并绘制曲线，如图6～图8所示。

由图6可以看出：①Ne＜60 时，Pa较高。这是因为Ne较少时，零件运动随机性较大，每一步运动的估计位置偏离目标位置较远，需要多次位姿变换才能接近目标位置，使得有效位姿变换次数较多，进而由式（7）可知Pa较高。②Ne＞70时，Pa随着Ne的增加而减小。这是因为Ne越大，装配件越容易与基体件发生碰撞，导致无效位姿变换次数增加，进而由式（7）可知会导致Pa减小。

由图7可以看出：①Ne＜40时，Np随着Ne的减小而增大。这是因为Ne减少会增加零件运动的随机性，导致有效位姿变换次数的增加，进而引起Np的增加。②Ne＞70时，Np随着Ne的增大而增大。这是因为Ne越大，无效位姿变换次数越多，导致Np越大。

由图8可以看出：随着Ne的增大，位置抽样和粒子加权求和的计算量增大，导致位姿变换的计算量变大，t0呈递增趋势。

因此，对于端盖的装配，为了得到良好的装配性能，提高零件的有效位姿变换率，降低位姿变换次数和平均位姿变换时间，需要将采样粒子数控制在一定范围内（30～60）。

4.1.2 零件外形对方法性能的影响

虚拟环境中，零件定位的前提是精确的碰撞检测。本文采用分层碰撞检测算法［23］对零件进行实时碰撞检测，首先进行包围盒层的初步碰撞检测，如果包围盒发生碰撞，则对面片层进行精确求交，得到碰撞响应。因为包围盒间的碰撞检测速度非常快，碰撞检测时间主要用于面片间的精确求交，所以零件表面形状的复杂程度决定了发生碰撞的可能性和碰撞检测时间的长短，从而影响了方法性能和装配效率。本节将分析零件外形对方法性能的影响，关于零件外形对装配效率的影响将在4.2.1 节详细讨论。

图9给出了齿轮齿条和紧固板件的装配场景。比较齿轮齿条、紧固板件和端盖三种装配模型的外形复杂程度可知，紧固板件最复杂，齿轮齿条次之，端盖最简单。利用本文方法分别对这三种装配模型进行20次装配实验，采样粒子数均为40，且每次实验装配件和基体件的初始坐标均相同，分别计算每种装配模型的Pa和Np，如图10和图11所示。

结合图10和图11可以看出：①对于外形简单的零件（端盖），Pa较高且Np较少，说明大多数装配操作有效，零件定位准确、容易装配，既减少了总体位姿变换次数，又提高了定位精度；②随着零件复杂程度的增加，Pa逐渐降低而Np逐渐增加，说明零件外形越复杂，越容易发生碰撞干涉，用来进行反复定位、寻找可行装配路径的无效位姿变换次数越多，装配越困难。以上结论与实际装配经验一致，说明本文方法性能好，结论可靠性高。

4.2 装配效率分析

零件的装配过程和装配难易程度，不但受自身外形的影响，而且受已装零件或者场景中其他障碍物的影响。已装零件或障碍物会影响待装零件的可行装配路径，增加定位难度和装配时间，甚至导致无法装配［24］。因此，对装配效率进行分析时，除了考虑采样粒子数和零件外形等因素外，还需要考虑已装零件的影响。

4.2.1 采样粒子数和零件外形对装配效率的影响

在4.1节分析的基础上，本节将采样粒子数和零件外形因素综合考虑，分析它们对装配效率的影响。为了对装配效率进行分析，定义装配总体耗时

式中tcd为单次碰撞检测时间。由式（8）可知，装配总体耗时由位姿变换时间和碰撞检测时间两部分组成。

根据4.1.2节的分析可知，在紧固板件、齿轮齿条和电机端盖三种装配模型中，紧固板件外形最复杂，其面片多，求交计算量大，碰撞检测时间最长；端盖外形最简单，其面片大且几何形状规则，求交速度快，碰撞检测时间最短。利用本文方法分别对这三种模型进行装配实验，选取不同数量的采样粒子，各进行20 次实验，计算装配总体耗时T，如图12所示。

从图12可以看出：①在采样粒子数相同的情况下，对于外形复杂的零件，由于碰撞检测耗时较多且容易发生碰撞干涉，装配总体耗时较多。②零件外形越简单，在装配时间最短时对应的采样粒子数越多。这是因为外形简单的零件的碰撞检测时间很短，对装配时间的影响较小，而较多的采样粒子可以使零件快速运动到目标位置。③外形简单的零件的曲线相对平缓，在一定范围内Ne对装配效率的影响很小，反映了形状简单的零件易于装配的特点。④当Ne＞100时，所有零件的装配耗时都会随着Ne的增加而迅速增长。这是因为过多的采样粒子会导致装配件每一步的估计位置过于接近目标位置，大量增加无效位姿变换次数，增加了装配整体耗时。

因此，为了获得较高的装配效率，需要根据零件外形的复杂程度选取合适数量的采样粒子，平衡位姿变换时间和碰撞检测时间对总体耗时的影响。对于外形简单、容易装配的零件，由于场景渲染和碰撞检测速度较快，可以选取较多的采样粒子（50＜Ne＜80），使装配件快速运动到目标位置；对于外形复杂、装配特征较多的零件，因为碰撞检测耗时较多，所以选取相对较少（10＜Ne＜30）的采样粒子，以减少面片层碰撞求交的次数，保证装配过程的流畅和方法实时性。

4.2.2 已装零件对装配效率的影响

本节将对已装零件对装配效率的影响进行分析。Hung-Yao Hsu等在文献［24］中研究了稳压器的装配过程，通过对零件可行运动区域的计算，定量评价了单个零件和产品的可装配性，并详细讨论了已装零件对待装零件的装配路径和装配效率的影响。稳压器三维模型如图13 所示，由5 个组件构成。下面参考文献［24］给出的装配顺序，利用本文方法对该模型进行装配仿真，采样粒子数均为40。首先，按照顺序1-4-2-0-3对各组件进行装配，利用式（8）计算每个组件的装配时间，如图14 所示；然后，分别按照顺序（1）1-4-2-0-3，（2）0-3-2-1-4，（3）3-0-2-1-4对稳压器进行装配，不同顺序下，将每个组件装配时间之和作为产品的装配时间，结果如图15所示。

由图14可以看出：①对于组件1，没有已装零件对其路径进行干涉，因此装配时间最短，定位最容易；②对于组件4，由于其装配路径受到已装组件1的影响，相对于1，装配时间略有增加；③受已装组件1，4和2的影响，组件0的装配难度最大，装配时间最长；④相对组件2，组件3由于受到组件0的影响，其装配时间较长。

由图15可知：①顺序2装配时间最短，为最优装配顺序；②比较顺序2和3，先装组件0或者3，稳压器总装配时间差别不大，因为这两个组件对其他未装组件的装配路径影响较小；③先装组件1，4和2会严重影响组件0的装配路径，导致装配时间大大增加。

以上结论与Hung-Yao Hsu 等在文献［24］中得到的结论基本一致。相对于Hung-Yao Hsu 等提出的装配规划评价方法，本文方法无需计算每个零件的空间路径和可行区域，而该计算过程非常复杂和耗时，因此本文方法的效率更高。

5 结束语

本文提出的基于粒子滤波的自动装配方法，有效解决了现有装配引导方法无法在虚拟环境中进行自动装配并反映真实装配过程的问题。利用随机样本描述零件空间位置的概率分布，反映了装配过程中操作者对零件控制的不确定性和模糊性。系统根据零件的当前位置和目标位置自动计算其下一时刻的估计位置，使装配引导具有智能性。实验结果表明，该方法可以有效引导零件完成虚拟装配并对装配过程进行实时仿真，同时由于利用计算机进行位姿估计，提高了虚拟装配的效率。下一步研究的重点是通过动态更新采样粒子数目降低算法的计算复杂度，进一步提高虚拟装配效率。

［1］JAYARAM S，JAYARAM U，WANG Y，et al.VADE：a virtual assembly design environment［J］.IEEE Computer Graphics and Applications，1999，19（6）：44-50.

［2］DEWAR R G，CARPENTER I D，RITCHIE J M.Assembly planning in a virtual environment［C］／／Proceedings of Portland International Conference on Management and Technology.Washington，D.C.，USA：IEEE Computer Society，1997：664-667.

［3］LIU Zhenyu.Research on the theory，method and application of process-and-history-oriented assembly modeling in virtual environment［D］.Hangzhou：Zhejing University，2001（in Chinese）.［刘振宇.面向过程与历史的虚拟环境中产品装配建模理论、方法及应用研究［D］.杭州：浙江大学，2001.］

［4］LIU Zhenyu，TAN Jianrong，ZHANG Shuyou.Research of motion guidance in virtual assembly based on semantics recognition［J］.Journal of Software，2002，13（3）：382-389（in Chinese）.［刘振宇，潭建荣，张树有.基于语义识别的虚拟装配运动引导研究［J］.软件学报，2002，13（3）：382-389.］

［5］WU Changsheng，WANG Daiyong，DAI Yingming，et al.Frame-based motion guidance for virtual assembly［J］.Journal of Computer Aided Design &Computer Graphics，2010，22（6）：997-1003（in Chinese）.［吴昌盛，汪代勇，代英明，等.基于框架的虚拟装配运动引导［J］.计算机辅助设计与图形学学报，2010，22（6）：997-1003.］

［6］WU Changsheng，LI Yunfei，WANG Daiyong，et al.Research on virtual assembly system based on framework［J］.Computer Engineering，2010，36（15）：216-218（in Chinese）.［吴昌盛，李云飞，汪代勇，等.基于框架的虚拟装配系统研究［J］.计算机工程，2010，36（15）：216-218.］

［7］LIU Jianhua，NING Ruxin，YAO Jun，et al.Research on exact placement technology of component in virtual assembly environment［J］.Computer Integrated Manufacturing Systems，2005，11（4）：498-502（in Chinese）.［刘检华，宁汝新，姚 珺，等.面向虚拟装配的零部件精确定位技术研究［J］.计算机集成制造系统，2005，11（4）：498-502.］

［8］WU Changsheng，WANG Daiyong，DAI Yingming.Research of assembly motion navigation in assembly simulation system［J］.Journal of System Simulation，2005，17（4）：926-929（in Chinese）.［吴昌盛，汪代勇，代英明.装配仿真系统中装配运动引导方法研究［J］.系统仿真学报，2005，17（4）：926-929.］

［9］ZHANG Zhixian，LIU Jianhua，NING Ruxin.Research on assembly constraint realization technology in virtual assembly［J］.Journal of System Simulation，2011，23（9）：1878-1888（in Chinese）.［张志贤，刘检华，宁汝新.虚拟装配中的装配约束实现技术研究［J］.系统仿真学报，2011，23（9）：1878-1888.］

［10］ZHANG Dan，ZUO Dunwen，JIAO Guangming，et al.Interactive aid technology for virtual assembly manipulation［J］.Journal of South China University of Technology：Natural Science Edition，2010，38（5）：127-132（in Chinese）.［张 丹，左敦稳，焦光明，等.面向虚拟装配操作的交互辅助技术［J］.华南理工大学学报：自然科学版，2010，38（5）：127-132.］

［11］YANG Rundang，WU Dianliang，FAN Xiumin，et al.Research on constraint-based virtual assembly technologies［J］.Computer Integrated Manufacturing Systems，2006，12（3）：413-419（in Chinese）.［杨润党，武殿梁，范秀敏，等.基于约束的虚拟装配技术研究［J］.计算机集成制造系统，2006，12（3）：413-419.］

［12］LIU Zhenyu，TAN Jianrong.Constrained behavior manipulation for interactive assembly in a virtual environment［J］.International Journal of Advanced Manufacturing Technology，2007，32（7）：797-810.

［13］XIA Zhixiang，ZHU Hongmin，WU Dianliang，et al.Reasoning method based on semantics for virtual assembly operation［J］.Computer Integrated Manufacturing Systems，2009，15（8）：1606-1613（in Chinese）.［夏之祥，朱洪敏，武殿梁，等.虚拟装配操作中基于语义的推理方法研究［J］.计算机集成制造系统，2009，15（8）：1606-1613.］

［14］DOUCET A，GODSLL S J，ANDRIEU C.On sequential Monte Carlo sampling methods for Bayesian filtering［J］.Statistics and Computing，2000，10（3）：197-208.

［15］ARULAMPALAM M S，MASKFLL S，GORDON N，et al.A tutorial on particle filters for on line nonlinear／non—Gaussian Bayesian tracking［J］.IEEE Transactions on Signal Processing，2002，50（20）：174-188.

［16］GORDON N J，SALOMND D J，SMITH A F M.Novel approach to nonlinear and non-Gaussian Bayesian state estimation［J］.IEEE Proceedings on Radar and Signal Processing，1993，140（2）：107-113.

［17］VAN DER MERWE R，DOUCET A，DE FREITAS N，et al.The unscented particle filter［R］.Cambridge，UK：Cambridge University，2000.

［18］XU Shifang，XIE Li，LIU Jilin.Robot localization based on MCMC particle filter［J］.Journal of Zhejiang University：Engineering Science，2007，41（7）：1083-1086（in Chinese）.［许士芳，谢 立，刘济林.基于MCMC粒子滤波的机器人定位［J］.浙江大学学报：工学版，2007，41（7）：1083-1086.］

［19］ANDRIEU C，DJURIC P M，DOUCET A.Model selection by MCMC computation［J］.Signal Process，2001，81（1）：19-37.

［20］CHIB S，NARDARI F S N.Markov chain Monte Carlo methods for stochastic volatility models［J］.Journal of Econometrics，2002，8（1）：281-316.

［21］LI Qunming.The localization and navigation approaches for indoor autonomous mobile robots［D］.Hangzhou：Zhejing University，2003（in Chinese）.［李群明.室内自主移动机器人的定位与导航方法研究［D］.杭州：浙江大学，2003.］

［22］GONG Ye.Random sampling techniques of the normal distribution［J］.Chinese Journal of Cpmputation Physics，1997，14（4）：566-568（in Chinese）.［宫野.正态分布的抽样方法［J］.计算物理，1997，14（4）：566-568.］

［23］WU Yanpeng，ZHANG Jianmin，WU Xianhe，et al.Study of a hierarchical collision detection method for virtual assembly［J］.Machinery Design and Manufacture，2003（6）：103-105（in Chinese）.［吴彦鹏，张建民，伍贤和，等.一种面向虚拟装配的分层碰撞检测方法研究［J］.机械设计与制造，2003（6）：103-105.］

［24］HUNG YAO，GRIER C I L.Quantitative measurement of component accessibility and product assemblability for design for assembly application［J］.Robotics and Computer Integrated Manufacturing，2002，18（1）：13-27.