针对手术机器人的避障规划方法和避障规划系统

摘要

本发明提供一种针对手术机器人的避障规划方法和避障规划系统。该避障规划方法包括：步骤S110：创建障碍模型，其中，障碍模型包括对象头部模型、头架模型和障碍墙模型；步骤S120：将障碍模型映射到机器人坐标系，以获得经映射的障碍模型，其中，所述经映射的障碍模型包括经映射的对象头部模型、经映射的头架模型和经映射的障碍墙模型；以及步骤S130：至少基于经映射的障碍模型进行路径规划，以获得针对手术机器人的一条或多条无碰撞路径。根据本发明提供的避障规划方法和避障规划系统，可以快速规划出无碰撞的机器人运动路径，从而可以有效避免手术机器人在运动过程中与对象及其周围的障碍相撞。

说明书

技术领域

本发明涉及计算机辅助医疗技术领域，具体地，涉及一种针对手术机 器人的避障规划方法和避障规划系统。

背景技术

头部立体定向手术操作(简称“立体定向操作”)是一种常见的手术操 作模式。在操作者需要对操作对象(简称“对象”)执行立体定向操作的应 用中，通常利用立体定向手术机器人(简称“手术机器人”或“机器人”) 来进行定位引导。对象可以是，例如实际的人或模拟对象。对于立体定向 操作，操作者首先依据对象头部的三维图像规划立体定向操作的操作路径； 随后，机器人根据在图像中定义的操作路径，在对象头部周围依次对各个 操作路径进行主动定位，即利用机器人手臂上的末端机构来确定操作路径 的方向；最后，由操作者按照机器人所限定的方向进行立体定向操作。利 用多自由度机器人手臂进行操作路径的主动定位引导，可以快速准确地定 位多条操作路径。然而，在机器人进行主动定位的动作(或说运动)过程 中，应该采取措施来防止机器人发生意外碰撞，保证操作的安全性。可以 理解的是，在机器人手臂上的末端机构向各个目标姿态运行时，机器人手 臂的各连杆均发生运动，此时应该注意避免机器人的各部分机构与对象及 操作室内的物体发生碰撞。对于机器人来说，不能碰撞的物体称为障碍。 通常，可以通过示教方法来规定机器人手臂的空间运动轨迹和姿态，从而 实现避障。示教方法适合应用于机器人每次针对的对象和其他障碍的位置 都不变的环境中，且耗时较长。比如在工业生产中，通过一次示教来存储 机器人的运动路径，之后机器人就可以按照存储的运动路径进行大量的重 复生产劳动。而在对多条操作路径进行主动定位这一应用中，每次操作中 机器人相对于对象的摆放位置不同，每个对象头部的多条操作路径的位置 也不同，因此不适合按照示教的方式来进行定位和避障。

因此，需要提供一种针对机器人的避障规划方法，以至少部分地解决 现有技术中存在的上述问题。

发明内容

为了至少部分地解决现有技术中存在的问题，根据本发明的一个方面， 提供一种针对手术机器人的避障规划方法。该避障规划方法包括：步骤 S110：创建障碍模型，其中，障碍模型包括对象头部模型、头架模型和障 碍墙模型；步骤S120：将障碍模型映射到机器人坐标系，以获得经映射的 障碍模型，其中，经映射的障碍模型包括经映射的对象头部模型、经映射 的头架模型和经映射的障碍墙模型；以及步骤S130：至少基于经映射的障 碍模型进行路径规划，以获得针对手术机器人的一条或多条无碰撞路径。

根据本发明的另一方面，提供一种避障规划系统。该避障规划系统包 括模型创建装置、映射装置和路径规划装置。模型创建装置用于创建障碍 模型，其中，障碍模型包括对象头部模型、头架模型和障碍墙模型。映射 装置用于将障碍模型映射到机器人坐标系，以获得经映射的障碍模型，其 中，经映射的障碍模型包括经映射的对象头部模型、经映射的头架模型和 经映射的障碍墙模型。路径规划装置用于至少基于经映射的障碍模型进行 路径规划，以获得针对手术机器人的一条或多条无碰撞路径。

根据本发明提供的避障规划方法和避障规划系统，可以快速规划出无 碰撞的机器人运行路径，从而可以有效避免手术机器人在运动过程中与对 象及其周围的障碍相撞。该避障规划方法和避障规划系统具有高效、安全 等特点，通用性强。

在发明内容中引入了一系列简化的概念，这些概念将在具体实施方式 部分中进一步详细说明。本发明内容部分并不意味着要试图限定所要求保 护的技术方案的关键特征和必要技术特征，更不意味着试图确定所要求保 护的技术方案的保护范围。

以下结合附图，详细说明本发明的优点和特征。

附图说明

本发明的下列附图在此作为本发明的一部分用于理解本发明。附图中 示出了本发明的实施方式及其描述，用来解释本发明的原理。在附图中，

图1示出根据本发明一个实施例的避障规划方法的流程图；

图2示出根据本发明一个实施例的、本发明应用于其中的立体定向操 作系统的示意图；

图3示出根据本发明一个实施例的手术机器人的示意图；

图4根据本发明一个实施例的对象头部包络球的示意图；

图5示出根据本发明一个实施例的头架及头架包络的示意图；

图6示出根据本发明一个实施例的障碍墙模型的示意图；

图7示出将对象头部包络球、头架包络长方体和障碍墙模型映射到机 器人坐标系的示意图；

图8示出一级路径的示意图；

图9示出根据本发明一个实施例的利用手术机器人对操作路径进行定 位的流程图；以及

图10示出根据本发明一个实施例的避障规划系统的示意性框图。

具体实施方式

在下文的描述中，提供了大量的细节以便能够彻底地理解本发明。然 而，本领域技术人员可以了解，如下描述仅涉及本发明的较佳实施例，本 发明可以无需一个或多个这样的细节而得以实施。此外，为了避免与本发 明发生混淆，对于本领域公知的一些技术特征未进行描述。

为解决手术机器人对多路径立体定向操作进行定位的过程中的避障问 题，根据本发明的一个方面，提供一种针对手术机器人的避障规划方法。 避障规划是指在具有障碍物的环境中，寻找一条从机器人的起始位姿点到 目标位姿点的无碰撞路径，以实现机器人手臂在运动过程中不与障碍发生 碰撞的目的。所述起始位姿点与机器人的起始位姿相对应，所述目标位姿 点与机器人的目标位姿相对应。如下文所述，位姿点可以用机器人手臂各 关节的角度来表示。无碰撞路径可以通过算法自动计算，快速生成，因此， 避障规划方法适用于解决机器人对立体定向操作的操作路径进行主动定位 的过程中的避障问题，尤其是具有多条操作路径的立体定向操作。图1示 出根据本发明一个实施例的避障规划方法100的流程图。图2示出根据本 发明一个实施例的、本发明应用于其中的立体定向操作系统的示意图。下 面结合图1和图2描述避障规划方法100。

为描述简便，首先介绍立体定向操作系统的结构。图2所示的立体定 向操作系统包括机器人1、光学跟踪定位设备2、头架4、探测器(也可以 称为“探针”)5、对象坐标系参考物6和工作站8。在图2中，还示出了对 象头部3和对象标识点7。

机器人1可以是一台六自由度关节型机械手臂机器人。图3示出根据 本发明一个实施例的机器人的示意图。参考图3，机器人1的末端机构由 导向模块12和套管13组成，套管13安装于导向模块12上。机器人1是 立体定向操作系统的执行装置，用于对操作路径进行定位，例如，利用末 端机构的套管13限定操作路径的方向。本发明生成的无碰撞路径可以传送 给机器人1，使得机器人1在运动过程中能够不与对象头部3、头架4以及 其他物体发生碰撞。

光学跟踪定位设备2可以用于采集来自探针的、与下文所述的第一坐 标系注册步骤所涉及的对象标识点7、第二坐标系注册步骤所涉及的特定 位置、障碍上的特定点等相关的光信号。该立体定向操作系统采用基于配 准点对的注册方法建立机器人坐标系、对象坐标系与图像坐标系之间的变 换关系，从而可以将在对象图像中定义的操作路径和在对象坐标系中获取 的障碍信息映射到机器人坐标系，进而用于规划针对机器人的无碰撞路径。

对象头部3是将在其内部执行所需操作的操作区域。在机器人1定位 操作路径时，需要防止机器人1的手臂和末端机构碰撞到对象头部3。

头架4用于在操作过程中固定对象头部3。头架4本身与诸如床的支 撑物连接，并将对象头部3牢固地固定于支撑物之外。

探针5可以是一个具有四个端点的刚性架子，每个端点各有一个反光 球。光学跟踪定位设备2可以接收来自探针5的光信号，识别出探针5的 形状，并根据探针5的形状准确定位探针5的针尖。因此，探针5可以用 于帮助拾取下文所述的第一坐标系注册步骤所涉及的对象标识点7、第二 坐标系注册步骤所涉及的特定位置、障碍上的特定点等。

对象坐标系参考物6可以是一个十字形状的刚性架子，每个端点各有 一个反光球。同样地，对象坐标系参考物6可被光学跟踪定位设备2识别 并定位。对象坐标系参考物6可以固定于头架4上，在立体定向操作中其 位置相对于头架4和对象头部3均不发生变化，所以可以使用其坐标系来 描述对象坐标系。可以理解的是，对象坐标系参考物6和探针5的构造之 间的差别应该足够大，以使得来自二者的光信号能够被区分开。

对象标识点7可以用于在注册步骤中标识配准点，其可在对象图像中 成像。可以将对象标识点7粘贴于对象头部3上，然后通过扫描对象头部 来对对象成像，以获得对象图像。因此，在对象图像中可获得与对象标识 点7相对应的图像标识点，图像标识点的位置可在图像坐标系中表示。通 过将图像标识点与利用光学跟踪定位设备2拾取的对象标识点7进行配准， 可得到对象坐标系与图像坐标系之间的变换关系。

工作站8可以是一台计算机，其与机器人1和光学跟踪定位设备2相 连接。可以在工作站8上运行各种软件平台，使得工作站8具有各种需要 的处理功能。例如，在工作站8上可以实现图像输入模块、图形交互模块、 注册模块、障碍信息获取模块、路径规划模块、路径优化模块、可视化虚 拟验证模块和机器人控制模块。

下面描述避障规划方法100。避障规划方法100包括以下步骤。

在步骤S110，创建障碍模型。该障碍模型包括对象头部模型、头架模 型和障碍墙模型。

障碍可以根据立体定向操作的实际操作场景来定义，并且可以创建合 适的障碍模型来帮助机器人进行避障。具体地，在立体定向操作中，对象 头部3被头架4牢固固定于支撑物之外，头架4与支撑物相连接，同时在 对象头部3上，例如在对象的口鼻部位，可能联结了一些与操作相关的设 备。为了防止机器人1与对象头部3和联结的设备等障碍发生碰撞，可以 设计一个能够包围对象头部3的球体，即对象头部包络(后文中称其为“对 象头部包络球”)，作为一个障碍。为了防止机器人1与头架4发生碰撞， 可以设计一个尺寸能够完全包围头架的头架包络(以其是长方体为例，后 文中称其为“头架包络长方体”)作为一个障碍。另外，考虑到操作路径主 要是经过对象头部3的上半部分且沿着头皮向外的直线，并且对象可能佩 戴诸如呼吸面罩的辅助设备，以及对象躯干上方的支架上可能放置着操作 器械，因此，机器人1无需并且不能运动到对象鼻尖以下(朝向躯干方向) 的区域。所以，可以设计一个通过对象鼻尖与双侧耳垂连线的正方形平面 (后文中称其为“障碍墙”)作为一个障碍，其边长可以设计为大于机器人 1的臂长的两倍，防止机器人1运动到对象鼻尖以后的空间。可以利用以 上三个障碍来保护机器人1不能碰撞的空间。

在步骤S120，将障碍模型映射到机器人坐标系，以获得经映射的障碍 模型。经映射的障碍模型包括经映射的对象头部模型、经映射的头架模型 和经映射的障碍墙模型。

将障碍模型映射到机器人坐标系，即机器人1的工作空间，就是根据 需要保护的物体的位置，建立障碍在机器人坐标系中的描述的过程。只有 在机器人坐标系下描述了障碍，才能进行后续的路径规划。在映射障碍模 型之前，可以首先进行坐标系注册，也就是获取两个坐标系之间的变换关 系，使得利用该变换关系可以将一个坐标系中的点转换到另一坐标系下进 行表示。例如，可以通过某种注册方法得到对象坐标系到机器人坐标系的 空间变换关系，使得对象坐标系中的点可以映射到机器人坐标系中。然后 可以通过拾取需要保护的物体上的点来定义障碍在对象坐标系中的位置， 并将其转换到机器人坐标系中。可以理解的是，两个坐标系之间的空间变 换关系可以以空间变换矩阵的形式来表示。本文所述的拾取是指将探针5 的针尖放置在待标识物体上，然后通过光学跟踪定位设备2采集来自探针 5的光信号。这样，可以根据该光信号确定探针5的位置，进而确定待识 别物体的位置。对于上述三种障碍模型，可以分别采取不同的拾取点的方 法。

例如，对于对象头部模型，可以提取对象头部轮廓并拟合出球心位置， 利用坐标系之间的空间变换关系，将球心映射到机器人坐标系中。随后在 机器人坐标系中构造球体，作为经映射的对象头部模型。

对于头架模型，以头架4为三点固定式头架为例，在创建头架模型时 可以将头架4的包络近似视为一个长方体，并在长方体上设置一个局部坐 标系来描述该长方体，该局部坐标系可以称为头架坐标系。在映射步骤中， 可以拾取头架4上的三个特定点，得到该三个特定点在对象坐标系中的坐 标，并将其转换到机器人坐标系中。随后，结合头架坐标系的定义，可以 计算头架坐标系与机器人坐标系之间的空间变换关系，从而将头架模型映 射到机器人坐标系中。

对于障碍墙模型，为了后续的映射计算，在创建障碍墙模型时可以在 障碍墙模型上设置一个局部坐标系来描述它，该局部坐标系可以称为障碍 墙坐标系。在映射步骤中，可以拾取对象的鼻尖和耳垂，得到其在对象坐 标系中的坐标，并将其转换到机器人坐标系中。随后，结合障碍墙坐标系 的定义，可以计算障碍墙坐标系与机器人坐标系之间的空间变换关系，从 而将障碍墙模型映射到机器人坐标系中。

在步骤S130，至少基于经映射的障碍模型进行路径规划，以获得针对 手术机器人的一条或多条无碰撞路径。

路径规划的过程就是根据障碍信息生成从起始位姿点到目标位姿点的 无碰撞路径的过程。首先需要明确路径规划的起始位姿点和目标位姿点， 然后结合障碍信息，即经映射的障碍模型，选择合适的路径规划算法进行 计算。路径规划的结果是生成了机器人的位姿点序列，机器人各关节依次 按照该位姿点序列运动，将会避开障碍，到达目标位姿点，进而定位到相 应的操作路径。

在操作过程中要实现机器人对多条操作路径的定位，需要进行多次路 径规划，而操作时间可能是有限的，要求路径规划能够在较短的时间完成。 另外机器人具有较高的自由度，因此可以选择一种适用于高维空间且速度 快的路径规划算法。基于概率的路径规划算法满足这样的特性，其中应用 广泛的两种概率方法是概率地图法(PRM)和快速扩展随机树法(RRT)， 它们都是在位姿空间进行路径规划的方法。PRM算法需要对信息进行预处 理，花费一定的时间构建概率地图，也就是对整个位姿空间进行采样，保 证自由位姿空间(不发生碰撞的位姿点的集合)内有足够采样点，连接两 个相邻的采样点，形成概率地图。之后可在概率地图中进行多条无碰撞路 径的查询。而RRT算法无需对信息的预处理，它是以起始位姿点为树根， 通过位姿空间的随机采样点，向自由位姿空间中扩展。在此基础上发展的 RRT-Connect算法将起始位姿点和目标位姿点分别作为一个树根，利用启 发函数，使两棵树交替地向着对方生长，直到相遇。RRT-Connect算法具 有速度快、效率高的特点，可用于高维空间的路径规划。因此可以采用 RRT-Connect算法进行针对机器人的路径规划。

根据本发明提供的避障规划方法，由于基于所创建的障碍模型进行路 径规划，因此，可以快速规划出无碰撞的机器人运行路径，从而可以有效 避免手术机器人在运动过程中与对象及其周围的障碍相撞。该避障规划方 法具有高效、安全等特点，通用性强。

可选地，在步骤S120之前，避障规划方法100可以进一步包括注册 步骤。注册步骤用于确定机器人坐标系、对象坐标系和图像坐标系之间的 变换关系。从而在步骤S120中，可以将障碍模型映射到机器人坐标系中； 在步骤S130中，确定机器人坐标系中的入点坐标、靶点坐标，进而用于规 划针对机器人的无碰撞路径。注册步骤可以采用基于配准点的注册方法， 借助光学跟踪定位设备、激光设备等工具拾取标识点作为配准点。也可以 是基于表面的注册方法，借助已知位置的视觉设备获取并重建操作的场景 信息。下面描述本文所述的注册步骤。本文所述的注册步骤采用基于配准 点的注册方法，借助了光学跟踪定位设备2拾取标识点作为配准点。其可 以在工作站8的注册模块中进行计算处理。本文所述的注册步骤包括第一 坐标系注册步骤、第二坐标系注册步骤和第三坐标系注册步骤。假设，定 义两个坐标系中各有一组点集，分别记为{W_i}和{M_i}，其中i为配准点的数 目。可以寻找一个最优的变换矩阵T，使得经过变换后的坐标T(W_i)与M_i坐 标最接近。刚体变换可分解为旋转变换R，平移变换t和尺度变换s。于是， 注册相当于求解最优的R、T和s，使得下式被满足：

$min\underset{i=1}{\overset{n}{\Sigma }}\left|\right|M_i-\left(sR\right(W_i)+t)|{|}^2$

对于上式，可以利用多种方法得到解析解，利用霍恩(Horn)提出的 独立四元数法，求解最优的变换矩阵。

第一坐标系注册步骤包括：获取对象图像；接收针对对象图像中的图 像标识点的标识点输入信号，并根据标识点输入信号确定图像标识点在图 像坐标系{I}中的第一坐标；采集来自对象坐标系参考物的第一参考光信号 与来自探测器的、与对象标识点相关的第一标识光信号，并根据第一参考 光信号和第一标识光信号确定对象标识点在对象坐标系{Re}中的第二坐 标，其中，对象标识点与图像标识点相对应；以及基于第一坐标和第二坐 标计算图像坐标系{I}与对象坐标系{Re}之间的第一变换关系。

可以通过第一坐标系注册步骤获取图像坐标系{I}与对象坐标系{Re} 之间的第一变换关系

第一坐标系注册步骤可以采用基于配准点的注册方法，下面举例说明。 在一个实施例中，可以在对对象进行成像，即采集对象图像之前，在对象 的头皮上粘贴可成像的对象标识点7。随后，利用诸如电子计算机断层扫 描(CT)技术、磁共振成像(MRI)技术等方法来采集对象图像。通过工 作站8的图像输入模块将对象图像导入工作站8，即获取对象图像。操作 者可以利用工作站8的图形交互模块在对象图像中选取图像标识点。相应 地，工作站8可以接收到针对图像标识点的标识点输入信号，根据该标识 点输入信号可以得到图像标识点在图像坐标系{I}中的第一坐标^Ip_object。 该图像标识点与对象标识点7相对应。对象标识点7及对应的图像标识点 可以具有任何合适的数目，本发明不对此进行限制。优选地，对象标识点 7可以是不共面的四个或多于四个标识点，对应的图像标识点的数目是四 个或多于四个。可以理解的是，当对象标识点7及对应的图像标识点的数 目是多个时，操作者可以依次选取每个图像标识点。随后，可以按照图像 标识点的选取顺序，将探针5依次放置于对象标识点7处，同时利用光学 跟踪定位设备2采集来自探针5的、与对象标识点7相关的第一标识光信 号。另外，还采集来自对象坐标系参考物6的第一参考光信号。可以根据 第一参考光信号和第一标识光信号确定对象标识点7在对象坐标系{Re}中 的第二坐标^Rep_object。由于对象标识点7与图像标识点相对应，因此可以 利用第一坐标^Ip_object和第二坐标^Rep_object这两组坐标求解出图像坐标系 {I}与对象坐标系{Re}之间的第一变换关系

第二坐标系注册步骤包括：控制手术机器人的末端机构运动到特定位 置处，其中，探测器固定于末端机构上；基于末端机构在机器人坐标系中 的坐标确定探测器在机器人坐标系中的第三坐标；采集来自对象坐标系参 考物的第二参考光信号与来自探测器的第二标识光信号，并根据第二参考 光信号和第二标识光信号确定探测器在对象坐标系中的第四坐标；以及基 于第三坐标和第四坐标计算机器人坐标系与对象坐标系之间的第二变换关 系。

可以通过第二坐标系注册步骤获取对象坐标系{Re}与机器人坐标系 {R}之间的第二变换关系

第二坐标系注册步骤同样可以采用基于配准点的注册方法。例如，可 以将探针5固定于机器人1的末端机构的套管13上。由于预先已标定探针 5的针尖在机器人1的末端机构的位置，因此可以基于末端机构在机器人 坐标系{R}中的坐标得到探针5在机器人坐标系{R}中的坐标。可选地，控 制机器人1的末端机构分别运行到不共面的四个或多于四个特定位置处。 在每个特定位置处，确定探针5在机器人坐标系{R}中的第三坐标。由光 学跟踪定位设备2采集来自对象坐标系参考物6的第二参考光信号与来自 探针5的第二标识光信号，以确定探针5在对象坐标系{Re}中的第四坐标。 可以根据第三坐标和第四坐标求得对象坐标系{Re}与机器人坐标系{R}之 间的第二变换关系

第三坐标系注册步骤包括：根据第一变换关系和第二变换关系计算机 器人坐标系与图像坐标系之间的第三变换关系。

可以通过第三坐标系注册步骤获取图像坐标系{I}与机器人坐标系{R} 之间的第三变换关系

根据空间变换关系的传递原理，第三变换关系可以由以下公式计算 得到：

${}_I{}^{R}T={}_{\mathrm{Re}}{}^{R}T·{}_I{}^{\mathrm{Re}}T$

需要说明的是，图2所示的立体定向操作系统利用光学跟踪定位设备 2完成注册步骤，目的是将在对象图像中定义的操作路径和在对象坐标系 中获取的障碍信息映射到机器人坐标系，进而用于规划针对机器人的无碰 撞路径。

可选地，对象头部模型的创建包括设计半径为预设半径的球体作为对 象头部模型。该预设半径大于对象头部的半径。图4示出根据本发明一个 实施例的对象头部包络球的示意图。参考图4，可以将对象头部3抽象为 球体，设计半径比对象头部3的半径略大的球体，即对象头部包络球9。 另外，也可以根据成年人的头围，将预设半径设计为大于常规的成年人头 部半径。在图4中，示出对象头部包络球9的球心为对象头部3的中心点O_s， 半径为预设半径R_s。对象头部包络球9的目的是防止机器人1与对象头部 3发生碰撞。

步骤S120可以包括：采集来自对象坐标系参考物的第三参考光信号 与来自探测器的、与对象头部上的拟合点相关的拟合光信号，并根据第三 参考光信号和拟合光信号确定拟合点在对象坐标系中的第五坐标；利用第 五坐标拟合对象头部的中心点在对象坐标系中的第六坐标；利用第二变换 关系将第六坐标转换到机器人坐标系中，以获得中心点在机器人坐标系中 的第七坐标；以及在机器人坐标系中以第七坐标为球心并且以预设半径为 半径构建球体，以获得经映射的对象头部模型。

可以利用探针5和光学跟踪定位设备2在对象头部上拾取较大范围的 拟合点。具体地，利用光学跟踪定位设备2采集来自对象坐标系参考物6 的第三参考光信号与来自探针5的、与对象头部上的拟合点相关的拟合光 信号，并根据第三参考光信号和拟合光信号确定拟合点在对象坐标系{Re} 中的第五坐标。利用这些拟合点拟合对象头部3的中心点(即球心)的第 六坐标^Rep_center。此时的球心是在对象坐标系{Re}下的，利用对象坐标系 {Re}与机器人坐标系之间的第二变换关系将其转换到机器人坐标系{R} 下，以获得第七坐标^Rp_center，即：

${{}^{R}p}_{center}={}_{\mathrm{Re}}{}^{R}T·{{}^{\mathrm{Re}}p}_{center}$

随后，在机器人坐标系{R}中，以第七坐标^Rp_center为球心并且以预设 半径R_s为半径构建球体。该球体即为经映射的对象头部模型。

可选地，头架模型的创建包括设计利用头架坐标系表示的、能够包围 头架的头架包络作为头架模型。

图5示出根据本发明一个实施例的头架及头架包络的示意图。如图5 所示，在创建头架模型时，根据所使用的头架4，设计能够包围头架4的 头架包络长方体10作为头架模型。头架包络长方体10的长度为a，宽度为 b，高度为c。以{r}表示头架包络长方体的局部坐标系，即头架坐标系。将 头架坐标系的原点设为O_r。创建头架模型的目的是防止机器人1与头架4 发生碰撞。

步骤S120可以包括：采集来自对象坐标系参考物的第四参考光信号 与来自探测器的、与头架上的头架标识点相关的第三标识光信号，根据第 四参考光信号和第三标识光信号确定头架标识点在对象坐标系中的第八坐 标；利用第二变换关系将第八坐标转换到机器人坐标系中，以获得头架标 识点在机器人坐标系中的第九坐标；基于第九坐标与头架标识点在头架坐 标系中的坐标计算头架坐标系与机器人坐标系之间的第四变换关系；以及 利用第四变换关系将头架包络映射到机器人坐标系，以获得经映射的头架 模型。

可以将探针5(即探针5的针尖)依次放置在头架4上的A、B、C三 点处，如图4所示。并利用光学跟踪定位设备2采集来自探针5的、与A、 B、C三点相关的第三标识光信号。同时利用光学跟踪定位设备2采集来自 对象坐标系参考物6的第四参考光信号。根据第四参考光信号和第三标识 光信号确定A、B、C三点在对象坐标系{Re}中的第八坐标。A、B、C三点 在对象坐标系{Re}中的坐标分别为^Rep_A、^Rep_B、^Rep_C。利用第八坐标与对 象坐标系{Re}与机器人坐标系{R}之间的第二变换关系将A、B、C三点 映射到机器人坐标系{R}下，即进而基于A、B、 C三点在机器人坐标系{R}中的第九坐标^Rp_A、^Rp_B、^Rp_C与A、B、C三点 在头架坐标系{r}中的坐标计算头架坐标系{r}与机器人坐标系{R}之间的 第四变换关系

通过以下步骤构造第四变换关系中的旋转矩阵

将和单位化(和均为列向量表示)：

将方向作为头架包络长方体10的x轴方向，即n_r＝p；

求解垂直于A、B、C三点所在平面的向量，作为头架包络长方体 10的z轴方向，即a_r＝p×q；

求解头架包络长方体10的y轴方向，即o_r＝a_r×n_r。

将头架包络长方体10的原点设置在线段AB的中点所在的位置处，即

构造第四变换关系${}_r{}^{R}T=\left(\begin{array}{cccc}& {}_r{}^{R}R& & p_r\\ 0& 0& 0& 1\end{array}\right).$

由此将头架包络长方体10映射到机器人坐标系{R}下。

可选地，障碍墙模型的创建包括设计利用障碍墙坐标系表示的、边长 大于手术机器人的臂长的两倍的正方形平面作为障碍墙模型。

图6示出根据本发明一个实施例的障碍墙模型的示意图。如图6所示，

根据机器人1的运动范围，设计边长为d_w的正方形平面作为障碍墙模型11。 其中，边长d_w可以设计为大于机器人1的臂长的两倍。用{w}表示障碍墙 模型11的局部坐标系，即障碍墙坐标系。将障碍墙坐标系{w}的原点设在 平面中心点O_w。期望经过后续的映射步骤，将障碍墙坐标系{w}的原点放 置在与对象鼻尖点(用D点表示)重合的位置上，并使得障碍墙模型11 成为经过对象鼻尖点和双侧耳垂点(用E点和F点表示)三点所确定的平 面，防止机器人1与对象可能佩戴的呼吸面罩以及对象躯干上方的支架上 放置的操作器械发生碰撞。可以理解的是，障碍墙模型11的形状和尺寸仅 是示例而非限制，障碍墙模型11可以具有任何其他合适的形状和尺寸。例 如，障碍墙模型11可以具有三角形或圆形等形状，障碍墙模型11可以是 平面、曲面或立方体等，当障碍墙模型11是正方形平面时，其边长可以是 大于机器人臂长的四倍，等等。

步骤S120可以包括：采集来自对象坐标系参考物的第五参考光信号 与来自探测器的、与障碍墙划分点相关的第四标识光信号，根据第五参考 光信号和第四标识光信号确定障碍墙划分点在对象坐标系中的第十坐标； 利用第二变换关系将第十坐标转换到机器人坐标系中，以获得障碍墙划分 点在机器人坐标系中的第十一坐标；基于第十一坐标与障碍墙划分点在障 碍墙坐标系中的坐标计算障碍墙坐标系与机器人坐标系之间的第五变换关 系；以及利用第五变换关系将正方形平面映射到机器人坐标系，以获得经 映射的障碍墙模型。

上文所描述的对象鼻尖点和双侧耳垂点，即D、E、F三点为障碍墙划 分点。期望将障碍墙坐标系{w}的原点放置在与对象鼻尖点重合的位置上， 障碍墙模型11的法向量设置为与对象鼻尖点和双侧耳垂点三点所确定平 面的法向量重合(如图6所示)。

可以首先将探针5放置在对象鼻尖点D点和双侧耳垂点E点和F点处， 利用光学跟踪定位设备2采集来自探针5的、与D、E、F三点相关的第四 标识光信号。同时利用光学跟踪定位设备2采集来自对象坐标系参考物6 的第五参考光信号。根据第五参考光信号和第四标识光信号确定D、E和 F三点在对象坐标系{Re}中的第十坐标。D、E、F三点在对象坐标系{Re} 中的坐标分别为^Rep_D、^Rep_E、^Rep_F。利用第十坐标与对象坐标系{Re}与机 器人坐标系{R}之间的第二变换关系将D、E、F三点映射到机器人坐 标系{R}下，即进而基于D、E、F三点在机器 人坐标系{R}中的第十一坐标^Rp_D、^Rp_E、^Rp_F与在障碍墙坐标系{w}中的 坐标来计算障碍墙坐标系{w}与机器人坐标系{R}之间的第五变换关系

通过以下步骤构造第五变换关系中的旋转矩阵 ${}_w{}^{R}R=\left(\begin{array}{ccc}{n}_w& o_w& a_w\end{array}\right):$

分别将单位化(和均为列向量表示)：

确定平面DEF的法向量，即障碍墙模型11的z轴方向：a_w＝k×m；

将垂直于与z轴的方向作为障碍墙模型11的y轴方向，即 o_w＝m×a_w；

求解障碍墙模型11的x轴方向，即n_w＝o_w×a_w。

将障碍墙模型11的原点设置在D点所在位置，即p_w＝^Rp_D。

构造第五变换关系${}_w{}^{R}T=\left(\begin{array}{cccc}& {}_w{}^{R}R& & p_w\\ 0& 0& 0& 1\end{array}\right).$

由此将障碍墙模型11映射到机器人坐标系{R}下。

将对象头部包络球9、头架包络长方体10和障碍墙模型11均映射到 机器人坐标系{R}之后的场景如图7所示。

可选地，步骤S130可以包括：对于一条或多条无碰撞路径中的每一 条，执行以下步骤：

步骤S1302：将机器人的当前位姿设定为起始位姿；步骤S1304：接 收分别针对对象图像中的靶点和入点的靶点输入信号和入点输入信号；步 骤S1306：根据靶点输入信号和入点输入信号确定靶点在图像坐标系中的 靶点坐标和入点在图像坐标系中的入点坐标；步骤S1308：利用第三变换 关系将靶点坐标和入点坐标转换到机器人坐标系中，以获得经转换的靶点 坐标和经转换的入点坐标；步骤S1310：根据经转换的入点坐标和经转换 的靶点坐标确定机器人运动的目标点在机器人坐标系中的目标点坐标；步 骤S1312：根据经转换的靶点坐标、经转换的入点坐标和目标点坐标确定 目标位姿；以及步骤S1314：基于起始位姿、目标位姿和经映射的障碍模 型计算该无碰撞路径。

为了使机器人手臂上的末端机构每次进行操作路径的定位前无需回到 某个专门预设的位姿，可以将机器人1的当前位姿作为每次实施定位时的 起点，即起始位姿，其与起始位姿点相对应。起始位姿点可以用q_init表示。 位姿点q可以用各关节的角度(θ₁,θ₂,θ₃,θ₄,θ₅,θ₆)来表示。

目标位姿是基于机器人手臂上的末端机构所要定位的目标点和操作路 径的方向，根据机器人逆运动学所计算出的机器人位姿。操作路径的方向 由机器人末端姿态来限定。一条操作路径由颅内靶点(简称“靶点”)和头 皮入点(简称“入点”)组成。可以根据在注册步骤中获得的图像坐标系与 机器人坐标系之间的第三变换关系，将操作者在对象图像中设计的操作路 径上的点映射到机器人坐标系中。例如，可以根据图像坐标系{I}与机器人 坐标系{R}之间的第三变换关系利用将图像坐标系{I} 中的操作路径上的靶点坐标^Ip_T和入点坐标^Ip_E转换到机器人坐标系{R} 中，将经转换的靶点坐标和经转换的入点坐标分别记作^Rp_T和^Rp_E。因此， 操作路径的方向为可以将机器人运动的目标点设定为在操作路 径方向上距离入点^Rp_E一定长度的、在头皮外的点^Rp_end。操作路径由五个 自由度确定，假设机器人1具有六个自由度，则根据机器人逆运动学求解 机器人的位姿可以得到无数组解，根据需要选择其中一组作为目标位姿， 其与目标位姿点相对应。目标位姿点可以用q_goal表示。

可以利用工作站8的路径规划模块进行如下处理。将起始位姿点q_init、 目标位姿点q_goal以及前文所述的、已映射到机器人坐标系{R}中的经映射 的障碍模型作为RRT-Connect算法的输入。RRT-Connect算法在机器人1 的自由位姿空间内搜索从起始位姿点q_init到目标位姿点q_goal的无碰撞路 径，输出针对机器人1的位姿点序列，即下文所述的一级路径。随后，可 以在工作站8的路径优化模块中对一级路径进行路径优化，以获得最终用 于机器人运动的无碰撞路径。

上文所描述的是一种适用于机器人对多条操作路径进行主动定位的运 动过程中的避障规划方法，可以使机器人安全地连续定位立体定向操作的 多条操作路径，而无需每次定位前返回预设位姿。

可选地，步骤S130可以包括：对于一条或多条无碰撞路径中的每一 条，执行以下步骤：

至少基于经映射的障碍模型计算一级路径；以及对一级路径进行路径 优化，以获得最终用于机器人运动的无碰撞路径。对一级路径进行路径优 化包括：重复执行子优化步骤直至子优化步骤的重复次数达到次数阈值为 止。无碰撞路径为在所有子优化步骤结束之后获得的一级路径。上述子优 化步骤可以包括：在一级路径中选择包括一系列相邻位姿点的二级路径； 判断二级路径是否满足预设条件；如果满足预设条件，则子优化步骤结束； 如果不满足预设条件，则在该二级路径的首部位姿点和尾部位姿点之间进 行直线插值，将包括二级路径的首部位姿点、尾部位姿点和插值点的位姿 点序列作为三级路径。对三级路径进行碰撞检测，如果发生碰撞，则子优 化步骤结束；如果不发生碰撞，则删除该二级路径，将该三级路径添加到 一级路径，以更新一级路径。

在立体定向操作这一场景中，由于机器人1的自由位姿空间较大，这 就意味着使用RRT-Connect算法进行针对机器人1的路径规划时，随机采 样点可能出现在较大的区域，从而两棵RRT树可以扩展的区域较大，所以 由两棵RRT树连接得到的路径通常都不是优化的路径。对机器人1的运动 来说，可能造成机器人1在定位过程中“绕远”，有一定的时间代价，因此 定位效率不够高。为了解决机器人1“绕远”的问题，可以对生成的一级 路径进行优化，缩短机器人1的运动路径、提高定位效率。

使机器人的运动路径缩短，则机器人各关节的运动幅度也减小，对应 到位姿空间中，就是要将从起始位姿点到目标位姿点的位姿点序列的长度 优化到最短，同时保持优化之后路径仍是无碰撞路径。

可以将上文所述的一级路径，即位姿点序列输入到工作站8的路径优 化模块中进行处理。为了直观，结合图8所示的二维路径14阐述路径优化 策略。可以将路径14视作一级路径，它的起点是起始位姿点，终点是目标 位姿点，中间是一系列相邻的位姿点。路径优化的基本思路可以是：

在一级路径14中选择包括一系列相邻位姿点的二级路径MN。二级路 径MN也是位姿点序列，即从位姿点M到位姿点N的位姿点序列，其可 以包括预定数目的位姿点。二级路径MN的选择方式可以是随机的。

随后，计算位姿点M与位姿点N之间的直线距离，即线段MN的长 度，以判断二级路径MN的长度是否为最短。

位姿点M与位姿点N之间的直线距离可以用d表示，二级路径MN 的路径长度可以用s表示。假设预设条件为d≥0.95s。将d与s相比较， 如果d≥0.95s，则可以认为二级路径MN已为最短，无需对其进行优化， 则针对该二级路径MN的优化可以结束。随后，可以在一级路径14中重 新随机选择二级路径。如果d＜0.95s，则认为二级路径MN需要优化。

如果二级路径MN需要优化，则可以在二级路径MN的位姿点M和位 姿点N之间直线插值，得到以M和N为首部位姿点和尾部位姿点的线段 作为三级路径。可以判断该三级路径的首部位姿点和尾部位姿点(即该三 级路径的两个端点)之间的直线连接，即以这两点为端点的线段，是否与 经映射的障碍模型发生碰撞。如果不发生碰撞，意味着机器人1直接从该 三级路径的首部位姿点经各插值点运动到尾部位姿点的过程中不会与障碍 发生碰撞，因此可以将该二级路径上的位姿点删除，将三级路径添加到一 级路径，以更新一级路径。随后，可以继续在经更新的一级路径中随机选 择二级路径并重复进行上述操作。如果以该三级路径的首部位姿点和尾部 位姿点为端点的线段与经映射的障碍模型发生碰撞，则重新选择二级路径 进行优化。

重复上述子优化步骤过程，直到达到一定的重复次数，例如预先设定 的次数阈值。在所有子优化步骤结束之后，即在停止执行子优化步骤之后， 所获得的一级路径即为所需要的无碰撞路径。

可选地，在上述步骤S1314之后，避障规划方法100可以进一步包括： 对于所述一条或多条无碰撞路径中的每一条，对手术机器人和障碍模型进 行三维建模，以获得手术机器人的三维模型和障碍模型的三维模型；显示 用于表示手术机器人的三维模型按照该无碰撞路径运动的图像序列，其中， 图像序列中的每个图像包含手术机器人的三维模型和障碍模型的三维模型； 接收针对该无碰撞路径的反馈信号；以及如果反馈信号是重新规划信号， 则执行步骤S1314。

可以将无碰撞路径传送给工作站8的可视化虚拟验证模块，以由可视 化虚拟验证模块执行可视化虚拟验证步骤。可视化虚拟验证模块可以通过 显示界面显示机器人1和包括障碍模型的障碍场景，并且通过动画(即图 像序列)显示机器人1沿着该无碰撞路径运动的运动状态，可供操作者判 断机器人1的运动路径是否安全合理，给操作者提供参考。

根据一个实施例，可以采用可视化工具，例如视觉化工具函式库(VTK， VisualizationToolkit)对机器人1(例如机器人1的手臂各连杆)、对象头 部包络球9、头架包络长方体10和障碍墙模型11进行三维建模。

根据一个示例，可以建立机器人1的D-H模型。可以基于机器人的 D-H方法，为机器人1的各连杆的三维模型设置各自的坐标系，并建立机 器人1的运动学模型。将位姿点序列中的位姿点即各关节角度输入到运动 学模型中，得到机器人1的各连杆的位置和姿态。

按照时间顺序依次将位姿点序列中的位姿点输入到机器人1的运动学 模型中，显示各连杆的位姿，形成动态的机器人1从起始位姿点运动到目 标位姿点的动画。同时，一直显示着各障碍的三维模型。此时机器人1实 际上并没有发生运动，因此操作者可以预先判断对机器人1的运动路径是 否满意，若不满意，可经由用户界面将反馈信号输入到工作站8的路径规 划模块。路径规划模块接收该反馈信号。如果反馈信号是重新规划信号， 即指示重新进行路径规划，则路径规划模块可以根据该反馈信号的指示重 新进行路径规划。可以理解的是，可视化虚拟验证模块可以继续对新生成 的无碰撞路径进行显示，以由操作者判断是否满意。路径规划和可视化虚 拟验证的步骤可以重复进行，直至操作者对最后生成的无碰撞路径满意为 止。若操作者对机器人1的运动路径满意，则可进行通过反馈信号指示使 机器人进行后续的定位操作。

随后，将最后生成的无碰撞路径，即位姿点序列传送给机器人1。可 以利用工作站8的机器人控制模块控制机器人1依次按照每个位姿点运动， 机器人的运动路径将与在可视化虚拟验证步骤中显示的运动路径一致，最 终在目标位姿对应的目标位姿点停止，即完成一条操作路径的定位。此时 套管13所指示的方向即为操作路径的方向。

可以针对每条操作路径，执行一次步骤S1314和/或上文所述的可视化 虚拟验证步骤。在需要多条操作路径的情况下，可以依次针对每条操作路 径执行步骤S1314和/或上文所述的可视化虚拟验证步骤。

图9示出了根据本发明一个实施例的利用机器人对操作路径进行定位 的流程图。如图9所示，定位方法900可以包括步骤S910～S980。在步骤 S910，创建障碍模型。在步骤S920，通过注册步骤获取各坐标系之间的变 换关系。在步骤S930，将障碍模型映射到机器人坐标系。在步骤S940， 确定路径规划的起始位姿点和目标位姿点。在步骤S950，进行针对机器人 的无碰撞路径规划。在步骤S960，对在步骤S950中获得的无碰撞路径(例 如上文所述的一级路径)进行路径优化。在步骤S970，进行可视化虚拟验 证。在步骤S980，机器人根据规划出来的无碰撞路径执行定位。随后，可 以返回步骤S940，重新开始对下一条操作路径的定位。

本发明结合立体定向操作场景设计了相应的障碍信息，将障碍信息映 射到机器人空间，进行针对机器人的路径规划。经过可视化虚拟验证之后， 机器人可以安全地定位操作路径，这个方法保证了机器人在主动定位过程 中能够避开对象和其他物体，完成对操作路径的安全定位和引导。并且该 方法可多次快速地规划出无碰撞的机器人运动路径，具有高效、安全等特 点，通用性强，在立体定向操作中具有明确的使用价值。

根据本发明另一方面，提供一种避障规划系统。图10示出根据本发明 一个实施例的避障规划系统1000的示意性框图。如图10所示，避障规划 系统1000包括模型创建装置1010、映射装置1020和路径规划装置1030。 模型创建装置1010用于创建障碍模型，其中，障碍模型包括对象头部模型、 头架模型和障碍墙模型。映射装置1020用于将障碍模型映射到机器人坐标 系，以获得经映射的障碍模型。路径规划装置1030用于至少基于经映射的 障碍模型进行路径规划，以获得针对手术机器人的一条或多条无碰撞路径。

可选地，避障规划系统1000可以进一步包括第一坐标系注册装置、 第二坐标系注册装置和第三坐标系注册装置(未示出)。

第一坐标系注册装置包括获取模块、接收模块、第一坐标确定模块、 第一采集模块、第二坐标确定模块和第一变换关系计算模块。获取模块用 于获取对象图像。接收模块用于接收针对对象图像中的图像标识点的标识 点输入信号。第一坐标确定模块用于根据标识点输入信号确定图像标识点 在图像坐标系中的第一坐标。第一采集模块用于采集来自对象坐标系参考 物的第一参考光信号与来自探测器的、与对象标识点相关的第一标识光信 号，其中，对象标识点与图像标识点相对应。第二坐标确定模块用于根据 第一参考光信号和第一标识光信号确定对象标识点在对象坐标系中的第二 坐标。第一变换关系计算模块，用于基于第一坐标和第二坐标计算图像坐 标系与对象坐标系之间的第一变换关系。

第二坐标系注册装置包括控制模块、第三坐标确定模块、第二采集模 块、第四坐标确定模块和第二变换关系计算模块。控制模块用于控制手术 机器人的末端机构运动到特定位置处，其中，探测器固定于末端机构上。 第三坐标确定模块用于基于末端机构在机器人坐标系中的坐标确定探测器 在机器人坐标系中的第三坐标。第二采集模块用于采集来自对象坐标系参 考物的第二参考光信号与来自探测器的第二标识光信号。第四坐标确定模 块用于根据第二参考光信号和第二标识光信号确定探测器在对象坐标系中 的第四坐标。第二变换关系计算模块用于基于第三坐标和第四坐标计算机 器人坐标系与对象坐标系之间的第二变换关系。

第三坐标系注册装置包括第三变换关系计算模块。第三变换关系计算 模块用于根据第一变换关系和第二变换关系计算机器人坐标系与图像坐标 系之间的第三变换关系。

在以上关于避障规划方法的描述中，已经详细描述了各步骤的实施方 式和功能作用等，本领域技术人员结合以上关于图1至图9的描述能够理 解避障规划系统1000的具体结构、运行方式及其优点等，为了简洁，本文 不对此进行赘述。

在本发明的描述中，需要理解的是，术语“上”、“下”、“左”、“右”、“顶”、 “底”、“内”、“外”等指示的方位或位置关系为基于正常使用时的方位或位 置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的 装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理 解为对本发明的限制。此外，术语“第一”、“第二”仅用于描述目的，而不 能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。 由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更 多个该特征。

本发明已经通过上述实施例进行了说明，但应当理解的是，上述实施 例只是用于举例和说明的目的，而非意在将本发明限制于所描述的实施例 范围内。此外本领域技术人员可以理解的是，本发明并不局限于上述实施 例，根据本发明的教导还可以做出更多种的变型和修改，这些变型和修改 均落在本发明所要求保护的范围以内。本发明的保护范围由附属的权利要 求书及其等效范围所界定。