[发明专利]一种机械臂关节空间转角最小路径求取方法

权利要求书

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，即可获得关节空间转角最小路径。