一种基于三维任务空间约束的机械臂避障方法及装置与流程

本发明涉及机械臂自主路径规划
技术领域：
，更具体涉及一种基于三维任务空间约束的机械臂避障方法及装置。
背景技术：
：机械臂避障路径规划技术，主要解决的是在障碍物环境下的多轴机械臂自主寻找一条从给定起点到终点路径的问题，规划出的路径能够使机械臂与障碍物及自身不发生任何碰撞。现阶段，避障路径规划分为两大类，一种是二维环境下移动机器人的避障路径规划，方法已经非常丰富，主要有dijkstra算法、a*算法、人工势场法等，另一种是三维环境下机械臂的避障路径规划，多轴机械臂是一种多输入多输出、强耦合、非线性的高维复杂系统，若直接将上述二维的路径规划方法移植到机械臂上，则分别存在时间长、计算量大、易陷入局部最优等缺点。基于随机采样的rrt算法速度更快，搜索能力强，不需要对障碍物进行精确建模，更适合于在机械臂关节的高维空间中进行搜索，不过正是由于随机采样概率完备的特点，容易出现机械臂的最终机械臂笛卡尔构型奇怪、路径不是最优等问题。中国专利申请号201911086247.9，公开了一种改进型动态rrt*的移动机器人运动规划方法。该方法剔除了原rrt*算法中所有的碰撞检测，通过在代价函数中增加碰撞风险评估分量的方法控制代价函数值。当采样点或边与障碍物发生碰撞时，该分量将显著增大，促使代价函数值同步显著增大，抑制了发生碰撞的点和边继续扩张的可能性，从而使算法具有避障的能力。因该方法没有碰撞检测，在移动机器人运动规划问题上比rrt*算法收敛的速度更快，效率更高，在越复杂的环境中优势越突出，但是其不涉及机械臂笛卡尔构型以及路径优化的问题。技术实现要素：本发明所要解决的技术问题在于现有技术机械臂避障方法容易出现机械臂的最终机械臂笛卡尔构型奇怪以及路径不是最优的问题。本发明通过以下技术手段实现解决上述技术问题的：一种基于三维任务空间约束的机械臂避障方法，所述方法包括：步骤一：获取关节空间中机械臂运动路径的起始点qs和目标点qd；步骤二：双向rrt搜索树包括搜索树rrtree1和搜索树rrtree2，初始化搜索树rrtree1的树根节点为起始点qs，搜索树rrtree2的树根节点为目标点qd，对搜索树rrtree1进行扩展得到当前扩展关节点；步骤三：依据设定的三维任务空间约束对当前扩展关节点new_point_temp进行约束更新得到更新后的扩展关节点；步骤四：得到更新后的扩展关节点后，进行环境障碍物及自碰撞检测并且对双向rrt搜索树进行扩展，提取出一条从起始点qs到达目标点qd的无碰撞原始路径；步骤五：对原始路径进行优化得到优化的路径；步骤六：根据优化后的路径，移动六轴机械臂从起始点qs无碰撞地运动到目标点qd。本发明通过在双向rrt搜索扩展过程中施加三维任务空间约束，约束机械臂末端在指定的范围内运动，从而得到更加优异平滑的运动规划路径，快速有效的实现多轴机械臂末端轨迹路径规划并对原始路径进行优化得到优化的路径，择优后路径具有可实施性，使得多轴机械臂具有自主规划能力，最终机械臂笛卡尔构型符合机器人运动规律。进一步地，所述步骤二包括：搜索时，先对搜索树rrtree1进行扩展，在关节空间范围内通过公式获取随机点，其中，rand是一个0～1的随机值，α是给定的0～1之间的值，random(χ)表示生成一个小于常数χ的随机数；在搜索树rrtree1的树节点中找到与随机点最近的树节点closestnode_1，从与随机点最近的树节点closestnode_1出发通过公式向着随机点扩展一个步长得到当前扩展关节点new_point_temp，其中，segmentlength表示搜索步长。进一步地，所述步骤三包括：利用给定约束矩阵获取给定的三维任务空间约束范围；根据公式获取机械臂末端工具相对于约束坐标系下的位姿矩阵，其中，表示机械臂末端工具坐标系的齐次变换矩阵，表示三维任务空间约束范围的参考变换；将机械臂末端工具相对于约束坐标系下的位姿矩阵通过转换为一个6维的位移矢量，其中，表示机械臂末端工具相对于约束坐标系的位移，后三行表示末端工具相对于约束坐标系的姿态，考虑给给定约束矩阵c的边界，利用公式得到该约束下的笛卡尔空间位移δx，cmax表示约束矩阵c的上边界，cmin表示约束矩阵c的下边界；通过公式δq＝jt(jjt)-1δx将笛卡尔空间位移δx投影到关节空间得到关节空间位移δq，其中，j表示雅克比矩阵；利用公式new_point_temp’＝new_point_temp-δq对当前扩展关节点new_point_temp进行更新得到更新后的扩展关节点new_point_temp’。进一步地，所述步骤四包括：得到更新后的扩展关节点new_point_temp’后，将其与其父节点之间形成的路径进行环境障碍物及自碰撞检测，如果发生碰撞则将更新后的扩展关节点new_point_temp’作为搜索树rrtree1中的一个安全构型new_point_1；如果没有发生碰撞，将更新后的扩展关节点new_point_temp’收录到搜索树rrtree1中，然后判断更新后的扩展关节点new_point_temp’与当前随机点之间的欧式距离是否小于衔接误差disth，若否则返回执行步骤二至步骤四直到欧式距离小于衔接误差disth，小于衔接误差disth后，交换搜索树rrtree2进行扩展，搜索树rrtree2的扩展方向是搜索树rrtree1扩展得到的安全构型new_point_1；在搜索树rrtree2中找到距离安全构型new_point_1最近的节点closestnode_2，从该节点出发朝着安全构型new_point_1按设定步长前进，并依据设定的三维任务空间约束对扩展后的点进行约束更新，得到满足约束且不与环境及自身发生碰撞的安全构型new_point_2，一旦扩展过程中发生碰撞，则交换树进行扩展，如此循环，如果安全构型new_point_1与安全构型new_point_2之间的欧式距离小于衔接误差disth，则说明搜索树rrtree1与搜索树rrtree2连到一起，由此提取出一条起始点qs到达目标点qd的无碰撞原始路径path。进一步地，所述步骤五包括：步骤501：在原始路径path中随机提取树节点i和树节点j，原始路径path中从树节点i到树节点j的路径为子路径path_ij；步骤502：从树节点i开始，依照给定的三维任务空间约束利用公式new_path_ij＝path_ij+δq更新子路径path_ij得到更新后的子路径new_path_ij；步骤503：判断子路径path_ij的长度是否小于更新后的子路径new_path_ij，若是，则将更新后的子路径new_path_ij替换子路径path_ij得到更新后的路径new_path，将更新后的路径new_path作为原始路径返回执行步骤501至步骤503，若否，则返回执行步骤501至步骤503，达到预设的迭代次数以后输出最终优化的路径。本发明还提供一种基于三维任务空间约束的机械臂避障装置，所述装置包括：起始点和目标点获取模块，用于获取关节空间中机械臂运动路径的起始点qs和目标点qd；初始化模块，用于双向rrt搜索树包括搜索树rrtree1和搜索树rrtree2，初始化搜索树rrtree1的树根节点为起始点qs，搜索树rrtree2的树根节点为目标点qd，对搜索树rrtree1进行扩展得到当前扩展关节点；更新模块，用于依据设定的三维任务空间约束对当前扩展关节点new_point_temp进行约束更新得到更新后的扩展关节点；原始路径获取模块，用于得到更新后的扩展关节点后，进行环境障碍物及自碰撞检测并且对双向rrt搜索树进行扩展，提取出一条从起始点qs到达目标点qd的无碰撞原始路径；优化模块，用于对原始路径进行优化得到优化的路径；控制模块，用于根据优化后的路径，移动六轴机械臂从起始点qs无碰撞地运动到目标点qd。进一步地，所述初始化模块还用于：搜索时，先对搜索树rrtree1进行扩展，在关节空间范围内通过公式获取随机点，其中，rand是一个0～1的随机值，α是给定的0～1之间的值，random(χ)表示生成一个小于常数χ的随机数；在搜索树rrtree1的树节点中找到与随机点最近的树节点closestnode_1，从与随机点最近的树节点closestnode_1出发通过公式向着随机点扩展一个步长得到当前扩展关节点new_point_temp，其中，segmentlength表示搜索步长。进一步地，所述更新模块还用于：利用给定约束矩阵获取给定的三维任务空间约束范围；根据公式获取机械臂末端工具相对于约束坐标系下的位姿矩阵，其中，表示机械臂末端工具坐标系的齐次变换矩阵，表示三维任务空间约束范围的参考变换；将机械臂末端工具相对于约束坐标系下的位姿矩阵通过转换为一个6维的位移矢量，其中，表示机械臂末端工具相对于约束坐标系的位移，后三行表示末端工具相对于约束坐标系的姿态，考虑给给定约束矩阵c的边界，利用公式得到该约束下的笛卡尔空间位移δx，cmax表示约束矩阵c的上边界，cmin表示约束矩阵c的下边界；通过公式δq＝jt(jjt)-1δx将笛卡尔空间位移δx投影到关节空间得到关节空间位移δq，其中，j表示雅克比矩阵；利用公式new_point_temp’＝new_point_temp-δq对当前扩展关节点new_point_temp进行更新得到更新后的扩展关节点new_point_temp’。进一步地，所述原始路径获取模块还用于：得到更新后的扩展关节点new_point_temp’后，将其与其父节点之间形成的路径进行环境障碍物及自碰撞检测，如果发生碰撞则将更新后的扩展关节点new_point_temp’作为搜索树rrtree1中的一个安全构型new_point_1；如果没有发生碰撞，将更新后的扩展关节点new_point_temp’收录到搜索树rrtree1中，然后判断更新后的扩展关节点new_point_temp’与当前随机点之间的欧式距离是否小于衔接误差disth，若否则返回执行初始化模块、更新模块以及原始路径获取模块直到欧式距离小于衔接误差disth，小于衔接误差disth后，交换搜索树rrtree2进行扩展，搜索树rrtree2的扩展方向是搜索树rrtree1扩展得到的安全构型new_point_1；在搜索树rrtree2中找到距离安全构型new_point_1最近的节点closestnode_2，从该节点出发朝着安全构型new_point_1按设定步长前进，并依据设定的三维任务空间约束对扩展后的点进行约束更新，得到满足约束且不与环境及自身发生碰撞的安全构型new_point_2，一旦扩展过程中发生碰撞，则交换树进行扩展，如此循环，如果安全构型new_point_1与安全构型new_point_2之间的欧式距离小于衔接误差disth，则说明搜索树rrtree1与搜索树rrtree2连到一起，由此提取出一条起始点qs到达目标点qd的无碰撞原始路径path。进一步地，所述优化模块还用于：步骤501：在原始路径path中随机提取树节点i和树节点j，原始路径path中从树节点i到树节点j的路径为子路径path_ij；步骤502：从树节点i开始，依照给定的三维任务空间约束利用公式new_path_ij＝path_ij+δq更新子路径path_ij得到更新后的子路径new_path_ij；步骤503：判断子路径path_ij的长度是否小于更新后的子路径new_path_ij，若是，则将更新后的子路径new_path_ij替换子路径path_ij得到更新后的路径new_path，将更新后的路径new_path作为原始路径返回执行步骤501至步骤503，若否，则返回执行步骤501至步骤503，达到预设的迭代次数以后输出最终优化的路径。本发明的优点在于：本发明通过在双向rrt搜索扩展过程中施加三维任务空间约束，约束机械臂末端在指定的范围内运动，从而得到更加优异平滑的运动规划路径，快速有效的实现多轴机械臂末端轨迹路径规划并对原始路径进行优化得到优化的路径，择优后路径具有可实施性，使得多轴机械臂具有自主规划能力，最终机械臂笛卡尔构型符合机器人运动规律。附图说明图1为本发明实施例所公开的一种基于三维任务空间约束的机械臂避障方法的流程图；图2为本发明实施例所公开的一种基于三维任务空间约束的机械臂避障方法中带末端工具的六轴机械臂及障碍物环境示意图；图3为本发明实施例所公开的一种基于三维任务空间约束的机械臂避障方法中基于六轴机械臂模型的d-h坐标系示意图；图4为本发明实施例所公开的一种基于三维任务空间约束的机械臂避障方法中步骤四的算法流程图；图5为本发明实施例所公开的一种基于三维任务空间约束的机械臂避障方法中碰撞检测时对障碍物以及机械臂进行包络的示意图；图6为本发明实施例所公开的一种基于三维任务空间约束的机械臂避障方法中碰撞检测时对障碍物及机械臂简化为线条的示意图；图7为本发明实施例所公开的一种基于三维任务空间约束的机械臂避障方法中步骤五的算法流程图；图8为本发明实施例所公开的一种基于三维任务空间约束的机械臂避障方法中路径优化示意图。具体实施方式为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。实施例1如图1所示，一种基于三维任务空间约束的机械臂避障方法，所述方法包括：步骤s1：获取关节空间中机械臂运动路径的起始点qs和目标点qd；具体过程为：获取关节空间中机械臂运动路径的起始点qs和目标点qd属于现有技术过程，如图2所示，其主要原理是利用d-h法对带末端抓取工具的六自由度机械臂进行建模，在获取障碍物环境的基础之上对障碍物采用obb包围盒技术建立模型，给出笛卡尔空间的抓取起点位姿与终点位姿，图3所示，对六自由度机械臂进行建模，表1为机械臂的d-h参数表。表1d-h参数表iαi-1αi-1diθi100d1θ1(0)2-pi/20d2θ2(-pt/2)30a2d3θ3(0)40a3d4θ4(-pt/2)5-pi/20d5θ5(0)6-pi/200θ6(0)7(tool)00d70继续参阅图3，以上使用的d-h坐标系的建立原则为：(1)zi轴沿关节轴i的轴向；(2)原点oi为关节轴i+1与i轴的交点或其公垂线与关节轴zi的交点；(3)xi轴沿公垂线ai轴的公垂线方向，由关节轴i指向关节轴i+1，如果关节轴i和关节轴i+1相交，则规定xi轴垂直于这两条关节轴所在的平面；(4)yi轴按照右手定则确定；(5)当第一个关节变量为0时，规定坐标系{0}和{1}重合。对于坐标系{n}，其原点和xn轴方向可以任意选取。但在选取时，通常尽量使连杆参数为0。根据上述建模变换过程，可将坐标系i-1变换到坐标系i，如下公式(1)所示：根据公式(1)计算各个连杆坐标系之间的变换矩阵，并将所有变换矩阵进行连乘就能够得到机械臂正运动学计算式，也就是末端坐标系{n}相对于机械臂基系{0}的变换矩阵，如下公式(2)所示：根据机械臂控制器的关节空间拖动示教功能，示教出两组不会与自身及环境障碍物发生碰撞的关节角，并根据上述正运动学分析过程计算得到两组变换矩阵，并将其作为避障拾取的初始及目标位姿，初始化机械臂的工作环境，主要是障碍物的位置。通过上述建立的d-h参数表，通过六轴解析逆运动学算法计算得到关节空间中机械臂运动路径的起始点qs与目标点qd。确定合适的搜索步长segmenlenth与衔接误差disth，同时给定最大迭代次数maxfailedattempts；本实例中，关节空间为六维，要对机械臂的六个关节进行搜索，搜索范围为各关节的运动范围，起始位姿为[-300,800,100,-0.175,0.262,0],前三个元素为笛卡尔空间的位置，后三个元素为笛卡尔空间的姿态；终点位姿为[800,100,100,0.175,-0.175,0.262]，进而运动学逆解择优计算得到起始点qs为[-1.037,-2.296,-1.200,2.099,-1.769,-0.726]，目标点qd为[-2.888,-2.119,-1.546,2.289,-1.818,1.491]，搜索步长segmentlength＝0.1，衔接误差disth＝0.01，最大迭代次数maxfailedattempts＝100。步骤s2：双向rrt搜索树包括搜索树rrtree1和搜索树rrtree2，初始化搜索树rrtree1的树根节点为起始点qs，搜索树rrtree2的树根节点为目标点qd，对搜索树rrtree1进行扩展得到当前扩展关节点；具体过程为：搜索时，先对搜索树rrtree1进行扩展，在关节空间范围内以概率α向目标点采样，以1-α在整个六维空间内随机采样，在关节空间范围内通过公式获取随机点，其中，rand是一个0～1的随机值，α是给定的0～1之间的值，random(χ)表示生成一个小于常数χ的随机数；在搜索树rrtree1的树节点中找到与随机点最近的树节点closestnode_1；从与随机点最近的树节点closestnode_1出发通过公式向着随机点扩展一个步长得到当前扩展关节点new_point_temp，其中，segmentlength表示搜索步长。传统的双向rrt搜索树在找到与随机点最近的树节点closestnode_1时就开始判断closestnode_1是否发生碰撞，然后接着扩展搜索树rrtree2，直到两棵树连接就找到路径。但是这个路径随机性太强，虽然能够搜索成功，但是路径不优且机械臂大概率会以奇怪的构型运动。本发明的方法通过在双向rrt搜索扩展过程中施加三维任务空间约束，约束机械臂末端在指定的范围内运动，从而得到更加优异平滑的运动规划路径。步骤s3：依据设定的三维任务空间约束对当前扩展关节点new_point_temp进行约束更新得到更新后的扩展关节点；具体过程为：利用给定约束矩阵获取给定的三维任务空间约束范围；其中，约束矩阵c的前三行表示约束的位置范围，后三行表示约束的姿态范围；结合公式(1)和(2)，根据更新前扩展的起始点qs进行正运动学运算，可以计算得到机械臂末端工具坐标系的齐次变换矩阵定义其为末端工具坐标系的参考变换，表示其相对于机械臂基系的齐次变换。另外，定义约束坐标系，约束坐标系通常是惯性系，可以是静止的(例如门的铰链)，也可以根据对象的姿势改变，结合公式(1)和(2)通过正运动学运算得到三维任务空间约束范围的参考变换根据公式获取机械臂末端工具相对于约束坐标系下的位姿矩阵，其中，表示机械臂末端工具坐标系的齐次变换矩阵，表示三维任务空间约束范围的参考变换；其中，正运动学运算属于本领域常规技术，在此不做赘述。将机械臂末端工具相对于约束坐标系下的位姿矩阵通过转换为一个6维的位移矢量，其中，表示机械臂末端工具相对于约束坐标系的位移，后三行表示末端工具相对于约束坐标系的姿态，考虑给给定约束矩阵c的边界，利用公式得到该约束下的笛卡尔空间位移δx，cmax表示约束矩阵c的上边界，cmin表示约束矩阵c的下边界；通过公式δq＝jt(jjt)-1δx(9)将笛卡尔空间位移δx投影到关节空间得到关节空间位移δq，其中，j表示雅克比矩阵；利用公式new_point_temp’＝new_point_temp-δq对当前扩展关节点new_point_temp进行更新得到更新后的扩展关节点new_point_temp’。步骤s4：得到更新后的扩展关节点后，进行环境障碍物及自碰撞检测并且对双向rrt搜索树进行扩展，提取出一条从起始点qs到达目标点qd的无碰撞原始路径；具体过程为：如图4所示，得到更新后的扩展关节点new_point_temp’后，将其与其父节点之间形成的路径进行环境障碍物及自碰撞检测，如果发生碰撞则判断是否为搜索树rrtree2，如果没有发生碰撞，将更新后的扩展关节点new_point_temp’收录到搜索树rrtree1中，然后判断更新后的扩展关节点new_point_temp’与当前随机点sample之间的欧式距离是否小于衔接误差disth，若否则返回执行步骤s2至步骤s4直到欧式距离小于衔接误差disth，小于衔接误差disth后，判断是否为搜索树rrtree2；如果不是搜索树rrtree2，则将更新后的扩展关节点new_point_temp’作为搜索树rrtree1中的一个安全构型new_point_1，在搜索树rrtree2的树节点中找到与new_point_1最近的树节点closestnode_2，从该节点出发朝着安全构型new_point_1按设定步长前进，并返回步骤s3至步骤s4依据设定的三维任务空间约束对扩展后的点进行约束更新；qs_target表示扩展方向。如果是搜索树rrtree2，则将更新后的扩展关节点new_point_temp’作为搜索树rrtree2中的一个安全构型new_point_2，判断安全构型new_point_2与安全构型new_point_1之间的欧式距离是否小于衔接误差disth，如果安全构型new_point_1与安全构型new_point_2之间的欧式距离小于衔接误差disth，则说明搜索树rrtree1与搜索树rrtree2连到一起，由此提取出一条起始点qs到达目标点qd的无碰撞原始路径path，如果安全构型new_point_1与安全构型new_point_2之间的欧式距离大于等于衔接误差disth，则交换搜索树，返回执行步骤s2至步骤s4。上述环境障碍物及自碰撞检测为现有技术过程，其主要原理为：对障碍物使用轴向包围盒进行包络，对机械臂以及末端工具连杆使用圆柱体进行包络，根据obb包围盒算法判断轴向包围盒是否与机械臂任意圆柱体连杆相交来判断是否发生碰撞。首先将机械臂以及末端工具连杆使用圆柱体进行包络，讨论每根连杆圆柱轴线的位置关系，简化来看即为空间两条异面线段之间的位置关系，根据任意两条空间异面线段之间最短距离是否小于对应两根连杆圆柱半径之和来判断是否发生碰撞；在本实例中，障碍物碰撞检测与机械臂连杆自碰撞检测的示意图如图5和图6所示，障碍物使用轴向包围盒2，连杆i用圆柱体1进行包络。连杆i半径为ri，碰撞检测时，将连杆的半径补偿至包围盒的长宽高上，则机械臂的连杆此时可以描述成由两点构成的线段，可以将线段进行离散化，然后判断离散化后的各个点是否在包围盒构成的区域r内，如果在区域r内则线段与障碍物发生碰撞，否则不发生碰撞；机械臂的自碰撞问题简化来看，就是将机械臂的各个连杆简化成圆柱体，讨论每根连杆圆柱轴线之间的位置关系。假设d为任意两根连杆圆柱轴线之间的距离，如果d<2ri，表示发生了自碰撞，否则不发生碰撞。步骤s5：对原始路径进行优化得到优化的路径；如图7和图8所示，具体过程为：步骤501：在原始路径path中随机提取树节点i和树节点j，原始路径path中从树节点i到树节点j的路径为子路径path_ij；步骤502：从树节点i开始，依照给定的三维任务空间约束利用公式new_path_ij＝path_ij+δq更新子路径path_ij得到更新后的子路径new_path_ij；步骤503：判断子路径path_ij的长度是否小于更新后的子路径new_path_ij，若是，则将更新后的子路径new_path_ij替换子路径path_ij得到更新后的路径new_path，将更新后的路径new_path作为原始路径返回执行步骤501至步骤503，若否，则返回执行步骤501至步骤503，达到预设的迭代次数以后输出最终优化的路径。步骤s6：根据优化后的路径，移动六轴机械臂从起始点qs无碰撞地运动到目标点qd。通过以上技术方案，本发明提供的一种基于三维任务空间约束的机械臂避障装置，通过在双向rrt搜索扩展过程中施加三维任务空间约束，约束机械臂末端在指定的范围内运动，从而得到更加优异平滑的运动规划路径，快速有效的实现多轴机械臂末端轨迹路径规划并对原始路径进行优化得到优化的路径，择优后路径具有可实施性，使得多轴机械臂具有自主规划能力，最终机械臂笛卡尔构型符合机器人运动规律。实施例2与本发明实施例1相对应的，本发明实施例2还提供一种基于三维任务空间约束的机械臂避障装置，所述装置包括：起始点和目标点获取模块，用于获取关节空间中机械臂运动路径的起始点qs和目标点qd；初始化模块，用于双向rrt搜索树包括搜索树rrtree1和搜索树rrtree2，初始化搜索树rrtree1的树根节点为起始点qs，搜索树rrtree2的树根节点为目标点qd，对搜索树rrtree1进行扩展得到当前扩展关节点；更新模块，用于依据设定的三维任务空间约束对当前扩展关节点new_point_temp进行约束更新得到更新后的扩展关节点；原始路径获取模块，用于得到更新后的扩展关节点后，进行环境障碍物及自碰撞检测并且对双向rrt搜索树进行扩展，提取出一条从起始点qs到达目标点qd的无碰撞原始路径；优化模块，用于对原始路径进行优化得到优化的路径；控制模块，用于根据优化后的路径，移动六轴机械臂从起始点qs无碰撞地运动到目标点qd。具体的，所述初始化模块还用于：搜索时，先对搜索树rrtree1进行扩展，在关节空间范围内通过公式获取随机点，其中，rand是一个0～1的随机值，α是给定的0～1之间的值，random(χ)表示生成一个小于常数χ的随机数；在搜索树rrtree1的树节点中找到与随机点最近的树节点closestnode_1，从与随机点最近的树节点closestnode_1出发通过公式向着随机点扩展一个步长得到当前扩展关节点new_point_temp，其中，segmentlength表示搜索步长。具体的，所述更新模块还用于：利用给定约束矩阵获取给定的三维任务空间约束范围；根据公式获取机械臂末端工具相对于约束坐标系下的位姿矩阵，其中，表示机械臂末端工具坐标系的齐次变换矩阵，表示三维任务空间约束范围的参考变换；将机械臂末端工具相对于约束坐标系下的位姿矩阵通过转换为一个6维的位移矢量，其中，表示机械臂末端工具相对于约束坐标系的位移，后三行表示末端工具相对于约束坐标系的姿态，考虑给给定约束矩阵c的边界，利用公式得到该约束下的笛卡尔空间位移δx，cmax表示约束矩阵c的上边界，cmin表示约束矩阵c的下边界；通过公式δq＝jt(jjt)-1δx将笛卡尔空间位移δx投影到关节空间得到关节空间位移δq，其中，j表示雅克比矩阵；利用公式new_point_temp’＝new_point_temp-δq对当前扩展关节点new_point_temp进行更新得到更新后的扩展关节点new_point_temp’。具体的，所述原始路径获取模块还用于：得到更新后的扩展关节点new_point_temp’后，将其与其父节点之间形成的路径进行环境障碍物及自碰撞检测，如果发生碰撞则将更新后的扩展关节点new_point_temp’作为搜索树rrtree1中的一个安全构型new_point_1；如果没有发生碰撞，将更新后的扩展关节点new_point_temp’收录到搜索树rrtree1中，然后判断更新后的扩展关节点new_point_temp’与当前随机点之间的欧式距离是否小于衔接误差disth，若否则返回执行初始化模块、更新模块以及原始路径获取模块直到欧式距离小于衔接误差disth，小于衔接误差disth后，交换搜索树rrtree2进行扩展，搜索树rrtree2的扩展方向是搜索树rrtree1扩展得到的安全构型new_point_1；在搜索树rrtree2中找到距离安全构型new_point_1最近的节点closestnode_2，从该节点出发朝着安全构型new_point_1按设定步长前进，并依据设定的三维任务空间约束对扩展后的点进行约束更新，得到满足约束且不与环境及自身发生碰撞的安全构型new_point_2，一旦扩展过程中发生碰撞，则交换树进行扩展，如此循环，如果安全构型new_point_1与安全构型new_point_2之间的欧式距离小于衔接误差disth，则说明搜索树rrtree1与搜索树rrtree2连到一起，由此提取出一条起始点qs到达目标点qd的无碰撞原始路径path。具体的，所述优化模块还用于：步骤501：在原始路径path中随机提取树节点i和树节点j，原始路径path中从树节点i到树节点j的路径为子路径path_ij；步骤502：从树节点i开始，依照给定的三维任务空间约束利用公式new_path_ij＝path_ij+δq更新子路径path_ij得到更新后的子路径new_path_ij；步骤503：判断子路径path_ij的长度是否小于更新后的子路径new_path_ij，若是，则将更新后的子路径new_path_ij替换子路径path_ij得到更新后的路径new_path，将更新后的路径new_path作为原始路径返回执行步骤501至步骤503，若否，则返回执行步骤501至步骤503，达到预设的迭代次数以后输出最终优化的路径。以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。当前第1页1 2 3 

起点商标作为专业知识产权交易平台，可以帮助大家解决很多问题，如果大家想要了解更多知产交易信息请点击 【在线咨询】或添加微信 【19522093243】与客服一对一沟通，为大家解决相关问题。

此文章来源于网络,如有侵权,请联系删除