CN109291046A - 一种七自由度拟人构型机械臂逆运动学规划方法 - Google Patents

Abstract

本发明涉及一种七自由度拟人构型机械臂逆运动学规划方法，包括以下步骤：首先将七个自由度配置成“肩‑肘‑腕”式结构，由基座至末端分别定义为1‑7关节，然后，求解肘关节(4关节)角度，并将“肩‑肘‑腕”三点组成的空间平面与参考平面之间夹角定义为冗余角。最后，根据优化的冗余角分别解算肩关节角度(1‑3关节)和腕关节角度(5‑7关节)。本发明可以解决六自由度操作空间位姿向七自由度关节空间映射的逆运动学规划问题，实现空间六自由度与关节七自由度映射关系；建立冗余角与肩关节和腕关节的函数关系方法，通过优化冗余角方法选取最优逆解。

Description

一种七自由度拟人构型机械臂逆运动学规划方法

技术领域

本发明涉及机器人领域，具体地说是一种七自由度拟人构型机械臂逆运动学规划方法，特别适用于机器人运动控制和轨迹规划。

背景技术

在与人协作类机器人中，七自由度拟人构型机械臂是一种常见结构。此类机械臂被广泛应用于柔性化工业生产线、电子元器件装配、医疗与服务等多种行业。与传统六自由度机械臂相比，七自由度拟人构型机械臂具有更大、更灵活的工作空间，特别适合应用在与人交互工作环境中。

机械臂逆运动学规划方法是轨迹规划与控制的基础。传统工业机械臂的逆运动学规划方法无法直接应用于七自由度拟人构型机械臂中。其最大区别在于六自由度操作空间轨迹如何映射到七自由度关节空间轨迹。目前已有的解方法多采用“数值解法”，即在每个迭代周期内通过求解雅可比伪逆方法求解。“数值解”虽然能够处理所有具有冗余自由度构型的机械臂逆运动学问题，但存在误差大、计算效率低等缺点。若将此类方法应用于轨迹规划中，会降低控制器的运行效率，甚至会降低实时性。因此，七自由度机械臂逆运动学规划问题仍然存在未完全解决的难点。本发明针对以上难点，发明一种七自由度拟人构型机械臂逆运动学规划方法，方法针对此种构型采用“解析解法”，实现了快速、高精度的逆运动学规划方法。

发明内容

针对现有技术的不足，本发明提出了一种七自由度拟人构型机械臂逆运动学规划方法。在该方法中，根据如何以最优化方式精确快速的将六自由度操作空间轨迹映射到七自由度关节空间轨迹问题，实现七自由度拟人构型机械臂的运动学逆解，进而实现机器人的轨迹规划。

本发明为实现上述目的所采用的技术方案是：一种七自由度拟人构型机械臂逆运动学规划方法，包括以下步骤：

步骤1：将机器人的七个自由度配置成肩-肘-腕结构，由基座至末端依次定义为关节1～7；其中，肩关节包含关节1～3的3个自由度且旋转中心重合于点P_s；肘关节即关节4包含1个自由度，旋转中心为P_e；腕关节包含关节5～7的3个自由度且旋转中心重合于点P_w；

步骤2：根据末端期望位置和姿态求解出肩-腕中心直线段在基坐标系中的向量

步骤3：根据肩-肘长即上臂长l_se、肘-腕长即下臂长l_ew和肩-腕长构成的三角形，求解肘关节即关节4角度θ₄；

步骤4：求解出肩-腕中心直线段在关节3坐标系中的向量再结合基坐标系中的向量同时取关节3角度固定不变情况θ₃＝0，综合求解此时关节1角度θ_{1_0}和2关节角度θ_{2_0}；将此时由θ_{1_0}和θ_{2_0}确定的肩-肘-腕平面定义为参考平面；

步骤5：由肩-肘-腕的三个旋转中心组成的空间平面与参考平面夹角为冗余角建立肩关节和腕关节6个关节的角度θ₁、θ₂、θ₃、θ₅、θ₆、θ₇与冗余角的函数关系其中θ＝[θ₁,θ₂,θ₃,θ₅,θ₆,θ₇]^T；

步骤6：将多变量优化问题转化为单变量优化问题；根据优化目标，求解代价函数且满足约束条件得到最优化冗余角

步骤7：根据冗余角和步骤5中关系，分别求解肩关节角度θ₁、θ₂、θ₃和腕关节角度θ₅、θ₆、θ₇。

所述向量表示基坐标系原点到关节1坐标原点距离，l_wt表示关节5坐标原点到末端距离。

所述向量分别表示关节4坐标系相对于关节3坐标系的旋转矩阵、关节3坐标系中上臂长、关节4坐标系中下臂长。

所述步骤3中，θ₄根据余弦定理求解，存在2组解：θ₄>0和θ₄<0；在离散轨迹情况下，θ₄由人为选取符号正负；在连续轨迹情况下，θ₄的选取根据前一周期角度θ_4,k-1的值确定符号的正负。

所述步骤6中，根据不同工作需求选取优化目标代价函数指标包括最小能量消耗、速度恒定、加速度最小。

所述步骤7，θ₂由求解其反余弦值函数得到，分别存在2组解：θ₂>0和θ₂<0；在离散轨迹点情况下，θ₂由人为选取符号正负；在连续轨迹点情况下，θ₂的选取根据前一周期角度θ_2,k-1的值确定符号的正负。

所述步骤7，θ₆由求解其反余弦值函数得到，分别存在2组解：θ₆>0和θ₆<0；在离散轨迹点情况下，θ₆由人为选取符号正负；在连续轨迹点情况下，θ₆的选取根据前一周期角度θ_6,k-1的值确定符号的正负。

根据得到的7个关节的旋转角度θ₁～θ₇的值得到机器人各关节期望轨迹，构成关节空间的运动轨迹，实现轨迹的规划。

根据关节空间的运动轨迹对机器人各关节进行控制。

本发明具有以下有益效果及优点：

1.本发明提出了定义参考平面和冗余角的方法，实现空间六自由度与关节七自由度映射关系；

2.本发明提出了建立冗余角与肩关节和腕关节的函数关系方法，通过优化冗余角方法选取最优逆解；

3.本发明提出了2、4、6关节多解的选取方式，离散轨迹情况下通过人工方式选取，连续轨迹情况下通过前一周期的关节角度选取。

附图说明

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

图2a为本发明中机械臂的结构配置示意图一；

图2b为本发明中机械臂的结构配置示意图二；

图3为本发明中操作空间运动轨迹示意图；

图4为本发明中关节运动轨迹曲线图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

本发明适用于七自由度拟人构型机械臂，此构型要求：1-3关节旋转中心(坐标系原点)重合于一点，5-7关节旋转中心(坐标系原点)重合于一点，如图2a～图2b所示。

如图1所示，为本发明流程示意图。

步骤1：将七个自由度配置成具有“肩-肘-腕”式结构，由基座至末端分别定义为关节1-7。其中，肩关节包含3个自由度(关节1-3)且旋转中心重合于点P_s；肘关节包含1个自由度(关节4)，旋转中心为P_e；腕关节包含3个自由度(关节5-7)且旋转中心重合于点P_w，如图2a～图2b所示。运动学参数简化为四组：l_bs、l_se、l_ew、l_wt分别表示：基坐标系原点到1关节坐标原点(同时也是2、3关节坐标原点)距离、1关节坐标原点(同时也是2、3关节坐标原点)到4关节坐标原点距离、4关节坐标原点到5关节坐标原点(同时也是6、7关节坐标原点)距离、5关节坐标原点(同时也是6、7关节坐标原点)到工具末端距离。点P_s、P_e、P_w分别肩、肘、腕各自坐标系的原点。

步骤2：根据末端期望位置和姿态求解出“肩-腕”中心直线段在基坐标系中的向量

其中，上标“0”表示在基坐标系的向量，为基坐标系中的原点到1关节坐标原点距离。

步骤3：根据上臂长l_se(“肩-肘”长)、下臂长l_ew(“肘-腕”长)和“肩-腕”长构成的三角形，求解肘关节(4关节)角θ₄，根据

求解θ₄。此时θ₄存在2种不同符号的解，当操作空间轨迹点为离散情况，实际确实存在2组合理解，由人为选取。当操作空间轨迹点为连续情况，实际存在1组合理解，即前后两个规划周期内的转角之差不发生大跳变(在阈值范围内)的情况，此时根据前一周期关节角度θ_4,k-1符号判断，当θ_4,k-1>0时θ₄>0，反之θ₄<0，θ₄＝0的情况不存在，已超出工作空间；

步骤4：求解出“肩-腕”中心直线段在3关节坐标系中的向量

再结合基坐标系中的向量同时取3关节角度固定不变情况θ₃＝0，综合求解此时1关节θ_{1_0}和2关节角度θ_{2_0}，分别表示4关节坐标系相对于3关节坐标系的旋转矩阵、3关节坐标系中上臂长、4关节坐标系中下臂长，

其中，表示当θ₃＝0时由关节1夹角θ_{1_0}确定的1关节相对于基座的旋转矩阵，表示当θ₃＝0时由关节2夹角θ_{2_0}确定的2关节相对1关节的旋转矩阵。将此时由θ_{1_0}和θ_{2_0}确定的“肩-肘-腕”平面定义为“参考平面”；表示3关节坐标系相对于2关节坐标系的旋转矩阵。

步骤5：定义由“肩-肘-腕”三点组成的空间平面与“参考平面”夹角为冗余角同时，建立肩关节和腕关节6个角度θ₁、θ₂、θ₃、θ₅、θ₆、θ₇与冗余角的函数关系其中θ＝[θ₁,θ₂,θ₃,θ₅,θ₆,θ₇]^T。的具体表达如下：

其中，参数a_sij，b_sij和c_sij分别为矩阵A_s，B_s和C_s的第i行j列的数值，A_s，B_s和C_s由下式确定

表示3关节坐标系到基坐标系的旋转矩阵；

其中，参数a_wij，b_wij和c_wij分别为矩阵A_w，B_w和C_w的第i行j列的数值，A_w，B_w和C_w由下式确定

表示7关节坐标系到关节坐标系的旋转矩阵。i、j＝1…3。

步骤6：将多变量优化问题转化为单变量优化问题。根据优化目标，求解代价函数优化目标可选择最小能量消耗、速度恒定、加速度最小等指标。且满足约束条件Ψ表示冗余角满足约束条件的可行解的集合，最后求解得到最优化冗余角优化目标根据不同应用人为选取。例如，可选择肩关节和腕关节运动速度最小为优化指标，则求解代价函数

为最小值时的冗余角作为其中θ＝[θ₁,θ₂,θ₃,θ₅,θ₆,θ₇]^T由函数关系表示，且满足约束条件这里Ψ可选取为关节角的上限与下限之间的集合。

步骤7：根据已求出冗余角和步骤5中关系，分别求解肩关节角度θ₁、θ₂、θ₃和腕关节角度θ₅、θ₆、θ₇。

所述步骤3，θ₄根据余弦定理求解，存在2组不同解：θ₄>0和θ₄<0(θ₄＝0为奇异点，应在轨迹规划中排出)。在离散轨迹情况下，θ₄由人为选取符号正负。在连续轨迹情况下，θ₄的选取根据前一周期角度θ_4,k-1的值确定符号的正负，当θ_4,k-1>0时θ₄>0，反之θ₄<0，θ₄＝0的情况不存在，已超出工作空间；

所述步骤6，根据不同工作需求选取优化目标代价函数可选择最小能量消耗、速度平稳、加速度最小等指标进行优化，该方法增加了七自由度运动灵活性。

所述步骤7，θ₂由求解其反余弦值函数得到，分别存在2组解：θ₂>0和θ₂<0(θ₂＝0为奇异点，应在轨迹规划中排出)。在离散轨迹情况下，θ₂由人为选取符号正负。在连续轨迹情况下，θ₂的选取根据前一周期k-1角度θ_2,k-1的值确定符号的正负，当θ_2,k-1>0时θ₂>0，反之θ₂<0，θ₂＝0的情况不存在，已超出工作空间。同理，θ₆存在2组解：θ₆>0和θ₆<0(θ₆＝0为奇异点，应在轨迹规划中排出)。在离散轨迹情况下，θ₆由人为选取符号正负。在连续轨迹情况下，θ₆的选取根据前一周期角度θ_6,k-1的值确定符号的正负，当θ_6,k-1>0时θ₆>0，反之θ₆<0，θ₆＝0的情况不存在，已超出工作空间。

整个解算过程要应用于机器人轨迹规划和控制方法中，每个控制周期对整个过程进行一次完整的解算。将解算得到的7个关节旋转角度θ₁～θ₇的值作为规划的期望轨迹，发送给各关节进行控制。

本发明的效果可以通过以下仿真进一步说明。

仿真内容：

七自由度拟人构型机械臂如图所示，运动学参数为l_bs＝347mm、l_se＝450mm、l_ew＝450mm、l_wt＝302.8mm。1-7关节初始位置为：[-35.19,95.02，-78.94，-85.13，3.04,49.73，-13.88]弧度。末端执行器运动轨迹为矩形，矩形四点在末端工具坐标系的位置分别为：[0,0,0]^T，[0,-200,0]^T，[-200,-200,-200]^T，[-200,0,-200]^T，姿态保持不变，冗余角度保持不变。将操作空间矩形轨迹(如图3)中每个采样点经步骤1-7解算得到关节轨迹(如图4)。

以上描述仅是本发明的具体实例，未构成对本发明的任何限制，显然对于本领域的专业人员来说，在了解了本发明内容和原理后，都可能在不背离本发明原理、结构的情况下，进行形式和细节上的各种修正和改变，但是这些基于本发明思想的修正和改变仍在本发明的权利要求保护范围之内。

Claims (9)

1.一种七自由度拟人构型机械臂逆运动学规划方法，其特征在于包括以下步骤：

步骤1：将机器人的七个自由度配置成肩-肘-腕结构，由基座至末端依次定义为关节1～7；其中，肩关节包含关节1～3的3个自由度且旋转中心重合于点P_s；肘关节即关节4包含1个自由度，旋转中心为P_e；腕关节包含关节5～7的3个自由度且旋转中心重合于点P_w；

步骤2：根据末端期望位置和姿态求解出肩-腕中心直线段在基坐标系中的向量

步骤3：根据肩-肘长即上臂长l_se、肘-腕长即下臂长l_ew和肩-腕长构成的三角形，求解肘关节即关节4角度θ₄；

步骤4：求解出肩-腕中心直线段在关节3坐标系中的向量再结合基坐标系中的向量同时取关节3角度固定不变情况θ₃＝0，综合求解此时关节1角度θ_{1_0}和2关节角度θ_{2_0}；将此时由θ_{1_0}和θ_{2_0}确定的肩-肘-腕平面定义为参考平面；

步骤5：由肩-肘-腕的三个旋转中心组成的空间平面与参考平面夹角为冗余角建立肩关节和腕关节6个关节的角度θ₁、θ₂、θ₃、θ₅、θ₆、θ₇与冗余角的函数关系其中θ＝[θ₁,θ₂,θ₃,θ₅,θ₆,θ₇]^T；

步骤6：将多变量优化问题转化为单变量优化问题；根据优化目标，求解代价函数且满足约束条件得到最优化冗余角

步骤7：根据冗余角和步骤5中关系，分别求解肩关节角度θ₁、θ₂、θ₃和腕关节角度θ₅、θ₆、θ₇。

2.根据权利要求1所述的一种七自由度拟人构型机械臂逆运动学规划方法，其特征在于所述向量 表示基坐标系原点到关节1坐标原点距离，l_wt表示关节5坐标原点到末端距离。

3.根据权利要求1所述的一种七自由度拟人构型机械臂逆运动学规划方法，其特征在于所述向量 分别表示关节4坐标系相对于关节3坐标系的旋转矩阵、关节3坐标系中上臂长、关节4坐标系中下臂长。

4.根据权利要求1所述的一种七自由度拟人构型机械臂逆运动学规划方法，其特征在于：所述步骤3中，θ₄根据余弦定理求解，存在2组解：θ₄>0和θ₄<0；在离散轨迹情况下，θ₄由人为选取符号正负；在连续轨迹情况下，θ₄的选取根据前一周期角度θ_4,k-1的值确定符号的正负。

5.根据权利要求1所述的一种七自由度拟人构型机械臂逆运动学规划方法，其特征在于：所述步骤6中，根据不同工作需求选取优化目标代价函数指标包括最小能量消耗、速度恒定、加速度最小。

6.根据权利要求1所述的一种七自由度拟人构型机械臂逆运动学规划方法，其特征在于：所述步骤7，θ₂由求解其反余弦值函数得到，分别存在2组解：θ₂>0和θ₂<0；在离散轨迹点情况下，θ₂由人为选取符号正负；在连续轨迹点情况下，θ₂的选取根据前一周期角度θ_2,k-1的值确定符号的正负。

7.根据权利要求1所述的一种七自由度拟人构型机械臂逆运动学规划方法，其特征在于：所述步骤7，θ₆由求解其反余弦值函数得到，分别存在2组解：θ₆>0和θ₆<0；在离散轨迹点情况下，θ₆由人为选取符号正负；在连续轨迹点情况下，θ₆的选取根据前一周期角度θ_6,k-1的值确定符号的正负。

8.根据权利要求1所述的一种七自由度拟人构型机械臂逆运动学规划方法，其特征在于根据得到的7个关节的旋转角度θ₁～θ₇的值得到机器人各关节期望轨迹，构成关节空间的运动轨迹，实现轨迹的规划。

9.根据权利要求8所述的一种七自由度拟人构型机械臂逆运动学规划方法，其特征在于根据关节空间的运动轨迹对机器人各关节进行控制。