第 5 章 柔性针穿刺轨迹规划
5.4 实验验证与分析
5.4.2 动态规划实验及结果
本小节是为验证在靶点和障碍物运动情况下动态轨迹规划算法的有效性和合理性。为 更好地观察组织假体内部靶点和障碍物的运动情况，实验中降低了聚乙烯醇 PVA 的含量 来制备硬度较小的假体材料，其聚乙烯醇质量（g）、去离子水体积（ml）和二甲基亚砜体 积（ml）三种成分的配比为 6 : 40 : 60，其尺寸为 20mm
× 80mm × 150mm，杨氏模量取值
范围为 10KPa−50KPa，假体材料硬度较低。穿刺针使用 22G
×150mm 型号的 PTC 穿刺针。实验过程中，也采用柔性更好的 23G×
150mm 型号的 PTC 穿刺针进行穿刺，其外部针套直径为 0.61mm，内部套针直径为 0.33mm。
与运动学验证实验相同，实验假体组织被夹在两个带孔的亚克力薄板之间，穿刺是柔性 针通过薄板的孔刺入组织内部来保证针体变形主要受到组织作用而非针座上偏移力作用；
实验中，CCD 相机以分辨率为 1296
× 966，频率为 12HZ 的状态记录针体轨迹、靶点和障
碍物标识物的运动。为了便于观测靶点和障碍物在组织内部的运动，在 PVA 假体组织内部嵌入圆形和 圆柱体标识物来表示靶点和障碍物，如图 5.11所示。图中，1 号到 5 号障碍物圆形标识
图中 1-5 为黑色圆形珠子，代表假体内障碍物；6 为黑色圆柱体，代表假体内目标靶点。
图 5.11 动态路径规划实验靶点和障碍物的标识物
质心的初始位置坐标分别为 xO1 = [20.83, 38.18, 4.98]、xO2 = [55.38, 39.87, 5.13]、xO3 = [19.25, 65.00, 5.20]、xO4 = [20.75, 68.00, 4.89] 和 xO5 = [53.60, 70.11, 5.15]；6 号目标靶点圆 柱体标识物质心初始位置坐标为 xG = [52.66, 97.64, 5.08]。所有障碍物和目标靶点均设计
104
浙江大学博士学位论文 第 5 章 柔性针穿刺轨迹规划
处于同一 XOY 平面内，以便于图像记录和测量；2 号、5 号障碍物和 6 号目标靶点在 X 轴方向较为接近，基本在同一直线上。
5.4.2.2 动态穿刺规划和实验设计
为了验证动态轨迹规划的有效性，设计两组实验分别为旋转三次和四次针座来改变针 尖斜角方向，其调整角度利用动态规划算法计算，具体轨迹规划和实验过程如下所述。
对动态环境下穿刺运动实验的设计，关键在于如何预测靶点和障碍物在连续时间序列 上的运动情况；对于旋转 3 次和 4 次的动态穿刺规划过程而言，针体被分为 3 段和 4 段，其连 续时间序列分别为 t3= [t1
, t
2, t
3] 和t
4 = [t1, t
2, t
3, t
4]。采用3.4小节的 Kriging 实时预测模型 可以计算出靶点和障碍物在穿刺过程中（即穿刺时间序列上）的运动位移。计算出连续时间 序列上目标靶点位置 xGn = [⃗x
G1, ⃗ x
G2, · · · , ⃗x
Gn] 和障碍物位置x
Ojn =[⃗
x
Oj1, ⃗ x
Oj2, · · · , ⃗x
Ojn](j = 1, 2, 3, 4, 5)；对于 3 次和 4 次旋转规划，时间序列个数分别取为 n = 3 和 n = 4。
进一步需要计算针体轨迹和针尖位置，并计算广义穿刺误差。利用逆运动学和运动学 模型可计算出针尖在转折点处的位置坐标，然后再采用样条曲线拟合获得整个针体的平滑 轨迹。利用规划出的调整角几何，计算出连续时间序列上的针尖位置 xt = [⃗
x
t1, ⃗ x
t2, · · · , ⃗x
tn]；CCD 相机按照 12HZ 的频率记录穿刺过程，实验穿刺时间约为 20s，故每次实验约记录约 240-280 张，因此在规划过程中取穿刺时间序列为 tn = [t1
, t
2, · · · , t
250]，并利用拟合的平滑 轨迹计算出针尖在穿刺时间序列 t 上的位置坐标作为针体轨迹，即 xn = [⃗x
t1, ⃗ x
t2, · · · , ⃗x
t250]。利用计算出的目标靶点、障碍物、针尖和针体轨迹信息可计算出连续时间序列 tn 上的广 义穿刺误差 [εt1
, ε
t2, · · · , ε
tn]，进一步进行动态轨迹规划。采用 3 次和 4 次旋转针尖斜角方向来规划动态环境下的针体轨迹，规划针段长度分 别为 33.33mm 和 25.00mm；动态规划过程中，计算广义穿刺误差的引力和斥力影响数及 针体与障碍物安全距离分别取 κg =1、κo =10 和 d0 =2.00mm。实验假体中靶点和障碍 物处于同一平面内，因此规划轨迹尽量选择在平面的轨迹；为了说明柔性针的可操控性，
将穿刺初始位置坐标选择靶点的正上方，即 xt0 = [53.00, 0.00, 5.00]。1 号、3 号和 4 号障 碍物与针体之间的距离大于安全距离，故按照广义穿刺误差定义，其障碍误差均为零；而 在进针过程中，需要具体计算上方的 2 号障碍物和距离靶点较近的 5 号障碍物的障碍误 差 εon 及针尖与靶点之间的靶点误差 εgt 来搜索可行路径。按照5.3.2.2小节的动态轨迹规 划算法，得到 3 次和 4 次旋转的规划调整角集合分别为 Φ3D = [0.20◦
, 178.86
◦, −0.24
◦] 和 Φ4D = [0.00◦, 175.70
◦, −0.18
◦, 0.12
◦]，其规划轨迹也基本处于同一平面内，且位于整个针体 轨迹均处于 2 号和 5 号障碍物的左侧。第 5 章 柔性针穿刺轨迹规划 浙江大学博士学位论文
而实验过程中，穿刺针按照规划的调整角集合 Φ3D进针，各段针段长度分别为 34.00mm、
33.00mm 和 33.00mm；按照规划的调整角集合 Φ4D，各段针段长度均为 25.00mm，穿刺 速度均为 5mm/s。图 5.11中 PVA 假体内部的标识物初始位置是在穿刺实验前进行测量的，
与预先设计的初始位置有偏差，如预先设计目标靶点位置为 xGd = [53.00, 98.00, 5.00] 而实 际位置是 xG= [52.66, 98.22, 5.08]，这是假体材料成型过程造成的变形偏差。
5.4.2.3 动态穿刺结果及分析
针刺初始位置设在目标靶点的正上方，主要是为说明柔性针绕过 2 号和 5 号障碍物 的操控能力。采用 22G 型号 PTC 针刺入 PVA 假体材料，并用 CCD 相机记录旋转 3 次和 4 次时穿刺实验过程，如图 5.12所示。图中，柔性针体均处于 2 号和 5 号障碍物左侧靠近
(a) 旋转 3 次穿刺过程 (b) 旋转 4 次穿刺过程 图 5.12 动态穿刺轨迹实验过程图片
目标靶点，这与按照 Φ3D和 Φ4D的规划轨迹是一致的。从图 5.12a和5.12b中的穿刺轨迹看，
针体与上方的 2 号障碍物之间距离较远，与 5 号障碍物之间距离较近。
实验过程中，PVA 假体组织内部的障碍物和目标靶点偏离初始位置产生运动。针体 旋转 3 次实验时，假体内部靶点和障碍物在 X 和 Y 方向的位移，如图 5.13所示；图中，
Obstacle 表示障碍物，Target 表示目标靶点。从图中可看出，1 号和 4 号障碍物（3 号与 4 号距离较近，情况类似，因此未在 (5.13) 中绘出）由于离针体位置较远，与针体轨迹干涉 较小，其位移也较小；2 号障碍物在 Y 方向的位移较大，最大值为 1.55mm，而在 X 方向位 移较小，最大值仅为 0.07mm，最大合位移为到 1.56mm，其最终位置坐标为 [55.34, 40.91]。
5 号在 X 方向的位移较大，最大值为 0.54mm，在 Y 方向的位移较 2 号要小，最大值为
106
浙江大学博士学位论文 第 5 章 柔性针穿刺轨迹规划
1.30mm，最大合位移为 1.41mm，其最终位置坐标为 [53.18, 70.91]。而目标靶点在 X 方向 的位移也较小，最大值仅为 0.05mm，在 Y 方向的位移最大值为 1.42mm，其在 XOY 平面 向的位移最大值为 1.93mm，最大合位移为 1.94mm，其最终位置坐标为 [55.37, 41.21]。而 目标靶点在 X 方向的位移最大值仅为 0.14mm，在 Y 方向的位移最大值为 1.76mm，其在 XOY 平面内的最终位置坐标为 [52.58, 99.00]。总体上，旋转四次时比旋转三次的靶点和障 碍物位移要大。
第 5 章 柔性针穿刺轨迹规划 浙江大学博士学位论文
Planned trajectory 表示算法规划轨迹，Experimental trajectory 表示实验穿刺轨迹，Obstacle 2、Obstacle 5 和 Target 分别为实验中 2 号、5 号障碍物和目标靶点的最终位置。
图 5.15 穿刺实验针体轨迹
尖从初始位置 [53.00, 0.00] 沿 Y 的负方向刺入。实验穿刺轨迹与规划轨迹之间存在误差，
相邻之间约有 0.1mm
− 0.5mm 的距离。对于旋转 3 次和 4 次时，规划得到针尖在 XOY
平面内最终位置分别为 [52.03, 98.23] 和 [52.52, 98.42]，而实验中针尖最终位置坐标分别为 [52.03, 97.73] 和 [52.46, 97.92]。为了说明穿刺轨迹的有效性，利用式 (4.21) 得到最终针尖位置与目标靶点之间的误差
3 段 2.05mm 1.16mm 1.13mm 4 段 2.15mm 1.20mm 1.09mm
108
浙江大学博士学位论文 第 5 章 柔性针穿刺轨迹规划
物之间距离均大于 1.16mm，针尖与目标靶点之间穿刺误差小于 1.20mm，所规划针体轨迹 能够有效避开组织内部运动的障碍物，并能到达运动的靶点。
从图 5.12可看出，针体均从 2 号和 5 号障碍物左侧刺入，即针尖仅向 X 负方向进行 挠曲。利用动态规划算法进行规划时，用 3 和 4 个针段规划，仅能获得障碍物同侧针体轨 迹，无法得到如图 5.16所示的绕过障碍物的针体轨迹。图 5.16a和5.16b中的针体轨迹分别
(a) 8 个针段针体轨迹 (b) 10 个针段针体轨迹 图 5.16 绕过障碍物的穿刺针体轨迹
用 8 次（即针体分为 8 个针段，针段长度为 12.50mm）和 10 次（即针体分为 10 个针段，
针段长度为 10.00mm）旋转进行规划得到的。但 22G 型号穿刺针无法实现该轨迹，故图 中轨迹是用柔性更好的 23G 型号穿刺针实现的。
综上实验结果，基于悬臂梁运动学的动态轨迹规划算法可实现动态环境下避障和中靶 要求，并能获得穿刺误差最小的针体轨迹。当取针段长度较小时，可规划出曲率较大的穿 刺轨迹，但实际操控需要使用柔性较好的穿刺针；但柔性大的穿刺针难以控制其按照规划 轨迹进针。本动态轨迹规划算法是在考虑柔性针穿刺运动可达性的基础上提出的，充分考 虑了针体轨迹的在组织内部的可行性和针体运动的可操控性，克服了文献中规划轨迹不 可实现的缺陷[81,94,101,159]。
5.5 本章小结
本章在讨论柔性针穿刺运动可达性和逆运动学解存在性的基础上，提出了无障碍环境 和静态环境下基于悬臂梁运动学和逆运动学的针体轨迹规划方法。在静态轨迹规划的基 础上，利用欧式距离和人工势场概念定义了综合考虑靶点和障碍物的广义穿刺误差，将动
第 5 章 柔性针穿刺轨迹规划 浙江大学博士学位论文
态轨迹规划问题转化为穿刺误差优化问题，结合 Kriging 软组织实时模型建立动态环境下 的针体轨迹规划算法。
我们设计了三组实验分别说明无障碍物静态轨迹规划、有障碍物静态轨迹规划及动 态环境下针体轨迹规划等算法的合理和有效性。无障碍静态靶点环境下，实验与规划靶点 之间的穿刺误差为 0.19mm；按照 3 段和 4 段对有障碍静态靶点环境下的穿刺轨迹进行规 划，实验穿刺轨迹的靶点穿刺误差分别为 0.76mm 和 1.22mm。按照 3 段和 4 段对动态靶 点和障碍物环境下进行穿刺轨迹规划；3 次旋转穿刺实验中，障碍物最大位移为 1.56mm，
目标靶点最大位移为 1.43mm，针体能够绕过障碍物到达靶点，靶点穿刺误差为 1.13mm；
4 次旋转穿刺实验中，障碍物最大位移为 2.23mm，目标靶点最大位移为 1.76mm，靶点穿 刺误差为 1.09mm。同时，采用柔性更好的 23G 型号穿刺针，按照 8 段和 10 段进行规划
4 次旋转穿刺实验中，障碍物最大位移为 2.23mm，目标靶点最大位移为 1.76mm，靶点穿 刺误差为 1.09mm。同时，采用柔性更好的 23G 型号穿刺针，按照 8 段和 10 段进行规划