CN108972536B - 机械臂的动力学参数确定系统、方法及存储介质 - Google Patents

Abstract

本发明公开了机械臂的动力学参数确定系统、方法及存储介质。其中，方法包括：利用与机械臂的子臂数据相同的MEMS传感器测量各子臂的传感器角速度和传感器方向，基于与基座相连的第一子臂的传感器角速度，得到第一子臂的关节速度，并根据各个子臂的关节速度之间的关系式，依次计算得到各子臂的关节速度，之后只对各子臂的关节速度进行一次求导便可得到各子臂的关节加速度，从而减少了二次求导过程中引入的噪声，提高了参数确定的准确性，并进一步保障了机械臂的控制性能。

Description

机械臂的动力学参数确定系统、方法及存储介质

技术领域

本发明涉及控制领域，特别是一种机械臂的动力学参数确定系统、方法及存储介质。

背景技术

对于基于动力学模型的控制机构来说，确定动力学模型中的动力学参数是构建该动力学模型的关键步骤，合适的动力学参数设置可以使控制机构具有良好的性能。对于机械臂的动力学模型来说，其动力学参数主要包括：质量、质心以及转动惯量。目前，用于确定动力学参数的方法主要有如下三种：

第一种：直接测量法。该方法使用特定的设备，如双线摆或三线摆来测量机械臂各节臂的质心位置和转动惯量。但具体测量时，需要将各节臂首先装配好并依次测量各个臂的参数。该方法需要花费较高的劳动成本。此外，很难测量像摩擦力这种非刚体力学的影响。

第二种：CAD估测法。该方法是确定动力学参数最方便的方法。但该方法需要首先建立机械臂所有组件的准确的3D模型，否则将很难避免误差的出现。事实上，由于制造误差和装配误差的存在，很难建立机械臂的准确3D模型。此外，该方法也无法对摩擦力的影响进行准确估计。

第三种：基于动力学模型的校正法。该方法被广泛应用于参数确定。首先，非线性动力学方程被转换为包含动力学参数的线性动力学方程。此外，该线性动力学方程中还包括关节输出力矩、关节位置、关节速度和关节加速度。其中，关节输出力矩可以通过力矩传感器测量得到或通过电机电流计算得到。此外，电机编码器可以提供关节位置的信息，之后通过对关节位置求导得到关节速度，通过对关节速度求导可以得到关节加速度。之后，将得到的关节输出力矩、关节位置、关节速度和关节加速度代入上述的线性动力学方程之后，便可计算出动力学参数。

但上述的关节速度和关节加速度是通过对关节位置一次求导和二次求导后得到，而求导过程中又会引入大量噪声。为此本领域内的技术人员还在致力于寻找其它的解决方案。

发明内容

有鉴于此，本发明实施例中一方面提出了机械臂的动力学参数确定系统，另一方面提出了机械臂的动力学参数确定方法及计算机可读存储介质，用以提高参数确定的准确性，进一步确保机械臂的控制性能。

本发明实施例中提出的机械臂的动力学参数确定系统，所述机械臂每个子臂的运动由对应的电机驱动器及电机控制，且对应每个电机安装有用于测量机械臂的对应关节位置的电机编码器；所述系统包括：

与机械臂的子臂数量相同的复数个MEMS传感器，一个MEMS传感器安装在一节子臂上，且每个MEMS传感器包括一陀螺仪、一加速计和一磁力计；所述陀螺仪用于测量得到对应子臂的传感器角速度，所述加速计和磁力计共同测量得到对应子臂的传感器方向；

第一数据接收单元用于在机械臂按照设定的路径运动的过程中，按照设定的测试步长(Δt)周期性接收来自各MEMS传感器的第一测试数据；所述第一测试数据包括来自各MEMS传感器的对应各子臂的传感器角速度和传感器方向；

第二数据接收单元用于在机械臂按照设定的路径运动的过程中，按照设定的测试步长(Δt)周期性接收来自各电机驱动器的第二测试数据；所述第二测试数据包括：各子臂的电机电流以及各子臂的关节位置；

中央处理单元用于对于每个测试步长接收的测试数据，将所述电机电流转换为关节输出力矩；并根据下面的公式(1)和(2)，利用所述传感器角速度和传感器方向，依次计算得到机械臂各子臂的关节转速

对计算得到的关节转速

一次求导得到关节加速度；将每次得到的各子臂的关节输出力矩、关节位置、关节转速、关节加速度作为一组已知参数向量；在驱动机械臂按照设定的路径运动的过程中得到的所有C组已知参数向量中选取M组已知参数向量，将所选取的M组已知参数向量代入到线性动力学方程中，求解出目标动力学参数；将所述目标参数确定为所述机械臂动力学模型的动力学参数；其中，M*n大于线性动力学方程中待求解的动力学参数数量；

其中，i＝0,1,2,…,n-1；n为机械臂的子臂数量；

为第i+1个子臂的关节转速，

为第i+1个子臂的坐标系的z轴方向，ⁱω_i为第i个子臂在子臂坐标系中的角速度，

为由第i个子臂的坐标系变换到第i+1个子臂的坐标系的旋转矩阵，ω_i+1为第i+1个子臂的传感器测量的传感器角速度，ⁱ⁺¹R为从传感器坐标系变换到第i+1个子臂的坐标系的旋转矩阵；k_x、k_y、k_z表示机械臂坐标系的旋转轴，

是根据第一个子臂的传感器测得的角速度利用公式(3)计算得到的一个标量。

在一个实施方式中，所述中央处理单元将所述目标参数确定为所述机械臂动力学模型的动力学参数之前，进一步从所述C组已知参数向量中选取除所述M组已知参数向量之外的N组已知参数向量作为测试参数向量或从所接收的第一测试数据和第二测试数据中选取N组测试数据作为测试参数向量；对所述N组测试参数向量中的每组测试参数向量，选取一个值作为测试值，将其他相关值以及求解出的目标动力学参数代入到所述线性动力学方程中，求解出对应所述测试值的验证值；计算所述验证值与所述测试值的差值，若N组测试参数向量得到的验证值与测试值的差值均小于一个设定误差阈值(ε)，则确定所述目标动力学参数为所述动力学模型的动力学参数；其中，N大于或等于1。

在一个实施方式中，所述中央处理单元将所述电机电流或所述关节输出力矩作为测试值。

本发明实施例中提供的机械臂的动力学参数确定方法，所述机械臂每个子臂的运动由对应的电机驱动器及电机控制；所述机械臂的每节子臂上安装有一MEMS传感器，且每个MEMS传感器包括一陀螺仪、一加速计和一磁力计；所述方法包括：

驱动机械臂按照设定的路径运动的过程中，按照设定的测试步长(Δt)周期性地接收测试数据；所述测试数据包括：各MEMS传感器的陀螺仪测量得到的对应各子臂的传感器角速度，由各MEMS传感器的加速计和磁力计共同测量得到的对应各子臂的传感器方向，以及来自各电机驱动器的各对应子臂的电机电流以及关节位置；

对于每个测试步长接收的测试数据，将所述电机电流转换为关节输出力矩；并根据下面的公式(1)和(2)，利用所述传感器角速度和传感器方向，依次计算得到机械臂各子臂的关节转速

对计算得到的关节转速

一次求导得到关节加速度；

其中，i＝0,1,2,…,n-1；n为机械臂的子臂数量；

为第i+1个子臂的关节转速，

为第i+1个子臂的坐标系的z轴方向，ⁱω_i为第i个子臂在子臂坐标系中的角速度，

为由第i个子臂的坐标系变换到第i+1个子臂的坐标系的旋转矩阵，ω_i+1为第i+1个子臂的传感器测量的传感器角速度，ⁱ⁺¹R为从传感器坐标系变换到第i+1个子臂的坐标系的旋转矩阵；k_x、k_y、k_z表示机械臂坐标系的旋转轴，

是根据第一个子臂的传感器测得的角速度利用公式(3)计算得到的一个标量；

将每次得到的各子臂的关节输出力矩、关节位置、关节转速、关节加速度作为一组已知参数向量；

在驱动机械臂按照设定的路径运动的过程中得到的所有C组已知参数向量中选取M组已知参数向量，将所选取的M组已知参数向量代入到线性动力学方程中，求解出目标动力学参数；其中，M*n大于线性动力学方程中待求解的动力学参数数量；

将所述目标参数确定为所述机械臂动力学模型的动力学参数。

在一个实施方式中，所述将目标参数确定为所述机械臂动力学模型的动力学参数之前，该方法进一步包括：

从所述C组已知参数向量中选取除所述M组已知参数向量之外的N组已知参数向量作为测试参数向量或从所接收的测试数据中选取N组测试数据作为测试参数向量；

对所述N组测试参数向量中的每组测试参数向量，选取一个值作为测试值，将其他相关值以及求解出的目标动力学参数代入到所述线性动力学方程中，求解出对应所述测试值的验证值；

计算所述验证值与所述测试值的差值，若N组测试参数向量得到的验证值与测试值的差值均小于一个设定误差阈值，则确定所述目标动力学参数为所述动力学模型的动力学参数；

其中，N大于或等于1。

在一个实施方式中，所述选取一个值作为测试值为：选取电机电流或关节输出力矩作为测试值。

本发明实施例中提供的计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述的机械臂动力学模型的动力学参数确定方法的步骤。

从上述方案中可以看出，由于本发明实施例中仅需设置与机械臂子臂数量相同的MEMS传感器来测量机械臂各个子臂的相对角速度，并根据各个子臂的关节速度之间的关系式，基于与基座相连的第一子臂的关节速度，依次计算得到各子臂的关节速度，之后只对各子臂的关节速度进行一次求导便可得到各子臂的关节加速度，从而减少了二次求导过程中引入的噪声，提高了参数确定的准确性，进一步确保了机械臂的控制性能。

进一步地，通过在求解出目标动力学参数之后，利用N组测试参数向量来验证动力学参数的正确性，可以进一步提高参数确定的准确性，并进一步保障机械臂的控制性能。

此外，由于只增加了MEMS传感器，因此成本较低，且MEMS传感器可安装在机械臂各子臂的任意位置，同时机械臂动力学模型的动力学参数确定系统可作为即插即用的插件使用，因此方便集成。并且，只需要机械臂按照预定轨迹运行一次，便可获取所有数据并确定出动力学参数，因此实现方便快捷。

附图说明

下面将通过参照附图详细描述本发明的优选实施例，使本领域的普通技术人员更清楚本发明的上述及其它特征和优点，附图中：

图1为本发明实施例中机械臂的动力学参数确定系统的示例性结构图。

图2为本发明实施例中机械臂的动力学参数确定方法的示例性流程图。

其中，附图标记如下：

具体实施方式

本发明实施例中，为了提高参数确定的准确性，以进一步确保机械臂的控制性能，不再通过对关节位置进行一次求导和二次求导后得到关节速度和关节加速度，而是通过测量机械臂各个子臂的相对角速度，基于与基座相连的第一子臂的相对角速度，得到第一子臂的关节速度，并根据各个子臂的关节速度之间的关系式，依次计算得到各子臂的关节速度，之后只对各子臂的关节速度进行一次求导便可得到各子臂的关节加速度，从而减少了二次求导过程中引入的噪声。为了测量机械臂各个子臂的相对角速度，本实施例中引入了微机电系统(MEMS，Microelectro Mechanical Systems)传感器。

为使本发明的目的、技术方案和优点更加清楚，以下举实施例对本发明进一步详细说明。

图1为本发明实施例中机械臂的动力学参数确定系统的示例性结构图。本实施例中，机械臂10每个子臂的运动由对应该子臂安装的电机驱动器20及电机(图中未示出)控制，且对应每个电机安装有用于测量对应子臂的关节位置的电机编码器(图中未示出)。相应地，对应各子臂的电机电流以及关节位置便可直接从对应的电机驱动器20获取。其中，电机可以为伺服电机，电机驱动器可以为伺服电机驱动器。如图1所示，本实施例中的机械臂的动力学参数确定系统可包括：复数个MEMS传感器110、第一数据接收单元120、第二数据接收单元130和中央处理单元140。

其中，MEMS传感器110的数量与机械臂的子臂数量相同，例如，对应图1中三节子臂的机械臂，MEMS传感器110的数量为3。又如，对应六节子臂的机械臂，MEMS传感器110的数量则为6。其中，一个MEMS传感器110安装在机械臂的一节子臂上，安装位置可以是该节子臂的任意位置，且每个MEMS传感器110可包括一个陀螺仪、一个加速计和一个磁力计；所述陀螺仪用于测量得到对应子臂的传感器角速度，所述加速计和磁力计共同测量得到对应子臂的传感器方向。

第一数据接收单元120用于在机械臂10按照设定的路径运动的过程中，按照设定的测试步长Δt周期性地接收来自各MEMS传感器110的第一测试数据；所述第一测试数据包括来自各MEMS传感器110的对应各子臂的传感器角速度和传感器方向。本发明实施例中，第一数据接收单元120可以为ARM处理器等硬件模块，也可以为其它集成电路。

第二数据接收单元130用于在机械臂10按照设定的路径运动的过程中，按照设定的测试步长Δt周期性地接收来自各电机驱动器的第二测试数据；所述第二测试数据包括：各子臂的电机电流以及各子臂的关节位置。本发明实施例中，第二数据接收单元130可以为工业个人计算机(IPC)的接口模块。此外，在其它实施方式中，第一数据接收单元120和第二数据接收单元130也可以为同一个数据接收单元。

中央处理单元140用于存储所述第一数据接收单元120和第二数据接收单元130在每个测试步长Δt接收的测试数据；并对于每个测试步长Δt接收的测试数据组，将其中的电机电流转换为关节输出力矩；并根据下面的公式(1)和(2)，利用所述传感器角速度和传感器方向，依次计算得到机械臂各子臂的关节转速

对计算得到的关节转速

一次求导得到关节加速度；将每次得到的各子臂的关节输出力矩、关节位置、关节转速、关节加速度作为一组已知参数向量；在驱动机械臂10按照设定的路径运动的过程中得到的所有C组已知参数向量中选取M组已知参数向量，将所选取的M组已知参数向量代入到线性动力学方程中，求解出目标动力学参数；将所述目标参数确定为所述机械臂动力学模型的动力学参数。其中，M*n大于线性动力学方程中待求解的动力学参数数量。

其中，i＝0,1,2,…,n-1；n为机械臂的子臂数量；

为第i+1个子臂的关节转速，

为第i+1个子臂的坐标系的z轴方向，ⁱω_i为第i个子臂在子臂坐标系中的角速度，

为由第i个子臂的坐标系变换到第i+1个子臂的坐标系的旋转矩阵，ω_i+1为第i+1个子臂的传感器测量的传感器角速度，ⁱ⁺¹R为从传感器坐标系变换到第i+1个子臂的坐标系的旋转矩阵；k_x、k_y、k_z表示机械臂坐标系的旋转轴，

是根据第一个子臂的传感器测得的角速度利用公式(3)计算得到的一个标量。其中，传感器坐标系与机械臂对应子臂的坐标系之间的转换关系可根据该子臂的关节位置、角速度以及传感器方向等数据得到。

本发明实施例中，中央处理单元可以为工业个人计算机IPC，也可以为其它类型的集成模块。

具体地，根据式(2)，可以得到第1个子臂在子臂坐标系中的角速度¹ω₁：

进而，根据式(4)可以得到第二个子臂的关节转速：

依次类推，在得到上一节子臂的角速度以及传感器方向之后，便可以依次得到每节子臂的关节转速。

本实施例中，线性动力学方程的表达式可如下式(6)所示：

I＝Φγ+f (6)

其中，I为电机电流表示的关节输出力矩，Φ为根据机械臂各子臂的关节位置、关节转速、关节加速度计算得到的已知参数矩阵，f为从预先确定的摩擦力模型中获取的摩擦力，γ为包括各子臂的质量、质心、转动惯量，以及转子转动惯量的动力学参数向量。

对于动力学参数向量中的各个参数，像各子臂的质量、质心等可以直接测量得到，也可以通过上述的计算得到。若是通过直接测量得到，则其可进一步作为已知量，求解动力学参数向量中的其它未知量。

在另一个实施方式中，中央处理单元140将所述目标参数确定为所述机械臂动力学模型的动力学参数之前，可进一步从所述C组已知参数向量中选取除所述M组已知参数向量之外的N组已知参数向量作为测试参数向量或在计算各子臂的关节转速之前，先从所存储的C组测试数据中选取N组测试数据作为测试参数向量；对所述N组测试参数向量中的每组测试参数向量，选取其中的一个值作为测试值，将其他相关值以及求解出的目标动力学参数代入到所述线性动力学方程中，求解出对应所述测试值的验证值；计算所述验证值与所述测试值的差值，若N组测试参数向量得到的验证值与测试值的差值均小于一个设定误差阈值ε，则可确定所述目标动力学参数为所述动力学模型的动力学参数。其中，N大于或等于1；否则，可重新确定目标动力学参数。

在一个实施方式中，可将电机电流或电机电流表示的关节输出力矩作为测试值。

以上对本发明实施例中的机械臂动力学模型的动力学参数确定系统进行了详细描述，下面再对本发明实施例中的机械臂动力学模型的动力学参数确定方法进行详细描述，本发明实施例中的方法可由本发明实施例中的系统实现，对于本发明实施例中的方法未披露的细节可参见本发明实施例中的系统的相应描述。

图2为本发明实施例中机械臂的动力学参数确定方法的示例性流程图。本实施例中，机械臂每个子臂的运动由对应该子臂安装的电机驱动器(图中未示出)及电机(图中未示出)控制，且对应每个电机安装有用于测量对应子臂的关节位置的电机编码器(图中未示出)。相应地，对应各子臂的电机电流以及关节位置便可直接从对应的电机驱动器获取。其中，电机可以为伺服电机，电机驱动器可以为伺服电机驱动器。此外，本实施例中机械臂的每节子臂上安装有一个MEMS传感器，且每个MEMS传感器包括一陀螺仪、一加速计和一磁力计。如图2所示，本实施例中的机械臂动力学模型的动力学参数确定方法可包括如下步骤：

步骤210，中央处理单元接收初始化数据。其中，初始化数据可包括：机械臂的运动路径、测试步长Δt、以及误差阈值，测试参数向量的组数N等。其中，N大于或等于1。

步骤220，驱动机械臂按照设定的路径运动的过程中，按照设定的测试步长Δt周期性地接收测试数据；所述测试数据可包括：各MEMS传感器的陀螺仪测量得到的对应各子臂的传感器角速度，由各MEMS传感器的加速计和磁力计共同测量得到的对应各子臂的传感器方向，以及来自各电机驱动器的各对应子臂的电机电流以及关节位置。

步骤230，对于每个测试步长接收的测试数据，将所述电机电流转换为关节输出力矩；并根据下面的公式(1)和(2)，利用所述传感器角速度和传感器方向，依次计算得到机械臂各子臂的关节转速

对计算得到的关节转速

一次求导得到关节加速度；

其中，i＝0,1,2,…,n-1；n为机械臂的子臂数量；

为第i+1个子臂的关节转速，

为第i+1个子臂的坐标系的z轴方向，ⁱω_i为第i个子臂在子臂坐标系中的角速度，

为由第i个子臂的坐标系变换到第i+1个子臂的坐标系的旋转矩阵，ω_i+1为第i+1个子臂的传感器测量的传感器角速度，ⁱ⁺¹R为从传感器坐标系变换到第i+1个子臂的坐标系的旋转矩阵；k_x、k_y、k_z表示机械臂坐标系的旋转轴，

是根据第一个子臂的传感器测得的角速度利用公式(3)计算得到的一个标量。

步骤240，将每次得到的各子臂的关节输出力矩、关节位置、关节转速、关节加速度作为一组已知参数向量存储。

步骤250，在驱动机械臂按照设定的路径运动的过程中得到的所有C组已知参数向量中选取M组已知参数向量，将所选取的M组已知参数向量代入到线性动力学方程中，求解出目标动力学参数。其中，M*n大于线性动力学方程中待求解的动力学参数数量，n为机械臂的子臂数量。

步骤260，从所述C组已知参数向量中选取除所述M组已知参数向量之外的N组已知参数向量作为测试参数向量；或在执行步骤230之前，从所接收的测试数据中选取N组测试数据作为测试参数向量。

步骤270，对所述N组测试参数向量中的每组测试参数向量，选取其中的一个值作为测试值，将其他值以及求解出的目标动力学参数代入到所述线性动力学方程中，求解出对应所述测试值的验证值。在一个实施方式中，可选取电机电流或电机电流表示的关节输出力矩作为测试值。

步骤280，计算所述验证值与所述测试值的差值，若N组测试参数向量得到的验证值与测试值的差值均小于设定误差阈值ε，则执行步骤260；否则，可返回执行步骤210，可重新调整各初始化参数，如机械臂的运动路径、测试步长Δt、测试参数向量的组数N等。

步骤290，确定所述目标动力学参数为所述动力学模型的动力学参数。

在有些实施方式中，也可不执行步骤260至步骤280，即执行步骤250之后，便直接执行步骤290。

本发明还提供了一种计算机可读的存储介质，存储用于使一计算机执行如本文所述的机械臂动力学模型的动力学参数确定方法的指令。具体地，可以提供配有存储介质的系统或者装置，在该存储介质上存储着实现上述实施例中任一实施例的功能的软件程序代码，且使该系统或者装置的计算机(或CPU或MPU)读出并执行存储在存储介质中的程序代码。

在这种情况下，从存储介质读取的程序代码本身可实现上述实施例中任何一项实施例的功能，因此程序代码和存储程序代码的存储介质构成了本发明的一部分。

用于提供程序代码的存储介质实施例包括软盘、硬盘、磁光盘、光盘(如CD-ROM、CD-R、CD-RW、DVD-ROM、DVD-RAM、DVD-RW、DVD+RW)、磁带、非易失性存储卡、ROM、PC机、手机以及各种智能设备等。可选择地，可以由通信网络从服务器计算机上下载程序代码。

此外，应该清楚的是，不仅可以通过执行计算机所读出的程序代码，而且可以通过基于程序代码的指令使计算机上操作的操作系统等来完成部分或者全部的实际操作，从而实现上述实施例中任意一项实施例的功能。

此外，可以理解的是，将由存储介质读出的程序代码写到插入计算机内的扩展板中所设置的存储器中或者写到与计算机相连接的扩展单元中设置的存储器中，随后基于程序代码的指令使安装在扩展板或者扩展单元上的CPU等来执行部分和全部实际操作，从而实现上述实施例中任一实施例的功能。

从上述方案中可以看出，由于本发明实施例中仅需设置与机械臂子臂数量相同的MEMS传感器来测量机械臂各个子臂的相对角速度，并根据各个子臂的关节速度之间的关系式，基于与基座相连的第一子臂的关节速度，依次计算得到各子臂的关节速度，之后只对各子臂的关节速度进行一次求导便可得到各子臂的关节加速度，从而减少了二次求导过程中引入的噪声，提高了参数确定的准确性，进一步确保了机械臂的控制性能。

进一步地，通过在求解出目标动力学参数之后，利用N组测试参数向量来验证动力学参数的正确性，可以进一步提高参数确定的准确性，并进一步保障机械臂的控制性能。

此外，由于只增加了MEMS传感器，因此成本较低，且MEMS传感器可安装在机械臂各子臂的任意位置，同时机械臂动力学模型的动力学参数确定系统可作为即插即用的插件使用，因此方便集成。并且，只需要机械臂按照预定轨迹运行一次，便可获取所有数据并确定出动力学参数，因此实现方便快捷。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.机械臂的动力学参数确定系统，所述机械臂每个子臂的运动由对应的电机驱动器及电机控制，且对应每个电机安装有用于测量机械臂的对应关节位置的电机编码器；其特征在于，所述系统包括：

与机械臂的子臂数量相同的多个MEMS传感器，一个MEMS传感器安装在一节子臂上，且每个MEMS传感器包括一陀螺仪、一加速计和一磁力计；所述陀螺仪用于测量得到对应子臂的传感器角速度，所述加速计和磁力计共同测量得到对应子臂的传感器方向；

第一数据接收单元用于在机械臂按照设定的路径运动的过程中，按照设定的测试步长Δt周期性接收来自各MEMS传感器的第一测试数据；所述第一测试数据包括来自各MEMS传感器的对应各子臂的传感器角速度和传感器方向；

第二数据接收单元用于在机械臂按照设定的路径运动的过程中，按照设定的测试步长Δt周期性接收来自各电机驱动器的第二测试数据；所述第二测试数据包括：各子臂的电机电流以及各子臂的关节位置；

中央处理单元用于对于每个测试步长接收的测试数据，将所述电机电流转换为关节输出力矩；并根据下面的公式(1)和(2)，利用所述传感器角速度和传感器方向，依次计算得到机械臂各子臂的关节转速

对计算得到的关节转速

一次求导得到关节加速度；将每次得到的各子臂的关节输出力矩、关节位置、关节转速、关节加速度作为一组已知参数向量；在驱动机械臂按照设定的路径运动的过程中得到的所有C组已知参数向量中选取M组已知参数向量，将所选取的M组已知参数向量代入到线性动力学方程中，求解出目标动力学参数；将所述目标动力学参数确定为机械臂动力学模型的动力学参数；其中，M*n大于线性动力学方程中待求解的动力学参数数量；

其中，i＝0,1,2,…,n-1；n为机械臂的子臂数量；

为第i+1个子臂的关节转速，

为第i+1个子臂的坐标系的z轴方向，ⁱω_i为第i个子臂在子臂坐标系中的角速度，

为由第i个子臂的坐标系变换到第i+1个子臂的坐标系的旋转矩阵，ω_i+1为第i+1个子臂的传感器测量的传感器角速度，ⁱ⁺¹R为从传感器坐标系变换到第i+1个子臂的坐标系的旋转矩阵；k_x、k_y、k_z表示机械臂坐标系的旋转轴，

是根据第一个子臂的传感器测得的角速度利用公式(3)计算得到的一个标量。

2.根据权利要求1所述的机械臂的动力学参数确定系统，其特征在于，所述中央处理单元将所述目标动力学参数确定为机械臂动力学模型的动力学参数之前，进一步从所述C组已知参数向量中选取除所述M组已知参数向量之外的N组已知参数向量作为测试参数向量或从所接收的第一测试数据和第二测试数据中选取N组测试数据作为测试参数向量；对所述N组测试参数向量中的每组测试参数向量，选取一个值作为测试值，将求解出的目标动力学参数代入到所述线性动力学方程中，求解出对应所述测试值的验证值；计算所述验证值与所述测试值的差值，若N组测试参数向量得到的验证值与测试值的差值均小于一个设定误差阈值ε，则确定所述目标动力学参数为所述动力学模型的动力学参数；其中，N大于或等于1。

3.根据权利要求2所述的机械臂的动力学参数确定系统，其特征在于，所述中央处理单元将所述电机电流或所述关节输出力矩作为测试值。

4.机械臂的动力学参数确定方法，所述机械臂每个子臂的运动由对应的电机驱动器及电机控制；其特征在于，所述机械臂的每节子臂上安装有一MEMS传感器，且每个MEMS传感器包括一陀螺仪、一加速计和一磁力计；所述方法包括：

驱动机械臂按照设定的路径运动的过程中，按照设定的测试步长(Δt)周期性地接收测试数据；所述测试数据包括：各MEMS传感器的陀螺仪测量得到的对应各子臂的传感器角速度，由各MEMS传感器的加速计和磁力计共同测量得到的对应各子臂的传感器方向，以及来自各电机驱动器的各对应子臂的电机电流以及关节位置；

对于每个测试步长接收的测试数据，将所述电机电流转换为关节输出力矩；并根据下面的公式(1)和(2)，利用所述传感器角速度和传感器方向，依次计算得到机械臂各子臂的关节转速

对计算得到的关节转速

一次求导得到关节加速度；

其中，i＝0,1,2,…,n-1；n为机械臂的子臂数量；

为第i+1个子臂的关节转速，

为第i+1个子臂的坐标系的z轴方向，ⁱω_i为第i个子臂在子臂坐标系中的角速度，

为由第i个子臂的坐标系变换到第i+1个子臂的坐标系的旋转矩阵，ω_i+1为第i+1个子臂的传感器测量的传感器角速度，ⁱ⁺¹R为从传感器坐标系变换到第i+1个子臂的坐标系的旋转矩阵；k_x、k_y、k_z表示机械臂坐标系的旋转轴，

是根据第一个子臂的传感器测得的角速度利用公式(3)计算得到的一个标量；

将每次得到的各子臂的关节输出力矩、关节位置、关节转速、关节加速度作为一组已知参数向量；

在驱动机械臂按照设定的路径运动的过程中得到的所有C组已知参数向量中选取M组已知参数向量，将所选取的M组已知参数向量代入到线性动力学方程中，求解出目标动力学参数；其中，M*n大于线性动力学方程中待求解的动力学参数数量；

将所述目标动力学参数确定为机械臂动力学模型的动力学参数。

5.根据权利要求4所述的机械臂的动力学参数确定方法，其特征在于，所述将目标动力学参数确定为机械臂动力学模型的动力学参数之前，该方法进一步包括：

从所述C组已知参数向量中选取除所述M组已知参数向量之外的N组已知参数向量作为测试参数向量或从所接收的测试数据中选取N组测试数据作为测试参数向量；

对所述N组测试参数向量中的每组测试参数向量，选取一个值作为测试值，将求解出的目标动力学参数代入到所述线性动力学方程中，求解出对应所述测试值的验证值；

计算所述验证值与所述测试值的差值，若N组测试参数向量得到的验证值与测试值的差值均小于一个设定误差阈值(ε)，则确定所述目标动力学参数为所述动力学模型的动力学参数；

其中，N大于或等于1。

6.根据权利要求5所述的机械臂的动力学参数确定方法，其特征在于，所述选取一个值作为测试值为：选取电机电流或关节输出力矩作为测试值。

7.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求4至6中任一项所述的机械臂的动力学参数确定方法的步骤。

Images