Page 24 - 《中国医疗器械杂志》2026年第2期

Chinese Journal of Medical Instrumentation 2026年 第50卷 第2期

医 疗 机 器 人

T
杆扭转角4个参数确定各个连杆之间的运动学关 式中：[p , p , p ] 代表末端工具坐标系相对于原点
z
y
x
系。如图4所示建立六自由度机械臂连杆坐标系， 的位置向量；[n, o, a]代表末端工具坐标系相对于
确定的DH参数如表1所示。 世界坐标系的旋转矩阵。

[8]
X
a 2 对机械臂进行控制时，通常对逆运动学 更感
Y 兴趣，即已知机械臂的连杆参数，给定末端工具坐
Z
d 1 1 标系的期望位置和姿态，找到相应的关节变量，使
d 4
0 2 4 d 5 末端工具坐标系能够到达期望位姿。
本文所研究的机械臂后3个关节轴交于一点，
3 6
a 3
5 满足Pieper准则，故逆运动学解析解存在。逆运动
学求解思路如下：首先，将关节变量逆向解耦为前
d 6
图4 机械臂3D模型和连杆坐标系分布 3个关节变量和后3个关节变量；然后，利用末端位
Fig.4 3D model of the robotic arm and distribution of link coordinate
frames 置求出前3个关节变量的值，利用末端朝向和已知

表1 机械臂DH参数 关节角求出后3个关节变量的值，即可得到机械臂
Tab.1 DH parameters of the robotic arm
的8组完整解析解，其逆解流程如图5所示。
关节i θ i /rad d i /m a i /m α i /rad
输入末端 求解腕心 求解关节 求解关节 输出可能
1 θ 1 0.152 0 π/2 位姿 T 6 位置 1~3位置 4~6姿态 关节角
0
2 θ 2 0 −0.425 0
图5 机械臂逆解流程
3 θ 3 0 −0.395 0
Fig.5 Inverse solution flowchart of the robotic arm

4 θ 4 0.102 0 π/2
首先计算前3个关节的腕心位置，腕心是后3个
5 θ 5 0.102 0 −π/2
关节轴线的交点，根据正运动学可得腕心位置：
6 θ 6 0.100 0 0
     
注：关节角θ i 、连杆长度a i 、连杆偏距d i 、连杆扭转角α i 。 0  wc x      p x      a x    









          (5)

P wc =  wc y  =  p y  −d 6  a y 

           
根据上文内容建立关节1~6的位姿变换关系 wc z p z a z
0
0 式中： P 为腕心位置；d =0.100 m为关节6偏移
T 。方法是通过矩阵变换相邻关节的DH变换矩阵 wc 6
6
T
链乘： 量；[a , a , a ] 是末端的z轴方向，来自旋转矩阵第
y
z
x
0 0 1 2 3 4 5 3列。
T 6 = T 1 × T 2 × T 3 × T 4 × T 5 × T 6 (1)
由机械臂DH参数可以得到连杆坐标系{i}相对 如图6所示，从基坐标系俯视xy平面，用解耦
于坐标系{i−1}的变换矩阵： 法求解关节1的θ ：
1
θ 1 = arctan2(wc y ,wc x )或θ 1 = arctan2(wc y ,wc x )+π (6)
 
 cosθ i −sinθ i cosα i sinθ i sinα i a i cosθ i 
 
 
 
i−1   sinθ i cosθ i cosα i −cosθ i sinα i a i sinθ i    (2) 此时产生2个候选解左侧构型和右侧构型，需


T i = 
 
 0 sinα i cosα i d i 
  根据实际情况选择。
 
 
0 0 0 1
y 腕心
由变换矩阵定义可知，连杆坐标系{0}相对于
wc y
世界坐标系{B}的变换矩阵和工具坐标系{7}相对于
连杆坐标系{6}的变换矩阵分别为： 关节2
   1 0 0 0 

 0 0 1 −d 0  
   
       
B  0 −1 0 0    6   0 0 −1 −d 7    (3)




T 0 =   ， T 7 =   
 1 0 0 0    0 1 0 0  



   
   
0 0 0 1 0 0 0 1
通过变换矩阵链乘可以得到末端工具坐标系 θ 1
{7}相对于世界坐标系{B}的变换矩阵： O 基座 wc x x
  图6 腕心几何关系俯视图
 n x o x a x p x 
[ ]     Fig.6 Top view of wrist center geometric relationships
n o a p  
B  n y o y a y p y  

T 7 = =     (4)
0 0 0 1    n z o z a z p z    如图7所示，在竖直平面内建立二连杆模型，
 
 
0 0 0 1 求解关节2、3，计算腕心到基座的距离：
138