一种考虑末端力的双臂机器人协同控制方法技术

本发明专利技术属于机器人控制技术领域，设计了一种考虑末端力的双臂机器人协同控制方法；过干扰观测器对双臂系统未建模部分和未知的干扰等进行观测并对改进滑模控制器输出进行补偿从而减小控制误差；力外环采用自适应阻抗控制器根据末端接触力误差情况对环境参数进行自适应进而获得最优的机械臂末端接触力和位置之间的关系，避免了繁琐的阻抗系统参数调节的过程，另外根据物体运动的实际情况随时调整机械臂的期望力，得以达到更精确的机械臂力控制和位置控制的效果；实现了双臂机器人协同搬运物体的操作任务中末端力和位置的双重控制。物体的操作任务中末端力和位置的双重控制。

【技术实现步骤摘要】
一种考虑末端力的双臂机器人协同控制方法


[0001]本专利技术属于机器人控制
，尤其涉及一种考虑末端力的双臂机器人协同控制方法。

技术介绍

[0002]随着社会科技的发展，机械臂在工业中得到了广泛的应用。近年来，由于生产任务越来越复杂和单机械臂自身的局限性，很多任务无法通过单机械臂完成，例如：协调搬运、协同焊接和救援行动等。双臂机器人相比于单机械臂具有更大的负载能力、更高的灵巧度等优势，可以完成更多复杂的工作，逐渐进入生产、生活领域。
[0003]然而，由于多自由度的双臂机器人系统是高度耦合的非线性系统，其动力学建模比传统的单臂机器人要复杂，模型的不确定性增加。不确定因素会对机械臂的性能产生不利影响，针对双臂机器人的非线性和不确定性，并且在工作中不可避免力接触和力协调等问题，很多研究被展开。目前，如何实现双臂机器人位置和力的双重控制是目前国内外研究人员关注的主要研究方向。

技术实现思路

[0004]针对现有技术的不足，本专利技术提供一种考虑末端力的双臂机器人协同控制方法，不仅仅实现末端轨迹的精确控制，还考虑了机械臂末端力的跟踪效果，实现双臂机器人协同搬运物体的操作任务中末端力和位置的双重控制。
[0005]一种考虑末端力的双臂机器人协同控制方法，具体为以下步骤：
[0006]步骤1：双臂机器人协同搬运物体属于紧协调操作任务，存在严格的运动约束关系，假设两个机械手都刚性地附在负载上，使抓握点与被抓物之间无滑移现象；当双臂机器人协同操作同一个物体时，机器人和物体形成一个闭链系统，定义：{O}为物体的中心位置，{O
R1
}、{O
R1
}分别为机器人R1和R2的基座坐标系，{O2}、{O1}分别为机器人R1和R2的工具坐标系；根据双臂机器人工作时的情况确定双臂机器人的运动约束关系为：
[0007][0008][0009]其中为坐标系{O
R1
}下机器人R2末端执行器的位姿，为坐标系{O
R1
}下机器人R1末端执行器的位姿；1T2为工具坐标系{O2}到{O1}的位姿变换矩阵，为：1T2＝Rot(z，π)
×
Trans(d1，0，0)；
[0010]式1为机器人R1和R2之间的运动约束，同时联系着R1和R2的运动学，若当已知机器人R1的构型或者末端执行器的位置，则通过运动约束求解机器人R2的正向运动学与逆向运动学，无需再对机器人R2进行运动学建模求解；
[0011]步骤2：依据牛顿第二定律对双臂机器人搬运的物体的受力情况进行受力分析结果如下：
[0012][0013]其中m为被搬运物体的质量；和分别为被双臂机器人搬运物体在x，y和z方向上的加速度；F1和F2分别是两个机械臂末端对物体的操作力；F
s1y
，F
s2y
，F
s1z
和F
s2z
分别是机械臂与物体的接触摩擦力在y轴和z轴方向的分力；μ是摩擦系数；
[0014]为保持物体平衡应该满足以下条件：
[0015]F
s1y2
+F
s1z2
＜(μF1)2[0016]F
s2y2
+F
s2z2
＜(μF2)2ꢀꢀꢀꢀ
(3)
[0017]双机械臂末端施加给被搬运物体的力由下式得到：
[0018][0019][0020][0021][0022]步骤3：运用拉格朗日方法，得到单机械臂的动力学方程为：
[0023][0024]其中M
i
(q
i
)是第i个机械臂的对称正定惯性矩阵；是科里奥利矩阵和离心矩阵；G
i
(q
i
)是引力矢量；J
i
(q
i
)是雅可比矩阵；F
i
是物体施加在手臂上的力矢量；w是外部环境对于机械臂关节的扰动力矩；τ
i
是机械臂关节的控制输入力矩；
[0025]根据单机械臂的动力学方程推广得到双机械臂在关节空间中的动力学方程为：
[0026][0027]其中，q＝[q
1T
，q
2T
]T
∈R6×1为双机械臂的各关节角度矩阵；M(q)＝diag[M1(q1)，M2(q2)]∈R6×6双机械臂的正定质量惯性矩阵；为双机械臂的科里奥利力与离心力矩阵；G(q)＝[G
1T
(q1)，G
2T
(q2)]T
∈R6×1为双机械臂的重力矩阵；τ＝[τ
1T
，τ
2T
]T
∈R6×1为双机械臂的各关节控制输入力矩矩阵；F＝[F1，F
s1y
，F
s1z
，F2，F
s2y
，F
s2z
]T
为外环境力矩阵。J(q)＝diag[J1(q1)，J2(q2)]∈R6×6为双机械臂的雅可比矩阵；w为干扰力矩矩阵；
[0028]双机械臂的正运动学模型表示为：
[0029][0030]其中，为机械臂末端的速度，为机械臂关节角速度；
[0031]双机械臂的逆运动学模型表示为：
[0032][0033][0034]其中，为机械臂末端的加速度，为机械臂关节角加速度，为J
‑1(q)的导数；
[0035]双机械臂各关节力矩与双机械臂末端执行器的输出力之间的关系为：
[0036]τ＝J
T
(q)F
x
ꢀꢀꢀꢀ
(9)
[0037]其中，F
x
为双机械臂末端执行器的输出力；
[0038]将式8和9代入6式中得到双机械臂在笛卡尔坐标系下的动力学方程：
[0039][0040]其中：
[0041]M
x
(q)＝J
‑
T
(q)M(q)J
‑1(q)；
[0042][0043]G
x
(q)＝J
‑
T
(q)G(q)；F
x
＝J
‑
T
(q)τ
[0044]步骤4：针对步骤3中提出的动力学模型方程式10，将外部干扰和未建模部分归类为干扰项，并用干扰观测器进行观测并与双曲正切滑模控制器相结合对双臂机器人末端进行位置控制；基于干扰观测器的改进滑模控制器为：
[0045][0046]其中：为干扰向量f
d
的观测值；z为辅助变量，定义为：的观测值；z为辅助变量，定义为：为干扰观测器增益矩阵，向量为干扰观测器辅助向量；和分别为实际值M
x
(q)，和G
x
(q)的估计值；K＝diag{k1，
…
k6}，k
i
＞0，i＝1，
…
6为滑模控制器的本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种考虑末端力的双臂机器人协同控制方法，其特征在于，具体为以下步骤：步骤1：双臂机器人协同搬运物体属于紧协调操作任务，存在严格的运动约束关系，假设两个机械手都刚性地附在负载上，使抓握点与被抓物之间无滑移现象；当双臂机器人协同操作同一个物体时，机器人和物体形成一个闭链系统，定义：{O}为物体的中心位置，{O
R1
}、{O
R1
}分别为机器人R1和R2的基座坐标系，{O2}、{O1}分别为机器人R1和R2的工具坐标系；根据双臂机器人工作时的情况确定双臂机器人的运动约束关系；步骤2：依据牛顿第二定律对双臂机器人搬运的物体的受力情况进行受力分析；步骤3：运用拉格朗日方法，得到单机械臂的动力学方程；根据单机械臂的动力学方程推广得到双机械臂在关节空间中的动力学方程；步骤4：针对步骤3中提出的动力学模型，将外部干扰和未建模部分归类为干扰项，并用干扰观测器进行观测并与双曲正切滑模控制器相结合对双臂机器人末端进行位置控制；步骤5：把物体的实际运动位置X
m
与期望运动位置进行比较，然后将误差进行运动/力的转换对开始期望力进行力补偿得到末端期望接触力F
d
，通过自适应控制器估计环境的刚度和位置得到阻抗控制的初始期望位置X
c
；步骤6：将双臂机器人末端的实际接触力与期望接触力的误差通过阻抗控制修正后得到期望位置；步骤7：将步骤6得到的期望位置作为由步骤4得到的位置控制器的期望输入位置，与实际位置比较做差得到位置误差并通过控制器输出机械臂的输出力，控制双臂机器人完成对被操作物体的搬运任务。2.根据权利要求1所述的一种考虑末端力的双臂机器人协同控制方法，其特征在于，步骤1所述根据双臂机器人工作时的情况确定双臂机器人的运动约束关系为：骤1所述根据双臂机器人工作时的情况确定双臂机器人的运动约束关系为：其中为坐标系{O
R1
}下机器人R2末端执行器的位姿，为坐标系{O
R1
}下机器人R1末端执行器的位姿；1T2为工具坐标系{O2}到{O1}的位姿变换矩阵，为：1T2＝Rot(z，π)
×
Trans(d1，0，0)；式1为机器人R1和R2之间的运动约束，同时联系着R1和R2的运动学，若当已知机器人R1的构型或者末端执行器的位置，则通过运动约束求解机器人R2的正向运动学与逆向运动学，无需再对机器人R2进行运动学建模求解。3.根据权利要求1所述的一种考虑末端力的双臂机器人协同控制方法，其特征在于，步骤2依据牛顿第二定律对双臂机器人搬运的物体的受力情况进行受力分析结果如下：其中m为被搬运物体的质量；和分别为被双臂机器人搬运物体在x，y和z方向上的加速度；F1和F2分别是两个机械臂末端对物体的操作力；F
s1y
，F
s2y
，F
s1z
和F
s2z
分别是机械臂与物体的接触摩擦力在y轴和z轴方向的分力；μ是摩擦系数；为保持物体平衡应该满足以下条件：
F
s1y2
+F
s1z2
＜(
μ
F1)2F
s2y2
+F
s2z2
＜(μF2)2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)双机械臂末端施加给被搬运物体的力由下式得到：械臂末端施加给被搬运物体的力由下式得到：械臂末端施加给被搬运物体的力由下式得到：械臂末端施加给被搬运物体的力由下式得到：。4.根据权利要求1所述的一种考虑末端力的双臂机器人协同控制方法，其特征在于，步骤3运用拉格朗日方法，得到单机械臂的动力学方程为：其中M
i
(q
i
)是第i个机械臂的对称正定惯性矩阵；是科里奥利矩阵和离心矩阵；G
i
(q
i
)是引力矢量；J
i
(q
i
)是雅可比矩阵；F
i
是物体施加在手臂上的力矢量；w是外部环境对于机械臂关节的扰动力矩；
τi
是机械臂关节的控制输入力矩；根据单机械臂的动力学方程推广得到双机械臂在关节空间中的动力学方程为：其中，q＝[q
1T
，q
2T
]
T
∈R6×1为双机械臂的各关节角度矩阵；M(q)＝diag[M1(q1)，M2(q2)]∈R6×6双机械臂的正定质量惯性矩阵；为双机械臂的科里奥利力与离心力矩阵；G(q)＝[G
1T
(
q1
)，G
2T
(
q2
)]
T
∈R6×1为双机械臂的重力矩阵；τ＝[τ
1T
，τ
2T
]
T
∈R6×1为双机械臂的各关节控制输...

【专利技术属性】
技术研发人员：王义娜，黄香玲，周勃，杨俊友，
申请(专利权)人：沈阳工业大学，
类型：发明
国别省市：