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

(19)国家知识产权局(12)发明专利申请(10)申请公布号CN115031739A(43)申请公布日2022.09.09(21)申请号202210969733.0(22)申请日2022.08.12(71)申请人中国科学院自动化研究所地址100190北京市海淀区中关村东路95号(72)发明人边桂彬李桢汤向荣(74)专利代理机构北京华夏泰和知识产权代理有限公司11662专利代理师邓菊香(51)Int.Cl.G01C21/20(2006.01)A61B10/02(2006.01)权利要求书2页说明书11页附图7页(54)发明名称连续体机器人路径规划方法、装置、电子设备及存储介质(57)摘要本发明实施例涉及一种连续体机器人路径规划方法、装置、电子设备及存储介质，涉及机器人控制领域；通过构建连续体机器人运动学模型；根据待检测体的目标器官扫描信息构建目标器官的自然腔道模型，并获取所述自然腔道模型中各自然腔道的中心线信息；根据所述自然腔道的中心线信息确定从自然腔道入口到目标器官上目标位置的初始连通路径；基于改进快速探索随机树算法对所述初始连通路径进行符合所述连续体机器人运动学模型的处理，获得规划路径，以控制所述连续体机器人根据所述规划路径到达目标器官上目标位置进行活检；即本发明实施例实现了连续体机器人完全自主导航到被检测体目标器官上的可疑病变位置，实现快速准确的活检。CN115031739ACN115031739A权利要求书1/2页1.一种连续体机器人路径规划方法，其特征在于，包括：构建连续体机器人运动学模型；根据待检测体的目标器官扫描信息构建目标器官的自然腔道模型，并获取所述自然腔道模型中各自然腔道的中心线信息；根据所述自然腔道的中心线信息确定从自然腔道入口到目标器官上目标位置的初始连通路径；基于改进快速探索随机树算法对所述初始连通路径进行符合所述连续体机器人运动学模型的处理，获得规划路径，以控制所述连续体机器人根据所述规划路径到达目标器官上目标位置进行活检。2.根据权利要求1所述的方法，其特征在于，所述根据所述自然腔道的中心线信息确定从自然腔道入口到目标器官上目标位置的初始连通路径，包括：从所述目标器官上目标位置开始，回溯上一级自然腔道的分支节点，直至回溯到所述自然腔道入口；将从自然腔道入口到分支节点，再到目标位置之间的中心线确定为多个分段路径，所述多个分段路径构成所述初始连通路径。3.根据权利要求2所述的方法，其特征在于，所述基于改进快速探索随机树算法对所述初始连通路径进行符合所述连续体机器人运动学模型的处理，获得规划路径，包括：将自然腔道入口作为起始点添加到点队列中；将每个分段路径的终点作为对应分段路径的子目标点，所述目标位置作为最终目标点，并将各子目标点和最终目标点添加到目标队列中；初始化规划路径队列为空队列；从j=1开始，迭代执行如下步骤，直至所述连续体机器人远端到达所述最终目标点，并获取规划路径队列中的点序列作为所述规划路径：基于改进快速探索随机树算法对第j个分段路径进行符合所述连续体机器人运动学模型的处理，并在连续体机器人远端的当前位置到达第j个分段路径的子目标点时，获得第j个分段路径的规划路径，所述第j个分段路径的规划路径包括路径点序列，并将所述路径点序列添加到所述点队列的尾部；将所述点队列中的点序列添加到上一迭代轮次获得的规划路径队列的尾部；清空点队列，并将连续体机器人远端的当前位置作为起始点添加到点队列中，并对j进行加一处理。4.根据权利要求3所述的方法，其特征在于，所述连续体机器人远端到达所述最终目标点，包括：若所述连续体机器人远端的当前位置与最终目标点的距离小于第一预设阈值，则确定所述连续体机器人远端到达最终目标点。5.根据权利要求3或4所述的方法，其特征在于，所述基于改进快速探索随机树算法对第j个分段路径进行符合所述连续体机器人运动学模型的处理，包括：确定第j个分段路径的采样区域，其中，所述采样区域是以第j个分段路径的子目标点为球心的球体，所述球体的半径根据第j个分段路径所处的自然腔道直径确定；迭代执行以下步骤，直至连续体机器人远端的当前位置到达第j个分段路径的子目标2CN115031739A权利要求书2/2页点：在所述采样区域内随机选取采样点，并将所述采样点的位置信息和第j个分段路径的子目标点的位置信息相加，获得新采样点；控制所述连续体机器人远端向所述新采样点移动预设距离，并获得连续体机器人远端的当前位置。6.根据权利要求5所述的方法，其特征在于，所述连续体机器人的当前位置到达第j个分段路径的子目标点，包括：若连续体机器人远端的当前位置与第j个分段路径的子目标点的距离小于第二预设阈值，则确定所述连续体机器人的当前位置到达第j个分段路径的子目标点。7.根据权利要求5所述的方法，其特征在于，所述方法还包括：若所述