[发明专利]基于A*算法的伴随机器人增量路径规划方法及系统

权利要求书

1.基于A*算法的伴随机器人增量路径规划方法，其特征是，包括：

构建环境地图：根据实际场景构建二维占据栅格地图，将每一个栅格标记为障碍区域或可行区域；

伴随机器人以及可移动目标点的初始化：在栅格地图中设定伴随机器人和可移动目标点的初始坐标位置；为机器人建立滑动窗口；

伴随机器人的路径规划步骤：

步骤(1)：伴随机器人将可移动目标点的当前位置在滑动窗口中的投影点作为新目标点，伴随机器人按照A*算法进行路径搜索得到伴随机器人与新目标点之间的最优路径；伴随机器人根据最优路径进行行走；滑动窗口以伴随机器人为基准，随着伴随机器人的移动而移动；

步骤(2)：按照设定时间间隔，对滑动窗口内的栅格地图进行更新，对可移动目标点的当前位置在滑动窗口中的投影点进行更新，并对步骤(1)的最优路径在滑动窗口内进行平移；

步骤(3)：伴随机器人判断更新后的滑动窗口内的栅格地图是否存在障碍物阻断最优路径；如果存在，就返回步骤(1)；如果不存在，就进入步骤(4)；

步骤(4)：按照增量规划路径方式对伴随机器人平移后在滑动窗口内剩余路径点中代价值最小的点与更新的投影点之间的路径进行重新规划，找到新的最优路径，将新的最优路径与伴随机器人当前路径进行连接形成新的推荐路径；伴随机器人按照新的推荐路径进行行走；返回步骤(2)；

所述步骤(1)伴随机器人将可移动目标点的当前位置在滑动窗口中的投影点作为新目标点，伴随机器人按照A*算法进行路径搜索得到伴随机器人与新目标点之间的最优路径的具体步骤为：

当可移动目标点在滑动窗口内，以伴随机器人当前位置为起点，以可移动目标点当前位置为终点，采用A*算法进行计算，得到最优路径；

当可移动目标点在滑动窗口外，且伴随机器人与可移动目标点的连线与滑动窗口的交点处没有障碍物时；以伴随机器人当前位置为起点，以可移动目标点在滑动窗口中的投影坐标为终点，采用A*算法进行计算，得到最优路径；

当可移动目标点在滑动窗口外，且伴随机器人与可移动目标点的连线与滑动窗口的交点处有障碍物时；以该交点为中心分别向滑动窗两侧进行寻找直到找到非障碍点；将找到的非障碍点为终点；以伴随机器人当前位置为起点，采用A*算法进行计算，得到最优路径；

所述步骤(2)的具体内容为：

对滑动窗口内的障碍物位置进行更新，对滑动窗口内可移动目标点的投影位置进行更新；按照伴随机器人位置变化对已规划的最优路径进行平移操作，删除移出滑动窗边界的路径点，保留滑动窗内的路径点，记为r；

所述步骤(4)中，按照增量规划路径方式对伴随机器人平移后在滑动窗口内剩余路径点中代价值最小的点与更新的投影点之间的路径进行重新规划，找到新的最优路径，将新的最优路径与伴随机器人当前路径进行连接形成新的推荐路径的具体步骤为：

伴随机器人更新步骤(2)中平移后滑动窗口内剩余路径点的评价函数：

g'(r)＝g(r)-g(r_b) (2)

h'(r)＝|x_t-x_r|+|y_t-y_r| (3)

f'(r)＝g'(r)+h'(r) (4)

其中，r_b为平移后路径r的起始点，g(r)表示上次规划从起始点到路径r的路径点的代价值,g(r_b)为上次规划从起始点到路径点r_b的代价值，g'(r)为本次更新后从起始点r_b到路径r的路径点的代价值；x_t表示当前目标投影点的在栅格地图中的x坐标，y_t表示当前目标投影点的在栅格地图中的y坐标，x_r为路径r的路径点的x坐标，y_r为路径r的路径点的y坐标；h'(r)为本次更新后路径r的路径点到目标投影点的代价值，f'(r)为本次更新后从起始点r_b经由路径r到目标投影点的代价值；r_b作为平移后路径r的起始点，由公式(2)可知，g(r_b)更新后的结果g'(r_b)为0；

从路径点r的最后一个点开始，依次比较路径点r上n个路径点的代价值f'(r)，并找出代价值f'(r)最小的路径点r_e；代价值f'(r)最小的路径点r_e作为重新规划的起点；若f'(r)出现相等情况时，将倒数第n个路径点作为重新规划的起点；然后，采用A*算法规划从重新规划的起点r_e到更新的目标投影点的新路径l；最后，新路径l与路径r进行拼接，从而构成新的推荐路径R。