一种基于任务驱动、轨迹引导的3D柔性避障方法与流程

本发明涉及避障，尤其涉及一种基于任务驱动、轨迹引导的3d柔性避障方法。

背景技术：

1、近年来，随着智能制造的快速发展，柔性化、自动化的生产线及仓储物流系统受到众多企业的青睐。agv(automated guided vehicle)是柔性生产线和智能仓储物流系统的关键设备之一，具有自动化程度高、灵敏、安全等特点，广泛应用在工业、军事、烟草、智能停车库等自动化搬运系统。agv实际应用场景错综复杂，会遇到诸多与安全相关的场景，如agv在行驶路径上遇到静态或动态障碍物，若不及时采取减速、制动措施，极易引发安全事故。或在厂区内有多辆agv同时作业时，可能出现因车辆调度系统任务指派不合理，而引发车辆碰撞的风险。如果agv的防区设置过大而频繁的误触发，会使其减速或停止，导致任务执行失败、效率低。为使agv能在复杂的工业场景更加安全、高效的运行，在其执行任务时能够实时获得障碍物信息并采取相应的安全措施提出了更高的要求。

2、目前agv在运行过程中的避障策略主要分为两种：自动绕障和停障。其中自动绕障是根据局部路径规划出起点和目标终点间较短的路径，避开障碍物；停障是在其行驶路径上检测到障碍物到agv距离小于安全距离后采取制动措施。自动绕障更适用于低速搬运、惯性较小的场景；如果agv搬运货物较重，速度较快，检测到前方有障碍物执行绕障动作，会造成所拉货物的前倾，带来一定的安全隐患。在安全性要求高的工业搬运项目中通常采用停障的安全策略来实现agv的减速停车，起到对人员、设备的保护。

3、在传感器选型方面，通常采用视觉传感器、激光雷达传感器、红外传感器、超声波传感器等进行非接触式探测，其中激光雷达具有检测精度高、稳定性好、相应速度快等优点，得到广泛应用。现有的技术中大都在雷达扫描平面设置不同属性尺寸固定的静态安全避障防区，当某个防区检测到障碍物后使agv执行减速或停止的动作，这种静态防区对速度的适用性较差，并且在速度较高下防区设置较小可能会无法及时刹车，如果防区设置较大，会频繁的误触发，致使agv运行效率低。此外基于2d雷达的静态防区只能检测雷达安装高度平面存在的障碍物而不是整个立体空间的，存在安全隐患。

4、现有agv避障方法如下：

5、中国专利“cn109725328a一种基于激光雷达传感器的agv障碍物检测系统及方法”所述检测方法对障碍物检测方案之间进行动态切换，依据agv运行路径的属性进行切换；障碍检测区域的形状、大小、数量可编辑、可配置；可以控制agv车体运行、改变运行路径、减速、启动、或停车等。这种方法提供一种在车体运行前设置好每个防区的属性，在对应的防区内检测到障碍物后采取对应的机制。这种多防区的设置方法适用于单一的工作场景，在频繁更换作业场景时，适用性较差，会导致误触发。

6、中国专利“cn110737274a一种agv避障方法、系统、agv及存储介质”提出控制单元内建有防区，防区包含多级功能区域，每两个功能区域的长度和/或避障宽度不同，方法包括：获取避障模块扫描到的全局区域内的障碍物信息；判断是否有障碍物落入任意一级功能区域内，是则触发对应于功能区域的预设操作；其中，预设操作包括减速、停车。这种方法提供一种基于2d避障雷达的静态防区，在转弯时的防区要大于直线行驶的防区，会频繁在正常行驶和减速状态间切换，运行效率低。

7、中国专利“cn111736599a基于多激光雷达的agv导航避障系统、方法、设备”公开了一种基于多激光雷达的agv设备的导航避障方法和一种agv设备，所述障碍物信息获取模块用于通过所述多个激光雷达传感器的数据，计算获得agv设备当前周围360度方向的障碍物的动态信息。这种方法是在车辆对角线位置安全两个2d激光雷达，实现360度障碍物检测方案，但是这种2d激光雷达只能检测到雷达安装平面上的障碍物，无法检测雷达上方是否有障碍物，运行时存在安全隐患。

8、目前，现有的避障策略多是基于2d激光雷达实现雷达安装平面障碍物检测，这种方法无法对雷达安装平面以上或以下部分进行探测，存在安全隐患(如图1所示)。

9、其次，现有障碍物检测策略通常将安全激光检测范围设置为尺寸固定的静态防区，不适用于安全性较高的工业场景。

10、1)在不同速度下行驶，静态防区尺寸无法随速度动态变化，减速\制动时间固定，安全性较低(如图2所示，静态防区尺寸l、h固定，不随速度v变化)

11、2)在不同轨迹下运行时，静态防区无法随轨迹动态变化，误检测导致频繁启停，效率低。(如图3所示，(a)(c)分别代表柔性避障和静态避障在转弯时减速区障碍物检测，障碍物出现在相同的位置，静态避障会误触发导致agv减速；(b)(d)分别代表柔性避障和静态避障在转弯时制动区障碍物检测，障碍物出现在相同的位置，静态避障会误触发导致agv停车)

12、3)在执行不同任务时，静态防区尺寸无法随任务类型变化，防区尺寸设置过大，误检测导致频繁启停，效率低。(如图4所示，其中empty_slow、empty_slow分别代表无货状态减速区和制动区的面积，load_slow、load_stop分别代表有货状态减速区和制动区的面积，load_slow＝empty_slow,load_stop＝empty_slow)

13、因此，有必要针对agv的障碍物检测与防撞策略进行优化。

技术实现思路

1、针对现有技术的不足，本发明提供一种基于任务驱动、轨迹引导的3d柔性避障方法；使agv在运行过程中使用带实时距离反馈的3d激光雷达，对360度空间内障碍物进行非接触式检测并根据检测结果决定是否启动安全机制，最终达到对人、设备以及agv自身的保护。

2、一种基于任务驱动、轨迹引导的3d柔性避障方法，包括以下步骤：

3、步骤1：对3d激光雷达数据进行处理；

4、基于激光雷达对车辆的周围环境进行实时感知，在激光测量过程中，激光雷达中的每个激光器发射激光束，所有激光束反射点的集合即为点云，点云中包含的点云数据包含每束激光雷达的检测距离、反射强度和时间戳信息；

5、步骤1.1：根据agv类型设置激光雷达的有效数据范围；

6、步骤1.2：当agv导航控制器接收到激光数据时，保存每束激光的角度和长度，并将每束激光的角度和长度转换成点云形式；

7、所述将换成点云形式具体为，首先根据每束激光的角度和长度计算激光数据在机器人坐标系下的坐标，根据定位信息获取当前机器人坐标系的全局位姿，获得机器人坐标系与全局坐标系之间的tf坐标关系；将机器人坐标系下的激光坐标根据tf坐标关系转换到全局坐标系下，即得到激光数据的点云形式；

8、步骤1.3：对激光雷达扫描获取的点云数据进行点云分割；

9、所述点云分割根据点云分布的整体特征和局部特征，将点云进行分割，从而形成若干个独立的子集；

10、步骤2：激光雷达检测到障碍物时，对agv车辆生成减速轨迹以及制动轨迹；

11、步骤2.1：预先设置相邻路径点时间间隔dt，将agv运行轨迹以n个路径点为一组切分成m个轨迹块，每组组名命名为k1,k2，...，km；

12、步骤2.2：在激光雷达检测到路径上的障碍物后将采取减速或制动，假设激光雷达检测到障碍物时车辆处于第k1个轨迹块的第i个路径点上，记录车辆此时的位置，根据车辆运行速度的大小解算出减速或制动所需要的时间t，车辆在制动时间t内会经过的路径点数n＝t/dt，则得知车辆在执行减速或制动后，车辆处于到第条轨迹块的第(i-1+n％10)个路径点上；把车辆检测到障碍物时对应的路径点的第i个路径点和执行减速或制动动作后停止的第j个路径点之间的路径点进行保存，则得到小车的减速轨迹以及制动轨迹；

13、步骤3：减速区域包络以及制动区域包络的生成；

14、避障防区里面包含减速区和制动区两部分，减速区和制动区形成的立体网格分别称为减速区包络以及制动区包络；当在减速区包络内检测到障碍物后，车辆执行减速任务；当在制动区包络内检测到障碍物后，车辆执行制动任务。

15、首先计算agv车辆的本体包络，本体包络将车辆的物理尺寸加入安全边距，形成一个覆盖agv及其安全运行所需空间的区域；本体包络分为两种情况：有货物状态包络以及无货物状态包络，两种情况下所生成的本体包络范围不同；

16、根据所生成的已知减速轨迹以及制动轨迹，结合激光雷达反馈数据，计算当前状态下车辆在全局坐标下的位姿，推算出在减速轨迹或制动轨迹的路径点相应的位姿，根据货物装载情况生成的包络范围在减速轨迹或制动轨迹上的路径点上逐一生成，在生成的减速轨迹或制动轨迹上形成一个包络集合，所有包络集合的积分形式即为减速状态或制动状态下的避障防区的范围；

17、步骤4：对agv进行运动控制；所述agv车辆运动包括两种状态：运动状态和静止状态；其中静止状态为车辆的速度为零时的状态，运动状态包括减速行驶状态、安全制动状态和正常行驶状态；车辆在行驶过程中，当减速区域检测到障碍物时，触发车辆减速行驶状态，当制动区检测到障碍物，触发车辆安全制动状态，否则，车辆将安全规划的轨迹正常行驶；

18、步骤4.1：接收3d激光雷达数据数据，将3d激光雷达数据转换为点云数据，并利用tf坐标变换转换为对应车辆的全局地图坐标；

19、步骤4.2：实时获取当前时刻车辆的运动状态，保存处理后的点云数据，遍历所有可行的激光点云数据；若有激光粒子落在制动区域内，则判定车辆处于制动状态，此时如果车辆不在制动状态，则发出制动命令，若车辆在制动状态，则保持制动状态，并持续进行循环检测操作；如果没有激光粒子落在制动区域和减速区域，判定车辆为行驶状态；若车辆当前在制动状态，则发出复原命令控制车辆正常行驶；若车辆不在制动状态，进一步判断小车是否在减速状态，若车辆在减速状态，则将车辆的减速状态切换为正常行驶状态，若车辆不在减速状态，则保持正常行驶；

20、若激光粒子没落在制动区域，但落在减速区域内的情况，判定车辆处在减速状态。若车辆当前处于减速状态，则一直保持减速状态，若车辆不是减速状态，将车辆设为减速状态。

21、采用上述技术方案所产生的有益效果在于：

22、本发明提供一种基于任务驱动、轨迹引导的3d柔性避障方法，克服了现有技术中的下列问题：

23、1、agv车辆在不同速度运行过程中使用单一避障防区的问题(如图6(a)、图6(b)所示，va和vb分别表示运行速度，l和h表示agv减速防区尺寸，其中vb>va,l2>l1,h2>h1)，提供一种与速度关联的动态防区设置。

24、2、agv车辆运行在不同行驶路径上使用单一安全避障防区的问题(如图6(c)、图6(d)分别代表转弯和直线行驶两种状态下安全避障防区设置)，提供一种随行驶路径变化的柔性防区。

25、3、agv车辆在执行不同任务的过程中使用单一安全避障防区的问题(如图6(e)、图6(f)所示，其中empty_slow、empty_slow分别代表无货状态减速区和制动区的面积，load_slow、load_stop分别代表有货状态减速区和制动区的面积，load_slow>empty_slow,load_stop>empty_slow)，提供一种和任务类型关联的避障防区设置。

26、本发明与现有技术相比具备以下有益效果：

27、1、本发明与现有的2d激光雷达相比，3d激光雷达能够对360度立体空间内的障碍物进行非接触式检测，保证整个运行过程更加安全(如图5(a)以及图5(b)所示)。

28、2、agv车辆在行驶过程中，其安全避障防区随速度动态变化，相比固定防区模式，具有更高的安全性。

29、3、agv车辆在行驶过程中，根据导航路径柔性生成避障防区，相比固定防区模式，提高了车辆在转弯处运行的可靠性。

30、4、agv车辆在行驶过程中，根据任务类型动态调整避障防区范围，相比固定防区模式，使车辆具有更高的执行效率。