一种基于扰动观测器的并联机器人阻抗控制方法与流程

本发明涉及一种机器人控制方法，特别涉及一种并联机器人阻抗控制方法。

背景技术：

目前，并联机器人具有刚度高、结构紧凑、承载能力大、动态响应好等优点，在工业和服务业具有非常重要的应用价值。在机器人执行磨削、搬运、装配、康复、手术等任务时，机器人与环境(人或被操作物)直接接触，过大的接触力容易损坏机器人或给环境带来伤害。因此，机器人在进行接触性作业时，需要具备一定的柔顺性，这对机器人的控制系统提出了更高的要求。阻抗控制是hogan于1985年提出的一种主动柔顺控制方法，意在期望轨迹跟踪误差与人机交互力之间建立-弹簧-阻尼动态系统，已成为人机交互中最重要的柔顺控制方法。因此，为并联机器人设计有效的阻抗控制具有重要的价值。在机器人动力学模型中，系统的不确定性和干扰总是存在的，且阻抗误差对模型的不确定项比较敏感，而阻抗误差的收敛对阻抗控制的稳定性和成功实现人机交互的柔顺性、安全性和可靠性至关重要。

现有技术中公开了公布号为：cn102306029a，公布日期为：2012-01-04的本发明公开了一种基于康复训练机器人的阻抗自适应的运动控制方法，其特征在于，由在线辨识器根据力传感器和位置传感器采集得到的患肢的作用力和位置计算出患肢的等效质量参数mh、等效阻尼参数bh和等效刚度参数kh，再利用这些参数的变化量δmh、δbh及δkh，对比例-微分-积分控制器的比例系数kp，积分系数ki和微分系数kd进行修正，比例-微分-积分控制器输出的电机功率信号转变成力矩电机的驱动信号，用于控制力矩电机的工作。该控制方法可提高康复训练机器人的控制柔顺性和安全性。但是该控制方法不能保证带有不确定项的机器人的阻抗误差收敛到零点或零点的小领域，不能保证阻抗控制的稳定性和有效性，无法克服现有机器人阻抗控制的不足，不能实现阻抗误差的收敛以及阻抗控制的稳定性和柔顺、安全、可靠的人机交互。

技术实现要素：

本发明的目的是提供一种基于扰动观测器的并联机器人阻抗控制方法，克服现有机器人阻抗控制的不足，实现阻抗误差的收敛，保证阻抗控制的稳定性和柔顺、安全、可靠的人机交互。

为了实现上述目的，本发明提供了种基于扰动观测器的并联机器人阻抗控制方法，包括以下步骤：

步骤1，建立五杆并联机器人的欧拉-拉格朗日模型；

步骤2，基于期望的阻抗动态模型，设计一个辅助轨迹，将阻抗控制设计问题转化为辅助轨迹的特殊跟踪问题；

步骤3，基于五杆并联机器人的欧拉-拉格朗日模型，设计扰动观测器，估计系统中的摩擦力和扰动；

步骤4，基于辅助轨迹跟踪误差，设计机器人-环境交互控制，抑制/鼓励机器人的错误/正确运动；

步骤5，结合上述步骤，设计阻抗控制，使得辅助轨迹跟踪误差及其一阶二阶导数收敛到零点一个小邻域，进而保证阻抗误差收敛到零点一个小邻域；

步骤6，根据仿真结果判断是否需要对参数进行调整，通过matlab仿真后，如果控制效果不能满足要求，返回步骤3-5继续调节参数，直到控制效果达到要求；如果控制效果满足要求，则设计结束。

与现有技术相比，本发明的有益效果在于，构建一个辅助轨迹并将阻抗控制设计问题转化为辅助轨迹的特殊跟踪问题，设计扰动观测器估计模型中的不确定项和扰动，设计机器人-环境交互控制鼓励/抑制机器人的正确/错误运动，在此基础上，设计阻抗控制确保参考轨迹跟踪误差及其一阶二阶导数的收敛，进而保证阻抗误差的收敛和阻抗控制的稳定性，克服现有机器人阻抗控制的不足，实现阻抗误差的收敛，保证阻抗控制的稳定性和柔顺、安全、可靠的人机交互，本发明可以用于机器人的柔顺控制。

作为本发明的进一步改进，所述步骤1中五杆并联机器人的欧拉-拉格朗日模型为：

其中：q＝[q1q2]^t∈r²为关节角度向量；为关节速度向量；为关节加速度向量；m(q)∈r^2×2为惯性矩阵；表示哥氏力矩和向心力矩；g(q)∈r²是重力矩；表示集总不确定项，其中表示摩擦力矩，td(t)表示外界扰动；t∈r²是控制输入；te＝j^t(q)fe∈r²是在关节空间中的机器人-环境交互力，其中j(q)是雅可比矩阵，fe＝[fe1fe2]^t∈r²是末端交互力；

将矩阵m(q)，和j(q)表示为：

其中：d11(q)＝m1l²c1+m3l²c3+m4l1²+i1+i3，d12(q)＝d21(q)＝(m3l2lc3+m4l1lc4)cos(q2-q1)，h＝-(m3l2lc3+m4l1lc4)sin(q2-q1)，l1,l2,l3,l4分别表示各个关节的长度，lc1,lc2,lc3,lc4表示各个关节到对应关节质心的距离，i1,i2,i4,i5为各关节惯性矩，m1,m2,m3,m4分别为各个关节的质量；

摩擦力的模型为：

其中γi∈r,i＝1,2...6为未知的正常数，tanh为双曲正切函数；摩擦力模型具有以下优点：可以表示静摩擦力；可以表示黏性力；可以表示库伦摩擦力；具有stribeck效应，其中摩擦系数从静摩擦系数随着原点附近的滑动速度增加而减小。

作为本发明的进一步改进，所述步骤2中，设期望的阻抗动态模型为：

其中：md，bd和kd为正定对角矩阵，分别表示期望的惯性矩阵，期望的阻尼矩阵，期望的刚度矩阵；

设计辅助轨迹qr∈r²为qr＝qd+α，其中α是由md^-1te通过下面滤波器得到的信号：

那么，阻抗误差可以表示为：

其中，e1＝qr-q为辅助轨迹跟踪误差，设计控制使得e1，和收敛，可以保证阻抗误差e的收敛，使得阻抗控制设计问题转化为设计控制使得e1，和收敛的问题。

作为本发明的进一步改进，所述步骤3中，设计扰动观测器为：

其中：ko为正的观测器参数，β为辅助向量，为式(1)所描述机器人模型中集总不确定项的估计值；

对集总不确定项的估计值求导数，可得

其中，表示不确定项的估计误差，该误差满足：

其中，

作为本发明的进一步改进，所述步骤4中，设计机器人-环境交互控制为：

其中，k1为正的控制参数，tanh为双曲正切函数；当交互力使得机器人偏离辅助轨迹，设计φ(e2,τe)＝τe可抑制机器人这个错误的运动；当交互力使得机器人跟踪辅助轨迹，设计φ(e2,te)＝0可以用来估计机器人这个正确的运动；而做为一个过度区域过来保证交互控制φ(e2,te)的连续性。

作为本发明的进一步改进，所述步骤5中，设计如下阻抗控制：

其中，k2为正的控制参数。

附图说明

图1为本发明实施步骤流程框图。

图2为本发明中五杆并联机器人的结构示意图。

图3为本发明中五杆并联机器人的阻抗误差图。

图4为本发明中五杆并联机器人的阻抗控制柜输入图。

具体实施方式

下面结合附图对本发明进一步说明：

如图1-4所示的一种基于扰动观测器的并联机器人阻抗控制方法，包括以下步骤：

步骤1，建立五杆并联机器人的欧拉-拉格朗日模型：

其中：q＝[q1q2]^t∈r²为关节角度向量；为关节速度向量；为关节加速度向量；m(q)∈r^2×2为惯性矩阵；表示哥氏力矩和向心力矩；g(q)∈r²是重力矩；表示集总不确定项，其中表示摩擦力矩，td(t)表示外界扰动；t∈r²是控制输入；te＝j^t(q)fe∈r²是在关节空间中的机器人-环境交互力，其中j(q)是雅可比矩阵，fe＝[fe1fe2]^t∈r²是末端交互力；

将矩阵m(q)，和j(q)表示为：

其中：d11(q)＝m1l²c1+m3l²c3+m4l1²+i1+i3，d12(q)＝d21(q)＝(m3l2lc3+m4l1lc4)cos(q2-q1)，h＝-(m3l2lc3+m4l1lc4)sin(q2-q1)，l1,l2,l3,l4分别表示各个关节的长度，lc1,lc2,lc3,lc4表示各个关节到对应关节质心的距离，i1,i2,i4,i5为各关节惯性矩，m1,m2,m3,m4分别为各个关节的质量；

摩擦力的模型为：

其中γi∈r,i＝1,2...6为未知的正常数，tanh为双曲正切函数；摩擦力模型具有以下优点：可以表示静摩擦力；可以表示黏性力；可以表示库伦摩擦力；具有stribeck效应，其中摩擦系数从静摩擦系数随着原点附近的滑动速度增加而减；

步骤2，基于期望的阻抗动态模型，设计一个辅助轨迹，将阻抗控制设计问题转化为辅助轨迹的特殊跟踪问题，设期望的阻抗动态模型为：

其中：md，bd和kd为正定对角矩阵，分别表示期望的惯性矩阵，期望的阻尼矩阵，期望的刚度矩阵；

设计辅助轨迹qr∈r²为qr＝qd+α，其中α是由md^-1te通过下面滤波器得到的信号：

那么，阻抗误差可以表示为：

其中，e1＝qr-q为辅助轨迹跟踪误差，设计控制使得e1，和收敛，可以保证阻抗误差e的收敛，使得阻抗控制设计问题转化为设计控制使得e1，和收敛的问题；

步骤3，五杆并联机器人的欧拉-拉格朗日模型，设计扰动观测器，估计系统中的摩擦力和扰动，设计扰动观测器为：

其中：ko为正的观测器参数，β为辅助向量，为式(1)所描述机器人模型中集总不确定项的估计值；

对集总不确定项的估计值求导数，可得

其中，表示不确定项的估计误差，该误差满足：

其中，

步骤4，基于辅助轨迹跟踪误差，设计机器人-环境交互控制，抑制/鼓励机器人的错误/正确运动；

设计机器人-环境交互控制为：

其中，k1为正的控制参数，tanh为双曲正切函数；当交互力使得机器人偏离辅助轨迹，设计φ(e2,te)＝te可抑制机器人这个错误的运动；当交互力使得机器人跟踪辅助轨迹，设计φ(e2,te)＝0可以用来估计机器人这个正确的运动；而做为一个过度区域过来保证交互控制φ(e2,te)的连续性；

步骤5，设计阻抗控制，使得辅助轨迹跟踪误差及其一阶二阶导数收敛到零点一个小邻域，进而保证阻抗误差收敛到零点一个小邻域；设计如下阻抗控制：

其中，k2为正的控制参数；

步骤6，根据仿真结果判断是否需要对参数进行调整，通过matlab仿真后，如果控制效果不能满足要求，返回步骤3-5继续调节参数，直到控制效果达到要求；如果控制效果满足要求，则设计结束。

工作时，人手握住机器人的末端执行器(p端)，手臂绕机器人中心向左或向右转动一定角度，根据动力学方程，对机器人进行分析，建立五杆并联机器人的欧拉-拉格朗日模型为：

其中：q＝[q1q2]^t∈r²为关节角度向量；为关节速度向量；为关节加速度向量；m(q)∈r^2×2为惯性矩阵；表示哥氏力矩和向心力矩；因为该机器人在平面运动，所以重力矩g(q)∈r²为0；表示集总不确定项，其中表示摩擦力矩，td(t)表示外界扰动；t∈r²是控制输入；te＝j^t(q)fe∈r²是在关节空间中的机器人-环境交互力，其中j(q)是雅可比矩阵，fe＝[fe1fe2]^t∈r²是末端交互力。矩阵m(q)，和j(q)可表示为：

其中：d11(q)＝m1l²c1+m3l²c3+m4l1²+i1+i3，d12(q)＝d21(q)＝(m3l2lc3+m4l1lc4)cos(q2-q1)，h＝-(m3l2lc3+m4l1lc4)sin(q2-q1)，各个关节的长度分别为：l1＝0.2m,l2＝l4＝0.4m,l3＝0.5m，各个关节到对应关节质心的距离分别为：lc1＝0.1m,lc2＝lc4＝0.2m,lc3＝0.25m，各关节惯性矩为：i1＝0.1kg.m²,i2＝i4＝0.2kg.m²,i3＝0.3kg.m²，各个关节的质量分别为：m1＝0.5kg,m2＝m4＝1kg,m3＝1.5kg；

而摩擦力为：

其中：γ1＝0.2,γ2＝2,γ3＝10,γ4＝20,γ5＝0.3。

步骤2：设期望的阻抗动态模型为：

其中：md＝i,bd＝10i,kd＝20i，交互力fe＝[fe1,fe2]^t为

设计辅助轨迹qr∈r²为qr＝qd+α，其中α是由md^-1τe通过下面滤波器得到的信号：

那么，阻抗误差可以表示为：

其中，e1＝qr-q为辅助轨迹跟踪误差，因而设计控制使得e1，和收敛，可以保证阻抗误差e的收敛，所以阻抗控制设计问题转化为设计控制使得e1，和收敛的问题。

步骤3：为估计机器人模型中的集总不确定项设计如下的扰动观测器：

其中：ko为观测器增益，β为辅助向量，为机器人模型中集总不确定项的估计值。对集总不确定项的估计值求导数，可得：

其中，表示不确定项的估计误差，该误差满足：

其中，

步骤4：设计机器人-环境交互控制为：

其中，k1,ε为正的控制参数，tanh为双曲正切函数；当交互力使得机器人偏离辅助轨迹，设计φ(e2,τe)＝te可抑制机器人这个错误的运动；当交互力使得机器人跟踪辅助轨迹，设计φ(e2,te)＝0可以用来估计机器人这个正确的运动；而做为一个过度区域用来保证交互控制φ(e2,te)的连续性。

步骤5：设计如下阻抗控制：

其中，k2为控制参数。

步骤6：根据仿真结果进行控制参数调整，如果控制效果不能满足要求，返回步骤3-5继续调节参数，直到控制效果达到要求；如果控制效果满足要求，则设计结束。

通过仿真和调参，发现当控制参数为k1＝2,k2＝4,ko＝30,ε＝0.02，可以得到满意的控制效果；在图4所描述阻抗控制之下，机器人辅助轨迹跟踪误差及其一阶二阶导数收敛到0点的一个小邻域，阻抗误差e收敛到0点一个小邻域，达到了满意的控制效果。

本发明不局限于上述实施例，在本公开的技术方案的基础上，本领域的技术人员根据所公开的技术内容，不需要创造性的劳动就可以对其中的一些技术特征作出一些替换和变形，这些替换和变形均在本发明的保护范围内。