本发明公开了一种基于物理建模的机器人避障路径规划方法，步骤如下：设立机器人工作区域的引力场栅格和距离信息栅格，建立机器人双重栅格信息图；基于上述双重栅格信息图，采用有向遍历法搜索所有可行路径，计算出引力值和距离值的综合评价值，取最大值所对应的路径方案即为机器人最优避障路径规划方案。该方法克服了机器人路径规划中对运动物体和障碍物几何属性不作考虑的缺点，该方法建立双重栅格后，进行路径搜索时，根据双重栅格的值进行机器人避障路径规划，兼顾了路径最短和运动安全的问题，提高了路径规划的效率，降低在进行路径寻优中可能发生的损害事故。
1.一种基于物理建模的机器人避障路径规划方法，该方法包括步骤如下：（1）、设立机器人工作区域的距离信息栅格和引力信息栅格，建立机器人工作环境的双重栅格信息图，其具体如下：（1-1）、初始化栅格 ，初始化距离信息
设立机器人工作区域的距离信息栅格，将机器人工作区域进行二维栅格化描述，将机器人不能通行的栅格标记为障碍栅格，将机器人能通行栅格标记为可行栅格，在栅格图上，有障碍物的栅格或障碍物未完全占满的栅格为障碍栅格；无障碍物的的栅格为可行域，对机器人工作区域的栅格进行编号 ，其中 表示栅格在 方向上的坐标， 分别表示 方向上的栅格总数目，设定机器人有八个运动方向， 的栅格为起始栅格， 的栅格为目标栅格，为避免反向搜索采用起始栅格到目标栅格的有向搜索，相邻栅格距离为1，斜向点接栅格距离为，如果不计是否穿越障碍，起始栅格和目标栅格直线距离为最短距离，最短距离计算公式为： （1-2）、初始化栅格引力场信息 ，建立双重栅格信息图设立机器人工作区域的引力信息栅格，在步骤（1-1）中已完成编号的栅格图基础之上，对所有可行域栅格赋予引力值，计算出每一个可行域栅格的引力值大小，该引力值由引力场函数设定，引力场函数计算公式为：；
建立机器人双重栅格信息图，将上述引力信息栅格和距离信息栅格绘制在栅格图上，即将栅格图 中的每一个栅格同时赋予距离值和引力值从而完成的栅格图，该栅格图称为双重栅格信息图；
（2）、基于上述双重栅格信息图的机器人避障路径规划，其具体步骤如下：（2-1）、确定机器人初始位置，启动路径搜索：确定机器人初始位置和状态，获取机器人在双重栅格信息图中的初始点，然后启动有向遍历式路径搜索；
从初始点出发，沿 轴正向搜索路径；判断搜索出来的路径方案的节点组合是否已经存在于禁忌数组 中机器人由初始点按照目标点所在位置设定行进方向为沿轴的正向，机器人规避禁止在 的栅格搜索，其中， 表示栅格的引力值，的栅格为障碍栅格，为避免重复无效搜索，按照根部求异法进行搜索，即搜索过程中先设 值从1逐步变化到 ，然后 值从1逐步变化到 ，……直到一次搜索结束，搜索过程中根据禁忌数组 中的路径方案，找出符合以下条件的路径方案：中的
路径方案总数目，路径方案 即为第i种路径方案，其中的 分别表示利用根部求异法进行路径搜索时第一个发生变化的栅格的坐标；
从初始点到终点遍历路径方案 中的路径节点，计算路径方案 的距离值，其计算公式为：
其中， 表示第 种路径方案的距离值，定义相邻栅格间的距离为1，斜向点接栅格间距离为 ； 表示第i种路径所遍历的总栅格数目， 示纵向和横向移动的栅格数；
（2-4）、计算路径方案i的引力值 ，计算路径方案 的引力值 ，其计算式为：其中， 表示第i种路径方案的引力值； 表示栅格的引力值； 表示栅格的坐标； 表示第i种路径所遍历的总栅格数目；
计算路径方案i的距离值和引力值的综合评价值 ，其计算式为：其中， 表示第i种路径方案的综合评价值；表示引力值权重， 表示距离值权重，且满足 ； 表示最短距离；
（2-6）、记录距离值 、引力值 和综合评价值 ；与节点信息 一并存入禁忌数组 中； 计算数据录入，记录第i种路径节点组合 、以及距离值 、引力值 、引力值和距离值的综合评价值 到禁忌数组 中，记录完成后 ，表示第i种路径方案记录完毕，i自增1，转步骤（2-7）；
（2-7）判断是否满足 ，判断是否已搜索出所有路径，如果 中的 值和 值分别满足 ，则表示已搜寻出所有路径，转步骤（2-8），如果 中的值和 值没有满足 ，则机器人没有搜寻出所有路径，则转步骤（2-2）；
（2-8）、计算 ，调取 中的路径信息，搜索出所有路径后，比较禁忌数组中全部路径方案的引力值和距离值的综合评价值 ，计算全部路径方案综合评价值的最大值 ，调取 为最大值时所对应的路径方案的信息；其中 的计算公式为：
（2-9）、输出 为最大值时所对应的路径方案的信息，避障路径规划结束。
一种基于物理建模的机器人避障路径规划方法
技术领域
[0001] 本发明涉及机器人避障路径规划技术，本方法基于物理建模进行机器人避障路径规划，适用于实体机器人最优避障路径规划，也可以用于虚拟机器人避障路径规划。
背景技术
[0002] 机器人避障路径规划是指，在给定的环境障碍条件下，选择一条从起始点到目标点的路径，使机器人可以安全、无碰撞地通过所有的障碍，这种自主地躲避障碍物并完成作业任务的方法，是机器人研究和应用中的一个重要内容。
[0003] 2001年禹建丽等在洛阳工学院学报上发表了一篇题为一种基于神经网络的机器人路径规划算法的文章，提出了一种基于神经网络的机器人路径规划方法,研究了障碍物形状和位置已知情况下的机器人路径规划方法,利用神经网络结构进行能量函数的定义,根据路径点位于障碍物内外不同位置选取不同的动态运动方程,规划出的路径为折线形的最短无碰路径,计算简单,收敛速度快。Lazona-Perze提出了基于C空间的自由空间法，C空间法又称可视化空间法，将障碍物映射到C空间，形成的障碍空间的补集即为自由空间，将起始点和终点扩充到集合中，然后连接起始点、终点和所有障碍物顶点，要求连线不能穿越障碍物，然后应用启发搜索算法，搜索距离最短路径即为最优路径。这两种方法都属于按照逻辑拓扑关系来建模进行路径规划的方法，规划的路径和机器人之间缺少缓冲地带，规划结果所得的最优路径往往和障碍物紧挨，考虑到机器人运动时由于震动等导致的摆动，这样规划出来的路径对于机器人的运动是很危险的，当机器人在按照规划的路径运动时，如果发生摆动，可能导致运动物体和路径旁障碍物发生碰撞，引发安全问题。
[0004] 人工势场法是为数不多的考虑了安全问题的机器人避障路径规划方法，人工势场法是由Khatib提出的一种虚拟力法，应用于机器人路径规划就是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动，应用势场法规划出来的路径一般是比较平滑并且安全的，但是这种方法存在局部最优，即容易出现局部收敛的问题；而且当两个障碍物位置比较接近时，根据人工势场法规则，它们之间的通道是不能通过的，因而此时利用人工势场法进行路径规划要么由于障碍物过近导致规划失败，要么就要沿障碍物外围绕远，导致规划出来的路径过长。此外，人工势场法规划出来的路径多为不规则曲线不符合机器人运动习惯。
发明内容
[0005] 为了解决以上现有技术存在的技术问题，本发明的目的是提出了一种基于物理建模的机器人避障路径规划方法，该方法在复杂的连续动态环境中获得相对安全且路程较短的路径，减少在进行路径寻优中可能发生的损害事故，提高工作效率。
[0006] 为了达到上述目的，本发明的一种基于物理建模的机器人避障路径规划方法，包括步骤如下：
[0007] （1）、设立机器人工作区域的距离信息栅格和引力信息栅格，建立机器人工作环境的双重栅格信息图，其具体如下：
[0008] （1-1）、初始化栅格 ，初始化距离信息
[0009] 设立机器人工作区域的距离信息栅格，将机器人工作区域进行二维栅格化描述，将机器人不能通行的栅格标记为障碍栅格，将机器人能通行栅格标记为可行栅格，在栅格图上，有障碍物的栅格或障碍物未完全占满的栅格为障碍栅格；无障碍物的的栅格为可行域，对机器人工作区域的栅格进行编号，其中 表示栅格在 方向上的坐标， 分别表示 方向上的栅格总数目，设定机器人有八个运动方向，的栅格为起始栅格， 的栅格为目标栅格，为避免反向搜索采用起始栅格到目标栅格的有向搜索，相邻栅格距离为1，斜向点接栅格距离为 ，如果不计是否穿越障碍，起始栅格和目标栅格直线距离为最短距离，最短距离计算公式为：
[0010]；
[0011] （1-2）、初始化栅格引力场信息 ，建立双重栅格信息图
[0012] 设立机器人工作区域的引力信息栅格，在步骤（1-1）中已完成编号的栅格图基础之上，对所有可行域栅格赋予引力值，计算出每一个可行域栅格的引力值大小，该引力值由引力场函数设定，引力场函数计算公式为：
[0013]；
[0014] 建立机器人双重栅格信息图，将上述引力信息栅格和距离信息栅格绘制在栅格图上，即将栅格图 中的每一个栅格同时赋予距离值和引力值从而完成的栅格图，该栅格图称为双重栅格信息图；
[0015] （2）、基于上述双重栅格信息图的机器人避障路径规划，其具体步骤如下：
[0016] （2-1）、确定机器人初始位置，启动路径搜索
[0017] 确定机器人初始位置和状态，获取机器人在双重栅格信息图中的初始点，然后启动有向遍历式路径搜索；
[0018] （2-2）、搜索出一条机器人未走过的路径：从初始点出发，沿 轴正向搜索路径；判断搜索出来的路径方案的节点组合是否已经存在于禁忌数组 中机器人由初始点按照目标点所在位置设定行进方向为沿 轴的正向，机器人规避禁止在 的栅格搜索，其中， 表示栅格的引力值， 的栅格为障碍栅格，为避免重复无效搜索，按照根部求异法进行搜索，即搜索过程中先设 值从1逐步变化到 ，然后值从1逐步变化到 ，……直到 一次搜索结束，搜索过程中根据禁忌数组 中的路径方案，找出符合以下条件的路径方案：
[0019]中的路径方案总数目，路径方案 即为第i种路径方案，其中的 分别表示利用根部求异法进行路径搜索时第一个发生变化的栅格的坐标；
[0020] （2-3）、计算路径方案i的距离值
[0021] 从初始点到终点遍历路径方案 中的路径节点，计算路径方案i的距离值，其计算公式为：
[0022]
[0023] 其中， 表示第 种路径方案的距离值，定义相邻栅格间的距离为1，斜向点接栅格间距离为 ； 表示第i种路径所遍历的总栅格数目， 示纵向和横向移动的栅格数；
[0024] （2-4）、计算路径方案i的引力值
[0025] 计算路径方案i的引力值 ，其计算式为：
[0026]
[0027] 其中， 表示第i种路径方案的引力值； 表示栅格的引力值； 表示栅格的坐标； 表示第i种路径所遍历的总栅格数目；
[0028] （2-5）、计算路径方案i的距离值
[0029] 计算路径方案i的距离值和引力值的综合评价值 ，其计算式为：
[0030]
[0031] 其中， 表示第i种路径方案的综合评价值；表示引力值权重， 表示距离值权重，且满足 ； 表示最短距离；
[0032] （2-6）、记录距离值 、引力值 、综合评价值 ；与节点信息 一并存入禁忌数组 中；
[0033] 计算数据录入，记录第i种路径节点组合 、以及距离值 、引力值 、引力值和距离值的综合评价值 到禁忌数组 中，记录完成后 ， 表示第i种路径方案记录完毕，i自增1，转步骤（2-7）；
[0034] （2-7）判断是否满足
[0035] 判断是否已搜索出所有路径，如果 中的 值和 值分别满足，则表示已搜寻出所有路径，转步骤（2-8），如果 中的 值和 值没有满足，则机器人没有搜寻出所有路径，则转步骤（2-2）；
[0036] （2-8）、计算 ，调取 中的路径信息
[0037] 搜索出所有路径后，比较禁忌数组 中全部路径方案的引力值和距离值的综合评价值 ，计算全部路径方案综合评价值的最大值 ，调取 为最大值时所对应的路径方案的信息；其中 的计算公式为：
[0038]
[0039] 其中， 为禁忌数组 中每种路径方案的综合评价值；
[0040] （2-9）、输出 为最大值时所对应的路径方案的信息，避障路径规划结束。
[0041] 本发明的一种基于物理建模的机器人避障路径规划方法与现有技术相比较具有如下显著优点：该方法将距离信息和运动物体及工作区域内的障碍物的引力信息纳入双重栅格进行建模，克服了机器人路径规划中对运动物体和障碍物几何属性不作考虑的缺点，该方法进行路径搜索时，根据双重栅格中栅格的距离值和引力值进行机器人避障路径规划兼顾了路径最短和运动安全的问题，提高了路径规划的效率，降低在进行路径寻优中可能发生的损害事故。
附图说明
[0042] 图1是本发明的一种基于物理建模的机器人避障路径规划方法的流程图；
[0043] 图2是本发明的一种基于物理建模的机器人避障路径规划方法中所述的机器人双重栅格信息图。
具体实施方式
[0044] 以下结合附图对本发明的实施例作进一步的详细描述。
[0045] 针对目前的机器人避障路径规划中对机器人本身和障碍物之间的安全间隙考虑不足的问题，本发明将距离信息和运动物体及工作区域内的障碍物的引力信息纳入双重栅格信息图，使规划的路径更具实用性，且简单、直观、易实现。
[0046] 如图1所示，本发明的一种基于物理建模的机器人避障路径规划方法，其步骤如下：
[0047] （1）、设立机器人工作区域的引力信息栅格和距离信息栅格，建立机器人双重栅格信息图，如图2所示，其具体如下；
[0048] （1-1）、初始化栅格 ，初始化距离信息，绘制栅格地图，设立机器人工作区域的距离信息栅格，其具体如下；
[0049] 设立机器人工作区域的距离信息栅格，将机器人工作区域进行二维栅格化描述，将机器人不能通行的栅格标记为障碍栅格，将机器人能通行栅格标记为可行栅格，如图1所示，在栅格图上，栅格11和21位置上的是移动机器人小车，五个圆桶和“L”形传送带，以及梯形储藏柜表示障碍物，它们所处栅格为障碍栅格，有障碍物的栅格或障碍物未完全占满的栅格为障碍栅格；无障碍物的的栅格为可行栅格；对机器人工作区域的栅格进行编号 ，表示每个栅格的序号，其中 ， 表示栅格在 方向上的坐标，图2中的编号为11的栅格为起始栅格，编号为2020的栅格为目标栅格；设定机器人有八个运动方向，为避免逆向搜索，采用起始点到终止点的有向搜索，对于图1所示的栅格为沿 轴的正方向，相邻栅格间距离为1，斜向点接栅格间距离为 ，如果不计是否穿越障碍，起始栅格和目标栅格的直线距离为机器人运动的最短距离，最短距离计算公式为：
[0050]；
[0051] （1-2）、初始化栅格引力场信息 ，建立双重栅格信息图，其具体如下；
[0052] 设立利用万有引力的势场法来绘制的机器人工作区域栅格，该工作区域栅格称为引力信息栅格，引力信息栅格中的运动物体和可供物体运动的空间信息通过引力值来表达，引力信息栅格中障碍物所在的栅格的引力值为0，引力信息栅格中可行域栅格的引力值为[0～1]区间的某一数值，该数值由引力场函数设定，引力场函数计算式为：
[0053]；
[0054] 由于 的大小要保证 ，如图1所示，设定 ；
[0055] 建立机器人双重栅格信息图，将上述引力场栅格和距离信息栅格绘制在栅格图上，即将栅格图 中的每一个栅格同时赋予距离值和引力值从而完成的栅格图，该栅格图称为双重栅格信息图；
[0056] （2）、基于上述双重栅格信息图的机器人避障路径规划，其具体步骤如下：
[0057] （2-1） 确定机器人初始位置，启动路径搜索
[0058] 确定机器人初始位置和状态，获取机器人在双重栅格信息图中的初始点；然后启动有向遍历式路径搜索；
[0059] （2-2）、搜索出一条机器人未走过的路径：从初始点出发，沿 轴正向搜索路径；判断搜索出来的路径方案的节点组合是否已经存在于禁忌数组 中
[0060] 机器人由初始点按照目标点所在位置设定行进方向为沿 轴的正向，机器人规避禁止在 的栅格搜索，其中， 表示栅格的引力值， 的栅格为障碍栅格，为避免重复无效搜索，按照根部求异法进行搜索，即搜索过程中先设 值从1逐步变化到20，然后 值从1逐步变化到20，……直到 一次搜索结束，搜索过程中根据禁忌数组 中的路径方案，找出符合以下条件的路径方案：
[0061]中的路径方案总数目，路径方案 即为第i种路径方案，其中的 分别表示利用根部求异法进行路径搜索时第一个发生变化的栅格的坐标；
[0062] （2-3）、计算路径方案i的距离值
[0063] 从初始点到终点遍历路径方案 中的路径节点，计算路径方案i的距离值，其计算式为：
[0064]
[0065] 其中， 表示第 种路径方案的距离值，定义边相邻栅格间的距离为1，斜向点接栅格间距离为 ； 表示第i种路径所遍历的总栅格数目，示纵向和横向移动的栅格数；
[0066] （2-4）、计算路径方案第i的引力值
[0067] 计算路径方案i的引力值 ，其计算式为：
[0068]
[0069] 其中， 表示第i种路径方案的引力值； 表示栅格的引力值； 表示栅格的坐标； 表示第i种路径所遍历的总栅格数目；
[0070] （2-5）、计算路径方案第i的综合评价值
[0071] 计算路径方案i的距离值和引力值的综合评价值 ，其计算式为：
[0072]
[0073] 其中， 表示第i种路径方案的综合评价值；表示引力值权重， 表示距离值权重，且满足 ； 表示最短距离，如图1所示，计算最短距离为：
[0074] ；
[0075] （2-6）、记录距离值 、引力值 、综合评价值 ；与节点信息 一并存入禁忌数组 中； 计算数据录入，记录第i种路径节点组合 、以及引力值、距离值 、引力值和距离值的综合评价值 到禁忌数组 中，记录完成后， 表示第i种路径方案记录完毕，i自增1，转步骤（2-7）；
[0076] （2-7）、判断是否满足
[0077] 判断是否已搜索出所有路径，如果 中的 值和 值分别满足，则表示已搜寻出所有路径，转步骤（2-8），如果 中的 值和 值没有
满足 ，则机器人没有搜寻出所有路径，则转步骤（2-2）；
[0078] （2-8）、计算 ，调取 中的路径信息
[0079] 搜索出所有路径后，比较禁忌数组 中全部路径方案的引力值和距离值的综合评价值 ，计算全部路径方案综合评价值的最大值 ，调取 为最大值时所对应的路径方案的信息；其中 的计算公式为：
[0080]
[0081] 其中， 为禁忌数组 中每种路径方案的综合评价值；
[0082] （2-9）、输出 为最大值时所对应的路径方案的信息，避障路径规划结束。
[0083] 对于本领域的普通技术人员来说可显而易见的得出其他的优点和修改。因此，本发明具有更广的应用，而不局限于这里所示的并且描述的具体的实施例。因此在不脱离随后权利要求及其等价体所定义的一般发明构思的精神和范围的情况下，可对其作出各种修改。
