CN104626152B - 工业机器人主动柔顺控制方法及装置 - Google Patents

Abstract

本发明公开一种工业机器人主动柔顺控制方法，S1、轨迹规划模块规划运动轨迹；S2、运动学计算模块进行运动学解耦计算；S3、电机驱动模块对运动学解耦结果加以实现；S4、位置与速度检测模块检测实际位置与速度；S5、重力项标定模块进行重力标定；S6、动力学计算模块计算动力学项；S7、力信息采集模块采集工具末端与工件之间的力信息；S8、力信息补偿模块进行补偿；S9、力/位控制模式切换模块获取补偿值加载；S10、阻尼特性转换模块进行阻尼线性转换；S11、雅可比转换模块求得各关节角速度。本发明技术方案提高系统的相应速度；采用选择矩阵的方式对工业机器人各方向进行位置控制和力控制的切换，使得力/位控制方式的转换变得简单便捷。

Description

工业机器人主动柔顺控制方法及装置

技术领域

本发明涉及机器人控制领域，具体涉及一种装配、打磨加工作业的工业机器人主动柔顺控制方法及装置。

背景技术

早期机器人完成的任务相对比较简单，机器人很少与外部物理环境发生接触，例如完成搬运、喷涂等工作任务，上述任务在控制时强调的是位置控制，操作臂与环境没有接触。

随着机器人应用领域的扩大，机器人在很多情况下会与外部环境发生接触，并产生接触力，例如机器人运用于装配、切割、抛光、磨削、擦洗、去毛刺、研磨等场合，上述情况下通常需要利用附加的力进行加工，同时把附加的力控制在一定范围内，因此附加力的引入对机器人控制提出了新的要求。

针对以上情况，产生了机器人主动柔顺控制，并得到广泛研究，机器人主动柔顺控制即力控制。其一为设计合理的力控制结构，解决机器人位置控制与力控制间的关系，目前力控制研究的方法大致可分为四类：阻抗控制、力/位混合控制、自适应控制和智能控制。

CN101195221A号专利中公开了一种阻抗控制的柔顺控制方法，为工业机器人的柔顺控制提供了新的可行方法，但其控制方法属于阻抗控制，不能够对接触力大小进行控制。

另一个研究是力的测量与补偿，由六维力传感器测量得到的力除需要得到的工具与工件接触力外，还包含其他的力，例如在第3412236号专利中公开了通过由力测定构件检测的力减去重力及回转力矩所产生的外力来修正力的内容，但是，由力感测元件检测出的力信息不仅包含上述的力，还包括由机器人手臂运动引起的惯性力项。

发明内容

本发明要解决的技术问题在于提供能够使工业机器人进行高精度柔性作业的主动柔顺控制方法及装置。

本发明的技术方案包括一种工业机器人主动柔顺控制方法，应用于工业机器人控制，包括步骤：S1、轨迹规划模块根据需求规划所述工业机器人的运动轨迹；S2、运动学计算模块计算所述工业机器人的正反运动学解，对所述运动轨迹进行运动学解耦计算；S3、电机驱动模块对所述运动学解耦结果加以实现；S4、位置与速度检测模块实时检测所述工业机器人的实际位置与速度，并反馈至所述电机驱动模块用于所述电机驱动模块进行闭环控制；S5、重力项标定模块对所述工业机器人加载的工具进行重力标定，获取重力项；S6、动力学计算模块计算所述工业机器人动力学项；S7、力信息采集模块实时采集所述工业机器人工具末端与工件之间的力信息；S8、力信息补偿模块对步骤S5、S6中得到的重力项和动力学项进行补偿；S9、力/位控制模式切换模块获取所述工业机器人各关节角速度作为补偿值加载；S10、阻尼特性转换模块对所述工业机器人末端工具与工件间的作用力进行阻尼线性转换，得到笛卡尔空间下各方向的线速度与角速度；S11、雅可比转换模块将所述笛卡尔空间下的线速度与角速度转化为关节空间下各关节角速度。

本发明的另一技术方案包括一种工业机器人主动柔顺控制装置，包括机器人控制器、电机控制器、力感测传感器、若干套电机驱动器及伺服电机；所述机器人控制器与所述电机控制器采用CAN协议进行通信；所述电机控制器和电机驱动器组成电机驱动模块；所述力感测传感器用于位置与速度检测模块和力信息采集模块；所述机器人控制器包括轨迹规划模块、运动学计算模块重力项标定模块、动力学计算模块、力信息补偿模块、力/位控制模式、阻尼特性转换模块、雅可比转换模块。

本发明的有益效果包括：将动力学项加入到六维力的力信息补偿项中，使得工业机器人检测出的工具末端与加工工件间力信息更接近真实值，提高控制精度；将力反馈信息通过一系列变换后作为速度补偿值加入到电机控制的速度环中，相比较将力反馈信息变换为位置补偿值加入到位置环的方式，提高了系统的相应速度；采用阻尼特性来调节机器人的刚性，省去了复杂的参数调节过程，使得力控制变得更直观、更便于调节；采用选择矩阵的方式对工业机器人各方向进行位置控制和力控制的切换，使得力/位控制方式的转换变得简单便捷。

附图说明

图1为本发明的工业机器人主动柔顺控制装置结构示意图；

图2为本发明的工业机器人主动柔顺控制方法模块图；

图3为本发明的工业机器人主动柔顺控制方法方案图；

图4为本发明的工业机器人主动柔顺控制方法流程图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细说明。

本发明的一种工业机器人主动柔顺控制装置，如图1和2所示，包括机器人控制器、电机控制器、力感测传感器、码盘、若干套电机驱动器及伺服电机。本实施中，电机驱动器及伺服电机为6套。

其中，如图2所述，机器人控制器11与电机驱动模块15采标准的CAN协议进行通讯，传递关节空间各关节目标角度值Θ和反馈角度值Θ'、力控制的速度补偿值；电机驱动模块15由电机控制器和电机驱动器组成；力感测传感器用于位置与速度检测模块14和力信息采集模块16。

进一步地，人机交互模块12，用户通过人机交互模块12设定运动指令和相关参数；电机驱动模块15包含三个闭环控制，分别为位置环、速度换和电流环，用以控制伺服电机；位置与速度检测模块14将实时检测当前位置与速度并传递回机器人控制器11与电机驱动模块15；力信息采集模块16实时采集所述工业机器人末端工具与加工工件间的力信息，并反馈至机器人控制器11中。

机器人控制器11包括轨迹规划模块21、运动学计算模块22、重力项标定模块24、动力学计算模块23、力信息补偿模块25、力/位控制模式切换模块28、阻尼特性转换模块26、雅可比转换模块28。

轨迹规划模块21，将用户输入的运动指令加以解析，规划出合理的运动轨迹；运动学模块22，通过运动学模型将运动轨迹点加以计算，得出关节空间下个关节转角；动力学计算模块23，实时计算出各关节动力学项；重力项标定模块24，标定出所述工业机器人所加载的工具重力；力信息补偿模块25，用工具重力及惯性力对所述工业机器人工具末端与加工工件间采集的力信息进行补偿；阻尼特性变换模块26，将力信息通过阻尼特性变换为笛卡尔空间下工具末端速度补偿值；雅可比转换模块27，实时将笛卡尔空间下工具末端速度补偿值通过雅可比矩阵转化为关节空间下各关节速度补偿值；力/位控制模式切换模块28，通过选择矩阵S将需要进行力控制和位置控制的方向进行划分，对需要进行力控制的方向进行力控制，对需要进行位置控制的方向进行位置控制。

本发明的实施例将动力学项加入到六维力的力信息补偿项中，使得工业机器人检测出的工具末端与加工工件间力信息更接近真实值，提高控制精度；将力反馈信息通过一系列变换后作为速度补偿值加入到电机控制的速度环中，相比较将力反馈信息变换为位置补偿值加入到位置环的方式，提高了系统的相应速度；采用阻尼特性来调节机器人的刚性，省去了复杂的参数调节过程，使得力控制变得更直观、更便于调节；采用选择矩阵的方式对工业机器人各方向进行位置控制和力控制的切换，使得力/位控制方式的转换变得简单便捷。

具体地，还包括一种工业机器人主动柔顺控制方法，包括步骤：

S1、轨迹规划模块21根据需求规划工业机器人的运动轨迹；

其中，运动轨迹为三次样条曲线。

S2、运动学计算模块22计算工业机器人的正反运动学解，对运动轨迹进行运动学解耦计算；

S3、电机驱动模块15对运动学解耦结果加以实现；电机驱动模块15包括三个闭环控制：位置环、速度环、电流环，所述三个闭环控制实现对所述各关节的转动。

S4、位置与速度检测模块14实时检测工业机器人的实际位置与速度，并反馈至电机驱动模块15用于电机驱动模块15进行闭环控制；

S5、重力项标定模块24对工业机器人加载的工具进行重力标定，获取重力项；

S6、动力学计算模块23计算工业机器人动力学项；

S7、力信息采集模块16实时采集工业机器人工具末端与工件之间的力信息；

S8、力信息补偿模块25对步骤S5、S6中得到的重力项和动力学项进行补偿；

S9、力/位控制模式切换模块28获取工业机器人各关节角速度作为补偿值加载；

S10、阻尼特性转换模块26对工业机器人末端工具与工件间的作用力进行阻尼线性转换，得到笛卡尔空间下各方向的线速度与角速度；

S11、雅可比转换模块27将笛卡尔空间下的线速度与角速度转化为关节空间下各关节角速度。

步骤S2中解耦计算具体为通过运动学计算模块对运动轨迹进行时间上的离散化，分解成为多个路径点，对每个路径点的笛卡尔空间下工业机器人位姿进行运动学反解得到关节空间下的各关节转角。

步骤S4中位置与速度检测模块14采用码盘，实时对关节空间下各关节转动角度和速度进行检测并反馈回电机驱动模块，并由电机驱动模块的三个闭环控制。

步骤S5中重力项标定模块24为力感测传感器，工业机器人进行作业前事先标定出工业机器人所加载的工具重力；并在工业机器人保持一个姿态不动且与工件未接触时由力感测传感器检测出的力信息计算得到工具的重力。

标定工具重力的方法为以工业机器人多组姿态下的力信息测量值作为原始数据，采用最小二乘法拟合出工具重力。

步骤S6中动力学计算模块23，在工业机器人运动中实时计算工业机器人运动产生的动力学项：

$\mathrm{\τ}=M\left(\mathrm{\Θ}\right)\stackrel{\·\·}{\mathrm{\Θ}}+B\left(\mathrm{\Θ}\right)\[\stackrel{\·}{\mathrm{\Θ}}\stackrel{\·}{\mathrm{\Θ}}\]+C\left(\mathrm{\Θ}\right)\[\stackrel{.}{{\mathrm{\Θ}}^2}\]+G\left(\mathrm{\Θ}\right)$

其中，τ为关机转矩，M(Θ)为加速度项，B(Θ)为科里奥利力项，C(Θ)为离心力项，G(Θ)为重力项，Θ为关节转角。

步骤S7中力信息采集模块16为力感测传感器，工业机器人运动中实时检测出工业机器人末端工具与加工工件间的力信息。

步骤S8中力信息补偿模块25对实时的力信息进行重力项和动力学项补偿，得出最接近工业机器人末端工具与加工工件间接触力的力信息。

步骤S9中力/位控制模式切换模块28为主对角线为1或0、其余元素为0的选择矩阵S，补偿值在选择矩阵S的作用下实现在不同方向上的力控制与位置控制的切换。

步骤S10中阻尼特性转换模块26具体为力/位混合控制的控制，包括阻尼控制和力/位混合控制。

步骤S11中雅可比矩阵转换模块27为实时根据工业机器人位姿计算雅可比矩阵，并由公式将笛卡尔空间下的工业机器人末端工具速度补偿值转化为关节空间下各关节的速度

其中，为关节空间速度补偿值，J^-1(Θ)为雅可比逆矩阵，ΔV为笛卡尔空间速度补偿值。

如图3所示，本发明实施例提供的工业机器人主动柔顺控制方案：首先，由力感测器件-六维力传感器采集工业机器人工具末端与加工工件间力信息，上述力并非是工具与工件间的真实接触力，其中还包含工具自身重力和由于机器人运动引起的惯性力，因此需要进行补偿；其次，通过选择矩阵筛选需要进行力控制的方向；再次，通过设定的力与采集的力进行差值运算，得到力补偿差值；再次，由阻尼变换和雅可比变换将力补偿差值变换为关节空间下各关节速度补偿值；最后，将速度补偿值与位置环给出的速度进行相加运算，将此和值作为速度环的输入，由此完成力控制闭环。

如图4所示，在所述工业机器人模型建立后，给定路径规划的情况下机器人控制器进行运动学与动力学的相关计算，判断笛卡尔空间下工具末端各方向是否需要进行力控制，并执行相应的力控制或位置控制策略。

本发明实施例的控制方法和装置，采样主动柔顺控制，可以使工业机器人更加顺利的完成各类主动柔顺加工作业，扩大了工业机器人的应用范围及领域。

以上本发明的具体实施方式，并不构成对本发明保护范围的限定。任何根据本发明的技术构思所作出的各种其他相应的改变与变形，均应包含在本发明权利要求的保护范围内。

Claims (15)

1.一种工业机器人主动柔顺控制方法，应用于工业机器人控制，其特征在于，包括步骤：

S1、轨迹规划模块根据需求规划所述工业机器人的运动轨迹；

S2、运动学计算模块计算所述工业机器人的正反运动学解，对所述运动轨迹进行运动学解耦计算；

S3、电机驱动模块对所述运动学解耦结果加以实现；

S4、位置与速度检测模块实时检测所述工业机器人的实际位置与速度，并反馈至所述电机驱动模块用于所述电机驱动模块进行闭环控制；

S5、重力项标定模块对所述工业机器人加载的工具进行重力标定，获取重力项；

S6、动力学计算模块计算所述工业机器人动力学项；

S7、力信息采集模块实时采集所述工业机器人工具末端与工件之间的力信息；

S8、力信息补偿模块对步骤S5、S6中得到的重力项和动力学项进行补偿；

S9、力/位控制模式切换模块获取所述工业机器人各关节角速度作为补偿值加载；

S10、阻尼特性转换模块对所述工业机器人末端工具与工件间的作用力进行阻尼线性转换，得到笛卡尔空间下各方向的线速度与角速度；

S11、雅可比转换模块将所述笛卡尔空间下的线速度与角速度转化为关节空间下各关节角速度。

2.根据权利要求1所述的工业机器人主动柔顺控制方法，其特征在于，步骤S1中所述运动轨迹为三次样条曲线。

3.根据权利要求1所述的工业机器人主动柔顺控制方法，其特征在于，步骤S2中所述解耦计算具体为通过运动学计算模块对所述运动轨迹进行时间上的离散化，分解成为多个路径点，对每个路径点的笛卡尔空间下所述工业机器人位姿进行运动学反解得到关节空间下的各关节转角。

4.根据权利要求1所述的工业机器人主动柔顺控制方法，其特征在于，步骤S3中所述电机驱动模块包括三个闭环控制：位置环、速度环、电流环，所述三个闭环控制实现对所述各关节的转动。

5.根据权利要求1所述的工业机器人主动柔顺控制方法，其特征在于，步骤S4中所述位置与速度检测模块采用码盘，实时对所述关节空间下各关节转动角度和速度进行检测并反馈回所述电机驱动模块，并由所述电机驱动模块的三个闭环控制。

6.根据权利要求1所述的工业机器人主动柔顺控制方法，其特征在于，步骤S5中所述重力项标定模块为力感测传感器，所述工业机器人进行作业前事先标定出所述工业机器人所加载的工具重力；并在所述工业机器人保持一个姿态不动且与工件未接触时由力感测传感器检测出的力信息计算得到工具的重力。

7.根据权利要求6所述的工业机器人主动柔顺控制方法，其特征在于，标定工具重力的方法为以所述工业机器人多组姿态下的力信息测量值作为原始数据，采用最小二乘法拟合出工具重力。

8.根据权利要求1所述的工业机器人主动柔顺控制方法，其特征在于，步骤S6中所述动力学计算模块，在所述工业机器人运动中实时计算所述工业机器人运动产生的动力学项：

$\mathrm{\τ}=M\left(\mathrm{\Θ}\right)\stackrel{\·\·}{\mathrm{\Θ}}+B\left(\mathrm{\Θ}\right)\[\stackrel{\·}{\mathrm{\Θ}}\stackrel{\·}{\mathrm{\Θ}}\]+C\left(\mathrm{\Θ}\right)\[{\stackrel{\·}{\mathrm{\Θ}}}^2\]+G\left(\mathrm{\Θ}\right)$

其中，τ为关机转矩，M(Θ)为加速度项，B(Θ)为科里奥利力项，C(Θ)为离心力项，G(Θ)为重力项，Θ为关节转角。

9.根据权利要求1所述的工业机器人主动柔顺控制方法，其特征在于，步骤S7中力信息采集模块为力感测传感器，所述工业机器人运动中实时检测出所述工业机器人末端工具与加工工件间的力信息。

10.根据权利要求1所述的工业机器人主动柔顺控制方法，其特征在于，步骤S8中所述力信息补偿模块对实时的所述力信息进行重力项和动力学项补偿，得出最接近所述工业机器人末端工具与加工工件间接触力的力信息。

11.根据权利要求1所述的工业机器人主动柔顺控制方法，其特征在于，步骤S9中所述力/位控制模式切换模块为主对角线为1或0、其余元素为0的选择矩阵S，所述补偿值在所述选择矩阵S的作用下实现在不同方向上的力控制与位置控制的切换。

12.根据权利要求1所述的工业机器人主动柔顺控制方法，其特征在于，步骤S10中所述阻尼特性转换模块具体为力/位混合控制的控制，包括阻尼控制和力/位混合控制。

13.根据权利要求1所述的工业机器人主动柔顺控制方法，其特征在于，步骤S11中所述雅可比矩阵转换模块为实时根据所述工业机器人位姿计算雅可比矩阵，并由公式将笛卡尔空间下的所述工业机器人末端工具速度补偿值转化为关节空间下各关节的速度

其中，为关节空间速度补偿值，J^-1(Θ)为雅可比逆矩阵，ΔV为笛卡尔空间速度补偿值。

14.一种工业机器人主动柔顺控制装置，其特征在于，包括机器人控制器、电机控制器、力感测传感器、若干套电机驱动器及伺服电机；

所述机器人控制器与所述电机控制器采用CAN协议进行通信；所述电机控制器和电机驱动器组成电机驱动模块；所述力感测传感器用于位置与速度检测模块和力信息采集模块；

所述机器人控制器包括轨迹规划模块、运动学计算模块重力项标定模块、动力学计算模块、力信息补偿模块、力/位控制模式切换模块、阻尼特性转换模块、雅可比转换模块；

所述工业机器人主动柔顺控制装置采用如权利要求1-13任意一项的方法控制。

15.根据权利要求14所述的工业机器人主动柔顺控制装置，其特征在于，包括电机驱动器及伺服电机为6套。