CN113771035B - 基于rrt*算法的冗余多自由度机械臂避障路径优化方法 - Google Patents
基于rrt*算法的冗余多自由度机械臂避障路径优化方法 Download PDFInfo
- Publication number
- CN113771035B CN113771035B CN202111118698.3A CN202111118698A CN113771035B CN 113771035 B CN113771035 B CN 113771035B CN 202111118698 A CN202111118698 A CN 202111118698A CN 113771035 B CN113771035 B CN 113771035B
- Authority
- CN
- China
- Prior art keywords
- path
- obstacle avoidance
- point
- angle
- mechanical arm
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Numerical Control (AREA)
Abstract
本发明公开了一种基于RRT*算法的冗余多自由度机械臂避障路径优化方法。本方法针对采用RRT*算法规划出的冗余多自由度机械臂的避障路径存在的路径折角过多和相邻路径折角过小等不利于机械臂运动的因素,首先通过判断两段相邻路径的夹角是否小于角度阈值来判断两段路径的连接点是否为折角过小的路径点;若该路径点属于折角过小点,则通过分别在相邻两段路径上距离折角点三分之一路径长度的位置各新增一个节点,去掉原来的折角点,然后直接连接新增的两个节点的方法,通过此方法可以去除避障路径上所有的折角过小的折角点,有效地解决避障路径上折角过小问题,实现了对避障路径的平滑优化,使得机械臂末端在运行时平稳、顺滑。
Description
技术领域
本发明涉及机械臂路径规划领域,具体是一种基于RRT*算法的冗余多自由度机械臂避障路径优化方法。
背景技术
RRT*算法是一种基于RRT算法改进的渐进最优算法,其所找到的路径是被理论验证的正确的算法。采用RRT*(即改进RRT,改进的快速扩展随机树)及基于RRT*改进的相关算法,由于此类算法本身随机性大的问题,其规划出的避障路径通常存在折角过小问题。同时对于冗余机械臂而言,由于其逆解有无数组的特点,将算法应用于冗余自由度机械臂避障规划时,规划出的路径通常会存在路径曲折,即相邻两段路径存在折角过小问题,这非常不利于机械臂末端的实际运行。
发明内容
针对现有技术的不足,本发明拟解决的技术问题是,提供一种基于RRT*算法的冗余多自由度机械臂避障路径优化方法。
本发明解决所述技术问题的技术方案是,提供一种基于RRT*算法的冗余多自由度机械臂避障路径优化方法,其特征在于,该方法包括以下步骤:
步骤1、利用RRT*算法生成冗余多自由度机械臂的原始避障路径X(x1、x2、…、xi…、xn),其中xi为原始避障路径X在三维空间中的路径点;
步骤2、在原始避障路径X中,由依次的xi-1、xi和xi+1三个路径点形成两段相邻路径,判断两段相邻路径所形成的折角点xi处的折角角度αxi与角度阈值αlimt的关系;
当折角角度αxi不小于角度阈值αlimt时,将路径点xi-1和xi保存到初步平滑避障路径Xnew中;下一次判断中,第一段路径由xi和xi+1组成;
当折角角度αxi小于角度阈值αlimt时,设向量的模长为l1,向量的模长为l2;在路径的距离折角点xi的1/3l1长度的位置新增一个路径点xi-1,i,在路径的距离折角点xi的1/3l2长度的位置新增一个路径点xi,i+1;用两个新增路径点代替原来的折角点xi,并将xi-1、xi-1,i和xi,i+1三个路径点保存到初步平滑避障路径Xnew中;下一次判断中,第一段路径由xi,i+1和xi+1组成;
所述角度阈值αlimt由实际情况决定;
步骤3、判断路径点xi+1是否为原始避障路径X的倒数第二个路径点xn-1;如果是,则得到初步平滑避障路径Xnew;如果不是,则在下一个判断中,对折角点xi+1重复步骤2,直至对原始避障路径X中的倒数第二个路径点xn-1重复步骤2,进而得到初步平滑避障路径Xnew;
步骤4、将初步平滑避障路径Xnew进行改进B样条曲线拟合,得到平滑避障路径。
与现有技术相比,本发明有益效果在于:
(1)本发明针对采用RRT*算法规划出的冗余多自由度机械臂的避障路径存在的路径折角过多和相邻路径折角过小等不利于机械臂运动的因素,首先通过判断两段相邻路径的夹角是否小于角度阈值来判断两段路径的连接点是否为折角过小的路径点;若该路径点属于折角过小点,则通过分别在相邻两段路径上距离折角点三分之一路径长度的位置各新增一个节点,去掉原来的折角点,然后直接连接新增的两个节点的方法,通过此方法可以去除避障路径上所有的折角过小的折角点,有效地解决避障路径上折角过小问题,实现了对避障路径的平滑优化,使得机械臂末端在运行时平稳、顺滑,同时为后续的路径进一步优化提供了良好的基础。
(2)本发明新增路径点的位置是距离折角点三分之一路径长度,主要考虑均匀性问题,如果相邻的三段路径形成的两个折角都不满足角度阈值条件,那么中间的路径取其原长的三分之一不至于使得该段路径过长或者过短,过短优化效果不明显,过长可能会使得机械臂在运行时与障碍物碰撞。
附图说明
图1为本发明的优化方法流程图;
图2为本发明的路径平滑原理图;
图3为本发明实施例1的空间障碍物模型及基于RRT*算法生成的无碰撞原始避障路径图;
图4为本发明实施例1的原始避障路径X图;图中,三角形代表的点为需要去除的折角点;
图5为本发明实施例1的初步平滑避障路径Xnew图;图中,圆形代表的点为新增的路径点;
图6为本发明实施例1的原始避障路径X和初步平滑避障路径Xnew经过改进B样条曲线拟合后生成的路径的对比图。
具体实施方式
下面给出本发明的一个具体实例。具体实施例仅用于进一步详细说明本发明,不限制本申请权利要求的保护范围。
本发明提供了一种基于RRT*算法的冗余多自由度机械臂避障路径优化方法(简称方法),其特征在于,该方法包括以下步骤:
步骤1、利用RRT*算法生成冗余多自由度机械臂的从起点到终点的无碰撞原始避障路径X(x1、x2、…、xi…、xn),其中xi为原始避障路径X在三维空间中的路径点;
步骤2、在原始避障路径X中,由依次的xi-1、xi和xi+1三个路径点形成两段相邻路径,判断两段相邻路径所形成的折角点xi处的折角角度αxi与角度阈值αlimt的关系;
当折角角度αxi不小于角度阈值αlimt时,保留两段路径中包含的路径点xi-1和xi并将路径点xi-1和xi保存到初步平滑避障路径Xnew中;下一次判断中,第一段路径由xi和xi+1组成;
当折角角度αxi小于角度阈值αlimt时,设向量的模长为l1,向量的模长为l2;在路径的距离折角点xi的1/3l1长度的位置新增一个路径点xi-1,i,在路径的距离折角点xi的1/3l2长度的位置新增一个路径点xi,i+1;用两个新增路径点代替原来的折角点xi,并将xi-1、xi-1,i和xi,i+1三个路径点保存到初步平滑避障路径Xnew中;下一次判断中,第一段路径由xi,i+1和xi+1组成;
所述角度阈值αlimt由实际情况决定;
优选地,步骤2中,所述角度阈值αlimt如此确定:原始避障路径X中存在的n个路径点形成的n-1段路径,n-1段路径形成n-2个折角,则角度阈值αlimt取n-2个折角角度的平均值。
步骤3、判断路径点xi+1是否为原始避障路径X的倒数第二个路径点xn-1;如果是,则得到初步平滑避障路径Xnew;如果不是,则在下一个判断中,对折角点xi+1重复步骤2,直至对原始避障路径X中的倒数第二个路径点xn-1重复步骤2,进而得到初步平滑避障路径Xnew;
步骤4、将初步平滑避障路径Xnew进行改进B样条曲线拟合,得到平滑避障路径。
优选地,步骤4中,改进B样条曲线拟合采用文献《董甲甲,王太勇,董靖川,张永宾,陶浩.改进B样条曲线应用于6R机器人轨迹优化[J].中国机械工程,2018,29(02):193-200.》中所述的方法。
实施例1
本实施例中,利用本方法优化基于RRT*算法规划出的七自由度机械臂避障路径,包括以下步骤:
步骤1、初始化参数和生成路径;
(1)根据D-H法建立连杆坐标系,进而建立七自由度机械臂的运动学模型;
表1D-H参数表
(2)由D-H法建立的连杆坐标系的齐次变换矩阵为:
上式中,Ri-1为旋转矩阵,Ti-1为平移矩阵,cθi为cosθi,cαi-1为cosαi-1,sθi为sinθi,sαi-1为sinαi-1,αi-1为连杆转角,ai-1为连杆长度,di为连杆偏距,θi为关节角;
由于七自由度机械臂的冗余性,使得其逆解有无数组;由于几何法求逆解的精度高并且计算效率高,所以本发明采用改变臂型角的求逆解方法,选取其中角度变化最小的一组,同时需要满足关节角约束;
优选地,所述改变臂型角的求逆解方法采用文献《高春艳,唐家豪,吕晓玲,张明路.一种冗余机械臂的简化运动学求解方法[J/OL].机械科学与技术:1-5[2021-09-23].》中公开的方法。
(3)初始化环境参数:机械臂末端在工作空间中的起点位姿是start[1 0 0 100;01 0 300;0 0 1 200;0 0 0 1],终点位姿是goal[1 0 0 551;0 1 0 563;0 0 1 500;0 0 01];四个障碍物的模型和位置分别是obs1(391,376,300)、obs2(300,450,550)、obs3(300,600,400)和obs4(300,450,400),边长分别为120mm、100mm、100mm和100mm;碰撞检测方法采用基于OBB包围盒分离轴的碰撞检测;
(4)利用RRT*算法生成七自由度机械臂的从起点到终点的无碰撞原始避障路径X(x1、x2、…、xi…、xn),如图3所示,路径由一系列路径点组成,其中xi为原始避障路径X在三维空间中的路径点,具体过程是:首先,以起点start为随机树的根节点,初始化随机树。然后在采样空间中生成随机采样点nrand,遍历随机树,找到距离nrand最近的点nnear。以nnear为起点,沿着nrand方向生成新节点nnew。新节点nnew是否无碰撞,若无碰撞则继续进行以下过程,若碰撞则返回重新采样。当nnew无碰撞时,检测是否存在逆解,若存在则继续进行以下过程,若不存在则返回重新采样。当nnew无碰撞且有逆解时,检测机械臂的连杆与障碍物是否发生碰撞,若无碰撞则继续进行以下过程,若碰撞则返回重新采样。当满足以上条件时,遍历随机树,找到nnew的邻域内代价最小节点nnearest作为nnew的父节点。判断nnew是否到达目标点邻域,若到达则按照父节点的顺序,找到一条从起点到终点的原始避障路径X,若未到达则返回重新采样,执行以上过程。
步骤2、在原始避障路径X中,由依次的xi-1、xi和xi+1三个路径点形成两段相邻路径,向量代表第一段路径,向量代表第二段路径,xi为两段相邻路径所形成的折角点;向量与向量之间的夹角αxi代表两段相邻路径所形成的折角点xi处的折角角度;
当折角角度αxi不小于角度阈值αlimt时,保留两段路径中包含的路径点xi-1和xi并将路径点xi-1和xi保存到初步平滑避障路径Xnew中;下一段路径由xi和xi+1组成;
当折角角度αxi小于角度阈值αlimt时,设向量的模长为l1,向量的模长为l2;在路径的距离折角点xi的1/3l1长度的位置新增一个路径点xi-1,i,在路径的距离折角点xi的1/3l2长度的位置新增一个路径点xi,i+1;用两个新增路径点代替原来的折角点xi,并将xi-1、xi-1,i和xi,i+1三个路径点保存到初步平滑避障路径Xnew中;下一段路径由xi,i+1和xi+1组成;
本实施例中,将原始避障路径X所形成的28个路径转角取平均值,得到角度阈值αlimt为48.3°;
步骤3、判断路径点xi+1是否为倒数第二个路径点xn-1;如果是,则得到初步平滑避障路径Xnew;如果不是,则对原始避障路径X中的下一个折角点xi+1重复步骤2,直至对原始避障路径X中的倒数第二个路径点xn-1重复步骤2,进而得到初步平滑避障路径Xnew;
步骤4、将初步平滑避障路径Xnew进行改进B样条曲线拟合,得到平滑避障路径。
由图4和图5对比可以看出,图5去掉了图4中折角过小的折角点,并用两个新增路径点代替需要去掉的对应的一个折角点,通过新增的路径点使得路径变得顺滑。同时由于基于RRT*算法生成的路径步长本身较小,所以本发明方法取距离折角点1/3路径长度的位置处增加路径点不会导致机械臂与障碍物发生碰撞,所以平滑后的路径也是无碰撞路径。
图6中,点划线表示将原始避障路径X进行改进B样条曲线拟合后生成的路径,实线表示将初步平滑避障路径Xnew进行改进B样条曲线拟合后得到平滑避障路径。由图6可以看出,本发明得到的平滑避障路径去掉了由RRT*算法规划出来的路径中的折角过小的节点,使得路径更加平滑,符合机械臂实际的实际运动路径,保证机械臂末端的平稳运行。
本发明未述及之处适用于现有技术。
Claims (4)
1.一种基于RRT*算法的冗余多自由度机械臂避障路径优化方法,其特征在于,该方法包括以下步骤:
步骤1、利用RRT*算法生成冗余多自由度机械臂的原始避障路径X(x1、x2、…、xi…、xn),其中xi为原始避障路径X在三维空间中的路径点;
步骤2、在原始避障路径X中,由依次的xi-1、xi和xi+1三个路径点形成两段相邻路径,判断两段相邻路径所形成的折角点xi处的折角角度αxi与角度阈值αlimt的关系;
当折角角度αxi不小于角度阈值αlimt时,将路径点xi-1和xi保存到初步平滑避障路径Xnew中;下一次判断中,第一段路径由xi和xi+1组成;
当折角角度αxi小于角度阈值αlimt时,设向量的模长为l1,向量的模长为l2;在路径的距离折角点xi的1/3l1长度的位置新增一个路径点xi-1,i,在路径的距离折角点xi的1/3l2长度的位置新增一个路径点xi,i+1;用两个新增路径点代替原来的折角点xi,并将xi-1、xi-1,i和xi,i+1三个路径点保存到初步平滑避障路径Xnew中;下一次判断中,第一段路径由xi,i+1和xi+1组成;
所述角度阈值αlimt由实际情况决定;
步骤3、判断路径点xi+1是否为原始避障路径X的倒数第二个路径点xn-1;如果是,则得到初步平滑避障路径Xnew;如果不是,则在下一个判断中,对折角点xi+1重复步骤2,直至对原始避障路径X中的倒数第二个路径点xn-1重复步骤2,进而得到初步平滑避障路径Xnew;
步骤4、将初步平滑避障路径Xnew进行改进B样条曲线拟合,得到平滑避障路径。
2.根据权利要求1所述的基于RRT*算法的冗余多自由度机械臂避障路径优化方法,其特征在于,步骤2中,原始避障路径X中存在的n个路径点形成的n-1段路径,n-1段路径形成n-2个折角,则角度阈值αlimt取n-2个折角角度的平均值。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111118698.3A CN113771035B (zh) | 2021-09-24 | 2021-09-24 | 基于rrt*算法的冗余多自由度机械臂避障路径优化方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111118698.3A CN113771035B (zh) | 2021-09-24 | 2021-09-24 | 基于rrt*算法的冗余多自由度机械臂避障路径优化方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113771035A CN113771035A (zh) | 2021-12-10 |
CN113771035B true CN113771035B (zh) | 2022-10-18 |
Family
ID=78852975
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111118698.3A Active CN113771035B (zh) | 2021-09-24 | 2021-09-24 | 基于rrt*算法的冗余多自由度机械臂避障路径优化方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113771035B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114406559A (zh) * | 2022-01-26 | 2022-04-29 | 中国铁建电气化局集团有限公司 | 一种用于自动焊接设备的机械臂回零装置以及方法 |
CN114939872B (zh) * | 2022-06-13 | 2023-08-25 | 合肥工业大学 | 基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法 |
Family Cites Families (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106695802A (zh) * | 2017-03-19 | 2017-05-24 | 北京工业大学 | 基于多自由度机械臂的改进式rrt*的避障运动规划方法 |
US11919170B2 (en) * | 2019-12-13 | 2024-03-05 | Edda Technology, Inc. | Fast method for robot path planning with obstacle avoidance |
CN110919661B (zh) * | 2019-12-26 | 2022-11-22 | 中国科学院沈阳自动化研究所 | 一种手套箱封闭空间内机械臂的运动规划方法 |
CN112356033B (zh) * | 2020-11-09 | 2021-09-10 | 中国矿业大学 | 一种融合低差异序列与rrt算法的机械臂路径规划方法 |
CN113296496B (zh) * | 2021-04-05 | 2023-06-02 | 青岛科技大学 | 基于多采样点的引力自适应步长双向rrt路径规划方法 |
CN113084811B (zh) * | 2021-04-12 | 2022-12-13 | 贵州大学 | 一种机械臂路径规划方法 |
CN113341984A (zh) * | 2021-06-15 | 2021-09-03 | 桂林电子科技大学 | 基于改进rrt算法的机器人路径规划方法和装置 |
-
2021
- 2021-09-24 CN CN202111118698.3A patent/CN113771035B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN113771035A (zh) | 2021-12-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113771035B (zh) | 基于rrt*算法的冗余多自由度机械臂避障路径优化方法 | |
CN110509279B (zh) | 一种仿人机械臂的运动路径规划方法及*** | |
CN110228069B (zh) | 一种机械臂在线避障运动规划方法 | |
CN108356819B (zh) | 基于改进a*算法的工业机械臂无碰撞路径规划方法 | |
CN109514133B (zh) | 一种基于线结构光感知的焊接机器人3d曲线焊缝自主示教方法 | |
CN111546347B (zh) | 一种适用于动态环境下的机械臂路径规划方法 | |
CN105773620B (zh) | 基于倍四元数的工业机器人自由曲线的轨迹规划控制方法 | |
CN112223291B (zh) | 一种基于三维任务空间约束的机械臂避障方法及装置 | |
CN101612734B (zh) | 管道喷涂机器人及其作业轨迹规划方法 | |
CN112356033B (zh) | 一种融合低差异序列与rrt算法的机械臂路径规划方法 | |
CN109397271B (zh) | 一种7自由度拟人机械臂及其控制方法和*** | |
CN108638055B (zh) | 一种七自由度空间机械臂自主避障规划方法 | |
CN109976347B (zh) | 一种基于快速扩展随机树和势场法的视觉伺服路径规划方法 | |
CN111251306B (zh) | 一种带有底盘误差的机械臂路径规划方法 | |
CN107953334A (zh) | 一种基于a星算法的工业机械臂无碰撞路径规划方法 | |
CN110275528B (zh) | 针对rrt算法改进的路径优化方法 | |
CN111300420B (zh) | 一种机械臂关节空间转角最小路径求取方法 | |
CN111761582B (zh) | 一种基于随机采样的移动机械臂避障规划方法 | |
WO2023024317A1 (zh) | 一种机器人避障方法、装置和机器人 | |
CN114633258B (zh) | 一种隧道环境下机械臂运动轨迹的规划方法及相关装置 | |
CN110609547A (zh) | 一种基于可视图引导的移动机器人规划方法 | |
CN110744543A (zh) | 基于ur3机械臂的改进式prm避障运动规划方法 | |
CN113799141B (zh) | 六自由度机械臂避障路径规划方法 | |
CN114939872B (zh) | 基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法 | |
CN116061173A (zh) | 一种带电作业的机械臂六自由度冗余任务轨迹规划方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |