CN104736303B - 多关节机器人的摆动控制装置 - Google Patents

Abstract

本发明提供一种抑制起因于驱动关节轴的电动机自身的动态特性、以及起因于其他关节轴的动作所带来的影响的摆动动作的误差的产生，能够实现高精度的摆动动作的多关节机器人的摆动控制装置。摆动控制装置(10)包含：信号运算部，其对各轴的目标位置信号进行运算；滤波器运算部(400)，其运算针对目标位置信号进行了低通滤波器处理后的目标指令信号；和电动机控制部(2500)，其以目标指令信号为输入对各轴进行驱动。电动机控制部(2500)的增益的频率特性构成为大致平坦，从目标位置到电动机输出角度的动态特性被近似于滤波器。摆动信号校正部(200)运算基于滤波器特性进行增益校正后的目标位置信号。

Description

多关节机器人的摆动控制装置

技术领域

本发明涉及例如用于电弧焊接的多关节机器人的控制，尤其是涉及能够以高轨迹精度进行摆动动作的多关节机器人的控制。

背景技术

在利用电弧焊接进行多个母材的焊接时，采用使焊接电极沿焊接方向前进的同时，一边在焊接线的左右方向上进行正弦波摆动动作一边进行焊接的摆动焊接。以往，该摆动焊接都是通过使焊炬本身左右摇动、或以焊炬本身为中心左右倾动来进行。在使多关节机器人进行这样的摆动焊接的情况下，要求高轨迹精度。

关于摆动动作，以下这样的技术是公知的。

日本特开2006-59037号公报(专利文献1)公开一种能够使得以预先示教的振幅可靠并且正确地进行摆动动作的焊接机器人控制装置。

当使焊接机器人进行摆动动作时，基于被指令的摆动频率、振幅，生成各关节的电动机的角度指令值，基于该指令值来控制摆动动作。但是，依赖于电动机的动态特性(反馈系统的控制时间常数)，电动机无法按照角度指令值那样进行动作，作为结果，不能以被指令的振幅进行摆动。因此，专利文献1所公开的技术如下所述解决了该问题。首先，将不进行摆动动作的情况与进行摆动动作的情况的各关节的角度指令值之差作为摆动信号来计算出来。然后，基于电动机的反馈系统的控制时间常数和摆动频率，计算出电动机的反馈系统的增益(输出振幅/指令振幅)，将该增益的倒数乘以摆动信号而得到的值与不进行摆动动作的情况下的各关节的角度指令值相加，由此计算出最终的各关节的角度指令值。

此外，日本特开平6-222817号公报(专利文献2)公开一种技术，能够应用于进行摆动焊接的机器人使其高精度地描绘摆动波形。

专利文献2所公开的技术为了解决依赖于电动机的动态特性而摆动振幅变动的问题点，求取基于机器人的姿势来决定的电动机驱动系统的模型(传递函数G(s))，将摆动动作时的各轴的角度指令值与1/G(s)进行乘法运算而得到的角度指令值输入到电动机。

在先技术文献

专利文献

专利文献1：日本国特开2006-59037号公报

专利文献2：日本国特开平6-222817号公报

发明内容

发明要解决的课题

在对使多关节机器人的各轴进行动作的电动机输入摆动动作那样的周期信号作为指令值的情况下，输出角度具有相对于指令值而言振幅/相位产生变化的特性。因此，为了使机器人前端位置正确地活动，需要对该变化特性进行预测来校正角度指令值。此外，涉及该振幅/相位的变化的主要原因有电动机自身的动态特性(反馈系统的控制时间常数)、和来自外部的外力(机器人的情况下其他轴活动所带来的影响)。若进行整理则问题点汇总如下。

(1)由于电动机的反馈系统的控制时间常数，相对于指令值而言电动机角度的振幅产生变化。

(2)由于各电动机的反馈系统的控制时间常数不同，因此相对于指令值而言各电动机角度的相位不同。

(3)由于其他轴的动作影响，相对于指令值而言电动机角度的振幅产生变化。

(4)由于其他轴的动作影响，相对于指令值而言各电动机角度的相位不同。

但是，上述的专利文献1是仅解决了这些问题点中的(1)的技术，关于(2)～(4)并未作任何考虑。

而且，作为用于解决上述这样的问题点的方法，可以列举出对角度指令值进行校正(专利文献1也采用该方法)。但是，为了进行校正，需要将输出角度相对于角度指令值的变化特性模型化，准确地预测产生什么程度的变化。若校正的量较大、或者模型变得复杂，则当然误差也会增加。为了降低该误差，说来还需要进行用于尽可能减小上述的变化特性所引起的振幅/相位的变化量的控制。

此外，专利文献2是解决了上述4个问题点中的(1)以及(2)的技术，关于(3)以及(4)并未作任何考虑。

而且，与专利文献1所公开的技术相同，专利文献2所公开的技术也通过将电动机的特性模型化，预测振幅/相位的变化量，校正角度指令值来解决问题。因此，若从根本上校正量不变小而校正的量较大、或者模型变得复杂则误差会较大。

也就是说，在这些专利文献1、2所公开的技术中，上述4个问题点未必全部都得到解决，并未能实现高轨迹精度下的摆动动作。

本发明鉴于上述的问题点而作，其目的在于提供一种多关节机器人的摆动控制装置，在具备多个轴的多关节机器人中，能够抑制起因于使多关节机器人的轴进行动作的电动机自身的动态特性(上述的问题点(1)以及(2))的摆动动作的误差、以及起因于其他轴动作所带来的影响(上述的问题点(3)、(4))的摆动动作的误差的产生，并能够实现以高轨迹精度进行摆动动作。

用于解决课题的手段

为了解决上述课题，本发明所涉及的多关节机器人的摆动控制装置采取以下的技术手段。

本发明所涉及的多关节机器人的摆动控制装置使多个关节轴进行驱动以使得安装于多关节机器人的工具描绘所希望的摆动轨迹。该摆动控制装置的特征在于，构成为包含：信号运算部，其基于所述摆动轨迹，对多关节机器人的各轴的目标位置信号进行运算；滤波器运算部，其针对所述运算出的目标位置信号，进行用于去除所述多关节机器人的固有振动分量的低通滤波器处理，对于所述多关节机器人的各轴运算进行滤波器处理后的目标指令信号；和电动机控制部，其以所述滤波器处理后的目标指令信号为输入，对多关节机器人的各轴进行驱动。所述电动机控制部中的增益的频率特性构成为大致平坦，所述信号运算部运算增益校正后的目标位置信号。

优选为，所述电动机控制部能够构成为包含：位置反馈部、速度反馈部和速度前馈部，所述信号运算部能够构成为，运算基于所述滤波器运算部中的滤波器特性以及所述摆动轨迹进行增益校正后的目标位置信号。

还优选为，能够构成为，所述低通滤波器处理中使用的滤波器在所有轴为同样的，所述信号运算部能够构成为，运算通过将根据所述滤波器运算部的滤波器特性而得到的摆动频率下的滤波器增益的倒数与目标位置信号进行乘法运算来进行增益校正后的目标位置信号。

还优选为，所述信号运算部能够构成为，运算基于所述滤波器运算部的滤波器特性以及根据所述滤波器运算部的滤波器特性而得到的摆动频率，在增益校正的基础上，进行相位补偿后的目标位置信号。

还优选为，所述电动机控制部能够构成为，还包含：预测通过本轴进行加减速而作用于本轴的惯性力、重力以及摩擦力，并加到从所述速度反馈部输出的转矩指令值中的前馈控制部。

还优选为，所述电动机控制部能够构成为，还包含：预测通过其他轴进行加减速而作用于本轴的惯性力以及离心科里奥利力，并加到从所述速度反馈部输出的转矩指令值中的前馈控制部。

发明效果

通过使用本发明所涉及的摆动控制装置，从而在具备多个轴的多关节机器人中，能够抑制起因于使多关节机器人的轴进行动作的电动机自身的动态特性的摆动动作的误差、以及起因于其他轴动作所带来的影响的摆动动作的误差产生，能够实现以高轨迹精度的摆动动作。

附图说明

图1是表示应用本发明的实施方式所涉及的摆动控制装置的多关节机器人的整体构成的示意图。

图2是本发明的第1实施方式所涉及的摆动控制装置的控制框图。

图3是图2的滤波器运算部以及电动机控制部的详细控制框图。

图4是图3所示的控制框图的简略版。

图5是表示按照图4所示的控制框图来控制的多关节机器人的输出波形的图。

图6是图2所示的控制框图的简略版。

图7是表示按照图6所示的控制框图来控制的多关节机器人的输出波形的图(其1)。

图8是表示按照图6所示的控制框图来控制的多关节机器人的输出波形的图(其2)。

图9是表示按照图6所示的控制框图来控制的多关节机器人的输出波形的图(其3)。

图10是本发明的第2实施方式所涉及的摆动控制装置的控制框图。

图11是二连杆机器人的示意图。

图12是表示按照图10所示的控制框图(其中仅速度FF)来控制的多关节机器人的输出波形的图。

图13是表示按照图10所示的控制框图(速度FF+3种FF)来控制的多关节机器人的输出波形的图。

图14是图10所示的控制框图的简略版。

图15是表示按照图14所示的控制框图来控制的多关节机器人的输出波形的图。

图16是本发明的第3实施方式所涉及的摆动控制装置的控制框图。

图17是表示按照图16所示的控制框图(速度FF+3种FF)来控制的多关节机器人的输出波形的图。

图18是表示按照图16所示的控制框图(速度FF+5种FF)来控制的多关节机器人的输出波形的图。

图19是图16所示的控制框图的简略版。

图20是表示按照图19所示的控制框图来控制的多关节机器人的输出波形的图。

图21是表示本发明的实施方式的变形例中的基本位置信号与摆动信号的生成方法的图。

图22是本发明的实施方式的变形例所涉及的摆动控制装置的控制框图。

具体实施方式

以下，基于附图来详细说明本发明的实施方式所涉及的多关节机器人的摆动控制装置。另外，在以下的说明中，对于同一部件标注相同的符号。它们的名称以及功能也相同。因此，不重复关于它们的的详细说明。

<第1实施方式>

[整体构成]

首先，对应用第1实施方式所涉及的摆动控制装置的垂直多关节机器人(以下有时简单记载为多关节机器人)的概要进行说明。

图1是使焊炬进行倾动动作(摆动动作)的机器人的一例，是表示应用本实施方式所涉及的摆动控制装置的多关节机器人1的概要的图。该多关节机器人1是垂直多关节型，具有J1～J6的6关节。在J6轴的前端设置焊炬，通过从焊炬送出的焊丝来进行电弧焊接。该多关节机器人1被设置为，以预先规定的焊接开始点与焊接结束点之间为焊接作业区间，进行沿连结焊接开始点与焊接结束点的焊接线方向移动的同时，使焊丝以预先规定的振幅以及频率进行倾动的动作(摆动动作)。

这样的多关节机器人1除了包含图示的多关节机器人1的主体以外，还包含控制装置和上位计算机(上位CPU)，该控制装置具备示教器并且对各轴进行伺服控制。通过这些控制装置以及上位计算机，实现了第1方式所涉及的摆动控制装置10。

控制装置(伺服控制部)进行控制以使设置于多关节机器人1的焊炬按照预先示教的程序，跟随上述的焊接线移动并进行摆动动作。示教程序有使用与控制装置连接的示教器来作成的情况、使用利用了上位计算机的脱机示教系统来作成的情况。无论是哪一种情况，示教程序都是在实际的动作之前预先制作出的。

[控制模块]

图2表示对图1的多关节机器人1进行控制的摆动控制装置10的控制框图。该摆动控制装置10使多个关节轴进行驱动以使安装于多关节机器人1的工具(在此为焊炬)进行所希望的动作(在此为摆动动作)。图2对应于多个关节轴(图1的例示中为6轴)中的某1个关节轴。

该摆动控制装置10包含信号运算部(摆动信号生成部100、摆动信号校正部200、基本位置信号生成部300)，该信号运算部基于摆动轨迹，对多关节机器人1的各轴的目标位置信号进行运算。此外，摆动控制装置10包含滤波器运算部400，该滤波器运算部针对运算出的目标位置信号，进行用于去除多关节机器人1的固有振动分量的低通滤波器处理，对于多关节机器人1的各轴运算进行滤波器处理后的目标指令信号。此外，摆动控制装置10包含电动机控制部500，该电动机控制部以滤波器处理后的目标指令信号为输入，对多关节机器人1的各轴进行驱动。在此，电动机控制部500包含位置反馈部510、速度反馈部和速度前馈部520。电动机控制部500中的增益的频率特性构成为大致平坦，信号运算部的摆动信号校正部200的特征在于，运算增益校正后的目标位置信号。在此，“大致平坦”意味着，在电动机控制部500中的增益的频率特性不会给良好的摆动动作造成影响的程度下构成为平坦。换言之，在用波特图示出了电动机控制部500的特性的情况下，波特图上的增益曲线成为平坦、或成为不会给良好的摆动动作造成影响的程度的平坦。

另外，以下，控制模块中的要素的特性(即使没有动态特性这样的记载)全是动态特性。此外，FB的记载意味着反馈，FF的记载意味着前馈。以下，关于各模块更加详细地进行说明。

基本位置信号生成部300生成基本位置信号作为用于描绘不进行摆动动作的情况下的多关节机器人1的前端轨迹的多关节机器人1的各轴的角度位置。

摆动信号生成部100生成摆动信号作为从用于描绘进行摆动动作的情况下的多关节机器人1的前端轨迹的多关节机器人1的各轴的角度位置中减去由基本位置信号生成部300生成的角度位置而得到的角度位置。该角度位置基本上成为所指定的摆动频率的周期信号。

摆动信号校正部200对由摆动信号生成部100生成的摆动信号进行校正，生成摆动校正信号。在第1实施方式中特征在于，摆动信号校正部200中的校正基于在滤波器运算部400中使用的滤波器来进行。关于该校正方法的详细在后面叙述。

滤波器运算部400针对将基本位置信号和摆动校正信号相加而得到的目标位置信号，进行用于去除多关节机器人1的固有振动分量的低通滤波器处理，生成滤波器后目标位置信号。在此生成的滤波器后目标位置信号根据电动机控制部500中的控制方法，除了包含角度位置以外，还包含角速度、角加速度。关于滤波器处理方法的详细在后面叙述。

电动机控制部500基于滤波器后目标位置信号，对电动机550进行控制。作为最一般的电动机控制方法，如作为图2的详细图的图3所示，进行基于编码器560的检测角度的位置(反馈)控制部510、速度(反馈)控制部530、电流控制部540所进行的控制。在第1实施方式中特征在于，除了这些以外，还包含通过将滤波器后目标速度信号与速度前馈增益进行乘法运算来进行速度前馈控制的速度前馈部520。关于电动机控制方法的详细在后面叙述。

第1实施方式所涉及的摆动控制装置10通过在电动机控制部500的控制方法中增加速度前馈控制，从而降低电动机550的动态特性所引起的延迟。此外，第1实施方式所涉及的摆动控制装置10的特征在于，能够用滤波器运算部400中使用的滤波器来近似从目标位置信号到由编码器检测到的当前位置信号的动态特性。

然后，特征在于，利用能够在任意设计的滤波器中近似从目标位置信号到当前位置信号的动态特性，考虑该滤波器特性对摆动信号进行校正，生成摆动校正信号。

由此，仅通过基于所设定的滤波器对由于在多关节机器人1的各轴的电动机550的动态特性所引起的延迟而杂乱的摆动波形进行校正，能够得到良好的摆动波形。

[滤波器处理方法以及电动机控制方法]

在此，对图3或图4所示的、第1实施方式所涉及的摆动控制装置10的滤波器运算部400以及电动机控制部500进行说明。

在此，T表示滤波器时间常数，Kxp表示位置控制部510的比例增益，Kvp和Kvi表示速度控制部530的比例增益和积分增益，Kvf表示速度前馈部520的前馈增益，s表示拉普拉斯算子。

另外，T、Kxp、Kvp、Kvi根据多关节机器人1的动态特性来设定，Kvf被设定为0～1的值。此外，在此，滤波器运算部400使用一阶低通滤波器，计算角度位置、角速度作为滤波器后目标位置信号。然后，基于角度位置、角速度，进行图3所示的电动机控制。另外，滤波器运算部400的低通滤波器也可以根据需要而使用二阶或三阶滤波器。

通过图2所示的控制方法来降低电动机550的动态特性所引起的延迟，如图3、4所示，能够通过滤波器运算部400中使用的一阶低通滤波器(F(s)＝1/(T·s+1))来近似从目标位置信号θ到当前位置信号θr的特性。参照图4来对这一情况进行说明。

图5表示使目标位置信号θ成为某正弦波信号的情况下的各输出波形。图5中，实线表示目标位置信号，虚线(broken line)表示滤波器后目标位置信号。此外，单点划线表示使用了速度前馈控制时的电动机550的当前位置信号，点线(dotted line)表示未使用速度前馈控制时的电动机550的当前位置信号。观察该图可知，速度前馈控制使用时的当前位置信号接近于滤波器后目标位置信号，能够使从目标位置信号θ到当前位置信号θr的特性近似于滤波器特性。此外，另一方面，对于未使用速度前馈控制时的当前位置信号而言，可以看出，其振幅大幅减少，在进行校正时则需要较大的校正。

[摆动信号校正方法]

通过使用上述的滤波器处理方法以及电动机控制方法，能够利用任意设计的滤波器的传递函数来近似从目标位置信号θ到当前位置信号θr的动态特性。摆动信号校正部200利用这一点来校正摆动信号。以下，说明2种校正方法(增益校正方法、增益相位校正方法)。

·增益校正方法

在该增益校正方法中，将由摆动信号生成部100生成的摆动信号(摆动频率为ω[Hz])与滤波器运算部400中使用的滤波器F(s)的频率ω时的增益(滤波器增益)的倒数进行乘法运算，来生成摆动校正信号。此时的摆动校正信号的计算公式可用下式(1)表示。

【式1】

${\stackrel{\&OverBar;}{\mathrm{\&theta;}}}_{\mathrm{\&omega;}}=|1\backslash F\left(\mathrm{j\&omega;}\right)|\&CenterDot;{\mathrm{\&theta;}}_{\mathrm{\&omega;}}$ (在此，j是虑数单位) (1)

由此，能够校正对电动机550输入周期信号时的振幅变化。另外，这里的校正不依赖于电动机550的动态特性，而仅基于任意设计的滤波器。

但是，在此由于并未对相位变化进行校正，因此若在各轴的滤波器运算部400中设定不同的滤波器，则各轴的相位变化量不同，不能得到良好的摆动波形。因此，使用该增益校正方法时，优选的是，在多关节机器人1的所有轴将滤波器运算部400中使用的滤波器设为相同的滤波器，使所有轴的相位变化量同等。

图7表示利用这样的增益校正方法(校正方法1)使用了控制模块(图6)的情况下的输出波形。

图7示出了基于校正方法1的输出波形、以及为了比较而实施了基于现有技术1(基于专利文献1的技术，以下记载为现有技术1)的校正时的输出波形。实线表示摆动信号，单点划线表示基于校正方法1的电动机550的当前位置信号，点线表示基于现有技术1的电动机550的当前位置信号。另外，在此由于设为仅单轴被驱动，因此没有其他轴动作所带来的影响。

观察该图可知，无论哪一个电动机550的当前位置信号的振幅都接近于摆动信号的振幅，能够适当地校正。但是，基于现有技术1的校正是假定为完全知晓电动机模型而进行的。

图8表示在这样的现有技术1中，电动机模型偏差时的结果。观察该图8可知，基于现有技术1的电动机550的当前位置信号的振幅偏差较大。也就是说，可知，若在基于现有技术1的校正中电动机模型产生偏差，则不能实现令人满意的控制。

·增益相位校正方法

在该增益相位校正方法中，对由摆动信号生成部100生成的摆动信号(摆动频率为ω[Hz])，进行基于滤波器运算部400中使用的滤波器F(s)以及摆动频率来推测了摆动信号的振幅变化以及相位延迟的补偿。例如，相位延迟可用下式(2)推测，振幅变化A可用下式(3)推测。基于这些值，进行相位超前补偿以及增益校正。

【式2】

φ＝∠F(jω) (2)

【式3】

A＝|F(jω)| (3)

由此，能够校正对电动机550输入周期信号时的振幅变化以及相位变化。

图9表示摆动信号(实线)、和利用这样的增益相位校正方法(校正方法2)使用了控制模块(图6)的情况下的输出波形(单点划线)。校正方法2与校正方法1不同，相位的延迟也进行补偿。因此，观察图9可知，与校正方法1相比，摆动信号与基于校正方法2的电动机550的当前位置信号的相位的偏差小。

如上所述，根据第1实施方式所涉及的摆动控制装置，通过在电动机控制部中实施速度前馈控制，能够使从目标位置到电动机输出角度的特性近似于低通滤波器处理中使用的滤波器。因为基于该滤波器来进行目标位置的振幅的校正，所以在多关节机器人中，起因于使多关节机器人的轴进行动作的电动机自身的动态特性的摆动动作的误差的产生得到抑制。由此，能够实现能以高轨迹精度进行摆动等动作。另外，各轴的相位的偏差通过所有轴使用相同滤波器而解决。结果，在现有技术中，由于进行连控制装置也包含在内的电动机的模型化并基于模型来进行目标位置的校正，因此在校正所涉及的计算量较多、模型的误差较大的情况下，存在不能良好地校正这样的问题。根据第1实施方式所涉及的摆动控制装置，能够解决这样的问题点。

<第2实施方式>

以下，对本发明的第2实施方式所涉及的摆动控制装置20进行说明。另外，本实施方式所涉及的摆动控制装置与上述的第1实施方式所涉及的摆动控制装置10在电动机控制部的控制特性的给予方法方面不同。除此以外的构成与第1实施方式相同，所以关于与上述说明重复的部分，在此不再重复。

在上述的第1实施方式中，通过在电动机控制部500中设置速度前馈部520，能够使从目标位置信号到当前位置信号的动态特性近似于任意设计的滤波器。但是，有时该近似精度并不充分。

这可以认为是基于如下理由。也就是说，多关节机器人1的各关节受到根据多关节机器人1的姿势而变化的惯性力或者/以及重力的影响、依赖于速度等的摩擦力的影响。因此，仅通过速度前馈控制，不能使从目标位置信号到当前位置信号的动态特性充分地近似于任意设计的滤波器。

第2实施方式所涉及的摆动控制装置20增加用于能够比任意设计的滤波器更严密地近似从目标位置信号到当前位置信号的动态特性的控制。若这样，则仅通过基于任意设计的滤波器来进行摆动信号的校正，就能够进一步提高摆动精度。

图10是第2实施方式所涉及的摆动控制装置20的控制框图。图10的控制框图也与图2同样地，对应于多关节机器人1的多个关节轴中的某1个关节轴。

摆动控制装置20在取代电动机控制部500而具备电动机控制部1500这一点、还具备向电动机控制部1500给予其他轴的信息的其他轴目标位置信号输出部600这一点上，与图2所示的摆动控制装置10不同。

电动机控制部1500在摆动控制装置10的电动机控制部500的构成的基础上，还具备惯性项前馈部1510、重力项前馈部1520、摩擦项前馈部1530。从其他轴目标位置信号输出部600输出的、滤波器后的目标位置信号(其他轴)被输入到惯性项前馈部1510、重力项前馈部1520以及摩擦项前馈部1530。在这3种前馈部中，因为基于滤波器后的目标位置信号(他轴)而知道多关节机器人1的姿势，所以知道本轴的状态，因此知道本轴的惯性等。以下，关于各模块详细进行说明。

其他轴目标位置信号输出部600输出在其他轴同样地生成的来自滤波器运算部400的输出即滤波器后目标位置信号(角度、角速度、角加速度)。例如，在多关节机器人1为6轴的情况下，其他轴目标位置信号输出部600将作为对象的轴以外的5个轴的滤波器后目标位置信号向电动机控制部1500输出。

惯性项前馈部1510预测由多关节机器人1的姿势决定的作用于本轴的惯性力，并作为电流控制部540的前馈控制的转矩指令值而输出。

重力项前馈部1520预测由多关节机器人1的姿势决定的作用于本轴的重力影响，并作为电流控制部540的前馈控制的转矩指令值而输出。

摩擦项前馈部1530预测作用于本轴的摩擦力，并作为电流控制部540的前馈控制的转矩指令值而输出。

第2实施方式所涉及的摆动控制装置20的特征在于，在摆动控制装置10的电动机控制部500中设置3种前馈控制部。由此，能够更加严密地将从目标位置信号到由编码器560检测到的当前位置信号的动态特性近似于滤波器运算部400中使用的滤波器。

根据该构成，仅通过在第1实施方式中说明的、基于滤波器来进行摆动信号的校正，就能够得到良好的摆动波形。

另外，在此实施的3种前馈控制基于多关节机器人1的模型(运动方程式)而设计。在此，以图11所示的二连杆机器人为对象，对各种前馈控制(惯性项、重力项、摩擦项)的计算方法进行说明。

若求取图11所示的二连杆机器人中的运动方程式，则作用于各关节的转矩τ1、τ2分别可用以下的式(4)以及式(5)表示。然后，如下面的式(6)～式(13)所示那样进行计算，与第1轴相关的3种前馈控制(惯性项、重力项、摩擦项)的输出如式(14)所示计算出。

【式4】

${\mathrm{\&tau;}}_1=J_1{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_1+J_{12}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_2+C_1+G_1+F_1---\left(4\right)$

【式5】

${\mathrm{\&tau;}}_2=J_2{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_2+J_{12}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_1+C_2+G_2+F_2---\left(5\right)$

【式6】

第1轴惯性项：J₁＝m₁l_g1 ²+I₁+m₂(1₁ ²+l_g2 ²+21₁l_g2cos(θ₂))+I₂ (6)

【式7】

第2轴惯性项：J₂＝m₂l_g2 ²+I₂ (7)

【式8】

干扰惯性项：J₁₂＝m₂(lg₂ ²+1₁l_g2cos(θ₂))+I₂ (8)

【式9】

第1轴离心科里奥利项： $C_1=-m_2{1}_1{l}_{g2}\sin \left({\mathrm{\&theta;}}_2\right)(2{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_1{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_2+{{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_2}^2)---\left(9\right)$

【式10】

第2轴离心科里奥利项： $C_2=-m_2{1}_1{l}_{g2}\sin \left({\mathrm{\&theta;}}_2\right){{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_1}^2---\left(10\right)$

【式11】

第1轴重力项：G₁＝m₁gl_g1cos(θ₁)+m₂g(l₁cos(θ₁)+l_g2cos(θ₁+θ₂)) (11)

【式12】

第2轴重力项：G₂＝m₂gl_g2cos(θ₁+θ₂) (12)

【式13】

第1轴摩擦项： $F_1=a_1\&CenterDot;\mathrm{sign}\left({\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_1\right)+b_1{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_1$

第2轴摩擦项： $F_2=a_2\&CenterDot;\mathrm{sign}\left({\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_2\right)+b_2{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_2$

※摩擦项假定为库伦摩擦和粘性摩擦a₁、a₁：库伦摩擦系数，b₁、b₁：粘性摩擦系数

$\mathrm{sign}\left(x\right)=\left\{\begin{array}{cc}1& (x>0)\\ 0& (x=0)\\ -1& (x<0)\end{array}\right.---\left(13\right)$

【式14】

与第1轴相关的各种前馈控制的输出

惯性项前馈部：

重力项前馈部：G₁

摩擦项前馈部：F₁ (14)

若实施上述3种前馈控制，则能够使从目标位置信号到由编码器560检测到的当前位置信号的动态特性与滤波器运算部400中使用的滤波器一致。另外，第2实施方式关于其他轴的动作的影响并未作考虑，所以限于单轴动作的情况。关于考虑了其他轴动作所带来的影响的情况，在后述的第3实施方式中进行研究。

此外，即使仅部分地实施上述3种前馈控制，也能够与第1实施方式相比，使从目标位置信号到由编码器560检测到的当前位置信号的动态特性更加接近于滤波器运算部400中使用的滤波器特性。

图12以及图13示出了对上述的二连杆机器人的第2轴输入了某正弦波信号作为目标位置信号的情况下的输出波形。第1轴的目标位置信号是零。实线表示目标位置信号，虚线表示滤波器后目标位置信号，单点划线表示电动机550的当前位置信号。

图12对应于第1实施方式，表示仅使用了速度前馈控制的情况下的输出波形。图13对应于第2实施方式，示出了使用了速度前馈控制、惯性项前馈控制、重力项前馈控制以及摩擦项前馈控制的情况下的输出波形。

观察它们可知，通过在速度前馈控制的基础上实施3种前馈控制，与仅实施速度前馈控制的情况相比，当前位置信号进一步接近于滤波器后目标位置信号。这样，能够提高将从目标位置信号到当前位置信号的动态特性近似于滤波器特性时的精度。

由此，如在第1实施方式中已说明的那样，仅通过基于滤波器来进行摆动信号的校正，就能够得到良好的摆动波形。

图15表示利用第1实施方式中说明的增益校正方法(校正方法1)、使用了控制模块(图14)的情况下的输出波形。图15中，示出将校正方法1应用于第2实施方式的情况下的输出波形，此外，为了比较而示出了将校正方法1应用于第1实施方式的情况下的输出波形。

图15示出了对第2轴输入了某正弦波信号作为摆动信号的情况下进行了校正方法1(增益校正)的情况下的第2轴的输出波形。实线表示摆动信号，单点划线表示在第2实施方式(实施例2)中进行了校正方法1的情况下的电动机550的当前位置信号，点线表示在第1实施方式(实施例1)中进行了校正方法1的情况下的电动机550的当前位置信号。另外，在此，由于设为仅单轴被驱动，因此没有其他轴动作所带来的影响。观察该图可知，在第2实施方式方面，电动机550的当前位置信号的振幅接近于摆动信号的振幅，得到了适当地校正。

如上所述，根据第2实施方式所涉及的摆动控制装置，在第1实施方式所涉及的摆动控制装置中的控制的基础上，预先预测作用于各轴的惯性力、重力以及摩擦力，作为适当的转矩指令值而向电动机输入。由此，能够提高将从目标位置到电动机输出角度的动态特性向滤波器进行近似的精度。通过基于该滤波器来进行目标位置的振幅的校正，从而在多关节机器人中，起因于使多关节机器人的轴进行动作的电动机自身的动态特性的摆动动作的误差、以及起因于其他轴动作所带来的本轴的影响的摆动动作的误差的产生得到抑制。因此，能够实现以高轨迹精度进行摆动等动作。

<第3实施方式>

以下，对本发明的第3实施方式所涉及的摆动控制装置30进行说明。另外，本实施方式所涉及的摆动控制装置在电动机控制部的控制特性的给予方法方面，与上述的第1实施方式所涉及的摆动控制装置10不同。除此以外的构成与第1实施方式相同，所以关于与上述说明重复的部分，在此不再重复。

在上述的第2实施方式中，通过在第1实施方式中安装的速度前馈部520设置3种前馈控制部，从而能够使从目标位置信号到当前位置信号的动态特性进一步近似于任意设计的滤波器。但是，在第2实施方式中，关于其他轴动作所带来的影响并未作考虑。但是，有时不得不考虑其他轴的影响来进行近似。

第3实施方式所涉及的摆动控制装置30增加用于能够比任意设计的滤波器更进一步严密地近似从目标位置信号到当前位置信号的动态特性的控制。若这样，则仅通过基于任意设计的滤波器来进行摆动信号的校正，就能够进一步提高摆动精度。

图16是第3实施方式所涉及的摆动控制装置30的控制框图。图16的控制框图也与图2以及图10同样地，对应于多关节机器人1的多个关节轴中的某1个关节轴。

摆动控制装置30在取代电动机控制部1500而具备电动机控制部2500这一点上，与图10所示的摆动控制装置20不同。

电动机控制部2500在摆动控制装置20的电动机控制部1500的构成的基础上，还具备干扰惯性项前馈部2510以及离心科里奥利项前馈部2520。从其他轴目标位置信号输出部600输出的滤波器后的目标位置信号(其他轴)被输入到这些干扰惯性项前馈部2510以及离心科里奥利项前馈部2520。另外，从其他轴目标位置信号输出部600向惯性项前馈部1510、重力项前馈部1520以及摩擦项前馈部1530输入滤波器后的目标位置信号(其他轴)方面与摆动控制装置20相同。以下，对各模块详细进行说明。

干扰惯性项前馈部2510预测因其他轴动作而作用于本轴的连杆的惯性力，并作为电流控制部540的前馈控制的转矩指令值而输出。

离心科里奥利项前馈部2520预测因其他轴动作而作用于本轴的连杆的离心科里奥利力，并作为电流控制部540的前馈控制的转矩指令值而输出。

第3实施方式所涉及的摆动控制装置30的特征在于，在第2实施方式所涉及的摆动控制装置20的电动机控制部1500中，还设置用于补偿其他轴动作时的影响的2种前馈控制部。由此，能够更严密地使从目标位置信号到由编码器560检测到的当前位置信号的动态特性近似于滤波器运算部400中使用的滤波器。

根据该构成，仅通过在第1以及第2实施方式中说明的、基于滤波器来进行摆动信号的校正，就能够得到良好的摆动波形。

另外，在此实施的2种前馈控制基于多关节机器人1的模型(运动方程式)而设计。在此，以图11所示的二连杆机器人为对象，对各种前馈控制(干扰惯性项、离心科里奥利项)的计算方法进行说明。

若求取图11所示的二连杆机器人中的运动方程式，则作用于各关节的转矩τ1、τ2分别可用以下的式(15)以及式(16)表示。然后，如以下的式(17)～式(19)所示进行计算，与第1轴相关的2种前馈控制(干扰惯性项、离心科里奥利项)的输出如式(20)所示计算出。另外，在下式中，未记载与干扰惯性项以及离心科里奥利项无关的部分。

【式15】

${\mathrm{\&tau;}}_1=J_1{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_1+J_{12}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_2+C_1+G_1+F_1---\left(15\right)$

【式16】

${\mathrm{\&tau;}}_2=J_2{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_2+J_{12}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_1+C_2+G_2+F_2---\left(16\right)$

【式17】

干扰惯性项：J₁₂＝m₂₍l_g2 ²+l₁l_g2cos(θ₂))+I₂(17)

【式18】

第1轴离心科里奥利项： $C_1=-m_2{1}_1{l}_{g2}\sin \left({\mathrm{\&theta;}}_2\right)(2{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_1{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_2+{{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_2}^2)---\left(18\right)$

【式19】

第2轴离心科里奥利项： $C_2=-m_2{1}_1{l}_{g2}\sin \left({\mathrm{\&theta;}}_2\right){{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_1}^2---\left(19\right)$

【式20】

与第1轴相关的各种前馈控制的输出

干扰惯性项前馈部：

离心科里奥利项前馈部：C₁(20)

若在第2实施方式所涉及的3种前馈控制的基础上实施上述的2种前馈控制，则即使在其他轴动作的情况下，也能够使从目标位置信号到由编码器560检测到的当前位置信号的动态特性与滤波器运算部400中使用的滤波器一致。

此外，仅通过部分地实施上述的2种前馈控制，就能够与第1以及第2实施方式相比，使从目标位置信号到由编码器560检测到的当前位置信号的动态特性进一步接近于滤波器运算部400中使用的滤波器特性。

图17以及图18示出了对上述的二连杆机器人的各关节输入了某正弦波信号作为目标位置信号的情况下的第2轴的输出波形。实线表示目标位置信号，虚线表示滤波器后目标位置信号，单点划线表示电动机550的当前位置信号。

此外，图17对应于第2实施方式，表示使用了速度前馈控制、惯性项前馈控制、重力项前馈以及摩擦项前馈控制的情况下的输出波形。图18对应于第3实施方式，示出了使用了干扰惯性项前馈控制以及离心科里奥利项前馈控制的情况下的输出波形。

观察它们可知，通过追加干扰惯性项前馈控制以及离心科里奥利项前馈控制，从而即使在其他轴动作的情况下，当前位置信号也更加接近于滤波器后目标位置信号。由此可知，能够提高使从目标位置信号到当前位置信号的动态特性近似于滤波器特性时的精度。

由此，如在第1以及第2实施方式中已经说明的那样，仅通过基于滤波器来进行摆动信号的校正，就能够得到良好的摆动波形。

图20表示利用在第1实施方式中说明的增益校正方法(校正方法1)使用了控制模块(图19)的情况下的输出波形。图20中示出了将校正方法1应用于第3实施方式的情况下的第2轴的输出波形，此外，为了比较而示出了将校正方法1应用于第2实施方式的情况下的第2轴的输出波形。

图20示出了在对各关节输入了某正弦波信号作为摆动信号的情况下，进行了校正方法1(增益校正)的情况下的第2轴的输出波形。实线表示摆动信号，单点划线表示在第3实施方式(实施例3)中进行了校正方法1的情况下的电动机550的当前位置信号，点线表示在第2实施方式(实施例2)中进行了校正方法1的情况下的电动机550的当前位置信号。观察该图可知，第3实施方式与第2实施方式相比，电动机550的当前位置信号的振幅更接近于摆动信号的振幅，更适当地得到了校正。

如上所述，根据第3实施方式所涉及的摆动控制装置，在第2实施方式所涉及的摆动控制装置中的控制的基础上，预先预测通过i轴(本轴)以外的轴进行加减速而作用于i轴的惯性力以及离心科里奥利力，并作为适当的转矩指令值而输入到电动机。由此，能够抑制其他轴动作所带来的影响，提高将从目标位置到电动机输出角度的动态特性向滤波器进行近似的精度。通过基于该滤波器来进行目标位置的振幅的校正，从而在多关节机器人中，起因于使多关节机器人的轴进行动作的电动机自身的动态特性的摆动动作的误差、以及起因于其他轴动作所带来的影响的摆动动作的误差的产生得到抑制。因此，能够实现以高轨迹精度进行摆动等动作。

<变形例>

以下，对本发明的实施方式的变形例所涉及的摆动控制装置进行说明。另外，本变形例能够应用于上述的第1～第3实施方式所涉及的摆动控制装置，与输入到滤波器运算部400的目标位置信号的生成方法相关。本变形例在除此以外的构成方面与第1～第3实施方式相同，所以关于与上述的说明重复的部分，在此不再重复。

在上述的实施方式中，以多关节机器人1的各轴坐标系来进行了摆动信号的校正。因此，如图21所示，需要以各轴坐标系来将基本位置信号和摆动信号分开来计算出。但是，若这样，则为了按照进行摆动的情况与不进行摆动的情况分别计算出各轴的角度指令值而需要进行逆变换，计算负荷增大。

因此，在本变形例中，能够更简便地进行校正。参照图22对本变形例进行说明。

如图22所示，本变形例的控制装置在上述的实施方式(图2、图10、图16)的控制模块中，还具备逆变换运算部700。以下，对各模块详细进行说明。

摆动波形生成部1100将为了以被指令的振幅以及频率进行来摆动而施加到上述的基本位置波形的波形生成为摆动波形。该摆动波形以用于表示前端位置的三维位置和用于表示前端姿势的角度来表现。

摆动波形校正部1200对由摆动波形生成部1100生成的摆动波形进行校正，生成摆动校正波形。关于校正方法，与在第1实施方式中已经说明的同样。

基本波形生成部1300生成作为不进行摆动的情况下的多关节机器人1的前端轨迹的基本位置波形。该基本位置波形以用于表示前端位置的三维位置和用于表示前端姿势的角度来表现。

逆变换运算部700针对在基本位置波形上施加摆动校正波形而得到的目标波形进行逆变换，生成各关节的目标位置信号。该目标波形以用于表示前端位置的三维位置和用于表示前端姿势的角度来表现。

这样，根据本变形例，由于逆变换仅1次即可，因此不用增加计算负荷就能够应用本发明的实施方式所涉及的校正方法。

另外，应当理解的是，本次公开的实施方式的所有的点均为例示而并非限制性的内容。本发明的范围由权利要求书示出而并非上述的说明，并旨在包含与权利要求书同等的含义以及范围内的所有的变更。本申请基于2012年10月19日申请的日本专利申请(特愿2012-231871)，并将其内容援引于此作为参考。

符号说明：

1 多关节机器人

10、20、30 摆动控制装置

100 摆动信号生成部

200 摆动信号校正部

300 基本位置信号生成部

400 滤波器运算部

500、1500、2500 电动机控制部

510 位置控制部

520 速度前馈部

530 速度控制部

540 电流控制部

550 电动机

560 编码器

600 其他轴目标位置信号输出部

700 逆变换运算部

1100 摆动波形生成部

1200 摆动波形校正部

1300 基本波形生成部

1510 惯性项前馈部

1520 重力项前馈部

1530 摩擦项前馈部

2510 干扰惯性项前馈部

2520 离心科里奥利项前馈部

Claims (6)

1.一种控制装置，其是使多个关节轴进行驱动以使得安装于多关节机器人的工具描绘所希望的摆动轨迹的多关节机器人的摆动控制装置，其特征在于，构成为包含：

信号运算部，其基于所述摆动轨迹，对所述多关节机器人的各轴的目标位置信号进行运算；

滤波器运算部，其针对所述运算出的目标位置信号，进行用于去除所述多关节机器人的固有振动分量的低通滤波器处理，对于所述多关节机器人的各轴运算进行滤波器处理后的目标指令信号；和

电动机控制部，其以所述滤波器处理后的目标指令信号为输入，对所述多关节机器人的各轴进行驱动，

所述电动机控制部中的增益的频率特性构成为大致平坦，

所述信号运算部运算增益校正后的目标位置信号，

所述电动机控制部包含通过将滤波器后目标速度信号与速度前馈增益进行乘法运算来进行速度前馈控制的速度前馈部。

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

所述电动机控制部包含位置反馈部和速度反馈部，

所述信号运算部运算基于所述滤波器运算部中的滤波器特性以及所述摆动轨迹进行增益校正后的目标位置信号。

3.根据权利要求1所述的控制装置，其特征在于，

所述低通滤波器处理中使用的滤波器在所有轴是同样的，

所述信号运算部运算通过将根据所述滤波器运算部的滤波器特性而得到的摆动频率下的滤波器增益的倒数与目标位置信号进行乘法运算来进行增益校正后的目标位置信号。

4.根据权利要求1所述的控制装置，其特征在于，

所述信号运算部运算基于所述滤波器运算部的滤波器特性以及根据所述滤波器运算部的滤波器特性而得到的摆动频率，在增益校正的基础上，进行相位补偿后的目标位置信号。

5.根据权利要求2所述的控制装置，其特征在于，

所述电动机控制部还包含：预测通过本轴进行加减速而作用于本轴的惯性力、重力以及摩擦力，并加到从所述速度反馈部输出的转矩指令值中的前馈控制部。

6.根据权利要求5所述的控制装置，其特征在于，

所述电动机控制部还包含：预测通过其他轴进行加减速而作用于本轴的惯性力以及离心科里奥利力，并加到从所述速度反馈部输出的转矩指令值中的前馈控制部。