机器人控制装置

摘要

提供一种机器人控制装置(5)，能够使机器人手臂高精度地追随任意移动的工件。该机器人控制装置(5)包括：特征点检测部(51)，用于以预定的周期从通过视觉传感器拍摄的对象物的图像中检测对象物中的多个特征点的位置；位置姿势计算部(52)，基于检测到的多个特征点的位置，在每个预定的周期更新多个特征点的各个运动方程式，并基于根据更新后的各运动方程式计算出的多个特征点的各自位置，计算出对象物的位置或者姿势；以及机器人手臂动作控制部(53)，基于计算出的对象物的位置或者姿势，控制机器人手臂(4)的动作，使其追随对象物。

权利要求

1.一种机器人控制装置，其包括：

特征点检测部，用于以预定的周期从通过视觉传感器拍摄的对象物的图像中检测所述对象物中的多个特征点的位置；

位置姿势计算部，基于所述检测到的多个特征点的位置，在每个所述预定的周期更新所述多个特征点的各个运动方程式，并基于根据更新后的各所述运动方程式计算出的所述多个特征点的各自位置，计算出所述对象物的位置或者姿势；以及机器人手臂动作控制部，基于所述计算出的对象物的位置或者姿势，控制机器人手臂的动作，以使所述机器人手臂追随所述对象物，所述位置姿势计算部基于所述多个特征点的检测频率，对所述对象物中的所述多个特征点进行加权。

2.根据权利要求1所述的机器人控制装置，其中，

所述机器人手臂动作控制部具有特异点回避部，所述特异点回避部用于回避所述机器人手臂的特异点。

说明书

机器人控制装置

技术领域

[0001] 本发明涉及一种适用于通过视觉伺服对机器人手臂进行控制的机器人控制装置。

背景技术

[0002] 以往，作为一种控制机器人手臂的技术，已知有通过摄像机等视觉传感器对连续移动的工件(对象物)的位置以及姿势进行连续计测，并基于该计测结果使机器人手臂追随对象物的视觉伺服(例如，参照专利文献1和以及专利文献2)。

[0003] 现有技术文献

[0004] 专利文献

[0005] 专利文献1：日本专利第4265088号公报

[0006] 专利文献2：日本特开2011-143494号公报

发明内容

[0007] 发明所要解决的问题

[0008] 然而，在上述专利文献1以及专利文献2所记载的机器人控制装置中，由于将固定工件的移动速度作为假设条件，因此针对任意移动的工件，存在无法使机器人手臂高精度地追随的问题。

[0009] 并且，由于在通过摄像机无法检测到工件时，无论工件的一部分是否进入到摄像机的视野内，都根据工件的运动方程式来计算工件的位置，因此存在计算出的工件位置与实际的工件位置之间出现偏差，导致追随精度下降的问题。

[0010] 本发明是鉴于上述情况而做出的，其目的在于提供一种能够使机器人手臂高精度地追随任意移动的工件的机器人控制装置。

[0011] 解决问题的手段

[0012] 为了实现上述目的，本发明提供如下手段。

[0013] 本发明的第一方面的机器人控制装置，其包括：特征点检测部，用于以预定的周期从通过视觉传感器拍摄的对象物的图像中检测所述对象物中的多个特征点的位置；位置姿势计算部，基于所述检测到的多个特征点的位置，在每个所述预定的周期更新所述多个特征点的各个运动方程式，并基于根据更新后的各所述运动方程式计算出的所述多个特征点的各自位置，计算出所述对象物的位置或者姿势；以及机器人手臂动作控制部，基于所述计算出的对象物的位置或者姿势，控制机器人手臂的动作，使其追随所述对象物。

[0014] 根据上述第一方面所述的机器人控制装置，通过特征点检测部，从由视觉传感器拍摄的对象物的图像中，以预定的周期检测对象物上的多个特征点的位置。其次，通过位置姿势计算部，基于通过特征点检测部检测到的多个特征点的位置，在每个预定的周期更新多个特征点的各个运动方程式。并且，通过位置姿势计算部，根据更新后的各运动方程式重新计算出多个特征点的各自位置，并且基于计算出的多个特征点的位置，计算出对象物的位置或者姿势。之后，通过机器人手臂动作控制部，基于由位置姿势算出部算出的对象物的位置或者姿势，控制机器人手臂使其追随对象物。

[0015] 像这样，由于各特征点的运动方程式在预定的周期得到更新，因此能够实现在线建立对象物的运动方程式。其结果，使机器人手臂能够高精度地追随任意移动的对象物。

[0016] 在上述机器人控制装置中，所述位置姿势计算部还可以被构成为，基于所述多个特征点的检测频率，对所述对象物中的所述多个特征点进行加权。

[0017] 通过这样，即使是在多个特征点中的一部分被障碍物等遮挡、视觉传感器无法直接检测到对象物的位置或姿势的情况下，也可以根据运动方程式计算出的各特征点的位置以及通过视觉传感器可能检测到的各特征点的位置进行基于检测频率的加权，计算出对象物的位置，从而能够使机器人手臂高精度地追随对象物。

[0018] 在上述机器人控制装置中，所述机器人手臂动作控制部还可以被构成为具有特异点回避部，用于回避所述机器人手臂的特异点。

[0019] 由于通过该构成可以回避特异点，因此可以避免机器人手臂中的特定关节异常高速旋转等的操作上的故障。

[0020] 发明效果

[0021] 根据本发明，能够实现使机器人手臂高精度地追随任意移动的工件的效果。

附图说明

[0022] 图1是示出根据本发明一实施方式的机器人系统的概要构成的图。

[0023] 图2是图1示出的机器人系统的框图。

[0024] 图3是示出图2示出的机器人控制装置中的处理的流程图。

[0025] 图4是示出图2示出的位置姿势计算部中的处理的流程图。

[0026] 图5是示出图2示出的机器人手臂动作控制部中的处理的流程图。

具体实施方式

[0027] 下面，参照附图详细说明涉及本发明的机器人系统的实施方式。需要说明的是，在本说明书中，在英文字母的上面标注的^符号表示“(英文字母)hat”，在英文字母的上面加的横杠符号表示“(英文字母)bar”，在英文字母的上面标注的点符号表示“(英文字母)dot”。

[0028] 如图1示出，本实施方式的机器人系统1具有摄像机2、用于支撑摄像机2的支架3、机器人手臂4、以及连接有摄像机2和机器人手臂4的机器人控制装置5。机器人控制装置5被构成为，基于从摄像机2获取的视觉信息进行视觉伺服的控制，以使机器人手臂4追随工件(对象物)7的移动。

[0029] 工件7放置在传送带或旋转台等输送装置6上，且工件7随着输送装置6的移动而进行移动。

[0030] 需要说明的是，虽然在本实施方式中摄像机2支承于支架3，但并不限定于此，摄像机2也可以装载于机器人手臂4。

[0031] 下面，参照图2详细说明机器人系统1所具有的机器人控制装置5的构成。需要说明的是，机器人控制装置5具有未图示的处理器，且被构成为该处理器执行图2示出的各功能模块中的处理。

[0032] 如图2示出，机器人控制装置5包括作为功能模块的特征点检测部51、位置姿势计算部52以及机器人手臂动作控制部53。并且，位置姿势计算部52具有作为功能模块的运动方程式更新部521、特征点位置计算部522、权重计算部523以及旋转和位置矩阵计算部524。机器人手臂动作控制部53具有作为功能模块的轨道生成部531、特异点回避部532以及加法部533。

[0033] 特征点检测部51与摄像机2、位置姿势计算部52所具有的运动方程式更新部521以及权重计算部523相连接。运动方程式更新部521与特征点位置计算部522相连接。特征点位置计算部522与权重计算部523以及旋转和位置矩阵计算部524相连接。权重计算部523与旋转和位置矩阵计算部524相连接。

[0034] 旋转和位置矩阵计算部524与机器人手臂动作控制部53所具有的轨道生成部531连接。轨道生成部531与特异点回避部532连接。轨道生成部531以及特异点回避部532与加法部533连接。加法部533与机器人手臂4连接。

[0035] 特征点检测部51被构成为，按照事先设定的形状对通过摄像机2拍摄到的包含工件7的图像进行模式匹配，并检测与该形状一致的图像中的工件7的多个特征点的位置yk(tik)。在此，检测到的位置yk(tik)具体表示的是第k个特征点的、在通过摄像机检测工件的时刻tik的包括x、y以及z方向的位置矢量。

[0036] 运动方程式更新部521被构成为，利用通过特征点检测部51检测到的各特征点的位置yk(tik)，对各特征点的运动方程式进行更新。对于从摄像机2拍摄的图像中检测的各特征点的位置，如果利用运动方程式表示，则可以如下述的公式(1)。

[0037] 【数学式1】

[0038]

[0039] 其中，fk[yk(ti-1k)，tik]表示各特征点的运动方程式，vik表示传感器噪声。即，基于k k利用前一个周期在时刻ti-1 检测的位置yk(ti-1)而更新的运动方程式和传感器噪声，来表示在时刻tik的检测的位置矢量。

[0040] 需要说明的是，各特征点的运动方程式可以通过例如公式(2)以及公式(3)表示。

[0041] 【数学式2】

[0042]

[0043] 【数学式3】

[0044] yk(t)＝[I3 0 0]xk(t)+v(t)  (3)

[0045] 其中，xk(t)表示在时刻t的包括位置、速度以及加速度的各特征点的9维矢量，w(t)表示作为特征点的速度变化主要原因的加速度矢量，v(t)表示通过摄像机进行检测时的噪声。

[0046] 更加具体地，运动方程式更新部521被构成为，按照下面的公式(4)以及公式(5)来计算ybark(tik)，并基于此更新各运动方程式。

[0047] 【数学式4】

[0048]

[0049] 【数学式5】

[0050]

[0051] 在此，yhatk(tik)是在时刻tik的通过更新后的运动方程式求出的计算值，且该运动方程式是利用前一周期的在时刻ti-1k的位置ybark(ti-1k)而更新的。并且，公式(5)中的ybarkk k k hat k(ti)表示考虑到摄像机2在时刻ti的检测值yk(ti)和利用公式(4)求出的计算值y k(ti)的双方而计算出的各特征点的位置。需要说明的是，在公式(5)中，Fk(tik)表示yk(tik)与yhatk(tik)的差分的增益。

[0052] 特征点位置计算部522被构成为，利用通过如上所述求出的在时刻tik的位置ybark(tik)而更新的运动方程式，如公式(6)那样，计算在当前时刻tnow的各特征点的位置yhatk(tnow)。

[0053] 【数学式6】

[0054]

[0055] 权重计算部523被构成为计算各特征点的权重。具体被构成为，计算出当前时刻tnow中的各特征点的位置的协方差Σhatk(tnow)作为各特征点的权重，以使通过摄像机2检测的次数(频率)越多的特征点的重要度越高，如公式(7)所示。

[0056] 【数学式7】

[0057]

[0058] 在此，如公式(8)所示，gk是对运动方程式fk求出协方差的结果。

[0059] 【数学式8】

[0060] gk＝Cov(fk)  (8)

[0061] 并且，Σbark(tik)是考虑了摄像机2在时刻tik检测的检测值y(tik)的协方差和根据运动方程式求出的计算值yhatk(tik)的协方差的双方而计算出的协方差。

[0062] 需要说明的是，当存在未能检测到的特征点时，利用上次检测到的信息来计算该特征点的位置。

[0063] 旋转和位置矩阵计算部524被构成为，针对各特征点，利用通过权重计算部523计算出的协方差Σhatk(tnow)、通过特征点位置计算部522计算出的特征点的位置yhatk(tnow))，按照公式(9)，计算对于各特征点进行加权的工件7的旋转矩阵以及位置矩阵。

[0064] 【数学式9】

[0065]

[0066] 在此，Rw表示机器人世界坐标系中的工件7的旋转矩阵，Tw表示机器人世界坐标系中的工件7的位置矩阵，sk表示从固定于工件7的物体坐标系观察到的特征点的位置(包括x、y以及z方向的矢量)。但是，当存在未能检测到的特征点时，利用上次检测到的结果作为该特征点的位置。

[0067] 轨道生成部531被构成为生成机器人手臂4的轨道。具体地，求出由机器人手臂动作控制部53生成的当前时刻的TCP的位置以及姿势(旋转矩阵Rr以及位置矩阵Tr)与通过位置姿势计算部52计算出的工件7的位置以及姿势(旋转矩阵Rw以及位置矩阵Tw)的差分，使该差分乘以雅可比的倒数(雅可比的逆矩阵)，计算出各轴角度的差分，即机器人手臂4的指令速度qdot*。

[0068] 【数学式10】

[0069]

[0070] 其中，J+是各轴位置与正交位置之间的雅可比变换的倒数。h(ΔR，ΔT)是表示在位置以及姿势的差分ΔR、ΔT上乘以比例增益的函数，基于需要追随的工件7的移动频率来调整比例增益。

[0071] 特异点回避部532被构成为，当由机器人手臂动作控制部53生成的当前时刻的TCPdot位于特异点附近时，通过公式(11)生成用于进行回避动作的插补速度Δq 。

[0072] 【数学式11】

[0073]

[0074] 在此，p(q)是用于校正速度的函数，是各轴的校正速度。可以根据公式(12)以及公式(13)求出各轴的校正速度p(q)。

[0075] 【数学式12】

[0076]

[0077] 【数学式13】

[0078]

[0079] H(q)是表示机器人可操纵性(manipulability)的公式。如公式(13)所示，根据H(q)的偏微分计算各轴的校正速度p(q)。

[0080] 在上述的公式(13)中，λ0表示用于回避特异点的比例增益。需要说明的是，比例增益在TCP的位置以及姿势不在特异点附近时，λ0＝0，TCP的位置以及姿势位于特异点附近时，λ0的值变大。

[0081] 加法部533被构成为，由轨道生成部531生成的指令速度qdot*和由特异点回避部计算出的插补速度Δqdot相加，从而计算出机器人手臂4的各轴速度指令qdot。

[0082] 【数学式14】

[0083]

[0084] 通过计算出的各轴速度指令qdot，对机器人手臂4进行控制。

[0085] 下面，参照图3至图5，对具有上述构成的机器人控制装置5的作用进行说明。图3至图5示出了机器人控制装置5所具有的未图示的处理器在每个预定周期，即机器人手臂4的每个插补周期执行的处理。

[0086] 首先，由摄像机2拍摄到的图像被输入到机器人控制装置5(图3的步骤S1)。之后，在特征点检测部51中，从输入到机器人控制装置5的图像中检测工件7中的多个特征点的位置(图3的步骤S2)。接着，在位置姿势计算部52中，基于检测到的工件7上的多个特征点的位置，计算工件7的位置以及姿势(图3的步骤S3)。之后，在机器人手臂动作控制部53中，基于位置姿势计算部52所计算的工件7的位置以及姿势，生成针对机器人手臂4的各轴的速度指令(图3的步骤S4)，并输出到机器人手臂4(图3的步骤S5)。

[0087] 其次，参照图4对图3的步骤S3中进行的工件7的位置姿势计算处理进行说明。

[0088] 首先，在运动方程式更新部521中，计算出基于各特征点的检测位置yk(tik)和在时刻tik的各特征点的算出位置yhatk(tik)而得到的时刻tik的各特征点的实际位置ybark(tik)k(参照公式(4)以及公式(5))，其中，所述各特征点的检测位置yk(ti)是通过特征点检测部
51在时刻tik检测的，所述各特征点的算出位置yhatk(tik)是根据在时刻ti-1k更新的运动方程式而计算的。之后，利用算出的实际位置ybark(tik)，更新运动方程式(图4的步骤S21)。

[0089] 接着，如公式(6)所示，在特征点位置计算部522中，利用更新后的运动方程式，计hat算当前时刻tnow的各特征点的位置y k(tnow)(图4的步骤S22)。

[0090] 这样，由于运动方程式在机器人手臂4的每个插补周期得到更新，所以即使在工件7任意移动时，也能够高精度地计算出工件7的当前位置。

[0091] 其次，在权重计算部523中，基于特征点检测部51在时刻tik检测到的各特征点的检k k k测位置yk(ti)的协方差和根据更新后的运动方程式在时刻ti-1计算的时刻ti 中的各特征点的算出位置yhatk(tik)的协方差的双方，按照公式(7)，计算出在当前时刻tnow的各特征点的位置yhatk(tnow)的协方差Σhatk(tnow)(步骤S23)。

[0092] 之后，在旋转和位置矩阵计算部524中，利用由权重计算部523计算的作为权重的协方差Σhatk(tnow)和由特征点位置计算部522计算的各特征点的位置yhatk(tnow)，按照公式(9)，计算出工件7的旋转矩阵Rw以及位置矩阵Tw(图4的步骤S24)。

[0093] 这样，由于对各特征点进行了加权，以使由摄像机2检测的次数越多其重要度越高，因此，即使是在例如障碍物等遮挡了工件7的一部分而无法检测到特征点的情况下，也能够使机器人手臂4精确地追随工件7。

[0094] 其次，参照图5对图3的步骤S4中进行的机器人手臂动作控制处理进行说明。

[0095] 首先，在轨道生成部531中，计算机器人手臂4的TCP的位置以及姿势(旋转矩阵Rr以及位置矩阵Tr)(图5的步骤S31)。接着，通过位置姿势计算部52计算的工件7的旋转矩阵Rw以及位置矩阵Tw，即与工件7的位置以及姿势的差分ΔR、ΔT(图5的步骤S32)被算出。根据该差分ΔR、ΔT，通过基于公式(10)进行的逆变换运算，计算出针对机器人手臂4的各轴的指令速度qdot*，从而进行轨道生成(图5的步骤S33)。

[0096] 其次，在特异点回避部532中，检查在轨道生成部531计算出的机器人手臂4的TCP的位置以及姿势是否在特异点附近(图5的步骤S34)。只有在TCP的位置以及姿势位于特异点附近时，生成回避动作，当TCP的位置以及姿势不在特异点附近时，不进行回避动作的生成，而进行下一个处理(图5的步骤S35)。具体地，在步骤S36中，根据公式(11)至公式(13)，生成回避动作的插补速度Δqdot。

[0097] 最后，对在轨道生成部531中生成的针对各轴的指令速度qdot*和在特异点回避部dot532中生成的插补速度Δq 进行加法计算，计算出对机器人手臂4的TCP的各轴的速度指令(图5的步骤S37)。

[0098] 通过这样，可以回避特异点。

[0099] 以上参照附图详细说明了本发明的实施方式，但是，具体构成并不限定于这些实施方式，还包括未脱离本发明宗旨范围的设计变更等。

[0100] 附图标记

[0101] 2：摄像机(视觉传感器)

[0102] 4：机器人手臂

[0103] 5：机器人控制装置

[0104] 7：工件(对象物)

[0105] 51：特征点检测部

[0106] 52：位置姿势计算部

[0107] 53：机器人手臂动作控制部

[0108] 521：运动方程式更新部

[0109] 522：特征点位置计算部

[0110] 523：权重计算部

[0111] 524：旋转和位置矩阵计算部

[0112] 531：轨道生成部

[0113] 532：特异点回避部