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

1.本发明属于双臂机器人的决策控制技术领域，涉及一种基于任务分解的双臂冗余机械臂动态协同控制方法。


背景技术：

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


技术实现要素：

9.本发明的目的在于克服上述现有技术的缺点，提供一种基于任务分解的双臂冗余机械臂动态协同控制方法。
10.本发明至少通过如下技术方案之一实现。
11.一种基于任务分解的双臂冗余机械臂动态协同控制方法，包括以下步骤：
12.步骤1、给定双臂各自的目标任务；
13.步骤2、在双臂设置多个特征点，表示双臂所在的空间位置；
14.步骤3、建立双臂系统的运动学方程，求解特征点的空间位置并相应确定控制点的空间位置，获取双臂控制点空间速度；
15.步骤4、确定双臂的自避障任务，建立冗余机械臂的关节速度通解方程，并加入动态决策因子和双臂的自避障任务；
16.步骤5、判断双臂是否完成目标任务，如未完成，至步骤3。
17.优选的，步骤1具体为：给定机械臂末端在笛卡尔空间的目标位置：
18.[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变化的快慢，σ
b
为避障速度开始产生作用的控制阈值；
[0041]
确定两个控制点的相对速度向量v
a
：
[0042]
v
a
＝v
l
‑
v
r
[0043]
相对位置向量与相对速度向量之间的夹角
[0044][0045]
设定闪避速度v
d
，当一侧臂持续迫近时，另一侧臂相应的控制点产生向侧面闪避的速度，其方向正交于相对位置向量和相对速度向量，其大小随着控制点距离变化：
[0046][0047]
式中，γ
d
控制动态决策因子随d变化的快慢，γ
d
为动态决策因子开始产生作用的控制阈值参数；
[0048]
将回退速度与闪避速度相加获得躲避速度v
h
：
[0049]
v
h
＝v
b
+v
d
[0050]
通过控制点的雅可比矩阵将躲避速度映射到关节空间
[0051][0052]
式中为控制点的雅可比矩阵的广义逆矩阵；
[0053]
对于冗余机械臂，假设其自由度为m，其第i个关节角度为q
i
(i＝1，2，
…
,m)，对应关节角度向量为q，完成目标轨迹y
d
所需关节速度的通解为：
[0054][0055]
式中，j为目标轨迹y
d
对应的雅可比矩阵，j
+
为j的广义逆矩阵，i
m
为m阶单位矩阵，k为任意的m维空间矢量，为目标轨迹的微分函数。
[0056]
优选的，所述加入动态决策因子α为：
[0057][0058]
其中动态决策因子α为：
[0059][0060]
式中，d表示双臂控制点之间的空间距离，γ控制动态决策因子随d变化的快慢，σ为动态决策因子开始产生作用的控制阈值；
[0061]
将机械臂实现自避障动作时各轴关节的角速度作为机械臂运动的子运动：
[0062][0063]
优选的，机械臂的当前位置与目标位置的误差在阈值内，则说明双臂完成目标任务，如果机械臂的当前位置与目标位置的误差超过阈值，则返回步骤3。
[0064]
优选的，所述步骤5具体为：
[0065]
通过正运动学方程获取机械臂的当前位置与姿态：
[0066]
p
e
＝f
e
(q)＝[x
e
,y
e
,z
e
]
[0067]
其中，p
e
是机械臂末端的位置，f
e
(
·
)为机械臂末端的正运动学公式，q为机械臂当前的关节角度向量，[x
e
,y
e
,z
e
]为p
e
的空间坐标值；
[0068]
给定误差阈值∈，如果机械臂的当前位置与目标位置的误差在阈值内：
[0069][0070]
控制结束，如果机械臂的当前位置与目标位置的误差超过阈值：
[0071][0072]
回至步骤3。
[0073]
与现有技术相比，本发明具有以下有益效果：
[0074]
通过多个特征点代表机械臂的空间位置，简化了多连杆结构的空间表达。综合考虑双臂控制点的相对距离和相对速度，建立双臂的自避障任务。利用动态决策因子，使双臂在不同情况下实现不同的控制策略。
附图说明
[0075]
图1为本实例的机械臂构型图；
[0076]
图2为本实例的双臂特征点选取示意图；
[0077]
图3为实例1在使用任务分解的动态决策控制方法时的右臂各关节角度位置仿真结果示意图；
[0078]
图4为实例1在使用任务分解的动态决策控制方法时的左臂各关节角度位置仿真结果示意图；
[0079]
图5为实例1在不使用任务分解的动态决策控制方法时的右臂关节各角度位置仿真结果示意图；
[0080]
图6为实例1在不使用任务分解的动态决策控制方法时的左臂关节各角度位置仿真结果示意图；
[0081]
图7为实例1在使用任务分解的动态决策控制方法时双臂最短距离示意图；
[0082]
图8为实例1在不使用任务分解的动态决策控制方法时双臂最短距离示意图；
[0083]
图9为实例1在使用任务分解的动态决策控制方法时右臂末端位置示意图；
[0084]
图10为实例2双臂位置示意图；
[0085]
图11为实例2在使用任务分解的动态决策控制方法时双臂位置实线示意图；
[0086]
图12为实例2在不使用任务分解的动态决策控制方法时双臂位置虚线示意图；
[0087]
图13为实例2在不使用任务分解的动态决策控制方法时双臂位置示意图；
[0088]
图14为实例2在不使用任务分解的动态决策控制方法时双臂末端位置示意图；
[0089]
图15为实例2在使用任务分解的动态决策控制方法时双臂末端位置图；
[0090]
图16为实例2在使用任务分解的动态决策控制方法时双臂末端位置示意图；
[0091]
图17为实例3在使用任务分解的动态决策控制方法时双臂末端位置示意图；
[0092]
图18为实例3在使用任务分解的动态决策控制方法时双臂末端位置示意图；
[0093]
图19为实例3在不使用任务分解的动态决策控制方法时双臂末端位置示意图；
[0094]
图20为实例3在不使用任务分解的动态决策控制方法时双臂末端位置示意图；
[0095]
图21为实例3在不使用任务分解的动态决策控制方法时双臂末端位置示意图；
[0096]
图22为实例3在使用任务分解的动态决策控制方法时双臂末端位置示意图；
[0097]
图23为实例3在使用任务分解的动态决策控制方法时双臂末端位置示意图；
[0098]
图24为实例4在使用任务分解的动态决策控制方法时双臂末端位置示意图；
[0099]
图25为实例4在使用任务分解的动态决策控制方法时双臂末端位置示意图；
[0100]
图26为实例4在不使用任务分解的动态决策控制方法时双臂末端位置示意图；
[0101]
图27为实例4在不使用任务分解的动态决策控制方法时双臂末端位置示意图；
[0102]
图28为实例4在不使用任务分解的动态决策控制方法时双臂末端位置示意图；
[0103]
图29为实例4在使用任务分解的动态决策控制方法时双臂末端位置示意图；
[0104]
图30为实例4在使用任务分解的动态决策控制方法时双臂末端位置示意图。
具体实施方式
[0105]
下面将结合本发明实施例中的附图，对本发明实施例进行清晰、完整地描述。
[0106]
如图1和图2所示，本实施例一种基于任务分解的双臂冗余机械臂动态协同控制方法，包括以下步骤：
[0107]
步骤1、给定双臂各自的目标任务，即给定机械臂末端在笛卡尔空间的目标位置：
[0108]
[x
d
,y
d
,z
d
]
[0109]
其中，x
d
，y
d
，z
d
分别为机械臂基座坐标系下的三维坐标值。
[0110]
步骤2、在双臂的关节与连杆处设定多个特征点，在机械臂的各个关节中心处设置特征点，并在较长连杆的中心处设置特征点。特征点近似表示双臂所在的空间位置，多个特征点作为双臂系统的空间表达。
[0111]
步骤3、建立双臂系统的运动学方程，求解特征点的空间位置并确定控制点的空间位置，通过雅可比矩阵获取双臂控制点空间速度，具体为：
[0112]
以右臂基座坐标系作为参考坐标系，通过正运动学方程获得双臂各个特征点的位置：
[0113]
p
ri
＝f
ri
(q
r
)＝(x
ri
，y
ri
，z
ri
)
[0114]
其中，(x
ri
，y
ri
，z
ri
)为点p
ri
的三维空间坐标值，f
ri
(
·
)为点p
ri
处的正运动学公式，q
r
为右臂的关节向量，p
ri
(i＝1，2，
…
,n)代表右臂第i个特征点的位置，n表示特征点的数量，用p
lj
(j＝1，2，
…
,n)代表左臂第j个特征点的位置：
[0115]
p
li
＝f
li
(q
l
)+[0，l，0]＝(x
li
，y
li
，z
li
)
[0116]
其中，f
li
(
·
)为点p
li
处的正运动学公式，q
l
为左臂的关节向量，(x
li
，y
li
，z
li
)为p
li
的三维空间坐标值。
[0117]
计算双臂各个特征点之间的距离，用d
ij
代表右臂第i个特征点和左臂第j个特征点之间的距离：
[0118]
d
ij
＝‖p
ri
‑
p
lj
‖
[0119]
维护一个n
×
n的距离为d，并选定双臂特征点距离最短的两个特征点作为双臂的控制点，用p
r
代表右臂的控制点位置，用p
l
代表左臂的控制点位置。
[0120]
通过此时双臂的关节速度向量和控制点处的雅可比矩阵，可获得控制点处的笛卡
尔空间速度：
[0121][0122][0123]
其中，j
r
和j
l
分别为p
r
与p
l
的雅可比矩阵，和分别为右臂与左臂此时的关节速度向量，v
r
代表右臂的控制点速度，v
l
代表左臂的控制点速度。
[0124]
步骤4、确定双臂的自避障任务，建立冗余机械臂的关节速度通解方程，并加入动态决策因子和双臂的自避障任务，具体为：
[0125]
确定两个控制点的相对位置向量p
a
：
[0126]
p
a
＝p
r
‑
p
l
[0127]
设定回退速度v
b
：扩大双臂控制点距离的速度。其方向与相对位置向量相同，其大小随着控制点距离变化：
[0128][0129]
式中，d表示双臂控制点之间的空间距离，γ
b
控制避障速度随d变化的快慢，σ
b
为避障速度开始产生作用的控制阈值；
[0130]
确定两个控制点的相对速度向量v
a
：
[0131]
v
a
＝v
l
‑
v
r
[0132]
相对位置向量与相对速度向量之间的夹角
[0133][0134]
设定闪避速度v
d
：向侧面闪避持续迫近的左臂控制点。其方向正交于相对位置向量和相对速度向量，其大小随着控制点距离变化：
[0135][0136]
式中，d表示双臂控制点之间的空间距离，γ
b
控制避障速度随d变化的快慢，σ
b
为避障速度开始产生作用的控制阈值。
[0137]
将回退速度与闪避速度相加获得躲避速度v
h
：
[0138]
v
h
＝v
b
+v
d
[0139]
通过控制点的雅可比矩阵将躲避速度映射到关节空间
[0140][0141]
式中为控制点的雅可比矩阵的广义逆矩阵。
[0142]
对于冗余机械臂，假设其自由度为m，其第i个关节角度为q
i
(i＝1，2，
…
，m)，对应关节向量为q，完成目标轨迹y
d
所需关节速度的通解为
[0143][0144]
式中，j为y
d
目标轨迹对应的雅可比矩阵，j
+
为j的广义逆矩阵，i
m
为m阶单位矩阵，k为任意的m维空间矢量，为目标轨迹的微分函数。
[0145]
加入动态决策因子α：
[0146][0147]
其中动态决策因子α为：
[0148][0149]
将机械臂实现自避障动作时各轴关节的角速度作为机械臂运动的子运动。
[0150][0151]
步骤5、判断双臂是否完成目标任务，如未完成，至步骤3。
[0152]
通过正运动学方程获取机械臂的当前位置与姿态：
[0153]
p
e
＝f
e
(q)＝[x
e
，y
e
，z
e
]
[0154]
其中，p
e
是机械臂末端的位置，f
e
(
·
)为机械臂末端的正运动学公式，q为机械臂当前的关节向量，[x
e
,y
e
,z
e
]为p
e
的三维空间坐标值。
[0155]
给定误差阈值∈，如果机械臂的当前位置与目标位置的误差在一定阈值内：
[0156][0157]
控制方法结束。如果机械臂的当前位置与目标位置的误差超过阈值：
[0158][0159]
回至步骤3。
[0160]
下面将本发明的方法应用到具体的实例1中：
[0161]
1)给定目标位置：
[0162]
2)左臂目标位置：[0.300；
‑
0.300；0.000]，
[0163]
3)左臂当前位置：
[0164]
4)q
l
[0.1；
‑
1.27；
‑
1.574；
‑
0.3045；2.854；0.0]
→
[
‑
0.4930；
‑
0.8244；
‑
1.1924；
‑
0.3680；3.1993；0.0]
[0165]
5)右臂目标位置：[0.500；0.200；
‑
0.250]，
[0166]
6)右臂当前位置：
[0167]
7)q
r
[0.538；
‑
0.883；
‑
0.8633；
‑
0.13；2.643；0]
→
[
‑
0.1959；
‑
0.4492；
‑
1.8690；
‑
1.3579；0.8448；
‑
0.0258]
[0168]
仿真结果如图3、图4、图7中实线、图8和图9所示，通过仿真结果可以看出，当使用冗余双臂运动学任务分解的动态决策控制方法时，双臂在保证安全距离的同时末端都已到达目标位置，保证安全的基础上完成目标任务。如图5、图6、图7中虚线所示，当不使用冗余双臂运动学任务分解的动态决策控制方法时，双臂在运动初期就已经发生了碰撞，无法继续完成任务，运动失败。
[0169]
下面将本发明的方法应用到具体的实例2中：
[0170]
1)给定目标位置：
[0171]
2)左臂目标位置：[0.350；
‑
0.260；0.050]，
[0172]
3)左臂当前位置：
[0173]
4)q
l
[0.1；
‑
1.27；
‑
1.574；
‑
0.3045；2.854；0.0]
→
[
‑
0.3791；
‑
1.1397；
‑
1.4941；
‑
0.3544；2.8478；0.0]
[0174]
5)右臂目标位置：[0.500；0.250；
‑
0.200]，
[0175]
6)右臂当前位置：
[0176]
7)q
r
[0.538；
‑
0.883；
‑
0.8633；
‑
0.13；2.643；0]
→
[
‑
0.0275；
‑
0.6141；
‑
1.8763；
‑
1.2149；0.9729；
‑
0.0115]
[0177]
仿真结果如图10、图11、图14中实线、图15和图16所示，通过仿真结果可以看出，当使用冗余双臂运动学任务分解的动态决策控制方法时，双臂在保证安全距离的同时末端都已到达目标位置，保证安全的基础上完成目标任务。如图12、图13、图14中虚线所示，当不使用冗余双臂运动学任务分解的动态决策控制方法时，双臂在运动初期就已经发生了碰撞，无法继续完成任务，运动失败。
[0178]
下面将本发明的方法应用到具体的实例3中：
[0179]
1)给定目标位置：
[0180]
2)左臂目标位置：[0.320；
‑
0.280；0.025]，
[0181]
3)左臂当前位置：
[0182]
4)q
l
[0.1；
‑
1.27；
‑
1.574；
‑
0.3045；2.854；0.0]
→
[
‑
0.4866；
‑
1.2512；
‑
1.7589；
‑
0.5077；2.6847；0.0]
[0183]
5)右臂目标位置：[0.450；0.230；
‑
0.200]，
[0184]
6)右臂当前位置：
[0185]
7)q
r
[0.538；
‑
0.883；
‑
0.8633；
‑
0.13；2.643；0]
→
[
‑
0.1263；
‑
0.6679；
‑
2.1252；
‑
1.4080；0.8633；
‑
0.0179]
[0186]
仿真结果如图17、图18、图21中实线、图22和图23所示，通过仿真结果可以看出，当使用冗余双臂运动学任务分解的动态决策控制方法时，双臂在保证安全距离的同时末端都已到达目标位置，保证安全的基础上完成目标任务。如图19、图20、图21中虚线所示，当不使用冗余双臂运动学任务分解的动态决策控制方法时，双臂在运动初期就已经发生了碰撞，无法继续完成任务，运动失败。
[0187]
下面将本发明的方法应用到具体的实例4中：
[0188]
1)给定目标位置：
[0189]
2)左臂目标位置：[0.320；
‑
0.260；
‑
0.050]，
[0190]
3)左臂当前位置：
[0191]
4)q
l
[0.1；
‑
1.27；
‑
1.574；
‑
0.3045；2.854；0.0]
→
[
‑
0.3855；
‑
0.9143；
‑
1.5747；
‑
0.6605；3.0188；0.0]
[0192]
5)右臂目标位置：[0.450；0.220；
‑
0.250]，
[0193]
6)右臂当前位置：
[0194]
7)q
r
[0.538；
‑
0.883；
‑
0.8633；
‑
0.13；2.643；0]
→
[
‑
0.0956；
‑
0.4515；
‑
2.0497；
‑
1.5670；0.9600；
‑
0.0159]
[0195]
仿真结果如图24、图25、图28中实线、图29和图30所示，通过仿真结果可以看出，当使用冗余双臂运动学任务分解的动态决策控制方法时，双臂在保证安全距离的同时末端都已到达目标位置，保证安全的基础上完成目标任务。如图26、图27、图28中虚线所示，当不使用冗余双臂运动学任务分解的动态决策控制方法时，双臂在运动初期就已经发生了碰撞，无法继续完成任务，运动失败。
[0196]
以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。