三维场景中实时路径规划优化算法

郑红波,左少华,程燕飞,秦绪佳,张美玉,徐晓刚

1(浙江工业大学 计算机科学与技术学院,杭州 310023) 2(海军大连舰艇学院 航海系,辽宁 大连 116018)

1 引 言

在三维空间场景中,运动物体需要随时了解自己所处的位置,掌握快速到达目的地的路径.由于运动物体都具有一定的体积和宽度,因此,保证所选择道路足够宽阔,能够顺利通过是一个重要的约束条件.在运动过程中,诸如房屋、树木等景物是无法穿透的,这是选择运动路线中的约束条件之一,需要进行碰撞检测.从上可以看出,在三维复杂场景中,往往因为复杂的环境和视点范围有限,无法识别出运动物体所在的位置,目的地位置也同样很难被快速辨认出,因此需要预先进行路径规划[1],而在此过程中,需要通过碰撞检测来获得可行的运动路径.

在碰撞检测方面已经有一些不错的工作.Jiménez、Segura提出了基于四面体的复杂多边形的碰撞检测算法[2],使得两个待检测的物体特征之间的相交测试数被大幅度地降低了,但与此同时,四面体的构建、包围体的分层和区域的划分过程较为复杂,并且多个物体间的重复区域未被排除导致重复多次检测;Chang、Kim 提出的面向包围盒(OBB)的检测算法[3]虽然能够一定程度上减少检测处理时间,但不易于碰撞点的坐标采集和碰撞的响应,是因为采用本地对象坐标而不是全局对象坐标来进行检测;Schauer、Nüchter提出了基于高效k-d树的碰撞检测算法[4],主要通过kd-CD和简单kd-CD检测算法、kd-PD和快速kd-PD碰撞程度计算方法进行碰撞检测,但该算法仅在数量点不多的情况下有较好的适应性.

在路径规划方面,前人也提出了多种不同的算法.Khalil和 Kazem 提出的机器人移动路径规划方法[5]通过求解多方向路径可行性概率,并利用概率递归函数来获得最佳路径,消除了Bezier曲线的缺点;Bircher、Kamel和Alexis等人提出了一种基于自主测试检查与覆盖的机器人路径规划算法[6],由于该算法在迭代计算视点路径开销过程中,早已走过的路径开销会被重复计算,从而导致算法的计算量和路径的规划时间有所增长;王殿君提出了一种改进的针对室内机器人移动路径规划的A*算法[7],该算法主要通过计算拐点、旋转方向和旋转最小角度来优化算法,减少了冗余点的计算和提高了机器人拐点处的调整速度,但由于计算点的增加,侧重于提高部分运作效率但影响了整体路径规划的效率和质量.

在上述路径规划工作中,基本上着力于算法的优化工作,没有考虑运动目标本身物理特征和道路特征对规划工作的影响,属于比较理想的状态,而实际目标运动中体积、大小、道路宽阔程度等都会影响到路径的选择.根据上述需求,本文提出了一种路径规划优化算法.算法在传统 A*算法的基础上,采用粗试探和精搜索的策略进行路径规划,用预碰撞筛选检测和精细碰撞检测的策略进行层次包围盒碰撞检测,规避视点小半径球根本不会与之碰撞的景物,使算法更接近于工程应用.

2 路径规划优化算法

本文提出的路径规划优化算法,主要从碰撞检测算法和A*算法两部分进行分析与研究.

2.1 碰撞检测算法

2.1.1 碰撞检测基本原理

碰撞检测[8-10]是指检测两个移动物体或者移动物体与三角平面之间有没有产生碰撞,即检测移动物体视点轨迹是否相交于三角平面.具体检测步骤如下:

1)计算检测平面S和平面S法线方向的最近距离.

2)判断前一帧视点位置OldPosition以及后一帧视点位置NewPosition和检测平面S之间的关系:若OldPosition和NewPosition同时分布在检测平面S的同一边,则跳转到步骤5);若如图1所示,则跳转到步骤3).

3)判断视点是否与平面相交.过平面S的三条边分别作平面S的垂面PS1、PS2和PS3,如图2所示.

图1 点与面的关系图2 碰撞检测处理图1Fig.1 Relationship Fig.2 Collision detectionbetween points and planeprocessing diagram 1

将平面PS1、PS2和PS3分别沿着各自法线方向向外移动距离L,以此来扩大防碰撞范围,提高碰撞检测的敏感度,如图3所示.

将平面PS1、PS2和PS3分别沿着各自法线方向向对顶角平移,得到平面PS4、PS5和PS6,来避免由于三角形向外扩张而导致检测区域过大,如图4所示.

此时若视点处在由平面PS1、PS2、PS3、PS4、PS5和PS6所围成的区域内,转至步骤4),不在则转至步骤5).

4)视点与三角面片发生碰撞,改变视点沿着平行于三角平面S的方向运动并将NewPosition更正为平行方向的位置.

5)视点没有与三角平面S发生碰撞,可沿当前方向继续运动.

平面PS6平面PS5平面PS4区域过大图3 碰撞检测处理图2图4 碰撞检测处理图3Fig.3 Collision detection Fig.4 Collision detection processing diagram 2processing diagram 3

2.1.2 碰撞检测算法

本文碰撞检测算法分为预碰撞筛选检测和精细碰撞检测,详细流程如下:

1)预碰撞筛选检测

预碰撞筛选检测是指筛选碰撞检查单元.先将三维场景进行八等分,划分为八个相同的单元,划分的结果就是场景中的物体可能处在一个或几个单元中,如图5所示;然后通过结点的方式,将单元组成八叉树模型;再筛选出与视点小半径球在同一最小子单元中的物体,如图6所示,物体B与视点小半径球A处在同一单元中,而物体C则不是.

ABC图5 八叉树模型示意图图6 预碰撞筛选检测Fig.5 Octree model Fig.6 Pre-collision screeningdiagramdetection

制定一定的控制规则并且选取适当的划分尺寸来避免空间占用浪费和计算效率的减低:

a.规定结点数量最大值Nmax和三角面片数量最小值Fmin.

图7 球心到平面的距离Fig.7 Distance form the center of the sphere to the plane

b.进一步细分单元结点直到该单元结点内三角面片数小于Fmin.

c.每次划分后统计单元结点数,若大于Nmax,则停止划分.

2)精细碰撞检测

通过预筛选去除不在同一单元的三角面片,然后判断处在同一单元中的视点小半径球与所有三角面片所在平面是否相交:若二者相交且存在处在三角面片上的交点,则发生碰撞.具体算法如下:

1)由公式求解出视点小半径球球心到三角面片所在平面的距离.如图7所示,其中VC(XC,YC,ZC)为球心坐标,R为半径,平面L0L1L2为三角面片所在平面,L0(x0.y0,z0)、L1(x1.y1,z1)和L2(x2.y2,z2)是三角面片三点的空间坐标,N为L0L1L2的法向量.

(1)

(2)

(3)

(4)

由公式(1)、(2)、(3)和(4),球心到三角面片所在平面的距离d可以被表示为:

(5)

2)判断距离d与球体半径R的大小关系:若d 小于R则代表视点小半径球相交于三角平面,计算出交点坐标;若d大于等于R,则代表视点小半径球和三角面片相离,碰撞不会发生.

yxVdNViO图8 球体与平面相交图9 交点与三角面片的关系Fig.8 Intersect of the sphereFig.9 Relationship between and the planeintersection and triangle

3)通过公式(6)求出交点坐标向量.如图8所示,球与平面相交于点Vi:

Vi=VC-dN

(6)

4)如果Vi位于三角面片内,则代表视点小半径球与三角面片发生碰幢,否则没有发生碰撞.具体判断方法如图9所示,连接Vi和三角面片的三个顶点ABC,再利用公式(7)、(8)和(9)计算连线夹角的度数,若夹角度数之和等于360度,则表示Vi位于三角面片内,否则Vi位于三角面片外部或者三边上.

(7)

(8)

(9)

2.2 路径规划优化算法

2.2.1 A*算法

路径规划算法中常用的是启发式搜索[11-13],是指利用求解问题的启发信息,指导搜索向最接近最优结果的方向进行.A*算法是一种使用评价函数计算拓展点代价估值并进行排序的有序搜索算法,公式(10)为评价函数.

f(c)=g(c)+h(c)

(10)

其中c为当前结点,函数f(c)为从初始点Str开始经由结点c而到达目的点Fin的代价估计值,函数g(c)为从初始点Str开始到结点c的实际代价值,而函数h(c)则为从当前结点c到达目的点Fin的最佳路径的代价估计值.

A*算法[14,15]具体步骤如下:

1)定义待选列表L1和已选列表L2.表L1保存代价评估值已知但领域拓展点的代价估计值未知的结点;表L2保存表L1中且对领域拓展点进行过代价评估的结点.

2)将c从表L1中移除并放入表L2中,然后通过碰撞检测找出结点c的所有无碰撞领域拓展点N,再将结点c记为N的父结点.

3)由公式(10)计算出N的代价估值,并将N放入表L1中.

4)若当前结点c为目的点Fin或者表L1为空,则算法结束.否则重新选取L1中最小代价估值的结点为结点c,重复步骤2)、3)和4).

实验结果表明,A*算法可以搜索出从Str到Fin的最优路径,并且函数g(c)、h(c)和f(c)遵从一致性原则,基于以上优点,A*算法可以被用来进行路径规划.

但是A*算法本身也具有一些明显的不足,A*算法的关键是设计一个合适的估价函数h(c),最好的情况当然是估价函数h(c)等于结点c到目的点Fin的实际代价值,在这种情况下保证可以得到最优解的同时搜索效率较高,而如果h(c)过度小于实际代价值会导致搜索结点过多,搜索效率低下.A*算法在路径规划过程中,一些潜在最优结点在较早搜索阶段被删除了,导致传统A*算法解决路径规划问题时,有时候规划出来的路径是一个较优的可行解但不是全局最优路径.并且传统A*算法的扩展邻域结点的优先级是一样的,导致在路径规划过程中碰撞检测的处理时间较多,时间效率随着碰撞检测处理的三角面片数量增加而明显下降.

2.2.2 本文算法

本文算法在原始A*算法的基础上,采用粗试探和精搜索的策略进行路径规划,其中粗试探和精搜索采用本文优化的碰撞检测算法进行碰撞检测.具体步骤如下:

1)粗试探.当前结点c作为中心,除去结点c的父结点方向和父结点反方向,在四周三维度上的六个方向中剩下的四个方向上以包围盒的形式进行碰撞检测,试探与场景中景物有没有产生碰撞.例如对于方向X,构建一个紧靠当前结点c所在包围盒的可无限扩展的包围盒T并进行碰撞检测,如果包围盒T没有与场景中物体对象发生碰撞,则说明当前结点c所在包围盒可以在包围盒T内自由移动且不会发生碰撞.随着c所在包围盒的移动,包围盒T也会随之移动,并一直进行碰撞检测.

2)精搜索.如果粗试探失败,先除去c的父结点方向,在c的四周三维度上剩下的五个方向依次进行碰撞检测,记未碰撞邻域为l.之后计算出未碰撞领域l的范围最大距离d,并比较d与视点的范围最大距离的大小关系,若d小于视点最大距离则计算l的代价估值并按降序的方式进行排序,然后添加到表L1中(代价估值越小在表L1中的位置越靠近前,代价估值越大在表L1中的位置越靠近后);否则将该碰撞领域l去除,重新进行精搜索.

3)从表L1中循环选取结点,就可以找到代价评估值越小的点.

本文优化改进的算法的伪代码如下:

while(待选列表L1不为空){

从待选列表L1前端中选取结点c;

if(c不在包围盒内)break;

while(true){

在已选列表L2中添加当前结点c;

对当前结点c进行粗试探;

if(粗试探成功){

经过当前结点c的路径已经遍历结束,跳转到下一结点循环;

}

else{

if(当前结点c的邻域为空) break;

else{

对当前结点c进行精搜索;

从待选列表L1前端中选取结点c;

}

}

}

}

3 实验结果与分析

3.1 碰撞检测算法

分别使用文献[16]的AABB树方法、文献[17]的OBB树方法以及本文碰撞检测方法,在同一种三维空间场景和同一种运动路径中分别进行多次视点与三维碰撞物体的碰撞试验,三种方法的三角形测试数、面测试数、真正涉及的三角形数以及总时间的均值如表1所示.图10为碰撞实验示意图,图10(a)为三维球体碰撞渲染图,图10(b)为三维球体碰撞网格图.

图10 碰撞示意图Fig.10 Collision diagram

由表1可知,与文献[16]的AABB树碰撞检测算法相比,本文优化的碰撞检测算法的三角形测试数和真正涉及的三角形数有所减少;与文献[17]的OBB树碰撞检测算法相比,三角形测试数和面测试数也有所减少,因而本文优化的碰撞检测算法的总时间小于其他两种算法,能够提高一定的碰撞检测效率.

3.2 本文路径规划优化算法

运用传统A*算法和本文优化改进的A*算法对同一模型的五条路径进行实验仿真,表2为两种算法的处理时间对比结果.如图11(a)所示,分别用数字1到5表示五条路径,其中有数字一端的为终点，没有数字的一端为起点.

表1 三种算法的测试数和总时间对比
Table 1 Comparison of the testing number and the total time between three algorithms

AABB树算法OBB树算法本文优化的碰撞检测算法三角形测试数168512141102面测试数590680590真正涉及的三角形数936584584总时间(s)0.0245080.0235930.020421

表2 两种算法的处理时间对比
Table 2 Comparison of processing time between two algorithms

名称路径1路径2路径3路径4路径5三角面片数57529890106381323411354物体数25678传统A∗算法(s)0.04647410.1079230.1223560.09288930.167038本文优化改进的算法(s)0.04018740.08683640.1126950.07564820.125513

(a)五种路径规划图(b)本文路径规划效果对比图12.011.511.010.510.09.5路径6路径7路径8代价评估值图11 路径规划图图12 路径代价评估值柱形图Fig.11 Path planning Fig.12 Column chart of pathdiagram cost evaluation

在图11(b)中,起点为汽车所在位置,终点为球体所在位置,汽车从起点开始前往终点运送货物,从图11中可以看出有三条运动路径,分别为路径6、路径7和路径8,图12为它们的代价评估值,路径6的代价评估值最小,路径8的代价评估值最大.其中采用传统A*算法规划结果为路径6,而本文算法规划结果为路径7.相对于路径8而言,传统A*算法和本文算法都能规划出代价评估值较小的路径.在不考虑实际应用情况下,最优路径选择确实是路径6,但因为卡车的体积无法通过两个柱状物之间的路径6,因此,排除掉不可能的路径6后,本文算法相对于路径8选择了代价评估值较小的路径7,更加符合实际应用情况.由实验结果可知,在路径规划的处理时间上,本文优化的路径规划算法比传统A*有所减少,路径的搜索效率得到提高,且对于无法通过的未碰撞区域进行了规避,择优选择其他代价评估低的路径,符合实际应用的要求,可以更好地应用于各种实际工程应用的路径规划问题.

4 结 论

本文在传统A*算法的基础上,针对三维场景的路径规划问题,提出了一种基于层次包围盒碰撞检测的路径规划优化算法.优化算法在原始A*算法的基础上,采用粗试探和精搜索的策略进行路径规划,并通过对层次包围盒碰撞检测算法的基本原理的研究,采用预碰撞筛选检测和精细碰撞检测的策略进行碰撞检测,实验验证表明,算法保持了很好的实时性,同时更接近工程应用的要求,可以规避无法通过的道路,选择出具有参考价值的优化路径,在达到效果的前提下,提高了时间效率.进一步的工作包括:根据运动目标的形状,建立更加合理包围盒;改进碰撞检测算法,使得检测效率更加高效,进而获得更加合理的优化路径.