基于六自由度机械臂的艾灸机器人手法控制算法

白善明, 崔娟*, 张志东, 张天生, 郑永秋, 薛晨阳

(1.中北大学仪器科学与动态测试教育部重点实验室, 太原 030051; 2.山西省针灸医院, 太原 030000)

艾灸是中国传统医学的重要组成部分,在中华民族的防病治病中发挥了重大作用,广泛应用于疾病治疗、康复保健、延缓衰老等领域。其具有效果明显、简便易行、经济实用等优点,在越来越多的国家得到推广。但在实际艾灸治疗中,由于施灸时间过长、医生疲惫不堪,既造成施灸不准,影响艾灸手法的重复性,也对医者手臂手腕形成损伤。以采用机械臂设计的艾灸机器人代替医生完成长时间的艾灸治疗工作,可减轻医生负担,降低医学消耗。

近年来医疗辅助机器人快速发展[1],且具有极高的针对性,包括穿戴式下肢骨骼机器人[2]、中医脉诊机器人[3]、辅助移位机器人[4]、针灸及艾灸机器人等。其中,艾灸机器人的发展由各类艾灸器具、艾灸盒的形式,逐步演化为智能化交互机器人和多姿态艾灸机械臂。唐菊丽等[5]提出了一种新型艾灸盒,基于巧妙的结构设计和电子技术,实现了艾灸设备的温控、除烟等功能;戴耀南等[6]通过对艾灸机理和机器人运动学的研究,研制了人体脊椎恒温艾灸机器人;钟义[7]引入传感与控制系统设计,提升了艾灸机器人的人机交互体验;乐文辉等[8]基于D-H参数法建立艾灸机器人模型,通过MATLAB仿真,验证了艾灸机器人良好的运动学性能;刁吉瑞等[9]集成2R1T并联机构,设计了五自由度的混联艾灸辅助机器人,可实现艾灸辅疗中的姿态转换。然而,人体穴位角度多样,绝大多数艾灸手法都需要以复杂的角度和轨迹来实现。基于上述研究发现,当前的艾灸机器人所用的机械臂自由度较低,虽然初步实现了艾灸辅疗过程中的姿态控制,但仍无法模拟医生复杂的手臂动作和艾灸手法,特别是无法对于人体侧方的穴位实施主动艾灸;除此之外,当前艾灸机器人智能化程度低,在治疗过程中控制系统无法根据实时的温度监测信息对艾灸轨迹进行重新规划。

因此,现基于六自由度艾灸机械臂,通过机器人操作系统(robot operating system, ROS)[10-11],致力于研制一种可模拟各类复杂艾灸手法的辅助型艾灸机器人。该机器人使用MoveIt平台对机械臂运动学控制进行快速配置[12],可以实现机械臂末端在任意坐标点的姿态变换。通过引入笛卡尔轨迹规划和多项式插值方法[13],建立不同艾灸手法所需的末端轨迹曲线求解算法,希望机械臂在任意姿态下快速完成轨迹规划,以及对各类艾灸手法的精准模拟。同时,在机械臂末端引入温度监测及反馈系统,达到实时反馈患者表皮温度变化的效果,并由此调节机械臂轨迹运行的高度、方向及速度等参数,有望真实模拟医生在艾灸治疗过程中根据温度变化做出的手法起伏,提高艾灸机器人操作过程的安全性、智能性和友好性。

1 机器人控制系统设计

智能艾灸机器人的核心为六自由度机械臂,如图1(a) 所示,其主要完成的工作是灵活调整关节、快速响应移动、长时间稳定的保持某种姿态,进而实现对人体手臂结构的真实模拟。同时,为了便于机械手臂在运行过程中对艾柱的固定,本文研究设计了艾柱夹具并将其安装在机械臂末端,如图1(b)所示,艾条竖直固定在夹具的中心位置,将各个非接触式传感器固定在夹具边缘位置,如图1(c)所示;除此之外,夹具本身的锥形设计还可以结合风机实现对艾烟的高效吸收。特别的,在实际模拟回旋灸手法的过程中,由于一些穴位处所需的施灸半径较小,这种情况下机械臂的整体运动显得过于笨重,无法高效便捷的实施艾灸,因此本文研究基于偏心轮设计了一款偏心旋转夹具,如图1(d)所示,可将不同规格的艾柱固定在夹具边缘处,由机械臂末端电机带动其沿某一方向转动,从而达到持艾柱在穴位点处走过特定圆形轨迹的效果。

图1 机械臂结构设计Fig.1 Structural design of mechanical arm

机器人模拟人体手臂进行艾灸治疗,需要机械臂具有更高的稳定性和灵活度,因此在艾灸机器人的设计过程中(图2),选用MBDHT2510伺服电机驱动器和MSMD042G1U伺服电机作为机械臂关节的执行机构。机械臂模拟艾灸手法时,往往需要其在低速状态下保持平稳运行,而大多数电机由于工作原理的限制,极易出现低频振动现象,这对于机器人的正常运转非常不利;相比之下,伺服电机具有共振抑制功能,根据系统内部的频率解析机能,可检测出机械结构的共振点,便于系统做出调整,运作十分平稳,即便是在低速时也不会出现振动现象。同时,伺服电机具有内置编码器,可以实时测量电机磁极位置、伺服电机转角及其转速,形成闭环控制系统,使机械臂具有更高的精度和稳定性。

图2 机械臂控制系统架构图Fig.2 Structure diagram of manipulator control system

在艾灸治疗过程中,为防止高温燃烧的艾柱烫伤人体,设计温度监测及反馈系统,需要在机械臂末端配置红外阵列传感器,实时监测施灸穴位点的皮肤温度;本文选用了型号为MXL90614的红外传感器,将传感芯片测得的温度信息转化为相对应的电压模拟量传输至机械臂的上位机系统,并据此作出反馈。

将STM32芯片作为机械臂控制系统的主控芯片,通过控制下发至驱动器的模拟量和脉冲频率,达到控制机械臂关节电机转速及其转角的效果,主要内容是通过脉宽调制信号(pulse width modulation, PWM)实现电机转动控制,输出高低电平实现电机转向控制,由驱动器内部提供的双闭环系统,实现电机方向和速度的突变。由于STM32所能提供的最大控制电压为3.3 V,不足以满足驱动器控制引脚的需要,因此加入一个光耦电路进行升压。选用伺服控制模式为速度控制,将电机的位置信号或直接负载的位置信息反馈给上层的控制系统,从而增加整个系统的定位精度。

2 机械臂轨迹规划算法

ROS中的MoveIt平台包含了大量针对机械臂的功能包[14],据此可以快速完成对机械臂运动学解算、碰撞检测、位置插补等一系列工作,因此在对艾灸手法轨迹进行规划前,需要完成对机械臂的基本建模和配置: 创建机械臂的URDF模型,其中包含了机械臂的整体结构,尺寸大小以及运动学参数等关键信息;据此配置其自碰撞矩阵,MoveIt会随机选取采样点生成碰撞参数,检测到永远不会产生碰撞的关节杆件;之后便可以为机械臂配置运动学求解器,运动学算法是机械臂各类算法的核心,它包含了正运动学求解和逆运动学求解等算法,MoveIt中默认采用运动学-动力学库(kinematics-dynamics library, KDL)完成对机械臂的运动学解算;最后定义机器人位姿,生成机械臂配置文件,并其中添加控制器插件,实现MoveIt与下层控制和驱动之间的通信。

2.1 基于艾灸手法的轨迹规划算法

在使用机械臂模拟实施艾灸治疗时,需要由医生通过拖动示教模式获取多个穴位点在笛卡尔空间中的坐标位置,之后再由机械臂的轨迹规划算法解算出基于当前坐标所展开的艾灸手法的运行轨迹,即计算出艾灸手法的起始点和终止点,通过定义中间点,对艾灸手法的轨迹曲线进行拟合,得到平滑稳定的运行轨迹,模拟真实艾灸动作。为了解决这一问题,依据正运动学算法,得出艾灸手法轨迹关键点坐标的求解方法。

模拟艾灸手法的核心是找到艾条的燃烧点相对于穴位点的运行轨迹,采用WTGAHRS3惯性导航模块,将其固定在艾条末端,并由医生实施专业的艾灸动作,进而捕获到相对于穴位点坐标系的艾灸手法运行轨迹,将完整的轨迹曲线根据速度变化情况分割成若干段不同的圆弧曲线,这些曲线段之间的连接点即为艾灸轨迹的关键点,这样取点也便于机械臂在模拟艾灸过程中对其速度进行精确控制,使其对艾灸手法的模拟更加真实。已知机械臂末端与穴位点之间的位置关系,即可求解出轨迹关键点在机械臂夹具末端中心坐标系(tool center point, TCP)中的表示方法。机械臂坐标系表示方法如图3所示。

由于人体穴位并非全部垂直向上,因此大多数艾灸手法都是以复杂姿态和轨迹呈现的,需要机械臂在当前TCP末端所处的平面上平缓走过特定的圆弧轨迹。笛卡尔轨迹规划算法至少需要3点才能确定一个三维空间内的圆弧,故各种艾灸手法轨迹的关键点通常均选取为3个。只需预先解算出机械臂末端所在的平面,以及此平面上圆弧轨迹中3个关键点在基底(Base)坐标系下的表示方法,机械臂便可通过逆运动学方法到达这些位置。

机械臂是由多关节组成的机械结构,每个关节的位置和姿态发生变化都会对其他关节产生影响,在各个关节分别建立坐标系,则关节在空间中的姿态变化可等效为各坐标系的相对变换,即以平移变换、旋转变换及其组合变换来表述。

图3 机械臂坐标系Fig.3 Coordinates of the manipulator

图4中,设坐标系B是由坐标系A经过平移、旋转后得到的,点P、Q、T是坐标系B的X-Y平面上圆弧轨迹中的3个点,且点P、Q、T相对于坐标系B的坐标是不变的;若把坐标系A视作机械臂的Base坐标系,把坐标系B视作机械臂末端的TCP坐标

图4 轨迹关键点Fig.4 Key points of trajectory

系,那么只需要求解出点P、Q、T在坐标系A下的坐标表示,即可得到机械臂随机运动后,TCP末端所处平面的圆形轨迹上的3个点在Base坐标系下的表示方法。以点P为例,设P在坐标系B下的表示为(x,y,z),则只经过坐标系平移变换后P点在坐标系A下的表示为

AP=APB+BP

(1)

(2)

式中:AP为P点在坐标系A下的坐标;APB为坐标系B相对于坐标系A的坐标,即为坐标系B对于坐标系A的平移变换。

只经过坐标系旋转变换P在坐标系A下的表示为

(3)

(4)

由式(4)可知,坐标系的旋转变换可由旋转矩阵表示,即B坐标轴上的单位向量在A坐标轴单位向量上的投影。经过坐标系平移、旋转变换整合后P在坐标系A下的表示为

(5)

(6)

(7)

由于机械臂是多连杆坐标系组合的结构,所以点P经过连续变换后在坐标系A下的表示方法为

(8)

同理,可以求得Q和T点经过随机变换后在坐标系A下的表示方法。

图5 轨迹关键点的求解方法Fig.5 The solution method of trajectory key point

至此可以得到TCP末端所在平面上圆弧轨迹中多个关键点的坐标位置,即艾灸手法所需要的起始点、终点或中间点。系统在这些点之间形成的轨迹是随机曲线,不仅无法完成对艾灸手法的模拟,甚至可能触碰并烫伤患者。采用笛卡尔空间轨迹规划对中间路径的拟合,可生成特定的直线、圆弧及其组合曲线; 运用坐标系变换的方法,计算出轨迹运行的方向及旋转角度,得到各插补点在坐标系中的值,从而真实模拟当前穴位下艾灸治疗手法的运动轨迹(图5)。

由ROS中的MoveIt系统完成机械臂路径规划后,MoveIt会根据控制指令,发布一个名为joint_path_command的话题消息,其中包含了运动学求解器插件对机械臂路径规划后的所有位点信息,展示了机械臂将会以何种姿态和轨迹运动,即笛卡尔轨迹规划后产生的所有插补点。然而MoveIt解算输出的轨迹规划数据仅有对于机械臂各个关节角度的插值数据,没有对于其角速度和角加速度的插值求解,这无疑增大了机械臂末端的跟踪误差,导致机械臂在运行过程中产生较强的抖动。为了解决这一问题,在MoveIt输出角度插值的基础上,加入一个数据处理环,通过五次多项式插值算法对各个角度的插值进行细分,从而得到连续的角速度和角加速度插值,即把机器人的关节变量变换成关于时间的函数,然后对角速度角加速度进行约束,由此便确保了机械臂在运行过程中的角速度是连续可导的,增强了机械臂在运行过程中的平稳性。图6中展示了3种艾灸手法的仿真结果,图7为机械臂轨迹规划算法的逻辑框图。

2.2 温度检测及反馈系统

在机械臂模拟人体手臂实施艾灸治疗的过程中,燃烧的艾柱离人体表皮的距离非常近,极易发生烫伤,对患者造成损失。本文研究引入温度监测及反馈系统,通过机械臂末端的温度传感器,对艾灸部位的皮肤温度进行实时监测,并将反馈数据上传到控制系统进行运算,求解出温度变化的范围和速度,据此对艾灸机器人的手法轨迹进行重新规划,改变其运行的方向、速度和位置高度等参数,获得并运行全新的艾灸手法轨迹,从而确保艾灸过程中,患者穴位点的艾灸温度可以一直保持在最佳范围内,同时也真实模拟了医生在艾灸治疗过程中手法上的起伏效果。当温度发生骤变,突然接近或高于人体耐受温度的阈值,反馈信号便会回传给控制系统,其控制机械臂快速抬升并中断运行程序,有效防止燃烧的艾柱烫伤人体,提高艾灸机器人的安全性。

图6 基于Moveit完成机械臂轨迹规划Fig.6 Trajectory planning of the manipulator based on Moveit

图7 机械臂轨迹规划算法逻辑图Fig.7 Logic diagram of trajectory planning algorithm for manipulator

3 系统验证

为验证艾灸机器人手法控制算法的可行性和精确度,设计了上位机系统和图形化操作界面,选取10位志愿者进行100次测试,对艾灸机器人进行各项测试,结果均符合要求,测试方法及测试结果如下。

3.1 测试方法

在使用其图形化界面操作艾灸机器人进行系统测试的过程中,首先建立上位机系统与下层控制器之间的连接;之后使用机器人的拖动示教模式,通过手动拖拽机械臂的方法,对多个穴位点的坐标进行捕捉,系统会自动根据这些穴位的坐标位置进行解算,得到真实模拟艾灸手法的运动轨迹;在图形化界面中为不同患者制定个性化的诊疗方案,包括选取不同位置和角度下的穴位点,设定艾灸手法、施灸时间、温度阈值等。记录艾灸机器人正常工作的次数,以及是否能够按照预设的时间进行艾灸;特别的,对于温度监测及其反馈系统,人为改变穴位点的皮肤温度,记录机器人正常反应的次数;除此之外,为了验证机械臂能否有效的模拟艾灸手法,控制器会以每秒两次的频率向上位机系统发送其运行状态和机械臂末端的空间坐标。根据这些信息对机械臂在运行过程中的轨迹曲线进行拟合,并与医生艾灸过程中的真实的轨迹曲线进行对比,从而判断艾灸手法控制系统的准确性。

3.2 测试结果

通过对10位不同志愿者进行的测试,得到表1中的测试结果,其中艾灸机器人的工作状态出现了1次失败,原因是中途停止并变更诊疗方案后,再次运行系统程序,先前设定机械臂回归原点的指令重复执行,对程序进行修改之后再未出现过类似现象。通过表格中的测试结果可知系统正确执行率较高,说明艾灸手法控制系统可正常运行,验证了其可行性。

表1 测试结果Table 1 Test results

通过艾灸手法控制算法,本文研究实现了控制机械臂顺利完成对各类复杂艾灸手法的真实模拟,包括摆尾灸、回旋灸和雀啄灸等。在艾灸机器人的运行过程中,捕获在不同时刻下所对应的机械臂的运行姿态,结果如图8所示。

图8 机械臂运行测试Fig.8 Operation test of mechanical arm

使用系统回传的状态参数,对机械臂的运行曲线进行拟合,与先前捕捉到的医生实际艾灸过程中的轨迹曲线进行比对。真实艾灸过程中各类艾灸手法都是基于特定平面实施的,为了简化计算,可将三维空间内的艾灸轨迹曲线转化为二维平面内的轨迹曲线进行对比,结果图9所示。

图9 艾灸手法轨迹对比Fig.9 Comparison of moxibustion technique trajectory

对真实艾灸轨迹进行采集可得到大量的离散数据,运用最小二乘法可将真实曲线进行拟合z并求解出其对应的多项式表达式,在机械臂的运行轨迹中随机选取100个点,将这些点代入表达式中得到的解,与机械臂运行过程中采样点的实际值做差,即为误差。通过计算可得,各类艾灸手法中,每个点的误差累加后得到的误差和均不大于1 mm,说明机械臂的运行轨迹和真实的艾灸轨迹高度一致,其艾灸手法控制算法具有较高的精确度。

此外,使用偏心旋转夹具在小范围内实施回旋灸时,艾柱末端可以基于穴位点平稳、准确地走过标准的圆形轨迹,与机械臂整体控制模拟回旋灸不同,这种特制的夹具优势更为显著,所能完成的圆形轨迹半径更小、曲率更标准且其运行状态更平稳,耗能更小。

4 结论

基于六自由度机械臂,提出了一种模拟艾灸手法的控制算法设计与实现。根据人体穴位点坐标,建立人体坐标与机械臂坐标的变换关系,捕获轨迹关键点,通过运动学求解方法和插值数据处理环,实现机械臂在全局坐标系的姿态变化,从而针对不同穴位艾灸需求,执行不同的轨迹曲线,实现对于多种艾灸手法的真实模拟;引入温度监测与反馈系统,提升了艾灸机器人的安全性、智能性及友好性。最后通过试验,验证了机械臂在模拟艾灸治疗过程中的可行性、精确度以及安全性。与现有的艾灸机器人相比,具有更高的自由度、稳定性和智能性,对艾灸手法的模拟更加真实,可实现更为复杂的治疗方案,有望解放艾灸医生双手,应用于医生辅助艾灸治疗。同时,该研究具有友好的人机交互及智能化设计,因此在家庭或个人的自助式艾灸理疗市场也具有广阔前景。