CN110000773B - 机器人的控制装置及机器人的逆变换处理方法 - Google Patents

Abstract

对于具备偏置臂的工业用机器人也执行逆变换处理。作为一例，以具备垂直六轴型的臂的机器人为对象，该垂直六轴型的臂的结构为具有配置5轴、并且连接4轴与6轴的连杆长度d5的第五偏置臂，4轴的轴心与6轴的轴心平行。以臂的前端部即指尖为控制点，对成为控制点目标的位置及姿态进行逆变换处理，计算出各轴的角度。暂时确定第六轴的角度并设为θ_6A，根据该角度求出偏置臂的方向，并根据第六轴的目标位置求出减去了连杆长度的暂时目标位置。对于暂时目标位置将连杆长度设定为“0”并执行逆变换处理。通过评价函数评价逆变换处理的结果，直至求出的6轴的角度与暂时确定的第六轴的角度之差为规定值以下为止，从6轴角度的暂时确定开始反复执行处理。

Description

机器人的控制装置及机器人的逆变换处理方法

技术领域

本发明涉及一种控制具有连接第四轴和第六轴的偏置臂的工业用机器人等机器人的动作的装置、以及对指尖位置进行逆变换处理的方法。

背景技术

一般的垂直六轴型机器人臂为第四轴至第六轴的各轴心在一点相交的结构。

在这种机器人臂中，为了使第五轴旋转以使包含第六轴的手腕部分移位，在臂的前端部保持手腕的部分中形成有间隙。如果存在这样的间隙，则可能会夹入异物。由此，例如外观设计专利权号1583755等中公开了通过具备连接第四轴和第六轴的偏置臂来消除手腕移位的部分的间隙的方式的工业用机器人。在该方式中，由于存在偏置臂，第四轴的轴心与第六轴的轴心成为彼此平行而不相交的关系。

关于这种方式的臂，如果假设执行根据指尖的位置、姿态求出各轴的角度的逆变换处理，则难以使用以往的方法进行。例如，专利文献1中公开的技术是，即使在包含机器人组装时的误差的情况下也能够进行基于DH参数的逆变换处理的技术。在专利文献1中，如果将偏置臂的长度设为d5，则在 d5＝0的前提下，假定与所述误差相对应的微小区域并将其线性化来进行收敛运算。

现有技术文献

专利文献

专利文献1：日本特开2017-61022号公报

发明内容

发明所要解决的技术问题

如果假设对d5≠0的机器人应用专利文献1的方法，则作为收敛运算的初始误差的d5为较大的值，因此有可能运算的迭代次数变得庞大、或者运算发散而不收敛。另外，由于存在偏移量，即使是原本伸臂达到的范围，偏移量未被正确地反映而有可能错误判定为伸臂没有达到。

鉴于上述实际情况，期望提供一种对于具备偏置臂的机器人也能够进行逆变换处理的机器人的控制装置、以及机器人的逆变换处理方法。

用于解决技术问题的技术手段

第一典型实施例的机器人的控制装置以具备垂直六轴型的臂的机器人为控制对象，该垂直六轴型的臂的结构为具有配置第五轴、并且连接第四轴和第六轴的连杆长度d5的偏置臂，从而使第四轴的轴心与第六轴的轴心平行。而且，通过以臂的前端部为控制点，并且对成为该控制点目标的位置及姿态进行逆变换处理，来计算各轴的角度。

角度暂时确定部暂时确定第六轴的角度，方向计算部根据所述角度求出偏置臂的方向，暂时目标位置计算部根据第六轴的目标位置，求出减去连杆长度d5得到的暂时目标位置。逆变换处理部对于暂时目标位置，将连杆长度 d5设定为“0”并进行逆变换处理。然后，评价部通过利用评价函数对逆变换处理的结果进行评价，直至求出的第六轴的角度与暂时确定的第六轴的角度之差小于等于规定值为止，从由角度暂时确定部进行的暂时确定开始反复执行处理。

根据这样的结构，通过暂时确定第六轴的角度，对于具有连接第四轴和第六轴的连杆长度d5的偏置臂的机器人也能够进行逆变换处理。

另外，在第二典型实施例的机器人的控制装置中，以具备具有与上述相同结构的垂直六轴型的臂的机器人为控制对象。

在该控制中，位置暂时确定部暂时确定第六轴的位置，并且基于该暂时确定的位置P6’将第五轴的位置暂时确定为P5’。动作范围判定部判定位置 P5’及P6’是否在基于机器人的连杆参数的动作范围内。如果位置P5’及 P6’在动作范围内，则逆变换处理部对于基于这些位置的齐次变换矩阵，将连杆长度d5设定为“0”并进行逆变换处理。另外，正变换处理部使用通过逆变换处理求出的各轴的角度进行正变换处理。

另外，评价值计算部求出控制点的目标位置与通过正变换处理得到的位置之差的位置矩阵p_E，并且对与目标位置相对应的旋转矩阵乘以通过正变换处理得到的旋转矩阵的逆矩阵，求出旋转矩阵R_E。而且，当位置矩阵p_E的范数超过阈值、或者通过旋转矩阵R_E求出的角度超过阈值时，迭代运算执行部通过反映了位置矩阵p_E及旋转矩阵R_E的同时变换矩阵来反复执行从逆变换处理开始的处理。也就是，如果位置矩阵p_E的范数小于等于阈值、并且通过旋转矩阵R_E求出的角度小于等于阈值，则逆变换处理结束。如果以这种方式构成，对于具有连接第四轴和第六轴的连杆长度d5的偏置臂的机器人，也暂时确定第六轴的角度并进行逆变换处理，评价对该处理结果进行正变换处理得到的矩阵p_E及矩阵R_E并使运算收敛，由此能够进行逆变换处理。

进一步，第三典型实施例的机器人的控制装置在第二典型实施例的机器人的控制装置中，位置暂时确定部暂时确定第六轴的位置，并且基于该暂时确定的位置P6’将第五轴的位置暂时确定为P5’。动作范围判定部判定位置P5’及P6’是否在基于机器人的连杆参数的动作范围内。如果位置P5’及 P6’在动作范围内，则逆变换处理部对于基于这些位置的齐次变换矩阵，将连杆长度d5设定为“0”并进行逆变换处理。另外，正变换处理部使用通过逆变换处理求出的各轴的角度进行正变换处理。

评价值计算部求出控制点的目标位置与通过正变换处理得到的位置之差的位置矩阵p_E，并且对与目标位置相对应的旋转矩阵乘以通过正变换处理得到的旋转矩阵的逆矩阵，求出旋转矩阵R_E。而且，当位置矩阵p_E的范数||p_E|| 超过阈值、或者通过所述旋转矩阵R_E求出的角度||R_E||超过阈值时，迭代运算执行部求出所述旋转矩阵R_E的旋转轴矢量q_E。另外，求出所述角度||R_E||与单位旋转轴矢量q_E/||q_E||之积即旋转矢量r_E。而且，根据对由位置矩阵p_E和旋转矢量r_E确定的位置矩阵P_E乘以雅可比矩阵的逆矩阵得到的结果，对所述角度进行更新，并从正变换处理开始反复执行，所述雅可比矩阵是根据由逆变换处理求出的各轴的角度求出的。

如果以这种方式构成，对于具有连接第四轴和第六轴的连杆长度d5的偏置臂的机器人，也暂时确定第六轴的角度并进行逆变换处理，评价对该处理结果进行正变换处理得到的矩阵p_E及矩阵R_E并使运算收敛，由此能够进行逆变换处理。

此外，还提供与上述的第一至第三典型实施例的控制装置同样的逆变换处理方法，并且可获得与上述相同的作用效果。

附图说明

图1是示出第一实施方式的机器人系统的结构的图。

图2是示出机器人的坐标系的图。

图3是在xz平面及yz平面中示出机器人的结构的图。

图4是在xy坐标及αr坐标中示出机器人的臂形态的图。

图5是在αz坐标中示出机器人的臂形态的图。

图6是示出在αz坐标中定义的臂的各形态的图。

图7是示出在第一实施方式中由控制器进行的逆变换处理的流程图。

图8是说明d2＝d3的情况下的动作范围的图。

图9是说明对于同一个指尖位置机器人臂可采取的四个手腕形态的图。

图10是示出在第二实施方式中由控制器进行的逆变换处理的流程图。

图11是示出在第三实施方式中由控制器进行的逆变换处理的流程图。

图12是示出在第四实施方式中由控制器进行的逆变换处理的流程图。

图13是说明在第四实施方式中在d2＝d3的情况下的动作范围的图。

图14是示出在第五实施方式中由控制器进行的逆变换处理的流程图。

附图标记说明

附图中，1表示机器人系统，2表示机器人，3表示控制器，4表示基座， 5表示肩部，6表示第二偏置臂，7表示第一臂，8表示第三偏置臂，9表示第二臂，10表示第五偏置臂，11表示手腕，12表示手部。

具体实施方式

以下，参照附图，对各种实施方式的机器人控制装置以及用于机器人控制的逆变换处理方法进行说明。

(第一实施方式)

以下，参照图1至图9，对第一实施方式进行说明。

如图1所示，机器人系统1具备垂直多关节型的机器人2，在基座4的内部具备对机器人2进行控制的控制器3。该机器人系统1用于一般的工业用途。机器人2是所谓的六轴垂直多关节型机器人。

肩部5经由具有z方向轴心的第一轴J1在水平方向上可旋转地连接到基座4上。向上方延伸的第一臂7的下端部经由第二偏置臂6在垂直方向上可旋转地连接到肩部5，该第二偏置臂6具备具有y方向轴心的第二轴J2并且在y方向上延伸。第二臂9经由第三偏置臂8在垂直方向上可旋转地连接到第一臂7的前端部，该第三偏置臂8具备具有y方向轴心的第三轴J3并且在 -y方向上延伸。第二臂9由基部9a及前端部9b构成。

第二臂9具备具有x方向轴心的第四轴J4，前端部9b可扭转旋转地连接于基部9a。手腕11经由第五偏置臂10在垂直方向上可旋转地连接于第二臂9 的前端部，该第五偏置臂10具备具有y方向轴心的第五轴J5并且在-y方向上延伸。图2所示的凸缘及手部12经由具有x方向轴心的第六轴J6可扭转旋转地连接于手腕11。在设置于机器人2的各轴J1～J6上，分别对应地设有作为驱动源的未图示的电机。

控制器3是机器人2的控制装置，通过在包括由未图示的CPU、ROM及 RAM等构成的计算机所组成的控制单元中执行计算机程序，对机器人2进行控制。具体地，控制器3具备由逆变器电路等构成的驱动部，基于由与各电机相对应设置的编码器检测到的电机的旋转位置，例如通过反馈控制对各轴 J1至J6各自的电机的驱动进行控制。

控制器3具备CPU(中央处理单元)3A、ROM(只读存储器)3B、RAM (随机存取存储器)3C、驱动电路3D和位置检测电路3E等的、对于机器人控制以及用于机器人控制的逆变换处理方法的实施所必须的电路元件。这些元件经由内部总线3F彼此可通信地连接，并且经由接口3G连接到驱动电路 3D及位置检测电路3E。在本实施方式中，显示器3G及输入器3H也连接到内部总线3F。这些组成元件除了由CPU3A执行的逆变换处理以外，是公知的。

ROM3B预先存储有机器人2的系统程序和动作程序等。RAM3B在执行这些程序时存储参数的值等。控制器3通过由CPU3A执行ROM3B中存储的程序，而在功能上相当于角度暂时确定部、方向计算部、暂时目标位置计算部、逆变换处理部、评价部及形态确定部。机器人2的各关节处设置的未图示的各编码器的检测信号分别输入到位置检测电路。位置检测电路3E基于各编码器的检测信号，检测各关节处设置的电机的旋转角度位置。驱动电路3D 基于控制器3的控制，对各轴J1～J6的电机进行驱动。

控制器3(即CPU3A)通过执行预先设定的动作程序，基于从位置检测电路3E输入的位置信息，对臂前端部的控制点的位置及姿态进行控制。

在本实施方式中，控制器3进行CP(Continuous Path：连续轨迹)控制。在CP控制中，将使臂前端部的控制点运动至目标时成为控制点目标的位置及姿态、也就是动作轨迹设定为时间函数。成为目标的位置及姿态中，除了包含所示教的位置及姿态以外，还包含基于所示教的位置及姿态内插的位置及姿态。控制器3通过CP控制，以使控制点的位置及姿态沿着动作轨迹的方式，控制臂中的各关节的角度。控制器3在位置及姿态的控制中，进行对用于实现成为当前所指示的目标的位置及姿态的第一轴至第六轴的角度进行计算的逆变换处理。

如图2所示，在机器人2的各关节中，规定了作为三维正交坐标系的第一至第六坐标系Σ₁至Σ₆。各坐标系Σ₁至Σ₆的原点被定在第一至第六轴线J1至 J6上的规定位置。作为各坐标系Σ₁至Σ₆的z轴的z1至z6轴与第一至第六轴线J1至J6一致。

在基座4上，规定了作为机器人坐标系的第零坐标系Σ₀。第零坐标系Σ₀是即使第一轴至第六轴旋转也不会变化的坐标系。在本实施方式中，坐标系Σ₀的原点被定在第一轴线J1上。另外，作为坐标系Σ₀的z轴的z0轴与第一轴线J1一致。

图2所示的d1至d6、a2及a3如下定义。

d1：从第零坐标系Σ₀的原点到第一坐标系Σ₁的原点为止的连杆长度

d2：从第一坐标系Σ₁的原点到第一臂7的基部为止的连杆长度

a2：从第一臂7的基部到第一臂7的前端部、第二坐标系Σ₂的原点为止的连杆长度

d3：从第二坐标系Σ₂的原点到第三偏置臂8的前端部为止的连杆长度

a3：第三轴J3、第四轴J4的轴心间距离

d4：从第三坐标系Σ₃的原点到第四坐标系Σ₄的原点为止的连杆长度

d5：从第四坐标系Σ₄的原点到第五坐标系Σ₅的原点为止的连杆长度

d6：关于从第五坐标系Σ₅的原点到第六坐标系Σ₆的原点为止的连杆长度关于a3参照图3。而且，上述d5相当于第五偏置臂10的连杆长度。

首先，作为用于说明本实施方式中的逆变换处理的前提，对正变换处理进行说明。

<正变换处理>

首先，在按照z轴旋转、z轴移动、x轴移动、x轴旋转的顺序的坐标变换中，如表1所示确定了DH参数。θ_i是从图2的状态开始的各关节的旋转角度。

【表1】

DH参数

	θ<sub>i</sub>	d<sub>i</sub>(S)	a<sub>i</sub>	α<sub>i</sub>
					1	θ<sub>1</sub>	d<sub>i</sub>	a<sub>1</sub>＝0	-90
2	θ<sub>2</sub>-90	d<sub>2</sub>	a<sub>2</sub>	0
					3	θ<sub>3</sub>+90	-d<sub>3</sub>	-a<sub>3</sub>	+90
4	θ4	d<sub>4</sub>	0	-90
					5	θ<sub>5</sub>	-d<sub>5</sub>	0	+90
6	θ<sub>6</sub>	d<sub>6</sub>	0	0

从作为基座坐标的∑₀到机械接口坐标∑₆为止的齐次变换矩阵如下。n、o、 a分别表示法矢量、定向矢量、逼近矢量。此外，为了简化标记，sinθ_i、cosθ_i分别记作s_i、c_i。另外，例如s₂₃表示sin(θ₂+θ₃)。

【数学式1】

【数学式2】

将式(8)至(10)展开，得到式(13)至(15)。

【数学式3】

另外，关于第五轴、第六轴的法矢量、定向矢量、逼近矢量各矢量以及位置坐标如下。

【数学式4】

由于是齐次变换矩阵，因此可以通过对工具坐标上的位置矢量从左侧开始相乘取得乘积，从而转换成基座坐标Σ₀上的位置矢量。也就是，能够根据关节角和DH参数求出基座坐标上的前端位置。姿态角由第六轴的法矢量、定向矢量、逼近矢量各矢量n₆、o₆、a₆表示。

＜逆变换处理＞

接着，对逆变换处理进行说明。设机器人臂的指尖的位置姿态通过齐次变换矩阵给出。将图2中基座坐标Σ₀上的位置矢量设为p_i。首先，根据逼近矢量a₆，求出手腕位置p₅。

p₅＝p₆－d₆a₆…(20)

在以往的六轴机器人的情况下，由于第四轴、第五轴和第六轴各自的旋转轴是在手腕位置处正交的结构，因此，通过相对于手腕位置首先解析求出第一轴、第二轴、第三轴，能够求出所有的关节角。即，这是因为，由于手腕位置位于第四轴、第五轴、第六轴各自的旋转轴上，因此，即使改变第四轴、第五轴、第六轴中的任一个或两个以上的轴的角度，手腕位置也不会发生变化。与此相对地，在本实施方式的六轴机器人2的情况下，由于p₄根据第四轴、第五轴、第六轴中的任一个或两个以上的轴的角度而变化，因此无法使用与以往相同的方法来求解。

于是，示出假定6轴角度来求解的方法。将6轴的旋转范围例如设为-180 度到180度，则在该范围内以适当的步长进行假定并进行搜索。该旋转范围有时称作SINGLE。如果6轴角度已确定，就可以根据式(11)、(12)来确定 p⁴。

p₄＝a₄d₆+p₆

＝o₆d₆+p₆

＝(o₆s₆+o₆c₆)d₆+p₆…(21)

这里，如图4所示，对使xy轴旋转1轴角度后得到的轴设定αr坐标。其中，由于设定了连杆参数a₁＝0，因此，在a₁≠0时数学式不同。由于p₄已确定，因此能够通过未使用1轴角度的式表示p₄的α坐标。而且，在这里，将α₁₄≥0 的臂形态定义为LEFTY，将α₁₄＜0的臂形态定义为RIGHTY。α₁₄为虚数的情况下，由于伸臂不能达到，所以无解。

【数学式5】

r₁₄＝d₂-d₃…(23)

此外，式(24)中的l₁₄是αr坐标的原点到p₄的距离。由此，可以如式(25)～ (28)那样求出轴角度的三角函数。

【数学式6】

x₄＝-r₁₄s₁+α₁₄c₁…(25)

y₄＝r₁₄c₁+α₁₄s₁…(26)

接着，如图5所示，在从侧面观察到机器人2的αz平面上考虑。满足p4 的2、3轴角度存在有两组。根据p1到p2的臂长、以及p2到p4的臂长，式(29)、 (30)成立。

【数学式7】

α₁₂ ²+z₁₂ ²＝a₂ ²…(29)

α₂₄ ²+z₂₄ ²＝a₃ ²+d₄ ²…(30)

另外，接下来的值已经能确定。

z₁₄＝z₄-d₁…(31)

将式(30)展开并且使用式(29)进行整理，能够求出p₂。

【数学式8】

(α₁₄-α₁₂)²+(z₁₄-z₁₂)²＝a₃ ²+d₄ ²…(33)

α₁₄ ²-2α₁₂α₁₄+z₁₄ ²-2z₁₂z₁₄＝a₃ ²+d₄ ²-a₂ ²…(34)

4z₁₄ ²α₁₂ ²+(-2α₁₄α₁₂+α₁₄ ²+z₁₄ ²-a₃ ²-d₄ ²+a₂ ²)²＝4z₁₄ ²a₂ ²…(37)

k₁＝α₁₄ ²+z₁₄ ²-a₃ ²-d₄ ²+a₂ ²…(39)

4m₁₄ ²α₁₂ ²-4α₁₄k₁α₁₂+k₁ ²-42₁₄ ²a₂ ²＝0…(40)

k₂＝4_m14 ²a₂ ²-k₁ ²…(43)

在k₂＜0的情况下，由于伸臂不能达到，所以无解。z₁₂也同样地求出。

【数学式9】

(-2z₁₂z₁₄+α₁₄ ²+z₁₄ ²-a₃ ²-d₄ ²+a₂ ²)²+4α₁₄ ²z₁₂ ²＝4α₁₄ ²a₂ ²…(47)

4m₁₄ ²z₁₂ ²-42₁₄k₁z₁₂+k₁ ²-4α₁₄ ²a₂ ²＝0…(48)

将式(44)、(51)代入到式(29)时，成为式(52)中的等号成立的条件，因此，除去符号和绝对值，成为下述复合同顺的组合。

【数学式10】

根据图5，下述式成立，因此能够求出2、3轴的三角函数。如果使用atan2 (y，x)，则能够求出1、2、3轴角度。

【数学式11】

α₁₂＝a₂s₂…(55)

z1₂＝a₂c₂…(57)

α₂₄＝-a₃c₂₃+d₄s₂₃…(59)

z₂₄＝a₃s₂₃+d₄c₂₃…(60)

θ_i＝atan2(s_i，c_i)…(63)

接着，如图6所示，在定义了臂形态BELOW、ABOVE的情况下，在LEFTY 的情况下，

则为ABOVE，

则为BELOW。另外，在RIGHTY的情况下，

则为BELOW，

则为ABOVE。也可以使用z12的符号来判定。

由式(64)表示。

至此1、2、3轴角度已确定，并且根据前端求出了p₄、p₅、p₆。由于p₄、 p₅、p₆在p₄处汇合，因此这里a₃与a₄正交并且内积为零的条件必须成立。由此，如果是接下来的评价式为零的6轴角度，则将满足给出的前端的位置姿态。

【数学式12】

由于在搜索6轴角度的步长的时机评价式为零的情况几乎没有，因此，通过内插求出零交叉点。使用求解为零交叉点的6轴角度重新求解1、2、3 轴角度，并确定剩余的4、5轴角度即可。根据式(18)，以注意使分母不为零的方式求出S5、C5。

【数学式13】

c₁n_x6+s₁n_y6＝c₂₃(c₄c₅c₆-s₄s₆)-s₂₃s₅c₆…(67)

s₂₃(c₁n_x6+s₁n_y6)+c₂₃n_z6＝-s₅c₆…(68)

c₁o_x6+s₁o_y6＝-c₂₃(c₄c₅s₆+s₄c₆)+s₂₃s₅s₆…(71)

s₂₃(c₁o_x6+s₁o_y6)+c₂₃o_z6＝s₅s₆…(72)

c₁a_x6+s₁a_y6＝c₂₃c₄s₅+s₂₃c₅…(75)

c₂₃a_z6+s₂₃(c₁a_x6+s₁a_y6)＝c₅…(76)

c₅＝c₂₃a_z6+s₂₃(c₁a_x6+s₁a_y6)…(77)

同样地，求出s₄、c₄。

【数学式14】

cosθ₁n_y6-sinθ₁n_x6＝cosθ₄sinθ₆+sinθ₄cosθ₅cosθ₆…(*5)

cosθ₁o_y6-sinθ₁o_x6＝cosθ₄cosθ₆-sinθ₄cosθ₅sinθ₆…(*6)

sinθ₆(cosθ₁n_y6-sinθ₁n_x6)+cosθ₆(cosθ₁o_y6-sinθ₁o_x6)＝cosθ₄…(*7)

cosθ₆(cosθ₁n_y6-sinθ₁n_x6)-sinθ₆(cosθ₁o_y6-sinθ₁o_x6)＝sinθ₄cosθ₅…(*8)

通过式(*7)、(*8)求出sinθ₄与cosθ₄，由此求出角度θ₄。另外，由于之前求出了角度θ₅，因此可以将式(*8)除以cosθ₅，也可以取而代之使用式(*9)、 (*10)。

【数学式15】

cosθ₁a_y6-sinθ₁a_x6＝sinθ₄sinθ₅…(*9)

cosθ₅(sinθ₄cosθ₅)+sinθ₅(sinθ₄sinθ₅)＝sinθ₄…(*10)

以上是逆变换处理的概略。接着，参照图7及图8，对本实施方式的作用进行说明。图7是示出由控制器3进行的逆变换处理的内容的流程图。

首先，计算6轴的动作范围(S1)。图8是说明所述动作范围的图，其以 d2＝d3的情况为前提。在这里，将3轴J3设定为使第一臂7及第二臂9伸长到最大限度的状态下的角度，改变1轴J1及2轴J2，求出根据基座4计算出的4轴位置P4可采取的球A。接着，改变6轴J6的角度，求出根据图2所示“凸缘”计算出的4轴位置P4可采取的圆B。

而且，圆B的区域中的位于球A外侧的区域在动作范围外(S2：是)，这种情况下，逆变换处理失败。如果是圆B∧球A(S2：否)，则计算6轴角度的初始值θ_6INT，并暂时确定为θ_6A(S3)。初始值θ_6INT例如设为在动作范围中可采取的6轴角度的下限值。此外，例如使用该时间点的6轴角度，如果所述角度在动作范围外，则也可以采用距离所述角度最近的动作范围内的角度。另外，还可以设定在圆B的中心。

另外，在d2≠d3的情况下，以如下方式进行动作范围内和行动作范围外的判定。步骤S2中判定为动作范围外的情况是式(24)或式(44)中的平方根的内部为负的情况。这里，如果以使其在动作范围内的方式加入修正，则在式(24)的情况下，可以设为

1₁₄ ²＝r₁₄ ²。由此，如果设为L＝|r₁₄|-|1₁₄|，则仅校正

w＝L/|1₁₄|，[wx₄ wy₄ 0]^T位置即可。

另外，在式(44)的情况下，

【数学式16】

设k₂＝0，

的情况下，

设

在αZ平面上向接近原点的方向移动。

此外，(24)式也进行了校正的情况下，由于该校正，(24)式不再成立，

因此仅校正Z轴使k₂＝0。

的情况下，

设

在αZ平面上向远离原点的方向移动。

然后，将零代入用于确定手腕形态的变量n时(S4)，计算暂时目标位置 pA(S5)。p是位置矢量[x，y，z]^T，暂时目标位置pA根据式(21)为p+ (nsinθ_6A+ocosθ_6A)d₅。齐次变换矩阵T_A根据旋转矩阵R和目标位置p_A确定。这里，暂时目标位置p_6A相对于目标位置p₆，处于p_6A＝p₆+(n₆sinθ_6A+o₆cosθ_6A) d₅的关系。即，从目标位置p₆减去偏移距离d₅得到的值成为暂时目标位置p_6A。

此外，变量n按照6轴角度从大到小的顺序对手腕形态预先进行编号，并且，如后所述，用于确定在第几号的手腕形态下确定逆变换处理。

接着，基于目标位置p_A，进行逆变换处理(S6)。这里，函数g(θ)＝(T, F)是设d₅＝0并且根据角度θ＝[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]^T来求出齐次变换矩阵T 及机器人2的形态F的函数。而且，g^-1(T,F)是设d₅＝0并且根据齐次变换矩阵T及形态F进行逆变换来求出角度θ的函数。

接着，求出通过评价函数Q(θ，θ_6A)对逆变换的结果进行评价而得到的值v(S7)。这里，作为评价函数Q，例如，通过上述的式(65)计算内积。然后，判断评价值v的绝对值是否小于等于设定在零附近的阈值v_M(S8)。如果所述绝对值超过阈值v_M(否)，则例如对6轴角度θ_6A追加β(＞0)进行更新(S11)。这里，对角度θ_6A是否为在动作范围内的角度进行检查(S12)，如果在动作范围内(是)，则返回到步骤S5。如果角度θ_6A在动作范围外(否)，则逆变换处理失败。

此外，以在6轴角度的整个范围内进行搜索的方式改变角度θ_6A。另外，也可以将初始值θ_6INT作为在动作范围中可采取的6轴角度的上限值，并减去β进行更新。此外，也可以以

θ_6A←θ_6A+α(θ₆-θ_6A),0＜α≤1的方式进行更新。另外，也可以针对每次迭代运算改变β、α，也可以使用黄金分割法或单纯形法等的优化方法。

这里，如图9所示，在如本实施方式那样具有第三偏置臂8而d5≠0的机器人的情况下，相对于一个指尖位置最多有四个手腕形态，并且满足步骤S9 中的评价的6轴角度也最多存在四个。由此，当在步骤S8中评价值v的绝对值小于等于阈值v_M时(是)，使变量n递增(S9)，并判断变量n的值是否与预先指定的第N个6轴角度一致(S10)。如果n＝N(是)，则使逆变换处理结束，如果n≠N(否)，则转移到步骤S11。此外，在步骤S8中判断为“是”的情况表示作为逆变换的结果的6轴角度θ₆大致等于暂时确定的角度θ_6A。

如上所述，根据本实施方式，控制器3以具备垂直六轴型的臂的机器人2 为控制对象，该垂直六轴型的臂的结构为具有配置5轴、并且连接4轴和6 轴的连杆长度d5的第五偏置臂10，从而使4轴的轴心与6轴的轴心平行。而且，控制器3将臂的前端部即指尖作为控制点，并且对成为该控制点的目标的位置及姿态进行逆变换处理，来计算各轴的角度。

暂时确定第六轴的角度并设为θ_6A时，根据该角度求出偏置臂8的方向，并且根据第六轴的目标位置P₆，求出减去连杆长度d5得到的暂时目标位置 P_6A时，对于暂时目标位置P_A将连杆长度d5设定为“0”并进行逆变换处理。而且，通过利用评价函数评价逆变换处理的结果，直至求出的6轴的角度θ₆与暂时确定的第六轴的角度θ_6A之差小于等于规定值为止，从6轴角度暂时确定开始反复执行处理。通过以如上方式构成，对于具有连杆长度d5的偏置臂10的机器人2，也能够进行逆变换处理。

而且，控制器3使用内积a3·a4作为评价函数。即，如果适当地进行逆变换处理，则逼近矢量a3与a4正交并且它们的内积应该为零。因此，能够适当地评价逆变换处理的结果。

另外，控制器3对于同一个指尖位置，将第六轴的角度按照从大到小的顺序预先进行编号，由此确定逆变换处理结果的手腕形态。即，具有连杆长度d5的偏置臂的机器人对于同一个指尖位置，最多可采取四个手腕形态。由此，将多个手腕形态中的、与所采用的手腕形态相对应的第四轴至第六轴中的任一轴的角度，按照从小到大的顺序或者从大到小的顺序预先进行编号。而且，如果作为逆变换结果得到的相对应的轴的角度与预先编号的大小相对应的角度一致，则采用该逆变换结果。由此，能够唯一地确定手腕形态。

(第二实施方式)

以下，对与第一实施方式相同的部分赋予相同的附图标记并省略说明，对不同的部分进行说明。

在第二实施方式中，对于第四轴或第五轴的角度而不是第6轴，按照从大到小的顺序对手腕形态进行编号。即，对于k(＝4，5)轴角度指定第N个角度。如图10所示，如果在步骤S9中判断为“是”，则将该时间点的逆变换结果即角度θ存储到存储器等中(S13)。而且，如果在步骤S12中判断为“否”，则将直至该时间点为止存储的角度θ中的k轴角度按照其大小顺序排序(S14)。而且，采用这些第N个角度作为解(S15)。

如上所述，根据第二实施方式，与第一实施方式同样地，能够唯一地确定逆变换处理结果的手腕形态。

(第三实施方式)

在第三实施方式中，为了唯一地确定在逆变换处理中获得的手腕形态，使用雅可比矩阵J。此外，在第三实施方式中，对与第一实施方式相同的部分赋予相同的附图标记并省略说明，对不同的部分进行说明。

在第一实施方式中，假设了“满足步骤S9中的评价的6轴角度最多存在四个”，但是划分这些边界的点应该是奇点(特异点)，雅可比矩阵J在奇点处行列式为零。

即，除了臂形态RIGHTY、LEFTY和肘形态ABOVE、BELOW以外，还具有手腕形态FLIP+、FLIP-、NONFLIP+、NONFLIP-的组合。由此，在图 11所示的流程图中，如果步骤S9中判断为“是”，则计算雅可比矩阵J的行列式(S16)。雅可比矩阵J由式(87)至(99)表示。

【数学式17】

【数学式18】

【数学式19】

而且，判断计算出的行列式的符号是否与所指定的手腕形态相符(S17)。

如上所述，根据第三实施方式，控制器3对于逆变换处理的结果计算雅可比矩阵J的行列式。而且，根据行列式的符号是否与预先指定的手腕形态相符，能够唯一地确定逆变换处理结果的手腕形态。

例如，在各轴的可移动范围为-180度至180度的情况下，如果移动5轴并在视觉上确认手腕形态，则可知最多为两个臂形态、两个肘形态及四个手腕形态的组合。

划分这四个手腕形态的边界的点应该为奇点，雅可比矩阵在奇点处行列式为零。由此，关于各形态的组合，根据逆变换处理结果的手腕形态与雅可比矩阵的行列式的符号是否对应，能够判定该形态是否为指定的形态。即，通过组合成4轴的角度属于-90度至90度、-180度至-90度或者90度至180 度中的某一个，最多能够判别四个手腕形态。

本发明不仅限于上述或者附图中记载的实施方式，可以进行如下的变形或扩展。

关于逆变换处理的结果，除上述以外，也可以在逆变换之后对包含d5在内进行正变换得到的结果的位置误差是否为零进行评价。

另外，也可以评价根据凸缘求出的5轴的方向Z4A、与通过逆变换求出的5轴的方向Z4是否一致，也就是它们的内积是否为“1”。

另外，也可以评价上述5轴的方向Z4A、与通过逆变换求出的4轴的方向Z3是否正交，也就是它们的内积是否为“0”。

关于迭代计算，也可以将动作范围分割为多个，并针对分割后的各角度范围求出评价值。

另外，在对动作范围进行了分割的情况下，也可以在通过评价值求出的解的附近，根据求出的评价值求出评价函数的近似函数，并使用该近似函数求出可靠的角度。

(第四实施方式)

对第四实施方式进行说明。

此外，在本实施方式中，对与第一实施方式相同的部分赋予相同的附图标记并省略说明，对不同的部分进行说明。

该第四实施方式中，逆变换处理的一部分与上述第一实施方式不同。具体地，上述的式(64)之后的处理不同。

如上述的图6所示，角度

通过上述的式(64)表示，在LEFTY的情况下，

则为臂形态ABOVE，

则为臂形态BELOW，并且，在RIGHTY 的情况下，

则为臂形态BELOW，

则为臂形态ABOVE。

接下来求出角度θ₅、θ₆。

【数学式12】

sinθ₅cosθ₆＝-sin(θ₂+θ₃)(cosθ₁n_x+sinθ₁n_y)-cos(θ₂+θ₃)n_z…(*1)

sinθ₅sinθ₆＝sin(θ₂+θ₃)(cosθ₁o_x+sinθ₁o_y)+cos(θ₂+θ₃)o_z…(*2)

sin²θ₅＝sin²θ₅(sin²θ₆+cos²θ₆)＝(sinθ₅sinθ₆)²+(sinθ₅cosθ₆)²…(*3)

通过式(*4)，求出角度θ₅。另外，通过式(*4)和式(*1)、(*2)求出 sinθ₆和cosθ₆，由此求出角度θ₆。

【数学式13】

cosθ₁n_y6-sinθ₁n_x6＝cosθ₄sinθ₆+sinθ₄cosθ₅cosθ6…(*5)

cosθ₁o_y6-sinθ₁o_x6＝cosθ₄cosθ₆-sinθ₄cosθ₅sinθ₆…(*6)

sinθ₆(cosθ₁n_y6-sinθ₁n_x6)+cosθ₆(cosθ₁o_y6-sinθ₁o_x6)＝cosθ₄…(*7)

cosθ₆(cosθ₁n_y6-sinθ₁n_x6)-sinθ₆(cosθ₁o_y6-sinθ₁o_x6)＝sinθ₄cosθ₅…(*8)

通过式(*7)、(*8)求出sinθ₄和cosθ₄，由此求出角度θ₄。此外，式(*4) 具有±符号，对此，在手腕形态的指定中采用满足4轴角度或5轴角度的条件的符号。另外，由于之前求出了角度θ₅，因此可以将式(*8)除以cosθ₅，也可以取而代之使用式(*9)、(*10)。

【数学式14】

cosθ₁a_y6-sinθ₁a_x6＝sinθ₄sinθ₅…(*9)

cosθ₅(sinθ₄cosθ₅)+sinθ₅(sinθ₄sinθ₅)＝sinθ₄…(*10)

以上是逆变换处理的概略。

接着，参照图12及图13，对本实施方式的作用进行说明。图12是示出由控制器3进行的逆变换处理的内容的流程图。

首先，暂时确定6轴的位置并设为P6’时，根据该位置P6’暂时确定5 轴的位置并设为P5’。然后，使用它们来确定齐次变换矩阵T’(S21)。然后，判断暂时确定的位置P5’是否在动作范围内(S22)。

图13是说明所述动作范围的图，其以d2＝d3的情况为前提。设定半径 L1的球A、以及半径(L1+L2)的球B。L1、L2分别通过式(*11)、式(*12) 定义。

【数学式15】

而且，如果位置P5’在球A之外(S22：动作范围外)，则将从根据齐次变换矩阵T’确定的位置到动作范围的边界为止的距离设为L(S29)。接着，判断距离L是否小于等于连杆长度d5(S30)。该步骤是判断暂时确定的位置 P6’是否在动作范围内。如果距离L超过连杆长度d5(否)，则设为错误并使处理结束。

另一方面，如果距离L小于等于连杆长度d5(是)，则以使位置P5’移动到球A的内侧的方式进行修正(S24)。例如，使位置P5’移动到连结位置 P1和位置P5’的直线与球A的交点即可。伴随着使位置P5’移动，位置P6’也发生移动。此外，在步骤S30中，也可以设L2＝d5并判断位置P5’是否在球B之外。

另外，在d2≠d3的情况下，以如下方式进行动作范围内和行动作范围外的判定。在步骤S22中判定为动作范围外的情况是式(24)或式(44)中平方根的内部为负的情况。这里，如果以使其在动作范围内的方式加入修正，则在式(24)的情况下，可以设为

l₁₄ ²＝r₁₄ ²。由此，当设为L＝|r₁₄|－|l₁₄|时，仅校正

w＝L/|l₁₄|，[wx₄ wy₄0]^T位置即可。

另外，在式(44)的情况下，

【数学式16】

设k₂＝0，

的情况下，

设

在αZ平面上向接近原点的方向移动。

此外，(24)式也进行了校正的情况下，由于该校正，(24)式不再成立，

因此仅校正Z轴使k₂＝0。

的情况下，

设

在αZ平面上向远离原点的方向移动。

如果在步骤S22中判断为“动作范围内”，并且执行步骤S34，则进行逆变换处理(S23)。这里，g^-1(T’，F)是设d₅＝0并且根据齐次变换矩阵T’、机器人2的形态F求出角度θ’＝[θ₁’，θ₂’，θ₃’，θ₄’，θ₅’，θ₆’]^T的函数。

接着，使用作为逆变换的结果求出的角度θ’进行正变换处理(S24)。将作为该正变换处理的结果得到的齐次变换矩阵设为T”，将形态设为F”。齐次变换矩阵T”作为要素包含旋转矩阵R”、位置矩阵p”。

接着，求出目标位置p与上述位置p”之差，并且求出位置误差矢量p_E。另外，对与目标位置p相对应的旋转矩阵R乘以旋转矩阵R”的逆矩阵，求出评价用的旋转矩阵R_E(S25)。而且，分别判断矢量p_E的范数||p_E||是否小于等于阈值p_EM(S26)，以及旋转矩阵R_E的角度||R_E||是否小于等于阈值R_EM(S27)。这里，||p_E||、||R_E||分别由式(*13)、式(*14)定义。

【数学式17】

如果在步骤S27中判断为“是”，则判断根据位置p”及旋转矩阵R”确定的手腕形态是否为指定的形态(S28)，如果获得了指定的形态，则使逆变换处理结束。另一方面，如果在步骤S26或S27中判断为“否”，则判断逆变换处理及正变换处理的反复执行次数是否超过了预先设定的规定数(S31)。这里，如果超过规定数(是)，则设为错误并使处理结束。如果未超过规定数 (否)，则对旋转矩阵R’乘以评价用的旋转矩阵R_E，并且对位置矩阵p’加上位置误差矢量p_E来更新齐次变换矩阵T’(S32)。然后，与步骤S22同样地进行动作范围内和行动作范围外的判定(S33)，如果在动作范围内，则转移到步骤S23，如果在动作范围外，则转移到步骤S34。

接着，对步骤S28的处理判定进行说明。如上述的第一实施方式中说明的图9所示，在如本实施方式那样具有第三偏置臂8而d5≠0的机器人的情况下，相对于一个指尖位置最多有四个手腕形态，并且满足步骤S29中的评价的6轴角度也最多存在四个。由此，在步骤S28中，为了唯一地确定在逆变换处理中得到的手腕形态，使用雅可比矩阵J。关于上述最多存在四个的6轴角度，划分这些边界的点应该是奇点，雅可比矩阵J在奇点处行列式为零。

通过雅可比矩阵的符号与以往的手腕形态的构思、4轴角度为-90度至90 度、-180度至-90度或者90度至180度中的某一个的组合，能够判别最多存在四个的手腕形态FLIP+、FLIP-、NONFLIP+、NONFLIP-。

即，除了臂形态RIGHTY、LEFTY和肘形态ABOVE、BELOW以外，还具有手腕形态FLIP+、FLIP-、NONFLIP+、NONFLIP-的组合。由此，在步骤S28中计算雅可比矩阵J的行列式。如上所述，雅可比矩阵J由式(87)至式(99)表示。

然后，判断计算出的行列式的符号是否与指定的手腕形态相符。

如上所述，根据本实施方式，控制器3以具备垂直六轴型的臂的机器人2 为控制对象，该垂直六轴型的臂的结构为具有配置5轴、并且连接4轴与6 轴的连杆长度d5的第五偏置臂10，从而使4轴的轴心与6轴的轴心平行。而且，控制器3将臂的前端部即指尖作为控制点，并且对成为该控制点的目标的位置及姿态进行逆变换处理，来计算各轴的角度。

另外，控制器3暂时确定第六轴的位置，并且基于该暂时确定的位置P6’将第五轴的位置暂时确定为P5’时，判定位置P5’及P6’是否在基于机器人2的连杆参数的动作范围内。如果位置P5’及P6’在动作范围内，则对于基于这些位置的齐次变换矩阵，将连杆长度d5设定为“0”并进行逆变换处理。然后，使用通过逆变换处理求出的各轴的角度进行正变换处理。

进一步，控制器3求出控制点的目标位置与通过正变换处理得到的位置之差的位置误差矢量p_E，并且对与目标位置相对应的旋转矩阵乘以通过正变换处理得到的旋转矩阵的逆矩阵，求出旋转矩阵R_E。而且，当位置误差矢量 p_E的范数超过阈值、或者通过旋转矩阵R_E求出的角度超过阈值时，通过反映了位置误差矢量p_E及旋转矩阵R_E的齐次变换矩阵来反复执行从逆变换处理开始的处理。如果位置误差矢量p_E的范数小于等于阈值、并且通过旋转矩阵R_E求出的角度小于等于阈值，则逆变换处理结束。

如果以这种方式构成，则对于具有连接第四轴与第六轴的连杆长度d5的偏置臂的机器人2，通过暂时确定第六轴的角度并进行逆变换处理，评价对该处理结果进行正变换处理得到的矩阵p_E及矩阵R_E并使运算收敛，也能够进行逆变换处理。

另外，如果位置P5’及P6’不在动作范围内，则控制器3以使位置P5’及P6’位于该动作范围内的方式对位置P5’及P6’进行修正。对于通过齐次变换矩阵T’确定的控制点的位置，也同样地判断是否在动作范围内，如果不在动作范围内，则以使其位于动作范围内的方式对齐次变换矩阵T’进行修正。由此，能够继续执行逆变换处理和迭代运算。

而且，控制器3对于逆变换处理的结果计算雅可比矩阵J的行列式。而且，根据行列式的符号是否与预先指定的手腕形态相符，能够唯一地确定逆变换处理结果的手腕形态。因此，关于通过雅可比矩阵J的运算来确定手腕形态来说，可获得与上述第一实施方式同样的作用效果。

(第五实施方式)

接着，对第五实施方式进行说明。

此外，该第五实施方式的结构与上述第四实施方式相同，即硬件结构与第一实施方式的硬件结构相同(使用相同的附图标记)，但逆变换处理不同，因此对其进行说明。在该说明中，存在一部分与第四实施方式的逆变换处理的说明重复的点，为了易于理解说明的上下文，从头开始说明由控制器3进行的逆变换处理的内容。

图14是示出由控制器3进行的逆变换处理的内容的流程图。首先，暂时确定6轴的位置并设为P6’，则根据该位置P6’暂时确定5轴的位置并设为 P5’。然后，使用它们来确定齐次变换矩阵T’(S41)。然后，判定暂时确定的位置P5’是否在动作范围内(S42)。

所述动作范围与在第一实施方式中说明的图7相同，其以d2＝d3的情况为前提。设定半径L1的球A、以及半径(L1+L2)的球B。L1、L2分别由式(*11)、式(*12)定义。

【数学式15】

而且，如果位置P5’在球A之外(S42；动作范围外)，则将从通过齐次变换矩阵T’确定的位置到动作范围的边界为止的距离设为L(S49)。接着，判断距离L是否小于等于连杆长度d5(S50)。该步骤是判断暂时确定的位置 P6’是否在动作范围内。如果距离L超过连杆长度d5(否)，则设为错误并使处理结束。

另一方面，如果距离L小于等于连杆长度d5(是)，则以使位置P5’移动到球A的内侧的方式进行修正(S51)。例如，使位置P5’移动到连接位置 P1和位置P5’的直线与球A的交点即可。伴随着使位置P5’移动，位置P6’也发生移动。此外，在步骤S50中，也可以设L2＝d5并判断位置P5’是否在球B之外。

另外，在d2≠d3的情况下，以如下所示的方式进行动作范围内和行动作范围外的判定。在步骤S42中判定为动作范围外的情况是式(24)或式(44) 中平方根的内部为负的情况。这里，如果以使其在动作范围内的方式加入修正，则在式(24)的情况下，可以设为

l₁₄ ²＝r₁₄ ²。由此，当设为L＝|r₁₄|-|l₁₄|时，仅校正

w＝L/|l₁₄|，[wx₄ wy₄0]^T位置即可。

另外，在式(44)的情况下，

【数学式16】

设k₂＝0，

的情况下，

设

在αZ平面上向接近原点的方向移动。

此外，(24)式也进行了校正的情况下，由于该校正，(24)式不再成立，

因此仅校正Z轴使k₂＝0。

的情况下，

设

在αZ平面上向远离原点的方向移动。

如果在步骤S42中判断为“动作范围内”，并且执行步骤S51，则进行逆变换处理(S43)。这里，g^-1(T’，F)是假设d5＝0并且根据齐次变换矩阵T’、机器人2的形态F求出角度θ’＝[θ₁’，θ₂’，θ₃’，θ₄’，θ₅’，θ₆’]^T的函数。

接着，使用作为逆变换的结果求出的角度θ’进行正变换处理(S44)。将作为该正变换处理的结果得到的齐次变换矩阵设为T”，将形态设为F”。齐次变换矩阵T”作为要素包含旋转矩阵R”、位置矩阵p”。

接着，求出目标位置p与上述位置p”之差，并且求出位置误差矢量p_E。另外，对与目标位置p相对应的旋转矩阵R乘以旋转矩阵R”的逆矩阵，求出评价用的旋转矩阵R_E(S45)。而且，分别判断矢量p_E的范数||p_E||是否小于等于阈值p_EM(S46)，以及旋转矩阵R_E的角度||R_E||是否小于等于阈值R_EM(S47)。这里，||p_E||、||R_E||分别由式(*13)、式(*14)定义。

【数学式17】

如果在步骤S47中判断为“是”，则判断根据位置p”及旋转矩阵R”确定的手腕形态是否为指定的形态(S48)，如果获得了指定的形态，则使逆变换处理结束。另一方面，如果在步骤S46或S47中判断为“否”，则判断逆变换处理及正变换处理的反复执行次数是否超过了预先设定的规定数(S52)。这里，如果超过规定数(是)，则设为错误并使处理结束。

如果未超过规定数(S52：否)，则根据作为旋转矩阵R_E的要素的法矢量、定向矢量、逼近矢量各矢量，通过下式求出旋转轴矢量q_E(S53)。

【数学式18】

接着，通过下式求出旋转矩阵R_E的角度||R_E||与单位旋转轴矢量q_E/||q_E|| 之积、即旋转矢量r_E(S54)。

【数学式19】

然后，根据对由位置矩阵p_E和旋转矢量r_E确定的位置矩阵P_E乘以雅可比矩阵J的逆矩阵得到的结果，对角度θ’进行更新(S55)，并转移到步骤S44，所述雅可比矩阵J是根据由逆变换处理求出的各轴的角度θ求出的。如上所述，雅可比矩阵J由式(87)至式(99)表示。

接着，对步骤S48的处理判定进行说明。如上述的图9所示，在如本实施方式那样具有第三偏置臂8而d5≠0的机器人的情况下，相对于一个指尖位置最多有四个手腕形态，并且满足步骤S49中的评价的6轴角度也最多存在四个。由此，在步骤S48中，为了唯一地确定在逆变换处理中获得的手腕形态，使用雅可比矩阵J。对于上述最多存在四个的6轴角度，划分这些边界的点应该是奇点，雅可比矩阵J在奇点处行列式为零。

通过雅可比矩阵的符号与以往的手腕形态的构思、4轴角度为-90度至90 度、-180度至-90度或者90度至180度中的某一个的组合，能够判别最多存在四个的手腕形态FLIP+、FLIP-、NONFLIP+、NONFLIP-。

即，除了臂形态RIGHTY、LEFTY与肘形态ABOVE、BELOW以外，还存在手腕形态FLIP+、FLIP-、NONFLIP+、NONFLIP-的组合。由此，在步骤S48中计算雅可比矩阵J的行列式。

然后，判断计算出的行列式的符号是否与指定的手腕形态相符。

如上所述，根据本实施方式，控制器3以具备垂直六轴型的臂的机器人2 为控制对象，该垂直六轴型的臂的结构为具有配置5轴、并且连接4轴与6 轴的连杆长度d5的第五偏置臂10，从而使4轴的轴心与6轴的轴心平行。而且，控制器3将臂的前端部即指尖作为控制点，并且对成为该控制点的目标的位置及姿态进行逆变换处理，来计算各轴的角度。

另外，控制器3暂时确定第六轴的位置，并且基于该暂时确定的位置P6’将第五轴的位置暂时确定为P5’时，判定位置P5’及P6’是否在基于机器人2的连杆参数的动作范围内。如果位置P5’及P6’在动作范围内，则对于基于这些位置的齐次变换矩阵，将连杆长度d5设定为“0”并进行逆变换处理。然后，使用通过逆变换处理求出的各轴的角度进行正变换处理。

进一步，求出控制点的目标位置与通过正变换处理得到的位置之差的位置误差矢量p_E，并且对与目标位置相对应的旋转矩阵乘以通过正变换处理得到的旋转矩阵的逆矩阵，求出旋转矩阵R_E。而且，当位置误差矢量p_E的范数 ||p_E||超过阈值、或者通过旋转矩阵R_E求出的角度||R_E||超过阈值时，根据作为旋转矩阵R_E的要素的法矢量、定向矢量、逼近矢量各矢量来求出旋转轴矢量q_E，并且求出角度||R_E||与单位旋转轴矢量q_E/||q_E||之积、即旋转矢量r_E。然后，根据对由位置矩阵p_E和旋转矢量r_E确定的位置矩阵P_E乘以雅可比矩阵J的逆矩阵得到的结果，对角度θ’进行更新，反复执行从正变换开始的处理，所述雅可比矩阵J是根据由逆变换处理求出的各轴的角度θ求出的。

如果以这种方式构成，对于具有连接第四轴和第六轴的连杆长度d5的偏置臂的机器人2，暂时确定第六轴的角度并进行逆变换处理，评价对该处理结果进行正变换处理得到的矩阵p_E及矩阵R_E并使运算收敛，由此能够进行逆变换处理。

另外，如果位置P5’及P6’不在动作范围内，则控制器3以使位置P5’及P6’位于该动作范围内的方式对位置P5’及P6’进行修正。对于通过齐次变换矩阵T’确定的控制点的位置，也同样地判断是否在动作范围内，如果不在动作范围内，则以使其位于动作范围内的方式对齐次变换矩阵T’进行修正。由此，能够继续执行逆变换处理和迭代运算。

进一步，在本第五实施方式中，对于通过雅可比矩阵J的运算来确定手腕形态来说，也可获得与上述第一实施方式同样的作用效果。

此外，在上述的各实施方式中，也可以将控制器3配置在机器人2的基座4之外。

Claims (24)

1.一种机器人的控制装置，适用于具备垂直六轴型的臂的机器人，该具备垂直六轴型的臂的机器人结构为具有配置第五轴、并且连接第四轴和第六轴的连杆长度d5的偏置臂，从而使第四轴的轴心与第六轴的轴心平行，所述机器人的控制装置通过以所述垂直六轴型的臂的前端部为控制点，并且对成为所述控制点的目标的位置及姿态进行逆变换处理，来计算各轴的角度，

所述机器人的控制装置具备：

角度暂时确定部，暂时确定第六轴的角度；

方向计算部，根据暂时确定的第六轴的角度，求出所述偏置臂的方向；

暂时目标位置计算部，根据第六轴的目标位置，求出减去连杆长度d5得到的暂时目标位置；

逆变换处理部，对于所述暂时目标位置，将连杆长度d5设定为0并进行逆变换处理；以及

评价部，通过利用评价函数对所述逆变换处理的结果进行评价，直至求出的第六轴的角度与暂时确定的第六轴的角度之差小于等于规定值为止，从由所述角度暂时确定部进行的暂时确定开始反复执行处理。

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

将第三轴、第四轴的逼近矢量分别设为a3、a4时，所述评价部将内积a3·a4作为所述评价函数。

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

具备形态确定部，所述形态确定部对于同一个指尖位置，对第四轴至第六轴中的任一轴的多个角度预先进行编号，从而确定逆变换处理结果的手腕形态。

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

具备形态确定部，所述形态确定部对于逆变换处理的结果计算雅可比矩阵的行列式，并且根据该行列式的符号是否与预先指定的手腕形态相符，来确定逆变换处理结果的手腕形态。

5.一种机器人的逆变换处理方法，适用于具备垂直六轴型的臂的机器人，该垂直六轴型的臂的机器人结构为具有配置第五轴、并且连接第四轴和第六轴的连杆长度d5的偏置臂，从而使第四轴的轴心与第六轴的轴心平行，所述机器人的逆变换处理方法通过以所述垂直六轴型的臂的前端部为控制点，并且对成为所述控制点的目标的位置及姿态进行逆变换处理，来计算各轴的角度，

所述机器人的逆变换处理方法包括：

对第六轴的角度进行暂时确定的步骤；

根据暂时确定的第六轴的角度，求出所述偏置臂的方向的步骤；

根据第六轴的目标位置，求出减去连杆长度d5得到的暂时目标位置的步骤；以及

对于所述暂时目标位置，将连杆长度d5设定为0并进行逆变换处理的步骤；

通过利用评价函数对所述逆变换处理的结果进行评价，直至求出的第六轴的角度与暂时确定的第六轴的角度之差小于等于规定值为止，从所述进行暂时确定的步骤开始反复执行处理。

6.根据权利要求5所述的机器人的逆变换处理方法，其中，

将第三轴、第四轴的逼近矢量分别设为a3、a4时，将内积a3·a4作为所述评价函数。

7.根据权利要求5或6所述的机器人的逆变换处理方法，其中，

对于同一个指尖位置，对第四轴至第六轴中的任一轴的多个角度预先进行编号，从而确定逆变换处理结果的手腕形态。

8.根据权利要求5或6所述的机器人的逆变换处理方法，其中，

具备形态确定部，所述形态确定部对于逆变换处理的结果计算雅可比矩阵的行列式，并且根据该行列式的符号是否与预先指定的手腕形态相符，来确定逆变换处理结果的手腕形态。

9.一种机器人的控制装置，适用于具备垂直六轴型的臂的机器人，该垂直六轴型的臂的机器人结构为具有配置第五轴、并且连接第四轴和第六轴的连杆长度d5的偏置臂，从而使第四轴的轴心与第六轴的轴心平行，所述机器人的控制装置通过以所述垂直六轴型的臂的前端部为控制点，并且对成为所述控制点的目标的位置及姿态进行逆变换处理，来计算各轴的角度，

所述机器人的控制装置具备：

位置暂时确定部，暂时确定第六轴的位置，并且基于该暂时确定的位置P6’将第五轴的位置暂时确定为P5’；

动作范围判定部，判定所述位置P5’及P6’是否在基于所述机器人的连杆参数的动作范围内；

逆变换处理部，如果所述位置P5’及P6’在动作范围内，则对于基于这些位置的齐次变换矩阵，将连杆长度d5设定为0并进行逆变换处理；

正变换处理部，使用通过所述逆变换处理求出的各轴的角度，进行正变换处理；

评价值计算部，求出所述控制点的目标位置与通过所述正变换处理得到的位置之差的位置矩阵p_E，并且对与所述目标位置相对应的旋转矩阵乘以通过所述正变换处理得到的旋转矩阵的逆矩阵，求出旋转矩阵R_E；以及

迭代运算执行部，当所述位置矩阵p_E的范数超过阈值或者通过所述旋转矩阵R_E求出的角度超过阈值时，所述迭代运算执行部通过反映了所述位置矩阵p_E及所述旋转矩阵R_E的齐次变换矩阵来反复执行从所述逆变换处理开始的处理。

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

如果所述位置P5’及P6’不在动作范围内，则所述逆变换处理部以使所述位置P5’及P6’位于该动作范围内的方式对所述位置P5’及P6’进行修正。

11.根据权利要求9或10所述的机器人的控制装置，其中，

所述迭代运算执行部判断通过所述齐次变换矩阵确定的所述控制点的位置是否在所述动作范围内，如果所述控制点的位置不在所述动作范围内，则以使其位于该动作范围内的方式对所述齐次变换矩阵进行修正。

12.根据权利要求9或10所述的机器人的控制装置，其中，

具备形态确定部，所述形态确定部对于所述逆变换处理的结果计算雅可比矩阵的行列式，并且根据该行列式的符号是否与预先指定的手腕形态相符，来确定逆变换处理结果的手腕形态。

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

具备形态确定部，所述形态确定部对于所述逆变换处理的结果计算雅可比矩阵的行列式，并且根据该行列式的符号是否与预先指定的手腕形态相符，来确定逆变换处理结果的手腕形态。

14.一种机器人的逆变换处理方法，适用于具备垂直六轴型的臂的机器人，该垂直六轴型的臂的机器人结构为具有配置第五轴、并且连接第四轴和第六轴的连杆长度d5的偏置臂，从而使第四轴的轴心与第六轴的轴心平行，所述机器人的逆变换处理方法通过以所述垂直六轴型的臂的前端部为控制点，并且对成为所述控制点的目标的位置及姿态进行逆变换处理，来计算各轴的角度，

所述机器人的逆变换处理方法包括：

暂时确定第六轴的位置，并且基于该暂时确定的位置P6’将第五轴的位置暂时确定为P5’的步骤；

判定所述位置P5’及P6’是否在基于所述机器人的连杆参数的动作范围内的步骤；

如果所述位置P5’及P6’在动作范围内，则对于基于这些位置的齐次变换矩阵，将连杆长度d5设定为0并进行逆变换处理的步骤；

使用通过所述逆变换处理求出的各轴的角度，进行正变换处理的步骤；以及

求出所述控制点的目标位置与通过所述正变换处理得到的位置之差的位置矩阵p_E，并且对与所述目标位置相对应的旋转矩阵乘以通过所述正变换处理得到的旋转矩阵的逆矩阵，求出旋转矩阵R_E的步骤；

当所述位置矩阵p_E的范数超过阈值或者通过所述旋转矩阵R_E求出的角度超过阈值时，通过反映了所述位置矩阵p_E及所述旋转矩阵R_E的齐次变换矩阵，来从所述进行逆变换处理的步骤开始反复执行。

15.根据权利要求14所述的机器人的逆变换处理方法，其中，

如果所述位置P5’及P6’不在动作范围内，则以使所述位置P5’及P6’位于该动作范围内的方式对所述位置P5’及P6’进行修正。

16.根据权利要求14或15所述的机器人的逆变换处理方法，其中，

判断通过所述齐次变换矩阵确定的所述控制点的位置是否在所述动作范围内，如果所述控制点的位置不在所述动作范围内，则以使其位于该动作范围内的方式对所述齐次变换矩阵进行修正。

17.根据权利要求14或15所述的机器人的逆变换处理方法，其中，

具备形态确定部，对于逆变换处理的结果计算雅可比矩阵的行列式，并且根据该行列式的符号是否与预先指定的手腕形态相符，来确定逆变换处理结果的手腕形态。

18.根据权利要求16所述的机器人的逆变换处理方法，其中，

具备形态确定部，对于逆变换处理的结果计算雅可比矩阵的行列式，并且根据该行列式的符号是否与预先指定的手腕形态相符，来确定逆变换处理结果的手腕形态。

19.一种机器人的控制装置，适用于具备垂直六轴型的臂的机器人，该垂直六轴型的臂的机器人结构为具有配置第五轴、并且连接第四轴和第六轴的连杆长度d5的偏置臂，从而使第四轴的轴心与第六轴的轴心平行，所述机器人的控制装置通过以所述垂直六轴型的臂的前端部为控制点，并且对成为所述控制点的目标的位置及姿态进行逆变换处理，来计算各轴的角度，

所述机器人的控制装置具备：

位置暂时确定部，暂时确定第六轴的位置，并且基于该暂时确定的位置P6’将第五轴的位置暂时确定为P5’；

动作范围判定部，判定所述位置P5’及P6’是否在基于所述机器人的连杆参数的动作范围内；

逆变换处理部，如果所述位置P5’及P6’在动作范围内，则对于基于这些位置的齐次变换矩阵，将连杆长度d5设定为0并进行逆变换处理；

正变换处理部，使用通过所述逆变换处理求出的各轴的角度，进行正变换处理；

评价值计算部，求出所述控制点的目标位置与通过所述正变换处理得到的位置之差的位置矩阵p_E，并且对与所述目标位置相对应的旋转矩阵乘以通过所述正变换处理得到的旋转矩阵的逆矩阵，求出旋转矩阵R_E；以及

迭代运算执行部，当所述位置矩阵p_E的范数||p_E||超过阈值或者通过所述旋转矩阵R_E求出的角度||R_E||超过阈值时，求出所述旋转矩阵R_E的旋转轴矢量q_E，

并且求出所述角度||R_E||与单位旋转轴矢量q_E／||q_E||之积即旋转矢量r_E，

根据对由所述位置矩阵p_E与所述旋转矢量r_E确定的位置矩阵P_E乘以雅可比矩阵的逆矩阵得到的结果，对所述角度进行更新，并反复执行从所述正变换处理开始的处理，所述雅可比矩阵是根据由所述逆变换处理求出的各轴的角度求出的。

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

如果所述位置P5’及P6’不在动作范围内，则所述逆变换处理部以使所述位置P5’及P6’位于该动作范围内的方式对所述位置P5’及P6’进行修正。

21.根据权利要求19或20所述的机器人的控制装置，其中，

具备形态确定部，对于所述逆变换处理的结果计算雅可比矩阵的行列式，并且根据该行列式的符号是否与预先指定的手腕形态相符，来确定逆变换处理结果的手腕形态。

22.一种机器人的逆变换处理方法，适用于具备垂直六轴型的臂的机器人，该垂直六轴型的臂的机器人结构为具有配置第五轴、并且连接第四轴和第六轴的连杆长度d5的偏置臂，从而使第四轴的轴心与第六轴的轴心平行，所述机器人的逆变换处理方法通过以所述垂直六轴型的臂的前端部为控制点，并且对成为所述控制点的目标的位置及姿态进行逆变换处理，来计算各轴的角度，

所述机器人的逆变换处理方法包括：

暂时确定第六轴的位置，并且基于该暂时确定的位置P6’将第五轴的位置暂时确定为P5’的步骤；

判定所述位置P5’及P6’是否在基于所述机器人的连杆参数的动作范围内的步骤；

如果所述位置P5’及P6’在动作范围内，则对于基于这些位置的齐次变换矩阵，将连杆长度d5设定为0并进行逆变换处理的步骤；

使用通过所述逆变换处理求出的各轴的角度，进行正变换处理的步骤；以及

求出所述控制点的目标位置与通过所述正变换处理得到的位置之差的位置矩阵p_E，并且对与所述目标位置相对应的旋转矩阵乘以通过所述正变换处理得到的旋转矩阵的逆矩阵，求出旋转矩阵R_E的步骤；

当所述位置矩阵p_E的范数||p_E||超过阈值或者通过所述旋转矩阵R_E求出的角度||R_E||超过阈值时，求出所述旋转矩阵R_E的旋转轴矢量q_E，

并且求出所述角度||R_E||与单位旋转轴矢量q_E／||q_E||之积即旋转矢量r_E，

根据对由所述位置矩阵p_E与所述旋转矢量r_E确定的位置矩阵P_E乘以雅可比矩阵的逆矩阵得到的结果，对所述角度进行更新，并从所述进行正变换处理的步骤开始反复执行，所述雅可比矩阵是根据由所述逆变换处理求出的各轴的角度求出的。

23.根据权利要求22所述的机器人的逆变换处理方法，其中，

如果所述位置P5’及P6’不在动作范围内，则以使所述位置P5’及P6’位于该动作范围内的方式对所述位置P5’及P6’进行修正。

24.根据权利要求22或23所述的机器人的逆变换处理方法，其中，

具备形态确定部，对于逆变换处理的结果计算雅可比矩阵的行列式，并且根据该行列式的符号是否与预先指定的手腕形态相符，来确定逆变换处理结果的手腕形态。

Images