基于混合多阶段优化的双臂手术机器人协同运动控制方法

Abstract

In minimally invasive surgery, surgical instruments must enter the human body through channels with diameters of only a few millimeters to perform collaborative operations in narrow, unstructured spaces with uncertain morphologies. This presents challenges for real-time safe collaborative control due to the high risk of collisions and the high redundancy of the system. To address these challenges, a cooperative motion control method based on hybrid multi-stage optimization for ultra-minimally invasive dual-arm continuum surgical robots is proposed. First, the continuum manipulators and the environment were geometrically characterized by parametric spatial curves and voxel grid downsampling, respectively, to establish a collision detection model. Second, the Hybrid Multi-Stage Optimization with Dynamic Weight Adaptation (HMSO-DWA) algorithm was proposed. A dynamic weight objective function, based on minimum distance, was designed with a view to achieving an adaptive balance between the priorities of obstacle avoidance and task tracking. Additionally, an active safety intervention mechanism was introduced to execute strong avoidance while retaining weak target guidance under extreme risks. Simulation results showed that the collaborative task success rate reached 98.7% under dynamic obstacle interference, and the minimum inter-arm distance was maintained above 3.28 mm. Prototype experiments indicated that the tracking errors of the two arms were (0.444 ± 0.326) mm and (0.418 ± 0.273) mm, respectively. The proposed method effectively achieves safe collaborative control and trajectory tracking in constrained environments.

0. 引言

随着微创手术理念的发展，以经自然腔道内镜手术^[1-2]和单孔腹腔镜手术^[3]为代表的微创外科术式已成为现代医学的重要发展方向。然而，在经鼻颅底、经尿道等狭窄非结构化病灶空间中，手术通道直径通常仅为数毫米至十余毫米且形态复杂。传统刚性手术器械因自由度（degree of freedom，DOF）缺失，难以绕过解剖障碍直达靶区^[4]。连续体机器人凭借其柔顺性与灵巧性，在微创手术中展现出显著的应用潜力^[5-7]。然而现有的单臂连续体机器人^[8]尽管具备一定的柔顺性，仍难以独立完成组织牵拉切割、缝合打结等复杂的双手协同操作任务。为满足上述临床操作需求，近年来研究工作开始探索多自由度多臂连续体手术机器人系统^[9-12]。该类系统通过双臂协同操作，有提升微创手术中操作灵巧性与任务执行能力的潜力。然而，微创手术中的手术通道直径往往小于10 mm，使得双臂器械的工作空间高度重叠，臂间运动呈现出显著的强耦合特性^[13]，极易引发器械杆部之间的相互干扰与碰撞，即医学上所称的“筷子效应”^[14-15]。这不仅限制了有效手术三角区的建立，还可能因意外碰撞对周围脆弱软组织造成医源性损伤。

为克服避障与协同控制难题，现有研究主要采用人工势场法和数值优化法。例如，Ataka等^[16]提出了基于势场的实时规划器，Rodríguez-Seda等^[17]进一步给出了具备避障保证的控制律。此类方法计算效率较高，但本质上基于局部梯度下降，极易陷入局部极小值，导致路径震荡。Li等^[18]提出的零空间投影策略试图在主任务的零空间内处理避障任务，但在双臂高度耦合的受限空间中，系统冗余度将被迅速耗尽，导致避障任务与主任务发生冲突。基于模型预测控制（model predictive control，MPC）^[19]或数值优化方法^[20]能够处理多重硬约束，规划轨迹平滑度与轨迹跟踪精度较高^[21-22]。但在面对非结构化环境时，传统固定权重的代价函数难以在避障安全与轨迹跟踪精度之间取得平衡。尽管Qin等^[23]近期提出了一种基于软约束MPC与自适应滑模控制的级联策略，增强了单臂连续体机器人的鲁棒性与避障柔顺性，但该方法主要针对单臂系统设计，无法处理双臂系统复杂的臂间耦合约束，其级联动力学解算带来了显著的计算负荷。

另一方面，当前双连续体机械臂协同避障的研究多采用主从式策略^[18]，即将其中一个机械臂视为主臂优先执行任务，而将另一个机械臂视为动态障碍物进行被动规避，未能充分利用双臂系统的整体冗余自由度进行全局协同规划。在空间极度受限的环境中，从臂将因无法主动调整姿态配合而陷入“死锁”状态^[24]，导致系统无法找到可行解。

为此，本文提出一种基于混合多阶段优化的双臂连续体机器人鲁棒协同控制方法。该方法摒弃传统主从式策略，将双臂视为整体进行联合优化，通过构建动态权重优化框架并引入主动安全干预机制，实现在极度受限的非结构化手术空间内的臂间协同避障与高精度轨迹跟踪。

1. 双臂手术机器人建模

本文研究对象为一种具有12自由度的双臂连续体机器人。该机器人以同心推拉管状机械臂^[25]（concentric push–pull robots，CPPR）为基础构建，由两个结构对称的机械臂组成。每个臂包含外径为2 mm的近端段和外径为1.4 mm的远端驱动段，每段均具备独立的伸缩、旋转与弯曲自由度。如图1所示，系统整体通过直径为8 mm的外径套管进行部署。双臂结构参数如表1所示，左右臂物理参数一致。

图 1.

Dual-arm surgical robot structure and DOF illustration

双臂手术机器人本体结构与自由度

表 1. Parameters of continuum robots.

机器人本体参数

结构部件	参数	符号	数值	单位
近端段	外管外/内径		2.0 / 1.8	mm
	内管外/内径		1.7 / 1.5	mm
	切槽部分长度		15.2	mm
	伸缩量		[0, 12]	mm
	旋转量		[0, 2π]	rad
	弯曲量		[0, 1.4]	rad
远端段	外管外/内径		1.4 / 1.2	mm
	内管外/内径		1.1 / 0.9	mm
	切槽部分长度		17.8	mm
	伸缩量		[0, 12]	mm
	旋转量		[0, 2π]	rad
	弯曲量		[0, 2.1]	rad
系统整体	双臂基座间距		4.4	mm

1.1. 协同运动控制问题描述

为在离散时间域内实现双臂连续体机器人的高精度轨迹跟踪与主动避障，将协同控制问题转化为带约束的非线性优化问题，构建系统的状态空间模型及广义优化目标函数。设控制周期为Δt，当前离散时间步为k。定义系统的状态向量为双臂当前的构型，控制输入向量为双臂关节角速度：

1

其中，、分别为左臂和右臂的构型向量，表达为，包含近端与远端两段连续体的伸缩、旋转与弯曲变量。、则分别为左臂、右臂的构型向量的一阶导数，表示每个关节的速度。

协同运动控制的目标是在满足物理限位与几何安全约束的前提下，求解最优控制输入，使系统性能指标最优。由此，问题被定义成以下广义优化目标函数：

2

其中，、、分别为任务跟踪误差项、避障势能项、运动平滑项，、、分别为各子目标的权重系数，为臂间及臂与周围物体之间的最小距离，为安全距离阈值。约束项中，第一项约束限制了构型参数的实际范围，第二项约束表示驱动的速度不得超过最大速度限制，第三项约束则用于限制安全距离，以保证系统安全。

1.2. 运动学建模

为描述多段柔性连续体的空间形态，在此建立从关节驱动空间到末端任务空间的解析映射模型。假设每个柔性区段在受力变形时，其中心骨架线呈现为一段几何参数恒定的圆弧^[26-27]。对于单个臂的第i段（），定义可弯曲部分中心线弧长为。遵循文献[28]中CAST的几何约束原理，弯曲角与连续体机械臂的推拉位移驱动量存在线性映射关系，其中和为结构中性层偏移量。基于此，构建第i段相对于前驱坐标系的齐次变换矩阵，其表达式：

3

其中，为伸缩自由度产生的轴向平移变换，为由弯曲角与弧长决定的齐次变换矩阵，其完整表达式参见附件1.1。左右两臂末端执行器相对于机器人基准系的位姿矩阵、表示为:

4

其中，、分别为左臂和右臂的安装偏置矩阵。由此建立12维构型空间到笛卡尔任务空间的映射。

2. 双臂协同运动控制方法

本文提出一种面向非结构化环境的混合多阶段双臂协同运动控制框架，如图2所示。系统根据任务轨迹与传感器获取的环境信息建立机器人与障碍物的几何表征，其中机器人采用参数化空间曲线建模，环境采用体素网格降采样形成稀疏球体模型，并据此实时计算臂间及臂-障最小安全距离。该距离信息作为输入驱动动态权值自适应混合多阶段优化（Hybrid Multi-Stage Optimization with Dynamic Weight Adaptation，HMSO-DWA）控制器。根据系统安全状态在阻尼最小二乘法（damped least squares，DLS）、动态权重序列二次规划（sequential quadratic programming with dynamic weighting functions，SQP-DW）和主动安全干预三种模式之间切换，得到满足安全约束的构型控制指令，实现双臂连续体机器人的稳定协同运动。

图 2.

Framework of cooperative control method for dual-arm continuum robots based on hybrid multi-stage optimization

基于混合多阶段优化的双臂连续体机器人协同控制方法框架

2.1. 机器人与环境的几何表征

现有研究已能通过双目视觉^[29]、结构光^[30]、NeRF^[30]和3D高斯溅射^[31]等方法，从术中图像重建出高密度的软组织点云模型，因此本文假设手术场景的环境点云已知。为降低高维连续体系统在复杂环境下的碰撞检测计算复杂度，本文分别建立机器人的参数化连续曲线模型与基于体素网格降采样的环境稀疏球体模型。

2.1.1. 机器人几何表征

根据1.1节建立的运动学模型，单臂的几何形态由四个串联的几何基元组成：近端直线段、近端弯曲段、远端直线段及远端弯曲段。将各段均匀采用得到有序单臂骨架点集，具体过程见附件1.2。直接对离散点集进行最小距离计算需进行遍历搜索，计算复杂度随采样点数量增加而显著上升，难以满足实时优化控制的需求^[32-33]。本文提出了一种参数化连续曲线的几何建模方法。设左右臂的归一化弧长参数，采用最小二乘法对该点云进行多项式曲线拟合，得到机械臂的参数化空间曲线：

5

其中，为多项式系数向量，曲线参数t对应机械臂沿长度方向的归一化位置。由此将对连续体机械臂的整体空间形态由离散转化为平滑、可微的数值形式表示，从而降低计算维度。

2.1.2. 环境几何表征

对于环境感知模块输出高密度的原始点云，本文引入体素网格降采样^[34-35]，将非凸、不规则的障碍物点云转化为一组稀疏的包络球集合。设定体素尺寸V = 10 mm，将原始点云划分为若干体素簇，以各簇几何中心为球心、簇内最远点距离为半径，最终将复杂的环境点云稀疏化表征为包络球集合。具体计算过程详见附件1.3。

2.2. 碰撞检测与最小距离计算

在前文构建的参数化曲线模型与环境稀疏球体模型基础上，双臂协同系统的安全性评估可转化为最小距离度量问题。图3展示了本文的臂间和机械臂-障碍物之间的最小距离计算方法。

图 3.

Collision detection and nearest distance calculation

碰撞检测与最近距离计算

2.2.1. 最小距离度量

由式(5)所得的参数化曲线，左右两个连续体机械臂之间的距离函数及其梯度为：

6

定义双臂系统的完整关节构型向量。令距离梯度函数，即可求解得双臂最近点参数和，对应的最近点对[, ]及臂间最小距离可表示为：

7

由于参数化曲线为机械臂的中心线，因此当时，表明双臂发生物理碰撞，本文为1 mm。

而机械臂-障碍物最小距离被定义为全集骨架点到环境模型中所有包络球表面的最短距离：

8

其中，与分别为第个障碍物包络球的球心坐标与半径。当时，表示机器人已侵入障碍物内部。

2.2.2. 安全约束与可行域构建

由于模型误差、传感器测量误差、传动误差等误差的存在，引入系统安全阈值，无碰撞约束条件为：

9

基于式(8)，定义系统的无碰撞可行域为构型空间的子集：

10

其中，为系统的综合最小安全距离。

2.3. HMSO-DWA混合控制策略

HMSO-DWA依据实时解算的综合最小安全距离，在DLS、SQP-DW与主动安全干预三种模式间动态切换，采用基于一阶泰勒展开的惯性状态预测的结果作为初始解以加速收敛。

当系统处于绝对安全区域，即时，采用DLS梯度投影法求解逆运动学，利用零空间冗余度维持次级任务，具体控制律参见附件1.4。

当检测到机器人逼近障碍物或臂间距离接近危险状态时，即时，系统切换成SQP-DW求解。由于各优化项的量纲不一致，引入归一化因子，分别是位置跟踪容许误差常数和单步最大关节驱动量与对应关节最大行程的归一化比值。此时的优化目标函数为：

11

其中避障项展开为：

12

其中，为极限安全距离。

一般方法通常采用固定增益或单纯的距离反比函数，在机器人逼近障碍物时往往产生剧烈的斥力突变，导致末端执行器在目标点附近发生高频震荡。为此，本文引入了动态权重函数：

13

其中，与分别为避障权重的下限与上限，权重参数的选取基于量级匹配原则，用于在安全距离外维持避障趋势，用于确保在极限最小距离处避障项能够主导优化目标函数，从而在极度危险的情况下优先执行避碰。为归一化的非线性映射算子，采用三次多项式插值函数以保证权重在边界处的一阶连续性：

14

以上策略能够有效处理大部分受限空间内的避障问题，但在环境极其复杂或障碍物突发扰动的情况下，系统仍可能陷入两类极端状态：一是机器人逼近障碍物的程度超过极限安全阈值，即，导致常规优化问题的可行域为空；二是陷入局部极小值点，导致优化器无法生成有效的数值解。因此，当检测到极端风险或SQP-DW求解器陷入数值死锁时，系统立即挂起主任务，触发主动安全干预机制，控制器转而求解以安全为主目标的优化目标函数：

15

其中，第一项为归一化的强斥力势能，为单位参考安全常数，取1.0 mm。第二项为归一化的弱目标引导，确保在逃离碰撞的同时保留向目标方向的微弱梯度。系数≫，取 = 500， = 1，确保排斥力占据主导地位，强制将机械臂构型推离碰撞区，项仅提供微弱的引导作用，防止机械臂在过程中末端过度偏离目标。

为避免在模式切换边界产生振荡，引入滞后比较逻辑。设定模式切换的滞后阈值，取 = 0.2。当最小距离超过+后，系统才被允许切换回SQP-DW控制模式。这种滞后机制有效过滤了因传感器噪声或数值计算微小波动导致的控制策略在临界点处的高频抖动。

3. 实验与分析

3.1. 实验设置

为了全面评估所提方法的可行性，研究中所提算法的仿真验证与运行均在个人计算机平台上完成，硬件配置为Intel^® Core^TM i5-13500H（2.60 GHz）处理器，运行内存为16 GB，软件环境采用MATLAB R2023b，采用fmincon求解器。根据连续体最大直径为2.0 mm，并考虑运动学模型与传感器综合误差 ≤ 1.5 mm，极限安全阈值设定为2.0 mm，混合多阶段优化框架中的安全阈值设定为3.5 mm。SQP-DW优化求解器中任务跟踪权重 = 50，控制平滑权重 = 1，避障项的动态权重区间为[10, 200]。位置跟踪容许误差常数为0.1 mm，同时设定归一化单步驱动上限为0.3。各关节单步驱动量不超过对应最大行程的30%，可保证关节角速度低于额定驱动能力，有效抑制构型突变。仿真模型控制系统的周期设定为Δt = 30 ms。

3.2. 障碍物干扰下的鲁棒性仿真验证

在手术环境中，由于呼吸心跳等影响，解剖结构形态发生周期性变化，对机器人系统在保证安全前提下持续执行精细操作提出了更高要求。为了评估算法在应对外部环境不确定性时的鲁棒性，在仿真环境中引入了障碍物，设计了一个双臂交叉圆轨迹跟踪任务，该任务要求双臂末端分别沿两个空间相交的圆形轨迹运动，左、右臂的目标轨迹中心分别为[–3, 0, 35] mm和[3, 0, 35] mm，半径均为8 mm，且轨迹平面绕X轴倾斜30°。同时，在工作空间内设置了一组障碍物群，其初始中心位于[0, 15, 22] mm，包围椭球半轴长为[12, 5, 4] mm。

为了模拟非结构化环境中的不确定干扰，根据成年人静息状态正常呼吸频率为12～20次/分，设置障碍物群以0.25 Hz的频率沿Y轴和Z轴分别做幅值为3.0 mm和2.5 mm的正弦往复运动。

仿真结果如图4所示，在动态障碍扰动下，HMSO-DWA能同时保证轨迹跟踪与避障安全，左、右臂跟踪均方根误差分别为0.007 6 mm和0.007 0 mm，最大误差均不超过0.06 mm。同时，臂间最小距离保持在4.14 mm以上，臂-障最小距离始终大于8.72 mm，均高于设定安全阈值。结果表明，所提方法能够在动态干扰仿真环境下实现稳定的协同避障与高精度轨迹跟踪。

图 4.

Simulation of intersecting circular trajectory tracking under obstacle interference

障碍物干扰下的交叉圆轨迹跟踪仿真结果

a. 三维空间轨迹跟踪结果；b. 双臂末端位置误差曲线；c. 臂间及臂-障最小距离曲线

a. three-view 3D trajectory tracking results; b. end-effector position error curves of both arms; c. minimum inter-arm and arm-obstacle distance curves

3.3. 方法对比与分析

在实验3.2的基础上，设计了三种方法的对比实验，进一步验证所提算法的有效性。第一种以常用的数值优化方法（Levenberg-Marquardt，LM）为基线模型，构建包含位置误差和避障罚函数的非线性最小二乘问题迭代求解逆运动学。该方法的权重参数经调试设定为：位置跟踪权重为50，避障惩罚权重10，阻尼权重5，单步最大迭代次数设为50。第二种是文献[18]提出的基于零空间投影的梯度投影法。依据其算法特性，设置闭环控制比例增益，伪逆阻尼系数，势场激活阈值为7.0 mm，对应的斥力势场增益系数为40.0。第三种是本文所提出的方法。实验中定义无任何碰撞情况发生且轨迹跟踪最大误差不超过0.5 mm为成功实现任务。

本文设计了两组实验进行分析，第一组实验为交叉圆轨迹跟踪对比实验，评估三种方法在特定复杂工况下的轨迹跟踪能力、避障响应特性及运动平滑性，详细结果参见附件1.5。第二组实验为大规模随机轨迹蒙特卡洛测试，随机生成2 000组目标轨迹及障碍物，全面评估算法在非结构化环境中的泛化能力与鲁棒性。

表2为第二组实验的结果。HMSO-DWA在位置误差、安全性及任务成功率等指标上均取得了最优性能。HMSO-DWA在复杂动态环境下的任务成功率达98.7%。χ²检验结果表明，本方法的任务成功率显著优于LM算法（χ² = 750.65，P < 0.001）和文献[18]方法（χ² = 536.36，P < 0.001）。在安全性方面，HMSO-DWA的最小臂间距离达到（3.28 ± 1.19）mm，最小臂-障距离为（8.38 ± 2.68）mm，均优于另外两种方法，表明其在应对动态干扰时具有更强的避险能力。在实时性上，HMSO-DWA的单步耗时为（0.013 1 ± 0.005 3）ms，按3σ准则最大耗时约为0.029 s，计算满足实时控制要求。同时，仿真环境中误差控制在（0.02 ± 0.01）mm。综上，本方法在兼顾安全性与精度方面具有优势。

表 2. Performance comparison of large-scale simulation.

算法大规模仿真测试性能对比

方法	/mm	/mm	/mm	每步耗时/s	成功率
LM	0.02 ± 0.01	2.12 ± 1.49	7.60 ± 3.62	0.017 2 ± 0.001 6	65.4%
文献[18]	0.03 ± 0.01	2.62 ± 1.44	7.36 ± 3.56	0.002 1 ± 0.000 2	73.2%
本文方法	0.01 ± 0.01	3.28 ± 1.19	8.38 ± 2.68	0.013 1 ± 0.005 3	98.7%

3.4. 双臂手术机器人样机实验

为验证所建立运动学模型在物理系统中的有效性及所提算法的臂间避障能力，搭建了如图5所示的双臂手术机器人实验平台。实验平台包含个人计算机、双臂手术机器人系统、近红外光学定位系统（OP‑M620，深圳艾目易科技，中国）、光学标记球。光学定位系统定位精度为0.12 mm（RMS）。考虑到当前实验平台尚未集成实时的三维环境感知传感器，本节主要在静态环境下验证双臂系统的运动学精度以及双臂自身互为障碍物时的协同避碰性能。

图 5.

Experimental platform of the dual-arm surgical robot

双臂手术机器人实验平台

实验任务设定为双臂协同跟踪同心圆弧轨迹，目标轨迹参数如下：左臂目标轨迹为半径8 mm的圆弧，右臂目标轨迹为半径14 mm的圆弧，圆心均位于[0, 0, 32] mm处，圆弧圆心角范围为。实验过程中，本研究将算法解算得到的双臂协同控制的关节指令序列下发至底层运动控制器驱动机械臂运动，利用双目近红外视觉定位系统实时捕捉机械臂末端示踪标记点的三维空间坐标。实验中，双臂机器人完成轨迹跟踪的同时全过程未发生臂间干涉与构型突变，仿真构型与物理样机实际执行状态的对比参见附件1.6，运动过程视频参见附件2。

图6展示了物理样机协同运动时轨迹跟踪实验结果。由于系统存在制造装配误差、光学定位系统测量误差以及连续体结构的迟滞效应，跟踪轨迹与理论轨迹存在一定偏差，但机械臂实际运动路径与理论目标轨迹在几何形态上保持较好的一致性。经统计分析，左臂、右臂轨迹跟踪误差分别为（0.444 ± 0.326）mm和（0.418 ± 0.273）mm。Shapiro-Wilk 检验显示两组误差均不服从正态分布，误差中位数分别为0.375 mm和0.391 mm。误差累积分布结果显示，95%误差界限分别为0.766 mm和0.831 mm，均在1 mm以内，表明本文方法在物理样机上具备稳定的轨迹跟踪性能。左臂在运动过程中出现的误差极值，主要原因是在建模时未考虑连续体机器人内外管之间的非线性摩擦的影响，导致末端出现轨迹偏离。

图 6.

Experiment results of prototype

样机实验结果

在先前的研究中，文献[36]的同心管连续体机器人平均定位误差为（1.2±0.8）mm。末端跟踪中值误差相对于机器人长度百分比上，文献[37]在三节段连续体机器人样机实验中为2.59%，文献[38]为3.3%，而本研究为1.18%。结果显示，本文双臂系统的连续体机器人跟踪精度均优于上述同类研究，误差处于临床微创手术可接受的误差范围内^[39]。并且，在实际临床应用中误差可通过医生主从操作进行实时补偿^[40-41]。此外，本研究还分析了速度曲线，以进一步验证系统的动态稳定性，相关介绍参见附件1.7。

4. 结束语

针对双臂连续体机器人在非结构化环境中面临的臂间强耦合避障及多臂协同控制难题，本文提出了一种混合多阶段优化协同运动控制方法。该方法首先通过机器人离散骨架与环境稀疏化建模，大幅降低了碰撞检测的计算复杂度；进而利用基于实时最小距离的动态权重自适应机制，实现了避障优先级与任务跟踪精度的动态平衡；同时引入主动安全干预机制，有效解决了传统优化算法易陷入局部极小值及数值死锁的问题。

本文所提出的方法能够适应微创手术中常见的狭窄通道和高度重叠的双器械工作空间，为双臂连续体手术机器人在复杂解剖环境中的安全协同操作提供了一种可行的控制框架。仿真实验表明，在存在外部干扰的复杂环境下，所提方法相较于基线方法，协同任务成功率提升至98.7%，在保证轨迹跟踪精度的同时，能够实时响应动态障碍物干扰，展现出较好的鲁棒性。物理样机实验验证了运动学模型的准确性及双臂在受限空间内的协同运动能力，证明了算法在实际系统部署的可行性。受限于当前实验平台尚未集成实时的环境感知传感器，本文的动态避障性能主要在仿真环境进行验证，未来的研究工作将引入环境感知系统，实现非结构化环境下的闭环动态避障验证。此外，当前研究仍主要依靠模型进行控制，尚未充分考虑软组织接触力、管间摩擦等非线性因素的影响。未来的研究工作将引入光纤光栅传感器、力传感器与电磁传感器等实现多模态感知下的闭环控制，构建面向真实非结构化手术环境的闭环协同控制系统，并进一步探索其在实际临床场景中的应用潜力。