一种多机械臂协同的快速自碰撞检测方法与流程

1.本发明涉及碰撞检测的技术领域，尤其涉及一种多机械臂协同的快速自碰撞检测方法。


背景技术：

2.随着机器人技术的发展，机械臂变得越来越灵活，为了保证多自由度机械臂的自身安全，这就需要机械臂能有快速自碰撞检测的功能。许多工程机械上装有多条机械臂，如多臂凿岩台车、拱架安装台车，这些设备既需要考虑单条臂内部各关节间的碰撞检测，又需要考虑多条工作臂之间的自碰撞检测，还需要考虑工作臂与吊篮臂之间的碰撞检测。
3.基于力传感器或电流传感器的碰撞检测是工业机械臂常用的碰撞检测方法，但这种方法需要安装高精度传感器，增加了机械臂的成本。申请号为202010323632.7的中国发明专利公开了一种空间机械臂的快速自碰撞检测方法，采用球体包围盒方法空间机械臂进行球体包围盒的模型包络，能够保证在有限资源下针对空间机器人嵌入式系统能够实时有效检测自身碰撞；采用两层碰撞检测方案，保证快速自碰撞检测方法有效。但是，机械臂的构件大多为条状，一个运动构件需要用多个球体进行包络，而且相邻两个球体间有较大的重叠部分，这导致碰撞体数目成倍增加，增加了内存成本和检测次数。


技术实现要素：

4.针对现有多条机械臂间的自碰撞检测需求，避免额外增加传感器增加成本的技术问题，本发明提出一种多机械臂协同的快速自碰撞检测方法，基于关节位置，避免了额外增加传感器，对工况没有任何限制，成本低，且计算操作简单，执行效率高。
5.为了达到上述目的，本发明的技术方案是这样实现的：一种多机械臂协同的快速自碰撞检测方法，其步骤如下：
6.步骤一：将多条机械臂各运动关节以及与之固连的结构件放大后等效为最大包络的长方体，并分别建立关节坐标系和长方体坐标系；
7.步骤二：根据机械臂运动关节的位置及关节坐标系之间、关节坐标系与长方体坐标系之间的关系，计算各长方体的位姿；
8.步骤三：根据步骤二中长方体的位姿，确定长方体在另一个长方体坐标系的顶点，根据顶点坐标和两个长方体在不同方向的投影是否重叠判断两个没有直接相连的长方体是否发生碰撞；
9.步骤四：重复步骤三直到某两个长方体发生碰撞，否则多机械臂自身不碰撞。
10.优选地，所述关节坐标系为第i条机械臂的第j个关节的关节坐标系(i,j)；与关节坐标系(i,j)相对应的长方体坐标系为长方体坐标系(i,j
′
)，长方体坐标系(i,j
′
)为第i条机械臂的第j个关节的最大包络的长方体的中心点上的坐标系。通过关系坐标系和长方体坐标系进行分别表示，实现它们之间的关联。
11.优选地，所述长方体坐标系(i,j
′
)的坐标轴方向与最大包络的长方体的棱线平
行。方便坐标的确定。
12.优选地，所述长方体坐标系(i,j
′
)在关节坐标系(i,j)中的位姿用齐次变换矩阵表示，最大包络的长方体沿长方体坐标系(i,j
′
)的三条轴线方向的长度表示为
i,j
′
l
x
、
i,j
′
l
y
和
i,j
′
l
z
，其中，
i,j
′
l
x
为沿x轴方向的长度，
i,j
′
l
y
为沿y轴方向的长度，
i,j
′
l
z
为沿z轴方向的长度。利用齐次变换矩阵方便确定两个坐标系的关系。
13.优选地，所述步骤二中计算各长方体的位姿的方法是：基于第i条机械臂上当前各关节的位置，计算第i条机械臂的第j个关节的关节关节坐标系(i,j)在上一个关节坐标系(i,j
‑
1)中的齐次变换矩阵为设第i条机械臂的基座在世界坐标系w中的齐次变换矩阵为则第i条机械臂的第j个关节的最大包络的长方体的长方体坐标系在世界坐标系w中的齐次变换矩阵为利用与世界坐标系之间的齐次变换矩阵确定长方体的位姿，从而可以方便地确定长方体顶点的坐标。
14.优选地，所述根据顶点坐标判断两个没有直接相连的长方体是否发生碰撞的方法为：
15.步骤s3.1：设长方体m的中心点上的坐标系为长方体坐标系a，长方体n的中心点上的坐标系为长方体坐标系b；则根据长方体m沿三条轴线方向的长度计算得到长方体m的8个顶点在长方体坐标系a中的第一坐标；
16.步骤s3.2：根据长方体坐标系a与长方体坐标系b之间的齐次变换矩阵和第一坐标，计算长方体m的8个顶点在长方体坐标系b中的第二坐标；
17.步骤s3.3：若第二坐标中的三坐标的任一最大值小于相应的第一坐标或第二坐标的三坐标的任一最小值大于相应的第一坐标，则长方体m与长方体n不发生碰撞；
18.步骤s3.4：交换长方体m和长方体n，重复执行步骤s3.31
‑
s3.3。通过计算长方体的坐标判断长方体是否碰撞，计算简单。
19.优选地，所述步骤三中确定长方体在另一个长方体坐标系的顶点即计算步骤s3.2中第二坐标的方法为：其中，
b
p
k
为长方体m的第k个顶点在坐标系b中的第二坐标，
a
p
k
为长方体m的第k个顶点在长方体坐标系a中的第一坐标，k＝1,2,...8；为长方体坐标系a在长方体坐标系b中的齐次变换矩阵，且其中，为长方体坐标系b在世界坐标系w中的齐次变换矩阵，齐次变换矩阵为齐次变换矩阵的逆，为长方体坐标系a在世界坐标系w中的齐次变换矩阵，r
11
‑
r
33
为与旋转变换相关的元素，p
x
、p
y
、p
z
为与平移变换相关的元素。
20.优选地，所述步骤s3.3中判断长方体m与长方体n不发生碰撞的方法为：满足下列任一种情况：
21.其
中，三坐标的x
max
、y
max
、z
max
分别为第二坐标
b
p
k
的x轴、y轴、z轴坐标的最大值，x
min
、y
min
、z
min
分别为第二坐标
b
p
k
的x轴、y轴、z轴坐标的最小值；分别为第一坐标
a
p
k
的x轴、y轴、z轴坐标的最小值，分别为第一坐标
a
p
k
的x轴、y轴、z轴坐标的最大值。一个长方体的顶点坐标不在另一长方体顶点坐标范围内在，则两个长方体不想碰撞。
22.优选地，所述根据两个长方体在不同方向的投影是否重叠判断两个没有直接相连的长方体是否发生碰撞的方法是：选择两个长方体坐标之间的齐次变换矩阵中9组方向的向量，判断两个长方体在9组方向的向量上的投影均不重叠，则两个长方体不发生碰撞，否者两个长方体碰撞。
23.优选地，所述9组方向的向量分别为：[0 r
31
ꢀ‑
r
21
]
t
、[0 r
32
ꢀ‑
r
22
]
t
、[0 r
33
ꢀ‑
r
23
]
t
、[
‑
r
31 0 r
11
]
t
、[
‑
r
32 0 r
12
]
t
、[
‑
r
33 0 r
13
]
t
、[r
21
ꢀ‑
r
11 0]
t
、[r
22
ꢀ‑
r
12 0]
t
、[r
23
ꢀ‑
r
13 0]
t
，其中t是向量的转置。
[0024]
与现有技术相比，本发明的有益效果：将多条机械臂各运动构件等效为最大包络的长方体，根据实时获取的各关节变量值计算得到用齐次变换矩阵表示的各个长方体包络在世界坐标系中的位姿，然后依次判断两个不相邻长方体是否发生碰撞，长方体数目与运动构件数目相同，不需要额外增加碰撞体的数目；对于两个机械臂中运动构件的碰撞分三步进行判断，首先在坐标系b中对长方体m的8个顶点进行判断，然后对两个长方体在9组方向的投影是否重叠进行判断，最后在坐标系a中对长方体n的8个顶点进行判断，直接使用了顶点的坐标与另一个长方体的棱长进行比较判断，减少了6个方向的计算复杂度。本发明基于关节位置，避免了额外增加传感器，对工况没有任何限制，成本低，且计算简单，执行效率高。本发明用于多机械臂协同的自碰撞检测，既适用于主动检测，又适用于碰撞预警。
附图说明
[0025]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0026]
图1为本发明的流程示意图。
[0027]
图2为本发明长方体包络的简化模型示意图。
[0028]
图3为本发明多条机械臂碰撞体的简化模型示意图。
[0029]
图4为本发明两个长方体间的碰撞检测模型示意图。
具体实施方式
[0030]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0031]
如图1所示，一种多机械臂协同的快速自碰撞检测方法，其步骤如下：
[0032]
步骤一：将多条机械臂各运动关节以及与之固连的结构件放大后等效为最大包络的长方体，并分别建立关节坐标系和长方体坐标系。
[0033]
将多条机械臂各运动关节以及与之固连的结构件经适当放大后等效为最大包络长方体，依据机械臂结构图纸获取最大包络长方体的尺寸信息，并根据机械臂的关节定位精度和安全等级确定放大的程度。
[0034]
所述关节坐标系为第i条机械臂的第j个关节的关节坐标系(i,j)，关节坐标系的原点位于关节转/移动轴中心线上；与关节坐标系(i,j)相对应的长方体坐标系为长方体坐标系(i,j
′
)，如图2所示，所述长方体坐标系(i,j
′
)的坐标轴方向与最大包络的长方体的棱线平行，原点为长方体的中心点，该坐标系可以减少坐标变换的计算量。
[0035]
所述长方体坐标系(i,j
′
)在关节坐标系(i,j)中的位姿用齐次变换矩阵表示，最大包络的长方体沿长方体坐标系(i,j
′
)的三条轴线方向的长度表示为
i,j
′
l
x
、
i,j
′
l
y
和
i,j
′
l
z
，其中，
i,j
′
l
x
为沿x轴方向的长度，
i,j
′
l
y
为沿y轴方向的长度，
i,j
′
l
z
为沿z轴方向的长度，各长度与长方体相应方向的棱长相等，与运动构件结构尺寸以及关节坐标系(i,j)和长方体坐标系(i,j
′
)的建立方式有关，在机械臂运动过程中不会发生变化。
[0036]
具体实例中如图3所示，共有三条机械臂，每条机械臂有7个关节。第一条机械臂上的关节坐标系包括1号臂基座坐标系(1,0)和用坐标系(1,1)
‑
坐标系(1,7)表示的7个关节坐标系，第二条机械臂上的关节坐标系包括2号臂基座坐标系(2,0)和用坐标系(2,1)
‑
坐标系(2,7)代表的7个关节坐标系，第三条机械臂上的关节坐标系包括3号臂坐标系(3,0)和用坐标系(3,1)
‑
坐标系(7,7)代表的7个关节坐标系。
[0037]
步骤二：根据机械臂运动关节的位置及关节坐标系之间、关节坐标系与长方体坐标系之间的关系，计算各长方体的位姿。
[0038]
所述步骤二中计算各长方体的位姿的方法是：基于第i条机械臂上当前各关节的位置，计算第i条机械臂的第j个关节的关节坐标系(i,j)在上一个关节坐标系(i,j
‑
1)中的齐次变换矩阵为设第i条机械臂的基座(固定不变)在世界坐标系w中的齐次变换矩阵为则第i条机械臂的第j个关节的最大包络的长方体的长方体坐标系在世界坐标系w中的齐次变换矩阵为为关节坐标系(i,1)在基座的关节坐标系(i,0)中的齐次变换矩阵。
[0039]
步骤三：根据步骤二中长方体的位姿，确定长方体在另一个长方体坐标系的顶点，根据顶点坐标和两个长方体在不同方向的投影是否重叠判断两个没有直接相连的长方体是否发生碰撞。
[0040]
所述根据顶点坐标判断两个没有直接相连的长方体是否发生碰撞的方法为：
[0041]
步骤s3.1：设长方体m的中心点上的坐标系为长方体坐标系a，长方体n的中心点上的坐标系为长方体坐标系b；则根据长方体m沿三条轴线方向的长度计算得到长方体m的8个顶点在长方体坐标系a中的第一坐标。
[0042]
长方体m沿坐标系a三条轴线方向的尺寸表示为
a
l
x
、
a
l
y
和
a
l
z
，长方体n沿坐标系b三
条轴线方向的尺寸表示为
b
l
x
、
b
l
y
和
b
l
z
；则计算长方体m的8个顶点在长方体坐标系a中的第一坐标分别为：一坐标分别为：
[0043][0044]
步骤s3.2：根据长方体坐标系a与长方体坐标系b之间的齐次变换矩阵和第一坐标，计算长方体m的8个顶点在长方体坐标系b中的第二坐标。
[0045]
所述步骤三中确定长方体在另一个长方体坐标系的顶点即计算步骤s3.2中第二坐标的方法为：其中，
b
p
k
为长方体m的第k个顶点在坐标系b中的第二坐标，
a
p
k
为长方体m的第k个顶点在长方体坐标系a中的第一坐标，k＝1,2,...8；为长方体坐标系a在长方体坐标系b中的齐次变换矩阵，且其中，为长方体坐标系b在世界坐标系w中的齐次变换矩阵，齐次变换矩阵为齐次变换矩阵的逆，为长方体坐标系a在世界坐标系w中的齐次变换矩阵，r
11
‑
r
33
为与旋转相关的9个元素，p
x
、p
y
、p
z
为与平移相关的3个元素。
[0046]
步骤s3.3：若第二坐标中的三坐标的任一最大值小于相应的第一坐标或第二坐标的三坐标的任一最小值大于相应的第一坐标，则长方体m与长方体n不发生碰撞。
[0047]
计算8个顶点
b
p
k
的三坐标的最大值x
max
、y
max
、z
max
和最小值x
min
、y
min
、z
min
。
[0048]
[1]所述步骤s3.3中判断长方体m与长方体n不发生碰撞的方法为：满足下列任一种情况：
[0049]
情况一：
[0050]
情况二：
[0051]
情况三：
[0052]
情况四：
[0053]
情况五：
[0054]
情况六：其中，三坐标的x
max
、y
max
、z
max
分别为第二坐标
b
p
k
的x轴、y轴、z轴坐标的最大值，x
min
、y
min
、z
min
分别为第二坐标
b
p
k
的x轴、y轴、z轴坐标的最小值；的x轴、y轴、z轴坐标的最小值；分别为第一坐标
a
p
k
的x轴、y轴、z轴坐标的最小值，分别为第一坐标
a
p
k
的x轴、y轴、z轴坐标的最大值。
[0055]
步骤s3.4：交换长方体m和长方体n的编号，重复执行步骤s3.31
‑
s3.3判断长方体m与长方体n是否不发生碰撞。
[0056]
所述根据两个长方体在不同方向的投影是否重叠判断两个没有直接相连的长方体是否发生碰撞的方法是：选择两个长方体坐标之间的齐次变换矩阵中9组方向的向量，判断两个长方体在9组方向的向量上的投影均不重叠，则两个长方体不发生碰撞，否者两个长方体碰撞。
[0057]
所述9组方向的向量分别为：[0 r
31
ꢀ‑
r
21
]
t
、[0 r
32
ꢀ‑
r
22
]
t
、[0 r
33
ꢀ‑
r
23
]
t
、[
‑
r
31 0 r
11
]
t
、[
‑
r
32 0 r
12
]
t
、[
‑
r
33 0 r
13
]
t
、[r
21
ꢀ‑
r
11 0]
t
、[r
22
ꢀ‑
r
12 0]
t
、[r
23
ꢀ‑
r
13 0]
t
，其中，t是向量的转置，这些向量可以直接从步骤s3.2计算的齐次变换矩阵中直接获取而不需要额外的计算。
[0058]
步骤四：重复步骤三直到某两个长方体发生碰撞，否则多机械臂自身不碰撞。
[0059]
重复步骤三直到某两个长方体发生碰撞，这时表示多机械臂自身发生了碰撞；若检查了所有长方体对，仍未发现碰撞，则多机械臂自身不碰撞。
[0060]
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。