一种针对闭链奇异点回避的双臂协同运动规划方法及系统

1.本发明涉及双臂协同运动规划技术领域，尤其涉及一种针对闭链奇异点回避的双臂协同运动规划方法及系统。


背景技术：

2.本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。
3.相比单臂机器人，双臂机器人拥有更好的负载能力，由于自由度和末端执行器数量的增加，双臂机器人可以完成更多单臂机器人无法完成的任务，例如双臂协同搬运物体工作，因此双臂机器人在制造业等领域拥有更加广泛的用途。
4.双臂机器人进行搬运作业时，根据环境约束对机器人进行运动规划是一个极为重要的环节，运动规划需要得到搬运作业的无碰撞路径，以保证被搬运物体和机器人本体的安全性。针对双臂协同搬运的运动规划，现有的工作主要分为两类：在关节空间中的规划方法和在任务空间中的规划方法，然而在环境约束较为复杂时，直接对在关节空间中进行规划无法保证被搬运物体的运行路径满足环境中的障碍物约束，常采用任务空间中的运动规划方法。任务空间中的运动规划方法主要通过设置双臂机器人的主臂和从臂，首先对主臂末端进行路径规划，然后根据双臂协同的运动学约束，计算出从臂末端和被搬运物体的连续路径，以完成运动规划任务。
5.双臂机器人在搬运物体的过程中，双臂末端存在着严格的运动学约束，此时双臂与被搬运物体形成了闭链。与单机械臂相似，该闭链也存在着奇异点，在闭链工作空间范围内，由于特殊关节配置导致末端坐标系在笛卡尔坐标系下失去某个方向自由度的奇异点称为闭链系统的末端执行器奇异点。zhou等人提出了在奇异点处通过一个机械臂固定物体、改变另一个机械臂的抓取配置，使得新配置下的闭链系统非奇异，这类重规划方法有效避免了奇异点处无解的情况，但是降低了规划效率。奇异点的出现与关节配置有关，同时也与闭链末端坐标系的位置有关，park等人通过改变末端坐标系位置增加系统的冗余度，从而改变闭链系统的雅可比构造实现闭链奇异点的穿越。而当作业环境较为简单时，奇异点穿越方法可以有效避免在奇异点处进行重规划或重抓取，提高规划效率。但是在闭链奇异点穿越方法中，由于末端坐标系位置发生了改变，在闭链奇异点附近无法确定被搬运物体在任务空间中的路径，可能导致在该区域内的规划路径与期望路径之间的误差过大，无法满足环境中的障碍物约束。


技术实现要素：

6.本发明为了解决上述问题，提出了一种针对闭链奇异点回避的双臂协同运动规划方法及系统，实现了对闭链系统奇异点的回避，保证了闭链系统在奇异点处关节解和被搬运物体在任务空间中解的确定性。
7.为实现上述目的，本发明采用如下技术方案：
8.第一方面，提出了一种针对闭链奇异点回避的双臂协同运动规划方法，包括：
9.获取被搬运物体的起始点和终止点；
10.根据被搬运物体的起始点和终止点，确定闭链系统的关节路径及雅可比；
11.根据闭链系统的雅可比确定闭链系统关节路径的奇异区域部分；
12.对于奇异区域部分，确定闭链系统的主臂和从臂，基于阻尼最小二乘的雅可比伪逆法确定主臂的关节速度；根据主臂的关节速度，确定从臂的关节速度；对各臂的关节速度进行积分，获得各臂的关节角度变化值；通过各臂的关节角度变化值，替换关节路径中的奇异区域部分，获得闭链系统无奇异点的规划路径。
13.当不处于奇异区域时，通过雅克比逆映射，确定双臂的关节速度。
14.第二方面，提出了一种针对闭链奇异点回避的双臂协同运动规划系统，包括：
15.起止点获取模块，用于获取被搬运物体的起始点和终止点；
16.雅可比确定模块，用于根据被搬运物体的起始点和终止点，确定闭链系统的关节路径及雅可比；
17.规划路径奇异区域确定模块，用于根据闭链系统的雅可比确定闭链系统关节路径的奇异区域部分；
18.闭链系统无奇异点的规划路径确定模块，用于对于奇异区域部分，确定闭链系统的主臂和从臂，基于阻尼最小二乘的雅可比伪逆法确定主臂的关节速度；根据主臂的关节速度，确定从臂的关节速度；对各臂的关节速度进行积分，获得各臂的关节角度变化值；通过各臂的关节角度变化值，替换关节路径中的奇异区域部分，获得闭链系统无奇异点的规划路径。
19.第三方面，提出了一种电子设备，包括存储器和处理器以及存储在存储器上并在处理器上运行的计算机指令，所述计算机指令被处理器运行时，完成一种针对闭链奇异点回避的双臂协同运动规划方法所述的步骤。
20.第四方面，提出了一种计算机可读存储介质，用于存储计算机指令，所述计算机指令被处理器执行时，完成一种针对闭链奇异点回避的双臂协同运动规划方法所述的步骤。
21.与现有技术相比，本发明的有益效果为：
22.1、本发明公开方法，在获取到闭链系统的关节路径后，对于关节路径中的奇异区域进行了确定，对处于奇异区域部分，确定闭链系统的主臂和从臂，基于阻尼最小二乘的雅可比伪逆法确定主臂的关节速度；根据主臂的关节速度，确定从臂的关节速度；对各臂的关节速度进行积分，获得各臂的关节角度变化值；通过各臂的关节角度变化值，替换关节路径中的奇异区域部分，实现了闭链奇异点的回避，保证闭链系统在奇异点处关节解和被搬运物体在任务空间中解的确定性，同时避免重规划导致的规划效率降低的问题。
23.2、本发明根据阻尼最小二乘的雅可比伪逆法进行路径规划时，不仅回避了闭链系统的奇异点，还保证了被搬运物体的实际运行路径与期望路径之间的误差最小。
24.本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
25.构成本技术的一部分的说明书附图用来提供对本技术的进一步理解，本技术的示
意性实施例及其说明用于解释本技术，并不构成对本技术的不当限定。
26.图1为实施例1公开方法的整体流程图；
27.图2为实施例1公开的双臂闭链系统的基本坐标系示意图；
28.图3为实施例1公开的任务空间中路径规划流程图；
29.图4为实施例1公开的基于阻尼最小二乘雅可比伪逆法的闭链奇异点回避算法流程图。
具体实施方式
30.下面结合附图与实施例对本发明作进一步说明。
31.应该指出，以下详细说明都是例示性的，旨在对本技术提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本技术所属技术领域的普通技术人员通常理解的相同含义。
32.实施例1
33.在该实施例中，公开了一种针对闭链奇异点回避的双臂协同运动规划方法，如图1-图4所示，包括：
34.s1：获取被搬运物体的起始点ss和终止点se。
35.s2：根据被搬运物体的起始点和终止点，确定闭链系统的关节路径及雅可比。
36.确定闭链系统的关节路径的过程为：
37.根据被搬运物体的起始点和终止点，确定双臂闭链系统在任务空间中的路径；
38.根据双臂闭链系统在任务空间中的路径和闭链运动学模型，确定闭链系统的关节路径。
39.根据闭链系统的关节路径，确定闭链系统的雅可比。
40.如图3所示，确定双臂闭链系统在任务空间中的路径的过程包括：
41.s21：将起始点和终止点分别分解为位置向量和姿态向量。
42.本实施例获取的起始点ss和终止点se均为被搬运物体在任务空间下的六维位姿向量，将被搬运物体在任务空间下的六维位姿向量分解为位置和姿态两个三维向量。
43.s＝p
⊕rꢀꢀꢀꢀ
(1)
44.式中，s为六维位姿向量，p为三维位置向量，r为三维姿态向量。
45.根据公式(1)将起始点ss和终止点se分别分解为位置向量和姿态向量。
46.ps＝[p
sx
,p
sy
,p
sz
]
ꢀꢀꢀꢀ
(2)
[0047]rs
＝[r
sx
,r
sy
,r
sz
]
ꢀꢀꢀꢀ
(3)
[0048]
pe＝[p
ex
,p
ey
,p
ez
]
ꢀꢀꢀꢀ
(4)
[0049]
re＝[r
ex
,r
ey
,r
ez
]
ꢀꢀꢀꢀ
(5)
[0050]
ps为起始点的位置向量，rs为起始点的姿态向量，pe为终止点的位置向量，re为终止点的姿态向量。
[0051]
s22：根据起止点的位置向量和终止点的位置向量，获得位置路径。
[0052]
本实施例以ps为起点、pe为终点，在任务空间中通过rrt算法进行三维位置的路径规划，得到仅关于位置的三维位置路径r
p
。
[0053]
s23：根据起止点的姿态向量和终止点的姿态向量，确定姿态路径。
[0054]
本实施例以路径r
p
为基础，在按照路径点序号遍历其路径点的过程中，以rs为起点、re为终点，通过rrt算法对三维姿态向量进行规划，得到三维姿态空间下的可行姿态路径rr。
[0055]
s24：将位置路径和姿态路径进行叠加，获得双臂闭链系统在任务空间中的路径r。
[0056]
本实施例在每个路径点上将三维位置和三维姿态进行叠加，获得双臂闭链系统在任务空间中的六维完整路径r。
[0057]
r＝r
p
⊕rr
ꢀꢀꢀꢀ
(6)
[0058]
其中，闭链运动学模型包括通过d-h法为双臂中每个单臂分别构建的机械臂运动学方程和双臂协同搬运运动学约束。
[0059]
闭链运动学模型的构建过程为：确定双臂闭链系统的基本坐标系，并通过d-h法建立单机械臂的运动学模型，令左臂基座坐标系作为世界坐标系，确定双臂协同搬运运动学约束：
[0060]wt
lele
ta≡wt
rr
t
rere
taꢀꢀꢀꢀ
(7)
[0061]
其中，wt
le
、
le
ta、wtr、rt
re
、
re
ta分别为世界坐标系到左臂末端坐标系、左臂末端坐标系到物体坐标系、世界坐标系到右臂基坐标系、右臂基座坐标系到右臂末端坐标系、右臂末端坐标系到物体坐标系的旋转变换矩阵。
[0062]
双臂闭链系统坐标系如图2所示，设左臂基座坐标系为l，右臂基座坐标系为r，左臂末端坐标系为le，右臂末端坐标系为re，参考坐标系w与l重合，被搬运物体坐标系为a。由参考坐标系到被搬运物体坐标系可分别通过左臂和右臂运动学得出以下两种变换：
[0063]wta＝wt
lele
taꢀꢀꢀꢀ
(8)
[0064]wta＝wt
rr
t
rere
taꢀꢀꢀꢀ
(9)
[0065]
式中，wta为由参考坐标系到被搬运物体坐标系的转换矩阵。
[0066]
将式(8)和式(9)联立可以得到双臂协同搬运运动学约束：
[0067]wt
lele
ta≡wt
rr
t
rere
ta[0068]
该约束在双臂与被搬运物体形成闭链过程中是一直存在的，保证了双臂末端的相对位姿保持不变。
[0069]
根据双臂闭链系统在任务空间中路径和双臂协同搬运运动学约束，确定双臂末端在任务空间中路径的每个路径点上的位姿矩阵；根据双臂末端位姿矩阵获得双臂由各自基座坐标系到末端坐标系的旋转变换矩阵，然后通过解析法求解机械臂运动学方程以获得闭链系统在任务空间中处于每个路径点时的关节角度，各关节角度构成闭链系统的关节路径。
[0070]
根据闭链系统的关节路径，确定每个单臂的雅可比，再根据两个单臂的雅可比，确定闭链系统的雅可比。
[0071]
通过速度矢量叠加原理构造双臂闭链系统的雅可比，具体为：
[0072]
当两机械臂末端接触形成闭链时，两机械臂末端坐标系原点重合，此时两机械臂末端在任务空间中的速度均为wva∈r6×1。设左右臂末端速度分别为
wvle
∈r6×1和
wvre
∈r6×1，则其与wva之间的关系为：
[0073]
[0074]
考虑实际情况，双臂搬运的物体一定存在体积，两臂末端速度与wva存在一定的速度映射关系，则被搬运物体和闭链系统关节空间中速度的实际映射关系为：
[0075][0076]
其中，x
l
∈r6×6和xr∈r6×6为左臂末端速度和右臂末端速度分别与a的质心速度的映射矩阵，j
l
∈r6×6和jr∈r6×6分别为左右单臂的雅可比，为闭链系统的关节速度。根据(11)得到闭链系统的雅可比j为：
[0077][0078]
s3：根据闭链系统的雅可比确定闭链系统关节路径的奇异区域部分。
[0079]
根据闭链系统的雅可比和双臂中每个单臂的雅可比，确定闭链系统关节路径的奇异区域部分，双臂闭链系统是否处于奇异区域的判别条件为：
[0080][0081]
如图4所示，根据关节路径中每个关节角度对应的雅可比，来判断该关节角度是否处于奇异区域，当关节角度对应的雅可比满足式(13)时，判定该关节角度处于奇异点，否则，不处于奇异点。
[0082]
s4：对于奇异区域部分，确定闭链系统的主臂和从臂，基于阻尼最小二乘的雅可比伪逆法确定主臂的关节速度；根据主臂的关节速度，确定从臂的关节速度；对各臂的关节速度进行积分，获得各臂的关节角度变化值；通过各臂的关节角度变化值，替换关节路径中的奇异区域部分，获得闭链系统无奇异点的规划路径。
[0083]
当处于奇异区域时，根据每个单臂的雅可比和每个单臂的雅可比的秩，确定闭链系统的主臂和从臂。基于阻尼最小二乘的雅可比伪逆法确定主臂的关节速度，其中，以机械臂末端实际速度与期望速度误差的范数最小为目标，以机械臂关节速度变化范数小于等于设定阈值为约束，采用序列二次规划的方法确定阻尼最小二乘的雅可比伪逆法中的阻尼因子。
[0084]
基于阻尼最小二乘的雅可比伪逆法中关节速度与末端速度的映射关系，确定主臂的关节速度，基于阻尼最小二乘的雅可比伪逆法中关节速度与末端速度的映射关系具体为：
[0085][0086]
其中，δx和δq分别为末端位姿和关节位置的一阶微分，in为n维的单位方阵，φ为任意的n维列向量，为阻尼最小二乘雅可比伪逆：
[0087][0088]
其中，λ为阻尼因子。对基于阻尼最小二乘的雅可比伪逆法优化，采用自适应阻尼因子，其变化方式为：
[0089][0090]
σd为阻尼因子，采用序列二次规划的方法对最佳阻尼因子进行寻优，确定最终的阻尼因子，该阻尼因子使机械臂末端实际速度与期望速度误差的范数最小，且机械臂关节速度变化范数小于等于设定阈值。
[0091]
将阻尼因子的寻优表示为一个二次规划问题：
[0092][0093]
其中，f(σd)是非线性目标函数，表示机械臂末端实际速度与期望速度误差的范数；g(σd)是非线性约束函数，表示机械臂关节速度变化范数，其值应小于给定的阈值δ。目标函数和约束函数表达式为：
[0094][0095][0096]
其中，为机械臂末端六维解算速度，为任务空间六维期望速度，为关节速度。
[0097]
根据主臂的关节速度和从臂关节速度更新方程，确定从臂的关节速度，其中，从臂关节速度更新方程为：
[0098][0099]
其中，d1和d2为奇异区域判断系数，当系统位于奇异区域时，d1＝1、d2＝0；λ1和λ2为主从臂判断系数，当左臂为主臂时，λ1＝1、λ2＝0，反之，λ1＝0、λ2＝1；和分别为左右臂的关节速度，和分别为基于阻尼最小二乘法的左右臂的雅可比伪逆，和分别为根据主臂的近似解得到的从臂雅可比伪逆。
[0100]
例如当左臂被判定为主臂、右臂为从臂时，先确定左臂的关节速度，之后根据左臂的关节速度和从臂关节速度更新方程确定右臂的关节速度，此时，从臂关节速度更新方程为：
[0101][0102]
通过上述算法在闭链奇异点处计算出关节速度，当相邻路径点之间的时间间隔是确定的时，通过对关节速度进行积分，得到关节角度的变化值，通过关节角度变化值，替换关节路径中的奇异区域部分，获得闭链系统无奇异点的规划路径，以实现闭链奇异点的回避。
[0103]
此外，对于不处于奇异区域的路径点，通过雅克比逆映射，确定双臂的关节速度。
[0104]
通过雅可比逆映射对各单臂的雅可比进行求解，确定每个单臂的关节速度的公式为：
[0105][0106]
本实施例公开方法，以被搬运物体为中心，在任务空间下通过rrt算法对其进行无碰撞路径规划，分析闭链系统的运动学约束并构造了闭链系统的雅可比，设计了基于阻尼最小二乘雅可比伪逆法的闭链奇异点回避算法，并将其应用至闭链运动规划中，避免了重规划或重抓取，同时保证了被搬运物体的实际运行路径与期望路径之间没有较大的误差，有效提高了双臂协同运动规划的效率，具有实际应用价值。
[0107]
实施例2
[0108]
在该实施例中，公开了一种针对闭链奇异点回避的双臂协同运动规划系统，包括：
[0109]
起止点获取模块，用于获取被搬运物体的起始点和终止点；
[0110]
雅可比确定模块，用于根据被搬运物体的起始点和终止点，确定闭链系统的关节路径及雅可比；
[0111]
规划路径奇异区域确定模块，用于根据闭链系统的雅可比确定闭链系统关节路径的奇异区域部分；
[0112]
闭链系统无奇异点的规划路径确定模块，用于对于奇异区域部分，确定闭链系统的主臂和从臂，基于阻尼最小二乘的雅可比伪逆法确定主臂的关节速度；根据主臂的关节速度，确定从臂的关节速度；对各臂的关节速度进行积分，获得各臂的关节角度变化值；通过各臂的关节角度变化值，替换关节路径中的奇异区域部分，获得闭链系统无奇异点的规划路径。
[0113]
实施例3
[0114]
在该实施例中，公开了一种电子设备，包括存储器和处理器以及存储在存储器上并在处理器上运行的计算机指令，所述计算机指令被处理器运行时，完成实施例1公开的一种针对闭链奇异点回避的双臂协同运动规划方法所述的步骤。
[0115]
实施例4
[0116]
在该实施例中，公开了一种计算机可读存储介质，用于存储计算机指令，所述计算机指令被处理器执行时，完成实施例1公开的一种针对闭链奇异点回避的双臂协同运动规划方法所述的步骤。
[0117]
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。

技术特征：
1.一种针对闭链奇异点回避的双臂协同运动规划方法，其特征在于，包括：获取被搬运物体的起始点和终止点；根据被搬运物体的起始点和终止点，确定闭链系统的关节路径及雅可比；根据闭链系统的雅可比确定闭链系统关节路径的奇异区域部分；对于奇异区域部分，确定闭链系统的主臂和从臂，基于阻尼最小二乘的雅可比伪逆法确定主臂的关节速度；根据主臂的关节速度，确定从臂的关节速度；对各臂的关节速度进行积分，获得各臂的关节角度变化值；通过各臂的关节角度变化值，替换关节路径中的奇异区域部分，获得闭链系统无奇异点的规划路径。2.如权利要求1所述的一种针对闭链奇异点回避的双臂协同运动规划方法，其特征在于，根据被搬运物体的起始点和终止点，确定双臂闭链系统在任务空间中的路径；根据双臂闭链系统在任务空间中的路径和闭链运动学模型，确定闭链系统的关节路径。3.如权利要求2所述的一种针对闭链奇异点回避的双臂协同运动规划方法，其特征在于，将起始点和终止点分别分解为位置向量和姿态向量；根据起止点的位置向量和终止点的位置向量，获得位置路径；根据起止点的姿态向量和终止点的姿态向量，确定姿态路径；将位置路径和姿态路径进行叠加，获得双臂闭链系统在任务空间中的路径。4.如权利要求2所述的一种针对闭链奇异点回避的双臂协同运动规划方法，其特征在于，闭链运动学模型包括通过d-h法为每个单臂分别构建的机械臂运动学方程和双臂协同搬运运动学约束。5.如权利要求1所述的一种针对闭链奇异点回避的双臂协同运动规划方法，其特征在于，还包括：对于不处于奇异区域的路径点，通过雅克比逆映射，确定双臂的关节速度。6.如权利要求1所述的一种针对闭链奇异点回避的双臂协同运动规划方法，其特征在于，根据主臂的关节速度和从臂关节速度更新方程，确定从臂的关节速度，其中，从臂关节速度更新方程为：其中，d1和d2为奇异区域判断系数，当系统位于奇异区域时，d1＝1、d2＝0；λ1和λ2为主从臂判断系数，当左臂为主臂时，λ1＝1、λ2＝0，反之，λ1＝0、λ2＝1；和分别为左右臂的关节速度，和分别为基于阻尼最小二乘法的左右臂的雅可比伪逆，和分别为根据主臂的近似解得到的从臂雅可比伪逆。7.如权利要求1所述的一种针对闭链奇异点回避的双臂协同运动规划方法，其特征在于，以机械臂末端实际速度与期望速度误差的范数最小为目标，以机械臂关节速度变化范数小于等于设定阈值为约束，采用序列二次规划的方法确定阻尼最小二乘的雅可比伪逆法中的阻尼因子。8.一种针对闭链奇异点回避的双臂协同运动规划系统，其特征在于，包括：起止点获取模块，用于获取被搬运物体的起始点和终止点；雅可比确定模块，用于根据被搬运物体的起始点和终止点，确定闭链系统的关节路径
及雅可比；规划路径奇异区域确定模块，用于根据闭链系统的雅可比确定闭链系统关节路径的奇异区域部分；闭链系统无奇异点的规划路径确定模块，用于对于奇异区域部分，确定闭链系统的主臂和从臂，基于阻尼最小二乘的雅可比伪逆法确定主臂的关节速度；根据主臂的关节速度，确定从臂的关节速度；对各臂的关节速度进行积分，获得各臂的关节角度变化值；通过各臂的关节角度变化值，替换关节路径中的奇异区域部分，获得闭链系统无奇异点的规划路径。9.一种电子设备，其特征在于，包括存储器和处理器以及存储在存储器上并在处理器上运行的计算机指令，所述计算机指令被处理器运行时，完成权利要求1-7任一项所述的一种针对闭链奇异点回避的双臂协同运动规划方法的步骤。10.一种计算机可读存储介质，其特征在于，用于存储计算机指令，所述计算机指令被处理器执行时，完成权利要求1-7任一项所述的一种针对闭链奇异点回避的双臂协同运动规划方法的步骤。

技术总结
本发明公开的一种针对闭链奇异点回避的双臂协同运动规划方法及系统，包括：获取被搬运物体的起始点和终止点；根据被搬运物体的起始点和终止点，确定闭链系统的关节路径及雅可比；根据闭链系统的雅可比确定闭链系统关节路径的奇异区域部分；对于奇异区域部分，确定闭链系统的主臂和从臂，基于阻尼最小二乘的雅可比伪逆法确定主臂的关节速度；根据主臂的关节速度，确定从臂的关节速度；对各臂的关节速度进行积分，获得各臂的关节角度变化值；通过各臂的关节角度变化值，替换关节路径中的奇异区域部分，获得闭链系统无奇异点的规划路径。实现了对闭链系统奇异点的回避，保证了闭链系统在奇异点处关节解和被搬运物体在任务空间中解的确定性。解的确定性。解的确定性。


技术研发人员：张勤 刘振宇 李岳炀
受保护的技术使用者：济南大学
技术研发日：2023.05.25
技术公布日：2023/8/6