一种腹腔手术机器人的全自动实时标定方法、装置及系统与流程

1.本发明涉及腹腔手术机器人技术领域，具体而言，涉及一种腹腔手术机器人的全自动实时标定方法、装置及系统。


背景技术：

2.随着医疗机器人行业的发展，腔镜手术机器人的地位上升到手术机器人顶端行列。手术机械臂作为腔镜手术机器人的重要组成部分，应具有较高的运动精度。但由于机械臂的制造装配引入的装配误差、臂杆和关节的柔性变形引入的柔性误差、机械臂作业环境的温度变化引入的热变形误差等存在，导致机械臂运动学参数的实际值与名义值之间会产生一定偏差，如果在实际应用中仍采用名义运动学参数对机械臂进行控制则会降低机械臂运动精度。


技术实现要素：

3.本发明解决的问题是如何通过标定提高机械臂运动精度。
4.为解决上述问题，本发明提供一种腹腔手术机器人的全自动实时标定方法，包括：建立机械臂的旋量运动学模型；获取所述机械臂的末端位姿，根据所述末端位姿确定所述机械臂的名义关节运动变量；根据运动学参数控制所述机械臂运动，获取所述机械臂的实际关节运动变量；根据所述名义关节运动变量和所述实际关节运动变量判断关节运动变量精度是否满足预设条件，当所述关节运动变量精度不满足所述预设条件时，采用遗传算法对所述运动学参数进行辨识以更新所述运动学参数。
5.本发明所述的腹腔手术机器人的全自动实时标定方法，利用运动学参数标定获得机械臂运动学参数的实际值，可以有效识别运动学参数，大幅度降低位姿误差，实现机械臂的自动实时精准控制；采用遗传算法完成运动学标定可寻的全局最优解，能够精准的辨识运动学参数，提高了标定精度，进而提高了机械臂运动精度。
6.可选地，所述建立机械臂的旋量运动学模型包括：根据旋量模型建立所述机械臂的正运动学模型，所述正运动学模型包括：其中，g(θ)表示机械臂关节运动参数为θi时的末端位姿，g(0)表示关节零位对应的末端位姿，表示与θi对应的运动旋量，i=1，2，...，11。
7.本发明所述的腹腔手术机器人的全自动实时标定方法，根据旋量模型建立机械臂的正运动学模型，进而能够有效识别运动学参数，大幅度降低位姿误差，实现机械臂的自动实时精准控制。
8.可选地，所述根据所述末端位姿确定所述机械臂的名义关节运动变量包括：根据第一公式确定所述名义关节运动变量，其中，所述第一公式包括：
其中，θn表示所述名义关节运动变量，f表示运动学逆解方程，x表示旋量运动学参数。
9.本发明所述的腹腔手术机器人的全自动实时标定方法，根据第一公式确定名义关节运动变量，进而能够有效识别运动学参数，大幅度降低位姿误差，实现机械臂的自动实时精准控制。
10.可选地，所述获取所述机械臂的末端位姿包括：通过动态跟踪仪实时跟踪多面体标靶的位姿，当所述机械臂运动并保持指定位姿，且所述多面体标靶的位姿在预设时间内未发生改变时，采集当前位姿，其中，所述多面体标靶安装在所述机械臂的末端。
11.本发明所述的腹腔手术机器人的全自动实时标定方法，通过动态跟踪仪实现机械臂末端位姿的实时跟踪，进而能够实现机械臂的自动实时精准控制。
12.可选地，所述根据所述名义关节运动变量和所述实际关节运动变量判断关节运动变量精度是否满足预设条件包括：将所述名义关节运动变量和所述实际关节运动变量代入所述旋量运动学模型得到两组位姿，当两组位姿的位置误差不大于0.5mm且姿态误差不大于1
°
时，判断所述关节运动变量精度满足预设条件。
13.本发明所述的腹腔手术机器人的全自动实时标定方法，根据名义关节运动变量和实际关节运动变量对应的位姿误差来判断关节运动变量精度是否满足预设条件，进行运动学参数辨识后重新进行精度验证直至精度满足要求为至，提高了机械臂标定进度，进而能够实现机械臂的自动实时精准控制。
14.可选地，所述采用遗传算法对所述运动学参数进行辨识以更新所述运动学参数包括：根据所述名义关节运动变量和所述实际关节运动变量构建目标函数，所述目标函数为：其中，f(θ,x)表示目标函数，x表示旋量运动学参数，θ
ir
表示所述实际关节运动变量，θ
in
表示所述名义关节运动变量。
15.本发明所述的腹腔手术机器人的全自动实时标定方法，以旋量运动学参数作为辨识参数，能够描述所有误差源的影响且避免奇异性，提高了机械臂标定进度，进而能够实现机械臂的自动实时精准控制。
16.可选地，所述采用遗传算法对所述运动学参数进行辨识以更新所述运动学参数还包括：设置所述旋量运动学参数为染色体，设置所述目标函数为适应度函数，设置名义运动学参数为染色体初值。
17.本发明所述的腹腔手术机器人的全自动实时标定方法，采用遗传算法完成运动学标定，可寻的全局最优解，能够精准的辨识运动学参数，提高了标定精度，进而能够实现机械臂的自动实时精准控制。
18.本发明还提供一种腹腔手术机器人的全自动实时标定装置,包括：建模模块，用于建立机械臂的旋量运动学模型；第一运动变量模块，用于获取所述机械臂的末端位姿，根据所述末端位姿确定所述机械臂的名义关节运动变量；第二运动变量模块，用于根据运动学参数控制所述机械臂运动，获取所述机械臂的实际关节运动变量；标定模块，用于根据所述名义关节运动变量和所述实际关节运动变量判断关节运动变量精度是否满足预设条件，当
所述关节运动变量精度不满足所述预设条件时，采用遗传算法对所述运动学参数进行辨识以更新所述运动学参数。本发明所述的腹腔手术机器人的全自动实时标定装置与上述腹腔手术机器人的全自动实时标定方法相对于现有技术所具有的优势相同，在此不再赘述。
19.本发明还提供一种腹腔手术机器人的全自动实时标定系统，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如上腹腔手术机器人的全自动实时标定方法。本发明所述的腹腔手术机器人的全自动实时标定系统与上述腹腔手术机器人的全自动实时标定方法相对于现有技术所具有的优势相同，在此不再赘述。
20.本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现如上腹腔手术机器人的全自动实时标定方法。本发明所述的计算机可读存储介质与上述腹腔手术机器人的全自动实时标定方法相对于现有技术所具有的优势相同，在此不再赘述。
附图说明
21.图1为本发明实施例的腹腔手术机器人的全自动实时标定方法的示意图；图2为本发明实施例的腹腔手术机器人的全自动实时标定系统的示例图。
具体实施方式
22.为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。
23.如图1所示，本发明实施例提供一种腹腔手术机器人的全自动实时标定方法，包括：建立机械臂的旋量运动学模型；获取所述机械臂的末端位姿，根据所述末端位姿确定所述机械臂的名义关节运动变量；根据运动学参数控制所述机械臂运动，获取所述机械臂的实际关节运动变量；根据所述名义关节运动变量和所述实际关节运动变量判断关节运动变量精度是否满足预设条件，当所述关节运动变量精度不满足所述预设条件时，采用遗传算法对所述运动学参数进行辨识以更新所述运动学参数。
24.具体地，在本实施例中，腹腔手术机器人的全自动实时标定方法包括：s1：建立机械臂的旋量运动学模型；s2：通过动态跟踪仪测量机械臂末端位姿矩阵，进而逆解出名义关节运动变量θn；s3：根据运动学参数，控制机械臂关节运动至指定位置，并输出实际关节运动变量θr；s4：综合n组θn与θr验证关节运动变量精度是否满足设计要求，若是则标定结束，若否则采用遗传算法进行运动学参数标定，并将得到的补偿结果反馈给s3中的运动学参数，再次控制机械臂运动。
25.本实施例可在无人干预的情况下完成s1-s4的标定流程，结合图2所示，动态跟踪仪、机械臂控制器、标定软件可实时通讯，动态跟踪仪可将标靶位姿传输给标定软件，机械臂控制器可将实际关节运动变量传输给标定软件，动态跟踪仪与机械臂控制器可联合判断位姿采集与机械臂运动启停的时机。标定软件根据标靶位姿和实际关节运动变量判断腹腔手术机器人的位姿精度是否满足设计要求，若不满足则利用遗传算法完成运动学参数辨
识，并将辨识后的运动学参数传输给机械臂控制器，完成机械臂控制器中运动学参数的更新。运动学误差模型具有完备性、连续性、无冗余性等优点，且模型变量均物理意义明确。
26.在本实施例中，利用运动学参数标定获得机械臂运动学参数的实际值，可以有效识别运动学参数，大幅度降低位姿误差，实现机械臂的自动实时精准控制；采用遗传算法完成运动学标定可寻的全局最优解，能够精准的辨识运动学参数，提高了标定精度，进而提高了机械臂运动精度。
27.可选地，所述建立机械臂的旋量运动学模型包括：根据旋量模型建立所述机械臂的正运动学模型，所述正运动学模型包括：其中，g(θ)表示机械臂关节运动参数为θi时的末端位姿，g(0)表示关节零位对应的末端位姿，表示与θi对应的运动旋量。
28.具体地，在本实施例中，在步骤s1中，基于旋量模型，构建机械臂的正运动学模型：g(θ)表示机械臂关节运动参数为θi时的末端位姿，g(0)表示关节零位对应的末端位姿，表示与θi对应的运动旋量，i=1，2，...，11。
29.依据李群李代数公式获得如下（i为单位矩阵，为ω的反对称矩阵）：其中：其中：其中：其中，v表示沿ω方向的线速度，ω、r分别表示各关节旋转轴线方向的单位向量与其上任意一点坐标。
30.在本实施例中，根据旋量模型建立机械臂的正运动学模型，进而能够有效识别运动学参数，大幅度降低位姿误差，实现机械臂的自动实时精准控制。
31.可选地，所述根据所述末端位姿确定所述机械臂的名义关节运动变量包括：根据第一公式确定所述名义关节运动变量，其中，所述第一公式包括：其中，θn表示所述名义关节运动变量，f表示运动学逆解方程，x表示旋量运动学参
数。
32.具体地，在本实施例中，为便于运动学标定，名义关节运动变量：其中，f表示运动学逆解方程，x表示旋量运动学参数。
33.在本实施例中，根据第一公式确定名义关节运动变量，进而能够有效识别运动学参数，大幅度降低位姿误差，实现机械臂的自动实时精准控制。
34.可选地，所述获取所述机械臂的末端位姿包括：通过动态跟踪仪实时跟踪多面体标靶的位姿，当所述机械臂运动并保持指定位姿，且所述多面体标靶的位姿在预设时间内未发生改变时，采集当前位姿，其中，所述多面体标靶安装在所述机械臂的末端。
35.具体地，在本实施例中，步骤s2和s3中，在机械臂末端安装多面体标靶，利用动态跟踪仪实时跟踪标靶位姿。在机械臂各关节运动范围内规划n组实际关节运动变量θr，当机械臂运动并保持某一指定位姿时，动态跟踪仪检测到标靶位姿2秒内未发生改变，则采集标靶位姿，标靶位姿采集完成后，机械臂继续运动至下一个位姿点，如此循环完成n组末端位姿的采集。将n组末端位姿与n组实际关节运动变量θr输入至标定软件中，根据n组末端位姿逆解n组名义关节运动变量θn。
36.在本实施例中，通过动态跟踪仪实现机械臂末端位姿的实时跟踪，进而能够实现机械臂的自动实时精准控制。
37.可选地，所述根据所述名义关节运动变量和所述实际关节运动变量判断关节运动变量精度是否满足预设条件包括：将所述名义关节运动变量和所述实际关节运动变量代入所述旋量运动学模型得到两组位姿，当两组位姿的位置误差不大于0.5mm且姿态误差不大于1
°
时，判断所述关节运动变量精度满足预设条件。
38.具体地，在本实施例中，根据名义关节运动变量和实际关节运动变量判断关节运动变量精度是否满足预设条件包括：根据n组θn与θr验证关节运动变量精度是否满足设计要求，若满足精度要求则无需标定，具体地：将θn与θr同时带入理论的运动学模型中，将得到两组位姿，当这两组位姿的位置误差不大于0.5mm、姿态误差不大于1
°
时，认为满足精度要求。若不满足精度要求，则利用n组θn与θr进行运动学参数辨识，并将辨识的运动学参数带回步骤s3中。以此运动学参数控制机械臂运动至一随机位姿点处，获取并验证此时机械臂的θn′
与θr′
是否满足精度要求。
39.若满足精度要求则标定结束，若不满足精度要求，则将该组θn′
、θr′
与n组θn、θr合并（即将数据θn′
、θr′
与数据θn、θr叠加组合在一起成为一组新的数据，如θn、θr构成2*m列、n行的矩阵（m代表机械臂关节个数即θ的个数，n代表规划的测试位姿组数），将θn′
、θr′
与n组θn、θr合并后，2*m列、n行的矩阵将变为2*m列、n 1行的矩阵）。利用n 1组θn与θr进行运动学参数辨识，并将辨识的运动学参数带回步骤s3中，重新进行精度验证。如此反复直至精度满足要求为止。
40.在本实施例中，根据名义关节运动变量和实际关节运动变量对应的位姿误差来判断关节运动变量精度是否满足预设条件，进行运动学参数辨识后重新进行精度验证直至精度满足要求为至，提高了机械臂标定进度，进而能够实现机械臂的自动实时精准控制。
41.可选地，所述采用遗传算法对所述运动学参数进行辨识以更新所述运动学参数包
括：根据所述名义关节运动变量和所述实际关节运动变量构建目标函数，所述目标函数为：其中，f(θ,x)表示目标函数，x表示旋量运动学参数，θ
ir
表示所述实际关节运动变量，θ
in
表示所述名义关节运动变量。
42.具体地，在本实施例中，在步骤s4中，使用残差平方和构建目标函数：其中，f(θ,x)表示目标函数，x表示旋量运动学参数，θ
ir
表示实际关节运动变量，θ
in
表示名义关节运动变量。以旋量运动学参数作为辨识参数能够描述所有误差源的影响，同时也能避免奇异性。
43.利用关节运动变量提出的标定目标函数，打破了只能用位置进行标定的传统方法，使位置与姿态同时达到较高的标定效果，且有效地解决了辨识矩阵病态和姿态奇异性问题，提高误差源辨识精度以及辨识结果的稳定性，进而提高补偿精度。
44.在本实施例中，以旋量运动学参数作为辨识参数，能够描述所有误差源的影响且避免奇异性，提高了机械臂标定进度，进而能够实现机械臂的自动实时精准控制。
45.可选地，所述采用遗传算法对所述运动学参数进行辨识以更新所述运动学参数还包括：设置所述旋量运动学参数为染色体，设置所述目标函数为适应度函数，设置名义运动学参数为染色体初值。
46.具体地，在本实施例中，基于遗传算法完成运动学的参数辨识，设置x为染色体，f(θ,x)为适应度函数,其中染色体初值为名义运动学参数。初始种群规模、交配概率、变异概率、最优个体系数、最大进化代数等参数根据实际情况进行设置。
47.在本实施例中，采用遗传算法完成运动学标定，可寻的全局最优解，能够精准的辨识运动学参数，提高了标定精度，进而能够实现机械臂的自动实时精准控制。
48.本发明另一实施例提供一种腹腔手术机器人的全自动实时标定装置,包括：建模模块，用于建立机械臂的旋量运动学模型；第一运动变量模块，用于获取所述机械臂的末端位姿，根据所述末端位姿确定所述机械臂的名义关节运动变量；第二运动变量模块，用于根据运动学参数控制所述机械臂运动，获取所述机械臂的实际关节运动变量；标定模块，用于根据所述名义关节运动变量和所述实际关节运动变量判断关节运动变量精度是否满足预设条件，当所述关节运动变量精度不满足所述预设条件时，采用遗传算法对所述运动学参数进行辨识以更新所述运动学参数。
49.本发明另一实施例提供一种腹腔手术机器人的全自动实时标定系统，包括存储有计算机程序的计算机可读存储介质和处理器，所述计算机程序被所述处理器读取并运行时，实现如上腹腔手术机器人的全自动实时标定方法。
50.本发明另一实施例提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现如上腹腔手术机器人的全自动实时标定方法。
51.虽然本发明公开披露如上，但本发明公开的保护范围并非仅限于此。本领域技术
人员在不脱离本发明公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。