CN116909268B - 基于5g的代步机器人路径规划方法、装置、设备及介质 - Google Patents
基于5g的代步机器人路径规划方法、装置、设备及介质 Download PDFInfo
- Publication number
- CN116909268B CN116909268B CN202310803938.6A CN202310803938A CN116909268B CN 116909268 B CN116909268 B CN 116909268B CN 202310803938 A CN202310803938 A CN 202310803938A CN 116909268 B CN116909268 B CN 116909268B
- Authority
- CN
- China
- Prior art keywords
- walking robot
- node
- path
- new
- path planning
- 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
- 238000000034 method Methods 0.000 title claims abstract description 54
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 43
- 238000005070 sampling Methods 0.000 claims abstract description 30
- 238000005516 engineering process Methods 0.000 claims abstract description 14
- 230000008447 perception Effects 0.000 claims abstract description 12
- 230000006870 function Effects 0.000 claims description 44
- 238000004590 computer program Methods 0.000 claims description 24
- 238000003860 storage Methods 0.000 claims description 12
- 230000001133 acceleration Effects 0.000 claims description 10
- 238000005265 energy consumption Methods 0.000 claims description 8
- 238000012986 modification Methods 0.000 claims description 7
- 230000004048 modification Effects 0.000 claims description 7
- 239000011159 matrix material Substances 0.000 claims description 6
- 238000007789 sealing Methods 0.000 claims description 6
- 230000004888 barrier function Effects 0.000 claims description 5
- 238000002939 conjugate gradient method Methods 0.000 claims description 5
- 238000011156 evaluation Methods 0.000 claims description 5
- 238000009499 grossing Methods 0.000 claims description 5
- 238000007781 pre-processing Methods 0.000 claims description 5
- 238000010586 diagram Methods 0.000 description 12
- 239000010931 gold Substances 0.000 description 7
- 229910052737 gold Inorganic materials 0.000 description 7
- 238000005457 optimization Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 6
- 238000012545 processing Methods 0.000 description 4
- 230000004927 fusion Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- PCHJSUWPFVWCPO-UHFFFAOYSA-N gold Chemical compound [Au] PCHJSUWPFVWCPO-UHFFFAOYSA-N 0.000 description 2
- 238000011478 gradient descent method Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000035772 mutation Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种基于5G的代步机器人路径规划方法、装置、设备及介质，通过根据代步机器人的性能参数构建包含横向和横摆二个方向自由度的代步机器人系统模型；采用激光SLAM技术将地理坐标转换到平面坐标，并进行多源感知数据的融合，生成高精地图；采用混合A*算法理论进行代步机器人的全局路径规划；采用采样算法对全局路径进行局部避障轨迹规划，确定避障路径。能够解决往返搜索的问题，提升路径规划效率和精度。
Description
技术领域
本发明涉及机器人路径规划技术领域，尤其涉及基于5G的代步机器人路径规划方法、装置、设备及介质。
背景技术
随着社会的发展和科技的进步，人们在日常生活中对于服务型机器人的需求快速增长，代步机器人是其中一个重要的发展方向。代步工具与基于5G专网技术结合，可以极大地提升人们的出行体验，尤其是满足大型交通设施对于行人旅行安全性和行李便携性的特殊需求。代步机器人的全局路径规划是在已知障碍物位置的情况下，在环境中规划一条连接起始点和目标终点的可行路径。
机器人的动态轨迹规划是在全局路径的基础上，机器人在路径跟踪过程中针对随时动态出现的障碍物，重新规划出一条躲避障碍物的避障路径的方案。但是现有的启发式算法存在往返搜索的问题，会造成搜索节点过多的情况，搜索效率较低，影响路径规划效率和精度。
发明内容
为解决上述技术问题，本发明提供一种基于5G的代步机器人路径规划方法、装置、设备及介质，能够解决往返搜索的问题，提升路径规划效率和精度。
本发明实施例提供一种基于5G的代步机器人路径规划方法，所述方法包括：
根据代步机器人的性能参数构建包含横向和横摆二个方向自由度的代步机器人系统模型；
采用激光SLAM技术将地理坐标转换到平面坐标，并进行多源感知数据的融合，生成高精地图；
采用混合A*算法理论进行代步机器人的全局路径规划；
采用采样算法对全局路径进行局部避障轨迹规划，确定避障路径。
优选地，所述代步机器人系统模型中系统状态空间模型为：：
所述代步机器人系统模型中轮胎垂向载荷模型为：
其中，Al、x、B分别为，δf为代步机器人的前轮转向角，x＝[β γ]T， m为代步机器人质量，vx为代步机器人质心处的纵向速度，Cf为前轮侧偏刚度，Cr后轮侧偏刚度；lf为代步机器人前轴到质心处的距离，lr为代步机器人后轴到质心处的距离，Iz为代步机器人绕Z轴的转动惯量，β为代步机器人质心处的质心侧偏角，γ为代步机器人质心处的横摆角速度；Fzf为前轮载荷，Fzr为后轮载荷，h为代步机器人质心高度，g为重力加速度，ax为代步机器人质心的纵向加速度。
作为一种优选方案，所述利用激光SLAM技术将地理坐标转换到平面坐标，并进行多源感知数据的融合，生成高精地图，具体包括：
通过激光雷达获取周边环境的激光点云，并通过GPS和IMU获取代步机器人的位置和姿态信息；
构建激光雷达的坐标系中的坐标与代步机器人在世界坐标系的坐标对应关系，确定高精地图汇总的齐次坐标；
其中，对应关系为P＝RPw+T，[X,Y,Z]T为激光雷达的坐标系中的坐标，[Xw,Yw,Zw]T为世界坐标系的坐标，齐次坐标R和T为变化参数，P为激光雷达的坐标系中的任一点，Pw为P在世界坐标系下的对应点。
优选地，所述采用混合A*算法理论进行代步机器人的全局路径规划，具体包括：
对栅格地图进行了预处理，对凹型障碍物区域进行封闭处理；
采用混合A*算法进行全局路径规划，并采用启发函数进行搜索，考虑横向、纵向与斜向45度的拓展的节点拓展，并添加航向角的约束，确定全局路径；其中，所述启发函数为f(n)＝g(n)+max(h1(n),h2(n))+h(p)，g(n)为起点到当前节点n的距离，h1(n)为启发函数的环境约束，h2(n)为启发函数的非完整约束部分，h(p)为父辈节点的约束。
优选地，所述采用采样算法对全局路径进行局部避障轨迹规划，确定避障路径，具体包括：
步骤S401，以代步机器人的初始节点QiNIT初始化随机树T；
步骤S402，在无障碍空间Xtree中均匀采样一个随机节点qrand；
步骤S403，通过距离评估函数遍历随机树T中节点，确定与随机节点qrand距离最近的最近节点qnearest；
步骤S404，从最近节点qnearest向随机节点qrand前进单位步长，生成新节点qnew；
步骤S405，如果从最近节点qnearest到新节点qnew之间没有发生碰撞，则将新节点qnew以及最近节点qnearest到新节点qnew的边添加到随机树T中；如果最近节点qnearest到新节点qnew之间发生碰撞，则抛弃新节点qnew，不对随机树T做任何修改；
步骤S406，当搜索到的新节点qnew不为目标节点qgoal且未进入预设的目标区域，且采样次数未达到预设上限时，返回步骤S402；
步骤S407，当搜索到的新节点qnew进入预设的目标区域，或采样次数达到预设上限时，返回搜索失败；
步骤S408，当搜索到的新节点qnew为目标节点qgoal时，通过回溯法在随机树T中得到从目标节点qgoal开始到初始节点qinit结束的规划路径，作为避障路径。
作为一种优选方案，所述方法还包括：
将路径规划的二次问题转化为二次规划问题，通过共轭梯度法计算符合二次规划问题的非零且共轭的向量组，对避障路径进行平滑处理。
优选地，所述代步机器人系统模型进行路径规划的具体指标包括：路径长度最短、能量消耗最小、规划无碰撞路径以及延迟时间最小。
本发明实施例还提供一种基于5G的代步机器人路径规划装置，所述装置包括：
模型构建模块，用于根据代步机器人的性能参数构建包含横向和横摆二个方向自由度的代步机器人系统模型；
地图生成模块，用于采用激光SLAM技术将地理坐标转换到平面坐标，并进行多源感知数据的融合，生成高精地图；
全局规划模块，用于采用混合A*算法理论进行代步机器人的全局路径规划；
局部规划模块，用于采用采样算法对全局路径进行局部避障轨迹规划，确定避障路径。
优选地，所述代步机器人系统模型中系统状态空间模型为：：
所述代步机器人系统模型中轮胎垂向载荷模型为：
其中，Al、x、B分别为，δf为代步机器人的前轮转向角，x＝[β γ]T，m为代步机器人质量，vx为代步机器人质心处的纵向速度，Cf为前轮侧偏刚度，Cr后轮侧偏刚度；lf为代步机器人前轴到质心处的距离，lr为代步机器人后轴到质心处的距离，Iz为代步机器人绕Z轴的转动惯量，β为代步机器人质心处的质心侧偏角，γ为代步机器人质心处的横摆角速度；Fzf为前轮载荷，Fzr为后轮载荷，h为代步机器人质心高度，g为重力加速度，ax为代步机器人质心的纵向加速度。
进一步地，所述地图生成模块具体用于：
通过激光雷达获取周边环境的激光点云，并通过GPS和IMU获取代步机器人的位置和姿态信息；
构建激光雷达的坐标系中的坐标与代步机器人在世界坐标系的坐标对应关系，确定高精地图汇总的齐次坐标；
其中，对应关系为P＝RPw+T，[X,Y,Z]T为激光雷达的坐标系中的坐标，[Xw,Yw,Zw]T为世界坐标系的坐标，齐次坐标R和T为变化参数，P为激光雷达的坐标系中的任一点，Pw为P在世界坐标系下的对应点。
优选地，所述全局规划模块具体用于：
对栅格地图进行了预处理，对凹型障碍物区域进行封闭处理；
采用混合A*算法进行全局路径规划，并采用启发函数进行搜索，考虑横向、纵向与斜向45度的拓展的节点拓展，并添加航向角的约束，确定全局路径；
其中，所述启发函数为f(n)＝g(n)+max(h1(n),h2(n))+h(p)，g(n)为起点到当前节点n的距离，h1(n)为启发函数的环境约束，h2(n)为启发函数的非完整约束部分，h(p)为父辈节点的约束。
优选地，所述局部规划模块具体用于：
步骤S401，以代步机器人的初始节点qinit初始化随机树T；
步骤S402，在无障碍空间Xtree中均匀采样一个随机节点qrand；
步骤S403，通过距离评估函数遍历随机树T中节点，确定与随机节点qrand距离最近的最近节点qnearest；
步骤S404，从最近节点qnearest向随机节点qrand前进单位步长，生成新节点qnew；
步骤S405，如果从最近节点qnearest到新节点qnew之间没有发生碰撞，则将新节点qnew以及最近节点qnearest到新节点qnew的边添加到随机树T中；如果最近节点qnearest到新节点qnew之间发生碰撞，则抛弃新节点qnew，不对随机树i做任何修改；
步骤S406，当搜索到的新节点qnew不为目标节点qgoal且未进入预设的目标区域，且采样次数未达到预设上限时，返回步骤S402；
步骤S407，当搜索到的新节点qnew进入预设的目标区域，或采样次数达到预设上限时，返回搜索失败；
步骤S408，当搜索到的新节点qnew为目标节点qgoal时，通过回溯法在随机树T中得到从目标节点qgoal开始到初始节点qinit结束的规划路径，作为避障路径。
优选地，所述装置还包括路径平滑模块，用于：
将路径规划的二次问题转化为二次规划问题，通过共轭梯度法计算符合二次规划问题的非零且共轭的向量组，对避障路径进行平滑处理。
优选地，所述代步机器人系统模型进行路径规划的具体指标包括：路径长度最短、能量消耗最小、规划无碰撞路径以及延迟时间最小。
本发明另一实施例提供一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行上述发明实施例所述的基于5G的代步机器人路径规划方法。
本发明另一实施例提供了一种基于5G的代步机器人路径规划装置，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现上述发明实施例所述的基于5G的代步机器人路径规划方法。
与现有技术相比，本发明提供的一种基于5G的代步机器人路径规划方法、装置、设备及介质，通过根据代步机器人的性能参数构建包含横向和横摆二个方向自由度的代步机器人系统模型；采用激光SLAM技术将地理坐标转换到平面坐标，并进行多源感知数据的融合，生成高精地图；采用混合A*算法理论进行代步机器人的全局路径规划；采用采样算法对全局路径进行局部避障轨迹规划，确定避障路径。能够解决往返搜索的问题，提升路径规划效率和精度。
附图说明
图1是本发明实施例提供的一种基于5G的代步机器人路径规划方法的流程示意图；
图2是本发明实施例提供的栅格地图的示意图；
图3是本发明实施例提供的避障路径计算流程示意图；
图4是本发明实施例提供的基于5G的代步机器人路径规划方法的工作原理示意图；
图5是本发明实施例提供的一种基于5G的代步机器人路径规划装置的结构示意图；
图6是本发明另一实施例提供的一种基于5G的代步机器人路径规划装置的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明实施例提供一种基于5G的代步机器人路径规划方法，参见图1，是本发明实施例提供的一种基于5G的代步机器人路径规划方法的流程示意图，所述方法步骤S1～S4：
S1，根据代步机器人的性能参数构建包含横向和横摆二个方向自由度的代步机器人系统模型；
S2，采用激光SLAM技术将地理坐标转换到平面坐标，并进行多源感知数据的融合，生成高精地图；
S3，采用混合A*算法理论进行代步机器人的全局路径规划；
S4，采用采样算法对全局路径进行局部避障轨迹规划，确定避障路径。
在本实施例具体实施时，根据代步机器人的性能参数基于5G云平台构建包含横向和横摆二个方向自由度的代步机器人系统模型，用于后期验证所规划路径的可行性；
基于5G云平台利用激光SLAM技术将地理坐标转换到平面坐标，并进行多源感知数据的融合，进行高精地图的采集和制作；
利用混合A*算法理论进行代步机器人的全局路径规划；
在全局路径规划的基础上采用采样算法进行局部避障轨迹规划，得到避障路径。
本发明考虑横向和横摆二个方向自由度，并通过激光SLAM技术能够精确实现从地理坐标到平面坐标的坐标转换过程，增加代步机器人路径规划的精确性；采用混合A*算法，能够减少往返搜索情况，提高拓展节点的连续性，增加算法搜索的效率，基于采样法的局部轨迹规划算法，能够控制其随机性的同时提高搜索效率。
在本发明提供的又一实施例中，所述代步机器人系统模型中系统状态空间模型为：
所述代步机器人系统模型中轮胎垂向载荷模型为：
其中，Al、x、B分别为，δf为代步机器人的前轮转向角，x＝[β γ]T，m为代步机器人质量，vx为代步机器人质心处的纵向速度，Cf为前轮侧偏刚度，Cr后轮侧偏刚度；lf为代步机器人前轴到质心处的距离，lr为代步机器人后轴到质心处的距离，Iz为代步机器人绕Z轴的转动惯量，β为代步机器人质心处的质心侧偏角，γ为代步机器人质心处的横摆角速度；Fzf为前轮载荷，Fzr为后轮载荷，h为代步机器人质心高度，g为重力加速度，ax为代步机器人质心的纵向加速度。
在本实施例具体实施时，以MATLAB Simulink为平台搭建数据处理模型，根据牛顿第二定律和力矩平衡方程进行分析，二自由度代步机器人模型可以表示为：
其中，m为机器人质量；vx为代步机器人质心处的纵向速度；Cf为前轮侧偏刚度；Cr为后轮侧偏刚度；lf为代步机器人前轴到质心处的距离；lr为代步机器人后轴到质心处的距离；Iz为代步机器人绕Z轴的转动惯量；δf为代步机器人的前轮转向角；β为代步机器人质心处的质心侧偏角；γ为代步机器人质心处的横摆角速度。
为了方便控制器设计，对上式进行换算，得到系统状态空间模型：
其中， Al、x、B分别为；
所述代步机器人系统模型中轮胎垂向载荷模型为：
式中，Fzf为前轮载荷，Fzr为后轮载荷，h为代步机器人质心高度，g为重力加速度，ax为代步机器人质心的纵向加速度。
在本发明提供的又一实施例中，所述步骤S2具体包括：
通过激光雷达获取周边环境的激光点云，并通过GPS和IMU获取代步机器人的位置和姿态信息；
构建激光雷达的坐标系中的坐标与代步机器人在世界坐标系的坐标对应关系，确定高精地图汇总的齐次坐标；
其中，对应关系为P＝RPw+T，[X,Y,Z]T为激光雷达的坐标系中的坐标，[Xw,Yw,Zw]T为世界坐标系的坐标，齐次坐标R和T为变化参数，P为激光雷达的坐标系中的任一点，Pw为P在世界坐标系下的对应点。
在本实施例具体实施时，基于5G云平台的激光SLAM技术能够精确实现从地理坐标到平面坐标的坐标转换过程；5G云平台模块共包含两个子模块，分别是云端感知融合与定位模块以及云端路径规划模块。
云端感知融合与定位模块不需要去进行实际的感知信息采集工作，未配备各种传感器设备，其主要工作是对代步机器人端和路侧上传到云端的感知数据进行融合，并将融合后的感知数据和车辆位置信息一起作为输入参数给到云端路径规划模块。
云端路径规划模块主要工作是根据云端感知融合与定位模块给的感知信息为车辆规划一条合适的路线使其能够代步无人驾驶；
生成高精地图过程具体包括：
在需要采集的区域内慢速运行并采集信息，激光雷达可以获取停车场周边环境的激光点云，GPS、IMU获取代步机器人的位置和姿态信息。激光雷达的坐标系中的坐标[X,Y,Z]T与其在世界坐标系Ow-xw-yw-zw中的坐标[Xw,Yw,Zw]T存在的对应关系为P＝RPw+T；
确定高精地图汇总的齐次坐标
其中，R和T为变化参数，P为激光雷达的坐标系中的任一点，Pw为P在世界坐标系下的对应点。
在本发明提供的又一实施例中，所述步骤S3具体包括：
对栅格地图进行了预处理，对凹型障碍物区域进行封闭处理；
采用混合A*算法进行全局路径规划，并采用启发函数进行搜索，考虑横向、纵向与斜向45度的拓展的节点拓展，并添加航向角的约束，确定全局路径；
其中，所述启发函数为f(n)＝g(n)+max(h1(n),h2(n))+h(p)，g(n)为起点到当前节点n的距离，h1(n)为启发函数的环境约束，h2(n)为启发函数的非完整约束部分，h(p)为父辈节点的约束。
在本实施例具体实施时，利用混合A*算法理论进行代步机器人的全局路径规划，估计函数的表达式如下：f(n)＝g(n)+h(n),h(n)≤h*(n)；
其中，g(n)表示从起始节点start到节点n的真实耗费值，h(n)表示从节点n到终止点destination的启发估计耗费值；所以，f(n)表示从起始点开始，经过节点n到达目标的启发估计耗费值；h*(n)是指经过节点n，到达终止目标点的实际最优消耗值；只有在满足h(n)≤h*(n)的条件下，A*算法才能保证是最优性和完备性。一般情况下，选择的启发函数都是可以满足h(n)≤h*(n)的。h(n)可采纳就一定能找到最短路径，但h(n)与实际值h*(n)不能差得太远；差得越远，A*最后的搜索拓扑就越接近一个完全的宽度优先搜索；最极端的情况：h(n)＝0时，A*就完全退化为宽度优先搜索。那么，h(n)要尽可能接近h*(n)；理论上来说，h(n)＝h*(n)是最好的，估计值就是实际值，但这在实际实施时无法做到；
常用估计函数包括曼哈顿距离函数h(n)＝abs(n.x-goal.x)+abs(n.y-goal.y)，切比雪夫距离函数h(n)＝MAX(abs(n.x-goal.x),abs(n.y-goal.y))，以及欧几里得距离函数h(n)＝sqrt((n.x-goal.x)2+(n.y-goal.y)2)。估计函数中n.x和n.y分别表示节点n的x坐标和y坐标，goal.x和goal.y分别表示目标节点goal的x坐标和y坐标；
本实施例对传统A*算法进行改进，全局路径规划过程具体包括：
对高精地图中栅格地图进行了预处理，参见图2，是本发明实施例提供的栅格地图的示意图，对凹型障碍物区域进行封闭处理，避免车辆在凹型障碍物内部进行节点搜索。
采用混合A*算法进行路径规划，改进启发函数，减少往返搜索情况，增加算法搜索的效率；在进行节点拓展时，除了考虑横向、纵向与斜向45度的拓展，还加入了对航向角的约束，可以保证生成的相邻路径点航向角是连续的；
其中改进后的A*算法启发函数为f(n)＝g(n)+max(h1(n),h2(n))+h(p)
其中，g(n)为起点到当前节点的距离，h1(n)为启发函数的环境约束，h2(n)为启发函数的非完整约束部分，h(p)为父辈节点的约束。
本实施例采用混合A*算法，能够减少往返搜索情况，提高拓展节点的连续性，增加算法搜索的效率。
在本发明提供的又一实施例中，参见图3，是本发明实施例提供的避障路径计算流程示意图，所述步骤S4具体包括：
步骤S401，以代步机器人的初始节点qinit初始化随机树T；
步骤S402，在无障碍空间Xtree中均匀采样一个随机节点qrand；
步骤S403，通过距离评估函数遍历随机树T中节点，确定与随机节点qrand距离最近的最近节点qnearest；
步骤S404，从最近节点qnearest向随机节点qrand前进单位步长，生成新节点qnew；
步骤S405，如果从最近节点qnearest到新节点qnew之间没有发生碰撞，则将新节点qnew以及最近节点qnearest到新节点qnew的边添加到随机树T中；如果最近节点qnearest到新节点qnew之间发生碰撞，则抛弃新节点qnew，不对随机树T做任何修改；
步骤S406，当搜索到的新节点qnew不为目标节点qgoal且未进入预设的目标区域，且采样次数未达到预设上限时，返回步骤S402；
步骤S407，当搜索到的新节点qnew进入预设的目标区域，或采样次数达到预设上限时，返回搜索失败；
步骤S408，当搜索到的新节点qnew为目标节点qgoal时，通过回溯法在随机树T中得到从目标节点qgoal开始到初始节点qinit结束的规划路径，作为避障路径。
在本实施例具体实施时，利用采样算法进行进一步的局部避障轨迹规划，基本步骤如下：
代步机器人的状态表示方法为q＝(x,y,θ)，以代步机器人的初始节点qinit初始化随机树T；
在无障碍空间Xtree中均匀采样一个随机节点qrand；
通过距离评估函数遍历随机树T中节点，确定与随机节点qrand距离最近的最近节点qnearest；例如，当采用两个节点之间的欧几里得距离进行距离评估时，最近节点qnearest为随机树T中到随机节点qrand的欧氏距离最近的节点。
从最近节点qnearest向随机节点qrand前进单位步长生成新节点qnew；
对生成的新节点的步长路径进行判断，如果从最近节点qnearest到新节点qnew之间没有发生碰撞，则将新节点qnew以及最近节点qnearest到新节点qnew的边添加到随机树T中；如果最近节点qnearest到新节点qnew之间发生碰撞，则抛弃新节点qnew，不对随机树T做任何修改；
当搜索到的新节点qnew不为目标节点qgoal且未进入预设的目标区域，且采样次数未达到预设上限时，不断重复以上随机采样和生成新节点，以及对新节点进行判断的步骤，直到新节点包含了目标节点或进入了目标区域或者达到了设定采样次数的上限；
当搜索到的新节点Qnew进入预设的目标区域，或采样次数达到预设上限时，返回搜索失败；
当搜索到的新节点Qnew为目标节点Qgoal时，通过回溯法在随机树T中得到从目标节点qgoal开始到初始节点Qinit结束的规划路径，作为避障路径。
在本发明提供的又一实施例中，所述方法还包括：
将路径规划的二次问题转化为二次规划问题，通过共轭梯度法计算符合二次规划问题的非零且共轭的向量组，对避障路径进行平滑处理。
在本实施例具体实施时，利用非线性共轭梯度优化方法平滑生成的轨迹，所采用的共轭梯度优化方法，其的性能介于梯度下降法和牛顿法之间，对于二次规划中的n维问题，它还能在n次迭代之内找到最优解，具体原理如下：
对于二次问题
其中，Q为正定矩阵，b为任意矩阵。
由于函数中的Q是正定的，因此原问题等价于存在任意矩阵A使得：
将二次问题转化为二次规划问题，共轭梯度法主要就是找到一组非零且共轭的向量组，与正定矩阵Q共轭且线性无关。
共轭梯度优化方法的性能介于梯度下降法和牛顿法之间，对于二次规划中的n维问题，它还能在n次迭代之内找到最优解，通过共轭梯度优化方法，能够使轨迹点更加连续平滑，从而减少安全事故的发生，增加所规划路径的实用性。
在本发明提供的又一实施例中，所述代步机器人系统模型进行路径规划的具体指标包括：路径长度最短、能量消耗最小、规划无碰撞路径以及延迟时间最小。
在本实施例具体实施时，代步机器人系统模型主要的控制目标，即在保证代步机器人安全性的同时，改进传统A*算法存在的节点突变和往返搜索问题，最大限度提高代步机器人路径规划的精确性和效率，路径规划的具体指标如下：
路径长度最短，即使得机器人走过的节点数最少；
能量消耗最小，即使得代步机器人所走过的路径能量消耗是最低的；
对于多代步机器人路径规划问题时，要多个机器人合作，规划无碰撞的优化路径，性能优化指标如下：
延迟时间最小，即为各个机器人规划协调路径，使得对于某个机器人或全部机器人，行进过程中的暂定时间之和最小。
能量消耗最小，即为各个机器人协调的路径，使得对于某个机器人或全部机器人，行进消耗能量最小。
路径长度最短，即为各个机器人规划协调的路径，使得某个机器人或全部机器人的路径最短。这个指标也可以作为机器人行进时间和能量的间接指标。
路径规划的具体指标在保证代步机器人安全性的同时，最大限度提高代步机器人路径规划的精确性和效率。
在本发明提供的又一实施例中，参见图4，是本发明实施例提供的基于5G的代步机器人路径规划方法的工作原理示意图；
在图4中，代步机器人输入信号输入性能参数用于建立二自由度模型，二自由度模型在路径规划部分，通过基于栅格地图的混合A*算法进行全局路径规划，通过改进RRT算法进行局部轨迹规划，还通过非线性共轨梯度优化平滑轨迹，确定避障路径，输出至运动控制器，运动控制器输出信号至代步机器人的执行机构，实现代步机器人的运动轨迹控制。
在本发明另一实施例提供中，参见图5，是本发明实施例提供的一种基于5G的代步机器人路径规划装置的结构示意图，所述装置包括：
模型构建模块，用于根据代步机器人的性能参数构建包含横向和横摆二个方向自由度的代步机器人系统模型；
地图生成模块，用于采用激光SLAM技术将地理坐标转换到平面坐标，并进行多源感知数据的融合，生成高精地图；
全局规划模块，用于采用混合A*算法理论进行代步机器人的全局路径规划；
局部规划模块，用于采用采样算法对全局路径进行局部避障轨迹规划，确定避障路径。
需要说明的是，本发明实施例提供的一种基于5G的代步机器人路径规划装置用于执行上述实施例的一种基于5G的代步机器人路径规划方法的所有流程步骤，两者的工作原理和有益效果一一对应，因而不再赘述。
参见图6，是本发明另一实施例提供的一种基于5G的代步机器人路径规划装置的结构示意图。该实施例的基于5G的代步机器人路径规划装置包括：处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，例如基于5G的代步机器人路径规划程序。所述处理器执行所述计算机程序时实现上述各个基于5G的代步机器人路径规划方法实施例中的步骤，例如图1所示的步骤S1～S4。或者，所述处理器执行所述计算机程序时实现上述各装置实施例中各模块的功能。
示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器中，并由所述处理器执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述基于5G的代步机器人路径规划装置中的执行过程。例如，所述计算机程序可以被分割成多个模块，各模块具体功能再次不作赘述。
所述基于5G的代步机器人路径规划装置可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述基于5G的代步机器人路径规划装置可包括，但不仅限于，处理器、存储器。本领域技术人员可以理解，所述示意图仅仅是基于5G的代步机器人路径规划装置的示例，并不构成对基于5G的代步机器人路径规划装置的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述基于5G的代步机器人路径规划装置还可以包括输入输出设备、网络接入设备、总线等。
所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述基于5G的代步机器人路径规划装置的控制中心，利用各种接口和线路连接整个基于5G的代步机器人路径规划装置的各个部分。
所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述基于5G的代步机器人路径规划装置的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(SecureDigital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
其中，所述基于5G的代步机器人路径规划装置集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。
需说明的是，以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外，本发明提供的装置实施例附图中，模块之间的连接关系表示它们之间具有通信连接，具体可以实现为一条或多条通信总线或信号线。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。
Claims (9)
1.一种基于5G的代步机器人路径规划方法，其特征在于，所述方法包括：
根据代步机器人的性能参数构建包含横向和横摆二个方向自由度的代步机器人系统模型；
采用激光SLAM技术将地理坐标转换到平面坐标，并进行多源感知数据的融合，生成高精地图；
采用混合A*算法理论进行代步机器人的全局路径规划；
采用采样算法对全局路径进行局部避障轨迹规划，确定避障路径；
所述采用混合A*算法理论进行代步机器人的全局路径规划，具体包括：
对栅格地图进行了预处理，对凹型障碍物区域进行封闭处理；
采用混合A*算法进行全局路径规划，并采用启发函数进行搜索，考虑横向、纵向与斜向45度的拓展的节点拓展，并添加航向角的约束，确定全局路径；
其中，所述启发函数为f(n)＝g(n)+max(h1(n)，h2(n))+h(p)，g(n)为起点到当前节点n的距离，h1(n)为启发函数的环境约束，h2(n)为启发函数的非完整约束部分，h(p)为父辈节点的约束。
2.如权利要求1所述的基于5G的代步机器人路径规划方法，其特征在于，所述代步机器人系统模型中系统状态空间模型为：
所述代步机器人系统模型中轮胎垂向载荷模型为：
其中，为状态向量x求导、Al是系数矩阵，Bl是输入矩阵，δf为代步机器人的前轮转向角，/>x＝[β γ]T，m为代步机器人质量，vx为代步机器人质心处的纵向速度，Cf为前轮侧偏刚度，Cr后轮侧偏刚度；lf为代步机器人前轴到质心处的距离，lr为代步机器人后轴到质心处的距离，Iz为代步机器人绕Z轴的转动惯量，β为代步机器人质心处的质心侧偏角，γ为代步机器人质心处的横摆角速度；Fzf为前轮载荷，Fzr为后轮载荷，h为代步机器人质心高度，g为重力加速度，ax为代步机器人质心的纵向加速度。
3.如权利要求1所述的基于5G的代步机器人路径规划方法，其特征在于，所述采用激光SLAM技术将地理坐标转换到平面坐标，并进行多源感知数据的融合，生成高精地图，具体包括：
通过激光雷达获取周边环境的激光点云，并通过GPS和IMU获取代步机器人的位置和姿态信息；
构建激光雷达的坐标系中的坐标与代步机器人在世界坐标系的坐标对应关系，确定高精地图汇总的齐次坐标；
其中，对应关系为P＝RPw+T，[X，Y，Z]T为激光雷达的坐标系中的坐标，[Xw，Yw，Zw]T为世界坐标系的坐标，齐次坐标R和T为变化参数，P为激光雷达的坐标系中的任一点，Pw为P在世界坐标系下的对应点。
4.如权利要求1所述的基于5G的代步机器人路径规划方法，其特征在于，
所述采用采样算法对全局路径进行局部避障轨迹规划，确定避障路径，具体包括：
步骤S401，以代步机器人的初始节点qinit初始化随机树T；
步骤S402，在无障碍空间Xtree中均匀采样一个随机节点qrand；
步骤S403，通过距离评估函数遍历随机树T中节点，确定与随机节点qrand距离最近的最近节点qnearest；
步骤S404，从最近节点qnearest向随机节点qrand前进单位步长，生成新节点qnew；
步骤S405，如果从最近节点qnearest到新节点qnew之间没有发生碰撞，则将新节点qnew以及最近节点qnearest到新节点qnew的边添加到随机树T中；如果最近节点qnearest到新节点qnew之间发生碰撞，则抛弃新节点qnew，不对随机树T做任何修改；
步骤S406，当搜索到的新节点qnew不为目标节点qgoal且未进入预设的目标区域，且采样次数未达到预设上限时，返回步骤S402；
步骤S407，当搜索到的新节点qnew进入预设的目标区域，或采样次数达到预设上限时，返回搜索失败；
步骤S408，当搜索到的新节点qnew为目标节点qgoal时，通过回溯法在随机树T中得到从目标节点qgoal开始到初始节点qinit结束的规划路径，作为避障路径。
5.如权利要求1所述的基于5G的代步机器人路径规划方法，其特征在于，所述方法还包括：
将路径规划的二次问题转化为二次规划问题，通过共轭梯度法计算符合二次规划问题的非零且共轭的向量组，对避障路径进行平滑处理。
6.如权利要求1所述的基于5G的代步机器人路径规划方法，其特征在于，所述代步机器人系统模型进行路径规划的具体指标包括：路径长度最短、能量消耗最小、规划无碰撞路径以及延迟时间最小。
7.一种基于5G的代步机器人路径规划装置，其特征在于，所述装置包括：
模型构建模块，用于根据代步机器人的性能参数构建包含横向和横摆二个方向自由度的代步机器人系统模型；
地图生成模块，用于采用激光SLAM技术将地理坐标转换到平面坐标，并进行多源感知数据的融合，生成高精地图；
全局规划模块，用于采用混合A*算法理论进行代步机器人的全局路径规划；
局部规划模块，用于采用采样算法对全局路径进行局部避障轨迹规划，确定避障路径；
所述全局规划模块具体用于：
对栅格地图进行了预处理，对凹型障碍物区域进行封闭处理；
采用混合A*算法进行全局路径规划，并采用启发函数进行搜索，考虑横向、纵向与斜向45度的拓展的节点拓展，并添加航向角的约束，确定全局路径；
其中，所述启发函数为f(n)＝g(n)+max(h1(n)，h2(n))+h(p)，g(n)为起点到当前节点n的距离，h1(n)为启发函数的环境约束，h2(n)为启发函数的非完整约束部分，h(p)为父辈节点的约束。
8.一种基于5G的代步机器人路径规划装置，其特征在于，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至6中任意一项所述的基于5G的代步机器人路径规划方法。
9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如权利要求1至6中任意一项所述的基于5G的代步机器人路径规划方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310803938.6A CN116909268B (zh) | 2023-06-30 | 2023-06-30 | 基于5g的代步机器人路径规划方法、装置、设备及介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310803938.6A CN116909268B (zh) | 2023-06-30 | 2023-06-30 | 基于5g的代步机器人路径规划方法、装置、设备及介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116909268A CN116909268A (zh) | 2023-10-20 |
| CN116909268B true CN116909268B (zh) | 2024-05-28 |
Family
ID=88362065
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310803938.6A Active CN116909268B (zh) | 2023-06-30 | 2023-06-30 | 基于5g的代步机器人路径规划方法、装置、设备及介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116909268B (zh) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111845774A (zh) * | 2020-07-20 | 2020-10-30 | 上海大学 | 一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法 |
| CN113465625A (zh) * | 2021-08-17 | 2021-10-01 | 清华大学 | 局部路径规划方法和装置 |
| CN113467456A (zh) * | 2021-07-07 | 2021-10-01 | 中国科学院合肥物质科学研究院 | 一种未知环境下用于特定目标搜索的路径规划方法 |
| CN113932823A (zh) * | 2021-09-23 | 2022-01-14 | 同济大学 | 基于语义道路地图的无人驾驶多目标点轨迹并行规划方法 |
| CN114812581A (zh) * | 2022-06-23 | 2022-07-29 | 中国科学院合肥物质科学研究院 | 一种基于多传感器融合的越野环境导航方法 |
| CN115373399A (zh) * | 2022-09-13 | 2022-11-22 | 中国安全生产科学研究院 | 一种基于空地协同的地面机器人路径规划方法 |
| CN115454083A (zh) * | 2022-09-23 | 2022-12-09 | 福州大学 | 基于快速扩展随机树与动态窗口法的双层路径规划方法 |
| CN116331256A (zh) * | 2023-03-13 | 2023-06-27 | 东南大学 | 分布式驱动电动汽车换道轨迹规划方法、设备及存储介质 |
-
2023
- 2023-06-30 CN CN202310803938.6A patent/CN116909268B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111845774A (zh) * | 2020-07-20 | 2020-10-30 | 上海大学 | 一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法 |
| CN113467456A (zh) * | 2021-07-07 | 2021-10-01 | 中国科学院合肥物质科学研究院 | 一种未知环境下用于特定目标搜索的路径规划方法 |
| CN113465625A (zh) * | 2021-08-17 | 2021-10-01 | 清华大学 | 局部路径规划方法和装置 |
| CN113932823A (zh) * | 2021-09-23 | 2022-01-14 | 同济大学 | 基于语义道路地图的无人驾驶多目标点轨迹并行规划方法 |
| CN114812581A (zh) * | 2022-06-23 | 2022-07-29 | 中国科学院合肥物质科学研究院 | 一种基于多传感器融合的越野环境导航方法 |
| CN115373399A (zh) * | 2022-09-13 | 2022-11-22 | 中国安全生产科学研究院 | 一种基于空地协同的地面机器人路径规划方法 |
| CN115454083A (zh) * | 2022-09-23 | 2022-12-09 | 福州大学 | 基于快速扩展随机树与动态窗口法的双层路径规划方法 |
| CN116331256A (zh) * | 2023-03-13 | 2023-06-27 | 东南大学 | 分布式驱动电动汽车换道轨迹规划方法、设备及存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116909268A (zh) | 2023-10-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Zhang et al. | Trajectory planning and tracking for autonomous vehicle based on state lattice and model predictive control | |
| JP6997211B2 (ja) | ポリゴンにおける中間点を低減する方法および装置 | |
| CN108089572A (zh) | 用于稳健且有效的车辆定位的算法和基础设施 | |
| Madeleine et al. | Vehicle platoon control with multi-configuration ability | |
| Rodrigues et al. | Autonomous navigation in interaction-based environments—A case of non-signalized roundabouts | |
| Sun et al. | Safe and smooth motion planning for Mecanum-Wheeled robot using improved RRT and cubic spline | |
| Li et al. | Semantic-level maneuver sampling and trajectory planning for on-road autonomous driving in dynamic scenarios | |
| CN113819917A (zh) | 自动驾驶路径规划方法、装置、设备及存储介质 | |
| JP7520444B2 (ja) | 乗り物に基づくデータ処理方法、データ処理装置、コンピュータ機器、及びコンピュータプログラム | |
| Chen et al. | Path Planning for Autonomous Vehicle Based on a Two‐Layered Planning Model in Complex Environment | |
| Arnay et al. | Safe and reliable path planning for the autonomous vehicle verdino | |
| Bertolazzi et al. | Efficient re-planning for robotic cars | |
| Chen et al. | Emergency Obstacle Avoidance Trajectory Planning Method of Intelligent Vehicles Based on Improved Hybrid A | |
| Guo et al. | Safe path planning with gaussian process regulated risk map | |
| Ma et al. | Path planning of mobile robot based on improved PRM based on cubic spline | |
| Fu et al. | Collision-free and kinematically feasible path planning along a reference path for autonomous vehicle | |
| Park et al. | Path generation algorithm based on crash point prediction for lane changing of autonomous vehicles | |
| CN116909268B (zh) | 基于5g的代步机器人路径规划方法、装置、设备及介质 | |
| CN108973995B (zh) | 用于辅助驾驶的环境感知数据处理方法、装置及车辆 | |
| Yang et al. | AAPF*: a safer autonomous vehicle path planning algorithm based on the improved A* algorithm and APF algorithm | |
| Gao et al. | Real-time motion planning for an autonomous mobile robot with wheel-ground adhesion constraint | |
| CN112825160B (zh) | 一种状态转移库构建方法、路径规划方法及相关设备 | |
| CN116136417A (zh) | 一种面向越野环境的无人驾驶车辆局部路径规划方法 | |
| Jia et al. | A dynamic lane-changing trajectory planning scheme for autonomous vehicles on structured road | |
| Rosero et al. | CNN-Planner: A neural path planner based on sensor fusion in the bird's eye view representation space for mapless autonomous driving |
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 |