硕士学位论文

微创手术机器人实时系统构建
与主从控制策略

REAL-TIME SYSTEM DEVELOPMENT AND

CONTROL STRATEGY FOR MINIMALLY
INVASIVE SURGICAL ROBOT

（工程型）

曲效锋

哈尔滨工业大学
2016 年 6 月

万方数据
 国内图书分类号：TP24 学校代码：10213
国际图书分类号：681.5 密级：公开

工程硕士学位论文

微创手术机器人实时系统构建
与主从控制策略
（工程型）

硕 士 研 究 生：曲效锋
导 师：王树国 教授
申 请 学 位：工程硕士
学 科：机械工程
所 在 单 位：机电工程学院
答 辩 日 期：2016 年 6 月
授 予 学 位 单 位：哈尔滨工业大学

万方数据
 Classified Index: TP24
U.D.C: 681.5

Dissertation for the Master Degree in Engineering

REAL-TIME SYSTEM DEVELOPMENT AND

CONTROL STRATEGY FOR MINIMALLY
INVASIVE SURGICAL ROBOT
(Engineering)

Candidate： Qu Xiaofeng

Supervisor： Prof. Wang Shuguo

Academic Degree Applied for： Master of Engineering
Speciality： Mechatronics Engineering

Affiliation： School of Mechatronics Engineering

Date of Defence： June, 2016
Degree-Conferring-Institution： Harb in Institute o f Tec hno logy

万方数据
 哈尔滨工业大学工程硕士学位论文

摘 要
微创手术具有创面小、痛感轻和术后恢复时间短等优点，得到了广泛的临床
应用。但也存在手眼不协调、灵活性不足等问题。由机器人技术与微创技术结合
的微创手术机器人系统能够克服这些缺陷，显著地提高手术效率与效果。
于临床中应用微创手术机器人系统，对系统的实时性和安全性要求很高。而
且手术机器人系统需要实现手术器械跟随医生手部以相同的趋势运动，使得医生
能够手眼协调操作。本文基于 QNX 系统构建了微创手术机器人的软硬件框架，
并研究了内窥镜视野下基于笛卡尔空间的主从一致性增量控制策略，从而构建了
完整的控制系统。进行了多种实验，检验其实时性能和控制策略的有效性。
采用分布式 I/O 与处理的控制系统结构，确保系统各部分的实时性能能够满
足系统整体的需要。将 QNX 系统移植到上位机中，并搭建起各部分之间的通信
网络。然后按照层级结构设计应用软件系统，根据功能划分任务，确定实时性任
务的结构和优先级。然后分析了系统的实时性能，结果表明系统实时性能优秀。
针对机器人主从异构的特点，研究了内窥镜视野下基于笛卡尔空间的主从一
致性增量控制策略。构建了机械臂 D-H 参数模型，并根据机械臂的结构特点，求
出了机械臂的逆运动学解析解。研究了内窥镜视野下的主从映射方法，并采用视
觉测量技术快速准确地测出机械臂之间的姿态关系。为了提升系统的使用性能，
开发了主从术中调整、主手抖动滤除和操作安全限制等主从控制功能。
针对所构建的控制系统开展了实时性能比较实验、主从控制实验和活体动物
实验。实验结果表明，从手机械臂能够快速及时地跟随主操作手运动，具有良好
的实时性能；控制系统能够使得操作者手眼协调、直觉地完成各种手术操作；主
从控制功能提高了机器人系统的稳定性、灵活性和安全性。实验的开展也为微创
手术机器人在临床中应用奠定了坚实的基础。

关键词：微创手术机器人；实时性能；主从控制；QNX 系统

万方数据
 哈尔滨工业大学工程硕士学位论文

Abstract
Minimally invasive surgery has advantages of small wound, less pain and shorter
recovery time. It has been widely performed in clinical application. But there are
drawbacks such as lack of hand-eye coordination and flexibility. Robot-assisted
minimally invasive surgery system produced by the combination of robotics and
minimally invasive techniques can overcome these drawbacks, and improve the
operation efficiency and effectiveness significantly.
Minimally invasive surgical robotic systems are used in clinical application, which
claims high real-time and safety requirements. And surgical robot system needs to
achieve that surgical instruments follow doctor hands in the same movement trend,
which guarantees the hand-eye coordination. In this paper, software and hardware
framework of minimally invasive surgical robot is built based on the QNX real-time
system, and the master-slave consistency incremental control strategy in Cartesian space
is studied. These two parts form a complete control system. Finally, a variety of
experiments are carried out on the control system to verify the effectiveness and
performance of real-time control strategy.
The distributed I/O and processing control system architecture is adopted. It has to
be ensured that the real-time performance of each part of the system must meet the
needs of the whole system. QNX system is ported to the IPC, and a communication
network among various parts is built up. The application software system is designed
based on the hierarchy architecture. According to various functions, the system is
partitioned into tasks. Prioritize real-time tasks and define the structure of them. The
real-time performance of the system is analyzed with rate monotonic analysis method.
The results show that the control system has good enough real-time performance to
meet the requirements of minimally invasive surgery.
For the master-slave heterogeneous structural characteristics of the surgical robot
system, the master-slave consistency incremental control strategy in Cartesian space is
studied. A D-H parameters model of slave robot arms is established, and the analytical
solution of inverse kinematics is obtained according to structural characteristics of the
arm. And the master-slave consistent mapping method in endoscope perspective is
studied. The orientation relationship between arms is measured quickly and accurately
with the monocular vision technology. In order to improve the perfo rmance of surgical
robot system, intraoperative master-slave adjustment, the master Tremor filtering and
intraoperative virtual fixture are developed as master-slave control functions.
The real-time performance comparison experiment, master-slave control
experiment, suture and knot experiments and surgery on living animal are carried out.

II

万方数据
 哈尔滨工业大学工程硕士学位论文

Experiment results show that the slave robot arm could follow the motion of master
manipulator with good real-time performance; control system could enable the operator
intuitively complete a variety of surgical procedures in hand-eye coordination; master-
slave control functions improve the stability, flexible and security of the robotic system.
The experiments have laid a solid foundation of the clinical application with minimally
invasive surgical robot.

Keywords : minimally invasive surgical robot, real- time performance, master-slave

control, QNX operating system

III

万方数据
 哈尔滨工业大学工程硕士学位论文

目 录

摘 要 ................................................................................................................................. I
Abstract ............................................................................................................................. II
第 1 章 绪论...................................................................................................................... 1
1.1 课题来源 .......................................................................................................... 1
1.2 课题研究背景及意义 ...................................................................................... 1
1.3 国内外研究现状 .............................................................................................. 3
1.3.1 国外研究现状 ........................................................................................ 3
1.3.2 国内研究现状 ........................................................................................ 6
1.3.3 国内外研究现状简析 ............................................................................ 8
1.4 主要研究内容 .................................................................................................. 9
第 2 章 基于 QNX 的实时控制系统构建 ..................................................................... 11
2.1 控制系统结构设计 ........................................................................................ 11
2.1.1 控制系统结构确定 .............................................................................. 11
2.1.2 控制系统结构实现方案 ...................................................................... 12
2.2 基于 QNX 的硬件系统构建.......................................................................... 15
2.2.1 QNX 系统裁剪和移植......................................................................... 16
2.2.2 控制系统通信网络构建 ...................................................................... 18
2.3 基于 QNX 的软件系统构建.......................................................................... 20
2.3.1 软件系统层级结构设计 ...................................................................... 20
2.3.2 任务划分与优先级分配 ...................................................................... 21
2.3.3 任务循环结构与任务间通讯 .............................................................. 24
2.4 控制系统实时性能分析 ................................................................................ 27
2.5 本章小结 ........................................................................................................ 28
第 3 章 主从控制策略与方法研究................................................................................ 29
3.1 主从控制策略 ................................................................................................ 29
3.1.1 微创手术机器人结构配置介绍 .......................................................... 29
3.1.2 笛卡尔空间内的增量控制策略 .......................................................... 30
3.2 从手机械臂运动学求解 ................................................................................ 31
3.2.1 从手机械臂运动学正解 ...................................................................... 31
3.2.2 从手机械臂运动学逆解 ...................................................................... 33
IV

万方数据
 哈尔滨工业大学工程硕士学位论文

3.3 主从一致性映射方法 .................................................................................... 36

3.3.1 内窥镜视野下主从一致性映射方法 .................................................. 36
3.3.2 机械臂间姿态角视觉测量 .................................................................. 38
3.4 主从控制功能实现 ........................................................................................ 42
3.4.1 主从术中调整 ...................................................................................... 42
3.4.2 主手抖动滤除 ...................................................................................... 44
3.4.3 操作安全限制 ...................................................................................... 46
3.5 本章小结 ........................................................................................................ 49
第 4 章 微创手术机器人实验研究................................................................................ 50
4.1 微创手术机器人系统集成 ............................................................................ 50
4.2 控制系统实时性能对比实验 ........................................................................ 52
4.2.1 运行时间对比实验 .............................................................................. 53
4.2.2 延迟时间对比实验 .............................................................................. 54
4.3 基于 QNX 系统的主从控制实验.................................................................. 56
4.3.1 主从轨迹对比实验 .............................................................................. 56
4.3.2 抓取和套圈实验 .................................................................................. 57
4.4 活体动物实验 ................................................................................................ 58
4.5 本章小结 ........................................................................................................ 60
结 论 .............................................................................................................................. 61
参考文献 ........................................................................................................................ 63
攻读硕士学位期间发表的学术论文及其他成果 ........................................................ 67
哈尔滨工业大学学位论文原创性声明和使用授权说明 ............................................ 68
致 谢 .............................................................................................................................. 69

万方数据
 哈尔滨工业大学工程硕士学位论文

第1章 绪论

1.1 课题来源

本课题来源于国家高技术研究发展计划（863 计划）项目《微创医疗机器人》
的相关课题《腹胸腔微创手术机器人共性关键技术与示范应用》（课题编号：
2012AA041601）。

1.2 课题研究背景及意义

微创外科手术是指通过单个或多个微小的创口，将内窥镜和特制手术器械等
送入人体内部腔体，对体内病变或创伤进行切除或修复等的外科手术。医生在视
觉反馈系统的引导下操纵器械完成手术。视角的调整由助手辅助完成。传统开放
式手术与微创手术的对比图如图 1-1 所示。

图 1-1 传统开放式手术与微创手术对比图

由图中可以看出，微创外科手术具有创面更小、痛感较轻和术后恢复时间短
的优点，故而得到了广泛的临床应用。但微创手术也存在缺陷。比如手术视野和
灵活性受到限制、存在“杠杆效应”（手眼不协调）等。由机器人技术和微创技
术结合产生的机器人辅助微创手术系统很大程度上克服了这些缺陷。微创手术机
器人系统可以：1）实现手眼协调，增强手术操作者的沉浸感和直观感；2）增强
手术操作的灵活性；3）提升手术操作的精确度和可靠性；4）提供三维立体视觉
和力反馈；5）实现远程操作；6）减轻操作者的工作负荷[1]。基于上述优点，微
创手术机器人系统获得了广泛的认可和长足的进步。
1

万方数据
 哈尔滨工业大学工程硕士学位论文

因为微创手术机器人系统要应用于临床操作，所以对系统安全性和实时性的
要求很高。任何无法及时完成的操作都被认为是危险的，甚至是致命的[2]。伴随
着航天技术的发展，计算机科学领域诞生了实时操作系统。实时操作系统是指当
外界事件发生或接收到数据时，能够及时进行处理，处理结果能够及时输出，并
使得实时任务协调运行的操作系统[3]。控制系统的设计者可以定义实时任务与非
实时的任务，确定任务的优先级，抢占性地剥夺系统资源，保证系统的实时运行。
将实时操作系统应用于微创手术机器人，可以保证手术操作被及时地响应和执行，
以此确保对手术过程的实时控制[4]。与传统的控制系统相比，实时系统的及时性
和可靠性完全能够满足临床的要求。
微创手术机器人绝大多数都使用主从形式进行设计。以目前商业上最成功的
达芬奇系统[5]为例，该系统主要由主控台、机械臂和视觉成像系统三部分组成，
其结构图如图 1-2 所示。

图 1-2 达芬奇手术系统

操作者坐在主控台前，根据视觉反馈操控主手，从手在控制系统的控制下跟
随复现操作者的手部动作[6]。主控台可以根据需要远离或者靠近手术台。这种主
从控制的方式显著增强了手术操作的精确性和可靠性，更为重要的是，可以实现
手眼协调，增强手术的沉浸感和直观感[7]。传统微创外科手术因为存在“杠杆现
象”，手术器械与医生手部动作方向相反，手眼不能协调配合。需要医生长期的
专业训练和临床经验，通过在医生脑中建立相应的映射，才能克服这一缺陷。这
明显增大了手术的难度和成本。采用主从形式的手术机器人系统，在主从控制算
法中建立起相应映射，控制从手机械臂和手术器械跟随复现医生手部的运动，实
现了手眼协调运动，降低了手术操作的难度。可见主从控制策略和方法对微创手
术机器人的研发有着十分重要的意义。而且，在抢险救灾、军事、极限环境探测
2

万方数据
 哈尔滨工业大学工程硕士学位论文

等方面，采用主从控制方式的机器人正得到越来越广泛的应用。微创手术机器人
的主从控制策略和方法对其有着显著的指导和借鉴意义。因此，主从控制策略和
方法的研究有着广泛的应用前景。另外为了满足手术过程中的需求，微创手术机
器人基本都具有手部抖动滤除、主从术中调整和操作安全限制等主从控制辅助功
能。这些功能能够极大地提升机器人的使用性能。

1.3 国内外研究现状

达芬奇手术机器人系统获得了巨大的成功，促使国内外很多研究机构开始竭
力研发新型的微创手术机器人，旨在性能上超越达芬奇系统。医疗领域已经成为
机器人研究的热门领域之一。

1.3.1 国外研究现状

对于微创手术机器人，控制系统的构建和主从控制策略的运用都是核心内容。
对于主手抖动滤除、主从术中调整和操作安全限制等辅助功能，虽然各种机器人
的功能性能大体相同，但具体的实现方式也各有特点。
达芬奇手术机器人系统作为目前商业上最为成功的产品，其参数严格保密，
所以对其的了解并不全面。达芬奇系统采用集中式处理和分布式 I/O 的控制架构
[8] 。该架构建立在实时高速通讯网络上，可以将几乎所有的控制计算集中在高性

能主机上实现，同时不增加主机的 I/O 负担。主机上安装实时操作系统。使用

FPGA 作为主机与电机和其他 I/O 硬件通讯的接口。其控制系统可以划分成五个
功能层级，分别是硬件 I/O 接口层、低级控制层、高级控制层、远程操作层和应
用层。软件系统结构图如图 1-3 所示。

图 1-3 达芬奇机器人软件系统结构图
3

万方数据
 哈尔滨工业大学工程硕士学位论文

硬件 I/O 层负责直接获取通讯总线上的原始 I/O 数据。低级控制层主要包含

PID 关节控制器等。高级控制层提供运动学正逆解、轨迹生成等函数抽象。远程
操作层提供主从映射的功能。应用层提供人机交互接口。这种层次结构分工明确，
简化了软件系统设计流程，同时方便之后的维护与升级。
整个系统的主控频率可以达到 1kHz 以上，具有极高的实时性与安全性。达
芬奇机器人系统采用“直观操作”的概念进行设计。其手术器械对主手的复现能
力极强，可见其主从控制策略是很成功的。另外手部抖动滤除、主从术中调整和
操作安全限制等控制功能也是达芬奇系统首先提出的，其功能也最接近临床需要。
德国宇航中心（DLR）开发了一款多用途手术机器人 MIRO [9]。该系统经过
专门的轻量化设计，具有简洁轻巧的特点，可以直接安装在手术台上。MIRO 手
术机器人系统外观图如图 1-4 所示。

图 1-4 MIRO 手术机器人系统外观图

该系统采用瑞士 Force Dimension 公司的 Sigma.7 触觉产品作为主手输入设备

[10] 。该系统能够提供力反馈功能，显著地提高了机器人的使用性能。MIRO 手术
机器人的控制软件系统也采用层级结构，主要分为四层，分别是关节空间控制层、
笛卡尔空间控制层、远程操作控制层和多机械臂协作控制层[11]。其软件系统结构
图如图 1-5 所示。

图 1-5 MIRO 机器人软件系统结构图

4

万方数据
 哈尔滨工业大学工程硕士学位论文

其中关节空间控制层负责控制关节的位置和转矩，笛卡尔空间控制层负责计
算机械臂的运动学和动力学。远程操作控制层提供主从映射抽象。多机械臂协作
控制层则将内窥镜视野下的主从运动映射关系考虑进来。这样的分层结构可以将
软件根据运行时需要划分成不同的模块，而且可以提供不同层次的抽象，明确软
件开发者的团队分工。
MIRO 机器人采用分布式的控制系统结构，其结构如图 1-6 所示。

图 1-6 MIRO 分布式系统结构图

六台安装有 QNX 系统的 PC 电脑组成分布式控制系统，之间的通讯网络采用

aRDnet 实现。控制软件在 MATLAB/Simulink 中进行开发，代码由 Real Time
Workshop 工具箱自动生成，这些代码编译后可以直接在 QNX 系统上运行。由于
MICA 手术器械[12]的力反馈采样时间较长，所以系统的主控频率在 100Hz 左右。
意大利圣安娜高等学校的 Marta Niccolini 等人研发了一种新型的单孔内窥镜
手术机器人 SPRINT[13]。与传统的微创手术机器人不同，这种机器人只在患者的
腔壁上开一个孔，并通过该孔把两条机械臂和内窥镜伸入患者体内，从而避免了
过多的切口。甚至可以利用之前的伤疤作为切口，有利于术后的美观。这种机器
人造型小巧，因而价格也较为低廉。其外观图如图 1-7 所示。

图 1-7 SPRINT 手术机器人外观图

5

万方数据
 哈尔滨工业大学工程硕士学位论文

其控制系统采用“上位机-下位机”的模式[14]。上位机采用高性能的 PC 机，
安装 Linux 的实时扩展版本 RTAI（Real Time Application Interface），软件系统在
MATLAB/Simulink 软件中开发，代码由 Real Time Workshop 模块自动生成。上位
机的采样频率可达 1KHz。在从手端的下位机负责以位置模式控制伺服电机。将
笛卡尔空间的轨迹通过机械臂逆运动学变换为关节空间的轨迹，然后使用 PID 控
制器控制关节跟随轨迹运动。针对其机械臂的结构特点，其运动学逆解采用雅可
比矩阵伪逆的微分方程进行计算，同时采用了带阻尼的最小方差滤波器来保证在
奇异点附近的数值稳定。其控制系统简图如图 1-8 所示。

图 1-8 SPRINT 手术机器人控制系统简图

加州大学圣克鲁斯分校和华盛顿大学的团队开发了 Raven 手术系统[15]，其最

初的设计目标是在战场上为受伤的士兵提供及时的治疗，所以其设计轻巧，并且
能够应对野外手术的恶劣条件。其运动学逆解可求得解析解。慕尼黑工业大学的
C. Staub 等人研发了一款新型的可以安装在天花板上的手术机器人[16]。其控制系
统也采用分布式的结构，共由三台 PC 机组成。一台负责控制与仿真，一台负责
视觉工作，最后一台负责 CAN 总线通信与传感器数据采集。卡耐基梅隆大学的
Stephen Tully 等人研发了一款蛇形手术机器人 HARP[17]，并提出了一种可以估计
形状和末端位置姿态的滤波算法。该系统可以生成更加精确的三维渲染图像，更
有利于引导手术。罗马尼亚克纳波卡技术大学的 Bogdan Gherman 等人开发了
PARASURG5M 混合并联机器人[18]，并基于动态等效集总质量构建了一种新的逆
动力学模型。

1.3.2 国内研究现状

国内的微创手术机器人研究开始较晚，但近几年得到了较快的发展。在国家
的大力支持下，各科研单位相继研制出了多款新型的手术机器人，各自在控制系
统结构与主从控制策略上都有一定的创新。
天津大学的王树新教授团队开发了三代显微外科手术机器人 Micro Hand I、II、
III，并在其基础上研制了 Micro Hand A 手术机器人系统[19]，如图 1-9 所示。
6

万方数据
 哈尔滨工业大学工程硕士学位论文

a) Micro Hand I b) Micro Hand II c) Micro Hand A

图 1-9 Micro Hand I、II、A 手术机器人

鞠浩等人为 Micro Hand A 系统设计了以 DSP 为核心，以 FPGA 为协处理器

的高性能嵌入式控制器，使得主从控制频率可达 300Hz 以上[20]。其控制软件系统
包括人机界面层、核心控制层和设备驱动层三层[21]。人机界面层负责操作者与机
器人的交互，核心控制层包括主从控制算法、同步控制等。设备驱动层是核心控
制层与电机等 I/O 器件的接口，负责通讯与底层控制。主从控制策略方面，先通
过逆雅可比矩阵将笛卡尔空间中的速度转化为各关节空间中的速度，再将瞬时位
移用瞬时关节速度替代，求出运动学逆解[22]。该机器人操作运动灵活，可实现切
除、缝合和打结等多种手术操作。而且也具备有主手抖动滤除、主从术中调整和
操作安全限制等主从控制功能。整个机器人系统稳定性高，安全性较好，已经进
行了多例动物实验。
哈尔滨工业大学机器人研究所国家重点实验室研发出了多款微创手术机器人。
如图 1-10 所示。

(a) (b)
图 1-10 哈尔滨工业大学研制的多款手术机器人

(a)图所示手术机器人采用“工控机+运动控制卡”的方式进行控制。其软件
控制系统分为三层，分别是应用层、实现层和底层[23]。将每个层次的控制功能封
装进不同的类中。每个大类又可以分为许多小类。这样就将整个系统模块化，有

万方数据
 哈尔滨工业大学工程硕士学位论文

利于软件的维护与升级。用逆雅可比矩阵求运动学逆解，减小了主从控制周期，
提升了系统实时性能[24]。(b)图所示机器人根据其结构特点，采用位姿分离的方式
进行主从控制，简化了控制算法，但是精度上有所损失，需要进行实时补偿。该
机器人系统采用增量控制的方式解决主从异构的问题[25]，取得了良好的效果。
上海交通大学的唐奥林等人研制了“神刀华佗”手术机器人[26]，并根据其结
构特点，提出了从手机械臂运动学逆解的快速解法。但是该算法存在固有误差，
需要进行实时补偿以保证运动精度。并针对该机器人系统，对基于手眼协调原则
的主从映射关系进行了研究，提出了主操作手布置方式的要求，并在 Open HRP3
开源平台上进行了虚拟仿真实验[27]。哈尔滨工程大学的于凌涛等人对内窥镜下多
机械臂系统的运动算法与轨迹规划进行了研究，针对其机器人系统的特点，提出
了一种新的内窥镜视野下主从映射控制策略[28]。

1.3.3 国内外研究现状简析

对于微创手术机器人的控制系统构建，国内外都进行了深入的研究。
在控制系统结构方面，主要有集中式 I/O 与处理、分布式 I/O 与处理以及集
中式处理与分布式 I/O 三种形式。集中式 I/O 与处理这一结构因为对主机的处理
和通信能力要求过高，已经很少被采用。分布式 I/O 与处理结构随着实时高速通
信协议与分布式计算技术的发展而逐渐流行。其对于处理器的要求较低，有利于
小型化与智能化。集中式处理与分布式 I/O 这一方式较为新颖，得以发挥高性能
计算机的处理能力，将高级和低级的控制算法在同一开发环境中实现，从而增大
控制算法的灵活性。
在控制系统实时性能方面，国外十分重视相关工作。绝大多数控制系统基于
Linux 扩展实时系统或者 QNX 实时系统进行构建。Linux 扩展实时系统因为其开
源的特性而倍受青睐，能够明显降低系统成本。但是由于其内核架构的问题，很
难实现硬实时[29]。而且很多实时扩展都不支持 POSIX 标准，给软件的开发和移
植带来了难度。QNX 系统响应时间短，可靠性高[30]。全面符合 POSIX 标准，能
够提供完全地址空间保护。采用微内核结构，能在线对软件模块进行扩展或卸载。
其缺点是使用费用较高。反观国内，大多数机器人系统还处在功能实现阶段，使
用 Windows 或非实时的 Linux 系统，对于控制系统实时性能研究较少。
在软件系统结构方面，绝大多数系统都采用层级结构。这样可以将软件根据
需要划分成不同的模块，将复杂问题进行分解，同时可以提供不同层次的抽象，
明确软件开发者的团队分工。而且采用层级结构也有利于软件的后续维护和升级。

万方数据
 哈尔滨工业大学工程硕士学位论文

对于主从控制策略与方法，国内外的不同团队针对其机器人的结构和控制特
点采取了不同的方案。绝大多数手术机器人的主手与从手的关节配置与结构参数
都不同，即主从异构。这样的结构形式决定了主从控制不能简单地在关节空间内
进行一一对应，而需要在笛卡尔空间内根据手眼协调准则进行主从映射。
将关节空间与笛卡尔空间进行联系的是机械臂的运动学。绝大多数手术机器
人的机械臂都采用六自由度串联的形式，其正运动学求解较为容易。但是运动学
逆解需要求解六维非线性方程组，难度较大。针对机械臂的结构特点，主要有解
析法、逆雅可比矩阵线性化和位姿分离近似求解三种方法。解析法求逆解，快速
而准确，有利于系统的实时控制。然而其消元的过程较为复杂，不符合 Pieper 准
则 [31] 的机械臂很难运用解析法。逆雅可比矩阵线性化求解是一种适用于不符合
Pieper 准则机械臂的数值解法。但是因为用速度代替瞬时位移存在误差，而且误
差逐渐累积，所以需要进行在线实时误差补偿。位姿分离近似求解是利用机械臂
末端手术器械的各关节尺寸较小的特点，求出近似解析解再进行误差补偿的方法。
但是其误差补偿方法缺少理论支撑，难以得到较好效果。
因为医生要在内窥镜视野下操作手术器械进行手术，所以笛卡尔空间中的主
从映射需要考虑内窥镜视野下的手眼协调。这涉及到内窥镜持镜臂的位置和姿态、
主操作手的摆放形式以及显示屏相对于操作者的位姿关系等等。不同的手术机器
人系统根据其设计需求采用了不同的持镜臂和主操作手配置方式，从而其内窥镜
视野下的主从映射策略也不尽相同。
另外，为了提升手术机器人的实用性能，微创手术机器人系统大多具有医生
手部抖动滤除、主从术中调整和操作安全限制等主从控制功能。
国内现有的研究成果，主要注重于功能的实现，对控制系统的实时性能关注
较少，更缺少从实时性能角度出发进行控制系统结构、软件结构和控制策略设计
的研究。因此，拟基于 QNX 实时操作系统构建机器人的实时控制系统，并根据
机器人的结构和配置特点，提出内窥镜视野下的主从控制策略和方法。为了提高
机器人的使用性能，拟开发主手抖动滤除、主从术中调整和操作安全限制等主从
控制功能。最终实现机器人优良的实时性能和使用性能。

1.4 主要研究内容

实时控制系统和主从控制策略是微创手术机器人控制的核心，是机器人能够
快速、便捷地完成手术，体现机器人辅助医疗巨大优势的重要保证。根据以上对
研究问题和现状的了解与分析，为了从实时性角度出发，全面构建机器人的控制

万方数据
 哈尔滨工业大学工程硕士学位论文

系统，提高其使用性能，并最终走向临床，本文将在以下几个方面进行深入研究。
（1）基于 QNX 系统的实时控制系统构建
基于微创手术机器人的性能要求和使用特点，在 QNX 系统的基础上构建实
时控制系统。充分利用 QNX 系统提供的各种实时性功能与服务，从实时性的角
度出发，从整体上对控制系统结构、软件系统结构和软件模块实现进行设计。设
计控制系统整体结构，保证各个部分都能达到对实时性能的要求。然后对 QNX
系统进行裁剪和移植，搭建通信网络，将各种外部设备通过不同的方式进行连接，
从而构建出控制系统的硬件部分。对软件系统结构进行设计，完成任务划分与优
先级分配、任务结构确定与任务间通信等内容。最后分析系统的实时性能。
（2）主从控制策略与方法研究
在构建了实时控制系统之后，需要根据机器人的结构和配置特点，确定内窥
镜视野下的主从控制策略与实现方法，从而实现医生的手眼协调操作。对手术机
器人的主从异构特点进行分析，提出内窥镜视野下基于笛卡尔空间的增量控制策
略。研究机器人运动学，实现关节空间与笛卡尔空间之间的转换。对微创手术手
眼一致原则进行分析，提出内窥镜视野下的主从映射方法。为了提升手术机器人
的使用性能，开发主手抖动滤除、主从术中调整和操作安全限制等主从控制功能。
操作者手部抖动的实时滤除可以明显提升手术精度；主从术中调整功能可以解决
主从工作空间不对应的问题。操作安全限制功能可以辅助手术高效进行。
（3）微创手术机器人实验研究
在完成实时控制系统构建与主从控制策略和方法的研究后，首先进行手术机
器人系统集成，然后将 QNX 下实时控制系统与 Windows 下控制系统进行实验对
比。进行主从控制实验，在训练模块上进行触碰、抓取和套圈等定位操作，并记
录主手末端和器械末端的空间位置，验证控制系统的实时性与主从控制策略实现
手眼协调的能力。最后进行活体动物实验，整体检验机器人的实时性和安全性等
性能。

10

万方数据
 哈尔滨工业大学工程硕士学位论文

第2章 基于 QNX 的实时控制系统构建

微创手术机器人对控制系统的实时性能有着非常高的要求，从底层开始开发
难度过大、周期过长，也不够经济，而在普通的操作系统上搭建控制系统又难以
保证实时性。QNX 实时系统能够实现真正的硬实时，提供各种内核级的实时性功
能与服务，并且符合 POSIX 标准，移植和开发也相对较为容易。因此基于 QNX
系统进行实时控制系统的构建。本章先进行了控制系统整体结构设计，然后完成
基于 QNX 系统的硬件和软件系统构建，最后对构建的控制系统进行了实时性能
分析。

2.1 控制系统结构设计

2.1.1 控制系统结构确定

为了满足微创手术机器人对于实时性和可靠性的要求，在设计控制系统结构
时就要从实时性的角度出发，保证各个环节的实时性能，从而保证整个系统的实
时性能。目前微创手术机器人的控制系统结构主要有三种，分别是集中式 I/O 与
处理、集中式处理且分布式 I/O 以及分布式 I/O 与处理。三种控制系统结构简图
如图 2-1 所示。

集中式I/O与处理 集中式处理且分布式I/O 分布式I/O与处理

应用层 应用层
应用层
高级控制算法（主从映射等） 高级控制算法（主从映射等）
高级控制算法（主从映射等）
低级控制算法（电机控制等） 低级控制算法（电机控制等）
高速实时 高速实时
通讯网络 通讯网络

低级控制算法
FPGA
FPGA FPGA
FPGA FPGA
FPGA FPGA
FPGA （电机控制等） 高速实时
电路
电路 电路
电路 电路
电路 电路
电路
通讯网络

电机 电机 电机 电机 电机 电机 电机 电机 电机 电机 电机 电机
驱动器 驱动器 驱动器 驱动器 驱动器 驱动器 驱动器 驱动器 驱动器 驱动器 驱动器 驱动器

图 2-1 三种控制系统结构简图

集中式 I/O 与处理这一方式因为对主机的处理和通信能力要求过高，难以满

11

万方数据
 哈尔滨工业大学工程硕士学位论文

足使用需求，现在已经很少被采用。分布式 I/O 与处理这一结构随着实时高速通

讯协议与分布式计算技术的发展而成为一种主流。其对于处理器的要求相对较低，
资源配置更加合理，可以充分利用实时高速通讯网络的优势进行实时硬件控制，
有利于系统扩展与智能化。集中式处理且分布式 I/O 这一方式是一种较新的技术。
其可以充分利用高性能计算机的处理能力优势，将高级和低级的控制算法在同一
开发环境中实现，从而增大控制算法的灵活性。
本文采用分布式 I/O 与处理的结构。因为这种结构技术比较成熟，有大量的
商业化部件可供选择，安全性与稳定性较高，可扩展性好，同时可以大幅度缩短
研发周期。而反观集中式处理且分布式 I/O 这一结构，需要关心太多的低级控制
算法细节，而且需要设计专门的 FPGA 来作为高速通讯协议与电机等设备的接口，
设计难度大，研发周期长。所以采用分布式 I/O 与处理的结构。

2.1.2 控制系统结构实现方案

具体实现上，采用“顶层控制（上位机）+关节伺服控制（下位机）”的方
案。微创手术机器人控制结构如图 2-2 所示。

图 2-2 微创手术机器人控制结构图

12

万方数据
 哈尔滨工业大学工程硕士学位论文

上位机采用研华公司的无风扇嵌入式工控机。采用工控机，是由于工控机在
可靠性、稳定性、防水防尘性、抗干扰性、可扩展性、EMC 兼容性能等方面具有
良好的性能。该型号工控机接口丰富，扩展方便，尺寸适中，计算能力强大。在
上位机安装 QNX 系统，使之成为实时控制系统的核心。
下位机采用 ELMO 公司的解决方案，其结构图如图 2-3 所示。

图 2-3 ELMO 下位机结构图

在图中，下位机主要分为两个部分。一部分称为 GMAS（Gold Maestro）多

轴网络控制器，其中封装了基于 PLCopen 标准的运动控制库，使用时可以方便地
调用函数来完成相应动作，极大地简化了开发过程。ELMO 公司为其提供了基于
Eclipse 开发的 EAS（ELMO Application Studio）应用软件，可以方便地在软件中
进行系统配置和调整电机 PID 参数等工作，还可以在线监控电机的运动。ELMO
公司提供的另一款软件是 GDS（G-MAS Development Studio）集成开发环境，在
其中可以编制程序实现 PVT 等复杂功能。需要指出的是，这两款软件都只有

13

万方数据
 哈尔滨工业大学工程硕士学位论文

Windows 版本，不能在 QNX 系统中安装。所以无法使用 EAS 软件的在线监控功

能，使用时需要事先在 EAS 中配置好系统，调整好电机，然后在 GDS 中编写好
程序下载到 GMAS 中。一个 GMAS 多轴网络控制器最多可控制 64 个单轴节点，
一个主控周期内最多可同时控制 20 个单轴节点，功能十分强大。
另一部分为 ELMO 驱动器，在其中实现伺服电机的位置环、速度环和电流环
的闭环。其内部采用“DSP+FPGA”的形式，处理速度快，稳定性高。两部分之
间用 EtherCat 通信网络进行连接。EtherCat 通讯协议是一种基于以太网的现场总
线协议，支持多种连接拓扑结构，传输速率和效率高，实时性强[32]。GMAS 多轴
网络控制器负责充当 EtherCat 通信的主站, ELMO 驱动器是 EtherCat 通信的从站。
GMAS 上安装的实时 Linux 系统可以驱动 EtherCat 网络上的多个从站，通讯循环
周期为 1ms，即可以保证 1ms 内所有网络中从站接收和发送信息完毕，从而保证
通信网络的实时性能。因为每个 ELMO 伺服电机驱动器控制一个关节电机，所以
从安装和使用性能的角度考虑，每条机械臂上安装一个 GMAS 多轴网络控制器。
即一共有三台 GMAS，其中持械臂上的每台 GMAS 负责控制 9 个 ELMO，持镜
臂上的 GMAS 负责控制 6 个 ELMO。
上、下位机之间通过 Modbus/TCP 协议实现通信。Modbus 协议支持多种底层
协议和电器接口，格式较为简单，许多厂商都支持这一协议，使用方便 [33]。而
TCP 协议是一种可靠的传输层协议[34]。Modbus/TCP 协议其实就是在开放式互联
系统模型 OSI 中，在应用层中运用 Modbus 协议，在传输层中运用 TCP 协议。其
结构图如图 2-4 所示。

图 2-4 Modbus/TCP 协议结构图

因为 Modbus 协议格式简单，其搭载在 TCP 协议上传输速度很快，可以达到

数百兆每秒，同时具有良好的实时性能[35]。在上、下位机之间采用 Modbus/TCP
协议能够满足系统的实时性要求。
另 外 ， 对于 微 创手 术 机器 人的 输 入设 备 ，采 用 Force Dimension 公 司 的
omega.7 系列产品。该产品的最大特点就是旋转工作空间与移动工作空间完全解
耦，也就是说，移动到工作空间的任意位置，都可以获得相同的旋转灵活性。这
是通过其特殊的关节结构配置实现的。其外形图如图 2-5 所示。
14

万方数据
 哈尔滨工业大学工程硕士学位论文

图 2-5 omega.7 主手外形图

由图中，该产品共有 7 个自由度，采用串并联混合的配置形式。前三个关节
采用 Delta 并联结构实现  12  16 cm 的圆柱形三维工作空间，而后三个关节采用
串联的形式来进行姿态信息采集，三个关节轴线重合于一点并互相垂直，实现姿
态角度的完全解耦。这一交点是腕部三个关节的旋转中心。在使用时，操作者手
部的自然中心与腕部关节旋转中心刚好重合，能够提升操作性能与舒适感，符合
人机工程学的要求。最后一个自由度是夹持关节。三个并联关节上安装有电机，
可以为三个移动方向提供最大 12N 的持续力输出。利用这一特点可以开发操作安
全限制等功能，提高手术机器人的性能。该主手设备最大刷新频率达到 4KHz，
采用 USB2.0 与安装有 QNX 系统的主机相连，传输速度快，完全可以满足机器人
的实时性与安全性需求。德国宇航中心的 MIRO 机器人也采用了相同的产品[11]。
主控台的按键控制电路和脚踏板控制电路基于 STC 系列单片机实现，其通过
RS232 串口协议与安装有 QNX 系统的主机通信。虽然单片机和串口协议的实时
性能较差，但是从实用功能角度考虑，按键功能与其他涉及到手术安全的功能相
比，并不需要很高的实时性。比如在术中医生想调整手术视野，则踩下相应的脚
踏开关，切断主从控制。这一过程对于时间延迟的容忍度比较高，也就是说实时
性比较弱。所以这一硬件方案可以满足使用需求。
以上采用分布式 I/O 与处理的控制系统结构，经过各部分的方案确定，保证
了各个控制系统主要环节的实时性能，继而保证了控制系统的整体实时性能。

2.2 基于 QNX 的硬件系统构建

在完成控制系统整体结构设计和确定各部分实现方案之后，需要构建控制系
统的硬件部分。具体内容是在工控机上移植 QNX 系统，然后将上一节确定的各

15

万方数据
 哈尔滨工业大学工程硕士学位论文

部分商业化高可靠性部件通过各种通信协议连接到工控机上，组成通信网络，实
现预期功能。

2.2.1 QNX 系统裁剪和移植

QNX 系统最根本的特征是采用了微内核架构和基于消息的进程间通信机制
[36] 。QNX 系统结构图如图 2-6 所示。

图 2-6 QNX 系统结构图

由图中可知，QNX 系统由一个微内核和众多功能组件构成。整个结构高度
“扁平化”，各组件进程在微内核的协调下相互作用。微内核起到了软总线的作
用，通过消息将组件联系到一起，从而可以实现组件的在线热插拔。因此，QNX
系统可以根据应用系统需要进行裁剪，具有极高的灵活性。
而 QNX 系统的移植，针对不同的硬件环境，移植的工作量相差很大[37]。因
为系统移植要求在不同硬件的环境下，开机都能够正常启动系统。系统启动主要
经历三个过程。首先处理器从 reset 处开始执行 IPL（Initial Program Loader）最初
加载程序，寻找操作系统映像，然后执行映像中的 startup 程序。该程序配置完操
作系统后则开始执行 procnto 程序。procnto 程序负责加载操作系统所需要的驱动
和用户程序。这一过程中，硬件架构不同，IPL 和 startup 程序的具体内容也就不
同，需要根据具体情况编写。
因为上位机采用研华公司的无风扇嵌入式工控机，其处理器使用的是 Intel i7
四核处理器，x86 架构。QNX 官网上提供了相应的板级支持包 BSP 和补丁文件，
这给开发带来了很大的便利。因为板级支持包中包含有针对特定硬件结构的 IPL、
startup 程序、设备驱动程序与功能服务程序等。本文将使用板级支持包构建 QNX
系统。
16

万方数据
 哈尔滨工业大学工程硕士学位论文

另外 QNX 公司提供专用的基于 eclipse 开发的集成软件开发环境（QNX

Momentics IDE），可安装在 Windows、Linux 和 QNX 系统下，将许多命令行操
作转换为面板按钮操作，极大地方便了系统的开发。本文将使用此工具进行裁剪
和移植。具体步骤如下。
先将 BSP 文件导入开发环境中，然后编译 BSP 文件。在 IDE 中依次选择 File
—Import—QNX—QNX Source Package and BSP—Next—Import from local archive
file—Browse，然后选择下载好的 BSP，点击 OK。注意需要事先将针对研华工控
机的补丁包复制到 BSP 主文件夹下，替换同名文件。补丁包中是针对特定硬件的
最新驱动程序。经过上述过程就导入了 BSP 文件。这时在 Project Viewer 窗口中，
出现 x86-generic-650SP1 和 x86-generic-650SP1-src 两个工程。软件界面如图 2-7
所示。

图 2-7 导入的 BSP 工程界面图

“src”代表“source”，表明后一个工程包含前一个工程的源代码。当编译
完成后，在 x86-generic-650SP1 工程中的 Images 文件夹中出现 x86-generic-7-
650SP1.ifs 文件，这就是 QNX 系统镜像文件。之后再进行编译时，可选择 Project
—Properties—Project Reference 中取消选择 x86-generic-650SP1-src 工程，这样加
快编译速度。
然后修改生成的 IFS 镜像文件。这一步骤就是 QNX 系统的裁剪。在图形化
的 buildfile 文件中，即图 2-7 中的 project.bld 文件中，可以根据系统应用需要，选
择添加或删除镜像中的内容，包括二进制代码，共享链接库和符号链接库等。图
形化的形式使得添加和删除十分方便，无需掌握特定的 shell 命令。因为本文采用
的工控机资源丰富，所以在默认的系统配置上添加一些显示功能，而无需裁剪。
再然后在图 2-7 的 x86-generic-650SP1.bsh 文件中修改和添加开机启动项，定
义开机启动的顺序。QNX 支持的驱动/文件系统有磁盘驱动、Flash 文件系统、网
络驱动和 USB 驱动等。这些驱动有一个共同的特点是它们都依赖一个程序，由
这个程序加载一个或更多的动态链接库文件。如果需要开机时需要启动某种设备，
比如启动磁盘，则需要在 bsh 文件中启动磁盘的驱动程序，即在命令行中输入相

17

万方数据
 哈尔滨工业大学工程硕士学位论文

应的驱动名称，然后这个驱动程序就会按顺序动态地加载 buildfile 文件列表中相

应的模块。需要注意的是，可以在 bsh 文件中编写执行另一个 sh 文件的语句。sh
文件与 bsh 文件相似，都是 shell 命令文件。只是 bsh 文件是在生成系统镜像前完
成的，一个 bsh 文件就对应着一个系统镜像，而 sh 文件可以放在外部存储器上，
需要时再加载执行，独立于镜像之外。所以可以把开机所需的基本内容放入 bsh
文件中，把需要经常改动的内容放入 sh 文件中，这样就不用经常镜像文件，即无
需经常地编译和下载，极大地方便了系统的安装。
上述修改完成后重新编译，生成裁剪和定制后的系统镜像文件。为了将得到
的系统镜像文件拷贝到工控机中，可以先将 ifs 文件先拷贝到 U 盘中，再从 U 盘
启动 QNX 系统。这一过程要求 U 盘是可启动的。可以在安装有 QNX 系统的另一
台电脑上运用 dinit 格式化命令和 dloader 加载命令等自己制作 USB 启动盘，本文
限于篇幅不进行详细介绍。
从 U 盘启动 QNX 系统后，再将 U 盘中的镜像文件复制到工控机的外存中，
采用与构建 USB 系统盘类似的方法将工控机外存设置为可自启动。这样就实现了
从工控机硬盘启动 QNX 系统。至此完成 QNX 系统的移植。

2.2.2 控制系统通信网络构建

在上位机中安装完 QNX 实时操作系统之后，需要将各种设备通过不同的通

信协议与上位机相连，从而建立起控制系统的实时通信网络，完成 QNX 下的硬
件系统的构建。主要包括主操作手、按键控制单片机和 GMAS 多轴网络控制器与
上位机之间的连接过程。
主操作手是采集医生手部动作信息的输入设备，通过 USB 协议与上位机连接。
主手供应商提供了 QNX 系统下主手设备的驱动和应用程序接口，极大地方便了
连接过程。其中两个头文件 dhdc.h 和 drdc.h 需要在应用程序工程中包含，两个名
为 libdhd.so 和 libdrd.so 的动态链接库需要在编译时链接到工程，这样就可以在应
用程序中使用厂商定义好的函数。另外，还需要 usbdi.so 动态链接库以实现 USB
通信。添加动态 链接库的具 体方法为 Project—Properties— Linker—Category—
Extra Libs，在其中选择想要添加的动态链接库，再顺序点击 Apply 和 OK 按钮。
按键控制单片机是采集医生按键动作和脚踏动作的设备，为医生提供控制系
统的接口。按键实现的功能有关节选择、机械臂选择和手术器械选择等。这一部
分包括按键、脚踏开关、STC 单片机以及控制电路，通过 RS232 串口协议与主机
QNX 系统进行连接。QNX 和 Linux 类似，也把设备当作特殊的文件进行处理，

18

万方数据
 哈尔滨工业大学工程硕士学位论文

所以可以用 open()打开一个串口，用 tcsetattr()设置串口奇偶校验等参数，用 read()

和 write()进行读写操作，最后用 close()函数关闭串口设备。需要注意的是应用工
程需要包含 termios 头文件。
下位机采用 ELMO 公司的解决 方案，其中 GMAS 多轴网络控 制器通过
MODBUS/TCP 协议与主机 QNX 系统进行通信。虽然对于 Windows 系统，有许
多 Modbus/TCP 协议的调试工具，但对于 QNX 系统，很难找到这样的资源，必
须自己根据 Modbus/TCP 协议进行开发。其通信流程图如图 2-8 所示。

socket()

MbusStartServer()
填写GMAS的
IP地址和端口
ConnectIPCEx()
建立连接
connect()

工 按照MODBUS GMAS多轴
控 协议格式填写报文 网络控制器
机 写数据
MbusReadHolding
send()
RegisterTable()
接收回执
recv()
循环 循环
数据处理
读数据
send()

接收数据 MbusWriteHolding
recv()
RegisterTable()

断开连接 MMC_CloseC
shutdown()
onnection()

MbusStop
Server()

图 2-8 MODBUS/TCP 协议下工控机与 GMAS 间通信流程图

在工控机端，因为在传输层使用的是 TCP 协议，所以要先在应用程序中创建

套接字，用 connect()函数将套接字与指定地址和端口的 GMAS 设备进行连接。然
后按照 MODBUS 协议规定的格式将报头和通信内容写入一个字节数组，即报文。
当设置报文中的功能码为写寄存器时，发送的报文中包含要发送的信息。使用
send()函数向套接字写入报文，recv()函数接受 GMAS 发来的回执，完成写操作。
当设置报文中的功能码为读寄存器时，同样调用 send()函数向套接字写入报文，
recv()函数收到的回信中包含要读取的信息，完成读操作。当通信结束或工控机上

19

万方数据
 哈尔滨工业大学工程硕士学位论文

程序退出时，使用 shutdown()关闭套接字，断开与 GAMS 的连接。

在 GMAS 端，有供应商封装好的函数可供使用。调用 ConnectIPCEx()函数和
MbusStartServer()函数进行初始化并打开通信从站服务器。建立好连接后，当主
站发来写请求时，调用 MbusReadHoldingRegisterTable()函数接收发来的数据，从
站 自 动 返回 回 执。 然 后 进行 数 据处 理 。当 主 站发 来 读请 求 时 ，调 用
MbusWriteHoldingRegisterTable()函数将所要读取的内容发送给主站。当接收到主
站的结束通信请求时，调用 MMC_CloseConnection()函数和 MbusStopServer()函数，
断开连接并关闭从站服务器。另外要添加 socket.so 动态链接库来实现套接字通信。

2.3 基于 QNX 的软件系统构建

首先根据微创手术机器人的使用要求，确定软件系统的层级结构。然后将复
杂问题分解为多个相互联系的任务，并区分出实时性任务与非实时性任务。再利
用 QNX 系统提供的实时功能，确定实时任务的结构与优先级，设计任务间的同
步与通信方式，使得各任务协调有序运行，从而实现基于 QNX 系统的软件系统。

2.3.1 软件系统层级结构设计

对于复杂的控制软件系统，可以将功能封装到不同的软件层次中。每一层内
部的实现细节对其余层是不可见的，相邻层次之间留有接口。这样就从逻辑上把
复杂问题进行了分解。高层次软件的编写者不需要知道底层软件的实现细节，只
要掌握接口的使用方式即可，从而实现了明确的分工，缩短了软件系统的开发周
期。采用层次结构还使得软件的维护与升级变得简便。
基于微创手术机器人的功能需求，设计软件系统层级结构如图 2-9 所示。

图 2-9 软件系统层级结构图
20

万方数据
 哈尔滨工业大学工程硕士学位论文

图中，软件系统共分为四个层次，分别是手术应用层、腹腔镜下主从控制层、
从手笛卡尔空间控制层和从手关节空间控制层。手术应用层是软件系统与手术操
作者的接口，负责接收操作者的按键命令进行控制，并实时监控系统运行。内窥
镜下主从控制层负责实现内窥镜视野下的主从一致性运动，即实现医生的手眼协
调。从手笛卡尔空间控制层负责实现从手机械臂在笛卡尔空间中的运动。从手关
节空间控制层负责实现从手机械臂的各个关节按照指令要求运动。另外还有主手
力平衡等辅助功能，可以直接被手术应用层调用。对于两条持械臂，内窥镜下主
从控制层将主手采集的操作者手部动作信息通过内窥镜下主从映射传递给从手笛
卡尔空间控制层，笛卡尔空间控制层再通过运动学逆解计算出各个主动关节的位
移量，传递给从手关节空间控制层。而对于持镜臂的运动，不需要进行内窥镜下
的主从映射，只要持镜臂能够在笛卡尔空间中跟随主手的运动即可，所以手术应
用层直接调用从手笛卡尔空间控制层。最底层即从手关节空间控制层可以调用
GMAS 中封装好的 PLCopen 标准函数，无需关注过多的细节。而上面的三层则需
要自己编程实现。由于功能比较复杂，所以要进行任务划分等工作。

2.3.2 任务划分与优先级分配

嵌入式实时控制软件中实现功能的基本单元是任务，任务划分是软件系统构
建之初的重要一步。应用程序功能需要被划分成不同的任务。任务划分需要遵循
并发性要求、功能独立性要求、实时性要求和重要性要求等诸多原则[38]。
任务的实现形式有进程和线程两种。采用进程形式，各个进程间通过管道、
消息和共享内存等方式进行通信，任务间解耦程度高，使得软件系统的配置可以
更加灵活，测试和维护更加方便。采用线程形式，各个任务作为同一个进程中的
线程共享同一地址空间，耦合程度较高，软件系统灵活性差。不过这一形式运行
速度较快，用内核级别的线程间同步方法就可以方便地实现任务间同步。而且在
同一地址空间中可以共享同一套进程信息，占用空间也较小。本文中任务采用线
程实现。
针对本文上面设计的软件系统结构，将软件功能划分为相对独立的九个任务，
即九个线程，分别是设备初始化线程，按键输入监测线程，左、右主手力平衡线
程，左、右主手采集线程和左持械臂控制线程、右持械臂控制线程及持镜臂控制
线程。如图 2-10 所示。

21

万方数据
 哈尔滨工业大学工程硕士学位论文

图 2-10 软件系统任务划分图

设备初始化线程即为程序主线程。在控制程序开始运行后进行主手、单片机
等设备的配置和连接，然后开启按键输入监测线程、左、右主手采集线程和左、
右主手力平衡线程等。上述过程完成后，设备初始化线程调用 pthread_join()函数
进入阻塞状态，等待其他线程都结束后断开与各个外围设备的连接，退出程序。
按键输入监测线程负责检测是否有按键或脚踏开关输入，判断是来自于哪一
个单片机的输入，然后对信号做出处理。该线程是软件系统与操作者交互的接口。
进行手术时，操作者可以根据需要，通过脚踏开关建立或断开主从连接，即通过
按键输入检测线程开启或退出三条机械臂的主从控制线程。还可以进行设置主从
映射比例，切换持镜臂和持械臂等诸多控制操作。以上两个线程处在手术应用层。
左、右主手力平衡线程负责循环执行主手厂商提供的 dhdSetForce()函数，使
主手设备上的三个电机输出力矩，以平衡手柄等部件产生的重力矩，减轻医生操
作主手设备时的负担，提高操作舒适度。该线程属于辅助功能。
左、右主手采集线程负责实时获取操作者的手部位姿信息。调用函数读取当
前时刻主手的位置和姿态角度，并对位置信息进行实时抖动滤波，减小手部抖动
对操作的影响。采集到的信息传递给机械臂控制线程。操作安全限制功能也在这
两个线程中实现。在安全限制模式下，根据当前主手与安全限制区域的位置关系，
调用 dhdSetForce()函数在合适的方向上施加力，实现限制主手运动自由度的效果。
左、右持械臂控制线程、持镜臂控制线程分别从相应的主手采集线程获取信
息，经过主从空间一致性映射，得到从手应到达的位姿矩阵。再经过机械臂逆运
动学计算出机械臂上每个主动电机的位移量，向 GMAS 发出运动指令。其中左、
右持械臂控制线程跨越了腹腔镜下主从控制层和从手笛卡尔空间控制层两个层次。
从实现的角度，这样设计能够使得三条机械臂控制线程具有相同的接口，从而降
22

万方数据
 哈尔滨工业大学工程硕士学位论文

低程序的复杂程度。由上述分析，软件执行流程图如图 2-11 所示。

左主手
采集线程

程序开始 右主手
采集线程

左主手
设备初始化线程
力平衡线程

右主手
力平衡线程
左持械臂主从
按键命令 控制线程

串口输入 右持械臂主从
监测线程 控制线程
退出
等待
持镜臂主从
清理 控制线程
数据结构

程序结束

图 2-11 软件执行流程图

QNX 系统采用抢占式的线程调度策略[39]。一旦有更高优先级的线程进入就
绪态，内核就会进行调度，剥夺较低优先级线程的资源使用权，将 CPU 等资源分
配给较高优先级的线程，从而保证了高优先级程序优先执行，增强了系统的实时
性。因此，在完成任务划分后，要根据实时性强弱分配任务优先级。QNX 系统有
256 个线程优先级（0~255 号）。数字大小代表优先级高低。其中 0 号优先级代表
空闲线程，在没有其他线程运行时占用 CUP。分配优先级时不能连续分配，在优
先级之间要留有一定的空余，以便将来对应用软件进行功能扩展时使用。
任务优先级分配的最终目标是使整个系统的效益最大化，需要遵循实时性要
求、中断相关性要求和任务等待时间最短要求等[38]。现按照不同线程的实时性要
求、执行时间等指标确定优先级。设备初始化线程只在程序开始时执行一次，所
需优先级较低，定为 10。串口输入监测线程需要及时检测到按键命令输入，实时
性要求较高，代码量小，运行时间短，优先级应较高，定为 40。两个主手力平衡
线程实现手术操作过程中的辅助性功能，实时性要求低，优先级定为 15。两个主
手采集线程代码量小、运行时间短，对实时性要求较高，优先级定为 35。三条机
械臂控制线程运算量大，耗时长，同时又需要很高的实时性以实现机械臂末端手
术器械对医生手部运动的良好跟随性能。所以需要较高的优先级占用 CUP 进行计
算。优先级定为 30。
23

万方数据
 哈尔滨工业大学工程硕士学位论文

2.3.3 任务循环结构与任务间通讯

在实时系统中，任务需要被设计成循环执行的程序。任务的循环结构主要有
自循环结构和事件触发结构两种[38]。自循环结构的任务能够自行执行完所需执行
的所有操作。这种结构主要由自循环执行操作和延时操作两部分组成。事件触发
结构的任务在受到外部事件触发后才开始执行。这种结构主要由等待事件操作和
执行操作两部分组成。
针对上面划分的各个任务的特点，依次确定任务循环结构。设备初始化线程
因为只执行一次，所以不采用循环结构。按键输入监测线程因为要监测按键信息
的输入，所以采用事件触发执行结构。该任务执行流程图如图 2-12 所示。

线程开始

进入循环

串口1 阻塞在 串口2

按键 多通道 按键输
输入 选择函数 入

读取数据

命令值？

切换到 切换 开始或断开 切换 切换 切换到

停止命令
持镜臂 手术器械 主从控制 左右手 映射比例 调试模式

线程结束

图 2-12 按键输入监测线程执行流程图

由图中所示，用多通道选择函数 select()阻塞线程，直到有按键按下，串口有
信号输入，这时线程解除阻塞状态，进入就绪态，因为设置按键输入监测线程的
优先级为最高，所以可以立刻开始执行下面的程序。因为按键较多，所以使用两
个单片机电路采集按键信号，分别连接到串口 1 和串口 2 上。用 FD_ISSET()宏检
测信号是由串口 1 还是串口 2 输入，然后对相应的串口执行读操作，对输入的数
据进行判断，根据输入的命令切换系统的工作模式，达到操作者控制系统的目的。
执行完上述操作后重新回到循环起点，在 select()函数处等待新的按键信号输入。
左、右主手力平衡线程采用自循环结构。因为主手厂商提供的函数内部设置
了等待结构，所以无需自行设置等待时间。只要主手设备处于开启状态，该线程
24

万方数据
 哈尔滨工业大学工程硕士学位论文

就一直循环执行。左、右主手采集线程采用自循环结构，但不设置线程周期，与
三个机械臂的控制线程协调运行。机械臂控制线程也采用自循环结构，具体实现
以左臂控制线程为例进行讨论。左臂线程执行流程图如图 2-13 所示。
线程开始

设定周期定时器

进入循环

等待定时器中断

定时时间到
收到线程
手动调整 电流模式 检测电机 退出命令
被动关节 模式位
位置模式 清除数据结构

检测左右主手对
线程退出
应标志

左主手对应左臂 右主手对应左臂

采集左主手信息 采集右主手信息

主从一致性映射

将电机运动增量发送给GMAS

循环查询GMAS标志位

图 2-13 左持械臂线程执行流程图

操作者通过按键开启左持械臂控制线程后，线程首先设置定时器，精确控制
主从控制周期的时间。定时器时间到达这一事件采用中断的方式通知线程。然后
开启定时器，进入循环体。在循环体中首先用 InterruptWait()阻塞线程，等待定时
器时间到再继续线程的执行。然后读 GMAS 寄存器的电机模式标志位，判断当前
机械臂的运动模式。如果是电流模式，则说明正在对机械臂姿态进行手动调整，
不需要主从控制，回到循环开始处等待下一次循环。如果是位置模式，则判断是
左主手还是右主手控制左持械臂，分别从相应的全局变量中读取主手数据。再将
主手数据通过内窥镜下主从映射对应得到左持械臂各个关节位移量。将这些位移
量发送给 GMAS 控制伺服电机运动。然后循环查询 GMAS 标志位，如果 GMAS
将数据发送给电机驱动器，则标志位置位，线程回到循环体开始处，阻塞等待定
时时间到，开始下一次主从控制。线程运行到任意位置时，如果手术操作者踩下
25

万方数据
 哈尔滨工业大学工程硕士学位论文

断开主从控制的脚踏开关，则串口输入监测线程会给该线程发停止消息。该消息
相当于软中断，线程接收到消息，立即清除必要的数据结构，安全退出。
为了实现任务间的协调与同步运行，需要进行任务间通信，主要包括数据通
信和资源共享、行为同步等内容。对于数据通信，因为本文中任务采用多线程的
形式，使得各个任务共享同一地址空间，任务间的数据通信可以简单地用全局变
量实现。因为在上面的任务划分中，三条机械臂的控制程序分别在三个线程中，
所以线程间耦合程度较小，全局变量的数量也较少。需要注意的是，打开的外部
设备资源也作为全局变量在各线程间共享。
对于行为同步，同样因为本文中任务采用多线程的形式，所以任务间行为同
步可以用内核级别的线程间同步方法实现，使得应用程序简单，运行速度快。
QNX 系统提供互斥量（mutex）、信号量（semaphore）、读写锁（reader/writer
lock）和条件变量（condition variables）等多种线程间同步方式，可以根据具体需
要进行选择。本文设计的线程间同步方式图如图 2-14 所示。
线程联合
设备初始化线程 串口输入监测线程

线程间信号传递

左持 持镜 右持
互斥量 械臂 读写锁 臂 读写锁 械臂
左、右主手 主从 主从
主从
采集线程 控制 控制 控制
线程 线程 线程

图 2-14 线程间同步方式图

设备初始化线程执行完初始化工作后，用 pthread_join()函数使线程处于阻塞
状态，等待串 口输入检测 线程结束 后进入就 绪状态。串 口输入检 测线程通过
pthread_create()函数打开各线程，通过 pthread_cancel()函数关闭线程。主手数据
采集线程和机械臂主从控制线程之间则采用互斥锁进行同步，防止机械臂控制线
程在读取主手数据的同时主手采集线程改变数据而导致错误。而持械臂线程与持
镜臂线程之间则采用读写锁进行同步。因为读写锁像互斥锁一样可以防止数据在
读取时被更改，并且读写锁还允许多个线程同时读取数据，这样两个持械臂线程
就可以同时读取持镜臂的姿态数据，而不需要像使用互斥锁一样等待另一个线程
读完，加快了程序的运行速度。
至此，完成了基于 QNX 系统的实时系统构建。实际上，这里构建的软硬件
系统还只是一个框架，而框架的内容，即整个控制系统的灵魂——主从控制算法
将在第三章进行介绍。
26

万方数据
 哈尔滨工业大学工程硕士学位论文

2.4 控制系统实时性能分析

在完成上述基于 QNX 系统的实时控制系统构建后，运用将在第三章介绍的

主从控制算法，编写程序并上机调试运行，最终编译生成可执行文件。虽然这时
的控制系统运行没有错误，可以达到控制机械臂进行主从运动的要求，但是无法
保证运行在最坏的情况下也不发生错误，即无法保证系统的实时性能。而且，机
械臂控制线程中的定时器周期，即主从控制周期还没有定量的确定过程。因此要
分析系统的实时性能。
任务的优先级很大程度上影响系统的实时性能。任务优先级的分配方法有静
态分配方法和动态分配方法。静态优先级分配方法是指事先分配好所有实时任务
的优先级，系统开始运行后，任务按照固定的优先级进行调度的方法[40]。速率单
调分析法[41]（RMA）是一种最优的静态优先级分析方法。虽然这一方法假设较多，
与 QNX 系统实际采用的方法有所出入，但是因为算法简单、可靠实用而得到了
广泛的应用。本文采用该算法分析实时性能。
单调速率分析法要求所有的硬实时任务都应该用最坏的执行时间和最坏到达
率来调度，而无实时性要求的任务用平均执行时间和平均到达率来调度。根据上
面的软件系统任务结构，系统工作最常用并且工作负担最重的情况是操作者操纵
两个主手控制两个持械臂在进行手术操作。这时同时有六个线程在同时运行，分
别是两个主手力平衡线程、两个主手数据采集线程和两个持械臂控制线程。主手
力平衡线程内只有一条语句，为了简便不计入调度分析。主手数据采集线程代码
量也很少，而且因为工控机采用四核处理器，程序设计上可以与机械臂控制线程
同时运行，不影响其运行时间，为了简化也不计入总的调度分析。两个持械臂主
从控制线程，实时性要求很高，属于硬实时任务，运行超过时限会发生严重的后
果，所以要采用最坏执行时间来调度。
由 4.2.1 节实验测得，左、右持械臂控制线程平均执行时间约为 4ms，最长执
行时间不超过 6.5ms，取两个线程的平均执行时间分别为 C1 =C2 =4ms，最长执行
时间分别为 L1 =L2 =6.5ms。线程周期应比最长执行时间长，但过长也会造成较大
的延迟，降低系统实时性能。线程周期分别设定为 T1 =T2 =10ms，则 CPU 利用率
U 1 =U 2 =0.65。因为工控机采用四核处理器，所以这一任务组的两个线程能够同时
得到 CPU 资源。总利用率 U=(0.65+0.65)/2=0.65，小于双线程任务组的常值边界
利用率 0.828。
上面基于实验得到的执行时间估算出了实际运行时的最大 CPU 利用率。下面
进行最长响应时间估算。将任务按照优先级排序，对优先级最低的任务 i 有公式:
27

万方数据
 哈尔滨工业大学工程硕士学位论文

Wi 0  Ci

 n 1  Wi n 
 i
W    C j  Ci (2-1)
 jhp ( i ) 
 T j 
 i 1
 Ri  Wi
式中 Wi n 1 ， Wi n ——计算过程中间量；
Ci ， C j ——任务 i 和任务 j 的执行时间；
hp (i ) ——任务组中优先级比 i 高的任务；
T j ——任务 j 的周期；

Ri ——任务 i 的最长响应时间。
将 C1  4ms ， C2  4ms ， T1  10ms ， T2  10ms 代入公式（2-1），得到
W20  4ms
W21  4  (4 / 10)  4ms  5.6ms (2-2)
R2  W22  4  (5.6 / 10)  4ms  6.24ms  10ms
公式（2-2）中，结果与 4.2.1 节实验测得的结果基本相符。结果表明这一任
务组中的两个任务都可以在周期内完成响应。所以上面基于 QNX 系统构建的控
制系统能够及时完成响应。

2.5 本章小结

本章从实时性能的角度，设计控制系统的结构与实现方案。控制系统采用分
布式 I/O 与处理的结构。该结构技术较为成熟，有大量的商业化部件可供选择，
安全性与稳定性较高。对各个部件与通信网络的实时性能进行了讨论，在部件选
择上保证了整体的实时性能。进行了基于 QNX 系统的硬件系统构建。将 QNX 实
时系统经过配置和剪裁，移植到工控机中，再将各个部件通过不同的通信协议与
工控机相连，完成通信网络的搭建。在此基础上进行了基于 QNX 系统的软件系
统构建。设计了软件系统的层级结构，高层与低层之间分工明确，有利于问题分
解和减少重复劳动。进行了任务划分和优先级确定，将复杂问题进一步分解。进
行了任务结构设计，并确定了任务间的通信方式，继而构建出完整的系统架构。
最后运用单调速率分析法对控制系统的实时性能进行了分析，结果表明系统能够
及时完成响应。这一章构建的实时控制系统实际上是一个框架，而框架的主体内
容，即整个控制系统的灵魂——主从控制算法将在第三章进行介绍。

28

万方数据
 哈尔滨工业大学工程硕士学位论文

第3章 主从控制策略与方法研究

在构建了基于 QNX 的实时控制系统框架后，进行主从控制策略和方法的研

究，两者相结合形成完整的控制系统以实现主从控制。本章针对主手和从手机械
臂的异构特点，提出了基于笛卡尔空间的增量控制策略，并对策略中的关键方法
进行了研究。建立了机械臂运动学模型。研究了内窥镜下主从运动一致的映射方
法。最后介绍了主手抖动滤除、主从术中调整和操作安全限制等微创手术机器人
主从控制功能的实现方法。

3.1 主从控制策略

3.1.1 微创手术机器人结构配置介绍

课题组研发的微创手术机器人系统由主控制台和三条独立的从手机械臂构成。
操作者通过主控制台上的主手和控制面板按键等设备控制从手机械臂的运动。三
条独立的机械臂分别是一条持镜臂夹持内窥镜，为医生提供腹腔内的手术视野，
两条持械臂夹持手术器械进行各种手术操作。
为了进行主从控制，首先要分析主手和从手机械臂的构型。主手和从手的构
型如图 3-1 所示。

dz
dy
dx 关节5
dα 关节2 关节3 关节6 关节7
关节4
dβ 关节1
关节10 关节8
关节9
dγ dη
(a) 主手构形图 (b) 从手构形图

图 3-1 主手和从手构形图

如图 3-1(a)所示，主手具有 7 个自由度，分别为 delta 并联机构的三个位移自

由度 dx、dy、dz，相互串联的三个转动自由度 dα、dβ、dη 以及一个手部捏合自
29

万方数据
 哈尔滨工业大学工程硕士学位论文

由度 dγ。左持械臂关节配置，如图 3-1 (b)所示。整个机械臂共有 10 个关节。关

节 1-3 用于术前调整机械臂远心点的位置。调整结束后，将三个关节锁死。关节
4-6 组成远心机构，轴线交于远心点，使得手术器械绕创口做远心运动[42]。其中
关节 4-5 具有主动和被动两种控制模式。主动模式用于主从控制。被动模式用于
术前或术中调整器械姿态。调整时按下机械臂上的按钮，手动引导调整机械臂，
调整结束后松开控制按钮，制动器闭合，自动切换为主动模式。关节 6 为移动关
节，可完成器械在体内的探入运动。因为关节 4-6 布置靠前，尺寸较大，所以主
要影响器械末端的位置。关节 7-10 布置在末端，尺寸较小，主要影响器械末端的
姿态，需要注意的是这四个关节的轴线并不交于一点，所以这四个关节的运动也
会影响到手术器械末端点的位置，从而造成位置和姿态的耦合。右持械臂与左持
械臂的关节配置完全相同。持镜臂只有前六个关节，并且与持械臂完全相同。
可见主手和从手的关节配置差别很大。虽然自由度数相同，但主手的关节配
置考虑的是关节位置采集和人机工程学，而从手的关节配置考虑的则是手术需求，
从而造成主从之间的“异构”，不能简单地进行关节之间的对应。

3.1.2 笛卡尔空间内的增量控制策略

由于结构的原因，不能将主手和从手的各个关节在关节空间中进行一一对应，
所以只能在笛卡尔空间中进行主从控制。同时，由于主手与从手的基坐标系不重
合，两者的工作空间也不一致，需要采用增量控制。而且由于需要在内窥镜视野
下进行手术，所以存在内窥镜视野下主从对应一致的问题。根据以上的分析，提
出笛卡尔空间内的增量控制策略。主从控制策略结构如图 3-2 所示。
t时刻
从手对应的 t时刻从手
主手姿态角 主手 实际姿态
逆运
动学 腹
腔 t+1时刻
镜 从手位 从手逆运动学
t时刻主手 t+1时刻 t+1时刻 t+1时刻
视 姿信息
串联关节 姿态角 主手姿 从手姿
主手 野
角度 增量 态矩阵 态矩阵
正运 下
动学 主
t+1时刻主手 从 从手机械臂
医生
串联关节角度 映 t+1时刻
t+1时刻 射 t+1时刻 从手位置
t时刻主手 t时刻 主手位 方 从手位
主 主手位置 置增量
并联关节角度 法 置增量
主手设备 手 从手正运动学
t时刻从手
正
t+1时刻 实际位置
运
动 主手位置
t+1时刻主手 学
并联关节角度

图 3-2 主从控制策略结构图
30

万方数据
 哈尔滨工业大学工程硕士学位论文

在图中，位置和姿态分离进行主从映射。因为位置在笛卡尔空间中是线性的，
所以主手的位置增量可以直接映射成从手的位置增量，再加上当前时刻从手机械
臂的实际位置即为从手的位置分量。但是由于姿态在笛卡尔空间中是非线性的，
所以要先将主手姿态角增量转化为主手姿态矩阵，再映射成从手的姿态矩阵。具
体方法为将从手当前姿态矩阵映射成主手当前的姿态矩阵，再通过主手的逆运动
学求得当前从手姿态对应的主手姿态角，将主手采集的姿态角增量加到该量上，
再将结果通过主手正运动学转换成主手姿态矩阵，并最终映射成从手的姿态矩阵。
将得到的位置和姿态分量合成便得到从手的位姿矩阵。最后对从手进行逆运动学
求解得到各关节的位移量，发送给 GMAS 完成一个周期的主从控制。
采用增量控制，能够保证主手和从手两端运动的趋势一致，但不能保证主手
和从手之间的一一对应。从医生的角度，运动趋势一致即能够满足手术要求。而
且对于主从异构的机器人，一一对应往往造成某些姿态难以到达，使得医生操作
不够灵活，或者处于容易疲劳的姿势，不符合人机工程学的要求。因此必须采用
增量控制的策略。

3.2 从手机械臂运动学求解

3.2.1 从手机械臂运动学正解

以左持械臂为例运用 D-H 法建立机械臂关节坐标系模型，如图 3-3 所示。

图 3-3 持械臂关节参考坐标系
31

万方数据
 哈尔滨工业大学工程硕士学位论文

左持械臂运动学参数如表 3-1 所示。

表 3-1 持械臂运动学参数

关节 θi di /mm li /mm ai 关节变量运动范围

1 0° d1 0 0° 0≤d1 ≤300
2 θ2 0 400 0° -90°≤θ2 ≤90°
3 θ3 -278.616 0 -106° -90°≤θ4 ≤90°
4 θ4 544.877 0 -74° -170°≤θ4 ≤170°
5 θ5 0 0 -51° 0°≤θ5 ≤150°
6 0° d6 0 0° 0≤d6 ≤188
7 θ7 0 0 90° -170°≤θ7 ≤170°
8 θ8 0 9 90° 0°≤θ8 ≤180°
9 θ9 0 11.5 0° -90°≤θ9 ≤90°
根 据 坐标 系 模型 ，可 得 各关 节之 间的 齐 次变 换矩 阵 ，定 义 cθi=cos(θi) ，
sθi=sin(θi)，sαi =sin(αi)，cαi=cos(αi)，则有
1 0 0 0  c 2  s 2 0 400c 2 
0 1 0 0  s c 2 0 400 s 2 
A1    A2   2 
0 0 1 d1   0 0 1 414.5 
   
0 0 0 1  0 0 0 1 
 c 3  s 3c 3 s 3 s 3 0   c 4  s 4 c 4 s 4 s 4 0 
 s   s
c 3c 3 c 3 s 3 0 c 4 c 4 c 4 s 4 0 
A3   3  A4   4 
 0 s 3 c 3 278.616   0 s 4 c 4 544.877 
   
 0 0 0 1   0 0 0 1 
 c 5  s 5c 5 s 5 s 5 0 1 0
0 0
 s 0 1 0 0
c 5c 5 c 5 s 5 0
A5   5  A6   
 0 s 5 c 5 0 0 0 1 d6 
   
 0 0 0 1 0 0 0 1
 c 7 0 s 7 0  c 8 0 s 8 9c 8 
 s 0  c 7 0  s 0 c 9 s 
A7   7  A8   8 8 8

 0 1 0 0  0 1 0 0 
   
 0 0 0 1  0 0 0 1 
 c 9  s 9 0 11.5c 9 
 s c 9 0 11.5 s 9 
A9   9 
 0 0 1 0 
 
 0 0 0 1 

32

万方数据
 哈尔滨工业大学工程硕士学位论文

因为在手术过程中，关节 1-3 锁死不动，所以关节 3 坐标系相对于基坐标系

的齐次变换矩阵 0T3 为定值。根据关节 2 与关节 3 编码器采集的关节位移量，预先
计算出 0T3 ：

T3  A1  A2  A3
0
(3-1)
主从控制开始以后，手术器械末端的位置和姿态由关节 4-10 决定。需要指出
的是，关节 9-10 构成的夹持自由度不计入总的自由度数，所以持械臂为六自由度
机械臂。手术器械末端关节 9 坐标系相对于关节 3 坐标系的转换矩阵为：
T9  A4 A5 A6 A7 A8 A9
3
(3-2)
因此，手术器械坐标系相对于基坐标系的转换矩阵为：
 nx ox ax px 
n oy ay py 
0
T9  T3 T9   y
0 3  (3-3)
 nz oz az pz 
 
0 0 0 1
将图 3-1 所示位姿的关节位移量 d1 =0，θ2 =0°，θ3 = -90°，θ4 =0°，θ5 =0°，d6 = 0，
θ7 =0°，θ8 =90°，θ9 =0°，代入公式（3-3）可得：
 0.777 0.629 0 907.838 
 0 0 1 0 
0
T9    (3-4)
 0.629 0.777 0 27.206 
 
 0 0 0 1 
公式（3-4）求解得到的手术器械位姿与图 3-1 所示的位姿是一致的，表示手
术器械在各关节为初始状态下的位姿。

3.2.2 从手机械臂运动学逆解

根据上面介绍的笛卡尔空间中增量控制策略，主手采集到的位置和姿态信息
分别映射到从手端，再组合成从手的目标位姿矩阵。而只有将该矩阵通过从手机
械臂的逆运动学求解才能得到各个关节电机的位移量。根据持械臂的结构特点，
关节 4-6 的轴线交于一点，符合 Piper 准则，所以可以通过矩阵变换求出解析解。
虽然线下的求解过程较为复杂，但是在线运行时，相比于数值解法[43]，解析解可
以直接计算出结果而无需迭代过程，运算速度快，有利于提升实时性能。
根据上面的分析，手术过程中，持械臂基座标系到关节 3 坐标系的齐次变换
矩阵为定值，记作 0T3 。

33

万方数据
 哈尔滨工业大学工程硕士学位论文

令 0T9 为主手位姿矩阵经过主从映射得到的已知量，即从手的位姿矩阵。于是
得到公式（3-5）：
( A1 A2 A3 ) 1  0T9  A4 A5 A6 A7 A8 A9 (3-5)
上式等号左边为已知量，右边为未知量，对上式进行变换可得：
( A7 A8 A9 ) 1  (( A1 A2 A3 ) 1  0T9 ) 1  A4 A5 A6 (3-6)
令
 n x1 o x1 a x1 p x1 
n o y1 a y1 p y1 
(( A1 A2 A3 )  T9 )   y1
1 0 1  (3-7)
 n z1 o z1 a z1 p z1 
 
0 0 0 1 
 nx 2 ox 2 ax 2 px 2 
n oy 2 ay2 py2 
(( A1 A2 A3 )  T9 ) A4 A5 A6   y 2
1 0 1  (3-8)
 nz 2 oz 2 az 2 pz 2 
 
 0 0 0 1 
对于公式（3-7）和公式（3-8）存在如下规律：
 px 2  a x 2 d 6  a x1  d 4  px1

 p y 2  a y 2 d 6  a y1  d 4  p y1 (3-9)
 p  a d  a  d  p
 z2 z2 6 z1 4 z1

于是利用公式（3-6）等号左右的矩阵对应元素相等，可以得到只含有  8 、
 9 和 d 6 三个未知量的等式：
 c 9  l8  l9  s8  c 9  d 6  a x1  d 4  p x1

 s 9  l8   s8  s 9  d 6  a y1  d 4  p y1 (3-10)
 0  c8  d 6  a z1  d 4  p z1

根据上式，可以求出  8 、  9 ：
 ( a y 1  d 4  p y1 )
9  arctan (3-11)
(l9  ax1  d 4  px1 )
当 9  0 时
(a y1  d 4  p y1  sin 9  l8 )
8  arctan (3-12)
sin 9  (az1  d 4  pz1 )
当 9  0 时
(l8  l9  a x1  d 4  p x1 )
8  arctan (3-13)
( a z1  d 4  p z1 )

34

万方数据
 哈尔滨工业大学工程硕士学位论文

在这里先不求解 d 6 ，因为易求出为负值的多解，而根据前面建立的坐标系，
d 6 应总为正值。 d 6 将在最后求解。此时 A8 、 A9 为已知量，分别乘到公式（3-5）
等号左侧，则有
 nx 3 ox 3 ax3 px3 
n oy3 ay3 py3 
A4 A5 A6 A7  ( A1 A2 A3 )  T9  ( A8 A9 )   y 3
1 0 1  (3-14)
 nz 3 oz 3 az 3 pz 3 
 
 0 0 0 1 
根据矩阵对应元素相等，可以解出  5 和  7 。
cos  4  cos  5  oz 3
 5  arccos (3-15)
sin  4  sin  5
CA
 7  arctan 2 (3-16)
sin  4  sin  5  C  a z 3
A  sin  4  cos 5  cos 5  cos  4  sin  5 (3-17)
A  nz 3  sin  4  sin  5  a z 3
C (3-18)
A2  sin 2  4  sin 2  5
再将 A7 乘到公式（3-14）的等号右侧，得到
 nx 4 ox 4 ax 4 px 4 
n oy 4 ay4 py4 
A4 A5 A6  ( A1 A2 A3 )  T9  ( A7 A8 A9 )   y 4
1 0 1  (3-19)
 nz 4 oz 4 az 4 pz 4 
 
 0 0 0 1 
令公式（3-19）左右两侧对应项相等，可以得到
B  cos  5
 4  arctan (3-20)
nx 2  B  cos  4  sin  5
ny 2  cos  4  tan 5  nx 2
B (3-21)
cos 5  cos 2  4  sin 5  tan 5
根据以上所求的各个关节角，可以得到
T5  A1 A2 A3 A4 A5 (3-22)
T6  0T9  ( A7 A8 A9 ) 1 (3-23)
式中 T5 ——远心点在基坐标系下的齐次转换矩阵；
T6 ——器械腕部在基坐标系下的齐次转换矩阵。
所以远心点的空间坐标为：
P5  ( Px , Py , Pz )  (T5 (1, 4), T5 (2, 4), T5 (3, 4)) (3-24)

35

万方数据
 哈尔滨工业大学工程硕士学位论文

腕部点空间坐标为：
P6  ( P ' x , P ' y , P ' z )  (T6 (1, 4), T6 (2, 4), T6 (3, 4)) (3-25)
根据坐标系关系，d6 可以通过远心点与器械腕部两个空间点的距离求出：
d 6  (T6 (1, 4)  T5 (1, 4)) 2  (T6 (2, 4)  T5 (2, 4)) 2  (T6 (3, 4)  T5 (3, 4)) 2 (3-26)
至此求出了完整的一组逆解。在 MATLAB 中进行仿真实验以检验上述运动
学逆解算法的正确性。具体过程为，在各个关节的角度范围内取 80 组关节角，
利用运动学正解公式，在 MATLAB 中画出器械末端点的运动轨迹，再利用上述
的逆解公式，解出每个轨迹点所对应的关节角组合。将得到的组合通过运动学正
解画出此时的器械末端轨迹，与逆解之前的运动轨迹进行对比。实验结果如图
3-4 所示。如图所示，两条曲线完全重合，证明上述运动学逆解算法是正确的。

图 3-4 逆运动学算法仿真验证

3.3 主从一致性映射方法

3.3.1 内窥镜视野下主从一致性映射方法

微创手术机器人相比于传统微创手术方法的一大优势就是能够让操作者直觉
地控制。器械可以很好地重复操作者手部的运动，就好像是双手在腔体中进行操
作。医生只需要专注于手术过程而不必考虑手部操作与手术器械之间的映射。微
创手术机器人将传统微创手术中医生头脑里的映射在控制系统中实现。所以主从
控制策略中的内窥镜视野下主从运动一致性映射方法对于实现手术机器人的直觉
控制至关重要。
在内窥镜视野下主从运动一致，即手术器械在显示器中的运动趋势与主操作
36

万方数据
 哈尔滨工业大学工程硕士学位论文

手的运动趋势相同。要实现这种一致性，首先要确定内窥镜镜头、3D 显示系统、
主手及机械臂之间的位置和姿态关系，各部分的坐标系定义如图 3-5 所示。

持械臂 持镜臂
{D}

{H}
{M}

{L} {S2}
{S1} {I}

图 3-5 机器人系统坐标系模型

图中{H}为主手末端控制点坐标系；{M}为主手基坐标系；{D}为3D显示器
坐标系；{S1 }、{S2 }分别为持械臂与持镜臂的基坐标系；{I}为手术器械末端坐标
系；{L}为内窥镜末端坐标系。
在手术过程中，内窥镜获取手术场景信息，并由显示器显示出来。操作者通
过这一视觉反馈系统操纵机器人。实际上内窥镜相当于一台摄像机，手术器械末
端相对于内窥镜的位置姿态关系即为3D显示屏中手术器械图像相对于屏幕的位置
姿态关系。而为了完成医生的手眼协调，医生手部的动作应与屏幕中器械的动作
运动趋势一致，即在屏幕坐标系下，主手末端与屏幕中图像的运动趋势一致。综
上，主手在显示器坐标系{D}下的转换矩阵应与手术器械在内窥镜坐标系{L}下的
转换矩阵保持一致。上述过程如图 3-6所示。

医生手眼协调 图像反馈系统
屏幕坐标系下 屏幕坐标系下 内窥镜坐标系下
主手的位姿 手术器械图像的位姿 手术器械位姿
运动趋势一致 一一对应

图 3-6 内窥镜视野下主从映射方法

用 MD R 表示主手基坐标系{M}在3D显示器坐标系{D}中的转换矩阵， S 1L R 表示
持械臂基坐标系{S1 }在内窥镜末端坐标系{L}中的转换矩阵，上述关系可表示为：
T  MHT 
D
M T  SS21T  S 1IT
L
S2 (3-27)
在公式（3-27）中，手术器械末端在其基坐标系下的转换矩阵 S 1IT 是待求量，
其他矩阵可以通过一定的方法检测测量。其中 MDT 可以根据主操作手与显示器在

37

万方数据
 哈尔滨工业大学工程硕士学位论文

实际空间中的位置直接测量得出； MH T 可以通过调用主手函数直接读出； SL2T 可以

读取持镜臂上的各关节编码器用运动学正解求出； SS21T 则需要使用第三方设备进
行测量。因此，公式（3-27）可以改写为:
S1
IT  SS 21T  S L2T  HDT (3-28)
 S 1R S1
P
S1
I T  I I
 (3-29)
 0 1 
式中 T ——持镜臂坐标系在持械臂坐标系下的转换矩阵。
S1
S2

根据上面笛卡尔空间中的增量控制策略，将公式（3-29）进行位姿分离，并
写成增量控制的形式，最终得到内窥镜视野下主从映射关系的表达式：
S1
R
I i 1  SN1R  SN2 R  SL2 R  HD Ri 1
(3-30)
S1
P  k  SN1R  SN2 R  SL2 R  ( HD Pi 1 - HD Pi )  SI1Pi
I i 1

式中 i、i+1——分别表示相邻的两个控制周期；
D
H R 、 HD P ——分别表示主手在3D显示器坐标系中的姿态和位置矩阵；
S1
I R 、 SI1 P ——分别表示手术器械在基坐标系中的姿态和位置矩阵；
k ——主从比例放大系数。

3.3.2 机械臂间姿态角视觉测量

为了提供更加灵活的术前摆位方式和更大的工作空间，两条持械臂和一条持
镜臂被设计成分别固定在三个移动基座上。术前摆位时可以根据手术需要拖动基
座，手术时则将移动基座固定。这一结构形式使得术前摆位具有极高的灵活性，
但也造成了三条机械臂之间的位置姿态关系是未知的。由公式（3-28）可知，为
了实现主从一致性映射，必须准确地获得这一关系。进一步由公式（3-30）可知，
只需要测量得到两个持械臂基坐标系相对于持镜臂基坐标系的姿态角即可。因为
采用增量控制的方式，所以并不需要获知机械臂之间的位置关系。而且因为手术
时移动基座固定，所以只需要固定后测量一次即可。
目前，常用陀螺仪、罗盘、角传感器等方式测量姿态。因为本机器人系统只
需要测量一次，所以陀螺仪并不适合。罗盘通过地磁判断姿态，但是手术空间中
存在着一定的磁干扰，无法保证罗盘的精确测量。而对于分体式的机械臂，也无
法使用编码器等角传感器。
以计算机视觉技术为基础，视觉测量技术可以快速准确地测量出目标特征之
间的位置和姿态关系[44]。在图像中提取目标特征的算法有很多，其中霍夫变换是
38

万方数据
 哈尔滨工业大学工程硕士学位论文

较为常用的一种。霍夫变换实现了从图像空间到累加器空间的转换[45]，将特征提
取转化为寻找累加器中的峰值。该算法较为简单，对噪声的鲁棒性好。根据本机
器人系统的需求特点，采用单目视觉技术，通过霍夫变换提取直线特征进行姿态
测量。测量布置图如图 3-7所示。

摄像机
云台 摄像机安装架

左持械臂 右持械臂
持镜臂
标志线

图 3-7 视觉测量布置图

由图所示，设计并加工了摄像机安装架，将其安装在持镜臂上，这样可以获
得较好的视野。将云台安装在摄像机安装架末端。在左右持械臂移动平台的上表
面以及持镜臂的基座上，沿各自的Y轴方向分别粘贴红色线段，作为图像中的目
标特征。红色与图像背景差异较大，便于特征提取。粘贴在上表面上不易被遮挡，
而且因为机械臂上表面可近似认为是水平面，调整云台使得摄像机垂直向下进行
拍摄，这样可以避免引入仿射变换，简化测量模型的同时也能提升精度。经过上
述的配置，就可以通过霍夫变换提取三条直线特征，再求出直线间夹角即得到了
机械臂之间的夹角。而且因为求的是直线间夹角，所以对摄像机在XY平面内的安
装精度没有要求，降低了安装的难度。
在测量过程中，首先使用摄像机进行图像采集并去除噪声。然后将图像中的
红色部分保留，滤除其他颜色部分。再对图像进行二值化。在霍夫变换前先实施
边缘检测，在保留直线特征的同时大大减少点的数目，提高霍夫变换的速度。边
缘检测的方法有很多，其中canny算子是比较流行的一种[45]。该算法能够降低噪
声影响，并能在正确的位置检测到边缘。本文采用该算法进行边缘检测。
边缘检测后对图像进行霍夫变换。因为最关心直线间的角度，所以将霍夫变
换的角度分辨率定为0.1°。然后在霍夫空间中寻找最高的六个峰值，这六个峰值
就代表了三条直线的六条长边。然后将持械臂上直线的角度与持镜臂上直线的角
39

万方数据
 哈尔滨工业大学工程硕士学位论文

度相减即得到机械臂间姿态角度。特征提取过程如图 3-8所示。由图中检测出的
直线，在图像极坐标系下，左持械臂的角度为2.5°，右持械臂的角度为4.9°，
持镜臂的角度为89.8°。则根据三个机械臂的基坐标系关系可得左持械臂与持镜
臂之间的角度为-87.3°，右持械臂与持镜臂之间的角度为84.9°。

(a) 测得图像 (b) 保留图像中的红色部分

(c) 进行边缘特征提取 (d) 霍夫变换直线特征检测

图 3-8 特征提取过程图

上述图像处理过程是在另一台电脑上通过MATLAB软件实现的。将测得的角
度输入控制系统，建立完整的主从映射关系，可以开始进行手术。测量结束后关
闭摄像机。可以上述测量过程流程图如所示。

计算直线
计算直线
去除噪声
去除噪声 霍夫变换
霍夫变换
机械臂
机械臂 图像
图像 二值化处理
二值化处理 间角度
间角度 关闭
关闭
开始
开始 提取红色
提取红色 直线特征
直线特征
术前摆位
术前摆位 采集
采集 边缘检测
边缘检测 输入控制
输入控制 摄像机
摄像机
部分
部分 提取
提取 系统
系统

图 3-9 视觉测量流程图

为了验证视觉测量的准确性，保持机械臂固定不动，再采用加拿大NDI公司
的Optotrak三维测量系统测量机械臂之间的姿态角度。该系统需要在机械臂上粘
贴探测标识点，标识点发出特定波长的红外光线，被位移传感器上的三个CCD摄
40

万方数据
 哈尔滨工业大学工程硕士学位论文

像机捕捉到，从而得到标识点的三维坐标。通过三个标识点可以确定机械臂的刚
体坐标系。测量持械臂与持镜臂的空间坐标系即可得到机械臂间的姿态角度关系。
该测量系统的位置精度很高，可达0.1mm，分辨率为0.01mm。该系统的缺点是在
实际手术应用中易受到遮挡，无法完成测量，而且体积太大，也不易于安装到手
术空间的上方。该系统的测量配置如图 3-10所示。

Y轴

NDI 三维测量设备 -X 轴

-Z 轴
Mark 点

(a) 测量配置图 (b) Mark点粘贴细节

图 3-10 使用 NDI 设备测量配置图

通过 Optotrak 三维测量系统得到左持械臂坐标系在持镜臂坐标系下，绕 Z 轴
旋转的角度为-87.71°，误差为 0.41°。右持械臂坐标系在持镜臂坐标系下，绕 Z
轴旋转的角度为 84.62°，误差为-0.28°。然后多次改变三个机械臂的位置和姿
态，重复上述过程，总共测得十组数据，如表 3-2 所示。
表 3-2 NDI 测量与视觉测量数据表

实验 左臂角度 左臂角度 右臂角度 右臂角度

误差 误差
序号 (NDI 测量) (图像测量) (NDI 测量) (图像测量)
1 -97.65 -97.2 0.45 86.28 86.5 0.22
2 -99.86 -99.5 0.36 83.00 83.3 0.30
3 -100.88 -101.5 -0.62 89.86 89.6 -0.26
4 -108.87 -108.64 0.23 96.46 96.0 -0.46
5 -103.58 -103.9 -0.32 78.04 78.4 0.36
6 -98.39 -98.8 -0.41 73.97 73.6 -0.37
7 -101.50 -101.8 -0.30 67.97 67.4 -0.57
8 -96.43 -96.8 -0.37 77.13 77.4 0.27
9 -93.50 -93.2 0.3 83.00 83.5 0.50
10 -92.87 -92.3 0.57 86.38 86.9 0.52

41

万方数据
 哈尔滨工业大学工程硕士学位论文

左 臂 角度 的误 差绝 对值 均 值为 0.393° 。右 臂角 度的 误差 绝 对值 均值 为
0.383°。因此，用视觉方法测量机械臂之间的姿态角度精度较高。而且系统配置
简单，采集处理速度快，所以可以达到本手术机器人系统的要求。

3.4 主从控制功能实现

3.4.1 主从术中调整

在主从一致性映射的位置映射中添加细分系数，将主手采集到的操作者手部
动作的位置增量，按照本次主从连接前设置的细分系数进行缩小，使得从手器械
的运动按比例缩小，达到提升操作分辨率的目的。本手术机器人将细分系数设置
为 1:5、1:3 以及 1:1 三档。而对于主从姿态映射，人手控制比较精准，所以无需
增加细分系数。
由于本手术机器人系统在结构上主从异构，控制策略采用增量控制的方式，
再加上比例系数的影响，使主手工作空间与从手工作空间难以对应，从而造成了
手术过程中，主手操作空间易到达极限位置或者操作不舒适位置等问题。而且如
果主从之间一直处于连接的状态，不仅难以实现术中更换手术器械，也大大增加
了手术操作者的精神和体力劳动强度。
研究主从术中调整的功能克服上述问题。根据人机工程学，为手术操作者提
供了连接\断开主从映射的脚踏开关。在手术过程中手术操作者可以在主手到达极
限位置或者是不舒服位置的时候断开主从连接，调整主手的位置和姿态，再重新
建立起主从连接。这一过程中从手保持不动，确保了手术的安全性。当手术进行
到需要精细操作的环节，操作者也可以踩下脚踏开关断开主从映射，重新选择细
分比例系数之后再重新开启主从映射。在更换手术器械时，踩下脚踏开关断开主
从映射，由助手将器械手动抽出，更换完新的器械后再插入腔体内，这时再踩下
脚踏开关开启主从映射，继续进行手术过程。需要注意的是，采用的 omega.7 型
主手设备在三个姿态关节上没有设置电机，无法锁住当前姿态角度，所以断开主
从连接后，主手的姿态一定会发生变化。
主从术中调整过程中，主手的位姿发生变化而从手静止不动。再次进行主从
映射，需要先采集一次主手关节角度，更新当前主手的位置和姿态矩阵，并将位
置和姿态增量设置为零。术中主从调整后建立主从位置映射的初始条件为：

 H P( t 1)  H P( t )
M M

 M (3-31)
d ( H P(t 1) )  H P(t 1)  H P(t )
M M

42

万方数据
 哈尔滨工业大学工程硕士学位论文

式中 t ——主从映射重新开始的时刻；
t+1——主从映射重新开始后的第一个控制周期到达时刻；
d ( HM P( t 1) ) ——主从映射重新开始后第一个控制周期内的增量。
姿态初始映射条件为：
  
 t 1     t

 d    t 1     t 1     t (3-32)
M
 R  R R R 
 H ( t 1)  d  0 d    0 d 0  t 1
式中 α、β、η——分别为主手三个旋转轴对应的转动角度；
d    t 1 ——主从映射重新开始后第一个控制周期的姿态角增量；
α0 、β0 、η0 ——主从映射重新开始时从手机械臂对应的主手姿态角；
为了检验上述主从术中调整功能的性能，在主从模式下进行实验，记录主手
末端和从手各关节的轨迹。将各关节轨迹经过运动学正解转换为从手末端的轨迹。
画出主手和从手的轨迹曲线。如图 3-11 所示。

主手调整阶段

图 3-11 主从术中调整图

由图中所示，为了直观，取两条轨迹在 XY 平面内的投影进行比较。主手先
沿蓝色轨迹运动，从手跟随主手的运动，其轨迹也用蓝色画出。这时断开主从映
射，对主手进行调整，从手保持固定不动。主手调整完成后，连接主从映射，主
从两端的运动轨迹均用红色画出。再次调整主手，完成后连接主从映射，轨迹用
绿色画出。主手和从手的运动趋势始终相同。主手调整结束后，手术器械末端点
从运动停止的位置继续运动，没有发生位置突变。另外主从两端轨迹曲线不同是
因为经过了内窥镜视野下的主从映射。主从轨迹在笛卡尔空间中并不一致，从手
的轨迹发生了与视角有关的变形。
43

万方数据
 哈尔滨工业大学工程硕士学位论文

3.4.2 主手抖动滤除

正常人手部的抖动可被分为病理性抖动和生理性抖动两种。其中生理性抖动
是非自主的手部运动，具有近似于正弦函数的运动规律。手部生理性抖动的频率
在 8-12Hz 左右，每个方向上的幅值在 50 微米左右[46]。当疲惫或精神高度紧张时，
幅值会有较大幅度的变化，频率也会在一定范围内变化。而自主的手部运动频率
不会超过 2Hz。
对于不同的手术类型，要求的手术精度也不同。一般来说，微创手术要求定
位精度达到 0.1~1 毫米左右。虽然外科医生经过专门的训练，手部的抖动幅值比
正常人小一些，但对于精细的微创手术来说，手部的生理性抖动还是会给手术精
度带来不小的影响。
运用微创手术机器人系统，在采集手部运动信息后就将信息中的抖动部分滤
除，使得抖动信号不会通过主从映射在从手机械臂端显现出来，从而提高了手术
的精度，对操作者的要求也相应降低。对于滤除抖动信号的方法，因为手术机器
人系统对于实时性的要求很高，传统的线性滤波器虽然可以抑制抖动，但会引入
时间延迟，而且延迟还与主从控制周期有关，主从控制周期越长，线性滤波器对
实时性的影响越大。所以采用的滤波器必须能够实时地区分出自主信号和非自主
的抖动，并且将抖动信号实时地滤除。
根据主动信号和抖动信号的频率特点，滤波算法应将较高频率的部分滤掉。
综合主手采集数据和工控机处理信号的能力，采用带限的线性傅立叶拟合算法[47]
（BMFLC）进行抖动滤波。
因为可以将抖动信号近似视为正弦函数，所以选取一组合适频率的正余弦函
数，经过加权求和可以得到对抖动信号的近似，进而从主手采集到的信号中将抖
动部分减去，得到比较纯净的主动输入。各频率分量的权重值不能是一成不变的，
应当随着输入信号进行自适应的变化。加权线性傅立叶拟合算法[48]（WFLC）的
基本思想是选 定一个基频 ，以基频 的整数倍 为一组频率 进行自适 应拟合。而
BMFLC 算法是在设定好的滤波频率带（f 0 , f）中，将 f 0 和 f 之间的 n 个等分点作
为基本拟合量的频率，相当于有 n 个基频在同时进行拟合。滤波效果比 WFLC 要
好得多。两种算法都采用最小均方算法实现权重值的自适应变化。BMFLC 递归
形式如公式（3-33）所示。

44

万方数据
 哈尔滨工业大学工程硕士学位论文

 r 1
sin(2 ( f 0  G ) k ),1  r  L
xrk  
cos(2 ( f  ( r  L )  1) k ), L  1  r  2 L
 0
G
 k  S k  Wk T X k (3-33)
Wk 1  Wk  2  X k  k
式中 k——时间序数；
L——频率分块个数，且 L  ( f  f 0 )  G ；
Sk——k 时刻主手采集到的信号值；
εk——k 时刻手部主动运动的估计值；
Wk——k 时刻的权重值；
μ——收敛速率系数。
收敛速率系数 μ 需要根据信号进行合理选择。μ 太小则收敛缓慢，μ 太大则序
列 发 散 。 设 计 仿 真 实 验 检 验 BMFLC 算 法 的 滤 波 性 能 。 对 于 信 号
x=sin(2×pi×0.5×t)+0.3×sin(2×pi×10×t)，选取 f 0 =8Hz，f=12Hz，L=40，μ=0.003，对
信号进行滤波，得到结果如图 3-12 所示。

图 3-12 对已知信号进行 BMFLC 滤波

由图中可看出振动波纹显著减小。将该算法应用到手术机器人系统中，对主
手采集到的三个位置分量信号进行滤波。对于算法参数的选择，为了保证整个系
统的实时性，不仅主手的采样频率不能过快，滤波算法中频率区间的划分也不能
过密。针对现有系统，主手采样频率设置为 1KHz，取各参数设置为 f 0 =8Hz，
f=12Hz，L=40，μ=0.001，对 x 方向的信号进行滤波得到结果如图 3-13 所示。由
图中所示，信号中频率为 10Hz 左右的抖动信号被滤掉，保留了低频的主动信号。

45

万方数据
 哈尔滨工业大学工程硕士学位论文

图 3-13 主手位置信号滤波

3.4.3 操作安全限制

由于本手术机器人系统尚未实现力反馈功能，在手术器械与人体组织等物体
接触时，不能给手术操作者提供力信息。在手术过程中，操作者只能通过三维视
觉显示系统提供的手术现场画面这一唯一信息源进行决策和判断。由于手术空间
狭小，内窥镜提供的视野有限，易造成视线的遮挡，妨碍操作者正确判断，从而
造成安全隐患。而且操作者必须始终密切关注显示画面，易造成操作者的精神压
力与疲劳。为了解决这一问题，目前最常用的方法是设计手术导航系统，引导和
约束手术器械在空间中的运动，实现操作安全限制。
针对本手术机器人系统，利用 omega.7 主手设备可以在空间中施加任意方向
力的特点，实现操作安全限制功能。操作者可以在手术过程中通过闭合夹持关节
选取空间中的点，建立特定形状的安全限制区域。这样医生就可以根据手术的实
际情况进行设置，辅助手术操作。因为是在手术过程中手动建立，所以安全限制
区域的形状不宜复杂，而且设置的过程要简便合理。本文为操作者提供直线、平
面和球形三种形状的安全限制模式。
直线安全限制模式可应用于插入、抽出等手术操作。设置直线安全限制区域，
操作者需要先在主从控制过程中踩一下操作安全限制脚踏开关。采用脚踏开关是
因为操作者的双手正操纵主手设备，无法按键。然后操作者就可以在主手工作空
间中的适当位置闭合主手上的夹持关节，选取当前的点坐标，再在下一个位置闭
合夹持关节，共选取两个点的位置坐标，这样就确定了主手工作空间中的一条直
线。主手只能沿这条直线运动，从而实现了直线安全限制区域的设置。
46

万方数据
 哈尔滨工业大学工程硕士学位论文

软件中具体的实现方法为：如果主手的当前位置偏离了这条直线，则计算当
前位置在直线上的投影，得到从当前位置点到投影点的空间向量，该向量与设置
的直线垂直。并采集主手当前的速度向量。利用弹簧阻尼模型计算该向量上应施
加的力 F ，示意图如图 3-14 所示。

k·P F

C·V

图 3-14 弹簧阻尼模型示意图

得到如下公式：
F  K  P  C V (3-34)
式中 F ——力向量；
P ——当前位置点到投影点的空间向量；
V ——当前速度向量；
K ——弹簧系数，设置为 2000；
C ——阻尼系数，设置为 20。
因为当前速度向量 V 与 P 不一定重合，计算得到的力 F 可能与 P 不重合，所
以还要将 F 投影到 P 上，保留 P 方向上的分量。最后调用 dhdSetForce()函数将这
一空间力矢量施加到末端执行器上。所以如果主手末端执行器离开设定的直线安
全限制区域，操作者就会感受到力的作用，在力的推动下回到安全限制区域。
平面安全限制模式主要为了防止误操作手术器械越过虚拟平面，伤及健康组
织器官。设置方式与直线安全限制模式类似，首先在主从控制模式下踩两下操作
安全限制脚踏开关，进入平面安全限制模式。然后在空间中选择三个点，确定虚
拟平面。并设定法向量的 X 轴分量为正为平面以里，即靠近操作者一边为内侧。
这样主手就只能在平面以里进行运动。具体实现方式为如果主手末端执行器到达
平面外，计算当前位置点在平面上的投影，然后利用弹簧阻尼模型计算力矢量，
并计算力矢量沿投影矢量的分量。将该分量施加到主手上则能将主手末端执行器
推回平面以内，从而实现操作安全限制的功能。
47

万方数据
 哈尔滨工业大学工程硕士学位论文

球形安全限制模式主要用于划定病灶区域，使得手术器械只能在该球形区域
内运动。设置方法为在主从控制模式下踩三次操作安全限制脚踏开关，转换到球
形安全限制模式，在主手工作空间中先选定球心，再选定球面上一点，确定空间
中的球形，主手末端执行器只能在这一球形中运动。具体实现方法为如果主手末
端执行器运动到球外，则计算当前位置到球心的向量。再利用弹簧阻尼模型计算
该向量上的力，施加到主手上，使得主手末端执行器被推回球形之内。
如果操作者要取消安全限制区域或对上一个选取的空间点位置不满意，可以
踩下取消操作安全限制脚踏开关，取消已经设定好的安全限制区域和选定的空间
位置点，回到无安全限制的主从控制状态。而且主手只能为同侧的从手机械臂设
置安全限制，另一侧的从手机械臂不受其影响。需要注意的是本文通过限制主手
末端执行器的空间自由度来限制从手端手术器械的空间自由度，实现操作安全限
制的功能。这一方案建立在上面讨论的内窥镜视野下主从运动一致控制策略上，
只能防止主手端操作者的误操作，不能防止从手机械臂出现的安全问题。而且因
为安全限制区域是建立在主手操作空间的，不随主手末端移动而改变，所以不能
在术中断开主从连接进行主手的位置和姿态调整。主手调整结束后可能已经处于
安全限制区域之外，而从手机械臂在调整过程中保持不动，还处在安全限制区域
之内，从而造成错误。所以一旦在操作安全限制模式下切断主从连接，则设置的
安全限制区域自动取消。为了在使用安全限制功能的过程中尽量避免进行主从术
中调整功能，应在设定安全限制区域前适当调整主手位置和姿态。
为了检验术中操作安全限制功能的有效性，模仿手术过程中分别设置三种安
全限制区域的过程，记录下主手运动的轨迹数据。分别画出三种安全限制模式下
主手运动的空间曲线，如图 3-15 所示。由图中所示，可以根据要求限制主手运动
的空间自由度。三幅图中安全限制区域外的轨迹是设定时造成的。设定完成后，
在主手施加的力作用下，主手末端在规定的范围内运动。

(a) 直线安全限制区域 (b) 平面安全限制区域

48

万方数据
 哈尔滨工业大学工程硕士学位论文

(c) 球形安全限制区域

图 3-15 三种安全限制模式下主手运动曲线

3.5 本章小结

本章首先介绍了手术机器人系统的结构配置，然后针对结构上主从异构的特
点，提出了内窥镜视野下基于笛卡尔空间的主从增量控制策略。这一控制策略使
得手术操作者可以手眼协调，直觉地操作手术机器人系统完成手术。针对这一策
略中的几个关键部分进行了讨论。构建了从手的坐标系模型，求解了机械臂的运
动学。研究了内窥镜视野下主从一致性映射方法，确定了主操作手和从手机械臂
之间的映射关系。由于建立这一映射关系需要得知机械臂之间的姿态关系，采用
视觉测量技术进行测量。采用霍夫变换进行直线特征提取，再根据直线间的角度
差求出机械臂之间的姿态关系。使用 NDI 设备验证了视觉测量的结果。结果表明
视觉测量精度很高。本章最后研究了几种主从控制功能。其中主从术中调整功能
使得操作者可以在手术过程中进行调整主手到舒适工作空间，切换主从映射比例
和更换手术器械等操作。采用带限的线性傅立叶拟合算法将操作者手部的生理性
抖动滤除。在主手上作用力实现操作安全限制功能。

49

万方数据
 哈尔滨工业大学工程硕士学位论文

第4章 微创手术机器人实验研究

上文研究了基于 QNX 的实时控制系统构建，搭建起了控制系统的软硬件框

架，又研究了主从控制策略与方法，是控制系统的核心内容。本章将这两部分整
合起来，构建完整的控制系统。并在此基础上开展实时性能对比实验和主从控制
实验。最后开展活体动物实验，验证系统整体的各项性能，为临床试验奠定扎实
的基础。

4.1 微创手术机器人系统集成

微创手术机器人系统如图 4-1 所示，共分为三个子系统：主控台、分体式从

手机械臂和 3D 视觉显示系统。下面对各部分功能和组成特点进行总结介绍。

(a) 从手端系统图 (b)主手端系统图

图 4-1 微创手术机器人系统图

分体式从手机械臂子系统由三条从手机械臂组成，有两条持械臂和一条持镜
臂，如图 4-1 (a)所示。每一条机械臂上都安装有一个多轴网络控制器 GMAS。根
据微创手术的特点，一般左持械臂夹持夹钳式手术器械，中间机械臂夹持内窥镜，
右持械臂夹持剪刀或高频电刀等手术器械。三条从手机械臂结构类似，都由一个
被动移动关节，两个被动旋转关节和一组远心定位关节组成。主要区别在于持械
臂末端设计有手术器械快换接口，而持镜机械臂末端设计有 3D 内窥镜的夹持机
构。三个机械臂分别固定在各自的可移动平台上，在术前由医生调整到理想的位
50

万方数据
 哈尔滨工业大学工程硕士学位论文

置和姿态后，将移动基座锁死。然后进行术前设置工作。当医生在患者的腔壁上
建立好手术通道后，手术助手手动调整移动关节 1、旋转关节 2 和旋转关节 3 将
机械臂与手术套管进行连接，然后将上述三个关节掉电锁死。将内窥镜安装到持
镜臂上，调整远心机构的远心点到腔壁上，将内窥镜插入到腔体内。在内窥镜的
视觉导引下，将手术器械通过建立的手术通道插入到腔体内，医生逐个调整远心
机构关节使手术器械指向理想的手术区域。在完成上述的术前设置后，医生便可
坐到主控台前进行操作。
主控制台如图 4-1 (b)所示，在手术过程中充当医生与机器人之间的人机接口。
工控机安装在主控台内。在工控机上移植 QNX 系统，分别通过 MODBUS/TCP
协议与 GMAS、USB 接口与主手设备、RS232 串口与控制面板单片机进行通信。
将第三 章研 究的 主从 控制 策略 填入 第二 章建 立的 软件 系统 框架 中， 在 QNX
Momentics IDE 集成开发环境中进行程序开发、编译和联机调试。将调试通过后
的可执行文件下载保存到工控机中，修改工控机外存中的 sh 文件，将控制程序设
置为开机自启动。
主手设备也安装在主控台上。主控台上还设置有脚踏开关方便医生在操作主
手的同时进行按键输入。六个脚踏开关分别实现“主从连接/断开”、“持械臂/
持镜臂切换”、 “电刀电切”、“电刀电凝”、“安全限制区域设置”和“取消
安全限制区域”功能。“主从连接/断开”脚踏开关控制主从映射的连接与断开，
踩一下连接上，再踩一下断开，可用于主从术中调整、器械更换、持械臂、持镜
臂切换等多种功能。“持械臂/持镜臂切换”脚踏开关可切换当前主手控制的机械
臂。该脚踏开关只能在主从映射断开时使用，确保了安全性。当切换到主手控制
持镜臂模式时，踩下“主从连接”脚踏开关，缺省设置为右主手与持镜臂之间建
立主从映射。“电刀电切”“电刀电凝”等帮助医生进行高频电刀操作。“安全
限制区域设置”与“取消安全限制区域”脚踏开关控制操作安全限制功能。
主控台上还设置有两块按键面板，如图 4-2 所示。与脚踏开关不同，控制面
板上的按键用于术前设置或者是术中主从映射断开后医生能够空出双手时的设置。
因为按键较多，根据功能分别设置在两块控制面板上。图 4-2(a)中，“比例尺”按
键多次按下可用于切换主从映射比例，“常规”指示灯表示 1:1，“精密”指示
灯表示 3:1，“超精密 ”指示灯表示 5:1。右边“主控制器联合功能”中“1”表
示左持械臂，“2”表示右持械臂，“3”表示持镜臂。“左”和“右”指示灯表
示是由左主手还是右主手控制。图 4-2(b)中，上半部分可选择任一条机械臂上的
任意一个主动关节，再按加号或减号按钮调整这一关节的位置。该功能主要用于
术前远心机构定位。下半部分包括器械选择和器械归零等按钮。
51

万方数据
 哈尔滨工业大学工程硕士学位论文

(a) (b)
图 4-2 按键控制面板图

另外主控制台上还设置有危险警示灯，当控制系统检测设备异常或机械臂运
动到极限位置，会发出相应的警告。控制台上设置有总电源开关以及急停开关，
当出现危险状况时可以紧急制动切断电源。
微创手术机器人所使用的手术器械都采用钢丝传动的形式，使得提供动力的
电机可以设置在远离操作区域的位置。系统配置的手术器械种类包括高频电刀、
持针器、组织钳和剪刀等，利用上述几种器械可以实现活体动物胆囊切除手术的
主要操作。3D 视觉反馈系统包括 3D 内窥镜、光源设备以及 Sony LMD-3251TC
型 3D 偏振式立体显示器。在操作时，医生佩戴偏振眼镜即可观察病灶区域三维
图像，获得较为全面的图像信息。

4.2 控制系统实时性能对比实验

在微创手术机器人领域，国内对于实时控制系统的发展重视不足。目前国内
发展较为成熟的手术机器人控制系统大部分都是基于 Windows 构建的。Windows
系统是目前最为流行一种操作系统，其支持的软件最多，人机界面友好，在其上
开发软件较为容易。然而 Windows 并非实时操作系统，无法保证在所有情况下的
及时响应。而且 Windows 提供的任务间同步方式等过于抽象，不利于底层开发。
实验室原有针对手术机器人开发的控制系统也是基于 Windows 构建的。基于
QNX 构建的控制系统，底层依靠 QNX 的实时资源分配与线程调度策略，可以实
现很高的实时性能。为了将两种操作系统进行比较，评价本文基于 QNX 构建的
控制系统的实时性能，将实验室原先基于 Windows 的控制系统进行修改，运用第
三章研究的主从控制策略与方法。这样使得两种控制系统控制算法相同，只有实
现的操作系统不同，使两者具有可比性。

52

万方数据
 哈尔滨工业大学工程硕士学位论文

4.2.1 运行时间对比实验

如果在控制周期内，系统在最坏的情况下也能完成所有的控制工作，并留有
一定的空闲时间，则认为系统能够对外界输入进行及时响应，即具有很高的实时
性能。所以周期内运行时间是一个关键的指标。分别测量两种控制系统下的周期
内运行时间，进行对比实验。
为了尽量模拟实际手术过程，在主从控制模式下，分别利用 Windows 下控制
系统和 QNX 下控制系统，操作主手控制机械臂的运动。为了达到较好的对比效
果，使用操作安全限制功能，令两次实验主手运动相同的直线轨迹，并尽量控制
手移动的速度一致。先对基于 Windows 的控制系统进行实验，使左右持械臂控制
线程各运行 3000 次，然后自动退出，采集并记录每次主从控制的运行时间。线
程周期设置为 20ms。在控制周期内，如果线程能够执行完主从控制任务，则当次
记录时间为周期内运行时间。如果周期时间到而主从控制还未结束，则线程重新
进入主从控制循环，当次记录时间为周期时间 20ms。然后对基于 QNX 的控制系
统，执行相同的过程，记录 3000 个周期的运行时间。基于 QNX 的控制系统中线
程周期设置为 10ms。统计实验数据，结果如表 4-1 所示。
表 4-1 两种系统线程运行时间对比表

Windows 系统下 Windows 系统下 QNX 系统下 QNX 系统下

左持械臂线程 右持械臂线程 左持械臂线程 右持械臂线程
平均值 6.0274ms 6.1105ms 3.9138ms 4.0152ms
最大值 13.4451ms 13.0063ms 6.2018ms 6.3493ms
均方差 4.2329e+11 5.0249e+11 2.5643e+10 2.8731e+10
画出两种控制系统运行时间对比图如图 4-3 所示。

(a) 基于 Windows 的控制系统中左、右臂线程运行时间图

53

万方数据
 哈尔滨工业大学工程硕士学位论文

(b) 基于 QNX 的控制系统中左、右臂线程运行时间图

图 4-3 两种系统线程运行时间对比图

从图表中可以看出基于 QNX 的控制系统运行速度更快，而且稳定性明显优

于基于 Windows 的控制系统，可以保证在 10ms 周期内做出响应。而 Windows 系
统因为内核机制的原因，即使增大周期长度也无法保证一定能在周期内做出响应。
所以基于 QNX 的控制系统实时性能优于基于 Windows 的控制系统。

4.2.2 延迟时间对比实验

延迟时间指的是从主手采集到数据开始，至电机运动到指定位置的时间，是
衡量系统实时性能的另一项重要指标。为了更好地体现出系统实时性能对主从控
制的影响，针对两种控制系统，分别测量延迟时间进行对比。
延迟时间可分为两部分，分别是控制系统延迟时间和电机延迟时间。控制系
统延迟时间指的是从采集到主手数据到 GMAS 发给电机运动指令的时间。两种控
制系统的差异主要体现在这一部分。而电机延迟时间则是电机从接收到指令到运
动完成的时间。这部分时间主要由电机驱动能力和机械结构决定，与控制系统无
关。首先测定电机延迟时间。选取第四关节电机进行测量。因为第四关节是第一
个主动运动关节，转动惯量最大，电机延时时间应最长。使用 EAS 软件控制第四
关节进行频率为 0.8Hz、幅度为 3000 个脉冲的正弦运动。记录电机的命令值和编
码器实际读取的位置值，分别画出两条曲线，如图 4-4 所示。
由图中可以看出，电机响应能力强，能够及时跟随命令曲线。一般延迟在 2
到 3 毫秒左右，在加速度峰值处，延迟较大，从局部放大图可以看出，最大延迟
可达 25 毫秒左右。综上，取 30 毫秒为电机最大延迟时间。

54

万方数据
 哈尔滨工业大学工程硕士学位论文

放大

(a) 正弦位置响应曲线 (b) 局部放大图

图 4-4 第四关节电机响应曲线图

然后测量控制系统延迟时间。为了实现实验的可重复性与可比性，不采用主
手进行数据输入，而是编制数据文件输入系统，使得机械臂末端分别在 X-Y 平面
内画出一个矩形。在机械臂运动过程中，采集并记录控制系统延迟时间和各个编
码器的读数，并运用运动学正解求出机械臂末端运动轨迹。Windows 下控制系统
和 QNX 下控制系统机械臂末端运动轨迹如图 4-5 所示。

图 4-5 两种控制系统下机械臂末端轨迹图

由图示，两种控制系统均能使得机械臂末端沿指定轨迹运动，而且轨迹较为
平滑，精度较高。统计实验数据，QNX 下控制系统平均延迟时间为 4.158ms，最
长延迟时间为 6.294ms。Windows 下控制系统平均延迟时间为 7.681ms，最长延迟
时间为 14.816ms。为了便于展示，将运动轨迹上每一点的控制系统延迟时间作为

55

万方数据
 哈尔滨工业大学工程硕士学位论文

Z 轴数据添加到图 4-5 中，画出延迟时间三维对比图如图 4-6 所示。

图 4-6 延迟时间三维对比图

由图所示，QNX 下控制系统延迟时间波动较小，而 Windows 下延迟时间波

动较大。将测得的控制系统延迟时间与最长电机延迟时间相加，即为总延迟时间。
两种系统总延迟时间都远远低于医生可忍受的极限延迟时间 330ms[49]。但 QNX
系统能够保证在主从控制周期内完成任务，所以可保证最大延迟时间不超过电机
延迟时间加周期时间，即 40ms，而 Windows 系统无法保证这一点。所以基于
QNX 的控制系统具有更好的实时性能，并可以应用于实际手术。

4.3 基于 QNX 系统的主从控制实验

4.3.1 主从轨迹对比实验

出于检验主从控制策略与方法是否正确有效的目的，在主从控制模式下控制
机械臂在内窥镜三维定位训练模块上进行触碰、抓取和套圈等操作。实验中先控
制手术器械触摸训练板上的各个圆柱，同时记录主手末端执行器和持械臂、持镜
臂上各关节的轨迹。将实验获得的持械臂关节轨迹转换为机械臂末端手术器械在
持械臂基座标下的轨迹。为了便于对比观察，通过持镜臂的运动学正解，将该轨
迹变换为内窥镜视野下的手术器械轨迹。画出主从两端轨迹图如图 4-7 所示。
将两条轨迹进行对比，可以发现从手跟随主手运动趋势相同，轨迹吻合度很
高。说明基于 QNX 的控制系统可以快速稳定地响应外部输入，而且本文研究的
56

万方数据
 哈尔滨工业大学工程硕士学位论文

主从控制策略能够使得机器人很好地复现操作者手部的运动。但是图中两条轨迹
也存在一定的偏差，主要原因有从手机械臂各关节正反方向运动换向时会产生一
定量的回程差；由钢丝驱动的手术器械在运动过程中也会产生一定的误差。

图 4-7 主从两端轨迹对比图

4.3.2 抓取和套圈实验

为进一步验证主从控制性能，进行了单手抓取和套圈实验。单手抓取和套圈
实验是用一个手术器械将圆环从圆柱上抓起，再放到另外一个圆柱上。实验过程
如图 4-8 所示。

图 4-8 单手抓取和套圈实验过程图
57

万方数据
 哈尔滨工业大学工程硕士学位论文

实验结果为操作者在经过简单的训练后，就可以操作本手术机器人系统轻松
完成这两个动作，表明其主从控制策略是成功的，能够很好地实现操作者的手眼
协调，具有良好的操控性能。但是也存在一定的问题。虽然三维视觉系统可以提
供较完整的视觉信息，但是因为没有实现力反馈功能，操作者缺少力信息还是会
做出不恰当的判断。从而出现图 4-8 中圆柱被撞歪的情况。这一缺陷可通过对操
作者进行一定的训练进行拟补。
综合上面两个主从控制实验，结果证明，基于 QNX 系统实现的主从控制策
略有能力在实际手术中进行应用，为下一步进行动物实验提供了经验和佐证。

4.4 活体动物实验

为研究本微创手术机器人系统在具有生命体征对象上应用的稳定性与安全性，
在专业外科医生的协助下，应用本手术机器人系统开展了多例猪胆囊切除手术。
胆囊切除微创手术是比较常用的微创手术之一，该手术将病变的胆囊器官从
腹腔中切除并从手术通道中取出。因为胆囊贴合在肝脏上，之间有结缔组织相连，
所以手术过程中需要用高频电刀将结缔组织仔细切开，同时不能伤及健康的肝脏
组织，也不能将胆囊切破造成胆汁流出。胆囊还与血管和胆囊管相连，需要在手
术过程中切断，并保证只有少量血液和胆汁流出。一般先在管道两边适当位置各
加一个钛夹，然后再用剪刀从中间切断管道。最后把切除的胆囊放入袋中，经手
术通道取出。选取猪作为实验动物，因为猪的肝胆结构与人类相似，体积大小也
相差不大。为了便于操作，选用未成年的小猪。
在实验准备阶段，所选取的猪重量为 30kg，手术所需要的仪器设备有高频电
刀系统、气腹机、麻醉机、内窥镜系统、电子吸附器、医用手术器械等。其中医
用手术器械包括夹钳及抓钳、刀及刀柄、套管(trocar)、气腹针、穿刺针、钛夹、
钛夹钳、输液架、注射器、备皮刀、无菌塑料套等。相关药剂有乙醚溶液、3%戊
巴妥钠溶液、安定试剂、生理盐水、葡萄糖溶液等。
在进行实验前三十分钟对猪进行麻醉操作。先在室外对猪实施吸醚操作，然
后将猪固定在手术台上，插上呼吸机，进行全身麻醉操作。按照猪的体重每千克
注射 1ml 3%戊巴妥钠溶液，15 分钟后猪进入深层麻醉状态。然后实施备皮操作，
清理腹腔外部皮肤。在猪的背后贴上高频电刀系统的负极板。再然后开始进行机
器人系统的初始设置操作。实验过程及结果如图 4-9 所示。图 4-9(a)展示了术前
的麻醉过程，麻醉师正在进行插管操作。(b)图展示了从手端的操作状态。(c)图展
示了手术画面的细节。这时左持械臂使用组织钳将胆囊提起，右持械臂使用高频

58

万方数据
 哈尔滨工业大学工程硕士学位论文

电刀将肝脏与胆囊之间的结缔组织切除。(d)图展示了主手端的操作状态。(e)图展
示了切除后取出的胆囊。(f)图展示了术后猪的成活情况。

(a) (b) (c)

(d) (e) (f)

图 4-9 动物实验过程及结果

手术过程中，医生坐在主控台前操作主手设备，手眼协调地控制器械完成操
作。主从增量控制的策略保证了主从两端运动趋势相同，主从术中映射功能使得
医生可以自由地调整主手的位置和姿态，提高了操作的舒适度。在胆囊与肝脏粘
连较为紧密的地方，提高主从映射的缩小比例，实现对手术器械更加精确的控制。
最终手术成功把猪胆囊从腹腔中切除并取出。胆囊没有破裂，流血量也很少，而
且没有造成对猪肝脏的损害。猪在手术结束 30 分钟后自然苏醒。饲养一周后未
发现任何异常。手术取得了圆满成功。
手术过程中也发现了一定的问题。在某些手术位置，肝脏会覆盖在胆囊上，
需要用手术器械将肝脏挑起。这时医生的双手正拉住胆囊进行切除操作，无法完
成挑起肝脏的操作，需要手术助手在一旁用器械辅助完成。这样就降低了手术机
器人系统的自动化程度。
在手术时间方面，手术准备时间，包括猪的清洗、麻醉和固定等，总共耗时
约一小时。手术操作时间为两个小时左右。
活体动物实验证明本手术机器人的控制系统具有很高的稳定性与安全性，能
59

万方数据
 哈尔滨工业大学工程硕士学位论文

够满足实际手术应用的需要。控制系统实时性能好，能够快速及时地反应医生的
主手运动输入操作及按键操作等。成功运用主从控制策略，使得医生能够手眼协
调，直觉地控制手术器械进行操作。主手抖动滤除、主从术中调整及操作安全限
制等控制功能使用灵活方便，提高了手术机器人系统的使用性能与手术表现。活
体动物实验为将来的人体临床试验打下了坚实的基础。

4.5 本章小结

本章对机器人系统的各部分功能进行了详细论述，将基于 QNX 构建的控制

系统软硬件结构与主从控制策略和方法相结合，形成了完整的控制系统。为验证
控制系统的整体性能，进行了一系列实验研究。开展了实时性能对比实验，突出
了基于 QNX 的控制系统实时性。开展了主从控制实验，检验了主从控制策略与
方法的有效性。开展了活体动物实验，验证了控制系统的综合性能。实验表明，
控制系统稳定性与安全性强，实时性能良好。各主从控制功能提高了系统的使用
性能。相关测试实验的开展，为微创手术机器人应用于临床打下了扎实的基础。

60

万方数据
 哈尔滨工业大学工程硕士学位论文

结 论

本文针对微创手术机器人系统高稳定性与高安全性的需求特点，从实时主从
控制的角度出发，开展了实时控制系统及主从控制方法研究。基于 QNX 实时操
作系统构建了控制系统软硬件框架。针对机器人系统主从异构的结构特点，研究
了内窥镜视野下基于笛卡尔空间的主从一致性增量控制策略。实现了主从术中调
整、主手抖动滤除和操作安全限制等主从控制功能，提升了机器人系统的操作性
能。进行了微创手术机器人控制系统集成，开展了系统对比试验、主从控制实验
和活体动物实验，验证手术机器人控制系统的整体性能。论文取得了以下研究成
果：
(1) 基于 QNX 实时系统构建了微创手术机器人控制系统的软硬件框架，按照
层级结构设计了控制软件。根据功能划分任务，确定了实时性任务的结构和优先
级。对系统的实时性能进行了分析与估算，结果表明控制系统实时性能优良，满
足微创手术的要求。
(2) 针对手术机器人系统主从异构的结构特点，研究了内窥镜视野下基于笛卡
尔空间的主从增量控制策略，实现了医生操作时的手眼协调控制。建立了从手机
械臂的运动学模型，实现了基于单目视觉的机械臂基座姿态角测量技术，提出了
内窥镜视野下的主从一致性映射方法。
(3) 实现了主从术中调整、主手抖动滤除和操作安全限制等主从控制功能，解
决了主从工作空间不匹配、人手操作生理性抖动和医生误操作等问题，提高了微
创手术机器人系统的使用性能。
(4) 开展了实时性能对比实验、主从控制实验和活体动物实验研究。实验结果
表明，控制系统能够快速及时地对外界输入做出反应，而且操作者能够手眼协调
地操控机器人进行手术操作，验证了控制系统的实时性能以及主从控制策略的有
效性。
针对今后的研究工作，在以下几个方面进行展望：
(1) 采用集中式处理与分布式 I/O 的控制系统结构。深入运动控制的底层进行
开发，充分利用高性能计算机的处理能力，将高级和低级的控制算法在同一开发
环境中实现，从而增大控制算法的灵活性，提升系统的整体性能。
(2) 采用 MATLAB-Simulink 的 RTW 工具箱进行软件系统开发。在 MATLAB
中专注于控制系统设计，经过测试与仿真之后，将模型直接转换为能在 QNX 系
统上运行的 C 语言代码。简化软件设计，便于软件系统的调试与仿真。
61

万方数据
 哈尔滨工业大学工程硕士学位论文

(3) 增加力反馈功能。单纯依靠三维视觉，医生只能进行位置控制，很难掌握
手术器械与人体组织的接触情况，增加操作失误的概率。而且只依靠视觉也容易
引起医生的疲劳。

62

万方数据
 哈尔滨工业大学工程硕士学位论文

参考文献
[1] Taylor R H. Medical Robotics and Computer-Integrated Surgery[C]// IEEE
International Computer Software and Applications Conference. IEEE Computer
Society, 2008:1.
[2] Valero R, Ko Y H, Chauhan S, et al. Robotic surgery: history and teaching
impact[J]. Actas Urologicas Espaolas, 2011, 35(9):540-5.
[3] Surhone L M, Tennoe M T, Henssonow S F. Real- Time Operating System[J].
Betascript Publishing, 2011, 21(1):211-225.
[4] Hiba S, Wadee A, John R. Real-time operating system (RTOS) with application to
play models[J]. 2014, 5(2):9-16.
[5] Turchetti G, Palla I, Pierotti F, et al. Economic evaluation of da Vinci-assisted
robotic surgery: a systematic review[J]. Surgical Endoscopy, 2012, 26(26):598-
606.
[6] 杜志江. 达芬奇手术机器人系统技术分析[J]. 机器人技术与应用, 2011(4):14-
16.
[7] Monsarrat N, Collinet P, Narducci F, et al. Robotic assistance in gynaecological
surgery: State-of-the-art[J]. Gynécologie Obstétrique & Fertilité, 2009, 37(5):415-
24.
[8] Kazanzidesf P, Chen Z, Deguet A, et al. An open-source research kit for the da
Vinci® Surgical System[C]// 2014:6434-6439.
[9] Konietschke R, Hagn U, Nickl M, et al. The DLR MiroSurge - A robotic system
for surgery[C]// IEEE International Conference on Robotics & Automation.
2009:1589-1590.
[10] Tobergte A, Helmer P, Hagn U, et al. The sigma.7 haptic interface for MiroSurge:
A new bi-manual surgical console[C]// IROS. 2011:3023-3030.
[11] Hagn U, Konietschke R, Tobergte A, et al. DLR MiroSurge: a versatile system for
research in endoscopic telesurgery.[J]. International Journal of Computer Assisted
Radiology & Surgery, 2010, 5(2):183-193.
[12] Seibold U, Kuebler B, Hirzinger G. Prototypic Force Feedback Instrument for
Minimally Invasive Robotic Surgery[M]// Medical Robotics. 2008:377-400.
[13] Piccigallo M, Scarfogliero U, Quaglia C, et al. Design of a Novel Bimanual
Robotic System for Single-Port Laparoscopy[J]. IEEE/ASME Transactions on
Mechatronics, 2011, 15(6):871-878.
[14] Niccolini M, Petroni G, Menciassi A, et al. Real-time control architecture of a
63

万方数据
 哈尔滨工业大学工程硕士学位论文

novel Single-Port lapaRoscopy bimaNual roboT (SPRINT)[C]// Robotics and

Automation (ICRA), 2012 IEEE International Conference on. IEEE, 2012:3395 -
3400.
[15] Rosen J, Lum M, Sinanan M, et al. Raven: Developing a Surgical Robot from a
Concept to a Transatlantic Teleoperation Experiment[M]// Surgical Robotics.
2011:159-197.
[16] Staub C, Lenz C, Panin G, et al. Contour-based surgical instrument tracking
supported by kinematic prediction[C]// Biomedical Robotics and Biomechatronics
(BioRob), 2010 3rd IEEE RAS and EMBS International Conference on. IEEE,
2010:746 - 752.
[17] Tully S, Kantor G, Zenati M A, et al. Shape estimation for image-guided surgery
with a highly articulated snake robot[C]// IEEE/RSJ International Conference on
Intelligent Robots & Systems. IEEE, 2011:1353-1358.
[18] Gherman B, Pisla D, Vaida C, et al. Development of inverse dynamic model for a
surgical hybrid parallel robot with equivalent lumped masses[J]. Robotics and
Computer-Integrated Manufacturing, 2012, 28(3):402-415.
[19] 李建民. 微创机器人机构设计方法与主从映射策略研究[D]. 天津大学, 2012.
[20] 鞠浩. 内窥镜微创外科手术机器人控制系统研究[D]. 南开大学, 2009.
[21] 梁科. 新型微创外科手术机器人控制系统研究[D]. 天津大学, 2012.
[22] 王树新, 丁杰男, 贠今天,等. 显微外科手术机器人——“妙手”系统的研究[J].
机器人, 2006, 28(2):130-135.
[23] 冯美. 腹腔微创手术机器人系统关键技术研究[D]. 哈尔滨工业大学, 2011.
[24] 王树国, 谢琦, 潘博,等. 腹腔微创手术机器人主从实时控制算法[J]. 机械与电
子, 2010(8):61-64.
[25] 马如奇. 微创腹腔外科手术机器人执行系统研制及其控制算法研究[D]. 哈尔
滨工业大学, 2013.
[26] Tang A, Cao Q, Tan H, et al. Motion Control of a Master–Slave Minimally
Invasive Surgical Robot Based on the Hand-Eye-Coordination[J]. 2016.
[27] 唐奥林. 面向主从式微创外科手术机器人的遥操作运动控制策略研究[D]. 上
海交通大学, 2014.
[28] Yu L, Wang Z, Yu P, et al. A new kinematics method based on a dynamic visual
window for a surgical robot[J]. Robotica, 2013, 32(4):571-589.
[29] Buttazzo G C. Hard Real-time Computing Systems: Predictable Scheduling
Algorithms And Applications (Real-Time Systems Series)[M]. Springer-Verlag
TELOS, 2004.
[30] 孙鲁毅. 四种流行的嵌入式实时操作系统的比较研究——VxWorks, QNX,
64

万方数据
 哈尔滨工业大学工程硕士学位论文

ucLinux, RTEMS[J]. 计算机应用与软件, 2007, 24(8):196-197.

[31] Pieper, Lee D, Pieper, et al. Kinematics o f manipulators under computer
control.[J]. 1968.
[32] Jansen D, Buttner H. Real-time Ethernet: The EtherCAT solution[J]. Computing
& Control Engineering, 2004, 15(1):16-21.
[33] Ke-Lian M A, Hao X H. Study of Real-time of Modbus/TCP Protocol of
Ethernet[J]. Lanzhou Petrochemical College of Vocational Technology, 2006.
[34] Stevens W R, Narten T. UNIX network programming.[M]// C++ network
programming =. :8-9.
[35] 徐银生, 王昕. 基于以太网的 Modbus/TCP 协议的实时性研究[J]. 网络新媒体
技术, 2007, 28(12):1256-1259.
[36] QNX Software Systems Limited. QNX Neutrino S ystem Architecture[M], 1001
Farrar Road Ottawa, Ontario Canada,2014
[37] QNX Software Systems Limited. Building Embedded Systems[M], 1001 Farrar
Road Ottawa, Ontario Canada,2014
[38] 刘利强. 实时嵌入式系统软件设计方法[M]. 哈尔滨工程大学出版社, 2012.
[39] Krten R. Get Programming with the QNX Neutrino RTOS[M]. Ontario: PARSE
Software Devices, 2001：58-400.
[40] Joseph Y.-T. Leung, Jennifer Whitehead. On the complexity of fixed-priority
scheduling of periodic, real- time tasks. [J]. Performance Evaluation, 1982,
2(4):237-250.
[41] Liu C L, Layland J W. Scheduling Algorithms for Multiprogramming in a Hard-
Real-Time Environment[J]. Journal of the Acm, 2002, 20(1):179-194.
[42] 赵凯. 微创手术机器人远心定位机构设计与优化[D]. 哈尔滨工业大学, 2013.
[43] Hayashibe M, Suzuki N, Hashizume M, et al. Robotic surgery setup simulation
with the integration of inverse-kinematics computation and medical imaging. [J].
Computer Methods & Programs in Biomedicine, 2013, 83(1):63-72.
[44] 黄桂平, 李广云, 王保丰, 等. 单目视觉测 量技术研究 [J]. 计量 学报, 2004,
25(4):314-317.
[45] Nixon M, Alberto S. Aguado. Feature Extraction and Image Processing[J].
Computer Journal, 2002, 26(1):78.
[46] Veluvolu K C, U. X. Tan, Latt W T, et al. Adaptive filtering of physiological
tremor for real-time compensation[C]// IEEE International Conference on
Robotics and Biomimetics. 2009:524-529.
[47] Veluvolu K C, Tan U X, Latt W T, et al. Bandlimited multiple Fourier linear
combiner for real-time tremor compensation.[C]// Conference proceedings :.

65

万方数据
 哈尔滨工业大学工程硕士学位论文

Annual International Conference of the IEEE Engineering in Medicine and

Biology Society. IEEE Engineering in Medicine and Biology Society. Annual
Conference. Conf Proc IEEE Eng Med Biol Soc, 2007:2847 - 2850.
[48] Riviere C N, Rader R S, Thakor N V. Adaptive canceling of physiological tremor
for improved precision in microsurgery. [J]. IEEE Transactions on Biomedical
Engineering, 1998, 45(7):839-46.
[49] Marescaux J, Leroy J, Gagner M, et al. Transatlantic robot-assisted telesurgery.[J].
Journal of Urology, 2002, 413(6854):379-80.

66

万方数据
 哈尔滨工业大学工程硕士学位论文

攻读硕士学位期间发表的学术论文及其他成果

（一）发表的学术论文
[1] Bo Pan, Xiaofeng Qu, Yue Ai, et al. Master-slave real- time control strategy in
Cartesian space for a novel surgical robot for minimally invasive surgery [C],
International Conference on Biomedical Engineering and Biotechnology (ICBEB
2016).
（二）已申请的专利
[1] 潘博，付宜利，曲效锋，王树国，艾跃. 用于具有三角形远心点机构的串联
机械臂逆运动学数值解法.申请号：CN201511521247

67

万方数据
 哈尔滨工业大学工程硕士学位论文

哈尔滨工业大学学位论文原创性声明和使用授权说明

学位论文原创性声明

本人郑重声明：此处所提交的学位论文《微创手术机器人实时系统构建与主
从控制策略》，是本人在导师指导下，在哈尔滨工业大学攻读学位期间独立进行
研究工作所取得的成果，且学位论文中除已标注引用文献的部分外不包含他人完
成或已发表的研究成果。对本学位论文的研究工作做出重要贡献的个人和集体，
均已在文中以明确方式注明。

作者签名： 日期： 2016 年 6 月 29 日

学位论文使用权限

学位论文是研究生在哈尔滨工业大学攻读学位期间完成的成果，知识产权归
属哈尔滨工业大学。学位论文的使用权限如下：
（1）学校可以采用影印、缩印或其他复制手段保存研究生上交的学位论文，
并向国家图书馆报送学位论文；（2）学校可以将学位论文部分或全部内容编入
有关数据库进行检索和提供相应阅览服务；（3）研究生毕业后发表与此学位论
文研究成果相关的学术论文和其他成果时，应征得导师同意，且第一署名单位为
哈尔滨工业大学。
保密论文在保密期内遵守有关保密规定，解密后适用于此使用权限规定。
本人知悉学位论文的使用权限，并将遵守有关规定。

作者签名： 日期： 2016 年 6 月 29 日

导师签名： 日期： 2015 年 6 月 29 日

68

万方数据
 哈尔滨工业大学工程硕士学位论文

致 谢

在攻读硕士期间，导师王树国教授给予了作者悉心的指导与亲切的关怀。导
师严谨的治学作风、深厚的专业素养、高尚的师德师风令我钦佩与景仰，不仅为
我指明了研究方向，更是我人生路上的一盏明灯。
衷心感谢副导师付宜利教授对我的谆谆教诲。付教授言传身教，身体力行，
为我的硕士课题提供了优异的基础环境与资源平台。付教授治学严谨，孜孜不倦、
一丝不苟，是我在学术道路上的楷模，给予我鞭策与鼓励。
感谢实验室的各位老师和同学在论文撰写过程中给予我的支持与帮助。感谢
潘博老师对课题进展的指导与监督，为我明确了努力的方向，增强了成功的信心；
感谢张福海老师、封海波老师和高文朋老师给予我的无私帮助与鼓励，为我在遇
到困难时增添了前进的动力与勇气；感谢艾跃师兄在程序编写与调试过程中给予
我的无私指导和帮助，使我加深了对课题的理解，大大加快了课题的进展；感谢
牛国君师兄在机械结构方面给予我的支持与帮助，使我能够更加有针对性地进行
课题研究；感谢李坤师兄、王涛师兄、曲家迪师兄、李国志师兄和邹水中师兄热
心的帮助与鼓励；感谢实验室胡守智工程师在机械加工方面提供的帮助。还要感
谢实验室的李振宏同学、储振同学、吴坤坤同学、闫佳雯同学、曹法艺同学和卢
钰同学。感谢实验室其他所有帮助过我的老师和同学。
我还要感谢我的女朋友徐艺婷，感谢她在生活中给予我的支持和鼓励，为我
的研究生生活增添了许多幸福的回忆。
最后，感谢我的父母一直以来给予我的支持、理解与关爱，为我在人生道路
上的前行提供了不懈的动力。
两年的时间不长，但是在大家的帮助与关怀下，让我在思想、学术、品德方
面又上了一个新的台阶。在论文完成之际，谨向所有帮助过支持过鼓励过我的老
师、同学、亲人表示由衷的感谢！

69

万方数据