CN101396830A - 机器人装置的控制方法以及机器人装置 - Google Patents

Abstract

本发明提供一种在控制机器人装置的机械手时，不必准备很多的基准图像，计算负担较轻，可进行迅速的控制，并且能够在三维空间内控制机械手对对象物的位置及姿势的机器人装置的控制方法。基于处理由摄像机(6)获得的对象物(101)的图像而得到的图像特征量，使用迭代最小二乘法推定图像特征量的变化速度和机械手(1)的各关节(2a、2b、2c、2d、2e、2f)的动作速度的关系式即雅可比矩阵，并与事先在目标位置获得的对象物(101)的图像特征量进行对比后，以机械手(1)的手头位置(5)接近目标位置的方式，求出上述各关节的动作速度并作为机械手(1)的指令信号，并基于该指令信号使机械手(1)动作。

Description

机器人装置的控制方法以及机器人装置

技术领域

本发明涉及在机械手的前端具备摄像机的机器人装置中，以使事先从目标状态算出并存储的目标特征量与从通过摄像机获得的图像所得到的特征量一致的方式，使机械手动作的机器人装置的控制方法以及机器人控制装置。

背景技术

以往，提出有具备机械手的机器人装置，另外，提出有用于控制这种机器人装置的机械手动作的机器人装置的控制方法。并且，作为机器人装置的控制方法，提出有各种用于将利用机械手进行夹持的对象物作为目标而使机械手的手头移动的控制方法。

例如，在专利文献1(日本特开2003-231078号公报)中，记载了以下机器人装置：预先准备多个对象物的基准图像和拍摄该图像时的摄像机位置与姿势，并且，获得通过安装在机械手的手头上的摄像机的图像信息，并通过检索图像信息中的接近对象物的形状的基准图像，以算出对象物和机械手的手头的位置关系。

另外，在专利文献2(日本特开2000-263482号公报)中，记载了以下机器人装置：预先准备对象物的基准图像，在由安装在机械手的手头上的摄像机拍摄图像的同时，使机械手移动一定量，直到得到接近基准图像的图像。

再有，在专利文献3(日本特开2006-318301号公报)中，记载了如下机器人装置：推定表示从利用摄像机在机械手动作过程中所拍摄的图像中求出的图像特征量和机械手的各关节的动作速度的关系的雅可比矩阵，并基于该雅可比矩阵生成对机械手的指令信号。

然而，专利文献1所记载的机器人装置，为了提高控制精度，需要准备大量的基准图像，机械手每进行一次动作，就需要进行全部基准图像的检索，因而计算负担很大，不能进行迅速的控制。

此外，专利文献2所记载的机器人装置，由于只能与二维平面内的机械手的位置及姿势一致应对，所以不能在三维空间内控制机械手对对象物的位置及姿势。还有，在这种机器人装置中，由于必须反复进行摄像、计测和机械手的动作，所以不能进行快速的控制。此外，在这种机器人装置中，还有必要进行摄像机镜头的校直或对正坐标***等的摄像机校准工作，而且，机械手联杆长度的误差或坐标***的错位所造成的误差会积累起来，从而会有动作精度变劣的危险。

并且，专利文献3所记载的机器人装置，为了推定雅可比矩阵，使用弗罗因德法求解雅可比矩阵的解析式，但由于其仅求解非线性方程式，所以存在对模型化误差较弱的问题。

发明内容

于是，本发明是鉴于上述实情而提出的技术方案，其目的在于，提供一种在控制机器人装置的机械手时，不必准备很多的基准图像，计算负担较轻，可进行迅速的控制，并且能够在三维空间内控制机械手对对象物的位置及姿势的机器人装置的控制方法及机器人装置。

为了解决上述问题，实现上述目的，涉及本发明的机器人装置的控制方法具有以下结构的任意一种。

(结构1)

涉及本发明的机器人装置的控制方法，该机械人装置具有六个自由度以上的机械手，在机械手的手头位置上具有摄像单元，该机器人装置的控制方法的特征在于，基于处理由摄像单元获得的对象物的图像而得到的图像特征量，使用迭代最小二乘法推定图像特征量的变化速度和机械手的各关节的动作速度的关系式即雅可比矩阵，并与事先在目标位置获得的对象物的图像特征量进行对比后，以机械手的手头位置接近目标位置的方式，求出各关节的动作速度并作为机械手的指令信号，并基于该指令信号使机械手动作，使手头位置成为目标位置。

(结构2)

具有结构1的机器人装置的控制方法，其特征在于，在雅可比矩阵的推定中，使忘却系数不是固定而是可动地变化。

(结构3)

具有结构1或结构2的机器人装置的控制方法，其特征在于，在推定雅可比矩阵之前，稍微驱动机械手后，求出各关节的动作速度和图像特征量的变化速度的关系，通过该关系决定雅可比矩阵的初始解。

(结构4)

涉及本发明的机器人装置，其特征在于，具备六个自由度以上的机械手；设置在机械手的手头位置上的摄像单元；以及生成对机械手的指令信号的控制单元，控制单元基于处理由摄像单元获得的对象物的图像而得到的图像特征量，使用迭代最小二乘法推定图像特征量的变化速度和机械手的各关节的动作速度的关系式即雅可比矩阵，并与事先在目标位置获得的对象物的图像特征量进行对比后，以机械手的手头位置接近目标位置的方式，求出各关节的动作速度并生成对机械手的指令信号，并基于该指令信号使机械手动作，使手头位置成为目标位置。

(结构5)

具有结构4的机器人装置，其特征在于，控制单元在雅可比矩阵的推定中，使忘却系数不是固定而是可动地变化。

(结构6)

具有结构4或结构5的机器人装置，其特征在于，控制单元在推定雅可比矩阵之前，稍微驱动机械手后，求出各关节的动作速度和图像特征量的变化速度的关系，通过该关系决定雅可比矩阵的初始解。

本发明具有以下效果。

涉及本发明的机器人装置的控制方法，通过具有结构1，基于处理由摄像单元获得的对象物的图像而得到的图像特征量，使用迭代最小二乘法推定图像特征量的变化速度和机械手的各关节的动作速度的关系式即雅可比矩阵，并与事先在目标位置获得的对象物的图像特征量进行对比后，以机械手的手头位置接近目标位置的方式，求出各关节的动作速度并作为机械手的指令信号，并基于该指令信号使机械手动作，使手头位置成为目标位置，因此，由于不必准备很多的基准图像，从而计算负担较轻，并可进行迅速的控制。并且，该机器人装置的控制方法，通过选择适当的初始解且使用迭代最小二乘法进行雅可比矩阵的推定，从而使雅可比矩阵的推定运算的收敛迅速化，并可提高可靠性。另外，能够在三维空间内控制机械手。

涉及本发明的多个机器人装置的控制方法，由于通过具有结构2，在雅可比矩阵的推定中，使忘却系数不是固定而是可动地变化，所以能够进行更加正确的推定。

涉及本发明的多个机器人装置的控制方法，由于通过具有结构3，在推定雅可比矩阵之前，稍微驱动机械手后，求出各关节的动作速度和图像特征量的变化速度的关系，通过该关系决定雅可比矩阵的初始解，因此，可选择雅可比矩阵的适当的初始解，使推定运算的收敛迅速化并可提高可靠性。

并且，涉及本发明的多个机器人装置，由于通过具有结构4，控制单元基于处理由摄像单元获得的对象物的图像而得到的图像特征量，使用迭代最小二乘法推定图像特征量的变化速度和机械手的各关节的动作速度的关系式即雅可比矩阵，并与事先在目标位置获得的对象物的图像特征量进行对比，以机械手的手头位置接近目标位置的方式，求出各关节的动作速度并生成机械手的指令信号，并基于该指令信号使机械手动作，使手头位置成为目标位置，因此，由于不必准备很多的基准图像，从而计算负担较轻，并可进行迅速的控制。并且，该机器人装置，通过选择适当的初始解且使用迭代最小二乘法进行雅可比矩阵的推定，从而使雅可比矩阵的推定运算的收敛迅速化，并可提高可靠性。另外，能够在三维空间内控制机械手。

涉及本发明的多个机器人装置，由于通过具有结构5，控制单元在雅可比矩阵的推定中，使忘却系数不是固定而是可动地变化，所以能够进行更加正确的推定。

涉及本发明的多个机器人装置，由于通过具有结构6，控制单元在推定雅可比矩阵之前，稍微驱动机械手后，求出各关节的动作速度和图像特征量的变化速度的关系，通过该关系决定雅可比矩阵的初始解，因此，可选择雅可比矩阵的适当的初始解，使推定运算的收敛迅速化并可提高可靠性。

即，本发明可提供一种在控制机器人装置的机械手时，不必准备很多的基准图像，计算负担较轻，可进行迅速的控制，并且能够在三维空间内控制机械手对对象物的位置及姿势的机器人装置的控制方法及机器人装置。

附图说明

图1是表示涉及本发明的机器人装置的结构的模式侧视图。

图2是表示涉及本发明的机器人装置的控制方法的顺序的流程图。

图中：

1—机械手，1a—第一联杆，1b—第二联杆，1c—第三联杆，1d—第四联杆，1e—第五联杆，1f—第六联杆，2a—第一关节，2b—第二关节，2c—第三关节，2d—第四关节，2e—第五关节，2f—第六关节，3—计算机，3a—存储器，3b—图像处理部，3c—指令信号生成部，5—前端工具机构，6—摄像机，101—对象物。

具体实施方式

下面，参照附图说明用于实施本发明的优选实施方式。

(机器人装置的结构)

图1是涉及本发明的机器人装置的结构的模式侧视图。

如图1所示，涉及本发明的机器人装置具有六个自由度的机械手1，通过该机械手1，可以夹持、搬运加工对象物101，或者把它组装到其它的部件上。

机械手1是由多个执行元件(驱动装置)和联杆(刚性的结构件)构成的，具有六个自由度。即，各联杆之间借助于能够转动(弯曲)或者可旋转的关节2a、2b、2c、2d、2e、2f连接，而且各自通过执行元件相对驱动。各执行元件由作为控制单元的计算机3所控制。

在该机械手1中，第一联杆(基端部的联杆)1a设置成通过第一关节2a将基端一侧连接在基台部分4上。第一关节2a是可绕垂直轴(z轴)旋转的关节。第二联杆1b的基端一侧通过第二关节2b连接在该第一联杆1a的前端一侧。第二关节2b是可使第二联杆1b绕水平轴转动的关节。第三联杆1c的基端一侧通过第三关节2c连接在第二联杆1b的前端一侧。第三关节2c是可使第三联杆1c绕水平轴转动的关节。

并且，第四联杆1d的基端一侧通过第四关节2d连接在第三联杆1c的前端一侧。第四关节2d是可使第四联杆1d绕第三联杆1c的轴旋转的关节。第五联杆1e的基端一侧通过第五关节2e连接在第四联杆1d的前端一侧。第五关节2e是可使第五联杆1e绕与第四联杆1d的轴垂直相交的轴转动的关节。第六联杆1f的基端一侧通过第六关节2f连接在第五联杆1e的前端一侧。第六关节2f是可使第六联杆1f绕第五联杆1e的轴旋转的关节。

这样，在机械手1中，由于交替地设置了共计六个可转动的关节和可旋转的关节，从而确保了六个自由度。

在第六联杆1f的前端一侧(下文中称为“手头”)上，设有夹持或者加工对象物101的前端工作机构5。该前端工作机构5，由计算机3或者图中未表示的其它控制装置所控制。另外，在手头附近，安装有构成为由摄像镜头及CCD或CMOS之类的固体摄像元件组成的摄像单元的摄相机6。

计算机3具有存储器3a。在该存储器3a中存储基于使机械手1的前端工具机构5处于目标位置以及目标姿势时由摄像机6获得的对象物101的图像的图像特征量。存储在存储器3a中的图像特征量被送到图像处理部3b。该图像处理部3b求出基于在机械手1的动作中由摄像机6获得的对象物101的图像的图像特征量，并送到指令信号生成部(雅可比矩阵推定部)3c。指令信号生成部3c使用迭代最小二乘法推定图像特征量的变化速度和机械手1的各关节2a、2b、2c、2d、2e、2f的动作速度的关系式即雅可比矩阵。并且，与存储在存储器3a中的图像特征量进行对比，求出使机械手1的前端工具机构5能够接近目标位置的前端工具机构5的速度，并从该前端工具机构5和雅可比矩阵求出各关节2a、2b、2c、2d、2e、2f的动作速度后作为机械手1的指令信号输出。该指令信号被送到机械手1。机械手1基于所送的指令信号而动作，使前端工具机构5移动到目标位置。即，在机械手1中，通过以第二联杆1b对第一联杆1a的位置、第三联杆1c对第二联杆1b的位置、第四联杆1d对第三联杆1c的位置的顺序依次控制前端一侧的联杆位置来控制前端工具机构5的位置，从而可使该前端工具机构5移动到规定位置。

(机器人装置的控制方法)

并且，在该机器人装置中，通过执行以下所示的涉及本发明的机器人装置的控制方法，可自动控制机械手1。即，在该机器人装置中，以使事先从在目标位置所获得的图像算出并存储的图像特征量与通过摄像机6获得的图像算出的图像特征量一致的方式，使机械手1动作。

在该机器人装置中，在雅可比矩阵的推定中，通过选择适当的初始解，而且使用迭代最小二乘法，能够加速推定运算的收敛，提高可靠性。

另外，在本发明中使用的雅可比矩阵，是图像特征量的变化速度△f(θ)对机械手1的某一关节的动作角度为θ的瞬间的关节动作速度△θ的关系，是以下所示矩阵的J。

△f(θ)＝J(θ)△θ......(公式1)

图2是表示涉及本发明的机器人装置的控制方法的顺序的流程图。

在该机器人装置中，如图2所示，计算机3执行涉及本发明的机器人装置的控制方法。即，计算机3若在步骤st1中开始控制，则进入步骤st2，稍微驱动机械手1，求出各关节2a、2b、2c、2d、2e、2f的动作速度与图像特征量的变化速度的关系，通过该关系决定雅可比矩阵的初始解。接着，进入步骤st3，使用迭代最小二乘法(以下称“RLS”)进行用于进行雅可比矩阵的推定的初始设定。

然后，进入步骤st4，开始图像伺服控制，并进入步骤st5。在步骤st5中，利用摄像机6获得对象物101的图像，从该图像进行图像特征量的算出，进入步骤st6。

在步骤st6中，使用RLS推定雅可比矩阵的值，进入步骤st7。在步骤st7中，基于由摄像机6得到的图像的图像特征量和存储在存储器3a中的目标位置的图像特征量的偏差，使用雅可比矩阵的逆矩阵，算出各关节2a、2b、2c、2d、2e、2f的动作速度，进入步骤st8。在步骤st8中，将算出的各关节2a、2b、2c、2d、2e、2f的动作速度作为指令信号输出到机械手1，进入步骤st9。

在步骤st9中，判断是否达到了控制的结束条件。作为该结束条件，例如是在对象物101静止的场合，由摄像机6得到的图像的图像特征量和存储在存储器3a中的目标位置上的图像特征量的偏差在阈值以下的场合。或者，在对象物101动作的场合，判断前端工具机构5是在对象物101的动作范围内还是到达了最终位置。或者，判断前端工具机构5是已经夹持了对象物101还是需要变更对象物101。如果没有达到结束条件，便返回步骤st5，如果达到了结束条件，则进入步骤st10。

在步骤st10中，结束图像伺服控制，进入步骤st11，结束控制。

(雅可比矩阵的初始解的算出)

如上所述，在进行雅可比矩阵的推定时，有必要适当地设定初始解。初始解的设定，简单地说，只要是适当的值例如单位矩阵等即可，但是为了进行更正确的推定，最好利用(公式1)求出在使机械手1的关节以速度△θ稍微动作时，图像特征量以何种速度△f动作。

具体地说，由于在机械手1中有六个关节，所以若求出排列了仅使第i关节(其中i＝1～6)稍微动作时的m个图像特征量的向量的变化速度△fj(其中j＝1～m)，则可得到以下公式。

Ji0＝[△f1…△fm]diag(△θ1，…，△θ6)^-1…(公式2)

此外，diag(△θ1，…，△θ6)作为对角元素是△θ1～△θ6所并列的6×6的对角矩阵，若图像特征量为m个，则[△f1…△f6]为m×6的矩阵。

(设定其他初始解)

将机械手1的关节的轴数设为n、将图像特征量的数设为m时，将(n+1)×(n+1)的零矩阵为P0、将m×n的雅可比矩阵的初始解为Ji0、将m×1的零矩阵为Jt0。

这里，使雅可比矩阵的推定所使用的忘却系数λ为比0大且在1以下的值。忘却系数λ越大，过去值越影响当前值，也就是说，由于在时间轴的数据的影响度变大，所以变得对干扰较强，但收敛变得迟缓。反之，若使忘却系数λ减小，则过去值不影响当前值，所以收敛速度加快，但对干扰或初始解的偏差等较弱。

(获得图像、算出特征量)

从通过摄像机6获得的图像中求出的图像特征量，例如对象物101为圆形，则是圆的中心位置和半径，若对象物101为椭圆，则是中心和长径、短径及倾角，若对象物为四边形，则是4个顶点的位置等。

将排列了这种图像特征量的向量设为fk(其中，k是指控制步骤数的整数)。将从最初获得的图像得到的图像特征量设为f0。

(雅可比矩阵的推定)

将从控制开始的时刻或当前的时刻设为tk(k是指控制步骤数的整数)，将控制刚刚开始后的时刻设为t0。将关节角度设为θk，将控制刚刚开始后的角度设为θ0。并且，将k＝1作为初始解，求解以下公式。

数学式1

Δf＝f_k-f_k-1；h_θ＝θ_k-θ_k-1；h_t＝t_k-t_k-1

$h=\left[\begin{array}{c}{h}_{\mathrm{\θ}}\\ h_t\end{array}\right]$

J_k＝[J_ik  J_tk]

ΔJ_k＝(Δf-J_k-1h)(λ+h^TP_k-1h)^-1h^TP_k-1

J_k＝J_k-1+ΔJ_k

$P_k=\frac{1}{\mathrm{\λ}}[P_{k-1}-\frac{P_{k-1}h{h}^T{P}_{k-1}}{\mathrm{\λ}+h^T{P}_{k-1}h}]$

${\mathrm{\θ}}_{k+1}={\mathrm{\θ}}_k-{\left(J_{\mathrm{ik}}^T{J}_{\mathrm{ik}}\right)}^{-1}(J_{\mathrm{ik}}^T{J}_{\mathrm{tk}}+J_{\mathrm{ik}}^T{J}_{\mathrm{tk}}h)$

这里，f是图像特征量，θ是机械手1的关节的动作角度，t是时刻，Jik是(公式1)中的雅可比矩阵，λ是忘却系数(调节参数)。其它不是具有物理意义的数值，而是进行迭代最小二乘法的计算过程中所计算的值。

在该推定中，每一个控制周期增加一个k值。通过在每个控制周期重复该计算，得到新的关节角度θk+1。并且，以机械手1的每个关节为该角度的方式使机械手1动作。

(包含忘却系数的适应调节的雅可比矩阵的推定)

如上所述，通过可动地变更忘却系数λ，能够进一步提高收敛速度。将λk的初始解λ0选择为适当的值，例如为1。在该场合，在个控制周期的计算式如下。

数学式2

△f＝f_k-f_k-1；h_θ＝θ_k-θ_k-1；h_t＝t_k-t_k-1

$h=\left[\begin{array}{c}{h}_{\mathrm{\θ}}\\ h_t\end{array}\right]$

J_k＝[J_ik  J_tk]

△J_k＝(△f-J_k-1h)(λ+h^TP_k-1h)^-1h^TP_k-1

J_k＝J_k-1+△J_k

$K_k=\frac{{\mathrm{\λ}}^{-1}{P}_{k-1}h}{1+{\mathrm{\λ}}^{-1}{h}^T{P}_{k-1}h}$

$S_k={\mathrm{\λ}}_{k-1}^{-1}[I-K_k{h}^T]S_{k-1}\left[I{-\mathrm{hK}}_k^T\right]+{\mathrm{\λ}}_{k-1}^{-1}{K}_k{K}_k^T-{\mathrm{\λ}}_{k-1}^{-1}{P}_{k-1}$

Ψ_k＝[I-K_kh^T]Ψ_k-1+(△f-J_k-1h)(S_kh)^T

${\mathrm{\λ}}_k=[{\mathrm{\λ}}_{k-1}+\mathrm{\α}{(\mathrm{\Δf}-J_{k-1}h)}^T{\mathrm{\ψ}}_{k}h]\begin{array}{c}{\mathrm{\λ}}_{+}\\ {\mathrm{\λ}}_{-}\end{array}$

$P_k=\frac{1}{{\mathrm{\λ}}_k}[P_{k-1}-\frac{P_{k-1}h{h}^T{P}_{k-1}}{{\mathrm{\λ}}_k+h^T{P}_{k-1}h}]$

${\mathrm{\θ}}_{k+1}={\mathrm{\θ}}_k-{\left(J_{\mathrm{ik}}^T{J}_{\mathrm{ik}}\right)}^{-1}(J_{\mathrm{ik}}^T{J}_{\mathrm{tk}}+J_{\mathrm{ik}}^T{J}_{\mathrm{tk}}h)$

这里，f是图像特征量，θ是机械手1的关节的动作角度，t是时刻，Jik是(公式1)中的雅可比矩阵，λk是忘却系数(调节参数)。

此外，α是微小的正数，是调节λk的可动地变化的比率的参数。若将α设为较大的值，则λk的值较大地变化，对干扰较弱。反之，越将α设定为较小的值，适应越慢，但变化稳定。

另外，求λk的公式中的[]^λ+ _λ-指，在[]内的计算结果为大于λ+的值时，使计算结果为λ+，在计算结果小于λ-的值时，使计算结果为λ-。这是为了因有必要使λ为比0大且在1以下的值，而使结算结果不超出该范围。另外，通过调节该值，限制可可动地得到的λk的值的范围，也能够调节收敛速度和可靠性。

Claims (6)

1.一种机器人装置的控制方法，该机器人装置具有六个自由度以上的机械手，在机械手的手头位置上具有摄像单元，该机器人装置的控制方法的特征在于，

基于处理由上述摄像单元获得的上述对象物的图像而得到的图像特征量，使用迭代最小二乘法推定图像特征量的变化速度和机械手的各关节的动作速度的关系式即雅可比矩阵，并与事先在目标位置获得的上述对象物的图像特征量进行对比后，以机械手的手头位置接近上述目标位置的方式，求出各关节的动作速度并作为机械手的指令信号，并基于该指令信号使上述机械手动作，使手头位置成为上述目标位置。

2.根据权利要求1所述的机器人装置的控制方法，其特征在于，

在上述雅可比矩阵的推定中，使忘却系数不是固定而是可动地变化。

3.根据权利要求1或2所述的机器人装置的控制方法，其特征在于，

在推定上述雅可比矩阵之前，稍微驱动上述机械手后，求出各关节的动作速度和图像特征量的变化速度的关系，并通过该关系决定上述雅可比矩阵的初始解。

4.一种机器人装置，其特征在于，具备：

六个自由度以上的机械手；

设置在上述机械手的手头位置上的摄像单元；以及

生成对上述机械手的指令信号的控制单元，

上述控制单元基于处理由上述摄像单元获得的上述对象物的图像而得到的图像特征量，使用迭代最小二乘法，推定图像特征量的变化速度和机械手的各关节的动作速度的关系式即雅可比矩阵，并与事先在目标位置获得的上述对象物的图像特征量进行对比后，以机械手的手头位置接近上述目标位置的方式，求出各关节的动作速度并生成对机械手的指令信号，并基于该指令信号使上述机械手动作，使手头位置成为上述目标位置。

5.根据权利要求4所述的机器人装置，其特征在于，

上述控制单元在上述雅可比矩阵的推定中，使忘却系数不是固定而是可动地变化。

6.根据权利要求4或5所述的机器人装置，其特征在于，

上述控制单元在推定上述雅可比矩阵之前，稍微驱动上述机械手后，求出各关节的动作速度和图像特征量的变化速度的关系，并通过该关系决定上述雅可比矩阵的初始解。

Images