本发明涉及机器人运动学、路径规划、碰撞检测领域，尤其涉及一种基于变密度搜索空间的工业机器人动态路径规划方法。
背景技术：
在工业机器人应用领域，离线编程因其直观、方便、安全的优势受到越来越多的关注。现有的离线软件在指令编辑及仿真方面已经做得比较完善，因此更多的研发重心开始转向智能化，其中路径规划就是一个重要的研究方向，在仿真环境中，若能使机器人自动避开障碍物，安全地执行运动指令，将会大大提升离线编程的使用效率。
目前，无碰撞路径规划主要存在两个重要的技术难点。其一是碰撞检测效率与精度的兼顾，虽然aabb、obb、凸包等碰撞包围盒的相关理论已较为成熟，能够对大部分不发生碰撞的情况进行快速判断，但是包围不紧密导致的检测精度问题却难以被解决，此外，对于大物体包含小物体时的碰撞检测，如工作台内安装有一台机器人的情况，仅采用包围盒无法给出准确的判断，而基于分离轴定理、devillers等算法的图元级(如三角面片)碰撞检测虽然精度很高，但是单个模型就可能有成千上万个三角片面，这就需要进行大量的碰撞检测，导致检测效率低下；其二是规划效率与成功率的兼顾，串联机器人的路径规划是一个复杂的高维空间规划问题，很多时候会存在搜索空间庞大、可行空间狭小的情况，对于此类问题一般会采用采样或优化的方法，过大的搜索空间会导致求解时间过长，若为了提升规划效率而过度剪枝，又可能会导致算法的概率不完备，无法保证在经过足够长的时间后必然能求出可行解。
此外，为了保证在商业化使用过程中不至于出现频繁的卡顿，如何提升重复规划的求解效率，如何加快搜索空间的构建速度，也都是不可忽视的技术难点。
技术实现要素：
本发明的目的在于提供一种基于变密度搜索空间的工业机器人动态路径规划方法，以解决现有离线编程软件难以实现高效无碰撞路径规划的问题，同时为实现兼顾效率与精度的机器人碰撞检测功能提供一种可供借鉴的方法。
为解决上述技术问题，本发明的一种基于变密度搜索空间的工业机器人动态路径规划方法的具体技术方案如下：
一种基于变密度搜索空间的工业机器人动态路径规划方法，其特征在于，包括如下步骤：
步骤1：基于三维工作空间构建八叉树的根节点；
步骤2：根据基本搜索步长生成八叉树，并局部细化搜索空间；
步骤3：构建有向距离场(sdf,signeddistancefield)；
步骤4：使用d*lite算法进行路径初步规划；
步骤5：进行路径二次优化，剔除初规划路径中的冗余路径点；
步骤6：在后续的规划中，采用局部更新提升搜索空间的重复构建效率。进一步地，步骤1利用蒙特卡洛法生成工业机器人的三维工作空间数据，并计算得出包含三维工作空间的最小立方体，立方体的长、宽、高方向与机械臂基坐标系的x、y、z三轴一一对应，在此条件下顶点位置可随机确定。进一步地，步骤2包括如下具体步骤：
步骤2.1：根据障碍物安全区域的放大距离确定基本搜索步长，计算出最大递归深度；
步骤2.2：以包含三维工作空间的最小立方体作为根节点，进行八等份的细分，直至达到最大递归深度；
步骤2.3：若有叶子节点处于k-dop包围盒内，则进一步加深深度，实现局部细分。
进一步地，步骤2.3中k-dop包围盒的构建方法如下：
k-dop是由k个面包围而成的离散有向包围盒，每个面都由一个法向和一个三维坐标来确定，法向的三个分量定义为±1，k值越大，则包围盒形状与物体越相似。
进一步地，步骤3的具体步骤如下：
步骤3.1：基于k-dop对各障碍物可能包含的节点进行快速初步判断；
步骤3.2：采用射线法进行准确判断，定义以节点为起点的任意方向射线，若射线穿过障碍物的次数为奇数，则为障碍点，将其置为1，反之则为可行点，将其置为0；
步骤3.3：对所有可行点计算距其最近的障碍点距离并保存，以matlab中的函数名bwdist命名该距离，障碍点自身的bwdist值即为0，构建有向距离场。
进一步地，d*lite算法中包括碰撞检测，所述碰撞检测的具体步骤如下：
在路径规划过程中，实时计算各碰撞球体的位置，并取距其最近的节点作近似判断，若该节点的bwdist值小于碰撞球半径，则认为发生碰撞，反之则没有发生碰撞。
进一步地，步骤4中d*lite算法的实现步骤具体如下：
从终点开始向起点进行搜索，首先对障碍点和需要进行搜索、判断的节点构建优先队列，定义g值用来保存既往搜索过程中已经产生的实际代价，g值是从终点出发搜索到当前点的实际代价；定义key值用来评价节点优先级的值，定义节点s的rhs值用来表示它的父代节点s’中g值加上这两点之间代价中的最小值；当优先队列中最小的key值小于“起点”的key值或者起点的rhs值大于它的g值时，更新、检查部分节点的值，并相应的对优先队列添加或删除节点，在该过程中，记录从终点搜索到当前点时的实际代价g，用当前点的启发函数h(n)计算节点n到起点的估计值：
其中dmin是节点n到起点坐标差值的最小分量的绝对值，dmid居中，dmax是最大分量的绝对值，在每次搜索中需要通过key值的两个分量k1,k2来判断一个节点的优先级：
k1(s)＝min(g(s),rhs(s)) h(s,sstart) km
k2(s)＝min(g(s),rhs(s))
其中km用于应对搜索空间发生变化的情况，其初值为0，当检测到有起点、终点或障碍物移动时：
km＝km h(slast,sstart)
其中slast代表的是上一个起点，sstart是机器人的当前位置，key值越小优先级越高，先比较k1值的大小，如果k1值相等再判断k2值，选出优先级最高的节点后，需要比较一个点的g值与rhs值，若相等则更新该点的key值，若g>rhs，则是当前有障碍物被删除或者第一次搜索路径，反之则是当前检测到了新的障碍物，每次搜索中，选择后继节点中rhs值最小的作为新的起点。
进一步地，步骤5的具体步骤如下：
步骤5.1：首先定义一对优化单元点a和b，在初始状况下，a点和b点都定义在终点处；
步骤5.2：a点向前遍历，若从a到b的关节插补运动过程中不发生碰撞，则用该段路径替换初规划路径中的a-b段，否则将b点移至a点的后一个路径点处；
步骤5.3：继续重复步骤5.1和步骤5.2，直至a点到达起点处，则冗余点剔除完成。
进一步地，步骤6的具体步骤如下：
当有障碍物增加或减少时，由于k-dop的投影特性，可以快速筛选出处于该障碍物k-dop包围盒内的节点，并基于射线法进行针对性的检测，当有障碍物发生移动时，则看作先后进行了一次减少障碍物和一次增加障碍物的操作，用相同的原理进行局部更新。
本发明的一种基于变密度搜索空间的工业机器人动态路径规划方法具有以下优点：本发明解决了现有离线编程软件难以实现高效无碰撞路径规划的问题，同时为实现兼顾效率与精度的机器人碰撞检测功能提供一种可供借鉴的方法。
附图说明
图1为本发明的映射方法流程图；
图2为变密度搜索空间二维示意图；
图3为初步规划效果图；
图4为冗余点剔除效果图；
图5为三维空间k-dop示意图(k＝18)；
图6为八叉树示意图；
图7为射线法(n＝1)示意图；
图8为射线法(n＝2)示意图；
图9为球体碰撞模型示意图；
图10为实施例1规划场景示意图；
图11为实施例2规划场景示意图。
具体实施方式
为了更好地理解本发明的技术方案，以下结合附图对本发明的实施方式作进一步的描述：
本发明的目的在于提供一种基于变密度搜索空间的工业机器人动态路径规划方法，以解决现有离线编程软件难以实现高效无碰撞路径规划的问题，同时为实现兼顾效率与精度的机器人碰撞检测功能提供一种可供借鉴的方法。
本发明提出的方法可以用于符合pieper准则的通用六轴机械臂，即后三关节转轴交于一点，形成球腕结构的机械臂，本发明中所用于仿真测试的机器人型号为新松sr4系列六轴机械臂。如图1所示，本发明的具体实施步骤如下：
步骤1：基于三维工作空间构建八叉树的根节点。
利用蒙特卡洛法生成工业机器人的三维工作空间数据，并计算得出包含三维工作空间的最小立方体，立方体的长、宽、高方向与机械臂基坐标系的x、y、z三轴一一对应，在此条件下顶点位置可随机确定；如图2所示为变密度搜索空间二维示意图。
步骤2：根据基本搜索步长生成八叉树，并局部细化搜索空间。
首先根据障碍物安全区域的放大距离确定基本搜索步长，从而计算出最大递归深度，以包含三维工作空间的最小立方体作为根节点，进行八等份的细分，如图6所示，直至达到最大递归深度，若有叶子节点处于k-dop包围盒内，则进一步加深深度，实现局部细分。对工作空间中存在的障碍物计算其k-dop包围盒，k-dop是由k个面包围而成的离散有向包围盒，如图5所示，每个面都由一个法向和一个三维坐标来确定，法向的三个分量定义为±1，k值越大，则包围盒形状与物体越相似。
步骤3：构建有向距离场(sdf,signeddistancefield)。
若有上述八叉树的叶子节点处于k-dop包围盒中，则进一步加深该节点深度，并构建有向距离场，首先基于k-dop对各障碍物可能包含的节点进行快速初步判断，再采用射线法进行准确判断，如图7、8所示，定义以节点为起点的任意方向射线，若射线穿过障碍物的次数为奇数，则为障碍点，将其置为1，反之则为可行点，将其置为0，对所有可行点计算距其最近的障碍点距离并保存，以matlab中的函数名bwdist命名该距离，障碍点自身的bwdist值即为0，有向距离场构建完毕。
步骤4：使用d*lite算法进行路径初步规划。
与传统d*lite算法不同的是，本发明没有将节点可达性作为先验知识，而是在规划过程中逐步进行判断，节点可达性判断包括机器人逆运动学计算及碰撞检测，在此采用球体对机器人的碰撞模型进行构建，如图9所示，在路径规划过程中，实时计算各碰撞球体的位置，并取距其最近的节点作近似判断，若该节点的bwdist值小于碰撞球半径，则认为发生碰撞，反之则没有发生碰撞。使用d*lite算法，从终点开始向起点进行搜索，首先对障碍点等需要进行搜索、判断的节点构建优先队列，并定义了g值来保存既往搜索过程中已经产生的实际代价，这与a*算法类似，但由于搜索方向和a*相反，此处的g值是从终点出发搜索到当前点的实际代价，然后定义了评价节点优先级的值，在d*lite算法中一般称其为key值，此外，还定义了节点s的rhs值，即它的父代节点s’中g值加上这两点之间代价中的最小值，当优先队列中最小的key值小于“起点”(机器人所在的位置)的key值或者起点的rhs值大于它的g值时，可以更新、检查部分节点的值，并相应的对优先队列添加或删除节点，启发函数h(n)则可计算节点n到起点的估计值：
其中dmin是节点n到起点坐标差值的最小分量的绝对值，dmid居中，dmax是最大分量的绝对值，在每次搜索中需要通过key值的两个分量k1,k2来判断一个节点的优先级：
k1(s)＝min(g(s),rhs(s)) h(s,sstart) km
k2(s)＝min(g(s),rhs(s))
其中km用于应对搜索空间发生变化的情况，其初值为0，当检测到有起点、终点或障碍物移动时：
km＝km h(slast,sstart)
其中slast代表的是上一个起点，sstart是机器人的当前位置，key值越小优先级越高，先比较k1值的大小，如果k1值相等再判断k2值，选出优先级最高的节点后，需要比较一个点的g值与rhs值，若相等则更新该点的key值，若g>rhs，则是当前有障碍物被删除或者第一次搜索路径，反之则是当前检测到了新的障碍物，每次搜索中，选择后继节点中rhs值最小的作为新的起点，如图3所示为初步规划效果图。
步骤5：进行路径二次优化，剔除初规划路径中的冗余路径点。
首先定义一对优化单元点a和b，在初始状况下，a点和b点都定义在终点处，a点向前遍历，若从a到b的关节插补运动过程中不发生碰撞，则用该段路径替换初规划路径中的a-b段，否则将b点移至a点的后一个路径点处，继续重复上述优化步骤，直至a点到达起点处，则冗余点剔除完成，如图4所示为冗余点剔除效果图。
步骤6：在后续的规划中，采用局部更新提升搜索空间的重复构建效率。
当有障碍物增加或减少时，由于k-dop的投影特性，可以快速筛选出处于该障碍物k-dop包围盒内的节点，并基于射线法进行针对性的检测，当有障碍物发生移动时，则看作先后进行了一次减少障碍物和一次增加障碍物的操作，用相同的原理进行局部更新。
下面根据实施例描述本发明，本发明的目的和效果将变得更加明显。
本发明性能测定：路径长度通过计算各相邻路径点的距离之和测定，规划用时通过计算机内置的高精度事件定时器(hpet)来计时。
实施例1：
规划场景如图10所示。
实施例2：
规划场景如图11所示。
可以理解，本发明是通过一些实施例进行描述的，本领域技术人员知悉的，在不脱离本发明的精神和范围的情况下，可以对这些特征和实施例进行各种改变或等效替换。另外，在本发明的教导下，可以对这些特征和实施例进行修改以适应具体的情况及材料而不会脱离本发明的精神和范围。因此，本发明不受此处所公开的具体实施例的限制，所有落入本申请的权利要求范围内的实施例都属于本发明所保护的范围内。
1.一种基于变密度搜索空间的工业机器人动态路径规划方法，其特征在于，包括如下步骤：
步骤1：基于三维工作空间构建八叉树的根节点；
步骤2：根据基本搜索步长生成八叉树，并局部细化搜索空间；
步骤3：构建有向距离场(sdf,signeddistancefield)；
步骤4：使用d*lite算法进行路径初步规划；
步骤5：进行路径二次优化，剔除初规划路径中的冗余路径点；
步骤6：在后续的规划中，采用局部更新提升搜索空间的重复构建效率。
2.根据权利要求1所述的基于变密度搜索空间的工业机器人动态路径规划方法，其特征在于，步骤1利用蒙特卡洛法生成工业机器人的三维工作空间数据，并计算得出包含三维工作空间的最小立方体，立方体的长、宽、高方向与机械臂基坐标系的x、y、z三轴一一对应，在此条件下顶点位置可随机确定。
3.根据权利要求1所述的基于变密度搜索空间的工业机器人动态路径规划方法，其特征在于，步骤2包括如下具体步骤：
步骤2.1：根据障碍物安全区域的放大距离确定基本搜索步长，计算出最大递归深度；
步骤2.2：以包含三维工作空间的最小立方体作为根节点，进行八等份的细分，直至达到最大递归深度；
步骤2.3：若有叶子节点处于k-dop包围盒内，则进一步加深深度，实现局部细分。
4.根据权利要求3所述的基于变密度搜索空间的工业机器人动态路径规划方法，其特征在于，步骤2.3中k-dop包围盒的构建方法如下：
k-dop是由k个面包围而成的离散有向包围盒，每个面都由一个法向和一个三维坐标来确定，法向的三个分量定义为±1，k值越大，则包围盒形状与物体越相似。
5.根据权利要求4所述的基于变密度搜索空间的工业机器人动态路径规划方法，其特征在于，步骤3的具体步骤如下：
步骤3.1：基于k-dop对各障碍物可能包含的节点进行快速初步判断；
步骤3.2：采用射线法进行准确判断，定义以节点为起点的任意方向射线，若射线穿过障碍物的次数为奇数，则为障碍点，将其置为1，反之则为可行点，将其置为0；
步骤3.3：对所有可行点计算距其最近的障碍点距离并保存，以matlab中的函数名bwdist命名该距离，障碍点自身的bwdist值即为0，构建有向距离场。
6.根据权利要求1所述的基于变密度搜索空间的工业机器人动态路径规划方法，其特征在于，d*lite算法中包括碰撞检测，所述碰撞检测的具体步骤如下：
在路径规划过程中，实时计算各碰撞球体的位置，并取距其最近的节点作近似判断，若该节点的bwdist值小于碰撞球半径，则认为发生碰撞，反之则没有发生碰撞。
7.根据权利要求6所述的基于变密度搜索空间的工业机器人动态路径规划方法，其特征在于，步骤4中d*lite算法的实现步骤具体如下：
从终点开始向起点进行搜索，首先对障碍点和需要进行搜索、判断的节点构建优先队列，定义g值用来保存既往搜索过程中已经产生的实际代价，g值是从终点出发搜索到当前点的实际代价；定义key值用来评价节点优先级的值，定义节点s的rhs值用来表示它的父代节点s’中g值加上这两点之间代价中的最小值；当优先队列中最小的key值小于“起点”的key值或者起点的rhs值大于它的g值时，更新、检查部分节点的值，并相应的对优先队列添加或删除节点，在该过程中，用记录从终点到搜索当前点时的实际代价g，用当前点的启发函数h(n)计算节点n到起点的估计值：
其中dmin是节点n到起点坐标差值的最小分量的绝对值，dmid居中，dmax是最大分量的绝对值，在每次搜索中需要通过key值的两个分量k1,k2来判断一个节点的优先级：
k1(s)＝min(g(s),rhs(s)) h(s,sstart) km
k2(s)＝min(g(s),rhs(s))
其中km用于应对搜索空间发生变化的情况，其初值为0，当检测到有起点、终点或障碍物移动时：
km＝km h(slast,sstart)
其中slast代表的是上一个起点，sstart是机器人的当前位置，key值越小优先级越高，先比较k1值的大小，如果k1值相等再判断k2值，选出优先级最高的节点后，需要比较一个点的g值与rhs值，若相等则更新该点的key值，若g>rhs，则是当前有障碍物被删除或者第一次搜索路径，反之则是当前检测到了新的障碍物，每次搜索中，选择后继节点中rhs值最小的作为新的起点。
8.根据权利要求1所述的基于变密度搜索空间的工业机器人动态路径规划方法，其特征在于，步骤5的具体步骤如下：
步骤5.1：首先定义一对优化单元点a和b，在初始状况下，a点和b点都定义在终点处；
步骤5.2：a点向前遍历，若从a到b的关节插补运动过程中不发生碰撞，则用该段路径替换初规划路径中的a-b段，否则将b点移至a点的后一个路径点处；
步骤5.3：继续重复步骤5.1和步骤5.2，直至a点到达起点处，则冗余点剔除完成。
9.根据权利要求4所述的基于变密度搜索空间的工业机器人动态路径规划方法，其特征在于，步骤6的具体步骤如下：
当有障碍物增加或减少时，由于k-dop的投影特性，可以快速筛选出处于该障碍物k-dop包围盒内的节点，并基于射线法进行针对性的检测，当有障碍物发生移动时，则看作先后进行了一次减少障碍物和一次增加障碍物的操作，用相同的原理进行局部更新。
技术总结