一种基于动力学模型的机器人自适应阻抗控制系统的制作方法

本发明属于工业机器人控制相关技术领域，更具体地，涉及一种基于动力学模型的机器人自适应阻抗控制系统。

背景技术：

随着科技的发展，工业机器人开始广泛用于智能制造和航空航天等各个领域。对机器人需求量和精度要求也越来越高，传统的控制方法都是对机器人关节的位置进行控制，可能导致机器人关节力矩超限或者是末端的接触力过大，造成搬运过程中工件损伤、误差存在的情况下零件装备失败或者切削加工精度低等问题，更严重的可能使机器人末端或者连杆受损。特别的，对某些复杂多变的工作环境或者高精度要求的装配工作，位置控制难以达到加工装配精度的要求。

目前，工业机器人系统常用的基于力矩的控制方法主要包括阻抗控制及力位混合控制。最简单的方法是通过在机器人末端安装力传感器或者力矩传感器来获取接触力矩，但是该方法提高了机器人结构的复杂度和成本。

其中，阻抗控制又分为基于位置的阻抗控制和基于力矩的阻抗控制，基于位置的阻抗控制是对机器人末端的位置和速度进行协调，无法控制末端力矩；基于力矩的阻抗控制常用于机器人与环境接触的情况，其在阻抗控制在机器人末端存在接触力时的应用价值较大，但是当接触力矩过大会影响pd控制器模型，降低了控制精度。相应地，本领域存在着发展一种精度较高的基于动力学模型的机器人自适应阻抗控制系统的技术需求。

技术实现要素：

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于动力学模型的机器人自适应阻抗控制系统，其基于现有阻抗控制特点，为了解决控制精度较低的问题，研究及设计了一种精度较好的基于动力学模型的机器人自适应阻抗控制系统。所述控制系统基于所建立的机器人动力学模型对机器人进行柔顺控制，不需要安装力矩传感器，可以通过动力学模型和电机电流估算外力矩，并基于自适应控制策略根据位置误差、速度误差、外力矩及给定的稳定外力矩的值来获得新的阻抗控制器的控制器参数，以实现位置和力矩的平衡控制，自适应性较好，精度较高。

为实现上述目的，按照本发明的一个方面，提供了一种基于动力学模型的机器人自适应阻抗控制系统，所述控制系统包括预处理模块、阻抗控制器及自适应策略控制模块，其中：

所述预处理模块用于根据机器人关节的库伦摩擦力和粘滞力、采用牛顿-欧拉法构建机器人动力学模型，并基于所述机器人动力学模型采用粒子群算法辨识机器人的动力学参数及摩擦力系数，进而对所述机器人动力学模型进行优化；同时，所述预处理模块还用于将优化后的机器人动力学模型传输给所述阻抗控制器；

所述阻抗控制器用于根据所述机器人动力学模型来实现机器人末端的力和位置的柔顺控制，并将自身计算得到的位置误差、速度误差及外力值传输给所述自适应策略控制模块；

所述自适应策略控制模块用于在机器人末端受到外力时根据接收到的数据及自身预存的数值进行判断，并根据判断结果相应地计算出控制器参数，同时将所述控制器参数传输给所述阻抗控制器，以更新所述阻抗控制器的参数，实现了机器人位置和力矩的平衡控制。

进一步地，所述预处理模块是基于所述机器人动力学模型，采用傅里叶级数作为激励轨迹来辨识机器人的动力学参数及摩擦力系数的。

进一步地，基于所述机器人动力学模型计算得到的理论关节力矩为：

式中，q,分别表示机器人关节角度、角速度和角加速度；表示机器人动力学模型预测的惯性矩；表示机器人动力学模型预测的离心力与科式力矩；表示机器人动力学模型计算的重力矩；表示机器人动力学模型预测的粘滞摩擦力矩和库伦摩擦力矩，d为粘滞摩擦系数，μ为库伦摩擦系数。

进一步地，机器人的连杆i的动力学参数为：

λi＝[mi,si,x,si,y,si,z,ii,xx,ii,yy,ii,zz,ii,xy,ii,xz,ii,yz,di,μi]^t

式中，i＝1,2…,n，n为关节数量；mi表示连杆i的质量；si,x表示连杆i的质心在xi方向的分量；si,y表示连杆i的质心在yi方向的分量；si,z表示连杆i的质心在zi方向的分量，ii,xx＝∫∫∫v(yi²+zi²)ρdυ、ii,yy＝∫∫∫v(xi²+zi²)ρdυ、ii,zz＝∫∫∫v(xi²+yi²)ρdυ分别表示机器人负载绕末端坐标轴xi,yi,zi的质量惯性矩；绕xiyi,xizi,yizi方向的惯性矩分别表示为ii,xy＝∫∫∫vxiyiρdυ、ii,xz＝∫∫∫vzixiρdυ、ii,yz＝∫∫∫vyiziρdυ；ρ表示连杆密度；υ表示连杆体积；di为关节i的粘滞摩擦系数；μi为关节i的库伦摩擦系数。

进一步地，所述阻抗控制器包括位置及速度控制模块、预测力矩前馈模块及接触力矩反馈模块，其中，所述位置及速度控制模块用于计算机器人在笛卡尔空间的位置误差和速度误差，进而依据计算得到的位置误差及速度误差计算消除对应的位置误差及速度误差需要给机器人末端施加的作用力，再将所述作用力转换为关节空间的作用力矩，进而控制机器人运动以减少机器人末端的位置误差及速度误差。

进一步地，工作时，所述预测力矩前馈模块基于动力学计算机器人的理论关节力矩，并将计算得到的理论关节力矩传输给所述接触力矩反馈模块；所述接触力矩反馈模块用于根据机器人的电机的电流计算得到机器人的实际关节力矩，并基于所述实际关节力矩及所述理论关节力矩来计算机器人末端在笛卡尔空间受到的外力，继而将所述外力传输给所述自适应策略控制模块，同时依据所述外力来控制所述机器人。

进一步地，所述阻抗控制器通过机器人的位姿变换矩阵和雅克比矩阵来实现关节空间的位置、速度及力矩到笛卡尔空间的位置、速度及力矩的转换，从而实现对机器人末端位置、速度和力矩的控制。

进一步地，机器人的动力学方程为：

式中，j表示机器人雅克比矩阵；kp、kd分别表示阻抗控制器的刚度系数矩阵和阻尼系数矩阵；xd、分别表示机器人末端的理论位置和理论速度；x、分别表示机器人末端的实际位置和实际速度；ke表示接触力系数矩阵；xe表示机器人末端表面未发生弹性形变时的位置。

进一步地，所述自适应策略控制模块在机器人末端有外力时开始作用，其基于bp神经网络算法计算机器人的控制器参数，并将得到的所述控制器参数传输给所述阻抗控制器。

进一步地，机器人末端位置误差、速度误差、力和力矩误差以及外力矩的稳定值作为该bp神经网络的输入信号f'＝(j^t)^-1·τ为机器人末端的实际力；为机器人末端的理论力，fε为机器人末端外力的稳定值；xd、分别表示机器人末端的理论位置和理论速度；x、分别表示机器人末端的实际位置和实际速度。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，本发明提供的基于动力学模型的机器人自适应阻抗控制系统主要具有以下有益效果：

1.本发明基于动力学模型预测机器人力矩的方式进行控制，可以有效减少跟随误差，提高了精度；同时，能够减少阻抗控制器参数取值对控制效果的影响，只需在机器人末端受到外力时进行控制器参数的自适应控制，可以有效减少机器人控制器的计算复杂度，提高了速率。

2.所述阻抗控制器通过机器人的位姿变换矩阵和雅克比矩阵来实现关节空间的位置、速度及力矩到笛卡尔空间的位置、速度及力矩的转换，无需安装力矩传感器，成本较低。

3.所述自适应策略控制模块在机器人末端有外力时开始作用，其基于bp神经网络算法计算机器人的控制器参数，并将得到的所述控制器参数传输给所述阻抗控制器，其中，将机器人末端外力矩的稳定值作为输入参数，可以根据机器人末端接触的环境或工作性质的不同自主设置外力矩的稳定值，实现阻抗控制器参数的自适应调节。

4.所述位置及速度控制模块计算机器人在笛卡尔空间的位置误差和速度误差，进而依据计算得到的位置误差及速度误差计算消除对应的位置误差及速度误差需要给机器人末端施加的作用力，再将所述作用力转换为关节空间的作用力矩，进而控制器机器人运动以减少机器人末端的位置误差及速度误差，提高了控制精度，适用性较强，灵活性较好。

5.本发明提供的机器人自适应阻抗控制系统的结构简单，易于实施，有利于推广应用。

附图说明

图1是本发明较佳实施方式提供的基于动力学模型的机器人自适应阻抗控制系统的阻抗控制器的控制框图；

图2是图1中的基于动力学模型的机器人自适应阻抗控制系统的局部控制框图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

请参阅图1及图2，本发明提供的基于动力学模型的机器人自适应阻抗控制系统，所述控制系统包括预处理模块、阻抗控制器及自适应策略控制模块，所述预处理模块在考虑机器人关节的库伦摩擦力和粘滞力的情况下，基于牛顿-欧拉法建立机器人动力学模型，并基于所述机器人动力学模型及粒子群算法辨识机器人的动力学参数及摩擦力系数，以优化所述机器人动力学模型。同时，所述预处理模块还将所述机器人动力学模型传输给所述阻抗控制器。

所述预处理模块基于所述机器人动力学模型，并采用傅里叶级数作为激励轨迹来辨识机器人的动力学参数及摩擦力系数，继而对所述机器人动力学模型进行优化。其中，基于所述机器人动力学模型计算的理论关节力矩为：

式中，q,分别表示机器人关节角度、角速度和角加速度；表示机器人动力学模型预测的惯性矩；表示机器人动力学模型预测的离心力与科式力矩；表示机器人动力学模型计算的重力矩；表示机器人动力学模型预测的粘滞摩擦力矩和库伦摩擦力矩；d为粘滞摩擦系数；μ为库伦摩擦系数。

所述预处理模块采用粒子群算法辨识机器人的动力学参数和摩擦力系数时，机器人的连杆i的动力学参数可以表示为：

λi＝[mi,si,x,si,y,si,z,ii,xx,ii,yy,ii,zz,ii,xy,ii,xz,ii,yz,di,μi]^t

式中，i＝1,2…,n，n为关节数量；mi表示连杆i的质量；si,x表示连杆i的质心在xi方向的分量；si,y表示连杆i的质心在yi方向的分量；si,z表示连杆i的质心在zi方向的分量；ii,xx＝∫∫∫v(yi²+zi²)ρdυ、ii,yy＝∫∫∫v(xi²+zi²)ρdυ、ii,zz＝∫∫∫v(xi²+yi²)ρdυ分别表示机器人负载绕末端坐标轴xi,yi,zi的质量惯性矩；绕xiyi,xizi,yizi方向的惯性矩分别表示为ii,xy＝∫∫∫vxiyiρdυ、ii,xz＝∫∫∫vzixiρdυ、ii,yz＝∫∫∫vyiziρdυ，ρ表示连杆密度；υ表示连杆体积；di为关节i的粘滞摩擦系数；μi为关节i的库伦摩擦系数。

所述阻抗控制器基于所述机器人动力学模型来实现机器人末端的力和位置的柔顺控制，并将自身计算得到的位置误差、速度误差及外力值传输给所述自适应策略控制模块。

所述阻抗控制器包括位置及速度控制模块、预测力矩前馈模块及接触力矩反馈模块。所述位置及速度控制模块计算机器人在笛卡尔空间的位置误差和速度误差，进而依据计算得到的位置误差及速度误差计算消除对应的位置误差及速度误差需要给机器人末端施加的作用力，再将所述作用力转换为关节空间的作用力矩，进而控制机器人运动以减少机器人末端的位置误差及速度误差。

所述预测力矩前馈模块基于动力学计算机器人的理论关节力矩，并将计算得到的理论关节力矩传输给所述接触力矩反馈模块。所述接触力矩反馈模块根据机器人的电机的电流计算得到机器人的实际关节力矩，并基于所述实际关节力矩及所述理论关节力矩来计算机器人末端在笛卡尔空间受到的外力，继而将所述外力传输给所述自适应策略控制模块，同时依据所述外力来控制所述机器人，从而保证外力矩过大时机器人向外力矩减小的方向运动，实现了机器人的保护。

所述阻抗控制器通过机器人的位姿变换矩阵和雅克比矩阵来实现关节空间的位置、速度及力矩到笛卡尔空间的位置、速度及力矩的转换，从而实现对机器人末端位置、速度和力矩的控制。

τ'为机器人运行过程中的实际关节力矩，所述阻抗控制器采用直接读取伺服电机的电流，然后通过电流乘以力矩电流比例系数的方式来获取该实际关节力矩。其中，机器人动力学方程表示为：

式中，τ表示根据控制率计算的关节驱动力矩；表示预测力矩前馈模块对应的力矩，即理论关节力矩；表示位置及速度控制模块对应的力矩；xd、分别表示机器人末端的理论位置和理论速度；x、分别表示机器人末端的实际位置和实际速度；kp、kd分别表示阻抗控制器的刚度系数矩阵和阻尼系数矩阵；j表示机器人雅克比矩阵；表示接触力矩反馈模块对应的力矩，τ'表示根据电机电流计算的实际关节力矩；ke表示接触力系数矩阵。

具体地，所述阻抗控制器首先利用机器人的位姿变换矩阵和雅克比矩阵将机器人在关节空间的角度和角速度转换为机器人末端在笛卡尔空间的位姿和速度，并计算机器人末端广义力；然后再利用机器人的雅克比矩阵计算得到机器人的理论关节力矩，如此可以实现对机器人末端的位姿和速度的控制。

同时，所述阻抗控制器利用机器人的雅克比矩阵将基于机器人动力学模型计算的理论关节力矩和机器人的实际关节力矩转换为机器人末端所受的外力和力矩，以得到机器人末端所受的外力矩，从而实现机器人末端位置和力矩的协调控制。

其中，当基于机器人动力学模型计算的理论关节力矩准确时，存在则机器人动力学方程可以表示为：

式中，为接触力矩，与机器人末端形变有关，可以表示为：

τe＝j^t[γ(x-xe)]。

式中，xe表示机器人末端表面未发生弹性形变时的位置；γ为弹性形变函数，当机器人末端发生弹性形变时，机器人末端受外力与形变量大小有关，因此弹性形变函数表示为

进而可得到新的机器人动力学方程：

当xd-x＝0，x-xe＝0时，机器人实际角度、角速度分别与理论角度、角速度相等，末端无弹性形变，此时机器人只存在预测力矩前馈作用，处于平衡状态。

所述自适应策略控制模块根据接收到的数据及自身预存的数值进行判断，并根据判断结果相应的计算出控制器参数，同时将所述控制器参数传输给所述阻抗控制器，以更新所述阻抗控制器的参数，实现了对所述阻抗控制器的控制。本实施方式中，所述自适应策略控制模块在机器人末端受到外力时开始作用，其基于bp神经网络算法计算机器人的控制器参数，并将得到的所述控制器参数传输给所述阻抗控制器。

其中，机器人末端位置误差、速度误差、力和力矩误差以及外力矩的稳定值(预先设定值)作为该bp神经网络的输入信号f'＝(j^t)^-1·τ为机器人末端的实际力；为机器人末端的理论力；fε为机器人末端外力的稳定值，根据机器人末端接触环境或者工作形式的不同，可以自主设置外力矩的稳定值。

再者，wlj表示bp神经网络的输入层第l个输入到第j个输出的权值，输入层的输出即隐藏层的输入，表示为输入层神经元所有输入的加权之和：

bp神经网络的隐藏层的神经元采用非线性激发函数sigmod激发σj得到隐藏层输出σ'j，有：

隐藏层的输出即输出层的输入，wjk表示输出层第j个输入到第k个输出的权值，输出层神经元输出yn(t)表示为：

bp神经网络输出与理想输出的误差e(t)和误差性能函数e(t)可以表示为：

e(t)＝y(t)-yn(t)

输出层及隐藏层的连接权值wjk学习算法的公式为：

wjk(t+1)＝wjk(t)+wjk+α(wjk(t)-wjk(t))。

隐藏层及输入层连接权值wlj学习算法的公式为：

wlj(k+1)＝wlj(k)+wlj+α(wlj(k)-wlj(k-1))

式中，η为学习速率，α动量因子，η∈[0,1]，α∈[0,1]。

本实施方式中，所述自适应策略控制模块在机器人末端存在接触力时开始作用，其根据给定的外力矩的稳定值来周期性地计算机器人的控制器参数，并将计算得到的控制器参数传输给所述阻抗控制器，以更新所述阻抗控制器的参数。

以下以具体实施例来对本发明进行进一步的详细说明。

实施例

本实施例以华中数控605型机器人为例来对本发明进行进一步的说明，其中华中数控605型机器人的关节数n＝6。具体地，所述预处理模块在考虑关节库伦摩擦力和粘滞阻力的情况下、基于牛顿-欧拉法建立了华中数控605型机器人的动力学模型，基于该动力学模型计算获得的理论关节力矩为：式中，q,分别表示机器人关节角度、角速度和角加速度；表示动力学模型预测的惯性矩；表示动力学模型预测的离心力与科式力矩；表示动力学模型计算的重力矩，表示动力学模型预测的粘滞摩擦力和库伦摩擦力，d为粘滞摩擦系数；μ为库伦摩擦系数。

进而，所述预处理模块基于该动力学模型，采用傅里叶级数作为激励轨迹，基于粒子群算法来辨识机器人的动力学参数及摩擦力系数，连杆i的动力学参数可以表示为：λi＝[mi,si,x,si,y,si,z,ii,xx,ii,yy,ii,zz,ii,xy,ii,xz,ii,yz,di,μi]^t，式中，i＝1,2…,6；mi表示连杆i的质量；si,x表示连杆i的质心在xi方向的分量；si,y表示连杆i的质心在yi方向的分量；si,z表示连杆i的质心在zi方向的分量；ii,xx＝∫∫∫v(yi²+zi²)ρdυ、ii,yy＝∫∫∫v(xi²+zi²)ρdυ、ii,zz＝∫∫∫v(xi²+yi²)ρdυ分别表示机器人负载绕末端坐标轴xi,yi,zi的质量惯性矩；绕xiyi,xizi,yizi方向的惯性矩分别表示为ii,xy＝∫∫∫vxiyiρdυ、ii,xz＝∫∫∫vzixiρdυ、ii,yz＝∫∫∫vyiziρdυ，其中ρ表示连杆密度；υ表示连杆体积；di为关节i的粘滞摩擦系数；μi为关节i的库伦摩擦系数。

所述阻抗控制器直接读取伺服电机的电流，然后通过电流乘以力矩电流比例系数的方式来获取实际关节力矩τ。其中，机器人的动力学方程表示为：式中，xd、分别表示机器人末端的理论位置和理论速度；x、分别表示机器人末端的实际位置和实际速度；kp、kd分别表示刚度系数矩阵和阻尼系数矩阵；j表示机器人雅克比矩阵；ke表示接触力系数矩阵，在本实例中初始值取：

由于kp、kd、ke为对角线矩阵，阻抗控制器的控制器参数可以表示为三个矩阵对角线元素组成的18×1矩阵。

所述阻抗控制器利用机器人的位姿变换矩阵和雅克比矩阵将机器人在关节空间的角度和角速度转换为机器人末端在笛卡尔空间的位姿和速度，进而计算得到机器人末端广义力；然后再利用雅克比矩阵来得到机器人的理论关节力矩，以实现对机器人末端的位姿和速度的控制。

所述阻抗控制器还根据机器人的雅克比矩阵将所述理论关节力矩及所述实际关节力矩转换为机器人末端所受到的力和力矩，以得到机器人末端所受的外力矩，从而实现机器人末端位置和力矩的协调控制。

当计算得到的理论关节力矩准确时，存在则机器人动力学方程可以表示为：其中，为接触力矩，与机器人末端形变有关，可以表示为：

τe＝j^t[γ(x-xe)]

式中，xe表示机器人末端表面未发生弹性形变时的位置；γ为弹性形变函数，当机器人末端发生弹性形变时，机器人末端受外力与形变量大小有关，因此弹性形变函数表示为

进而可得到新的机器人动力学方程：

当xd-x＝0，x-xe＝0时，机器人的实际角度、角速度与理论角度、角速度相等，末端无弹性形变，此时机器人只存在预测力矩前馈作用，处于平衡状态。

本实施方式中，所述自适应策略控制模块采用bp神经网络自适应控制算法来训练所述阻抗控制器的控制参数模型，并根据机器人各个时刻不同的位置误差、速度误差、力矩误差及外力矩稳定值来计算对应的控制器参数，并将得到的控制器参数传输给所述阻抗控制器。

其中，机器人末端位置误差、速度误差、力和力矩误差以及外力矩的稳定值作为bp神经网络的输入信号其中f'＝(j^t)^-1·τ为机器人末端的实际力；为机器人末端的理论力；fε＝0.4n·m为机器人末端外力矩的稳定值。本实施例中，wlj表示bp神经网络的输入层第l个输入到第j个输出的权值，l＝24，j＝18，输入层的输出即隐藏层的输入，表示为输入层神经元所有输入的加权之和：

其中，bp神经网络的隐藏层的神经元采用非线性激发函数sigmod函数激发σj得到隐藏层输出σ'j，有：

隐藏层的输出即输出层的输入，wjk表示输出层第j个输入到第k个输出的权值，本实例中k＝18，输出层神经元输出yn(t)表示为：

网络输出与理想输出的误差e(t)和误差性能函数e(t)可以表示为：

e(t)＝y(t)-yn(t)

输出层及隐藏层的连接权值wjk学习算法公式为：

wjk(t+1)＝wjk(t)+wjk+α(wjk(t)-wjk(t))。

隐藏层及输入层连接权值wlj学习算法公式为：

wlj(k+1)＝wlj(k)+wlj+α(wlj(k)-wlj(k-1))

式中，η＝0.8为学习速率，α＝0.6为动量因子。机器人按给定激励轨迹运动时与环境接触存在接触力作用，所述自适应策略控制模块开始作用及计算得到控制器参数，并将得到的控制器参数发送给所述阻抗控制器。

本发明提供的基于动力学模型的机器人自适应阻抗控制系统，所述控制系统利用机器人动力学模型预测各时刻的机器人关节力和力矩，防止运行中关节力矩超限造成的机器人损伤，同时可以减少机器人加速过程的时间，以提高运动性能。此外，基于机器人动力学模型的控制器可以优化机器人的运动轨迹，实现了高精度的控制；同时，该控制系统可以用于工业机器人工作过程中需要与环境交互的情况，通过协调机器人末端的位置和力矩来实现柔顺控制。

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