CN103781602A - 机器人系统及机器人控制装置 - Google Patents

Abstract

本发明提供提高冗余机器人的操作性的机器人系统（10）以及机器人控制装置（14）。机器人系统（10）包括：机器人（12），具有通过伺服马达驱动并包含冗余轴的多个关节轴；以及机器人控制装置（14），控制机器人（12）。机器人控制装置（14）具有：第一指令生成部（14a），生成机器人（12）的第一关节角速度指令；极限回避指令生成部（14b），生成用于回避机器人（12）达到动作界限的状态的机器人（12）的关节角速度指令A；姿态最优化指令生成部（14c），生成用于使机器人（12）的姿态最优化的机器人（12）的关节角速度指令B；零空间矩阵计算部（14d），计算与控制点有关的雅可比矩阵的零空间矩阵；第二指令生成部（14e），基于关节角速度指令A、关节角速度指令B以及零空间矩阵，生成机器人（12）的第二关节角速度指令；以及控制部（14f），控制伺服马达。

Description

机器人系统及机器人控制装置

技术领域

本发明涉及一种机器人系统及机器人控制装置。

背景技术

在专利文献1中记载了一种机器人动作轨迹示教方法，其使用除了具有由机器人执行的作业所要求的自由度以外、还具有不影响机器人作业的冗余自由度的机器人，能够创建动作轨迹的示教数据。

该动作轨迹示教方法的特征在于：将冗余自由度作为要素，基于表示机器人的关节角度空间的余裕的评价函数来确定冗余自由度。

现有技术文献

专利文献

专利文献1：日本特开平6-187020号公报

发明内容

本发明所要解决的问题

本发明的目的是提供与不具有本发明的结构的情况相比提高了冗余机器人的操作性的机器人系统及机器人控制装置。

用于解决问题的方法

为了解决上述问题，根据本发明的一个方面，应用一种机器人系统，包括：

机器人，所述机器人具有通过伺服马达驱动并包含冗余轴的多个关节轴；以及

机器人控制装置，所述机器人控制装置控制所述机器人，

所述机器人控制装置具有：

第一指令生成部，所述第一指令生成部基于所输入的所述机器人的控制点的速度指令，生成所述机器人的第一关节角速度指令；

极限回避指令生成部，所述极限回避指令生成部在信号A被输入的期间，基于评价函数A，生成用于回避所述机器人达到动作极限的状态的该机器人的关节角速度指令A；

姿态最优化指令生成部，所述姿态最优化指令生成部在信号B被输入的期间，基于评价函数B，生成用于使所述机器人的姿态最优化的该机器人的关节角速度指令B；

零空间矩阵计算部，所述零空间矩阵计算部计算与所述控制点有关的雅可比矩阵的零空间矩阵；

第二指令生成部，所述第二指令生成部基于所述关节角速度指令A、所述关节角速度指令B以及所述零空间矩阵，生成所述机器人的第二关节角速度指令；以及

控制部，所述控制部基于第三关节角速度指令控制所述伺服马达，所述第三关节角速度指令是所述第一和第二关节角速度指令之和。

另外，根据本发明的另一方面，应用一种机器人控制装置，具有：

第一指令生成部，所述第一指令生成部基于具有包含冗余轴的多个关节轴的机器人的控制点的速度指令，生成所述机器人的第一关节角速度指令；

极限回避指令生成部，所述极限回避指令生成部在信号A被输入的期间，基于评价函数A，生成用于回避所述机器人达到动作极限的状态的该机器人的关节角速度指令A；

姿态最优化指令生成部，所述姿态最优化指令生成部在信号B被输入的期间，基于评价函数B，生成用于使所述机器人的姿态最优化的该机器人的关节角速度指令B；

零空间矩阵计算部，所述零空间矩阵计算部计算与所述控制点有关的雅可比矩阵的零空间矩阵；

第二指令生成部，所述第二指令生成部基于所述关节角速度指令A、所述关节角速度指令B以及所述零空间矩阵，生成所述机器人的第二关节角速度指令；以及

控制部，所述控制部基于第三关节角速度指令控制伺服马达，所述第三关节角速度指令是所述第一以及第二关节角速度指令之和。

发明效果

根据本发明，与不具有本发明的结构的情况相比，能够提高冗余机器人的操作性。

附图说明

图1是本发明的第一实施方式涉及的机器人系统的结构图。

图2是表示该机器人系统的动作的流程图。

图3是表示该机器人系统的极限回避动作的流程图。

图4是表示该机器人系统的姿态最优化动作的流程图。

图5是设置在该机器人系统所具有的示教器上的显示画面的说明图。

图6是该机器人系统所具有的示教器的功能框图。

图7是该机器人系统具有的机器人控制装置所具有的极限回避指令生成部所计算的评价函数Ｖ_ＪＬＣ的说明图。

图8是该机器人系统具有的机器人控制装置所具有的极限回避指令生成部所计算的评价函数Ｖ_ＯAＣ的说明图。

图9是本发明第二实施方式涉及的机器人系统的示教器的说明图。

图10是表示该机器人系统的动作的流程图。

具体实施方式

接下来，参照附图对将本发明具体化的实施例进行说明，以供理解本发明。此外，在各附图中，对于与说明无关的部分，有时省略图示。

<第一实施方式>

如图1所示，本发明的第一实施方式涉及的机器人系统10包括：具有第一至第七关节轴J1至J7的机器人12；控制该机器人12的动作的机器人控制装置14；以及用于示教机器人12的位置姿态的示教器16。

图1所示的机器人坐标系Cr例如是将位于机器人12的第一关节轴J1上的点作为原点、由向机器人12的前后方向延伸的X轴、向左右方向延伸的Y轴以及向上下方向延伸的Z轴组成的直角坐标系。

机器人12为冗余机器人，例如包含关节轴J3作为冗余轴的七轴垂直多关节机器人。各关节轴J1至J7通过内置有编码器的伺服马达（图未示）驱动。机器人12不限于七轴垂直多关节机器人。作为机器人的其他的例子，可列举针对机器人作业空间的自由度、冗余地具有关节轴的机器人。例如，只具有分别绕机器人坐标系Cr的Y轴旋转的三个关节的三轴机器人在XZ平面上具有冗余性。因此，该三轴机器人是对于在XZ平面上的动作、冗余地具有关节轴的机器人。即，机器人与关节的轴数或轴结构无关，只要是具有冗余自由度的冗余机器人即可。

机器人控制装置14与机器人12连接。机器人控制装置14具有：第一指令生成部14a；极限回避指令生成部14b；姿态最优化指令生成部14c、零空间矩阵计算部14d、第二指令生成部14e、位置速度控制部（控制部的一个例子）14f和监视部14g。机器人控制装置14搭载有图未示的CPU，图1所示的各方框的功能通过由CPU执行的软件程序、硬件实现。

当输入机器人12的手端的速度指令（以下，简称“手端速度指令”）V时，第一指令生成部14a能够基于速度指令V生成关节角速度指令（第一关节角速度指令）ωref1。

当从示教器16输入信号A时，极限回避指令生成部14b能够输出机器人12用于回避极限的关节角速度指令（关节角速度指令A）φ_ＬＭＴ。关于信号A，在以下阐述。

在此，所谓极限是指机器人12处于动作极限的状态，例如，以下的第一至第三极限。

第一极限是关节轴到达可动界限角度时的极限。对于可动界限角度，在机器人12的设计上，存在有以机械方式确定的可动界限角度以及使用软件任意设定的可动界限角度，可以应用这两者中的任一界限角度。

第二极限是机器人12接近障碍物（例如，周围机器或其他的机器人）直至使用软件所设定的距离时的极限。

第三极限是机器人12接近特异姿态直至使用软件所设定的范围时的极限。过大的关节角速度指令被赋予到达第三极限的机器人。在此，机器人的特异姿态是指由于机器人关节位置和旋转轴方向等运动学的状态导致失去特定的自由度的姿态。例如，在机器人12的第二关节轴J2、第四关节轴J4以及第六关节轴J6排列在一条直线上的姿态下，当向特定的方向赋予动作指令时，这些关节的关节角速度指令变得过大。

极限回避指令生成部14b能够与所述的极限种类相应地，分别生成1）用于回避第一极限的关节角速度指令φ_ＪＬＣ、2）用于回避第二极限的关节角速度指令φ_ＯAＣ、以及3）用于回避第三极限的关节角速度指令φ_ＳAＣ。关于这些关节角速度指令φ_ＪＬＣ、φ_ＯAＣ、φ_ＳAＣ的详细的生成方法，在下面阐述。

极限回避指令生成部14b能够将所生成的关节角速度指令φ_ＪＬＣ、φ_ＯAＣ、φ_ＳAＣ之和作为关节角速度指令φ_ＬＭＴ进行输出。

姿态最优化指令生成部14c当从示教器16输入信号B时，能够输出用于使机器人12的姿态最优化的关节角速度指令（关节角速度指令B）φ_ＯＰＴ。关于信号B，在下面阐述。

在此，关于机器人12姿态的最优化，例如，存在有以下的第一及第二最优化。第一最优化是用于缩短机器人12动作时间的最优化，第二最优化是提高机器人12的可操作度的最优化。

姿态最优化指令生成部14c能够与最优化种类相应地，分别生成1）用于缩短动作时间的关节角速度指令φ_ＴＭＣ、以及2）用于提高可操作度的关节角速度指令φ_ＳAＣ。关于这些关节角速度指令φ_ＴＭＣ、φ_ＳAＣ的详细的生成方法，在下面阐述。此外，基于从示教器16输出的信号B_ＳＥＬ值，判定最优化的种类。

姿态最优化指令生成部14c能够将所生成的各关节角速度指令φ_ＴＭＣ、φ_{ＳA Ｃ}作为关节角速度指令φ_ＯＰＴ输出。

零空间矩阵计算部14d能够基于机器人12的关节角度，计算与机器人12的手端（控制点的一个例子）有关的雅可比矩阵J的零空间矩阵N。详细而言，零空间矩阵计算部14d能够根据关节角度（关节角度的当前值）求解机器人12的手端的位置姿态以及各关节轴J1至J7的方向，并计算与机器人12的手端有关的雅可比矩阵的零空间矩阵。

该零空间矩阵是指用于在冗余机器人中在维持手端的位置姿态的状态下生成冗余自由度的动作指令的运算矩阵。在此，该动作指令不是仅针对冗余轴这样的特定轴的指令，而是针对冗余自由度（不影响手端位置姿态的关节自由度）的指令，控制对象轴是包含冗余轴的所有的轴或一部分轴，并且根据每个控制周期的机器人的姿态和动作方向而不同。在此，关于该控制对象轴，以仅具有分别绕机器人坐标系Cr的Y轴旋转的三个关节轴（第一至第三关节轴）的三轴机器人为例进行说明。作为该情况下的冗余自由度的控制对象轴，不限于三轴机器人的第一至第三关节轴中的一个关节轴，而是在维持手端位置的状态下能够绕机器人坐标系Cr的Y轴旋转（能够使姿态改变）的第一至第三关节轴中的至少一个关节轴。关于将哪个关节轴作为控制对象轴，使用零空间矩阵进行计算。例如，在第三关节轴到达动作极限而无法对使手端向Z轴方向或X轴方向移动的动作产生作用的情况下，作为冗余自由度的控制对象轴成为第一关节轴以及第二关节轴。

基于与手端有关的雅可比矩阵J、雅可比矩阵的伪逆矩阵Ｊ^＋和单位矩阵I，通过下式，计算零空间矩阵N。

Ｎ＝Ｉ－Ｊ＋Ｊ     式（1）

当将机器人12的手端以外的部位作为控制对象部位（控制点）时，使用控制对象部位的雅可比矩阵。

此外，基于将从编码器中反馈的编码器值乘以减速比得到的结果，求解机器人12的关节角度及关节角速度。

第二指令生成部14e能够基于关节角速度指令φ_ＬＭＴ、关节角速度指令φ_{ＯＰ Ｔ}以及零空间矩阵N，生成关节角速度指令（第二关节角速度指令）ωref2。所生成的关节角速度指令ωref2被累加到第一指令生成部14a所生成的关节角速度指令ωref1，而成为关节角速度指令（第三关节角速度指令）ωref3（参照图1）。

位置速度控制部14f能够基于关节角速度指令ωref3与关节角速度反馈ωfb之差e，控制伺服马达，并控制各关节位置速度。在本实施方式中，作为向位置速度控制部14f输出的指令，使用关节角速度指令ωref3。此外，作为向位置速度控制部14f输出的指令，也可以使用在机器人12的当前关节角度上累加关节角速度指令ωref3得到的关节角度指令。

此外，关于位置速度控制部14f，设置与构成机器人12的关节轴J1至J7的数量相对应的数量的位置速度控制部14f。

监视部14g能够判定机器人12是否达到极限。在机器人12达到极限的情况下，监视部14g能够使机器人12的动作停止并输出警报。

具体而言，监视部14g能够通过监视各关节轴J1至J7的角度，来判定是否达到第一极限。

另外，监视部14g能够基于预先登录的障碍物的位置以及尺寸计算机器人12与障碍物之间的距离，从而判定是否达到第二极限。在障碍物的位置以及尺寸没有被预先登录到机器人控制装置14的情况下，操作者通过目视来判定是否到达第二极限，也能够使机器人12的动作停止。

另外，由于机器人12接近特异姿态时生成过大的关节角速度指令ωref3，因此监视部14g通过监视机器人12的姿态（机器人12的关节角度的当前值θfb）与关节角速度指令ωref3，能够判定是否到达第三极限。机器人12的特异姿态是指第二关节轴J2、第四关节轴J4以及第六关节轴J6排列在一条直线上的姿态。机器人的特异姿态根据连杆的结构而不同。

示教器16与机器人控制装置14连接。示教器16上设置有点动操作按键20、第一极限回避按键(第一信号输出部的一个例子)21、第一姿态最优化按键（第二信号输出部的一个例子）22以及显示画面32。

机器人12的操作者能够使用示教器16上设置的点动操作按键20对机器人12进行点动操作，从而进行机器人12的作业程序的示教。

另外，操作者能够使用示教器16上设置的演示按键19，进行作业程序的演示动作。

另外，操作者能够使用示教器16，使机器人12的控制对象部位（控制点）基于指定的坐标系进行动作。

在此，控制对象部位例如是机器人的手端。作为控制对象部位的其他的例子，可以列举出机器人12的关节的特定部位或安装在手端上的末端执行器的前端等。

另外，指定坐标系例如是机器人坐标系Cr。在选择机器人坐标系Cr作为指定坐标系的情况下，能够使机器人12的手端在X轴方向、Y轴方向及Z轴方向上、以及围绕X轴的旋转方向、围绕Y轴的旋转方向及围绕Z轴的旋转方向上移动。作为指定坐标系的其他的例子，可以列举出关节坐标系或机器人12的操作者任意设定的坐标系等。

点动操作按键20是操作者用于对机器人12进行点动操作的按键。当点动操作按键20被按压时，示教器16将预先设定的手端速度指令V输出。

第一极限回避按键21是操作者用于指示回避极限的按键。当第一极限回避按键21被按压时，示教器16将信号A输出。第一极限回避按键21瞬时动作。即，示教器16在第一极限回避按键21被按压的期间将信号A输出。

第一姿态最优化按键22是操作者用于指示使机器人12的姿态最优化的按键。示教器16在第一姿态最优化按键22被按压时将信号B输出。第一姿态最优化按键22瞬时动作。即，示教器16在第一姿态最优化按键22被按压的期间将信号B输出。

在示教器16的显示画面32上，设置有与机器人控制装置14所具有的功能相对应的显示领域，并且在该显示领域内显示文字和图像等信息。在执行下述的用于辅助机器人12的姿态最优化的姿态最优化辅助功能时，在显示画面32上，如图5所示，在左侧设置有显示领域D1，在右侧上方设置有显示领域D2，在右侧下方设置有显示领域D3。在显示领域D1上显示记录了机器人12的示教数据的示教程序。在显示领域D2上显示表示当前机器人12的姿态的图形图像。在显示领域D3上显示表示姿态被最优化的机器人12的图形图像。

如图6所示，示教器16还具有：姿态最优化指令生成部16c、零空间矩阵计算部16d、第二指令生成部16e、以及机器人模型描绘部（描绘部的一个例子）16r。

姿态最优化指令生成部16c与设置在机器人控制装置14上的姿态最优化指令生成部14c实质上相同地进行动作。姿态最优化指令生成部16c能够在信号S被输入时，输出用于使机器人12的姿态最优化的关节角速度指令φ_ＯＰＴa。

零空间矩阵计算部16d与设置在机器人控制装置14上的零空间矩阵计算部14d实质上相同地进行动作。零空间矩阵计算部16d能够基于机器人模型描绘部16r输出的关节角度θ_ｍfb，计算与机器人模型的手端有关的雅可比矩阵的零空间矩阵Na。

第二指令生成部16e与设置在机器人控制装置14上的第二指令生成部14e实质上相同地进行动作。第二指令生成部16e能够基于关节角速度指令φ_ＯＰＴa以及零空间矩阵Na生成关节角速度指令ωref2a。所生成的关节角速度指令ωref2a在积分器16i中被积分，被累加到从机器人控制装置14发送的机器人12的关节角度的当前值θfb，而成为关节角度θ_ｍ。

机器人模型描绘部16r能够基于机器人12的关节角度，创建机器人12的图形图像并将该图形图像描绘在显示画面32上。机器人模型描绘部16r能够基于从机器人控制装置14发送的机器人12关节角度的当前值θfb，将该关节角度θfb的机器人模型的图形图像描绘在显示领域D2。如果机器人12的关节角度θfb变化，则机器人模型描绘部随时更新图形图像的描绘。

另外，机器人模型描绘部16r能够基于关节角度θ_ｍ，将关节角度为角度θ_ｍ的机器人模型的图形图像描绘在显示领域D3。

在示教器16上设置有切换按键25（参照图1），该切换按键25将操作者所操作的对象切换成实体机器人12及显示领域D3上显示的机器人模型的任一者。

此外，姿态最优化指令生成部16c、零空间矩阵计算部16d、第二指令生成部16e以及机器人模型描绘部16r不一定必须设置在示教器16上。即，姿态最优化指令生成部16c、零空间矩阵计算部16d、第二指令生成部16e以及机器人模型描绘部16r中的一部分例如可以设置在机器人控制装置14上，机器人控制装置14和示教器16通过相互通讯来收发数据，也能够实现相同的结构。

接下来，参照图2，说明机器人系统10的动作。

操作者能够按压点动操作按键20，对机器人12的手端的位置姿态进行操作。在对手端的位置姿态进行操作的期间机器人12到达极限的情况下，操作者能够按压第一极限回避按键21，使包含冗余轴J3的机器人12的所有的关节轴J1至J7动作，维持手端的位置姿态的同时回避极限。另外，操作者能够按压第一姿态最优化按键22，使包含冗余轴J3的机器人12的所有的关节轴J1至J7动作，维持手端的位置姿态的同时使机器人12的姿态最优化。

（步骤Sa1）

操作者按压点动操作按键20，使机器人12的手端向期望的位置姿态移动。

手端速度指令V从示教器16被输入到图1所示的第一指令生成部14a。第一指令生成部14a基于手端速度指令V以及与机器人12的手端有关的雅可比矩阵的伪逆矩阵Ｊ^＋，使用式（2）生成关节角速度指令ωref1。

ωref1＝Ｊ＋Ｖ     式（2）

第一指令生成部14a也可以在不使用式（2）的情况下生成关节角速度指令ωref1。例如，第一指令生成部14a能够在手端速度指令V被积分并切换成手端位置指令之后，通过进行逆运动学计算，生成关节角速度指令ωref1。

（步骤Sa2）

监视部14g判定机器人12是否到达极限。

监视部14g在判定为机器人12到达极限的情况下，使机器人12停止，并输出警报。之后，执行下一步骤Sa3中所示的用于回避极限的极限回避动作。

监视部14g在判定为机器人12没有到达极限的情况下，进入到步骤Sa4。

（步骤Sa3）

当机器人12的操作者将手指从点动操作按键20移开并按压第一极限回避按键21时，信号A被输入到极限回避指令生成部14b（参照图1），以下的步骤Sa31以及步骤Sa32（参照图3）按照顺序被进行处理。

其结果是，机器人12执行用于回避极限的极限回避动作。

（步骤Sa31）

极限回避指令生成部14b计算用于分别回避与监视部14所输出的警报相对应的第一至第三极限的关节角速度指令φ_ＪＬＣ、φ_ＯAＣ、φ_ＳAＣ。以下，说明各个关节角速度指令φ_ＪＬＣ、φ_ＯAＣ、φ_ＳAＣ的计算方法。

（1）关节角速度指令φ_ＪＬＣ的计算

基于评价函数Ｖ_ＪＬＣ（评价函数A的一个例子），通过式（3）、式（4a）、式（4b），计算用于回避第一极限的关节角速度指令φ_ＪＬＣ。

φ_ＪＬＣ＝－ｇｒaｄ（Ｖ_ＪＬＣ）     式（3）

θ_-＜θ＜θ_＋的情况：

Ｖ_ＪＬＣ＝0     式（4a）

θ_-＜θ＜θ_＋以外的情况：

Ｖ_ＪＬＣ＝0.5Ｋ_ＪＬＣ（θ_ＬＩＭ－θ）²     式（4b）

式（3）、式（4a）以及式（4b）的处理是针对各关节轴J1至J7进行的。系数Ｋ_ＪＬＣ是关节角速度指令的增益（权重）。

角度θ_ＬＩＭ是被设定作为用于限制各关节的可动范围的角度（可动界限角度）的设定角度。角度θ是各关节角度的当前值。

角度θ_－以及角度θ_＋分别是设定角度θ_ＬＩＭ的上限以及下限，表示计算评价函数Ｖ_ＪＬＣ的限制区域（参照图7）。

如式（4a）以及式（4b）所示，仅对侵入限制范围内的关节，计算评价函数Ｖ_ＪＬＣ，对于限制范围以外的关节，将评价函数设为零。此外，当第一极限的回避结束时，关节角度指令变为零。

如式（4b）所示，基于设定角度θ_ＬＩＭ与各关节的角度反馈（关节角度的当前值）θ之差的大小，求解评价函数Ｖ_ＪＬＣ。

因此，根据评价函数Ｖ_ＪＬＣ的梯度，以使关节远离关节可动区域的方式，计算关节角速度指令φ_ＪＬＣ。

（2）关节角速度指令φ_ＯAＣ的计算

基于评价函数Ｖ_ＯAＣ（评价函数A的一个例子），通过式（5）、式（6a）、式（6b），计算用于回避第二极限的关节角速度指令φ_ＯAＣ。

φ_ＯAＣ＝－Ｊ_Ｐ ^Ｔｇｒaｄ（Ｖ_ＯAＣ）     式（5）

Ｄ_-＜Ｄ＜Ｄ_＋的情况：

Ｖ_ＯAＣ＝0     式（6a）

Ｄ_―＜Ｄ＜Ｄ_＋以外的情况：

Ｖ_ＯAＣ＝0.5Ｋ_ＯAＣ（Ｄ_ＬＩＭ－Ｄ）²     式（6b）

在式（5）中，矩阵Ｊ_Ｐ是根据机器人12的与障碍物最接近的部位（最近点）计算的最近点的雅可比矩阵。此外，当在机器人12回避极限的期间机器人12的关节角度的当前值发生改变时，重新计算最近点。

式（5）的处理是针对各关节轴J1至J7进行的。式（6a）以及式（6b）的处理针对机器人坐标系Cr的各轴（X轴、Y轴和Z轴）进行。系数Ｋ_ＯAＣ是回避指令的增益（权重）。

距离Ｄ_ＬＩＭ是被设定作为机器人12能够接近障碍物的距离的设定距离。关于障碍物，例如，通过读入CAD数据或离线仿真模型数据，在机器人控制装置14中作为模型数据而设定。另外，作为其他的例子，通过使用摄像机或激光传感器等实际测量障碍物数据来设定障碍物。此外，作为其他的例子，通过由操作者操作示教器16来设定障碍物。

距离Ｄ是从障碍物到机器人12的最短距离。

距离Ｄ_－以及距离Ｄ_＋分别是距离Ｄ_ＬＩＭ的上限以及下限，表示计算评价函数Ｖ_ＯAＣ的限制范围（参照图8）。

如式（6a）以及式（6b）所示，对机器人12侵入限制范围内的情况，计算评价函数Ｖ_ＯAＣ，对于机器人12在限制范围以外的情况，将评价函数Ｖ_ＯAＣ设为零。此外，当第二极限的回避结束时，关节角速度指令φ_ＯAＣ变为零。

如式（6b）所示，基于距离Ｄ_ＬＩＭ与距离Ｄ之差的大小，求解评价函数Ｖ_{Ｏ AＣ}。

因此，根据评价函数Ｖ_ＯAＣ的梯度以及雅可比矩阵Ｊ_Ｐ，以使机器人12远离障碍物的方式，计算关节角速度指令φ_ＯAＣ。

（3）关节角速度指令φ_ＳAＣ的计算

基于评价函数V_ＳAＣ（评价函数A的一个例子），通过式（7）、式（8a）、式（8b），计算用于回避第三极限的关节角速度指令φ_ＳAＣ。

φ_ＳAＣ＝－ｇｒaｄ（V_ＳAＣ）     式（7）

ω_―＜ω＜ω_＋的情况：

V_ＳAＣ=0     式（8a）

ω_―＜ω＜ω_＋以外的情况：

[数学式1]

$V_{\mathrm{SAC}}=K_{\mathrm{SAC}}\sqrt{\det \left(J^{T}J\right)}$ 式（8b）

式（7）、式（8a）以及式（8b）的处理是针对各关节轴J1至J7进行的。系数Ｋ_ＳAＣ是关节角速度指令的增益（权重）。

角速度指令ω是各关节的角速度指令。

角速度ω_－以及角速度ω_＋分别是关节角速度指令ω的上限以及下限，表示计算评价函数Ｖ_ＳAＣ的限制角速度。

如式（8a）以及式（8b）所示，仅对超出限制角速度ω_－、ω_＋并且被赋予关节角速度指令ω的关节，计算评价函数Ｖ_ＳAＣ，对于除此之外的关节，将评价函数Ｖ_ＳAＣ设为零。此外，当第三极限的回避结束时，关节角速度指令φ_ＳAＣ变为零。

基于角速度指令ω进行式（8a）、式（8b）的情况划分。但是，在情况划分之前进行特异姿态的判定。即，事先实施与机器人12的姿态有关的判定，在判定为是特异姿态之后，对式（8a）、式（8b）进行运算。

如式（8b）所示，基于机器人12的手端的可操作度，求解评价函数Ｖ_ＳAＣ。

因此，根据评价函数Ｖ_ＳAＣ的梯度，以使关节远离特异点的方式，计算关节角速度指令φ_ＳAＣ。

极限回避指令生成部14b将所生成的各关节角速度指令φ_ＪＬＣ、φ_ＯAＣ、φ_{ＳA Ｃ}作为关节角速度指令φ_ＬＭＴ输出。

但是，极限回避指令生成部14b将所生成的各个关节角速度指令φ_ＪＬＣ、φ_ＯAＣ、φ_ＳAＣ之和作为关节角速度指令φ_ＬＭＴ输出。即，在发生了两个以上极限的情况下，用于分别回避这些极限的关节角速度指令之和成为关节角速度指令φ_{Ｌ ＭＴ}。

（步骤Sa32）

第二指令生成部14e通过下式计算用于使用零空间矩阵N维持手端的位置姿态的同时回避极限的关节角速度指令ω_ＬＭＴ，并将计算出的关节角速度指令ω_ＬＭＴ生成作为关节角速度指令ωref2。如上所述，通过使用零空间矩阵N在各控制周期中确定作为冗余自由度的控制对象轴，并计算关节角速度指令ω_ＬＭＴ。

ω_ＬＭＴ＝Nφ_ＬＭＴ     式（9）

在此，关节角速度指令ω_ＬＭＴ是极限回避指令生成部14b所输出的用于回避极限的关节角速度指令φ_ＪＬＣ、φ_ＯAＣ、φ_ＳAＣ之和。

此外，具有除了使用式（9）以外、使用式（10）至式（12）维持手端的位置姿态的同时计算回避极限的关节角速度指令ω_ＬＭＴ的方法。

Ｖ_ＬＭＴ＝Ｊφ_ＬＭＴ     式（10）

在此，Ｖ_ＬＭＴ是由φ_ＬＭＴ计算出的虚拟的手端速度指令。

[数学式2]

式（11）

在此，

是由Ｖ_ＬＭＴ计算出的虚拟的关节角速度指令。

[数学式3]

式（12）

式（12）的第一项（φ_ＬＭＴ）是用于回避极限的关节角速度。式（12）的第二项是生成Ｖ_ＬＭＴ的关节角速度之中角速度标准最小的关节角速度指令。由于第一项与第二项不同，因此能够维持手端的位置姿态的同时回避极限。

当机器人12的操作者将手指从第一极限回避按键21移开时，第二指令生成部14e输出的关节角速度指令ωref2变为零。

以上，步骤Sa31以及步骤Sa32按顺序被处理，由此进行步骤Sa3所示的极限回避动作。

由此，在机器人12到达极限的情况下，通过使用第一极限回避按键21，例如，机器人12的操作者使冗余轴向哪个方向移动多少次才能够回避极限这样的对回避动作进行关注的繁琐事情减少。即，操作者通过按压第一极限回避按键21就能够在线地回避极限。另外，操作者通过调整按压第一极限回避按键21的时间，能够确定实体的机器人12的关节动作角度。

（步骤Sa4）

操作者判断机器人12的手端的位置姿态是否到达期望的位置姿态。

在操作者判断为手端的位置姿态到达期望位置姿态的情况下，执行下面的步骤Sa5。

另一方面，操作者判定为手端的位置姿态没有到达期望的位置姿态的情况下，执行步骤Sa1。即，操作者使用示教器16使机器人12再次移动。但是，在机器人12到达动作范围界限（到达界限）的情况下，机器人12无法再进一步向规定方向移动。到达界限是根据机器人12的连杆长度或关节可动区域等设计上的参数所确定的。到达界限被预先设定在机器人控制装置14中。

（步骤Sa5）

在手端的位置姿态到达期望的位置姿态时，操作者选择最优化的种类（第一最优化或第二最优化），当机器人12的操作者按压第一姿态最优化按键22时，信号B_ＳＥＬ以及信号B被输入到姿态最优化指令生成部14c。之后，以下的步骤Sa51以及步骤Sa52(参照图4)按照顺序被进行处理。

其结果是，使用机器人12的冗余自由度，执行维持手端的位置姿态的同时使具有冗余自由度的机器人12的姿态最优化的姿态最优化动作。

（步骤Sa51）

图1所示的姿态最优化指令生成部14c根据操作者所选择的最优化种类（信号B_ＳＥＬ的值），计算用于缩短动作时间的关节角速度指令φ_ＴＭＣ或用于提高可操作度的关节角速度指令φ_ＳAＣ。此外，极限回避指令生成部14b所生成的用于回避第三极限的φ_ＳAＣ与姿态最优化指令生病部14c所生成的用于提高可操作度的φ_ＳAＣ两者都是使机器人从异常点远离的指令，因此使用相同的变量φ_ＳAＣ。

（1）计算关节角速度指令φ_ＴＭＣ

基于评价函数Ｖ_ＴＭＣ（评价函数B的一个例子），通过式（13）、式（14）计算用于缩短动作时间的关节角速度指令φ_ＴＭＣ。

φ_ＴＭＣ＝－ｇｒaｄ（Ｖ_ＴＭＣ）     式（13）

Ｖ_ＴＭＣ＝0.5Ｋ_ＴＭＣ（θ_ＴＭＣ－θ）²     式（14）

在式（13）以及式（14）中，变量i表示关节轴的编号（1至7）。系数Ｋ_ＴＭＣ是关节角速度指令的增益（权重）。

角度θ_ＴＭＣ是前示教点（现示教点的前一个示教点）的关节角度。通过控制现示教点上的机器人12的关节轴的关节角度使其接近前示教点上的机器人12的关节轴的关节角度，从前示教点到现示教点的机器人12的动作时间缩短。关于将哪个轴作为控制对象（赋予用于缩短动作时间的关节角速度指令），根据每个控制周期的机器人的姿态、动作方向而不同，使用零空间矩阵N进行计算。

角度θ是各关节角度的当前值。

如式（14）所示，根据角度θ_ＴＭＣ与各关节的角度反馈（各关节的角度的当前值）θ之差的大小，求解评价函数Ｖ_ＴＭＣ。

因此，根据评价函数Ｖ_ＴＭＣ的梯度，计算接近前示教位置的关节角速度指令、即、缩短动作时间的关节角速度指令φ_ＴＭＣ。

通过将最大动作速度较大的关节的增益设定得较大，能够进一步缩短动作时间。即，通过使动作速度快的关节大幅地动作并使动作速度慢的关节小幅地动作，生成更高效的动作指令。

（2）关节角速度指令φ_ＳAＣ的计算

基于评价函数Ｖ_ＳAＣ（评价函数B的一个例子），通过式（15）、式（16），计算用于提高可操作度的关节角速度指令φ_ＳAＣ。

φ_ＳAＣ＝－ｇｒaｄ（Ｖ_ＳAＣ）     式（15）

[数学式4]

$V_{\mathrm{SAC}}=K_{\mathrm{SACi}}\sqrt{\det \left(J^{T}J\right)}$ _式（₁₆）

如式（15）以及式（16）所示，基于机器人12的可操作度计算评价函数Ｖ_ＳAＣ，通过与步骤Sa31的回避异常点相同的方法生成关节角速度指令φ_ＳAＣ。

（步骤Sa52）

图1所示的第二指令生成部14e通过下式计算用于使用零空间矩阵N维持手端的位置姿态的同时使机器人12姿态最优化的关节角速度指令ω_ＯＰＴ，将所计算出的关节角速度指令ω_ＯＰＴ生成作为关节角速度指令ωref2。

ω_ＯＰＴ＝Ｎφ_ＯＰＴ     式（17）

在此，关节角速度指令φ_ＯＰＴ是姿态最优化指令生成部14c所输出的、用于使机器人12的姿态最优化的关节角速度指令φ_ＴＭＣ、φ_ＳAＣ中的任一者。

当机器人12的操作者将手指从第一姿态最优化按键22移开时，第二指令生成部14e所输出的关节角速度指令ωref2变为零。

以上，步骤Sa51以及步骤Sa52按顺序被处理，由此进行步骤Sa5所示的姿态最优化动作。

如此，通过使用与手端有关的零空间矩阵N，能够维持手端的位置姿态的同时使机器人12的姿态最优化。

在此，在步骤Sa5中，机器人12的操作者使用机器人姿态最优化辅助功能，能够预测机器人12的姿态最优化动作中的行动。以下，说明机器人姿态最优化辅助功能。

如上所述，通过图6所示的机器人模型描绘部16r，在显示领域D2中，描绘当前机器人模型的图形图像。另外，通过机器人模型描绘部16r，在显示领域D3中描绘使姿态最优化后的机器人模型的图形图像。

首先，在机器人12的手端到达期望的位置姿态后，操作者按压图1所示的切换按键25，将操作对象从实体的机器人12向显示领域D3中表示的机器人模型切换。另外，选择最优化种类（第一最优化或第二最优化）。

接下来，操作者按压第一姿态最优化按键22。信号S（参照图6）被输入到示教器16的姿态最优化指令生成部16c。姿态最优化指令生成部16c计算与机器人12的操作者所选择的最优化种类相对应的关节角速度指令φ_ＯＰＴa。第二指令生成部16e基于姿态最优化指令生成部16c计算出的关节角速度指令φ_ＯＰＴa、以及零空间矩阵计算部16d计算出的零空间矩阵Na，生成关节角速度指令ωref2a。

机器人模型描绘部16r基于关节角度θ_ｍ在显示领域D3上描绘该关节角度θ_ｍ的机器人模型的图形图像，所述关节角度θ_ｍ是从机器人控制装置14发送的机器人12的关节角度的当前值θfb累加被积分器16i积分后的关节角速度指令得到的。即，在显示领域D3上显示使姿态最优化后的机器人12的图形图像。

此外，操作者通过调整按压第一姿态最优化按键22的时间，能够确定机器人模型的关节的动作角度（最优化的程度）。

在图5所示的显示领域D3中，描绘有通过第二最优化（用于提高可操作度的最优化）被最优化的机器人12。与显示领域D2中描绘的当前机器人12的姿态相比，显示领域D3中描绘的机器人12的手端的可操作度更高。

如此，通过由操作者执行姿态最优化辅助功能，能够在显示领域D3上确认使姿态最优化后的机器人12的图形图像。

另外，针对每个示教位置选择个别的评价指标，通过所选择的评价指标能够使冗余自由度最优化，而不是使用预先确定的统一的评价指标（例如，评价函数）来确定与所有示教数据有关的冗余自由度。换言之，操作者能够针对每个示教点选择最优化手段，而不是将与动作程序内的示教数据有关的位置全部用相同的评价指标进行最优化。

接下来，操作者在显示领域D3上确认机器人模型的姿态后，再次按压切换按键25（参照图1），将操作对象从显示领域D3上显示的机器人模型向实体进行切换。

操作者将实体机器人12的冗余自由度姿态最优化，从而能够确定最终示教位置。

此外，操作者通过调整按压第一姿态最优化按键22的时间，能够确定实体机器人12的关节动作角度（最优化的程度）。

如上所述，本实施方式涉及的机器人系统能够在机器人12到达极限之前，维持手端的位置姿态的同时回避极限。另外，能够在机器人12到达目标位置后，维持手端的位置姿态的同时使冗余自由度的姿态最优化。即，由于操作者能够在线地确定包含冗余自由度的机器人12的关节角度，因此冗余机器人的操作性提高。

<第二实施方式>

接下来，说明本发明的第二实施方式涉及的机器人系统。对于与第一实施方式涉及的机器人系统10相同的构成要素，标注相同的附图标记，有时省略详细的说明。

如图9所示，在示教器116上还设置有第二极限回避按键121以及第二姿态最优化按键122。

第二极限回避按键121是操作者用于指示回避极限的按键。当第二极限回避按键121被按压时，示教器116将信号A输出。第二极限回避按键121交替动作。即，示教器116在第二极限回避按键121被按压一次时，直至第二极限回避按键121被再次按压为止，将信号A输出。

第二姿态最优化按键122是操作者用于指示使机器人12姿态最优化的按键。当第二姿态最优化按键122被按压时，示教器116将信号B输出。第二姿态最优化按键122交替动作。即，示教器116在第二姿态最优化按键122被按压一次，直至第二姿态最优化按键122被再次按压为止，将信号B输出。

接下来，基于图10说明第二实施方式涉及的机器人系统的动作。

（步骤Sb1）

机器人控制装置14读入机器人12的CAD数据、从离线仿真器中读入障碍物和机器人12的模型数据。其结果是，障碍物的模型被设定在机器人控制装置14中。

（步骤Sb2）

操作者按压示教器116上设置的第二极限回避按键121以及第二姿态最优化按键122。从示教器116输出的信号A以及信号B分别被输入到机器人控制装置14的极限回避指令生成部14b以及姿态最优化指令生成部14c。

机器人控制装置14的极限回避指令生成部14b以及姿态最优化指令生成部14c分别输出关节角速度指令φ_ＬＭＴ、φ_ＯＰＴ。

（步骤Sb3）

另外，操作者选择最优化种类（第一最优化或者第二最优化）。与最优化种类相对应的信号B_ＳＥＬ被输入到姿态最优化指令生成部14c。

（步骤Sb4）

操作者为了使机器人12的手端向所期望的位置姿态移动而按压点动操作按键20，当对机器人12进行点动操作时，机器人自动执行极限回避动作以及姿态最优化动作。

用于极限回避动作以及姿态最优化动作的关节角速度指令分别通过与在第一实施方式中说明的步骤Sa3和步骤Sa5所示的处理相同的方法求解。即，在本步骤Sb4中，基于式（2）实现第一指令生成部14a所输出的手端速度指令V的关节角速度指令ωref1与第二指令生成部14e所输出的关节角速度指令ωref2被累加，并生成机器人12的各关节角速度指令ωref3。该关节角速度指令ωref2是关节角速度指令ω_ＬＭＴ（参照式（9））与关节角速度指令ω_ＯＰＴ（参照式（17））之和，该关节角速度指令ω_ＬＭＴ是用于维持手端的位置姿态的同时回避极限的指令，该关节角速度指令ω_ＯＰＴ是用于维持手端的位置姿态的同时使机器人12的姿态最优化的指令。

当第二极限回避按键121及第二姿态最优化按键122被再次按压时，示教器116停止输出信号A以及信号B，极限回避动作或姿态最优化动作的执行被解除。

此外，操作者还能够仅按压第二极限回避按键121和第二姿态最优化按键122中的任一者，仅执行极限回避动作和姿态最优化动作中的任一者。

根据本实施方式所示的方法，由于自动执行极限回避动作以及姿态最优化动作，因此冗余机器人的操作性提高。

本发明不限于上述的实施方式，能够在不改变本发明的主旨的范围内进行变更。例如，将所述实施例或变形例的一部分或全部组合而构成本发明的情况也被包含于本发明的技术范围内。

可以在示教器上仅设置第一和第二极限回避按键以及第一和第二姿态最优化按键中的与必要的功能相对应的按键。

按键不限于由硬件实现的按键。按键也可以是由软件实现的按键。

本发明不仅可应用于如上述实施方式所示使用实体机器人的情况，也可以应用于使用机器人的仿真模型的情况。即，也可以用于使用离线模拟装置来确认示教程序的合理性的情况。

附图标记说明

10：机器人系统12：机器人14：机器人控制装置14a：第一指令生成部14b：极限回避指令生成部14c：姿态最优化指令生成部14d：零空间矩阵计算部14e：第二指令生成部14f：位置速度控制部14g：监视部16：示教器16c：姿态最优化指令生成部16d：零空间矩阵计算部16e：第二指令生成部16i：积分器16r：机器人模型描绘部19：演示按键20：点动操作按键21：第一极限回避按键22：第一姿态最优化按键25：切换按键32：显示画面116：示教器121：第二极限回避按键122：第二姿态最优化按键

Claims (9)

1.一种机器人系统，包括：

机器人，所述机器人具有通过伺服马达驱动并包含冗余轴的多个关节轴；以及

机器人控制装置，所述机器人控制装置控制所述机器人，

所述机器人控制装置具有：

第一指令生成部，所述第一指令生成部基于所输入的所述机器人的控制点的速度指令，生成所述机器人的第一关节角速度指令；

极限回避指令生成部，所述极限回避指令生成部在信号A被输入的期间，基于评价函数A，生成用于回避所述机器人达到动作极限的状态的该机器人的关节角速度指令A；

姿态最优化指令生成部，所述姿态最优化指令生成部在信号B被输入的期间，基于评价函数B，生成用于使所述机器人的姿态最优化的该机器人的关节角速度指令B；

零空间矩阵计算部，所述零空间矩阵计算部计算与所述控制点有关的雅可比矩阵的零空间矩阵；

第二指令生成部，所述第二指令生成部基于所述关节角速度指令A、所述关节角速度指令B以及所述零空间矩阵，生成所述机器人的第二关节角速度指令；以及

控制部，所述控制部基于第三关节角速度指令控制所述伺服马达，所述第三关节角速度指令是所述第一和第二关节角速度指令之和。

2.如权利要求1所述的机器人系统，其中，

基于被设定作为所述机器人的各关节的可动界限角度的设定角度与该各关节的角度的当前值之差的大小，计算所述评价函数A，

所述极限回避指令生成部基于所述评价函数A的梯度，生成所述关节角速度指令A。

3.如权利要求1所述的机器人系统，其中，

基于被设定作为所述机器人能够接近障碍物的距离的设定距离与从该障碍物到该机器人的距离之差的大小，计算所述评价函数A，

所述极限回避指令生成部基于所述评价函数A的梯度，生成所述关节角速度指令A。

4.如权利要求1所述的机器人系统，其中，

基于所述机器人的可操作度，计算所述评价函数A，

所述极限回避指令生成部基于所述评价函数A的梯度，生成所述关节角速度指令A。

5.如权利要求1至4中任一项所述的机器人系统，其中，

基于所述机器人的前示教点上的各关节的角度与该各关节的角度的当前值之差的大小，计算所述评价函数B，

所述姿态最优化指令生成部基于所述评价函数B的梯度，生成所述关节角速度指令B。

6.如权利要求1至4中任一项所述的机器人系统，其中，

基于所述机器人的可操作度，计算所述评价函数B，

所述姿态最优化指令生成部基于所述评价函数B的梯度，生成所述关节角速度指令B。

7.如权利要求1至6中任一项所述的机器人系统，其中，

还包括示教器，所述示教器用于示教所述机器人的位置姿态，

所述示教器具有用于输出所述信号A的第一信号输出部以及用于输出所述信号B的第二信号输出部。

8.如权利要求7所述的机器人系统，其中，

所述示教器还具有显示画面以及描绘部，

所述描绘部在所述显示画面上分别描绘表示当前的所述机器人的姿态的图形图像、以及使该姿态最优化的所述机器人的图形图像。

9.一种机器人控制装置，具有：

第一指令生成部，所述第一指令生成部基于具有包含冗余轴的多个关节轴的机器人的控制点的速度指令，生成所述机器人的第一关节角速度指令；

极限回避指令生成部，所述极限回避指令生成部在信号A被输入的期间，基于评价函数A，生成用于回避所述机器人达到动作极限的状态的该机器人的关节角速度指令A；

姿态最优化指令生成部，所述姿态最优化指令生成部在信号B被输入的期间，基于评价函数B，生成用于使所述机器人的姿态最优化的该机器人的关节角速度指令B；

零空间矩阵计算部，所述零空间矩阵计算部计算与所述控制点有关的雅可比矩阵的零空间矩阵；

第二指令生成部，所述第二指令生成部基于所述关节角速度指令A、所述关节角速度指令B以及所述零空间矩阵，生成所述机器人的第二关节角速度指令；以及

控制部，所述控制部基于第三关节角速度指令控制伺服马达，所述第三关节角速度指令是所述第一以及第二关节角速度指令之和。

Images