定位方法、装置、电子设备及计算机可读存储介质制造方法及图纸

本公开提供一种定位方法、装置、电子设备及计算机可读存储介质，涉及手术机器人技术领域，该方法包括：获取当前时刻手术工具的当前图像特征，根据世界坐标系下的手术工具的导航路径以及由导航路径的终点建立的导航路径坐标系计算手术工具到达终点处的期望图像特征，基于当前图像特征计算手术机器人下一时刻的位姿控制量，通过下一时刻的位姿控制量调整手术机器人的位姿，获取手术工具的下一时刻图像特征，当下一时刻图像特征与期望图像特征间的实际图像特征误差小于预设误差阈值时，确定手术工具定位完成，否则，重新计算上述的期望图像特征，直至实际图像特征误差小于预设误差阈值。本公开提高了手术机器人的定位速度。本公开提高了手术机器人的定位速度。本公开提高了手术机器人的定位速度。

【技术实现步骤摘要】
定位方法、装置、电子设备及计算机可读存储介质


[0001]本公开涉及手术机器人
，尤其涉及一种定位方法、装置、电子设备及计算机可读存储介质。

技术介绍

[0002]手术机器人学是一门集临床医学、生物医学工程、计算机、机器人学等学科为一体的新兴交叉学科。现有的手术机器人实现定位所设计和使用的图像特征主要包括手术工具上的标记球在光学定位系统中的三维坐标，没有考虑手术工具的方向，从而导致计算的图像特征误差收敛速度慢，降低了手术机器人的定位速度。
[0003]需要说明的是，在上述
技术介绍
部分公开的信息仅用于加强对本公开的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。

技术实现思路

[0004]本公开提供了一种定位方法、装置、电子设备及计算机可读存储介质，至少能够提高手术机器人的定位速度。
[0005]本公开的其他特性和优点将通过下面的详细描述变得显然，或部分地通过本公开的实践而习得。
[0006]根据本公开的一个方面，提供一种定位方法，应用于手术机器人系统，上述手术机器人系统包括手术机器人、手术工具和光学定位系统，上述手术工具安装于上述手术机器人，上述定位方法包括：
[0007]获取当前时刻上述手术工具在第一位置下的当前图像特征；
[0008]根据上述手术工具的导航路径、由上述导航路径的终点所建立的第一坐标系，确定上述手术工具到达上述终点处的期望图像特征；其中，上述导航路径在上述光学定位系统所在的第二坐标系下；
[0009]基于上述当前图像特征确定上述手术机器人下一时刻的位姿控制量，并根据上述下一时刻的位姿控制量调整上述手术机器人的位姿，以使上述手术工具运动至第二位置；
[0010]获取上述第二位置下上述手术工具的下一时刻图像特征；
[0011]返回执行上述根据上述手术工具的导航路径、由上述导航路径的终点所建立的第一坐标系，确定上述手术工具到达上述终点处的期望图像特征的步骤，直至上述下一时刻图像特征与上述期望图像特征之间的实际图像特征误差小于预设误差阈值，确定上述手术工具定位完成。
[0012]可选地，上述根据上述手术工具的导航路径、由上述导航路径的终点所建立的第一坐标系，确定上述手术工具到达上述终点处的期望图像特征的步骤包括：获取上述第二坐标系下关于上述终点和上述导航路径的起点各自的位置坐标；根据上述第一坐标系的原点和上述位置坐标，确定上述期望图像特征。
[0013]可选地，上述获取当前时刻上述手术工具在第一位置下的当前图像特征的步骤之
前，上述定位方法还包括：构建上述手术工具的初始图像雅可比矩阵；将上述初始图像雅可比矩阵重排为列向量，得到上述手术机器人系统的初始系统状态向量。
[0014]可选地，上述构建上述手术工具的初始图像雅可比矩阵的步骤包括：控制上述手术机器人到达初始设定位姿参数对应的初始位姿，并获取上述初始位姿下的上述手术工具的初始图像特征；其中，上述初始设定位姿参数包括i个元素，i大于或者等于6；分别为上述初始设定位姿参数中的每个元素增加预设偏移量，得到i个偏移位姿参数；控制上述手术机器人到达第i个偏移位姿参数对应的偏移位姿；基于上述第i个偏移位姿参数对应的偏移位姿，获取上述手术工具的第i个中间图像特征；确定上述第i个中间图像特征与上述初始图像特征之间的第i个特征差值，得到i个特征差值；根据上述i个特征差值和上述预设偏移量构建上述手术工具的初始图像雅可比矩阵。
[0015]可选地，上述基于上述当前图像特征确定上述手术机器人下一时刻的位姿控制量的步骤包括：确定上述当前图像特征与上述期望图像特征之间的差值，得到上述手术工具对应的当前图像特征误差；基于上述当前时刻，确定上述手术机器人的当前位姿以及位姿变化量；获取上述手术工具在上述上一时刻的图像雅可比矩阵；基于上述初始系统状态向量确定上述手术工具在上述当前时刻的图像雅可比矩阵；根据上述位姿变化量、上述上一时刻的图像雅可比矩阵、上述当前时刻的图像雅可比矩阵、上述当前图像特征误差以及上述当前位姿，确定上述下一时刻的位姿控制量。
[0016]可选地，上述基于上述初始系统状态向量确定上述手术工具在上述当前时刻的图像雅可比矩阵的步骤包括：根据上述位姿变化量确定上述手术机器人系统的当前系统观测矩阵；按照行顺序将上述上一时刻的图像雅可比矩阵重排为列向量，得到上述手术机器人系统在上述上一时刻的系统状态向量；获取上述手术机器人系统的系统观测量、过程噪声协方差矩阵、测量噪声协方差矩阵、上述上一时刻的误差协方差矩阵的第一平方根以及上述当前时刻的误差协方差矩阵的第二平方根；基于平方根容积卡尔曼滤波器、上述初始系统状态向量、上述上一时刻的系统状态向量、上述第一平方根、上述当前系统观测矩阵、上述系统观测量、上述过程噪声协方差矩阵、上述测量噪声协方差矩阵以及上述第二平方根，确定上述手术机器人系统在上述当前时刻的系统状态向量；对上述当前时刻的系统状态向量进行重排，得到上述当前时刻的图像雅可比矩阵。
[0017]可选地，获取上述第二位置下上述手术工具的下一时刻图像特征的步骤包括：基于上述手术工具的针尖位置构建第四坐标系；获取上述手术工具的针尖在上述第二坐标系下的针尖坐标；确定上述第四坐标系各个坐标轴上的单位点的单位坐标，并对每个上述单位坐标进行转换，得到各个上述单位点在上述第二坐标系下的转换坐标；根据上述针尖坐标和得到的的上述转换坐标，确定上述下一时刻图像特征。
[0018]可选地，上述基于上述手术工具的针尖位置构建第四坐标系的步骤包括：将上述手术工具的针尖位置作为上述第四坐标系的原点；将上述手术工具的针尖的方向作为上述第四坐标系的X轴；将上述手术工具的平面的法向量作为上述第四坐标系的Z轴；将上述X轴与上述Z轴的叉积作为上述第四坐标系的Y轴。
[0019]根据本公开的另一个方面，提供一种定位装置，配置于手术机器人系统，上述手术机器人系统包括手术机器人、手术工具和光学定位系统，上述手术工具安装于上述手术机器人上；上述定位装置包括：
[0020]第一计算模块，用于获取当前时刻上述手术工具在第一位置下的当前图像特征；
[0021]第二计算模块，用于根据上述手术工具的导航路径、由上述导航路径的终点所建立的第一坐标系，确定上述手术工具到达上述终点处的期望图像特征；其中，上述导航路径在上述光学定位系统所在的第二坐标系下；
[0022]位姿调整模块，用于基于上述当前图像特征确定上述手术机器人下一时刻的位姿控制量，并根据上述下一时刻的位姿控制量调整上述手术机器人的位姿，以使上述手术工具运动至第二位置；
[0023]第三计算模块，用于获取上述第二位置下上述手术工具的下一时刻图像特征；
[0024]误差比较模块，用于返回执行上述根据上述手术工具的导航路径、由上述导航路径的终点所建立的第一坐标系，确定上述手术工具到达上述终点处的期望图像特征的步骤，直至上述下一时刻图像特征与上述期望图像特征之间的实际图像特征误差小于预设误差阈值，确定上本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种定位方法，其特征在于，应用于手术机器人系统，所述手术机器人系统包括手术机器人、手术工具和光学定位系统，所述手术工具安装于所述手术机器人；所述定位方法包括：获取当前时刻所述手术工具在第一位置下的当前图像特征；根据所述手术工具的导航路径、由所述导航路径的终点所建立的第一坐标系，确定所述手术工具到达所述终点处的期望图像特征；其中，所述导航路径在所述光学定位系统所在的第二坐标系下；基于所述当前图像特征确定所述手术机器人下一时刻的位姿控制量，并根据所述下一时刻的位姿控制量调整所述手术机器人的位姿，以使所述手术工具运动至第二位置；获取所述第二位置下所述手术工具的下一时刻图像特征；返回执行所述根据所述手术工具的导航路径、由所述导航路径的终点所建立的第一坐标系，确定所述手术工具到达所述终点处的期望图像特征的步骤，直至所述下一时刻图像特征与所述期望图像特征之间的实际图像特征误差小于预设误差阈值，确定所述手术工具定位完成。2.如权利要求1所述的定位方法，其特征在于，所述根据所述手术工具的导航路径、由所述导航路径的终点所建立的第一坐标系，确定所述手术工具到达所述终点处的期望图像特征的步骤包括：获取所述第二坐标系下关于所述终点和所述导航路径的起点各自的位置坐标；根据所述第一坐标系的原点和所述位置坐标，确定所述期望图像特征。3.如权利要求1所述的定位方法，其特征在于，所述获取当前时刻所述手术工具在第一位置下的当前图像特征的步骤之前，所述定位方法还包括：构建所述手术工具的初始图像雅可比矩阵；将所述初始图像雅可比矩阵重排为列向量，得到所述手术机器人系统的初始系统状态向量。4.如权利要求3所述的定位方法，其特征在于，所述构建所述手术工具的初始图像雅可比矩阵的步骤包括：控制所述手术机器人到达初始设定位姿参数对应的初始位姿，并获取所述初始位姿下的所述手术工具的初始图像特征；其中，所述初始设定位姿参数包括i个元素，i大于或者等于6；分别为所述初始设定位姿参数中的每个元素增加预设偏移量，得到i个偏移位姿参数；控制所述手术机器人到达第i个偏移位姿参数对应的偏移位姿；基于所述第i个偏移位姿参数对应的偏移位姿，获取所述手术工具的第i个中间图像特征；确定所述第i个中间图像特征与所述初始图像特征之间的第i个特征差值，得到i个特征差值；根据所述i个特征差值和所述预设偏移量构建所述手术工具的初始图像雅可比矩阵。5.如权利要求3所述的定位方法，其特征在于，所述基于所述当前图像特征确定所述手术机器人下一时刻的位姿控制量的步骤包括：确定所述当前图像特征与所述期望图像特征之间的差值，得到所述手术工具对应的当

【专利技术属性】
技术研发人员：关沛峰，
申请(专利权)人：广州艾目易科技有限公司，
类型：发明
国别省市：