CN115031739A - 连续体机器人路径规划方法、装置、电子设备及存储介质 - Google Patents

连续体机器人路径规划方法、装置、电子设备及存储介质 Download PDF

Info

Publication number
CN115031739A
CN115031739A CN202210969733.0A CN202210969733A CN115031739A CN 115031739 A CN115031739 A CN 115031739A CN 202210969733 A CN202210969733 A CN 202210969733A CN 115031739 A CN115031739 A CN 115031739A
Authority
CN
China
Prior art keywords
path
continuum robot
point
target
natural cavity
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210969733.0A
Other languages
English (en)
Inventor
边桂彬
李桢
汤向荣
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Institute of Automation of Chinese Academy of Science
Original Assignee
Institute of Automation of Chinese Academy of Science
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Institute of Automation of Chinese Academy of Science filed Critical Institute of Automation of Chinese Academy of Science
Priority to CN202210969733.0A priority Critical patent/CN115031739A/zh
Publication of CN115031739A publication Critical patent/CN115031739A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B10/00—Other methods or instruments for diagnosis, e.g. instruments for taking a cell sample, for biopsy, for vaccination diagnosis; Sex determination; Ovulation-period determination; Throat striking implements
    • A61B10/02—Instruments for taking cell samples or for biopsy
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B10/00—Other methods or instruments for diagnosis, e.g. instruments for taking a cell sample, for biopsy, for vaccination diagnosis; Sex determination; Ovulation-period determination; Throat striking implements
    • A61B10/02—Instruments for taking cell samples or for biopsy
    • A61B2010/0208—Biopsy devices with actuators, e.g. with triggered spring mechanisms

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Surgery (AREA)
  • Veterinary Medicine (AREA)
  • Molecular Biology (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Animal Behavior & Ethology (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Medical Informatics (AREA)
  • Biomedical Technology (AREA)
  • Pathology (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Manipulator (AREA)

Abstract

本发明实施例涉及一种连续体机器人路径规划方法、装置、电子设备及存储介质，涉及机器人控制领域；通过构建连续体机器人运动学模型；根据待检测体的目标器官扫描信息构建目标器官的自然腔道模型，并获取所述自然腔道模型中各自然腔道的中心线信息；根据所述自然腔道的中心线信息确定从自然腔道入口到目标器官上目标位置的初始连通路径；基于改进快速探索随机树算法对所述初始连通路径进行符合所述连续体机器人运动学模型的处理，获得规划路径，以控制所述连续体机器人根据所述规划路径到达目标器官上目标位置进行活检；即本发明实施例实现了连续体机器人完全自主导航到被检测体目标器官上的可疑病变位置，实现快速准确的活检。

Description

连续体机器人路径规划方法、装置、电子设备及存储介质
技术领域
本发明涉及机器人控制领域，尤其涉及一种连续体机器人路径规划方法、装置、电子设备及存储介质。
背景技术
研究数据表明，癌症是居民的首要死因。而包括肺癌、消化道癌等许多癌症在内的患者都是在中晚期阶段被诊断出来，而早期发现这类癌症有助于患者尽早治疗诊断，进而提高癌症患者的生存率。
传统的癌症诊断方法，主要是依靠临床医生通过医学影像数据识别病变区域，但是无法足够准确地判断病变性质。在医生的操作下，采用经人体自然腔道活检的方法，将专业诊断仪器送入到人体内部的可疑部位进行诊断，可以对疑似病变组织进行采样。但由于人体器官，如支气管和腹腔等部位形态多变、迂回曲折，导致许多地方无法探查到。
发明内容
本发明提供了一种连续体机器人路径规划方法、装置、电子设备及存储介质，以解决现有技术无法在复杂的人体器官环境中快速探查病变位置的技术问题。
第一方面，本发明提供了一种连续体机器人路径规划方法，包括：构建连续体机器人运动学模型；根据待检测体的目标器官扫描信息构建目标器官的自然腔道模型，并获取所述自然腔道模型中各自然腔道的中心线信息；根据所述自然腔道的中心线信息确定从自然腔道入口到目标器官上目标位置的初始连通路径；基于改进快速探索随机树算法对所述初始连通路径进行符合所述连续体机器人运动学模型的处理，获得规划路径，以控制所述连续体机器人根据所述规划路径到达目标器官上目标位置进行活检。
作为本发明可选的实施例，所述根据所述自然腔道的中心线信息确定从自然腔道入口到目标器官上目标位置的初始连通路径，包括：从所述目标器官上目标位置开始，回溯上一级自然腔道的分支节点，直至回溯到所述自然腔道入口；将从自然腔道入口到分支节点，再到目标位置之间的中心线确定为多个分段路径，所述多个分段路径构成所述初始连通路径。
作为本发明可选的实施例所述基于改进快速探索随机树算法对所述初始连通路径进行符合所述连续体机器人运动学模型的处理，获得规划路径，包括：将自然腔道入口作为起始点添加到点队列中；将每个分段路径的终点作为对应分段路径的子目标点，所述目标位置作为最终目标点，并将各子目标点和最终目标点添加到目标队列中；初始化规划路径队列为空队列；从j=1开始，迭代执行如下步骤，直至所述连续体机器人远端到达所述最终目标点，并获取规划路径队列中的点序列作为所述规划路径：基于改进快速探索随机树算法对第j个分段路径进行符合所述连续体机器人运动学模型的处理，并在连续体机器人远端的当前位置到达第j个分段路径的子目标点时，获得第j个分段路径的规划路径，所述第j个分段路径的规划路径包括路径点序列，并将所述路径点序列添加到所述点队列的尾部；将所述点队列中的点序列添加到上一迭代轮次获得的规划路径队列的尾部；清空点队列，并将连续体机器人远端的当前位置作为起始点添加到点队列中，并对j进行加一处理。
作为本发明可选的实施例，所述连续体机器人远端到达所述最终目标点，包括：若所述连续体机器人远端的当前位置与最终目标点的距离小于第一预设阈值，则确定所述连续体机器人远端到达最终目标点。
作为本发明可选的实施例，所述基于改进快速探索随机树算法对第j个分段路径进行符合所述连续体机器人运动学模型的处理，包括：确定第j个分段路径的采样区域，其中，所述采样区域是以第j个分段路径的子目标点为球心的球体，所述球体的半径根据第j个分段路径所处的自然腔道直径确定；迭代执行以下步骤，直至连续体机器人远端的当前位置到达第j个分段路径的子目标点：在所述采样区域内随机选取采样点，并将所述采样点的位置信息和第j个分段路径的子目标点的位置信息相加，获得新采样点；控制所述连续体机器人远端向所述新采样点移动预设距离，并获得连续体机器人远端的当前位置。
作为本发明可选的实施例，所述连续体机器人的当前位置到达第j个分段路径的子目标点，包括：若连续体机器人远端的当前位置与第j个分段路径的子目标点的距离小于第二预设阈值，则确定所述连续体机器人的当前位置到达第j个分段路径的子目标点。
作为本发明可选的实施例，所述方法还包括：若所述连续体机器人远端向所述新采样点移动预设距离时，所述连续体机器人远端前表面的预设圆形区域与自然腔道发生碰撞，则放弃新采样点，并重新执行所述在所述采样区域内随机选取采样点的步骤。
第二方面，本发明提供一种连续体机器人路径规划装置，包括：第一构建模块，用于构建连续体机器人运动学模型；第二构建模块，用于根据待检测体的目标器官扫描信息构建目标器官的自然腔道模型，并获取所述自然腔道模型中各自然腔道的中心线信息；初始路径模块，用于根据所述自然腔道的中心线信息确定从自然腔道入口到目标器官上目标位置的初始连通路径；路径优化模块，用于基于改进快速探索随机树算法对所述初始连通路径进行符合所述连续体机器人运动学模型的处理，获得规划路径，以控制所述连续体机器人根据所述规划路径到达目标器官上目标位置进行活检。
第三方面，本发明提供了一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；存储器，用于存放计算机程序；处理器，用于执行存储器上所存放的程序时，实现第一方面任一项所述的连续体机器人路径规划方法的步骤。
第四方面，本发明提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现第一方面任一项所述的连续体机器人路径规划方法的步骤。
本发明实施例提供的连续体机器人路径规划方法、装置、电子设备及存储介质，通过构建连续体机器人运动学模型；根据待检测体的目标器官扫描信息构建目标器官的自然腔道模型，并获取所述自然腔道模型中各自然腔道的中心线信息；根据所述自然腔道的中心线信息确定从自然腔道入口到目标器官上目标位置的初始连通路径；基于改进快速探索随机树算法对所述初始连通路径进行符合所述连续体机器人运动学模型的处理，获得规划路径，以控制所述连续体机器人根据所述规划路径到达目标器官上目标位置进行活检；即本发明的实施例在待检测体目标器官的自然腔道中介入连续体机器人，并通过改进快速探索随机树算法获得连续体机器人在目标器官的自然腔道中的规划路径，从而实现了连续体机器人完全自主导航到人体或其他被检动物体目标器官上的可疑病变位置，实现快速准确的活检，提高癌症早期发现的机率。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种连续体机器人路径规划方法的流程示意图；
图2为本发明实施例提供的另一种连续体机器人路径规划方法的流程示意图；
图3为本发明实施例提供的一种采样区域的示意图；
图4为本发明实施例提供的一种采样点的获取流程示意图；
图5为本发明实施例提供的一种连续体机器人远端的检测区域示意图；
图6为本发明实施例提供的一种基于改进快速探索随机树算法对初始连通路径进行处理的流程示意图；
图7为本发明实施例提供的一种连续体机器人路径规划装置的结构示意图；
图8为本发明实施例提供的一种电子设备的结构示意图；
1-连续体机器人；2-自然腔道；3-采样区域。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
研究数据表明，癌症是居民的首要死因。而包括肺癌、消化道癌等许多癌症在内的患者都是在中晚期阶段被诊断出来，而早期发现这类癌症有助于患者尽早治疗诊断，进而提高癌症患者的生存率。
传统的癌症诊断方法，主要是依靠临床医生通过医学影像数据识别病变区域，但是无法足够准确地判断病变性质。在医生的操作下，采用经人体自然腔道活检的方法，将专业诊断仪器送入到人体内部的可疑部位进行诊断，可以对疑似病变组织进行采样。但由于人体器官，如支气管和腹腔等部位形态多变、迂回曲折，导致许多地方无法探查到。
针对上述技术问题，本发明的技术构思在于：在待检测体目标器官的自然腔道中介入连续体机器人，并通过改进快速探索随机树算法获得连续体机器人在自然腔道中的规划路径，从而实现了连续体机器人完全自主导航到人体器官的可疑病变位置，实现快速准确的活检，提高癌症早期发现的机率。
图1为本发明实施例提供的一种连续体机器人路径规划方法的流程示意图，如图1所示，该连续体机器人路径规划方法包括：
步骤S101、构建连续体机器人运动学模型。
需要说明的是，本实施例的执行主体可以为连续体机器人路径规划装置，或者部署有连续体机器人路径规划的电子设备。本步骤中，构建连续体机器人的运行学模型。
可选的，所述步骤S101包括：基于分段常曲率假设构建连续体机器人运动学模型，获得连续体机器人的第i个关节段的构型空间到连续体机器人远端工作空间的变换矩阵T i ，如公式（1）所示：
Figure 582256DEST_PATH_IMAGE001
（1）
其中，L i 表示连续体机器人第i个关节段的弯曲弧长，φ i 表示连续体机器人第i个关节段的弯曲角，θ i 表示连续体机器人第i个关节段的弧面角，c表示余弦函数cos，s表示正弦函数sin。
具体来说，连续体机器人的运动学模型采用已有的分段常曲率法。分段常曲率模型的建立基于如下假设：连续体机器人的结构材料不会被压缩，连续体机器人的主动段在弯曲过程中，各部分曲率相同；同时主动段各段中心轴线的长度不变。将连续体机器人的每个关节段看做常曲率圆弧，则整个主动段是由若干个相切的圆弧构成，即一段主动段可看作一整段圆弧。
由于连续体机器人的仿真模型只关心机器人位形与环境的碰撞，因此仅分析机器人不依赖于驱动形式的构型空间到工作空间的映射，即连续体机器人每个关节段的构型空间到连续体机器人远端工作空间的变换矩阵即可。其中，构型空间由连续体机器人的广义坐标所描述，通常由圆弧长L ，弯曲角φ和弧面角θ组成，广义坐标个数就是连续体机器人的自由度数；工作空间是指连续体机器人的远端执行器的位置和姿态所张成的空间，工作空间参数由机械臂远端的坐标参数 (x,y,z) 和姿态描述参数 (α,β,γ) 所组成。
步骤S102、根据待检测体的目标器官扫描信息构建目标器官的自然腔道模型，并获取所述自然腔道模型中各自然腔道的中心线信息。
本实施例中，待检测体可以为人或其他被检动物体，目标器官可以为具有自然腔道的器官，如肺部支气管。为了构建真实的目标器官模型，首先使用CT等影像设备对活检部位扫描，获得医学影像数据，然后，使用医学三维软件根据医学影像数据进行重建，生成树型的自然腔道模型。例如，可以采用深度优先搜索(Depth-First-Search，简称DFS)方法构造以气管入口为节点的树型肺部支气管模型。
另外，在医学三维软件中，还可以提取目标器官主自然腔道的中心线数据，用于后续改进快速探索随机树算法中上层路径信息的获取，还要提取各支自然腔道的中心线数据，比如肺部的气管和各支气管的中心线数据，可选的，获得的中心线数据为IGS格式的线段，以便于在后续使用时进行相应处理。
步骤S103、根据所述自然腔道的中心线信息确定从自然腔道入口到目标器官上目标位置的初始连通路径。
具体来说，以目标器官的自然腔道入口为起点，目标器官选定的目标位置（一般为病变部位）为终点，建立初始连通路径，该初始连通路径由起点和终点之间的自然腔道中心线表示。例如构建肺部从气管入口到支气管病变位置的初始连通路径。
作为可选的实施例，所述步骤S103包括：从所述目标器官上目标位置开始，回溯上一级自然腔道的分支节点，直至回溯到所述自然腔道入口；将从自然腔道入口到分支节点，再到目标位置之间的中心线确定为多个分段路径，所述多个分段路径构成所述初始连通路径。
具体来说，获得初始连通路径的方法是在树型的自然腔道结构中采用回溯方法，即由目标位置的中心线位置开始，不断向上搜索上一级自然腔道的父节点，直到寻找到自然腔道入口处，完成搜索，所得到的节点间的中心线即为初始连通路径，该初始连通路径是由多个折线段（即多个分段路径）构成。
步骤S104、基于改进快速探索随机树算法对所述初始连通路径进行符合所述连续体机器人运动学模型的处理，获得规划路径，以控制所述连续体机器人根据所述规划路径到达目标器官上目标位置进行活检。
具体来说，由人体器官的解剖结构可以看出，人体内的自然腔道向体内延伸，并不断产生分叉或进入下一级腔道，同一级腔道间不会产生交叉，所以可以采用快速探索随机树（Rapidly Exploring Random Trees，简称RRT）算法来规划连续体机器人的运动路径。本实施例中，采用了改进的知情采样快速探索随机树（Informed RRT*）进行连续体机器人的路径规划，该算法围绕初始连通路径，探索连续体机器人运动的可行路径，并返回探索结果，因此，该算法又可称为用于连续体机器人的快速探索随机树算法。
更进一步的说，以肺部支气管为例，初始连通路径包括气管入口（作为起始点P start ）的位置、支气管目标位置（作为最终目标点P goal ）位置，以及肺气管中心线信息，也就是说，可将起始点P start 和目标点P goal 的位置，以及肺气管中心线信息作为本实施例所采用的用于连续体机器人的快速探索随机树算法的输入，算法的输出为一组通往最终目标点P goal 且考虑连续体机器人运动学模型（即连续体机器人的限制）的路径点序列ρ = [P 1 ,..,P I ]，当找到可行路径时，机器人远端位置P fwd 与目标点位置P goal 的距离dist必须尽可能小，距离可采用欧氏距离度量。路径点之间的距离需要以轨迹规划方式生成运动轨迹序列Tr =[S 0 ,...,S I-1 ]，其中，轨迹规划的输出为齐次变换矩阵
Figure 499397DEST_PATH_IMAGE002
其中R i ∈ SO(3) 为旋转矩阵，P i ∈R 3 为平移矩阵。
本发明实施例提供的连续体机器人路径规划方法，通过构建连续体机器人运动学模型；根据待检测体的目标器官扫描信息构建目标器官的自然腔道模型，并获取所述自然腔道模型中各自然腔道的中心线信息；根据所述自然腔道的中心线信息确定从自然腔道入口到目标器官上目标位置的初始连通路径；基于改进快速探索随机树算法对所述初始连通路径进行符合所述连续体机器人运动学模型的处理，获得规划路径，以控制所述连续体机器人根据所述规划路径到达目标器官上目标位置进行活检；即本发明的实施例在待检测体目标器官的自然腔道中介入连续体机器人，并通过改进快速探索随机树算法获得连续体机器人在目标器官的自然腔道中的规划路径，从而实现了连续体机器人完全自主导航到人体或其他被检动物体目标器官上的可疑病变位置，实现快速准确的活检，提高癌症早期发现的机率。
在上述实施例的基础上，图2为本发明实施例提供的另一种连续体机器人路径规划方法的流程示意图，如图2所示，该连续体机器人路径规划包括：
步骤S201、构建连续体机器人运动学模型。
步骤S202、根据待检测体的目标器官扫描信息构建目标器官的自然腔道模型，并获取所述自然腔道模型中各自然腔道的中心线信息。
步骤S203、从所述目标器官上目标位置开始，回溯上一级自然腔道的分支节点，直至回溯到所述自然腔道入口。
步骤S204、将从自然腔道入口到分支节点，再到目标位置之间的中心线确定为多个分段路径，所述多个分段路径构成所述初始连通路径。
步骤S205、将自然腔道入口作为起始点添加到点队列中。
步骤S206、将每个分段路径的终点作为对应分段路径的子目标点，所述目标位置作为最终目标点，并将各子目标点和最终目标点添加到目标队列中。
步骤S207、初始化规划路径队列为空队列。
然后从j=1开始，迭代执行步骤S208-S210，直至所述连续体机器人远端到达最终目标点。
步骤S208、基于改进快速探索随机树算法对第j个分段路径进行符合所述连续体机器人运动学模型的处理，并在连续体机器人远端的当前位置处于第j个分段路径的子目标点时，获得第j个分段路径的规划路径，所述第j个分段路径的规划路径包括路径点序列，并将所述路径点序列添加到所述点队列的尾部。
步骤S209、将所述点队列中的点序列添加到上一迭代轮次获得的规划路径队列的尾部。
步骤S210、清空点队列，将连续体机器人远端的当前位置作为起始点添加到点队列中，并对j进行加一处理。
步骤S211、获取规划路径队列中的点序列作为所述规划路径，以控制所述连续体机器人根据所述规划路径到达目标器官上目标位置进行活检。
本发明实施例中的步骤S201和S202的实现方式与上述实施例中的步骤S101和S102的实现方式类似，步骤S203、S204的实现方式与上述实施例中的步骤S103的实现方式类似，此处不再赘述。
与上述实施例的区别在于，本发明实施例进一步限定了利用改进快速探索随机树算法围绕初始连通路径探索可行路径的具体实现方式。在本实施例中，将自然腔道入口作为起始点添加到点队列中；将每个分段路径的终点作为对应分段路径的子目标点，所述目标位置作为最终目标点，并将各子目标点和最终目标点添加到目标队列中；初始化规划路径队列为空队列；从j=1开始，迭代执行如下步骤，直至所述连续体机器人远端到达所述最终目标点，并获取规划路径队列中的点序列作为所述规划路径：基于改进快速探索随机树算法对第j个分段路径进行符合所述连续体机器人运动学模型的处理，并在连续体机器人远端的当前位置到达第j个分段路径的子目标点时，获得第j个分段路径的规划路径，所述第j个分段路径的规划路径包括路径点序列，并将所述路径点序列添加到所述点队列的尾部；将所述点队列中的点序列添加到上一迭代轮次获得的规划路径队列的尾部；清空点队列，并将连续体机器人远端的当前位置作为起始点添加到点队列中，并对j进行加一处理；然后，获取规划路径队列中的点序列作为所述规划路径，以控制所述连续体机器人根据所述规划路径到达目标器官上目标位置进行活检。
本实施例中，以肺部支气管为例，首先进行初始化，包括将气管入口作为起始点添加到点队列V中；将初始连通路径的各个分段路径的终端作为对应分段路径的子目标点，将支气管目标位置作为最终目标点，将各子目标点和最终目标点添加到目标队列G中，即G =[P goal1 , ..., P goalJ , P goal ]；同时初始化规划路径队列为空队列。然后，基于改进的快速探索随机树算法探索第一个（j=1）分段路径的可行路径，并在到达第一个分段路径的子目标点后，回溯第一个分段路径到V队列，得到第一个分段路径的规划路径点序列，将其添加至路径规划队列τ中。然后，清空V队列，并将当前位置重新添加到V队列，开始下一段路径探索。如此重复进行以上过程，直至到达最终目标点P goal ，即连续体机器人先后到达各子目标点后继续前进，最终到达目标点P goal ，并返回最终路径τ，以控制连续体机器人根据最终路径到达人体或其他被检动物体支气管的可疑病变位置进行活检。
另外，在初始化时，还要将连续体机器人的基坐标系{B}和目标坐标系{G}等也变换为世界坐标系{W}。
作为可选的实施例，所述步骤S208包括：确定第j个分段路径的采样区域，其中，所述采样区域是以第j个分段路径的子目标点为球心的球体，所述球体的半径根据第j个分段路径所处的自然腔道直径确定；迭代执行以下步骤，直至更新后的连续体机器人的当前位置处于第j个分段路径的子目标点：在所述采样区域内随机选取采样点，并将所述采样点的位置信息和第j个分段路径的子目标点的位置信息相加，获得新采样点；控制所述连续体机器人远端向所述新采样点移动预设距离，获得连续体机器人远端的当前位置。
具体来说，Informed RRT*算法的采样点只能在形状为椭球体的知情采样集内选择，其中，初始知情集的建立依赖于初始连通路径的确立，为了加快用于连续体机器人的快速探索随机树算法的探索过程，本实施例首先根据据已有的肺气管中心线信息，提前限制了采样区域。图3为本发明实施例提供的一种采样区域的示意图，图4为本发明实施例提供的一种采样点的获取流程示意图，现结合图3和图4对本可选实施例进行说明。
首先，确定连续体机器人1在第j个分段路径探索过程的采样区域3，该采样区域3为子目标点P goalj 周围的球体区域，球体的半径R j 与所在的自然腔道2的直径有关，如图3所示；然后，在采样区域3内随机选取采样点，即采样点坐标X rand ，包括采样点与球心的距离r、方位角φ和仰角θ随机取值；然后，将坐标X rand 与P goalj 相加，形成新采样点X sample ，并返回X sample ，如图4所示。然后，连续体机器人1由已有树中的节点向新采样点X sample 运动一小段时间，即向新采样点X sample 延伸一小段（预设）距离，所达到的新位置即为生成的点X new ，然后，将新探索的点X new 添加到点队列V中，同时将相应位姿添加到边队列E中，从而使随机树生长。最后，重复上述采样、移动的过程，直到连续体机器人1远端的X new 到达子目标点P goalj 。也就是说，在实际操作过程中，连续体机器人1将首先匀速下降一小段距离，随后开始随机采样。
树型结构中的点包含机器人远端位置的坐标，机器人主动段姿态和该节点的父节点指针信息。当完成搜索时，由点队列中最后一个节点向上回溯，即可得到完整的生成路径。
作为可选的实施例，若所述连续体机器人远端向所述新采样点移动预设距离时，所述连续体机器人远端前表面的预设圆形区域与自然腔道发生碰撞，则放弃新采样点，并重新执行所述在所述采样区域内随机选取采样点的步骤。
具体来说，随机树的生长过程必须通过碰撞测试，才能将新生成的X new 添加到路径规划队列中。在连续体机器人运动过程中，由于其转向必须依赖与环境的接触产生的反作用力来实现，从而连续体机器人本体侧面必然与气管壁产生接触。因此在本实施例中，仅检测连续体机器人远端前部的表面是否发生碰撞，来作为机器人发生碰撞的判断依据，图5为本发明实施例提供的一种连续体机器人远端的检测区域示意图，如图5所示，其具体检测范围为连续体机器人远端前表面的一个圆形区域（见图5中的阴影部分）。当连续体机器人前表面与气管壁发生碰撞时，则放弃当前采样过程生成的位姿，重新进行采样。
作为可选的实施例，所述连续体机器人到达最终目标点，包括：若所述连续体机器人远端的当前位置与最终目标点的距离小于第一预设阈值，则确定所述连续体机器人到达最终目标点；所述连续体机器人的当前位置处于第j个分段路径的子目标点，包括：若连续体机器人远端的当前位置与第j个分段路径的子目标点的距离小于第二预设阈值，则确定所述连续体机器人的当前位置处于第j个分段路径的子目标点。
具体来说，为了能够满足用于手术的连续体机器人所携器械进一步的工作需要，连续体机器人远端位置P fwd 必须尽可能接近目标点位置。在本实施例中，将连续体机器人远端位置P fwd 到目标点P goal 的距离dist小于一个特定值时，即视为实现规划目标，可选的，距离dist采用欧氏距离度量。当连续体机器人对初始连通路径分段探索时，机器人远端位置P fwd 距离P goalj 小于特定值时，即视为到达该子目标点，并从当前状态起将目标设为P goalj+1 。
图6为本发明实施例提供的一种基于改进快速探索随机树算法对初始连通路径进行处理的流程示意图，如图6所示，在获得初始连通路径后，初始化点队列V、目标队列G、路径规划队列τ,以及随机树边队列E；然后判断连续体机器人远端是否达到最终目标点P goal ，若是，说明算法的当前状态是完成了对初始连通路径的最后一个分段路径的探索，则直接将探索的路径返回τ队列，若否，则判断是否达到子目标点P goalj ；若达到子目标点P goalj ，则说明算法的当前状态是完成了对第j个分段路径的探索，则将第j个分段路径的探索路径加入τ队列，并清空V队，更新P goalj ,若没有达到子目标点P goalj ，则可以在P goalj 周围的球体区域进行采样；然后控制机器人远端向采样点前进一段距离，并判断前表面是否发生碰撞，若是，则重新采样，若否，将机器人远端的当前位置加入V队列，并将前进的这一段距离作为生长的随机树的边加入到E队列中。
本发明实施例提供的连续体机器人路径规划方法，通过将自然腔道入口作为起始点添加到点队列中；将每个分段路径的终点作为对应分段路径的子目标点，所述目标位置作为最终目标点，并将各子目标点和最终目标点添加到目标队列中；初始化规划路径队列为空队列；从j=1开始，迭代执行如下步骤，直至所述连续体机器人远端到达所述最终目标点，并获取规划路径队列中的点序列作为所述规划路径：基于改进快速探索随机树算法对第j个分段路径进行符合所述连续体机器人运动学模型的处理，并在连续体机器人远端的当前位置到达第j个分段路径的子目标点时，获得第j个分段路径的规划路径，所述第j个分段路径的规划路径包括路径点序列，并将所述路径点序列添加到所述点队列的尾部；将所述点队列中的点序列添加到上一迭代轮次获得的规划路径队列的尾部；清空点队列，并将连续体机器人远端的当前位置作为起始点添加到点队列中，并对j进行加一处理；然后，获取规划路径队列中的点序列作为所述规划路径，以控制所述连续体机器人根据所述规划路径到达目标器官上目标位置进行活检；即本发明实施例通过改进快速探索随机树算法对初始连通路径进行分段探测可行路径，最终获得完整规划路径，实现了在曲折幽微的待检测体器官环境中快速探索到达病变位置的路径规划。
为了更进一步了解本发明，还提供了一种连续体机器人路径规划方法，其包括如下步骤：
一、构建机器人运动学模型
采用已有的分段常曲率法构建连续体机器人的运动学模型，获得连续体机器人不依赖于驱动形式的每个关节段的构型空间到连续体机器人远端工作空间的映射，其变化矩阵如公式（1）所示。
二、构建目标器官的自然腔道模型
通过医学影像设备获取人体的器官空间信息，建立三维器官模型并提取自然腔道中心线信息。
三、获取初始连通路径
根据目标点的位置建立自然腔道入口到目标点之间的初始连通支气管路径。
四、利用已知信息采样的连续体机器人路径规划方法。
在初始连通路径中基于改进快速探索随机树算法完成考虑连续体机器人运动学的路径规划，获得由连续体机器人远端在自然腔道中的路径点表示的规划路径。
综上，本发明的实施例除了实现在形态多变、迂回曲折的自然腔道结构中运动到指定位置，并使用所携带的可操纵针等设备对病变部位进行活检以外，还实现了完全自主导航，相比于传统的机器人主从交互控制或半自动控制方式需要医生手动操作或者持续监督，来避免机器人与路径中的肺气管壁相碰撞，显著减轻医生操作的疲劳和学习成本，并且采用的用于连续体机器人的快速探索随机树算法相比与传统的RRT方法，在狭窄空间中更容易探索前进，提高探索成功率，并且缩短了手术导航时间，提高了导航精度。
图7为本发明实施例提供的一种连续体机器人路径规划装置的结构示意图，如图7所示，该连续体机器人路径规划装置包括第一构建模块10、第二构建模块20、初始路径模块30以及路径优化模块40；
其中，所述第一构建模块10，用于构建连续体机器人运动学模型；第二构建模块20，用于根据待检测体的目标器官扫描信息构建目标器官的自然腔道模型，并获取所述自然腔道模型中各自然腔道的中心线信息；初始路径模块30，用于根据所述自然腔道的中心线信息确定从自然腔道入口到目标器官上目标位置的初始连通路径；路径优化模块40，用于基于改进快速探索随机树算法对所述初始连通路径进行符合所述连续体机器人运动学模型的处理，获得规划路径，以控制所述连续体机器人根据所述规划路径到达目标器官上目标位置进行活检。
作为本发明的可选实施例，所述初始路径模块30，具体用于：从所述目标器官上目标位置开始，回溯上一级自然腔道的分支节点，直至回溯到所述自然腔道入口；将从自然腔道入口到分支节点，再到目标位置之间的中心线确定为多个分段路径，所述多个分段路径构成所述初始连通路径。
作为本发明的可选实施例，所述路径优化模块40，具体用于：将自然腔道入口作为起始点添加到点队列中；将每个分段路径的终点作为对应分段路径的子目标点，所述目标位置作为最终目标点，并将各子目标点和最终目标点添加到目标队列中；初始化规划路径队列为空队列；从j=1开始，迭代执行如下步骤，直至所述连续体机器人远端到达所述最终目标点，并获取规划路径队列中的点序列作为所述规划路径：基于改进快速探索随机树算法对第j个分段路径进行符合所述连续体机器人运动学模型的处理，并在连续体机器人远端的当前位置到达第j个分段路径的子目标点时，获得第j个分段路径的规划路径，所述第j个分段路径的规划路径包括路径点序列，并将所述路径点序列添加到所述点队列的尾部；将所述点队列中的点序列添加到上一迭代轮次获得的规划路径队列的尾部；清空点队列，并将连续体机器人远端的当前位置作为起始点添加到点队列中，并对j进行加一处理。
作为本发明的可选实施例，所述路径优化模块40，还用于：若所述连续体机器人远端的当前位置与最终目标点的距离小于第一预设阈值，则确定所述连续体机器人到达最终目标点。
作为本发明的可选实施例，所述路径优化模块40，具体用于：确定第j个分段路径的采样区域，其中，所述采样区域是以第j个分段路径的子目标点为球心的球体，所述球体的半径根据第j个分段路径所处的自然腔道直径确定；迭代执行以下步骤，直至连续体机器人的当前位置处于第j个分段路径的子目标点：在所述采样区域内随机选取采样点，并将所述采样点的位置信息和第j个分段路径的子目标点的位置信息相加，获得新采样点；控制所述连续体机器人远端向所述新采样点移动预设距离，获得连续体机器人远端的当前位置。
作为本发明的可选实施例，所述路径优化模块40，还用于：若连续体机器人远端的当前位置与第j个分段路径的子目标点的距离小于第二预设阈值，则确定所述连续体机器人的当前位置处于第j个分段路径的子目标点。
作为本发明的可选实施例，所述路径优化模块40，还用于：若所述连续体机器人远端向所述新采样点移动预设距离时，所述连续体机器人远端前表面的预设圆形区域与自然腔道发生碰撞，则放弃新采样点，并重新执行所述在所述采样区域内随机选取采样点的步骤。
本发明实施例提供的连续体机器人路径规划装置，其实现原理和技术效果与上述实施例类似，此处不再赘述。
如图8所示，本发明实施例提供了一种电子设备，包括处理器111、通信接口112、存储器113和通信总线114，其中，处理器111，通信接口112，存储器113通过通信总线114完成相互间的通信，
存储器113，用于存放计算机程序；
在本发明一个实施例中，处理器111，用于执行存储器113上所存放的程序时，实现前述任意一个方法实施例提供的连续体机器人路径规划的步骤。
本发明实施例提供的电子设备，其实现原理和技术效果与上述实施例类似，此处不再赘述。
上述存储器113可以是诸如闪存、EEPROM（电可擦除可编程只读存储器）、EPROM、硬盘或者ROM之类的电子存储器。存储器113具有用于执行上述方法中的任何方法步骤的程序代码的存储空间。例如，用于程序代码的存储空间可以包括分别用于实现上面的方法中的各个步骤的各个程序代码。这些程序代码可以从一个或者多个计算机程序产品中读出或者写入到这一个或者多个计算机程序产品中。这些计算机程序产品包括诸如硬盘，光盘（CD）、存储卡或者软盘之类的程序代码载体。这样的计算机程序产品通常为便携式或者固定存储单元。该存储单元可以具有与上述电子设备中的存储器113类似布置的存储段或者存储空间等。程序代码可以例如以适当形式进行压缩。通常，存储单元包括用于执行根据本发明的实施例的方法步骤的程序，即可以由例如诸如111之类的处理器读取的代码，这些代码当由电子设备运行时，导致该电子设备执行上面所描述的方法中的各个步骤。
本发明的实施例还提供了一种计算机可读存储介质。上述计算机可读存储介质上存储有计算机程序，上述计算机程序被处理器执行时实现如上所述的连续体机器人路径规划的步骤。
该计算机可读存储介质可以是上述实施例中描述的设备/装置中所包含的；也可以是单独存在，而未装配入该设备/装置中。上述计算机可读存储介质承载有一个或者多个程序，当上述一个或者多个程序被执行时，实现根据本发明实施例的方法。
根据本发明的实施例，计算机可读存储介质可以是非易失性的计算机可读存储介质，例如可以包括但不限于：便携式计算机磁盘、硬盘、随机访问存储器（RAM）、只读存储器（ROM）、可擦式可编程只读存储器（EPROM或闪存）、便携式紧凑磁盘只读存储器（CD-ROM）、光存储器件、磁存储器件、或者上述的任意合适的组合。在本发明中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行***、装置或者器件使用或者与其结合使用
需要说明的是，在本文中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
以上所述仅是本发明的具体实施方式，使本领域技术人员能够理解或实现本发明。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所申请的原理和新颖特点相一致的最宽的范围。

Claims (10)

1.一种连续体机器人路径规划方法，其特征在于，包括：
构建连续体机器人运动学模型；
根据待检测体的目标器官扫描信息构建目标器官的自然腔道模型，并获取所述自然腔道模型中各自然腔道的中心线信息；
根据所述自然腔道的中心线信息确定从自然腔道入口到目标器官上目标位置的初始连通路径；
基于改进快速探索随机树算法对所述初始连通路径进行符合所述连续体机器人运动学模型的处理，获得规划路径，以控制所述连续体机器人根据所述规划路径到达目标器官上目标位置进行活检。
2.根据权利要求1所述的方法，其特征在于，所述根据所述自然腔道的中心线信息确定从自然腔道入口到目标器官上目标位置的初始连通路径，包括：
从所述目标器官上目标位置开始，回溯上一级自然腔道的分支节点，直至回溯到所述自然腔道入口；
将从自然腔道入口到分支节点，再到目标位置之间的中心线确定为多个分段路径，所述多个分段路径构成所述初始连通路径。
3.根据权利要求2所述的方法，其特征在于，所述基于改进快速探索随机树算法对所述初始连通路径进行符合所述连续体机器人运动学模型的处理，获得规划路径，包括：
将自然腔道入口作为起始点添加到点队列中；
将每个分段路径的终点作为对应分段路径的子目标点，所述目标位置作为最终目标点，并将各子目标点和最终目标点添加到目标队列中；
初始化规划路径队列为空队列；
从j=1开始，迭代执行如下步骤，直至所述连续体机器人远端到达所述最终目标点，并获取规划路径队列中的点序列作为所述规划路径：
基于改进快速探索随机树算法对第j个分段路径进行符合所述连续体机器人运动学模型的处理，并在连续体机器人远端的当前位置到达第j个分段路径的子目标点时，获得第j个分段路径的规划路径，所述第j个分段路径的规划路径包括路径点序列，并将所述路径点序列添加到所述点队列的尾部；
将所述点队列中的点序列添加到上一迭代轮次获得的规划路径队列的尾部；
清空点队列，并将连续体机器人远端的当前位置作为起始点添加到点队列中，并对j进行加一处理。
4.根据权利要求3所述的方法，其特征在于，所述连续体机器人远端到达所述最终目标点，包括：
若所述连续体机器人远端的当前位置与最终目标点的距离小于第一预设阈值，则确定所述连续体机器人远端到达最终目标点。
5.根据权利要求3或4所述的方法，其特征在于，所述基于改进快速探索随机树算法对第j个分段路径进行符合所述连续体机器人运动学模型的处理，包括：
确定第j个分段路径的采样区域，其中，所述采样区域是以第j个分段路径的子目标点为球心的球体，所述球体的半径根据第j个分段路径所处的自然腔道直径确定；
迭代执行以下步骤，直至连续体机器人远端的当前位置到达第j个分段路径的子目标点：
在所述采样区域内随机选取采样点，并将所述采样点的位置信息和第j个分段路径的子目标点的位置信息相加，获得新采样点；
控制所述连续体机器人远端向所述新采样点移动预设距离，并获得连续体机器人远端的当前位置。
6.根据权利要求5所述的方法，其特征在于，所述连续体机器人的当前位置到达第j个分段路径的子目标点，包括：
若连续体机器人远端的当前位置与第j个分段路径的子目标点的距离小于第二预设阈值，则确定所述连续体机器人的当前位置到达第j个分段路径的子目标点。
7.根据权利要求5所述的方法，其特征在于，所述方法还包括：
若所述连续体机器人远端向所述新采样点移动预设距离时，所述连续体机器人远端前表面的预设圆形区域与自然腔道发生碰撞，则放弃新采样点，并重新执行所述在所述采样区域内随机选取采样点的步骤。
8.一种连续体机器人路径规划装置，其特征在于，包括：
第一构建模块，用于构建连续体机器人运动学模型；
第二构建模块，用于根据待检测体的目标器官扫描信息构建目标器官的自然腔道模型，并获取所述自然腔道模型中各自然腔道的中心线信息；
初始路径模块，用于根据所述自然腔道的中心线信息确定从自然腔道入口到目标器官上目标位置的初始连通路径；
路径优化模块，用于基于改进快速探索随机树算法对所述初始连通路径进行符合所述连续体机器人运动学模型的处理，获得规划路径，以控制所述连续体机器人根据所述规划路径到达目标器官上目标位置进行活检。
9.一种电子设备，其特征在于，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；
存储器，用于存放计算机程序；
处理器，用于执行存储器上所存放的程序时，实现权利要求1-7任一项所述的连续体机器人路径规划方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-7任一项所述的连续体机器人路径规划方法的步骤。
CN202210969733.0A 2022-08-12 2022-08-12 连续体机器人路径规划方法、装置、电子设备及存储介质 Pending CN115031739A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210969733.0A CN115031739A (zh) 2022-08-12 2022-08-12 连续体机器人路径规划方法、装置、电子设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210969733.0A CN115031739A (zh) 2022-08-12 2022-08-12 连续体机器人路径规划方法、装置、电子设备及存储介质

Publications (1)

Publication Number Publication Date
CN115031739A true CN115031739A (zh) 2022-09-09

Family

ID=83130466

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210969733.0A Pending CN115031739A (zh) 2022-08-12 2022-08-12 连续体机器人路径规划方法、装置、电子设备及存储介质

Country Status (1)

Country Link
CN (1) CN115031739A (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080183073A1 (en) * 2007-01-31 2008-07-31 The Penn State Research Foundation Methods and apparatus for 3d route planning through hollow organs
CN111650941A (zh) * 2020-06-10 2020-09-11 湖南爱米家智能科技有限公司 基于RRT-Connect改进的医用移动机器人路径规划方法、装置、介质及设备
CN114066906A (zh) * 2020-07-31 2022-02-18 上海微创卜算子医疗科技有限公司 导航路径规划方法、***和可读存储介质
CN114081625A (zh) * 2020-07-31 2022-02-25 上海微创卜算子医疗科技有限公司 导航路径规划方法、***和可读存储介质
CN114700937A (zh) * 2022-01-13 2022-07-05 深圳市越疆科技有限公司 机械臂及其运动路径规划方法、控制***、介质及机器人

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080183073A1 (en) * 2007-01-31 2008-07-31 The Penn State Research Foundation Methods and apparatus for 3d route planning through hollow organs
CN111650941A (zh) * 2020-06-10 2020-09-11 湖南爱米家智能科技有限公司 基于RRT-Connect改进的医用移动机器人路径规划方法、装置、介质及设备
CN114066906A (zh) * 2020-07-31 2022-02-18 上海微创卜算子医疗科技有限公司 导航路径规划方法、***和可读存储介质
CN114081625A (zh) * 2020-07-31 2022-02-25 上海微创卜算子医疗科技有限公司 导航路径规划方法、***和可读存储介质
CN114700937A (zh) * 2022-01-13 2022-07-05 深圳市越疆科技有限公司 机械臂及其运动路径规划方法、控制***、介质及机器人

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
郭超 等，: ""支气管介入诊断机器人的分段知情采样规划算法与动力学仿真"", 《中国胸心血管外科临床杂志》 *

Similar Documents

Publication Publication Date Title
JP7293265B2 (ja) 管状網のナビゲーション
US20220125530A1 (en) Feedback continuous positioning control of end-effectors
JP5442993B2 (ja) ３ｄ器具経路計画立案、シミュレーションおよび制御システム
CN110167477A (zh) 图像引导手术的配准***和方法
KR20210018858A (ko) 관상 네트워크의 경로-기반 내비게이션
Lyons et al. Planning active cannula configurations through tubular anatomy
Sganga et al. Offsetnet: Deep learning for localization in the lung using rendered images
CN103957834A (zh) 用于半自动路径规划的自动深度滚动和方向调节
CN115348847A (zh) 机器人支气管镜导航的***和方法
Li et al. Rl-tee: Autonomous probe guidance for transesophageal echocardiography based on attention-augmented deep reinforcement learning
Al-Zogbi et al. Autonomous robotic point-of-care ultrasound imaging for monitoring of COVID-19–induced pulmonary diseases
JP2023552577A (ja) ナビゲーション気管支鏡検査のための動的変形追跡
Pore et al. Autonomous navigation for robot-assisted intraluminal and endovascular procedures: A systematic review
CN113876420A (zh) 用于规划手术路径的路径规划方法、***、设备和介质
CN115031739A (zh) 连续体机器人路径规划方法、装置、电子设备及存储介质
CN115553925B (zh) 内窥镜操控模型训练方法及装置、设备、存储介质
JP2023031301A (ja) 心房の解剖学的に正確な再構成
CN117941008A (zh) 利用机器学习进行体内导航的***和方法
CN116157088A (zh) 用于规划和执行活检程序的***及相关方法
Kukuk A model-based approach to intraoperative guidance of flexible endoscopy
JP7515495B2 (ja) 機械学習モデルのための訓練データ収集
CN117745772A (zh) 肺部气管的配准方法、***、计算设备及计算机存储介质
CN117958968A (zh) 一种呼吸介入机器人的多源感知和运动控制方法
Bian et al. Design and Nonlinear Error Compensation of a Multi-segment Soft Continuum Robot for Pulmonary Intervention
CN116993981A (zh) 目标对象分割方法、装置、电子设备及存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20220909