CN111300420B - 一种机械臂关节空间转角最小路径求取方法 - Google Patents

Abstract

本发明一种机械臂关节空间转角最小路径求取方法属于现代智能制造技术领域，涉及工业机器人领域一种机械臂的关节空间转角最小路径求取方法。该方法首先结合混沌现象构造一个能够收敛到机械臂单个位姿对应的全部逆运动学解的迭代起点集合，将迭代起点依次带入LM迭代算法中求解逆运动学解。并剔除重复的结果和超出机械臂各关节运动范围的结果，得到单个位姿对应的全部逆运动学可行解。然后，再依次求取全部逆运动学可行解，得到关节空间运动路径全部可行解；最后，建立搜索算法计算机械臂关节空间转角最小路径。本方法有效避免了关节空间路径位置突变和位移量大的问题，具有计算精度高、路径光顺的特点，更加符合机械臂实际工作的需求。

Description

一种机械臂关节空间转角最小路径求取方法

技术领域

本发明属于现代智能制造技术领域，涉及工业机器人领域一种机械臂的关节空间转角最小路径求取方法。

背景技术

一般构型机械臂不符合Pieper准则，其逆运动学问题是在给定机械臂末端坐标系位姿的情况下，求取机械臂所有关节的转角，是正向运动学的逆过程。而逆向运动学的求解则相对复杂，且极有可能出现多解的情况。因此，将一般构型机械臂工作空间中的运动路径通过逆运动学算法转换至关节空间中的过程中，由于多解问题的存在，关节空间的运动路径存在多种可能。然而在机械臂关节空间的多种运动路径中，并不是所有路径都符合实际工作要求。有些路径存在超出关节运动范围的问题，这会导致机械臂末端不能达到期望的位姿；有些运动路径存在位置突变的问题，这会给轨迹速度规划带来负担；有些运动路径存在位移量大的问题，这会增加机械臂的运动时间，降低工作效率。因此，为了避免上述问题，需要在机械臂关节空间运动路径的众多可能中寻找到满足机械臂各关节运动范围的各关节转角最小的连续路径，这也是机械臂速度规划问题所期望的运动路径。然而目前针对机械臂轨迹规划的研究中对这一内容并没有过多的关注，多是直接给出一组理想的机械臂关节空间运动路径进行轨迹规划，实际上这样做是不符合实际工作要求的。

目前，机械臂将工作空间运动路径转换至关节空间所采用的方法是，连续地使用迭代法求解机械臂工作空间运动路径上各个位姿所对应的逆运动学问题。在求解过程中，将上一个位姿迭代求解逆运动学问题的结果作为下一个位姿的迭代起点，从而期望得到连续的机械臂关节空间轨迹。但是，迭代的方法很难结合机械臂的各关节运动范围控制迭代方向，并且由于迭代最终只会收敛到机械臂逆运动学问题的一个解，所以这种方法无法避免机械臂关节空间轨迹出现超出关节运动范围而导致的位置突变问题。安凯等人的文献“一种3R机械臂最短路径规划方法”，光电工程，2014，41(12)：1-6，首先推导出了机械臂末端路径长度的表示式，并将最短路径问题归结为一个泛函极值问题；其次，为简化求解过程，将泛函极值问题转化成另一个同解的泛函极值问题，并利用变分法求出了表示后一问题解的微分方程组；最后，利用上三角矩阵逆矩阵的表示式，将该微分方程组转化成了标准状态方程组进行计算，与微分几何方法相比，避免了逆矩阵计算。然而该方法过程复杂，计算量大，且当机械臂自由度数量增加时计算量也将急剧增加，具有较大的局限性。

发明内容

本发明目的在于克服现有技术的不足，发明一种机械臂关节空间转角最小路径求取方法，用于获取一般构型机械臂关节空间中满足各关节运动范围的各关节转角最小的连续路径。该方法首先结合混沌现象构造一个能够收敛到机械臂单个位姿对应的全部逆运动学解的迭代起点集合，将迭代起点依次带入LM迭代算法中求解逆运动学解，并剔除重复的结果和超出机械臂各关节运动范围的结果，得到单个位姿对应的全部逆运动学可行解；然后，对工作空间路径上所有机械臂位姿依次求取全部逆运动学可行解，得到关节空间运动路径全部可行解；最后，将在关节空间运动路径全部可行解中寻找一条关节转角最小的路径问题转化为多段有向图最短路径搜索问题，结合动态搜索思想，构建搜索算法计算机械臂关节空间转角最小路径。本方法结合混沌现象和LM迭代法，求得关节空间运动路径全部可行解，使用多段有向图动态搜索算法得到机械臂关节空间转角最小路径，有效避免了关节空间路径位置突变和位移量大的问题。本方法行之有效，过程简单明了。

本发明采用的技术方案是一种机械臂关节空间转角最小路径求取方法，该方法首先建立机械臂数学模型，设定迭代目标；结合混沌现象构造一个能够收敛到机械臂单个位姿对应的全部逆运动学解的迭代起点集合，将迭代起点依次带入LM迭代算法中求解逆运动学解，并剔除重复的结果和超出机械臂各关节运动范围的结果，得到单个位姿对应的全部逆运动学可行解；然后，对工作空间路径上所有机械臂位姿依次求取全部逆运动学可行解，得到关节空间运动路径全部可行解；最后，将在关节空间运动路径全部可行解中寻找一条关节转角最小的路径问题转化为多段有向图最短路径搜索问题，结合动态搜索思想，构建搜索算法计算机械臂关节空间转角最小路径。方法的具体步骤如下：

步骤一，建立机械臂数学模型，设定迭代目标。

机械臂由基座A、末端执行器G、5个连杆和6个旋转关节组成，即连杆1、连杆2、连杆3、连杆4、连杆5(B、C、D、E、F)，关节1、关节2、关节3、关节4、关节5、关节6(1、2、3、4、5、6)。使用改进的DH法确定自由度为n的机械臂几何参数，建立机械臂数学模型，按照公式(1)将希望求解逆运动学问题的机械臂末端执行器位姿T(θ_end)设为LM迭代算法中的机械臂末端目标位姿T_end。

T_end＝T(θ_end)(1)

其中，θ_end为T(θ_end)对应的一个逆运动学解，包含机械臂各关节转角。

步骤二，结合混沌现象构造迭代起点集合。

确定迭代起点个数j，结合混沌现象，按照公式(2)使用伪随机数构造共包含j个迭代起点的集合。

θ_j＝-π+2π×rand(1,n)(2)

步骤三，设定LM迭代初始条件。

将第j个迭代起点θ_j带入正运动学算法中，计算此迭代起点对应的机械臂末端坐标系位姿T(θ_j)。按照公式(3)将T(θ_j)设定为迭代算法中的机械臂末端当前位姿T_cur，按照公式(4)将迭代起点θ_j设定为迭代算法中的机械臂当前关节值θ_cur。

T_cur＝T(θ_j)(3)

θ_cur＝θ_j(4)

步骤四，判断LM迭代是否结束。

按照公式(5)计算微分算子Δ，通过微分算子Δ可以确定微分运动向量D＝[dx dydz rx ry rz]^T和微分运动向量D的二阶范数||D||₂，判断||D||₂＜ε是否成立，其中ε为设定的迭代精度。如果成立，返回当前关节值θ_cur作为求解逆运动学问题的机械臂末端执行器位姿T(θ_end)对应的一个逆解；如果不成立，执行步骤五。

步骤五，计算LM迭代步。

根据T_cur和θ_cur计算机械臂雅可比矩阵J。按照公式(6)和公式(7)计算第k步迭代步dθ_k，并按照公式(8)更新θ_cur。再通过正运动算法计算θ_cur对应的机械臂末端坐标系位姿T(θ_cur)，并按照公式(9)更新T_cur。

dθ_k＝-(J(θ_k)^TJ(θ_k)+μ_kI)^-1J(θ_k)^TD_k(6)

μ_k＝0.001×||D_k||(7)

θ_cur＝θ_cur+dθ(8)

T_cur＝T(θ_cur)(9)

步骤六，计算机械臂单个位姿对应的全部逆运动学可行解。

重复步骤四和五，直到||D||₂＜ε成立，返回θ_cur作为求解逆运动学问题的机械臂末端执行器位姿T(θ_end)对应的一个逆解。或直到重复次数超过迭代上限，返回迭代失败。

重复步骤三至六，直到将j个迭代起点全部带入计算得到迭代结果。整理所有迭代结果，剔除重复的结果和超出机械臂关节运动范围的结果，得到机械臂末端单个位姿T(θ_end)对应的全部逆运动学可行解。

步骤七，计算机械臂关节空间轨迹全部可行解。

依次计算机械臂工作空间路径上所有位姿T₁,T₂,……,T_i对应的全部逆运动学可行解，得到机械臂关节空间轨迹全部可行解θ₁,θ₂,……,θ_i。其中，θ_i中包含T_i全部逆运动学可行解。

步骤八，将求机械臂关节空间转角最小路径转化为多段有向图搜索问题。

当多段有向图搜索问题具有多个起点或终点时，需要人为设定搜索的起点和终点或按照某种顺序依次遍历起点和终点的组合。为了避免这一问题，可以人为设定唯一的虚拟起点和终点。同时，将虚拟起点与全部真实起点连接，距离均设为0。将虚拟终点与全部真实终点连接，距离均设为0。

将求机械臂关节空间转角最小路径转化为多段有向图搜索问题时，需在全部逆运动学解前后分别增加只包含一个逆解的源点θ₀和汇点θ_i+1。并设定源点θ₀到θ₁中每一个逆解的距离均为0，汇点θ_i+1到θ_i中每一个逆解的距离均为0。

步骤九，结合机械臂参数设定初始条件。

按照公式(10)设定系数矩阵W，其中w_m为第m个关节最大运动速度的倒数。按照公式(11)，计算θ_i中第u个逆解与θ_i-1中第v个逆解之间的距离s_i,u,v。依次计算θ_i与中θ_i-1所有逆解两两之间的距离S_i。

s_i,u,v＝||(θ_i,u-θ_i-1,v)W||(11)

步骤十，建立机械臂关节空间转角最小路径动态搜索算法。

按照公式(12)，确定从源点θ₀到θ_i中所有逆运动学解的最小转角。其中，p_i为源点θ₀到θ_i-1中所有逆运动学解的最小转角。记录使θ_i中第u个逆解到源点θ₀转角最小的θ_i-1中逆解的序号V_i,u＝v。

p_i＝min(p_i-1+S_i)(12)

步骤十一，计算机械臂关节空间转角最小路径。

从θ₁到θ_i+1依次重复步骤九和十，最终获得源点θ₀和汇点θ_i+1的最小转角。按照最短路径逆解序号V_i,u＝v，从θ_i到θ₁反向搜索全部逆运动学解θ₁,θ₂,……,θ_i，即可获得关节空间转角最小路径。

本发明的有益效果是该方法结合混沌现象和LM迭代算法求得机械臂关节空间运动路径全部可行解，使用多段有向图动态搜索算法得到机械臂关节空间转角最小路径，有效避免了机械臂关节空间路径位置突变和位移量大的问题。本发明中提出的一般构型机械臂关节空间转角最小路径求解过程简单明了行之有效，对不满足Pieper准则的一般构型机械臂同样适用，具有计算精度高、路径光顺的特点，更加符合机械臂实际工作的需求。

附图说明：

附图1—般构型机械臂关节空间转角最小路径求取方法流程图。

附图2—种六自由度一般构型机械臂的结构图。其中，O-基坐标系原点，A-基座，B-连杆1，C-连杆2，D-连杆3，E-连杆4，F-连杆5，G-末端执行器，a₂-连杆2的长度，a₃-连杆3的长度，a₄-连杆4的长度，d₄-连杆3与连杆4的距离，d₅-连杆4与连杆5之间的安装误差，d₆-连杆5与末端执行器之间的距离。

附图3—种六自由度一般构型机械臂的结构简图及各关节坐标系。其中，1-关节1，2-关节2，3-关节3，4-关节4，5-关节5，6-关节6；

附图4—使用本发明计算的关节空间转角最小运动路径，黑色虚线代表关节位置界限。其中，图4(a)、(b)、(c)、(d)、(e)、(f)-分别为关节1、关节2、关节3、关节4、关节5、关节6位置变化曲线。

附图5—使用目前采用的连续迭代方法计算的关节空间运动路径，黑色虚线代表关节位置界限。其中，图5(a)、(b)、(c)、(d)、(e)、(f)-分别为关节1、关节2、关节3、关节4、关节5、关节6位置变化曲线。

具体实施方式

下面结合附图和技术方案详细叙述本发明的具体实施方式。

附图1为本发明一般构型机械臂关节空间转角最小路径求取方法流程图，附图2为一种六自由度一般构型机械臂的结构图，一般构型机械臂各关节坐标系如附图3所示。机械臂由基座A、末端执行器G与5个连杆，即连杆1、连杆2、连杆3、连杆4、连杆5(B、C、D、E、F)，6个旋转关节，即关节1、关节2、关节3、关节4、关节5、关节6(1、2、3、4、5、6)组成。各坐标系的确定方法采取改进的DH参数法，各关节均绕着该关节坐标系的z轴旋转，具体的，z_i轴沿第i+1关节的轴线，x_i沿z_i轴和z_i-1轴的公垂线，指向背离z_i-1轴的方向，y_i轴由右手直角坐标系规则确定；实施例将第一个关节坐标系的初始位置设置在机械臂的基座上与基坐标系{O₀:x₀,y₀,z₀}重合，基坐标系始终保持不变。

该方法流程的具体步骤如下：

步骤一,基于改进的DH参数法，建立机械臂各关节坐标系,如附图3所示。给定机械臂相邻关节之间的4个结构几何参数：连杆转角θ_m、连杆扭角α_m、连杆长度a_m、连杆距离d_m。本实施例中，机械臂各连杆的DH参数为：m＝1时，连杆转角为θ₁，连杆距离为0，连杆长度为0，连杆扭角为0；m＝2时，连杆转角为θ₂+90°，连杆距离为0，连杆长度为a₂，连杆扭角为90°；m＝3时，连杆转角为θ₃，连杆距离为0，连杆长度为a₃，连杆扭角为0；m＝4时，连杆转角为θ₄，连杆距离为d₄，连杆长度为a₄，连杆扭角为90°；m＝5时，连杆转角为θ₅，连杆距离为d₅，连杆长度为0，连杆扭角为-90°；m＝6时，连杆转角为θ₆+90°，连杆距离为d₆，连杆长度为0，连杆扭角为90°。在本实施例中，d₆包含工具长度。

基于改进的DH参数法，求出实施例中各结构的参数值如下：

a₂＝160,a₃＝790,a₄＝155,d₄＝795,d₅＝10,d₆＝145

由于该六自由度机械臂关节5处存在安装误差d₅，因此不满足Pieper准则，所以是一个一般构型机械臂。

按照公式(1)将机械臂末端坐标系位姿T(θ_end)设定为设为LM迭代算法中的目标位姿T_end，其中给定的机械臂末端位姿如下：

步骤二，确定迭代起点个数30，结合混沌现象，按照公式(2)使用伪随机数构造共包含30个迭代起点的集合，见表1。

表1

步骤三至六，将30组迭代起点依次带入LM迭代算法中，按照公式(3)-(9)进行循环计算，剔除掉重复的结果得到给定机械臂末端位姿对应的全部8个逆运动学解，计算结果误差小于1×10^-7°，见表2。

表2

剔除超出机械臂关节运动范围的结果得到给定末端位姿对应的全部6个逆运动学可行解，见表3。

表3

步骤七，将起点和终点如下所示的机械臂工作空间路径离散为50个位姿，依次计算路径上所有位姿T₁,T₂,……,T₅₀对应的全部逆运动学可行解，得到机械臂关节空间轨迹全部可行解θ₁,θ₂,……,θ₅₀。其中，θ_i中包含T_i至多8个逆运动学可行解。

步骤八，将寻找机械臂关节空间转角最小路径问题转化为多段有向图最短路径搜索问题，在θ₁,θ₂,……,θ₅₀前后分别添加只包含一个逆解源点θ₀和汇点θ₅₁。并设定源点θ₀到θ₁中每一个逆解的距离均为0，汇点θ₅₁到θ₅₀中每一个逆解的距离均为0。

步骤九，设定多段有向图最短路径搜索问题的初始条件，按照公式(10)，(11)设定系数矩阵W，并计算逆解之间的距离s_i,u,v。

步骤十和十一，按照公式(12)构建搜索算法，计算关节空间转角最小路径距离。并按照最短路径逆解序号V_i,u＝v，从θ₅₁到θ₁反向搜索全部逆运动学解θ₁,θ₂,……,θ₅₀，即可获机械臂关节空间转角最小路径，结果如附图4所示。使用目前所采用的连续迭代的方法将机械臂将工作空间运动路径转换至关节空间结果如附图5所示。

附图5为使用目前采用的连续迭代方法计算的关节空间运动路径图，在附图5中，关节3的运动路径超出了机械臂关节运动范围，从而导致了关节转角突变。此外，由于关节3中关节转角突变，关节4-6的运动路径的光顺性也受到一定影响。对比附图4中使用本发明计算的机械臂关节空间转角最小运动路径，有效避免了关节3路径超出关节运动范围而导致关节转角突变的问题。同时，其他关节路径也变得更加光顺。这为进一步规划关节空间路径速度提供了方便。

本发明提出的机械臂关节空间转角最小路径求解过程简单明了，有效避免了关节空间路径位置突变和位移量大的问题，对不满足Pieper准则的一般构型机械臂同样适用，具有计算精度高、路径光顺的特点，更加符合机械臂实际工作的需求。

Claims (1)

1.一种机械臂关节空间转角最小路径求取方法，其特征是，该方法首先建立机械臂数学模型，设定迭代目标；结合混沌现象构造一个能够收敛到机械臂单个位姿对应的全部逆运动学解的迭代起点集合，将迭代起点依次带入LM迭代算法中求解逆运动学解，并剔除重复的结果和超出机械臂各关节运动范围的结果，得到单个位姿对应的全部逆运动学可行解；然后，对工作空间路径上所有机械臂位姿依次求取全部逆运动学可行解，得到关节空间运动路径全部可行解；最后，将在关节空间运动路径全部可行解中寻找一条关节转角最小的路径问题转化为多段有向图最短路径搜索问题，结合动态搜索思想，构建搜索算法计算机械臂关节空间转角最小路径；方法的具体步骤如下：

步骤一，建立机械臂数学模型，设定迭代目标；

机械臂由基座(A)、末端执行器(G)、5个连杆和6个旋转关节组成，即连杆1、连杆2、连杆3、连杆4、连杆5(B、C、D、E、F)，关节1、关节2、关节3、关节4、关节5、关节6(1、2、3、4、5、6)；使用改进的DH法确定自由度为n的机械臂几何参数，建立机械臂数学模型，按照公式(1)将希望求解逆运动学问题的机械臂末端执行器位姿T(θ_end)设为LM迭代算法中的机械臂末端目标位姿T_end；

T_end＝T(θ_end)(1)

其中，θ_end为T(θ_end)对应的一个逆运动学解，包含机械臂各关节转角；

步骤二，结合混沌现象构造迭代起点集合；

确定迭代起点个数j，结合混沌现象，按照公式(2)使用伪随机数构造共包含j个迭代起点的集合，n为机械臂的自由度；

θ_j＝-π+2π×rand(1,n)(2)

步骤三，设定LM迭代初始条件；

将第j个迭代起点θ_j带入正运动学算法中，计算此迭代起点对应的机械臂末端坐标系位姿T(θ_j)；按照公式(3)将T(θ_j)设定为迭代算法中的机械臂末端当前位姿T_cur，按照公式(4)将迭代起点θ_j设定为迭代算法中的机械臂当前关节值θ_cur；

T_cur＝T(θ_j)(3)

θ_cur＝θ_j(4)

步骤四，判断LM迭代是否结束；

按照公式(5)计算微分算子Δ，通过微分算子Δ确定微分运动向量D＝[dx dy dz rxry rz]^T和微分运动向量D的二阶范数||D||₂，判断||D||₂＜ε是否成立，其中ε为设定的迭代精度；如果成立，返回当前关节值θ_cur作为求解逆运动学问题的机械臂末端执行器位姿T(θ_end)对应的一个逆解；如果不成立，执行步骤五；

步骤五，计算LM迭代步；

根据T_cur和θ_cur计算机械臂雅可比矩阵J；按照公式(6)和公式(7)计算第k步迭代步dθ_k，并按照公式(8)更新θ_cur；再通过正运动算法计算θ_cur对应的机械臂末端坐标系位姿T(θ_cur)，并按照公式(9)更新T_cur；

dθ_k＝-(J(θ_k)^TJ(θ_k)+μ_kI)^-1J(θ_k)^TD_k(6)

μ_k＝0.001×||D_k||(7)

θ_cur＝θ_cur+dθ(8)

T_cur＝T(θ_cur)(9)

步骤六，计算机械臂单个位姿对应的全部逆运动学可行解；

重复步骤四和五，直到||D||₂＜ε成立，返回θ_cur作为求解逆运动学问题的机械臂末端执行器位姿T(θ_end)对应的一个逆解；或直到重复次数超过迭代上限，返回迭代失败；

重复步骤三至六，直到将j个迭代起点全部带入计算得到迭代结果；整理所有迭代结果，剔除重复的结果和超出机械臂关节运动范围的结果，得到机械臂末端单个位姿T(θ_end)对应的全部逆运动学可行解；

步骤七，计算机械臂关节空间轨迹全部可行解；

依次计算机械臂工作空间路径上所有位姿T₁,T₂,……,T_i对应的全部逆运动学可行解，得到机械臂关节空间轨迹全部可行解θ₁,θ₂,……,θ_i；其中，θ_i中包含T_i全部逆运动学可行解；

步骤八，将求机械臂关节空间转角最小路径转化为多段有向图搜索问题；

当多段有向图搜索问题具有多个起点或终点时，需要人为设定搜索的起点和终点或按照某种顺序依次遍历起点和终点的组合；为了避免这一问题，人为设定唯一的虚拟起点和终点；同时，将虚拟起点与全部真实起点连接，距离均设为0；将虚拟终点与全部真实终点连接，距离均设为0；

将求机械臂关节空间转角最小路径转化为多段有向图搜索问题时，需在全部逆运动学解前后分别增加只包含一个逆解的源点θ₀和汇点θ_i+1；并设定源点θ₀到θ₁中每一个逆解的距离均为0，汇点θ_i+1到θ_i中每一个逆解的距离均为0；

步骤九，结合机械臂参数设定初始条件；

按照公式(10)设定系数矩阵W，其中w_m为第m个关节最大运动速度的倒数；按照公式(11)，计算θ_i中第u个逆解与θ_i-1中第v个逆解之间的距离s_i,u,v；依次计算θ_i与中θ_i-1所有逆解两两之间的距离S_i；

s_i,u,v＝||(θ_i,u-θ_i-1,v)W||(11)

步骤十，建立机械臂关节空间转角最小路径动态搜索算法；

按照公式(12)，确定从源点θ₀到θ_i中所有逆运动学解的最小转角；其中，p_i为源点θ₀到θ_i-1中所有逆运动学解的最小转角；记录使θ_i中第u个逆解到源点θ₀转角最小的θ_i-1中逆解的序号V_i,u＝v；

p_i＝min(p_i-1+S_i)(12)

步骤十一，计算机械臂关节空间转角最小路径；

从θ₁到θ_i+1依次重复步骤九和十，最终获得源点θ₀和汇点θ_i+1的最小转角；按照最短路径逆解序号V_i,u＝v，从θ_i到θ₁反向搜索全部逆运动学解θ₁,θ₂,……,θ_i，即可获得关节空间转角最小路径。

Images