CN112045687A - 用于操作机械臂的动力学计算方法及机械臂的中央控制方法和系统 - Google Patents

Abstract

本发明公开了一种用于操作机械臂的动力学计算方法及机械臂的中央控制方法和系统，该机械臂动力学计算方法包括：以关节转角作为广义坐标，通过递推的方式计算所述机械臂上各连杆坐标系的线加速度及角加速度；计算所述机械臂的整体高斯拘束量及所述整体高斯拘束量对各个关节的关节角加速度的一阶偏导数；利用高斯最小拘束原理计算关节角加速度，积分计算关节速度及关节角度；计算关节力矩；计算系统动量、系统能量；基于动力学计算结果在中央控制器中实现动力学仿真、重力补偿、位置控制、力控制、碰撞检测等功能。本发明有助于降低机械臂的动力学问题计算中所需的计算量，提高计算结果的精确度，并能够适应新型机械臂的操作对中央控制器的需求。

Description

用于操作机械臂的动力学计算方法及机械臂的中央控制方法 和系统

技术领域

本发明涉及机械臂或机械臂中央控制系统的技术领域，尤其涉及一种用于操作机械臂的机械臂动力学计算方法、机械臂的中央控制方法和系统。

背景技术

操作机械臂的动力学仿真、重力补偿、位置控制、力控制和碰撞检测等功能都需要进行动力学计算。机械臂的仿真及碰撞检测等功能需要通过对动力学正问题进行计算以获得系统的加速度、动能、动量，因而要求动力学计算方法具有长时间的稳定性及较高的计算精度。机械臂的重力补偿、位置控制、力控制等功能需要对动力学反问题进行计算以获得关节力矩，要求动力学计算方法具有较高的计算效率以及较高的实时性能。完整的动力学计算流程包括数据输入、建立动力学方程、求解动力学方程、结果输出等步骤。

在对机械臂的动力学问题进行建模时，最常用的方法包括拉格朗日法以及牛顿-欧拉法。基于拉格朗日法的计算方法取关节转角作为广义坐标，利用第二类拉格朗日方程推导得到系统的动力学方程。拉格朗日法建模得到的动力学方程中，通过计算惯性力项、广义力项并求和后可用于计算关节力矩以完成动力学反问题的计算，对质量阵求逆后可利用动力学模型计算关节角加速度，进行积分后可获得系统的速度及位置，从而完成动力学正问题的计算。对于常见的6自由度机械臂，基于拉格朗日法的计算方法得到的是高度非线性且冗长的动力学方程，在实际使用时需要根据具体的机械臂构型推导动力学方程，对于不同类型的机械臂建模得到的是不同的动力学方程，此方法建模难度大，计算效率差。只有在针对特定型号的机械臂进行开发的机器人控制器中才能使用此方法作为动力学问题的计算方法，不能在通用型机器人中央控制器中作为建模方法进行动力学计算。

基于牛顿-欧拉法的计算方法通过正向递推计算机械臂上各连杆的质心速度、加速度，通过逆向递推计算关节力矩。牛顿-欧拉法建模得到的模型可以直接用于机械臂动力学反问题的计算，通过多次调用牛顿-欧拉法可以间接计算动力学方程中的质量阵，从而计算关节角加速度以完成动力学正问题的计算。基于牛顿-欧拉法的计算方法需要进行正向递推及反向递推，递推过程会不断累积舍入误差，从而导致计算结果的精确度较差。对于动力学正问题，基于牛顿-欧拉法的计算方法由于需要通过多次递推计算质量阵，其计算效率差，且计算结果仅能对短时间的动力学行为进行预测，不能用于长时间的动力学仿真。通常，机器人控制器中仅使用该方法进行动力学反问题的计算，而不能同时具有计算动力学正问题的功能，无法为基于模型的位置控制、力控制及碰撞检测等功能提供必要的支持。

现有机器人控制器中仅对动力学计算功能进行直接集成，只能针对特定任务设计动力学问题计算方案，没有通用的软件架构，控制器的整体架构不会根据动力学模块进行优化。无法实现动力学计算的模块化、通用化，现有动力学计算方法无法处理同时涉及多种动力学问题计算需求的情形，导致机器人控制器的功能单一、性能不佳，不能适应新型机械臂的操作对中央控制器的需求。

因此，亟需设计一种新的用于操作机械臂的机械臂动力学计算方法，并基于新的动力学计算方法设计中央控制方法和系统，以至少部分解决现有技术存在的上述技术问题。

发明内容

本发明要解决的技术问题是为了克服现有的机械臂系统中涉及动力学计算功能的方法无法兼顾多种动力学问题的计算需求因而限制了相关机械臂及机器人控制系统的功能和性能的缺陷，提出一种新的用于操作机械臂的机械臂动力学计算方法、机械臂的中央控制方法和中央控制系统。

本发明是通过采用下述技术方案来解决上述技术问题的：

本发明提供了一种用于操作机械臂的机械臂动力学计算方法，其特点在于，所述机械臂包括多个连杆及连接所述连杆的多个关节，其中基于高斯原理通过递推计算各个关节及连杆的相关数据，所述机械臂动力学计算方法包括以下步骤：

步骤S1、以关节转角作为广义坐标，通过递推的方式计算所述机械臂上各连杆坐标系的线加速度及角加速度；

步骤S2、判断各连杆的拘束量是否需要对各个关节的关节角加速度进行求导操作，若是则利用前一关节的关节角加速度的求导结果进行递推计算，若否则赋零；

步骤S3、计算所述机械臂的整体高斯拘束量及所述整体高斯拘束量对各个关节的关节角加速度的一阶偏导数；

步骤S4、通过求解基于高斯最小拘束原理的极值问题计算得到各个关节的关节角加速度，并积分得到关节速度及关节角度；

步骤S5、计算各个关节处的关节力矩；

步骤S6、计算所述机械臂的系统动量、系统能量。

其中，应理解的是，在此所称的各个关节及连杆的相关数据，涉及本文中所描述的和机械臂的关节及连杆相关联的一系列数据，诸如连杆线加速度、连杆角加速度、关节角加速度、关节角速度、关节角度、关节力矩、高斯拘束量及其偏导数。

根据本发明的一种实施方式，在所述步骤S1中，根据下式(1)计算各连杆坐标系的角加速度：

根据下式(1)计算各连杆坐标系的线加速度：

其中，ⁱx表示变量x在坐标系{i}中的分量，右下标为关节的编号x_i，表示在第i个关节/连杆对应的变量x，ⁱω_j是第j个连杆的角速度在坐标系{i}中的分量，

是第j个连杆的角加速度在坐标系{i}中的分量，θ_i是第i个关节的转角，ⁱR_j是从坐标系{j}至坐标系{i}进行变换的方向余弦阵，

是连杆坐标系{j}的原点加速度在坐标系{i}中的分量，^i-1P_i是连杆坐标系{i}的原点在坐标系{i-1}中的分量，ⁱT_i是第i个关节的转轴对应的单位向量。

根据本发明的一种实施方式，在所述步骤S2中，判断各连杆的拘束量是否需要对关节角加速度进行求导操作的判据为下式(3)：

其中，Z_i是第i个连杆的拘束量，θ_i是第i个关节的转角。

根据本发明的一种实施方式，在所述步骤S3中，根据下式(4)计算各连杆的拘束量对关节角加速度的一阶偏导数的计算方法为：

其中，Z_i是第i个连杆的拘束量，m_i是第i个连杆的质量，J_i是第i个连杆的惯性张量，

是ⁱω_i对应的叉乘矩阵。

根据本发明的一种实施方式，在所述步骤S3中，首先计算各连杆的高斯拘束量Z_i，然后根据下式(5)及各连杆的高斯拘束量Z_i计算所述机械臂的整体高斯拘束量Z：

根据本发明的一种实施方式，在所述步骤S3中，根据下式(6)计算所述整体高斯拘束量对各个关节的关节角加速度的一阶偏导数：

根据本发明的一种实施方式，在所述步骤S5中根据下式(7)计算关节力矩：

其中，τ_i是第i个关节处的关节力矩。

根据本发明的一种实施方式，所述机械臂动力学计算方法包括以下步骤：

步骤S0、获取所述机械臂的特征参数，所述特征参数和所述机械臂包含的所述连杆及所述关节相关联。

本发明还提供了一种机械臂的中央控制方法，其特点在于，所述中央控制方法采用基于动力学算法设计的软件架构构成中央控制器，从而所述中央控制器被配置为能够执行如上所述的用于操作机械臂的机械臂动力学计算方法，所述中央控制器还被配置为能够根据所述机械臂动力学计算方法所获得的计算结果，在所述中央控制器中实现以下功能中的部分或全部：动力学仿真、重力补偿、位置控制、力控制、碰撞检测。

本发明还提供了一种机械臂的控制系统，其特点在于，所述控制系统包括中央控制器和机械臂系统，所述机械臂系统包括机械臂本体、关节控制器和传感器，所述中央控制器包括信号处理模块、动力学模块、碰撞检测模块、状态预测模块、补偿量计算模块和决策模块；

其中，所述关节控制器被配置为能够基于接收到的关节控制指令控制所述机械臂本体的操作，所述传感器被配置为能够采集和所述机械臂本体的操作相关联的信号并将所述信号反馈至所述信号处理模块；

其中，所述信号处理模块被配置为能够处理所述传感信号并将其提供至所述动力学模块，所述动力学模块被配置为能够执行如上所述的用于操作机械臂的机械臂动力学计算方法，并将计算结果分别提供至所述碰撞检测模块、所述状态预测模块、所述补偿量计算模块；

所述碰撞检测模块、所述状态预测模块、所述补偿量计算模块分别被配置为能够基于其中预置的预设算法根据收到的所述计算结果形成碰撞检测信息、状态预测信息和补偿量信息并提供至所述决策模块，所述决策模块被配置为能够根据收到的所述三类信息生成所述关节控制指令，并将所述关节控制指令发送至所述关节控制器。

在符合本领域常识的基础上，上述各优选条件，可任意组合，即得本发明各较佳实例。

本发明的积极进步效果在于：

根据本发明的用于操作机械臂的机械臂动力学计算方法、机械臂的中央控制方法和控制系统，有助于降低机械臂的动力学问题计算中所需的计算量，提高计算结果的精确度，并且适于为各种机械臂系统涉及的动力学计算过程提供标准化、模块化的方法，能够适应新型机械臂的操作对中央控制器的需求。

附图说明

图1为根据本发明优选实施例的用于操作机械臂的机械臂动力学计算方法中的一部分步骤的示意图。

图2为根据本发明优选实施例的用于操作机械臂的机械臂动力学计算方法中的另一部分步骤的示意图

图3为根据本发明优选实施例的机械臂操作系统的示例性系统架构图。

具体实施方式

下面结合说明书附图，进一步对本发明的优选实施例进行详细描述，以下的描述为示例性的，并非对本发明的限制，任何的其他类似情形也都将落入本发明的保护范围之中。

在以下的具体描述中，方向性的术语，例如“左”、“右”、“上”、“下”、“前”、“后”等，参考附图中描述的方向使用。本发明各实施例中的部件可被置于多种不同的方向，方向性的术语是用于示例的目的而非限制性的。

图1和图2示出了根据本发明优选实施方式的用于操作机械臂的机械臂动力学计算方法的示例性方法流程，其包括如下所述的步骤S1至S6。其中，应理解的是，在实际实施或应用本发明的用于操作机械臂的机械臂动力学计算方法时，通常还可包括一步骤，其中，获取或者输入所述机械臂的特征参数，所述特征参数和所述机械臂包含的所述连杆及所述关节相关联。可选地，该特征参数可由布置于机械臂各处的传感器采集并传输回诸如机械臂系统的中央控制器或处理单元等部分。在输入的参数的基础上，可执行如下详述的示例性方法步骤。

如图1所示为根据本发明优选实施方式的用于操作机械臂的机械臂动力学计算方法的示例性方法流程中涉及对操作机械臂动力学正问题的计算步骤。

其中，在步骤S1中，以关节转角作为广义坐标，通过递推的方式计算机械臂上各连杆坐标系的线加速度及角加速度。

首先，基于下式对各连杆坐标系的角速度进行递推计算：

以及，基于下式连杆坐标系的角加速度的计算方法为：

连杆坐标系的线加速度的计算方法为：

利用连杆坐标系的角加速度及线加速度，可以计算得到各连杆质心的线加速度：

以上公式中，ⁱx表示变量x在坐标系{i}中的分量，右下标为关节编号x_i，表示在第i个关节/连杆对应的变量x，ⁱω_j是第j个连杆的角速度在坐标系{i}中的分量，

是第j个连杆的角加速度在坐标系{i}中的分量，θ_i是第i个关节的转角，ⁱR_j是从坐标系{j}至坐标系{i}进行变换的方向余弦阵，

是连杆坐标系{j}原点加速度在坐标系{i}中的分量，^i-1P_i是连杆坐标系{i}原点在坐标系{i-1}中的分量，ⁱT_i是第i个关节转轴对应的单位向量。

在步骤S2中，判断各连杆的拘束量是否需要对关节角加速度进行求导操作，若是则利用前一个关节角加速度的求导结果进行递推，若否则直接赋零。

为了达到减少计算量，降低舍入误差的目的，本发明中首先判断各连杆拘束量对各个关节角加速度是否需要进行求导操作，若不需要进行求导操作则直接对该偏导数进行赋零操作，否则计算对应的偏导数。判断是否需要进行求导操作的判据为：

其中偏导数的计算方法为：

其中的连杆质心加速度对关节角加速度的偏导数的计算方法为：

连杆坐标系对关节角加速度的偏导数的计算方法为：

在步骤S3中，计算系统整体高斯拘束量及其对加速度的一阶偏导数。对各连杆的拘束量求和得到系统整体的拘束量：

各连杆的拘束量的计算方法为：

在以上第一步计算结果(即基于上式(5)的计算)的基础上，对各连杆对关节角加速度的一阶偏导数进行求和得到系统整体拘束量对关节角加速度的一阶偏导数：

在步骤S4中，通过求解极值问题得到关节角加速度，积分得到关节速度及关节角度。

高斯最小拘束原理表明，系统的真实加速度使得系统的拘束量取最小值，故通过求解以下极值问题

可得到系统的角加速度

此求解过程可通过使用最速降法、牛顿法、共轭梯度法等数值最优化方法实现，求解时所需的一阶偏导数已在上述步骤S3中计算得到。求得系统的角加速度后，求解以下形式的二阶常微分方程：

即可获得系统在下一个时刻的关节角速度

和关节角度θ。

步骤S6，计算系统的动量、能量。

利用步骤S5中计算得到的关节角速度

和关节角度θ，可以计算系统的动量

动能

以及重力势能V(θ)作为输出量。

如图2所示为根据本发明优选实施方式的用于操作机械臂的机械臂动力学计算方法的示例性方法流程中的对操作机械臂动力学反问题的计算步骤。

根据本发明的优选实施方式，针对操作机械臂的反问题的计算方法如图2所示地包括如下步骤：

步骤S5，计算关节处的力矩。

当使用关节角度θ作为广义坐标时，系统整体的拘束量关于关节角加速度的一阶偏导数即为关节力矩：

其中，τ_i是第i个关节处的关节力矩。

换言之，关节力矩的计算方法可如下式所示：

其中，τ是机械臂关节力矩列阵。

如图3所示，根据本发明的一些优选实施方式的中央控制系统，包括中央控制器和机械臂系统两个主要部分。在图3中以模块P1表示中央控制器部分，以模块P2表示机械臂系统部分。

其中，机械臂系统包括机械臂本体、关节控制器和传感器，所述中央控制器包括信号处理模块、动力学模块、碰撞检测模块、状态预测模块、补偿量计算模块和决策模块。

其中，所述关节控制器被配置为能够基于接收到的关节控制指令控制所述机械臂本体的操作，所述传感器被配置为能够采集和所述机械臂本体的操作相关联的传感信号并将所述传感信号反馈至所述信号处理模块。

其中，所述信号处理模块被配置为能够处理所述传感信号并将其提供至所述动力学模块，所述动力学模块被配置为能够执行如上所述的用于操作机械臂的机械臂动力学计算方法，并将计算结果分别提供至所述碰撞检测模块、所述状态预测模块、所述补偿量计算模块。

所述碰撞检测模块、所述状态预测模块、所述补偿量计算模块分别被配置为能够基于其中预置的预设算法根据收到的所述计算结果形成碰撞检测信息、状态预测信息和补偿量信息并提供至所述决策模块，所述决策模块被配置为能够根据收到的所述三类信息生成所述关节控制指令，并将所述关节控制指令发送至所述关节控制器。

基于上述方案，在中央控制器内部，运动学及规划模块可向动力学模块输入运动指令，信号处理模块向动力学模块输入机械臂传感器信号，动力学模块向碰撞检测功能模块输出能量及动量的计算结果，动力学模块向状态预测功能模块输出位置、速度的预测结果，动力学模块向计算补偿功能模块输出期望关节力矩。

根据本发明的上述优选实施方式，通过递推格式减少动力学问题的计算所需的计算量，以达到提高计算效率的目的，并通过缩短计算流程的方法达到减小舍入误差、提高计算精度的目的，并通过对动力学计算进行标准化、模块化的设计，提出了用于机器人中央控制器动力学计算的软件架构及控制系统，并给出了机械臂系统相关信号及参数传递关系，可以在机器人中央控制器中对各种类型的操作机械臂的动力学问题进行计算，并向其它功能模块提供数据以实现特定的功能。

虽然以上描述了本发明的具体实施方式，但是本领域的技术人员应当理解，这些仅是举例说明，本发明的保护范围是由所附权利要求书限定的。本领域的技术人员在不背离本发明的原理和实质的前提下，可以对这些实施方式做出多种变更或修改，而且这些变更和修改均落入本发明的保护范围。

Claims (10)

1.一种用于操作机械臂的机械臂动力学计算方法，其特征在于，所述机械臂包括多个连杆及连接所述连杆的多个关节，其中基于高斯原理通过递推计算各个关节及连杆的相关数据，所述机械臂动力学计算方法包括以下步骤：

步骤S1、以关节转角作为广义坐标，通过递推的方式计算所述机械臂上各连杆坐标系的线加速度及角加速度；

步骤S2、判断各连杆的拘束量是否需要对各个关节的关节角加速度进行求导操作，若是则利用前一关节的关节角加速度的求导结果进行递推计算，若否则赋零；

步骤S3、计算所述机械臂的整体高斯拘束量及所述整体高斯拘束量对各个关节的关节角加速度的一阶偏导数；

步骤S4、通过求解基于高斯最小拘束原理的极值问题计算得到各个关节的关节角加速度，并积分得到关节速度及关节角度；

步骤S5、计算各个关节处的关节力矩；

步骤S6、计算所述机械臂的系统动量、系统能量。

2.如权利要求1所述的机械臂动力学计算方法，其特征在于，在所述步骤S1中，根据下式(1)计算各连杆坐标系的角加速度：

根据下式(1)计算各连杆坐标系的线加速度：

其中，ⁱx表示变量x在坐标系{i}中的分量，右下标为关节的编号x_i，表示在第i个关节/连杆对应的变量x，ⁱω_j是第j个连杆的角速度在坐标系{i}中的分量，

是第j个连杆的角加速度在坐标系{i}中的分量，θ_i是第i个关节的转角，ⁱR_j是从坐标系{j}至坐标系{i}进行变换的方向余弦阵，

是连杆坐标系{j}的原点加速度在坐标系{i}中的分量，^i-1P_i是连杆坐标系{i}的原点在坐标系{i-1}中的分量，ⁱT_i是第i个关节的转轴对应的单位向量。

3.如权利要求1所述的机械臂动力学计算方法，其特征在于，在所述步骤S2中，判读各连杆的拘束量是否需要对关节角加速度进行求导操作的判据为下式(3)：

其中，Z_i是第i个连杆的拘束量，θ_i是第i个关节的转角。

4.如权利要求2所述的机械臂动力学计算方法，其特征在于，在所述步骤S3中，根据下式(4)计算各连杆的拘束量对关节角加速度的一阶偏导数的计算方法为：

其中，Z_i是第i个连杆的拘束量，m_i是第i个连杆的质量，J_i是第i个连杆的惯性张量，

是ⁱω_i对应的叉乘矩阵。

5.如权利要求4所述的机械臂动力学计算方法，其特征在于，在所述步骤S3中，首先计算各连杆的高斯拘束量Z_i，然后根据下式(5)及各连杆的高斯拘束量Z_i计算所述机械臂的整体高斯拘束量Z：

6.如权利要求5所述的机械臂动力学计算方法，其特征在于，在所述步骤S3中，根据下式(6)计算所述整体高斯拘束量对各个关节的关节角加速度的一阶偏导数：

7.如权利要求6所述的机械臂动力学计算方法，其特征在于，在所述步骤S5中根据下式(7)计算关节力矩：

其中，τ_i是第i个关节处的关节力矩。

8.如权利要求1-7中任一项所述的机械臂动力学计算方法，其特征在于，所述机械臂动力学计算方法包括以下步骤：

步骤S0、获取所述机械臂的特征参数，所述特征参数和所述机械臂包含的所述连杆及所述关节相关联。

9.一种机械臂的中央控制方法，其特征在于，所述中央控制方法采用基于动力学算法设计的软件架构构成中央控制器，从而所述中央控制器被配置为能够执行如权利要求1-8中任意一项所述的用于操作机械臂的机械臂动力学计算方法，所述中央控制器还被配置为能够根据所述机械臂动力学计算方法所获得的计算结果，在所述中央控制器中实现以下功能中的部分或全部：动力学仿真、重力补偿、位置控制、力控制、碰撞检测。

10.一种机械臂的中央控制系统，其特征在于，所述中央控制系统包括中央控制器和机械臂系统，所述机械臂系统包括机械臂本体、关节控制器和传感器，所述中央控制器包括信号处理模块、动力学模块、碰撞检测模块、状态预测模块、补偿量计算模块和决策模块；

其中，所述关节控制器被配置为能够基于接收到的关节控制指令控制所述机械臂本体的操作，所述传感器被配置为能够采集和所述机械臂本体的操作相关联的传感信号并将所述传感信号反馈至所述信号处理模块；

其中，所述信号处理模块被配置为能够处理所述传感信号并将其提供至所述动力学模块，所述动力学模块被配置为能够执行如权利要求1-8中任意一项所述的用于操作机械臂的机械臂动力学计算方法，并将计算结果分别提供至所述碰撞检测模块、所述状态预测模块、所述补偿量计算模块；

所述碰撞检测模块、所述状态预测模块、所述补偿量计算模块分别被配置为能够基于其中预置的预设算法根据收到的所述计算结果形成碰撞检测信息、状态预测信息和补偿量信息并提供至所述决策模块，所述决策模块被配置为能够根据收到的所述三类信息生成所述关节控制指令，并将所述关节控制指令发送至所述关节控制器。

Images