CN113799127B - 光学双目定位系统下六自由度机械臂无标定位姿定位方法 - Google Patents

光学双目定位系统下六自由度机械臂无标定位姿定位方法 Download PDF

Info

Publication number
CN113799127B
CN113799127B CN202111084140.8A CN202111084140A CN113799127B CN 113799127 B CN113799127 B CN 113799127B CN 202111084140 A CN202111084140 A CN 202111084140A CN 113799127 B CN113799127 B CN 113799127B
Authority
CN
China
Prior art keywords
mechanical arm
degree
jacobian matrix
angle
positioning
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111084140.8A
Other languages
English (en)
Other versions
CN113799127A (zh
Inventor
杨荣骞
蔡丽晶
郑凌翔
黄岳山
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN202111084140.8A priority Critical patent/CN113799127B/zh
Publication of CN113799127A publication Critical patent/CN113799127A/zh
Application granted granted Critical
Publication of CN113799127B publication Critical patent/CN113799127B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/70—Manipulators specially adapted for use in surgery
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
    • A61B2034/2046—Tracking techniques
    • A61B2034/2055—Optical tracking systems
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
    • A61B2034/2068—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis using pointers, e.g. pointers having reference marks for determining coordinates of body points

Abstract

本发明公开了一种光学双目定位系统下六自由度机械臂无标定位姿定位方法，包括：1)构建角度雅可比矩阵和位置雅可比矩阵；2)将角度雅可比矩阵和位置雅可比矩阵展开为一维向量，以展开后的一维向量作为状态量分别构建平方根无迹卡尔曼滤波非线性模型；3)在以角度雅可比矩阵和位置雅可比矩阵为状态量构建的平方根无迹卡尔曼滤波非线性模型下分别对状态量进行在线估计，根据估计得到的图像雅可比矩阵计算六自由度机械臂任务空间欧拉角控制量和位置控制量，最终得到最优状态量实现六自由度机械臂位姿定位。本方法无需采集大量数据进行术前标定，不受光学双目定位系统与六自由度机械臂之间相对位置改变影响，精度高，可提高手术机器人系统临床适用性。

Description

光学双目定位系统下六自由度机械臂无标定位姿定位方法
技术领域
本发明涉及医疗机器人的技术领域，尤其是指一种光学双目定位系统下六自由度机械臂无标定位姿定位方法。
背景技术
近年来，手术机器人在骨科、神经外科、牙科等领域得到良好发展并初步应用于临床，在定位方式中较为广泛的应用有光学定位和电磁定位，其中光学定位具有精度高、使用方便、不受电磁环境干扰等优点。光学双目定位系统引导下的机械臂定位方法可分为有标定和无标定方法。现有手术机器人系统大多采用有标定定位方法，通过固定在手术机械臂末端执持器上的光学靶标运动确定机器人系统内各部件之间的空间位置关系，实现双目定位系统三维空间到机械臂任务空间的转换。然而为减小机械臂运动误差影响，有标定定位方法需采集大量机械臂运动数据，耗时较长；且一旦光学双目定位系统与机械臂基座之间相对位置改变则需重新计算标定参数。机械臂无标定定位方法无需事先标定，在机械臂步进过程中对未知参数进行实时在线估计，最终系统偏差收敛完成机械臂位姿定位。
发明内容
本发明的目的在于克服现有技术的缺点与不足，提出一种光学双目定位系统下六自由度机械臂无标定位姿定位方法，无需术前标定，可实现手术工具的角度与位置快速定位，提高临床适用性。
为实现上述目的，本发明采用以下技术方案：光学双目定位系统下六自由度机械臂无标定位姿定位方法，该方法需要用到带有反光标记球的手术工具，所述手术工具固定于六自由度机械臂的末端，并位于光学双目定位系统的视野范围内，其包括以下步骤：
1)分别以光学双目定位系统下的手术工具的角度和位置为图像特征构建角度雅可比矩阵和位置雅可比矩阵；
2)将角度雅可比矩阵和位置雅可比矩阵展开为一维向量，以展开后的一维向量作为状态量分别构建平方根无迹卡尔曼滤波非线性模型；
3)在以角度雅可比矩阵和位置雅可比矩阵为状态量构建的平方根无迹卡尔曼滤波非线性模型下分别对状态量进行在线估计，根据估计得到的图像雅可比矩阵计算六自由度机械臂任务空间欧拉角控制量和位置控制量，最终得到最优状态量实现六自由度机械臂位姿定位。
进一步，在步骤1)，构建的角度雅可比矩阵Jd和位置雅可比矩阵Jp具体如下：
Figure BDA0003263570830000021
Figure BDA0003263570830000022
式中，
Figure BDA0003263570830000023
为光学双目定位系统下手术工具角度向量，/>
Figure BDA0003263570830000024
为六自由度机械臂欧拉角控制量，/>
Figure BDA0003263570830000025
为光学双目定位系统下手术工具位置向量，/>
Figure BDA0003263570830000026
为六自由度机械臂末端位置控制量。/>
进一步，在步骤2)，以角度雅可比矩阵展开的一维向量为系统状态量构建平方根无迹卡尔曼滤波非线性模型：
Figure BDA0003263570830000027
Figure BDA0003263570830000028
式中，状态量
Figure BDA0003263570830000029
为k时刻角度雅可比矩阵/>
Figure BDA00032635708300000210
的一维展开向量，观测值/>
Figure BDA00032635708300000211
为k时刻手术工具角度变化量/>
Figure BDA00032635708300000212
列向量，Hd为观测矩阵，/>
Figure BDA00032635708300000213
和/>
Figure BDA00032635708300000214
分别为k时刻系统噪声、观测噪声，符合高斯分布，协方差矩阵分别为/>
Figure BDA00032635708300000215
以位置雅可比矩阵展开的一维向量为系统状态量构建平方根无迹卡尔曼滤波非线性模型：
Figure BDA0003263570830000031
Figure BDA0003263570830000032
式中，状态量
Figure BDA0003263570830000033
为k时刻位置雅可比矩阵/>
Figure BDA0003263570830000034
的一维展开向量，观测值/>
Figure BDA0003263570830000035
为k时刻手术工具位置变化量/>
Figure BDA0003263570830000036
列向量，Hp为观测矩阵，/>
Figure BDA0003263570830000037
和/>
Figure BDA0003263570830000038
分别为k时刻系统噪声、观测噪声，符合高斯分布，协方差矩阵分别为/>
Figure BDA0003263570830000039
进一步，在步骤3)，进行在线估计，包括以下步骤：
31)系统初始化，设初始角度雅可比矩阵为
Figure BDA00032635708300000310
展开得到/>
Figure BDA00032635708300000311
设初始状态估计误差协方差为/>
Figure BDA00032635708300000312
其cholesky分解因子为/>
Figure BDA00032635708300000313
设初始位置雅可比矩阵为/>
Figure BDA00032635708300000314
展开得到/>
Figure BDA00032635708300000315
设初始状态估计误差协方差为/>
Figure BDA00032635708300000316
其cholesky分解因子为/>
Figure BDA00032635708300000317
32)在角度雅可比矩阵一维展开向量构建的平方根无迹卡尔曼滤波模型下，根据k-1时刻状态量
Figure BDA00032635708300000318
及其误差协方差平方根/>
Figure BDA00032635708300000319
估计k时刻状态量/>
Figure BDA00032635708300000320
及其误差协方差平方根/>
Figure BDA00032635708300000321
k时刻状态量转换成角度雅可比矩阵估计值/>
Figure BDA00032635708300000322
后计算六自由度机械臂k+1时刻欧拉角控制量；在位置雅可比矩阵一维展开向量构建的平方根无迹卡尔曼滤波模型下，根据k-1时刻状态量/>
Figure BDA00032635708300000323
及其误差协方差平方根/>
Figure BDA00032635708300000324
估计k时刻状态量/>
Figure BDA00032635708300000325
及其误差协方差平方根
Figure BDA00032635708300000326
k时刻状态量转换成位置雅可比矩阵估计值/>
Figure BDA00032635708300000327
并计算六自由度机械臂k+1时刻位置控制量；
33)将六自由度机械臂上述欧拉角控制量和位置控制量输入控制系统并控制其到达后，获取当前手术工具的角度向量和位置向量，计算其与目标角度向量和位置向量之差，若小于误差阈值则表示定位成功，否则回到步骤32)。
进一步，在步骤32)，计算六自由度机械臂的欧拉角控制量和位置控制量，具体如下：
计算六自由度机械臂k+1时刻欧拉角控制量：
Figure BDA00032635708300000328
/>
Figure BDA00032635708300000329
式中，
Figure BDA0003263570830000041
为六自由度机械臂k+1时刻欧拉角控制量，/>
Figure BDA0003263570830000042
为k时刻手术工具的角度向量与目标角度向量之差，Kd为常量步进值，/>
Figure BDA0003263570830000043
为计算中间值；
计算六自由度机械臂k+1时刻位置控制量：
Figure BDA0003263570830000044
Figure BDA0003263570830000045
式中，
Figure BDA0003263570830000046
为六自由度机械臂k+1时刻欧拉角控制量，/>
Figure BDA0003263570830000047
为k时刻手术工具位置向量与目标位置向量之差，Kp为常量步进值，/>
Figure BDA0003263570830000048
为计算中间值。
本发明与现有技术相比，具有如下优点和有益效果：
1、本发明提出了光学双目定位系统下六自由度机械臂无标定位姿定位，无需采集大量数据进行术前标定，不受光学双目定位系统与六自由度机械臂之间相对位置改变影响。
2、本发明提出的光学双目定位系统下六自由度机械臂无标定位姿定位方法基于平方根无迹卡尔曼滤波框架实现，精度高，收敛性好，可解决有标定技术中的耗时长问题。
3、本发明提出的光学双目定位系统下六自由度机械臂无标定位姿定位方法操作简单，使用灵活，可大大提高手术机器人系统临床适用性。
附图说明
图1为本发明实例的场景构成示意图。
图2为本发明的无标定位姿定位逻辑流程示意图。
具体实施方式
下面结合具体实施实例对本发明做进一步说明，但本发明的实施方式不限于此。
本实施例公开了一种光学双目定位系统下六自由度机械臂无标定位姿定位方法，如图1所示，该方法需要用到带有反光标记球的手术工具3，所述手术工具固定于六自由度机械臂1的末端，并位于光学双目定位系统2的视野范围内。如图2所示，该方法包括以下步骤：
1)分别以光学双目定位系统下的手术工具的角度和位置为图像特征构建角度雅可比矩阵Jd和位置雅可比矩阵Jp：
Figure BDA0003263570830000051
其中，
Figure BDA0003263570830000052
为光学双目定位系统下手术工具角度向量，θx、θy、θz分别为手术工具针尖向量与x轴、y轴、z轴夹角，θf为手术工具平面法向量与YZ平面夹角，
Figure BDA0003263570830000053
为六自由度机械臂任务空间欧拉角。
Figure BDA0003263570830000054
其中，
Figure BDA0003263570830000055
为光学双目定位系统下手术工具位置向量，/>
Figure BDA0003263570830000056
六自由度机械臂任务空间末端位置。
2)Jd、Jp展开成一维向量得到：
Figure BDA0003263570830000057
Figure BDA0003263570830000058
以Xd为状态量构建平方根无迹卡尔曼滤波非线性模型：
Figure BDA0003263570830000061
Figure BDA0003263570830000062
其中，状态量
Figure BDA0003263570830000063
为k时刻角度雅可比矩阵/>
Figure BDA0003263570830000064
的一维展开向量，观测值/>
Figure BDA0003263570830000065
为k时刻手术工具角度变化量/>
Figure BDA0003263570830000066
列向量，Hd为观测矩阵，/>
Figure BDA0003263570830000067
和/>
Figure BDA0003263570830000068
分别为k时刻系统噪声、观测噪声，符合高斯分布，协方差矩阵分布为/>
Figure BDA0003263570830000069
以Xp为系统状态量构建平方根无迹卡尔曼滤波非线性模型：
Figure BDA00032635708300000610
/>
Figure BDA00032635708300000611
其中，状态量
Figure BDA00032635708300000612
为k时刻位置雅可比矩阵/>
Figure BDA00032635708300000613
的一维展开向量，观测值/>
Figure BDA00032635708300000614
为k时刻手术工具位置变化量/>
Figure BDA00032635708300000615
列向量，Hp为观测矩阵，/>
Figure BDA00032635708300000616
和/>
Figure BDA00032635708300000617
分别为k时刻系统噪声、观测噪声，符合高斯分布，协方差矩阵分布为/>
Figure BDA00032635708300000618
3)系统初始化，设初始角度雅可比矩阵为
Figure BDA00032635708300000619
展开得到/>
Figure BDA00032635708300000620
设初始状态估计误差协方差为/>
Figure BDA00032635708300000621
其cholesky分解因子为/>
Figure BDA00032635708300000622
设初始位置雅可比矩阵为/>
Figure BDA00032635708300000623
展开得到/>
Figure BDA00032635708300000624
设初始状态估计误差协方差为/>
Figure BDA00032635708300000625
其cholesky分解因子为/>
Figure BDA00032635708300000626
4)角度雅可比矩阵一维展开向量构建的平方根无迹卡尔曼滤波非线性模型根据k-1时刻状态量
Figure BDA00032635708300000627
及其误差协方差/>
Figure BDA00032635708300000628
估计k时刻状态量/>
Figure BDA00032635708300000629
及其误差协方差，位置雅可比矩阵一维展开向量构建的平方根无迹卡尔曼滤波非线性模型根据k-1时刻状态量/>
Figure BDA00032635708300000630
及其误差协方差/>
Figure BDA00032635708300000631
估计k时刻状态量/>
Figure BDA00032635708300000632
及其误差协方差/>
Figure BDA00032635708300000633
5)将k时刻状态量
Figure BDA00032635708300000634
转换成角度雅可比矩阵估计值/>
Figure BDA00032635708300000635
后计算六自由度机械臂k+1时刻欧拉角控制量：
Figure BDA00032635708300000636
Figure BDA00032635708300000637
其中，
Figure BDA0003263570830000071
为六自由度机械臂k+1时刻欧拉角控制量，/>
Figure BDA0003263570830000072
为k时刻手术工具角度向量与目标角度向量之差，Kd为常量步进值，/>
Figure BDA0003263570830000073
为计算中间值。
将k时刻状态量
Figure BDA0003263570830000074
转换成位置雅可比矩阵估计值/>
Figure BDA0003263570830000075
并计算六自由度机械臂k+1时刻末端位置控制量：
Figure BDA0003263570830000076
Figure BDA0003263570830000077
其中，
Figure BDA0003263570830000078
为六自由度机械臂k+1时刻欧拉角控制量，/>
Figure BDA0003263570830000079
为k时刻手术工具位置向量与目标位置向量之差，Kp为常量步进值，/>
Figure BDA00032635708300000710
为计算中间值。
6)将六自由度机械臂上述欧拉角控制量和位置控制量输入控制系统使其到达，然后获取当前手术工具的角度向量和位置向量计算其与目标位姿之差，若小于误差阈值则表示定位成功，否则回到步骤4)。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (4)

1.光学双目定位系统下六自由度机械臂无标定位姿定位方法，其特征在于：该方法需要用到带有反光标记球的手术工具，所述手术工具固定于六自由度机械臂的末端，并位于光学双目定位系统的视野范围内，其包括以下步骤：
1)分别以光学双目定位系统下的手术工具的角度和位置为图像特征构建角度雅可比矩阵和位置雅可比矩阵；
2)将角度雅可比矩阵和位置雅可比矩阵展开为一维向量，以展开后的一维向量作为状态量分别构建平方根无迹卡尔曼滤波非线性模型；
3)在以角度雅可比矩阵和位置雅可比矩阵为状态量构建的平方根无迹卡尔曼滤波非线性模型下分别对状态量进行在线估计，根据估计得到的图像雅可比矩阵计算六自由度机械臂任务空间欧拉角控制量和位置控制量，最终得到最优状态量实现六自由度机械臂位姿定位；其中，进行在线估计，包括以下步骤：
31)系统初始化，设初始角度雅可比矩阵为
Figure FDA0004128300890000011
展开得到/>
Figure FDA0004128300890000012
设初始状态估计误差协方差为/>
Figure FDA0004128300890000013
其cholesky分解因子为/>
Figure FDA0004128300890000014
设初始位置雅可比矩阵为/>
Figure FDA0004128300890000015
展开得到/>
Figure FDA0004128300890000016
设初始状态估计误差协方差为/>
Figure FDA0004128300890000017
其cholesky分解因子为/>
Figure FDA0004128300890000018
32)在角度雅可比矩阵一维展开向量构建的平方根无迹卡尔曼滤波非线性模型下，根据k-1时刻状态量
Figure FDA0004128300890000019
及其误差协方差平方根/>
Figure FDA00041283008900000110
估计k时刻状态量/>
Figure FDA00041283008900000111
及其误差协方差平方根/>
Figure FDA00041283008900000112
k时刻状态量转换成角度雅可比矩阵估计值/>
Figure FDA00041283008900000113
后计算六自由度机械臂k+1时刻欧拉角控制量；在位置雅可比矩阵一维展开向量构建的平方根无迹卡尔曼滤波非线性模型下，根据k-1时刻状态量/>
Figure FDA00041283008900000114
及其误差协方差平方根/>
Figure FDA00041283008900000115
估计k时刻状态量/>
Figure FDA00041283008900000116
及其误差协方差平方根/>
Figure FDA00041283008900000117
k时刻状态量转换成位置雅可比矩阵估计值/>
Figure FDA00041283008900000118
并计算六自由度机械臂k+1时刻位置控制量；
33)将六自由度机械臂上述欧拉角控制量和位置控制量输入控制系统并控制其到达后，获取当前手术工具的角度向量和位置向量，计算其与目标角度向量和位置向量之差，若小于误差阈值则表示定位成功，否则回到步骤32)。
2.根据权利要求1所述的光学双目定位系统下六自由度机械臂无标定位姿定位方法，其特征在于，在步骤1)，构建的角度雅可比矩阵Jd和位置雅可比矩阵Jp具体如下：
Figure FDA0004128300890000021
Figure FDA0004128300890000022
式中，
Figure FDA0004128300890000023
为光学双目定位系统下手术工具角度向量，/>
Figure FDA0004128300890000024
为六自由度机械臂欧拉角控制量，/>
Figure FDA0004128300890000025
为光学双目定位系统下手术工具位置向量，/>
Figure FDA0004128300890000026
为六自由度机械臂末端位置控制量。
3.根据权利要求1所述的光学双目定位系统下六自由度机械臂无标定位姿定位方法，其特征在于，在步骤2)，以角度雅可比矩阵展开的一维向量为系统状态量构建平方根无迹卡尔曼滤波非线性模型：
Figure FDA0004128300890000027
Figure FDA0004128300890000028
/>
式中，状态量
Figure FDA0004128300890000029
为k时刻角度雅可比矩阵/>
Figure FDA00041283008900000210
的一维展开向量，观测值/>
Figure FDA00041283008900000211
为k时刻手术工具角度变化量/>
Figure FDA00041283008900000212
列向量，Hd为观测矩阵，/>
Figure FDA00041283008900000213
和/>
Figure FDA00041283008900000214
分别为k时刻系统噪声、观测噪声，符合高斯分布，协方差矩阵分别为/>
Figure FDA00041283008900000215
以位置雅可比矩阵展开的一维向量为系统状态量构建平方根无迹卡尔曼滤波非线性模型：
Figure FDA00041283008900000216
Figure FDA00041283008900000217
式中，状态量
Figure FDA00041283008900000218
为k时刻位置雅可比矩阵/>
Figure FDA00041283008900000219
的一维展开向量，观测值/>
Figure FDA00041283008900000220
为k时刻手术工具位置变化量/>
Figure FDA00041283008900000221
列向量，Hp为观测矩阵，/>
Figure FDA00041283008900000222
和/>
Figure FDA00041283008900000223
分别为k时刻系统噪声、观测噪声，符合高斯分布，协方差矩阵分别为/>
Figure FDA00041283008900000224
4.根据权利要求1所述的光学双目定位系统下六自由度机械臂无标定位姿定位方法，其特征在于，在步骤32)，计算六自由度机械臂的欧拉角控制量和位置控制量，具体如下：
计算六自由度机械臂k+1时刻欧拉角控制量：
Figure FDA0004128300890000031
Figure FDA0004128300890000032
式中，
Figure FDA0004128300890000033
为六自由度机械臂k+1时刻欧拉角控制量，/>
Figure FDA0004128300890000034
为k时刻手术工具的角度向量与目标角度向量之差，Kd为常量步进值，/>
Figure FDA0004128300890000035
为计算中间值；
计算六自由度机械臂k+1时刻位置控制量：
Figure FDA0004128300890000036
Figure FDA0004128300890000037
式中，
Figure FDA0004128300890000038
为六自由度机械臂k+1时刻欧拉角控制量，/>
Figure FDA0004128300890000039
为k时刻手术工具位置向量与目标位置向量之差，Kp为常量步进值，/>
Figure FDA00041283008900000310
为计算中间值。/>
CN202111084140.8A 2021-09-15 2021-09-15 光学双目定位系统下六自由度机械臂无标定位姿定位方法 Active CN113799127B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111084140.8A CN113799127B (zh) 2021-09-15 2021-09-15 光学双目定位系统下六自由度机械臂无标定位姿定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111084140.8A CN113799127B (zh) 2021-09-15 2021-09-15 光学双目定位系统下六自由度机械臂无标定位姿定位方法

Publications (2)

Publication Number Publication Date
CN113799127A CN113799127A (zh) 2021-12-17
CN113799127B true CN113799127B (zh) 2023-05-23

Family

ID=78941228

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111084140.8A Active CN113799127B (zh) 2021-09-15 2021-09-15 光学双目定位系统下六自由度机械臂无标定位姿定位方法

Country Status (1)

Country Link
CN (1) CN113799127B (zh)

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103878770A (zh) * 2014-04-08 2014-06-25 哈尔滨工业大学 基于速度估计的空间机器人视觉时延误差补偿方法
CN108592902A (zh) * 2018-05-08 2018-09-28 清华大学 一种基于多传感器的定位设备及定位方法、系统和机械臂
CN109048890A (zh) * 2018-07-13 2018-12-21 哈尔滨工业大学（深圳） 基于机器人的协调轨迹控制方法、系统、设备及存储介质
CN109159151A (zh) * 2018-10-23 2019-01-08 北京无线电测量研究所 一种机械臂空间轨迹跟踪动态补偿方法和系统
CN109186601A (zh) * 2018-07-05 2019-01-11 南京理工大学 一种基于自适应无迹卡尔曼滤波的激光slam算法
CN109877840A (zh) * 2019-04-02 2019-06-14 哈尔滨工程大学 一种基于相机光轴约束的双机械臂标定方法
CN110039542A (zh) * 2019-06-13 2019-07-23 东北大学 具有速度方向控制的视觉伺服跟踪控制方法及机器人系统
CN110076772A (zh) * 2019-04-03 2019-08-02 浙江大华技术股份有限公司 一种机械臂的抓取方法及装置
CN110340887A (zh) * 2019-06-12 2019-10-18 西安交通大学 一种基于图像的加油机器人视觉引导的方法
CN111174795A (zh) * 2020-01-31 2020-05-19 武汉大学 一种基于混合卡尔曼滤波的自由漂浮目标位姿预测方法
CN111716345A (zh) * 2019-03-19 2020-09-29 深圳市优必选科技有限公司 一种运动控制方法、运动控制装置及机械臂
CN112025772A (zh) * 2020-07-28 2020-12-04 沈阳建筑大学 一种基于视觉测量的机械臂自主标定方法
CN112734823A (zh) * 2020-12-30 2021-04-30 东北大学 一种基于图像的视觉伺服的雅可比矩阵深度估计方法
CN112847334A (zh) * 2020-12-16 2021-05-28 北京无线电测量研究所 一种基于视觉伺服的机械臂目标跟踪方法

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103878770A (zh) * 2014-04-08 2014-06-25 哈尔滨工业大学 基于速度估计的空间机器人视觉时延误差补偿方法
CN108592902A (zh) * 2018-05-08 2018-09-28 清华大学 一种基于多传感器的定位设备及定位方法、系统和机械臂
CN109186601A (zh) * 2018-07-05 2019-01-11 南京理工大学 一种基于自适应无迹卡尔曼滤波的激光slam算法
CN109048890A (zh) * 2018-07-13 2018-12-21 哈尔滨工业大学（深圳） 基于机器人的协调轨迹控制方法、系统、设备及存储介质
CN109159151A (zh) * 2018-10-23 2019-01-08 北京无线电测量研究所 一种机械臂空间轨迹跟踪动态补偿方法和系统
CN111716345A (zh) * 2019-03-19 2020-09-29 深圳市优必选科技有限公司 一种运动控制方法、运动控制装置及机械臂
CN109877840A (zh) * 2019-04-02 2019-06-14 哈尔滨工程大学 一种基于相机光轴约束的双机械臂标定方法
CN110076772A (zh) * 2019-04-03 2019-08-02 浙江大华技术股份有限公司 一种机械臂的抓取方法及装置
CN110340887A (zh) * 2019-06-12 2019-10-18 西安交通大学 一种基于图像的加油机器人视觉引导的方法
CN110039542A (zh) * 2019-06-13 2019-07-23 东北大学 具有速度方向控制的视觉伺服跟踪控制方法及机器人系统
CN111174795A (zh) * 2020-01-31 2020-05-19 武汉大学 一种基于混合卡尔曼滤波的自由漂浮目标位姿预测方法
CN112025772A (zh) * 2020-07-28 2020-12-04 沈阳建筑大学 一种基于视觉测量的机械臂自主标定方法
CN112847334A (zh) * 2020-12-16 2021-05-28 北京无线电测量研究所 一种基于视觉伺服的机械臂目标跟踪方法
CN112734823A (zh) * 2020-12-30 2021-04-30 东北大学 一种基于图像的视觉伺服的雅可比矩阵深度估计方法

Also Published As

Publication number Publication date
CN113799127A (zh) 2021-12-17

Similar Documents

Publication Publication Date Title
CN111775146B (zh) 一种工业机械臂多工位作业下的视觉对准方法
CN108324373B (zh) 一种基于电磁定位系统的穿刺手术机器人精确定位实现方法
Yoshimi et al. Alignment using an uncalibrated camera system
CN114343847B (zh) 基于光学定位系统的手术机器人的手眼标定方法
CN113843792B (zh) 一种手术机器人的手眼标定方法
EP1769768A1 (en) Surgical instrument calibration
CN111546328A (zh) 基于三维视觉测量的手眼标定方法
CN113974835B (zh) 一种基于远心不动点约束的手术机器人运动控制方法
CN112603542B (zh) 手眼标定方法、装置、电子设备和存储介质
CN109978991A (zh) 基于视觉快速实现复杂构件装夹位姿误差在线测量的方法
Liu et al. High precision calibration for three-dimensional vision-guided robot system
CN116035705A (zh) 一种手术机器人整体运动学空间定位方法
CN113211445A (zh) 一种机器人参数标定方法、装置、设备及存储介质
CN115553945A (zh) 一种导航式牙科手机钻针的配准方法
CN113524201B (zh) 机械臂位姿主动调节方法、装置、机械臂和可读存储介质
CN113799127B (zh) 光学双目定位系统下六自由度机械臂无标定位姿定位方法
CN113781558B (zh) 一种姿态与位置解耦的机器人视觉寻位方法
CN112894814B (zh) 一种基于最小二乘法的机械臂dh参数辨识方法
CN112587237A (zh) 一种骨科手术机器人降低操作误差的方法
CN110900608B (zh) 基于最优测量构型选择的机器人运动学标定方法
CN114654466B (zh) 自动标定方法、装置、系统、电子设备及存储介质
CN116125906A (zh) 一种数控加工的运动规划方法、装置、设备和存储介质
CN116135169A (zh) 定位方法、装置、电子设备及计算机可读存储介质
JP3104448B2 (ja) 視覚センサ付きロボットの座標系の設定方法
CN112762822B (zh) 一种基于激光跟踪仪的机械臂校准方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant