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

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

A path planning method of 6-DOF manipulator based on improved RRT algorithm

【技术实现步骤摘要】
基于改进RRT算法的六自由度机械臂路径规划方法
本专利技术涉及机械臂路径规划
，尤其涉及一种基于改进RRT算法的六自由度机械臂路径规划方法。
技术介绍
在机械臂的应用中，六自由度是完成空间定位能力的最小自由度数，在工业场景中被广泛应用。机械臂的路径规划是机器人领域的基本问题，主要解决的是为机械臂找到一条从起始状态到目标状态的运动路径，并且在运动过程中避开障碍物。随着机器人学的发展，国内外学者已经提出许多路径规划算法，例如A*算法、Dijkstra算法等，在应用这些算法时常把机器人简化为一个质点，然而六自由度机械臂是一个高非线性、强耦合的复杂系统，若将其简化为笛卡尔空间的质点会无法描述机械臂的运动，因此大多数的路径规划算法无法在机械臂中直接使用，快速搜索随机树算法(Rapidly-exploringRandomTree，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)Zi-1轴为关节i的运动轴正方向。(2)xi轴垂直于Zi-1轴并指向离开Zi-1轴的方向。(3)yi轴根据右手定则建立。建立相应的D-H参数表，如表1所示。其中参数α和d表示机械臂连杆长度参数。表1D-H参数表连杆iθi/(°)αi-1/(°)ai-1/mdi/m关节角/(°)1θ1000-360～3602θ2907.90-105～903θ308.10-100～704θ4905.35.3-360～3605θ5905.00-80～706θ6901.21.2-360～360表中：αi-1是从Zi-1到Zi沿着xi-1的距离；di是从xi-1到xi沿着Zi的距离；ai-1本文档来自技高网...

【技术保护点】
1.一种基于改进RRT算法的六自由度机械臂路径规划方法，其特征在于，/n根据连杆上的固定坐标系得到的齐次变换矩阵，建立机械臂运动学模型；/n获取随机树，并连接选择的状态点，得到设定路径；/n获取并初始化Q函数，更新迭代至Q值表全部更新完成；/n利用所述Q函数对节点池进行选择，得到目标路径。/n

【技术特征摘要】
1.一种基于改进RRT算法的六自由度机械臂路径规划方法，其特征在于，
根据连杆上的固定坐标系得到的齐次变换矩阵，建立机械臂运动学模型；
获取随机树，并连接选择的状态点，得到设定路径；
获取并初始化Q函数，更新迭代至Q值表全部更新完成；
利用所述Q函数对节点池进行选择，得到目标路径。


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


3.如权利要求2所述的一种基于改进RRT算法的六自由度机械臂路径规划方法，其特征在于，所述根据连杆上的固定坐标系得到的齐次变换矩阵，建立机械臂运动学模型，还包括：
将关节的运动轴正方向标定为Z轴，将垂直于Z轴并指向离开Z轴的方向标定为X轴，根据右手定则建立Y轴，完成所述固定坐标系的设定。


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

【专利技术属性】
技术研发人员：牛军浩，戴冰，李玉虎，李智，许川佩，朱爱军，
申请(专利权)人：桂林电子科技大学，
类型：发明
国别省市：广西;45