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

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

医 疗 机 器 人

√
2
L = r +h 2 (7) 至此求得逆解，但对于六自由度机械臂理论上
最多有8组解 。由关节范围可知，θ 和θ 不可能等
[9]

z 腕心 2 4
r 于π，则奇异位形有以下3种情况：当θ = 0时，只
4
能求出关节变量θ 与θ 的和； 当θ = 0时，只能求
L 3 3 5 2
ϕ
关节2 β 出关节变量θ 与θ 的和；当θ = 0且θ = 0时，只能求
2
1
3
4
出关节变量θ 、θ 与θ 的和。当机械臂处于奇异位
5
3
1
L 2 h 形时，逆运动学存在无数组解。
L
γ 3 基于RGB-D相机的多点穴位识别算法

O 基座 x 在使用机械臂模拟艾灸治疗时，需要明确目标
穴位的三维信息。本文拟采用基于辅助标记点位置
图7 二连杆模型图
[10]
Fig.7 Two-link manipulator model diagram 的视觉定位系统 ，医师在患者需要艾灸的穴位处

贴上黑色X标记点，上位机通过固定在艾灸盒末端
计算辅助角：
的深度相机获取患者艾灸区位图像，继而获取标记
 
2 2 2
 L + L − L 
 2 3 
  点在相机坐标系下的三维信息，再将相机坐标系下
ϕ = arccos     (8)
 
2L 2 L 3
的三维信息转换到机械臂下后通过TCP协议发送给
β = arctan2(h,r) (9) 机械臂，对机械臂进行定位控制。
  视觉系统的主要任务就是获取标记点在相机坐
2 2 2
 L − L + L 
 2 3 
γ = arccos       (10) 标系下的三维坐标，艾灸机器人系统框架如图8所示。
2L 2 L
 

通过计算可得两组解： 上位机系统
构型一，肘部向上： 指令输入 视觉系统 双向通信 下位机
系统
θ 2 = β+γ,θ 3 = π−ϕ (11) 图像获取
用户控制
构型二，肘部向下： 界面
标记识别
θ 2 = β−γ,θ 3 = ϕ−π (12) 运动控制 FR5机
√ 数据显示 计算处理 械臂
2
2
参数定义：腕心水平投影距离 r= wc x +wc y ；
输出定位
垂 直 高 度 差 h=wc −d =wc −0.152； 连 杆 长 度 L =
1
z
2
z
0.425 m，L =0.395 m。 图8 艾灸机器人系统框架
3
现已求得θ 、θ 、θ ，此时对球形腕关节进行 Fig.8 The system framework of the moxibustion robot
1 2 3
姿态解耦，计算前3个关节的旋转矩阵 R ： 采集到患者背部图像后，基于OpenCV的工业
3
0
( )
3 0 T 0 视觉检测系统实现了对X形标记的实时识别与定位。
R 0 = R 3 × R 6 (13)
首先通过高斯模糊和二值化进行预处理。然后，通
ZYZ欧拉角分，对应关节4、5、6：
过算法进行轮廓分析，结合几何特征验证实现标记
 
 r 11 r 12 r 13 
 
3       的精确识别。最后输出X形标记点中心像素坐标。
R 0 =  r 21 r 22 r 23   (14)

 
  通过安装Intel RealSense SDK可以获得深度相
r 31 r 32 r 33
求解公式： 机点云空间分布深度信息Z 和内参矩阵K，基于针
c
[11]
 孔相机模型的透视投影方程 为：
θ 4 = arctan2(r 23 /sinθ 5 ,r 13 /sinθ 5 )


 √    

 2 2
θ 5 = arctan2( r +r ,r 33 ) ,(sinθ 5 > 0) (15)  u    X C  


 31 32   1  
        
  v  = (17)
 K  Y C  
 θ 6 = arctan2(r 32 /sinθ 5 ,r 31 /sinθ 5 )       
  Z c  
1 Z C
或
对其进行逆变换得到：

θ 4 = arctan2(−r 23 /sinθ 5 ,−r 13 /sinθ 5 )    


 √  X C   u 

 2 2        
θ 5 = arctan2(− r +r ,r 33 ) ,(sinθ 5 < 0) (16) P C =  Y C  = Z c K −1     (18)




 31 32   v 
       
    

 θ 6 = arctan2(−r 32 /sinθ 5 ,r 31 /sinθ 5 ) Z C 1
139