基于启发函数的服务机器人最优路径规划方法

文档序号:10489476阅读:799来源:国知局
基于启发函数的服务机器人最优路径规划方法
【专利摘要】基于启发函数的服务机器人最优路径规划方法,涉及到移动式服务机器人导航系统中的路径规划方法技术领域。解决目前常用的最优路径规划方法效率低,搜索的准确度差的技术不足,采用改进启发函数的A?Star算法,鉴于方向角与距离量纲不同的问题,对每一个关联节点相应的距离和方向角度进行归一化处理,避免了对同一节点的重复访问,极大地缩小了搜索空间,从而缩短了算法的运行时间。本发明提出的启发函数包含了方向和距离2个因素,并且针对距离和方向两个量纲不统一的问题,提出将距离和方向归一化处理,提高了搜索效率,规划了最优路径。
【专利说明】
基于启发函数的服务机器人最优路径规划方法
技术领域
[0001] 本发明涉及到移动式服务机器人导航系统中的路径规划方法技术领域。
【背景技术】
[0002] 放眼当今世界,机器人的研究、开发与制造水平标志着一个国家或地区的综合科 技实力。荟萃着世界高、精、尖科学技术成果及理论在内的新型机器人体现了一个国家和民 族的尖端科技水平。随着制造技术、计算机技术、传感技术、信息处理技术、人工智能理论等 的不断成熟和发展,机器人研究已经发展到一个暂新的阶段。服务机器人作为一个重要分 支,形形色色不同种类的服务机器人正被研制用来满足家庭、商业、军事、国防、反恐救灾、 医疗娱乐等国民经济及国防建设各项领域的需求。
[0003] 移动性是服务机器人的核心要素之一,而路径规划技术是移动式服务机器人导航 系统中不可缺少的重要组成部分。路径规划不仅是服务机器人完成任务的安全保障,同时 也是服务机器人智能化程度的重要标志。
[0004] 路径规划中的最短路径问题属于图论范畴,遵循能量最优原则,旨在寻找图中两 结点之间的最短路径。最短路经计算分为静态最短路计算和动态最短路计算。静态最短路 径算法是外界环境不变,计算最短路径。动态最短路径算法是外界环境不断发生变化,在不 能预测的情况下计算最短路径。
[0005] 目前常用的最优路径规划方法包括有Dijkstra(迪杰斯特拉)算法、A-Star算法 等。
[0000] I、Di jkstra算法是典型的最短路径路由算法,用于计算一个节点到其他所有节点 的最短路径。主要特点是以起始点为中心向外层层扩展,直到扩展到终点为止。Dijkstra算 法能得出最短路径的最优解,但由于它遍历计算的节点很多,所以效率低。Di jkstra-般的 表述通常有两种方式,一种用永久和临时标号方式,一种是用OPEN, CLOSE表方式;大概过 程:创建两个表,Open表和Cl 〇 se表。OPEN表保存所有已生成而未考察的节点,CLOSE表中记 录已访问过的节点。1.访问路网中距离起始点最近且没有被检查过的点,把这个点放入 OPEN组中等待检查。2.从OPEN表中找出距起始点最近的点,找出这个点的所有子节点,把这 个点放到CLOSE表中。3 .遍历考察这个点的子节点。求出这些子节点距起始点的距离值,放 子节点到OPEN表中。4.重复第2和第3步,直到OPEN表为空,或找到目标点。
[0007] 2 .IA-Star 算法
[0008] A-Star算法是建立在Di jkstra算法基础上的启发式搜索算法,其主要特点在于选 择下一个路径节点时引入了已知的路网信息,特别是目标点信息,计算所有候选节点到目 标点之间的某种目标函数(比如最短距离、最小时间等),以此作为评价候选节点是否属于 最优路径节点的指标,优先选择具有最优目标函数的候选节点作为下一个路径节点。
[0009] A-Star算法的关键是确立启发函数,启发函数可表示为:
[0010] F(n) =G(n)+H(n)
[0011] 式中,G(n)为从初始节点到任意节点η的实际代价,H(n)为从节点η到目标节点最 佳路径的估计代价。
[0012] 通过控制启发函数可以对A-Star算法的行为进行控制。当H(n) =O时,只有G(n)发 挥作用,则A-Star算法退化为Di jkstra算法,能够找到最短路径但是计算量较大;H(n)不大 于从节点η到目标节点的实际代价,则A-Star能够得出最短的路径,H(n)越小,需要计算的 节点越多,运行速度越慢;当H(n)等于节点η到目标节点的实际代价时,运行速度和路径都 是最优的;当Η(η)大于节点η到目标结点的实际代价时,无法得到最短的路径。
[0013] 2.2A-Star算法的启发函数
[0014] 服务机器人的导航地图几乎都是栅格地图,在栅格地图中较为常用的启发函数有 曼哈顿距离、欧几里得距离等。
[0015] 曼哈顿距离:
[0016] H(n)= |n(x)-goal(x) | + |n(y)-goal(y)
[0017] 欧几里得距离:
[0018] H(n) =sqrt( (n(x)-goal(x) )2+(n(y)-goal (y) )2)
[0019] 对角线距离的启发函数为:
[0020] // (/;) yflll _ diagonal (?) + (// _sirai〇hi - 2 !:? // _ diagonal)
[0021 ] H_diagonal(n)=min(In(x)_goal(x)I,In(y)_goal(y)I )
[0022] H_straight(n)= In(x)_goal(x)I + In(y)_goal(y)
[0023] 传统的以欧氏距离或者曼哈顿距离为估价函数的启发值与实际的代价相差太大, 对角线距离没有考虑方向因素,导致了 A*算法搜索速度的下降。

【发明内容】

[0024] 综上所述,本发明的目的在于考虑到实际地图环境的复杂性,解决目前常用的最 优路径规划方法效率低,搜索的准确度差的技术不足,而提出基于启发函数的服务机器人 最优路径规划方法。
[0025] 为解决本发明所提出的技术问题,采用的技术方案为:基于启发函数的服务机器 人最优路径规划方法,其特征在于所述方法使用的启发函数包含了方向和距离2个因素,根 据:
[0026] /? (/?) = α, ·θ + α, D
[0027] D=max( |n(x)_goal(x) I,|n(y)_goal(y) I )
[0028] 式中,(^为方向启发因子,其决定方向信息所占的权重;Θ为候选节点与起点构成 的矢量跟终点与起点构成的矢量之间的夹角,其反映了搜索的方向是否指向终点,Θ值越 小,搜索越朝着终点进行;α 2为距离启发因子,其决定距离信息所占的权重;D为候选节点与 终点间的对角距离;
[0029]对每一个关联节点相应的距离和方向角度进行归一化处理,改进后的启发函数可 表示夕
[0030]
[0031]
[0032]
[0033] 式中,j为关联节点的个数;歹即先计算当前节点所有关联节点相应的方向角度值, 然后求平均值;5即先计算当前节点所有关联节点相应的对角线距离值,然后求平均值; [0034]所述方法实施步骤如下:
[0035] 1)初始化设置;构造两个链表,分别存储待扩展的节点和已扩展的节点,分别称为 Open表和Close表,将起点信息加载到Open表中,Close表赋值为空,令G(n)=0;
[0036] 2)搜索起点周围的8个节点,求"(1)的最小值;将节点1从Open表中删除并加载到 Close表中;
[0037] 3)搜索离当前节点最近的节点,求F(n),(η>1)的最小值,将节点η从Open表中删除 并加载到Close表中;判断节点η是否为终点,如果是,终点转向步骤5;否则转向步骤4;
[0038] 4)判断节点η的节点信息是否在确定的范围内,如果在范围内,则扩展节点η;否则 加载节点η的节点信息并进行扩展,转向步骤3;由于启发函数包含了方向与对角线距离信 息,搜索效率更高;
[0039] 5)从节点η开始,利用回溯的方法输出起始节点到目标节点的最优路径,以及最短 距离,算法终止。
[0040] 本发明的有益效果为:本发明在路径规划运行过程中,避免了对同一节点的重复 访问,极大地缩小了搜索空间,从而缩短了算法的运行时间。A-Star算法作为一种启发式路 径规划搜索算法,它利用启发函数减小搜索空间,从而提高搜索效率,因此启发函数在A-Star算法中起着关键的作用。本专利提出的启发函数包含了方向和距离2个因素,并且针对 距离和方向两个量纲不统一的问题,提出将距离和方向归一化处理,提高了搜索效率,规划 了最优路径。
【附图说明】
[0041]图1为本发明具体案例不意图一;
[0042]图2为本发明具体案例示意图二;
[0043]图3为本发明具体案例示意图三;
[0044] 图4为本发明具体案例示意图四;
[0045] 图5为本发明具体案例示意图五。
【具体实施方式】
[0046] 以下结合附图对本发明的路径规划方法作进一步地说明。
[0047]本发明采用改进启发函数的A-Star算法
[0048]考虑到实际地图环境的复杂性,本发明所使用的启发函数包含了方向和距离2个 因素:
[0049] //(/7) = (2, θλ-α, D
[0050] D=max( In(x)_goal(x) I,In(y)_goal(y) I )
[0051] 式中,(^为方向启发因子,其决定方向信息所占的权重;Θ为候选节点与起点构成 的矢量跟终点与起点构成的矢量之间的夹角,其反映了搜索的方向是否指向终点,Θ值越 小,搜索越朝着终点进行;Ct2为距离启发因子,其决定距离信息所占的权重;D为候选节点与 终点间的对角距离。这样在搜索下一节点时,既考虑了该点的方位信息,又兼顾了距离信 息,加快了搜索的过程,提高了搜索的准确度。方向角与距离的单位存在不统一,即使角度 的单位为弧度、距离的单位为千米,它们之间也很有可能出现距离值远大于角度值的情况, 特别是在大区域路径规划过程中,问题更明显。当D0时,方向不再有约束力,而使得启发函 数失去意义。
[0052]鉴于方向角与距离量纲不同的问题,对每一个关联节点相应的距离和方向角度进 行归一化处理,改进后的启发函数可表示为:
[0053]
[0054]
[0055]
[0056] 式中,j为关联节点的个数;歹即先计算当前节点所有关联节点相应的方向角度 值,然后求平均值;S即先计算当前节点所有关联节点相应的对角线距离值,然后求平均 值。
[0057]本发明方法实施步骤如下:
[0058] 1)初始化设置;构造两个链表,分别存储待扩展的节点和已扩展的节点,分别称为 Open表和Close表,将起点信息加载到Open表中,Close表赋值为空,令G(n)=0;
[0059] 2)搜索起点周围的8个节点,求/)⑴的最小值;将节点1从Open表中删除并加载到 Close表中;
[0000] 3)搜索离当前节点最近的节点,求F(n),(η>1)的最小值,将节点η从Open表中删除 并加载到Close表中;判断节点η是否为终点,如果是,终点转向步骤5;否则转向步骤4;
[0061] 4)判断节点η的节点信息是否在确定的范围内,如果在范围内,则扩展节点η;否则 加载节点η的节点信息并进行扩展,转向步骤3;由于启发函数包含了方向与对角线距离信 息,搜索效率更高;
[0062] 5)从节点η开始,利用回溯的方法输出起始节点到目标节点的最优路径,以及最短 距离,算法终止。
[0063] 路径规划运行过程中,避免了对同一节点的重复访问,极大地缩小了搜索空间,从 而缩短了算法的运行时间。A-Star算法作为一种启发式路径规划搜索算法,它利用启发函 数减小搜索空间,从而提高搜索效率,因此启发函数在A-Star算法中起着关键的作用。本专 利提出的启发函数包含了方向和距离2个因素,并且针对距离和方向两个量纲不统一的问 题,提出将距离和方向归一化处理,提高了搜索效率,规划了最优路径。
[0064] 以下结合附图和具体实施例对发明作进一步地说明:
[0065] 参照图1中所示,以10*10的栅格地图XY中,假设中间灰色区域为障碍物,AO为起 点,TO为终点。〇1= 1,α2 = 1。每个方格的边长为1。本发明的路径规划方法步骤如下:
[0066] 1)如图 2 所示,将 AO 放到 Open 表中;Close 表为 O;0(〇)=〇,D(O)=4,G(1)=O
[0067]
[0068]
[0069]
[0070]
[0071]
[0072]
[0073]
[0074]
[0075] 最小的H(I)为Al,F(1)最小的也是Al。将节点Al从Open表中删除并加载到Close表 中。
[0076] 2)如图3中所示,搜索Al周围的节点,G(J) = VI,由于有障碍物与起点,所以需计 算的节点为:
[0077]
[0078]
[0079]
[0080]
[0081]
[0082] 最小的H(2)为BI,F(2)最小的也是Bl。将节点Bl从Open表中删除并加载到Close表 中。
[0083] 3)如图4中所示,搜索Bl周围的节点,G⑶=2七;由于有障碍物与Close节点,所 以需计算的节点为:
[0084]
[0085]
[0086]
[0087]
[0088]
[0089]
[0090] 最小的H(3)为C2,F(3)最小的也是C2。将节点C2从Open表中删除并加载到Close表 中。
[0091] 4)如图5中所示,搜索C2周围的节点,(7(4) = 2、/? + Ι,由于周围有终点,搜索结束。
[0092] 5)最终得出的路径规划为:AO-Al -BI-C2-T0。
【主权项】
1.基于启发函数的服务机器人最优路径规划方法,其特征在于所述方法使用的启发函 数包含了方向和距离2个因素,根据: '//('?? = α、'θ + α:.。 D=max( |n(x)-goal(x) | , |n(y)-goal(y) | ) 式中,αι为方向启发因子,其决定方向信息所占的权重;θ为候选节点与起点构成的矢量 跟终点与起点构成的矢量之间的夹角,其反映了捜索的方向是否指向终点,Θ值越小,捜索 越朝着终点进行;02为距离启发因子,其决定距离信息所占的权重;D为候选节点与终点间 的对角距离; 对每一个关联节点相应的距离和方向角度进行归一化处理,改进后的启发函数可表示 为:式中,j为关联节点的个数;f即先计算当前节点所有关联节点相应的方向角度值,然后 求平均值;忍即先计算当前节点所有关联节点相应的对角线距离值,然后求平均值; 所述方法实施步骤如下: 1) 初始化设置;构造两个链表,分别存储待扩展的节点和已扩展的节点,分别称为化en 表和Close表,将起点信息加载到化en表中,Close表赋值为空,令G(n)=0; 2) 捜索起点周围的8个节点,求房的最小值;将节点1从化en表中删除并加载到Close 表中; 3) 捜索离当前节点最近的节点,求F(n),(n〉l)的最小值,将节点η从化en表中删除并加 载到Close表中;判断节点η是否为终点,如果是,终点转向步骤5;否则转向步骤4; 4) 判断节点η的节点信息是否在确定的范围内,如果在范围内,则扩展节点η;否则加载 节点η的节点信息并进行扩展,转向步骤3; 5) 从节点η开始,利用回溯的方法输出起始节点到目标节点的最优路径,W及最短距 离,算法终止。
【文档编号】G06Q10/04GK105844364SQ201610216367
【公开日】2016年8月10日
【申请日】2016年4月8日
【发明人】栾毓敏
【申请人】上海派毅智能科技有限公司
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1