重离子放疗辅助医用机械臂避撞路径规划研究
引言
随着医疗技术和工业机器人技术的不断发展，医用机械臂应用技术已成为医科技术的重要研究方向[1-3]。医用机械臂在放疗过程中将病患运送至治疗位置，其高精度、高可靠性的特点使相关应用技术得到国内外专家的重视[4-7]。在放射治疗环境下，辅助治疗设备的存在使得摆位难度增加，因此医用机械臂路径规划的研究变得极为重要。目前，针对医用机械臂的典型路径规划方法包括自由空间法和人工势场法两种[8-10]。该文针对医用机械臂的特殊结构，将医用机械臂和障碍模型进行简化；将障碍域投影到关节空间，得到安全摆位空间；通过A*算法在安全摆位空间内搜索摆位路径；最后，通过仿真验证该方法的安全可行性。
1 碰撞模型
1.1 医用机械臂模型
该文研究的重离子放疗辅助医用机械臂为4自由度机械臂，由于其结构不规则，因此其模型采用圆柱模型，将各不规则突出部分包含在各简化的圆柱包络模型内。医用机械臂实物图，见图1a；其D-H坐标系，见图1b。图1b中各坐标系为关节子坐标系，其中杆1为关节1与关节2轴线间实体部分简化模型，直径为0.38 m，杆2为关节2与关节3轴线间实体部分简化模型，直径为0.56 m，杆3为关节3与关节4轴线间实体部分简化模型，直径为0.85 m，杆4为末端碳纤维板简化后模型，直径为0.48 m。
图1 医用机械臂实物及D-H参数坐标系图
注：a. 医用机械臂实物图；b. 医用机械臂D-H坐标系。
医用机械臂D-H参数，见表1。表1中qi为关节变量，
为关节角，di为关节平移量，ai为杆长，
为扭角。关节1为旋转关节，关节变量为
，关节平移量为0，杆长a1=0.96 m，扭角为0；关节2为平移关节，关节变量为d2，关节角和杆长为0，扭角为-90度；关节3为平移关节，关节变量为d3，关节角为-90度，杆长a3=0.15 m，扭角为90度；关节4为平移关节，关节变量为d4，关节角、扭角均为0度。
表1 医用机械臂D-H参数表
images/BZ_69_236_770_1195_1007.png
由医用机械臂D-H参数可写出相邻杆件的位姿矩阵，将相邻杆件的位姿矩阵依次相乘，则有：
由位姿矩阵通式可得该医用机械臂的运动学方程：
其中，式中
1.2 障碍模型
重离子放疗辅助医用机械臂的工作环境为重离子放射治疗室，治疗室内的障碍主要有摆位验证检测机、治疗头、水平束流遮挡装置。由于各障碍物几何形状不规则，文中采用各障碍物的圆柱体包络来近似建立障碍模型，这种建模方法使得障碍域范围有所扩大，但使得障碍域形状大为简化，医用机械臂路径规划的效率得到提高，同时由于障碍域有所扩大，间接提高了医用机械臂路径规划的安全性[11-13]。
摆位验证障碍检测机障碍域采用圆柱包络描述为S1(P1,r1,h1)，其在水平面的投影形成的平面区域S1’(P1,r1)，如图2a所示。其中P1为圆柱体障碍域在基坐标系xoy平面投影圆心的坐标，r1为障碍域的半径，h1为障碍域的高度，由于图中为投影平面区域，无法提现高度，因此图中无标注。r1=1.02 m，h1=1.76 m。
治疗头障碍域采用圆柱包络描述为S2(P2,r2,h2)，由于治疗头紧靠墙壁，所以S2”(P2,r2)为有效障碍域在水平面内的投影，其在水平面投影形成的平面区域S2’(P2,r2)及有效障碍域S2”(P2,r2)，如图2b所示。其中P2为圆柱障碍域在基坐标系xoy平面投影圆心的坐标，r2为障碍域半径，h2为障碍域高度，由于图中为投影平面区域，无法提现高度，因此图中无标注。r2=1.46 m，h2=2.20 m。
水平束流遮挡装置障碍域采用圆柱包络描述为S3(P3,r3,h3)，其在基坐标系水平面的投影形成的平面区域S3’(P3,r3)，如图2c所示。其中r3为障碍域半径，h3为障碍域高度，由于图中为投影平面区域，无法提现高度，因此图中无标注。r3=0.35 m，h3=1.52 m。
图2 障碍域在水平面的投影
注：a. 摆位验证检测机障碍域在水平面的投影；b. 治疗头障碍域在水平面的投影；c. 水平束流遮挡装置障碍域在水平面的投影。
2 笛卡尔空间障碍向构型空间的转化
2.1 总体思路
在医用机械臂摆位过程中，多数情况下摆位验证检测机处于静止状态，摆位完成后，其进入工作状态，同时为了便于阐述该文提出的避障规划算法，以下均针对医用机械臂摆位过程中，摆位验证检测机处于静止的情况进行讨论。在摆位过程中，其相对于医用机械臂存在运动的情况下，医用机械臂的安全摆位空间通过多次求解即可得到，具体求解方法与两者相对静止情况相同。本节着重说明求解医用机械臂安全摆位空间的方法。
由于各障碍物形成的障碍域为圆柱形包络，因此除升降关节的运动外，其余关节的运动都有可能导致医用机械臂与障碍物之间发生碰撞。为得到医用机械臂各部分与障碍物碰撞的条件，采用分级讨论的方法求解。
总体思路为：① 求解旋转关节1的运动可能导致医用机械臂初始位姿状态与障碍物发生碰撞的运动范围，后续各移动关节的运动导致碰撞发生的情况均在该范围基础上分析讨论；② 由于各障碍域为圆柱包络模型，所以升降平移关节2的单独运动不会导致碰撞的发生，在第一步得到范围基础上推导关节3运动与障碍发生碰撞的条件；③ 在关节3运动与障碍物不发生碰撞的移动范围内，分析关节4的运动导致与障碍发生碰撞的条件。
2.2 医用机械臂各部分简化为杆件后与障碍碰撞条件分析
重离子放疗辅助医用机械臂简化后的杆件模型中，由于杆3的长度0.15 m小于杆3圆柱模型半径0.425 m，因此杆3与障碍发生碰撞的情况已包含在其关节变量与障碍发生碰撞的讨论过程中。以下针对杆1、杆3、杆4与障碍发生碰撞的条件进行讨论。
2.2.1 导致碰撞发生的关节1运动范围
为了分析医用机械臂与障碍发生碰撞的条件，首先求解由关节1的运动可能导致碰撞发生的运动范围。将医用机械臂简化后的杆件模型向xoy平面投影，则关节1运动导致碰撞发生的4个极限位置如图3所示。
图3 旋转关节1运动极限位置图
由图3可求出障碍域1中心在平面投影与坐标原点的连线和x轴夹角：
求解医用机械臂运动至障碍域1附近的两个极限位置与障碍域1中心投影与坐标原点连线的夹角
：
式中，d11为障碍域1中心在xoy平面的投影到坐标原点的距离，d12和d13分别为障碍域中心投影到医用机械臂两极限位置中轴线的距离。
令
，则在医用机械臂摆位摆位运动过程中与障碍域1可能发生碰撞的旋转关节1运动范围为：
。
同理可得医用机械臂摆位运动过程中与障碍域2可能发生碰撞的关节1运动范围为：
，其中
综上，由关节1运动可能导致碰撞发生的运动范围为：
2.2.2 关节3的运动与障碍发生碰撞的条件
在可能发生碰撞的关节角
范围外，任意选取一个
，
，求出关节3偏置杆的初始位置和方向向量，并通过障碍域中心投影到关节3偏置杆轴线投影的距离判断关节3的运动是否会导致碰撞的发生。关节3运动范围示意图如图4所示。
图4 关节3运动范围示意图
图中
点为关节3坐标系原点，p1为障碍域中心投影，r1为障碍域半径，rg3为关节3偏置杆半径，关节3的运动行程为[-0.23,0.23]m。
如果
，则障碍域不在关节3运动范围内，无需考虑碰撞；否则，进行如下分析：
（1）计算d31：
（2）判断关节3的行程
若
-d31，则障碍域不在关节3运动范围内，无须继续分析；若
，则关节3行程为
。当障碍域位于坐标系三、四象限时分析方法与此相同，此时
由以上分析可得，对于给定的
，当障碍域在正行程范围内时，关节3的行程范围
；当障碍域在负行程范围内时，关节3的行程范围
（3）关节4的运动与障碍发生碰撞的条件。在
范围内任意给定
，讨论关节4运动导致碰撞发生的条件。具体过程与关节3类似，同理可得对于给定的 、d3,杆4运动无碰条件为
，其中
在此不再赘述。关节4运动范围示意图如图5所示。
图5 关节4运动范围示意图
以上讨论过程采用解析法进行，可以将笛卡尔空间的各障碍域模型实时转化至关节空间中，进而得到重离子放疗辅助医用机械臂的安全摆位空间。
3 基于A＊算法的无碰路径规划
A*算法是在Dijkstra算法基础上加了约束条件的启发性搜索算法，其具有可纳性、最优性，用于搜索状态空间的最好优先路径[14-15]。
3.1 算法思想
A*算法是应用于人工智能中的典型启发式搜索方法之一，这些方法都使用了启发函数，但节点搜索策略有所不同。
A*算法估价函数的设计是其算法思想的核心[16]。估价函数定义为：
式中
表示起始点到目标点的路径代价，由两部分构成。 表示从起点到节点 的最小实际代价，
为启发函数，表示从节点到目标点的路径估计代价。
3.2 基于A＊算法的路径规划方法
为了方便使用A*算法对重离子放疗辅助医用机械臂的摆位路径进行规划，定义医用机械臂的四个关节变量为一个四维数组，各起始关节变量为qfirst[4]；关节变量搜索步长为dstep[4]；第i步最佳关节变量为qbest_i[4]；后继节点关节变量为qfol_i[4]；期望关节变量为qexp[4]。
定义起始点到第i步最小实际代价
启发函数
两者相加便可得到第i步后继关节变量估价函数fi(q)。将fi(q)值最小的后继关节变量作为qbest_i[4]，根据步长生成下一步关节变量，重复以上过程便可得到每一步的最优关节变量，从而得到医用机械臂起始关节位置到目标位置的关节空间最优路径。
设定OPEN、CLOSE、后继节点表，初始化
，置CLOSE表为空，将qfirst[4]放入OPEN表。重复以下步骤，若OPEN表为空，则失败；否则，进入循环。
（1） 生成初始关节位置搜索图，将qfirst[4]放入OPEN表。
（2）生成CLOSE表，初始值为空。
（3）若OPEN表为空，失败。
（4）将OPEN表中f值最小的点作为qbest_i[4]放入CLOSE表，并将其从OPEN表中删除。
（5）判断qbest_i[4]是否是qexp[4]，若是，成功，否则进行下一步。
（6）根据dstep[4]生成后继节点qfol_i[4]。
（7）对qfol_i[4]进行如下过程：① 判断qfol_i[4]是否在医用机械臂安全摆位空间内，若在，建立返回qbest_i[4]的指针，若不在，返回（6）；② 计算
③ 判断qfol_i[4]是否在OPEN表中，若在，进入下一步，否则进入⑤；④ 定义qnew_i[4]，令，将qnew_i[4]放入qbest_i[4]后继节点表，并判g(qnew_i[4])＞g(qbest_i[4])是否成立，若成立，将qnew_i[4]上一节点作为qbest_i[4]，令qfol_i[4]=qnew_i[4]，进入⑦，否则q(qnew_i[4]=q(qfol_i[4])，直接进入⑦；⑤ 判断qfol_i[4]是否在CLOSE表中，若在进入④，否则进入下一步；⑥ 将qfol_i[4]放入OPEN表，并放入qbest_i[4]后继节点表；⑦ 计算f(qfol_i[4])，进入（4）。
循环结束，CLOSE表中数据即为医用机械臂摆位最优路径。其算法流程图如图6所示。
4 试验与仿真
对以上提出的医用机械臂路径规划方法进行仿真验证。经Solidworks软件建立医用机械臂拟真工作环境，并将其导入Adams软件得到完整的医用机械臂仿真虚拟样机模型如图7所示。
图6 算法流程图
为了验证避障的有效性，设计了运动仿真试验。试验中医用机械臂的初始位姿均为{0，0，0，0}。试验期望关节变量为{168.4，86.5，53.7，237.3}。在软件中设置仿真环境，仿真结果如图8所示。
图7 医用机械臂仿真模型图
注：a. 医用机械臂拟真工作环境；b. 医用机械臂仿真虚拟样机模型。
图8 仿真结果图
注：a. 仿真实验过程；b. 等中心回转运动角速度和角加速度曲线；c. X和Y向运动速度和加速度曲线。
图8中三幅图分别为仿真过程图、等中心回转运动角速度和角加速度曲线图、X和Y向运动速度和加速度曲线图。由图8可知，医用机械臂摆位过程中，由于自重较大，等中心回转运动过程中有较小的速度突变，会导致摆位过程存在轻微冲击振动现象，但影响较小，X和Y向运动过程平稳，基本满足摆位过程稳定性要求。仿真过程中无碰撞的情况发生，该路径规划方法能满足医用机械臂摆位避障要求。
5 结论
文中针对医用机械臂在放射治疗环境下摆位过程中可能与辅助设备发生碰撞的情况，对医用机械臂路径规划方法进行研究。首先将障碍模型进行合理简化，并将其转化至关节空间，从而求解出医用机械臂的安全摆位空间，并分析得到其各部分与障碍域碰撞条件；在此基础上，基于A*算法于安全摆位空间内进行路径搜索，得到医用机械臂摆位最优路径。仿真试验表明，提出的应用于医用机械臂摆位过程的路径规划方法是安全可行的。
[1]王田苗,张大朋,刘达.医用机器人的发展方向[J].中国医疗器械杂志,2008,32(4):235-238.
[2]陈斌.我国医疗设备发展情况的研究[J].中国医疗设备,2018,33(3):136-139.
[3]许慧雯,王越,杨晓芳,等.医疗器械标准体系进展[J].中国医疗器械杂志,2018,42(1):49-52.
[4]Taylor RH.A perspective on medical robotics[J].Proc IEEE,2006,94(9),1652-1664.
[5]杜志江,孙立宁,富力新.医疗机器人发展概况综述[J].机器人,2003,25(2):182-187.
[6]Wang Y.A fast workspace-density-driven inverse kinematics method for hyper-redundant manipulators[J].Robotica,2006,24(5):649-655.
[7]Ramk DM,Sabourin C,Madani K.Autonomous knowledge acquisition based on artificial curiosity: application to mobile robots in an indoor environment[J].Rob Autonom Syst,2013,61(12):1680-1695.
[8]杨俊成,李淑霞,蔡增玉.路径规划算法的研究与发展[J].控制工程,2017,24(7):1473-1480.
[9]吴宏.增维启发式搜索路径规划算法[J].电脑知识与技术,2016,12(36):188-191.
[10]王其军,杜建军.MOTOMAN机器人逆运动学新分析[J].哈尔滨工业大学学报,2010,42(3):451-454.
[11]张智,邹盛涛,董然,等.复杂环境建模与机器人避障规划研究[J].哈尔滨工程大学学报,2016,37(10):1373-1380.
[12]Sekmen A,Challap P.Assessment of adaptive human-robot interactions[J].Knowledge-Based Systems,2013,42:49-59.
[13]张毅,代恩灿,罗元.基于改进遗传算法的移动机器人路径规划[J].计算机测量与控制,2016,24(1):313-316.
[14]贾庆轩,陈钢,孙汉旭,等.基于A*算法的空间机械臂避障路径规划[J].机械工程学报,2010,46(3):109-115.
[15]汪首坤,邸智,王军政,等.基于A*改进算法的机械臂避障路径规划[J].北京理工大学学报,2011,31(11):1302-1306.
[16]赵真明,孟正大.基于加权A*算法的服务型机器人路径规划[J].华中科技大学学报,2008,(S1):196-198.
Collision Avoidance Path Planning of Medical Manipulator Assisted by Heavy Ion Radiotherapy