血管介入机器人接触感知轨迹规划与控制框架解析

1. 项目概述：当柔性机器人“触”入血管

血管介入手术，无论是放置支架、取栓还是进行肿瘤栓塞，对医生的手眼协调能力和操作精度要求都极高。传统上，医生在X射线等影像引导下，通过推送、旋转导丝和导管，在复杂的血管网络中“盲操”，这不仅考验经验，更伴随着血管壁损伤、穿孔等风险。而连续体机器人，凭借其类似章鱼触手或大象鼻子的连续柔性结构，理论上能完美适应血管的蜿蜒曲折，为微创介入带来了革命性的想象空间。

但这个“完美”背后，有一个核心难题：如何在确保机器人末端精准到达病灶的同时，又能实时感知并规避与脆弱血管壁的接触力，防止“硬闯”造成的内膜损伤？这就是“基于接触感知的轨迹规划与控制框架”要解决的核心问题。它不是一个简单的运动指令，而是一个集成了“触觉”反馈的智能闭环系统。简单来说，就是让机器人不仅“看得见”（通过术前/术中影像），更要“摸得着”（通过力传感器），在行进中实时调整自己的“身姿”，实现“零接触”或“安全接触”下的灵巧穿行。

这个框架的价值，远不止于提升单次手术的成功率。它意味着介入手术可能从高度依赖专家个人经验的“手艺活”，向标准化、可复制的“技术流程”演进。对于基层医院，它降低了高难度血管介入手术的门槛；对于患者，它意味着更短的手术时间、更少的辐射暴露和更低的并发症风险。我接触过不少临床医生和工程师，大家都对这个方向充满期待，但也都清楚，从实验室的原理样机到真正能上手术台的产品，中间隔着无数需要填平的“坑”。

2. 核心思路与框架设计：感知、规划与控制的三角闭环

一套可靠的系统，其顶层设计决定了它的能力和上限。在这个项目中，我们构建的是一个典型的“感知-规划-执行”三层闭环架构，但每一层都针对血管介入的特殊性做了深度定制。

2.1 感知层：从“看见”到“感知”

在血管这个非结构化的动态环境中，仅靠视觉（如DSA造影）是远远不够的。血管壁会随着心跳和呼吸轻微搏动，且其力学特性（如弹性模量、摩擦系数）个体差异巨大。因此，感知层的核心任务是获取机器人本体与血管环境的交互信息。

1. 接触力/力矩的直接测量：这是最直观的方式。通常会在机器人末端或关键关节集成微型光纤布拉格光栅传感器或应变片。FBG传感器通过测量光波长的变化来反推应变，进而计算接触力，其优势在于尺寸小、抗电磁干扰、可多路复用，非常适合在狭小空间和强辐射的介入环境中使用。但难点在于标定复杂，且将力信号精确映射到机器人的具体接触点上（即“力位映射”）是个数学上的挑战。

实操心得：我们在初期尝试过在机器人外鞘管上粘贴微型应变片阵列，希望能实现分布式力感知。但实测下来，血管内血液流动、机器人自身弯曲产生的应力都会形成巨大噪声。后来改为在末端执行器（如微导管头端）内部集成单点FBG传感器，专注于测量尖端与血管壁的正面接触力，信号纯净度大幅提升。这给我的教训是：在资源有限时，优先保证关键测量点的精度和可靠性，比追求大而全的分布式感知更实际。

2. 基于运动学/动力学模型的间接估计：对于连续体机器人，其形状由驱动器的位移（如推拉钢丝的长度、气动压力）决定。通过高精度的编码器测量这些驱动量，并结合精确的机器人运动学与动力学模型，可以反向推算出机器人形变所需的内力。当机器人自由运动时，这个内力有一个基准值；一旦与血管壁接触，额外的接触力会导致模型预测的形变与实际形变出现偏差，通过观测这个偏差（即“残差”），就能间接估计出接触力的大小和方向。这种方法无需额外传感器，成本低，但对模型的准确性依赖极高。

3. 多模态信息融合：最理想的方案是直接测量与间接估计相结合，再融合术前CT/MRI提供的三维血管模型和术中二维透视影像。例如，用视觉信息进行大范围的定位和粗避障，用力感知进行精细的局部避障和力控制。我们框架中采用了一个扩展卡尔曼滤波器，将机器人的运动学状态、FBG测力信号和影像分割出的血管边界信息进行融合，实时估计出一个更可靠的“接触态势图”。

2.2 规划层：从“离线路径”到“在线反应”

传统的轨迹规划通常是离线的：根据术前影像规划一条从入口到病灶的理想路径，然后让机器人严格跟随。这在静态、刚性的工业环境中可行，但在动态、柔性的血管内就是灾难。我们的规划层必须是在线、实时、反应式的。

1. 全局参考路径生成：这仍然是起点。我们利用术前CTA或MRA数据，通过中心线提取算法（如快速行进法）得到血管的中心线。这条中心线就是我们的“理想高速公路”。但请注意，它只是参考，不是必须严格贴合的轨道。

2. 局部实时轨迹优化：这是规划层的核心。我们采用了一种基于模型预测控制思想的优化算法。在每个控制周期（通常是毫秒级），算法会：

• 预测：根据机器人当前状态和控制输入，预测未来短时间内（如未来0.5秒）多条可能的运动轨迹。
• 评估：用一个代价函数评估每条预测轨迹的“好坏”。这个函数通常包含：
  • 跟踪误差项：轨迹偏离全局参考路径的程度。
  • 接触力项：预测轨迹上机器人各点与血管壁的预估接触力（来自感知层），力越大，代价越高。
  • 平滑性项：机器人关节运动（或驱动量变化）的加速度、加加速度（Jerk），确保运动平稳，避免对血管造成冲击。
  • 进度项：鼓励机器人向目标点前进。
• 优化：通过数值优化方法（如二次规划、序列二次规划），快速找到使总代价最小的未来控制序列。
• 执行：只输出该序列的第一个控制指令给下层控制器，然后在下个周期重复整个过程。这就是“滚动时域”优化，它能不断根据最新的感知信息（尤其是接触力）调整计划，非常灵活。

3. 安全走廊构建：为了给优化问题增加硬约束，我们根据血管模型和一定的安全裕度（例如，血管半径减去机器人半径再减0.5mm），在血管内构建一个动态的“安全走廊”。优化算法必须保证机器人的预测轨迹完全位于这个走廊内，这从几何上提供了第一道安全保证。

2.3 控制层：从“位置跟踪”到“阻抗调节”

规划层输出了“应该怎么走”的期望轨迹（包括末端位姿和形状），控制层的任务就是让机器人的实际运动精准地跟上这个期望。这里的关键转变是从纯位置控制切换到阻抗控制或导纳控制。

1. 阻抗控制原理：你可以把阻抗控制想象成让机器人末端具备“弹簧-阻尼”特性。其控制律通常表示为：F = M * (¨x_d - ¨x) + B * (˙x_d - ˙x) + K * (x_d - x)。其中，F是控制器需要输出的力，x_d是期望位置，x是实际位置，M, B, K分别是设定的惯性、阻尼和刚度矩阵。

• 当没有接触时，机器人努力跟踪期望轨迹（减小位置误差x_d - x）。
• 一旦感知到接触力，控制器不再强行消除位置误差，而是允许机器人“退让”。例如，如果期望轨迹让机器人向右，但右侧血管壁产生了接触力F_contact，根据公式，这个接触力会被等效为一个位置偏差。控制器会“顺从”这个力，自动调整实际位置，使得接触力维持在一个安全、舒适的范围内。这就实现了“柔顺”的交互。

2. 参数整定与自适应：M, B, K这三个参数决定了机器人与环境交互的“性格”。刚度K大，则机器人显得“硬”，跟踪精度高但接触力大；刚度K小，则机器人“软”，避让性好但跟踪可能滞后。在血管介入中，我们需要根据不同的血管段（如较直的主动脉 vs. 弯曲的颅内血管）和不同的手术阶段（快速通过 vs. 精细操作）在线调整这些参数。我们的框架集成了一个简单的自适应律：当持续检测到接触力超过阈值时，自动调低局部方向的刚度K，增加阻尼B，让机器人变得更“柔顺”；当处于自由空间时，则恢复较高的刚度以保证跟踪性能。

3. 底层驱动控制：对于连续体机器人，其驱动器可能是推拉钢丝、气动肌肉或形状记忆合金。阻抗控制器计算出的力F，需要通过机器人的逆运动学或逆静力学模型，映射到各个驱动器的设定值（如钢丝拉力、气压值）。这一步的精度直接决定了高层控制思想能否被忠实执行。我们采用了基于李雅普诺夫稳定性理论设计的鲁棒控制器，来补偿钢丝传动中的摩擦、迟滞等非线性因素，确保驱动器的输出力能精准跟踪指令。

3. 核心环节实现与系统集成

理论框架搭建好后，真正的挑战在于如何将其实现为一个稳定、实时运行的系统。我们基于机器人操作系统（ROS）搭建了软件框架，并针对血管介入场景进行了大量细节上的打磨。

3.1 软硬件平台选型与搭建

硬件平台：我们采用了一种多段腱驱动连续体机器人作为实验平台。选择腱驱动（推拉钢丝）是因为其结构相对简单、输出力大、响应快，更适合介入手术中需要的快速、有力动作。机器人直径3.2mm，长度1.2米，具有两个可独立弯曲的段，每段由三根钢丝驱动，可实现三维空间的灵巧运动。末端集成了前文提到的单点FBG力传感器。驱动单元是六个高精度的直流伺服电机配合减速箱，用于收放钢丝。

软件框架（ROS）：

• 感知节点：订阅FBG解调仪的话题（/force_sensor）和电机编码器话题（/motor_position），运行EKF融合算法，发布/estimated_contact_force和/robot_shape话题。
• 规划节点：订阅/vascular_map（血管模型）、/target_pose（目标点）和/estimated_contact_force。核心是一个C++编写的实时优化器（使用OSQP库求解二次规划问题），以100Hz的频率发布/desired_trajectory（包含未来若干时间步的期望位姿序列）。
• 控制节点：订阅/desired_trajectory和/robot_shape，运行阻抗控制算法，计算出电机力矩指令，通过/motor_torque_cmd话题发送给底层电机驱动器。
• 仿真与可视化（Rviz/Gazebo）：我们建立了机器人的URDF模型和一个参数化的血管环境模型，可以在Gazebo中进行物理仿真，在Rviz中实时显示机器人的估计形状、接触力矢量和安全走廊，极大方便了调试。

3.2 关键算法实现细节

1. 连续体机器人运动学建模：我们采用了常曲率假设结合Cosserat杆理论的混合模型。对于单段弯曲，使用常曲率模型，计算速度快，满足实时性要求。但对于长距离、大变形以及计算机器人内部张力分布时，则切换到更精确的Cosserat杆模型。两者通过一个状态管理器切换，在大部分路径跟踪阶段使用常曲率模型，在需要进行精细力估计或遇到复杂接触时启用Cosserat模型。

2. 模型预测控制器（MPC）实现：代价函数设计为：J = Σ (‖x(k) - x_ref(k)‖_Q + ‖F_est(k)‖_R + ‖u(k) - u(k-1)‖_S)其中，x是状态，u是控制输入（钢丝长度变化率）。Q, R, S是对角权重矩阵。

• Q：跟踪误差权重。在血管直道部分设置较大，鼓励沿中心线走；在弯曲部分适当调小，为避让留出空间。
• R：接触力权重。这是最重要的安全参数。我们将其设计为自适应：R = R_base + α * ‖F_est‖，即估计接触力越大，对力的惩罚权重R也越大，系统会越发倾向于避让。
• S：控制输入变化率权重，保证运动平滑。 我们将机器人的运动学方程线性化后作为预测模型，将安全走廊作为线性不等式约束，从而将问题转化为一个带约束的二次规划问题，利用OSQP这类高效求解器实时求解。

3. 阻抗控制器与底层驱动接口：阻抗控制律在任务空间（笛卡尔空间）计算。得到所需的末端力F_desired后，利用机器人的力雅可比矩阵转置，将其映射到关节空间（驱动钢丝的张力）：τ = J^T * F_desired。 这里有一个关键处理：钢丝只能提供拉力，不能提供推力。因此，我们需要在张力分配环节解决这个单向约束问题。我们将其表述为一个优化问题：在满足τ = J^T * F_desired的前提下，最小化各钢丝张力之和，并约束所有张力大于一个小的正数（预紧力）。这同样用一个小型的QP求解器在线完成。

3.3 仿真与初步物理实验验证

在将系统用于动物实验前，我们在仿真和离体血管模型中进行了大量测试。

1. Gazebo仿真测试：我们在Gazebo中构建了一个包含主动脉弓和颈动脉分叉的柔性血管模型，并设置了物理属性（弹性、摩擦）。测试场景是让机器人从股动脉入口导航至大脑中动脉。

• 场景一：自由空间导航。关闭接触感知，MPC权重R设为0。机器人能精准跟踪血管中心线，但偶尔会“撞”上血管壁，在仿真中表现为模型穿透和抖动。
• 场景二：开启接触感知避障。在血管的一个弯道处放置了一个柔软的“斑块”突起。当R权重设置合适时，机器人末端在接近斑块前就感知到微小的力变化，MPC提前生成了一个绕行轨迹，平滑地避开了突起，接触力全程保持在0.05N以下的安全阈值内。
• 场景三：狭窄通道通过。模拟血管狭窄，安全走廊变得很细。通过调整MPC的预测时域和权重，机器人会像“蠕虫”一样调整自身姿态，以更顺滑的曲线缓慢通过狭窄段，避免了“硬挤”可能造成的模拟损伤。

2. 离体猪血管实验：我们搭建了恒温灌注系统，将新鲜的猪主动脉及其分支血管保持在37℃的生理盐水中。将机器人插入血管。

• 最大挑战：传感器噪声和漂移。真实的生物组织环境比仿真复杂得多。FBG传感器的信号会因温度、血液模拟液中的微粒而产生波动。我们不得不增加了一个在线卡尔曼滤波器对力信号进行降噪，并设置了死区阈值（小于0.02N的力变化忽略）。
• 关键成果：在通过主动脉弓时，系统成功实现了“主动避让”。当机器人尖端即将与血管壁外弯侧接触时，力传感器检测到压力上升至0.03N，MPC立即调整计划，使机器人向内侧轻微弯曲，同时整体前进速度放缓，最终接触力回落并稳定在0.01N左右，安全通过。而未启用接触感知的对照组，则记录到了数次超过0.1N的瞬时力峰值。

注意事项：离体实验与活体有巨大差异。活体血管有搏动、内皮细胞更脆弱、血液具有非牛顿流体特性。我们的力安全阈值（0.05N）是基于文献和离体实验的初步设定，在活体中必须重新进行严格的标定和验证，这需要与临床医生深度合作。

4. 挑战、优化方向与未来展望

尽管框架在原理和初步实验中表现出了潜力，但要走向临床，还有漫长的路要走，也暴露出许多亟待解决的问题。

4.1 当前面临的主要技术挑战

1. 感知的准确性与鲁棒性：

• 力传感的局限性：目前的点式力传感只能提供末端信息，对于机器人中段与血管壁的接触“盲区”无法感知。分布式触觉传感是方向，但如何在直径仅2-3mm的器械上实现高密度、可靠的力感知阵列，是材料和微纳制造的巨大挑战。
• 模型依赖的估计误差：基于模型的力估计严重依赖运动学/动力学模型的准确性。机器人在长期使用后的钢丝蠕变、关节磨损都会导致模型失配，使估计力产生漂移。如何设计在线模型参数自校正算法，是一个关键研究点。

2. 实时性与计算复杂度：我们的MPC优化周期目标是10ms（100Hz）。在标准台式机上，对于具有两个弯曲段、预测时域20步的模型，求解QP问题可以勉强达到。但如果增加机器人自由度（更多段）或加长预测时域以提高预见性，计算量会呈指数增长。未来可能需要专用硬件（如FPGA）加速优化求解，或者采用更轻量化的非线性MPC近似算法。

3. 人机交互与安全冗余：目前框架是全自动的，但在真实手术中，医生必须拥有最高决策权。如何设计 intuitive 的人机交互接口？是共享控制（医生操纵主手，机器人辅助避障），还是监督式自主（机器人自主规划，医生点头确认关键步骤）？此外，必须设计多层安全监控和急停机制，例如，当估计接触力超过绝对安全阈值、或规划器长时间找不到可行解时，系统必须能自动停止并报警，将控制权交还医生。

4.2 框架的潜在优化方向

1. 引入学习与自适应：纯模型的方法总有局限性。可以引入机器学习来增强系统。例如：

• 用深度学习补偿模型误差：收集大量机器人运动数据和对应的真实接触力（在高端实验平台上用高精度传感器获取），训练一个神经网络来补偿基于模型的力估计误差。
• 模仿学习医生操作：记录专家医生在模拟器或动物实验中的操作数据（手柄输入、影像、结果），让机器人学习在特定解剖结构下的高效、安全导航策略，作为MPC代价函数中“经验项”的补充。

2. 多机器人协同：复杂的神经介入或冠脉介入往往需要多器械协同（如导丝开路，导管跟进，支架输送）。可以扩展本框架，研究多连续体机器人之间的协同轨迹规划与控制。例如，导丝作为“先锋”进行探索和路径软化，后续的导管机器人则利用导丝传递回来的路径和力信息，进行更高效安全的跟进，这需要解决多智能体之间的通信、避碰和任务分配问题。

3. 与术中实时影像深度融合：目前的感知主要依赖力觉，视觉信息利用不充分。下一代框架应能直接与术中OCT、IVUS或DSA的实时三维重建接口。将高分辨率的腔内影像提供的精确几何和斑块信息，与力感知提供的力学信息融合，构建一个“几何-力学”双模态的环境模型，将使规划和控制更加精准和智能。

这个基于接触感知的框架，其意义在于为血管介入机器人提供了一套“触觉智能”的底层范式。它让机器人从“盲人摸象”变成了“眼明手轻”的微创手术助手。从我个人的工程实践来看，最大的体会是：在医疗机器人领域，可靠性永远比先进性更重要。一个能在99%的时间里完美工作，但1%时间会出不可预测问题的系统，是绝对无法被临床接受的。因此，未来的工作重心，除了继续提升算法的性能边界，更要花大力气在系统的验证、验证、再验证上，通过海量的仿真、离体实验和严格的动物实验，去证明其在各种极端 corner case 下的安全性和鲁棒性。这条路很长，但每解决一个实际问题，都让我们离那个“更安全、更精准、更可及”的血管介入未来更近一步。