CN109702751A - 一种七自由度串联机械臂的位置级逆解方法 - Google Patents

Abstract

一种七自由度串联机械臂的位置级逆解方法，机械臂包括七个关节，七个关节串联布置，相邻两关节之间相互垂直，方法包括：S1，将机械臂转化为四个连杆和三个节点，其中，第一连杆、第一节点、第二连杆、第二节点、第三连杆、第三节点、第四连杆依次相连；S2，将第一连杆的起始端置于直角坐标系的原点，获取第四连杆末端的位置坐标和姿态坐标，并根据第四连杆末端的位置坐标和姿态坐标，计算第三节点的位置坐标；S3，根据第三节点的位置坐标，计算第二节点的位置坐标。通过逆向求解各个节点的位置坐标，利用位置坐标求解各关节的旋转角度，实现机械臂的位置级逆解求解，拓展冗余机械臂的应用，实现复杂场景下机械臂灵活控制，具有重要的实际意义。

Description

一种七自由度串联机械臂的位置级逆解方法

技术领域

本发明涉及机械臂技术领域，尤其涉及一种七自由度串联机械臂的位置级逆解方法。

背景技术

机械臂逆解求解是机械臂实现各类操作和精细控制的基础，机械臂一般由多个连杆组成，机械臂求逆解可以分为位置级逆解和速度级逆解两大类，速度级逆解是已知机械臂末端六维速度向量，基于机械臂雅可比矩阵，求解各关节速度。速度级逆解通过迭代，能够获得一段时间内位姿变化对应的各关节角度变化。而位置级逆解则是根据机械臂末端六维位姿直接求解对应的各关节角度。随着目前机械臂的应用环境越来越广泛，实时控制的需求越来越高，凸显出了位置级逆解的重要性。目前针对串联机械臂的位置级逆解多集中在6DOF(degree of freedom)机械臂上，6DOF机械臂在其工作空间中具备16个实数解，可以通过分离消元和多元方程求得其位置级逆解的封闭解析解。而针对冗余度机械臂求逆解，则多针对平面冗余度机械臂，如4DOF和5DOF。而随着人工智能时代的到来，对机械臂的智能化操作要求增多，灵巧操作、避障和避奇异的需求使得空间6个自由度已经无法满足很多场合下的机械臂应用，具备一个空间冗余度(在达到六维位姿的前提下具备一个额外的自由度)的7DOF的机械臂在工作空间中具有无穷多解，能够在到达指定位姿的前提下优化自身构型。

目前工业和服务机械臂等均向着冗余方向发展，针对7DOF机械臂的位置级逆解研究，需要涉及到位置级逆解的快速解算、通解表征和优化选解三方面的内容。目前针对具备一个空间冗余度机械臂的位置级逆解的研究开展较少，一种是采用速度级逆解求积分的方法，但会扩大误差，本质上仍是速度级逆解；另一种采用数值方法进行求解，数值法的弊端有两个方面，一方面得到的解集是全部逆解的子集，且难以反映各个解之间的关系，不利于优化选解；另一方面是数值计算耗费巨大的硬件资源，不利于机械臂的实时控制。因此，提出一种快速且能够反映解集关系的7DOF冗余机械臂的位置级逆解方法，对于拓展冗余机械臂的应用，面向复杂场景进行机械臂实时灵巧控制，具有重要的实际应用意义。

发明内容

(一)要解决的技术问题

本发明提供了一种七自由度串联机械臂的位置级逆解方法，可以快速解算出机械臂各关节角度。

(二)技术方案

本发明提供了一种七自由度串联机械臂的位置级逆解方法，机械臂包括七个关节，七个关节串联布置，相邻两关节之间相互垂直，方法包括：S1，将机械臂构型转化为四个连杆和三个节点，其中，第一连杆、第一节点、第二连杆、第二节点、第三连杆、第三节点、第四连杆依次相连；S2，将第一连杆的起始端置于直角坐标系的原点，获取第四连杆末端的位置坐标和姿态坐标，并根据第四连杆末端的位置坐标和姿态坐标，计算第三节点的位置坐标；S3，根据第三节点的位置坐标，计算第二节点的位置坐标。

可选地，步骤S2中并根据第四连杆末端的位姿，计算第三节点的位置坐标具体为：获取第四连杆末端的位姿相对于直角坐标系的变换矩阵第三节点相对于直角坐标系的变换矩阵第四连杆末端相对于第三节点的变换矩阵为得到根据以及DH参数计算得到第三节点的位置坐标。

可选地，步骤S1还包括获取四个连杆上每一连杆的长度，其中，第一连杆、第二连杆、第三连杆以及第四连杆的连杆长度分别为d₁、d₂、d₃以及d₄。

可选地，第四连杆末端的位姿为[d_x d_y d_z δ_x δ_y δ_z]^T，其中，d_x、d_y、d_z为第四连杆末端的位置坐标，δ_x、δ_y、δ_z为第四连杆末端的的姿态坐标，根据以及DH参数计算得到第三节点的位置坐标具体为：

根据DH参数将展开得到：

得出：

其中，[x_T y_T z_T]^T＝[d_x d_y d_z]^T，[x₃ y₃ z₃]^T为第三节点的位置坐标，其中，n(n_x，n_y，n_z)，o(o_x，o_y，o_z)，a(a_x，a_y，a_z)为旋转矩阵的三个列向量，三个向量均为单位向量且两两正交，R(z，δ_z)为绕z轴的旋转矩阵，R(y，δ_y)为绕y轴的旋转矩阵，R(x，δ_x)为绕x轴的旋转矩阵。

可选地，第二节点的可行位置构成圆形轨迹，根据第三节点的位置坐标，计算第二节点的位置坐标(x₂，y₂，z₂)具体为根据第三节点的位置坐标解算圆形轨迹的轨迹方程，其中第二节点的轨迹方程为：

x₂＝o₁+rcosφa₁+rsinφb₁

y₂＝o₂+rcosφa₂+rsinφb₂

z₂＝o₃+rcosφa₃+rsinφb₃

其中，r为圆形轨迹的半径，a(a₁，a₂，a₃)和b(b₁，b₂，b₃)是位于圆形轨迹平面内的两个互相正交的单位向量，n为垂直于运动轨迹平面的法向量，o(o₁，o₂，o₃)为第二节点的圆形轨迹的圆心O的坐标，φ为圆形轨迹的相位角。

可选地，根据如下公式计算所述第二节点的圆形轨迹的圆心O的坐标：

其中，d_1，o为第一节点与所述圆心O的距离d_1，3为第一节点与第三节点之间的距离，l_1，3为第一节点指向第三节点的向量。

可选地，根据如下公式计算第二节点的圆形轨迹的半径r：

可选地，根据如下公式计算所述运动轨迹平面内的两个互相正交的单位向量a(a₁，a₂，a₃)和b(b₁，b₂，b₃)：

其中，l_0，1为第一连杆起始端指向第一节点的向量。

可选地，还包括：S4，根据第三节点、第二节点、第一节点、第一连杆的起始端以及第四连杆末端的位置坐标，分别求得所述第四连杆、第三连杆、第二连杆以及第一连杆的向量，根据向量求得每一关节的旋转角度。

可选地，通过如下公式计算每一关节的旋转角度，其中，第i关节的旋转角为θ_i：

θ₁＝arctan(y₂，x₂)

θ₆＝acosT₃₃

其中，n₁₂₃为θ₃等于0时P₁P₂P₃平面的法向量，n₁₂₃′为θ₃等于0时P₁P₂P₃’平面的法向量，T_ij表示T中第i行第j列的元素，其中T为：

(三)有益效果

本发明提供了一种七自由度串联机械臂的位置级逆解方法，通过逆向求解各个节点的位置坐标，通过位置坐标求解各关节的旋转角度，拓展冗余机械臂的应用，面向复杂场景进行机械臂实时灵巧控制，具有重要的实际应用意义。

附图说明

图1示意性示出了本公开实施例中的七自由度串联机械臂的初始状态的结构示意图；

图2示意性示出了本公开实施例中的七自由度串联机械臂的简化结构示意图；

图3示意性示出了本公开实施例中的工作状态下的七自由度串联机械臂的结构示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。

本发明提供了一种七自由度串联机械臂的位置级逆解方法，该机械臂包括七个关节，七个关节串联布置，相邻两关节之间相互垂直，方法包括：S1，将机械臂构型转化为四个连杆和三个节点，其中，第一连杆、第一节点、第二连杆、第二节点、第三连杆、第三节点、第四连杆依次相连；S2，将第一连杆的起始端置于直角坐标系的原点，获取第四连杆末端的位置坐标和姿态坐标，并根据第四连杆末端的位置坐标和姿态坐标，计算第三节点的位置坐标；S3，根据第三节点的位置坐标，计算第二节点的位置坐标。

本发明实施例中以图1中所示的机械臂为例进行详细说明，参见图1，该机械臂位于基直角坐标系X₀Y₀Z₀中，图1为该机械臂的初始状态，该机械臂由七个关节组成，分别为第一关节1、第二关节2、第三关节3、第四关节4、第五关节5、第六关节6以及第七关节7，X_mY_mZ_m分别为第m关节的坐标系，1≤m≤7的整数，参见图1中，黑色旋转箭头标示出每个关节的可活动方向，由图1可知，第一关节1可以绕Z₀轴旋转，第二关节2可以绕Z₁轴旋转，第三关节3可以绕Z₂轴旋转，第四关节4可以绕Z₃轴旋转，第五关节可以绕Z₄轴旋转，第六关节可以绕Z₅轴旋转，第七关节可以绕Z₆轴旋转。在图1所示的初始状态下各关节坐标系的位置关系见表1，表1可知，第二关节2、第四关节4以及第六关节6的等效长度l均为0mm，该机械臂可简化如图2所示的具有四个连杆和三个节点的机械臂。

表1

表1中，α_i-1为从坐标轴z_i-1到z_i绕坐标轴x_i-1旋转的角度；a_i-1为从坐标轴z_i-1到z_i沿坐标轴x_i-1测量的距离；为从坐标轴x_i-1到坐标轴x_i绕坐标轴z_i旋转的角度；l_i为从坐标轴x_i-1到坐标轴x_i沿坐标轴z_i测量的距离。对机械臂进行简化的过程中以两相邻坐标系Z轴间的公垂线长度为连杆长度，将初始横向排列连杆作为节点，其中，第二关节2、第四关节4及第六关节6为横向关节并且长度均为0，因此该机械臂中第一关节被简化为长度为d₁的第一连杆，第三关节被简化为长度为d₂的第二连杆，第五关节被简化为长度为d₃的第三连杆，第七关节被简化为长度为d₄的第四连杆，第二关节简化为第一节点P₁，第四关节简化为第二节点P₂，第六关节简化为第三节点P₃，第一连杆的起始端为节点P₀，第四连杆的末端节点Pe。

S1，将机械臂构型转化为四个连杆和三个节点，其中，第一连杆、第一节点、第二连杆、第二节点、第三连杆、第三节点、第四连杆依次相连；

在本发明实施例中，简化后的模型包括第一关节被简化为长度为d₁的第一连杆，第三关节被简化为长度为d₂的第二连杆，第五关节被简化为长度为d3的第三连杆，第七关节被简化为长度为d₄的第四连杆，第二关节简化为第一节点P₁，第四关节简化为第二节点P₂，第六关节简化为第三节点P₃，第一连杆的起始端为节点P₀，第四连杆的末端节点Pe。

S2，将第一连杆的起始端置于直角坐标系的原点，获取第四连杆末端的位置坐标和姿态坐标，并根据第四连杆末端的位置坐标和姿态坐标，计算第三节点的位置坐标；

将步骤S1获得的简化模型设于直角坐标系X₀Y₀Z₀中，使第一连杆起始端P₀与基直角坐标系的原点重合，参见图2，从第一连杆起始端P₀开始依次为第一连杆(对应于第一关节1)、第二连杆(对应于第三关节3)、第三连杆(对应于第五关节5)以及第四连杆(对应于第七关节7)，其中，第一连杆和第二连杆连接点为第一节点P₁、第二连杆和第三连杆连接点为第二节点P₂、第三连杆和第四连杆连接点为第三节点P₃。

通过测量可以得到第一连杆、第二连杆、第三连杆以及第四连杆对应的连杆长度依次为d₁、d₂、d₃以及d₄，在第四连杆末端Pe的位姿固定时，第一连杆与基直角坐标系的Z轴始终重合，也即P₁点的坐标为(0 0 d1)，节点P₂的可行轨迹为圆形(如图2所示)，因此可以通过求解该圆形运动轨迹的参数方程，获得在第四连杆末端Pe的位姿下第二节点P₂对应的所有可行位置，进一步得到各关节角度的解。

在第四连杆末端Pe位姿已知时，本发明实施例中若想求得第二节点P₂的位置坐标必须首先求得第三节点P₃的位置坐标，具体如下：

S21，获取第四连杆末端的位姿相对于直角坐标系的变换矩阵

S22，第三节点相对于直角坐标系的变换矩阵

S23，第四连杆末端相对于第三节点的变换矩阵为得到

S24，根据以及DH参数计算得到第三节点的位置坐标。

对应的本发明实施例中首先获取第四连杆末端P_e的位姿为P_e＝[d_x d_y d_z δ_x δ_yδ_z]^T，其中，前三项d_x、d_y、d_z为该第四连杆末端P_e的位置坐标，δ_x、δ_y、δ_z为第四连杆末端P_e的姿态坐标，在姿态坐标以ZYX欧拉角表示，将第四连杆末端P_e位姿相对于直角坐标系进行变换得到变换矩阵首先设定第三节点也即节点P3的位置坐标为[x₃y₃ z₃]^T，第三节点P3相对于直角坐标系进行变换得到变换矩阵于是得到根据DH参数，可以展开得到如下关系式：

推导得出：

其中[x_T y_T z_T]^T＝[d_x d_y d_z]^T，其中，n(n_x，n_y，n_z)，o(o_x，o_y，o_z)，a(a_x，a_y，a_z)为旋转矩阵的三个列向量，三个向量均为单位向量且两两正交，R(z，δ_z)为绕z轴的旋转矩阵，R(y，δ_y)为绕y轴的旋转矩阵，R(x，δ_x)为绕x轴的旋转矩阵。

至此可以求得第三节点P₃的位置坐标为[x₃ y₃ z₃]^T。

S3，根据第三节点的位置坐标，计算第二节点的位置坐标。；

由步骤S2可以求得第三节点P₃对应的位置坐标[x₃ y₃ z₃]^T，第二节点P₂对应的可行的圆形运动轨迹的圆心坐标设为o(o₁，o₂，o₃)，第二节点P₂对应的圆形运动轨迹的参数方程为：

其中，r为轨迹圆半径，a(a₁，a₂，a₃)和b(b₁，b₂，b₃)是位于原平面内的两个互相正交的单位向量，n为垂直于轨迹圆平面的法向量，φ为该圆形轨迹的相位角。

可以通过以下方式求解圆心O的坐标，首先根据几何法得到如下公式：

其中，d_1，3表示第一节点P₁与第三节点P₃间的距离，d_1，o表示节点P₁与圆心O的距离；

通过上式(3)可得到：

进而得到该圆形运动轨迹的半径r以及其圆心坐标O：

其中，l_1，3为第一节点P₁指向第三节点P₃的向量，由上可以计算出第三节点P₃的位置坐标，由以上可知P₁的坐标为(0 0 d₁)，至此可以求得圆心O的坐标。

可以通过以下方式求圆形运动轨迹平面内任意的两个正交单位向量a(a₁，a₂，a₃)和b(b₁，b₂，b₃)：

其中，l_0，1为节点P₀指向节点P₁的向量。

将公式(4)～(7)代入公式(2)即可得到在给定φ下节点P₂的坐标(x₂y₂，z₂)^T。

S4，根据第三节点、第二节点、第一节点、第一连杆的起始端以及第四连杆末端的位置坐标，分别求得第四连杆、第三连杆、第二连杆以及第一连杆的向量，根据所述向量求得每一关节的旋转角度。

由上述步骤S1～S3可以求得第三节点P₃、第二节点P₂以及第一节点P₁的位置坐标，因此可以求得第四连杆、第三连杆、第二连杆以及第一连杆的向量，进一步得出每一关节的旋转角度θ，参见图3可知，第一关节1对应于简化模型中的第一连杆，第三关节3对应于简化模型中的第二连杆，第五关节5对应于简化模型中的第三连杆，第七关节对应于简化模型中的第四连杆，第一关节1也即第一连杆的旋转角度为θ₁，第一关节1始终垂直于本发明实施例中的XY平面，第二连杆在第二关节2的带动下只能沿垂直于第二关节2的轴线的方向转动，第一关节1的旋转角度θ₁即为第二连杆在xy平面上投影后与X轴的夹角，θ₂为第二关节2的旋转角度，第二连杆在第二关节2的带动下只能沿垂直于第二关节2的轴线的方向转动，因此θ₂的大小即为第一连杆与第二连杆的夹角，由于由上计算得到P₀，P₁，P₂的坐标，因此很容易得到第一连杆与第二连杆的向量，进而计算到θ₂，同理由P₁，P₂，P₃计算出第二连杆和第三连杆的向量，进而计算出第二连杆和第三连杆之间的夹角也即第四关节4的转角θ₄，因此可得到第一关节1、第二关节2和第四关节4的转角θ₁，θ₂，θ₄的计算公式为：

在初始位置时P₁，P₂，P₃在同一平面上，当第三关节3旋转某一角度θ₃时，使得第三节点的位置为P’₃不在原P₃位置处，因此θ₃的大小为平面P₁，P₂，P₃与P₁，P₂，P’₃的夹角，为了求解该二面角，θ₃为0时，第四关节4的坐标系相对于P₀所在的原点的基坐标系的变换矩阵为其中，为第一关节1的坐标系相对于P₀所在的原点的基坐标系的变换矩阵，为第二关节2的坐标系相对于第一关节1的坐标系的变换矩阵，为第三关节3转动θ₃角度后的坐标系相对于第二关节2的坐标系的变换矩阵，为第四关节4的坐标系相对于第三关节3的坐标系的变换矩阵。

其中，与式(8)结合得到

与公式(1)同理得到：

其中，P₃′＝[x₃′，y₃′，z₃′]^T；

简化后得到：

从而得到P’₃的坐标，进而得到l_2，3′，由于l_1，2和l_2，3是平面P₁P₂P₃不共线的两个向量，l_1，2和l_2，3′是平面P₁P₂P₃′不共线的两个向量，则两个平面的法向量为：

从而得到二面角为：

根据上述求得的θ₁～θ₄，可以求得基于其中，为第五关节5的坐标系相对于第四关节4的坐标系的变换矩阵，为第六关节6的坐标系相对于第五关节5的坐标系的变换矩阵，为第七关节7的坐标系相对于第六关节6的坐标系的变换矩阵，为第四关节4的坐标系相对于基坐标系的实际变换矩阵，可以展开得到：

令其中T_ij表示T中第i行第j列的元素，于是根据式(13)，可以求解θ₅～θ₇各角度值。

通过以上方式，即可求各关节的坐标以及各关节的旋转角度。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种七自由度串联机械臂的位置级逆解方法，所述机械臂包括七个关节，所述七个关节串联布置，相邻两关节之间相互垂直，其特征在于，所述方法包括：

S1，将所述机械臂构型转化为四个连杆和三个节点，其中，第一连杆、第一节点、第二连杆、第二节点、第三连杆、第三节点、第四连杆依次相连；

S2，将所述第一连杆的起始端置于直角坐标系的原点，获取所述第四连杆末端的位置坐标和姿态坐标，并根据所述第四连杆末端的位置坐标和姿态坐标，计算所述第三节点的位置坐标；

S3，根据所述第三节点的位置坐标，计算所述第二节点的位置坐标。

2.根据权利要求1所述的方法，其特征在于，所述步骤S2中所述并根据所述第四连杆末端的位姿，计算所述第三节点的位置坐标具体为：

获取所述第四连杆末端的位姿相对于所述直角坐标系的变换矩阵

所述第三节点相对于所述直角坐标系的变换矩阵

所述第四连杆末端相对于第三节点的变换矩阵为得到

根据以及DH参数计算得到所述第三节点的位置坐标。

3.根据权利要求2所述的方法，其特征在于，所述步骤S1还包括获取所述四个连杆上每一连杆的长度，其中，第一连杆、第二连杆、第三连杆以及第四连杆的连杆长度分别为d₁、d₂、d₃以及d₄。

4.根据权利要求3所述的方法，其特征在于，所述第四连杆末端的位姿为[d_x d_y d_z δ_xδ_y δ_z]^T，其中，d_x、d_y、d_z为所述第四连杆末端的位置坐标，δ_x、δ_y、δ_z为所述第四连杆末端的的姿态坐标，其特征在于，所述根据以及DH参数计算得到所述第三节点的位置坐标具体为：

根据DH参数将所述展开得到：

得出：

其中，[x_T y_T z_T]^T＝[d_x d_y d_z]^T，[x₃ y₃ z₃]^T为第三节点的位置坐标，其中，n(n_x，n_y，n_z)，o(o_x，o_y，o_z)，a(a_x，a_y，a_z)为旋转矩阵的三个列向量，三个向量均为单位向量且两两正交，R(z，δ_z)为绕z轴的旋转矩阵，R(y，δ_y)为绕y轴的旋转矩阵，R(x，δ_x)为绕x轴的旋转矩阵。

5.根据权利要求4所述的方法，所述第二节点的可行位置构成圆形轨迹，其特征在于，所述根据所述第三节点的位置坐标，计算所述第二节点的位置坐标(x₂，y₂，z₂)具体为根据第三节点的位置坐标解算所述圆形轨迹的轨迹方程，其中所述第二节点的轨迹方程为：

x₂＝o₁+rcosφa₁+rsinφb₁

y₂＝O₂+rcosφa₂+rsinφb₂

z₂＝o₃+rcosφa₃+rsinφb₃

其中，r为所述圆形轨迹的半径，a(a₁，a₂，a₃)和b(b₁，b₂，b₃)是位于所述圆形轨迹平面内的两个互相正交的单位向量，n为垂直于所述运动轨迹平面的法向量，o(o₁，o₂，o₃)为第二节点的圆形轨迹的圆心O的坐标，φ为所述圆形轨迹的相位角。

6.根据权利要求5所述的方法，其特征在于，根据如下公式计算所述第二节点的圆形轨迹的圆心O的坐标：

其中，d_1，o为第一节点与所述圆心O的距离d_1，3为第一节点与第三节点之间的距离，l_1，3为第一节点指向第三节点的向量。

7.根据权利要求6所述的方法，其特征在于，根据如下公式计算所述第二节点的圆形轨迹的半径r：

。

8.根据权利要求6或7所述的方法，其特征在于，根据如下公式计算所述运动轨迹平面内的两个互相正交的单位向量a(a₁，a₂，a₃)和b(b₁，b₂，b₃)：

其中，l_0，1为第一连杆起始端指向第一节点的向量。

9.根据权利要求1所述的方法，其特征在于，还包括：

S4，根据所述第三节点、第二节点、第一节点、第一连杆的起始端以及第四连杆末端的位置坐标，分别求得所述第四连杆、第三连杆、第二连杆以及第一连杆的向量，根据所述向量求得每一关节的旋转角度。

10.根据权利要求9所述的方法，其特征在于，通过如下公式计算每一关节的旋转角度，其中，第i关节的旋转角为θ_i：

θ₁＝arctan(y₂，x₂)

θ₆＝a cosT₃₃

其中，n₁₂₃为θ₃等于0时P₁P₂P₃平面的法向量，n₁₂₃′为θ₃等于0时P₁P₂P₃’平面的法向量，T_ij表示T中第i行第j列的元素，其中T为：

。