基于迭代优化和神经网络补偿的超冗余机械臂半参数动力学模型辨识

周宇飞， 李中灿， 李 毅， 崔靖凯， 贺顺锋， 盛展翊， 朱明超*

（1. 中国科学院 长春光学精密机械与物理研究所，吉林 长春 130033；2. 中国科学院大学，北京 100049；3. 宁夏大学 机械工程学院，宁夏 银川 750021）

1 引 言

机械臂的动力学模型可用于先进控制算法设计［1-2］、碰撞检测［3］、人机交互［4］、多臂协作［5］等领域。高精度的动力学模型能够提高机械臂的控制性能。然而，机械臂制造商通常不提供机械臂的动力学参数，从三维建模软件中导出的动力学参数往往与实际模型相差很大，实验辨识是获得动力学参数的可靠途径［6］。

典型的实验辨识过程主要包含以下几个步骤：动力学建模、激励轨迹设计、参数辨识和模型验证。动力学建模常用的方法是牛顿-欧拉法，通过将关节角相关部分和待辨识参数分离实现动力学线性化。为了减少辨识参数提高辨识精度，Khalil 等人剔除了动力学冗余参数，将全参数集线性组合成最小参数集［7］，建立了与最小参数集对应的回归矩阵。动力学建模还包括关节摩擦建模，传统的方法是使用库伦-黏性摩擦模型来拟合关节摩擦，但是该方法无法描绘关节速度非常低时的摩擦特性，与摩擦的非线性特性不符［8］。为了更好地拟合关节摩擦，改进库伦-黏性摩擦模型［9］和Stribeck 模型［10］被应用于动力学辨识，提高了辨识精度。本文为了提高整体动力学辨识精度，采用迭代方法优化关节摩擦模型。

激励轨迹的设计对提高辨识精度至关重要。常用的激励轨迹衡量指标有回归矩阵条件数［11-12］，子回归矩阵条件数［17］，回归矩阵的log{det(∙)}值等［6，13］。因此激励轨迹的设计重点在于满足约束的情况下尽可能降低回归矩阵条件数和子回归矩阵条件数，从而使动力学模型中的惯性项、重力项、摩擦力项都得到充分激发。Atkeson 提出了采用关节五次多项式作为激励轨迹［14］，但是实验过程中采集的关节角和关节力矩存在噪声与误差，影响数据采集精度。为了降低数据采集误差，基于傅立叶级数［15］和改进傅立叶级数的激励轨迹模型被提出［16］，这种周期性激励轨迹的好处是能够重复激励机械臂走激励轨迹，通过多次采样取平均降低数据采集误差，提高数据采集信噪比。

目前最简单常见的动力学参数估计方法有最小二乘法［18］、加权最小二乘法［19］、最大似然估计［20］、卡尔曼滤波［21］等。在进行算法识别之前通常会首先对采集的关节角进行滤波和中心差分，得到关节速度和关节加速度。将滤波之后的数据利用最小二乘算法和加权最小二乘算法进行处理辨识动力学参数，其优点是处理速度快，简单高效，但是缺点也很明显：对异常数据敏感、辨识的参数不满足物理可行性约束、模型泛化能力差。因此，一些学者提出了机械臂动力学参数物理可行性约束概念，建立了最小参数集和全参数集之间的双向映射，提出了快速判别辨识最小参数集是否满足物理可行性约束的方法［22-24］。针对异常数据问题，韩勇［4］根据辨识残差对数据集进行加权剔除异常数据，并且建立了三层循环网络来优化辨识过程，提高了辨识精度。

机械臂动力学模型辨识方法总体上可以分为三类：全参数动力学辨识、半参数动力学辨识和无模型动力学辨识。无模型动力学辨识是指不依赖动力学模型进行模型识别，如Arjan 利用增强稀疏谱高斯过程回归进行实时模型学习［25］，这种方法需要大量的数据集进行训练，增加了训练成本。全参数动力学辨识只利用动力学模型拟合数据集，这种方法在关节速度很低时，辨识误差较大，这是因为摩擦力模型无法完全拟合静摩擦力，给动力学辨识结果带来了较大偏差。半参数动力学模型辨识是指在参数化模型拟合的基础上再利用无参数模型进行补偿，整体动力学辨识由参数化模型和无参数模型两部分组成，如Jin hu 利用刚体动力学模型和多层感知器辨识机械臂动力学模型［26］，该方法的优点是结合了模型的泛化能力和无模型辨识方法的非线性拟合能力，能够有效地提高辨识精度。

本文针对九自由度超冗余模块化机械臂，使用迭代学习和神经网络补偿的方法辨识超冗余机械臂半参数动力学模型。创新之处在于：（1）在保证辨识精度的基础上，提出使用两层迭代优化学习网络辨识机械臂动力学参数，相比于三层迭代优化网络缩短了辨识时间；（2）本文将辨识残差输入给BP 神经网络进行模型补偿，利用半参数模型进一步提高了辨识精度。一系列实验结果表明，本文的半参数动力学辨识方法相比与传统的动力学辨识方法，辨识精度更高。

2 超冗余机械臂动力学建模

2.1 超冗余机械臂结构

超冗余机械臂总共包含9 个关节，每个关节都是模块化结构设计。图1 为超冗余机械臂的关节坐标系图。该机械臂的改进DH 建模参数如表1 所示。每个关节都包含一个增量式编码器和一个绝对式编码器，增量式编码器用于实时获取机械臂的关节角速度，绝对式编码器用于实时获取机械臂的关节位置。通过对关节速度进行滤波和中心差分可以获得关节角加速度。该九自由度超冗余机械臂没有关节力矩传感器，因此关节力矩通过减速比η和电机的额定力矩系数K计算得到，如式（1）所示。每个关节都安装了柔轮减速器，以此来保证关节输出端能够以较大的驱动力矩驱动关节运动，柔轮减速器减速比为160。该机械臂关节电机参数如表2 所示。

表1 超冗余机械臂改进DH 建模参数Tab.1 Hyper-redundant modular manipulator modified DH parameters

表2 超冗余机械臂关节电机参数Tab.2 Hyper-redundant modular manipulator joint motor parameters

图1 超冗余机械臂结构示意图Fig.1 Structure of hyper-redundant modular manipulator

2.2 超冗余机械臂动力学模型

超冗余机械臂的动力学模型如公式（2）所示：

其中：M(q)̈为关节空间下的连杆惯性项，C(q，̇)̇为关节空间下的连杆离心力和科里奥力项，G(q)为关节空间连杆重力项。q，̇，̈分别为关节位置、关节速度和关节加速度。Ff(̇)为关节摩擦力，Fe为外力。如果没有外力的情况下，可以去除外力部分，式（2）可以变为如下形式：

针对关节摩擦，传统的摩擦力模型为库伦-粘滞摩擦力模型，其摩擦力模型如下所示：

其中：fci代表关节i的库伦摩擦系数，fvi代表关节i的黏滞摩擦系数，Si为关节摩擦偏置。

为了更好地拟合实际的关节摩擦力，本文采用一种非线性摩擦模型，如公式（5）所示。黏滞摩擦力和关节速度并不是线性关系，而是与关节速度的α次幂成线性关系，这种非线性关系能够更好地拟合实际的关节摩擦力，提高辨识精度。

注意到，公式（5）中的机器人动力学模型并不是线性的，机器人的动力学参数以非线性的方式隐藏在M(q)，C(q，̇)，G(q)和Ff(̇)中。通常的做法是通过线性变换，将机器人动力学模型中与关节角、关节速度和关节加速度相关的项放到一个矩阵中，这个矩阵称为回归矩阵，同时将待辨识参数放到一个参数矩阵中，从而将动力学模型线性化，便于后续的辨识。动力学线性化的形式为：

其中：Y(q，̇，̈)∈Rn*i×N为回归矩阵，n代表关节个数，i代表采样次数，N代表机械臂动力学全参数个数，Φb为全参数集，Φb∈RN×1。Φb中每个关节i包含14 个参数个数，如下所示：

其中：Iixx，Iixy，Iixz，Iiyy，Iiyz，Iizz代表关节i在坐标系原点下的惯性参数，pix，piy，piz代表关节质心在关节坐标系下的位置，代表关节电机转子的转动惯量，mi代表关节质量。

通过公式（6）和公式（7）推导的回归矩阵Y中的列向量并不是完全相互独立的。为了减少辨识参数的个数，提高辨识精度，Khalil 等人［20］提出了最小参数集的概念，通过提取回归矩阵中的最大线性无关组，可以得到一个与最小参数集匹配的回归矩阵Y*。

其中，Φmin为最小参数集向量。

本文辨识的九自由度超冗余模块化机械臂共有126 个动力学参数，通过使用symPybotics 工具箱可以导出最小参数集，一共有91 个待辨识参数，因此Φmin∈R91×1。动力学辨识目标就是通过采集的力矩和关节角信息辨识Φmin矩阵。

3 激励轨迹设计

激励轨迹的设计包括激励轨迹模型的选取和激励轨迹参数的优化。为了提高数据的信噪比，本文选取由五阶傅里叶级数组成的周期性激励轨迹，通过让机械臂重复多次走激励轨迹并对数据取平均值，可以降低数据采集误差，同时也便于后期处理数据时选择合适的滤波频率。激励轨迹模型如公式（10）所示：

其中：q0i是激励轨迹的初始化关节角，qi，，分别为关节i的激励轨迹角度、角速度和角加速度，l为激励轨迹系数，ωf是激励轨迹的基频，ial，ibl是激励轨迹中待优化参数，t为激励轨迹运行时间。

在上述五阶傅里叶级数组成的激励轨迹中，一个关节包含11 个待优化参数，因此超冗余机械臂的激励轨迹总共包含99 个待优化参数。在优化过程中，为了保证机械臂的安全性，必须保证激励轨迹的关节角度、关节速度和关节加速度在可行性范围内。另外，为了保证机械臂启停时刻的安全性，激励轨迹还必须满足在启停时刻的关节速度和关节加速度为0。因此可以得出以下激励轨迹优化约束条件：

超冗余机械臂的关节运动范围极限如表3 所示。本文设计的激励轨迹周期为25 s，即tf=25 s。激励频率为f=0.04 Hz，基础频率ωf=2πf。实际实验采样周期为200 Hz，在优化激励轨迹过程中，为了提高计算速度，采样频率可以适当降低，本文在优化激励轨迹过程中的采样频率设计为20 Hz。

表3 超冗余机械臂关节运动极限Tab.3 Hyper-redundant manipulator joint motion restriction

表4 激励轨迹参数Tab.4 Identification trajectory parameters for hyper-redundant manipulator

在激励轨迹优化目标的选取上，采用经典的回归矩阵条件数作为优化目标，优化目标函数如公式（12）所示。回归矩阵条件数越低，机械臂沿着激励轨迹运动所覆盖范围就越大，从而可以提高辨识精度。

激励轨迹优化问题属于带约束条件下非线性优化问题。为了得到最优激励轨迹，通过使用Matlab 自带的遗传算法工具箱进行激励轨迹优化。激励轨迹经过遗传算法优化后的条件数为341，最优激励轨迹关节角位置曲线、激励轨迹关节角速度曲线和激励轨迹关节角加速度曲线分别如图2~图4 所示。从图中可以看出关节角、关节角速度和关节角加速度都在约束范围内，同时也实现了启停时刻关节角速度和关节角加速度为0 的约束目标。机械臂末端运动轨迹图如图5所示。

图2 最优激励轨迹关节角位置曲线Fig.2 Joint angle position of optimal excitation trajectory

图3 最优激励轨迹关节角速度曲线Fig.3 Joint angle velocity of optimal excitation trajectory

图4 最优激励轨迹关节角加速度曲线Fig.4 Joint angle acceleration of optimal excitation trajectory

图5 最优激励轨迹下机械臂末端运动轨迹Fig.5 Trajectory of manipulator’s end-effector in world coordinate system

4 动力学辨识

4.1 传统辨识方法

完成激励轨迹设计之后，通过让机械臂沿着激励轨迹运动，采集到关节角和关节力矩信息构建超正定方程：

上述公式中ti代表采样时间点，在一个激励轨迹周期内包含M次数据采样。根据采样电流和关节运动信息可以计算出τ*和Y*，根据最小二乘方法可以计算出最小参数集，如公式（14）所示：

使用最小二乘方法辨识误差为ε*=τ*-Y*ΦLS，辨识方差为，为了使辨识方差最小，Gautier［17］提出了加权最小二乘方法，如下所示：

其中，G=diag(Λ1，Λ2，…，Λn)，根据各个关节辨识的残差计算得到。

上述最小二乘算法和迭代加权最小二乘算法虽然简单高效，能够快速辨识得到最小参数集，但是存在一系列缺陷使得辨识结果不够准确，主要包括以下几个方面：（1）没有对数据集中的异常值进行处理，辨识结果容易受到异常值的干扰；（2）辨识得到的最小参数集可能不符合物理可行性；（3）泛化能力差，拟合别的运动轨迹时误差较大。为了解决上述问题，需要在使用迭代优化辨识算法之前建立机械臂动力学参数的物理可行性约束。

4.2 物理可行性约束

对于单关节i，每个关节的质量和转动惯量必须大于0，电机转子转动惯量、关节库伦摩擦系数和粘滞摩擦系数也必须大于0，物理可行性约束可以表述为：

公式（18）和公式（19）是基于全参数集推导得到的物理可行性约束，但是方程（13）构建的是基于最小参数集的回归矩阵，辨识得到的是最小参数集，为了判断辨识的最小参数集是否满足物理可行性约束，首先对回归矩阵和参数集进行分解［20］。

上述方程中Yd代表回归矩阵中与最小参数集计算无关的列。Pb，Pd代表Y矩阵的前b列和最后d列。Φd代表冗余参数。最小参数集和全参数集有如下关系：

其中:K是系数矩阵并且不唯一，且。

判断辨识得到的最小参数集是否符合物理可行性约束需要满足D(Φmin，Φd)>0，所有满足物理可行性约束的集合Ω定义为如下所示：

4.3 迭代优化

超冗余机械臂半参数动力学辨识整体算法流程如图6 所示。首先需要对采集数据进行预处理。本文采用四阶巴特沃斯滤波器对采集的关节力矩信息和关节角度信息进行滤波，再利用RLOESS 方法进行平滑处理。采用中心差分算法通过采集的关节角计算得到关节角速度和关节角加速度，如公式（24）所示：

图6 迭代优化算法流程框图Fig.6 Overall procedure of the iterative optimization algorithm

迭代优化整体包含两层循环，内层循环更新权重W是为了剔除异常值，外层循环是为了拟合关节摩擦力。首先为了估计出加权最小二乘的权重，需要在最小二乘的基础上计算出残差，即辨识误差，如式（25）所示：

其中：机械臂采样点为M，每次采样包含n个关节的信息，因此R中包含M×n个元素。迭代加权最小二乘的加权系数为G=diag(Λ1，Λ2，…，Λn)，Λi为机械臂的关节残差，根据下面公式计算得到：

其中：Zi为关节i的辨识残差，为关节i的最小参数集个数。加权最小二乘算法如下所示：

由于采样过程中不可避免会产生异常点，这些异常点对辨识结果有较大影响。为了剔除异常点，在回归矩阵和力矩中添加加权函数W，加权函数与辨识残差有关，如式（28）所示：

每次计算出一组最小惯性集参数，都会进行残差计算，如果加权函数没有收敛，那么将重新更新回归矩阵和对应的力矩值，更新完之后再重新计算知道加权函数W收敛。回归矩阵的更新如下所示：

需要注意的是，在使用迭代加权最小二乘算法辨识最小参数集的过程中，需要添加上一小节提出的物理可行性约束。这是一个带约束的凸优化问题，使用CVX 工具箱能够快速得到最优解。

相比于文献［3］中提出的迭代学习算法，本文将其第一层循环和第二层循环合二为一，不再计算协方差，不再判断协方差是否收敛，直接使用加权最小二乘替代协方差循环。后续通过实验结果可以验证，本文提出的迭代优化网络在保证辨识精度的基础上，具有更快的收敛速度。

迭代优化的外层循环是为了保证摩擦模型的准确性，通过将关节摩擦力相关的动力学参数置为0 可以计算关节摩擦力，如式（31）所示：

其中，Φb代表将摩擦力参数置为0 的最小参数集。

根据计算得到的关节摩擦力以及关节角速度信息，可以辨识关节摩擦模型参数。关节摩擦模型总共包含4 个待辨识参数：fci，fvi，Si，αi，本文使用matlab 中的非线性多元函数最小值求解fmincon 函数进行关节摩擦模型优化。

4.4 神经网络补偿

为了进一步减少模型误差，使用BP 神经网络来对建模非线性误差进行补偿。BP 神经网络的结构如图7 所示。该神经网络包含输入层、隐藏层和输出层。本文使用关节速度作为训练输入，训练输出为动力学辨识误差。隐藏层选用双曲正切函数作为激活函数：，隐藏层层数为36。神经网络补偿量记为τBPNN=N(q̇)。总体辨识的超冗余机械臂半参数动力学模型包含两部分：刚体动力学模型组成的线性部分和神经网络补偿组成的非线性部分，可以表达为如式（33）所示的形式：

图7 BP 神经网络结构图Fig.7 Architectural graph of the BP neural network

5 实 验

5.1 实验平台

为了验证本文提出的基于迭代优化的动力学辨识算法的有效性，使用实验室自主研发的九自由度超冗余模块化机械臂进行动力学模型辨识。整体辨识实验平台如图8 所示。其中硬件平台包括PC 上位机、倍福控制器、通讯端子模块，电机驱动器和机械臂。软件平台包括TwinCAT运动控制系统和EtherCAT 通讯协议。实验中，通过上位机将激励轨迹编写在VS 环境Twin-CAT 3 软件中，并且将关节角与NC 轴绑定，通过编译将控制程序导入到倍福控制器驱动关节沿着激励轨迹运动。倍福控制器作为主站，其余模块作为从站，通过主站发报文实现电机伺服控制，通信周期为5 ms，保证了通信的实时信。

图8 超冗余机械臂动力学辨识实验平台Fig.8 Experiment platform of hyper-redundant manipulator dynamic model identification

本文优化的激励轨迹运行一个周期时间为25 s，实验时让机械臂连续走五遍激励轨迹，通过取平均值的方法降低数据采集误差和数据噪声。因此，一次实验持续时间为125 s，总共采集25 000 组关节角和关节电流信息，机械臂的运动轨迹如图9 所示。考虑到神经网络进行模型补偿需要大量的数据集进行训练，因此实际中让机械臂连续走了五组轨迹，其中一组实验数据用来辨识参数化动力学模型，一组轨迹用来验证模型的准确性，其余数据用来训练神经网络。

图9 机械臂运行轨迹Fig.9 Experimental trajectory of the manipulator

5.2 实验结果

辨识过程中，摩擦模型α的初始值设置为1，加权函数初始值也为1。实验中采用零相位巴特沃斯滤波器对采集的关节角和关节力矩进行滤波，通带截止频率和阻带截止频率分别设置为0.12 Hz 和0.2 Hz。

为了验证本文提出算法的有效性，使用自主研发的九自由度超冗余机械臂进行动力学模型辨识。分别将本文提出的两层迭代优化算法和迭代优化与神经网络补偿的半参数辨识算法与传统的最小二乘算法、加权最小二乘算法、文献［3］算法进行对比，从而验证提出的算法的有效性和优越性。

图10 记录了内层循环迭代收敛情况。内层循环函数W中的元素为0 或1，1 代表该数据有效，反之0 代表无效数据。为了判断内层循环的收敛情况，本文选用‖ΔW‖2作为判断依据，其中ΔW=Wt-Wt-1，ΔW能够说明加权函数的变化幅度，当加权函数基本不发生变化时，就可以认为内层迭代已经收敛。从图10 中可以看出，初始时刻‖ΔW‖2=31.225，这个值之所以这么大，是因为辨识得到的动力学模型与实际模型具有较大偏差，不能很好地拟合实际模型。随着迭代的进行，‖ΔW‖2不断减小直至收敛稳定。稳定时刻，‖ΔW‖2大概维持在7.81 左右，这说明每次迭代权重系数已经变化很小，参数趋于稳定。内层循环想要达到稳定需要迭代20~30 次。

图10 内层循环迭代收敛情况Fig.10 Iteration convergence of inner loop

外层循环收敛判断依据是Δα，图12 反映了外层摩擦模型α的收敛情况。从图中可以看出，初始时刻各个关节的摩擦模型参数α=1，随着迭代的进行，摩擦模型参数逐渐收敛到稳定值并且不再变化。在这个过程中，摩擦模型能够更好地匹配采集到的数据，逐步提高模型辨识精度。摩擦力拟合结果如图11 所示。从图中可以观察到，在低速情况下摩擦力不稳定，摩擦模型也无法完全拟合关节摩擦。这是因为低速属于预滑动区域，静摩擦力不稳定，受温度、负载、构型、噪声等因素的影响摩擦力具有较大误差，这个误差属于非线性影响因素。经过迭代，算法找到了最能够反映关节摩擦模型的系数α，并且也趋于稳定。

图11 各关节摩擦力拟合结果Fig.11 Fitting results of joint friction force

图12 关节摩擦系数迭代收敛情况Fig.12 Convergence of the joint friction model α

各个关节的辨识力矩残差均方根（RMS）值如表5 所示。使用本文提出的二层迭代优化网络辨识得到的最小参数集如表6 所示，该辨识结果均采用国际单位制。为了验证本文提出的算法的优越性，与传统的最小二乘算法、加权最小二乘算法和文献［3］的辨识算法进行了比较。不加神经网络补偿，本文提出的算法关节RMS 值之和为35.606 3 N·m，最小二乘算法和加权最小二乘算法的辨识结果分别为52.998 9 N·m 和46.704 8 N·m，本文的算法相较于传统的这两种算法RMS 值之和分别提高了32.81% 和23.76%文献［3］中的算法辨识结果为35.564 7 N·m，本文提出的算法辨识结果与文献［3］提出的三层迭代优化网络相比，辨识的结果几乎一样。然而，本文的算法解算时间只需要20 min，文献［3］提出的算法的解算时间需要两个小时，这是因为本文提出的算法少了文献［3］中的协方差内层循环，通过使用加权最小二乘算法能够实现协方差循环层几乎一样的效果，因此本文提出的算法具有更高的计算效率。

表5 不同方法辨识的力矩残差均方根（RMS）值1*Tab.5 Identify torque residual root mean square （RMS） values by various methods

表6 动力学参数辨识结果 1*Tab.6 Dynamic parameter identification results

为了提高动力学模型辨识精度，本文使用BP 神经网络对参数化辨识结果进行进一步补偿。实验过程中总共跑了五组轨迹，采集了25 000 组实验数据，使用其中的20 000 组实验数据训练BP 神经网络，剩余5 000 组实验数据用来验证神经网络补偿的有效性。神经网络的输入层数为9，隐藏层层数36，输出层为9。训练的输入集为关节角速度，输出集为模型辨识误差。

经过神经网络补偿之后，各个关节的力矩残差均方根之和由原来的35.606 3 N·m 提高到了27.216 7 N·m，总共提高了23.56%。由此说明了模型补偿的有效性。基于本文提出的半参数动力学模型算法辨识得到的模型验证结果如图13 所示，从上到下依次为1~9 关节。从图中可以看到，辨识的关节力矩与实验采集的关节力矩重合程度较好，验证了辨识结果的准确性。

6 结 论

本文针对九自由度超冗余模块化机械臂，设计了基于迭代优化和神经网络补偿的半参数动力学辨识算法。分别使用最小二乘算法、加权最小二乘算法、三层网络迭代优化算法和本文提出的算法进行了参数辨识，并对辨识结果进行了讨论分析。实验结果表明，相比于传统的最小二乘算法和加权最小二乘算法，本文的算法辨识结果关节辨识力矩残差均方根（RMS）之和分别提高了32.81%和23.76%，半参数动力学模型相比于全参数动力学模型力矩残差均方根之和提高了23.56%。所提出的辨识算法能够有效准确地辨识机械臂的动力学模型。