[发明专利]一种基于视触多尺度定位的机器人操作位姿控制方法

权利要求书

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)中，所述目标检测方法采用背景相减法、光流计算法或帧差法。