一种主手操作臂运动学精确定位可视化验证方法与流程

本发明涉及腹腔镜微创手术机器人，具体而言，涉及一种主手操作臂运动学精确定位可视化验证方法。

背景技术：

1、随着机器人技术的不断创新，医疗机器人辅助医疗工作人员进行手术的技术得到了快速发展，原因在于医疗机器人不仅可以帮助医疗工作人员进行一系列的医疗诊断和辅助治疗，还能有效缓解医疗资源紧张的问题。通常，医疗机器人包括用于执行操作的从动工具及用于控制从动工具运动的主手操作臂。在实际场景中，从动工具被设置成能够进入操作区域，医疗工作人员通过遥操作主手操作臂进而控制从动工具在操作区域中的运动，实现医疗操作。一般医疗机器人通过主手操作臂与从动工具之间的运动转换——主从映射，实现主手操作臂对从动工具的运动控制。

2、腹腔镜微创手术是指利用腹腔镜、胸腔镜等现代医疗器械及相关设备进行的手术。与传统的开放手术相比，微创手术具有创口小、疼痛轻、术中出血量少、术后恢复快、感染风险低等优势，因此在外科手术领域得到了广泛的认可与应用。

3、参考授权公告号为cn109091237b，名称为微创手术器械辅助系统的中国发明专利以及授权公告号为cn109091238b、名称为分体式微创手术器械辅助系统的中国发明专利，以及授权公告号为cn210872029u、名称为医生操作台的实用新型专利，以及，微创手术器机器人被设计为在内窥镜下，由医生在手术室内操作，控制手术器械做手术的设备系统。医生坐在医生操作台前，通过观看3d影像显示器，操作医生机械臂，医生可如同平常手术般，利用患者机械臂精准的控制手术器械执行各种手术动作。

4、参考公开号为cn105286999a、cn105286989a的发明专利申请，手术器械可以执行不同功能，包括夹持、切除、切割、缝合、吻合等。手术器械具有不同的配置，其包括执行末端，腕节、器械杆、器械盒等。

5、现有微创手术机器人系统，医生通过手持多自由度的机械式运动输入装置来输出动作指令，通过主从运动映射算法，可以将采集到的动作之指令转换为机械臂的关节运动信息，从机械臂跟随主机械臂的运动。参考授权公告号为cn112716608b的发明专利，就公开了一种基于d-h主从映射方法。

6、主手操作臂在发送关键关节的操控指令时，在关节运动范围内，通过一定的映射比例，可将主手操作臂的运动学关系映射到从手端，使得医生在设定的比例情景下，精准、高效的完成期望规划路径的手术操控。因此，如何保证从手端精准、高效的完成期望规划路径，如何验证是否准确按照期望规划路径动作，是本领域技术人员亟待解决的技术问题。

技术实现思路

1、本发明就是为了解决现有基于主从映射的微创手术机器人，如何保证从手端精准、高效的完成期望规划路径，如何验证是否准确按照期望规划路径动作的技术问题，提供了一种主手操作臂运动学精确定位可视化验证方法。

2、本发明在精准医疗手术操控需求背景下，利用dh参数化方法，获取主手操作臂正向运动学模型的解析，利用simulink/ros可视化技术，对比验证了主手操作臂两种方法下的3d动态仿真效果和机构末端精确定位准确性。

3、本发明提供一种主手操作臂运动学精确定位可视化验证方法，包括以下步骤：

4、步骤s1.基于修正dh方法的主手操作臂正向运动学；

5、采用齐次变换法描述刚体的位姿，可将连杆间的运动关系转化为数学计算，在笛卡尔坐标系{a}中，空间中任意点p用3×1向量描述其位置矢量ap为：

6、

7、式中

8、px——点p在x轴方向的分量；

9、py——点p在y轴方向的分量；

10、pz——点p在z轴方向的分量；

11、ap——a表示坐标系{a}；

12、为了表示三维空间中任意刚体b的姿态，用表示坐标系{b}在坐标系{a}中的原点位置，表示坐标系{b}在坐标系{a}中的坐标轴姿态，刚体b的姿态：则齐次矩阵的一般通式为

13、

14、步骤s2.tf欧拉方法建立主手操作臂运动学；

15、主手操作臂3d模型导入ros系统，构建基于tf树生成机构模型运动学系统，从(tf)欧拉变换方法建模原理看，tf库的目的是实现系统中任一个点在所有坐标系之间的坐标变换，那么，以主动臂基座底端中心为原点，构建基坐标系c0-0；

16、步骤s3.主手操作臂运动学精确定位求解的可视化验证

17、步骤1.dh方法与tf方法单点验证仿真结果比较

18、1)ros系统的tf数据导入

19、在搭建的主手操作臂ros平台上，利用可视化调控界面，设置tf坐标系关键运动学数据参数，实现ros系统的tf数据导入，结合表2所示的tf坐标系关键运动学参数，调控可视化界面参数，主手操作臂末端位置为

20、

21、2)零位坐标系对齐

22、对比表1-2，区别于(tf)欧拉变换的绕z轴关节旋转角度坐标系零位为

23、tf0＝[0 0 0 0 0 0]t                 (17)

24、dh变换下的坐标需要经过初始变换

25、

26、由于初始状态的不一致，dh变换下求解的机构末端位置满足变换规则：(tf)变换下x轴坐标值对齐于(dh)变换下y轴坐标值，(tf)变换下y轴坐标值(tf)对齐于x轴坐标值的负值(dh)；

27、

28、3)基于零位对齐dh方法的机构末端位置求解

29、结合变换矩阵(19)，将经过初始变换(18)后，dh变换下对齐的6个关节旋转角度坐标系零位，(其余参数dhset＝{a1,d1,d3,d4,d5,d6}相对固定)代入到s1表示的dh求解方法，得出主手操作臂末端位置：

30、

31、步骤2.基于两种方法的主手操作臂运动学精确定位可视化验证

32、利用ros平台可视化调控界面，设置tf坐标系关键运动学数据参数，实现ros系统的tf数据导入，可视化调控ros系统界面上数据参数，记录各个参数界面状态下机构运动学关键数据，得到主手操作臂6个关节旋转角度，机构末端位姿；

33、在运动学参数描述下，分别得到主手操作臂6个关节旋转角度和末端位置；

34、规划出dh参数调控simulink-simscape可视化界面。

35、优选地，所述步骤s1中：

36、步骤1.搭建主手操作臂dh坐标系

37、主手操作臂dh坐标系下物理模型，dh参数坐标系构建准则的详细步骤如下：

38、(1)以基座底端为原点，构建基坐标系c0-0；

39、(2)沿垂直向上方向为z轴，绕z轴逆时针旋转角度θ1，沿z轴平移距离d1，确定出第1坐标系c0-1；

40、(3)从z1到z2沿x1轴偏置距离a1，绕z轴逆时针旋转角度θ2，再顺时针旋转确定出第2坐标系c1-2；

41、(4)绕x轴逆时针旋转角度绕z轴逆时针旋转角度θ3，再逆时针旋转角度沿z轴负方向平移距离d3(z2与z4交于z3轴的距离)，确定出第3坐标系c2-3；

42、(5)绕x轴逆时针旋转角度αt，再绕z轴逆时针旋转角度θ4，再绕z轴顺时针旋转角度沿z轴正方向平移距离d4(z3与z5交于z4轴的距离)，确定出第4坐标系c3-4；

43、(6)绕x轴顺时针旋转角度绕z轴逆时针旋转角度θ5，再沿z轴正方向平移距离d5(z4与z6交于z5轴的距离)，确定出第5坐标系c4-5；

44、(7)绕x轴逆时针旋转角度绕z轴逆时针旋转角度θ6，再沿z轴负方向平移距离d6(自定义末端点与z5轴线交于z6轴的距离)，确定出第6坐标系c5-6。

45、这里，利用dh参数变换的思想，搭建主手操作臂从基座到机构末端的正向运动学映射关系；

46、步骤2.构造主手操作臂dh参数表

47、对任意连杆i，作如下定义：

48、ai表示使用连杆长度：从zi-1关节轴线到zi关节轴线的垂直距离(沿xi轴偏置距离)等价于连杆i-1长度；

49、αi表示连杆转角：i-1关节与i关节轴线的夹角(绕xi由zi-1转到zi轴的转角)；

50、di表示连杆的偏置距离：连杆i相对于连杆i-1的偏置，即两条公垂线的距离——zi-1与zi+1交于zi轴的距离(从xi-1到xi沿zi-1轴的距离)；

51、θi表示连杆的关节角：连杆i相对于连杆i-1绕i轴的旋转角度；

52、步骤3.主手操作臂相邻关节齐次变换矩阵

53、在主手操作臂基座为原点的基坐标系下，修正dh方法变换规则是dh参数的xz类变换(mdh变换顺序为a→α→θ→d)，根据该规则可建立基于修正dh方法的正向运动学映射关系。因此，坐标系{i}相对坐标系{i-1}的相邻关节齐次变换矩阵通式为

54、

55、其中，tz,d表示相对于参数di的变换，tz,θ表示相对于参数θi的变换，tx,a表示相对于参数ai的变换，tx,α表示相对于参数αi的变换，cθi＝cosθi，sθi＝sinθi，i＝1,…,6。

56、步骤4.主手操作臂从基座到机构末端的映射关系

57、这里，可参考表1的dh参数配置代入表达式(3)，得出各相邻关节转移矩阵

58、

59、

60、

61、

62、

63、

64、考虑到相邻关节运动的连续性，结合表达式(4)-(9)，从基座到机构末端的齐次变换矩阵数学表达为

65、

66、其中，ci＝cosθi，si＝sinθi，i＝1,…,6，

67、

68、cij＝cos(θi+θj)，sij＝sin(θi+θj)，i≠j,i,j＝1,2，

69、

70、

71、

72、

73、

74、

75、

76、

77、

78、

79、

80、

81、代表法向矢量，

82、代表方位矢量，末端法方向与末端相邻关节于末端平面的投影连线方向；

83、代表接近矢量，依据右手定则，确定末端相邻关节于末端平面的投影连线方向的垂直方向；

84、代表位置矢量，相对基坐标系原点，主手操作臂末端中心位置。步骤5.主手操作臂末端位置与姿态向量

85、利用齐次变换法描述刚体位姿的描述和表达式(10)，主手操作臂末端位置为

86、

87、其中，{px,py,pz}定义于(10)。

88、相对于表达式(10)，主手操作臂末端法方向为

89、

90、

91、

92、这里，det(@)表示矩阵行列式的值，表示中第2，3行/列元素组成的矩阵，表示中第1，3行/列元素组成的矩阵，表示中第1，2行/列元素组成的矩阵。结合公式(10)，(12)-(14)，计算得到主手操作臂末端法方向

93、

94、其中，

95、

96、

97、本发明的有益效果是：使得医生在设定的映射比例情景下，精准、高效的完成期望规划路径的手术操控，解决现有的医生操持医疗器械与患者工作台配准定位困难的问题。

98、不仅通过模块化设计使得主手操作臂整体运动学的求解变得可解耦，还通过利用dh方法确定出主手操作臂末端空间位置，以便于确定出主从映射关系，用以解决现有的医疗器械与手术工作台对准定位困难的问题。这项发明也便于后续多臂机器人运动控制策略等问题提供重要的理论支撑。因此在工业机器人、医疗机器人、空间机器人等领域中均可有广泛的应用。

99、本发明进一步的特征和方面，将在以下参考附图的具体实施方式的描述中，得以清楚地记载。