CN102350700A - 一种基于视觉的机器人控制方法 - Google Patents

Abstract

本发明提供一种基于视觉的机器人控制方法，包括步骤：（1）通过摄像机获取人手手势图像；（2）提取手势图像中人手的特征点；（3）对特征点进行三维重建，得到人手特征点在三维空间的位置关系；（4）把人手特征点对应的坐标点变换到机器人基坐标下。（5）利用人手在机器人基坐标系下的位姿关系进行反解计算，得到机器人的关节角度；（6）利用计算得到的关节角度驱动机器人运行。本发明的优点在于：1）控制直观，机器人手抓姿态直接对应人手姿态；2）控制灵活，无需与繁重的交换工具接触；3）利用虚拟现实技术可以辅助操作者更准确更安全地进行操作；4）允许中途中断恢复或更换操作者；5）操作者无需大范围走动，减少操作者的操作压力。

Description

一种基于视觉的机器人控制方法

技术领域

本发明属于机器人人机交互领域，特别涉及一种一种基于视觉的机器人控制方法。

背景技术

随着机器人应用领域的日益扩大，特别是机器人进入人们的日常生活，人和机器人对等交互(peer-to-peer interaction)研究越来越被重视，在对等交互中，人和机器人更多的是伙伴关系，而不是简单的人和工具关系。

遥操作机器人系统和全自主机器人系统存在一些不足，使得人-机器人协作系统具有很大的潜力。在这类系统中，人和机器人成员根据自身能力相互协作共同完成目标任务，这种团队模式的优势在于集成人和机器人的智能来有效的完成目标任务。

人-机器人交互主题的研究涉及许多不同领域，例如不同的交互模式、认知模型以及评价方法等。在交互模式研究中，符合人际交流习惯的交互模式是当前研究的热点，手势作为一种自然而直观的人际交流模式而被重视，其中手势跟踪是手势识别的基础。另外由于手势本事具有的多样性、多义性、以及时间和空间上的差异性等特点，当前实用的手势交互往往根据应用特点设计特定语义库，以保证手势交互的准确性和有效性。

机械臂作业需要一系列复杂的指令，简单的特定语义库并不能满足机械臂的控制要求。机械臂操作的难点在于姿态的控制。由于机械臂仿效人手的结构进行设计，那么用人手控制机械臂是一个很直观的方法。

发明内容

本发明的目的在于克服上述现有技术的缺点与不足，提供一种灵活自然的一种基于视觉的机器人控制方法。为达上述目的，本发明采用如下的技术方案：

一种基于视觉的机器人控制方法，其特征在于，包括如下步骤：

S1、通过摄像机获取人手手势图像；

S2、提取手势图像中人手的特征点；

S3、对特征点进行三维重建，得到人手特征点在三维空间的位置关系；

S4、把人手特征点对应的坐标点变换到机器人基坐标下；

S5、利用人手在机器人基坐标系下的位姿关系进行反解计算，得到机器人的关节角度；

S6、利用计算得到的关节角度驱动机器人运动。

2、根据权利要求1所述的基于视觉的机器人控制方法，其特征在于，所述步骤S1包括：根据双目定位原理，在人手的上方安装两个摄像头，实时捕捉人手运动图像。

上述基于视觉的机器人控制方法中，所述步骤S2包括：根据人手图像中人手特征的特点，通过特征点提取的方法进行图像处理，得到人手特征点的像素区域，然后提取人手特征区域的中心点作为人手特征点。在特征点提取中需要用到24位的(R，G，B)颜色模型，在RGB颜色模型中，所有颜色都可以由R，G，B三种颜色组成，不同的组合呈现不同的颜色。为了能识别图像中的特征点，利用红颜色标记手部的特征点，而黄色的手套是为了便于上色，下面给出特征点提取的具体颜色模型。

假设图像中像素i(i取正整数)的值为(R_i，G_i，B_i)，那么识别图中红色像素的模型为：

$\left\{\begin{array}{c}{R}_i>G_i+{\mathrm{\δ}}_g\\ R_i>B_i+{\mathrm{\δ}}_b\\ R_i>\mathrm{\δ}\end{array}\right.$

其中δ_g，δ_b，δ为颜色阀值，表示像素中R值必须超过不等式右边的值才是标记点，上述不等式表示标志点的R值比G，B值要大。

上述基于视觉的机器人控制方法中，所述步骤S3包括：在步骤S2中可以得到红色标记点在左右图像中的位置，为了重建出标记点的三维坐标，采用双目重建原理进行三维重建。

双目定位深度计算：

$\frac{T+x_r-x_l}{Z-f}=\frac{T}{Z}$

令：d＝x_l-x_r得：

$Z=f\frac{T}{d}$

其中P为测量点，T为宽线，f为焦距，Z为测量点到宽线的距离，Q_l为左摄像机的光心，Q_r为右摄像机的中心，P_l为测量点在左摄像机上的投影，P_r为测量点在右摄像机上的投影，x₁为左图像中心到左投影点P_l的向量，x_r为右图像中心到右投影点P_r的向量。

上述基于视觉的机器人控制方法中，步骤S4包括：包括以下步骤：

S41、通过人手控制机器人的位置变化；

S42、通过人手控制机器人的姿态变化；

S43、人手姿态与机器人末端姿态进行映射。

上述基于视觉的机器人控制方法中，步骤S41包括：

由于在进行机械臂控制过程中无须操作者大范围走动，而机械臂的运动空间又是比较大的，以小空间到大空间的映射会造成精度损失，所以采取差分的定位方法。首先在初始化机械臂后，通过正解算法可以得到手抓末端的初始位置(x^p，y^p，z^p)，接下来在摄像机拍摄范围内规定一个工作空间(working space)，人手只能在该工作空间运动，否则指令失效，然后再定义一个方向空间(Direction Space)，方向空间与工作空间形成一个空隙，该空隙用于改变机械臂的位置：

x^p＝x^p+Δx*σ

y^p＝z^p+Δy*σ

z^p＝z^p+Δz*σ

Δx、Δy和Δz分别为人手在三个方向轴上的位移，而σ是一个可调的参数，则控制端位置可以是一个无限的空间，可以通过修改σ的值来达到粗控制与微控制的效果。

上述基于视觉的机器人控制方法中，步骤S42包括：

手抓末端的姿态与人手中拇指端，食指端和拇指根部与食指根部之间的凹槽点三点组成的姿态是一致的。

上述基于视觉的机器人控制方法中，步骤S43包括：

先不考虑平移，假设人手坐标系与控制台坐标系的原点重合。变换矩阵为一个3*3的矩阵M，则在人手坐标系中的一个点A，变换到控制台基坐标系中为A′，有A′＝MA。

其中：

$M=\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]$

在人手定位中，人手坐标系中x轴单位向量P1[1，0，0]，y轴单位向量P2[0，1，0]，z轴单位向量P3[0，0，1]在摄像机坐标系下为：[x₁，x₂，x₃]，[y₁，y₂，y₃]，[z₁，z₂，z₃]，那么有：

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]$

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]=\left[\begin{array}{c}{y}_1\\ y_2\\ y_3\end{array}\right]$

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=\left[\begin{array}{c}{z}_1\\ z_2\\ z_3\end{array}\right]$

由上式得：

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]$

由于人手相对于控制台坐标系的姿态变换矩阵与手抓末端相对于基坐标系的姿态变换矩阵是一致的，而手抓末端相对于基坐标系的平移关系在定位模型已经给出，所以最后得到位姿的变换矩阵为：

$M^{\′}=\left[\begin{array}{cccc}{x}_1& y_1& z_1& p_1\\ x_2& y_2& z_2& p_2\\ x_3& y_3& z_3& p_3\\ 0& 0& 0& 1\end{array}\right]$

其中[p₁，p₂，p₃]为手抓末端相对于基坐标系的平移矩阵。

上述基于视觉的机器人控制方法中，步骤S5包括：包括以下步骤：

在Denavit-Hartenberg表示法中，A_i表示从坐标系i-1到坐标系i的齐次坐标变换阵(i取正整数)，一般地有：

$A_i=\left[\begin{array}{cccc}\cos {\mathrm{\θ}}_i& -\sin {\mathrm{\θ}}_i\cos {\mathrm{\α}}_i& \sin {\mathrm{\θ}}_i\sin {\mathrm{\α}}_i& l_i\cos {\mathrm{\θ}}_i\\ {\sin \mathrm{\θ}}_i& {\cos \mathrm{\θ}}_i{\cos \mathrm{\α}}_i& -{\cos \mathrm{\θ}}_i{\sin \mathrm{\α}}_i& l_i{\sin \mathrm{\θ}}_i\\ 0& {\sin \mathrm{\α}}_i& {\cos \mathrm{\α}}_i& r_i\\ 0& 0& 0& 1\end{array}\right]$

对于一个具有n(n≥6)关节的机器人，从机座坐标架到最后一个坐标架的齐次变换阵定义为：

$T_6=A_1{A}_2...A_n=\left[\begin{array}{cccc}{n}_n^0& s_n^0& a_n^0& p_n^0\\ 0& 0& 0& 1\end{array}\right]$

式中

为手爪的法向矢量，

为滑动矢量，为接近矢量，

为位置矢量。

利用上面两式有：

T_n＝M

通过求解上式得到n个关节运动角度值：(θ₁，θ₂，…，θ_n)。

上述基于视觉的机器人控制方法中，步骤S6包括：包括以下步骤：利用步骤S5计算得到n个关节角角度驱动机器人运动，从而使得机器人末端达到期望位置。

本发明相对于现有技术具有如下的优点及技术效果：

1、控制直观，机器人手抓姿态直接对应人手姿态。

2、控制灵活，无需与繁重的交换工具接触。

3、利用虚拟现实技术可以辅助操作者更准确更安全地进行操作。

4、允许中途中断恢复或更换操作者。

5、操作者无需大范围走动，减少操作者的操作压力。

附图说明

图1为实施方式中的框架模型图；

图2为定位模型图；

图3a、图3b分别是手抓末端的姿态与人手中拇指端、食指端和拇指根部与食指根部之间的凹槽点三点组成的姿态模型图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此实施例，图1给出的是框架模型图。

本根据基于视觉控制机器人方法，包括如下步骤：

S1、通过摄像机获取人手手势图像；

S2、提取手势图像中人手的特征点；

S3、对特征点进行三维重建，得到人手特征点在三维空间的位置关系；

S4、把人手特征点对应的坐标点变换到机器人基坐标下；

S5、利用人手在机器人基坐标系下的位姿关系进行反解计算，得到机器人的关节角度；

S6、利用计算得到的关节角度驱动机器人运动。

所述步骤S1包括以下步骤：

S11、根据双目定位原理，在人手的上方安装两个摄像头，实时捕捉人手运动图像。

所述步骤S2包括以下步骤：

S21、根据权利要求1所述的视觉控制机器人方法，其特征在于，所述步骤S2包括：根据人手图像中人手特征的特点，通过特征点提取的方法进行图像处理，得到人手特征点的像素区域，然后提取人手特征区域的中心点作为人手特征点。在特征点提取中需要用到24位的(R，G，B)颜色模型，在RGB颜色模型中，所有颜色都可以由R，G，B三种颜色组成，不同的组合呈现不同的颜色。为了能识别图像中的特征点，利用红颜色标记手部的特征点，而黄色的手套是为了便于上色。

所述步骤S3包括以下步骤：

S31、在人手识别系统中，可以得到红色标记点在左右图像中的位置，为了重建出标记点的三维坐标，采用双目重建原理进行三维重建。

所述步骤S4包括以下步骤：

S41、由于在进行机械臂控制过程中无须操作者大范围走动，而机械臂的运动空间又是比较大的，以小空间到大空间的映射会造成精度损失，所以采取差分的定位方法。首先在初始化机械臂后，通过正解算法可以得到手抓末端的初始位置(x^p，y^p，z^p)，如图2所示，接下来在摄像机拍摄范围内规定一个工作空间(working space)，人手只能在该工作空间运动，否则指令失效，然后再定义一个方向空间(Direction Space)，方向空间与工作空间形成一个空隙，该空隙用于改变机械臂的位置：

x^p＝x^p+Δx*σ

y^p＝z^p+Δy*σ

z^p＝z^p+Δz*σ

Δx、Δy和Δz分别为人手在三个方向轴上的位移，而σ是一个可调的参数，则控制端位置可以是一个无限的空间，可以通过修改σ的值来达到粗控制与微控制的效果。

S42、手抓末端的姿态与人手中拇指端，食指端和拇指根部与食指根部之间的凹槽点三点组成的姿态是一致的，如图3a和图3b所示。

S43、先不考虑平移，假设人手坐标系与控制台坐标系的原点重合。变换矩阵为一个3*3的矩阵M，则在人手坐标系中的一个点A，变换到控制台基坐标系中为A′，有A′＝MA。

其中：

$M=\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]$

在人手定位中，人手坐标系中x轴单位向量P1[1，0，0]，y轴单位向量P2[0，1，0]，z轴单位向量P3[0，0，1]在摄像机坐标系下为：[x₁，x₂，x₃]，[y₁，y₂，y₃]，[z₁，z₂，z₃]，那么有：

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]$

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]=\left[\begin{array}{c}{y}_1\\ y_2\\ y_3\end{array}\right]$

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=\left[\begin{array}{c}{z}_1\\ z_2\\ z_3\end{array}\right]$

由上式得：

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]$

由于人手相对于控制台坐标系的姿态变换矩阵与手抓末端相对于基坐标系的姿态变换矩阵是一致的，而手抓末端相对于基坐标系的平移关系在定位模型已经给出，所以最后得到位姿的变换矩阵为：

$M^{\′}=\left[\begin{array}{cccc}{x}_1& y_1& z_1& p_1\\ x_2& y_2& z_2& p_2\\ x_3& y_3& z_3& p_3\\ 0& 0& 0& 1\end{array}\right]$

其中[p₁，p₂，p₃]为手抓末端相对于基坐标系的平移矩阵。

所述步骤S5包括以下步骤：

S51、在Denavit-Hartenberg表示法中，A_i表示从坐标系i-1到坐标系i的齐次坐标变换阵(i取正整数)，一般地有：

$A_i=\left[\begin{array}{cccc}\cos {\mathrm{\θ}}_i& -\sin {\mathrm{\θ}}_i\cos {\mathrm{\α}}_i& \sin {\mathrm{\θ}}_i\sin {\mathrm{\α}}_i& l_i\cos {\mathrm{\θ}}_i\\ {\sin \mathrm{\θ}}_i& {\cos \mathrm{\θ}}_i{\cos \mathrm{\α}}_i& -{\cos \mathrm{\θ}}_i{\sin \mathrm{\α}}_i& l_i{\sin \mathrm{\θ}}_i\\ 0& {\sin \mathrm{\α}}_i& {\cos \mathrm{\α}}_i& r_i\\ 0& 0& 0& 1\end{array}\right]$

对于一个具有n(n≥6)关节的机器人，从机座坐标架到最后一个坐标架的齐次变换阵定义为：

$T_6=A_1{A}_2...A_n=\left[\begin{array}{cccc}{n}_n^0& s_n^0& a_n^0& p_n^0\\ 0& 0& 0& 1\end{array}\right]$

式中为手爪的法向矢量，

为滑动矢量，

为接近矢量，为位置矢量。

利用上面两式有：

T_n＝M

通过求解上式得到n个关节运动角度值：(θ₁，θ₂，…，θ_n)。

所述步骤S6包括以下步骤：

S61、利用步骤S5计算得到n个关节角角度驱动机器人运动，从而使得机器人末端达到期望位置。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.一种基于视觉的机器人控制方法，其特征在于，包括如下步骤：

S1、通过摄像机获取人手手势图像；

S2、提取手势图像中人手的特征点；

S3、对特征点进行三维重建，得到人手特征点在三维空间的位置关系；

S4、把人手特征点对应的坐标点变换到机器人基坐标下；

S5、利用人手在机器人基坐标系下的位姿关系进行反解计算，得到机器人的关节角度；

S6、利用计算得到的关节角度驱动机器人运动。

2.根据权利要求1所述的基于视觉的机器人控制方法，其特征在于，所述步骤S1包括：根据双目定位原理，在人手的上方安装两个摄像头，实时捕捉人手运动图像。

3.根据权利要求1所述的基于视觉的机器人控制方法，其特征在于，所述步骤S2包括根据人手图像中人手特征的特点，通过特征点提取的方法进行图像处理，得到人手特征点的像素区域，然后提取人手特征区域的中心点作为人手特征点；在特征点提取中需要用到24位的RGB颜色模型，在RGB颜色模型中，所有颜色由R，G，B三种颜色组成，不同的组合呈现不同的颜色；为了能识别图像中的特征点，利用红颜色标记手部的特征点，而黄色的手套是为了便于上色，下面给出特征点提取的具体颜色模型：

假设图像中像素i的值为(R_i，G_i，B_i)，i取正整数，那么识别图中红色像素的模型为：

$\left\{\begin{array}{c}{R}_i>G_i+{\mathrm{\δ}}_g\\ R_i>B_i+{\mathrm{\δ}}_b\\ R_i>\mathrm{\δ}\end{array}\right.$

其中δ_g，δ_b，δ为颜色阀值，表示像素中R值必须超过不等式右边的值才是标记点，上述不等式表示标志点的R值比G，B值要大。

4.根据权利要求1所述的基于视觉的机器人控制方法，其特征在于，所述步骤S3包括：在步骤S2中得到红色标记点在左右图像中的位置，为了重建出标记点的三维坐标，采用双目重建原理进行三维重建，

双目定位深度计算：

$\frac{T+x_r-x_l}{Z-f}=\frac{T}{Z}$

令：d＝x_l-x_r得：

$Z=f\frac{T}{d}$

其中P为测量点，T为宽线，f为焦距，Z为测量点到宽线的距离，Q_l为左摄像机的光心，Q_r为右摄像机的中心，P_l为测量点在左摄像机上的投影，P_r为测量点在右摄像机上的投影，x₁为左图像中心到左投影点P_l的向量，x_r为右图像中心到右投影点P_r的向量。

5.根据权利要求1所述的基于视觉的机器人控制方法，其特征在于，步骤S4包括：包括以下步骤：

S41、通过人手控制机器人的位置变化；

S42、通过人手控制机器人的姿态变化；

S43、人手姿态与机器人末端姿态进行映射。

6.根据权利要求5所述的基于视觉的机器人控制方法，其特征在于，步骤S41包括：

首先在初始化机械臂后，通过正解算法可以得到手抓末端的初始位置(x^p，y^p，z^p)，接下来在摄像机拍摄范围内规定一个工作空间，人手只能在该工作空间运动，否则指令失效，然后再定义一个方向空间，方向空间与工作空间形成一个空隙，该空隙用于改变机械臂的位置：

x^p＝x^p+Δx*σ

y^p＝z^p+Δy*σ

z^p＝z^p+Δz*σ

Δx、Δy和Δz分别为人手在三个方向轴上的位移，而σ是一个可调的参数，则控制端位置是一个无限的空间，通过修改σ的值来进行粗控制与微控制。

7.根据权利要求5所述的基于视觉的机器人控制方法，其特征在于，步骤S42包括：

手抓末端的姿态与人手中拇指端、食指端和拇指根部与食指根部之间的凹槽点三点组成的姿态是一致的。

8.根据权利要求5所述的基于视觉的机器人控制方法，其特征在于，步骤S43包括：

假设人手坐标系与控制台坐标系的原点重合；变换矩阵为一个3×3的矩阵M，则在人手坐标系中的一个点A，变换到控制台基坐标系中为A′，有A′＝MA，

其中：

$M=\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]$

在人手定位中，人手坐标系中x轴单位向量P1[1，0，0]，y轴单位向量P2[0，1，0]，z轴单位向量P3[0，0，1]在摄像机坐标系下为：[x₁，x₂，x₃]，[y₁，y₂，y₃]，[z₁，z₂，z₃]，那么有：

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]$

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]=\left[\begin{array}{c}{y}_1\\ y_2\\ y_3\end{array}\right]$

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=\left[\begin{array}{c}{z}_1\\ z_2\\ z_3\end{array}\right]$

由上式得：

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]$

由于人手相对于控制台坐标系的姿态变换矩阵与手抓末端相对于基坐标系的姿态变换矩阵是一致的，而手抓末端相对于基坐标系的平移关系在定位模型已经给出，所以最后得到位姿的变换矩阵为：

$M^{\′}=\left[\begin{array}{cccc}{x}_1& y_1& z_1& p_1\\ x_2& y_2& z_2& p_2\\ x_3& y_3& z_3& p_3\\ 0& 0& 0& 1\end{array}\right]$

其中[p₁，p₂，p₃]为手抓末端相对于基坐标系的平移矩阵。

9.根据权利要求1所述的基于视觉的机器人控制方法，其特征在于，步骤S5包括：包括以下步骤：

在Denavit-Hartenberg表示法中，A_i表示从坐标系i-1到坐标系i的齐次坐标变换阵，i取正整数，有：

$A_i=\left[\begin{array}{cccc}\cos {\mathrm{\θ}}_i& -\sin {\mathrm{\θ}}_i\cos {\mathrm{\α}}_i& \sin {\mathrm{\θ}}_i\sin {\mathrm{\α}}_i& l_i\cos {\mathrm{\θ}}_i\\ {\sin \mathrm{\θ}}_i& {\cos \mathrm{\θ}}_i{\cos \mathrm{\α}}_i& -{\cos \mathrm{\θ}}_i{\sin \mathrm{\α}}_i& l_i{\sin \mathrm{\θ}}_i\\ 0& {\sin \mathrm{\α}}_i& {\cos \mathrm{\α}}_i& r_i\\ 0& 0& 0& 1\end{array}\right]$

对于一个具有n关节的机器人，n≥6，从机座坐标架到最后一个坐标架的齐次变换阵定义为：

$T_6=A_1{A}_2...A_n=\left[\begin{array}{cccc}{n}_n^0& s_n^0& a_n^0& p_n^0\\ 0& 0& 0& 1\end{array}\right]$

式中为手爪的法向矢量，为滑动矢量，

为接近矢量，

为位置矢量；

利用上面两式有：

T_n＝M

通过求解上式得到n个关节运动角度值：(θ₁，θ₂，…，θ_n)。

10.根据权利要求1所述的基于视觉的机器人控制方法，其特征在于，步骤S6包括利用步骤S5计算得到n个关节角角度驱动机器人运动，从而使得机器人末端达到期望位置。

Images