经颅磁六轴机械臂运动学分析与路径规划

嵇长委，尚会超，韩鑫磊，付晓莉，李志强

(1.中原工学院机械工程学院，河南郑州 450007；2.河南翔宇医疗设备股份有限公司，河南安阳 455000)

0 前言

据古文献记载，在中国很早就有将磁石应用于治疗头痛、关节痛等疾病的例子。近年来，国内外学者对磁场的生物医学效应进行了深入研究。大量数据表明，磁场可以通过经络和穴位的作用改善微循环，促进细胞代谢，加速细胞内废物和有害物质的排泄，平衡内分泌失调，从而实现对某些疾病的辅助治疗[1]。

经颅磁刺激(Transcranial Magnetic Stimulation，TMS)是一种电磁疗法，作为一种无创、非侵入、无辐射的物理靶向治疗，已成为治疗精神和神经系统疾病等多种临床难治性疾病的有效手段。例如，脑卒中是近年来死亡率和致残率非常高的老年人十大疾病之一，严重危及老年人的生命和健康安全，且患病年龄趋于年轻化。根据世界卫生组织的数据，在接受康复治疗的情况下仍会有60%的患者会有不同程度的肢体功能障碍[2]，严重影响患者的日常生活质量。随着经颅磁刺激治疗技术的发展，现阶段脑卒中的救治率获得了显著提高，这大大促进了患者的康复[3]。但是，经颅磁刺激技术在临床应用中存在诸多问题，其中精准度、稳定性、操作人员的误差制约了这项技术的临床推广和进一步发展[4-7]。本文作者旨在设计一款辅助经颅磁治疗的机械臂，通过建模设计、运动学分析工作空间建立、运动路径规划等一系列研究，使经颅磁刺激治疗过程更加智能化，减轻医疗人员的工作强度。

1 机械臂尺寸与建模

人体的头部呈现不规则球体，头颅的尺寸也因人而异。在设计六轴机械臂的尺寸时，本着实用最大化、均衡化的原则，根据GB 2428—1998《成年人头面部尺寸》进行尺寸设计，并以此作为研究目标。经颅磁六轴机械臂主要由基座、旋转关节、连接件、末端执行器构成，相邻的关节与轴两两垂直，整体SolidWorks三维模型如图1所示。

图1 三维模型Fig.1 3D model

1986年KHALIL、KLEINFINGER提出一种建立连杆坐标系的改进D-H参数法[8]，在建立关节坐标系时将坐标系固结于该连杆的近端，而非远端，更符合直观理解，方便产品实现以及工程人员使用。本文作者采用改进D-H参数法建立连杆坐标系，简化后的机械臂数学模型如图2所示：坐标系C0固定在关节1基座，坐标系C1、C2固定在关节2肩部，坐标系C3固定在关节3肘部，坐标系C4、C5固定在关节4、5腕部，目标坐标系C6固定在末端执行器的中心[9]。改进D-H参数表如表1所示。其中d1=0.158 m，d4=0.105 5 m，d5=0.105 5 m，d6=0.080 m，a2=0.268 m，a3=0.256 m。

表1 机械臂D-H参数Tab.1 Mechanical arm D-H parameters

图2 连杆坐标系分布Fig.2 Distribution of connecting rod coordinate system

2 正运动学求解

机械臂是由一系列运动关节连接的运动链。机械臂的运动学分析分为正运动学和逆运动学。正运动学可以简单地理解为给定一组关节角度，正运动学的解总是唯一的。相反，在逆运动学求解的过程中，将会出现没有解、唯一解或多解的情况。因此，国内外学者在求逆解的过程中提出了不同的方法。

根据图1、图2建立的D-H连杆坐标系和表1机械臂D-H参数，得到关节6末端执行器相对于基坐标系的总位姿变换矩阵，即式(1):

(1)

(2)

计算过程简化后可得经颅磁机械臂末端执行器的空间位置向量为

其中：ci=cosθi，si=sinθi，c23=cos(θ2+θ3)，s23=sin(θ2+θ3)。

3 逆运动学求解

当待治疗点的三维坐标已知时，末端执行器的位置可通过待治疗点的位置坐标获得，末端执行器的方向可由与待治疗中心点垂直的法线得到，最终获得末端执行器的位置和方向，即(n，o，a，p)已知。而求解机械臂逆运动学问题直接转化为计算各关节变量θi。逆运动学求解方法主要有几何法[10-11]、解析法[12]、数值法[13-14]、智能算法[15]等。本文作者在综合对比之后采用逆变换求解各关节变量。由于关节2、3、4旋转轴方向相同，因此可以表示为

(3)

(4)

求解逆解需要选择合适的方程有序地求解，这将会很大程度上降低求解难度。对比式(3)(4)得到式(5)：

(5)

经三角函数、万能公式计算可得各关节变量θi。

其中A1、A2为根据矩阵对比所得：

4 经颅磁机械臂仿真

为了在仿真中验证正、逆运动学方程推导的正确性，根据D-H参数使用MATLAB中的机器人工具箱建立了经颅磁机械臂运动学模型。通过调用相关程序函数建模，设置一组关节角度θ=[0，0，0，0，0]，获得了图3中的机器人示教姿态和图4中的初始姿态模型。

图3 调试位姿Fig.3 Debug pose

经颅磁六轴机械臂的工作空间是指六轴机械臂的末端执行器所能达到的最大空间区域，它是衡量机械臂性能的重要指标[16]。根据每个连杆的尺寸和末端执行器的位置矢量，在分析软件MATLAB中使用蒙特卡罗方法求解机械臂的工作空间，仿真结果如图5、6所示。

图5 左视机械臂工作空间Fig.5 Left view robotic arm workspace

图6 上视机械臂工作空间Fig.6 Upview robotic arm workspace

据图5—6所得，经颅磁机械臂的工作空间可近似为直径1 000 mm的球体。对比人体男女生头围以及高度，该机械臂的工作空间能够覆盖头颅上治疗目标范围，满足实际治疗的工作空间要求。

5 经颅磁机械臂的路径规划

5.1 经颅磁机械臂的工作过程

首先，根据治疗师的治疗策略，对人体头部主要治疗穴位进行颜色标记，通过设置的单目摄像头在头颅上方、左方、右方、后方对标记点进行图像数据采集，获得治疗穴位的三维坐标信息。

其次，将收集的治疗穴位标记点按照科学治疗方法进行排序，求解机械臂工作范围内每个治疗穴位对应的机械臂逆解，根据穴位的治疗角度和机械臂的逆解，在机械臂的工作空间范围内设置预治疗点或最终治疗点。

最终，控制器控制机械臂末端执行器从初始位置移动到首个治疗穴位点，待完成一段有效时间治疗后，控制机械臂移动到下一个治疗穴位点进行治疗。以此类推，直到最后一个标记穴位治疗完成后，机械臂回到初始位置，等待下一个患者进行治疗。

通过对以上工作过程进行分析，在整个治疗过程中经颅磁机械臂的路径规划涉及两个阶段：阶段一是对穴位顺序进行规划，阶段二是对人体头颅整个治疗路径进行轨迹规划[17-18]。

5.2 穴位顺序的规划

根据国家经穴部位标准可知，头部的穴位非常多，经颅磁治疗中最常用的穴位包括头顶中间部位的百会、风府、哑门、神庭等主穴，以及四周的攒竹、天柱、四神聪等腧穴(辅穴)，涉及的治疗范围也比较大，这就需要经颅磁机械臂在工作过程中对治疗的穴位进行优先顺序的路径规划。

经颅磁机械臂在治疗穴位的优先顺序上秉承中医治疗策略中的先针刺患侧穴或健侧穴、后针刺病侧穴的所谓“补健侧、泻患侧”原则。基于此治疗原则，将得到一系列的治疗穴位点Qi(i=1，2，…，n)。 其中，在这些治疗穴位点的Qi集合中，将主穴Ri(i=1，2，…，n)与辅穴Gi(i=1，2，…，n)进行区分。在治疗路径划分中，一条路径会由一个主穴或一个主穴与多个辅穴组成，如存在多个辅穴将对辅穴进行排序。排序方法如下：通过单目摄像头扫描治疗穴位点Ri、Gi(i=1，2，…，n)获取其三维空间位置坐标(Xi，Yi，Zi)(i=1，2，…，n)。因为穴位点的位置常常分布于头颅顶部或者头颅后脑区域，所以需要将人体头颅由上到下、由外到里(以放置机械臂方位为里)进行划分，如图7所示。设穴位点中的主穴在平面中的坐标为(x0，y0)，辅穴的坐标(xi，yi)，分别计算需要治疗的辅穴与主穴两穴位之间在投影平面上的距离di(i=1，2，…，n)由公式(6)计算得出：

图7 由上到下(a)、由外向里(b)划分Fig.7 Divided from top to bottom (a)and from outside to inside (b)

(6)

最后按照辅穴与主穴距离由小到大原则对辅穴进行排序，输出新的治疗点集Pi(i=1，2，…，n)， 即求得头颅治疗穴位的治疗点集。

5.3 路径轨迹规划

获取一条路径中所治疗穴位的点集之后，下面对所经过点进行路径规化。机械臂的路径规划有多方法，基于搜索的路径规划可分为Dijkstra、A*等，基于采样的路径规划可分为RRT、RRT*、Informd RRT*等，基于智能算法的路径规划可分为遗传算法、蚁群算法等。

无碰撞路径规划是治疗过程中需要解决的问题之一。在整个操作过程中，人体头部呈现不规则球体，目标穴位点位于人体头部表面，人的头部将成为障碍物。为了将目标穴位快速扩展到机械臂的方向，需要对机械臂进行碰撞检测，以便于快速搜索出到达目标穴位点的最佳扩展路径。经颅磁机械臂的碰撞检测方法是将头颅规则化，并将头颅包括在长方体或圆柱体中。类似地，机械臂的连杆也简化为圆柱体[19-20]。为了方便计算，机械臂等效为一条线段。此后包络后的头颅进行膨胀展开。判定头颅与机械臂碰撞的情况是当简化机械臂的线段与简化头颅圆柱题的面相交时，认为发生了碰撞。

经过研究对比，本文作者采用了改进RRT算法中的RRT-connect算法，该算法具有收敛速度块、时间短、成本低、接近最优、常适用高维空间等优点。算法的伪代码如下：

T1←qstart；T2←qgoal；

Whilei

qrand←Sanple()；

qnearest←Nearest(T1，qrand)；

qnew←Steer(qnearest，qrand，ε)；

if Obstacle Free(qnew)then

if Obstacle Free(qnearest，qnew)then

T1←qnew；

else break；

return Path (T1，T2)；

else Swap(T1，T2)；

其中：Sample()表示通过随机采样获得采样点qrand；Steer(qnearest，qrand，ε)是指在连接采样点和最近节点的线上选择新点qnew，使得新点与最近节点之间的距离等于搜索步长ε；Nearest(T，qrand)是指搜索随机树中最接近采样点的节点qnearest；Obstacle Free(qnew)指生成的新点不与障碍物碰撞；Obstacle Free (qnearest，qnew)是指新点和最近节点之间的线不与障碍物碰撞；Path(T1，T2)指随机树T1和随机树T2相遇以获得无冲突路径；Swap(T1，T2)表示切换随机树T1和随机树T2。

6 试验与结果

为了验证经颅磁性六轴机械臂的可行性，利用MATLAB建立了经颅磁六轴机械臂的半仿真平台，对经颅磁六轴机械臂的路径规划进行仿真测试。测试在主频率为1.8 GHz、内存为8 GB的计算机上进行。

首先，所搭建仿真平台如图8所示，在人体头颅模型上，用红色进行穴位模拟标记，单目摄像头可在头颅上方、左方、右方、后方对标记点进行图像数据采集，图8所示摄像头位置在平台右后方，经颅磁机械臂采用D-H参数相同的MATLAB Robotics Toolbox机器人工具箱代替。

图8 仿真平台搭建Fig.8 Simulation platform

其次，因为不同患者、不同穴位的治疗时间存在差异，所以治疗穴位时间根据医师治疗策略进行设置。当机械臂从初始位置移动到第一个治疗主穴时，记录为第一段移动轨迹；当治疗完主穴后，将对此治疗路径中的辅穴进行依次治疗，记录主穴到第一个辅穴的治疗路径为第二段移动轨迹。因为经颅磁机械臂治疗辅穴过程中需要设置一定的高度，避免对人体组织器官造成损伤，记录贴近第一个辅穴治疗下潜的过程为第三段轨迹。以此类推。

最后，将3段路径进行拟合优化，在编程中采用了多项式插值函数对3段路径轨迹进行处理，得出完整的经颅磁治疗路径，如图9所示。并且输出3段路径轨迹的速度、角速度、角加速度随时间变化的信息，如图10—12所示。仿真试验结果表明：所设计的六轴机械臂能够依次完成从初始点-主穴-辅穴-初始点的轨迹规划，满足经颅磁治疗的基本工作要求。

图9 运行轨迹Fig.9 Running trajectory

图10 第一轨迹信息图Fig.10 The first trajectory information map：(a)joint angle； (b)angular velocity；(c)angular acceleration

图11 第二轨迹信息图Fig.11 The second trajectory information map：(a)joint angle； (b)angular velocity；(c)angular acceleration

图12 第三轨迹信息图Fig.12 The third trajectory information map：(a)joint angle； (b)angular velocity；(c)angular acceleration

7 结论

(1)根据中国成年人头型系列尺寸设计一款适用于经颅磁治疗仪的六轴机械臂，可以代替人工手持工作。机械臂整体由SolidWorks三维软件建模，该机械臂的末端可以夹取不同形状的磁疗、电疗器具，具有精确、高效、灵活、稳定、通用性高的特点。

(2)经颅磁机械臂采用改进D-H参数法，结合MATLAB软件中机器人工具箱，建立机械臂的运动学模型，推导机械臂正、逆运动学方程并求解，得到机械臂末端执行器与各大关节变量间的逻辑关系。在MATLAB仿真中验证了逻辑的正确性，采用蒙特卡罗方法验证机械臂的性能，仿真结果表明：该机械臂能满足实际治疗的工作空间要求。

(3)提出的方法解决了一条路径中多目标治疗穴位路径规划问题，应用双向RRT算法实现了无碰撞路径轨迹。编程中采用了多项式插值函数对轨迹进行处理，得出完整的路径轨迹。搭建半仿真试验平台，结果表明：所设计的六轴机械臂能够依次完成从初始点-主穴-辅穴-初始点的轨迹规划，基本满足经颅磁治疗的工作要求。