专利名称：基于矢量法的路径规划预处理方法
技术领域：
本发明涉及路径规划预处理方法，尤其涉及一种基于矢量法的路径规划预处理方 法，用于仿真技术领域。
背景技术：
路径规划(Path Planning)是指按照某个评价标准(如最短路径长度、最短行进 时间、最小能量消耗等)，满足某个约束条件(如避免与障碍物碰撞等)，规划一条从起始点 位置到达目标点位置最优(或次优)的路径(详细内容可参考文献[戴光明.避障路径规 划的算法研究[D].武汉华中科技大学，2004])。路径规划作为一个经典的人工智能问 题，在仿真、机器人、无人驾驶车辆等领域得到了广泛而深入的研究。目前，在这些领域使用 的路径规划方法主要有图搜索算法(如A*算法[苏浩，李钦富，蔡骏.A*算法在基于道路 网的路径规划中的应用.中国电子科学研究院学报，2010，5(4): 419-422.], Dijkstra 算法[WAN Ming, ZHANG Wei, MURRAY Marie 0.，KAUFMAN Arie. Automatic target tracking on multi-resolution terrain. Journal of Zhejiang University, 2006, 7(7): 1275-1281.]、基于软计算的规划方法(如遗传算法[梁晓辉，吴威，赵沁平.大规 模真实地形数据中的全局路径规划方法——基于遗传算法的研究.计算机研究与发展， 2002，39(3) 301-306.]，蚁群算法[TAN Guan-zheng, HE Huan, Aaron Sloman. Global optimal path planning for mobile robot based on improved Dijkstra algorithm and ant system algorithm. Journal of Central South University of Technology, 2006，13(1) : 80-86]、基于随机采样的规划方法(如快速扩展随机树(Rapidly-exploring Random Tree，RRT [樊晓平，彭展，张恒，罗熊.基于快速扩展随机树的机器人路径规划仿真 实验平台研究.铁道科学与工程学报，2005，2(2): 86-92])等。上述方法都建立在一定 的空间数据结构表示方法之上，常用的有矢量法和栅格法。在仿真系统中，实时性是非常重要的因素。如果出现滞后或延迟，会导致仿真系统 的真实感下降。而在路径规划这方面，已有的技术专注于系统运行期间的一些优化技术，而 未关注前期的数据预处理。
发明内容
本发明目的是针对目前仿真系统的路径规划时间过长，提出一种基于矢量法的路 径规划预处理方法，该方法可以有效提高仿真系统路径规划时的实时处理速度。本发明的思路是针对仿真环境中的移动主体不能够穿越距离小于自身的障碍物 这一事实，根据路径规划中最大移动主体的尺寸设置一个阈值，将距离小于该阈值的障碍 物进行合并，从而降低仿真系统中后续路径规划的工作量，提高整个系统的实时处理能力。具体而言，本发明采用以下技术方案
一种基于矢量法的路径规划预处理方法，用于矢量法表示的仿真系统路径规划，该方 法包括以下步骤
4步骤A、将仿真环境中的每个障碍物均模型化为凸多边形；
步骤B、将步骤A中得到的凸多边形中距离小于预先设定的阈值的任意两个凸多边形 融合处理为一个新的凸多边形，并依次迭代处理，直到最终得到的凸多边形中任意两个的 距离均大于上述预先设定的阈值；其中，所述预先设定的阈值等于路径规划中最大移动主 体的尺寸；
步骤C、将步骤B得到的结果作为仿真系统中路径规划的初始状态。仿真系统中的建模方法有很多，如三维建模、栅格法等，为了简化计算并便于后续 的相交判断和融合处理，本发明在将障碍物模型化为凸多边形时优选在欧式平面中实现。其中，凸多边形之间的距离采用最小距离法定义，即两个凸多边形上距离最小的 两个点之间的距离。步骤B具体包括以下各步骤
步骤Bi、设置变量的初始值为步骤A得到的凸多边形的数量；
步骤B2、设置变量、的值为
m=1，n = 1 ；
步骤B3、计算第n个凸多边形与第n + 1个凸多边形的距离并判断该距离是否大于一 个预先设定的阈值，如是，则转步骤B5 ；如否，则转步骤B4 ；
步骤B4、将第n个凸多边形与第n+1个凸多边形融合为一个新的凸多边形并删除原 第个与第m +1个凸多边形,将况的值修改为N-1后转步骤B2 ；
步骤B5、判断是否m= N,如是，则转步骤B6;如否，则将的值修改为m + 1后转步骤
B3 ；
步骤B6、判断是否n=p,如是，则结束；如否，则将m的值修改为n并将n的值修改为n+1 并后转步骤Β3。本发明中，将两个凸多边形融合处理为一个新的凸多边形，具体按照以下方法
以其中一个多边形的中心为原点，该多边形的两个顶点的连线为纵轴，两个多边形的 中心连线为横轴，建立相对坐标系；在该相对坐标系中，在两个多边形中寻找进行纵坐标绝 对值最大的4个顶点作为一级融合点；如果一级融合点的连线与多边形相交，则在一级融 合点集合中寻找二级融合点，使之连线与多边形不相交，同时删除该二级融合点取代的一 级融合点；将现有融合点连在一起合成一个凸多边形，完成两个多边形的融合。本发明基于常用的路径规划空间矢量表示法，首先将仿真环境中的障碍物模型化 为欧氏平面中的凸多边形；然后对距离小于某一阈值的任意两个凸多边形融合处理为一个 新的凸多边形，并依次迭代处理，直到最终得到的凸多边形中任意两个的距离均大于上述 预先设定的阈值。即将距离小于阈值的多障碍物合成一个，从而有效降低路径规划的计算 量。相比现有技术，本发明的优点在于针对仿真环境中的移动主体不能够穿越距离小于自 身的障碍物这一事实，将距离小于某一阈值的障碍物进行合并，对路径规划进行预处理，从 而有效加快实际的路径规划时间，提高系统的实时处理速度。
图1是本发明的基于矢量法的路径规划预处理方法流程图； 图2是本发明方法中两个凸多边形之间距离计算的示意图； 图3是本发明方法中点到图形的距离计算示意图，其中(a)为点到多边形的距离计 算，(b)为点到直线段的距离计算；
图4是本发明方法中融合点的计算示意图，其中(a)为计算过程，(b)为计算结果； 图5是具体实施方式
中所述射线与多边形的相交判断方法示意图； 图6是本发明方法在融合处理过程中不同阶段的示意图； 图7是分别采用栅格法和本发明方法进行路径规划的结果比较； 图8是分别采用栅格法和本发明方法进行路径规划的时间结果比较。
具体实施例方式下面结合附图及实施例对本发明的技术方案进行详细说明
本实施例采用如图6 (a)所示的二维虚拟仿真环境，经模型化处理后共有26个用凸多 边形表示的障碍物，根据该仿真环境中移动主体的尺寸将阈值胃设定为5。然后按照以下方
法对这26个凸多边形进行处理(处理流程如附图1所示)
步骤Bi、设置变量—的初始值为步骤A得到的凸多边形的数量，本实施例中y的初始
值即为26;
步骤B2、设置变量_、■的值为= = 1 ；
步骤Β3、计算第^个凸多边形与第· + 1个凸多边形的距离并判断该距离是否大于一个
预先设定的阈值，如是，则转步骤Β5 ；如否，则转步骤Β4 ；
步骤Β4、将第_个凸多边形与第个凸多边形融合为一个新的凸多边形并删除原第
■个与第· + 1个凸多边形,将y的值修改为f-Ι后转步骤Β2 ；
步骤B5、判断是否_% _ I,如是，则转步骤B6 ；如否，则将■的值修改为^ + 1后转步骤
B3 ；
步骤B6、判断是否^ 况’如是，则结束；如否，则将■的值修改为■并将■的值修改为 ,| + 1并后转步骤83。本发明中，任意两个多边形的距离采用最小间距法进行计算并根据预先设定的阈 值判断是否进行融合，
采用最小间距判断法计算两个多边形之间的距离，首先如附图2所示，建立以0
为原心，0込方向为横坐标轴方向的相对坐标系"X/，在该坐标系下(Jj Jj)
(I = !,2 广-M)為=(gj 為‘ )(j = 12" - ,Λ)，O1= (0，0)，O2: 0)，I12=先计算0的所有顶点距离O2的最小值
Siep ι 比较,的横坐标，得到顶点且;=max(i1j, -!ia);
6磊聊2令尽=‘，计算顶点虞山、蟊j、虞山与多边形02的距离，分别为‘、乙、毛；罩锄3若嘲乙‘；毛Ui，则羞土蜃；1，‘毛，转蹋磡4。若鹹‘山；如u当，则与乙，转月瓣8。若嘲乙乙，毛)山，则氈一誊斗]，与，当，转罩霉咖6；墘頜4计算顶点躇山与多边形0，的距离毛；聶聊5若，士晶，则蜃土矗二!，‘‘，转聶锄4。若，立正，则转聶锄8；髒咖6计算顶点舔山与多边形0，的距离毛；驟蟧7若2女晶，则出生女子!，‘’厶，转墘咖6。若毛逗点，则转墘咖8；雕蟧8输出0、的所有顶点距离0，的最小值‘。[OO、6] 再计算02的所有顶点距离o、的最小值 舀锄l比较轧的横坐标，得到顶点屯，且乙三嘲氈√蟧爭；；；；乩)； 驪中2令吕=默，计算顶点轧十bo、轧毒l与多边形0、的距离，分别为‘、乙、毛； 黝3若鹹‘j‘。毛齲，屿三譬二!，毛‘，转酶4。若鹹‘，‘。‘U沙毛乙，转月锄8。若鹹小‘i毛)山，则氈三女子]，山乙，转轟啣6；鑼蟧4计算顶点轧二l与多边形0、的距离‘；黝5若乩<山，灿土占二l，山‘，转鞠4。若；止山，则转黝8；雕咖6计算顶点轧毒l与多边形0、的距离毛；黝7若Z如山，鹏士抖1，毛’毛，转鞠6。若厶乏山，则转黝8；驟雛8输出02的所有顶点距离0，的最小值厶。盛‘瘤
以上计算多边形之间距离的过程中需要求解点到多边形的距离，本具体实施方式
中采用以下方法如图3‘a，，0、昂一蹿少女；三’檀山是平面Oxy内一个多边形，矽为该平面内任一点。以矽点为原点，以。、矽连线为横坐标轴，建立相对坐标系。’J’／’；在该坐标系下吗一‘气j的，‘r争L乙 !i脚，，仁‘。，。，，Ol=‘j12，。，，／12----鸥卣]，矽到。、的最近距离‘可计算如下 磊聊l比较吗的横坐标，得到顶点翠，，且‘苎砵崠蟧湳!，’‘戴乙3； 舀聊2令手=z，计算点矽与线段气／j的距离毛，点矽与线段尽j气缸的距离乙； 驟蟧3若毛√当，则女士聾二1，L’毛，转驟蟧4；若，√五，则女鲁云一!，L’毛，转鱷咖6；驟咖4计算点矽与线段軋扇吼的距离九；5若则转没—5。若泰M ,则转没—8 ；
6计算点Q与线段吣吣丨丨的距离I2
7:若4<￡,则5:=^+1丄=4,转沒印5。若^则转没響8 ；
8输出点々与多边形0的距离/,。
以上计算多边形之间距离的过程中需要求解点到直线段的最近距离，本具体实施 方式中采用以下方法如图3 (b) ,P1P2为一位于平面fly内的直线段，々为该平面内任一点。 以该线段的任一端点为原点(图中选择的是弋点)，以线段所在直线为横坐标轴，建立相对 坐标系，显然，Λ点和O，点重合。设々点在下的坐标值为(W1),Λ点在 O'X y下的坐标值为(^，(^，则以到/^/^的最近距离丄可计算如下 (1)当& < 0 时,￡ =
⑵ ^i>yim,L=d(QsP2)；
(3)当丨3511彡12时,￡:计算0的所有顶点距离O2的最小值以及点与多边形O2的距离时，都是从最有可 能产生最小距离的顶点开始，然后往两边扩展，直到遇到距离大于该点的情况，此时的最小 距离就是所要求的值。将两个凸多边形融合处理为一个新的凸多边形，具体按照以下方法
Step 1 如图2所示建立的坐标系中，寻找两个多边形中纵坐标绝对值最大的4个顶
点，将这4个顶点作为一级融合点，见图4(a)中的A、A、K3和& ；
Step 2 运用直线与多边形相交算法，对直线AA和多边形α进行计算，判断二者是否
相交Or1点除外)，若相交，求出二级融合点来代替A点； Step 3 对直线AA和多边形o2 'KliStep 2操作
Step 4 对直线AA、多边形O1和多边形O2重复总φ 23，直到^vr2与O1和O2
不再相交为止；
5 对直线&r4、多边形和多边形O2重复2^Up 3和S⑩4 ；
SHAPE \* MERGEF0RMAT陶6 按图形的顺时针方向，设定多边形的融合起点和融
合终点，保留两点之间的顶点，合成为一个新多边形，并且删除原有多边形。在图4(a)中， 多边形0的融合起点为&，融合终点是&，则按照顺时针方向，保留从&到A的0的顶点； 多边形込的融合起点为A融合终点是&，同样按照顺时针方向，保留从^到&的込的顶点。 按照顺时针方向将保留的0和込的顶点连接起来，得到了新的多边形久如图4 (b)所示。在上述求解融合点的计算步骤中，需要进行直线、射线、线段与多边形相交的判 断，相应的判断方法如下
(1)直线与多边形相交判断算法在相对坐标系中，如果多边形的各顶点A、B、C、D、E、 F的纵坐标值都是同号，即都是正值或都是负值，则直线与该多边形不相交；如果存在异号 情况，则相交。
(2)射线与多边形相交判断算法如图5所示，P1P2为一条位于平面内的射 线，P1为端点，建立如图中所示的相对坐标系，Z1J2为该平面内一多边形的两个位 于横坐标轴两侧的点，&为Z1么与横轴的交点，石的横坐标值为x3。并且，弋点不在该多边 形内，则该多边形与射线ΛΛ的位置关系可判断如下
=1 \* GB3①当^ < O时，两者不相交；
=2 \* GB3②mU3 > O时，两者相交。(3)线段与多边形相交判断算法分别以线段的两个端点为起点，按照线段的方 向建立两条射线，然后对这两条射线与多边形进行相交判断，与这两条射线都相交的多边 形与该线段相交。附图6显示了这个融合实例的结果，整个融合过程一共进行了 12步，这里仅显示 了其中4个步骤的结果。融合后，原来的障碍物用浅颜色线条的多边形来表示，说明该障碍 物已经不存在了，其外围的深颜色多边形表示融合后的障碍物。融合结束后，障碍物之间的 距离都大于5，障碍物的数量变成了 12个。将融合结束后的结果作为仿真系统中路径规划的初始状态，此时即可采用现有的 各种基于矢量法的方法进行路径规划。为了验证本发明的预处理方法的效果，采用以下方法进行进行验证经过本发明 进行预处理之后的仿真环境采用遗传算法进行路径规划，并和常用的A*算法进行比较。针 对图7所示的仿真环境进行路径规划比较。图7显示了本发明和A*算法进行路径规划的 结果比较，其中胃的值代表不同尺寸的移动主体。可以看出本发明所得的规划路径与A*算
法所得的路径大致是相同的。图8显示了本发明和A*算法在路径规划时间上的比较。其中，^是规划空间生成 的时间，对于A*算法就是规划空间的栅格化过程，对于本发明就是对障碍物进行融合预处 理所花费的时间。^是算法搜索路径的时间，ι是最终生成的路径长度。的单位是秒。 根据图8所示的实验结果，可以得到以下结论
(1)当移动主体外形尺寸较小时，规划空间中的栅格数量较多，空间栅格化以及A*算 法搜索过程花费时间都较长。而本发明算法此时没有需要融合的障碍物，也就不用运行GA 算法，因此路径搜索过程非常快，特别是当冒=2时的路径搜索时间甚至不到1毫秒，说明 了该算法的规划速度较快；
(2)当移动主体外形尺寸逐渐变大时，规划空间中的栅格数量随之减少，空间栅格化 以及A*算法搜索过程花费时间都相应得迅速减少。而本发明算法中由于出现了需要融合 的障碍物，搜索时间增加，导致本发明算法的路径搜索过程所花费时间(ff也随之增加；
(3)随着移动主体外形尺寸的增加，两种算法规划得到的路径长度都在增加，但是本 发明算法得到的路径长度绝大多数都比A*算法的小。
权利要求
一种基于矢量法的路径规划预处理方法，用于矢量法表示的仿真系统路径规划，其特征在于，包括以下步骤步骤A、将仿真环境中的每个障碍物均模型化为凸多边形；步骤B、将步骤A中得到的凸多边形中距离小于预先设定的阈值的任意两个凸多边形融合处理为一个新的凸多边形，并依次迭代处理，直到最终得到的凸多边形中任意两个的距离均大于上述预先设定的阈值；其中，所述预先设定的阈值等于路径规划中最大移动主体的尺寸；步骤C、将步骤B得到的结果作为仿真系统中路径规划的初始状态。
2.如权利要求1所述基于矢量法的路径规划预处理方法，其特征在于，步骤A中所述将 障碍物模型化为凸多边形是在欧式平面中实现。
3.如权利要求1所述基于矢量法的路径规划预处理方法，其特征在于，步骤B中所述凸 多边形之间的距离采用最小间距判断法计算。
4.如权利要求1所述基于矢量法的路径规划预处理方法，其特征在于，所述步骤B具体 包括以下各步骤步骤B1、设置变量F的初始值为步骤A得到的凸多边形的数量；步骤B2、设置变量的值为= Ij = 1;步骤B3、计算第厕个凸多边形与第个凸多边形的距离并判断该距离是否大于一个 预先设定的阈值，如是，则转步骤B5 ；如否，则转步骤B4 ；步骤B4、将第灣个凸多边形与第个凸多边形融合为一个新的凸多边形并删除原第 ■个与第ffl+1个凸多边形,将AT的值修改为后转步骤B2 ；步骤B5、判断是否潔=f,如是，则转步骤B6 ；如否，则将fff的值修改为膽+ 1后转步骤B3 ；步骤B6、判断是否》= iY,如是，则结束；如否，则将顯的值修改为H并将■的值修改为并后转步骤B3。
5.如权利要求1至4中任一项所述基于矢量法的路径规划预处理方法，其特征在于，所述两个凸多边形融合为一个新的凸多边形具体按照以下方法以其中一个多边形的中心为原点，该多边形的两个顶点的连线为纵轴，两个多边形的 中心连线为横轴，建立相对坐标系；在该相对坐标系中，在两个多边形中寻找进行纵坐标绝 对值最大的4个顶点作为一级融合点；如果一级融合点的连线与多边形相交，则在一级融 合点集合中寻找二级融合点，使之连线与多边形不相交，同时删除该二级融合点取代的一 级融合点；将现有融合点连在一起合成一个凸多边形，完成两个多边形的融合；所述连线 包括直线、射线、线段。
6.如权利要求5所述基于矢量法的路径规划预处理方法，其特征在于，进行直线、射 线、线段与凸多边形相交的判断时分别采用以下判断方法直线与多边形相交判断方法在相对坐标系中，如果多边形的各顶点的纵坐标值都是 同号，则直线与该多边形不相交；如果存在异号情况，则相交；射线与多边形相交判断方法以该射线的顶点作为原点，以该射线作为横轴，建立相对坐标系，多边形的边与相对坐标系的交点的横坐标，如果小于0，则该多边形与射线不相交； 如果大于0，该多边形与射线相交；线段与多边形相交判断方法分别以线段的两个端点为起点，按照线段的方向建立两 条射线，然后对这两条射线与多边形进行相交判断，与这两条射线都相交的多边形与该线 段相交。
全文摘要
本发明公开了一种基于矢量法的路径规划预处理方法，用于矢量法表示的仿真系统路径规划。本发明方法首先将仿真环境中的每个障碍物均模型化为凸多边形；然后将凸多边形中距离小于预先设定的阈值的任意两个凸多边形融合处理为一个新的凸多边形，并依次迭代处理，直到最终得到的凸多边形中任意两个的距离均大于上述预先设定的阈值，其中，所述阈值是根据路径规划中最大移动主体的尺寸进行设置；最后将融合的结果作为仿真系统中路径规划的初始状态。本发明针对仿真环境中的移动主体不能够穿越距离小于自身的障碍物这一事实，将距离小于移动主体尺寸的障碍物进行合并，从而降低了仿真系统中后续路径规划的工作量，提高了整个系统的实时处理能力。
文档编号G09B29/00GK101996516SQ20101055274
公开日2011年3月30日 申请日期2010年11月22日 优先权日2010年11月22日
发明者孟宪权, 赵英男 申请人:南京信息工程大学