一种快速终端滑模阻抗控制算法的制作方法

文档序号：20915620发布日期：2020-05-29 13:29阅读：555来源：国知局

本发明涉及力反馈设备与物体之间的交互，用于提高阻抗控制的抗干扰能力、动态响应速度以及跟踪精度，能够降低力反馈设备与物体接触产生的相互作用力，从而保护力反馈设备和接触物体。

背景技术：

众所周知，人类的感觉主要由听觉、嗅觉、味觉、视觉和力触觉组成。力触觉又可分为力觉和触觉，其中力觉是对压力、刚度等的感知，而触觉是对纹理、温度等的感知。力触觉是我们日常生活非常倚重的感觉，例如：完成开门、书写、打球等接触性的活动都需要依赖于力触觉。marcoernst在《nature》中指出人的力触觉反馈信息可以修正视觉上的偏差。研究表明，在人机交互的实验中，完成插销入孔、组装工件等任务，与仅有图像显示相比，将视觉和力触觉反馈相结合，能够极大地提高操作的精度和效率。因此，与其他感觉相比，力触觉更加微妙、细腻和直接，在人类的感知、决策和判断中起着非常重要的作用。

力触觉的实现依赖于力触觉反馈设备(或者力反馈设备)。一个理想的力反馈设备应当具有如下特点：当操作者操作该设备与虚拟环境或者远端环境进行人机交互时，操作者在交互过程中所感觉到的力触觉与亲临现场进行真实操作所感觉到的一样。因此，如何使力反馈设备能够构建真实、准确、可靠的临场感效果，是力触觉反馈设备设计中最具有挑战性的问题。然而，考虑到通常情况下，力反馈设备未添加被动柔顺装置，设备的机械阻抗是固定且较高，这会导致接触力过大，损坏设备甚至威胁操作者的安全。因此，常使用阻抗控制算法控制力反馈设备。阻抗控制算法通过建立位置与接触力的关系，使力反馈设备在受限环境中实现位置控制的同时，能够保持预先设定接触力，从而有效地保护力反馈设备和接触物。

现有的阻抗控制算法有：vuminhhung等人提出了一种新的自适应控制算法(详见：modelreferenceadaptivecontrolofahapticfeedbackdeviceforimprovingforceperformance[j].science&technologydevelopment,2014,17(k1):102～114.)，该算法将人手施加到力反馈设备上的力模拟为扰动，采用模型参考自适应控制算法改进力的跟踪性能，但该方法需要实时调整自适应率修正控制器的参数，将会降低控制算法实时性。李二超等人提出了一种模糊滑模阻抗控制算法(详见：未知环境下机器人模糊滑模阻抗控制[j].电气自动化,2010,32(4):12-13.)，利用模糊控制来克服滑模控制的抖振问题，但在模糊阻抗控制中，若模糊控制器将信息简单地模糊处理，会降低控制精度，若处理较为复杂，则失去模糊控制的优势。kim等人提出了一种基于径向基函数(rbf)神经网络算法的滑模阻抗控制方法(详见：impedancecontrolofrobotmanipulatorusingartificialintelligence[c].iccas2010.ieee,2010:1891-1894.)，用于实时估计所需阻抗模型的惯性、阻尼、刚度等设计参数，而在神经网络阻抗控制中，若使用的网络学习能力不够，将导致对系统不确定项的逼近精确不够。garcia等人提出了一种无抖振、二阶滑模控制算法，并且与阻抗控制相结合(详见：observer-basedhigher-orderslidingmodeimpedancecontrolofbilateralteleoperationunderconstantunknowntimedelay[c].2006ieee/rsjinternationalconferenceonintelligentrobotsandsystems.ieee,2006:1692-1699.)，以保证在未知常数时延下的鲁棒跟踪。

综上所述可知，自适应阻抗控制算法、模糊阻抗控制算法以及神经网络阻抗控制算法分别在算法的实时性、处理信息的复杂程度以及算法精确度方面存在一定的问题。相较而言，滑模阻抗控制算法具有抗扰动能力强，结构简单，对系统模型精确度要求不高等优点，更适用于力反馈设备与物体之间的交互。

技术实现要素：

基于上述背景，本发明提出了一种快速终端滑模阻抗控制算法。将快速终端滑模控制引入阻抗控制中，保证了系统状态能够在有限时间内收敛到平衡点，提高阻抗控制的抗干扰能力、动态响应速度以及跟踪精度，使系统具备抗干扰能力强和快速、精准收敛的特点。与滑模阻抗控制算法相比，快速终端滑模阻抗控制具有更小的位置跟踪误差。本发明是通过以下技术方案实现的。

本发明所述的一种快速终端滑模阻抗控制算法，按以下步骤：

步骤1、将力反馈设备简化为双连杆模型；

步骤2、求解力反馈设备模型的运动学；

步骤3、求解力反馈设备模型的动力学；

步骤4、建立阻抗模型；

步骤5、设计快速终端滑模控制器。

进一步的，所述步骤1中将力反馈设备简化为双连杆模型具体如下：

力反馈设备具有n个可旋转的关节j1、j2…jn，(n＜＜7)，关节j1控制的是设备末端在水平面的运动，即设备的左右转动。控制算法所需要补偿的重力以及末端在工作空间中的运动主要来自关节j2…jn。为提高工作效率，将力反馈设备简化为双连杆模型，并设定连杆为均匀质杆。简化后的双连杆模型关节1记作j'1，关节2记作j'2。其中，j'1由原力反馈设备的关节j2…jn-1合并而来，j'2是原力反馈设备的关节jn，并将原力反馈设备的的关节j1省去。

进一步的，所述步骤2中具体求解力反馈设备模型运动学的算式如下：

(a)正运动学求解算式：

式中，l1和l2分别是简化后模型关节j'1的连杆长和关节j'2的连杆长，θ1是l1与水平面间的夹角，θ2是l2和竖直平面之间的夹角，(x1,x2)是设备末端在二维平面上的位置坐标。

(b)逆运动学求解算式：

式中，r是设备末端到关节j'1之间的距离

式中，β是r与水平面的夹角

再通过余弦定理得到：

式中，γ是l1与r之间的夹角

考虑力反馈设备的实际工作空间，γ>0，因此θ1的计算公式为：

θ1＝γ+β

通过余弦定理，得到角α的计算公式：

α为杆长l1和l2之间的夹角

得到θ2的计算公式：

定义x＝[x1x2]，θ＝[θ1θ2]，得雅可比矩阵公式：

将正运动学公式带入上式，可得：

雅可比矩阵的微分为：

进一步的，所述步骤3中具体求解力反馈设备模型动力学的算式如下：

连杆l1动能为：

连杆l2质心c在x1上的线速度为：

连杆l2质心c在x2上的线速度为：

可得：

连杆l2的动能：

系统的总动能为：

系统的总势能：

将系统的总动能和系统的总势能代入拉格朗日函数中，得到：

关节力矩的计算

简化模型的关节j'1的驱动力矩计算公式如下：

式中：

得到力矩τ1的计算公式：

简化模型的关节j'2的驱动力矩计算公式如下：

式中：

得到力矩τ2的计算公式：

整理力矩τ1和τ2的计算公式，最终得到力矩的表达式为：

进一步的，所述步骤4阻抗模型的建立具体为：

力反馈设备末端的接触力为fe，将fe与理想位置误差xc-xd建立阻抗关系，描述为：

其中，xc为末端位置的指令轨迹，xd为理想的阻抗轨迹，x(0)＝xc(0)，md、bd、kd为阻抗参数，分别是质量、阻尼和刚度系数矩阵。通过上述的阻抗关系公式计算出需要跟踪的理想阻抗轨迹，再根据动力学模型，设计力反馈设备末端的控制律fx。

进一步的，所述步骤5快速终端滑模控制器的设计具体为：

定义快速终端滑模切换面：

其中，xd(t)为理想轨迹，和分别为理想速度和加速度。γ1和γ2为大于零的正定常数矩阵，α和β(β>α)为正奇数。

定义中间参考变量：

对上式微分，可得：

则有：

将中间参考变量的微分式代入上式中，可得：

设计快速终端滑模控制器为：

其中，k>0，ε>0，tanh为双曲正切函数。

与现有技术相比，本发明的有益效果是：

(1)引入了终端吸引子，保证了系统状态能够在有限时间内收敛到平衡点；

(2)保留了线性滑模控制部分，因此具有在平衡点附近快速收敛的特点。快速终端滑模阻抗控制使系统状态能够快速、精准的收敛到平衡点。与滑模阻抗控制算法相比，快速终端滑模阻抗控制具有更小的位置跟踪误差。

附图说明

图1为力反馈设备两连杆简化模型分析图。图中：x1和x2分别表示二维平面中的横轴和纵轴，(x1,x2)是设备末端在二维平面上的位置坐标，l1和l2分别是关节j'1的连杆长和关节j'2的连杆长，θ1为l1与横轴间的夹角，θ2为l2与纵轴间的夹角，α为杆长l1和l2之间的夹角，r是设备末端到关节j'1之间的距离，β是r与水平面的夹角，γ是l1与r之间的夹角。

具体实施方式

本发明将通过以下实例作进一步说明。

步骤1，将力反馈设备简化为连杆模型具体为：

力反馈设备具有3个可旋转的关节j1、j2、j3，关节j1控制的是设备末端在水平面的运动，即设备的左右转动。控制算法所需要补偿的重力以及末端在工作空间中的运动主要来自关节j2、j3。为提高工作效率，将力反馈设备简化为双连杆模型，并设定连杆为均匀质杆。简化后的双连杆模型关节1记作j'1，关节2记作j'2。其中，j'1为原力反馈设备的关节j2，j'2是原力反馈设备的关节j3，并将原力反馈设备的的关节j1省去。

步骤2，求解力反馈设备模型的运动学算式如下：