CN116652932A - 一种基于改进式rrt*算法的机械臂自主避障方法 - Google Patents
一种基于改进式rrt*算法的机械臂自主避障方法 Download PDFInfo
- Publication number
- CN116652932A CN116652932A CN202310403611.XA CN202310403611A CN116652932A CN 116652932 A CN116652932 A CN 116652932A CN 202310403611 A CN202310403611 A CN 202310403611A CN 116652932 A CN116652932 A CN 116652932A
- Authority
- CN
- China
- Prior art keywords
- new
- node
- path
- nodes
- point
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 37
- 238000005070 sampling Methods 0.000 claims abstract description 15
- 238000013138 pruning Methods 0.000 claims abstract description 10
- 238000009499 grossing Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 5
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
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
- B25J9/1666—Avoiding collision or forbidden zones
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J18/00—Arms
-
- 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/1674—Programme controls characterised by safety, monitoring, diagnostic
- B25J9/1676—Avoiding collision or forbidden zones
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Feedback Control In General (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种基于改进式RRT*算法的机械臂自主避障方法，属于智能机器人路径规划技术领域，包括：对变量进行初始化；生成随机采样点，寻找距离随机点最近的节点，利用动态步长产生新节点，并用连线连接近邻点和新节点；重新选择父节点和重布线随机树；判断各个连线是否与空间障碍物发生碰撞；判断新节点是否进入目标点区域；对已生成的路径利用剪枝策略进行优化。该方法在传统算法中改进了新节点的生长方式，利用调整新节点的生长方向进行实现；后对已生成的路径利用剪枝策略进行优化，使路径长度更短；最后是对于应用到机械臂上面时，将三次B样条曲线函数与得到的路径进行拟合，可以使路径更加平滑，使机械臂满足避障且平稳地达到目标点。
Description
技术领域
本发明属于智能机器人路径规划技术领域，具体涉及一种基于改进式RRT*算法的机械臂自主避障方法。
背景技术
随着科技不断发展，机器人智能化程度越来越高，可以越来越多地代替人类完成一些工作，机械臂是一种非常常见的机器人类型。在工业生产实际应用过程中，需要机械臂实现灵活的任务层面的操纵，避开各种障碍物，顺利到达指定位置。这就要求机械臂必须具有自主、高效的路径规划能力，能够在复杂的障碍物环境中快速的选择出一条从起始位置到目标位置的无碰撞路径。
RRT算法是一种基于随机采样的算法，它的特性就是会向还未采样的区域进行扩展，一直到采样结束或寻到路径。这种算法具有在高维度中较高效的搜索能力，利用状态空间中的任意采样点将搜寻引至空白区域，由此找到一条由起始点至目标点的规划路线。该算法利用所在区域内的随机点进行碰撞检测，不需要建立环境模型，可以有效地处理具有较高维度和较多限制条件下的轨迹规划问题。与其它规划算法相比，RRT搜索算法不仅非常适用于解决高维问题，而且采样过程中也不依赖于精确的环境模型，这种优势使得RRT算法在近年来得到了高度的关注并且在机械臂的路径规划领域得到了广泛应用。S.Karaman等提出了一种基于RRT的渐进最优快速扩展随机树算法RRT*算法，在基本RRT算法的基础上加入了代价函数，重新为当前节点选择父节点、重布线随机树。RRT*算法规划出的路径较RRT算法有了优化，但仍存在路径冗余、路径过于曲折等问题。
发明内容
为了解决现有技术中存在的RRT*算法规划出的路径冗余、路径过于曲折等问题，本发明提供了一种基于改进式RRT*算法的机械臂自主避障方法，在传统RRT*算法中改进了新节点的生长方式，利用调整新节点的生长方向进行实现；之后则是对已生成的路径利用剪枝策略进行优化，使路径长度更短；最后是对于应用到机械臂上面时，将三次B样条曲线函数与得到的路径进行拟合，可以使路径更加平滑，使机械臂满足避障且平稳地达到目标点。
本发明通过如下技术方案实现：
一种基于改进式RRT*算法的机械臂自主避障方法，具体包括如下步骤：
步骤一：对变量进行初始化；
步骤二：生成随机采样点，寻找距离随机点最近的节点，利用动态步长产生新节点，并用连线连接近邻点Xnear和新节点Xnew；
步骤三：重新选择父节点和重布线随机树；
步骤四：判断步骤二和步骤三各个连线是否与空间障碍物发生碰撞；
若产生碰撞，则返回步骤二，并采用有碰撞的步长；若不发生碰撞，进行步骤五；
步骤五：判断新节点是否进入目标点区域；
若进入目标点邻域，则直接连线新节点xnew与目标点Xgoal，并生成到达目标点的路径，退出循环，进行步骤六；若未进入目标点邻域，则返回步骤二，直至找到到达目标点的路径或者达到最大迭代次数退出循环；
步骤六：对已生成的路径利用剪枝策略进行优化。
进一步地，步骤一中，所述变量包括起始点Xstart、目标点Xgoal、扩展步长expand_dis，最大迭代次数max_iter、空间障碍物信息obstacle_list、重计算时的圆半径radius及采样区域rand_area。
进一步地，步骤二中，具体包括如下内容：
A1、从起始点Xstart出发，产生随机树T，起始点Xstart为树的根节点，在采样区域生成随机点Xrand，在随机树T中寻找距离随机点最近的近邻点Xnear；
A2、沿着与/>合向量的方向以动态步长expand_dis生成新节点xnew，并将Xnew节点加入到随机树T中。
进一步地，步骤三中，具体包括如下内容：
在新产生的节点xnew附近以定义的半径范围内寻找近邻，作为替换xnew父节点的备选；依次计算近邻节点到起点的路径代价加上Xnew到每个近邻的路径代价，在原始父节点Xnear和近邻节点中选取使路径代价最小的节点作为新的父节点；然后，在为xnew重新选择父节点之后，为进一步使得随机树节点间连接的代价最小化，为随机树进行重新布线；将Xnew附近的近邻节点的父节点设置为Xnew，若该节点到起点的代价减小，则该节点的父节点更改为Xnew。
进一步地，步骤四中，在连线与障碍物未检测到碰撞时，ρ1＜ρ2，ρ1为随机点方向生长步长，ρ2为目标点方向生长步长；连线与障碍物检测到碰撞时，ρ1＞ρ2；其中，新产生的节点xnew，近邻点xnear，在采样区域生成随机点xrand，目标点Xgoal。
进一步地，步骤六具体包括如下内容：
上述有效节点构成的路径Path中，从第一个路径节点即Xstart开始，依次连接路径上的后序节点，若两个节点之间的路径和障碍物之间不发生碰撞，则删除两个节点之间的路径，并将这两个节点用直线连接；以此类推，直到发生碰撞，此时以碰撞点的父节点作为新起点，再次执行上述操作，如此递归进行，直至到达目标点。
进一步地，所述方法还包括：基于三次B样条平滑路径对剪枝后的路径平滑处理，使机器人在运行过程中更加柔顺。
与现有技术相比，本发明的优点如下：
本发明的一种基于改进式RRT*算法的机械臂自主避障方法，在传统RRT*算法中改进了新节点的生长方式，利用调整新节点的生长方向进行实现；之后则是对已生成的路径利用剪枝策略进行优化，使路径长度更短；最后是对于应用到机械臂上面时，将三次B样条曲线函数与得到的路径进行拟合，可以使路径更加平滑，使机械臂满足避障且平稳地达到目标点。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍。在所有附图中，类似的元件或部分一般由类似的附图标记标识。附图中，各元件或部分并不一定按照实际的比例绘制。
图1为本发明的一种基于改进式RRT*算法的机械臂自主避障方法的流程示意图；
图2为重新选择父节点的示意图；
图3为重布线随机树的示意图；
图4为剪枝策略的示意图。
具体实施方式
为清楚、完整地描述本发明所述技术方案及其具体工作过程，结合说明书附图，本发明的具体实施方式如下：
实施例1
如图1所示为本实施例的一种基于改进式RRT*算法的机械臂自主避障方法的流程示意图，所述方法包括如下步骤：
步骤一：对变量进行初始化，包含起始点Xstart、目标点Xgoal、扩展步长expand_dis，最大迭代次数max_iter、空间障碍物信息obstacle_list、重计算时的圆半径radius、采样区域rand_area；
步骤二：从起始点Xstart出发，产生随机树T，起始点Xstart为树的根节点，在采样区域生成随机点xrand，在树T中寻找距离随机点最近的近邻点Xnear；
步骤三：沿着与/>合向量的方向以动态步长expand_dis生成新节点Xnew，并将Xnew节点加入到树T中，并用连线连接Xnear和Xnew；
步骤四：在新产生的节点Xnew附近以定义的半径范围内寻找近邻，作为替换Xnew父节点的备选；依次计算近邻节点到起点的路径代价加上Xnew到每个近邻的路径代价，在原始父节点Xnear和近邻节点中选取使路径代价最小的节点作为新的父节点，重新选择父节点示意图如图2所示；然后，在为Xnew重新选择父节点之后，为进一步使得随机树节点间连接的代价尽量小，为随机树进行重新布线，重布线随机树的示意图如图3所示；将xnew附近的近邻节点的父节点设置为Xnew，若该节点到起点的代价减小，则将该节点的父节点更改为Xnew；
步骤五：判断前面各个步骤连线是否与空间障碍物发生碰撞；通过点到直线的距离计算公式计算障碍物中心到连线的距离，将距离与障碍物半径比较，从而判断以上步骤连线是否与空间障碍物发生碰撞，在连线与障碍物未检测到碰撞时，ρ1＜ρ2，ρ1为随机点方向生长步长，ρ2为目标点方向生长步长；在连线与障碍物检测到碰撞时，ρ1＞ρ2。若产生碰撞，则返回步骤二，当再次进入到步骤三时，采用有碰撞的步长；若不发生碰撞，进行步骤六。
步骤六：当生成新节点Xnew后，判断Xnew节点是否进入目标点邻域，若进入目标点邻域，则直接连线Xnew与Xgoal，生成到达目标点的路径，退出循环，进行步骤七；反之，返回步骤二，直至找到到达目标点的路径或者达到最大迭代次数退出循环；
步骤七：首先，通过上述的改进型RRT*算法可以求得一条从初始点到目标点由一系列有效节点构成的路径，然后从第一个路径节点(即Xstart)开始，依次连接路径上的后序节点，只要这两个节点之间的路径和障碍物之间不发生碰撞，那么，它们之间的路径节点即可删除，并将这两个节点用直线连接。以此类推，直到发生碰撞，此时以碰撞点的父节点作为新起点，再次执行上述操作，如此递归进行，直至到达目标点，剪枝策略示意图如图4所示。
步骤八：最后对所得的路径利用三次B样条平滑处理。B样条曲线的总方程为：其中Pi是控制曲线的特征点，Bi，k(u)则是K阶B样条基函数(k≥1),u为自变量；基函数满足如下递推关系：
三次B样条基函数为：
将三次B样条基函数代入到总方程得到三次B样条曲线方程P(u)＝P0B0,3(u)+P1B1,3(u)+P2B2,3(u)+P3B3,3(u)。
由于三次B样条曲线曲率连续，用各段三次B样条曲线将各节点进行平滑连接，得到一条完整且光滑的B样条曲线，形成最终路径。
以上结合附图详细描述了本发明的优选实施方式，但是，本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种简单变型，这些简单变型均属于本发明的保护范围。
另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合，为了避免不必要的重复，本发明对各种可能的组合方式不再另行说明。
此外，本发明的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明的思想，其同样应当视为本发明所公开的内容。
Claims (7)
1.一种基于改进式RRT*算法的机械臂自主避障方法，其特征在于，具体包括如下步骤：
步骤一：对变量进行初始化；
步骤二：生成随机采样点，寻找距离随机点最近的节点，利用动态步长产生新节点，并用连线连接近邻点Xnear和新节点Xnew；
步骤三：重新选择父节点和重布线随机树；
步骤四：判断步骤二和步骤三各个连线是否与空间障碍物发生碰撞；
若产生碰撞，则返回步骤二，并采用有碰撞的步长；若不发生碰撞，进行步骤五；
步骤五：判断新节点是否进入目标点区域；
若进入目标点邻域，则直接连线新节点Xnew与目标点Xgoal，并生成到达目标点的路径，退出循环，进行步骤六；若未进入目标点邻域，则返回步骤二，直至找到到达目标点的路径或者达到最大迭代次数退出循环；
步骤六：对已生成的路径利用剪枝策略进行优化。
2.如权利要求1所述的一种基于改进式RRT*算法的机械臂自主避障方法，其特征在于，步骤一中，所述变量包括起始点Xstart、目标点Xgoal、扩展步长expand_dis，最大迭代次数max_iter、空间障碍物信息obstacle_list、重计算时的圆半径radius及采样区域rand_area。
3.如权利要求1所述的一种基于改进式RRT*算法的机械臂自主避障方法，其特征在于，步骤二中，具体包括如下内容：
A1、从起始点Xstart出发，产生随机树T，起始点Xstart为树的根节点，在采样区域生成随机点Xrand，在随机树T中寻找距离随机点最近的近邻点Xnear；
A2、沿着与/>合向量的方向以动态步长expand_dis生成新节点Xnew，并将Xnew节点加入到随机树T中。
4.如权利要求1所述的一种基于改进式RRT*算法的机械臂自主避障方法，其特征在于，步骤三中，具体包括如下内容：
在新产生的节点Xnew附近以定义的半径范围内寻找近邻，作为替换Xnew父节点的备选；依次计算近邻节点到起点的路径代价加上Xnew到每个近邻的路径代价，在原始父节点Xnear和近邻节点中选取使路径代价最小的节点作为新的父节点；然后，在为Xnew重新选择父节点之后，为进一步使得随机树节点间连接的代价最小化，为随机树进行重新布线；将Xnew附近的近邻节点的父节点设置为Xnew，若该节点到起点的代价减小，则该节点的父节点更改为Xnew。
5.如权利要求1所述的一种基于改进式RRT*算法的机械臂自主避障方法，其特征在于，步骤四中，在连线与障碍物未检测到碰撞时，ρ1<ρ2，ρ1为随机点方向生长步长，ρ2为目标点方向生长步长；连线与障碍物检测到碰撞时，ρ1>ρ2；其中，新产生的节点Xnew，近邻点Xnear，在采样区域生成随机点Xrand，目标点Xgoal。
6.如权利要求1所述的一种基于改进式RRT*算法的机械臂自主避障方法，其特征在于，步骤六具体包括如下内容：
有效节点构成的路径Path中，从第一个路径节点即Xstart开始，依次连接路径上的后序节点，若两个节点之间的路径和障碍物之间不发生碰撞，则删除两个节点之间的路径，并将这两个节点用直线连接；以此类推，直到发生碰撞，此时以碰撞点的父节点作为新起点，再次执行上述操作，如此递归进行，直至到达目标点。
7.如权利要求1所述的一种基于改进式RRT*算法的机械臂自主避障方法，其特征在于，所述方法还包括：基于三次B样条平滑路径对剪枝后的路径平滑处理，使机器人在运行过程中更加柔顺。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310403611.XA CN116652932A (zh) | 2023-04-17 | 2023-04-17 | 一种基于改进式rrt*算法的机械臂自主避障方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310403611.XA CN116652932A (zh) | 2023-04-17 | 2023-04-17 | 一种基于改进式rrt*算法的机械臂自主避障方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116652932A true CN116652932A (zh) | 2023-08-29 |
Family
ID=87717893
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310403611.XA Pending CN116652932A (zh) | 2023-04-17 | 2023-04-17 | 一种基于改进式rrt*算法的机械臂自主避障方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116652932A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117444985A (zh) * | 2023-12-20 | 2024-01-26 | 安徽大学 | 一种机械臂小车控制方法及系统 |
-
2023
- 2023-04-17 CN CN202310403611.XA patent/CN116652932A/zh active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117444985A (zh) * | 2023-12-20 | 2024-01-26 | 安徽大学 | 一种机械臂小车控制方法及系统 |
| CN117444985B (zh) * | 2023-12-20 | 2024-03-12 | 安徽大学 | 一种机械臂小车控制方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110509279B (zh) | 一种仿人机械臂的运动路径规划方法及系统 | |
| CN110609547B (zh) | 一种基于可视图引导的移动机器人规划方法 | |
| CN109990796B (zh) | 基于双向扩展随机树的智能车路径规划方法 | |
| CN108762270B (zh) | 变概率双向快速搜索随机树改进路径规划算法 | |
| CN111546347B (zh) | 一种适用于动态环境下的机械臂路径规划方法 | |
| WO2023155371A1 (zh) | 室内移动机器人的平稳移动全局路径规划方法 | |
| CN110275528B (zh) | 针对rrt算法改进的路径优化方法 | |
| CN116652932A (zh) | 一种基于改进式rrt*算法的机械臂自主避障方法 | |
| CN112356033B (zh) | 一种融合低差异序列与rrt算法的机械臂路径规划方法 | |
| CN111251306B (zh) | 一种带有底盘误差的机械臂路径规划方法 | |
| CN113189988B (zh) | 一种基于Harris算法与RRT算法复合的自主路径规划方法 | |
| CN113467476B (zh) | 考虑转角约束的无碰撞检测快速随机树全局路径规划方法 | |
| CN112683278A (zh) | 一种基于改进a*算法和贝塞尔曲线的全局路径规划方法 | |
| CN115167398A (zh) | 一种基于改进a星算法的无人船路径规划方法 | |
| CN115047880A (zh) | 一种未知动态环境下机器人智能路径规划方法 | |
| CN113721622A (zh) | 一种机器人路径规划方法 | |
| CN116852367A (zh) | 一种基于改进RRTstar的六轴机械臂避障路径规划方法 | |
| CN116627132A (zh) | 基于rrt改进的移动机器人路径规划方法 | |
| CN114779788A (zh) | 一种改进a*算法的路径规划方法 | |
| CN115167388A (zh) | 一种基于目标引导的rrt多机器人编队路径规划算法 | |
| CN115328111A (zh) | 一种用于自动驾驶车辆的避障路径规划方法 | |
| CN117451057B (zh) | 基于改进a*算法无人机三维路径规划方法、设备和介质 | |
| CN117824652A (zh) | 一种基于安全人工势场和rrt*的机器人路径规划方法 | |
| CN117021094B (zh) | 一种适用于狭窄空间的盾构机换刀机器人路径规划方法 | |
| CN112945254B (zh) | 一种基于快速拓展随机树的无人车曲率连续路径规划方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication |