第 1 章 绪 论
1.2 国内外研究现状
1.2.3 穿刺运动规划
穿刺动轨迹规划是指通过控制穿刺参数（如针座速度和位姿），引导针尖合理避开障 碍物（如体内关键部位、血管等）和命中病灶点的运动过程[95,96]，是一个动力学问题。针 穿刺过程中，由于软组织的粘弹、非线性和不均匀等特性造成的软组织变形和柔性针挠曲 使得穿刺路径规划和运动轨迹控制变得复杂。文献报道关于穿刺运动规划与控制方法主 要可归纳为：人工势场法、概率规划模型、基于逆运动学的三维运动规划等。
1.2.3.1 人工势场法
人工势场（Artificial Potential Field, APF）法用于机器人路径规划和避障问题，最早 被 DiMaio 等用于穿刺针轨迹规划，可以实现静态环境下针体避障和针尖命中靶点的目
标[78,95]。APF 法基于梯度下降搜索法（Gradient Descent Search Method）寻找最小势函数[97]。
APF 法将操作空间定义为引力场和斥力场的叠加，障碍物周围产生斥力势场，而目标靶点 周围产生引力势场。DiMaio 等利用针尖位置和针尖方向构造出二位平面内的针尖操作空 间，当针尖沿着引力势场和斥力势场构成的整体势场最小的轨迹运行，就能保证针轨迹避 开障碍物，且命中病灶靶点，其仿真结果如图 1.9a。最小势场原理本质上是实现穿刺的最 短路径。但，最小势场搜索法可能出现局部最小而陷入某一区域而无法达到靶点。为有效 找到满足条件的针体轨迹，天津大学江杉提出了一种改进的共轭梯度法，通过针尖局部网 格来解算其路径，解决了局部最小点问题[98]；同时采用间隔搜索方法实现避障功能，其仿 真结果如图 1.9b。人工势场法为针穿刺路径规划提供了可能，但靶点和障碍物运动的不确 定性限制了该方法在实际中的应用，而且割裂了针和软组织之间相互影响的动力学规律。
1.2.3.2 概率规划模型
Webster 等利用 Lie 群理论提出了带斜角针尖柔性针的非完整约束模型，使带斜角针 尖的柔性针得到很好控制，但他们未讨论路径规划问题。非完整约束模型很好地解释了斜
10
浙江大学博士学位论文 第 1 章 绪 论
(a) DiMaio 等针轨迹规划仿真[78]
4 3 2 1
4 3
2 1
target
obstacle
(b) Jiang 等针轨迹规划仿真[99]
图 (a)：组织内包含三个障碍物和一个目标点；图 (b)：圆柱形物体为障碍物，圆形物体为靶点。
图 1.9 人工势场法针轨迹规划
角穿刺针的可控性，即分解成针穿刺（Insertions）和针旋转（Rotations）的组合控制针体 轨迹和针尖位姿[100,101]。但由于软组织多样性，针体轨迹存在不确定性，部分学者提出了 基于概率的规划模型来改善穿刺精度。主要有两种方法，一种是轨迹概率模型，另一种是 Markov 决策模型。
轨迹概率模型在非完整约束模型基础上，Park 等人提出了轨迹概率模型（Path-Of-Probability, POP），可实现针体避障[101–103]；该方法近似将针尖位姿的概率密度函数 (Prob-ability Density Function, PDF) 看作高斯分布函数。采用 POP 算法，针体轨迹为针尖之间 经过若干点的拼接，图图 1.10示意了算法过程。图中，算法经过 M 个中间步骤从初始位
图（a）：gi到达目标靶点 ggoal的概率低；图（b）：gi到达目标靶点 ggoal的概率高[101,103]。 图 1.10 POP 规划算法示意图
姿 g0 运动到目标靶点 ggoal。齐次变换矩阵 gi
∈SE (3) (i=1,2,…,M) 表示第 i 个坐标系相对
于第 i-1 个坐标系的位姿；假设 i-1 步位姿 gi, g
2, · · · , g
i−1 已确定，需要优化中间步 gi 使第 1 章 绪 论 浙江大学博士学位论文
针尖到达靶点。图中，阴影椭圆部分表示第 i 之后 M-i 步针尖的概率密度函数，因此选 择图 1.10b 中概率高的 gi作为中间位姿步进行后续规划。运用样条曲线对各中间位姿点进 行拟合，可以得到整体针体轨迹。计算每个规划步中经过路径是否碰到障碍物，可以实现
避障规划[101,103]。
Markov 决策模型 Alterovitz 等定义针体轨迹规划问题为基于状态空间离散的 Morkov
决策问题 (Markov Decision Problem, MDP)[100,104]。定义了穿刺过程中针状态用针尖位置p = (p
y, p
z)、方向角 θ 和斜角方向 b 等三个参数来描述，如图1.11所示。利用状态空间离图（a）：穿刺针状态用针尖位置 p、方向角 θ 和斜角方向 b 等三个参数来描述；图（b）：旋转针座改变 针尖斜角方向 b = 1，而针尖位置不变。针体路径沿着斜角方向呈圆弧状[100]。
图 1.11 针状态示意图
散方法，对针状态 w = [p, θ, b] 进行离散。针体运动规划问题就是解算出每个离散状态下 的最优动作，以保证穿刺靶点概率最大。针在状态 w 下穿刺成功概率 ps(w)；若状态 w 的 位置在靶点空间内，ps(w) = 1；若状态 w 的位置在障碍空间内，ps(w) = 0。穿刺运动规 划目标就是计算到达状态 wi 的最优动作环节 ui，保证穿刺成功概率 ps(w) = 0 最大，这 样就运动规划问题转化为一个 Markov 决策问题。采用无限循环动态规划 (Infinite Horizon Dynamic Programming, DP) 法计算穿刺成功概率最大化的环节。该算法结合术中医疗图像，
可实现针穿刺最优路径规划[100]。Alterovitz 等还提出了一种基于靶点误差的优化模型，考 虑了靶点运动的不确定性，采用 Euclidean 距离定义了靶点误差，实现了无传感运动针体 轨迹规划[96]。
1.2.3.3 基于逆运动学的三维运动规划
与二维平面内非完整约束运动学不同，Duindam 等提出了三维运动操控算法。按照运 动学方法，将针位姿定义为常俯仰角速度 (constant pitch speed)，零偏航角 (zero yaw) 以及 可控的倾斜角 (controlled roll angle)[81]。他们假设针在静态刚性空间中运动，针体运动完 全取决于针尖，针尖沿着半径为 r 的圆弧进行移动；同时，针座旋转运动直接传递到针尖，
而没有摩擦迟滞。采用旋量理论建立了穿刺针的运动学方程，并基于逆运动学（Inverse
12
浙江大学博士学位论文 第 1 章 绪 论
Kinematics, IK）提出了二维和三维情况下的运动规划方法[81,105]。其逆运动学的几何解，
如图 1.12所示。
图（a）：绕 zs旋转 β1，使针的 (y, z) 平面包含 q，刺入 rβ2使 q 和 z2在同一直线上；图（b）：绕 z2旋 转 β3使针的 (y, z) 平面包含 pg。点 q 可在通过 pg的 zg方向上任意选取；而 IK 算法可用于求解
β4,5,6[100]。
图 1.12 逆运动学几何解推导
中国科学技术大学的王建军博士简化了 Duindam 等人的工作，采用一种 stop-and-turn(实际上是一种准静态思想) 控制策略，将路径规划转化为求解转动次数最少情况下的
最短路径[94,106]。类似 IK 方法，三维规划问题中，用两个平面来进行避障处理，选择两平
面交线上一个中间点，作为第一个平面规划问题的目标点和第二个平面规划问题的起始 点，整个穿刺路径是可逆的。