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的计算公式:
定义x=[x1x2],θ=[θ1θ2],得雅可比矩阵公式:
将正运动学公式带入上式,可得:
雅可比矩阵的微分为:
4.根据权利要求3所述的一种快速终端滑模阻抗控制算法,其特征在于:所述步骤3中具体求解力反馈设备模型动力学的算式如下:
连杆l1动能为:
连杆l2质心c在x1上的线速度为:
连杆l2质心c在x2上的线速度为:
可得:
连杆l2的动能:
系统的总动能为:
系统的总势能:
将系统的总动能和系统的总势能代入拉格朗日函数中,得到:
关节力矩的计算
简化模型的关节j'1的驱动力矩计算公式如下:
式中:
得到力矩τ1的计算公式:
简化模型的关节j'2的驱动力矩计算公式如下:
式中:
得到力矩τ2的计算公式:
整理力矩τ1和τ2的计算公式,最终得到力矩的表达式为:
5.根据权利要求4所述的一种快速终端滑模阻抗控制算法,其特征在于:所述步骤4阻抗模型的建立具体为:
力反馈设备末端的接触力为fe,将fe与理想位置误差xc-xd建立阻抗关系,描述为:
其中,xc为末端位置的指令轨迹,xd为理想的阻抗轨迹,x(0)=xc(0),
6.根据权利要求5所述的一种快速终端滑模阻抗控制算法,其特征在于:所述步骤5快速终端滑模控制器的设计具体为:
定义快速终端滑模切换面:
其中,xd(t)为理想轨迹,
定义中间参考变量:
对上式微分,可得:
则有:
将中间参考变量的微分式代入上式中,可得:
设计快速终端滑模控制器为:
其中,k>0,ε>0,tanh为双曲正切函数。