CN111216132A

CN111216132A - 基于改进rrt算法的六自由度机械臂路径规划方法

Info

Publication number: CN111216132A
Application number: CN202010069986.3A
Authority: CN
Inventors: 牛军浩; 戴冰; 李玉虎; 李智; 许川佩; 朱爱军
Original assignee: Guilin University of Electronic Technology
Current assignee: Guilin University of Electronic Technology
Priority date: 2020-01-21
Filing date: 2020-01-21
Publication date: 2020-06-02

Abstract

本发明公开了一种基于改进RRT算法的六自由度机械臂路径规划方法，根据连杆上的固定坐标系得到的齐次变换矩阵，建立机械臂运动学模型，获取机械臂状态空间中的初始点，并构建随机树，并将按照设定步长扩展出新的状态点连接起来，得到设定路径，根据所述设定路径进行强化学习，并设定一个马尔可夫决策过程，得到Q函数并初始化为零，并利用单纯形法进行优化，更新迭代至Q值表全部更新完成，根据强化学习后的所述状态点建立节点池，并利用所述Q函数按照设定概率对所述状态点进行选择，完成树的扩展采样，得到目标路径，提高路径的规划效率，并有效逃离局部极小值。

Description

基于改进RRT算法的六自由度机械臂路径规划方法

技术领域

本发明涉及机械臂路径规划技术领域，尤其涉及一种基于改进RRT算法的六自由度机械臂路径规划方法。

背景技术

在机械臂的应用中，六自由度是完成空间定位能力的最小自由度数，在工业场景中被广泛应用。机械臂的路径规划是机器人领域的基本问题，主要解决的是为机械臂找到一条从起始状态到目标状态的运动路径，并且在运动过程中避开障碍物。随着机器人学的发展，国内外学者已经提出许多路径规划算法，例如A*算法、Dijkstra算法等，在应用这些算法时常把机器人简化为一个质点，然而六自由度机械臂是一个高非线性、强耦合的复杂系统，若将其简化为笛卡尔空间的质点会无法描述机械臂的运动，因此大多数的路径规划算法无法在机械臂中直接使用，快速搜索随机树算法(Rapidly-exploring Random Tree，RRT)是一种采用增量方式增长的随机采样算法，可以应用在搜索空间维数较高的场景中，六自由度机械臂的路径规划维度较高，RRT算法可以解决它的路径规划问题。但由于RRT算法的不完善性，规划结果容易陷入局部极小值，导致最终得到的路径并非理想结果。

发明内容

本发明的目的在于提供一种基于改进RRT算法的六自由度机械臂路径规划方法，提高路径的规划效率，并有效逃离局部极小值。

为实现上述目的，本发明提供了一种基于改进RRT算法的六自由度机械臂路径规划方法，包括：

根据连杆上的固定坐标系得到的齐次变换矩阵，建立机械臂运动学模型；

获取随机树，并连接选择的状态点，得到设定路径；

获取并初始化Q函数，更新迭代至Q值表全部更新完成；

利用所述Q函数对节点池进行选择，得到目标路径。

其中，所述根据连杆上的固定坐标系得到的齐次变换矩阵，建立机械臂运动学模型，包括：

获取每个连杆上设定的固定坐标系，得到末端坐标系相对于基坐标系的齐次变换矩阵，并根据所述齐次变换矩阵建立机械臂运动学模型。

其中，所述根据连杆上的固定坐标系得到的齐次变换矩阵，建立机械臂运动学模型，还包括：

将关节的运动轴正方向标定为Z轴，将垂直于Z轴并指向离开Z轴的方向标定为X轴，根据右手定则建立Y轴，完成所述固定坐标系的设定。

其中，所述获取随机树，并连接选择的状态点，得到设定路径，包括：

获取机械臂状态空间中的初始点，并构建随机树，并通过随机采样的方式在所述状态空间中选择随机节点，遍历所述随机树找到设定节点，并按照设定步长扩展出新的状态点，并判断在扩展过程中是否检测到碰撞，直至迭代搜索到设定点后，依次将所述状态点连接起来，得到设定路径。

其中，所述判断在扩展过程中是否检测到碰撞，包括：

若在扩展过程中没有检测到碰撞，则将所述状态点添加到所述随机树中；

若在扩展过程中检测到碰撞，则重新产生所述随机节点。

其中，所述获取并初始化Q函数，更新迭代至Q值表全部更新完成，包括：

根据所述设定路径进行强化学习，并设定一个马尔可夫决策过程，得到Q函数并初始化为零，并从当前状态出发，选择并执行下一个动作，得到下一个状态和强化信号，同时利用单纯形法设定下一个状态的设定动作，直至Q值表全部更新迭代完成。

其中，利用所述Q函数对节点池进行选择，得到目标路径，包括：

根据强化学习后的所述状态点建立节点池，并利用所述Q函数按照设定概率对所述状态点进行选择，得到扩展节点，并将未被选择的所述状态点删除，完成树的扩展采样，得到目标路径。

本发明的一种基于改进RRT算法的六自由度机械臂路径规划方法，根据连杆上的固定坐标系得到的齐次变换矩阵，建立机械臂运动学模型，获取机械臂状态空间中的初始点，并构建随机树，并将按照设定步长扩展出新的状态点连接起来，得到设定路径，根据所述设定路径进行强化学习，并设定一个马尔可夫决策过程，得到Q函数并初始化为零，并利用单纯形法进行优化，更新迭代至Q值表全部更新完成，根据强化学习后的所述状态点建立节点池，并利用所述Q函数按照设定概率对所述状态点进行选择，完成树的扩展采样，得到目标路径，提高路径的规划效率，并有效逃离局部极小值。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明提供的一种基于改进RRT算法的六自由度机械臂路径规划方法的步骤示意图。

图2是本发明提供的RRT搜索扩展示意图。

图3是本发明提供的强化学习基本原理框架图。

图4是本发明提供的树的节点选择原理图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

请参阅图1，本发明提供一种基于改进RRT算法的六自由度机械臂路径规划方法，包括：

S101、根据连杆上的固定坐标系得到的齐次变换矩阵，建立机械臂运动学模型。

具体的，获取采用D-H法在机械臂每个连杆上设定的固连坐标系，所述固定坐标系是用来描述杆件之间、杆件与关节之间的位移关系，得出末端坐标系相对于基座坐标系的齐次变换矩阵，并根据所述齐次变换矩阵建立机械臂运动学模型，所述齐次变换矩阵是各关节变量的函数，用于从机械臂的关节空间到笛卡尔空间的求运动学正解，运动学正解是机械臂的关节空间到笛卡尔空间的映射，由此可求出在给定机械臂姿态和关节角下，机械臂的末端位姿，从而实现路径规划。所述固定坐标系的建立规则为：

(1)Z_i-1轴为关节i的运动轴正方向。

(2)x_i轴垂直于Z_i-1轴并指向离开Z_i-1轴的方向。

(3)y_i轴根据右手定则建立。

建立相应的D-H参数表，如表1所示。其中参数α和d表示机械臂连杆长度参数。

表1 D-H参数表

连杆i	θ<sub>i</sub>/(°)	α<sub>i-1</sub>/(°)	a<sub>i-1</sub>/m	d<sub>i</sub>/m	关节角/(°)
						1	θ<sub>1</sub>	0	0	0	-360～360
2	θ<sub>2</sub>	90	7.9	0	-105～90
						3	θ<sub>3</sub>	0	8.1	0	-100～70
4	θ<sub>4</sub>	90	5.3	5.3	-360～360
						5	θ<sub>5</sub>	90	5.0	0	-80～70
6	θ<sub>6</sub>	90	1.2	1.2	-360～360

表中：α_i-1是从Z_i-1到Z_i沿着x_i-1的距离；d_i是从x_i-1到x_i沿着Z_i的距离；a_i-1是∑₀从Z_i-1到Z_i绕着x_i-1旋转的角度；θ_i是从x_i-1到x_i绕着Z_i旋转的角度。可以推导出机械臂各连杆的齐次变换矩阵，则机械臂的末端坐标系∑_m到参考坐标系∑₀的齐次变换矩阵为：

S102、获取随机树，并连接选择的状态点，得到设定路径。

具体的，利用RRT算法在工作环境中的探索，为后续的强化学习提供先验信息。快速搜索随机树算法(Rapidly-exploring Random Tree，RRT)是一种采用增量方式增长的随机采样算法，用于解决有代数约束(障碍带来的)和微分约束(非完整性和动态环境带来的)的高维空间问题。RRT算法搜索空间的覆盖率高，搜索的范围广，可以尽可能的探索未知区域。利用所述RRT算法在获取的机械臂状态空间中的初始点x_start构建随机树，并通过随机采样的方式在所述状态空间中选择随机节点，遍历所述随机树找到设定节点，所述设定节点为离所述随机节点最近的节点x_near，并按照设定步长扩展出新的状态点，并判断在扩展过程中是否检测到碰撞，若在扩展过程中没有检测到碰撞，则将所述状态点添加到所述随机树中若在扩展过程中检测到碰撞，则重新产生所述随机节点x_new，直至迭代搜索到设定点x_rand后，依次将所述状态点连接起来，得到设定路径，其中，RRT搜索扩展的示意图如图2所示。

S103、获取并初始化Q函数，更新迭代至Q值表全部更新完成。

具体的，利用得到的所述设定路径进行强化学习，强化学习是模拟人类学习的过程，目标的状态受执行的动作发生改变，环境同时受其影响产生一个奖励或惩罚的信号作为反馈，该信号即强化信号，接着影响下一个动作，目标执行的动作同时影响下一刻的状态和接收的强化信号的值，强化学习的基本原理框架如图3所示。为了描述学习的方式，首先设定一个马尔可夫决策过程(S,A,T,R,γ)，其中

为机械臂所处的状态空间；

为机械臂的动作空间，即机械臂在任何状态下的所有动作空间的集合；R:S×A→R为激励函数，代表机械臂在动作a下，从状态s到状态s'所得到的激励；T:S×A→S为转换函数，代表机械臂在状态s和动作a下，到达下一个状态s'的概率；γ∈[0,1]为激励的衰减因子，下一个时刻的激励将按照这个因子进行衰减。

由于空间的连续性，动作-状态值不能用于所有的状态和动作。为了减少训练数据的数量以及对类似的状态动作值进行经验总结，建立Q函数Q(s，a)，并初始化为零，设所述Q函数的权值为：

w＝[w₁,...,w_n]；s,s'∈S；a∈A

则所述Q函数为：

Q(s,a)＝w₁φ₁(s,a)+w₂φ₂(s,a)+...+w_nφ_n(s,a)

为了找到J(w)的最小值，用梯度下降法更新w，方法如下：

w←w+αΔw

其中，

α为每一步的长度。

为了在给定状态下得到接下来的最佳动作，需要一个方法进行选择。在这个过程中，给定的状态是树中的一个节点，动作是树从该节点的扩展。在此使用单纯形法(Nelder-Mead，NM)优化方法，通过最大限度地利用所述Q函数来寻找节点的扩展。因为树扩展的过程中步长是确定的值,下一个状态s'位于一个超球面上。因此，通过归一化将NM过程的顶点变换投影到超球面上。因为顶点的变换在任何迭代中都不会发生显著的变化，所以这种方法适用于该场景。最后，为了克服NM方法的局部极小问题，需要检查主轴上的状态值，然后生成初始顶点，数量为n+1个，且主轴周围的高斯随机样本具有最大的状态值。通过这种优化方法，可以找到最优的动作a(s)和状态值v(s)，直至Q值表全部更新迭代完成，从而使给定状态的状态动作值最大化，具体如下：

S104、利用所述Q函数对节点池进行选择，得到目标路径。

具体的，根据强化学习后的所述状态点建立节点池，并利用所述Q函数按照设定概率对所述状态点进行选择，其中，所述设定概率为所述节点池中概率最大的所述状态点为选择依据，得到最优的扩展节点，并将未被选择的所述状态点删除，完成树的扩展采样，得到目标路径，如图4所示，从一个新节点x_new开始，经过RRT的随机采样得到一个节点池x₁,x₂,x₃,...x_n，使用期望状态值选择池中的一个节点，并用Q函数对所选节点进行扩展。

以上所揭露的仅为本发明一种较佳实施例而已，当然不能以此来限定本发明之权利范围，本领域普通技术人员可以理解实现上述实施例的全部或部分流程，并依本发明权利要求所作的等同变化，仍属于发明所涵盖的范围。

Claims

1.一种基于改进RRT算法的六自由度机械臂路径规划方法，其特征在于，

获取随机树，并连接选择的状态点，得到设定路径；

获取并初始化Q函数，更新迭代至Q值表全部更新完成；

利用所述Q函数对节点池进行选择，得到目标路径。

2.如权利要求1所述的一种基于改进RRT算法的六自由度机械臂路径规划方法，其特征在于，所述根据连杆上的固定坐标系得到的齐次变换矩阵，建立机械臂运动学模型，包括：

3.如权利要求2所述的一种基于改进RRT算法的六自由度机械臂路径规划方法，其特征在于，所述根据连杆上的固定坐标系得到的齐次变换矩阵，建立机械臂运动学模型，还包括：

4.如权利要求1所述的一种基于改进RRT算法的六自由度机械臂路径规划方法，其特征在于，所述获取随机树，并连接选择的状态点，得到设定路径，包括：

5.如权利要求4所述的一种基于改进RRT算法的六自由度机械臂路径规划方法，其特征在于，所述判断在扩展过程中是否检测到碰撞，包括：

若在扩展过程中检测到碰撞，则重新产生所述随机节点。

6.如权利要求5所述的一种基于改进RRT算法的六自由度机械臂路径规划方法，其特征在于，所述获取并初始化Q函数，更新迭代至Q值表全部更新完成，包括：

7.如权利要求6所述的一种基于改进RRT算法的六自由度机械臂路径规划方法，其特征在于，利用所述Q函数对节点池进行选择，得到目标路径，包括：