一种基于任务分解的双臂冗余机械臂动态协同控制方法技术

本发明专利技术公开了一种基于任务分解的双臂冗余机械臂动态协同控制方法，包括以下步骤：给定双臂的目标任务；为双臂选定多个特征点；建立双臂系统的运动学方程，求解双臂的控制点位置与速度；通过控制点建立双臂的自避障任务，建立冗余机械臂的关节速度通解方程，并加入动态决策因子和双臂的自避障任务；直至双臂完成目标任务。实现了冗余双臂运动学任务分解的动态决策控制方法，可以根据双臂实时的运动状态采取不同的控制策略，使双臂在保证安全的基础上完成给定的目标任务。上完成给定的目标任务。上完成给定的目标任务。

【技术实现步骤摘要】
一种基于任务分解的双臂冗余机械臂动态协同控制方法


[0001]本专利技术属于双臂机器人的决策控制
，涉及一种基于任务分解的双臂冗余机械臂动态协同控制方法。

技术介绍

[0002]随着科技的发展和工业生产的进步，单机械臂系统往往难以满足机器人的自动化、智能化要求，双冗余机械臂系统凭借其高适应性、强协作性与高灵活性被作为机器人研究的重点之一。因为双臂系统存在的操作空间重叠的问题，双臂协调与避碰方法的研究是双臂系统的重点。
[0003]目前，针对双臂的协调控制问题，已经有多种方法和策略。很多研究的重点在运动控制的过程中，让双臂保持合理的安全距离；或者在可行空间中求取一条无碰路径。针对双臂自避障的问题求解，目前有以下解决办法：
[0004](1)在构型空间中规划一条从起点到终点的无碰路径；
[0005](2)将初始的规划路径归一化，建立协调空间，改变路径点上的速度使双臂在发生碰撞前互相避让；
[0006](3)将双臂之间的距离作为运动控制的限制条件，维持双臂的安全距离；
[0007](4)为双臂建立势场，双臂之间互有斥力，引导双臂保持较为安全的距离。
[0008]但是，在上述方法中，多自由度的双臂系统在构型空间与协调空间存在高维计算的问题。多自由度机械臂系统具有较高的计算复杂度，难以满足实时控制的及时性要求；限制双臂距离函数仅仅考虑了双臂之间的相对距离，没有将相对速度作为考虑内容，在机械臂高速运行时，难以避免高速迫近发生碰撞的特殊情况。吴长征等人在《双臂机器人自碰撞检测及其运动规划》中提出了一种基于空间向量几何距离的机械臂自碰撞检测的方法，并通过改进的人工势场法，将线性斥力作为算子进行自避障规划。构造势场等方法通过对机械臂连杆的积分构建相应的场函数，将场函数作为机械臂控制的依据。一方面三维空间下构建场提高了计算复杂度，另一方面势场法存在使机械臂陷入局部极小值的问题，最终难以完成任务。针对上述问题，本方法一方面将双臂之间的相对速度作为控制依据之一，可应对双臂之间高速迫近的危险情况；另一方面直接在工作空间构造避障任务，避免了场函数或其他空间的转化，提高了规划效率，增强了机械臂控制的实时性。

技术实现思路

[0009]本专利技术的目的在于克服上述现有技术的缺点，提供一种基于任务分解的双臂冗余机械臂动态协同控制方法。
[0010]本专利技术至少通过如下技术方案之一实现。
[0011]一种基于任务分解的双臂冗余机械臂动态协同控制方法，包括以下步骤：
[0012]步骤1、给定双臂各自的目标任务；
[0013]步骤2、在双臂设置多个特征点，表示双臂所在的空间位置；
[0014]步骤3、建立双臂系统的运动学方程，求解特征点的空间位置并相应确定控制点的空间位置，获取双臂控制点空间速度；
[0015]步骤4、确定双臂的自避障任务，建立冗余机械臂的关节速度通解方程，并加入动态决策因子和双臂的自避障任务；
[0016]步骤5、判断双臂是否完成目标任务，如未完成，至步骤3。
[0017]优选的，步骤1具体为：给定机械臂末端在笛卡尔空间的目标位置：
[0018][x
d
,y
d
,z
d
][0019]其中，x
d
,y
d
,z
d
分别为机械臂基座坐标系下的三维坐标值。
[0020]优选的，步骤2具体为：在双臂的关节与连杆处设定多个特征点，在机械臂的各个关节中心处设置特征点，并在机械臂的前三个连杆的中心处设置特征点。
[0021]优选的，确定控制点的空间位置具体如下：
[0022]以某一侧臂基座坐标系作为参考坐标系，通过正运动学方程获得双臂2n个特征点的位置，用P
ri
(i＝1,2,
…
,n)代表该侧臂第i个特征点的位置，n表示特征点数量，用P
lj
(j＝1,2,
…
,n)代表另一侧臂臂第j个特征点的位置：
[0023]P
ri
＝f
ri
(q
r
)＝(x
ri
,y
ri
,z
ri
)
[0024]f
ri
(
·
)为点P
ri
处的正运动学公式，q
r
为该侧臂的关节角度向量，(x
ri
,y
ri
,z
ri
)为P
ri
的三维空间坐标值；
[0025]P
li
＝f
li
(q
l
)+[0,L,0]＝(x
li
,y
li
,z
li
)
[0026]f
li
(
·
)为点P
li
处的正运动学公式，q
l
为另一侧臂的关节角度向量，(x
li
,y
li
,z
li
)为P
li
的坐标值；计算双臂各个特征点之间的距离，用d
ij
代表一侧臂第i个特征点和另一侧臂第j个特征点之间的距离：
[0027]d
ij
＝‖P
ri
‑
P
lj
‖
[0028]维护一个n
×
n的距离为D，并选定双臂特征点距离最短的两个特征点作为双臂的控制点，用P
r
代表一侧臂的控制点位置，用P
l
代表另一侧臂的控制点位置。
[0029]优选的，步骤3中，通过雅可比矩阵获取双臂控制点空间速度。
[0030]优选的，获取双臂控制点空间速度具体为：
[0031]通过双臂的关节速度向量和控制点处的雅可比矩阵，获得控制点处的笛卡尔空间速度，用v
r
代表一侧臂的控制点速度，用v
l
代表另一侧臂的控制点速度：
[0032][0033][0034]其中，J
r
和J
l
分别为P
r
与P
l
的雅可比矩阵，和分别为双臂此时的关节速度向量。
[0035]优选的，所述步骤4具体为：
[0036]确定两个控制点的相对位置向量P
a
：
[0037]P
a
＝P
r
‑
P
l
[0038]设定回退速度为v
b
，扩大双臂控制点距离的速度，速度方向与相对位置向量方向相同，速度的大小随着控制点距离变化：
[0039][0040]式中，d表示双臂控制点之间的空间距离，γ
b
控制避障速度随d变化的快慢，σ
本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，包括以下步骤：步骤1、给定双臂各自的目标任务；步骤2、在双臂设置多个特征点，表示双臂所在的空间位置；步骤3、建立双臂系统的运动学方程，求解特征点的空间位置并相应确定控制点的空间位置，获取双臂控制点空间速度；步骤4、确定双臂的自避障任务，建立冗余机械臂的关节速度通解方程，并加入动态决策因子和双臂的自避障任务；步骤5、判断双臂是否完成目标任务，如未完成，至步骤3。2.根据权利要求1所述的基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，步骤1具体为：给定机械臂末端在笛卡尔空间的目标位置：[x
d
，y
d
，z
d
]其中，x
d
，y
d
，z
d
分别为机械臂基座坐标系下的三维坐标值。3.根据权利要求2所述的基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，步骤2具体为：在双臂的关节与连杆处设定多个特征点，在机械臂的各个关节中心处设置特征点，并在机械臂的前三个连杆的中心处设置特征点。4.根据权利要求3所述的基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，确定控制点的空间位置具体如下：以某一侧臂基座坐标系作为参考坐标系，通过正运动学方程获得双臂2n个特征点的位置，用P
ri
(i＝1，2，...，n)代表该侧臂第i个特征点的位置，n表示特征点数量，用P
lj
(j＝1，2，...，n)代表另一侧臂臂第j个特征点的位置：P
ri
＝f
ri
(q
r
)＝(x
ri
，y
ri
，z
ri
)f
ri
(
·
)为点P
ri
处的正运动学公式，q
r
为该侧臂的关节角度向量，(x
ri
，y
ri
，z
ri
)为P
ri
的三维空间坐标值；P
li
＝f
li
(q
l
)+[0，L，0]＝(x
li
，y
li
，z
li
)f
li
(
·
)为点P
li
处的正运动学公式，q
l
为另一侧臂的关节角度向量，(x
li
，y
li
，z
li
)为P
li
的坐标值；计算双臂各个特征点之间的距离，用d
ij
代表一侧臂第i个特征点和另一侧臂第j个特征点之间的距离：d
ij
＝||P
ri
‑
P
lj
||维护一个n
×
n的距离为D，并选定双臂特征点距离最短的两个特征点作为双臂的控制点，用P
r
代表一侧臂的控制点位置，用P
l
代表另一侧臂的控制点位置。5.根据权利要求4所述的基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，步骤3中，通过雅可比矩阵获取双臂控制点空间速度。6.根据权利要求5所述的基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，获取双臂控制点空间速度具体为：通过双臂的关节速度向量和控制点处的雅可比矩阵，获得控制点处的笛卡尔空间速度，用v
r
代表一侧臂的控制点速度，用v
l<...

【专利技术属性】
技术研发人员：张勤，刘丰溥，
申请(专利权)人：华南理工大学，
类型：发明
国别省市：