基于单IMU的手术设备位姿识别方法及系统与流程

本发明涉及手术设备位姿识别的，特别涉及一种基于单imu的手术设备位姿识别方法及系统。

背景技术：

1、在手术中使用的手术设备根据手术部位的位置以及将如何处理该外科手术部位而具有不同的结构。近年来，开发了使用机器人的各种类型的手术装备，但无法实时观测手术操作过程中手术设备的使用时的位姿状态，在处理细微的手术操作时，需要医生仔细操作手术设备操作的角度，这不仅要求医生具备丰富的手术经验，还十分耗费手术过程中医生的精力。

2、在现有技术中，如中国专利公开号为：cn115721417a所述的一种手术机器人末端位姿全视野测量装置及方法，利用连接单元和支撑杆作为多面体标记单元的安装基础，避免出现被末端手术器械的导航识别、定位和手术执行装置等遮挡，确保检测效果，在识别手术设备的位姿时需要对手术设备的多个面进行标记，利用多面体的特性，使得末端手术器械运动至任意位置时至少有一个具有多面体标记单元的表面外露，在识别时具有一定的局限性，同时无法基于可视化的界面进行实时显示，会导致手术设备实时姿态测量不准确，造成一定误差，对手术造成一定的影响。

技术实现思路

1、为了解决以上存在的技术问题，本发明提供一种基于单imu的手术设备位姿识别方法及系统。

2、为实现上述目的，本发明提供如下技术方案：

3、第一方面，本发明提供一种基于单imu的手术设备位姿识别方法，

4、包括以下步骤：

5、s1.将imu测量单元设置在手术设备上，imu测量单元将测量信息传输至上位机；

6、s2.在姿态检测开始时，上位机将手术设备处于初始状态，即对位置进行初始化上传至可视化界面；

7、s3.基于四元数和卡尔曼滤波对手术设备姿态解算，首先根据imu测量单元的数据求得初始四元数以及四元数形式的姿态矩阵；

8、s4.根据imu测量单元获得姿态角的测量值，将测量值根据所述姿态矩阵转化为四元数形式，作为卡尔曼滤波的测量值；

9、s5.基于卡尔曼滤波对手术设备姿态进行估计，得到实时姿态四元数，将实时姿态四元数转换为欧拉角形式；

10、s6.基于imu中的加速度计数据进行位置矩阵更新，将更新后的位置矩阵以及欧拉角输入至可视化端，以实时显示手术设备的位姿。

11、进一步的，还包括在所述上位机可视化界面中设置载体坐标系和导航坐标系；根据载体坐标系与导航坐标系得到旋转矩阵，根据旋转矩阵得出姿态角的计算公式；

12、所述姿态角由载体坐标系到导航坐标系之间的相对角位置关系来表示,用欧拉角来对手术设备姿态进行描述；欧拉角包括俯仰角、横滚角和偏航角；

13、俯仰角是坐标系沿y轴旋转的角度，旋转范围为-90°至+90°；横滚角是坐标系绕x轴旋转的角度，旋转范围为-180°至+180°；偏航角是坐标系绕z轴旋转的角度,旋转范围为0°至360°。

14、进一步的，在步骤s2中，旋转矩阵如下：

15、

16、其中，为导航坐标系到载体坐标系的旋转矩阵，γ为横滚角，θ为俯仰角，为偏航角；

17、上述旋转矩阵可记为：

18、

19、通过上述两个矩阵的关系可以得知横滚角、俯仰角、偏航角的计算公式分别为：

20、

21、θc＝sin-1(-c31)

22、

23、其中，γ为横滚角，θ为俯仰角，为偏航角。

24、进一步的，在步骤s3中，设置初始四元数q0、q1、q2、q3为：

25、

26、其中，q0、q1、q2、q3分别为四个四元数，γ为横滚角，θ为俯仰角，为偏航角；

27、四元数表示的姿态矩阵为：

28、

29、其中，为四元数的姿态矩阵，q0、q1、q2、q3分别为四个四元数。

30、进一步的，在步骤s4中，将测量值根据所述姿态矩阵转化为四元数形式包括：

31、基于imu测量单元中加速度计获得横滚角与俯仰角：

32、当载体坐标系与导航坐标系重合时，加速度计所得的数据为重力加速度：

33、an＝[0 0 g]t

34、其中，an为标准加速度向量，g为重力加速度标量值；

35、设在运动状态时的加速度计测量值为：

36、ab＝[ax qy az]t

37、其中，ab为测量加速度向量，ax、ay、az分别为加速度计三轴所测量标准值；

38、根据旋转矩阵可得ab、an之间的关系：

39、

40、所以可得俯仰角和横滚角为：

41、

42、进一步的，在步骤s4中，基于imu测量单元中的磁力计获得偏航角：

43、设磁力计在导航坐标系下的测量值为：

44、

45、其中，mn为磁力计测量向量，分别为磁力计三轴所测量标准值；

46、则，可得偏航角为：

47、

48、设载体坐标系下磁力计的测量值为：

49、

50、根据旋转矩阵得到两者关系为：

51、

52、可得：

53、

54、带入偏航角求取公式中可得偏航角。

55、进一步的，在步骤s5中，基于卡尔曼滤波对手术设备姿态进行估计包括以下步骤：

56、设置四元数将测量噪声r和过程噪声q设置为高斯分布；

57、四元数的微分形式为：

58、

59、使用四阶龙格库塔法对微分方程进行求解即可得到四元数的更新形式为：

60、

61、通过四元数的更新形式即可得到状态矩阵k，则先验估计形式为：

62、

63、其中，wk为过程噪声；qk为过程噪声协方差矩阵；为协方差矩阵；h为观测矩阵；为先验估计状态fk为状态矩阵；上一时刻后验估计状态；wk-1为上一时刻过程噪声；pk-1为上一时刻后验协方差矩阵；为角标t的转置符号，为代数数学通用符号；卡尔曼增益为：

64、

65、其中，rk为观测噪声值，k为迭代次数；

66、通过imu测量单元测量的观测值所得的四元数可以得到最终的估计值，z为观测值，即为由imu测量单元得到的姿态四元数为：

67、

68、最后更新误差协方差矩阵：

69、

70、上述公式所得的即为实时姿态四元数，使用下述公式即可转换为欧拉角形式：

71、

72、其中，q0、q1、q2、q3分别为四个四元数。

73、进一步的，在步骤s6中，通过加速度计数据进行位置矩阵更新的步骤包括：

74、获取当前的姿态四元数qest；

75、将重力向量在物体坐标系中表示为一个四元数，记为qgravity，重力在物体坐标系中的表示为[0,0,-1,0]；

76、使用四元数的旋转操作，将重力向量进行补偿，通过以下公式实现：

77、

78、其中,表示四元数的乘法运算，qcompensated为补偿后重力四元数；qest为补偿四元数、qgravity为标准重力四元数、为补偿四元数的逆运算；

79、从补偿后的四元数中提取出重力补偿后的线性加速度，可以通过以下公式计算得到：

80、linear_acccompensated＝[ax,ay,az]

81、＝2[qcompensated[2],qcompensated[3],qcompensated[4]]

82、其中，linear_acccompensated为加速度向量，ax,ay,az分别为加速度计三轴所测量标量值；

83、位置积分为：

84、

85、其中，v为速度向量，p为位置向量；at表示当前时间的加速度。

86、第二方面，本发明所述的基于imu的手术设备位姿识别系统，包括：imu测量单元、上位机、传输单元和可视化界面；

87、所述imu测量单元设置在手术设备上，用于对手术设备的加速度值、角速度值进行测量，获得横滚角、俯仰角和偏航角的测量值；

88、所述imu测量单元将测得的数据通过传输单元发送至上位机，所述上位机基于互补滤波的数据处理后上传至可视化界面。

89、进一步的，所述imu测量单元设置在手术设备的尾部，所述imu测量单元包括陀螺仪、加度计和磁力计。

90、综上所述，本发明与现有技术相比，具有如下的有益技术效果：

91、本发明所述的一种基于单imu的手术设备位姿识别方法，将imu测量单元置于手术设备尾部，imu测量单元对手术设备尾部的加速度值、角速度值进行测量，imu测量单元所测得的数据发送至上位机，通过上位机进行基于互补滤波的数据处理后上传至可视化界面，通过可视化界面对手术设备姿态进行实时复现，通过欧拉角表示手术设备的姿态角，实现了可靠且迅速的手术设备位姿捕捉，大大提高了手术设备解算的精度和动态性能，同时，所得到的欧拉角计算中不含有重力加速度，完全消除了重力加速度在求解欧拉角时的影响，提高了姿态检测的精度。