基于障碍物虚拟膨胀的伴随机器人路径规划方法及系统技术方案

本发明专利技术公开了基于障碍物虚拟膨胀的伴随机器人路径规划方法及系统，包括：构建环境地图：根据实际场景构建二维占据栅格地图，将每一个栅格标记为障碍区域或可行区域；在栅格地图中设定伴随机器人和可移动目标点的初始坐标位置；为机器人建立滑动窗口；对障碍区域进行膨胀处理：按照机器人中心与机体边缘的最短距离对障碍物所处的栅格进行初始膨胀，在最小不可通行区域基础上确定出外扩的栅格层数，将外扩后区域内的栅格作为障碍物虚拟膨胀栅格标记障碍物虚拟膨胀栅格受障碍物影响的危险等级；基于A*算法和增量路径规划方法对伴随机器人的路径进行规划。采用上一时刻路径进行增量式路径更新，节省路径规划的时间，提升伴随机器人的反应速度。

Path planning method and system for adjoint robot based on virtual expansion of obstacles

The invention discloses a path planning method and system for a companion random robot based on virtual expansion of obstacles, including: constructing an environmental map: constructing a two-dimensional occupied grid map according to the actual scene, marking each grid as an obstacle area or a feasible area; setting the companion random robot and a movable target point in the grid map at the beginning. Initial coordinate position; Sliding window for robot; Expansion of obstacle area: According to the shortest distance between the center of robot and the edge of body, initial expansion of the barrier grid is carried out, and the number of grid layers is determined on the basis of the smallest impassable area, and the grid in the extended area is regarded as an obstacle. Object virtual expansion grids mark the danger level of obstacle virtual expansion grids affected by obstacles; Based on the A* algorithm and incremental path planning method, path planning of the robot is carried out. Incremental path updating based on the previous time path saves the time of path planning and improves the response speed of the robot.

【技术实现步骤摘要】
基于障碍物虚拟膨胀的伴随机器人路径规划方法及系统
本专利技术涉及机器人
，特别是涉及基于障碍物虚拟膨胀的伴随机器人路径规划方法及系统。
技术介绍
目前机器人领域普遍采用基于栅格的路径规划算法，该种规划方法将环境用规则化的栅格进行表示，并把路径规划转化为对联通栅格的搜索问题。A*算法是该类算法中应用最广泛的一种，它可以搜索出连接起始点与目标点的距离最优路径。但基于栅格的环境描述方法往往将机器人等同为质点，并将障碍物按照特定尺寸进行膨胀来满足机器人的避障需要。该种处理方式较适用于圆形或方形机器人，因为可以选择机器人的半径或机器人中心与机体边缘的最大距离作为障碍物膨胀半径。然而外形为长方形的机器人，如四足机器人，其障碍物的膨胀半径较难选择，如若选择机器人中心与边缘的最小距离作为膨胀半径则容易让机器人与障碍距离过近从而引发发生碰撞，然而如果选择最大距离又容易堵塞狭窄通道。同时，A*算法是一种静态搜索算法，这意味着当路径重规划时该算法不得不从起点开始重新规划。特别是在机器人跟随领航员进行伴随行走条件下，路径规划相邻时刻的机器人及目标点所处的位置变化很小，因而其所处的环境信息很难发生剧烈的变化。所以在路径更新时若机器人每次都从起点对路径进行重规划则会使得新旧路径具有较多的重复部分，从而导致路径搜索效率的下降，增加机器人自身的能耗损失。
技术实现思路
为了解决现有技术的不足，本专利技术提供了基于障碍物虚拟膨胀的伴随机器人路径规划方法及系统，尤其适用于以四足机器人为载体，以跟随行走为运动方式时的特殊应用环境。为了解决上述技术问题，本专利技术采用如下技术方案：作为本专利技术的第一方面，提供了基于障碍物虚拟膨胀的伴随机器人路径规划方法；基于障碍物虚拟膨胀的伴随机器人路径规划方法，包括：构建环境地图：根据实际场景构建二维占据栅格地图，将每一个栅格标记为障碍区域或可行区域；伴随机器人以及可移动目标点的初始化：在栅格地图中设定伴随机器人和可移动目标点的初始坐标位置；为机器人建立滑动窗口；对障碍区域进行膨胀处理：按照机器人中心与机体边缘的最短距离对障碍物所处的栅格进行初始膨胀，障碍物与初始膨胀后的栅格共同构成最小不可通过区域；依据机器人实际运行时所需的安全距离，在最小不可通行区域基础上确定出外扩的栅格层数，将外扩后区域内的栅格作为障碍物虚拟膨胀栅格，标记障碍物虚拟膨胀栅格受障碍物影响的的危险等级；伴随机器人的路径规划步骤：步骤(1)：伴随机器人将可移动目标点的当前位置在滑动窗口中的投影点作为新目标点，基于栅格的通行代价值，伴随机器人按照A*算法进行路径搜索得到伴随机器人与新目标点之间的最优路径；伴随机器人根据最优路径进行行走；滑动窗口以伴随机器人为基准，随着伴随机器人的移动而移动；步骤(2)：按照设定时间间隔，对滑动窗口内的栅格地图进行更新，对可移动目标点的当前位置在滑动窗口中的投影点进行更新，将最优路径在滑动窗口内进行平移；步骤(3)：伴随机器人判断更新后的滑动窗口内的栅格地图是否存在最小不可通过区域阻断路径；如果存在，就返回步骤(1)；如果不存在，就进入步骤(4)；步骤(4)：基于栅格的通行代价值，按照增量规划路径方式，对伴随机器人平移后在滑动窗口内剩余路径点中通行代价值最小的点与更新的投影点之间的路径进行重新规划，找到新的最优路径，将新的最优路径与伴随机器人当前路径进行连接形成新的推荐路径；伴随机器人按照新的推荐路径进行行走；返回步骤(2)。进一步的，所述滑动窗口的构建过程，包括：在栅格地图中以伴随机器人当前位置为原点建立二维坐标平面；在二维坐标平面中选择2N*2N的栅格区域为滑动窗口；所述滑动窗口在二维坐标平面第一象限的栅格区域为N*2N；所述滑动窗口在二维坐标平面第二象限的栅格区域为N*2N；所述滑动窗口的底边经过原点；N表示栅格个数。进一步的，栅格的通行代价值的具体计算公式为：式中，g(s)表示从起点到栅格s的通行代价值，g(s-1)表示从起点到栅格s-1的通行代价值，c(s-1,s)表示栅格s-1到栅格s的通行代价值，d(s)表示虚拟膨胀栅格s受障碍物影响的危险等级，d(s-1)表示虚拟膨胀栅格s-1受障碍物影响的危险等级，α表示路径远离障碍物一个栅格条件下所需的额外代价。障碍物虚拟膨胀栅格属于可行区域。障碍物虚拟膨胀栅格受障碍物影响的危险等级等于外扩的栅格层数减去当前栅格与障碍物虚拟膨胀栅格的距离之后再加1。进一步的，路径远离障碍物一个栅格条件下所需的额外代价大于相邻栅格距离值的最小增量与栅格边长之和。进一步的，所述步骤(1)伴随机器人将可移动目标点的当前位置在滑动窗口中的投影点作为新目标点，基于栅格的通行代价值，伴随机器人按照A*算法进行路径搜索得到伴随机器人与新目标点之间的最优路径的具体步骤为：当可移动目标点在滑动窗口内，以伴随机器人当前位置为起点，以可移动目标点当前位置为终点，依据栅格的通行代价值，采用A*算法进行计算，得到最优路径；当可移动目标点在滑动窗口外，且伴随机器人与可移动目标点的连线与滑动窗口的交点处没有障碍物时；以伴随机器人当前位置为起点，以可移动目标点在滑动窗口中的投影坐标为终点，依据栅格的通行代价值，采用A*算法进行计算，得到最优路径；当可移动目标点在滑动窗口外，且伴随机器人与可移动目标点的连线与滑动窗口的交点处有障碍物时；以该交点为中心分别向滑动窗两侧进行寻找直到找到非障碍点；将找到的非障碍点为终点；以伴随机器人当前位置为起点，依据栅格的通行代价值，采用A*算法进行计算，得到最优路径。进一步的，所述步骤(2)的具体内容为：对滑动窗口内的障碍物位置进行更新，对滑动窗口内可移动目标点的投影位置进行更新。按照伴随机器人位置变化对已规划的最优路径进行平移操作，删除移出滑动窗边界的路径点，保留滑动窗内的路径点，记为r。进一步的，所述步骤(4)中，基于栅格的通行代价值，按照增量规划路径方式对伴随机器人平移后在滑动窗口内剩余路径点中代价值最小的点与更新的投影点之间的路径进行重新规划，找到新的最优路径的具体步骤为：伴随机器人更新步骤(2)中平移后滑动窗口内剩余路径点的评价函数：g'(r)＝g(r)-g(rb)+d(rb)*α(2)h'(r)＝|xt-xr|+|yt-yr|(3)f'(r)＝g'(r)+h'(r)(4)其中，rb为平移后路径r的起始点，g(r)表示上次规划从起始点到路径r的路径点的代价值,g(rb)为上次规划从起始点到路径点rb的代价值，d(rb)表示路径起点rb受障碍物影响的危险等级；g'(r)为本次更新后从起始点rb到路径r的路径点的代价值；xt表示当前目标投影点的在栅格地图中的x坐标，yt表示当前目标投影点的在栅格地图中的y坐标，xr为路径r的路径点的x坐标，yr为路径r的路径点的y坐标；h'(r)为本次更新后路径r的路径点到目标投影点的代价值，f'(r)为本次更新后从起始点rb经由路径r到目标投影点的代价值；从路径点r的最后一个点开始，依次倒序比较路径点r的n个路径点的代价值f'(r)，并找出代价值f'(r)最小的路径点re；代价值f'(r)最小的路径点re作为重新规划的起点；若f'(r)出现相等情况时，将倒数第n个路径点作为重新规划的起点；然后，依据栅格的通行代价值，采用A*算法规本文档来自技高网...

【技术保护点】
1.基于障碍物虚拟膨胀的伴随机器人路径规划方法，其特征是，包括：构建环境地图：根据实际场景构建二维占据栅格地图，将每一个栅格标记为障碍区域或可行区域；伴随机器人以及可移动目标点的初始化：在栅格地图中设定伴随机器人和可移动目标点的初始坐标位置；为机器人建立滑动窗口；对障碍区域进行膨胀处理：按照机器人中心与机体边缘的最短距离对障碍物所处的栅格进行初始膨胀，障碍物与初始膨胀后的栅格共同构成最小不可通过区域；依据机器人实际运行时所需的安全距离，在最小不可通行区域基础上确定出外扩的栅格层数，将外扩后区域内的栅格作为障碍物虚拟膨胀栅格，标记障碍物虚拟膨胀栅格受障碍物影响的的危险等级；伴随机器人的路径规划。

【技术特征摘要】
1.基于障碍物虚拟膨胀的伴随机器人路径规划方法，其特征是，包括：构建环境地图：根据实际场景构建二维占据栅格地图，将每一个栅格标记为障碍区域或可行区域；伴随机器人以及可移动目标点的初始化：在栅格地图中设定伴随机器人和可移动目标点的初始坐标位置；为机器人建立滑动窗口；对障碍区域进行膨胀处理：按照机器人中心与机体边缘的最短距离对障碍物所处的栅格进行初始膨胀，障碍物与初始膨胀后的栅格共同构成最小不可通过区域；依据机器人实际运行时所需的安全距离，在最小不可通行区域基础上确定出外扩的栅格层数，将外扩后区域内的栅格作为障碍物虚拟膨胀栅格，标记障碍物虚拟膨胀栅格受障碍物影响的的危险等级；伴随机器人的路径规划。2.如权利要求1所述的基于障碍物虚拟膨胀的伴随机器人路径规划方法，其特征是，伴随机器人的路径规划的具体步骤为：步骤(1)：伴随机器人将可移动目标点的当前位置在滑动窗口中的投影点作为新目标点，基于栅格的通行代价值，伴随机器人按照A*算法进行路径搜索得到伴随机器人与新目标点之间的最优路径；伴随机器人根据最优路径进行行走；滑动窗口以伴随机器人为基准，随着伴随机器人的移动而移动；步骤(2)：按照设定时间间隔，对滑动窗口内的栅格地图进行更新，对可移动目标点的当前位置在滑动窗口中的投影点进行更新，将最优路径在滑动窗口内进行平移；步骤(3)：伴随机器人判断更新后的滑动窗口内的栅格地图是否存在最小不可通过区域阻断路径；如果存在，就返回步骤(1)；如果不存在，就进入步骤(4)；步骤(4)：基于栅格的通行代价值，按照增量规划路径方式，对伴随机器人平移后在滑动窗口内剩余路径点中通行代价值最小的点与更新的投影点之间的路径进行重新规划，找到新的最优路径，将新的最优路径与伴随机器人当前路径进行连接形成新的推荐路径；伴随机器人按照新的推荐路径进行行走；返回步骤(2)。3.如权利要求1所述的基于障碍物虚拟膨胀的伴随机器人路径规划方法，其特征是，所述滑动窗口的构建过程，包括：在栅格地图中以伴随机器人当前位置为原点建立二维坐标平面；在二维坐标平面中选择2N*2N的栅格区域为滑动窗口；所述滑动窗口在二维坐标平面第一象限的栅格区域为N*2N；所述滑动窗口在二维坐标平面第二象限的栅格区域为N*2N；所述滑动窗口的底边经过原点；N表示栅格个数。4.如权利要求2所述的基于障碍物虚拟膨胀的伴随机器人路径规划方法，其特征是，栅格的通行代价值的具体计算公式为：式中，g(s)表示从起点到栅格s的通行代价值，g(s-1)表示从起点到栅格s-1的通行代价值，c(s-1,s)表示栅格s-1到栅格s的通行代价值，d(s)表示虚拟膨胀栅格s受障碍物影响的危险等级，d(s-1)表示虚拟膨胀栅格s-1受障碍物影响的危险等级，α表示路径远离障碍物一个栅格条件下所需的额外代价；路径远离障碍物一个栅格条件下所需的额外代价大于相邻栅格距离值的最小增量与栅格边长之和。5.如权利要求2所述的基于障碍物虚拟膨胀的伴随机器人路径规划方法，其特征是，所述步骤(1)伴随机器人将可移动目标点的当前位置在滑动窗口中的投影点作为新目标点，基于栅格的通行代价值，伴随机器人按照A*算法进行路径搜索得到伴随机器人与新目标点之间的最优路径的具体步骤为：当可移动目标点在滑动窗口内，以伴随机器人当前位置为起点，以可移动目标点当前位置为终点，依据栅格的通行代价值，采用A*算法进行计算，得到最优路径；当可移动目标点在滑动窗口外，且伴随机器人与可移动目标点的连线与滑动窗口的交点处没有障碍物时；以伴随机器人当前位置为起点，以可移动目标...

【专利技术属性】
技术研发人员：张慧，荣学文，李贻斌，李彬，刘斌，
申请(专利权)人：齐鲁工业大学，
类型：发明
国别省市：山东,37