CN108356819B - 基于改进a*算法的工业机械臂无碰撞路径规划方法 - Google Patents

Abstract

基于改进A*算法的工业机械臂无碰撞路径规划方法，先对六自由度机械臂进行参数化建模，并建立机械臂的正逆运动学模型；再建立机械臂及环境的OBB包围盒；然后设置A*算法的估价函数和搜索步长，采用基于分离轴的碰撞检测算法在搜索的每一步中判断机械臂是否与环境发生碰撞；再根据关节转动角度之和最小且不发生碰撞的原则，在机械臂的六维关节空间中进行搜索，得到无碰撞的路径；本发明将传统的A*算法的搜索步长设置为变步长，能够实现六自由度的机械臂在C空间的无碰撞路径搜索，具有计算简单，搜索效率高的优点。

Description

基于改进A*算法的工业机械臂无碰撞路径规划方法

技术领域

本发明涉及工业机器人路径规划技术领域，具体涉及基于改进A*算法的工业机械臂无碰撞路径规划方法。

背景技术

路径规划是控制工业机器人按照人工预设的路径运动完成作业任务，决定了机器人的工作方式与工作效率。路径规划技术是工业机器人自动化生产的基础，也是智能制造业必不可缺的重要组成部分。针对六自由度的工业机械臂，传统的无碰撞路径规划方法是基于C空间法(构型空间法)的路径规划，但是在求解C空间的障碍物边界时，需要将工作空间的障碍物区域转换到C空间，这需要大量的计算，并且精确的障碍域难以获得。

现有的基于A*算法的路径规划，很多是在工作空间(笛卡尔空间)进行路径搜索，这样规划出的最短路径并不是关节变化量最小的路径，所以需要在C空间进行路径规划。而针对C空间的路径规划，由于在六维空间的搜索问题的复杂性，现有的算法大多忽略了几个自由度，对搜索空间进行了一定的简化，这样得到的路径也是简化后的，并不能够保证路径的安全性。为了得到准确避障的路径，往往需要设置较小的搜索步长，搜索步长与避障准确度是相互矛盾的，如何在保证无碰撞的前提下提高搜索效率是需要解决的问题。

综上所述，针对六自由度机械臂在C空间的路径规划，传统的无碰撞路径规划的方法需要大量计算，并且精确的障碍域难以获得。传统的A*算法不适用于六维空间的搜索问题，并且搜索效率有待提高。

发明内容

为了解决上述问题，本发明的目的在于提供一种基于改进A*算法的工业机械臂无碰撞路径规划方法，可以实现六自由度机械臂的无碰撞路径规划，具有计算简单，搜索效率高的优点。

为了达到上述目的，本发明采取的技术方案为：

基于改进A*算法的工业机械臂无碰撞路径规划方法，包括以下步骤：

步骤1、对六自由度机械臂进行参数化建模，并建立机械臂的正逆运动学模型：

采用D-H法描述机械臂的位姿，建立机械臂的连杆坐标系，确定机械臂的D-H参数；

根据D-H参数，连杆i和连杆i-1相对位置的齐次变换矩阵^i-1A_i为：

式中C_i＝cosθ_i，S_i＝sinθ_i，其中i表示机械臂的连杆个数，i＝1,2,3,4,5,6；

将各关节的齐次变换矩阵相乘，得到了机械臂末端执行器的齐次变换矩阵为：

其中，p_x,p_y,p_z表示平移变换在x，y，z三个方向的分量，n＝[n_x n_y n_z]，o＝[o_x o_yo_z]，a＝[a_x a_y a_z]，分别为绕x，y，z三个方向旋转矢量；

步骤2、建立机械臂及环境的OBB包围盒：利用soildwork建模软件建立机械臂及环境模型，并将机械臂及环境模型另存为STL格式文件，读取STL格式文件，从而得到模型中各个物体的三角形网格模型，采用OBB包围盒算法建立机械臂及环境障碍物的OBB包围盒；OBB包围盒方法流程为：

2.1)通过物体的三角形网格模型中的三角面片信息计算每个三角形平面的法向矢量，并在每个三角面片上建立一个坐标系，原点为三角形任意顶点，三角形平面为x-y平面，z轴方向为三角形平面的法向矢量；

2.2)遍历每一个三角面片的平面法向矢量，依次绕每一个法向矢量旋转三角面片的坐标系，寻找物体的三角形网格模型投影到三角面片的x-y平面面积最小的方向，记录下在旋转后的坐标系下的包围盒的体积；

2.3)将最小体积对应的坐标系的方向作为包围盒的三个方向向量，将物体的三角形网格模型中所有顶点投影到三个方向向量中，求出每个方向的最大值和最小值，得到OBB包围盒的八个顶点；

步骤3、设置A*算法的估价函数和搜索步长，采用基于分离轴的碰撞检测算法在搜索的每一步中判断机械臂是否与环境发生碰撞：

机械臂在关节空间中某个节点的构型矢量为q_i，其中包括六个旋转关节的角度变幅，即q_i＝(q_i1,q_i2,q_i3,q_i4,q_i5,q_i6)，距离估价函数为所有关节转动的角度之和，在估价函数中加入一项，即当前节点到目标节点的各关节位移变化量之和，估价函数如下：

在机械臂的作业场景中，障碍物区域设置0.1到0.2度之间较小的搜索步长，以提高搜索精度；而在非障碍物区域设置1到1.5度之间较大的搜索步长，以提高搜索速度；

采用基于分离轴相交测试的方法在搜索过程中进行碰撞检测，对于包围盒子A与B的碰撞检测算法流程为：

3.1)计算两个包围盒子潜在的15条分离轴，对每一条分离轴进行相交测试；

3.2)对每一条分离轴，将八个顶点做投影，得到两个包围盒投影的最大值和最小值分别为maxA、minA、maxB和minB,若maxA<mimB或maxB<minA，则在此分离轴下，两个包围盒分离，否则相交；

3.3)若对所有的分离轴均满足maxA<mimB或maxB<minA，则两个包围盒分离，否则两个包围盒相交；

步骤4、根据关节转动角度之和最小且不发生碰撞的原则，在机械臂的六维关节空间中进行搜索，得到无碰撞的路径；

设置两个存储矩阵，分别为OPEN表和CLOSED表，其中OPEN存放所有搜索节点，CLOSED中存放每一步搜索到的最佳节点，并将它们均初始化为空，机械臂起始位姿的构型矢量为q_ori，目标位姿的构型矢量为q_fin，机械臂当前位姿的构型矢量为q_cur，当前位姿的相邻位姿构型矢量为q_{nei_i}；

将机械臂的初始位姿的构型矢量、它的父亲位姿的构型矢量，即它本身和与之对应的g、h、f合并为一个向量放入OPEN表的第一行，即：

OPEN(1,:)＝(q_ori,q_ori,g(q_ori),h(q_ori),f(q_ori))

OPEN(1,:)是一个1×15的矢量，存放到OPEN表的第一行，并将这个向量放入CLOSED表的第一行；

重复进行以下步骤，直到q_cur＝q_fin，完成搜索任务，若OPEN表变为空，则跳出循环，搜索失败；

4.1)OPEN表中找到f(q_i)值最小的那一行向量，将该行向量作为当前搜索节点，并将该节点向量放入CLOSED表中，同时从OPEN表中删除该节点向量；

4.2)判断当前搜索节点是否为目标节点，如果是，那么搜索成功；否则继续下一步；

4.3)对于机械臂而言，每一个关节有三种变化方式，分别是保持不变、增加步长，减少步长，这样在一个六维空间中，当前搜索节点就有3⁶-1＝728个邻近的搜索节点，设第i个邻近节点为q_{nei_i}；

4.4)对于每一个邻近的搜索节点执行如下操作：

a)对邻近的搜索节点进行碰撞检测，如果发生了碰撞，则忽略该节点；

b)判断该邻近的搜索节点是否超出了该关节的最大角度或者最小角度，如果超限，则转至下一个邻近节点；

c)若邻近节点既没有发生碰撞也没有超限，则往下执行；

d)判断该节点矢量是否在OPEN表中，如果在OPEN表中，则通过节点索引函数查找该节点在OPEN表中的位置，然后判断当前q_{nei_i}节点中的g_{nei_i}与OPEN表中已经存在的g_{old_i}的大小关系，如果g_nei-i＜g_{old_i}，则就将当前的节点、当前节点的父亲节点以及新的g、h、f更新至OPEN表中原来旧节点的位置；

e)若该节点矢量不在OPEN表中，则直接把该节点矢量、当前节点矢量、该节点矢量的g、h、f值放入到OPEN表新的一行中，同时将当前节点、当前节点的父亲节点、当前节点的g、h、f值放入到CLOSED表新的一行中；

当搜索成功后，从CLOSED表中的最后一行开始，通过节点索引函数依次查找当前节点的父亲节点，一直到起始节点，就完成了机械臂在关节空间的构型搜索任务。

本发明的有益效果为：

1.本发明无需求解障碍域，通过碰撞检测规避障碍，具有计算简单、方便，能够实现较准确的避障。

2.本发明对传统的A*算法的估价函数进行了改进，克服了六维空间搜索问题的复杂性，能够实现六自由度的机械臂在C空间的无碰撞路径搜索。

3.本发明将传统的A*算法的搜索步长设置为变步长，能够提高搜索效率。

附图说明

图1为本发明方法的流程图。

图2为机械臂的初始位姿。

图3为机械臂的目标位姿。

图4为路径规划的结果。

具体实施方式

下面结合附图和实例对本发明作详细描述。

参照图1，基于改进A*算法的工业机械臂无碰撞路径规划方法，包括以下步骤：

步骤1、对六自由度机械臂进行参数化建模，并建立机械臂的正逆运动学模型：

本实施例以博实RBT-6T/S03S机械臂为例，采用D-H法描述机械臂的位姿，建立机械臂的连杆坐标系，确定机械臂的D-H参数，该机械臂的D-H参数如表1所示，

表1博实RBT-6T/S03S机械臂D-H参数

根据D-H参数，连杆i和连杆i-1相对位置的齐次变换矩阵^i-1A_i为：

式中C_i＝cosθ_i，S_i＝sinθ_i，其中i表示机械臂的连杆个数，i＝1,2,3,4,5,6；

将各关节的齐次变换矩阵相乘，得到了机械臂末端执行器的齐次变换矩阵为：

其中，p_x,p_y,p_z表示平移变换在x，y，z三个方向的分量，n＝[n_x n_y n_z]，o＝[o_x o_yo_z]，a＝[a_x a_y a_z]，分别为绕x，y，z三个方向旋转矢量；

步骤2、建立机械臂及环境的OBB包围盒：利用soildwork建模软件建立机械臂及环境模型，并将机械臂及环境模型另存为STL格式文件，读取STL格式文件，从而得到模型中各个物体的三角形网格模型，采用OBB包围盒算法建立机械臂及环境障碍物的OBB包围盒；OBB包围盒方法流程为：

2.1)通过物体的三角形网格模型中的三角面片信息计算每个三角形平面的法向矢量，并在每个三角面片上建立一个坐标系，原点为三角形任意顶点，三角形平面为x-y平面，z轴方向为三角形平面的法向矢量；

2.2)遍历每一个三角面片的平面法向矢量，依次绕每一个法向矢量旋转三角面片的坐标系，寻找物体的三角形网格模型投影到三角面片的x-y平面面积最小的方向，记录下在旋转后的坐标系下的包围盒的体积；

2.3)将最小体积对应的坐标系的方向作为包围盒的三个方向向量，将物体的三角形网格模型中所有顶点投影到三个方向向量中，求出每个方向的最大值和最小值，得到OBB包围盒的八个顶点；

步骤3、设置A*算法的估价函数和搜索步长，采用基于分离轴的碰撞检测算法在搜索的每一步中判断机械臂是否与环境发生碰撞：

机械臂在关节空间中某个节点的构型矢量为q_i，其中包括六个旋转关节的角度变幅，即q_i＝(q_i1,q_i2,q_i3,q_i4,q_i5,q_i6)，距离估价函数为所有关节转动的角度之和，为了提高估价函数的启发性，在估价函数中加入一项，即当前节点到目标节点的各关节位移变化量之和，估价函数如下：

在机械臂的作业场景中，障碍物区域设置0.1到0.2度之间较小的搜索步长，以提高搜索精度；而在非障碍物区域设置1到1.5度之间较大的搜索步长，以提高搜索速度；

采用基于分离轴相交测试的方法在搜索过程中进行碰撞检测，对于包围盒A与B的碰撞检测算法流程：

3.1)计算两个包围盒潜在的15条分离轴，对每一条分离轴进行相交测试；

3.2)对每一条分离轴，将八个顶点做投影，得到两个包围盒投影的最大值和最小值分别为maxA、minA、maxB和minB,若maxA<mimB或maxB<minA，则在此分离轴下，两个包围盒分离，否则相交；

3.3)若对所有的分离轴均满足maxA<mimB或maxB<minA，则两个包围盒分离，否则两个包围盒相交；

步骤4、根据关节转动角度之和最小且不发生碰撞的原则，在机械臂的六维关节空间中进行搜索，得到无碰撞的路径；

其中OPEN存放所有搜索节点，CLOSED中存放每一步搜索到的最佳节点，并将它们均初始化为空；机械臂初始位姿的构型矢量为q_ori，如图2所示，目标位姿的构型矢量为q_fin，如图3所示，机械臂当前位姿的构型矢量为q_cur，当前位姿的相邻位姿构型矢量为q_{nei_i}，A*算法的流程：

将机械臂的初始位姿的构型矢量、它的父亲位姿的构型矢量，即它本身和与之对应的g、h、f合并为一个向量放入OPEN表的第一行，即：

OPEN(1,:)＝(q_ori,q_ori,g(q_ori),h(q_ori),f(q_ori))

OPEN(1,:)是一个1×15的矢量，存放到OPEN表的第一行，并将这个向量放入CLOSED表的第一行；

重复进行以下步骤，直到q_cur＝q_fin，完成搜索任务；若OPEN表变为空，则跳出循环，搜索失败；

4.1)OPEN表中找到f(q_i)值最小的那一行向量，将该行向量作为当前搜索节点，并将该节点向量放入CLOSED表中，同时从OPEN表中删除该节点向量；

4.2)判断当前搜索节点是否为目标节点，如果是，那么搜索成功；否则继续下一步；

4.3)对于机械臂而言，每一个关节有三种变化方式，分别是保持不变、增加步长，减少步长，这样在一个六维空间中，当前搜索节点就有3⁶-1＝728个邻近的搜索节点，设第i个邻近节点为q_{nei_i}；

4.4)对于每一个邻近的搜索节点执行如下操作：

a)对邻近的搜索节点进行碰撞检测，如果发生了碰撞，则忽略该节点；

b)判断该邻近的搜索节点是否超出了该关节的最大角度或者最小角度，如果超限，则转至下一个邻近节点；

c)若邻近节点既没有发生碰撞也没有超限，则往下执行；

d)判断该节点矢量是否在OPEN表中，如果在OPEN表中，则通过节点索引函数查找该节点在OPEN表中的位置，然后判断当前q_{nei_i}节点中的g_{nei_i}与OPEN表中已经存在的g_{old_i}的大小关系，如果g_nei-i＜g_{old_i}，则就将当前的节点、当前节点的父亲节点以及新的g、h、f更新至OPEN表中原来旧节点的位置；

e)若该节点矢量不在OPEN表中，则直接把该节点矢量、当前节点矢量、该节点矢量的g、h、f值放入到OPEN表新的一行中，同时将当前节点、当前节点的父亲节点、当前节点的g、h、f值放入到CLOSED表新的一行中；

当搜索成功后，从CLOSED表中的最后一行开始，通过节点索引函数依次查找当前节点的父亲节点，一直到起始节点，就完成了机械臂在关节空间的构型搜索任务。

参照图4，图4为机械臂的路径规划的结果，固定步长和变步长的搜索效率对比如表2所示，从表2可以看出变步长的A星算法相对与固定步长的A星算法，搜索点数减少了23.7％，搜索时间减少了19.35％，验证了变步长的A星算法的优越性。

表2变步长和固定步长搜索效率对比

Claims (1)

1.基于改进A*算法的工业机械臂无碰撞路径规划方法，其特征在于，包括以下步骤：

步骤1、对工业机械臂进行参数化建模，并建立机械臂的正逆运动学模型：

采用D-H法描述机械臂的位姿，建立机械臂的连杆坐标系，确定机械臂的D-H参数；

根据D-H参数，连杆i和连杆i-1相对位置的齐次变换矩阵^i-1A_i为：

式中C_i＝cosθ_i，S_i＝sinθ_i，其中i表示机械臂的连杆个数，i＝1,2,3,4,5,6；

将各关节的齐次变换矩阵相乘，得到了机械臂末端执行器的齐次变换矩阵为：

其中，p_x,p_y,p_z表示平移变换在x，y，z三个方向的分量，n＝[n_x n_y n_z]，o＝[o_x o_y o_z]，a＝[a_x a_y a_z]，分别为绕x，y，z三个方向旋转矢量；

步骤2、建立机械臂及环境的OBB包围盒：利用soildwork建模软件建立机械臂及环境模型，并将机械臂及环境模型另存为STL格式文件，读取STL格式文件，从而得到模型中各个物体的三角形网格模型，采用OBB包围盒算法建立机械臂及环境障碍物的OBB包围盒；OBB包围盒方法流程为：

2.1)通过物体的三角形网格模型中的三角面片信息计算每个三角形平面的法向矢量，并在每个三角面片上建立一个坐标系，原点为三角形任意顶点，三角形平面为x-y平面，z轴方向为三角形平面的法向矢量；

2.2)遍历每一个三角面片的平面法向矢量，依次绕每一个法向矢量旋转三角面片的坐标系，寻找物体的三角形网格模型投影到三角面片的x-y平面面积最小的方向，记录下在旋转后的坐标系下的包围盒的体积；

2.3)将最小体积对应的坐标系的方向作为包围盒的三个方向向量，将物体的三角形网格模型中所有顶点投影到三个方向向量中，求出每个方向的最大值和最小值，得到OBB包围盒的八个顶点；

步骤3、设置A*算法的距离估价函数和搜索步长，采用基于分离轴的碰撞检测算法在搜索的每一步中判断机械臂是否与环境发生碰撞：

机械臂在关节空间中某个节点的构型矢量为q_i，其中包括六个旋转关节的角度变幅，即q_i＝(q_i1,q_i2,q_i3,q_i4,q_i5,q_i6)，距离估价函数为所有关节转动的角度之和，在距离估价函数中加入一项，即当前节点到目标节点的各关节位移变化量之和，距离估价函数如下：

在机械臂的作业场景中，障碍物区域设置0.1到0.2度之间较小的搜索步长；而在非障碍物区域设置1到1.5度之间较大的搜索步长；

采用基于分离轴相交测试的方法在搜索过程中进行碰撞检测，对于包围盒子A与B的碰撞检测算法流程为：

3.1)计算两个包围盒子潜在的15条分离轴，对每一条分离轴进行相交测试；

3.2)对每一条分离轴，将八个顶点做投影，得到两个包围盒投影的最大值和最小值分别为maxA、minA、maxB和minB,若maxA<mimB或maxB<minA，则在此分离轴下，两个包围盒分离，否则相交；

3.3)若对所有的分离轴均满足maxA<mimB或maxB<minA，则两个包围盒分离，否则两个包围盒相交；

步骤4、根据关节转动角度之和最小且不发生碰撞的原则，在机械臂的六维关节空间中进行搜索，得到无碰撞的路径；

设置两个存储矩阵，分别为OPEN表和CLOSED表，其中OPEN存放所有搜索节点，CLOSED中存放每一步搜索到的最佳节点，并将它们均初始化为空，机械臂起始位姿的构型矢量为q_ori，目标位姿的构型矢量为q_fin，机械臂当前位姿的构型矢量为q_cur，当前位姿的相邻位姿构型矢量为q_{nei_i}；

将机械臂的初始位姿的构型矢量、它的父亲位姿的构型矢量，即它本身和与之对应的g、h、f合并为一个向量放入OPEN表的第一行，即：

OPEN(1,:)＝(q_ori,q_ori,g(q_ori),h(q_ori),f(q_ori))

OPEN(1,:)是一个1×15的矢量，存放到OPEN表的第一行，并将这个向量放入CLOSED表的第一行；

重复进行以下步骤，直到q_cur＝q_fin，完成搜索任务，若OPEN表变为空，则跳出循环，搜索失败；

4.1)OPEN表中找到f(q_i)值最小的那一行向量，将该行向量作为当前搜索节点，并将该节点向量放入CLOSED表中，同时从OPEN表中删除该节点向量；

4.2)判断当前搜索节点是否为目标节点，如果是，那么搜索成功；否则继续下一步；

4.3)对于机械臂而言，每一个关节有三种变化方式，分别是保持不变、增加步长，减少步长，这样在一个六维空间中，当前搜索节点就有3⁶-1＝728个邻近的搜索节点，设第i个邻近节点为q_{nei_i}；

4.4)对于每一个邻近的搜索节点执行如下操作：

a)对邻近的搜索节点进行碰撞检测，如果发生了碰撞，则忽略该节点；

b)判断该邻近的搜索节点是否超出了该关节的最大角度或者最小角度，如果超限，则转至下一个邻近节点；

c)若邻近节点既没有发生碰撞也没有超限，则往下执行；

d)判断该节点矢量是否在OPEN表中，如果在OPEN表中，则通过节点索引函数查找该节点在OPEN表中的位置，然后判断当前q_{nei_i}节点中的g_{nei_i}与OPEN表中已经存在的g_{old_i}的大小关系，如果g_{nei_i}＜g_{old_i}，则就将当前的节点、当前节点的父亲节点以及新的g、h、f更新至OPEN表中原来旧节点的位置；

e)若该节点矢量不在OPEN表中，则直接把该节点矢量、当前节点矢量、该节点矢量的g、h、f值放入到OPEN表新的一行中，同时将当前节点、当前节点的父亲节点、当前节点的g、h、f值放入到CLOSED表新的一行中；

当搜索成功后，从CLOSED表中的最后一行开始，通过节点索引函数依次查找当前节点的父亲节点，一直到起始节点，就完成了机械臂在关节空间的构型搜索任务。

Images