一种快速终端滑模阻抗控制算法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN111208730A(43)申请公布日2020.05.29(21)申请号202010019659.7(22)申请日2020.01.08(71)申请人南昌大学地址330000江西省南昌市红谷滩新区学府大道999号(72)发明人李春泉何永华杨峰(74)专利代理机构北京众合诚成知识产权代理有限公司11246代理人袁红梅(51)Int.Cl.G05B13/04(2006.01)权利要求书5页说明书11页附图1页(54)发明名称一种快速终端滑模阻抗控制算法(57)摘要本发明提供了一种快速终端滑模阻抗控制算法。主要包括以下步骤：步骤1，将力反馈设备简化为双连杆模型；步骤2，求解简化模型的运动学；步骤3，求解简化模型的动力学；步骤4，建立阻抗模型；步骤5，设计快速终端滑模控制器。本发明的优点在于：一、引入了终端吸引子，保证了系统状态能够在有限时间内收敛到平衡点；二，保留了线性滑模控制部分，因此具有在平衡点附近快速收敛的特点。快速终端滑模阻抗控制使系统状态能够快速、精准的收敛到平衡点。与滑模阻抗控制算法相比，快速终端滑模阻抗控制具有更小的位置跟踪误差。CN111208730ACN111208730A权利要求书1/5页1.一种快速终端滑模阻抗控制算法，其特征在于：按以下步骤：步骤1、将力反馈设备简化为双连杆模型；步骤2、求解力反馈设备模型的运动学；步骤3、求解力反馈设备模型的动力学；步骤4、建立阻抗模型；步骤5、设计快速终端滑模控制器。2.根据权利要求1所述的一种快速终端滑模阻抗控制算法，其特征在于：所述步骤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省去。3.根据权利要求2所述的一种快速终端滑模阻抗控制算法，其特征在于：所述步骤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的计算公式：2CN111208730A权利要求书2/5页定义x＝[x1x2]，θ＝[θ1θ2]，得雅可比矩阵公式：将正运动学公式带入上式，可得：雅可比矩阵的微分为：4.根据权利要求3所述的一种快速终端滑模阻抗控制算法，其特征在于：所述步骤3中具体求解力反馈设备模型动力学的算式如下：连杆l1动能为：连杆l2质心C在x1上的线速度为：连杆l2质心C在x2上的线速度为：可得：连杆l2的动能：系统的总动能为：系统的总势能：3CN111208730A权利要求书3/5页将系统的总动能和系统的总势能代入拉格朗日函数中，得到：关节力矩的计算简化模型的关节J'1的驱动力矩计算公式如下：式中：得到力矩τ1的计算公式：简化模型的关节J'2的驱动力矩计算公式如下：式中：得到力矩τ2的计算公式：4CN111208730A权利要求书4/5页整理力矩τ1和τ2的计算公式，最终得到力矩的表达式为：5.根据权利要求4所述的一种快速终端滑模阻抗控制算法，其特征在于：所述步骤4阻抗模型的建立具体为：力反馈设备末端的接触力为Fe，将Fe与理想位置误差xc-xd建立阻抗关系，描述为：其中，xc为末端位置的指令轨迹，xd为理想的阻抗轨迹，x(0)＝xc(0)，Md、Bd、Kd为阻抗参数，分别是质量、阻尼和刚度系数矩阵，通过上述的阻抗关系公式计算出需要跟踪的理想阻抗轨迹，再根据动力学模型，设计力反馈设备末端的控制律Fx。6.根据权利要求5所述的一种快速终端滑模阻抗控制算法，其特征在于：所述步骤5快速终端滑模控制器的设计具体为：定义快速终端滑模切换面：其中，xd(t)为理想轨迹，和分别为理想速度和加速度，γ1和γ2为大于零的正定常数矩阵，α和β(β>α)为正奇数；定义中间参考变量：对上式微分，可得：则有：将中间参考变量的微分式代入上式中，可得：设计快速终端滑模控制器为：5CN111