一种机器人末端姿态控制方法、装置、设备及存储介质与流程

1.本发明涉及机器人技术领域，尤其涉及一种机器人末端姿态控制方法、装置、设备及存储介质。


背景技术：

2.在机器人控制过程中，为了保护机器人自身与协作人员的安全，通常需要约束机器人的运动范围。
3.该范围一般包括笛卡尔空间的位置约束和机器人的姿态约束。传统方法只是设置固定的笛卡尔空间的位置约束，并设置机器人姿态的最大姿态约束。如果该约束过大，虽然机器人能在运行中保持在笛卡尔空间的位置约束内，但导致机器人的操作性能下降；如果该约束过小，机器人其他部位就会有较大区域超出位置约束边界，达不到安全保护的目的。对机器人姿态的最大姿态约束的调整通常需要统计大量的机器人运行数据，并由技术人员根据运行数据对机器人姿态的最大姿态约束进行调整，不仅效率低，在运行数据有限的情况下，调整结果不够准确，难以保证机器人的操作性能和安全性需求。


技术实现要素：

4.本发明提供一种机器人末端姿态控制方法、装置、设备及存储介质，以在保证安全的前提下，提高机器人的操作性能。
5.第一方面，本发明实施例提供了一种机器人末端姿态控制方法，包括：
6.获取末端执行器处于姿态角度约束区间内的预设角度时的末端执行器位姿；
7.根据所述末端执行器位姿与机器人的预设位置约束空间的边界面的相对位置关系，确定所述末端执行器位姿与所述预设位置约束空间的边界面交点；
8.根据所述边界面交点、末端执行器近端位置和末端执行器长度，调整所述姿态角度约束区间，调整后的所述姿态角度约束区间用于机器人末端姿态的控制。
9.第二方面，本发明实施例提供了一种机器人末端姿态控制装置，包括：
10.末端位姿获取模块，用于获取末端执行器处于姿态角度约束区间内的预设角度时的末端执行器位姿；
11.边界面交点确定模块，用于根据所述末端执行器位姿与机器人的预设位置约束空间的边界面的相对位置关系，确定所述末端执行器位姿与所述预设位置约束空间的边界面交点；
12.角度约束调整模块，用于根据所述边界面交点、末端执行器近端位置和末端执行器长度，调整所述姿态角度约束区间，调整后的所述姿态角度约束区间用于机器人末端姿态的控制。
13.第三方面，本发明实施例提供了一种电子设备，所述事故数据分电子设备包括：
14.一个或多个处理器；
15.存储器，用于存储一个或多个程序；
16.当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如本发明任意实施例提供的机器人末端姿态控制方法。
17.第四方面，本发明实施例提供了一种包含计算机可执行指令的存储介质，所述计算机可执行指令在由计算机处理器执行时用于执行如本发明任意实施例提供的机器人末端姿态控制方法。
18.本发明实施例提供的一种机器人末端姿态控制方法、装置、设备及存储介质，通过末端执行器位姿和预设位置约束空间的边界面的位置关系，调整机器人末端的姿态角度约束区间，解决了机器人姿态约束调整效率低不准确的问题，实现了在保证安全的前提下，提高机器人的操作性能的效果。
附图说明
19.图1为本发明实施例一提供的一种机器人末端姿态控制方法的流程图；
20.图2为本发明实施例二提供的一种机器人末端姿态控制方法的流程图；
21.图3为本发明实施例三提供的一种机器人末端姿态控制装置的结构示意图；
22.图4为本发明实施例四提供的一种电子设备的结构示意图；
23.图5为本发明实施例中机器人的结构示意图；
24.图6为本发明实施例一中机器人末端执行器角度约束的示意图；
25.图7a为本发明实施例一中的一种末端执行器投影与约束空间投影的位置关系示意图；
26.图7b为本发明实施例一中的又一种末端执行器投影与约束空间投影的位置关系示意图；
27.图8为本发明实施例一中机器人末端执行器调整后的角度约束的示意图。
具体实施方式
28.下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。
29.实施例一
30.图1为本发明实施例一提供的一种机器人末端姿态控制方法的流程图，本实施例可适用于机器人末端姿态进行控制，如对多自由度机械臂末端执行器的姿态进行控制的情况，该方法可以由机器人末端姿态控制装置来执行，该装置可以由硬件和/或软件来实现，并一般可以集成在电子设备中，电子设备可以为机器人或机器人远端控制设备，该机器人末端姿态控制方法具体包括：
31.步骤110、获取末端执行器处于姿态角度约束区间内的预设角度时的末端执行器位姿；
32.其中，机器人可以是多自由度机械臂，且机械臂末端连接有末端执行器，例如图5中所示的机器人。而末端执行器可以是超声检测装置。姿态角度约束区间是对机械臂末端关节的最大姿态约束，是对转动角度的约束，可以预先设定，对于机器人整体会设定预设位置约束空间，该预设位置约束空间是预先设定的笛卡尔空间的位置约束。末端执行器在机
器人的运行时如果存在超出预设位置约束空间内的情况，一般是末端执行器的位姿处于靠近姿态角度约束区间的两个端点位置。所以，预设角度可以取靠近姿态角度约束区间的两个端点的角度范围，具体可以选取姿态角度约束区间的两个端点作为预设角度。考察末端执行器在姿态角度约束区间的两个端点是否存在超出预设位置约束空间内的情况。
33.步骤120、根据所述末端执行器位姿与机器人的预设位置约束空间的边界面的相对位置关系，确定所述末端执行器位姿与所述预设位置约束空间的边界面交点；
34.其中，判断末端执行器位姿与预设位置约束空间的边界面是否存在交点，该交点可以用末端执行器中心与预设位置约束空间的边界面的交点表示。预设位置约束空间可以设置为立方体，选取预设的笛卡尔坐标系设置预设位置约束空间的边界参数，例如可以选取机器人基座坐标系，立方体的长宽高各边与基座坐标系各坐标轴平行。可以通过判断末端执行器位姿下末端执行器远端位置和预设位置约束空间的边界面的相对位置，判断末端执行器位姿与预设位置约束空间的边界面交点。
35.步骤130、根据所述边界面交点、末端执行器近端位置和末端执行器长度，调整所述姿态角度约束区间，调整后的所述姿态角度约束区间用于机器人末端姿态的控制。
36.其中，有了末端执行器位姿与预设位置约束空间的边界面交点，结合末端执行器近端位置和末端执行器长度，可以推测出末端执行器转动角度的限定位置，从而调整姿态角度约束区间，调整后的姿态角度约束区间用于机器人末端姿态的控制，即可实现保证安全的前提下，提高机器人的操作性能。上述方法调整最大姿态约束，结果准确可靠，如果预设位置约束空间进行过调整，也可以快速地完成姿态角度约束区间的相应调整。
37.本实施例的技术方案，通过末端执行器位姿和预设位置约束空间的边界面的位置关系，调整机器人末端的姿态角度约束区间，解决了机器人姿态约束调整效率低不准确的问题，实现了在保证安全的前提下，提高机器人的操作性能的效果。
38.实施例二
39.图2为本发明实施例二提供的一种机器人末端姿态控制方法的流程图，本实施例在上述技术方案的基础上进一步细化，可以是所述获取末端执行器处于姿态角度约束区间内的预设角度时的末端执行器位姿，包括：获取机械臂末端旋转方向、所述末端执行器近端位置和所述末端执行器长度；其中，所述末端执行器的近端与机械臂的末端固定连接；根据所述机械臂末端位置、所述末端执行器近端位置和所述末端执行器长度，确定所述末端执行器处于所述姿态角度约束区间的区间端值时的所述末端执行器位姿。该方法具体包括：
40.步骤210、获取机械臂末端旋转方向、所述末端执行器近端位置和所述末端执行器长度；
41.其中，所述末端执行器的近端与机械臂的末端固定连接，末端执行器是由机械臂末端的转动带动。
42.步骤220、根据所述机械臂末端位置、所述末端执行器近端位置和所述末端执行器长度，确定所述末端执行器处于所述姿态角度约束区间的区间端值时的所述末端执行器位姿；
43.其中，机械臂末端旋转方向可以是末端姿态旋转的方向向量，可以表示为：v
z
＝[r
d
(0,2),r
d
(1,2),r
d
(2,2)]
t
，其中机器臂末端的期望姿态为3
×
3旋转矩阵以r
d
表示，设机械臂末端的当前位置坐标可以表示为p
c
(x
c
,y
c
,z
c
)，该p
c
位于位置预设位置约束空间内。设机
器人预设位置约束空间的范围可表示为:在x方向[x
min
,x
max
]，在y方向[y
min
,y
max
]，在z方向[z
min
,z
max
]，则机器人末端在基座坐标系{a}任意位置坐标(x,y,z)的约束条件为：x
min
<＝x<＝x
max；
y
min
<＝y<＝y
max；
z
min
<＝z<＝z
max
，即为机器人末端的预设位置约束空间。根据p
c
与v
z
，计算末端执行器远端中心p
distal
的位置向量：p
distal
＝p
c
l
tcp
*(v
z
/|v
z
|)。设末端执行器远端中心到机器人末端的长度为l
tcp
，机械臂末端位置p
c
与姿态偏转角θ如图6所示。末端执行器远端和近端都确定了，末端执行器的位姿也就可以确定下来。
[0044]
步骤230、根据所述末端执行器位姿与机器人的预设位置约束空间的边界面的相对位置关系，确定所述末端执行器位姿与所述预设位置约束空间的边界面交点；
[0045]
可选的，所述根据所述末端执行器位姿与机器人的预设位置约束空间的边界面的相对位置关系，确定所述末端执行器位姿与所述预设位置约束空间的边界面交点，包括：
[0046]
将机器人的所述预设位置约束空间和所述末端执行器位姿投影至预设平面内，得到所述预设平面内的约束空间投影和末端执行器投影；
[0047]
若所述末端执行器投影的远端超出所述约束空间投影的边界，根据所述预设平面内的边界线交点位置和所述末端执行器投影与所述末端执行器位姿的对应关系，确定所述末端执行器位姿与所述预设位置约束空间边界面的边界面交点。
[0048]
可选的，所述将机器人的预设位置约束空间和所述末端执行器位姿投影至预设平面内，得到所述预设平面内的约束空间投影和末端执行器投影，包括：
[0049]
将所述预设位置约束空间和所述末端执行器位姿投影至基座坐标系的xoy平面，得到xoy平面内的所述约束空间投影和所述末端执行器投影；
[0050]
所述若所述末端执行器投影的远端超出所述约束空间投影的边界，根据所述预设平面内的边界线交点位置和所述末端执行器投影与所述末端执行器位姿的对应关系，确定所述末端执行器位姿与所述预设位置约束空间边界面的边界面交点，包括：
[0051]
判断所述末端执行器投影的远端是否处于所述约束空间投影的范围内；其中，所述约束空间投影为矩形且矩形边与所述基座坐标系的x轴和y轴分别平行；
[0052]
若所述末端执行器投影的远端超出所述约束空间投影的范围，根据所述末端执行器投影的远端坐标、所述约束空间投影的边界线坐标、所述末端执行器位姿的远端坐标和所述末端执行器位姿的近端坐标，确定所述边界面交点。
[0053]
其中，将p
distal
投影到基座坐标系的xoy平面内，得到p
dxy
(x
dxy
,y
dxy
)，而以z
dxy
表示p
distal
的z轴坐标。
[0054]
判断p
dxy
与预设位置约束空间在xoy平面内的约束空间投影的关系：
[0055]
(1)如果p
dxy
在约束空间投影内，则不更新机器人末端最大姿态约束角度，即机器人末端可以到达期望姿态r
d
。
[0056]
(2)如果p
dxy
未在约束空间投影内，则需要确定p
c
与p
dxy
两点所在的直线与预设位置约束空间边界的交点p
in
(x
in
,y
in
,z
in
)。
[0057]
p
dxy
未在约束空间投影内，包括x
dxy
>x
max
,x
dxy
<x
min
,y
dxy
>y
max
,y
dxy
<y
min
4种情况中的至少一种。下面以x
dxy
>x
max
为例，分2种情况介绍计算p
c
与p
dxy
两点所在直线与预设位置约束空间边界交点p
in
的方法。
[0058]
第一种情况(如图7a)：则存在x
in
＝x
max
,根据下式，可以计算出p
in
的另外两个坐标y
in
,z
in
。
[0059]
(x
in
–
x
c
)/(x
dxy
‑
x
c
)＝(y
in
–
y
c
)/(y
dxy
‑
y
c
)＝(z
in
–
z
c
)/(z
dxy
‑
z
c
)
[0060]
如果计算的y
in
>y
max
或者y
in
<y
min
，则如图7b中所示的情况。此时，令y
in
＝y
max
或者y
in
＝y
min
，然后按照下式，计算出新的x
in
和z
in
。
[0061]
(y
in
–
y
c
)/(y
dxy
‑
y
c
)＝(x
in
–
x
c
)/(x
dxy
‑
x
c
)＝(z
in
–
z
c
)/(z
dxy
‑
z
c
)
[0062]
至此，可以计算出p
c
与p
dxy
两点所在的直线与位置约束空间边界的交点p
in
的坐标。
[0063]
步骤240、根据所述边界面交点、末端执行器近端位置和末端执行器长度，调整所述姿态角度约束区间，调整后的所述姿态角度约束区间用于机器人末端姿态的控制。
[0064]
可选的，所述根据所述边界面交点、末端执行器近端位置和末端执行器长度，调整所述姿态角度约束区间，调整后的所述姿态角度约束区间用于机器人末端姿态的控制，包括：
[0065]
根据所述边界面交点和所述末端执行器近端位置，确定所述边界面交点和所述末端执行器近端位置之间的距离；
[0066]
根据所述距离和所述末端执行器长度，确定对应的末端姿态修正角度；
[0067]
将所述末端姿态修正角度作为所述姿态角度约束区间的端值，得到调整后的所述姿态角度约束区间。
[0068]
其中，根据p
in
、p
c
和l
tcp
，可以计算得到新的机器人末端姿态偏转角θ’为：
[0069]
d＝||p
in
—p
c
||
[0070]
θ’＝asin(d/l
tcp
)
[0071]
根据新的机器人末端姿态偏转角θ’可以确定调整后的姿态角度约束区间，并基于调整后的姿态角度约束区间控制机器人末端姿态。新的机器人末端姿态偏转角θ’和原始的姿态偏转角θ对比如图8所示，p’distal
为调整后的姿态角度约束区间下末端执行器最大偏转角时的末端执行器远端位置。可以的理解是，p
in
表示的是末端执行器中心线与预设位置约束空间的边界面交点，在计算得到的新的机器人末端姿态偏转角θ’之后，还可以进一步根据末端执行器远端的直径，对θ’做出修正。
[0072]
本实施例的技术方案，通过机器人末端、边界面交点和末端执行器长度，可以确定末端执行器实际上允许的最大偏转角，从而调整姿态角度约束区间。
[0073]
实施例三
[0074]
图3为本发明实施例三提供的一种机器人末端姿态控制装置的结构示意图，如图3所示，该机器人末端姿态控制装置，包括：末端位姿获取模块310、边界面交点确定模块320和角度约束调整模块330，其中：
[0075]
末端位姿获取模块310，用于获取末端执行器处于姿态角度约束区间内的预设角度时的末端执行器位姿；
[0076]
边界面交点确定模块320，用于根据所述末端执行器位姿与机器人的预设位置约束空间的边界面的相对位置关系，确定所述末端执行器位姿与所述预设位置约束空间的边界面交点；
[0077]
角度约束调整模块330，用于根据所述边界面交点、末端执行器近端位置和末端执行器长度，调整所述姿态角度约束区间，调整后的所述姿态角度约束区间用于机器人末端姿态的控制。
[0078]
本实施例提供的机器人末端姿态控制装置，通过末端执行器位姿和预设位置约束
空间的边界面的位置关系，调整机器人末端的姿态角度约束区间，解决了机器人姿态约束调整效率低不准确的问题，实现了在保证安全的前提下，提高机器人的操作性能的效果。
[0079]
可选的，所述末端位姿获取模块，包括：
[0080]
末端数据获取单元，用于获取机械臂末端旋转方向、所述末端执行器近端位置和所述末端执行器长度；其中，所述末端执行器的近端与机械臂的末端固定连接；
[0081]
末端位姿确定单元，用于根据所述机械臂末端位置、所述末端执行器近端位置和所述末端执行器长度，确定所述末端执行器处于所述姿态角度约束区间的区间端值时的所述末端执行器位姿。
[0082]
可选的，所述边界面交点确定模块，包括：
[0083]
平面投影单元，用于将机器人的所述预设位置约束空间和所述末端执行器位姿投影至预设平面内，得到所述预设平面内的约束空间投影和末端执行器投影；
[0084]
边界面交点确定单元，用于若所述末端执行器投影的远端超出所述约束空间投影的边界，根据所述预设平面内的边界线交点位置和所述末端执行器投影与所述末端执行器位姿的对应关系，确定所述末端执行器位姿与所述预设位置约束空间边界面的边界面交点。
[0085]
可选的，所述平面投影单元，具体用于：
[0086]
将所述预设位置约束空间和所述末端执行器位姿投影至基座坐标系的xoy平面，得到xoy平面内的所述约束空间投影和所述末端执行器投影；
[0087]
所述边界面交点确定单元，具体用于：
[0088]
判断所述末端执行器投影的远端是否处于所述约束空间投影的范围内；其中，所述约束空间投影为矩形且矩形边与所述基座坐标系的x轴和y轴分别平行；
[0089]
若所述末端执行器投影的远端超出所述约束空间投影的范围，根据所述末端执行器投影的远端坐标、所述约束空间投影的边界线坐标、所述末端执行器位姿的远端坐标和所述末端执行器位姿的近端坐标，确定所述边界面交点。
[0090]
可选的，所述角度约束调整模块，具体用于：
[0091]
根据所述边界面交点和所述末端执行器近端位置，确定所述边界面交点和所述末端执行器近端位置之间的距离；
[0092]
根据所述距离和所述末端执行器长度，确定对应的末端姿态修正角度；
[0093]
将所述末端姿态修正角度作为所述姿态角度约束区间的端值，得到调整后的所述姿态角度约束区间。
[0094]
本发明实施例所提供的机器人末端姿态控制装置可执行本发明任意实施例所提供的机器人末端姿态控制方法，具备执行方法相应的功能模块和有益效果。
[0095]
实施例四
[0096]
图4为本发明实施例四提供的一种电子设备的结构示意图，如图4所示，该电子设备包括处理器410、存储器420、输入装置430和输出装置440；电子设备中处理器510的数量可以是一个或多个，图4中以一个处理器410为例；电子设备中的处理器410、存储器420、输入装置430和输出装置440可以通过总线或其他方式连接，图5中以通过总线连接为例。
[0097]
存储器420作为一种计算机可读存储介质，可用于存储软件程序、计算机可执行程序以及模块，如本发明实施例中的机器人末端姿态控制方法对应的程序指令/模块(例如，
机器人末端姿态控制装置中的末端位姿获取模块310、边界面交点确定模块320和角度约束调整模块330)。处理器410通过运行存储在存储器420中的软件程序、指令以及模块，从而执行电子设备的各种功能应用以及数据处理，即实现上述的机器人末端姿态控制方法。
[0098]
存储器420可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序；存储数据区可存储根据终端的使用所创建的数据等。此外，存储器420可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实例中，存储器420可进一步包括相对于处理器410远程设置的存储器，这些远程存储器可以通过网络连接至事故数据分析设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
[0099]
输入装置430可用于接收输入的数字或字符信息，以及产生与事故数据分析设备的用户设置以及功能控制有关的键信号输入。输出装置440可包括显示屏等显示设备。
[0100]
实施例五
[0101]
本发明实施例五还提供一种包含计算机可执行指令的存储介质，所述计算机可执行指令在由计算机处理器执行时用于执行一种机器人末端姿态控制方法，包括：
[0102]
获取末端执行器处于姿态角度约束区间内的预设角度时的末端执行器位姿；
[0103]
根据所述末端执行器位姿与机器人的预设位置约束空间的边界面的相对位置关系，确定所述末端执行器位姿与所述预设位置约束空间的边界面交点；
[0104]
根据所述边界面交点、末端执行器近端位置和末端执行器长度，调整所述姿态角度约束区间，调整后的所述姿态角度约束区间用于机器人末端姿态的控制。
[0105]
当然，本发明实施例所提供的一种包含计算机可执行指令的存储介质，其计算机可执行指令不限于如上所述的方法操作，还可以执行本发明任意实施例所提供的机器人末端姿态控制方法中的相关操作。
[0106]
通过以上关于实施方式的描述，所属领域的技术人员可以清楚地了解到，本发明可借助软件及必需的通用硬件来实现，当然也可以通过硬件实现，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如计算机的软盘、只读存储器(read
‑
only memory,rom)、随机存取存储器(random access memory,ram)、闪存(flash)、硬盘或光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。
[0107]
值得注意的是，上述机器人末端姿态控制装置的实施例中，所包括的各个单元和模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能单元的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。
[0108]
虽然，上文中已经用一般性说明、具体实施方式及试验，对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。