第 5 章 柔性针穿刺轨迹规划
5.3 动态环境下穿刺轨迹规划
5.3.2 动态轨迹规划
如前所述，柔性针与软组织相互作用必然导致产生组织内靶点和障碍物运动，而使针 尖无法到达靶点或者针体碰撞障碍物。动态轨迹规划，就是考虑在组织变形的动态环境 中，寻找实现躲避运动障碍物和到达运动靶点的可行针体轨迹。利用广义穿刺误差的概 念，动态轨迹规划即是寻找穿刺误差最小的针体运动轨迹。
动态轨迹规划的关键是确定柔性针和软组织的动态信息。对斜角柔性穿刺针运动学 建模时，穿刺过程按照准静态思想被分为 n 个子过程；以穿刺初始时刻为起点，针尖在 连续时间序列 t = [t0
, t
1, t
2, · · · , t
n] 中每个时刻调整针尖斜角方向，其对应的调整角为 Φ = [ϕ0, ϕ
1, ϕ
2, · · · , ϕ
n]（通常 ϕ0 =0◦）。将调整角集合 Φ 代入运动学模型可计算出 t 时间 序列内各时刻的针尖位置。所有时间序列针尖位置连接起来形成的折线就是针体轨迹；对 于子过程 n 较少时，需要用插值来得到针体的平滑轨迹。障碍物和靶点的动态信息是随着针体与软组织之间作用而变化的，因此需要根据软 组织变形规律来预测其实时动态信息。对动态轨迹规划而言，需要预测软组织环境中目 标靶点和障碍物在穿刺时间序列 t 内各时刻的位置信息。应用第三章提出的基于 Kriging 元模型的软组织运动模型，可对组织内部靶点和每个障碍物的实时信息进行预测，即可 得到 t 内各时刻靶点位置信息 xg = [⃗
x
g(t1), ⃗x
g(t2),· · · , ⃗x
g(tn)] 和障碍物位置信息x
oj = [⃗ x
oj(t1), ⃗x
oj(t2),· · · , ⃗x
oj(tn)]，其中 j = 1, 2,
· · · , N
o。将柔性针、靶点和障碍物的动态信息代入式 (5.8) 可得到穿刺时间序列 t 内各时刻的 广义穿刺误差 ε = [ε(t1), ε(t2),
· · · , ε(t
n)] 。如同静态环境下针体轨迹规划一样，对于存在 障碍物的穿刺场景能够满足避障和中靶的可行路径若存在，通常存在多条可行路径；或者 不存在。动态轨迹规划需要寻找一条优化的可行针体轨迹进入组织，即保证任意 i 时刻的 穿刺误差 ε(ti) (i = 1, 2,· · · , n) 最小。因此，当目标靶点初始位置 ⃗x
g(t0) 已知时，动态轨 迹规划问题可以转化为优化问题，如公式 5.9。min
[ε(t1), ε(t2),· · · , ε(t
n)]S.t. g
b,n(Φ) = g (⃗x
g(tn)) Φ = [ϕ1, ϕ
2, · · · , ϕ
n]− π ⩽ ϕ
i ⩽ π i = 1, 2, · · · , n(5.9)
式 (5.9) 中优化目标是每步进针使得广义穿刺误差最小；由于 ε(t) 是根据人工势场法定义 的，保证穿刺误差最小即是保证穿刺轨迹势场能量最小。针尖最终位置与穿刺最后时刻 tn
96
浙江大学博士学位论文 第 5 章 柔性针穿刺轨迹规划
时的靶点位置相重合，但实际计算中难以保证完全重合。因此采用
∥g
b,n(Φ)−g (⃗x
g(tn))∥ ≤ ϵ
代替上式中的恒等式，ϵ 为针尖与目标靶点之间可容许的微小偏移量。动态轨迹规划具 体流程如图 5.4所示。图中，初始化参数包括针体长度 L、初始针段长度 l、针尖斜角वᤜ㓴㓷઼䪸փ৲ᮠˈࡪޕ⛩ˈ䶦⛩ս㖞ˈ䳌
⺽⢙ս㖞ˈクࡪॺᖴˈ䚯䳌ᆹޘ䐍ㅹ
䇑㇇ਟ㹼䪸փ䖘䘩 䀓㇇䈳ᮤ䀂䳶ਸ
Krigingݳ⁑ර
YES
ࡍ㇇䶦⛩઼䳌⺽⢙ᯠս㖞
(ti)
NO
图中，− ▶ 表示数据流动方向，−▷ 表示计算需要的模型或方法。
图 5.4 动态轨迹规划流程图
角度 α、针段挠曲量 ω 和挠曲转角角度 θ。穿刺初始信息，包括穿刺初始位置，即针 尖在 t0 时刻的位置 ⃗xt(t0)、目标靶点的初始位置 ⃗
x
g(t0) 和障碍物的初始位置 xo(t0) = [⃗x
o1(t0), ⃗x
o2(t0),· · · , ⃗x
oNo(t0)]T 等。5.3.2.2 在线滚动优化轨迹
利用公式 5.9对穿刺轨迹优化过程中，靶点和障碍物信息在每一步迭代过程中都需要 更新，其更新过程采用在线滚动优化策略。模型计算得到的广义穿刺误差仅在当前规划步 执行，下一规划步利用新的靶点和障碍物信息重新计算。在线滚动优化过程如图 5.5所示。
图中，前一步规划得到调整角 ϕk-1 作为当前第 k 步规划的输入，基于模型预测信息在线
䘀ࣘᆖ
k-1 xt(tk) k k
xg(tk+1) xo(tk+1)
Ӫᐕ࣯൪
Kriging亴⍻
䘶䘀ࣘᆖ !(tk) Ոॆ
图 5.5 在线滚动信息更新流程图
第 5 章 柔性针穿刺轨迹规划 浙江大学博士学位论文
滚动优化穿刺误差，进而比较穿刺误差得到本次规划步的优化调整角 ϕk，具体过程如下。
对于运动到第 k-1 步的针段，其调整角 ϕk−1 代入运动学方程，可计算出第 k 步针尖位置
x
t(tk)。将针尖当前位置和Kriging 预测的靶点未来位置 xg(tk+1) 代入到逆运动学方程，根 据不同初值可得到 M 调整角组成的集合 Φk，即 M 条可行的穿刺路径，均由 ntk个针段构 成。然后将 Kriging 模型预测的未来障碍物位置 xo(tk+1) 代入得到 tk时刻的广义穿刺误差ε(t
k)，进行比较优化得到误差最小的候选调整角 ϕk。如此循环（图中虚线表示下一规划 步，以本次规划步 ϕk作为输入），直至针尖运动到达靶点。在已知目标靶点位置的情况下，与静态穿刺轨迹类似，我们仍可采用图 5.2，并结合医 生经验来确定针尖的穿刺初始位置 xt(t0)。依据图5.5的在线滚动策略，首先基于 Kriging 元模型的组织变形预测模型来计算出靶点在 t1 时刻的位置 xg(t1)；然后根据穿刺初始位 置 xt(t0) 和靶点位置 xg(t1)，依据无障碍物环境下可计算M 条由 nt1 个针段构成的可行轨 迹，针段长度记为 l1。可行轨迹对应的第一步可行调整角集合 Φ1 = [ϕ11
, ϕ
12, · · · , ϕ
1M]；其 中，ϕ1j = [ϕj1, · · · , ϕ
jnt1] , j = 1, 2,· · · , M。再根据 Kriging 模型计算出的 t
1时刻障碍物位置x
o(t1)，进一步计算出广义穿刺误差并比较，可在调整角集合 Φ1选择出优化的调整角 ϕ1， 具体针尖更新过程如图 5.6所示。图 5.6中绿色曲线为穿刺初始位置到 t1 时刻针尖位置的1=[ 11, 12, … , 1M] ...
...
xt(t1)
xt(t2)
... ...
Insertion location
Target xg(tn)
图中 Insertion location 为初始位置，Target 为目标靶点位置，Φ1= [ϕ11, ϕ12,· · · , ϕ1nt1] 为穿刺初始位置 到达 t1时刻时靶点位置 xg(t1) 的调整角集合；绿色曲线为从穿刺初始位置到达 t1时刻位置 xt(t1) 的可
行轨迹，绿色圆球为针尖在 t1时刻的位置；紫色曲线为针尖从 t1时刻位置 xt(t1) 到达 t2时刻位置 xt(t2) 的可行轨迹，紫色圆球为针尖在 t2时刻的位置。
图 5.6 动态穿刺优化轨迹更新示意图
优化轨迹，其对应的调整角即为优化调整角集合 Φ 中第一个元素 ϕ1。
由优化的调整角 ϕ1 可计算出新的针尖位置 xt(t1)；然后由Kriging 模型计算更新得到 在 t2时刻新的目标靶点位置 xg(t2) 和新的障碍物位置
x
o(t2)。此时以 t1时刻针尖位置 xt(t1)98
浙江大学博士学位论文 第 5 章 柔性针穿刺轨迹规划
为起始点，以 t2时刻靶点位置 xg(t2) 为目标靶点，按照无障碍物策略，可重新规划出M 条 由 nt2（可与 nt1不相等）个针段构成的可行轨迹。t1 至 t2之间针段记为 L2 = (L
− l
1)/nt2。 计算得到相应的由 t1 时刻针尖位置到 t2 时刻靶点位置的调整角 Φ2 = [ϕ21, ϕ
22, · · · , ϕ
2M]，其中 ϕ2j = [ϕj1
, · · · , ϕ
jnt2] , j = 1, 2,· · · , M。通过更新障碍物位置信息，计算其在 t
2 时刻的 广义穿刺误差，并得到优化调整角集合的第二个元素 ϕ2，图 5.6中的紫色曲线为其对应的 从 t1 时刻到 t2 时刻的优化可行轨迹。如此反复，可计算出任意 ti 时刻针尖位置 xt(ti) 到t
i+1时刻靶点位置 xg(ti+1) 的调整角集合 Φi，并优化得到调整角 ϕi∈ Φ
i。得到完整的穿刺 轨迹及优化的调整角集合 Φ = [ϕ1, ϕ
2, · · · , ϕ
n] 及其对应的针段长度集合 L = [l1, l
2, · · · , l
n]。利用针体正向运动学可得到完整优化穿刺轨迹，并满足针尖与目标靶点重合，针体与障碍 物之间保持安全距离。
5.3.2.3 针段参数更新策略
每步规划过程中，针段长度的选取决定了针尖能否到达靶点和避开障碍物。针段长度 越大，则针体旋转次数越少，在临床方便医生操作和减少病人疼痛；但针段过长往往无法 实现较为复杂的避障路径，因此要合理选择针段长度。为减少规划的步数，首先选择长针 段。对于第 k 步规划，其针尖到靶点的长度 Lk = L
−
∑k-1i=1
l
i，首先选择针段数 ntk0进行规划。若所规划轨迹经穿刺误差验证，可以满足避障条件，则确定第 k 步针段数 ntk =ntk0；若不 满足避障条件，则选取 ntk1 = 2
× n
tk0作为针段数继续规划，若满足避障条件，则 ntk =ntk1。 如此成倍增加针段数来减小针段长度，直至寻找合适的针段数 ntk =ntkj= 2j× n
tk0。由于 受到穿刺半径和穿刺深度的约束限制，可能无法规划出可行的穿刺路径，因此不能无限增 大针段数，否则迭代过程陷入死循环。后文实验中，设置针段数上限为 50，超过则停止规 划。实际的临床操作中，过于细小的针段操作也难以实现。对于整个规划过程中，初始规划步的针段长度一般要大于后期的针段长度，即满足
l
k−1≥ l
k。因此，第一步规划针段一般给定初始值，而中间任意第 k 步则首先选择 lk-1 作 为初始针段长度，按照上述更新策略来确定针段长度。对于整个规划过程而言，针段数 n（即规划步）不是固定的，而是动态的。
5.3.2.4 算法收敛性讨论
在轨迹优化过程中，由于广义穿刺误差是根据人工势场法来定义的，在优化过程中沿 势场梯度方向寻找最小势能区域，可能出现局部最小而陷入某一区域而无法达到靶点[158]。 而式 (4.20) 计算运动学解集时，不同初值对应不同解集，在连续空间上存在无限多个由针 尖到靶点的可行轨迹；若连续空间上的无限解集通过广义穿刺误差来优化，可能导致优化
第 5 章 柔性针穿刺轨迹规划 浙江大学博士学位论文
过程陷入局部最小。
为了避免局部最小情况出现，通过给定有限个初值得到有限个（前文用 M 表示）不 同解集，然后从中搜索得到最小穿刺误差的轨迹。有限解保证了误差优化的收敛性，但 是由于没有在连续空间内搜索，所规划路径不能保证全局最优，仅在局部解域内最优。
为尽可能在全部解域内优化轨迹，选择均匀分布的初值 ϕ0j = [(j
− 1) × ∆ϕ, 0
◦, · · · , 0
◦]T（j = 1, 2,
· · · , M）生成可行轨迹，如图 5.7所示。图中，穿刺起始点 Start point 坐标为
-1 -0.5 0 0.5 1 1.5 -1
0 0 1
2 4 6 8 10
Y axis X axis
Z axis
Goal
Start point
图 5.7 均匀分布初值生成充满空间的可行轨迹
[0cm, 0cm, 0cm]，目标 Goal 坐标为 [0.8cm, 1.2cm, 9.8cm]，∆ϕ = 18◦，得到 M = 360/18+1 = 21 条可行轨迹。从图中看，所生成轨迹较充满整个空间，在此基础上选择优化轨迹，充分 考虑了空间整个解域内的可行轨迹。若实现更优化解，则需要生成更多条充满空间的可行 轨迹。实际规划过程中，既要充分考虑整个空间解域，也尽量减少迭代计算次数，因此选 择 M = 21 较为合适。后文规划过程中，每个规划步均生成 21 条可行轨迹，从中搜索最优 轨迹，即广义穿刺误差最小的穿刺轨迹。