CN113093549B - 一种多轴数控装备的复合控制方法 - Google Patents

一种多轴数控装备的复合控制方法 Download PDF

Info

Publication number
CN113093549B
CN113093549B CN202110376222.3A CN202110376222A CN113093549B CN 113093549 B CN113093549 B CN 113093549B CN 202110376222 A CN202110376222 A CN 202110376222A CN 113093549 B CN113093549 B CN 113093549B
Authority
CN
China
Prior art keywords
numerical control
control equipment
axis
axis numerical
coordinate system
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110376222.3A
Other languages
English (en)
Other versions
CN113093549A (zh
Inventor
张驰
王宝磊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Ningbo Institute of Material Technology and Engineering of CAS
Original Assignee
Ningbo Institute of Material Technology and Engineering of CAS
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Ningbo Institute of Material Technology and Engineering of CAS filed Critical Ningbo Institute of Material Technology and Engineering of CAS
Priority to CN202110376222.3A priority Critical patent/CN113093549B/zh
Publication of CN113093549A publication Critical patent/CN113093549A/zh
Application granted granted Critical
Publication of CN113093549B publication Critical patent/CN113093549B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Numerical Control (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明公开了一种多轴数控装备的复合控制方法，其包括如下步骤：任务示教编程，特征点初始定位，路径规划，伺服反馈控制，特征点动态定位，逆解关节位置，有限时间状态运算，有限时间输出空置率运算以及自适应前馈补偿。本发明提供的多轴数控装备的复合控制方法在工程上容易实施、抗干扰能力强、鲁棒性程度高、具有一定自适应能力，并且该复合控制方法计算简单、控制周期短、控制频率高、响应快、误差小、精度高、滞后少、对多轴数控装备一致性要求低，有效解决了基于双目视觉定位的受时变不确定扰动影响的高性能数控装备有限时间输出前馈补偿控制的设计和实施问题。

Description

一种多轴数控装备的复合控制方法
技术领域
本发明属于多轴数控装备控制技术领域，具体涉及一种多轴数控装备的复合控制方法。
背景技术
高性能多轴数控装备系统动力学前馈控制与通用机电系统动力学控制基本思想一致，即根据多轴数控装备系统物理结构特点建立多轴数控装备系统动力学模型，计算多轴数控装备系统关节驱动力，然后将驱动力转换为伺服电机的前馈控制电流。同时，机械臂路径规划模块根据机械臂任务空间特征点和特征曲线生成伺服电机的参考位置点序列，参考位置点与当前时刻的反馈位置点比较后在伺服电机位置环控制器的作用下生成伺服电机参考速度，伺服电机参考速度与当前时刻的伺服电机反馈速度比较后在伺服电机速度环控制器的作用下生成伺服电机参考电流。前馈电流和参考电流叠加与当前时刻反馈电流比较后再伺服电机电流环调节器作用后经空间矢量正弦控制器控制伺服电机，驱动关节运动完成既定任务。
上述控制过程如图1所示，反馈控制部分为伺服电机属于通用控制技术，前馈控制部分核心在于建立数学模型完成多轴数控装备系统动力学计算模块，建模计算主要通过下式进行。
τ＝M(q)q″+C(q，q′)q′+G(q)+d
τ＝[τ1 … τn]T
Figure BDA0003009544560000011
C(q，q′)＝[c1(q，q′) … cn(q，q′)]T
G(q)＝[G1(q) … Gn(q)]T
上式中，n表示多轴数控装备系统关节数量；τ表示各关节驱动力矩的向量；q、q′、q″分别表示各关节当前位置、速度、加速度的向量；M(q)为惯性矩阵C(q，q′)为科氏力系数矩阵；G(q)为重力矩；d为干扰力矩，包括关节摩擦力矩、模型参数辨识误差引入的干扰、高阶未建模引入的干扰、多轴数控装备系统外部不可预测干扰等。这些系数的计算可以通过拉格朗日法或牛顿欧拉法完成。
多轴数控装备系统动力学前馈控制技术除上述背景技术的直接应用方法外，还包括以下三类改进方法：以东南大学孙玉阳和孟正大在其学位论文《重载机器人动力学建模及前馈控制方法研究与实现》中所述的前馈方法为代表的简化应用法；以广州弘度信息科技有限公司申请的发明专利“一种基于动力学的机械臂柔性控制方法”(CN201910651925.5)为代表的直接力矩反馈控制改进法；以华中科技大学叶伯生等人在其专利“一种基于动力学模型的机器人自适应阻抗控制系统”(CN201910352004.9)中所述的自适应阻抗法。
上述技术在多轴数控装备系统应用时，存在的缺点包括：①控制计算复杂、周期长、频率低，直接应用响应慢；②控制精度低；③前馈电流存在滞后性，对高速机械臂系统干扰较大，不适合于高速机械臂；④对多轴数控装备系统本体一致性要求高，调试结果在同型号不同个体机器人之间的通用性受限；⑤前馈电流与伺服电机速度环产生的电流结合过于简单，容易引起多轴数控装备系统位置和速度的超调，需要较长的时间进行稳定，潜在风险大；⑥控制方法需要被控对象多轴数控装备系统能够直接反馈关节位置信号，通用性差。
导致上述缺点①的原因为：多轴数控装备系统动力学模型算法复杂度相对于驱动各关节运动的伺服电机动力学计算要大很多，如典型的一种多轴数控装备系统六轴机械臂的动力学模型计算需要至少4000次乘法和加法才能完成，而最基本的伺服电机动力学位置环、速度环、电流环的计算却仅需要十多次乘法和加法运算。因此，多轴数控装备系统动力学控制运算一次所需要的时间往往是关节伺服电机控制所需运算时间的上百倍。进而，产生控制周期长、频率低、响应慢的问题。
导致上述缺点②的原因为：一方面，多轴数控装备系统动力学模型算法中的部分模型参数辨识在实际工程中较为困难，具有时变性，辨识误差较大，比如其中的科氏力矩和干扰力矩，都是不易辨识且在时刻变化的，因此，直接将参数代入到模型中计算动力学结果会产生较大的偏差，导致精度低；另一方面，为减轻缺点①的影响，部分技术人员采取了对多轴数控装备系统动力学计算模型进行直接离散化的思路进行改进，即把当前时刻多轴数控装备系统动力学计算结果与前一时刻机械臂动力学计算结果按时间平均离散成与伺服电机控制周期相同的序列结果，再把该序列结果作为前馈值加入到控制中。该方法忽视了多轴数控装备系统的非线性本质，采取简单的线性离散技术对非线性系统进行离散，因此结果是不准确的，进而会造成控制精度低的问题。
导致上述缺点③的原因为：如导致缺点①原因中所述，多轴数控装备系统动力学模型计算所需时间较长，因此，在当前时刻采样的计算所需变量(各关节的位置、速度、加速度等)在计算完成时，高速多轴数控装备系统已经偏离这些变量(各关节的位置、速度、加速度等)较大，计算出的前馈结果已经严重滞后，不仅不再有利于当前的控制，而且还会对当前的控制行程较大的干扰。
导致上述缺点④的原因为：多轴数控装备系统动力学模型以严格精确的模型参数为基础，因此调试计算的结果对多轴数控装备系统的一致性要求较高。在大规模生产中，由于加工、工艺、材料的不确定性，实际生产的多轴数控装备系统往往是有差异的，因此研发设计时确定的多轴数控装备系统动力学模型参数很难直接应用于批量生产时的多轴数控装备系统。另外，即使通过对每一台多轴数控装备系统进行动力学模型参数辨识的方式获取各自的多轴数控装备系统动力学模型参数，再进行动力学计算，也仅仅适用于多轴数控装备系统的近期应用中，当实际应用超过一定时间后，运行过程中的磨损就会改变动力学参数，将再次导致模型参数不准，产生不良控制现象。
造成上述缺点⑤的原因在于：多轴数控装备系统动力学模型计算出的前馈电流和伺服电机速度环产生的参考电流直接相加会较大，往往会直接到达饱和值，关节伺服电机经常运行于饱和值工况下，进而造成位置环和速度环的超调。
导致上述缺点⑥的原因在于：多轴数控装备系统动力学控制需要多轴数控装备系统反馈关节的准确位置，而目前主流的多轴数控装备系统反馈的是关节伺服电机动子的转动位置，关节和关节伺服电机是通过减速机相连的，减速机中存在的摩擦、变形、迟滞等非线性因素属于一种干扰，因此将关节伺服电机转动位置进行简单线性计算给动力学控制使用是不合适的。学术上一种弥补方法为重新设计多轴数控装备、在重新设计的过程中加装关节转动定位装置，如空心编码器、电滑环等，但是工程上应用的大部分都是商用多轴数控装备系统，商用多轴数控装备系统的结构紧凑型决定了是无法加装这类装置的。
发明内容
本发明目的在于提供一种能够在工程上容易实施的、抗干扰能力强、鲁棒性程度高、具有一定自适应能力的多轴数控装备系统复合控制方法，以解决目前多轴数控装备系统动力学前馈控制周期长、频率低、响应慢、误差大、精度低、滞后多、对多轴数控装备系统一致性要求高等问题。
本发明的技术方案是这样实现的：
一种多轴数控装备的复合控制方法，其包括如下步骤：
步骤1、任务示教编程：将多轴数控装备工作路径关键点及路径信息示教编程到多轴数控装备控制器中；
步骤2、特征点初始定位：双目视觉相机识别任务区域特征点并定位任务区域特征点在双目视觉相机坐标系中的坐标位置F1，然后将坐标位置F1转换成任务区域特征点在多轴数控装备末端坐标系中的坐标位置F2，控制器读取各轴编码器位置，根据减速机的减速比换算出各轴关节的转动角度位置Q，根据各轴关节的转动角度位置Q计算出多轴数控装备末端坐标系与多轴数控装备基座标系之间的平移旋转关系F3，最后根据平移旋转关系F3将任务区域特征点在多轴数控装备末端坐标系中的坐标位置转换成任务区域特征点在多轴数控装备基座标系中的坐标位置F4；
步骤3、路径规划：将多轴数控装备工作路径关键点及路径信息进行转换，计算出各轴伺服电机运动参考位置序列；
步骤4、伺服反馈控制：将所述各轴伺服电机运动参考位置序列输入各轴伺服驱动器，各轴伺服驱动器实时控制各轴伺服电机转动，带动多轴数控装备开始工作；
步骤5、特征点动态定位、逆解关节位置：双目视觉相机实时识别任务区域特征点并定位任务区域特征点在双目视觉相机坐标系中的动态坐标位置FD1，然后将该动态坐标位置FD1转换成任务区域特征点在多轴数控装备末端坐标系中的动态坐标位置FD2，再根据任务区域特征点在多轴数控装备基座标系中的坐标位置计算出当前多轴数控装备末端在多轴数控装备基座标系中的动态坐标位置FD3，最后根据动态坐标位置FD3逆解出多轴数控装备各轴关节的实时转动角度位置QD；
步骤6、有限时间状态运算：根据所述多轴数控装备各轴关节的实时转动角度位置QD估计出多轴数控装备未知时变干扰的状态及其各阶导数；
步骤7、有限时间输出控制律运算：根据所述多轴数控装备各轴关节的实时转动角度位置QD和未知时变干扰的状态及其各阶导数估计结果，计算出控制力矩；
步骤8、自适应前馈补偿：将所述控制力矩转换成伺服电机前馈控制电流，将前馈控制电流加权运算后，输入到伺服驱动器，对各轴伺服驱动器实时控制各轴伺服电机的转动进行前馈补偿。
优选地，任务区域特征点在双目视觉相机坐标系中的坐标位置F1如公式(1)所示：
Figure BDA0003009544560000041
其中，
Figure BDA0003009544560000051
表示坐标位置F1在x轴方向上的分量，
Figure BDA0003009544560000052
表示坐标位置F1在y轴方向上的分量，
Figure BDA0003009544560000053
表示坐标位置F1在z轴方向上的分量，fl为左目相机焦距，fr为右目相机焦距，识别任务区域特征点在左、右目相机图像坐标系坐标分别为(xl，yl)和(xr，yr)，tx表示右目相机坐标系原点相对于左目相机坐标系原点在x轴方向上的平移位置量，ty表示右目相机坐标系原点相对于左目相机坐标系原点在y轴方向上的平移位置量，tz表示右目相机坐标系原点相对于左目相机坐标系原点在z轴方向上的平移位置量，r1、r2、...、r9分别表示左、右目相机的相对旋转关系的元素。
优选的，所述任务区域特征点在多轴数控装备末端坐标系中的坐标位置F2如公式(2)所示：
Figure BDA0003009544560000054
其中，
Figure BDA0003009544560000055
为双目视觉相机坐标系与多轴数控装备末端坐标系的相对旋转和平移位置关系。
优选的，根据各轴关节的转动角度位置Q＝[q1 q2 … qn]计算出多轴数控装备末端坐标系与多轴数控装备基座标系之间的平移旋转关系F3，如公式(3)所示：
Figure BDA0003009544560000056
其中，
Figure BDA0003009544560000057
表示关节i-1和关节i之间的齐次坐标变换，1≤i≤n。
优选的，将区域特征点在机械臂末端坐标系中的坐标位置转换计算成区域特征点在机械臂基座标系中的坐标位置F4的计算过程如公式(4)所示：
F4＝F3·F2 (4)。
优选地，所述步骤(3)中，路径规划采用基于带有阻滞因子的引力场系数改进的加权最小范数法，通过将阻滞因子和引力场系数引入到最小范数法中，使多轴数控装备的关节运行至接近限位段时，受到抑制作用后减速防止进入限位段，然后通过引力场系数作用退回至常规运行段，整个过程无需切换权重因子。
优选地，所述步骤(3)中，路径规划具体步骤如下：
首先将多轴数控装备末端轨迹离散化，得到多轴数控装备末端参考位置离散出来的速度轨迹，然后由公式(5)将末端速度轨迹转化为各轴速度轨迹：
Figure BDA0003009544560000058
其中，右侧第一项为路径规划主导运动部分，第二项为抑制运动部分，第三项为引力场作用部分；q＝[q1，q2，…，qn]为多轴数控装备关节转动角度，
Figure BDA0003009544560000061
为关节速度，J为多轴数控装备雅克比矩阵，
Figure BDA0003009544560000062
为加权系数矩阵，
Figure BDA0003009544560000063
计算如式(6)所示，λ1I、λ2I、λ3I为防奇异因子，G(q)＝[g1，g2，…，gn]T为阻滞因子，gi计算如式(7)所示，Z＝[ζ1，ζ2，…，ζn]T为引力回归因子，ζi计算如式(8)所示：
Figure BDA0003009544560000064
Figure BDA0003009544560000065
Figure BDA0003009544560000066
其中，k1、k2、k3、k4、κg和εz为调整系数；
得出各轴速度轨迹后，按各轴伺服电机位置环控制采样周期进行累积，即得出各轴位置轨迹。
优选地，所述步骤(6)中，有限时间状态运算具体包括如下步骤：
步骤(6.1)、对多轴数控装备进行动力学建模，如公式(10)所示：
Figure BDA0003009544560000067
其中，M(QD)为多轴数控装备惯量矩阵，
Figure BDA0003009544560000068
为科氏力和离心力矩阵，G(QD)为重力矩阵，h(t)为外部扰动和高阶未建模干扰，τ(t)为控制力矩矩阵；
步骤(6.2)、将模型中的惯量矩阵分为名义惯量矩阵和动态惯量矩阵部分，然后将动态惯量矩阵、科氏力和离心力矩阵、重力矩阵、外部扰动和高阶未建模干扰统一视作总的时变干扰，将模型(10)改写为；
Figure BDA0003009544560000069
其中，Mn为名义惯量矩阵，
Figure BDA00030095445600000610
为总的时变干扰；
步骤(6.3)、将多轴数控装备的总的扰动看作一个新加状态，把模型(11)改为状态空间表达形式，如公式(12)所示：
Figure BDA0003009544560000071
其中，x0＝QD(t)，x1＝Q′D(t)，z1＝H(t)，…，zm-1＝H(m-1)(t)；
步骤(6.4)、设计有限时间观测器如式(13)所示，将所述多轴数控装备各轴关节的实时转动角度位置输入到有限时间扩展状态观测器中，估计出多轴数控装备未知时变干扰的状态及其各阶导数；
Figure BDA0003009544560000072
其中，
Figure BDA0003009544560000073
分别为x0，x1，z0，z1，…，zm-1的估计，k1，k2，…，km+2，β1，β2，…，βm+2为观测器调节系数。
优选地，所述步骤(7)中，所述控制力矩采用公式(14)计算：
Figure BDA0003009544560000074
其中，c0，c1，α1，α2为控制器调节系数。
优选地，所述步骤(8)中，所述加权运算的加权系数由自适应可变增益动态决定，自适应可变增益由参考位置和反馈位置的位置误差决定；所述位置误差大时，通过所述加权运算加强伺服电机反馈控制、减弱视觉前馈控制；所述位置误差小时，通过所述加权运算减弱伺服电机反馈控制、增强视觉前馈控制。
与现有技术相比，本发明的多轴数控装备的复合控制方法在工程上容易实施、抗干扰能力强、鲁棒性程度高、具有一定自适应能力，该方法计算简单、控制周期短、控制频率高、响应快、误差小、精度高、滞后少、对多轴数控装备一致性要求低。本发明有效解决了基于双目视觉定位的受时变不确定扰动影响的高性能数控装备有限时间输出前馈补偿控制的设计和实施问题。
附图说明
图1现有多轴数控装备前馈控制方法示意图；
图2是本发明实施例1中的一种多轴机械臂控制系统的结构示意图；
图3是本发明实施例1中的一种复合控制方法框图；
图4是本发明实施例1中的一种复合控制方法流程图。
具体实施方式
下面结合附图对本发明的较佳实施例进行详细阐述，以使本发明的优点和特征能更易于被本领域技术人员理解，从而对本发明的保护范围做出更为清楚明确的界定。
实施例1
本实施例以一种典型多轴数控装备系统——六轴机械臂为例，对本发明的多轴数控装备的复合控制方法进行详细描述。
基于双目视觉定位的有限时间输出前馈补偿的多轴数控装备系统复合控制方法涉及到的硬件装置包括控制装置、示教编程器、被控多轴数控装备系统、双目视觉相机。
如图2所示为该六轴机械臂控制系统的结构示意图，其中，控制装置主要由主控计算机、多个伺服驱动器(例如伺服驱动器1-伺服驱动器6)、开关电源、抱闸制动继电器、隔离IO模块、低压电器等组成，本发明所涉及的控制方法运行在主控计算机及伺服驱动器上。被控多轴数控装备系统主要由底座、各轴关节(例如关节1-关节6，其中各关节包括伺服电机、编码器、抱闸、减速机等)、伺服电机、编码器、抱闸、减速机、多个连杆(例如连杆0-连杆6)、末端安装法兰等构成。双目视觉相机主要由相机安装底座、外壳、左目相机、右目相机、左目镜头、右目镜头、光源、透明护罩组成，双目视觉相机安装在多轴数控装备系统末端。
下面结合多轴数控装备系统的典型代表机械臂对本发明多轴数控装备的复合控制方法进行详细描述。如图3、图4所示，多轴数控装备的复合控制方法包括如下步骤：
步骤(1)，任务示教编程：操作员通过示教编程器将机械臂工作路径关键点及路径信息示教编程到机械臂控制器中。
步骤(2)，特征点初始定位：机械臂操作员通过示教编程器将机械臂末端操作到机械臂任务区域，锁紧抱闸，机械臂末端双目视觉相机识别任务区域特征点并定位任务区域特征点在双目视觉相机坐标系中的坐标位置F1，然后将坐标位置转换成任务区域特征点在机械臂末端坐标系中的坐标位置F2，控制系统读取当前的各轴编码器位置，根据减速机的减速比换算出各轴关节的转动角度位置Q，根据各轴关节的转动角度位置Q计算出机械臂末端坐标系与机械臂基座标系之间的平移旋转关系F3，最后根据该关系F3将区域特征点在机械臂末端坐标系中的坐标位置转换计算出区域特征点在机械臂基座标系中的坐标位置F4。
本步骤涉及到的计算过程如公式(1)-公式(4)所示。
机械臂末端双目视觉相机定位任务区域特征点在双目视觉相机坐标系中的坐标位置F1如公式(1)所示。
Figure BDA0003009544560000091
其中，fl为左目相机焦距，fr为右目相机焦距，识别任务区域特征点在左右目相机图像坐标系坐标分别为(xl，yl)和(xr，yr)，tx、ty、tz、r1、r2、......、r9分别为表示左右目相机的位置关系的旋转矩阵元素和平移向量元素。
将坐标位置F1转换成任务区域特征点在机械臂末端坐标系中的坐标位置F2如公式(2)所示。
Figure BDA0003009544560000092
其中，
Figure BDA0003009544560000093
为双目视觉相机坐标系与机械臂末端坐标系的相对旋转和平移位置关系。
Figure BDA0003009544560000094
表示一种平移和旋转的位置姿态变换关系，当双目视觉相机在机械臂末端安装装配完成后，平移和旋转的位置姿态变换关系也就确定了，就可以测量或标定出
Figure BDA0003009544560000095
这个矩阵的具体参数。E表示装置的末端，由于末端安装的是双目视觉相机，因此这里E实际上表示末端是双目视觉相机坐标。
根据各轴关节角度位置Q＝[q1 q2 … q6]计算出机械臂末端坐标系与机械臂基座标系之间的平移旋转关系F3，如公式(3)所示。
Figure BDA0003009544560000096
将区域特征点在机械臂末端坐标系中的坐标位置转换计算成区域特征点在机械臂基座标系中的坐标位置F4的计算过程如公式(4)所示。
F4＝F3·F2 (4)
步骤(3)，路径规划：机械臂中路径规划模块将机械臂工作路径关键点及路径信息转换计算出各轴伺服电机运动参考位置序列。本方法中所采用的路径规划模块采用基于带有阻滞因子的引力场系数改进的加权最小范数法，该方法通过将阻滞因子和引力场系数引入到最小范数法中，使关节运行至接近限位段时，受到抑制作用后减速防止进入限位段，然后通过引力场系数作用退回至常规运行段。整个过程无需切换权重因子，可以较为完善的解决路径规划中的关节位置约束问题。计算时首先将机械臂末端轨迹离散化，得到机械臂末端参考位置离散出来的速度轨迹，然后由公式(5)将末端速度轨迹转化为各轴速度轨迹。
Figure BDA0003009544560000101
其中，右侧第一项为路径规划主导运动部分，第二项为抑制运动部分，第三项为引力场作用部分。q＝[q1，q2，…，q6]为机械臂关节转动角度，
Figure BDA0003009544560000102
为关节速度，J为机械臂雅克比矩阵，
Figure BDA0003009544560000103
为加权系数矩阵，
Figure BDA0003009544560000104
计算如式(6)所示，λ1I、λ2I、λ3I为防奇异因子，G(q)＝[g1，g2，…，g6]T为阻滞因子，gi计算如式(7)所示，Z＝[ζ1，ζ2，…，ζ6]T为引力回归因子，ζi计算如式(8)所示。
Figure BDA0003009544560000105
Figure BDA0003009544560000106
Figure BDA0003009544560000107
其中，k1、k2、k3、k4、κg和εz为调整系数；
Figure BDA0003009544560000108
表示关节运行范围的负极限值，
Figure BDA0003009544560000109
表示关节运行范围的正极限值，
Figure BDA00030095445600001010
表示接近关节运行范围的负极限值的阈值，
Figure BDA00030095445600001011
表示接近关节运行范围的正极限值的阈值。
得出各轴速度轨迹后，按各轴伺服电机位置环控制采样周期进行累积，即可得出各轴位置轨迹。
步骤(4)，伺服反馈控制：各轴伺服电机运动参考位置序列输入各轴伺服驱动器参考位置采集模块，各轴伺服驱动器实时控制各轴伺服电机转动，带动机械臂开始工作。
步骤(5)，特征点动态定位，逆解关节位置：机械臂工作过程中，双目视觉相机实时的识别任务区域特征点并定位任务区域特征点在双目视觉相机坐标系中的动态坐标位置FD1，然后将该动态坐标位置转换成在机械臂末端坐标系中的动态坐标位置FD2，再根据任务区域特征点在机械臂基座标系中的坐标位置计算出当前机械臂末端在机械臂基座标系中的动态坐标位置FD3，最后根据动态坐标位置FD3逆解出机械臂各轴关节的实时转动角度位置QD。该步骤中，求解FD1和FD2的过程与步骤(2)中求解F1和F2类似，此处不再赘述。求解FD3如式(9)所示。
Figure BDA0003009544560000111
步骤(6)，有限时间状态观测器运算：将步骤(5)中逆解出的机械臂各轴关节的实时转动角度位置QD输入到本实施例的有限时间扩展状态观测器中，将机械臂系统中未知时变干扰的状态及其各阶导数估计出来。有限时间扩展状态观测器设计过程如下列具体步骤中的公式(10)-公式(13)所示。
步骤(6.1)，对六轴机械臂进行动力学建模，如公式(10)所示。
Figure BDA0003009544560000112
其中，QD是步骤(5)的输出结果，M(QD)为机械臂惯量矩阵，
Figure BDA0003009544560000113
为科氏力和离心力矩阵，G(QD)为重力矩阵，h(t)为外部扰动和高阶未建模干扰，τ(t)为控制力矩矩阵。在机械臂实际运行过程中，这些矩阵都是实时变化的，计算较为复杂。
步骤(6.2)，对上述模型进行改进，将模型中的惯量矩阵分为名义惯量矩阵和动态惯量矩阵部分，然后将动态惯量矩阵、科氏力和离心力矩阵重力矩阵、外部扰动和高阶未建模干扰统一视作总的时变干扰，基于此，模型(10)可以改写为。
Figure BDA0003009544560000114
其中，Mn为名义惯量矩阵，
Figure BDA0003009544560000115
为总的时变干扰。
步骤(6.3)，将机械臂的总的扰动看作一个新加状态，把模型(11)改为状态空间表达形式，如公式(12)所示。
Figure BDA0003009544560000121
其中，x0＝QD(t)，x1＝Q′D(t)，z1＝H(t)，…，zm-1＝H(m-1)(t)。
步骤(6.4)，设计有限时间观测器如式(13)所示。
Figure BDA0003009544560000122
其中，
Figure BDA0003009544560000123
分别为x0，x1，z0，z1，…，zm-1的估计，k1，k2，…，km+2，β1，β2，…，βm+2为观测器调节系数。
步骤(7)，有限时间输出控制律运算：将步骤(5)中逆解出的机械臂各轴关节的实时转动角度位置QD和步骤(6)求取的未知时变干扰的状态及其各阶导数估计结果输入到设计的有限时间输出控制律中，如式(14)所示，计算出控制力矩。
Figure BDA0003009544560000124
其中，c0，c1，α1，α2为控制器调节系数。
步骤(8)，自适应前馈补偿：将步骤(7)中计算出的控制力矩转换成伺服电机前馈控制电流，将前馈控制电流加权运算后，输入到伺服驱动器前馈电流采集模块中，对各轴伺服驱动器实时控制各轴伺服电机的转动进行前馈补偿。加权系数由自适应可变增益动态决定，自适应可变增益由参考位置和反馈位置的位置误差决定。自适应增益1与位置误差呈正比例关系，自适应增益2与位置误差呈反比例关系，具体比例系数在调试时根据实际系统表现进行确定。换而言之，位置误差大时，自适应增益1增大，自适应增益2减小，此时伺服电机反馈控制加强、视觉前馈控制减弱；位置误差小时，自适应增益1减小，自适应增益2增大，此时伺服电机反馈控制减弱、视觉前馈控制加强；
如图4所示，上述步骤可以分为两个阶段，即运行准备阶段和实时运行极端，其中步骤(1)-(3)为运行准备阶段，步骤(4)-(8)为实时运行阶段。
尽管已参考说明性实施例描述了本发明，但所属领域的技术人员将理解，在不背离本发明的精神及范围的情况下可做出各种其它改变、省略及/或添加且可用实质等效物替代所述实施例的元件。另外，可在不背离本发明的范围的情况下做出许多修改以使特定情形或材料适应本发明的教示。因此，本文并不打算将本发明限制于用于执行本发明的所揭示特定实施例，而是打算使本发明将包含归属于所附权利要求书的范围内的所有实施例。

Claims (9)

1.一种多轴数控装备的复合控制方法，其特征在于包括如下步骤：
步骤1、任务示教编程：将多轴数控装备工作路径关键点及路径信息示教编程到多轴数控装备控制器中；
步骤2、特征点初始定位：双目视觉相机识别任务区域特征点并定位任务区域特征点在双目视觉相机坐标系中的坐标位置F1，然后将坐标位置F1转换成任务区域特征点在多轴数控装备末端坐标系中的坐标位置F2，控制器读取各轴编码器位置，根据减速机的减速比换算出各轴关节的转动角度位置Q，根据各轴关节的转动角度位置Q计算出多轴数控装备末端坐标系与多轴数控装备基座标系之间的平移旋转关系F3，最后根据平移旋转关系F3将任务区域特征点在多轴数控装备末端坐标系中的坐标位置转换成任务区域特征点在多轴数控装备基座标系中的坐标位置F4；
步骤3、路径规划：将多轴数控装备工作路径关键点及路径信息进行转换，计算出各轴伺服电机运动参考位置序列；
步骤4、伺服反馈控制：将所述各轴伺服电机运动参考位置序列输入各轴伺服驱动器，各轴伺服驱动器实时控制各轴伺服电机转动，带动多轴数控装备开始工作；
步骤5、特征点动态定位、逆解关节位置：双目视觉相机实时识别任务区域特征点并定位任务区域特征点在双目视觉相机坐标系中的动态坐标位置FD1，然后将该动态坐标位置FD1转换成任务区域特征点在多轴数控装备末端坐标系中的动态坐标位置FD2，再根据任务区域特征点在多轴数控装备基座标系中的坐标位置计算出当前多轴数控装备末端在多轴数控装备基座标系中的动态坐标位置FD3，最后根据动态坐标位置FD3逆解出多轴数控装备各轴关节的实时转动角度位置QD；
步骤6、有限时间状态运算：根据所述多轴数控装备各轴关节的实时转动角度位置QD估计出多轴数控装备未知时变干扰的状态及其各阶导数；
步骤7、有限时间输出控制律运算：根据所述多轴数控装备各轴关节的实时转动角度位置QD和未知时变干扰的状态及其各阶导数估计结果，计算出控制力矩；
步骤8、自适应前馈补偿：将所述控制力矩转换成伺服电机前馈控制电流，将前馈控制电流加权运算后，输入到伺服驱动器，对各轴伺服驱动器实时控制各轴伺服电机的转动进行前馈补偿。
2.如权利要求1所述的多轴数控装备的复合控制方法，其特征在于：所述任务区域特征点在双目视觉相机坐标系中的坐标位置F1如公式(1)所示：
Figure FDA0003804356980000021
其中，
Figure FDA0003804356980000025
表示坐标位置F1在x轴方向上的分量，
Figure FDA0003804356980000026
表示坐标位置F1在y轴方向上的分量，
Figure FDA0003804356980000027
表示坐标位置F1在z轴方向上的分量，fl为左目相机焦距，fr为右目相机焦距，识别任务区域特征点在左、右目相机图像坐标系坐标分别为(xl，yl)和(xr，yr)，tx表示右目相机坐标系原点相对于左目相机坐标系原点在x轴方向上的平移位置量，ty表示右目相机坐标系原点相对于左目相机坐标系原点在y轴方向上的平移位置量，tz表示右目相机坐标系原点相对于左目相机坐标系原点在z轴方向上的平移位置量，r1、r2、...、r9分别表示左、右目相机的相对旋转关系的元素；
所述任务区域特征点在多轴数控装备末端坐标系中的坐标位置F2如公式(2)所示：
Figure FDA0003804356980000022
其中，
Figure FDA0003804356980000023
为双目视觉相机坐标系与多轴数控装备末端坐标系的相对旋转和平移位置关系。
3.如权利要求1所述的多轴数控装备的复合控制方法，其特征在于包括：根据各轴关节的转动角度位置Q＝[q1q2 … qn]计算出多轴数控装备末端坐标系与多轴数控装备基座标系之间的平移旋转关系F3的计算过程如公式(3)所示：
Figure FDA0003804356980000024
其中，
Figure FDA0003804356980000028
表示关节i-1和关节i之间的齐次坐标变换，1≤i≤n。
4.如权利要求1所述的多轴数控装备的复合控制方法，其特征在于包括：将区域特征点在机械臂末端坐标系中的坐标位置转换计算成区域特征点在机械臂基座标系中的坐标位置F4的计算过程如公式(4)所示：
F4＝F3·F2 (4)。
5.如权利要求1所述的多轴数控装备的复合控制方法，其特征在于：所述步骤3中，路径规划采用基于带有阻滞因子的引力场系数改进的加权最小范数法，通过将阻滞因子和引力场系数引入到最小范数法中，使多轴数控装备的关节运行至接近限位段时，受到抑制作用后减速防止进入限位段，然后通过引力场系数作用退回至常规运行段，整个过程无需切换权重因子。
6.如权利要求1或5所述的多轴数控装备的复合控制方法，其特征在于：所述步骤3中，路径规划具体步骤如下：
首先将多轴数控装备末端轨迹离散化，得到多轴数控装备末端参考位置离散出来的速度轨迹，然后由公式(5)将末端速度轨迹转化为各轴速度轨迹：
Figure FDA0003804356980000031
其中，右侧第一项为路径规划主导运动部分，第二项为抑制运动部分，第三项为引力场作用部分；q＝[q1，q2，…，qn]为多轴数控装备关节转动角度，
Figure FDA0003804356980000032
为关节速度，J为多轴数控装备雅克比矩阵，
Figure FDA0003804356980000033
为加权系数矩阵，
Figure FDA0003804356980000034
计算如式(6)所示，λ1I、λ2I、λ3I为防奇异因子，G(q)＝[g1，g2，…，gn]T为阻滞因子，gi计算如式(7)所示，Z＝[ζ1，ζ2，…，ζn]T为引力回归因子，ζi计算如式(8)所示：
Figure FDA0003804356980000035
Figure FDA0003804356980000036
Figure FDA0003804356980000037
其中，k1、k2、k3、k4、κg和εz为调整系数；
Figure FDA0003804356980000038
表示关节运行范围的负极限值，
Figure FDA0003804356980000039
表示关节运行范围的正极限值，
Figure FDA00038043569800000310
表示接近关节运行范围的负极限值的阈值，
Figure FDA00038043569800000311
表示接近关节运行范围的正极限值的阈值；
得出各轴速度轨迹后，按各轴伺服电机位置环控制采样周期进行累积，即得出各轴位置轨迹。
7.如权利要求1所述的多轴数控装备的复合控制方法，其特征在于：所述步骤6中，有限时间状态运算具体包括如下步骤：
步骤(6.1)、对多轴数控装备进行动力学建模，如公式(10)所示：
Figure FDA0003804356980000041
其中，M(QD)为多轴数控装备惯量矩阵，
Figure FDA0003804356980000042
为科氏力和离心力矩阵，G(QD)为重力矩阵，h(t)为外部扰动和高阶未建模干扰，τ(t)为控制力矩矩阵；
步骤(6.2)、将模型中的惯量矩阵分为名义惯量矩阵和动态惯量矩阵部分，然后将动态惯量矩阵、科氏力和离心力矩阵、重力矩阵、外部扰动和高阶未建模干扰统一视作总的时变干扰，将模型(10)改写为；
Figure FDA0003804356980000043
其中，Mn为名义惯量矩阵，
Figure FDA0003804356980000044
为总的时变干扰；
步骤(6.3)、将多轴数控装备的总的扰动看作一个新加状态，把模型(11)改为状态空间表达形式，如公式(12)所示：
Figure FDA0003804356980000045
其中，x0＝QD，x1＝Q′D，z1＝H(t)，…，zm-1＝H(m-1)(t)；
步骤(6.4)、设计有限时间观测器如式(13)所示，将所述多轴数控装备各轴关节的实时转动角度位置输入到有限时间扩展状态观测器中，估计出多轴数控装备未知时变干扰的状态及其各阶导数；
Figure FDA0003804356980000046
其中，
Figure FDA0003804356980000047
分别为x0，x1，z0，z1，…，zm-1的估计，k1，k2，…，km+2，β1，β2，…，βm+2为观测器调节系数。
8.如权利要求1所述的多轴数控装备的复合控制方法，其特征在于：所述步骤7中，所述控制力矩采用公式(14)计算：
Figure FDA0003804356980000051
其中，c0，c1，α1，α2为控制器调节系数。
9.如权利要求1所述的多轴数控装备的复合控制方法，其特征在于：所述步骤8中，所述加权运算的加权系数由自适应可变增益动态决定，自适应可变增益由参考位置和反馈位置的位置误差决定；所述位置误差大时，通过所述加权运算加强伺服电机反馈控制、减弱视觉前馈控制；所述位置误差小时，通过所述加权运算减弱伺服电机反馈控制、增强视觉前馈控制。
CN202110376222.3A 2021-04-07 2021-04-07 一种多轴数控装备的复合控制方法 Active CN113093549B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110376222.3A CN113093549B (zh) 2021-04-07 2021-04-07 一种多轴数控装备的复合控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110376222.3A CN113093549B (zh) 2021-04-07 2021-04-07 一种多轴数控装备的复合控制方法

Publications (2)

Publication Number Publication Date
CN113093549A CN113093549A (zh) 2021-07-09
CN113093549B true CN113093549B (zh) 2022-10-28

Family

ID=76675188

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110376222.3A Active CN113093549B (zh) 2021-04-07 2021-04-07 一种多轴数控装备的复合控制方法

Country Status (1)

Country Link
CN (1) CN113093549B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101733749A (zh) * 2009-12-22 2010-06-16 哈尔滨工业大学 空间机器人多领域统一建模与仿真系统
CN105345824A (zh) * 2015-12-21 2016-02-24 广东工业大学 一种视觉伺服机械臂的自适应磁滞消去装置
WO2016193781A1 (en) * 2015-05-29 2016-12-08 Benemérita Universidad Autónoma De Puebla Motion control system for a direct drive robot through visual servoing
CN108803324A (zh) * 2018-06-06 2018-11-13 黄山学院 多关节工业机械臂反步有限时间滑模控制方法
WO2020062499A1 (zh) * 2018-09-28 2020-04-02 东南大学 一种针对电子节气门系统的非线性抗干扰控制方法及装置
CN111360827A (zh) * 2020-03-06 2020-07-03 哈尔滨工业大学 一种视觉伺服切换控制方法及系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110238831B (zh) * 2019-07-23 2020-09-18 青岛理工大学 基于rgb-d图像及示教器的机器人示教系统及方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101733749A (zh) * 2009-12-22 2010-06-16 哈尔滨工业大学 空间机器人多领域统一建模与仿真系统
WO2016193781A1 (en) * 2015-05-29 2016-12-08 Benemérita Universidad Autónoma De Puebla Motion control system for a direct drive robot through visual servoing
CN105345824A (zh) * 2015-12-21 2016-02-24 广东工业大学 一种视觉伺服机械臂的自适应磁滞消去装置
CN108803324A (zh) * 2018-06-06 2018-11-13 黄山学院 多关节工业机械臂反步有限时间滑模控制方法
WO2020062499A1 (zh) * 2018-09-28 2020-04-02 东南大学 一种针对电子节气门系统的非线性抗干扰控制方法及装置
CN111360827A (zh) * 2020-03-06 2020-07-03 哈尔滨工业大学 一种视觉伺服切换控制方法及系统

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
D. Raina ; P. Mithun ; S. V. Shah ; S. Kumar.A Novel Image-based Path Planning Algorithm for Eye-in-Hand Visual Servoing of a Redundant Manipulator in a Human Centered Environment.《2019 28th IEEE International Conference on Robot and Human Interactive Communication (RO-MAN)》.2020, *
刘威 ; 陈进华 ; 张驰 ; 崔志琴.考虑轴间填充物的高速永磁电机转子强度分析.《电工技术学报》.2018, *
受限机械臂视觉伺服跟踪控制算法研究;张宇;《中国博士学位论文全文数据库 信息科技辑》;20200615;全文 *
基于双目视觉的枸杞采摘机器人系统设计与研究;丁雷鸣;《中国优秀硕士学位论文全文数据库 农业科技辑》;20200615;全文 *
基于预测控制的机械臂约束视觉伺服研究;邱周静子;《中国博士学位论文全文数据库 信息科技辑》;20200615;全文 *
运动控制系统的抗干扰控制理论与应用研究;王会明;《中国博士学位论文全文数据库 信息科技辑》;20171215;全文 *

Also Published As

Publication number Publication date
CN113093549A (zh) 2021-07-09

Similar Documents

Publication Publication Date Title
CN108628172B (zh) 一种基于扩张状态观测器的机械臂高精度运动控制方法
CN111546350B (zh) 一种多关节重载液压机器人系统及高精度运动控制方法
JP5327722B2 (ja) ロボットの負荷推定装置及び負荷推定方法
CN108453732B (zh) 控制体系封闭机器人自适应动态力/位置混合控制方法
JPS6010876B2 (ja) マニピュレ−タ及び工業用ロボットの制御方法
CN112417755A (zh) 一种主从式手术机器人轨迹预测控制方法
CN109648564B (zh) 一种基于递阶结构mpc的多自由度柔性关节机械臂系统的控制方法
CN113093549B (zh) 一种多轴数控装备的复合控制方法
JP6151857B2 (ja) 協働マニピュレータのアームのセグメントを制御する制御方法
CN113143465A (zh) 一种基于关节力矩的机械臂拖动引导定位的方法
Zheng et al. Simple online smooth trajectory generations for industrial systems
Herbin et al. The torque control system of exoskeleton ExoArm 7-DOF used in bilateral teleoperation system
Chen et al. Observer-based adaptive robust precision motion control of a multi-joint hydraulic manipulator
CN109773781B (zh) 一种无传感器的工业机器人直线软浮动控制方法
CN111673742A (zh) 一种工业机器人轨迹跟踪控制算法
Yakub et al. Comparative study on control method for two-mass systems
Ren et al. Joint torque control of a collaborative robot based on active disturbance rejection with the consideration of actuator delay
Wapenhans et al. Optimal trajectory planning with application to industrial robots
Zhu et al. Adaptive control of harmonic drives based on virtual decomposition
JP3599127B2 (ja) オブザーバ制御演算装置
CN113325711B (zh) 一种用于柔性机械臂预定精度定位的智能控制方法
Yen A two-loop robust controller for compensation of the variant friction force in an over-constrained parallel kinematic machine
Shiferaw et al. High performance control of Stewart platform manipulator using sliding mode control with synchronization error
US20230302637A1 (en) Systems and Hybrid Position Force Control Processes of an Industrial Robot
Ito et al. Robust decoupling control for articulated robot

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant