基于RRTConCon算法的船舶装配拆卸高斯采样路径规划

闫富玉 朱晓军 彭 飞

海军工程大学船舶与动力学院，湖北武汉 430033

1 引 言

维修性是舰船装备的重要质量特性，是舰船战斗力的主要因素之一。舰船装备设计方案确定后，维修性就已基本确定。因此在舰船设计初期，就必须考虑舰船装备的维修可达性，即装备装配拆卸的可达性。装配拆卸路径规划是用于检验装备装配拆卸可达性的一种有效方法，其基本任务是为装备的可拆卸单元 （Local Replaceable Unit，LRU）寻找使其能从装配体中顺利进出的路径。目前，解决此类问题的一种重要方法是在虚拟的三维环境中利用智能路径规划技术寻求一条从起始点到目标点的路径，当LRU沿此路径装配拆卸时不会与环境中其它物体发生碰撞［1］。这种自动路径规划技术对于复杂空间中的产品布置和装配拆卸设计非常重要，如潜艇舱室、舰上狭窄通道等狭小空间。因此，在舰船装备设计的初期充分考虑解决装备维修的可达性将有利于提高舰船设计的质量和维修性，提高舰船生命力。

2 装配拆卸路径规划问题

装配拆卸路径规划问题本质上可归结为机器人的运动规划问题。在三维环境中，LRU可视为具有n个自由度的刚体机器人，而装配体则可看作是静态障碍物［2-3］。为便于计算，将LRU运动的物理空间转化为数学上的构型空间（C 空间）［4］，则C空间可以分为两部分：障碍空间（Cobs）和自由空间（Cfree）。LRU运动的路径对应于自由空间中一条连接初始位姿点与目标位姿点的曲线。由此，路径规划问题便转化为求解C空间中一条曲线的数学问题。

路径规划算法大致可分为基于自由空间几何构造的规划、前向图搜索算法和近年兴起的以解决高维姿态空间和复杂环境中路径规划为目的的基于随机采样的运动规划。前两种算法的计算复杂度与运动物体的自由度呈指数关系，对于船舶等复杂空间不适用。因此，1990年Barraquand和Latombe提出了基于随机采样的RPP规划算法［5］。在此基础上，逐步发展出了PRM算法［6］和 RRT 算法［7］，这两种算法非常适合于解决高维空间中的路径规划问题［8］。此后，RRT算法又发展出一些改进算法，如 RRTExtExt［9］、RRTConCon［10］等。

在现有的路径规划算法中，RRTConCon算法的规划效率较高，但该算法是采用随机采样的方法选取位姿点，在解决船舶舱室内狭窄通道的路径规划问题时效率不高。为解决这一问题，本文在RRTConCon算法的基础上，采用高斯分布函数进行分区采样，即在大的开阔区域设置较少的采样点，在复杂区域或狭窄通道设置较多的采样点，然后进行局部规划，找出拆装路径，提出了一种基于高斯采样的RRTGaussion算法，以提高规划效率。

3 基于高斯采样的RRTConCon算法

3.1 RRTConCon 算法

RRTConCon算法是RRT算法的一种改进。RRT算法的核心思想是通过随机采样使搜索树G（NG，EG）向C空间中的未探索区域逐步扩展，以实现高维状态空间中的快速路径规划。算法开始时，xinit形成树的根节点。每一次循环中，利用Random_State函数从C空间X中选择一个随机点xrandom∈X，同时按照给定的距离度量函数ρ，利用Nearest_Neighbor函数从G中选择最近的节点xnear∈NG。然后利用Control函数选择最佳输入点来对最近点xnear进行扩展。Control函数执行时首先以xnear为基础，选择输入u∈U，利用增量仿真函数产生一个新的位姿点xnew∈Xfree。如果xnear至xnew的路径满足全局约束，并且xnew和xrandom之间的距离在所有由U产生的状态中最小，则将xnew加入到RRT搜索树中。当xnew和xgoal之间的距离满足事先约定的最小距离时，便认为已经找到节点，搜索成功。

RRTConCon算法对RRT算法的改进主要有两个。一个是RRTConCon算法分别从初始位姿点和目标位姿点各产生一棵树，只要两棵树之间的距离满足事先约定的最小距离，就认为搜索成功。而RRT算法只是从初始位姿点产生一颗树，因此RRTConCon算法更容易找到路径。另一个改进之处在于扩展函数的不同，RRT算法的扩展函数为Extend函数，而RRTConCon算法的扩展函数为Connect函数。两者的不同之处在于：当xnear至xnew的路径满足全局约束，并且xnew和xrandom之间的距离在所有由U产生的状态中最小时，Extend函数会将xnew加入到RRT搜索树中，然后再继续产生随机点。而Connect函数不会将xnew加入到RRT搜索树中，而是在xnew和xrandom之间继续产生新的x′new，直到x′new不再满足条件，便再产生下一个随机点。与Extend函数相比，Connect函数的优点在于它可以减少采样点的个数，缩短采样时间。但是在狭窄通道或者大的开阔区域，所需的采样的数量是不同的，因此需要用到下面的高斯采样。

3.2 高斯采样

一个合理的采样点的集合应该这样分布：在大的开放区域有较少的采样点，而在靠近障碍物的复杂区域或者狭窄通道则有大量的采样点，因此，一个采样点加入树中的概率由它附近的禁止点的数量决定。为了得到这样的分布，这里用到了高斯密度函数。

定义一个n维的高斯密度函数：

式中，σ为高斯函数的规模（或者宽度）。为了在复杂区域或者狭窄通道设置较多的采样点，定义障碍函数Obs（c）在与障碍物碰撞时为1，其它为0。并定义：

那么f（c；σ）即为C空间中任意一点被采样的概率。显然，采样点附近障碍物越多，f（c；σ）取值便越大。为了避免禁止点，我们定义：

也就是说，在障碍物内，g（c；σ）＝0，其它情况下 g（c；σ）＝f（c；σ）。 g（c；σ）就是我们想要的概率分布。

根据上述分布，设计一个基于高斯分布的算法以获得采样点的集合，算法的伪代码如下：

loop

1.c1←产生一个随机点

2.m←产生一个满足正态分布的随机数

3.d←将m变成一个距离（如果m为负值，将它变为正值）

4.c2←在距离c1为d的点中随机选取一个点

5.如果 c1∈Cfree且 c2∉Cfree那么

6.将 c1加到树中

7.如果 c2∈Cfree且 c1∉Cfree那么

8.将 c2加到树中

9.否则

10.丢弃两个位姿点

根据高斯分布，如果产生了一个禁止位姿点和一个自由位姿点，则只增加一个自由位姿点到树中。这种采样方法称为高斯采样，因为它是根据分布g（c；σ）产生的一个采样集。

将高斯采样与RRTConCon算法结合便产生了一种新的算法——RRTGaussion算法。该算法和RRTConCon算法一样，在概率上是完备的，也就是说，如果在通道中存在一条路径，当采样点足够多时，找到可行路径的概率为1。但RRTGaussion算法同其它基于采样的路径规划算法一样，都不能保证其搜索结果为最优。

有一个参数发挥了重要作用：高斯分布的方差σ2，其对应于高斯采样的规模σ。根据高斯分布，选择的方差越小，靠近障碍物的位姿点就越多。反之，选择的方差越大，均匀分散在自由空间的位姿点就越多。

在三维环境中，当LRU带有旋转和平移的自由度时，选择的方差应该使大部分位姿点落在复杂区域或者狭窄通道内，但是允许LRU绕其旋转轴旋转，而不与障碍物发生碰撞。另外，旋转自由度的处理方式要与平移自由度不同。根据距离d，改变第2个采样点的平移自由度，而旋转自由度则可任取。

4 算法实例与讨论

4.1 Flange模型装配拆卸仿真分析

Flange模型由GE公司的研究与发展中心提供，是用于检验维修性设计的标准模型。它是将图1b中的LRU模型移入图1a的装配拆卸环境模型中（图1c），属狭窄通道的路径规划问题。下面将用Flange模型来比较RRTConCon算法与RRTGaussion算法的优劣。

Flange模型经处理后转变为多边形模型。装配拆卸环境模型含990个多边形，LRU模型含5 306个多边形，仿真程序采用C＋＋语言实现，图形的渲染采用OpenGL函数库，碰撞检测函数库采用PQP算法库。所有的调试都在一个4核、8 GB内存的HPXW8600工作站上进行。

为避免随机性，将程序运行100次，取其平均值，得到如表1、表2所示的仿真结果。

表1 Flange模型RRTConCon算法仿真结果Tab.1 RRTConCon simulation results of Flange model

表2 RRTGaussion算法仿真结果Tab.2 RRTGaussion simulation restuls of Flange model

从表1和表2的仿真结果可知：

1）RRTConCon算法和RRTGaussion算法是概率完备的，但是可以看到都有失败。这是因为在算法中，设定如果在5 000个采样点之内找不到路径，即认为规划失败。在失败次数方面，RRTGaussion算法比RRTConCon算法要少，这说明RRTGaussion算法比RRTConCon算法更容易找到路径。

2）在采样点的数量上，RRTGaussion算法要比RRTConCon算法少一些。这是因为采用高斯采样后使采样点的分布更加合理，所以RRTGaussion算法只用较少的采样点就可以规划出路径。碰撞检测次数的变化规律与采样点数量的变化规律是一致的。

3）在规划时间上，总体而言，RRTGaussion算法要比RRTConCon算法少15%左右。这是因为需要的采样点减少了，碰撞检测次数也减少了，相应地，规划时间也就减少了。

综合来说，RRTGaussion算法提高了Flange模型路径规划的效率，是一种比较有效的算法。

4.2 船舶机舱齿轮泵装配拆卸仿真分析

下面选取某型船机舱的一部分作为装配拆卸环境模型，某型齿轮泵作为LRU模型（图2），对RRTGaussion和RRTConCon算法的效率进行仿真对比分析。其装配拆卸路径图如图3所示。

船舶机舱模型和齿轮泵模型都是采用法国达索公司的Catia软件构造，经过处理后转变为多边形模型。机舱含多边形203 024个，齿轮泵含多边形2 536个，采用的仿真程序、函数库等与Flange模型一致。所有的调试都在一个4核、8 GB内存的HPXW6600工作站上进行。

同Flange模型测试一样，为避免随机性，将程序运行50次，取其平均值，得到了如表3和表4所示的结果。

表3 船舶机舱环境下RRTConCon算法仿真结果Tab.3 RRTConCon simulation results for ship engine room

表4 船舶机舱环境下RRTGaussion算法仿真结果Tab.4 RRTGaussion simulation results for ship engine room

由表3和表4的仿真结果可知：

1）在这个模型中，设定如果在1 000个采样点之内找不到路径，即认为规划失败。比较表3和表4可以看出，在算法失败次数方面，RRTConCon算法要比RRTGaussion算法少。

2）在采样点数量上的变化规律和碰撞检测次数的变化规律与Flange模型分析结果类似。

3）在规划时间上，总体而言，RRTGaussion算法要比RRTConCon算法少10%左右。

5 结 论

实验结果表明，高斯采样改进了基于随机采样的RRTConCon算法，提高了规划效率，但仍有几个问题需要解决：

1）在产生随机点的过程中，丢弃了很多采样点，实际上这些采样点本身就包含了C空间的一些信息。因此，如果在高斯采样的同时将这些点回收利用，将能进一步提高算法的效率。

2）RRTGaussion算法在检验模型——Flange模型中使得失败次数减少，但在船舶机舱测试环境中又使得失败次数增加，该问题还有待于进一步的研究。

［1］CHANG H，LI T-Y.Assembly maintainability study with motion planning ［C］／／Proceeding of IEEE International Conference on Robotics and Automation.Nagoya： IEEE Computer Society Press，1995：1012-1017.

［2］GARBER M，LIN，M.Constraint-based motion planning for virtual prototyping ［C］／／Proceeding of ACM Symposium on Solid Model and Application，Saarbrucken，Germany，2002.

［3］ZHANG L，HUANG X，KIM Y.D-Plan：Efficient collision-free path computation for part removal and sisassembly［J］.Journal of Computer-Aided Design and Applications，2008：774-786.

［4］LOZANO-PEREZ T，WESLEY M A.An algorithm for planning collision-free paths among polyhedral obstacles［J］.Communication of the ACM，1979，22（10）：560-570.

［5］BARRAQUAND J，LATOMBE J C.A monte-carlo algorithm for path planning with many degrees of freedom［C］//Proceeding of IEEE International Conference on Robotics and Automation，Cincinnati：IEEE Computer Society Press，1990：1712-1717.

［6］KAVRAKI L，LATOMBE J.Randomized preprocessing of configuration space for fast path planning ［C］//Proceeding of IEEE International Conference on Robotics and Automation，SanDiego： IEEE Computer Society Press，1994：2138-2139.

［7］LAVALLE S M.Rapidly-exploring random trees： a new tool for path planning ［R］.Technical Report TR98-11，Computer Science Department，Iowa State University，1998.

［8］刘华军，杨静宇，陆建峰.移动机器人运动规划研究综述［J］.中国工程科学，2006，8（1）：85-94.

［9］LAVALLE S M，KUFFNER J J.Rapidly-exploring random trees： progress and prospects ［M］.Algorithmic and Computational Robotics：New Directions.DONALD BR，LYNCH K M，RUS D，Eds.Massachu setls：Wellesley，2001：293-308.

［10］唐华斌，孙增圻.结合启发式函数的随机运动规划方法［J］.清华大学学报（自然科学版），2006，46（4）：580-583.