一种机器人的关节控制系统

摘要

本发明公开了一种机器人的关节控制系统。具体地，该机器人具有机械臂，机械臂包含若干个关节模组和机械臂末端，每一关节模组设置有对应的编码器和驱动电机，机械臂末端设置有末端执行器和多维力传感器，该关节控制系统包括：状态反馈模块、目标输入模块以及位控制模块。从而，本申请所述的机器人的关节控制系统采用PID控制和虚拟分解控制组合的控制手段，并基于运动学模型进行动态力补偿，能在极大降低运算量的同时提高系统的带宽，进而提高机器人的控制精度。

权利要求

1.一种机器人的关节控制系统，其特征在于，该机器人具有机械臂，机械臂包含若干个关节模组和机械臂末端，每一关节模组设置有对应的编码器和驱动电机，机械臂末端设置有末端执行器和多维力传感器，该关节控制系统包括：状态反馈模块，用于获取各关节模组的编码器反馈的关节位置数据；

目标输入模块，用于获取外部输入的机械臂末端的末端目标位置；

位控制模块，包括并联设置的PID控制单元和虚拟分解控制单元，其中：PID控制单元用于根据机械臂末端的末端目标位置分别计算各关节模组的关节目标位置、并根据各关节模组的关节目标位置以及各关节模组的关节位置数据分别计算各关节模组的第一驱动电流；

虚拟分解控制单元用于根据机械臂末端的末端目标位置分别计算各关节模组的关节目标位置、并根据各关节模组的关节目标位置以及各关节模组的关节位置数据分别计算各关节模组的第二驱动电流；

并且，PID控制单元和虚拟分解控制单元计算得到的每个关节模组的第一驱动电流和第二驱动电流叠加后输出至该关节模组的驱动电机。

2.根据权利要求1所述的关节控制系统，其特征在于，位控制模块包括分别对应于每个关节的一对并联设置的PID控制单元和虚拟分解控制单元。

3.根据权利要求1所述的关节控制系统，其特征在于，每个关节模组包括关节和连杆，并且虚拟分解控制单元包括：

模型创建子单元，用于为每个关节模组创建运动学模型；

参数设定子单元，用于设定每个关节模组的运动学模型的模型参数；

力矩计算子单元，用于调用各关节模组的运动学模型、并以各关节模组的关节位置数据为输入计算各关节模组对应的力矩；

电流计算子单元，用于将各关节模组对应的力矩转换为各关节模组对应的第二驱动电流。

4.根据权利要求1所述的关节控制系统，其特征在于，所述关节控制系统进一步包括导纳控制模块，用于以末端执行器与外界的接触力对末端目标位置进行偏差补偿。

5.根据权利要求4所述的关节控制系统，其特征在于，所述导纳控制模块进一步用于获取多维力传感器的末端受力数据、并根据末端受力数据对输入至PID控制单元和虚拟分解控制单元的末端目标位置进行补偿。

说明书

一种机器人的关节控制系统

技术领域

[0001] 本发明涉及机器人控制领域，特别涉及一种机器人的关节控制系统。

背景技术

[0002] 现有机器人控制技术通常在关节进行PID(比例(proportion)、积分(integral)、微分(differential))控制，同时基于模型对重力、关节摩擦力进行补偿，这样的传统控制
方法的控制带宽较低，动态性能因此受限。

[0003] 同时现有的大部分机器人不进行力控制。少数机器人基于关节力矩传感器进行力控制，力控制精度较差。

发明内容

[0004] 有鉴于此，本发明提供了一种机器人的关节控制系统，具有更精准的关节控制和更高的控制带宽，从而实现高精度的机器人控制，满足更精细化的工业加工需求。

[0005] 本申请的一个实施例提供了一种机器人的关节控制系统，该机器人具有机械臂，机械臂包含若干个关节模组和机械臂末端，每一关节模组设置有对应的编码器和驱动电
机，机械臂末端设置有末端执行器和多维力传感器，该关节控制系统包括：

[0006] 状态反馈模块，用于获取各关节模组的编码器反馈的关节位置数据；

[0007] 目标输入模块，用于获取外部输入的机械臂末端的末端目标位置；

[0008] 位控制模块，包括并联设置的PID控制单元和虚拟分解控制单元，其中：

[0009] PID控制单元用于根据机械臂末端的末端目标位置分别计算各关节模组的关节目标位置、并根据各关节模组的关节目标位置以及各关节模组的关节位置数据分别计算各关
节模组的第一驱动电流；

[0010] 虚拟分解控制单元用于根据机械臂末端的末端目标位置分别计算各关节模组的关节目标位置、并根据各关节模组的关节目标位置以及各关节模组的关节位置数据分别计
算各关节模组的第二驱动电流；

[0011] 并且，PID控制单元和虚拟分解控制单元计算得到的每个关节模组的第一驱动电流和第二驱动电流叠加后输出至该关节模组的驱动电机。

[0012] 可选地，位控制模块包括分别对应于每个关节的一对并联设置的PID控制单元和虚拟分解控制单元。

[0013] 可选地，每个关节模组包括关节和连杆，并且虚拟分解控制单元包括：

[0014] 模型创建子单元，用于为每个关节模组创建运动学模型；

[0015] 参数设定子单元，用于设定每个关节模组的运动学模型的模型参数；

[0016] 力矩计算子单元，用于调用各关节模组的运动学模型、并以各关节模组的关节位置数据为输入计算各关节模组对应的力矩；

[0017] 电流计算子单元，用于将各关节模组对应的力矩转换为各关节模组对应的第二驱动电流。

[0018] 可选地，运动学模型以运动学方程和动力学方程为约束条件；

[0019] 其中，所述运动学方程为：

[0020]

[0021]

[0022]

[0023]

[0024] 其中，i∈{1,2,…,n}，ri表示任务空间，Vri为任务空间的速度， vri为任务空间的线速度，ωri为任务空间的角速度；

[0025] 表示与任务空间不同的坐标系；

[0026] σi和 代表关节i的类型，且移动关节的σi＝1， 旋转关节的σi＝0， z3T T
＝[0，0，1，0，0，0]、z6＝[0，0，0，0,0，1]代表关节的驱动轴；

[0027] 所述动力学方程为：

[0028]

[0029]

[0030]

[0031] 其中，i∈{n，n‑1，…，1}，*表示合力，Fri为任务空间中刚体i受到的合力，fri为任务空间中的力，mri为任务空间中的力矩；

[0032] 分别代表关节位置、速度和加速度向量； 分别代表任务空间的位姿、速度和加速度向量；雅克比矩阵为 则

[0033] 代表关节空间惯量矩阵， 代表哥氏力和离心力矩阵，代表重力项向量；

[0034] 代表任务空间惯量矩阵， 代表哥氏力和离心力矩阵，代表重力项向量，且

[0035] 为θi的估计值， 为正对角矩阵；

[0036] 为惯性参数向量，k为惯性参数个数； 分别任务空间和关节空间惯性参数的回归矩阵，且

[0037] 可选地，运动学方程的初始条件为基座的速度和加速度为0，即且

[0038] 可选地，动力学方程的初始条件为末端执行器的所受外力，即

[0039] 可选地，运动学模型子单元进一步确定任务空间的参考速度为 以及关节空间的参考速度为

[0040] 可选地，参数设定子单元进一步利用确定各关节模组之间的自适应律，并利用确定的自适应律方程补偿各关节模组的动态力，其中，所述自适应律方程为：

[0041]

[0042] 其中，Γ为正定矩阵。

[0043] 可选地，所述关节控制系统进一步包括导纳控制模块，用于以末端执行器与外界的接触力对末端目标位置进行偏差补偿。

[0044] 可选地，所述导纳控制模块进一步用于获取多维力传感器的末端受力数据、并根据末端受力数据对输入至PID控制单元和虚拟分解控制单元的末端目标位置进行补偿。

[0045] 由上可知，本申请所述的机器人的关节控制系统采用PID控制和虚拟分解控制组合的控制手段，并基于运动学模型进行动态力补偿，能在极大降低运算量的同时提高系统
的带宽，进而提高机器人的控制精度。

附图说明

[0046] 以下附图仅对本发明做示意性说明和解释，并不限定本发明的范围。

[0047] 图1为本申请一个实施例的关节控制系统的控制原理图；

[0048] 图2为本申请一个实施例的关节控制系统的结构示意图；

[0049] 图3为本申请一个实施例的导纳控制模块的控制原理图；

[0050] 图4为本申请一个实施例的导纳控制配合关节控制的控制原理图；

[0051] 图5为本申请一个实施例的导纳控制配合关节控制的结构示意图。

[0052] 标号说明

[0053] 100  机器人

[0054] 101  编码器

[0055] 102  多维力传感器

[0056] 110  状态反馈模块

[0057] 120  目标输入模块

[0058] 130  位控制模块

[0059] 131  模型创建子单元

[0060] 132  参数设定子单元

[0061] 133  力矩计算子单元

[0062] 134  电流计算子单元

[0063] 140  导纳控制模块

具体实施方式

[0064] 为了对发明的技术特征、目的和效果有更加清楚的理解，现对照附图说明本发明的具体实施方式，在各图中相同的标号表示相同的部分。

[0065] 在本文中，“示意性”表示“充当实例、例子或说明”，不应将在本文中被描述为“示意性”的任何图示、实施方式解释为一种更优选的或更具优点的技术方案。

[0066] 为使图面简洁，各图中的只示意性地表示出了与本发明相关部分，而并不代表其作为产品的实际结构。另外，以使图面简洁便于理解，在有些图中具有相同结构或功能的部
件，仅示意性地绘示了其中的一个，或仅标出了其中的一个。

[0067] 在本文中，“第一”、“第二”等仅用于彼此的区分，而非表示重要程度及顺序、以及互为存在的前提等。

[0068] 为使本发明的目的、技术方案及优点更加清楚明白，以下参照附图并举实施例，对本发明进一步详细说明。

[0069] 图1为本申请一个实施例的关节控制系统的控制原理图；图2为本申请一个实施例的关节控制系统的结构示意图。

[0070] 请参见图1，本申请的一个实施例提供了一种机器人的关节控制系统，该机器人100具有机械臂，机械臂包含若干个关节模组和机械臂末端，每一关节模组设置有对应的编
码器101和驱动电机，机械臂末端设置有末端执行器和多维力传感器102，该关节控制系统
包括：状态反馈模块110、目标输入模块120以及位控制模块130。

[0071] 其中，状态反馈模块110用于获取各关节模组的编码器101反馈的关节位置数据；目标输入模块120用于获取外部输入的机械臂末端的末端目标位置，目标输入模块120的输
入数据来源可以为操作者的指令和多维力传感器102的信号；位控制模块130包括并联设置
的PID控制单元和虚拟分解控制单元，

[0072] PID控制单元用于根据机械臂末端的末端目标位置分别计算各关节模组的关节目标位置、并根据各关节模组的关节目标位置以及各关节模组的关节位置数据分别计算各关
节模组的第一驱动电流；

[0073] 虚拟分解控制单元用于根据机械臂末端的末端目标位置分别计算各关节模组的关节目标位置、并根据各关节模组的关节目标位置以及各关节模组的关节位置数据分别计
算各关节模组的第二驱动电流；

[0074] 并且，PID控制单元和虚拟分解控制单元计算得到的每个关节模组的第一驱动电流和第二驱动电流叠加后输出至该关节模组的驱动电机。

[0075] VDC(虚拟分解控制)是较新颖的复杂控制方法且难以调试；PID是传统的控制方法，调试方法成熟简单。本申请通过将VDC中与PID在数学上等价的部分单独提取并进行调
试，通过这一方法实现了和原有VDC的相同的效果，同时极大的降低了调试的难度。其中，所
谓在数学上等价的部分指的是VDC中包含的等同于PID中积分环节的项，因此在进行PID设
计的时候可以不设计I环节(积分环节)。

[0076] PID的参数具有实际的物理意义，这样的物理意义在原本的VDC中是无法体现的，经过这一简化之后，就将参数与机器人的物理表现对应起来，详细来说就是将关节视为一
个质量‑弹簧‑阻尼系统，速度控制的P对应的是阻尼效果，D对应的是质量(惯性)的效果，I
对应的是弹簧(刚度)的效果。

[0077] 从而，使用VDC可以节省运算量，同时可以补偿所有的动态力(包括惯性力、旋转引起的陀螺效应、负载重力以及机器人本体重力)，因而机器人的响应会更快。

[0078] 为了分别计算每个关节模组的驱动电机的驱动电流，位控制模块130包括分别对应于每个关节的一对并联设置的PID控制单元和虚拟分解控制单元。

[0079] 请参见图2，每个关节模组包括关节和连杆，并且虚拟分解控制单元包括：

[0080] 模型创建子单元131，用于为每个关节模组创建运动学模型；

[0081] 参数设定子单元132，用于设定每个关节模组的运动学模型的模型参数；

[0082] 力矩计算子单元133，用于调用各关节模组的运动学模型、并以各关节模组的关节位置数据为输入计算各关节模组对应的力矩；

[0083] 电流计算子单元134，用于将各关节模组对应的力矩转换为各关节模组对应的第二驱动电流。

[0084] 进一步地，运动学模型以运动学方程和动力学方程为约束条件；

[0085] 其中，运动学方程为：

[0086]

[0087]

[0088]

[0089]

[0090] 其中，i∈{1，2，…，n}，ri表示任务空间，Vri为任务空间的速度， vri为任务空间的线速度，ωri为任务空间的角速度；

[0091] 表示与任务空间不同的坐标系；

[0092] σi和 代表关节i的类型，且移动关节的σi＝1, 旋转关节的σi＝0，T T
z3＝[0，0，1，0，0，0]、z6＝[0,0，0，0，0，1]代表关节的驱动轴；

[0093] 动力学方程为：

[0094]

[0095]

[0096]

[0097] 其中，i∈{n，n‑1,…,1}，*表示合力，Fri为任务空间中刚体i受到的合力，fri为任务空间中的力，mri为任务空间中的力矩；

[0098] 分别代表关节位置、速度和加速度向量； 分别代表任务空间的位姿、速度和加速度向量；雅克比矩阵为 则

[0099] 代表关节空间惯量矩阵， 代表哥氏力和离心力矩阵，代表重力项向量；

[0100] 代表任务空间惯量矩阵， 代表哥氏力和离心力矩阵，代表重力项向量，且

[0101] 为θi的估计值， 为正对角矩阵；

[0102] 为惯性参数向量，k为惯性参数个数； 分别任务空间和关节空间惯性参数的回归矩阵，且

[0103] 进一步地，运动学方程的初始条件为基座的速度和加速度为0，即且

[0104] 进一步地，动力学方程的初始条件为末端执行器的所受外力，即

[0105] 进一步地，运动学模型子单元进一步确定任务空间的参考速度为 以及关节空间的参考速度为

[0106] 为了增强关节控制系统的控制效果，参数设定子单元132进一步利用确定各关节模组之间的自适应律，并利用确定的自适应律方程补偿各关节模组的动态力，其中，自适应
律方程为：

[0107]

[0108] 其中，Γ为正定矩阵。

[0109] 进一步地，关节模组的输出力矩方程为：

[0110]

[0111] 其中， 分别代表关节的驱动力向量和在任务空间的表达，且τ＝T
JFu；

[0112] 分别代表末端执行器作用于环境的接触力向量在关节空间和任T
务空间的表达，且τe＝JFe。

[0113] 自适应律可以实时的辨识出模型参数，并可利用这些参数使得控制效果达到最优化，进而控制相应速度更快。

[0114] 图3为本申请一个实施例的导纳控制模块的控制原理图；图4为本申请一个实施例的导纳控制配合关节控制的控制原理图；图5为本申请一个实施例的导纳控制配合关节控
制的结构示意图。

[0115] 请参见图3，关节控制系统进一步包括导纳控制模块140，用于以末端执行器与外界的接触力对末端目标位置进行偏差补偿。

[0116] 请参见图4和图5，导纳控制模块进一步用于获取多维力传感器102的末端受力数据、并根据末端受力数据对输入至PID控制单元和虚拟分解控制单元的末端目标位置进行
补偿。

[0117] 导纳控制的控制目标是执行器末端，通过这样的方法可以将导纳控制模块和关节控制系统相互独立。在需要的情况下，具备直接迁移至其他机器人控制程序的优势。

[0118] 本实施例同时对导纳控制进行了简化，将导纳控制模块中位置和加速度的部分去掉，简化了调试的难度。

[0119] 可以理解的是，由于机器人的关节机构位置与速度、加速度之间存在换算关系，所以，本申请并不排斥其他针对速度、加速度进行补偿的情况。

[0120] 机器人在本实施例提供的关节控制系统控制下，可以实现的模式包括：

[0121] 位置保持模式，即，使机器人保持现有的位置且速度保持为零；

[0122] 单关节位置命令模式，即，给机械臂关节角度指令，使得其运动到指定构型；

[0123] 单关节速度命令模式，即，给定关节的速度指令，经过正弦加速至指令速度并保持；

[0124] 任务空间(笛卡尔空间)末端位置命令模式，即，为了实现任务空间(笛卡尔空间)的轨迹运动；

[0125] 任务空间(笛卡尔空间)末端速度命令模式，即，给机械臂末端6维速度指令，规划出按正弦加速至指令速度并保持的过程，然后使用逆运动学进行解算，将对末端速度的规
划转换为对关节速度的指令，实现运动；

[0126] 拖动示教模式，即，基于力传感器的反馈实现末端拖动，即使用力传感器读取操作人员施加在机器人末端的力/力矩，并根据读数赋予机器人相应的速度指令，使其产生近似
于直接被操作人员拖动的运动效果；

[0127] 零力模式，即，在对机械臂的重力、关节间摩擦力、动态力进行补偿后，实现机械臂近似于处于无重力的状态；

[0128] 任务空间(笛卡尔空间)末端位置模式&力控模式，即，在固定的维度实现OCPM运动的同时，通过力传感器的反馈和力控制的算法使得机器人末端与环境接触的力/力矩快速
向期望的指令值变化并保持稳定；

[0129] 任务空间(笛卡尔空间)末端速度模式&力控模式，即，在固定的维度实现MAM运动的同时，通过力传感器的反馈和力控制的算法使得机器人末端与环境接触的力/力矩快速
向期望的指令值变化并保持稳定。

[0130] 其中，在拖动示教模式下，导纳控制模块对末端执行器的末端目标位置进行位置补偿以响应于末端执行器的外界拖拽力。其中，末端拖动模式为末端执行器120收到外界拖
拽力的情况。基于多维力传感器感测到外界拖拽力，并通过导纳控制模块为控制装置提供
关节模组的关节目标位置，以使关节模组产生位移分量并带动末端执行器移动。

[0131] 进一步地，关节模组的位置使用上述的PID控制单元和虚拟分解控制单元进行补偿计算。

[0132] 上文所列出的一系列的详细说明仅仅是针对本发明的可行性实施方式的具体说明，而并非用以限制本发明的保护范围，凡未脱离本发明技艺精神所作的等效实施方案或
变更，如特征的组合、分割或重复，均应包含在本发明的保护范围之内。