一种机器人自主路径规划的方法与流程

本发明属于机器人控制技术技术领域，具体涉及一种机器人自主路径规划的方法。

背景技术：

随着机器人控制技术的不断发展，智能搬运机器人被逐步广泛的应用于仓储物流、生产搬运、电商分拣等行业场景中。在智能搬运机器人应用的过程中，机器人安全性受到了大家的重点关注，机器人的主动安全避障传感装置被大量用于机器人设计中，如：超声波距离感应器、红外距离传感器、激光测距传感器、点云障碍物传感器等。但大多数应用了测距传感器的机器人，仅实现了机器人主动停止等待，待障碍物移除后再继续按照预定路径完成运动的方式。如果障碍物一直存在，则机器人始终会在原地等待，大大降低了机器人的运行效率。

亟待提出一种在机器人探测到障碍物后能够进行自主绕行，完成自主通过障碍物的技术方案来解决上述技术问题。

技术实现要素：

针对上述问题，本发明提供了一种在机器人探测到障碍物后能够进行自主绕行，完成通过障碍物的机器人自主路径规划的方法，以期提高在复杂运行环境下的机器人运行效率。

具体技术方案是：一种机器人自主路径规划的方法，包括如下步骤：

步骤101，机器人按照调度系统下方的预定行走路径进行运行，并实时获取机器人在地图中的坐标位置；

步骤102，机器人在正常行走过程中，通过激光测距传感器反馈的周边轮廓信息，通过导引定位算法计算出在地图系统中机器人的绝对坐标位置及航向角；

步骤103，机器人根据激光测距传感器反馈的测距信号判断是否有障碍物存在，如果在一定距离内没有反馈障碍信号则继续执行步骤101，向前继续运行；如果发现障碍物则进入步骤104；

步骤104，如果判断在机器人行走前方有障碍物信号，则根据激光测距传感器扫描的平面点阵数据对障碍物进行分割，获取障碍物特征点，割方法如下：

分割方法如下：

（1）取起始角度值作为最近点、左边界点、右边界点的值；

（2）取下一角度值（角度步长由传感器决定），计算与上一值得变化量；

（3）若变化量小于-200mm,则将截至到上一角度值作为一个障碍物；若变化量小于200mm，则更新左边界值和最近点的值；若变化量大于200，则往后取10个值判断是否都大于200mm,若是则将截至到上一角度值作为一个障碍物，若否则更新左边界值和最近点的值；

（4）若到达终止角度则将当前数据作为一个障碍物，若未到达终止角度则返回步骤（2）；

步骤105，将障碍物特征点分割提取后，对机器人将要行走的路径进行分割，形成包络线；

分割方法如下：

若曲线段的最小曲率半径大于等于10000mm，整个段等效为直线；

若曲线段的最小曲率半径小于10000mm,则需要对曲线段切割：

（1）求出起点u和对应的曲率半径，作为小段的起始值；

（2）将u增加步长值取0.005，求出新的曲率半径，若u大于等于1.0，则生成一个小直线段并结束分割；

（3）若小段的起始曲率半径大于50000mm，新曲率半径小于50000，则生成一个小直线段，使用新的u和曲率半径去更新小段的起始值并返回到步骤（2）；

（4）若小段的起始曲率半径小于50000mm，新曲率半径大于50000mm，则生成一个小圆弧段，使用新的u和曲率半径去更新小段的起始值并返回到步骤（2）；

（5）若新曲率半径与小段起始曲率半径的差值大于小段起始曲率半径的5%，则生成一个小段，使用新的u和曲率半径去更新小段的起始值并返回到步骤（2）；

完成机器人路径分割后，需要根据每个分割段，计算机器人运行到该分割段上时的包络线范围，计算方法如下：

1、当切割的段是一个直线段：

为避免机器人运动偏差和等效切割带来的偏差，需要将机器人的机械尺寸做膨胀，目前将机器人的四条边均往外膨胀50mm；根据直线段ef可以生成对应的的矩形由四个顶点abcd,且a、b、c、d四点的坐标是可计算的；

车辆宽：w=实际车宽+50*2;

车辆前长：l1=实际前车长+50;

车辆后长：l2=实际后车长+50;

2、当切割段是一个曲线段：

切割得到的曲线并不是圆弧，为了便于计算将其等效为圆弧；圆弧的起点和终点即曲线的起点和终点，圆弧的等效半径取曲线起点半径和终点半径的平均值，根据圆弧的几何性质可以求出等效圆心点o，根据切割曲线段ef可以生成部分圆环o-abcd；

圆环等效半径：

圆环内半径：

圆环外半径：

步骤106，当形成机器人包络线后，会将整个机器人在曲线内运行的整体轮廓轨迹获得，此时需要将障碍物特征点放入包络线轨迹中进行判断是否存在包含关系；

判断方法如下：

车辆运动扫过的区域被等效为多个矩形和部分圆环，要判断特征点是否在包络线内即判断特征点是否在一个或多个矩形和部分圆弧内；

(1)判断一个点是否在矩形内

假设存在点p和矩形abcd,则点p在矩形内的充要条件为：

*>0.0且*<0.0且*>0.0且*<0.0

(2)判断一个点是否在部分圆环内

假设存在点p和部分圆环0-abcd,则点p在部分圆环内的充要条件为：

<<且<||<

步骤107，当判断障碍物特征点处于包络线矩形内或者部分圆环内时开始进行自主路径规划，具体方法如下：

a.基本型值点选择：

角度选择：选取上一障碍物与本障碍物在初始路径上的投影形成的向量方向作为本次型值点的角度；

坐标选择：在障碍物特征点的基础上沿本型值点角度的垂直方向往外延伸半车宽（包括膨胀宽度）得到型值点坐标；

b.扩展分析：

(1)通过基本型值点选择得到的型值点并不一定可通过，需要对型值点构成的矩形进一步做障碍物检测，若检测失败则继续往外延伸，直到检测成功或超出允许的距离；

(2)在绕行障碍物的过程中必然会经过障碍物的一侧，所以在分析新障碍物时先做边缘检测，即根据物体的左右特征点分别计算两个型值点，并选取较优的一个，进而确定绕行此障碍物的方向；

定义评价函数量化型值点的质量:

其中，beforedistance是上一型值点到本型值点的距离；leftdistance是本型值点到本初始路径段的终点的距离；viadiffangle是本型值点与上一型值点的角度差；originaldistance是本型值点与投影点的距离；分母上表示的是对应参数的左右型值点参数之和；q值越小则对应的型值点质量越高；

(3)通过测试发现型值点密集易造成曲率半径变化极不均匀，故将离上一型值点或本障碍物边界型值点距离小于半车长的型值点舍弃；

c.额外型值点：

由于仅依靠特征点产生的型值点会造成曲率半径不均匀，还需要根据由特征点计算而来的型值点增加额外的型值点；

（1）相邻型值点为同向扩张：

当两型值点距离大于半车长，将两型值点的中点沿型值点向量的垂直方向延伸100mm，作为额外型值点；

（2）相邻型值点为非同向扩张：

将上一基本型值点和本次基本型值点的连线称为弦；

若弦在上一基本型值点方向上的投影大于半车长，则将上一基本型值点沿自身方向和弦方向的角平分线延伸投影距离，作为额外型值点；

若弦在本次基本型值点的反方向上的投影大于半车长，则将本次基本型值点沿自身方向的反方向和弦方向的角平分线延伸投影距离，作为额外型值点；

根据以上计算获取到机器人所需要行走的路径的坐标序列；

步骤108，机器人在自主规划路径的过程中还需要不断检测是否有新的障碍物出现，如果有则继续执行步骤104重新对障碍物特征点和已经自主规划的路径进行比较，之后重新规划路径；

步骤109，当机器人未检测到运行路径上有障碍物时回到原规划路径，进行行走。

进一步，优选的是，

一种机器人自主避障的路径规划的方法在系统间协作关系中还包括：

机器人在判断障碍物处于未来运行路径范围之内后，还需要向管控的调度系统上报需要进入自主路径规划模式。

调度系统接收到机器人进入自主路径规划后，将不再向机器人下达路径序列，仅接收机器人上报的位置及角度等运行数据。

调度系统接收到的自主路径规划机器人的运行数据后需要调度其他在附近运行的机器人进行主动避让，避免由于多个机器人在同一区域同时开始自主绕行导致相互干扰。

机器人完成自主绕行障碍物后，回到预定路径点后，向调度系统发送信息重新接受调度系统统一管理，调度系统会从机器人最后恢复的点重新计算规划路径，并分批发送到机器人，机器人恢复调度系统管控。

本发明的有益效果：机器人探测到障碍物后能够进行自主绕行，完成通过障碍物的机器人自主路径规划的方法，能提高在复杂运行环境下的机器人运行效率。

附图说明

图1是本发明的机器人自主绕行障碍物的路径规划方法的基础流程图。

图2是本发明的直线段分割包络线绘制原理图。

图3是本发明的曲线段分割包络线绘制原理图。

图4是包络线绘制效果图。

具体实施方式

为了使本发明所解决的技术问题、技术方案更加清楚明白，以下结合附图及实施例，对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。

实施例

机器人进行自主路径规划逻辑如图1的实施步骤中所描述的流程进行完成；本发明中的机器人自主绕行障碍物的路径规划方法适应于多种驱动方式的机器人控制中，如：差速结构移动机器人、舵轮结构移动机器人、麦克拉姆轮结构移动机器人、履带结构移动机器人、车桥结构移动机器人。

步骤101，机器人按照调度系统下方的预定行走路径进行运行，并实时获取机器人在地图中的坐标位置；即机器人从调度系统获取未来需要行走路径的序列，并根据机器人存储的地图系统进行路径匹配。

步骤102，机器人在正常行走过程中，通过激光测距传感器反馈的周边轮廓信息，通过导引定位算法计算出在地图系统中机器人的绝对坐标位置及航向角；即机器人在运行过程中激光测距传感器通过激光反射测距信号，将激光测距平面点阵数据发送到机器人控制系统中。

步骤103，机器人根据激光测距传感器反馈的测距信号判断是否有障碍物存在，如果在一定距离内没有反馈障碍信号则继续执行步骤101，向前继续运行；如果发现障碍物则进入步骤104；即机器人在运动过程中通过激光测距传感器反馈的周边轮廓信息；如果发现障碍物，机器人同时可以获取障碍物轮廓数据。

步骤104，如果判断在机器人行走前方有障碍物信号，则根据激光测距传感器扫描的平面点阵数据对障碍物进行分割，获取障碍物特征点。即通过机器人存储地图系统中坐标位置和障碍物轮廓进行重叠计算，完成障碍物的分割，将障碍物抽象成最近点、左边界点、右边界点、系列中间点等几个特征点。割方法如下：

障碍物原始数据是由激光测距传感器通过一组极坐标的方式给出，可将障碍物表示为最近点、左边界点、右边界点、系列中间点。

分割方法如下：

（1）取起始角度值作为最近点、左边界点、右边界点的值；

（2）取下一角度值（角度步长由传感器决定），计算与上一值得变化量；

（3）若变化量小于-200mm,则将截至到上一角度值作为一个障碍物；若变化量小于200mm，则更新左边界值和最近点的值；若变化量大于200，则往后取10个值判断是否都大于200mm,若是则将截至到上一角度值作为一个障碍物，若否则更新左边界值和最近点的值；

（4）若到达终止角度则将当前数据作为一个障碍物，若未到达终止角度则返回步骤（2）；

通过以上方法可以将点云数据抽象为障碍物，为了便于计算要根据激光雷达的偏移参数和机器人坐标将极坐标数据转化为直角坐标数据。

步骤105，将障碍物特征点分割提取后，对机器人将要行走的路径进行分割，形成包络线。即根据机器人预设运行路径的直线段和曲线段的曲率半径大小，对机器人运行段进行分割，在分割的每一段路径上，完成机器人包络线的绘制。

分割方法如下：

若曲线段的最小曲率半径大于等于10000mm，整个段等效为直线；

若曲线段的最小曲率半径小于10000mm,则需要对曲线段切割：

（1）求出起点u和对应的曲率半径，作为小段的起始值；

（2）将u增加步长值取0.005，求出新的曲率半径，若u大于等于1.0，则生成一个小直线段并结束分割；

（3）若小段的起始曲率半径大于50000mm，新曲率半径小于50000，则生成一个小直线段，使用新的u和曲率半径去更新小段的起始值并返回到步骤（2）；

（4）若小段的起始曲率半径小于50000mm，新曲率半径大于50000mm，则生成一个小圆弧段，使用新的u和曲率半径去更新小段的起始值并返回到步骤（2）；

（5）若新曲率半径与小段起始曲率半径的差值大于小段起始曲率半径的5%，则生成一个小段，使用新的u和曲率半径去更新小段的起始值并返回到步骤（2）；

完成机器人路径分割后，需要根据每个分割段，计算机器人运行到该分割段上时的包络线范围，计算方法如下：

1、当切割的段是一个直线段。

为避免机器人运动偏差和等效切割带来的偏差，需要将机器人的机械尺寸做膨胀，目前将机器人的四条边均往外膨胀50mm。根据直线段ef可以生成对应的的矩形由四个顶点abcd,且a、b、c、d四点的坐标是可计算的，如图2所示。

车辆宽：w=实际车宽+50*2;

车辆前长：l1=实际前车长+50;

车辆后长：l2=实际后车长+50;

2、当切割段是一个曲线段。

切割得到的曲线并不是圆弧，为了便于计算将其等效为圆弧。圆弧的起点和终点即曲线的起点和终点，圆弧的等效半径取曲线起点半径和终点半径的平均值，根据圆弧的几何性质可以求出等效圆心点o。如图3所示，根据切割曲线段ef可以生成部分圆环o-abcd。

圆环等效半径：

圆环内半径：

圆环外半径：

步骤106，当形成机器人包络线后，会将整个机器人在曲线内运行的整体轮廓轨迹获得，此时需要将障碍物特征点放入包络线轨迹中进行判断是否存在包含关系。即机器人在预定路径上的运行包络线绘制完成后，与障碍物特征点坐标进行平面比对，机器人自主判断在机器人包络线范围内是否有障碍物特征点存在，如果存在则启动自主绕行障碍物路径规划过程，如图4所示。

判断方法如下：

车辆运动扫过的区域被等效为多个矩形和部分圆环，要判断特征点是否在包络线内即判断特征点是否在一个或多个矩形和部分圆弧内。

(1)判断一个点是否在矩形内

对应图1，假设存在点p和矩形abcd,则点p在矩形内的充要条件为：

*>0.0且*<0.0且*>0.0且*<0.0

(2)判断一个点是否在部分圆环内

对应图3，假设存在点p和部分圆环0-abcd,则点p在部分圆环内的充要条件为：

<<且<||<

步骤107，当判断障碍物特征点处于包络线矩形内或者部分圆环内时开始进行自主路径规划，路径规划方法如下:

d.基本型值点选择：

角度选择：选取上一障碍物与本障碍物在初始路径上的投影形成的向量方向作为本次型值点的角度。

坐标选择：在障碍物特征点的基础上沿本型值点角度的垂直方向往外延伸半车宽（包括膨胀宽度）得到型值点坐标。

e.扩展分析：

(1)通过基本型值点选择得到的型值点并不一定可通过，需要对型值点构成的矩形进一步做障碍物检测，若检测失败则继续往外延伸，直到检测成功或超出允许的距离。

(2)在绕行障碍物的过程中必然会经过障碍物的一侧，所以在分析新障碍物时先做边缘检测，即根据物体的左右特征点分别计算两个型值点，并选取较优的一个，进而确定绕行此障碍物的方向。

定义评价函数量化型值点的质量:

其中，beforedistance是上一型值点到本型值点的距离；leftdistance是本型值点到本初始路径段的终点的距离；viadiffangle是本型值点与上一型值点的角度差；originaldistance是本型值点与投影点的距离。分母上表示的是对应参数的左右型值点参数之和。q值越小则对应的型值点质量越高。

(3)通过测试发现型值点密集易造成曲率半径变化极不均匀，故将离上一型值点或本障碍物边界型值点距离小于半车长的型值点舍弃。

f.额外型值点：

由于仅依靠特征点产生的型值点会造成曲率半径不均匀，还需要根据由特征点计算而来的型值点增加额外的型值点。

（1）相邻型值点为同向扩张

当两型值点距离大于半车长，将两型值点的中点沿型值点向量的垂直方向延伸100mm，作为额外型值点。

（2）相邻型值点为非同向扩张

将上一基本型值点和本次基本型值点的连线称为弦。

若弦在上一基本型值点方向上的投影大于半车长，则将上一基本型值点沿自身方向和弦方向的角平分线延伸投影距离，作为额外型值点。

若弦在本次基本型值点的反方向上的投影大于半车长，则将本次基本型值点沿自身方向的反方向和弦方向的角平分线延伸投影距离，作为额外型值点。

根据以上计算获取到机器人所需要行走的路径的坐标序列。

步骤108，机器人在自主规划路径的过程中还需要不断检测是否有新的障碍物出现，如果有则继续执行步骤104重新对障碍物特征点和已经自主规划的路径进行比较，之后重新规划路径；即机器人在启动自主绕行路径规划后，按照路径运行过程中，采用分段检测分段规划的方式进行，当机器人绕行过程中发现新的障碍物出现时，需要重新分割障碍物特征点，并再次放入地图系统中进行路径计算，得到新的绕行路径。

步骤109，当机器人未检测到运行路径上有障碍物时回到原规划路径，进行行走；即机器人在执行障碍物绕行后，检测在预定路径上不存在障碍物后，机器人选择原始路径上的一个点作为自主规划路径终点，当机器人回到原路径运行后整个自主路径规划结束。

以上通过具体的和优选的实施例详细的描述了本发明，但本领域技术人员应该明白，本发明并不局限于以上所述实施例，凡在本发明的精神和原则之内，所作的任何修改、等同替换等，均应包含在本发明的保护范围之内。