CN116009558A - 一种结合运动学约束的移动机器人路径规划方法 - Google Patents
一种结合运动学约束的移动机器人路径规划方法 Download PDFInfo
- Publication number
- CN116009558A CN116009558A CN202310160304.3A CN202310160304A CN116009558A CN 116009558 A CN116009558 A CN 116009558A CN 202310160304 A CN202310160304 A CN 202310160304A CN 116009558 A CN116009558 A CN 116009558A
- Authority
- CN
- China
- Prior art keywords
- robot
- path
- list
- node
- grid
- 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 32
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 17
- 238000013139 quantization Methods 0.000 claims abstract description 4
- 239000013598 vector Substances 0.000 claims description 12
- 238000011156 evaluation Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 4
- 238000013459 approach Methods 0.000 claims description 3
- 238000002939 conjugate gradient method Methods 0.000 claims description 3
- 238000009499 grossing Methods 0.000 claims description 3
- 238000005457 optimization Methods 0.000 claims description 3
- 238000007619 statistical method Methods 0.000 claims description 3
- 230000007613 environmental effect Effects 0.000 abstract 1
- 238000013519 translation Methods 0.000 description 10
- 238000000354 decomposition reaction Methods 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 238000012887 quadratic function Methods 0.000 description 2
- 241000486463 Eugraphe sigma Species 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000009828 non-uniform distribution Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Numerical Control (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供了一种结合运动学约束的移动机器人路径规划方法,其属于路径规划技术领域,包括以下步骤:通过移动机器人载有的相机获取环境信息以构建栅格地图,设定起始点Pstart和目标点Pgoal;将栅格地图划分为障碍物区域和自由空间区域,并赋予每个栅格不同的可通过性量化指标;使用Hybird A*算法计算规划移动机器人实际移动路径,本发明能够获得符合动力学约束且机器人实际可行的路径规划结果。
Description
技术领域
本发明涉及路径规划技术领域,具体涉及一种结合运动学约束的移动机器人路径规划方法。
背景技术
路径规划是移动机器人智能控制中的重要任务之一,移动机器人的路径规划可以定义为:在运行空间和目标点一定、以及满足机器人运动约束的情况下,规划一条连通起点与目标点的无碰撞路径。路径规划的步骤一般为:(1)规划空间环境建模,需要对现实环境中的障碍物进行映射,构造规划算法可以使用的地图。(2)寻找路径,在构造的地图中,进行搜索,寻找连接起点与目标点的无碰撞路径,并需要满足机器人的运动约束。目前路径规划的方式主要分为基于搜索的路径规划和基于采样的路径规划,基于这两种方式常见的算法有A*、RRT、D*等,但使用这些算法生成的轨迹都是离散的,难以满足机器人的运动学约束,且并非实际可行的。
发明内容
本发明提供了一种结合运动学约束的移动机器人路径规划方法,能够获得符合动力学约束且机器人实际可行的路径规划结果。
本发明采用如下技术方案:
一种结合运动学约束的移动机器人路径规划方法,包括以下步骤:
步骤1:输入数字高程地图,利用栅格法把数字高程地图划分成m×n大小的栅格地图,由输入的起始点Pstart和目标点Pgoal的世界坐标转换成对应的栅格坐标,获得栅格起始点和目标点;
步骤2:对栅格地图内的三维信息进行平面拟合和统计分析,得到该栅格的基本地貌特征的描述信息(如地形坡度、粗糙度等),依据可通过性分析评价准则,进一步完成其可通过性评价,给出各单元格的可通过性量化指标。计算可通过性指标需先拟合平面,采用最小二乘法来拟合平面,即通过将误差的平方和最小化得到最佳的匹配值。假设拟合的平面方程的表达式为:
z=a0x+a1y+a2
那么就需要满足的条件为
所以就有:
即
最终可以求得
而该栅格的倾斜度dslope则为:
dslope=β
在得到栅格邻域内的拟合平面后,粗糙度由邻域内每个栅格到平面的距离的均方根表征。因此,可以根据以下式子计算得到。
其中,di表示第i个栅格到拟合平面的距离;
步骤3:使用结合运动学约束的Hybird A*算法在连续坐标系下进行启发式搜索得到初始全局路径,对初始全局路径进行平滑处理得到机器人实际移动路径。
动力学约束通过以下公式得到:
两者联立可以得到动力学约束为:
进一步,所述步骤3中,对初始全局路径进行平滑处理得到机器人实际移动路径,具体为:
对路径的顶点坐标进行非线性优化规划问题的建模,采用共轭梯度法进行非参数化插值求解目标函数,求得实际移动路径。
进一步,所述步骤3中,使用Hybird A*算法得到初始全局路径,包括以下步骤:
步骤1:建立一个优先队列open_list和列表close_list,将起始栅格节点存入open_list中。
步骤2:设置开始进行Reedshepp曲线拓展的循环数N,即每循环N次后进行一次查询当前节点状态到目标状态是否存在Reedshepp曲线,将N的大小设置为当前节点位置到目标位置欧式距离的四分之一。
步骤3:从open_list中选出F值最小的节点作为父节点,如果找到相应的Reedshepp曲线则不再扩展节点,并把当前节点作为最终节点,直接选取路径代价最小的Reedsheep曲线,并将曲线的路经点加入到最终的路径序列中。否则继续扩展子节点,如果子节点在close_list中,则舍弃,如果不在close_list中,而在open_list中,若子节点的G值比在open_list中相应节点的G值小,则更新为子节点。如果既不在close_list中,也不在open_list中,则直接将子节点加入到open_list中,并重新执行步骤3。
步骤4:最后将open_list中的所有节点和Reedshepp曲线连接中间点位姿与目标位姿的路径拼接作为Hybrid A*规划出的路径。
进一步,所述Reedshepp曲线,具体为:
ReedShepp曲线是一种路线规划方法。假设机器人能以固定的半径转向,且机器人能够前进和后退,那么ReedShepp曲线就是机器人在上述条件下从起点到终点的最短路径。Reeds-Shepp曲线由几段半径固定的圆弧和一段直线段拼接组成,而且圆弧的半径就是机器人的最小转向半径。
机器人的起点和终点的位置姿态是难以穷举的,所以一般在计算之前,会将机器人的姿态归一化。
假设机器人的初始姿态为qI=(x1,y1,θ1),目标姿态qG=(x2,y2,θ2),机器人的转向半径r=ρ
应用旋转矩阵,将机器人的起点朝向转到x轴正向:
将机器人转向半径缩放到1,于是最终得到机器人运动的起始姿态:
目标姿态:
进一步,所述F值,具体为:
F值的计算如下:
F=G+H
其中H是Hybrid A*算法中的启发式函数,在Hybrid A*算法中将启发式函数分成两种:无障碍物的非完整性约束启发代价和有障碍物的完整性启发式代价。一般取二者的最大值。
H=max{non holonomic no obstacles cost,2D Euclidean Distance}
其中非完整性约束启发代价结合了机器人的运动学约束,忽略了环境中的障碍物等信息,只考虑机器人的运动学特性,计算从该点到其他点的最短路径。这种启发式约束考虑了当前航向角以及转弯半径,可以确保机器人以正确的航向接近目标位姿。
有障碍物的完整性启发式代价则忽略了机器人的运动学特性,只考虑了障碍物。一般使用普通A*搜索计算每个节点到终点的最短路径,并使用欧几里得距离表示。
附图说明
图1为一种结合运动学约束的移动机器人路径规划方法流程图。
图2为结合运动学约束的Hybird A*算法流程图。
具体实施方式
本发明提供了一种结合运动学约束的移动机器人路径规划方法,能够获得符合动力学约束且机器人实际可行的路径规划结果。
本发明采用如下技术方案:
一种结合运动学约束的移动机器人路径规划方法,包括以下步骤:
步骤1:输入数字高程地图,利用栅格法把数字高程地图划分成m×n大小的栅格地图,由输入的起始点Pstart和目标点Pgoal的世界坐标转换成对应的栅格坐标,获得栅格起始点和目标点;
步骤2:对栅格地图内的三维信息进行平面拟合和统计分析,得到该栅格的基本地貌特征的描述信息(如地形坡度、粗糙度等),依据可通过性分析评价准则,进一步完成其可通过性评价,给出各单元格的可通过性量化指标。计算可通过性指标需先拟合平面,采用最小二乘法来拟合平面,即通过将误差的平方和最小化得到最佳的匹配值。假设拟合的平面方程的表达式为:
z=a0x+a1y+a2
那么就需要满足的条件为
所以就有:
即
最终可以求得
而该栅格的倾斜度dslope则为:
dslope=β
在得到栅格邻域内的拟合平面后,粗糙度由邻域内每个栅格到平面的距离的均方根表征。因此,可以根据以下式子计算得到。
其中,di表示第i个栅格到拟合平面的距离;
步骤3:使用结合运动学约束的Hybird A*算法在连续坐标系下进行启发式搜索得到初始全局路径,对初始全局路径进行平滑处理得到机器人实际移动路径。
动力学约束通过以下公式得到:
两者联立可以得到动力学约束为:
优选地,所述步骤3中,对初始全局路径进行平滑处理得到机器人实际移动路径,具体为:
对路径的顶点坐标进行非线性优化规划问题的建模,采用共轭梯度法进行非参数化插值求解目标函数,求得实际移动路径。
优选地,所述目标函数,具体为:
目标函数由以下四部分组成。
障碍物项Pobs:惩罚与障碍物的碰撞。公式中xi是路径节点的坐标,oi是最近障碍物的坐标,是最大障碍物的距离。选择二次函数作为惩罚项的目的是放大障碍物与节点越来越靠近的效果。从公式可以看出,节点靠近障碍物的时候,第一项的值会减小,因此该差值的二次方变大。
光滑度项Psmo:光滑度项计算每个节点之间位移向量的差值的平方。这一项将损失值赋给非均匀分布和方向变化的节点,以保证路径的平滑性。
安全项Psafe:这一项基于voronoi场函数,voronoi场的值随着导航中所有可行空间的大小成比例缩放,使路径远离障碍物。其中do,dv分别代表路径节点到最近的障碍物和最近的广义voronoi图的长度,α>0控制场的衰减率。
优选地,所述步骤3中,使用Hybird A*算法得到初始全局路径,包括以下步骤:
步骤1:建立一个优先队列open_list和列表close_list,将起始栅格节点存入open_list中。
步骤2:设置开始进行Reedshepp曲线拓展的循环数N,即每循环N次后进行一次查询当前节点状态到目标状态是否存在Reedshepp曲线,将N的大小设置为当前节点位置到目标位置欧式距离的四分之一。
步骤3:从open_list中选出F值最小的节点作为父节点,如果找到相应的Reedshepp曲线则不再扩展节点,并把当前节点作为最终节点,直接选取路径代价最小的Reedsheep曲线,并将曲线的路经点加入到最终的路径序列中。否则继续扩展子节点,如果子节点在close_list中,则舍弃,如果不在close_list中,而在open_list中,若子节点的G值比在open_list中相应节点的G值小,则更新为子节点。如果既不在close_list中,也不在open_list中,则直接将子节点加入到open_list中,并重新执行步骤3.3。
步骤4:最后将open_list中的所有节点和Reedshepp曲线连接中间点位姿与目标位姿的路径拼接作为Hybrid A*规划出的路径。
优选地,所述Reedshepp曲线,具体为:
ReedShepp曲线是一种路线规划方法。假设机器人能以固定的半径转向,且机器人能够前进和后退,那么ReedShepp曲线就是机器人在上述条件下从起点到终点的最短路径。Reeds-Shepp曲线由几段半径固定的圆弧和一段直线段拼接组成,而且圆弧的半径就是机器人的最小转向半径。
机器人的起点和终点的位置姿态是难以穷举的,所以一般在计算之前,会将机器人的姿态归一化。
假设机器人的初始姿态为qI=(x1,y1,θ1),目标姿态qG=(x2,y2,θ2),机器人的转向半径r=ρ
应用旋转矩阵,将机器人的起点朝向转到x轴正向:
将机器人转向半径缩放到1,于是最终得到机器人运动的起始姿态:
目标姿态:
进一步,所述F值,具体为:
F值的计算如下:
F=G+H
其中H是Hybrid A*算法中的启发式函数,在Hybrid A*算法中将启发式函数分成两种:无障碍物的非完整性约束启发代价C1和有障碍物的完整性启发式代价C2。一般取二者的最大值。
H=max{non holonomic no obstacles cost,2D Euclidean Distance}
其中非完整性约束启发代价结合了机器人的运动学约束,忽略了环境中的障碍物等信息,只考虑机器人的运动学特性,计算从该点到其他点的最短路径。这种启发式约束考虑了当前航向角以及转弯半径,可以确保机器人以正确的航向接近目标位姿。
有障碍物的完整性启发式代价则忽略了机器人的运动学特性,只考虑了障碍物。一般使用普通A*搜索计算每个节点到终点的最短路径,并使用欧几里得距离表示。
Claims (5)
1.一种结合运动学约束的移动机器人路径规划方法,其特征在于,所述方法包括以下步骤:
步骤1:输入数字高程地图,利用栅格法把数字高程地图划分成m×n大小的栅格地图,由输入的起始点Pstart和目标点Pgoal的世界坐标转换成对应的栅格坐标,获得栅格起始点和目标点;
步骤2:对栅格地图内的三维信息进行平面拟合和统计分析,得到该栅格的基本地貌特征的描述信息(如地形坡度、粗糙度等),依据可通过性分析评价准则,进一步完成其可通过性评价,给出各单元格的可通过性量化指标。计算可通过性指标需先拟合平面,采用最小二乘法拟合平面,即通过将误差的平方和最小化得到最佳的匹配值。假设拟合的平面方程的表达式为:
z=a0x+a1y+a2
其中,
已知拟合平面的法矢量后,可以根据以下式子得到平面坡度角β和侧滚角γ。
而该栅格的倾斜度dslope则为:
dslope=β
在得到栅格邻域内的拟合平面后,粗糙度由邻域内每个栅格到平面的距离的均方根表征。因此,可以根据以下式子计算得到。
其中,di表示第i个栅格到拟合平面的距离;
步骤3:使用结合运动学约束的Hybird A*算法在连续坐标系下进行启发式搜索得到初始全局路径,对初始全局路径进行平滑处理得到机器人实际移动路径。
由于研究的是差分轮式机器人,其不能直接向左向右平移,为此需考虑动力学约束,动力学约束公式如下:
其中,机器人的速度是机器人当前的朝向为θ。
2.根据权利要求1所述的一种结合运动学约束的移动机器人路径规划方法,其特征在于,所述步骤3中,对初始全局路径进行平滑处理得到机器人实际移动路径,具体为:
对路径的顶点坐标进行非线性优化规划问题的建模,采用共轭梯度法进行非参数化插值求解目标函数,求得实际移动路径。
3.根据权利要求1所述的一种结合运动学约束的移动机器人路径规划方法,其特征在于,所述步骤3中,使用Hybird A*算法得到初始全局路径,包括以下步骤:
步骤1:建立一个优先队列open_list和列表close_list,将起始栅格节点存入open_list中。
步骤2:设置开始进行Reedshepp曲线拓展的循环数N,即每循环N次后进行一次查询当前节点状态到目标状态是否存在Reedshepp曲线,将N的大小设置为当前节点位置到目标位置欧式距离的四分之一。
步骤3:从open_list中选出F值最小的节点作为父节点,如果找到相应的Reedshepp曲线则不再扩展节点,并把当前节点作为最终节点,直接选取路径代价最小的Reedsheep曲线,并将曲线的路经点加入到最终的路径序列中。否则继续扩展子节点,如果子节点在close_list中,则舍弃,如果不在close_list中,而在open_list中,若子节点的G值比在open_list中相应节点的G值小,则更新为子节点。如果既不在close_list中,也不在open_list中,则直接将子节点加入到open_list中,并重新执行步骤3。
步骤4:最后将open_list中的所有节点和Reedshepp曲线连接中间点位姿与目标位姿的路径拼接作为Hybrid A*规划出的路径。
4.根据权利要求3所述的一种结合运动学约束的移动机器人路径规划方法,其特征在于,Reedshepp曲线,具体为:
ReedShepp曲线是一种路线规划方法。假设机器人能以固定的半径转向,且机器人能够前进和后退,那么ReedShepp曲线就是机器人在上述条件下从起点到终点的最短路径。ReedsShepp曲线由几段半径固定的圆弧和一段直线段拼接组成,而且圆弧的半径就是机器人的最小转向半径。
机器人的起点和终点的位置姿态是难以穷举的,所以一般在计算之前,需要将机器人的姿态归一化。
假设机器人的初始姿态为qI=(x1,y1,θ1),目标姿态qG=(x2,y2,θ2),机器人的转向半径r=ρ
机器人运动的起始姿态:
目标姿态:
其中,。
5.根据权利要求3所述的一种结合运动学约束的移动机器人路径规划方法,其特征在于,F值,具体为:
F值的计算如下:
F=G+H
其中H是Hybrid A*算法中的启发式函数,在Hybrid A*算法中将启发式函数分成两种:无障碍物的非完整性约束启发代价和有障碍物的完整性启发式代价。一般取二者的最大值。启发式函数计算公式如下:
H=max{non holonomic no obstacles cost,2D Euclidean Distance}
其中非完整性约束启发代价结合了机器人的运动学约束,忽略了环境中的障碍物等信息,只考虑机器人的运动学特性,计算从该点到其他点的最短路径。这种启发式约束考虑了当前航向角以及转弯半径,可以确保机器人以正确的航向接近目标位姿。
有障碍物的完整性启发式代价则忽略了机器人的运动学特性,只考虑了障碍物。一般使用普通A*搜索计算每个节点到终点的最短路径,并使用欧几里得距离表示。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310160304.3A CN116009558A (zh) | 2023-02-20 | 2023-02-20 | 一种结合运动学约束的移动机器人路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310160304.3A CN116009558A (zh) | 2023-02-20 | 2023-02-20 | 一种结合运动学约束的移动机器人路径规划方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116009558A true CN116009558A (zh) | 2023-04-25 |
Family
ID=86030179
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310160304.3A Pending CN116009558A (zh) | 2023-02-20 | 2023-02-20 | 一种结合运动学约束的移动机器人路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116009558A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116540748A (zh) * | 2023-07-07 | 2023-08-04 | 上海仙工智能科技有限公司 | 一种在导航路径上规划机器人绕行路径的方法及*** |
-
2023
- 2023-02-20 CN CN202310160304.3A patent/CN116009558A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116540748A (zh) * | 2023-07-07 | 2023-08-04 | 上海仙工智能科技有限公司 | 一种在导航路径上规划机器人绕行路径的方法及*** |
CN116540748B (zh) * | 2023-07-07 | 2023-10-31 | 上海仙工智能科技有限公司 | 一种在导航路径上规划机器人绕行路径的方法及*** |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107702716B (zh) | 一种无人驾驶路径规划方法、***和装置 | |
CN109782779B (zh) | 基于种群超启发式算法的洋流环境下auv路径规划方法 | |
Ali et al. | Path planning of mobile robot with improved ant colony algorithm and MDP to produce smooth trajectory in grid-based environment | |
Lacaze et al. | Path planning for autonomous vehicles driving over rough terrain | |
CN109434831B (zh) | 机器人运行方法、装置、机器人、电子设备及可读介质 | |
Júnior et al. | EKF-LOAM: An adaptive fusion of LiDAR SLAM with wheel odometry and inertial data for confined spaces with few geometric features | |
Magid et al. | Voronoi-based trajectory optimization for UGV path planning | |
Huang et al. | Efficient anytime clf reactive planning system for a bipedal robot on undulating terrain | |
Ding et al. | Trajectory tracking of redundantly actuated mobile robot by MPC velocity control under steering strategy constraint | |
CN114779785A (zh) | 一种基于pso参数整定的移动机器人平滑轨迹规划方法 | |
CN116009558A (zh) | 一种结合运动学约束的移动机器人路径规划方法 | |
CN117705123B (zh) | 一种轨迹规划方法、装置、设备及存储介质 | |
Signifredi et al. | A general purpose approach for global and local path planning combination | |
Wu et al. | Real-time three-dimensional smooth path planning for unmanned aerial vehicles in completely unknown cluttered environments | |
Zhang et al. | Motion planning and tracking control of a four-wheel independently driven steered mobile robot with multiple maneuvering modes | |
Zhao et al. | A hybrid A* path planning algorithm based on multi-objective constraints | |
Huy et al. | A practical and optimal path planning for autonomous parking using fast marching algorithm and support vector machine | |
Yu et al. | RDT-RRT: Real-time double-tree rapidly-exploring random tree path planning for autonomous vehicles | |
Sulaiman et al. | Modeling of a wheeled humanoid robot and hybrid algorithm-based path planning of wheel base for the dynamic obstacles avoidance | |
Kazim et al. | Recent advances in path integral control for trajectory optimization: An overview in theoretical and algorithmic perspectives | |
Vilca et al. | An overall control strategy based on target reaching for the navigation of an urban electric vehicle | |
Hamdan et al. | Study of Path Planning Methods in Two-Dimensional Mapped Environments | |
Aravindan et al. | An integrated approach for path planning and control for autonomous mobile robots | |
Singh et al. | Planning stable trajectory on uneven terrain based on feasible acceleration count | |
Lim et al. | Safe Trajectory Path Planning Algorithm Based on Rrt* While Maintaining Moderate Margin from Obstacles |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication |