一种基于椭圆切线构造的机器人路径规划方法与流程

本发明属于机器人自主路径规划领域，具体涉及一种基于椭圆切线构造的机器人路径规划方法。

背景技术：

随着现代化技术的不断进步，机器人技术已经取得了迅速发展，机器人在生活生产中的应用也变得更加广泛。自主路径规划作为移动机器人技术领域中的一个重要组成部分，重要性尤为突出。移动机器人路径规划是指在存有障碍物的环境中，机器人参照一定的准则，如最短路径、最少拐弯次数等，找出一条从起点位置到终点位置的最优的安全无碰撞路径。

现有的路径规划方法可以根据机器人对环境信息感知程度的不同分为两类：全局路径规划方法和局部路径规划方法。全局路径规划是机器人对环境信息完全已知的一种路径规划，又分为静态全局路径规划和动态全局路径规划。文献(Guo Y,Li S.Path Planning for Robot Based on Improved Ant Colony Algorithm[J].Computer Measurement&Control,2009,17(1):187-153.)中将蚁群算法用于静态环境下的机器人路径规划，该算法使用网格离散化的方法对带有障碍物的环境进行建模，通过模拟蚂蚁群体觅食的智能行为，由多只蚂蚁协作完成最优路径的搜索。但该算法不适用于实时发生变化的环境，调节能力差。文献(Stentz A.Optimal and efficient path planning for partially-known environments[C]//IEEE International Conference on Robotics and Automation,1994.Proceedings.1994:3310-3317.)中提出的D-Star算法能够在动态环境中规划出最短路径，但由于该算法计算量大，无法满足移动机器人实时路径规划的需求，同时生成的路径不够平滑，不利于机器人的移动。

局部路径规划是机器人对环境未知或部分未知，通过感知器实时获取环境信息的一种路径规划，文献(Khatib O.Real-time obstacle avoidance for manipulators and mobile robots[J].International Journal of Robotics Research,1985,1(5):500-505.)中提出的人工势场法将机器人在周围环境中的运动设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。该算法所生成的路径一般比较平滑并且安全。但当物体离目标点比较远时，引力将变的特别大，由障碍物产生的斥力相对较小导致移动机器人在路径上可能会碰到障碍物，而且该算法只能对局部的路径进行规划而不能对全局进行优化。

文献(Marder-Eppstein E,Berger E,Foote T,et al.The Office Marathon:Robust navigation in an indoor office environment[J].2010,58(8):300-307.)中同时使用了A-Star算法和滑动窗口算法来分别对环境进行全局和局部的路径规划，但该方法不够一体化，算法的配合过程较为繁琐。文献(Chen H,Chang K,Agate C S.UAV Path Planning with Tangent-plus-Lyapunov Vector Field Guidance and Obstacle Avoidance[J].IEEE Transactions on Aerospace Electronic Systems,2013,49(2):840-856.)中使用了圆形来对障碍区域进行包围，实现了移动机器人的局部动态避障。但圆形的包围效率较低，很多情况下无法准确的对障碍物区域进行描述，容易过多的将可行区域变为不可行，造成空间浪费。

综上所述，如何找到一种一体化的方法，使其既能对全局环境进行快速规划，又能在局部动态环境中获得安全且平滑的路径，同时能够尽量避免过多的将可行区域变为不可行，显得十分重要。

技术实现要素：

针对现有技术的上述问题，本发明提出了一种基于椭圆切线构造的机器人路径规划方法，该方法既能对全局环境进行快速规划，又能提高对局部动态环境的适应性。其特征是借助椭圆对障碍物区域进行包围，利用点对椭圆的切线生成进行路径的二分探索，同时借助椭圆的边界对搜索得到的路径进行平滑过渡。相比圆形，椭圆能够对障碍物区域进行更好的描述，而且椭圆切线构造的方法没有对地图进行栅格搜索，只是对部分障碍物区域进行最小包围椭圆生成，减少了计算量，同时椭圆的弧边能够辅助平滑路径，克服了传统方法计算量大、路径不够平滑等问题，能在较少的时间内得到较优且平滑的路径，可以满足机器人二维空间内快速路径规划的应用需求。

机器人通过激光测距或视觉检测获取当前的环境数据，机器人配备的机载计算机根据接收到的环境数据，对数据进行坏点过滤、膨胀、查找障碍物轮廓并生成最小包围椭圆等预处理；以预处理后的数据为基础，进行点对椭圆、椭圆对椭圆的切线生成，进而将切线与椭圆边界进行衔接以实现起点到终点的多条备选路径生成；通过对生成的路径进行代价评估，从而挑选出最优的路径。

本发明的技术方案包括以下步骤：

第一步，数据的预处理和代价地图的生成

对移动机器人获取的环境数据进行坏点剔除，进而生成局部的二维栅格地图。在机器人进行路径规划的过程中，根据自身的位置以及已知的障碍物地图信息进行最优路径搜寻。但是由于这个过程并没有将机器人自身的形状体积考虑进去，默认将机器人中心点代替整个机器人，只能确保这一点不会与障碍物碰撞。现引入代价地图来解决这个问题，代价地图是根据机器人的形状参数，在原始地图的基础上，在实际障碍物附近设定一个膨胀区域。图1所示为一个代价地图，其中实心黑色区域表示实际的障碍物，虚线区域表示膨胀后的障碍物。在实际应用中，机器人在避障时，其轮廓不应该碰到实际障碍物区域，这对于路径规划算法的实现比较困难，因为机器人形状可能多种多样。引入代价地图后就可将思路转变成机器人中心点不能碰到代价地图的膨胀区域，即接下来的路径规划是在代价地图的基础上完成的。

第二步，障碍物区域的椭圆拟合

对已经生成的代价地图中的障碍物区域进行最小外接椭圆生成是进行下一步路径规划的基础。椭圆拟合算法包括直接计算法、最小二乘法、最小平方中值法，出于效率和精度考虑，该部分使用了最小二乘拟合算法来对轮廓的边缘点进行椭圆拟合，但拟合的过程中容易出现少数点不被椭圆包围的情况，如图2所示。为了使该椭圆能够全部包围对应障碍物区域，采用了如下算法：

1)判断椭圆是否包含了当前障碍物区域的所有边界点，若全部包含，结束算法，若不是全部包含，进入步骤2)；

2)对椭圆的长轴增加单位尺寸，判断当前椭圆是否包含了当前障碍物区域的所有边界点，若全部包含，结束算法，若不是全部包含，对椭圆的短轴增加单位尺寸，进入步骤1)。

至此生成的区域最小外接椭圆如图3所示。

第三步，椭圆外的点对椭圆的切线生成

求出椭圆外一点对该椭圆的切线，使用解方程逼近的方法计算量较大，这里采用了一种利用辅助圆的方法来准确得到椭圆切线。

设有椭圆O的焦点为F₁、F₂，P为椭圆外的一点，如图4所示，通过如下步骤获得P点对椭圆O的切线：

1)以椭圆O的中心为圆心，长轴为直径作椭圆的辅助圆；

2)以PF₁为直径作圆交椭圆O的辅助圆于D、E两点；

3)连接PD、PE，即为所求的切线，如图5所示。

第四步，椭圆对椭圆的近似公切线生成

当在椭圆之间生成路径时，若采用一个椭圆上的点对另一个椭圆进行切线生成的方法，会出现切线穿过当前椭圆的问题，这将会使机器人与椭圆内的障碍物发生碰撞，同时也会出现直线路径和椭圆边界衔接不够平滑的问题。如图6所示，椭圆A上有点P，过点P作椭圆B的切线PD、PE可发现，切线穿过了椭圆A，同时两条切线没有把两个椭圆平滑的衔接在一起。

椭圆公切线没有精确的求解方法，现有的迭代方法计算量过大，无法满足机器人实时导航的需求，这里采用了两次逼近的方法来获得椭圆的近似公切线，步骤如下：

1)过D点作椭圆A靠近P点的切线，得到DP_D；过E点作椭圆A靠近P点的切线，得到EP_E，如图7所示；

2)过P_D点作椭圆B靠近D点的切线，得到P_DD₁；过P_E点作椭圆B靠近E点的切线，得到P_EE₁，如图8所示。

通过以上步骤，将获得的公切线用于椭圆之间的衔接，使生成的路径更加安全且平滑。

第五步，基于椭圆切线构造的路径规划

设定移动机器人的起始点为S，目标点为E，通过如下步骤进行路径规划：

1)遵循两点之间线段最短的原则，从目标点E进行回溯，作线段ES，如图9所示，若没有触碰到障碍物，则该线段为最优路径，路径规划结束，若有触碰到障碍物，如图10所示，进入步骤2)；

2)当从E回溯的路径碰到障碍物K₀时，对障碍物区域K₀进行椭圆拟合，如图11中所示的最小包围椭圆O₀，进入步骤3)；

3)过E点和S点分别对椭圆O₀作切线得到ES₁、ES₂和SE₁、SE₂，如图12所示；

4)对于S₁E、S₂E、SE₁、SE₂四条子路径，分别以S₁、S₂、S、S为起始点，E、E、E₁、E₂为目标点分别进行从步骤1)开始的递归判断，其中一个椭圆上的点对另一个椭圆的切线按照第四步中的椭圆近似公切线生成及优化方法进行优化，直至所有子路径都与障碍区域无碰撞点时结束递归，此时获得的所有子路径如图13所示；

5)根据两点间线段最短的原则，对每条路径其间无障碍物碰撞的非相邻点进行连通优化处理，如图14所示，至此生成了以S为起始点，E为目标点的所有备选路径；

6)对每条路径进行代价评判，代价评判的评价函数为距离代价、转角代价、时间代价和速度代价中的一种或两种以上。

优选采用距离代价和转角代价为例，按公式(1)所示对每条路径进行评估

P(S,E)＝D(S,E)+Y(S,E) (1)

其中P(S,E)为机器人选择当前路径所付出的总代价，D(S,E)为机器人选择当前路径所付出的距离代价，Y(S,E)为机器人选择当前路径所付出的转角代价。

7)得到所有备选路径中的最优路径，如图15所示的SABCDE，其中SA、BC、DE为线段，AB、CD为对应椭圆的弧边。至此，路径规划完毕。

本发明首先对获取的数据进行坏点剔除、代价地图生成并对特定障碍物区域进行最小包围椭圆生成等操作，进而通过进行点对椭圆的切线生成和椭圆对椭圆的近似公切线生成等一系列处理，获得从起始点到目标点的所有备选路径，最终通过代价函数挑选出最优路径。本发明提出的路径规划方法克服了传统方法计算量大、路径不够平滑等问题，能在较少的时间内获得较优且平滑的路径，可以满足机器人二维空间内快速路径规划的应用需求。

附图说明

图1为代价地图。

图2为椭圆拟合优化前的效果图。

图3为椭圆拟合优化后的效果图。

图4为椭圆和椭圆外一点的示意图。

图5为椭圆外的点对椭圆的切线示意图。

图6为一个椭圆上的点对另一个椭圆作切线的示意图。

图7为一次逼近后的椭圆近似公切线示意图。

图8为两次逼近后的椭圆近似公切线示意图。

图9为初始点与目标点的示意图。

图10为最短路径碰到障碍区域的示意图。

图11为对特定障碍区域生成最小包围椭圆的示意图。

图12四条子路径的示意图。

图13生成的所有子路径示意图。

图14优化后的所有子路径示意图。

图15最优路径示意图。

图16移动机器人在相同起始点下对不同目标点的路径规划结果图。

具体实施方式

以下结合技术方案和附图详细叙述本发明的具体实施方式。

在准备阶段，以给移动机器人配备二维激光来对环境进行感知为例，该激光有效距离为0.06米到10.0米，频率为40赫兹，检测角度为270度，角分辨率为0.25度。移动机器人长1米、宽1米、高0.2米，为了防止激光数据受到移动机器人凸起结构的影响，现将0.71米内的数据进行剔除，因此，移动机器人所能获得的障碍物有效距离大于0.71米，小于10米。接着需要选取合适的实验场地，场地的选取无太多限制，能保证环境中有适量的障碍物即可。将移动机器人放置在选好的场地，对设备进行通电，系统初始化后，设置目标点，机器人开始启动并从二维激光读入环境中的障碍物数据进行分析处理。

首先，对激光数据进行过滤，即留下0.71米到10.0米的数据，接着对障碍物区域按照移动机器人的尺寸进行膨胀生成代价地图。基于生成的代价地图，从目标点按与起始点的连接线段进行线段路径回溯搜索，对碰到的第一个障碍物区域进行最小外接椭圆生成。进而以起始点和目标点对该外界椭圆进行切线生成，得到四条子路径。将每条子路径按照子起始点和子目标点的匹配关系进行递归计算，直至所有子路径均能以线段和椭圆弧边相衔接的方式绕过障碍物区域，即生成了从起始点到目标点的所有备选路径。接着对每条备选路径进行优化，避免了连通的两点发生绕行的情况。最后对生成的每条路径进行距离代价和转角代价的联合评价，得到最适合该移动机器人的路径，如图16所示为移动机器人在相同起始点下对不同目标点的路径规划结果。至此，完成了基于椭圆切线构造的机器人路径规划工作。