CN111553239B - 一种机器人关节视觉伺服控制方法、终端设备及存储介质 - Google Patents

Abstract

本发明涉及一种机器人关节视觉伺服控制方法、终端设备及存储介质，该方法中包括：S1：采集机器人图像特征s(t)作为反馈量，构建机器人图像闭环控制；S2：机器人位姿p(t)作为反馈量，构建机器人位姿闭环控制；S3：建立目标函数，并设定目标函数的约束条件，使得机器人图像轨迹和机器人运动轨迹同时最短。本发明基于视觉反馈控制技术，建立图像空间与机器人运动空间控制模型，并且针对IBVS和PBVS视觉伺服技术的局限，建立图像特征轨迹和机器人运动轨迹约束规划模型，实现了图像特征轨迹和机器人末端轨迹最优控制，既能保证机器人系统鲁棒稳定，又使得伺服系统具有一定的环境自适应性，适用于机器人视觉反馈系统。

Description

一种机器人关节视觉伺服控制方法、终端设备及存储介质

技术领域

本发明涉及机器人视觉技术领域，尤其涉及一种机器人关节视觉伺服控制方法、终端设备及存储介质。

背景技术

当前，具备视觉感知能力的机器人已被公认为机器人发展的主流趋势，将视觉感技术与机器人运动控制相结合，由此产生的视觉伺服控制方法为机器人自主操作能力的实现带来了新的思路，代表了机器人先进控制技术，也是促进机器人智能化发展的一个重要驱动。可见，视觉系统对于智能机器人的开发是必不可少的，视觉伺服技术在机器人运动控制中将具有不可替代的作用。

机器人视觉反馈控制技术可分为基于位置的视觉伺服(PBVS)和基于图像的视觉伺服(IBVS)。IBVS和PBVS方法各有固有的优缺点。对于IBVS而言，控制器只能在2D图像空间内，实施对图像特征的有效运动控制，而无法对机器人的三维位姿进行有效控制，机器人轨迹容易超出其工作运动范围，将导致伺服任务失败。对于PBVS而言，控制器只能在3D笛卡尔空间内，实施对机器人运动轨迹的有效控制，无法对图像空间内特征点的轨迹控制，容易使得目标特征脱离机器人的视野，导致特征丢失。在实际应用中，IBVS需要考虑机器人三维运动轨迹约束，PBVS需要考虑图像视场范围等约束。

发明内容

为了克服上述IBVS和PBVS方法的缺点，本发明提出了一种机器人关节视觉伺服控制方法、终端设备及存储介质。该方法基于六自由度“眼在手”机器人系统，结合约束规则与最优化控制理论，保留了IBVS和PBVS方法的优点，同时又克服了它们各自的缺点，即该方法具有对图像空间特征点进行有效控制的特点，同时具有对笛卡尔空间机器人轨迹进行有效控制的特点，可以同时保证图像轨迹和机器人运动轨迹最优，适用于机器人视觉反馈优化控制。

具体方案如下：

一种机器人关节视觉伺服控制方法，包括以下步骤：

S1：采集机器人图像特征作为反馈量，构建机器人图像闭环控制：

e_s(t)＝s(t)-s*

其中，s^*表示期望图像特征，s(t)表示t时刻的图像特征，e_s(t)表示t时刻的图像特征与期望图像特征之间的图形误差，

为J_s(t)的广义逆，J_s(t)表示t时刻的图像的雅可比矩阵，上标T表示矩阵的转置，λ＞0为控制系数，

表示通过机器人图像闭环控制反馈得到的机器人关节速度，

分别表示通过机器人图像闭环控制反馈得到的机器人6个方向的速度；

S2：将机器人位姿作为反馈量，构建机器人位姿闭环控制：

e_p(t)＝p(t)-p*

其中，p*表示期望位姿，p(t)表示t时刻的位姿，e_p(t)表示t时刻的位姿与期望位姿之间的图形误差，

为J_p(t)的广义逆，J_p(t)表示t时刻的位姿的雅可比矩阵，

表示通过机器人位姿闭环控制得到的机器人关节速度，

分别表示通过机器人位姿闭环控制得到的机器人6个方向的速度；

S3：建立目标函数，并设定目标函数的约束条件，使得机器人图像轨迹和机器人运动轨迹同时最短。

进一步的，目标函数为：

min(βd_s(t)+(1-β)d_p(t))

其中，β∈(0,1)表示权重因子，d_s(t)和d_p(t)均为中间变量。

进一步的，目标函数的约束条件包括图像误差约束和位姿误差约束：

e_s(t+1)＝e_s(t)-λJ_s(t)U_s(t)Δt

e_p(t+1)＝e_p(t)-λJ_p(t)U_p(t)Δt

||e(t+1)||≤γ||e(t)||

其中，γ∈(0，1)为权重系数，Δt表示t时刻与t+1时刻之间的时间间隔，e(t)表示联合误差，e(t)＝(e_s(t),e_p(t))^T。

进一步的，目标函数的约束条件包括相机视野约束：

其中，e_s ，

分别表示图像特征向量的取值下界和上界，Δt表示时刻t与时刻t+1之间的时间间隔。

进一步的，目标函数的约束条件包括关节角约束和关节速度约束：

其中，θ(t)表示t时刻机器人的关节角，θ和

分别表示机器人的关节角的上限与下限，

表示t时刻机器人的关节速度，

和

分别表示机器人的关节速度的上限与下限，Δt表示t时刻与t+1时刻之间的时间间隔。

一种机器人关节视觉伺服控制终端设备，包括处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现本发明实施例上述的方法的步骤。

一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现本发明实施例上述的方法的步骤。

本发明采用如上技术方案，基于视觉反馈控制技术，建立图像空间与机器人运动空间控制模型，并且针对IBVS和PBVS视觉伺服技术的局限，建立图像特征轨迹和机器人运动轨迹约束规划模型，实现了图像特征轨迹和机器人末端轨迹最优控制，既能保证机器人系统鲁棒稳定，又使得伺服系统具有一定的环境自适应性，适用于机器人视觉反馈系统。

附图说明

图1所示为本发明实施例一中机器人系统结构示意图。

图2所示为该实施例中系统对应的控制流程图。

图3所示为该实施例中IBVS方法特征轨迹图。

图4所示为该实施例中IBVS方法机器人运动轨迹图。

图5所示为该实施例中IBVS方法机器人关节速度图。

图6所示为该实施例中PBVS方法特征轨迹图。

图7所示为该实施例中PBVS方法机器人运动轨迹图。

图8所示为该实施例中PBVS方法机器人关节速度图。

图9所示为该实施例中方法特征轨迹图。

图10所示为该实施例中方法机器人运动轨迹图。

图11所示为该实施例中方法机器人关节速度图。

具体实施方式

为进一步说明各实施例，本发明提供有附图。这些附图为本发明揭露内容的一部分，其主要用以说明实施例，并可配合说明书的相关描述来解释实施例的运作原理。配合参考这些内容，本领域普通技术人员应能理解其他可能的实施方式以及本发明的优点。

现结合附图和具体实施方式对本发明进一步说明。

实施例一：

本实施例提供了一种约束规划的机器人关节视觉伺服方法，如图1所示，该方法中以六自由度单目工业机器人为实验平台，摄像机固定安装在机器人手臂末端，构成“眼在手”(Eye-In-Hand)机器人系统。该系统对应的控制流程如图2所示，其中包括机器人图像闭环控制和机器人位姿闭环控制两个闭环控制，两个闭环控制的输入分别为各时刻采集到的图像特征和姿态，输出均为机器人的关节速度。

该方法的主要实现流程如下：

S1：采集机器人图像特征作为反馈量，构建机器人图像闭环控制。

该实施例中图像特征通过安装在机器人手臂末端的摄像机进行采集。

机器人图像闭环控制的构建方法为：

S11：在图像平面内计算当前时刻t的图像特征s(t)与期望图像特征s^*之间的图像误差e_s(t)：

e_s(t)＝s(t)-s* (1)

式中，s^*表示期望图像特征向量。

S12：标定相机内参后，引入图像的雅可比矩阵J_s(t)，构建机器人运动与图像特征变化关系式：

λe_s(t)＝J_s(t)U_s(t) (2)

根据式(1)和式(2)构建机器人图像闭环控制：

式中，

为通过图像闭环控制得到的机器人关节速度；

分别表示通过机器人图像闭环控制反馈得到的机器人6个方向的速度；λ＞0为控制系数；

为J_s(t)的广义逆，由式(4)计算得到：

式中，上标T表示矩阵的转置。

S2：将机器人位姿p(t)作为反馈量，构建机器人位姿闭环控制。具体的构建方法为：

S21：在笛卡尔空间中计算机器人t时刻的位姿p(t)与期望位姿p*之间的误差e_p(t)：

e_p(t)＝p(t)-p* (5)

式中，p*为机器人期望位姿向量。

S22：标定机器人系统参数后，引入机器人的雅可比矩阵J_p(t)，构建机器人运动与机器人末端位姿之间关系式：

λe_p(t)＝J_p(t)U_p(t) (6)

根据式(6)在笛卡尔空间中构建机器人位姿闭环控制：

式中，

为通过机器人位姿闭环控制得到的机器人关节速度；

分别表示通过机器人位姿闭环控制得到的机器人6个方向的速度；

由式(8)计算得到：

S3：在步骤S1和S2中，难以既保证特征点在相机视场范围内，又保证机器人运动在机器人工作范围内，即难以保证图像轨迹和机器人运动轨迹同时最优。为了解决这一问题，该实施例中进一步设计一个目标函数使得图像轨迹和机器人运动轨迹同时最短，步骤如下：

S31：在图像空间中，根据下式计算下一时刻(t+1时刻)的图像特征s(t+1)到t时刻的图像特征s(t)与期望图像特征s*连线的距离d_s(t)：

令中间变量

化简式(9)得：

S32：在笛卡尔空间中，根据下式计算下一时刻(t+1时刻)的机器人位姿p(t+1)到t时刻位姿p(t)与期望位姿p*连线的距离d_p(t)：

同理化简可得：

d_p(t)＝||(Q_p(t)e_p(t+1))||

     ＝||Q_p(t)[e_p(t)-J_p(t)U_p(t)Δt]|| (13)

式中中间变量

S33：为了同时保证特性运动与机器人运动轨迹最优，根据S31和S32得到的d_s(t)和d_p(t)，建立优化目标函数：

min(βd_s(t)+(1-β)d_p(t)) (15)

式中，β∈(0,1)为权重因子，该目标函数的目的是寻找一个最优的机器人关节速度，使得特征运动轨迹和机器人末端运动轨迹同时直线运动。

S4：为求解最优的机器人关节速度使上述步骤S33目标函数成立，需要在2D图像空间和3D机器人笛卡尔空间中建立必要的约束条件。

该实施例中包括以下约束条件：

(1)在图像空间中，图像特征误差与机器人关节速度需要满足以下图像误差约束：

e_s(t+1)＝e_s(t)-λJ_s(t)U_s(t)Δt (16)

同时，在机器人笛卡尔空间中机器人位姿误差与机器人关节速度需要满足以下位姿误差约束：

e_p(t+1)＝e_p(t)-λJ_p(t)U_p(t)Δt (17)

为了保证图像误差和机器人姿态误差同时收敛，那么联合误差e(t)必须满足递减约束，如下：

||e(t+1)||≤γ||e(t)|| (18)

式中，γ∈(0，1)；e(t)＝(e_s(t),e_p(t))^T。

(2)为了保证图像特征不丢失，图像特征必须约束在相机视场内，所以下一时刻图像特征向量s(t+1)需要满足以下相机视野约束：

式中，e_s ，

分别表示图像特征向量的取值下界和上界，Δt表示时刻t与时刻t+1之间的时间间隔。

(3)从机器人工作空间来看，机器人的下一时刻关节角必须满足以下的关节角约束：

式中，θ(t)表示t时刻机器人的关节角，θ，

为机器人的关节角的上限与下限。

同时机器人关节速度

也需要满足以下关节速度约束：

式中，

表示t时刻机器人的关节速度，

为机器人的关节速度的上限与下限。

综合上述三点约束条件和目标函数，将最优控制问题转化为二阶锥优化模型：

min(βd_s(t)+(1-β)d_p(t))

S.T.

e_s(t+1)＝e_s(t)-λJ_s(t)U_s(t)Δt

e_p(t+1)＝e_p(t)-λJ_p(t)U_p(t)Δt

||e(t+1)||≤γ||e(t)||

从而建立带约束规划的视觉伺服控制模型，保证图像轨迹和机器人末端轨迹同时最优。

实验结果：

该实施例中以经典PBVS和IBVS视觉伺服方法为比较对象，来验证本实施例方法的性能与优点：一方面为对2D图像空间特征点的运动控制的性能，另一方面为对3D笛卡尔空间机器人末端运动控制的性能。为了体现机器人视觉伺服任务的一般性，该实施例中设定机器人平移组合旋转运动控制实验。

(1)IBVS方法实验效果分析

IBVS方法实验结果如图3、图4和图5所示。虽然由图3图像特征点的运动轨迹可看出，IBVS方法图像特征以直线运动轨迹从初始位置收敛到期望位置，该实验结果和本实施例中步骤S1中设计的机器人图像闭环控制的几何意义相一致，这是因为IBVS控制器在图像空间内以特征点位置为控制对象，直接控制特征点的运动，能够确保特征直线轨迹运动。但是由图4机器人末端运动轨迹可以看出，机器人路径近似随机运动，IBVS控制器不能保证机器人直线轨迹运动，这样反映了IBVS固有的缺陷，即无法对三维笛卡尔空间内的机器人姿态进行有效控制。由图5关节速度的变化曲线可以看出，在整个伺服任务过程中控制量会出现较剧烈的突变，这是由于图像雅可比矩阵接近奇异造成的。通过上述实验说明IBVS视觉伺服方法，机器人运动轨迹难以保持直线运动，而且图像雅可比矩阵容易发生奇异，导致系统稳定性较弱。

(2)PBVS方法实验效果分析

PBVS方法的实验结果如图6、图7和图8所示。从图6可以看出，图像空间内特征点轨迹运动不理想，特征点不能保持直线运动，在特殊场合下特征点容易超出相机视场范围，导致特征消失而伺服任务失败，这正是由PBVS方法无法对图像空间内的图像特征直接控制造成的。由图7机器人末端运动轨迹可看出，PBVS方法机器人以直线运动轨迹从初始位姿收敛到期望位姿，这是因为PBVS控制器在笛卡尔空间内以机器人姿态为控制对象，直接控制机器人运动轨迹，能够确保运动轨迹保持直线运动，该实验结果和本实施例步骤S2中机器人位姿闭环控制的几何意义相一致。由图8关节速度随时间的变化曲线可以看出，曲线变化平稳，没有明显的突变出现，这反映了机器人雅可比矩阵在伺服过程中始终具有良好的非奇异特性。通过上述实验，说明PBVS方法在伺服过程中，图像特征轨迹难以保持直线运动，容易出现特点丢失导致任务失败。

(3)本实施例方法的实验效果分析

本实施例方法实验结果如图9、图10和图11所示。由图9可看出，本实施例方法特征点运动轨迹十分接近IBVS方法，即图像特征轨迹以直线方式从初始位置收敛到期望位置。由图10可看出，本实施例方法相机运动轨迹十分接近PBVS方法，即机器人末端以直线运动从始位姿收敛到期望位姿，机器人运动稳定无震荡。图11为机器人关节速度，关节速度变化情况介于IBVS和PBVS方法之间，变化平缓、光滑。

通过以上实验比较可知，相对于IBVS和PBVS方法，本实施例方法既具有IBVS对图像空间进行有效控制的特点，也具有PBVS对三维空间进行有效控制的特点，可以同时保证图像轨迹和机器人运动轨迹最短。本实施例方法中的规划器不仅起到了控制器的作用而且还起到了在线路径规划的作用，机器人运动控制效果大大提升，即使在经典视觉伺服无法完成的极端情况下，仍然具有良好的动态性能和控制精度。另外本实施例方法中定义的多种约束准则，使得规划器具有较强的环境适应性和功能拓展性。

实施例二：

本发明还提供一种机器人关节视觉伺服控制终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现本发明实施例一的上述方法实施例中的步骤。

进一步地，作为一个可执行方案，所述机器人关节视觉伺服控制终端设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述机器人关节视觉伺服控制终端设备可包括，但不仅限于，处理器、存储器。本领域技术人员可以理解，上述机器人关节视觉伺服控制终端设备的组成结构仅仅是机器人关节视觉伺服控制终端设备的示例，并不构成对机器人关节视觉伺服控制终端设备的限定，可以包括比上述更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述机器人关节视觉伺服控制终端设备还可以包括输入输出设备、网络接入设备、总线等，本发明实施例对此不做限定。

进一步地，作为一个可执行方案，所称处理器可以是中央处理单元(CentralProcessing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital SignalProcessor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述机器人关节视觉伺服控制终端设备的控制中心，利用各种接口和线路连接整个机器人关节视觉伺服控制终端设备的各个部分。

所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述机器人关节视觉伺服控制终端设备的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序；存储数据区可存储根据手机的使用所创建的数据等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现本发明实施例上述方法的步骤。

所述机器人关节视觉伺服控制终端设备集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)以及软件分发介质等。

尽管结合优选实施方案具体展示和介绍了本发明，但所属领域的技术人员应该明白，在不脱离所附权利要求书所限定的本发明的精神和范围内，在形式上和细节上可以对本发明做出各种变化，均为本发明的保护范围。

Claims (3)

1.一种机器人关节视觉伺服控制方法，其特征在于，包括以下步骤：

S1：采集机器人图像特征作为反馈量，构建机器人图像闭环控制：

e_s(t)＝s(t)-s*

其中，s^*表示期望图像特征，s(t)表示t时刻的图像特征，e_s(t)表示t时刻的图像特征与期望图像特征之间的图形误差，

为J_s(t)的广义逆，J_s(t)表示t时刻的图像的雅可比矩阵，上标T表示矩阵的转置，λ>0为控制系数，

表示通过机器人图像闭环控制反馈得到的机器人关节速度，

分别表示通过机器人图像闭环控制反馈得到的机器人6个方向的速度；

S2：将机器人位姿作为反馈量，构建机器人位姿闭环控制：

e_p(t)＝p(t)-p*

其中，p*表示期望位姿，p(t)表示t时刻的位姿，e_p(t)表示t时刻的位姿与期望位姿之间的图形误差，

为J_p(t)的广义逆，J_p(t)表示t时刻的位姿的雅可比矩阵，

表示通过机器人位姿闭环控制得到的机器人关节速度，

分别表示通过机器人位姿闭环控制得到的机器人6个方向的速度；

S3：建立目标函数，并设定目标函数的约束条件，使得机器人图像轨迹和机器人运动轨迹同时最短；目标函数为：

min(βd_s(t)+(1-β)d_p(t))

其中，β∈(0,1)表示权重因子，d_s(t)和d_p(t)均为中间变量；

目标函数的约束条件包括图像误差约束和位姿误差约束：

e_s(t+1)＝e_s(t)-λJ_s(t)U_s(t)Δt

e_p(t+1)＝e_p(t)-λJ_p(t)U_p(t)Δt

||e(t+1)||≤γ||e(t)||

其中，γ∈(0，1)为权重系数，Δt表示t时刻与t+1时刻之间的时间间隔，e(t)表示联合误差，e(t)＝(e_s(t),e_p(t))^T；

目标函数的约束条件包括相机视野约束：

其中，e_s ，

分别表示图像特征向量的取值下界和上界，Δt表示时刻t与时刻t+1之间的时间间隔；

目标函数的约束条件包括关节角约束和关节速度约束：

其中，θ(t)表示t时刻机器人的关节角，θ和

分别表示机器人的关节角的上限与下限，

表示t时刻机器人的关节速度，

和

分别表示机器人的关节速度的上限与下限，Δt表示t时刻与t+1时刻之间的时间间隔。

2.一种机器人关节视觉伺服控制终端设备，其特征在于：包括处理器、存储器以及存储在所述存储器中并在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1中所述方法的步骤。

3.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于：所述计算机程序被处理器执行时实现如权利要求1中所述方法的步骤。

Images