一种基于图注意力网络的机器人运动规划方法及相关装置

1.本发明属于路径规划技术领域，涉及一种基于图注意力网络的机器人运动规划方法及相关装置。


背景技术：

2.针对机器人运动规划问题提出和使用的算法主要可以分为基于几何构造的空间搜索法、基于轨迹优化的运动规划算法及基于采样的运动规划算法等三类。在处理高维机器人运动规划问题时，基于采样的运动规划较为高效。其基本思想是确定一个能充分表示自由空间连通性的有限避障位形集合，用于解决含有障碍物环境中运动规划问题的路径图。随机采样规划中目前最常用的两种算法是先学习再查询的概率路线图(probabilistic roadmap method,prm)和一边学习一边查询的快速扩展随机树(rapidly-exploring random tree,rrt)。刘华军等在移动机器人运动规划研究综述中对prm和rrt进行比较，相比与prm，rrt算法充分考虑了机器人客观存在的约束(如非完整约束、运动动力学约束等)，得到的轨迹也更为合理。rrt虽然可以找到路径解，但解的质量以概1收敛至次优，即永远无法找到最优路径。针对这一问题，sertac等人提出rrt*，在rrt的基础上增加重选父节点和重新布线环节。如果存在最优解，随着迭代次数趋于无穷，rrt*可以保证找到最优解的概率趋于1。但在均匀全局采样下，rrt*通过渐近地寻找从初始状态到问题域中每个状态的最优路径来得到规划问题的最优解，与其单查询性质(注重找到可行路径，即快速性)不一致且效率低。为了在找到优解的同时保持算法效率，一个通用的方法是先找到一个可行的初始解，然后在此基础上进行优化。初始解的质量与求解过程中的采样点分布密切相关，使得这类运动规划对采样分布敏感，难以保证初始解的质量和算法收敛时间。
3.一些算法通过不同的启发式方法来突破这一局限性，如路径偏置，目标偏置等。akgun等人提出双向rrt*，一旦找到初始解，算法将以用户定义的迭代次数对当前解进行迭代优化，优化过程中首先在当前路径解上随机选取状态，然后在其voronoi域内进行采样，优化过程中采样点向当前解路径偏置，试图在当前解周围找到更优解。nasir等人提出rrt*-smart，找到初始解后首先对路径进行光滑处理，将路径中的状态数目减至最小，然后对当前解进行路径偏置优化。gammell等人提出informed rrt*，首先基于rrt*得到初始解，根据初始解路径代价定义了一个椭球区域作为后续优化过程中待采样区域，在该区域内进行再次采样以寻求更优路径，根据更优路径进一步调整采样空间，自适应采样构型空间以进行最优路径规划，通过偏置采样区域提高了规划算法的速度，但初始解求解过程并未在rrt*基础上进行改进。qureshi等人提出p-rrt*和apgd-rrt*，结合人工势场法(artificial potential field,apf)和rrt*，将随机采样点导向目标，降低随机树搜索和扩展的随机性提高了rrt*的速度。梁中一、徐娜在基本rrt*中引入目标偏置思想，在新节点生成时以一定概率设置目标构型为随机采样点，即，加速随机树向目标方向拓展。
4.但上述规划方法中，目标偏置不等同于最优路径偏置；路径偏置大都只对优化过程中的采样区域进行约束，通过限制路径优化过程中的采样区域加速了算法收敛，其初始
路径求解本身仍未在rrt*的基础上得到改进。且这些方法中所定义的启发式并不通用，只在某些环境中有效，尽管在计算速度上有了一些改进，但是这些启发式方法在提升初始解的质量方面并不完善，仍需进一步研究。


技术实现要素：

5.本发明的目的在于解决现有技术中的问题，提供一种基于图注意力网络的机器人运动规划方法及相关装置，本发明基于gat构建神经网络模型对给定规划条件下的路径点分布进行预测，为informed rrt*的采样过程提供参考，降低了采样过程中的随机性，提高了初始解的质量，加速了算法收敛。
6.为了实现上述目的，本发明采用以下技术方案予以实现：
7.第一方面，本发明提供一种基于图注意力网络的机器人运动规划方法，包括以下步骤：
8.初始化机器人运动状态，并求解初始解；
9.根据求解初始解的情况选择采样方式，通过采样方式得到关节构型集合；
10.从关节构型集合中随机选取关节构型作为采样点；
11.通过函数nearest得到当前树上已有节点中距离采样点s
rand
最近的节点θ
nearest
，并通过正运动学得到各关节和末端执行器的位置x
nearest
；
12.根据各关节和末端执行器的位置x
nearest
，通过函数steer进行节点扩展得到关节构型θ
new
，结合正运动学得到机器人各关节和末端执行器的位置x
new
；
13.对各关节和末端执行器的位置x
new
进行碰撞检测，直至到达最大迭代次数，输出最优路径。
14.第二方面，本发明提供一种基于图注意力网络的机器人运动规划系统，包括：
15.初始化模块，用于初始化机器人运动状态，并求解初始解；
16.采样方式选择模块，用于根据求解初始解的情况选择采样方式，通过采样方式得到关节构型集合；
17.采样点选取模块，用于从关节构型集合中随机选取关节构型作为采样点；
18.第一计算模块，用于通过函数nearest得到当前树上已有节点中距离采样点s
rand
最近的节点θ
nearest
，并通过正运动学得到各关节和末端执行器的位置x
nearest
；
19.第二计算模块，用于根据各关节和末端执行器的位置x
nearest
，通过函数steer进行节点扩展得到关节构型θ
new
，结合正运动学得到机器人各关节和末端执行器的位置x
new
；
20.最优路径输出模块，用于对各关节和末端执行器的位置x
new
进行碰撞检测，直至到达最大迭代次数，输出最优路径。
21.第三方面，本发明提供一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述方法的步骤。
22.第四方面，本发明提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上述方法的步骤。
23.与现有技术相比，本发明具有以下有益效果：
24.本发明基于gat所构建的神经网络模型可以很好的学习图型结构数据的特征，对
不同环境(障碍物分布、起始点和目标点)下的路径点分布进行预测，在对具体规划问题进行求解时，该算法以一定概率进行非均匀随机采样，即以一定概率选取预测路径点集中的点作为采样点，降低了采样点生成过程中的随机性，提高了采样点的质量，因此得到了高质量的初始解，加速了算法优化过程，实现了快速运动规划。
附图说明
25.为了更清楚的说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
26.图1为本发明方法的流程图。
27.图2为本发明系统的原理图。
28.图3为本发明中基于gat的神经网络模型结构示意图
29.图4为本发明运动规划方法(gat-informedrrt*)整体流程示意图；
30.图5为本发明数据集可视化示意图；
31.图6为本发明中informedrrt*和gat-informedrrt*在不同场景下的初始解路径；
32.图7为本发明中不同场景下算法收敛速率图。
具体实施方式
33.为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。
34.因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
35.应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。
36.在本发明实施例的描述中，需要说明的是，若出现术语“上”、“下”、“水平”、“内”等指示的方位或位置关系为基于附图所示的方位或位置关系，或者是该发明产品使用时惯常摆放的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
37.此外，若出现术语“水平”，并不表示要求部件绝对水平，而是可以稍微倾斜。如“水平”仅仅是指其方向相对“竖直”而言更加水平，并不是表示该结构一定要完全水平，而是可以稍微倾斜。
38.在本发明实施例的描述中，还需要说明的是，除非另有明确的规定和限定，若出现术语“设置”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆
卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
39.下面结合附图对本发明做进一步详细描述：
40.参见图1，本发明实施例公开了一种基于图注意力网络的机器人运动规划方法，包括以下步骤：
41.s1初始化机器人运动状态，并求解初始解；所述机器人运动状态包括基座的位置、姿态、关节角的初始构型、末端执行器的初始位置x0和目标位置x
goal
。
42.s2根据求解初始解的情况选择采样方式，通过采样方式得到关节构型集合；
43.s3从关节构型集合中随机选取关节构型作为采样点；
44.s4通过函数nearest得到当前树上已有节点中距离采样点s
rand
最近的节点θ
nearest
，并通过正运动学得到各关节和末端执行器的位置x
nearest
；
45.s5根据各关节和末端执行器的位置x
nearest
，通过函数steer进行节点扩展得到关节构型θ
new
，结合正运动学得到机器人各关节和末端执行器的位置x
new
；
46.s6对各关节和末端执行器的位置x
new
进行碰撞检测，直至到达最大迭代次数，输出最优路径。
47.在本发明一个可行的实施例中，所述求解初始解，包括：
48.通过机器人逆运动学求解出目标构型θ
goal
；
49.针对目标构型θ
goal
和给定环境信息，利用训练好的神经网络模型对可行路径中的关节构型进行预测，得到预测关节构型集合θ
pred
。
50.在本发明一个可行的实施例中，所述根据求解初始解的情况选择采样方式，包括：
51.判断是否已经得到初始解来选择不同的采样方式；
52.a.若已经得到初始解则开始进行路径优化：
53.计算目前已有解的代价，比较得到最优的代价c
best
；利用informedsample模块进行采样，具体如下：
54.a1)基于代价c
best
在笛卡尔空间中定义一个启发式采样区域，该区域中的点具有提升当前解质量的可能；
55.a2)为充分利用笛卡尔空间采样区域的优势和关节空间采样的便利，通过随机数产生概率p1，若p1《0.5，则在关节空间中进行随机采样得到关节构型并添加至关节构型集合s
rand
中；若p1≥0.5，则在基于代价c
best
定义的启发式采样区域中随机采样得到笛卡尔空间中的采样点x
rand
，然后通过逆运动学得到对应关节空间中关节角变量，并将所得关节构型添加至关节构型集合s
rand
中；
56.b.若还未得到初始解，则通过nnsample模块生成采样点，nnsample模块包含均匀采样和非均匀采样两种采样方式；通过随机数产生概率p2，若p2《0.5，则在关节空间中进行随机采样得到关节构型并添加至集合s
rand
中；若p2≥0.5，则：
57.b1)遍历树上的节点，找到末端执行器距离目标最近的节点对应的关节构型及此时末端执行器与目标的距离
58.b2)以末端执行器与目标的距离作为预测关节构型集进一步筛选的参考
构型θ
reference
，遍历预测关节构型集合θ
pred
中的关节构型，按照式(1)计算预测关节构型集合θ
pred
中的关节构型与参考构型θ
reference
之间的距离cost
pred
：
59.cost
pred
＝||θ
pred
,θ
reference
||2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
60.其中，δ为预设参数；
61.若cost
pred
《δ，则将该构型添加至进一步筛选得到的关节构型集θ
sampleset
中；否则继续遍历；
62.b3)以末端执行器与目标的距离作为参考代价cost
reference
；遍历关节构型集θ
sampleset
中的关节构型；首先通过机器人正运动学计算其在当前基座状态下对应的末端执行器位置x
pred
，按照式(2)计算笛卡尔空间中从当前状态到达目标状态的代价：
63.cost＝||x
pred
,x
goal
||2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
64.若cost《cost
reference
，则将预测关节构型添加至关节构型集合s
rand
中。
65.在本发明一个可行的实施例中，所述对各关节和末端执行器的位置x
new
进行碰撞检测，包括：
66.若发生碰撞则重新从关节构型集合s
rand
中随机选取关节构型作为采样点，直至遍历关节构型集合s
rand
；若不发生碰撞则：
67.1)对新节点进行重选父节点和重连接操作；
68.2)判断是否已经到达目标，若未到达，则重新从关节构型集合s
rand
中随机选取关节构型作为采样点，直至遍历关节构型集合s
rand
；若已到达，则：
69.(a)更新解所在的集合；
70.(b)判断是否已遍历关节构型集合s
rand
，若已经遍历，则判断到达最大迭代次数，输出最优路径；否则重新从关节构型集合s
rand
中随机选取关节构型作为采样点，直至遍历关节构型集合s
rand
。
71.如图2所示，本发明实施例公开了一种基于图注意力网络的机器人运动规划系统，包括：
72.初始化模块，用于初始化机器人运动状态，并求解初始解；
73.采样方式选择模块，用于根据求解初始解的情况选择采样方式，通过采样方式得到关节构型集合；
74.采样点选取模块，用于从关节构型集合中随机选取关节构型作为采样点；
75.第一计算模块，用于通过函数nearest得到当前树上已有节点中距离采样点s
rand
最近的节点θ
nearest
，并通过正运动学得到各关节和末端执行器的位置x
nearest
；
76.第二计算模块，用于根据各关节和末端执行器的位置x
nearest
，通过函数steer进行节点扩展得到关节构型θ
new
，结合正运动学得到机器人各关节和末端执行器的位置x
new
；
77.最优路径输出模块，用于对各关节和末端执行器的位置x
new
进行碰撞检测，直至到达最大迭代次数，输出最优路径。
78.本发明的原理：
79.rrt及基于rrt的运动规划方法，通过在状态空间中构建随机扩展树来找到位于起始构型与目标构型之间的可行路径，其本质上一种随机生成的数据结构-树。树型结构是节点之间有分支且存在层次关系的数据结构，图型结构数据具有网状结构且更强调节点之间的关系，是比树型结构更复杂的数据结构。树型结构也可视为图型结构的一种特例，是受到
限制的图型结构。图神经网络(graph neural networks,gnns)是一种有效的图型结构数据学习网络。本发明提出gat-informed rrt*算法，基于图神经网络中的图注意力网络gat构建了图3所示神经网络模型对给定规划条件下的路径点分布进行预测，以预测点作为采样过程中的参考点，降低采样过程中的随机性，提高所得初始解路径的质量，加速算法收敛过程。
80.如图4所示，图4为本发明公开的另一个可行的实施例，具体为一种运动规划方法，包括以下步骤：
81.步骤1：初始化机器人运动状态，包括基座的位置、姿态、关节角的初始构型、末端执行器的初始位置x0和目标位置x
goal
。
82.步骤2：根据机器人的运动状态求解初始解；首先通过机器人逆运动学求解出目标构型θ
goal
，然后针对目标构型θ
goal
和给定环境信息(障碍物)，利用训练好的神经网络模型对可行路径中的关节构型进行预测得到预测关节构型集合θ
pred
。
83.步骤3：判断是否已经得到初始解来选择不同的采样方式。
84.a.若已经得到初始解则开始进行路径优化，首先计算目前已有解的代价，比较得到最优的代价c
best
。然后利用informedsample模块进行采样，具体如下：
85.3-1)基于代价c
best
在笛卡尔空间中定义一个启发式采样区域，该区域中的点具有提升当前解质量的可能；
86.3-2)为充分利用笛卡尔空间采样区域的优势和关节空间采样的便利，通过随机数产生概率p1，若p1《0.5则在关节空间中进行随机采样得到关节构型并添加至关节构型集合s
rand
中；若p1≥0.5，则在基于代价c
best
定义的启发式采样区域中随机采样得到笛卡尔空间中的采样点x
rand
，然后通过逆运动学得到对应关节空间中关节角变量，并将所得关节构型添加至关节构型集合s
rand
中，此时关节构型集合s
rand
中只包含一组关节构型。
87.b.若还未得到初始解，则通过nnsample模块生成采样点，为保证算法的概率完备性，该模块包含均匀采样和非均匀采样两种采样方式。通过随机数产生概率p2，若满足p2《0.5，则在关节空间中进行随机采样得到关节构型并添加至集合s
rand
中；若不满足p2《0.5，则：
88.3-2-1)遍历树上的节点，找到末端执行器距离目标最近的节点对应的关节构型及此时末端执行器与目标的距离
89.3-2-2)以作为预测关节构型集进一步筛选的参考构型θ
reference
，遍历θ
pred
中的关节构型，计算其与参考构型θ
reference
之间的距离cost
pred
，计算方式如下式所示：
90.cost
pred
＝||θ
pred
,θ
reference
||2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
91.若满足cost
pred
《δ则将该构型添加至进一步筛选得到的关节构型集θ
sampleset
中，其中δ为预设参数，若不满足则继续遍历。这一步通过剔除掉预测构型集合中与参考构型相差过大的预测关节构型减少了后续采样过程中待采样点数目。
92.3-2-3)以作为参考代价cost
reference
。遍历集合θ
sampleset
中的关节构型，首先通过机器人正运动学计算其在当前基座状态下对应的末端执行器位置x
pred
，计算笛卡尔空间中从当前状态到达目标状态的代价，计算方法如下式所示：
93.cost＝||x
pred
,x
goal
||2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
94.若满足cost《cost
reference
则将预测关节构型添加至关节构型集合s
rand
中。
95.步骤4：从关节构型集合s
rand
中随机选取关节构型作为采样点s
rand
。
96.步骤5：通过函数nearest得到当前树上已有节点中距离采样点s
rand
最近的节点θ
nearest
，并通过正运动学得到笛卡尔空间中各关节和末端执行器的位置x
nearest
；
97.步骤6：通过函数steer进行节点扩展得到θ
new
，结合正运动学得到机器人各关节和末端执行器的位置x
new
；
98.步骤7：对机器人各关节和末端执行器的位置x
new
进行碰撞检测，若发生碰撞则转至步骤4直至遍历关节构型集合s
rand
；若不发生碰撞则：
99.7-1)对新节点进行重选父节点和重连接操作；
100.7-2)判断是否已经到达目标，若未到达，则转至步骤4直至遍历关节构型集合s
rand
；若已到达，则：
101.(a)更新解所在的集合；
102.(b)判断是否已遍历关节构型集合s
rand
，若已经遍历则转至步骤8，否则转至步骤4直至遍历关节构型集合s
rand
。
103.步骤8：判断是否到达最大迭代次数，若否则转至步骤3；若是则输出最优路径，运动规划结束。
104.本发明以平面障碍物环境中质点的运动规划为例进行数值仿真。gat-informed rrt*算法进行运动规划前需要对gat-cvae模型进行训练得到模型参数，因此首先要进行平面障碍物环境下质点运动规划数据集，基于此对基于gat的神经网络模型进行训练。在对质点进行运动规划时，设置两种不同的规划条件，利用gat-informed rrt*分别在两种规划条件下对质点运动进行规划，将所得规划结果与informed rrt*所得规划结果进行对比分析。
105.针对平面障碍物环境中质点的运动规划问题，本实施例中通过对起点、终点及障碍物分布进行调整来构造不同的规划场景，具体设置如下：随机生成尺寸为100
×
100的二维栅格地图，在每张地图中随机放置50个的方形障碍物，每个障碍物边长从[1,3,5]中随机选取，然后在无障碍物区域随机选取采用a*算法及jps算法进行运动规划来得到不同场景下的路径数据，最终共得到10000个不同规划条件下的规划数据，数据可视化示例如图6所示。
[0106]
torch_geometric是一个基于pytorch的库，基于此可以轻松进行图神经网络模型构建和训练，或进行图型结构数据集构建。本实施例中利用其中数据集构建模块将所得路径规划数据转换为图型结构数据，以路径信息中路径点作为graph中节点，路径点坐标为节点特征。规划结果因障碍物分布、起点和终点设置的不同而不同，因此将起点、终点坐标及障碍物分布设置为条件变量，用0表示规划环境中无障碍物区域，1表示存在障碍物的区域，因此障碍物环境分布用一个1
×
10000的向量表示。最终所得条件变量尺寸为1
×
10004，节点特征尺寸为1
×
2。
[0107]
基于gat的神经网络模型训练时使用adam优化器，并采用其默认参数。训练过程中超参数设置如下：学习率为0.005，训练过程中学习率无衰减；batch为64，epoch为100。
[0108]
仿真过程中共设置两个场景，其中障碍物分布相同，起始点和目标点位置不同。障碍物设置与数据集构建时相同，即随机设置50个方形障碍物，障碍物边长从[1,3,5]中随机选取。场景一中设置起始点为[5,3]，目标点为[90,70]；场景二中起始点设置为[40,10]，目
标点设置为[55,70]。本章中采用欧拉距离计算路径代价，具体计算公式如下：
[0109][0110]
利用gat-informed rrt*和informed rrt*算法分别对不同场景进行运动规划，仿真结果如图6及下表所示。
[0111]
图6中(a)为场景一下informed rrt*规划得到的初始解，(b)为场景一下gat-informed rrt*规划得到的初始解，(c)为场景二下informed rrt*规划得到的初始解，(d)为场景二下gat-informed rrt*规划得到的初始解。由图可得，在相同的场景下，引入了预测路径点信息的gat-informed rrt*在规划过程中构建的树更为简洁，树中节点和边更少，且初始解路径的代价可直观看出明显改进。相比之下，利用informed rrt*规划得到初始解的过程中所构建的树更为复杂，树中节点和边更多。两种算法在不同场景下所得初始解的相关数据具体如下表所示。在两个场景下，gat-informed rrt*所得初始解路径代价均优于informed-rrt*，且采样点数目大幅降低，初始解求解所需时间减少。
[0112]
图7所示为不同场景下两种算法的收敛速率，由图可得在不同场景下，gat-informed rrt*均具有更高质量的初始解，使得算法可以更快收敛。且在相同时间内，gat-informed rrt*得到的最终解路径代价小于informed rrt*，即gat-informed rrt*可以用更短的时间收敛至更优的解路径。
[0113]
不同算法得到的初始解路径数据
[0114][0115]
rrt*算法原理分析
[0116]
rrt*算法是karaman于2010年基于rrt(rapidly-exploring random tree)提出的一个渐进最优的路径规划算法，在rrt的基础上添加了重选父节点和重连接环节，提高了所得解的质量，随着规划时间趋于无穷，通过该算法得到最优解的概率趋于1。rrt*算法伪代码如下表所示，利用rrt*进行路径规划流程如下：
[0117]
首先初始化整个状态空间，定义起始点x
start
、目标点x
goal
、采样点数k、确定邻域集合的邻域半径r
rrt*
，以起始点x
start
作为树t的根节点进行扩展。在每次迭代中，通过在状态空间中均匀随机采样得到随机采样点x
rand
；利用函数nearest在树上的节点的集合中找到距离随机采样点x
rand
最近的点x
nearest
；然后利用转向函数steer来生成新的节点x
new
。
[0118]
对于新节点，首先判断从x
nearest
到x
new
之间是否存在障碍物，若存在则舍弃该点，否则以x
nearest
为父节点将该点添加至树中。以r
rrt*
为半径，遍历树上的节点，通过函数near查找节点x
new
的邻域节点集合x
near
。接下来进行重选父节点，遍历x
near
中的节点作为x
new
的父节点，将使得x
new
距离起始点的距离最近的点设置为x
new
新的父节点；重连接过程中，遍历x
near
中的节点，对于其中的节点，计算以x
new
为父节点时距离起始点的距离，若以x
new
为父节点代价更小，则将x
new
设置为该节点父节点。重复以上步骤直至达到最大迭代次数。
[0119]
rrt*算法伪代码
[0120][0121][0122]
rrt*对给定规划问题进行求解时，求解得到初始解之后便在初始解的基础上利用新的采样点进行解质量的提升。在求解过程中，rrt*在整个状态空间中进行随机采样得到随机点。在整个状态空间中进行随机采样有利于对状态空间进行探索，近似捕获状态空间的连通性；但相比于解所在的区域，整个状态空间过于庞大，因此可能出现随机采样得到的
点无法对求解问题提供帮助的情况。对采样点分布的敏感使得利用rrt*算法进行规划时无法保证得到的初始解的质量。
[0123]
informed rrt*算法原理分析
[0124]
利用基于采样的运动规划方法求解得到一个高质量解的时候，一个常用的思想是先求得一个可行的初始解，然后在初始解的基础上进行迭代优化。informed rrt*
[36]
是gammell等人于2014年提出的一个最优运动规划方法，首先通过rrt*规划得到初始解，然后对解进行优化。informed rrt*算法伪代码如下表所示。
[0125]
informed rrt*算法伪代码
[0126]
[0127][0128]
informed rrt*首先通过rrt*规划得到初始解，然后利用当前解的质量定义一个启发式采样区域，在后续迭代优化的过程中直接从启发式采样区域进行采样，通过缩小待采样空间的范围来提高采样点的质量，加速算法收敛过程。启发式采样区域定义过程如下所示。
[0129]
对于给定的运动规划问题，从起点到终点且通过状态x∈x的最优路径的代价f(x)，等于起点x
start
到当前状态x的最优路径的代价g(x)和当前状态x到目标点x
goal
。对于最小化路径长度的规划问题，可以使用欧拉距离对上述两部分代价进行估计，在状态空间中，可能进一步提高当前解的状态子集可以由当前解的代价c
best
表示为：
[0130][0131]
上式也为n维长椭球体的一般方程。
[0132]
对于二维平面问题，该状态子集为以起始点和目标点为焦点的椭圆状态子集，起始点和目标点之间的距离(两焦点之间的距离)为最小代价c
min
，椭圆长轴为当前最优路径代价c
best
，短轴为通过将优化过程中的采样点限制在该区域，使得采样得到的点总是有可能提升路径质量，提升了采样点的质量；此外该采样区域的大小与整个采样区域的大小无关，因此无论规划问题尺寸大小，该方法总是能有效的在初始解的基础上进行路径优化。
[0133]
以二维平面启发式采样区域为例，直接在椭圆中进行均匀随机较为困难，可以利用椭圆与圆之间的关系，先在单位圆内进行均匀随机采样，然后将其转换至椭圆内的采样点。为此首先对超椭球体矩阵s进行cholesky分解得到转换矩阵，即：
[0134]
ll
t
≡s
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0135]
上式中l即为所需转换矩阵，s为超椭球体矩阵，满足下式：
[0136]
(x-x
centre
)
t
s(x-x
centre
)＝1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0137]
通过转换单位球内的均匀随机采样结果得到椭球体中的均匀采样，即首先从单位球内进行均匀随机采样得到x
ball
，然后通过下式变换得到椭球体内的均匀采样：
[0138]
x
ellipse
＝lx
ball
+x
centre
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0139]
上式中x
ellipse
～u(x
ellipse
)为椭球体内的均匀采样，x
ball
～u(x
ball
)为单位球内的均匀随机采样，其中x
ball
＝{x∈x|||x||2≤1}，x
centre
＝(x
f1
+x
f2
)/2为超椭球体的中心点，x
f1
和x
f2
为其焦点。
[0140]
informed rrt*算法基于当前解的代价定义了一个椭圆的状态空间子集(以二维平面内为例)，该子集中的每个点都有可能提升当前解的质量，在路径优化的过程中，将随机采样的范围限制在该子集中来得到一个高质量的采样点，加速了算法优化的过程。但在初始解求解的过程中，informed rrt*并未在rrt*的基础上进行改进，依旧延续了rrt*固有的缺点，即在整个状态空间中进行随机采样时得到的采样点不一定有助于问题的求解。使得informed rrt*延续了对采样分布敏感导致难以保证初始解的质量的问题。而informed rrt*优化的过程与初始解质量息息相关，以二维平面路径最短规划问题为例，一个更高质量的初始解路径长度更短，基于此定义的椭圆状态空间子集将更小，即待采样区域更小，因此优化过程中得到的采样点将可以提升当前解的质量的可能性将更大，算法收敛速度将更快。综上所述，informed rrt*存在对采样分布敏感，无法保证初始解的质量以及算法收敛时间。
[0141]
机器人运动规划算法nnsample模块伪代码
programmable gate array，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。
[0150]
所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述计算机设备的各种功能。
[0151]
所述计算机设备集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(rom，read-only memory)、随机存取存储器(ram，random access memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
[0152]
以上仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种基于图注意力网络的机器人运动规划方法，其特征在于，包括以下步骤：初始化机器人运动状态，并求解初始解；根据求解初始解的情况选择采样方式，通过采样方式得到关节构型集合；从关节构型集合中随机选取关节构型作为采样点；通过函数nearest得到当前树上已有节点中距离采样点s
rand
最近的节点θ
nearest
，并通过正运动学得到各关节和末端执行器的位置x
nearest
；根据各关节和末端执行器的位置x
nearest
，通过函数steer进行节点扩展得到关节构型θ
new
，结合正运动学得到机器人各关节和末端执行器的位置x
new
；对各关节和末端执行器的位置x
new
进行碰撞检测，直至到达最大迭代次数，输出最优路径。2.根据权利要求1所述的基于图注意力网络的机器人运动规划方法，其特征在于，所述机器人运动状态包括基座的位置、姿态、关节角的初始构型、末端执行器的初始位置x0和目标位置x
goal
。3.根据权利要求1或2所述的基于图注意力网络的机器人运动规划方法，其特征在于，所述求解初始解，包括：通过机器人逆运动学求解出目标构型θ
goal
；针对目标构型θ
goal
和给定环境信息，利用训练好的神经网络模型对可行路径中的关节构型进行预测，得到预测关节构型集合θ
pred
。4.根据权利要求1所述的基于图注意力网络的机器人运动规划方法，其特征在于，所述根据求解初始解的情况选择采样方式，包括：判断是否已经得到初始解来选择不同的采样方式；a.若已经得到初始解则开始进行路径优化：计算目前已有解的代价，比较得到最优的代价c
best
；利用informedsample模块进行采样，具体如下：a1)基于代价c
best
在笛卡尔空间中定义一个启发式采样区域，该区域中的点具有提升当前解质量的可能；a2)为充分利用笛卡尔空间采样区域的优势和关节空间采样的便利，通过随机数产生概率p1，若p1<0.5，则在关节空间中进行随机采样得到关节构型并添加至关节构型集合s
rand
中；若p1≥0.5，则在基于代价c
best
定义的启发式采样区域中随机采样得到笛卡尔空间中的采样点x
rand
，然后通过逆运动学得到对应关节空间中关节角变量，并将所得关节构型添加至关节构型集合s
rand
中；b.若还未得到初始解，则通过nnsample模块生成采样点，nnsample模块包含均匀采样和非均匀采样两种采样方式；通过随机数产生概率p2，若p2<0.5，则在关节空间中进行随机采样得到关节构型并添加至集合s
rand
中；若p2≥0.5，则：b1)遍历树上的节点，找到末端执行器距离目标最近的节点对应的关节构型及此时末端执行器与目标的距离b2)以末端执行器与目标的距离作为预测关节构型集进一步筛选的参考构型θ
reference
，遍历预测关节构型集合θ
pred
中的关节构型，按照式(1)计算预测关节构型集合
θ
pred
中的关节构型与参考构型θ
reference
之间的距离cost
pred
：cost
pred
＝||θ
pred
,θ
reference
||2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)其中，δ为预设参数；若cost
pred
<δ，则将该构型添加至进一步筛选得到的关节构型集θ
sampleset
中；否则继续遍历；b3)以末端执行器与目标的距离作为参考代价cost
reference
；遍历关节构型集θ
sampleset
中的关节构型；首先通过机器人正运动学计算其在当前基座状态下对应的末端执行器位置x
pred
，按照式(2)计算笛卡尔空间中从当前状态到达目标状态的代价：cost＝||x
pred
,x
goal
||2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)若cost<cost
reference
，则将预测关节构型添加至关节构型集合s
rand
中。5.根据权利要求1所述的基于图注意力网络的机器人运动规划方法，其特征在于，所述对各关节和末端执行器的位置x
new
进行碰撞检测，包括：若发生碰撞则重新从关节构型集合s
rand
中随机选取关节构型作为采样点，直至遍历关节构型集合s
rand
；若不发生碰撞则：1)对新节点进行重选父节点和重连接操作；2)判断是否已经到达目标，若未到达，则重新从关节构型集合s
rand
中随机选取关节构型作为采样点，直至遍历关节构型集合s
rand
；若已到达，则：(a)更新解所在的集合；(b)判断是否已遍历关节构型集合s
rand
，若已经遍历，则判断到达最大迭代次数，输出最优路径；否则重新从关节构型集合s
rand
中随机选取关节构型作为采样点，直至遍历关节构型集合s
rand
。6.一种基于图注意力网络的机器人运动规划系统，其特征在于，包括：初始化模块，用于初始化机器人运动状态，并求解初始解；采样方式选择模块，用于根据求解初始解的情况选择采样方式，通过采样方式得到关节构型集合；采样点选取模块，用于从关节构型集合中随机选取关节构型作为采样点；第一计算模块，用于通过函数nearest得到当前树上已有节点中距离采样点s
rand
最近的节点θ
nearest
，并通过正运动学得到各关节和末端执行器的位置x
nearest
；第二计算模块，用于根据各关节和末端执行器的位置x
nearest
，通过函数steer进行节点扩展得到关节构型θ
new
，结合正运动学得到机器人各关节和末端执行器的位置x
new
；最优路径输出模块，用于对各关节和末端执行器的位置x
new
进行碰撞检测，直至到达最大迭代次数，输出最优路径。7.一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1-5任一项所述方法的步骤。8.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-5任一项所述方法的步骤。

技术总结
本发明公开了一种基于图注意力网络的机器人运动规划方法及相关装置，基于GAT所构建的神经网络模型可以很好的学习图型结构数据的特征，对不同环境(障碍物分布、起始点和目标点)下的路径点分布进行预测，在对具体规划问题进行求解时，该算法以一定概率进行非均匀随机采样，即以一定概率选取预测路径点集中的点作为采样点，降低了采样点生成过程中的随机性，提高了采样点的质量，因此得到了高质量的初始解，加速了算法优化过程，实现了快速运动规划。规划。规划。


技术研发人员：朱战霞 李洁 马城城 孙成璐 王闯
受保护的技术使用者：西北工业大学
技术研发日：2023.06.20
技术公布日：2023/9/22