CN112809666B - 一种基于神经网络的5-dof机械臂力位跟踪算法 - Google Patents

Abstract

本发明一种基于神经网络的5‑DOF机械臂力位跟踪算法，通过定义机械臂中各个关节点、连杆长度，建立连杆坐标系，通过力传感器得到末端接触力，建立末端接触力与机械臂参考坐标系之间的切换数学模型，描述了末端接触力在参考坐标系下的矩阵，为末端接触力转换为各个关节力矩提供了前提基础；然后，给出了一般的机械臂动力学方程，建立了机械臂第5关节状态变量与其他关节状态变量之间的数学模型，建立带有约束力的动力学方程；最后，利用惯性矩阵估计惯性矩阵，并利用RBF神经网络拟合其他项，给出了时变输出约束状态的神经网络自适应力位控制方法。本发明能够解决传统的力/位混合控制无法克服未知模型的影响，增加算法收敛性，降低动力学建模复杂度。

Description

一种基于神经网络的5-DOF机械臂力位跟踪算法

技术领域

本发明涉及一种基于神经网络的5-DOF机械臂力位跟踪算法，属于机械臂的控制算法技术领域。

背景技术

随着人工智能化的发展，机器人越来越普及，因此冗余机械臂控制系统的设计与研究成为该领域重要的研究方向。机械臂在运动过程中可分为自由运动和受力运动，其中，自由运动主要考虑的是跟踪关节位置、速度状态，目前工业界的机械臂一般都是自由运动，主要应用场合为喷漆、点焊等场景。受力运动需要考虑到与环境进行交互作用的情况(即机械臂在运动的情况下与环境接触从而导致机械臂受到环境给的接触力)，需要对机械臂受到的力和位置同时跟踪，一般应用在打磨、搬运、装配以及钻孔等场合。如在抓取场合，机械臂在运动过程中由于没有对力进行控制，可能会造成对现场的破坏从而导致任务失败，因此机械臂的力/位混合控制的深入研究随之成为热点。

在机械臂的力位混合控制研究中，柔顺控制已经成为了研究热点，但是依赖于机械臂动力学建模，且随着机械臂自由度的增加，动力学建模越复杂。为了克服动力学建模的复杂度，一种时变延时估计的动力学建模随着产生，可用于逼近任意的动力学建模，具有较强的泛华性。

由于估计的动力学建模在实际应用中存在着大量的不确定因素，因此对控制算法的收敛性有一定的影响。为了克服不确定因素的影响，诸如自适应控制、神经网络控制、模糊控制、反步控制等已经被人深入研究。RBF神经网络相比多层前馈BP网络，其具有良好的泛华能力，网络结构简单，以避免不必要和冗长的计算而获得人们的认同。多数数据表明，RBF神经网络能够在一个紧凑和任意精度下，逼近非线性函数，因此利用RBF神经网络实现模型位置部分的自适应逼近，可有效地降低模糊增益，加快系统收敛性。然而随着机械臂自由度的增加，机械臂关节之间的耦合性也随之变得复杂，从而影响机械臂各关节之间的控制。面临此种情况，一种具有时变约束输出状态的神经网络自适应控制算法随之产生，该算法通过限制输出状态，从而使神经网络拟合方向具有明确性，提高了未知模型拟合的精确性，克服了机械臂关节之间的耦合性带来的问题，从而加快了算法的收敛性。

发明内容

有鉴于此，本发明提供了一种基于神经网络的5-DOF机械臂力位跟踪算法，用以改善机械臂动力学建模复杂以及关节控制精度低的问题。

一种基于神经网络的5-DOF机械臂力位跟踪算法，该算法基于5-DOF机械臂末端执行器固定连接力传感器，末端执行器夹持物体与环境接触时，力传感器输出力信息并与各关节状态信息一起反馈给控制器。

该算法通过定义机械臂中各个关节点、连杆长度，建立连杆坐标系，然后通过力传感器得到末端接触力，计算得出末端接触力在机械臂参考坐标系下的力信息；然后建立了带有约束力的动力学方程；最后，利用估计惯性矩阵估计惯性矩阵，并利用RBF神经网络拟合其他项，在此前提下，给出了具有时变输出约束状态的神经网络自适应力位控制方法，达到同时跟踪力位的效果。

进一步地，所述控制算法的计算步骤如下：

第一步：建立坐标系

机械臂的基座上建立参考坐标系{H}，在末端执行器的舵机上建立接触力测量坐标系{e}使其与末端执行器坐标系重合，末端夹持物体与环境的接触方向为测量坐标系的Z轴。

第二步：求解机械臂末端执行器接触力在参考坐标系下的力信息

机械臂末端与环境接触时，其接触力在测量坐标系{e}中为f_e，接触力在参考坐标系下的表达式为：

其中，θ为机械臂的关节角，f_H为变换成参考坐标系下的接触力，

为坐标系{e}相对于参考坐标系的齐次变换矩阵：

则式(1)可变为：

其中，n＝[n_x，n_y，n_z]，o＝[o_x，o_y，o_z]，a＝[a_x，a_y，a_z]。通过求解式(3)方程组可得到在参考坐标系下的接触力f_H。

第三步：给出机械臂的一般动力学方程，并建立带有约束力的动力学方程

给出一般的机械臂动力学方程表达式为；

其中，W(θ)为5×5的正定惯性矩阵，

为关节角加速度，

为关节角加速度，

为科里奥利力和向心力的5×5的矩阵，G(θ)表示5×1的重力矩阵，τ_d为未知外加扰动，τ_f为约束力矩，τ为输入力矩。τ_f，可表示为：

τ_f＝J^T(θ)λ (5)

其中，λ为f_H在约束方向上的力，J(θ)为约束条件雅克比矩阵，表达式如下：

其中，φ(h(θ))＝φ(x)＝0为位置约束方程。

由于5-DOF机械臂受到一个力的约束，故机械臂由五个自由度变为四个自由度，此时可以取θm＝[θ₁，θ₂，θ₃，θ₄]为约束运动的变量，θ₅为剩余的冗余变量，则θ₅可由θ_m来表示：

则：

其中，L(θm)为约束雅克比矩阵的正交向量，可通过式(7)求出。

则：

由于J(θ)L(θm)＝L^T(θm)J^T(θ)＝0，式(4)转化，可得：

其中，W_L(θ)＝L^T(θm)W(θ)L(θm)，

G_L(θ)＝L^T(θm)G(θ)，通过此式可以求出在相应的输入力矩τ下的关节输出状态。

第四步：选取估计惯性矩阵估计惯性矩阵，并利用RBF神经网络拟合动力学方程中的其他项

根据多次观察、记录动力学方程中的惯性矩阵W(θ)，以便选取估计惯性矩阵

从而式(4)可变为：

其中，

为RBF神经网络拟合项，ε为拟合项误差。

第五步：通过所设计的控制器使接触力和关节状态达到期望值

设计假设机械臂输出状态约束为：

其中，

为约束上边界，k _m(t)＝[k _m1(t)；…；k _mn(t)]为约束下界，n＝5。

假设分段函数

同时定义误差变量转换：

其中，e₁＝[e₁₁；e₁₂；…；e_1n]＝θ-θ_d，k_a(t)＝θ_d(t)-k _m(t)，

θ_d为期望关节角度。

设计Lyapunov方程：

其中，

p＝2，

α为虚拟控制器，可定义为：

其中，e_λ＝J^Tλd-λ，λd为期望力，

为期望关节角速度。

结合权利要求5中的式(10)，可得V2导数为：

其中，u_i＝h_1i/(k_bi ^2p-w_2i ^2p)+(1-h_1i)/(k_ai ^2p-e_2i ^2p)。

设计控制器如下：

其中，所输入的τ与权利要求4中的式(9)相结合，可以得到关节输出状态，从而进行闭环反馈，K₂，k₃为控制器增益。

通过(14)和(15)相联合，可得：

其中，k_2i ^*-0.5＞0。易观察得：

易得，

从而-1＜ξ＜1，-k_ai＜e_1i＜k_bi，即角位移误差收敛。

由于

且e_1i收敛，L^T(θm)J^T(θ)＝0，所以L(θm)e2收敛，由此可得

收敛，则角速度误差收敛。通过式(17)易得

因此，输出约束力误差e_λ收敛，易得α收敛。

综上通过选取合适的K₂，k₃矩阵，可以使得整个闭环系统渐近稳定。

有益效果：

1、本发明克服了控制算法中动力学建模复杂的问题，利用估计惯性矩阵与神经网络拟合未知项相结合对动力学模型进行估计，简化了算法的复杂性，具有较强的泛华性。

2、本发明利用时变约束输出状态与估计动力学模型相结合对机械臂进行力位控制，克服了无约束状态下的控制误差大的问题，从而使机械臂与环境具有良好的交互性。

附图说明

图1为本发明机械臂控制场景示意图。

图2为本发明机械臂控制框架图。

图3为本发明神经网络自适应迭代流程图。

具体实施方式

下面结合附图并举实例，对本发明进行详细描述。

本发明基于5-DOF机械臂末端执行器固定连接力传感器，末端执行器夹持物体与环境接触时，力传感器输出力信息并与各关节状态信息一起反馈给控制器。

通过定义机械臂中各个关节点、连杆长度，建立连杆坐标系，然后通过力传感器得到末端接触力，计算得出末端接触力在机械臂参考坐标系下的力信息；然后建立了带有约束力的动力学方程；最后，利用估计惯性矩阵估计惯性矩阵，并利用RBF神经网络拟合其他项，在此前提下，给出了具有时变输出约束状态的神经网络自适应力位控制方法，达到同时跟踪力位的效果。

本发明的控制算法的计算步骤如下：

第一步：建立坐标系

机械臂控制场景图如图1所示，机械臂的基座上建立参考坐标系{H}，在末端执行器的舵机上建立接触力测量坐标系{e}使其与末端执行器坐标系重合，末端夹持物体与环境的接触方向为测量坐标系的Z轴。

第二步：求解机械臂末端执行器接触力在参考坐标系下的力信息

机械臂末端与环境接触时，其接触力在测量坐标系{e}中为f_e，接触力在参考坐标系下的表达式为：

其中，θ为机械臂的关节角，f_H为变换成参考坐标系下的接触力，

为坐标系{e}相对于参考坐标系的齐次变换矩阵：

则式(1)可变为：

其中，n＝[n_x，n_y，n_z]，o＝[o_x，o_y，o_z]，a＝[a_x，a_y，a_z]。通过求解式(3)方程组可得到在参考坐标系下的接触力f_H。

第三步：给出机械臂的一般动力学方程，并建立带有约束力的动力学方程

给出一般的机械臂动力学方程表达式为；

其中，W(θ)为5×5的正定惯性矩阵，

为关节角加速度，

为关节角加速度，

为科里奥利力和向心力的5×5的矩阵，G(θ)表示5×1的重力矩阵，τ_d为未知外加扰动，τ_f为约束力矩，τ为输入力矩。τ_f，可表示为：

τ_f＝J^T(θ)λ (5)

其中，λ为f_H在约束方向上的力，J(θ)为约束条件雅克比矩阵，表达式如下：

其中，φ(h(θ))＝φ(x)＝0为位置约束方程。

由于5-DOF机械臂受到一个力的约束，故机械臂由五个自由度变为四个自由度，此时可以取θm＝[θ₁，θ₂，θ₃，θ₄]为约束运动的变量，θ₅为剩余的冗余变量，则θ₅可由θ_m来表示：

则：

其中，L(θm)为约束雅克比矩阵的正交向量，可通过式(7)求出。

则：

由于J(θ)L(θm)＝L^T(θm)J^T(θ)＝0，式(4)转化，可得：

其中，W_L(θ)＝L^T(θm)W(θ)L(θm)，

G_L(θ)＝L^T(θm)G(θ)，通过此式可以求出在相应的输入力矩τ下的关节输出状态。

第四步：选取估计惯性矩阵估计惯性矩阵，并利用RBF神经网络拟合动力学方程中的其他项

根据多次观察、记录动力学方程中的惯性矩阵W(θ)，以便选取估计惯性矩阵

从而式(4)可变为：

其中，

为RBF神经网络拟合项，其拟合过程如图3所示，ε为拟合项误差。

第五步：通过所设计的控制器使接触力和关节状态达到期望值

所设计的控制框架如图2所示，具体控制过程如下：

设计假设机械臂输出状态约束为：

其中，

为约束上边界，k _m(t)＝[k _m1(t)；…；k _mn(t)]为约束下界，n＝5。

假设分段函数

同时定义误差变量转换：

其中，e₁＝[e₁₁；e₁₂；…；e_1n]＝θ-θ_d，k_a(t)＝θ_d(t)-k _m(t)，

θ_d为期望关节角度。

设计Lyapunov方程：

其中，

p＝2，

α为虚拟控制器，可定义为：

其中，e_λ＝J^Tλd-λ，λd为期望力，

为期望关节角速度。

结合权利要求5中的式(10)，可得V2导数为：

其中，u_i＝h_1i/(k_bi ^2p-e_2i ^2p)+(1-h_1i)/(k_ai ^2p-e_2i ^2p)。

设计控制器如下：

其中，所输入的τ与权利要求4中的式(9)相结合，可以得到关节输出状态，从而进行闭环反馈，K₂，k₃为控制器增益。

通过(14)和(15)相联合，可得：

其中，k_2i ^*-0.5＞0。易观察得：

易得，

从而-1＜ξ＜1，-k_ai＜e_1i＜k_bi，即角位移误差收敛。

由于

且e_1i收敛，L^T(θm)J^T(θ)＝0，所以L(θm)e2收敛，由此可得

收敛，则角速度误差收敛。通过式(17)易得

因此，输出约束力误差e_λ收敛，易得α收敛。因此，通过选取合适的K₂，k₃矩阵，可以使得整个闭环系统渐近稳定。

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

Claims (2)

1.一种基于神经网络的5-DOF机械臂力位跟踪算法，其特征在于，该算法基于5-DOF机械臂末端执行器固定连接力传感器，末端执行器夹持物体与环境接触时，力传感器输出力信息并与各关节状态信息一起反馈给控制器；

该算法通过定义机械臂中各个关节点、连杆长度，建立连杆坐标系，然后通过力传感器得到末端接触力，计算得出末端接触力在机械臂参考坐标系下的力信息；然后建立了带有约束力的动力学方程；最后，利用估计惯性矩阵估计惯性矩阵，并利用RBF神经网络拟合其他项，在此前提下，给出了具有时变输出约束状态的神经网络自适应力位控制方法，达到同时跟踪力位的效果；所述控制算法的具体步骤如下：

第一步：建立坐标系；

第二步：求解机械臂末端执行器接触力在参考坐标系下的力信息；

第三步：给出机械臂的一般动力学方程，并建立带有约束力的动力学方程；

第四步：根据惯性矩阵，选取估计惯性矩阵，并利用RBF神经网络拟合动力学方程中的其他项；

第五步：通过所设计的控制器使接触力和关节状态达到期望值；

其中，所述第一步中坐标系的建立过程为：机械臂的基座上建立参考坐标系{H}，在末端执行器的舵机上建立接触力测量坐标系{e}使其与末端执行器坐标系重合，末端夹持物体与环境的接触方向为测量坐标系的Z轴；

其中，所述第二步中的求解过程为：

机械臂末端与环境接触时，其接触力在测量坐标系{e}中为f_e，接触力在参考坐标系下的表达式为：

其中，θ为机械臂的关节角，f_H为变换成参考坐标系下的接触力，

为坐标系{e}相对于参考坐标系的齐次变换矩阵：

则式(1)可变为：

其中，n＝[n_x，n_y，n_z]，o＝[o_x，o_y，o_z]，a＝[a_x，a_y，a_z]，通过求解式(3)方程组可得到在参考坐标系下的接触力f_H；

其中，所述第三步中的带有约束力的动力学方程的求解过程为：

给出一般的机械臂动力学方程表达式为；

其中，W(θ)为5×5的正定惯性矩阵，

为关节角加速度，

为关节角加速度，

为科里奥利力和向心力的5×5的矩阵，G(θ)表示5×1的重力矩阵，τ_d为未知外加扰动，τ_f为约束力矩，τ为输入力矩；τ_f，可表示为：

τ_f＝J^T(θ)λ (5)

其中，λ为f_H在约束方向上的力，J(θ)为约束条件雅克比矩阵，表达式如下：

其中，φ(h(θ))＝φ(x)＝0为位置约束方程；

由于5-DOF机械臂受到一个力的约束，故机械臂由五个自由度变为四个自由度，此时可以取θm＝[θ₁，θ₂，θ₃，θ₄]为约束运动的变量，θ₅为剩余的冗余变量，则θ₅可由θ_m来表示：

则：

其中，L(θ_m)为约束雅克比矩阵的正交向量，可通过式(7)求出；

则：

由于J(θ)L(θm)＝L^T(θm)J^T(θ)＝0，式(4)转化，可得：

其中，W_L(θ)＝L^T(θm)W(θ)L(θm)，

G_L(θ)＝L^T(θm)G(θ)，通过此式

可以求出在相应的输入力矩τ下的关节输出状态。

2.如权利要求1所述的一种基于神经网络的5-DOF机械臂力位跟踪算法，其特征在于，所述第四步中的拟合动力学方程的求解过程为：

根据多次观察、记录动力学方程中的惯性矩阵W(θ)，以便选取估计惯性矩阵从而式(4)可变为：

其中，

为RBF神经网络拟合项，ε为拟合项误差。

Images