肩 关节中心点和腕关节中心点的连线转动的角度。腕关节中心点和执行末端姿态已知时，肩 关节中心点、肘关节中心点和腕关节中心点组成的平面可以绕腕关节中心点矢量转动，称 其为自运动平面，对应转角称为自运动角，并规定当工具坐标点为（x，y，z)时，自运动角为 零时自运动平面的法矢方向为（_y，X，0)。自运动角的大小为其余自运动平面对应的自运动 角大小为自运动平面法矢与自运动角为零时自运动平面法矢方向的夹角。
[0053] 3、下面介绍本发明结合实际机构尺寸的逆运动学计算过程及结果：
[0054] 如图2所示，建立SRS构型冗余度机械臂的数学模型，建立坐标系，当腕关节在初 始状态时，腕关节的坐标系由肘关节坐标系平移得到，坐标系必须这样建立才能进一步优 化计算；其中肩关节转动中心为A，肘关节转动中心为B，腕关节转动中心为C，AB之间的距 离为I 1, BC之间的距离为12, AC之间的距离为d。
[0055] 如图1所示，初始状态下的机械臂状态。机械臂上电后初始化，初始位置自运动角 Θ Q的数值为零。给定一条运动轨迹的插补点序列〇，1，2, 3···。由初始位置自运动角和插补 点〇计算关节空间的运动角度。设上一步计算的腕关节姿态矩阵的Z轴与腕关节中心点矢 量所在的平面M，计算M与大臂小臂平面的夹角Ci 1。新的自转角新的自运动角Q1*如下公 式：θ 1= θ Q+P*a i+lX Θ。- Θ。）求出，其中P = 〇. 2, I = 0. 1。通过调节PI可以改善关 节空间的运动平滑性。存储当前插补点〇处的自运动角S1。已知η处自运动角时，由插补 点η处自运动角和插补点η+1处给定末端位姿计算插补点η+1处关节空间的运动角度。设 上一步计算的腕关节姿态矩阵的Z轴与腕关节中心点矢量所在的平面Μ，计算M与大臂小臂 平面的夹角α η+1。插补点η+1处的自运动角θη+2由如下公式：θ η+1= θ η+Ρ*αη+1+Ι*(θη-θ 〇)求出，其中P = 〇. 2, I = 0. 1。由初始状态的值和递推公式可以求得所有插补点处的 关节运动角度。将计算得到的结果传递给下位机控制器，驱动机械臂完成给定的末端轨迹。
[0056] 上文所述给定自运动角Θ和执行末端位姿时，计算关节空间转动角度的步骤如 下：
[0057] 1)设腕关节中心点矢量AC为（x，y，z)，则自转角为0时大臂小臂平面ABC的法向 量 N 为（_y, X，0)。
[0058] 2)将1)中所述矢量N绕腕关节中心点矢量AC转动自运动角值Θ得到大臂小臂 平面ABC的法矢量N 1;
[0059] 3)大臂长度I1和小臂长度I2已知，已知AC为（x，y，z)，求得
【主权项】
1. 一种面向SRS仿人手臂的自运动角计算方法，其特征在于：它包括如下步骤： 步骤一：建立SRS构型冗余度机械臂的数学模型，根据结构特点建立肩关节、肘关节、 腕关节坐标系； 步骤二：由当前状态下的自运动角通过PID算法计算给定工具坐标系位姿时的自运动 角； 步骤由步骤二的自运动角和工具坐标系位姿计算关节空间运动角度； 步骤四；将步骤=的求解结果传递给下位机控制器，驱动机械臂实现给定工具坐标系 位姿的运动。
2. 根据权利要求1所述的一种面向SRS仿人手臂的自运动角计算方法，其特征在于： 步骤一所述"建立SRS构型冗余度机械臂的数学模型"，其实现过程如下：将测量肩关 节中屯、到肘关节中屯、的距离和肘关节中屯、到腕关节中屯、的距离，其几何模型为一个=动杆 机构。
3. 根据权利要求1所述的一种面向SRS仿人手臂的自运动角计算方法，其特征在于： 基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，所述SRS构型S为球副，R 转动副，文中所述中肩关节、肘关节、腕关节分别对应S球副、R转动副、S球副。
4. 根据权利要求1所述的一种面向SRS仿人手臂的自运动角计算方法，其特征在于： 基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，坐标系建立如下所述：将 肩关节转动中屯、、轴关节转动中屯、的连线称为大臂，肘关节转动中屯、、腕关节转动中屯、的连 线称为前臂，在初始状态下肩关节坐标系Z轴与大臂平行，并由肩关节转动中屯、指向肘关 节转动中屯、；Y轴平行于肘关节转动轴线，肘关节坐标系Z轴与小臂平行，并由肘关节转动 中屯、指向腕关节转动中屯、，Y轴为肘关节转动轴线方向，腕关节处坐标系为肘关节坐标系沿 前臂平移。
5. 根据权利要求1所述的一种面向SRS仿人手臂的自运动角计算方法，其特征在于： 基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，参数自运动角0的含义： 腕关节中屯、点和执行末端姿态已知时，肩关节中屯、点、肘关节中屯、点和腕关节中屯、点组成 的平面绕腕关节中屯、点矢量转动，称其为自运动平面，对应转角称为自运动角，并规定当工 具坐标点为（x，y，z)时，自运动角为零时自运动平面的法矢方向为（-y，x，0);其余自运动 平面对应的自运动角大小为自运动平面法矢与自运动角为零时自运动平面法矢方向的夹 角。
6. 根据权利要求1所述的一种面向SRS仿人手臂的自运动角计算方法，其特征在于： 基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，其中步骤二中所述PID算 法具体过程如下： 1) 由当前状态的自运动角0。和给定的执行末端点的位姿确定各个关节运动角度； 2) 将步骤二计算得到的关节角度，计算腕关节姿态矩阵的Z轴与腕关节中屯、点矢量构 成的平面与大臂小臂平面之间的夹角a。； 3) 新的自运动角0。4由如下公式；0。4= 0 n+P*a。+1*( 0。- 0。）求出； 式中符号说明如下： 0。+1为新的自运动角，0。为上一次计算得到的自运动角，0。为初始状态的自运动角， P、I为PI可调节参数； 4)在内存中存储自运动角0。+1; 该算法需给定初始状态时的自运动角0。，由初始状态时的自运动角0。按照上面 1)-4)所述递推。
7.根据权利要求1所述的一种面向SRS仿人手臂的自运动角计算方法，其特征在于： 基于自运动角的SRS构型冗余度机械臂逆运动学计算方法，由自运动角和执行末端位 姿计算关节角度的步骤如下： 步骤1)由腕关节中屯、点求出自运动角为0时大臂小臂的法向矢量N ;若腕关节中屯、点 为（X，y, Z)，则上面所述矢量N为（-y, X，0); 步骤2)将步骤1)中所述矢量N绕腕关节中屯、点（x，y，z)转动自运动角值0。得到大 臂小臂平面的法矢量Ni; 步骤3)通过余弦定理求出肩关节中屯、点，肘关节中屯、点和腕关节中屯、点构成的S角 形的S个角度，大臂与肩关节、腕关节连线的夹角用符号a表示，大臂小臂之间的夹角用 符号0表不； 步骤4)将腕关节中屯、点矢量绕步骤2)求出的法矢量Ni转动步骤3)求出的角度a， 得出大臂的矢量方向AB，AB方向即肩关节转动后的Z轴方向，步骤2)求出的法矢量Ni为 肩关节转动后的Y轴，转动后肩关节的坐标确定； 步骤5)肘关节的转动角度即步骤3)中所求角度JT-P ; 步骤6)运动后腕关节的坐标是通过肩关节的旋转矩阵乘W肘关节的旋转矩阵，然后 对两矩阵的乘积求逆，最后将求得的逆矩阵乘W给定执行末端的姿态矩阵得到。
【专利摘要】一种面向SRS仿人手臂的自运动角计算方法，它包括如下步骤：步骤一：建立SRS构型冗余度机械臂的数学模型，根据结构特点建立肩关节、肘关节、腕关节坐标系；步骤二：由当前状态下的自运动角通过PID算法计算给定工具坐标系位姿时的自运动角；步骤三：由步骤二的自运动角和工具坐标系位姿计算关节空间运动角度；步骤四：将步骤三的求解结果传递给下位机控制器，驱动机械臂实现给定工具坐标系位姿的运动。本发明给出了一种冗余度机械臂的逆运动学求法，和传统的计算方法相比，算法简单，计算速度快，可以得到解析解，在降低了求解难度的同时对关节运动做了一定的优化。
【IPC分类】G05D3-12
【公开号】CN104635762
【申请号】CN201510017243
【发明人】胡宁, 胡磊, 胡玥
【申请人】北京航空航天大学
【公开日】2015年5月20日
【申请日】2015年1月13日