一种基于距离判断和角度偏转的移动机器人路径规划方法与流程

本发明涉及一种基于距离判断和角度偏转的移动机器人路径规划方法，属于移动机器人路径规划领域。

背景技术：

路径规划是移动机器人领域的关键技术之一，在一定程度上，它标志着移动机器人智能水平的高低，它是指依据各类指标在一个障碍物环境中搜寻一条从起点到目标点的最优无碰撞路径。路径规划本身又可以分为全局路径规划和局部路径规划。全局路径规划是在已知的环境中，给机器人规划一条路径，路径规划的精度取决于环境获取的准确度，全局路径规划可以找到最优解，但是需要预先知道环境的准确信息，当环境发生变化，如出现未知障碍物，全局规划效果就会不理想；局部路径规划能够通过传感器反馈的信息规避环境中出现的未知障碍物。

移动机器人进行路径规划需要同时用到全局路径规划器和局部路径规划器，以保证移动机器人运动的稳定性，目前，使用较多的是使用a*或dijkstra算法进行全局路径规划，在此基础上结合局部路径规划dwa算法，实现机器人导航。

局部路径规划器dwa主要根据机器人的自身速度限制以及机器人电机速度性能范围(v,w)中采样多组速度，并模拟机器人在这些速度下的轨迹，根据方位角、距障碍物距离、速度、平滑程度设置评价函数，然后对这些轨迹一一进行评价，计算量大且实时性低，严重影响机器人的效率。

本发明基于现有技术存在的问题，针对具有导航功能和自定义编程功能的移动机器人，设计一种基于距离判断和角度偏转的移动机器人路径规划方法，在全局路径规划a*的基础之上，结合机器人传感器获取距障碍物距离和障碍物角度范围信息，规避动态环境中的障碍物，完成移动机器人实时路径规划。

技术实现要素：

针对现有技术存在的问题，本发明为解决现有技术中存在的问题采用的技术方案如下：

一种基于距离判断和角度偏转的移动机器人路径规划方法，其特征在于，包括如下步骤：步骤1：利用slam建图算法构建的全局栅格地图作为先验信息，用于全局路径规划得到预规划路径，在环境没有发生改变的静态环境中，移动机器人按照预规划路径进行运动，步骤2：当环境发生改变，即预规划路径上出现障碍物时，移动机器人根据激光雷达扫描的信息获取距离信息和障碍物角度范围信息，指导移动机器人偏转移动，达到回归全局预规划路径的目的，进而实现机器人在动态环境下的导航。

所述步骤1中根据全局栅格地图作为先验信息，获取预规划路径的具体步骤为：

步骤1.1、将栅格地图划分为二维网格，每个网格都叫做节点，每个节点只有可通行和不可通行两种状态，设有两个列表，开放列表(openlist)和关闭列表(closelist)，假设起点为m，终点为n；

步骤1.2、设每个节点的总代价值为f，该节点到起点的成本为t，从该节点移动到目标节点的成本为u，则f＝t+u；

步骤1.3、将起点m放入到开放列表中，检查m附近与其相连的节点，如果相连节点是障碍物或者不能通过则不作处理，如果节点是可通过的，就将节点加入到开放列表中，同时将新加入的节点设置为m的子节点；

步骤1.4、将节点m从开放列表移动到关闭列表中，m节点周围的子节点加入到了开放列表中，每个节点都有箭头指向m，此时计算每个节点的总代价值f，将具有最小值f的子节点加入到关闭列表中；

步骤1.5、取步骤1.4中所选的子节点，排除与之相连但不能通过以及存在于关闭列表中的子节点，将其他节点加入到开放列表中，同时将步骤4中所选节点设置为新加入开放列表中节点的父节点，若准备加入到开放列表的节点已存在于开放列表中，则检查当前所选节点的父节点到达已经在开放列表中的节点是否会有更小的t值，如果没有，忽略该操作，如果t值更小，则将已经存在开放列表中的节点设置为当前所选节点的父节点；

步骤1.6、在开放列表中选择新的最小的f值节点，重复上述过程，当存在多个最小值时，依次探索节点，当目标点n也加入到开放列表中时，从目标点开始一直寻找当前节点的父节点直到起始点，构成的路径即为预规划路径。

所述步骤2中的障碍物为之前环境中存在的，但因位置发生了变化，挡住了路径形成的障碍物，或者新出现的障碍物。

所述步骤2中预规划路径上出现障碍物时机器人进行的局部偏转回归具体步骤为：

步骤2.1、假设机器人的直径为l，设置距离阈值d和角度阈值r，机器人位置点为o，移动机器人通过蒙特卡洛算法进行实时定位，在预规划路径上移动过程中探测到障碍物并阻碍机器人移动时，机器人获取障碍物距离信息和障碍物角度范围信息，当距障碍物距离小于d或者障碍物角度范围大于r时，机器人开始局部路径规划；

步骤2.2、假设距障碍物距离s首先小于等于d时，此时障碍物角度范围为q，障碍物边际点分别为a、b，机器人到边际点a、b的距离分别为c、d，以机器人当前朝向做射线oe，将障碍物角度范围q分为q1、q2两个角，且角q1与点a同向，角q2与点b同向，比较q1、q2的大小，若q1<q2，则以边c为斜边点a为顶点向射线oe做垂线ag，垂足为g，反向延长ag至h，使得ah＝1.5l；若q1>q2，则以边d为斜边点b为顶点向射线oe做垂线bg，垂足为g，反向延长bg至h,使得bh＝1.5l；

上述两种情况下均可根据解三角形计算出∠goh的大小，该角即为机器人偏转角；

步骤2.3、假设扫描到障碍物角度范围j首先大于等于r时，障碍物边际点分别为a、b，机器人到边际点a、b的距离分别为c、d，以机器人当前朝向做射线oe，将障碍物角度范围j分为j1、j2两个角，且角j1与点a同向，角j2与点b同向，比较j1、j2的大小，若j1<j2，则以边c为斜边点a为顶点向射线oe做垂线ag，垂足为g，反向延长ag至h，使得ah＝1.5l；若j1>j2，则以边d为斜边点b为顶点向射线oe做垂线bg，垂足为g，反向延长bg至h,使得bh＝1.5l；上述两种情况均根据解三角形计算出∠goh的大小，该角即为机器人偏转角；

步骤2.4、机器人根据所计算的偏转角进行旋转，机器人沿线段oh运动至h点后，机器人反向旋转，大小等于∠goh的大小，此时机器人再次扫描障碍物的边界点，可以得出当前机器人坐标点到边界点的距离与机器人当前朝向与边界线的角度，以边界点为直角顶点，向机器人朝向方向线做射线，以边界点为起点向着射线方向，有一点k，且k点距该边界点1.5l，连接kh，根据解三角形计算出机器人当前朝向旋转至kh方向的偏转角度；

步骤2.5、机器人沿kh方向移动，直至与全局预规划路径重合，重合之后机器人按照预规划路径开始移动，这样即实现机器人局部偏转回归，从而实现避障效果。

进一步的所述步骤2.5的机器人局部偏转回归包括如下步骤：机器人的局部偏转回归均是以单个障碍物为单位进行的，且针对的障碍物均为原环境中新增或变动的障碍物，但机器人在进行障碍物角度范围判断和边界点判断的过程中，在机器人视角中会出现两或多个不同的障碍物发生重叠的现象，也有会扫描到原先存在障碍物，为了区分，根据激光雷达获取的点云信息进行判断，若点云发生突变，则判断为多个障碍物，在取偏转角时以突变点作为边界点，当机器人扫描到原先障碍物时，在不影响偏转回归的前提下不考虑，若影响则将之当做新增或变动障碍物进行偏转避开处理，若机器人避开第一个障碍物后，其后还有若干个障碍物阻隔导致机器人不能回归预规划路径，则机器人进行若干次偏转移动，直至回归。

本发明具有如下优点：

传统局部路径规划方法，如dwa动态窗口法，需要根据机器人速度范围及电机性能参数规划若干模拟路径通，再通过复杂的评价函数计算最优路径，导致计算量大实时性低，而通过本发明方法，只需要已有传感器获取障碍物距离信息和角度范围信息，通过简单解三角形即可计算出避障轨迹，有效降低了机器人的计算量，大大增强了移动机器人运动的实时性，提高机器人的移动效率，且本文算法简单易行，为移动机器人的导航避障提供了新思路。

附图说明

图1是单个障碍物情况下，距障碍物距离小于等于阈值d，且q1<q2时，移动机器人进行偏转回归的局部示意图；

图2是多个障碍物情况下，移动机器人进行偏转回归的局部示意图；

图3是单个障碍物下，本发明方法移动机器人的移动路径；

图4是多个障碍物下，本发明方法移动机器人的移动路径；

图5是本发明方法的总体流程图。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明，一种基于距离判断和角度偏转的移动机器人路径规划方法，包括如下步骤：

利用slam建图算法构建全局栅格地图，将栅格地图划分为二维网格，每个网格都叫做节点，每个节点只有可通行和不可通行两种状态，设有两个列表，开放列表(openlist)和关闭列表(closelist)，初始化openlist和closelist，首先将起点放入到openlist中，检查起点附近与之相连的节点，去除不可用的节点，将可通行节点加入到openlist中，并将它们设置为起始节点的子节点；

将起始节点移动至closelist中，根据曼哈顿距离或者欧几里得距离来估算起始节点子节点的总代价值f，得出最小值并将其移至closelist；

检查上一次移至closelist节点周围的节点，将可通行的节点加入openlist，并将其设置为上一次移至closelist节点的子节点。若刚加入openlist的节点已存在于列表中，则检查经由当前closelist节点到已存在于openlist节点是否有更小t值，若t值更小，则将其设置为当前closelist节点的父节点，反之，不作处理；

重复上述过程，当存在多个f值最小的节点，依次探索节点即可，直至终点加入到openlist中，则从终点开始寻找当前节点的父节点直到起点，构成路径即为全局预规划路径，流程图如附图5所示；

得到全局预规划路径后，机器人会按照预规划路径进行移动，在机器人移动的过程中，检测到预规划路径上出现障碍物，则根据障碍物的距离信息或角度范围信息进行局部偏转回归，局部偏转回归实施步骤如下：

当距离障碍物距离首先小于等于阈值d时，扫描障碍物在机器人的视角范围为q，障碍物边际点分别为a、b，根据激光雷达信息可知机器人到边际点的距离分别为c、d，以机器人当前朝向做射线oe，将障碍物角度范围q分为q1、q2两个角，令q1与a对应，q2与b对应，比较q1、q2的大小；

若q1<q2，则以c为斜边，点a为顶点，向射线oe做垂线ag，垂足为g，反向延长ag至h，使得ah＝1.5l，如图1所示，则偏转角

若q1>q2，则以d为斜边，点b为顶点，向射线oe做垂线bg，垂足为g，反向延长bg至h，使得bh＝1.5l，则偏转角

当扫描到的障碍物角度范围j首先大于等于r时，障碍物边际点分别为a、b，根据激光雷达信息可知机器人到边际点的距离分别为c、d，以机器人当前朝向做射线oe，将障碍物角度范围j分为j1、j2两个角，令j1与a对应，j2与b对应，比较j1、j2的大小；

若j1<j2，则以c为斜边，点a为顶点，向射线oe做垂线ag，垂足为g，反向延长ag至h，使得ah＝1.5l，则偏转角

若j1>j2，则以d为斜边，点b为顶点，向射线oe做垂线bg，垂足为g，反向延长bg至h，使得bh＝1.5l，则偏转角

机器人根据计算的偏转角进行旋转，机器人沿线段oh运动到h点后，机器人反向旋转，角度等于∠goh的大小，此时机器人再次扫描障碍物的边界点，得出当前机器人坐标点到边界点的距离为e与机器人当前朝向与边界线的角度p，以边界点为直角顶点，向机器人朝向方向线做射线，以边界点为起点向着射线方向，有一点k，且k点距该边界点1.5l，连接kh，根据解三角形可以计算出机器人当前朝向旋转至kh方向的偏转角度为：

机器人沿kh方向移动，直至与全局预规划路径重合，重合之后机器人按照预规划路径开始移动，实现避障效果，如图3所示，为本文算法的整体示意图，若有多个障碍物阻隔导致机器人不能回归预规划路径，则机器人进行若干次偏转移动，直至回归，如图2和图4所示，分别为多个障碍物下，移动机器人进行偏转回归的局部示意图和整体算法示意图。

根据以上具体实施步骤，能够有效实现移动机器人在环境发生改变的动态环境下避障，且该文方法简单易行，计算量低，提高了移动机器人移动效率。

本发明的保护范围并不限于上述的实施例，显然，本领域的技术人员可以对本发明进行各种改动和变形而不脱离本发明的范围和精神。倘若这些改动和变形属于本发明权利要求及其等同技术的范围内，则本发明的意图也包含这些改动和变形在内。