CN108705532A - 一种机械臂避障路径规划方法、设备及存储设备 - Google Patents

Abstract

本发明提供了一种机械臂避障路径规划方法、设备及存储设备，一种机械臂避障路径规划方法，采用改进的A*算法，搜索机械臂末端从起始节点到目标节点的避障路径，解决机械臂与障碍物碰撞的问题，并根据三次均匀B样条曲线方法对所述避障路径进行平滑处理，解决由于机械臂抖动而引起的控制误差大的问题；一种机械臂避障路径规划设备及存储设备，用于实现机械臂避障路径规划方法。本发明的有益效果是：本发明提供的技术方案避免了机械臂在工作过程中与障碍物发生碰撞，降低了机械臂的控制误差，提高了所述机械臂的路径跟踪精度，减少了机械臂在运动过程中产生的机械震动，延长了机械臂的使用寿命。

Description

一种机械臂避障路径规划方法、设备及存储设备

技术领域

本发明涉及机械臂避障技术领域，尤其涉及一种机械臂避障路径规划方法、设备及存储设备。

背景技术

在人类科学技术日新月异的发展潮流中，机器人技术以其广泛的应用范围、灵活的使用方式，以及在特种作业中的独特需求，成为了高新技术的必然发展趋势之一，也因此受到了各个国家越来越多的关注。近几年来，机器人技术在航空航天领域中的应用变得越来越重要。毫无疑问，机械臂在航空航天领域中起着至关重要的作用。机械臂工作性能的强弱、稳定性能的高低，直接影响着在轨操作的诸多方面。避障路径规划是机械臂在运动过程中很重要的一个环节，其关系到机械臂在移动过程中的精确度和稳定性，以及响应时间。特别的，在较为复杂的环境当中，机械臂需要在确保自身安全的情况下规划出一条可行的工作路径。目前，关于机械臂避障路径规划的研究多不考虑关于实时避障路径规划问题以及环境中出现动态障碍物的情况，导致了相关方法无法普遍应用于较为复杂的场景当中。因此，研究如何实现机械臂实时避障路径规划以及考虑环境中出现障碍物的情况具有重要的意义。

发明内容

为了解决上述问题，本发明提供了一种机械臂避障路径规划方法、设备及存储设备，一种机械臂避障路径规划方法，主要包括以下步骤：

S101：通过机械臂中的外部传感器，检测作业目标及障碍物的位姿数据，以所述机械臂末端的初始位置作为避障路径规划的起始节点，以作业目标的位置作为所述避障路径规划的目标节点；

S102：采用C空间法对所述机械臂的作业空间进行三维建模,得到C空间的网格化模型；

S103：根据所述网格化模型，采用改进的A*算法在网格化的C空间内进行避障路径搜索，得到所述机械臂末端自起始节点到目标节点的避障路径；

S104：采用三次均匀B样条曲线方法对所述避障路径进行平滑处理，将平滑处理过的所述避障路径作为最终的路径规划结果。

进一步地，在步骤S102中，使用网格法将所述C空间用若干个相等的立方体表示，每一个所述立方体都对应有一个单独的索引值，所述障碍物空间内的所述立方体对应的索引值为index(X,Y,Z)，所述自由空间内的所述立方体对应的索引值为index(x,y,z)；索引值index(m,n,l)中的(m,n,l)分别表示从原点0到所述索引点处在x、y和z方向上所述立方体的个数，取所述立方体的边长为length，则所述索引点处的坐标为：其中，或由所述自由空间和障碍物空间中各个索引点的坐标得到C空间的网格化模型。

进一步地，在步骤S103中，建立改进的A*算法的估价函数如公式(1)所示：

f(n)＝g(n)+h(n) (1)

其中，f(n)为当前节点的估价函数，g(n)为起始节点到当前节点的实际代价，表示搜索广度的优先趋势，h(n)为启发函数，表示从当前节点到目标节点的最佳路径的估计代价，体现了搜索中的启发信息；

采用改进的A*算法搜索避障路径的过程为：

S201：将初始节点的坐标S(x,y,z)添加到open列表；

S202：检测所述open列表是否为空？若是，则到步骤S203；若否，则到步骤S204；

S203：结束路径搜索，搜索不到避障路径；

S204：选择所述open列表中未计算过的一个节点，并计算所有节点的f(n)值；

S205：判断是否所述节点的f(n)值最小？若是，则到步骤S206；若否，则到步骤S207；f(n)值最小的节点被称为当前节点；

S206：将路径点代入逆运动学方程，得到逆解，采用时间—能量最优法选择一组逆解，所述逆解即为所述机械臂末端到达目标节点的关节角；采用D-H法将关节角代入正运动学方程，得到关节线段方程，然后到步骤S208；

S207：将所述节点从所述open列表中删除，加入到closed列表中，回到步骤S202；

S208：求解所述关节线段方程与采用长方体包络法得到的障碍物包络的位置关系，进行碰撞检测；

S209：判断所述关节线段方程与所述障碍物包络是否碰撞？若是，则到步骤S210；若否，则到步骤S211；

S210：判断是否还有未检测的逆解？若是，则到步骤S212；若否，则回到步骤S202；

S211：将当前节点加入所述open列表，然后到步骤S213；

S212：将当前关节角舍弃，在剩下的逆解中选择一组逆解，回到步骤S206；

S213：设置当前节点为父节点，扩展后继节点，将生成的后继节点加入所述open列表；

S214：建立后继节点返回到当前节点的指针；

S215：根据所述指针，判断所述机械臂末端是否找到目标节点？若是，则到步骤S216；若否，则回到步骤S202；

S216：根据起始节点、路径点和目标节点，并经过碰撞检测，最终找到所述避障路径，并保存所述避障路径。

进一步地，在步骤S104中，采取所述三次均匀B样条对所述避障路径进行平滑处理的过程为：每四个相邻的控制点可以确定一段所述三次均匀B样条曲线，n+2个控制点可确定n-1条三次均匀B样条曲线列{S₁,S₂,…,S_n-1}，单条曲线S_i是参变量u的函数，如公式(2)所示：

其中，U为参量阵，且U＝[1 u u² u³]，u∈[0,1]，M为所述三次均匀B样条曲线的常系数矩阵，C_i为控制点阵，且C_i＝[ω_i-1c_i-1 ω_ic_i ω_i+1c_i+1 ω_i+2c_i+2]，ω_i为权因子阵，且ω_i＝[ω_i-1 ω_i ω_i+1 ω_i+2]，i＝1,2，…,n-1；

将所述三次均匀B样条曲线做归一化处理，将每一条所述三次均匀B样条曲线的局部参变量u∈[0,1]依次拼接得到全局参变量t∈[0,n-1]，所述三次均匀B样条曲线列在全局参变量下表示为分段函数：

其中，t∈[0,n-1]为全局参变量，S(i)为第i条所述三次均匀B样条曲线，i＝1,2，…,n-2；

根据所述三次均匀B样条曲线对所述避障路径进行平滑处理，使所述机械臂能够平稳的沿着所述避障路径从开始节点到达目标节点，且避开障碍物，提高了所述机械臂的路径跟踪精度。

一种存储设备，所述存储设备存储指令及数据用于实现一种机械臂避障路径规划方法。

一种机械臂避障路径规划设备，包括：处理器及所述存储设备；所述处理器加载并执行所述存储设备中的指令及数据用于实现一种机械臂避障路径规划方法。

本发明提供的技术方案带来的有益效果是：本发明提供的技术方案避免了机械臂在工作过程中与障碍物发生碰撞，降低了机械臂的控制误差，提高了所述机械臂的路径跟踪精度，减少了机械臂在运动过程中产生的机械震动，延长了机械臂的使用寿命。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明实施例中一种机械臂避障路径规划方法的流程图；

图2是本发明实施例中采用改进的A*算法进行路径搜索的流程图；

图3是本发明实施例中硬件设备工作的示意图。

具体实施方式

为了对本发明的技术特征、目的和效果有更加清楚的理解，现对照附图详细说明本发明的具体实施方式。

本发明的实施例提供了一种机械臂避障路径规划方法、设备及存储设备。

请参考图1，图1是本发明实施例中一种机械臂避障路径规划方法的流程图，具体包括如下步骤：

S101：通过机械臂中的外部传感器，得到作业目标及障碍物的位姿数据，以所述机械臂末端的初始位置作为避障路径规划的起始节点，以作业目标的位置作为所述避障路径规划的目标节点；

S102：采用C空间法对所述机械臂的作业空间进行三维建模,得到C空间的网格化模型；所述C空间用来表示物体自由度的参数空间，所述C空间分为自由空间和障碍物空间，所述自由空间为所述机械臂及其他固定物体互不干涉的C空间，所述障碍物空间为所述C障碍物所处的C空间，所述C空间中的障碍物被称为C障碍物，为避免所述机械臂与所述C障碍物发生碰撞，需对所述障碍物空间进行扩展，所述障碍物空间的扩展过程为：将所述机械臂外形简化为圆柱形，取所述机械臂大臂的半径为d，对所述障碍物空间向外以所述C障碍物为圆心，d为半径做各个方向上的扩展，凡是与扩展范围内相交的立方体都纳入到所述障碍物空间；使用网格法将所述C空间用若干个相等的立方体表示，每一个所述立方体都对应有一个单独的索引值，所述障碍物空间内的所述立方体对应的索引值为index(X,Y,Z)，所述自由空间内的所述立方体对应的索引值为index(x,y,z)，所述索引值index(x,y,z)对应于所述自由空间内的所述立方体某个定点，可以是所述立方体的8个顶点中的任一顶点，所述顶点被称为索引点，所述自由空间中的所述索引点的选择必须同一方向，在本实施例中，统一选择所述立方体最接近于原点0的顶点为所述索引点；索引值index(m,n,l)中的(m,n,l)分别表示从原点0到所述索引点处在x、y和z方向上所述立方体的个数，取所述立方体的边长为length，则所述索引点处的坐标为：其中，或由所述自由空间和障碍物空间中各个索引点的坐标得到C空间的网格化模型；

S103：根据所述网格化模型，采用改进的A*算法在网格化的所述自由空间内进行路径搜索，得到所述机械臂末端自起始节点到终止节点的避障路径；将网格宽度作为路径搜索过程中的基本步长，网格中的任意两个节点的连接线段可构成C空间中的一条路径，判断所述路径安全的两个条件为：所述路径两端点必须处于自由空间和所述路径不能与所述C障碍物相交或相切；其中，建立改进的A*算法的估价函数如公式(1)所示：

f(n)＝g(n)+h(n) (1)

其中，f(n)为当前节点的估价函数，g(n)为起始节点到当前节点的实际代价，表示搜索广度的优先趋势，h(n)为启发函数，表示从当前节点到目标节点的最佳路径的估计代价，体现了搜索中的启发信息；

估计代价h(i)取当前节点与目标节点在C空间中的距离，如公式(2)所示：

其中，h(i)为当前节点与目标节点在C空间中的距离，(x_i,y_i,z_i)为当前节点的坐标，(x_d,y_d,z_d)为目标节点的坐标；

请参考图2，图2是本发明实施例中采用改进的A*算法进行路径搜索的流程图，具体步骤包括：

S201：将初始节点的坐标S(x,y,z)添加到open列表；

S202：检测所述open列表是否为空？若是，则到步骤S203；若否，则到步骤S204；

S203：结束路径搜索，搜索不到避障路径；

S204：选择所述open列表中未计算过的一个节点，并计算所有节点的f(n)值；

S205：判断是否所述节点的f(n)值最小？若是，则到步骤S206；若否，则到步骤S207；f(n)值最小的节点被称为当前节点；

S206：将路径点代入逆运动学方程，得到逆解，采用时间—能量最优法选择一组逆解，所述逆解即为所述机械臂末端到达目标节点的关节角；采用D-H法将关节角代入正运动学方程，得到关节线段方程，然后到步骤S208；采用动态步长的方式，通过估价函数f(n)选出所述路径点；采用时间—能量最优法选择逆解的方法为：如果所述C空间中没有所述C障碍物，则采用最短行程原则，即尺寸大的所述机械臂关节角移动距离少，尺寸小的所述机械臂关节角移动距离大；如果所述C空间中有所述C障碍物，若得到多组可行解，则结合最短行程原则选择1组最优解，所述最优解的选取与路径点的选取有关，即将多组可行解分别与前一个路径点的解做差值，并比较所述差值大小，选择所述差值最小的1组可行解为最优解；在实施例中，以六自由度的机械臂为例，则所述最短行程原则的优化步骤如下：

S301：已知所述机械臂各关节角θ₁、θ₂、θ₃、θ₄、θ₅、θ₆；

S302：通过所述机械臂的正逆运动学方程，得到8组逆解θ_n1、θ_n2、θ_n3、θ_n4、θ_n5、θ_n6，其中n＝1,2,3,…,8；

S303：根据所述机械臂各关节角的变化范围删除部分不可行解，各关节解的变化范围为：min(θ_ni)≤θ_i≤max(θ_ni)，n＝1,2,3,…,8，i＝1,2,3,…,6；

S304：根据最短行程原则的优化步骤S303中的所述机械臂各关节角的解集，得到1组最优解，即min[⁰T₁(θ_n1-θ₁),¹T₂(θ_n2-θ₂),²T₃(θ_n3-θ₃)],n＝1,2,3,...,8；

S207：将所述节点从所述open列表中删除，加入到closed列表中，回到步骤S202；

S208：求解所述关节线段方程与障碍物包络的位置关系，进行碰撞检测；

S209：判断所述关节线段方程与所述障碍物包络是否碰撞？若是，则到步骤S210；若否，则到步骤S211；采用长方体包络法，包络L个障碍物的长方体分别为{O₁,O₂,…,O_L}，通过检测所述机械臂的各关节所连接的线段与{O₁,O₂,…,O_L}各个面是否有交点，把碰撞问题转化为空间直线段与四边形之间的位置关系的判断，位置关系的判断方法是：已知空间线段上2个端点p₁(x₁,y₁,z₁)和p₂(x₂,y₂,z₂)、空间四边形的法向量M＝[m₁ m₂ m₃]以及四个顶点的坐标，由空间解析几何知识可知，线段与平面位置关系可以用空间线段方向向量与空间四边形法向量之间的关系来判断，其中空间线段方向向量T＝[x₂-x₁ y₂-y₁ z₂-z₁]；若M·T＝0，则空间线段与平面平行，没有交点；若M·T≠0，则空间线段与平面有交点，设所述交点为p₀(x₀,y₀,z₀)；判断所述交点是否为碰撞点？若是，则满足两个条件：(1)所述交点p₀(x₀,y₀,z₀)在空间线段上和所述交点在空间四边形内；(2)所述障碍物包络成的长方体的前面、后面平行于z轴，左面、右面平行于y轴，上面、下面平行于x轴，以平行于x轴的上平面为例，上平面的4个顶点为：s₁(x₃,y₃,z₃)，s₂(x₄,y₄,z₄)，s₃(x₅,y₅,z₅)，s₄(x₆,y₆,z₆)，若有：min(z₁,z₂)≤z₀≤max(z₁,z₂)，min(x₃,x₄)≤x₀≤max(x₃,x₄)，min(y₃,y₄)≤y₀≤max(y₃,y₄)，则所述交点p₀(x₀,y₀,z₀)是碰撞点；若否，则所述交点p₀(x₀,y₀,z₀)不是碰撞点；

S210：判断是否还有未检测的逆解？若是，则到步骤S212；若否，则回到步骤S202；

S211：将当前节点加入所述open列表，然后到步骤S213；

S212：将当前关节角舍弃，在剩下的逆解中选择一组逆解，回到步骤S206；

S213：设置当前节点为父节点，扩展后继节点，将生成的后继节点加入所述open列表；后继节点的选取原则是：在所述当前节点与所述后继节点的连线路径与所述C障碍物无碰撞的前提下，f值最小时的节点；

S214：建立后继节点返回到当前节点的指针；

S215：根据所述指针，判断所述机械臂末端是否找到目标节点？若是，则到步骤S216；若否，则回到步骤S202；

S216：根据起始节点、路径点和目标节点，并经过碰撞检测，最终找到所述避障路径，并保存所述避障路径；

S104：采用三次均匀B样条曲线方法对所述避障路径进行平滑处理，将平滑处理过的所述避障路径作为最终的路径规划结果；采取所述三次均匀B样条对所述避障路径进行平滑处理的过程为：每四个相邻的控制点可以确定一段所述三次均匀B样条曲线，n+2个控制点可确定n-1条三次均匀B样条曲线列{S₁,S₂,…,S_n-1}，单条曲线S_i是参变量u的函数，如公式(3)所示：

其中，U为参量阵，且U＝[1 u u² u³]，u∈[0,1]，M为所述三次均匀B样条曲线的常系数矩阵，C_i为控制点阵，且C_i＝[ω_i-1c_i-1 ω_ic_i ω_i+1c_i+1 ω_i+2c_i+2]，ω_i为权因子阵，且ω_i＝[ω_i-1 ω_i ω_i+1 ω_i+2]，i＝1,2，…,n-1；

将所述三次均匀B样条曲线做归一化处理，将每一条所述三次均匀B样条曲线的局部参变量u∈[0,1]依次拼接得到全局参变量t∈[0,n-1]，所述三次均匀B样条曲线列在全局参变量下表示为分段函数：

其中，t∈[0,n-1]为全局参变量，S(i)为第i条所述三次均匀B样条曲线，i＝1,2，…,n-2；

根据所述三次均匀B样条曲线对所述避障路径进行平滑处理，使所述机械臂能够平稳的沿着所述避障路径从开始节点到达目标节点，且避开障碍物，提高了所述机械臂的路径跟踪精度。

请参见图3，图3是本发明实施例的硬件设备工作示意图，所述硬件设备具体包括：一种机械臂避障路径规划设备301、处理器302及存储设备303。

一种机械臂避障路径规划设备301：所述一种机械臂避障路径规划设备301实现所述一种机械臂避障路径规划方法。

处理器302：所述处理器302加载并执行所述存储设备303中的指令及数据用于实现所述一种机械臂避障路径规划方法。

存储设备303：所述存储设备303存储指令及数据；所述存储设备303用于实现所述一种机械臂避障路径规划方法。

本发明的有益效果是：本发明提供的技术方案避免了机械臂在工作过程中与障碍物发生碰撞，降低了机械臂的控制误差，提高了所述机械臂的路径跟踪精度，减少了机械臂在运动过程中产生的机械震动，延长了机械臂的使用寿命。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种机械臂避障路径规划方法，其特征在于：包括以下步骤：

S101：通过机械臂中的外部传感器，检测作业目标及障碍物的位姿数据，以所述机械臂末端的初始位置作为避障路径规划的起始节点，以作业目标的位置作为所述避障路径规划的目标节点；

S102：采用C空间法对所述机械臂的作业空间进行三维建模,得到C空间的网格化模型；

S103：根据所述网格化模型，采用改进的A*算法在网格化的C空间内进行避障路径搜索，得到所述机械臂末端自起始节点到目标节点的避障路径；

S104：采用三次均匀B样条曲线方法对所述避障路径进行平滑处理，将平滑处理过的所述避障路径作为最终的路径规划结果。

2.如权利要求1所述的一种机械臂避障路径规划方法，其特征在于：在步骤S102中，所述C空间分为自由空间和障碍物空间，使用网格法将所述C空间用若干个相等的立方体表示，每一个所述立方体都对应有一个单独的索引值，所述障碍物空间内的所述立方体对应的索引值为index(X,Y,Z)，所述自由空间内的所述立方体对应的索引值为index(x,y,z)；索引值index(m,n,l)中的(m,n,l)分别表示从原点0到所述索引点处在x、y和z方向上所述立方体的个数，取所述立方体的边长为length，则所述索引点处的坐标为：其中，或由所述自由空间和障碍物空间中各个索引点的坐标得到C空间的网格化模型。

3.如权利要求1所述的一种机械臂避障路径规划方法，其特征在于：在步骤S103中，建立改进的A*算法的估价函数如公式(1)所示：

f(n)＝g(n)+h(n) (1)

其中，f(n)为当前节点的估价函数，g(n)为起始节点到当前节点的实际代价，表示搜索广度的优先趋势，h(n)为启发函数，表示从当前节点到目标节点的最佳路径的估计代价，体现了搜索中的启发信息；

采用改进的A*算法搜索避障路径的过程为：

S201：将起始节点的坐标S(x,y,z)添加到open列表；

S202：检测所述open列表是否为空？若是，则到步骤S203；若否，则到步骤S204；

S203：结束路径搜索，搜索不到避障路径；

S204：选择所述open列表中未计算过的一个节点，并计算所有节点的f(n)值；

S205：判断是否所述节点的f(n)值最小？若是，则到步骤S206；若否，则到步骤S207；f(n)值最小的节点被称为当前节点；

S206：将路径点代入逆运动学方程，得到逆解，采用时间—能量最优法选择一组逆解，所述逆解即为所述机械臂末端到达目标节点的关节角；其中，采用动态步长的方式，通过估价函数f(n)选出所述路径点；采用D-H法将关节角代入正运动学方程，得到关节线段方程，然后到步骤S208；

S207：将所述节点从所述open列表中删除，加入到closed列表中，回到步骤S202；

S208：求解所述关节线段方程与采用长方体包络法得到的障碍物包络的位置关系，进行碰撞检测；

S209：判断所述关节线段方程与所述障碍物包络是否碰撞？若是，则到步骤S210；若否，则到步骤S211；

S210：判断是否还有未检测的逆解？若是，则到步骤S212；若否，则回到步骤S202；

S211：将当前节点加入所述open列表，然后到步骤S213；

S212：将当前关节角舍弃，在剩下的逆解中选择一组解，回到步骤S206；

S213：设置当前节点为父节点，扩展后继节点，将生成的后继节点加入所述open列表；

S214：建立后继节点返回到当前节点的指针；

S215：根据所述指针，判断所述机械臂末端是否找到目标节点？若是，则到步骤S216；若否，则回到步骤S202；

S216：根据起始节点、路径点和目标节点，并经过碰撞检测，最终找到所述避障路径，并保存所述避障路径。

4.如权利要求1所述的一种机械臂避障路径规划方法，其特征在于：在步骤S104中，采取所述三次均匀B样条对所述避障路径进行平滑处理的过程为：每四个相邻的控制点可以确定一段所述三次均匀B样条曲线，n+2个控制点可确定n-1条三次均匀B样条曲线列{S₁,S₂,…,S_n-1}，单条曲线S_i是参变量u的函数，如公式(2)所示：

其中，U为参量阵，且U＝[1 u u² u³]，u∈[0,1]，M为所述三次均匀B样条曲线的常系数矩阵，C_i为控制点阵，且C_i＝[ω_i-1c_i-1 ω_ic_i ω_i+1c_i+1 ω_i+2c_i+2]，ω_i为权因子阵，且ω_i＝[ω_i-1 ω_i ω_i+1 ω_i+2]，i＝1,2，…,n-1；

将所述三次均匀B样条曲线做归一化处理，将每一条所述三次均匀B样条曲线的局部参变量u∈[0,1]依次拼接得到全局参变量t∈[0,n-1]，所述三次均匀B样条曲线列在全局参变量下表示为分段函数：

其中，t∈[0,n-1]为全局参变量，S(i)为第i条所述三次均匀B样条曲线，i＝1,2，…,n-2；

根据所述三次均匀B样条曲线对所述避障路径进行平滑处理，使所述机械臂能够平稳的沿着所述避障路径从开始节点到达目标节点，且避开障碍物，提高了所述机械臂的路径跟踪精度。

5.一种存储设备，其特征在于：所述存储设备存储指令及数据用于实现权利要求1～4所述的任意一种机械臂避障路径规划方法。

6.一种机械臂避障路径规划设备，其特征在于：包括：处理器及所述存储设备；所述处理器加载并执行所述存储设备中的指令及数据用于实现权利要求1～4所述的任意一种机械臂避障路径规划方法。