一种机械臂空间轨迹跟踪动态补偿方法和系统技术方案

本申请实施例提供了一种机械臂空间轨迹跟踪动态补偿方法和系统，其中，所述方法步骤包括：S1、根据机械臂末端期望位姿矩阵T1和机械臂末端当前位姿矩阵T0，计算机械臂末端位姿误差δ；S2、利用当前机械臂末端位姿下的雅克比矩阵J(q)、位姿误差δ和当前关节角q，计算关节目标角q*；S3、根据当前机械臂末端位姿下的雅克比矩阵J(q)和机械臂末端的期望速度v*，计算关节目标角速度

A Dynamic Compensation Method and System for Space Trajectory Tracking of Manipulator

The embodiment of this application provides a dynamic compensation method and system for space trajectory tracking of a manipulator, in which the steps of the method include: S1, calculating the position and attitude error of the end of the manipulator based on the expected position and attitude matrix T1 of the end of the manipulator and the current position and attitude matrix T0 of the end of the manipulator; S2, using the Jacobian matrix J (q), the position and attitude error Delta and the current joint angle under the current position and attitude of the end of the manipulator. Q, calculating the joint target angle q*; S3, calculating the joint target angular velocity according to the Jacobian matrix J (q) and the expected velocity v*, under the current position and posture of the end of the manipulator.

【技术实现步骤摘要】
一种机械臂空间轨迹跟踪动态补偿方法和系统
本申请涉及机械臂空间轨迹跟踪补偿领域，特别涉及一种六自由度机械臂空间轨迹跟踪动态补偿方法和系统。
技术介绍
机械臂尤其是六自由度机械臂在工业生产领域已经得到了广泛的应用，机械臂控制技术是机械臂系统的重要部分，决定着机械臂的性能，随着现代工业的迅速发展，尤其是“工业4.0”、“中国制造2025”等战略的提出，对机械臂的精度、稳定性、快速性提出了更高的要求。机械臂空间轨迹跟踪是实现末端执行器沿着给定的连续轨迹运动，整个运动轨迹都需要严格控制，机械臂空间轨迹跟踪精度是其控制性能的重要指标。弧焊、喷漆等应用都对机械臂的空间轨迹跟踪精度有很高的要求。现有技术中，针对机械臂空间轨迹跟踪控制，一般采用两种方法：第一种方法是在关节控制中直接将关节目标角差分得到目标速度，作为速度前馈给到关节速度控制器进行动态补偿，这种方法实施简单但是会存在滞后特性；第二种方法是根据机械臂的动力学模型进行控制，但是六自由度机械臂的动力学模型复杂，难以得到精确的动力学模型，导致控制效果不理想。
技术实现思路
为解决上述问题之一，本申请提供了一种六自由度机械臂空间轨迹跟踪动态补偿方法和系统。根据本申请实施例的第一个方面，提供了一种机械臂空间轨迹跟踪动态补偿方法，其特征在于，该方法的步骤包括：S1、根据机械臂末端期望位姿矩阵T1和机械臂末端当前位姿矩阵T0，计算机械臂末端位姿误差δ；S2、利用当前机械臂末端位姿下的雅克比矩阵J(q)、位姿误差δ和当前关节角q，计算关节目标角q*；S3、根据当前机械臂末端位姿下的雅克比矩阵J(q)和机械臂末端的期望速度v*，计算关节目标角速度S4、将关节目标角速度与关节位置控制器输出量相加，得到关节速度指令，驱动机械臂各关节运动。根据本申请实施例的第二个方面，提供了一种机械臂空间轨迹跟踪动态补偿系统，其特征在于，该系统包括：位姿误差计算模块，根据机械臂末端期望位姿矩阵T1和机械臂末端当前位姿矩阵T0，计算机械臂末端位姿误差δ；关节目标角度计算模块，利用当前机械臂末端位姿下的雅克比矩阵J(q)、位姿误差δ和当前关节角q，计算关节目标角q*；关节目标角速度计算模块，根据当前机械臂末端位姿下的雅克比矩阵J(q)和机械臂末端的期望速度v*，计算关节目标角速度控制模块，将关节目标角速度与关节位置控制器输出量相加，得到关节速度指令，驱动机械臂各关节运动。本申请所述技术方案计算复杂度低，占用资源少；不需要考虑复杂的动力学模型，简单实用，易于实施；通过末端轨迹规划输出的空间速度直接得到各关节目标速度作为前馈量，不存在滞后，实时性好。附图说明此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：图1示出本方案所述一种机械臂空间轨迹跟踪动态补偿方法的示意图；图2示出本方案所述一种机械臂空间轨迹跟踪动态补偿方法的控制原理图。具体实施方式为了使本申请实施例中的技术方案及优点更加清楚明白，以下结合附图对本申请的示例性实施例进行进一步详细的说明，显然，所描述的实施例仅是本申请的一部分实施例，而不是所有实施例的穷举。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。本方案的核心思路是通过末端轨迹规划输出的空间速度直接得到各关节目标速度作为前馈量，不存在滞后，实时性好。如图1和图2所示，本方案公开了一种机械臂空间轨迹跟踪动态补偿方法，该方法不需考虑六自由度机械臂的动力学模型，能够简单有效的实施六自由度机械臂空间轨迹的跟踪动态补偿。该方法的步骤具体包括：步骤一、根据六自由度机械臂的D-H参数构建其运动学模型，并根据当前关节角q计算出机械臂末端当前位姿矩阵，记为T0；步骤二、根据六自由度机械臂当前关节角与运动学模型计算末端在当前位姿下的雅可比矩阵，记为J(q)；步骤三、根据轨迹规划器输出的机械臂末端期望位置，得到机械臂末端期望位姿矩阵，记为T1；步骤四、计算机械臂末端期望位姿矩阵T1与机械臂末端当前位姿矩阵T0的位姿误差，记为δ；所述位姿误差的计算公式为：其中，为位姿误差，为机械臂末端当前位姿矩阵，为机械臂末端期望位姿矩阵。为机械臂末端当前姿态的旋转矩阵，为机械臂末端当前空间位置坐标向量，为机械臂末端期望姿态的旋转矩阵，为机械臂末端期望空间位置的坐标向量。对于斜对称矩阵步骤五、根据当前位姿下的雅可比矩阵J(q)，位姿误差δ，当前关节角q，计算得到关节目标角q*，给到关节位置控制器；所述机械臂各关节目标角度计算公式为：q*＝q+J-1(q)δ其中为机械臂各关节目标角度向量，为机械臂各关节当前角度向量，为机械臂当前位姿下的雅可比矩阵。步骤六、根据当前位姿下的雅可比矩阵J(q)，轨迹规划器输出的机械臂末端期望速度v*，计算得到关节目标角速度所述机械臂各关节目标角速度公式为：其中为机械臂各关节目标角速度向量，为轨迹规划器输出的机械臂末端空间速度向量。步骤七、关节位置控制器的输出与关节目标角速度相加，得到关节速度指令，驱动机械臂各关节运动。本方案进一步公开了一种机械臂空间轨迹跟踪动态补偿系统，该系统包括：矩阵构建模块，根据利用机械臂D-H参数构建的运动学模型和当前关节角q，计算机械臂末端当前位姿矩阵T0；和当前位置下的雅克比矩阵J(q)；位姿误差计算模块，根据机械臂末端期望位姿矩阵T1和机械臂末端当前位姿矩阵T0，计算机械臂末端位姿误差δ；关节目标角度计算模块，利用当前机械臂末端位姿下的雅克比矩阵J(q)、位姿误差δ和当前关节角q，计算关节目标角q*；关节目标角速度计算模块，根据当前机械臂末端位姿下的雅克比矩阵J(q)和机械臂末端的期望速度v*，计算关节目标角速度控制模块，将关节目标角速度与关节位置控制器输出量相加，得到关节速度指令，驱动机械臂各关节运动。本方案中，所述机械臂空间轨迹跟踪动态补偿方法也可以通过例如关节位置控制器等电子设备实现其控制功能，所述电子设备包括：存储器，一个或多个处理器；存储器与处理器通过通信总线相连；处理器被配置为执行存储器中的指令；所述存储介质中存储有用于执行如上所述方法中各个步骤的指令。本方案中，所述机械臂空间轨迹跟踪动态补偿方法也可以记载于计算机可读存储介质中，通过计算机可读存储介质上存储有计算机程序实现控制功能，该程序被处理器执行时实现如上所述方法的步骤。综上所述，本方案计算复杂度低，占用资源少；不需要考虑复杂的动力学模型，简单实用，易于实施；通过末端轨迹规划输出的空间速度直接得到各关节目标速度作为前馈量，不存在滞后，实时性好。本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以本文档来自技高网...

【技术保护点】
1.一种机械臂空间轨迹跟踪动态补偿方法，其特征在于，该方法的步骤包括：S1、根据机械臂末端期望位姿矩阵T1和机械臂末端当前位姿矩阵T0，计算机械臂末端位姿误差δ；S2、利用当前机械臂末端位姿下的雅克比矩阵J(q)、位姿误差δ和当前关节角q，计算关节目标角q*；S3、根据当前机械臂末端位姿下的雅克比矩阵J(q)和机械臂末端的期望速度v*，计算关节目标角速度

【技术特征摘要】
1.一种机械臂空间轨迹跟踪动态补偿方法，其特征在于，该方法的步骤包括：S1、根据机械臂末端期望位姿矩阵T1和机械臂末端当前位姿矩阵T0，计算机械臂末端位姿误差δ；S2、利用当前机械臂末端位姿下的雅克比矩阵J(q)、位姿误差δ和当前关节角q，计算关节目标角q*；S3、根据当前机械臂末端位姿下的雅克比矩阵J(q)和机械臂末端的期望速度v*，计算关节目标角速度S4、将关节目标角速度与关节位置控制器输出量相加，得到关节速度指令，驱动机械臂各关节运动。2.根据权利要求1所述的补偿方法，其特征在于，所述步骤S1中，根据利用机械臂D-H参数构建的运动学模型和当前关节角q，计算机械臂末端当前位姿矩阵T0；和当前位置下的雅克比矩阵J(q)。3.根据权利要求1所述的补偿方法，其特征在于，所述步骤S1中的机械臂末端位姿误差δ为：其中，为位姿误差，为机械臂末端当前位姿矩阵，为机械臂末端期望位姿矩阵。为机械臂末端当前姿态的旋转矩阵，为机械臂末端当前空间位置坐标向量，为机械臂末端期望姿态的旋转矩阵，为机械臂末端期望空间位置的坐标向量；对于斜对称矩阵4.根据权利要求1所述的补偿方法，其特征在于，所述步骤S2中的机械臂关节目标角q*为：q*＝q+J-1(q)δ其中，为机械臂各关节目标角度向量，为机械臂各关节当前角度向量，为机械臂当前位姿下的雅可比矩阵。5.根据权利要求1所述的补偿方法，其特征在于，所述步骤S3中的机械臂关节目标角速度为：其中，为机械臂各关节目标角速度向量，为轨迹规划器输出的机械臂末端空间速度向量。6.一种机械臂空...

【专利技术属性】
技术研发人员：韩华涛，高景一，李竹奇，吕泽杉，李源，耿金鹏，吴磊，李冬伍，魏彦彪，
申请(专利权)人：北京无线电测量研究所，
类型：发明
国别省市：北京,11