CN117414198A - 基于光学定位系统的机器人末端位姿在线补偿方法及装置 - Google Patents

Abstract

本发明公开一种基于光学定位系统的机器人末端位姿在线补偿方法及装置，方法包括：通过光学定位系统获取机器人末端的当前位姿，并将当前位姿与目标位姿作线性插值，得到足够小变化后的插值位姿；将当前姿态和插值姿态转换为第一旋转矩阵和第二旋转矩阵，并根据第一旋转矩阵和第二旋转矩阵，获得第三旋转矩阵；根据第三旋转矩阵得到当前姿态到插值姿态的角度偏差，再得到笛卡尔空间下的位置偏差，进而得到当前位姿和插值位姿在笛卡尔空间下的位姿偏差；通过机器人雅可比矩阵将机器人末端在笛卡尔空间的位姿偏差映射到机器人的关节空间中；通过机器人关节编码器读取当前关节角，并根据当前关节角和生成关节位置指令以控制机器人的运动。

Description

基于光学定位系统的机器人末端位姿在线补偿方法及装置

技术领域

本发明涉及一种机器人领域，具体涉及一种基于光学定位系统的机器人末端位姿在线补偿方法及装置。

背景技术

与传统手术比较，精准化、微创化程度更高的骨科手术机器人具有广阔应用潜力。骨科手术机器人主要由机械臂、影像系统和计算机系统(主控台、手术导航系统及其它软硬件)三部分组成。手术导航系统使用三维位置传感器跟踪目标器官、手术工具或植入物，通过对应病人术前或术中的医学影像数据和病人术中位置等信息引导机器人末端手术器械到达病灶位置，然后完成相应的操作。手术机器人导航的本质是空间定位，通过空间定位技术实时反馈机器人末端手术器械位姿，可对机器人末端位姿进行闭环控制。基于光学定位的导航方式以精度高和使用方便等特点成为骨科手术机器人导航系统空间定位设备的主要发展方向。

机器人末端手术器械与患者的相对位姿精度决定了手术机器人系统执行手术规划的精度。受机器人几何误差、非几何误差和系统误差影响，机器人末端期望位姿与实际位姿会存在偏差。另一方面，手术环境包含机器人基坐标系、患者坐标系和图像坐标系等，这些坐标系之间的配准精度对手术机器人末端位姿精度也有较大的影响。如何基于光学定位系统反馈的手术器械位姿提高机器人末端位姿精度是手术机器人面临的关键问题之一。

机器人末端位姿误差补偿方法可分为离线补偿和在线补偿两类方法。离线补偿方法的基本原理是在机器人工作空间内测量若干不同的末端位姿，建立机器人运动学误差模型，然后辨识机器人运动学参数或者建立机器人笛卡尔空间或关节空间的误差预测模型，最后进行误差补偿。离线补偿的方法通过对机器人的运动学模型进行补偿，在一定程度上对机器人的绝对位置精度有所改进，但由于非几何误差和系统误差影响，无法将运动学模型补偿得很理想。在线补偿法利用激光跟踪仪测量机器人末端位姿并得到末端位姿误差，然后通过误差补偿算法修正机器人末端位姿，使机器人在工作过程中不断调整末端位姿逼近理想位姿。但激光跟踪仪的价格昂贵，且单个激光跟踪仪只能对末端位置进行补偿，无法补偿姿态。

发明内容

本发明目的在于提出一种基于光学定位系统的机器人末端位姿在线补偿方法及装置，以改善上述问题。

本发明通过以下技术方案实现：

一种基于光学定位系统的机器人末端位姿在线补偿方法，其包括：

重复以下步骤直至偏差小于预设的阈值：

通过光学定位系统获取机器人末端的当前位姿(^aimq_cur,^aimv_cur)，并将当前位姿(^aimq_cur,^aimv_cur)与目标位姿(^aimq_tar,^aimv_tar)作线性插值，得到足够小变化后的插值位姿(^aimq_t,^aimv_t)；

将当前姿态^aimq_cur和插值姿态^aimq_t转换为第一旋转矩阵和第二旋转矩阵并根据第一旋转矩阵/>和第二旋转矩阵/>获得当前姿态^aimq_cur变换到插值姿态^aimq_t的第三旋转矩阵/>

根据所述第三旋转矩阵得到当前姿态到插值姿态的角度偏差(Δα Δβ Δγ)，再由/>得笛卡尔空间下的位置偏差(Δx Δy Δz)，进而得到当前位姿和插值位姿在笛卡尔空间下的位姿偏差ΔX＝[Δx Δy Δz Δα Δβ Δγ]；

通过公式Δθ＝J(θ)^-1ΔX将机器人末端在笛卡尔空间的位姿偏差映射到机器人的关节空间中；其中，J(θ)是机器人雅可比矩阵，其为关节转动速度与笛卡儿空间速度/>的传动比；

通过机器人关节传感器读取当前关节角θ_cur，并根据当前关节角θ_cur和Δθ生成关节位置指令以控制机器人的运动到θ_t＝θ_cur+Δθ；

读取当前位姿(^aimq_cur,^aimv_cur)并与目标位姿(^aimq_tar,^aimv_tar)作二范数，得到当前偏差。

优选地，还包括：

将光学定位工具通过夹具固定安装在机器人的末端法兰上，根据机器人的D-H参数和夹具的尺寸，可以得到光学定位工具坐标系到机器人基座标系的变换矩阵：

通过变换矩阵得到光学定位工具在机器人基座标系的坐标；

通过光学定位系统的双目图像采集获取光学定位系统坐标系到光学定位工具坐标系的变换矩阵

通过变换矩阵得到光学定位工具在光学定位系统坐标系下的坐标。

优选地，还包括：

通过多次改变机器人末端姿态，获取机器人末端的光学定位工具在光学定位系统坐标系和机器人基座标系的坐标，通过对坐标进行SVD分解得到为常数矩阵；

通过将光学定位工具在光学定位系统下的位姿变换到机器人基座标系。

优选地，计算插值位姿的插值公式如下：

其中，q＝[q₀ q₁ q₂ q₃]是四元数，用于描述姿态；v＝[x y z]是位置向量，以上位姿均为在光学定位系统坐标系的描述。

优选地，插值系数的取值为(0,1)，且选取Sigmod函数作为插值系数：

优选地，通过机器人关节传感器读取当前关节角θ_cur＝[θ_cur1 θ_cur2 θ_cur3 θ_cur4 θ_cur5θ_cur6]^T，通过机器人逆运动学控制机器人运动到目标位姿其中/>为目标位姿在光学定位系统坐标系下的描述。

优选地，所述偏差ε＝0.01。

优选地，机器人末端在笛卡尔坐标空间的速度向量机器人在关节空间的速度向量

本发明实施例还提供了一种基于光学定位系统的机器人末端位姿在线补偿装置，其包括：

线性插值单元，用于通过光学定位系统获取机器人末端的当前位姿(^aimq_cur,^aimv_cur)，并将当前位姿(^aimq_cur,^aimv_cur)与目标位姿(^aimq_tar,^aimv_tar)作线性插值，得到足够小变化后的插值位姿(^aimq_t,^aimv_t)；

旋转单元，用于将当前姿态^aimq_cur和插值姿态^aimq_t转换为第一旋转矩阵和第二旋转矩阵/>并根据第一旋转矩阵/>和第二旋转矩阵/>获得当前姿态^aimq_cur变换到插值姿态^aimq_t的第三旋转矩阵/>

位姿偏差获取单元，用于根据所述第三旋转矩阵得到当前姿态到插值姿态的角度偏差(Δα Δβ Δγ)，再由/>得笛卡尔空间下的位置偏差(Δx Δy Δz)，进而得到当前位姿和插值位姿在笛卡尔空间下的位姿偏差ΔX＝[ΔxΔy Δz Δα Δβ Δγ]；

映射单元，用于通过公式Δθ＝J(θ)^-1ΔX将机器人末端在笛卡尔空间的位姿偏差映射到机器人的关节空间中；其中，J(θ)是机器人雅可比矩阵，其为关节转动速度与笛卡儿空间速度/>的传动比；

运动控制单元，用于通过机器人关节传感器读取当前关节角θ_cur，并根据当前关节角θ_cur和Δθ生成关节位置指令以控制机器人的运动到θ_t＝θ_cur+Δθ；

偏差计算单元，用于读取当前位姿(^aimq_cur,^aimv_cur)并与目标位姿(^aimq_tar,^aimv_tar)作二范数，得到当前偏差。

本实施例利用光学定位系统测量反馈机器人末端位姿，基于雅可比矩阵迭代的机器人末端位姿误差在线补偿，实现高效、简便的机器人末端位姿闭环控制，提高机器人末端位姿精度。本实施例无需标定机器人运动学模型参数，将机器人的几何误差和非几何误差、机器人基坐标系与光学定位仪坐标系的配准误差都归结于机器人关节误差，通过雅可比矩阵把机器人末端位姿误差映射到机器人关节空间，迭代改变机器人关节位置，使机器人末端位姿逐渐逼近目标位姿。

附图说明

图1为本发明第一实施例提供的基于光学定位系统的机器人末端位姿在线补偿方法的流程示意图。

图2为位置误差补偿仿真的示意图。

图3为姿态误差补偿仿真的示意图。

图4为本发明第二实施例提供的基于光学定位系统的机器人末端位姿在线补偿装置的结构示意图。

具体实施方式

下面结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。

请参阅图1，本发明第一实施例提供了一种基于光学定位系统的机器人末端位姿在线补偿方法，其可由手术机器人来执行，特别的，由所述手术机器人内的一个或者多个处理器来执行，以实现如下步骤：

重复以下步骤直至偏差小于预设的阈值：

S101，通过光学定位系统获取机器人末端的当前位姿(^aimq_cur,^aimv_cur)，并将当前位姿(^aimq_cur,^aimv_cur)与目标位姿(^aimq_tar,^aimv_tar)作线性插值，得到足够小变化后的插值位姿(^aimq_t,^aimv_t)。

在本实施例中，整个系统由三部分组成，即光学定位工具、手术机器人以及光学定位系统，其中，光学定位工具可通过夹具固定安装在机器人的末端法兰，光学定位工具上有四个反光标记球，四个反光标记球的相对位置固定，光学定位系统通过两个近红外摄像机采集标记球的图像，根据四个标记球的三维坐标可以得到光学定位工具的位姿。

为了实现本实施例，首先需要确定光学定位工具、手术机器人以及光学定位系统的坐标之间的转换。其中：

首先，根据机器人的D-H参数和夹具的尺寸，可以得到光学定位工具坐标系到机器人基座标系的变换矩阵

其中，这里/>可通过机器人的D-H参数获得，/>可通过夹具的尺寸获得。

然后，通过变换矩阵得到光学定位工具在机器人基座标系的坐标。

接着，通过光学定位系统的双目图像采集获取光学定位系统坐标系到光学定位工具坐标系的变换矩阵

再接着，通过变换矩阵得到光学定位工具在光学定位系统坐标系下的坐标。

再接着，通过多次改变机器人末端姿态，获取机器人末端的光学定位工具在光学定位系统坐标系和机器人基座标系的坐标，通过对坐标进行SVD分解得到

其中，因为光学定位系统坐标系和机器人基座标系是固定不变的，所以是一个常数矩阵。

最后，通过将光学定位工具在光学定位系统下的位姿变换到机器人基座标系。

S102，将当前姿态^aimq_cur和插值姿态^aimq_t转换为第一旋转矩阵和第二旋转矩阵并根据第一旋转矩阵/>和第二旋转矩阵/>获得当前姿态^aimq_cur变换到插值姿态^aimq_t的第三旋转矩阵/>

在本实施例中，可通过四元数正规化线性插值和位置线性插值得到足够小变化后的插值位姿(^aimq_t,^aimv_t)，插值公式如下：

其中，q＝[q₀ q₁ q₂ q₃]是四元数，用于描述姿态；v＝[x y z]是位置向量。以上位姿均为在光学定位系统坐标系(aim)的描述。

在本实施例中，为了获取足够小变化后的位姿，插值系数一开始取值应较小，随着当前位姿与目标位姿越来越接近的时候，插值系数也应增大，插值系数的取值为(0,1)，这里可选取Sigmod函数作为插值系数：

当然应当理解的是，在本发明的其他实施例中，也可以采用其他的函数来定义插值系数，本发明不做具体限定。

在本实施例中，根据公式：

可以将当前姿态^aimq_cur和插值姿态^aimq_t转换为第一旋转矩阵和第二旋转矩阵当前姿态^aimq_cur变换到插值姿态^aimq_t的第三旋转矩阵为/>

S103，根据所述第三旋转矩阵得到当前姿态到插值姿态的角度偏差(Δα ΔβΔγ)，再由/>得笛卡尔空间下的位置偏差(Δx Δy Δz)，进而得到当前位姿和插值位姿在笛卡尔空间下的位姿偏差ΔX＝[Δx Δy Δz Δα Δβ Δγ]。

在本实施例中，根据第三旋转矩阵可以得到当前姿态到插值姿态的角度偏差(Δα Δβ Δγ)，再由/>得笛卡尔空间下的位置偏差(Δx Δy Δz)，进而得到当前位姿和插值位姿在笛卡尔空间下的位姿偏差ΔX＝[Δx Δy Δz Δα Δβ Δγ]。

S104，通过公式Δθ＝J(θ)^-1ΔX将机器人末端在笛卡尔空间的位姿偏差映射到机器人的关节空间中；其中，J(θ)是机器人雅可比矩阵，其为关节转动速度与笛卡儿空间速度/>的传动比。

在本实施例中，机器人雅可比矩阵J(θ)是关节转动速度与笛卡儿空间速度/>的传动比，其定义式为：

其中，是机器人末端在笛卡尔坐标空间的速度向量，/>是机器人在关节空间的速度向量。当位姿变化足够小时，该式子可以近似写成下式：

ΔX＝J(θ)Δθ。

S105，通过机器人关节传感器读取当前关节角θ_cur，并根据当前关节角θ_cur和Δθ生成关节位置指令以控制机器人的运动到θ_t＝θ_cur+Δθ。

S106，读取当前位姿(^aimq_cur,^aimv_cur)并与目标位姿(^aimq_tar,^aimv_tar)作二范数，得到当前偏差。

在本实施例中，通过机器人关节传感器读取当前关节角θ_cur＝[θ_cur1 θ_cur2 θ_cur3θ_cur4 θ_cur5 θ_cur6]^T，通过机器人逆运动学控制机器人运动到目标位姿其中为目标位姿在光学定位系统坐标系下的描述。

在本实施例中，特别的，所述偏差ε＝0.01。当然，在本发明的其他实施例中，也可以选取其他的偏差值，这些方案均在本发明的保护范围之内。

为便于对本发明的理解，下面将以一些仿真过程来说明本发明的应用。

其中，插值系数Sigmod函数初值设置为x＝-5，k＝-0.5。设置五组位姿误差如表1所示：

表1

仿真数据如图2和图3所示，可以看到随着迭代次数的增加，本实施例能有效提高机器人的末端位姿精度。

综上所述，本实施例利用光学定位系统测量反馈机器人末端位姿，基于雅可比矩阵迭代的机器人末端位姿误差在线补偿，实现高效、简便的机器人末端位姿闭环控制，提高机器人末端位姿精度。本实施例无需标定机器人运动学模型参数，将机器人的几何误差和非几何误差、机器人基坐标系与光学定位仪坐标系的配准误差都归结于机器人关节误差，通过雅可比矩阵把机器人末端位姿误差映射到机器人关节空间，迭代改变机器人关节位置，使机器人末端位姿逐渐逼近目标位姿。

请参阅图4，本发明第二实施例还提供了一种基于光学定位系统的机器人末端位姿在线补偿装置，其包括：

线性插值单元210，用于通过光学定位系统获取机器人末端的当前位姿(^aimq_cur,^aimv_cur)，并将当前位姿(^aimq_cur,^aimv_cur)与目标位姿(^aimq_tar,^aimv_tar)作线性插值，得到足够小变化后的插值位姿(^aimq_t,^aimv_t)；

旋转单元220，用于将当前姿态^aimq_cur和插值姿态^aimq_t转换为第一旋转矩阵和第二旋转矩阵/>并根据第一旋转矩阵/>和第二旋转矩阵/>获得当前姿态^aimq_cur变换到插值姿态^aimq_t的第三旋转矩阵/>

位姿偏差获取单元230，用于根据所述第三旋转矩阵得到当前姿态到插值姿态的角度偏差(Δα Δβ Δγ)，再由/>得笛卡尔空间下的位置偏差(Δx Δy Δz)，进而得到当前位姿和插值位姿在笛卡尔空间下的位姿偏差ΔX＝[Δx Δy Δz Δα Δβ Δγ]；

映射单元240，用于通过公式Δθ＝J(θ)^-1ΔX将机器人末端在笛卡尔空间的位姿偏差映射到机器人的关节空间中；其中，J(θ)是机器人雅可比矩阵，其为关节转动速度与笛卡儿空间速度/>的传动比；

运动控制单元250，用于通过机器人关节传感器读取当前关节角θ_cur，并根据当前关节角θ_cur和Δθ生成关节位置指令以控制机器人的运动到θ_t＝θ_cur+Δθ；

偏差计算单元260，用于读取当前位姿(^aimq_cur,^aimv_cur)并与目标位姿(^aimq_tar,^aimv_tar)作二范数，得到当前偏差。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、同等替换、改进等，均应包含在本发明的保护范围之类。

Claims (9)

1.一种基于光学定位系统的机器人末端位姿在线补偿方法，其特征在于，包括：

重复以下步骤直至偏差小于预设的阈值：

通过光学定位系统获取机器人末端的当前位姿(^aimq_cur,^aimv_cur)，并将当前位姿(^aimq_cur,^aimv_cur)与目标位姿(^aimq_tar,^aimv_tar)作线性插值，得到足够小变化后的插值位姿(^aimq_t,^aimv_t)；

将当前姿态^aimq_cur和插值姿态^aimq_t转换为第一旋转矩阵和第二旋转矩阵/>并根据第一旋转矩阵/>和第二旋转矩阵/>获得当前姿态^aimq_cur变换到插值姿态^aimq_t的第三旋转矩阵/>

根据所述第三旋转矩阵得到当前姿态到插值姿态的角度偏差(Δα Δβ Δγ)，再由/>得笛卡尔空间下的位置偏差(Δx Δy Δz)，进而得到当前位姿和插值位姿在笛卡尔空间下的位姿偏差ΔX＝[Δx Δy Δz Δα Δβ Δγ]；

通过公式Δθ＝J(θ)^-1ΔX将机器人末端在笛卡尔空间的位姿偏差映射到机器人的关节空间中；其中，J(θ)是机器人雅可比矩阵，其为关节转动速度与笛卡儿空间速度/>的传动比；

通过机器人关节传感器读取当前关节角θ_cur，并根据当前关节角θ_cur和Δθ生成关节位置指令以控制机器人的运动到θ_t＝θ_cur+Δθ；

读取当前位姿(^aimq_cur,^aimv_cur)并与目标位姿(^aimq_tar,^aimv_tar)作二范数，得到当前偏差。

2.根据权利要求1所述的基于光学定位系统的机器人末端位姿在线补偿方法，其特征在于，还包括：

将光学定位工具通过夹具固定安装在机器人的末端法兰上，根据机器人的D-H参数和夹具的尺寸，可以得到光学定位工具坐标系到机器人基座标系的变换矩阵：

通过变换矩阵得到光学定位工具在机器人基座标系的坐标；

通过光学定位系统的双目图像采集获取光学定位系统坐标系到光学定位工具坐标系的变换矩阵

通过变换矩阵得到光学定位工具在光学定位系统坐标系下的坐标。

3.根据权利要求2所述的基于光学定位系统的机器人末端位姿在线补偿方法，其特征在于，还包括：

通过多次改变机器人末端姿态，获取机器人末端的光学定位工具在光学定位系统坐标系和机器人基座标系的坐标，通过对坐标进行SVD分解得到 为常数矩阵；

通过将光学定位工具在光学定位系统下的位姿变换到机器人基座标系。

4.根据权利要求3所述的基于光学定位系统的机器人末端位姿在线补偿方法，其特征在于，计算插值位姿的插值公式如下：

其中，q＝[q₀ q₁ q₂ q₃]是四元数，用于描述姿态；v＝[x y z]是位置向量，以上位姿均为在光学定位系统坐标系的描述。

5.根据权利要求4所述的基于光学定位系统的机器人末端位姿在线补偿方法，其特征在于，插值系数的取值为(0,1)，且选取Sigmod函数作为插值系数：

6.根据权利要求1所述的基于光学定位系统的机器人末端位姿在线补偿方法，其特征在于，

通过机器人关节传感器读取当前关节角θ_cur＝[θ_cur1 θ_cur2 θ_cur3 θ_cur4 θ_cur5 θ_cur6]^T，通过机器人逆运动学控制机器人运动到目标位姿其中/>为目标位姿在光学定位系统坐标系下的描述。

7.根据权利要求1所述的基于光学定位系统的机器人末端位姿在线补偿方法，其特征在于，所述偏差ε＝0.01。

8.根据权利要求1所述的基于光学定位系统的机器人末端位姿在线补偿方法，其特征在于，其中，机器人末端在笛卡尔坐标空间的速度向量机器人在关节空间的速度向量/>

9.一种基于光学定位系统的机器人末端位姿在线补偿装置，其特征在于，包括：

线性插值单元，用于通过光学定位系统获取机器人的当前位姿(^aimq_cur,^aimv_cur)，并将当前位姿(^aimq_cur,^aimv_cur)与目标位姿(^aimq_tar,^aimv_tar)作线性插值，得到足够小变化后的插值位姿(^aimq_t,^aimv_t)；

旋转单元，用于将当前姿态^aimq_cur和插值姿态^aimq_t转换为第一旋转矩阵和第二旋转矩阵/>并根据第一旋转矩阵/>和第二旋转矩阵/>获得当前姿态^aimq_cur变换到插值姿态^aimq_t的第三旋转矩阵/>

位姿偏差获取单元，用于根据所述第三旋转矩阵得到当前姿态到插值姿态的角度偏差(Δα Δβ Δγ)，再由/>得笛卡尔空间下的位置偏差(Δx Δy Δz)，进而得到当前位姿和插值位姿在笛卡尔空间下的位姿偏差ΔX＝[Δx Δy Δz Δα Δβ Δγ]；

映射单元，用于通过公式Δθ＝J(θ)^-1ΔX将机器人末端在笛卡尔空间的位姿偏差映射到机器人的关节空间中；其中，J(θ)是机器人雅可比矩阵，其为关节转动速度与笛卡儿空间速度/>的传动比；

运动控制单元，用于通过机器人关节传感器读取当前关节角θ_cur，并根据当前关节角θ_cur和Δθ生成关节位置指令以控制机器人的运动到θ_t＝θ_cur+Δθ；

偏差计算单元，用于读取当前位姿(^aimq_cur,^aimv_cur)并与目标位姿(^aimq_tar,^aimv_tar)作二范数，得到当前偏差。