CN114137991A - 一种基于二阶贝塞尔曲线的机器人连续路径优化方法 - Google Patents
一种基于二阶贝塞尔曲线的机器人连续路径优化方法 Download PDFInfo
- Publication number
- CN114137991A CN114137991A CN202210116904.5A CN202210116904A CN114137991A CN 114137991 A CN114137991 A CN 114137991A CN 202210116904 A CN202210116904 A CN 202210116904A CN 114137991 A CN114137991 A CN 114137991A
- Authority
- CN
- China
- Prior art keywords
- order bezier
- bezier curve
- path
- points
- robot
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 72
- 238000005457 optimization Methods 0.000 title claims abstract description 59
- 238000011156 evaluation Methods 0.000 claims abstract description 95
- 239000013598 vector Substances 0.000 claims abstract description 29
- 238000013528 artificial neural network Methods 0.000 claims abstract description 13
- 238000005070 sampling Methods 0.000 claims abstract description 11
- 238000012549 training Methods 0.000 claims abstract description 9
- 230000008569 process Effects 0.000 claims description 26
- 150000001875 compounds Chemical class 0.000 claims description 13
- 241000577395 Thenus Species 0.000 claims description 7
- 210000002569 neuron Anatomy 0.000 claims description 6
- 230000002068 genetic effect Effects 0.000 claims description 4
- 239000002245 particle Substances 0.000 claims description 4
- 238000002922 simulated annealing Methods 0.000 claims description 4
- 230000007704 transition Effects 0.000 claims description 4
- 238000012545 processing Methods 0.000 abstract description 7
- 238000010586 diagram Methods 0.000 description 10
- 238000004364 calculation method Methods 0.000 description 6
- 230000007547 defect Effects 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 230000002035 prolonged effect Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000001537 neural effect Effects 0.000 description 1
- 238000012805 post-processing Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
- Feedback Control In General (AREA)
Abstract
本申请提供有一种基于二阶贝塞尔曲线的机器人连续路径优化方法,首先用二阶贝塞尔曲线控制点得到的第一控制向量对BP神经网络进行训练得到预测模型,然后以预测模型为基础,综合考虑路径可行性、安全性、运行效率因素,构建评价函数,将路径规划问题转化为最优化求解问题,对实际路径进行迭代求解,得到满足终止条件的最优路径二阶贝塞尔曲线控制点组成的集合,输入二阶贝塞尔曲线函数求解出各转折点对应的二阶贝塞尔曲线,得到优化路径。本申请能够对图搜索类和随机采样类传统路径规划算法进行后优化处理,较好的满足了机器人连续行进的要求,既提高了电机使用寿命,也提高了路径优化处理的效率,具有求解速度快、安全性高、平滑性好的优点。
Description
技术领域
本申请属于移动机器人路径规划技术领域,具体涉及一种基于二阶贝塞尔曲线的机器人连续路径优化方法。
背景技术
移动机器人的路径规划算法包括图搜索类、随机采样类、势场类和目标优化类,其中图搜索类和随机采样类路径规划算法具有运算效率高、求解路径长度短等优点而普遍应用,但也存在路径转折点多、安全性和平滑性较差等问题,如图3所示为采用图搜索类或随机采样类传统路径规划算法得到的路径示意图,必须选择能够原地转向的机器人,而且每次行走到路径转折点处,机器人必须停止行走并进行原地转向才能沿下一阶段路径行走,电机的频繁启动加剧了电机的损耗降低了电机寿命。由于贝塞尔曲线在路径的平滑过渡上具有良好的性能,目前大都采用贝塞尔曲线对移动机器人的行走路径进行后处理优化,但现有技术中较多的是使用三阶以上的贝塞尔曲线,而且贝塞尔曲线控制点采用人工选择的方式确定,存在求解速度慢、优化处理结果主观性较强的缺点。
发明内容
鉴于上述缺陷或不足,本申请旨在提供一种基于二阶贝塞尔曲线的机器人连续路径优化方法,通过BP神经网络构建二阶贝塞尔曲线路径控制点的最大偏移量预测模型,消除了人工选择路径控制点的主观性影响因素,并以最大偏移量预测模型为基础构建评价函数,将路径规划问题转化为最优化求解问题,然后用求解得到的最优二阶贝塞尔曲线控制点求解实际路径中各转折点对应的二阶贝塞尔曲线,得到优化路径,既满足了机器人连续行进的要求,提高了电机使用寿命,也提高了优化处理效率。
本申请提供一种基于二阶贝塞尔曲线的机器人连续路径优化方法,包括以下步骤:
S1:构建第一控制向量,所述第一控制向量包括由3个顺序相邻的二阶贝塞尔曲线控制点计算得到的第一间距、第二间距和第一夹角,3个顺序相邻的二阶贝塞尔曲线控制点中位于中间的控制点为转折点;
S2:训练预测模型,所述预测模型的输入为所述第一控制向量;所述预测模型的输出为计算所述第一控制向量的二阶贝塞尔曲线控制点中转折点对应的最大偏移量;
S3:以所述预测模型构建评价函数;
S4:加载环境地图,得到第一路径;
S5:提取所述第一路径中的所有控制点,得到第三集合,所述第三集合包括起始点、终止点和其它所有转折点;
S6:识别所述第三集合中的所有转折点;
S7:逐一选择所述第三集合中的转折点,并随机获取与所述转折点顺序相邻的两侧控制点,组成第四集合;
S8:计算所述第四集合中所有转折点对应的第一控制向量,得到第五集合;
S9:将第五集合输入至评价函数,得到第一评价值;
S10:重复执行S7~S9,得到第二评价值;
S11:比较所述第一评价值与所述第二评价值,保留二者中的较小值及其对应的第四集合,所述较小值作为第一评价值继续参与下一次比较;
S12:重复执行S10~S11,直至达到终止条件,得到最小评价值;
S13:获取最小评价值对应的第四集合;
S14:逐一选取最小评价值对应的第四集合中的转折点及与其顺序相邻的两侧控制点,输入二阶贝塞尔曲线函数,得到所有转折点对应的二阶贝塞尔曲线;
S15:从起始点开始顺序连接所有转折点对应的二阶贝塞尔曲线及终止点,得到优化路径。
具体的,所述第一路径为采用图搜索类或随机采样类传统路径规划算法得到的实际路径。
所述评价函数为
式中,K i 表示评价值。
所述二阶贝塞尔曲线函数为
B i = (1-t i )2P i-1 + 2t i (1-t i )P i + t i 2P i+1 (7)
式中,B i 表示第一路径转折点P i 对应的二阶贝塞尔曲线;P i-1和P i+1表示与第一路径转折点P i 顺序相邻的两侧二阶贝塞尔曲线控制点;t i 表示比例系数且t i ∈[0,1]。
根据本申请实施例,所述预测模型为
S i = f (d 1i , d 2i ,β i ) (1)
式中,S i 为二阶贝塞尔曲线转折点对应的最大偏移量,i≥1且为自然数;d 1i 表示第一间距;d 2i 表示第二间距;β i 表示第一夹角。
具体的,d 1i 、d 2i 、β i 分别对应所述预测模型中的3个输入层,S i 对应所述预测模型中的输出层。
根据本申请实施例,所述预测模型采用BP神经网络构建,包括输入层、输出层和隐含层,所述隐含层包括60个神经元。
具体的,BP神经网络为三层神经网络拓扑结构,包括3个输入层、1个输出层和1个隐含层,由万能近似理论可知,三层神经网络拓扑结构能够以任意精度实现输入矢量到目标输出向量的非线性映射,而隐含层的神经元个数越多,拟合精度越高,但模型求解速度越慢,所以综合考虑求解精度和计算速度,隐含层神经元个数选择为60个。
根据本申请实施例,在步骤S7之后还包括以下步骤:
S71:取所述第四集合中顺序相邻的控制点P i 、P i+1、P i+2及P i+3;
其中,所述控制点为P i 和P i+3为所述第一路径中相邻的两个转折点,所述控制点P i+1、P i+2为位于P i 和P i+3之间路径上的两个二阶贝塞尔曲线控制点;
具体的,在路径优化过程中,如果相邻两段二阶贝塞尔曲线存在相交的情况,则机器人在离开当前路径圆弧曲线后无法顺利进入下一路径圆弧曲线,必然会出现机器人偏离规划路径的现象,所以必须综合考虑机器人行走路径中相邻两段二阶贝塞尔曲线是否存在相交的情况。故在评价函数中引入距离惩罚因子p i ,设定在第一路径的某一直线P i →P i+3行程内,经过优化求解后,得到两个二阶贝塞尔曲线控制点P i+1、P i+2,如果<,则p i =0,反之p i =1,此时需要重新获取二阶贝塞尔曲线控制点P i+1、P i+2继续优化求解。
在评价函数(2)的基础上引入距离惩罚因子p i 后,评价函数的具体公式如下,
式中,p i 表示距离惩罚因子。
根据本申请实施例,在步骤S72之后还包括以下步骤:
S73:逐一选取所述第四集合中的转折点P i 及与其顺序相邻的两侧控制点P i-1和P i+1,输入二阶贝塞尔曲线函数,得到所有转折点对应的二阶贝塞尔曲线;
S74:分别获取所有转折点对应的二阶贝塞尔曲线上所有点的坐标,组成第一坐标集合;
S75:分别获取所述环境地图中各障碍物边缘上所有点的坐标,组成第二坐标集合;
S76:获取所述第一坐标集合中任一点与所述第二坐标集合中任一点之间的间距,得到第三间距集合;
S77:获取所述第三间距集合中的最小间距;
S78:比较所述最小间距与机器人运行最小安全距离,
如果所述最小间距≥最小安全间距,则c i =0,说明所述第四集合有效,继续执行下一步骤;
如果所述最小间距<最小安全间距,则c i =1,说明所述第四集合无效,返回执行步骤S7;
其中,所述c i 为碰撞惩罚因子。
具体的,在路径优化过程中,还需要考虑机器人运行安全情况,即优化路径是否与环境地图中的障碍物距离是否满足机器人运行的最小安全距离,如果不满足机器人运行的最小安全距离,则机器人在沿优化路径运行时必然会与障碍物发生碰撞,发生安全事故。故在评价函数中引入碰撞惩罚因子,反映移动机器人的运行安全性,即优化后路径与障碍物之间的距离≥最小安全距离时,c i =0,反之c i =1,表示此时二阶贝塞尔曲线控制点位置不满足路径优化要求,需要返回重新求解计算。
在评价函数(3)的基础上引入碰撞惩罚因子c i 后,评价函数具体公式如下,
式中,c i 表示碰撞惩罚因子。
根据本申请实施例,在步骤S78之后还包括以下步骤:
S79:分别获取所有转折点对应的二阶贝塞尔曲线上的最大曲率,组成第一曲率集合;
S710:获取所述第一曲率集合中的最大曲率;
S711:比较所述最大曲率与机器人转弯最大曲率,
如果所述最大曲率≤机器人转弯最大曲率,则r i =0,说明所述第四集合有效,继续执行下一步骤;
如果所述最大曲率>机器人转弯最大曲率,则r i =1,说明所述第四集合无效,返回执行步骤S7;
其中,所述r i 为路径可行性惩罚因子。
具体的,在路径优化过程中,还需要考虑在优化后路径中二阶贝塞尔曲线的曲率是否能够满足机器人的转弯要求,如果不能满足机器人的转弯要求,必然会出现机器人偏离规划路径的现象。故在评价函数中引入路径可行性惩罚因子r i ,即如果优化后得到的各段二阶贝塞尔曲线的最大曲率≤机器人转弯最大曲率,则r i =0,反之r i =1,表示此时二阶贝塞尔曲线控制点不满足路径优化要求,需要返回重新求解计算。
在评价函数(4)的基础上引入路径可行性惩罚因子r i 后,评价函数具体公式如下,
式中,r i 表示路径可行性惩罚因子。
根据本申请实施例,在步骤S711之后还包括以下步骤:
S712:分别获取所有转折点对应的每一条二阶贝塞尔曲线上的最小临界速度,组成第一速度集合;
S713:获取所述第一速度集合中的最小速度;
S714:比较所述最小速度与机器人运行速度,
如果所述最小速度≥机器人运行速度,则u i =0,说明所述第四集合有效,继续执行下一步骤;
如果所述最小速度<机器人运行速度,则u i =1,说明所述第四集合无效,返回执行步骤S7;
其中,所述u i 为运动速度惩罚因子。
具体的,在路径优化过程中,还需要考虑机器人运行速度是否小于各转折点对应的二阶贝塞尔曲线的最小临界速度,如果机器人的运行速度>转折点对应的二阶贝塞尔曲线的最小临界速度,必然会出现机器人偏离规划路径的现象,故在评价函数中引入运动速度惩罚因子u i ,即如果机器人运行速度≤各转折点对应的二阶贝塞尔曲线的最小临界速度,则u i =0,反之,则u i =1,表示此时二阶贝塞尔曲线控制点位置不满足路径优化要求,需要返回重新求解计算。
在评价函数(5)的基础上引入运动速度惩罚因子u i 后,评价函数具体公式如下,
式中,u i 表示运动速度惩罚因子。
根据本申请实施例,用拉丁超立方抽样方法获取预设数量的所述第一控制向量。
具体的,转折点为P i ,控制点P i-1和P i+1表示与所述转折点P i 顺序相邻的两侧二阶贝塞尔曲线控制点,所述第一间距为所述转折点P i 与控制点P i-1之间的间距d 1i ;所述第二间距为所述转折点P i 与控制点P i+1之间的间距d 2i ;所述第一间距与所述第二间距之间形成第一夹角β i ;所述d 1i 和所述d 2i 的取值范围为0~50;所述β i的取值范围为0°~180°,为了保证所述最大偏移量预测模型的准确性,在各取值范围内,采用拉丁超立方抽样的方法取1000组数据组成第一集合,用以对所述预测模型进行训练。
根据本申请实施例,求解优化路径的算法包括遗传算法、模拟退火算法、粒子群算法。
具体的,可以选择遗传算法、模拟退火算法或粒子群算法以评价函数为目标函数进行迭代计算,最终求解出在满足预设终止条件下,第一路径中各转折点P i 对应的最大偏移量S i 之和最大时二阶贝塞尔曲线控制点的布置方案,此时目标值K i 最小。
综上所述,本申请提出了一种基于二阶贝塞尔曲线的机器人连续路径优化方法,该方法通过顺序选择三个二阶贝塞尔曲线控制点(其中位于中间的控制点为转折点),计算三个控制点对应的第一间距、第二间距和第一夹角,并以第一间距、第二间距和第一夹角构建得到第一控制向量;该方法还计算该转折点所对应的最大偏移量,也即:二阶贝塞尔曲线中转折点对应有第一控制向量和最大偏移量。获取足够量的样本后,以第一控制向量为输入,最大偏移量为输出,训练神经网络得到预测模型,当输入一组第一控制向量至该预测模型后,该预测模型即可输出最大偏移量,也即此步骤,建立了第一控制向量与最大偏移量之间的预测关系。
在实际进行路径规划的过程中,通过加载环境地图,计算能够得到环境地图上所显示的第一路径,该第一路径上存在多个控制点,以第一路径行进时,必须选择能够原地转向的机器人,而且当机器人行进至转折点时必须停止并调整方向才能沿下一阶段路径行进,这会导致机器人不能连续行走、效率低,而且电机频繁启停加剧损耗降低使用寿命。本方法以第一路径上的所有控制点构建第三集合,即:所述第三集合所包括的是沿第一路径上分布的所有控制点的坐标,且除起始点和终止点两个控制点外,其它控制点均为转折点。
提取第三集合中所有转折点的坐标,逐一选择转折点,然后随机获得第一路径上位于该转折点两侧顺序相邻的二阶贝塞尔曲线控制点,得到第四集合,也即:第四集合包括:各转折点及每个转折点两侧的控制点。计算第四集合中各转折点所对应的第一控制向量,输入以预测模型构建的评价函数,得到第一评价值。
再随机获得第一路径上位于各转折点两侧顺序相邻的二阶贝塞尔曲线控制点,得到第四集合,也即:第四集合包括:各转折点及每个转折点两侧的控制点。计算第四集合中各转折点所对应的第一控制向量,输入以预测模型构建的评价函数,得到第二评价值。
比较所述第一评价值与所述第二评价值,保留二者中的较小值及其对应的第四集合,所述较小值作为第一评价值继续参与下一次比较;重复:再随机获得第一路径上位于各转折点两侧顺序相邻的二阶贝塞尔曲线控制点,得到第四集合,也即:第四集合包括:各转折点及每个转折点两侧的控制点。计算第四集合总各转折点所对应的第一控制向量,输入以预测模型构建的评价函数,得到第二评价值。比较所述第一评价值与所述第二评价值,保留二者中的较小值及其对应的第四集合,所述较小值作为第一评价值继续参与下一次比较;往复循环,直至达到终止条件,得到最小评价值及其对应的第四集合。
获取最小评价值对应的第四集合,也即与最小评价值对应的各转折点及每个转折点两侧的控制点。
在第四集合中,逐一选择转折点及其顺序相邻的两侧控制点,分别输入二阶贝塞尔曲线函数,求解得到所有转折点对应的二阶贝塞尔曲线,最后从起始点开始顺序连接所有转折点对应的二阶贝塞尔曲线及终止点,从而获得满足机器人连续行进的优化路径。
本方法针对第一路径上的转折点进行重点优化规划,首先将第一路径上的转折点提取,并针对所有转折点,在第一路径上随机选择位于其两侧的点作为两侧二阶贝塞尔曲线控制点,通过迭代计算,得到与各转折点对应且使得评价函数输出数值最小的两侧最终二阶贝塞尔曲线控制点,以各转折点和其两侧最终二阶贝塞尔曲线控制点构成第四集合,再以第四集合规划二阶贝塞尔曲线,得到能够使机器人在各转折点处平滑过渡,然后从起始点到终止点,将所有曲线顺序连接,即得到最优化路径。相较于现有技术而言,本方法能够充分地对第一路径上的转折点处路径进行优化规划,有效地解决了现有技术中机器人种类选择范围窄、电机频繁启停寿命低的问题,并提高了优化处理速度,消除了优化结果主观性较强的缺点。
附图说明
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述,本申请的其它特征、目的和优点将会变得更明显。
图1为二阶贝塞尔曲线转折点的最大偏移量示意图。
图2为路径优化处理的示意图。
图3为采用传统路径规划算法得到的实际路径示意图。
图4为本申请实施例采用传统路径规划算法得到的第一路径示意图。
图5为采用本申请优化方法对图4所示的第一路径优化处理后得到的优化路径示意图。
图6为本申请连续路径优化方法流程图。
具体实施方式
下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是,此处所描述的具体实施例仅仅用于解释相关发明,而非对该发明的限定。另外还需要说明的是,为了便于描述,附图中仅示出了与发明相关的部分。
下面将参考附图并结合实施例来详细说明本申请。
如图1所示的二阶贝塞尔曲线转折点的最大偏移量示意图,包括三个顺序相邻的二阶贝塞尔曲线控制点P0、P1、P2,其中P1为转折点。
控制点P0和P1的间距为第一间距,即的模长d 11;控制点P1和P2的间距为第二间距,即的模长d 21,第一间距和第二间距形成第一夹角β 1,即∠P0P1P2。利用第一间距、第二间距、第一夹角,通过几何计算可以得到转折点P1的最大偏移量S 1。
用第一间距、第二间距、第一夹角构建第一控制向量(、、∠P0P1P2),设定第一间距、第二间距的取值范围均为0~50,第一夹角的取值范围为0°~180°。在取值范围内,采用拉丁超立方抽样方法获取1000组第一控制向量,组成第一集合,即(、∠P i-1P i P i+1),如表1所示。
计算所述第一集合中所有转折点P i 对应的最大偏移量S i ,组成第二集合,如表1所示。
表1
采用BP神经网络创建二阶贝塞尔曲线转折点P i 对应的最大偏移量S i 的预测模型。BP神经网络为三层神经网络拓扑结构,包括3个输入层、1个输出层和1个隐含层,由万能近似理论可知,三层神经网络拓扑结构能够以任意精度实现输入矢量到目标输出向量的非线性映射,而隐含层的神经元个数越多,拟合精度越高,模型求解速度越慢,所以综合考虑求解精度和计算速度,隐含层神经元个数选择为60个。
以第一集合为输入,以第二集合为为输出,用如表1所示的训练样本训练BP神经网络,得到能够准确预测二阶贝塞尔曲线转折点P i 对应的最大偏移量S i 的预测模型,并用以下公式表示预测模型,
S i = f (d 1i , d 2i ,β i ) (1)
式中,S i 为二阶贝塞尔曲线转折点对应的最大偏移量,i≥1且为自然数;d 1i 表示第一间距;d 2i 表示第二间距;β i 表示第一夹角。
分析可知,二阶贝塞尔曲线转折点P i 对应的最大偏移量S i 越大,表示二阶贝塞尔曲线越短越平滑,所以当第一路径中各转折点P i 对应的最大偏移量S i 之和最大时,得到优化路径最短最平滑,为将路径规划问题转化为最优化求解问题,采用预测模型的倒数构建用于评价路径优化程度的评价函数,具体公式如下,
式中,K i 表示评价值。
可以看出,评价值K i 越小表示优化后路径越短越平缓,经过最优求解最终将得到评价值K i 最小时对应的二阶贝塞尔曲线控制点P0、P1、P2,用二阶贝塞尔曲线函数经过求解计算得到二阶贝塞尔曲线,二阶贝塞尔曲线函数具体公式如下,
B i = (1-t i )2P i-1 + 2t i (1-t i )P i + t i 2P i+1 (7)
式中,B i 表示转折控制点P i 对应的二阶贝塞尔曲线;P i-1和P i+1表示转折点P i 前后顺序相邻的两个二阶贝塞尔曲线控制点;t i 表示比例系数且t i ∈[0,1]。
实施例1
按照如图6所示的流程图,采用本申请优化方法对如图4所示的第一路径进行优化,横轴为x坐标,纵轴为y坐标,图中白色区域表示无障碍物区域,其它颜色区域为障碍物区域,包括以下步骤:
S1:构建所述第一控制向量;
S2:训练预测模型;
S3:以所述预测模型构建评价函数(2);
S4:加载如图4所示的环境地图,采用图搜索类或随机采样类传统路径规划算法得到的如图4所示的第一路径A-B-C-D-E,即传统路径;
S5:提取所述第一路径中的所有控制点,得到第三集合,所述第三集合包括第一路径的所有控制点,如图4所示从起始点A到终止点E各控制点坐标依次为A(0.5,0.5)、B(0.5,6.5)、C(2.5,8.5)、D(8.5,8.5)、E(9.5,9.5);
S6:识别所述第三集合中的所有转折点B、C、D;
S7:逐一选取所述第三集合中的所述转折点B、C、D,与随机获取的二阶贝塞尔曲线控制点P0、P1、P2、P3、P4、P5,组成第四集合;
S9:选择遗传算法,设定终止条件,将所述第五集合为输入评价函数(2),得到第一评价值K 1;
其中,所述终止条件为最大迭代次数为100次,种群大小为50,交叉率为0.6,变异率为0.05;
S10:重复执行S7~S9,得到第二评价值K 2;
S11:比较所述第一评价值K 1与所述第二评价值K 2,如K 1<K 2,则保留K 1及K 1对应的第四集合,所述第一评价值K 1继续参与下一次比较;
S12:重复执行S10~S11,直至达到终止条件,得到最小评价值K min;
S13:获取最小评价值K min对应的第四集合,包括二阶贝塞尔曲线控制点P0、B、P1、P2、C、P3、P4、D、P5;
S14:选取最小评价值对应的第四集合中所述转折点B及与其顺序相邻的两侧二阶贝塞尔曲线控制点P0、P1,输入二阶贝塞尔曲线函数(7)可以得到所述转折点B对应的二阶贝塞尔曲线,以此类推得到其它转折点C、D对应的二阶贝塞尔曲线;
S15:从起始点A开始顺序连接所述转折点B、C、D对应的二阶贝塞尔曲线及终止点E,得到如图5所示的优化路径。
进一步的,如图2所示的路径优化处理的示意图,图中黑色为障碍物,A、P1、P4、B为第一路径A-P1-P4-B的控制点,其中P1、P4为转折点,也是二阶贝塞尔曲线最大偏移量控制点,而A-P0--P2-P3--P5-B为模拟的优化路径,其中P0、P2、P3、P5为二阶贝塞尔曲线控制点,为转折点P1对应的二阶贝塞尔曲线,为转折点P4对应的二阶贝塞尔曲线。
在路径优化过程中,如果相邻两段二阶贝塞尔曲线和存在相交的情况,则机器人在离开当前路径圆弧曲线后无法顺利进入下一路径圆弧曲线,必然会出现机器人偏离规划路径的现象,所以必须综合考虑机器人行走路径中相邻两段二阶贝塞尔曲线是否存在相交的情况。在第一路径的P1→P4行程内包括两个贝塞尔曲线控制点P2、P3,如果,则p i =0,否则p i =1,此时需要重新选择控制点P0、P2、P3、P5继续优化求解。
在评价函数(2)的基础上引入距离惩罚因子p i 后评价函数具体公式如下,
式中,p i 表示距离惩罚因子。
实施例2
采用如实施例1所述优化方法对如图4所示的第一路径进行优化,不同之处在于:
S3:以所述预测模型构建评价函数(3);
在步骤S7后还包括以下步骤:
S71:取所述第四集合中的控制点B、P1、P2、C、P3、P4、D;
进一步的,如图2所示的路径优化处理的示意图,在路径优化过程中,还需考虑机器人运行路径是否与环境地图中的各障碍物距离是否满足机器人运行的最小安全距离,如果不满足机器人运行的最小安全距离,则机器人在沿优化路径运行时必然会与障碍物发生碰撞,发生安全事故,故在评价函数中引入碰撞惩罚因子c i ,反映移动机器人的运行安全性。规定当机器人沿优化后路径运行不与障碍物发送碰撞时,即优化路径A-P0--P2-P3--P5-B上各点与障碍物之间的距离均≥最小安全距离时,c i =0,否则c i =1,表示此时路径控制点P0、P2、P3、P5位置不满足路径优化要求,需要重新选择控制点P0、P2、P3、P5优化求解。在评价函数(3)的基础上引入碰撞惩罚因子c i 后评价函数具体公式如下,
式中,c i 表示碰撞惩罚因子。
实施例3
采用如实施例2所述优化方法对如图4所示的第一路径进行优化,不同之处在于:
S3:以所述预测模型构建评价函数(4);
在步骤S72后还包括以下步骤:
S73:选取所述第四集合中所述转折点B及与其顺序相邻的两侧二阶贝塞尔曲线控制点P0、P1,输入二阶贝塞尔曲线函数(7)可以得到所述转折点B对应的二阶贝塞尔曲线,以此类推得到其它转折点C、D对应的二阶贝塞尔曲线;
S74:分别获取所述转折点B、C、D对应的二阶贝塞尔曲线上所有点的坐标,得到第一坐标集合;
S75:分别获取所述环境地图中黑色区域障碍物边缘上所有点的坐标,得到第二坐标集合;
S76:获取所述第一坐标集合中任一点与所述第二坐标集合中任一点之间的间距,得到第三间距集合;
S77:获取所述第三间距集合中的最小间距dmin;
S78:比较所述最小间距dmin与机器人运行最小安全距离Dmin,
如果所述最小间距dmin≥最小安全间距Dmin,则c i =0,说明所述第四集合有效,继续执行下一步骤;
如果所述最小间距dmin<最小安全间距Dmin,则c i =1,说明所述第四集合无效,返回执行步骤S7。
进一步的,如图2所示的路径优化处理的示意图,在路径优化过程中,还需要考虑优化后路径中二阶贝塞尔曲线的曲率是否能够满足机器人的转弯要求,如果不能满足机器人的转弯要求,必然会出现机器人偏离规划路径的现象,即如果优化后得到的二阶贝塞尔曲线且的最大曲率均≤机器人转弯最大曲率,则r i =0,反之r i =1,表示此时曲线控制点P0、P2、P3、P5不满足路径优化要求,需要重新选择控制点P0、P2、P3、P5优化求解。在评价函数(4)的基础上引入路径可行性惩罚因子r i 后评价函数具体公式如下,
式中,r i 表示路径可行性惩罚因子。
实施例4
采用如实施例3所述优化方法对如图4所示的第一路径进行优化,不同之处在于:
S3:以所述预测模型构建评价函数(5);
在步骤S78后还包括以下步骤:
S79:分别获取所述转折点B、C、D对应的各二阶贝塞尔曲线上的最大曲率RB max、RC max、RD max,组成第一曲率集合;
S710:获取所述第一曲率集合中的最大曲率RD max;
S711:比较所述最大曲率RD max与机器人转弯最大曲率Rmax,
如果所述最大曲率RD max≤机器人转弯最大曲率Rmax,则r i =0,说明所述第四集合有效,继续执行下一步骤;
如果所述最大曲率RD max>机器人转弯最大曲率Rmax,则r i =1,说明所述第四集合无效,返回执行步骤S7。
进一步的,如图2所示的路径优化处理的示意图,在路径优化过程中,还需要考虑机器人运行速度是否小于各转折点对应的二阶贝塞尔曲线上的最小临界速度,如果机器人的运行速度>任一二阶贝塞尔曲线上的最小临界速度,必然会出现机器人偏离规划路径的现象,故在评价函数中引入运动速度惩罚因子u i 。
设定机器人的运行速度≤任一转折点对应的二阶贝塞尔曲线的最小临界速度,则u i =0,如机器人的运行速度>任一转折点对应的二阶贝塞尔曲线的最小临界速度,则u i =1,表示此时二阶贝塞尔曲线控制点P0、P2、P3、P5位置不满足路径优化要求,需要重新选择控制点P0、P2、P3、P5优化求解。在评价函数(5)的基础上引入运动速度惩罚因子u i 后评价函数具体公式如下,
式中,u i 表示运动速度惩罚因子。
实施例5
采用如实施例4所述优化方法对如图4所示的第一路径进行优化,不同之处在于:
S3:以所述预测模型构建评价函数(6);
在步骤S711后还包括以下步骤:
S712:分别获取所述转折点B、C、D对应的各二阶贝塞尔曲线上的最小临界速度VB min、VC min、VD min,组成第一速度集合;
S713:获取所述第一速度集合中的最小速度VC min;
S714:比较所述最小速度VC min与机器人运行速度V,
如果所述最小速度VC min≥机器人运行速度V,则u i =0,说明所述第四集合有效,继续执行下一步骤;
如果所述最小速度VC min<机器人运行速度V,则u i =1,说明所述第四集合无效,返回执行步骤S7。
实施例6
采用如实施例5所述优化方法对如图4所示的第一路径进行优化,不同之处在于:
S9:选择模拟退火算法,设定终止条件,将所述第五集合为输入评价函数(6),得到第一评价值K 1;
其中,所述终止条件为最大迭代次数100次,设置扰动次数为20次、最开始的温度为0.1℃、每次衰减的比例为0.99、种群个数为10个、每次扰动的次数位为5、扰动的概率为0.5。
实施例7
采用如实施例5所述优化方法对如图4所示的第一路径进行优化,不同之处在于:
S9:选择粒子群算法,设定终止条件,将所述第五集合为输入评价函数(6),得到第一评价值K 1;
其中,所述终止条件为最大迭代次数为100次,种群个数为50个。
综合以上技术方案,本申请公开有一种基于二阶贝塞尔曲线的机器人连续路径优化方法,首先采用拉丁超立方取样方法获取控制二阶贝塞尔曲线的第一集合和第二集合,并以第一集合和第二集合组成的训练样本对采用BP神经网络构建的预测模型进行训练,得到精度高、可靠性好的预测模型,提高了二阶贝塞尔曲线控制点求解的准确性,消除了人工选择路径控制点的主观性影响因素,然后以预测模型为基础,综合考虑路径可行性、安全性、运行速度因素,构建评价函数,将路径规划问题转化为最优化求解问题,然后用此评价函数对在第一路径上随机获取的第四集合进行迭代求解,得到由满足各类约束条件的最优路径二阶贝塞尔曲线控制点组成的第四集合,然后在第四集合中逐一选取各转折点及与其前后顺序相邻的两个控制点输入二阶贝塞尔曲线函数得到各转折点对应的二阶贝塞尔曲线,最后从起始点开始顺序连接所有转折点对应的二阶贝塞尔曲线及终止点,进而得到优化路径,实现移动机器人行进路径的后优化平滑处理,较好的满足了移动机器人的连续行进的要求,既拓宽了机器人种类的选择范围,提高了机器人电机使用寿命,也提高了路径优化处理的效率,具有求解速度快、安全性高、平滑性好的优点。
以上描述仅为本申请的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解,本申请中所涉及的发明范围,并不限于上述技术特征的特定组合而成的技术方案,同时也应涵盖在不脱离所述发明构思的情况下,由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本申请中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。
Claims (9)
1.一种基于二阶贝塞尔曲线的机器人连续路径优化方法,其特征在于,所述优化方法包括以下步骤:
S1:构建第一控制向量,所述第一控制向量包括由3个顺序相邻的二阶贝塞尔曲线控制点计算得到的第一间距、第二间距和第一夹角,3个顺序相邻的二阶贝塞尔曲线控制点中位于中间的控制点为转折点;
S2:训练预测模型,所述预测模型的输入为所述第一控制向量;所述预测模型的输出为计算所述第一控制向量的二阶贝塞尔曲线控制点中转折点对应的最大偏移量;
S3:以所述预测模型构建评价函数;
S4:加载环境地图,得到第一路径;
S5:提取所述第一路径中的所有控制点,得到第三集合,所述第三集合包括起始点、终止点和其它所有转折点;
S6:识别所述第三集合中的所有转折点;
S7:逐一选取所述第三集合中的转折点,并随机获取与所述转折点顺序相邻的两侧控制点,组成第四集合;
S8:计算所述第四集合中所有转折点对应的第一控制向量,得到第五集合;
S9:将第五集合输入至评价函数,得到第一评价值;
S10:重复执行S7~S9,得到第二评价值;
S11:比较所述第一评价值与所述第二评价值,保留二者中的较小值及其对应的第四集合,所述较小值作为第一评价值继续参与下一次比较;
S12:重复执行S10~S11,直至达到终止条件,得到最小评价值;
S13:获取最小评价值对应的第四集合;
S14:逐一选取最小评价值对应的第四集合中的转折点及与其顺序相邻的两侧控制点,输入二阶贝塞尔曲线函数,得到所有转折点对应的二阶贝塞尔曲线;
S15:从起始点开始顺序连接所有转折点对应的二阶贝塞尔曲线及终止点,得到优化路径。
2.根据权利要求1所述的一种基于二阶贝塞尔曲线的机器人连续路径优化方法,其特征在于,所述预测模型为
S i = f (d 1i , d 2i ,β i )
式中,S i 为二阶贝塞尔曲线转折点对应的最大偏移量,i≥1且为自然数;d 1i 表示第一间距;d 2i 表示第二间距;β i 表示第一夹角。
3.根据权利要求2所述的一种基于二阶贝塞尔曲线的机器人连续路径优化方法,其特征在于,所述预测模型采用BP神经网络构建,包括输入层、输出层和隐含层,所述隐含层包括60个神经元。
4.根据权利要求2所述的一种基于二阶贝塞尔曲线的机器人连续路径优化方法,其特征在于,在步骤S7之后还包括以下步骤:
S71:取所述第四集合中顺序相邻的控制点P i 、P i+1、P i+2及P i+3;
其中,所述控制点为P i 和P i+3为所述第一路径中相邻的两个转折点,所述控制点P i+1、P i+2为位于P i 和P i+3之间路径上的两个二阶贝塞尔曲线控制点;
5.根据权利要求4所述的一种基于二阶贝塞尔曲线的机器人连续路径优化方法,其特征在于,在步骤S72之后还包括以下步骤:
S73:逐一选取所述第四集合中的转折点P i 及与其顺序相邻的两侧控制点P i-1和P i+1,输入二阶贝塞尔曲线函数,得到所有转折点对应的二阶贝塞尔曲线;
S74:分别获取所有转折点对应的二阶贝塞尔曲线上所有点的坐标,组成第一坐标集合;
S75:分别获取所述环境地图中各障碍物边缘上所有点的坐标,组成第二坐标集合;
S76:获取所述第一坐标集合中任一点与所述第二坐标集合中任一点之间的间距,得到第三间距集合;
S77:获取所述第三间距集合中的最小间距;
S78:比较所述最小间距与机器人运行最小安全距离,
如果所述最小间距≥最小安全间距,则c i =0,说明所述第四集合有效,继续执行下一步骤;
如果所述最小间距<最小安全间距,则c i =1,说明所述第四集合无效,返回执行步骤S7;
其中,所述c i 为碰撞惩罚因子。
6.根据权利要求5所述的一种基于二阶贝塞尔曲线的机器人连续路径优化方法,其特征在于,在步骤S78之后还包括以下步骤:
S79:获取所有转折点对应的二阶贝塞尔曲线上的最大曲率,组成第一曲率集合;
S710:获取所述第一曲率集合中的最大曲率;
S711:比较所述最大曲率与机器人转弯最大曲率,
如果所述最大曲率≤机器人转弯最大曲率,则r i =0,说明所述第四集合有效,继续执行下一步骤;
如果所述最大曲率>机器人转弯最大曲率,则r i =1,说明所述第四集合无效,返回执行步骤S7;
其中,所述r i 为路径可行性惩罚因子。
7.根据权利要求6所述的一种基于二阶贝塞尔曲线的机器人连续路径优化方法,其特征在于,在步骤S711之后还包括以下步骤:
S712:获取所有转折点对应的每一条二阶贝塞尔曲线上的最小临界速度,组成第一速度集合;
S713:获取所述第一速度集合中的最小速度;
S714:比较所述最小速度与机器人运行速度,
如果所述最小速度≥机器人运行速度,则u i =0,说明所述第四集合有效,继续执行下一步骤;
如果所述最小速度<机器人运行速度,则u i =1,说明所述第四集合无效,返回执行步骤S7;
其中,所述u i 为运动速度惩罚因子。
8.根据权利要求2所述的一种基于二阶贝塞尔曲线的机器人连续路径优化方法,其特征在于,用拉丁超立方抽样方法获取预设数量的所述第一控制向量。
9.根据权利要求7所述的一种基于二阶贝塞尔曲线的机器人连续路径优化方法,其特征在于,求解优化路径的算法包括遗传算法、模拟退火算法、粒子群算法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210116904.5A CN114137991B (zh) | 2022-02-08 | 2022-02-08 | 一种基于二阶贝塞尔曲线的机器人连续路径优化方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210116904.5A CN114137991B (zh) | 2022-02-08 | 2022-02-08 | 一种基于二阶贝塞尔曲线的机器人连续路径优化方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114137991A true CN114137991A (zh) | 2022-03-04 |
CN114137991B CN114137991B (zh) | 2022-04-26 |
Family
ID=80381961
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210116904.5A Active CN114137991B (zh) | 2022-02-08 | 2022-02-08 | 一种基于二阶贝塞尔曲线的机器人连续路径优化方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114137991B (zh) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
AU2007202027A1 (en) * | 2007-05-04 | 2008-11-20 | Canon Kabushiki Kaisha | Path Optimisation |
CN107085437A (zh) * | 2017-03-20 | 2017-08-22 | 浙江工业大学 | 一种基于eb‑rrt的无人机航迹规划方法 |
CN109990796A (zh) * | 2019-04-23 | 2019-07-09 | 成都信息工程大学 | 基于双向扩展随机树的智能车路径规划方法 |
CN112462763A (zh) * | 2020-11-18 | 2021-03-09 | 河北工业大学 | 一种基于栅格地图的割草机器人路径规划方法 |
CN113686345A (zh) * | 2021-10-25 | 2021-11-23 | 上海仙工智能科技有限公司 | 多段导航路径曲率连续拼接优化处理机、方法及存储介质 |
-
2022
- 2022-02-08 CN CN202210116904.5A patent/CN114137991B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
AU2007202027A1 (en) * | 2007-05-04 | 2008-11-20 | Canon Kabushiki Kaisha | Path Optimisation |
CN107085437A (zh) * | 2017-03-20 | 2017-08-22 | 浙江工业大学 | 一种基于eb‑rrt的无人机航迹规划方法 |
CN109990796A (zh) * | 2019-04-23 | 2019-07-09 | 成都信息工程大学 | 基于双向扩展随机树的智能车路径规划方法 |
CN112462763A (zh) * | 2020-11-18 | 2021-03-09 | 河北工业大学 | 一种基于栅格地图的割草机器人路径规划方法 |
CN113686345A (zh) * | 2021-10-25 | 2021-11-23 | 上海仙工智能科技有限公司 | 多段导航路径曲率连续拼接优化处理机、方法及存储介质 |
Non-Patent Citations (4)
Title |
---|
JIAYAO LI,等: "Roaming path generation algorithm and optimization based on bezier curve", 《IFAC PAPERSONLINE》 * |
XUERUI LI,等: "A Two‑Stage Genetic Algorithm for Molding Parameters Optimization for Minimized Residual Stresses in Composite Laminates During Curing", 《APPLIED COMPOSITE MATERIALS》 * |
吴博,等: "一种内河海事无人艇路径规划算法设计与仿真", 《计算机工程与应用》 * |
段书用,等: "具有确保安全距离的A*路径优化方法", 《机械工程学报》 * |
Also Published As
Publication number | Publication date |
---|---|
CN114137991B (zh) | 2022-04-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Chen et al. | Robot navigation in crowds by graph convolutional networks with attention learned from human gaze | |
Wang et al. | Mobile robot path planning in dynamic environments through globally guided reinforcement learning | |
CN110487279B (zh) | 一种基于改进a*算法的路径规划方法 | |
WO2021135554A1 (zh) | 一种无人车全局路径规划方法和装置 | |
Park et al. | Leveraging future relationship reasoning for vehicle trajectory prediction | |
CN109597425B (zh) | 基于强化学习的无人机导航和避障方法 | |
CN112526988B (zh) | 一种自主移动机器人及其路径导航和路径规划方法、*** | |
CN110488835A (zh) | 一种基于双反向传播神经网络的无人***智能局部路径规划方法 | |
Banzhaf et al. | Learning to predict ego-vehicle poses for sampling-based nonholonomic motion planning | |
CN116804879A (zh) | 一种改进蜣螂算法融合dwa算法的机器人路径规划框架方法 | |
CN113325799A (zh) | 一种曲面工件的点焊机器人操作空间平滑路径规划方法 | |
CN116050245A (zh) | 基于复杂网络理论的高速公路自动驾驶商用车轨迹预测与决策方法与*** | |
Liu et al. | Research on multi-AGVs path planning and coordination mechanism | |
Meng et al. | Trajectory prediction for automated vehicles on roads with lanes partially covered by ice or snow | |
Chen et al. | Optimization of robotic task sequencing problems by crowding evolutionary algorithms | |
CN116734877A (zh) | 基于改进a*算法与动态窗口法的机器人动态避障方法 | |
CN114137991B (zh) | 一种基于二阶贝塞尔曲线的机器人连续路径优化方法 | |
CN117740020A (zh) | 基于A-star算法和三次B样条曲线融合的平滑路径改进方法 | |
Fan et al. | Hierarchical path planner for unknown space exploration using reinforcement learning-based intelligent frontier selection | |
Arbabi et al. | Planning for autonomous driving via interaction-aware probabilistic action policies | |
CN116202526A (zh) | 有限视场下结合双重卷积网络及循环神经网络的人群导航方法 | |
WO2023137938A1 (zh) | 虚拟角色的路径规划方法及装置、电子设备、存储介质 | |
CN115826591A (zh) | 一种基于神经网络估计路径代价的多目标点路径规划方法 | |
Kala | Sampling based mission planning for multiple robots | |
CN115933637A (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 |