基于双刚体相对位姿的口腔种植机器人微动补偿控制系统

摘要: 本发明涉及机器人控制领域,具体涉及一种基于双刚体相对位姿的口腔种植机器人微动补偿控制系统。背景技术:1、随着静态导板、动态导航系统和口腔种植机器人的发展,术前规...
基于双刚体相对位姿的口腔种植机器人微动补偿控制系统

本发明涉及机器人控制领域,具体涉及一种基于双刚体相对位姿的口腔种植机器人微动补偿控制系统。

背景技术:

1、随着静态导板、动态导航系统和口腔种植机器人的发展,术前规划坐标系、患者坐标系和机器人坐标系之间的空间配准已经能够在静态状态下达到较高精度。然而,实际术中环境并非完全静止。患者即使在头枕、支撑装置或口内固位装置辅助固定下,仍会因呼吸、吞咽、肌颤、紧张和下颌微动等因素产生小幅度但不可忽略的头颌部运动。

2、现有口腔种植机器人导航或定位方案通常能够获取患者参考标记或种植工具的空间位姿,并据此进行路径显示、位置提示或机器人定位控制。然而,在患者术中发生头颌部微动时,仅根据患者参考点的平移量进行补偿,难以准确反映末端执行器工作点处的实际位置变化。尤其当患者参考点与末端执行器工作点之间存在空间偏置矢量时,患者参考刚体的微小姿态变化会使末端执行器工作点产生附加线位移,该附加线位移并不能由患者参考点平移量直接表示。

3、此外,现有方案即使能够分别跟踪患者参考标记或机器人末端标记,也往往缺乏将患者参考刚体位姿和机器人末端刚体位姿严格对齐至同一机器人控制周期的处理机制。若患者参考刚体位姿与机器人末端刚体位姿采样时刻不一致,则计算得到的相对位姿会混入时间同步误差,进而导致补偿控制滞后、过补偿或振荡。对于外部定位频率、机器人控制周期和通信延迟不完全一致的系统,仅依靠普通时间戳记录难以保证相对位姿具有同周期物理意义。

4、因此,需要提出一种面向口腔种植机器人术中微动环境的控制方法,使控制器能够在同一外部定位坐标系和同一机器人控制周期下同步获得患者参考刚体和机器人末端刚体位姿,计算机器人末端刚体相对于患者参考刚体的相对位姿矩阵,并进一步利用患者参考点至末端执行器工作点的空间偏置矢量显式计算患者转动运动在末端执行器工作点处产生的转动位移补偿量,从而使末端执行器在动态环境中保持相对于患者参考坐标系的预定位置和姿态。

5、因此,需要一种能够将患者参考刚体位姿和机器人末端刚体位姿严格对齐至同一机器人控制周期的基于双刚体相对位姿的口腔种植机器人微动补偿控制系统。

技术实现思路

1、本发明的主要目的在于提供一种基于双刚体相对位姿的口腔种植机器人微动补偿控制系统,以解决患者头颌部微动条件下患者参考刚体与机器人末端刚体采样不同步、患者转动引起的tcp附加位移补偿不充分、定位噪声和短时遮挡影响控制连续性以及异常工况下控制输出缺少分级约束的问题。

2、为实现上述目的,本发明提供一种基于双刚体相对位姿的口腔种植机器人微动补偿控制系统,包括:空间映射模块、双刚体同步模块、相对位姿模块、空间偏置矢量补偿模块、自适应滤波模块、前馈预测模块、反馈控制模块和安全控制模块。

3、空间映射模块,用于获取机器人基坐标系、外部定位坐标系、患者参考坐标系、机器人末端刚体坐标系与末端执行器工作点坐标系之间的空间变换关系;双刚体同步模块,用于在同一外部定位坐标系和同一时间基准下同步获取患者参考刚体位姿和机器人末端刚体位姿;相对位姿计算模块,用于计算机器人末端刚体相对于患者参考刚体的相对位姿矩阵;空间偏置矢量补偿模块,用于计算患者平移、旋转力臂位移以及末端位置和姿态补偿量,并将补偿量转换至机器人基坐标系;自适应滤波模块,用于对患者参考刚体位姿进行滤波,并在短时遮挡时进行有限时域预测;前馈预测模块,用于生成延迟感知的短时预测状态;反馈控制模块,用于生成机器人末端执行器补偿控制量;安全控制模块,用于执行正常补偿、降级补偿以及保持或停止控制。

4、进一步地,在空间映射模块获取机器人基坐标系、外部定位坐标系、患者参考坐标系、机器人末端刚体坐标系以及末端执行器工作点坐标系之间的空间变换关系,并获取末端执行器工作点相对于患者参考坐标系的目标相对位姿,其中空间变换关系用于将机器人源坐标系中的坐标映射至目标坐标系,并按如下齐次变换形式表示:

5、 (1);

6、其中,表示目标坐标系,表示机器人源坐标系,为第个控制周期下坐标系相对于坐标系的旋转矩阵,为平移向量。

7、空间变换关系包括:机器人基坐标系与外部定位坐标系之间的手眼变换关系、机器人末端法兰坐标系与末端执行器工作点之间的工具变换关系、患者参考坐标系与术前规划坐标系之间的配准变换关系、机器人末端刚体坐标系与末端执行器工作点之间的刚体标定关系。

8、末端执行器工作点在机器人基坐标系下的补偿指令位置满足:

9、 (2);

10、其中,为第个控制周期机器人基坐标系下末端执行器工作点的补偿指令位置 ,为末端执行器工作点在机器人基坐标系下的目标位置,为第个控制周期将患者参考坐标系中的向量旋转表示到机器人基坐标系的旋转矩阵,为患者参考坐标系下由患者平移与旋转力臂效应合成的末端位置补偿量。

11、进一步地,在双刚体同步模块中,在同一外部定位坐标系下,同步获取患者参考刚体位姿和机器人末端刚体位姿,并根据患者参考刚体采样时间戳、机器人末端刚体采样时间戳和机器人控制周期时间戳,将患者参考刚体位姿和机器人末端刚体位姿对齐至同一机器人控制周期;具体包括:

12、将患者参考刚体位姿数据帧和机器人末端刚体位姿数据帧写入环形缓冲区;

13、依据采样时间戳、控制周期时间戳和刚体识别编号选取最新有效数据帧;

14、按照同步误差计算式判断所选患者参考刚体位姿数据帧与机器人末端刚体位姿数据帧是否构成同一控制周期下的有效位姿对:

15、 (3);

16、其中,为患者参考刚体第个有效位姿数据帧的采样时间戳,为机器人末端刚体第个有效位姿数据帧的采样时间戳,为两帧采样时刻的绝对时间差;当时直接形成有效位姿对,当时对时间较早的一路进行插值或受限外推,当时丢弃该位姿对或触发安全控制;为直接配对的同步阈值,为允许插值或短时外推的最大补偿阈值,且。

17、插值包括:平移线性插值和姿态球面线性插值,插值系数和插值位姿按下式计算:

18、 (4);

19、其中,为查询时刻在相邻两有效采样时刻之间的归一化插值系数,为当前机器人控制周期对应的目标查询时刻,和为同一路刚体相邻两帧有效采样时刻,为查询时刻的插值位置向量,和分别为相邻两帧位置向量,为查询时刻的插值单位四元数,和分别为相邻两帧单位四元数,为单位四元数球面线性插值运算。

20、进一步地,在相对位姿模块,根据对齐后的患者参考刚体位姿和机器人末端刚体位姿,计算机器人末端刚体相对于患者参考刚体的相对位姿矩阵:

21、 (5);

22、其中,为第个控制周期下患者参考刚体相对于外部定位坐标系的齐次变换矩阵,为第个控制周期下机器人末端刚体相对于外部定位坐标系的齐次变换矩阵,表示将机器人末端刚体坐标系映射至患者参考坐标系的相对位姿矩阵。

23、进一步地,在空间偏置矢量补偿模块,计算患者平移、旋转力臂位移以及末端姿态补偿量,具体包括:

24、根据患者参考刚体当前位姿与初始位姿或目标位姿之间的差异,获得患者参考刚体的平移变化量和姿态变化矩阵:

25、 (6);

26、其中,为第个控制周期患者参考刚体相对于基准状态的平移变化向量,为患者参考刚体相对于基准状态的姿态变化矩阵,和分别为当前状态与基准状态下患者参考点的位置向量,和分别为当前状态与基准状态下患者参考刚体的旋转矩阵,为转置。

27、获取患者参考点至末端执行器工作点的空间偏置矢量,并根据姿态变化矩阵和空间偏置矢量计算患者转动运动在末端执行器工作点处引起的转动位移补偿量:

28、 (7);

29、其中,为三阶单位矩阵,为患者参考刚体姿态变化通过空间偏置矢量传播至末端执行器工作点产生的附加线位移补偿量。

30、将患者平移变化量与转动位移补偿量合成为末端位置补偿量,并根据患者参考刚体姿态变化矩阵计算末端姿态补偿量:

31、 (8);

32、其中,为由患者平移变化量与旋转力臂附加位移合成的末端位置补偿量,为与患者姿态变化相对应的末端姿态补偿矩阵。

33、空间偏置矢量通过工具中心点标定、机器人末端刚体标定或患者参考坐标系配准获得,并按照患者参考坐标系、外部定位坐标系和机器人基坐标系之间的空间变换关系,转换为机器人基坐标系下的补偿指令,其中位移和姿态增量的坐标变换满足:

34、(9);

35、其中,为第个控制周期将患者参考坐标系中的向量旋转表示到机器人基坐标系的旋转矩阵,和分别为转换至机器人基坐标系后的末端位置补偿量和末端姿态增量。

36、进一步地,在自适应滤波模块,对患者参考刚体位姿进行滤波;具体包括:

37、基于自适应卡尔曼滤波,以患者参考刚体的位置、局部旋转向量、线速度和角速度构成状态向量;姿态在传播和输出时转换为单位四元数表示,并按照预测、更新和测量噪声自适应调整过程进行状态估计:

38、 (10);

39、其中,为患者参考刚体位置向量,为局部姿态参数或等效旋转向量,为线速度向量,为角速度向量。

40、自适应卡尔曼滤波的状态预测和测量更新满足:

41、 (11);

42、其中,为第时刻基于前一时刻信息得到的先验状态估计,为状态转移矩阵,为先验估计误差协方差矩阵,为过程噪声协方差矩阵,为归一化新息,为外部定位系统在第时刻的测量向量,为测量矩阵,为kalman增益,为当前测量噪声协方差矩阵,为融合当前测量后的后验状态估计。

43、测量噪声协方差矩阵根据新息向量、定位标记点可见数量和刚体重投影误差动态调整:

44、 (12);

45、其中,为基础测量噪声协方差矩阵,自适应系数按下式计算:

46、 (13);

47、其中,表示限幅函数;为第时刻实际可见定位标记点数量,为刚体完整可见或标定状态下的基准标记点数量,为第时刻的刚体重投影误差,、、为分别对应新息、可见标记数量和重投影误差的非负权重系数,为新息幅值的基准归一化尺度,为重投影误差的基准归一化尺度,和分别为自适应测量噪声放大系数的下限和上限,表示取正部,即。

48、若患者参考刚体定位标记发生遮挡,且遮挡持续时间小于第一遮挡阈值,则依据遮挡前的滤波状态通过运动模型预测患者参考刚体位姿。其中运动模型预测患者参考刚体位姿满足:

49、 (14);

50、其中,和分别为从第个控制周期向前预测个控制周期后的位置与姿态,、、和分别为遮挡发生前最后一次可信测量更新后的后验位置、姿态、线速度和角速度,为预测步数,为控制周期,表示单位四元数乘法;为旋转向量至单位四元数的指数映射。

51、若遮挡持续时间大于或等于第一遮挡阈值,则停止使用预测位姿进行正常补偿,并触发机器人保持、减速、暂停或报警控制。

52、进一步地,在前馈预测模块,基于患者参考刚体的当前位置、线速度、姿态和角速度建立短时前馈预测量,将短时前馈预测量叠加至末端位置补偿量和末端姿态补偿量,得到预测补偿目标位姿。

53、短时前馈预测量根据系统延迟或预设预测时域确定,系统延迟根据外部定位采样时间戳、数据接收时间戳、控制周期时间戳和机器人执行响应时间估计:

54、 (15);

55、其中,为第个控制周期用于短时前馈预测的有效时延,为外部定位位姿的采样时间戳,为控制计算单元接收到该位姿数据的时间戳,为当前机器人控制周期开始计算补偿指令的时间戳,为状态估计、坐标变换及控制计算所需的等效计算时延,为机器人接收补偿指令至末端产生有效响应的等效执行时延。

56、并根据系统延迟预测患者参考刚体的平移和姿态:

57、 (16);

58、其中,、、和分别为患者参考刚体的滤波位置、滤波姿态单位四元数、线速度和角速度;和分别为预测到时刻的位置与姿态,表示单位四元数乘法,表示将旋转向量映射为单位四元数的指数映射。

59、进一步地,在反馈控制模块,根据预测补偿目标位姿与机器人末端刚体实际相对位姿之间的位置误差、姿态误差及误差变化率,在线调整反馈控制参数,并生成机器人末端执行器的补偿控制量;具体包括:

60、计算预测补偿目标位姿与实际相对位姿之间的位置误差和姿态误差:

61、 (17);

62、其中,为预测补偿目标与实际相对位姿之间的位置误差向量,为对应的最小姿态误差向量,和分别为患者参考坐标系下的目标相对位置和实测相对位置,和分别为目标相对姿态和实测相对姿态;表示上的矩阵对数映射,右上标为vee算子。

63、并构造综合误差和综合误差变化率:

64、 (18);

65、其中,和分别为位置误差与姿态误差的非负权重,和分别为位置与姿态误差的归一化尺度,为机器人上层补偿控制周期,为无量纲综合误差,为综合误差关于时间的离散变化率。

66、反馈控制参数由联合高斯隶属函数模糊pid控制器在线调整;联合高斯隶属函数模糊pid控制器以综合误差和综合误差变化率为输入,并通过高斯隶属函数计算负大、负中、负小、零、正小、正中、正大七个模糊语言集合的隶属度:

67、 (19);

68、其中,为综合误差对第个模糊语言集合的隶属度,为自然指数函数,为第个模糊语言集合的中心参数,为对应的宽度参数;依次表示负大、负中、负小、零、正小、正中和正大。

69、进一步地,联合高斯隶属函数模糊pid控制器包括维mamdani型模糊规则库,模糊规则库输出比例增益修正量、积分增益修正量和微分增益修正量;对于任一增益,,增益的修正量按加权重心法计算:

70、 (20);

71、其中,和分别为综合误差与综合误差变化率对应的模糊语言集合索引,和分别为对应语言集合的隶属度,为第条模糊规则针对第类pid增益修正量预设的规则输出值,为防止分母为零而设置的极小正则化常数;其中分别对应比例、积分和微分增益通道。

72、在线更新后的pid参数满足:

73、 (21);

74、其中,为第类增益的名义值或初始值,为第个控制周期由模糊规则在线计算得到的增益修正量,为更新后的实际增益。

75、机器人末端执行器补偿控制量按前馈预测项和反馈调节项叠加生成:

76、 (22);

77、其中,为第个控制周期预测到有效时延后的前馈补偿分量,为对应的前馈权重,、和为在线更新后的比例、积分和微分增益,为第个历史控制周期的综合误差,为控制周期。

78、进一步地,在安全控制模块,根据机器人末端执行器补偿控制量控制机器人末端执行器运动,使末端执行器工作点在患者头颌部微动条件下保持相对于患者参考坐标系的预定位置和姿态;具体包括:

79、构造归一化安全指数:

80、 (23);

81、其中,为患者参考刚体相对于基准状态的平移变化向量,为允许的患者平移工程阈值;为患者姿态变化矩阵对应的旋转向量,为允许的患者转角工程阈值;和分别为末端相对位置误差和姿态误差,和分别为对应的工程上限;为补偿控制量,为单周期控制量或控制接口允许的幅值上限;为当前有效时延,为允许的最大时延;为当前连续遮挡持续时间,为允许使用预测状态维持补偿的最大遮挡时长。

82、并根据归一化安全指数得到安全门控系数:

83、 (24);

84、其中,为由降级补偿转入保持或停止状态的严重度阈值,为当前安全门控系数,为降级补偿状态下的衰减系数,为经过门控和限幅后的实际输出控制量,为饱和限幅函数。

85、当时,机器人执行正常补偿;当时,机器人执行降级补偿、限速或阻尼增强;当时,机器人进入保持、暂停或报警状态。

86、本发明具有如下有益效果:

87、1.本发明通过同一外部定位坐标系和同一机器人控制周期下的双刚体同步相对位姿计算,直接获得机器人末端相对于患者参考坐标系的动态偏差,降低采样不同步引入的相对位姿误差。

88、2.本发明通过相对位姿矩阵计算机器人末端刚体相对于患者参考刚体的实际位姿,使闭环反馈量直接反映末端执行器相对于患者参考坐标系的实际偏移。

89、3.本发明通过空间偏置矢量转动位移补偿公式显式计算患者转动运动在末端执行器工作点处产生的附加线位移,避免仅根据患者参考点平移量进行补偿而遗漏末端工作点附加位移。

90、4.本发明通过患者平移变化量与转动位移补偿量合成末端位置补偿量,使末端位置补偿目标更加符合患者头颌部刚体运动规律。

91、5.本发明通过短时前馈预测与相对位姿反馈闭环控制相结合,可同时补偿系统延迟和模型残余误差,提高动态环境下末端位姿保持能力。

92、6.本发明通过自适应卡尔曼滤波和遮挡预测处理,降低定位噪声对控制输入的影响,并在短时遮挡与遮挡超时之间形成分级处理机制。

93、7.本发明通过患者运动阈值、定位标记遮挡阈值、相对位姿误差阈值、控制量阈值和数据链路延迟阈值的多重安全切换,提高机器人在异常工况下的安全性。