主从式眼科手术机器人及其RCM机构运动控制方法与流程
本发明涉及医疗器械，尤其涉及一种主从式眼科手术机器人及其rcm机构运动控制方法。
背景技术：
1、在执行眼底手术的过程中，需要将手术器械从巩膜套管中深入眼内，该种操作模式要求眼科手术机器人末端执行rcm运动（rcm为remote motion center的缩写，即远程运动中心）。rcm运动有4个自由度，分别为偏摆、俯仰、自转、进给，如图1所示。现有主从式眼科手术机器人大多采用被动式rcm机构，即每个电机对应控制一个rcm自由度。
2、现有主从式眼科手术机器人在姿态映射时通常采用绝对式映射。该种映射方式是将主手的旋转矩阵分解为绕x、y、z三个轴方向的转角rx、ry、rz表示，随后设置从手的末端跟随至该位置。但该方法在应用到高精度的rcm运动中会存在一下问题：由于高精度rcm机构中通常采用较大的映射比，因此其最高速度和加速度有限，而人手姿态转角的速度和加速度则远大于高精度rcm机构能达到的最大速度和加速度。直接设置绝对式映射跟随时，当人手运动速度和加速度超出高精度rcm机构的承受能力时会造成电机超负载而报错的故障。
3、在使用上述主从式眼科手术机器人进行手术的过程中，在将rcm点对准巩膜套管后还需要将器械轴线尽量对齐套管轴线以方便进针，由于高精度被动式rcm机构具有较高的减速比，直接使用绝对式映射可能会出现电机速度和加速度跟不上主手轨迹造成故障的问题。而采用单轴对齐的操作较为繁琐，存在效率较低的问题。
4、为解决上述问题，需要设计合适的规划算法限制rcm机构跟随轨迹的最大速度和加速度。目前已有的规划算法并不能完全解决这一问题，比如采用规划位置为当前位置和目标位置中间的位置。该方法仅是缓解了这一问题，当输入的速度和加速度过快时仍有可能报错；而当输入速度和加速度较慢时又无法发挥出高精度rcm机构的全部性能。
技术实现思路
1、鉴于上述的分析，本发明实施例旨在提供一种主从式眼科手术机器人及其被动式rcm机构运动控制方法，用以解决现有技术人手与rcm机构的运动速度和加速度不匹配的问题。
2、一方面，本发明实施例提供了一种主从式眼科手术机器人的被动式rcm机构运动控制方法，包括如下步骤：
3、s1.设置rcm机构的器械末端轨迹划分规则，将划分后的每一轨迹段作为一个单独的控制单位；
4、s2.在rcm机构的器械末端轨迹开始后，在每一控制单位的起始时刻，获取器械末端姿态，得出器械末端在rcm点坐标系下的初始姿态；
5、s3.获取跟随目标在rcm点坐标系下的姿态；
6、s4.根据设定的跟随控制规则，将跟随目标在rcm点坐标系下的姿态分解为器械末端在rcm点坐标系下的目标姿态；
7、s5.对器械末端的姿态进行非线性轨迹规划，使得该控制单位内器械末端从初始姿态运动至目标姿态，且运动过程中器械末端加速度时刻小于等于设置的最大加速度、器械末端速度时刻小于等于设置的最大速度。
8、上述技术方案的有益效果如下：公开了一种基于动态规划轴线非线性跟随算法的器械末端跟随手动轨迹的跟随控制方法，相比现有技术的绝对式映射，具有更好的操作直观性，且能显著地提高对齐操作的效率。将姿态运动分解到rcm点坐标系下的姿态后使用非线性滤波规划方法进行规划。该方法可以设置人为规定的最大速度和最大加速度，当轨迹段开始后rcm机构的器械轴线即可自动以设定的最大速度和最大加速度跟随到主手轴线当前位置。
9、基于上述方法的进一步改进，步骤s2中的获取器械末端姿态，得出器械末端在rcm点坐标系下的初始姿态包括如下子步骤：
10、s21.获取在rcm点坐标系下器械末端的偏摆角、俯仰角、自转角；
11、s22.根据上述偏摆角、俯仰角、自转角，结合被动式rcm机构的结构特征，建立器械末端在rcm点坐标系下的旋转矩阵，作为器械末端在rcm点坐标系下的初始姿态，
12、。
13、进一步，步骤s3包括如下子步骤：
14、s31.建立跟随目标的坐标系，记为；
15、s32.通过下面公式建立跟随目标在rcm点坐标系下的旋转矩阵，作为跟随目标在rcm点坐标系下的姿态，
16、
17、式中，为rcm点坐标系变换至主手基座标系的旋转矩阵；为主手末端在主手基坐标系下表示的姿态；为由主手末端变换到跟随目标的旋转矩阵。
18、进一步，步骤s31中，将跟随目标的坐标系规定为主手坐标系h绕其x轴方向旋转180°后形成的坐标系；并且，步骤s32中，所述通过下面公式得出：
19、
20、式中，为主手坐标系绕其x轴逆时针旋转180°的变化矩阵。
21、进一步，步骤4包括如下子步骤：
22、s41.根据设定的跟随控制规则，将跟随目标在rcm点坐标系下的姿态分解为器械末端在rcm点坐标系下的姿态；
23、s42.通过下面公式得到跟随目标的偏摆角、俯仰角，
24、
25、s43.将上述偏摆角、俯仰角作为器械末端在rcm点坐标系下的目标姿态。
26、进一步，步骤s5包括如下子步骤：
27、s51.通过非线性轨迹规划算法对器械末端的偏摆角、俯仰角分别进行非线性轨迹规划，使得在设定时段内器械末端从所述初始姿态运动到所述目标姿态；
28、s52.验证上述轨迹规划对应的控制单位内每一时刻是否均满足器械末端加速度时刻小于等于设置的最大加速度、器械末端速度小于等于设置的最大速度，若是，执行下一步，否则，返回至步骤s51重新进行非线性轨迹规划；
29、s53.控制rcm机构执行所述轨迹规划。
30、进一步，步骤s53进一步包括：
31、s531.获取上一时刻的控制信号，根据上一时刻的控制信号得出当前时刻器械末端的规划位置和规划速度，
32、
33、式中，为上一时刻器械末端的规划加速度，为采样时间间隔，为上一时刻器械末端的规划位置，为上一时刻器械末端的规划速度；
34、s532.根据上述规划位置和规划速度，通过下面公式获取当前时刻器械末端的位置归一化误差、速度归一化误差，
35、
36、
37、式中，为当前时刻器械末端的目标位置，为当前时刻器械末端的目标速度，为归一化系数，为设置的最大加速度；
38、s533.根据上述位置归一化误差、速度归一化误差，通过下面公式得到下一时刻的控制信号，
39、
40、式中，、、、为中间变量，为设置的最大速度，为取整函数，为符号函数，为饱和函数；
41、s534.重复上述步骤s531~s533，直到得到控制单位内每一时刻的控制信号；
42、s535.控制被动式rcm机构执行上述控制单位内每一时刻的控制信号。
43、进一步，所述轨迹规划包括从手的目标位置为器械轴线和主手轴线在空间中对齐的位置。
44、进一步，所述器械末端轨迹划分规则包括等时间划分规则、等距离划分规则、等幅度划分规则中的至少一种。
45、另一方面，本发明实施例提供了一种主从式眼科手术机器人，包括6轴机械臂和6轴机械臂末端附带的被动式rcm机构；并且，
46、所述主从式眼科手术机器人执行上述被动式rcm机构运动控制方法。
47、提供
技术实现要素：
部分是为了以简化的形式来介绍对概念的选择，它们在下文的具体实施方式中将被进一步描述。发明内容部分无意标识本发明的重要特征或必要特征，也无意限制本发明的范围。
技术特征：
1.一种主从式眼科手术机器人的被动式rcm机构运动控制方法，其特征在于，包括如下步骤：
2.根据权利要求1所述的主从式眼科手术机器人的被动式rcm机构运动控制方法，其特征在于，步骤s2中的获取器械末端姿态，得出器械末端在rcm点坐标系下的初始姿态包括如下子步骤：
3.根据权利要求2所述的主从式眼科手术机器人的被动式rcm机构运动控制方法，其特征在于，步骤s3包括如下子步骤：
4.根据权利要求3所述的主从式眼科手术机器人的被动式rcm机构运动控制方法，其特征在于，步骤s31中，将跟随目标的坐标系规定为主手坐标系h绕其x轴方向旋转180°后形成的坐标系；并且，步骤s32中，所述通过下面公式得出：
5.根据权利要求4所述的主从式眼科手术机器人的被动式rcm机构运动控制方法，其特征在于，步骤4包括如下子步骤：
6.根据权利要求5所述的主从式眼科手术机器人的被动式rcm机构运动控制方法，其特征在于，步骤s5包括如下子步骤：
7.根据权利要求5所述的主从式眼科手术机器人的被动式rcm机构运动控制方法，其特征在于，步骤s53进一步包括：
8.根据权利要求1-7任一项所述的主从式眼科手术机器人的被动式rcm机构运动控制方法，其特征在于，所述轨迹规划包括从手的目标位置为器械轴线和主手轴线在空间中对齐的位置。
9.根据权利要求8所述的主从式眼科手术机器人的被动式rcm机构运动控制方法，其特征在于，所述器械末端轨迹划分规则包括等时间划分规则、等距离划分规则、等幅度划分规则中的至少一种。
10.一种主从式眼科手术机器人，其特征在于，包括6轴机械臂和6轴机械臂末端附带的被动式rcm机构；并且，
技术总结
本发明提供了一种主从式眼科手术机器人及其被动式RCM机构运动控制方法，属于医疗器械技术领域，解决了现有技术人手与RCM机构的运动速度和加速度不匹配的问题。该方法包括：设置器械末端轨迹划分规则，将划分后的每一轨迹段作为一个控制单位；在轨迹开始后，在每一控制单位的起始时刻，获取器械末端在RCM点坐标系下的初始姿态，并通过跟随目标在RCM点坐标系下的姿态获取器械末端在RCM点坐标系下的目标姿态；对器械末端的姿态进行非线性轨迹规划，使得该控制单位内器械末端从初始姿态运动至目标姿态，器械末端加速度、速度时刻小于等于设置的最大加速度、速度。该方法在轨迹开始后可实现手术器械与跟随目标在空间中的自动对齐。
技术研发人员：林闯,牛孜睿,欧阳铜,张春,马维敏,郭莎莎
受保护的技术使用者：北京衔微医疗科技有限公司
技术研发日：
技术公布日：2024/9/23