CN112060085B - 一种基于视触多尺度定位的机器人操作位姿控制方法 - Google Patents

Abstract

本发明提出了一种基于视触多尺度定位的机器人操作位姿控制方法，本发明首先通过视觉传感器测量获取操作环境和待操作目标物体的状态，根据采集图像信息并考虑安全距离，初步得到待操作目标物体的估计位姿和机械臂系统的估计目标位姿；然后，通过触觉传感器测量获取机器臂系统末端与待操作目标物体的相对位姿偏差；最后，根据机器人操作精度要求，通过迭代控制和调整机械臂系统的估计目标位姿。本发明通过视触多尺度定位方法，利用触觉感知增加局部信息，对机器人操作位姿进行精准控制；本发明提出了基于触觉传感器的位姿误差测量方法，在六维误差空间内精确测量机械臂系统末端与待操作目标物体的相对位姿偏差，提高了机器人操作位姿误差测量精度。

Description

一种基于视触多尺度定位的机器人操作位姿控制方法

技术领域

本发明涉及一种基于视触多尺度定位的机器人操作位姿控制方法，属于机器人视触觉感知和机器人操作的技术领域。

背景技术

在机器人操作平台中，视觉和触觉感知作为机器人的眼睛和皮肤，是机器人与外界环境交互时的重要感觉功能，是智能机器人操作的关键技术之一。视觉信息能够为机器人提供周围环境信息，存在物体的数量、分布信息，以及各个物体的轮廓、尺寸等信息；触觉信息能够为机器人提供物体表面特性信息，机器人与物体的接触状态信息，以及更加精确的物体方位信息等。

目前在机器人领域中，多利用视觉测量进行目标识别与分类。视觉测量系统能够提供机器人操作平台的全局信息。但在实际的视觉测量中，不可避免地会存在受光线影响、背景杂乱干扰、以及物体之间重叠、遮挡等情况，使得机器人很难对目标物体状态有充分的分析和判断；另外，在视觉测量中对深度的估计存在误差较大等问题。触觉传感器在机器人操作中，多与机械手结合，为机器人操作提供目标物体的细节信息。但在实际的触觉测量中，存在缺乏对操作环境和目标物体的整体认识等问题。这些情况都会严重影响机器人操作过程中的准确性和安全性。

如现有的一种基于GelSlim触觉传感器(Tactile-Based Insertion for DenseBox-Packing,IROS 2019:7953-7960.)，对机器人收纳中的插入摆放过程进行了误差预测和控制。其具体实施方案是将触觉传感器安装于二指手指腹处，利用采集到的图像序列训练两个神经网络(Alexnet+LSTM)，用DirectionNN估计误差方向对应的误差象限，即误差组合分类；用MagnitudeNN预测误差值的大小。但该方法中考虑的误差空间维度只有二维，即x方向的位移偏差和绕z轴的转角偏差，无法满足实际应用中在六维误差空间的定位需求。

综上，为了使机器人操作具有更好的精准度、灵巧度和稳定性，传统的基于视觉和触觉的测量方法已无法满足机器人操作需求。

发明内容

本发明的目的是为了克服已有技术的不足之处，提出一种基于视触多尺度定位的机器人操作位姿控制方法。本发明基于视觉和触觉信息，对机器人操作过程中的目标位姿状态进行有效估计，实现操作机器人在笛卡尔空间中的六自由度精准定位，可为机器人精准操作任务提供理论指导和系统支撑。

为了实现上述目的，本发明采用如下技术方案：

本发明提出的一种基于视触多尺度定位的机器人操作位姿控制方法，其特征在于，该方法涉及的机器人操作平台包括机械臂系统、视觉传感器、触觉传感器和支撑平台；所述机械臂系统包括模块化机械臂和机械手，由模块化机械臂带动机械手抓取放置于支撑平台上的待操作目标物体；视觉传感器安装于机械臂系统的斜上方；触觉传感器安装于机械手的手掌处；所述机器人操作位姿控制方法包括以下步骤：

1)利用视觉传感器对待操作目标物体进行粗定位

1.1)机器人操作平台根据操作任务，通过视觉传感器进行数据采集，以获取操作环境和待操作目标物体的状态；

1.2)对待操作目标物体进行目标检测，并对该待操作目标物体的位姿进行初步估计，具体包括以下步骤：

1.2.1)采用目标检测方法对待操作目标物体进行识别；

1.2.2)基于视觉的待操作目标物体位姿估计

将待操作目标物体顶部表面的中心记为O₁点，利用视觉传感器采集待操作目标物体的顶部表面轮廓信息，计算得出O₁点在视觉坐标系O_camera内的三维坐标(x₁，y₁，z₁)，以此作为待操作目标物体的位置；所述视觉坐标系O_camera的原点建立在视觉传感器主摄像头的中心，以摄像头的视野截面为视觉坐标系O_camera的xy平面，以摄像头的轴线方向为视觉坐标系O_camera的z轴；

提取待操作目标物体顶部各点的深度值U(k)，若待操作目标物体顶部各点的深度差值在第一设定阈值内，则待操作目标物体顶部平面的单位法向量R₁＝(α₁，β₁，γ₁)＝(0，0，1)；若待操作目标物体顶部各点的深度值差异超出第一设定阈值，则计算待操作目标物体顶部深度值相差最大的两个点或两条边的深度差值ΔU(k)、距离D以及该距离D与视觉坐标系O_camerax轴的夹角ρ，利用以下公式计算待操作目标物体顶部平面分别绕视觉坐标系O_camera的x,y,z轴的转角α,β,γ以及待操作目标物体顶部平面的单位法向量R₁＝(α₁，β₁，γ₁)：

其中，α₁，β₁，γ₁分别为操作目标物体顶部平面单位法向量R₁在视觉坐标系O_camera的x,y,z轴方向上的分量；

将计算得到待操作目标物体的位置和姿态作为待操作目标物体的估计位姿，记为T₁＝(x₁，y₁，z₁，α₁，β₁，γ₁)。

1.2.3)基于机械手到待操作目标物体的安全距离，设置机械臂系统的第一估计目标位姿T₂为：

其中，Δd为设定的安全距离，即沿着待操作目标物体顶部平面的法向向外的距离；x₂，y₂，z₂为机械臂系统的估计目标位置在视觉坐标系O_camera下沿x,y,z轴的三维坐标；α₂，β₂，γ₂为机械臂系统的估计目标姿态在视觉坐标系O_camera下绕x，y，z轴的转角；

1.2.4)机械臂系统运动至机械臂系统的第一估计目标位姿T₂；

2)利用触觉传感器对待操作目标物体进行精定位

2.1)设控制迭代变量为i，并初始化为1；

2.2)机械臂系统控制机械手末端姿态保持估计目标姿态(α₂，β₂，γ₂)不变，并沿着机械手的末端轴线方向，向靠近待操作目标物体的方向移动，直至机械手的手掌平面按压在待操作目标物体的表面上，并且利用触觉传感器测得的按压力达到第二设定阈值，此时机械手末端在触觉坐标系O_touch下的位移为(Δz_i+Δd)，计算机械手末端位姿

与待操作目标物体的实际位姿在触觉坐标系O_touch下z轴方向的偏差为Δz_i；

2.3)根据触觉传感器采集到的数据对待操作目标物体实际位姿与机械手末端位姿

之间的偏差进行估计，具体包括以下步骤：

2.3.1)设触觉传感器平面中心为P点，P点在触觉坐标系O_touch中的位置为(x_P，y_P)；根据触觉传感器采集的数据，提取待操作目标物体顶部平面的拟合轮廓，并计算该拟合轮廓的中心C点在触觉坐标系O_touch中的位置(x_Ci，y_Ci)，即计算P点到C点的位移Δxy_i＝(Δx_i，Δy_i)＝(x_Ci-x_P，y_Ci-y_P)；计算待操作目标物体坐标系O_object的x轴与触觉坐标系O_touch的x轴的锐角夹角，将其记为Δγ_i，即待操作目标物体顶部平面绕触觉坐标系O_touchz轴的姿态调整偏角；所述触觉坐标系O_touch的原点建立在机械臂系统末端的机械手手掌中心，即触觉传感器的表面中心，以触觉传感器的表面为触觉坐标系O_touch的xy平面，以触觉传感器摄像头的轴线方向为触觉坐标系O_touch的z轴；所述待操作目标物体坐标系O_object的原点建立在待操作目标物体顶部平面的中心，以操作桌面所在的平面为待操作目标物体坐标系O_object的xy平面，以垂直于操作桌面的方向为待操作目标物体坐标系O_object的z轴方向；

2.3.2)沿着触觉传感器表面法向的方向按压待操作目标物体顶部，产生触觉信号，根据该触觉信号对待操作目标物体顶部平面的拟合轮廓进行检测，比较拟合轮廓上各点在触觉传感器表面产生的位移变化，找到位移最大的位置P_1i点和位移最小的位置P_2i点；以线段CP_1i和线段CP_2i为边组成的锐角或钝角记为角θ_i，将角θ_i的角平分线记为L_i，将角θ_i的角平分线L_i与触觉坐标系O_touch的x轴的锐角夹角记为θ_Li；以角θ_i的角平分线L_i作为(α，β)姿态调整平面的法线，根据P_1i点和P_2i点的位移差ΔB_i，估算待操作目标物体顶部平面的法向与机械手末端姿态(α₂，β₂，γ₂)在(α，β)姿态调整平面的相对姿态偏角θ_xyi的大小；根据角θ_i的角平分线L_i与触觉坐标系O_touch的x轴的夹角θ_Li，将相对姿态偏角θ_xyi分配到绕触觉坐标系O_touch的x轴和y轴的转动上，由此计算出待操作目标物体顶部平面的法向与机械手末端姿态(α₂，β₂，γ₂)的相对姿态偏角Δα_i和Δβ_i，计算公式分别如下：

θ_xyi＝f(ΔB_i)

Δα_i＝θ_xyicosθ_Li

Δβ_i＝θ_xyisinθ_Li

其中，f(·)表示P_1i点和P_2i点的位移差ΔB_i与待操作目标物体顶部平面的法向与机械手末端姿态(α₂，β₂，γ₂)在(α，β)姿态调整平面的相对姿态偏角θ_xyi之间的映射函数，表征位移差与相对姿态偏角之间的函数关系；

由计算出待操作目标物体实际位姿与机械手末端位姿

之间的相对偏差，记为ΔT_i＝(Δx_i，Δy_i，Δz_i，Δα_i，Δβ_i，Δγ_i)；

3)计算并调整机械臂系统的第一估计目标位姿

3.1)根据步骤1.2.3)得到的机械臂系统的第一估计目标位姿T₂和步骤2.3.2)得到的待操作目标物体位姿与机械手末端位姿之间的相对偏差ΔT_i计算机械臂系统的第二估计目标位姿T_di，计算公式如下：

其中，n为当前姿态调整控制迭代的总次数；

机械臂系统根据其第二估计目标位姿T_di运动到指定位姿，完成第i次机器人操作位姿调整；

3.3)判断ΔT_i是否小于根据机器人操作精度要求设定的位姿误差阈值ε，若ΔT_i<ε，则机器人操作位姿控制结束；若ΔT_i≥ε，则执行步骤3.4)；

3.4)令i＝i+1，并返回步骤2.2)，再次利用触觉传感器对待操作目标物体进行精定位，计算待操作目标物体位姿与机械手末端位姿的偏差，直至ΔT_i<ε，机器人操作位姿控制结束。

本发明具有以下特点及有益效果：

本发明方法主要分为基于视觉传感器的操作目标物体粗定位、基于触觉传感器的操作目标物体精定位和机器人操作位姿计算三个阶段，首先通过视觉传感器测量获取操作环境和待操作目标物体的状态，根据采集图像信息并考虑安全距离，初步得到待操作目标物体的估计位姿和机械臂系统的估计目标位姿；然后，通过触觉传感器测量获取机器臂系统末端与待操作目标物体的相对位姿偏差；最后，根据机器人操作精度要求，通过迭代控制和调整机械臂系统的估计目标位姿。

本发明通过视触多尺度定位方法，对机器人操作位姿进行精准控制，弥补了单一视觉测量存在的受光线影响、背景杂乱干扰、以及物体之间重叠、遮挡等问题，利用触觉感知增加局部信息，提高了对待操作目标物体的位姿估计精度。同时，本发明提出了基于触觉传感器的位姿误差测量方法，在六维误差空间内精确测量机械臂系统末端与待操作目标物体的相对位姿偏差，提高了机器人操作位姿误差测量精度，为机器人精准操作任务的实现提供有力的理论指导和方法支撑，有助于推进智能机器人操作和应用的不断发展。

附图说明

图1为本发明的一种基于视触多尺度定位的机器人操作位姿控制方法涉及的机器人操作平台示意图。

图2为本发明的一种基于视触多尺度定位的机器人操作位姿控制方法的流程图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施方式仅仅用以解释本发明，并不限定本发明的保护范围。

为了更好地理解本发明，以下详细阐述一个本发明的一种基于视触多尺度定位的机器人操作位姿控制方法的应用实例。

本发明提出的一种基于视触多尺度定位的机器人操作位姿控制方法所涉及的机器人操作平台，参见图1，主要包括机械臂系统1、视觉传感器2、触觉传感器3、主控计算机4和支撑平台5。其中，机械臂系统1包括模块化机械臂和机械手两部分，将机械手安装于机械臂末端，模块化机械臂另一端安装于支撑平台5上，由模块化机械臂带动机械手抓取放置于支撑平台5上的待操作目标物体6。视觉传感器2安装于机械臂系统1的斜上方，保证其视野范围宽广，用于进行操作环境和待操作目标物体6的检测。触觉传感器3安装于机械手的手掌处，本实施例的触觉传感器3采用公开号为CN106052914A的一种基于视觉的触觉测量传感器，对于其他类型的触觉传感器，本发明方法同样适用。主控计算机4分别与机械臂系统1、视觉传感器2和触觉传感器3相连接，实现对机械手的操作位姿控制。

参见图1，机器人操作平台共涉及五个坐标系，分别为机器人操作平台的基础坐标系O_base、机械臂系统坐标系O_robot、视觉坐标系O_camera、触觉坐标系O_touch和待操作目标物体坐标系O_object。其中，机器人操作平台的基础坐标系O_base的原点可选定支撑平台5的操作桌面上的任一点，以操作桌面所在的平面为基础坐标系O_base的xy平面，以垂直于操作桌面的方向为基础坐标系O_base的z轴方向。机械臂系统坐标系O_robot的原点建立在机械臂系统安装平面上的一点，机械臂系统坐标系O_robot的各坐标轴方向与机器人操作平台的基础坐标系O_base的各坐标轴方向一致。视觉坐标系O_camera的原点建立在视觉传感器主摄像头的中心，以摄像头的视野截面为视觉坐标系O_camera的xy平面，以摄像头的轴线方向为视觉坐标系O_camera的z轴。触觉坐标系O_touch的原点建立在机械臂系统末端的机械手手掌中心，即触觉传感器的表面中心，该点也是微视觉摄像头轴线上的一点，以触觉传感器的表面为触觉坐标系O_touch的xy平面，以微视觉摄像头的轴线方向为触觉坐标系O_touch的z轴。待操作目标物体坐标系O_object的原点建立在待操作目标物体顶部平面的中心，待操作目标物体坐标系O_object的各坐标轴方向与机器人操作平台的基础坐标系O_base的各坐标轴方向一致。机械臂系统坐标系O_robot和视觉坐标系O_camera与基础坐标系O_base的相对位姿关系，可分别表示为T_robot和T_camera；触觉坐标系O_touch与机械臂系统坐标系O_robot的相对位姿关系，可表示为T_touch。

本发明实施例的基于视触多尺度定位的机器人操作位姿控制方法，包括以下步骤：主要分为基于视觉传感器的操作目标物体粗定位、基于触觉传感器的操作目标物体精定位和机器人操作位姿计算三个阶段。

1)利用视觉传感器对待操作目标物体进行粗定位：

1.1)机器人操作平台根据操作任务，通过视觉传感器进行数据采集，以一定速率向主控计算机传送图像和深度信息，以获取操作环境和待操作目标物体的状态；

1.2)主控计算机对采集到的数据进行处理，首先通过对待操作目标物体进行目标检测，并对该待操作目标物体的位姿进行初步估计，具体包括以下步骤：

1.2.1)采用背景相减法、光流计算法和帧差法等目标检测方法实现识别待操作目标物体。本实施例采用基于深度值的背景相减法，计算公式为：

S(k)＝|N(k)-B(k)|

其中，B(k)为背景图像上k点的深度值，N(k)为当前图像上k点的深度值，S(k)为当前图像与背景图像上k点的深度值差值，δ为设定阈值，通常根据待操作目标物体的尺寸和视觉传感器的深度值测量精度而确定，当S(k)≥δ时，G(k)＝1时，表示k点为待操作目标物体上的点；当S(k)<δ时，G(k)＝0，表示k点为背景图像上的点。

1.2.2)基于视觉的待操作目标物体位姿估计

将待操作目标物体顶部表面的中心记为O₁点，利用位于机械臂系统斜上方的视觉传感器采集待操作目标物体的顶部表面轮廓信息，计算得出O₁点在视觉坐标系O_camera内的三维坐标(x₁，y₁，z₁)，以此作为待操作目标物体的位置。

提取待操作目标物体顶部各点的深度值U(k)，若待操作目标物体顶部平面平行于操作桌面，则待操作目标物体顶部各点的深度值大致相同，深度差值在第一设定阈值内，则待操作目标物体顶部平面的单位法向量R₁＝(α₁，β₁，γ₁)＝(0，0，1)；若待操作目标物体顶部各点的深度值差异超出第一设定阈值，则计算待操作目标物体顶部深度值相差最大的两个点或两条边的深度差值ΔU(k)、距离D以及该距离D与视觉坐标系O_camerax轴的夹角ρ，利用以下公式计算待操作目标物体顶部平面分别绕视觉坐标系O_camera的x，y，z轴的转角α，β，γ以及待操作目标物体顶部平面的单位法向量R₁＝(α₁，β₁，γ₁)：

其中，α₁，β₁，γ₁分别为操作目标物体顶部平面单位法向量R₁在视觉坐标系O_camera的x，y，z轴方向上的分量。

将计算得到待操作目标物体的位置和姿态作为待操作目标物体的估计位姿，记为T₁＝(x₁，y₁，z₁，α₁，β₁，γ₁)。

1.2.3)考虑到机械手到待操作目标物体的安全距离，设置机械臂系统的第一估计目标位姿T₂为：

其中，Δd为设定的安全距离，即沿着待操作目标物体顶部平面的法向向外的距离，通常根据经验设定，一般为10～15mm。x₂，y₂，z₂为机械臂系统的估计目标位置在视觉坐标系O_camera下沿x，y，z轴的三维坐标。α₂，β₂，γ₂为机械臂系统的估计目标姿态在视觉坐标系O_camera下绕x，y，z轴的转角。

1.2.4)主控计算机根据T_camera和T_robot进行坐标系转换，将机械臂系统的第一估计目标位姿T₂从视觉坐标系O_camera转换至机械臂系统坐标系O_robot下，并发送给机械臂系统的运动控制系统，机械臂系统接收控制指令并根据T_touch确定各关节角位置，控制机械手末端运动到该第一估计目标位姿T₂。

至此，机器人操作平台完成了基于视觉传感器的待操作目标物体粗定位。

2)利用触觉传感器对待操作目标物体进行精定位。具体步骤如下：

2.1)设控制迭代变量为i，并初始化为1。

2.2)利用机器人操作平台的触觉传感器进行数据采集，不同触觉传感器的数据形式不同，在本实施例中此处为微视觉图像数据。此时，机械手末端位姿

为T₂＝(x₂，y₂，z₂，α₂，β₂，γ₂)，机械臂系统控制机械手末端姿态保持估计目标姿态(α₂，β₂，γ₂)不变，并沿着机械手的末端轴线方向，向靠近待操作目标物体的方向移动，直至机械手的手掌平面按压在待操作目标物体的表面上，并且按压力达到第二设定阈值(该第二设定阈值范围通常根据待操作目标物体的材料属性及抗压性而确定)，此时机械手末端在触觉坐标系O_touch下的位移为(Δz_i+Δd)，计算机械手末端位姿

与待操作目标物体的实际位姿在触觉坐标系O_touch下z轴方向的偏差为Δz_i，其中，Δd为在步骤1.2.3)中设定的安全距离。由于待操作目标物体受阻碍和摩擦力的作用，触觉传感器的表面会出现变形，本实施例中，表现为基于视觉的触觉测量传感器中标记点的位置会发生移动，同时受挤压处到触觉传感器内部光源的距离也会发生变化，这些变化会由触觉传感器内部的微摄像头拍摄成图像信号，并以一定速率传送图像信息至主控计算机。

2.3)主控计算机对触觉传感器采集到的数据进行处理(如去噪等)，并对待操作目标物体实际位姿与机械手末端位姿

之间的偏差进行估计，具体包括以下步骤：

2.3.1)设触觉传感器平面中心为P点，P点在触觉坐标系O_touch中的位置为(x_P，y_P)。根据触觉传感器采集的数据，提取待操作目标物体顶部平面的拟合轮廓，并计算该拟合轮廓的中心C点在触觉坐标系O_touch中的位置(x_Ci，y_Ci)，即计算P点到C点的位移Δxy_i＝(Δx_i，Δy_i)＝(x_Ci-x_P，y_Ci-y_P)。进一步地，计算待操作目标物体坐标系O_object的x轴与触觉坐标系O_touch的x轴的锐角夹角，将其记为Δγ_i，即待操作目标物体顶部平面绕触觉坐标系O_touchz轴的姿态调整偏角。

2.3.2)沿着触觉传感器表面法向的方向按压待操作目标物体顶部，随着触觉传感器表面产生形变，在按压点处会产生触觉信号。本实施例中，随着按压位移不同，挤压点到触觉传感器内部光源的距离不同，在触觉传感器微视觉视野中的亮度就不同。按压产生的形变/位移越大，该点在微视觉视野中的亮度就越大。由此，根据触觉信号对待操作目标物体顶部平面的拟合轮廓进行检测，并以拟合轮廓中心为中心，顺时针或逆时针遍历，比较拟合轮廓上各点在触觉传感器表面产生的位移变化，找到位移最大的位置P_1i点和位移最小的位置P_2i点。以线段CP_1i和线段CP_2i为边组成的锐角或钝角记为角θ_i，将角θ_i的角平分线记为L_i，将角θ_i的角平分线L_i与触觉坐标系O_touch的x轴的锐角夹角记为θ_Li。以角θ_i的角平分线L_i作为(α，β)姿态调整平面(即姿态调整转角α和β组成的平面)的法线，根据P_1i点和P_2i点的位移差ΔB_i，估算待操作目标物体顶部平面的法向与机械手末端姿态(α₂，β₂，γ₂)在(α，β)姿态调整平面的相对姿态偏角θ_xyi的大小。根据角θ_i的角平分线L_i与触觉坐标系O_touch的x轴的夹角θ_Li，将相对姿态偏角θ_xyi分配到绕触觉坐标系O_touch的x轴和y轴的转动上，由此计算出待操作目标物体顶部平面的法向与机械手末端姿态(α₂，β₂，γ₂)的相对姿态偏角Δα_i和Δβ_i。计算公式分别如下：

θ_xyi＝f(ΔB_i)

Δα_i＝θ_xyicosθ_Li

Δβ_i＝θ_xyisinθ_Li

其中，f(·)表示ΔB_i与θ_xyi之间的映射函数，可采用神经网络建模确定。

由此，可计算出待操作目标物体实际位姿与机械手末端位姿

之间的相对偏差，记为ΔT_i＝(Δx_i，Δy_i，Δz_i，Δα_i，Δβ_i，Δγ_i)，Δx_i，Δy_i，Δz_i，Δα_i，Δβ_i，Δγ_i分别是待操作目标物体实际位姿与机械手末端位姿

沿触觉坐标系O_touch的x，y，z轴方向上的位移偏差和绕触觉坐标系O_touch的x，y，z轴的转角偏差。

至此，机器人操作平台完成了基于触觉传感器的操作目标物体精定位。

3)为了保证机械手末端位姿与待操作目标物体位姿一致，计算并调整机械臂系统的第一估计目标位姿。具体步骤如下：

3.1)根据步骤1.2.3)得到的机械臂系统的第一估计目标位姿T₂和步骤2.3.2)得到的待操作目标物体位姿与机械手末端位姿之间的相对偏差ΔT_i计算机械臂系统的第二估计目标位姿T_di，计算公式如下：

其中，n为当前姿态调整控制迭代的总次数。

主控计算机根据T_camera和T_robot进行坐标系转换，将计算得到的机械臂系统的第二估计目标位姿T_di从视觉坐标系O_camera转换至机械臂系统坐标系O_robot下，并发送到机械臂系统的运动控制系统，机械臂系统接收控制指令并根据T_touch确定各关节角位置，控制机械手末端运动到指定位姿，完成第i次机器人操作位姿调整。

3.3)判断ΔT_i是否小于根据机器人操作精度要求设定的位姿误差阈值ε，若ΔT_i<ε，则机器人操作位姿控制结束；若ΔT_i≥ε，则执行步骤3.4)。

3.4)令i＝i+1，并返回步骤2.2)，再次利用触觉传感器对待操作目标物体进行精定位，计算待操作目标物体位姿与机械手末端位姿的偏差，直至ΔT_i<ε，机器人操作位姿控制结束。

经过上述三个阶段，即可实现基于视触多尺度定位的机器人操作位姿测量与调整，为机器人操作平台实现精准操作任务奠定基础。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (2)

1.一种基于视触多尺度定位的机器人操作位姿控制方法，其特征在于，该方法涉及的机器人操作平台包括机械臂系统、视觉传感器、触觉传感器和支撑平台；所述机械臂系统包括模块化机械臂和机械手，由模块化机械臂带动机械手抓取放置于支撑平台上的待操作目标物体；视觉传感器安装于机械臂系统的斜上方；触觉传感器安装于机械手的手掌处；所述机器人操作位姿控制方法包括以下步骤：

1)利用视觉传感器对待操作目标物体进行粗定位

1.1)机器人操作平台根据操作任务，通过视觉传感器进行数据采集，以获取操作环境和待操作目标物体的状态；

1.2)对待操作目标物体进行目标检测，并对该待操作目标物体的位姿进行初步估计，具体包括以下步骤：

1.2.1)采用目标检测方法对待操作目标物体进行识别；

1.2.2)基于视觉的待操作目标物体位姿估计

将待操作目标物体顶部表面的中心记为O₁点，利用视觉传感器采集待操作目标物体的顶部表面轮廓信息，计算得出O₁点在视觉坐标系O_camera内的三维坐标(x₁，y₁，z₁)，以此作为待操作目标物体的位置；所述视觉坐标系O_camera的原点建立在视觉传感器主摄像头的中心，以摄像头的视野截面为视觉坐标系O_camera的xy平面，以摄像头的轴线方向为视觉坐标系O_camera的z轴；

提取待操作目标物体顶部各点的深度值U(k)，若待操作目标物体顶部各点的深度差值在第一设定阈值内，则待操作目标物体顶部平面的单位法向量R₁＝(α₁，β₁，γ₁)＝(0，0，1)；若待操作目标物体顶部各点的深度值差异超出第一设定阈值，则计算待操作目标物体顶部深度值相差最大的两个点或两条边的深度差值ΔU(k)、距离D以及该距离D与视觉坐标系O_camerax轴的夹角ρ，利用以下公式计算待操作目标物体顶部平面分别绕视觉坐标系O_camera的x，y，z轴的转角α，β，γ以及待操作目标物体顶部平面的单位法向量R₁＝(α₁，β₁，γ₁)：

其中，α₁，β₁，γ₁分别为操作目标物体顶部平面单位法向量R₁在视觉坐标系O_camera的x，y，z轴方向上的分量；

将计算得到待操作目标物体的位置和姿态作为待操作目标物体的估计位姿，记为T₁＝(x₁，y₁，z₁，α₁，β₁，γ₁)；

1.2.3)基于机械手到待操作目标物体的安全距离，设置机械臂系统的第一估计目标位姿T₂为：

其中，Δd为设定的安全距离，即沿着待操作目标物体顶部平面的法向向外的距离；x₂，y₂，z₂为机械臂系统的估计目标位置在视觉坐标系O_camera下沿x，y，z轴的三维坐标；α₂，β₂，γ₂为机械臂系统的估计目标姿态在视觉坐标系O_camera下绕x，y，z轴的转角；

1.2.4)机械臂系统运动至机械臂系统的第一估计目标位姿T₂；

2)利用触觉传感器对待操作目标物体进行精定位

2.1)设控制迭代变量为i，并初始化为1；

2.2)机械臂系统控制机械手末端姿态保持估计目标姿态(α₂，β₂，γ₂)不变，并沿着机械手的末端轴线方向，向靠近待操作目标物体的方向移动，直至机械手的手掌平面按压在待操作目标物体的表面上，并且利用触觉传感器测得的按压力达到第二设定阈值，此时机械手末端在触觉坐标系O_touch下的位移为(Δz_i+Δd)，计算机械手末端位姿

与待操作目标物体的实际位姿在触觉坐标系O_touch下z轴方向的偏差为Δz_i；

2.3)根据触觉传感器采集到的数据对待操作目标物体实际位姿与机械手末端位姿

之间的偏差进行估计，具体包括以下步骤：

2.3.1)设触觉传感器平面中心为P点，P点在触觉坐标系O_touch中的位置为(x_P，y_P)；根据触觉传感器采集的数据，提取待操作目标物体顶部平面的拟合轮廓，并计算该拟合轮廓的中心C点在触觉坐标系O_touch中的位置(x_Ci，y_Ci)，即计算P点到C点的位移Δxy_i＝(Δx_i，Δy_i)＝(x_Ci-x_P，y_Ci-y_P)；计算待操作目标物体坐标系O_object的x轴与触觉坐标系O_touch的x轴的锐角夹角，将其记为Δγ_i，即待操作目标物体顶部平面绕触觉坐标系O_touchz轴的姿态调整偏角；所述触觉坐标系O_touch的原点建立在机械臂系统末端的机械手手掌中心，即触觉传感器的表面中心，以触觉传感器的表面为触觉坐标系O_touch的xy平面，以触觉传感器摄像头的轴线方向为触觉坐标系O_touch的z轴；所述待操作目标物体坐标系O_object的原点建立在待操作目标物体顶部平面的中心，以操作桌面所在的平面为待操作目标物体坐标系O_object的xy平面，以垂直于操作桌面的方向为待操作目标物体坐标系O_object的z轴方向；

2.3.2)沿着触觉传感器表面法向的方向按压待操作目标物体顶部，产生触觉信号，根据该触觉信号对待操作目标物体顶部平面的拟合轮廓进行检测，比较拟合轮廓上各点在触觉传感器表面产生的位移变化，找到位移最大的位置P_1i点和位移最小的位置P_2i点；以线段CP_1i和线段CP_2i为边组成的锐角或钝角记为角θ_i，将角θ_i的角平分线记为L_i，将角θ_i的角平分线L_i与触觉坐标系O_touch的x轴的锐角夹角记为θ_Li；以角θ_i的角平分线L_i作为(α，β)姿态调整平面的法线，根据P_1i点和P_2i点的位移差ΔB_i，估算待操作目标物体顶部平面的法向与机械手末端姿态(α₂，β₂，γ₂)在(α，β)姿态调整平面的相对姿态偏角θ_xyi的大小；根据角θ_i的角平分线L_i与触觉坐标系O_touch的x轴的夹角θ_Li，将相对姿态偏角θ_xyi分配到绕触觉坐标系O_touch的x轴和y轴的转动上，由此计算出待操作目标物体顶部平面的法向与机械手末端姿态(α₂，β₂，γ₂)的相对姿态偏角Δα_i和Δβ_i，计算公式分别如下：

θ_xyi＝f(ΔB_i)

Δα_i＝θ_xyicosθ_Li

Δβ_i＝θ_xyisinθ_Li

其中，f(·)表示P_1i点和P_2i点的位移差ΔB_i与待操作目标物体顶部平面的法向与机械手末端姿态(α₂，β₂，γ₂)在(α，β)姿态调整平面的相对姿态偏角θ_xyi之间的映射函数，表征位移差与相对姿态偏角之间的函数关系；

由计算出待操作目标物体实际位姿与机械手末端位姿

之间的相对偏差，记为ΔT_i＝(Δx_i，Δy_i，Δz_i，Δa_i，Δβ_i，Δγ_i)；

3)计算并调整机械臂系统的第一估计目标位姿

3.1)根据步骤1.2.3)得到的机械臂系统的第一估计目标位姿T₂和步骤2.3.2)得到的待操作目标物体位姿与机械手末端位姿之间的相对偏差ΔT_i计算机械臂系统的第二估计目标位姿T_di，计算公式如下：

其中，n为当前姿态调整控制迭代的总次数；

机械臂系统根据其第二估计目标位姿T_di运动到指定位姿，完成第i次机器人操作位姿调整；

3.3)判断ΔT_i是否小于根据机器人操作精度要求设定的位姿误差阈值ε，若ΔT_i＜ε，则机器人操作位姿控制结束；若ΔT_i≥ε，则执行步骤3.4)；

3.4)令i＝i+1，并返回步骤2.2)，再次利用触觉传感器对待操作目标物体进行精定位，计算待操作目标物体位姿与机械手末端位姿的偏差，直至ΔT_i＜ε，机器人操作位姿控制结束。

2.根据权利要求1所述的机器人操作位姿控制方法，其特征在于，步骤1.2.1)中，所述目标检测方法采用背景相减法、光流计算法或帧差法。

Images