CN114115299A - 一种移动机器人平滑回归给定轨迹的路径规划方法及装置 - Google Patents

一种移动机器人平滑回归给定轨迹的路径规划方法及装置 Download PDF

Info

Publication number
CN114115299A
CN114115299A CN202210083604.1A CN202210083604A CN114115299A CN 114115299 A CN114115299 A CN 114115299A CN 202210083604 A CN202210083604 A CN 202210083604A CN 114115299 A CN114115299 A CN 114115299A
Authority
CN
China
Prior art keywords
path
mobile robot
index
given
regression
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
Application number
CN202210083604.1A
Other languages
English (en)
Other versions
CN114115299B (zh
Inventor
张瑶港
赵越
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Xiangong Intelligent Technology Co ltd
Original Assignee
Shanghai Xiangong Intelligent Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shanghai Xiangong Intelligent Technology Co ltd filed Critical Shanghai Xiangong Intelligent Technology Co ltd
Priority to CN202210083604.1A priority Critical patent/CN114115299B/zh
Publication of CN114115299A publication Critical patent/CN114115299A/zh
Application granted granted Critical
Publication of CN114115299B publication Critical patent/CN114115299B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria
    • 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)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

本发明提供的该移动机器人平滑回归给定轨迹的路径规划方法及装置，通过五次多项式生成回归路径，克服了给定轨迹与回归路径在连接点处曲率不连续的情况，有利于移动机器人平滑的回归到给定轨迹。其次通过找到移动机器人距离给定轨迹最小距离的索引，并在该索引到给定轨迹终点索引的区间内寻找最优回归路径，从而确保回归路径的行驶方向与给定轨迹的行驶方向一致，再而通过设置松弛变量和可行解集的方式找到最优的回归路径，保证了回归路径的曲率和最小以及路径长度最短，同时满足最大曲率限制。

Description

一种移动机器人平滑回归给定轨迹的路径规划方法及装置
技术领域
本发明涉及移动机器人路径规划技术领域，尤其涉及移动机器人基于五次多项式从给定轨迹外平滑回归到给定轨迹的路径规划方法及装置。
背景技术
当移动机器人接收指令前往目的地的过程中，需要根据预先计算的路径规划进行移动，然而在实际应用中，预先规划的路径上往往不可避免的会存在障碍物，使得移动机器人不得不暂时偏离预定路径轨迹以避障。如在智能仓储领域，该移动机器人可以是仓库中的无人仓储车，其按照给定的轨迹进行行驶是实际工作中必不可少的一部分，尤其是按照给定轨迹运送货物。
但是由于很多原因，无人仓储车并不能确保每时每刻都在给定的轨迹上，所以当仓储车不在给定轨迹上时，如何快速且最优的回归到给定轨迹，这是一个非常重要的问题，不仅关系到能否准确的将货物运送到指定点，还关系到仓储管理人员的安全问题，因为无人仓储车如果不按照给定轨迹行驶时，仓库的管理人员将不得不使用额外的精力来留意无人仓储车。这一方面，会影响工作人员的工作，同时会给工作人员带来额外的安全隐患。
目前针对于在给定轨迹外平滑回归到给定轨迹的问题，业内并没有统一的标准和较好的标杆解决方案。然而发明人认为在当前技术条件下，如果能在确保回归路径与给定轨迹在连接点曲率连续的前提下，通过对回归路径的最大曲率、曲率和以及回归路径的长度等约束来找到最优的回归点，可能是引导此类工作条件下的无人仓储车回归到给定轨迹的较优方式。
发明内容
本发明的主要目的在于提供一种移动机器人平滑回归给定轨迹的路径规划方法，以实现背景技术中发明人提出的方案。
为了实现上述目的，根据本发明的一个方面，提供了一种移动机器人平滑回归给定轨迹的路径规划方法，其步骤包括：
S1 路径规划装置接收移动机器人的位姿与给定轨迹信息，以生成移动机器人距离给定轨迹最近点的索引；
S2 路径规划装置从步骤S1的索引开始遍历，根据位姿信息及遍历到的给定轨迹的点以五次多项式参数方程形式生成路径；
S3路径规划装置对步骤S2生成的路径的最大曲率进行判断，当回归路径的最大曲率小于1/Rmin时，将路径加入到可行解集中；
S4 路径规划装置计算步骤S3中各可行解集的回归路径长度，以选择最短的回归路径作为最优回归路径。
在可能的优选实施方式中，步骤S1中采用HFRI算法生成移动机器人距离给定轨迹最近点的索引，其步骤包括：
S11 在任意一个有限区间，在起始区间内随机生成n个点，且n大于2，将自变量索引最小的点的索引值赋给Left，自变量索引最大的点的索引值赋给Right，通过比较n个随机点的代价值的大小，找出代价值最小的索引MinCostIndex，根据MinCostIndex所在的位置对区间进行收缩；
S12 循环执行步骤S11，直至整个迭代区间长度小于预设值，以生成移动机器人距离给定轨迹最近点的索引。
在可能的优选实施方式中，步骤S11中根据MinCostIndex所在的位置对区间进行收缩的步骤包括：
判断若MinCostIndex = Left，则认为求解问题的最优解在Left的左侧，则需要向左拓宽搜索区间：LeftLimit = max (1, Left - ((Right-Left) - mod (Right-Left,Step))/Step)，其中Step表示区间收缩或者拓宽的步长，mod表示一个取余；max函数是一个取最大值的操作；
判断若MinCostIndex = Right，则认为求解问题的最优解在Right的右侧，则需要向右拓宽搜索区间：RightLimit = min (Size, Right + ((Right-Left)- mod(Right-Left, Step))/Step)，其中Size表示该求解问题定义域索引的最大值；
判断若MinCostIndex在索引Right与Left之间，则认为求解问题的最优解在索引Left和Right之间，此时需要将区间向MinCostIndex处收缩：
LeftLimit = Left + ((MinCostIndex - Left) – mod (MinCostIndex - Left,Step))/Step；
RightLimit = Right - ((Right - MinCostIndex) – mod (Right -MinCostIndex, Step))/Step。
在可能的优选实施方式中，步骤S2中根据位姿信息及遍历到的给定轨迹的点进行五次多项式路径的生成的步骤包括：
S21将给定轨迹点的集合（x(i), y(i)）中的每一个索引i进行归一化，并转变为对应的t，以化为（x(t), y(t)）的集合；
S22 根据S21获取的给定轨迹点的集合，以五次多项式参数方程形式生成路径：
Figure 421476DEST_PATH_IMAGE001
其一阶导数与二阶导数的表达式为：
Figure 797093DEST_PATH_IMAGE002
当移动机器人的位姿（x0, y0, th0）作为t = 0时已知状态，遍历到的给定轨迹点(x(i), y(i))作为t = 1时的已知状态,并记为（x1, y1）则：
Figure 404661DEST_PATH_IMAGE003
S23 引入松弛变量x’(t=0) = a, y’(t=0) = a*tan(th0)，通过给定的a值区间，以路径曲率和最小为目标，找出每次五次多项式拟合最佳a值的路径。
在可能的优选实施方式中，步骤S23中路径曲率和表示回归路径的平滑度，其中曲率计算的方式通过隐式方程曲率计算公式进行计算，对于步骤S2求解出的路径序列（x[1:end]，y[1:end]），首先通过离散点的分别计算出x和y的一阶导x’(t)、y’(t)和二阶导数x’’(t)、y’’ (t)；
x’(t) = diff(x)/diff(t)，y’(t) = diff(y)/diff(t)
x”(t) = diff(x’(t))/diff(t)，y”(t) = diff(y’(t))/diff(t)
然后通过隐式方程的曲率计算公式计算曲率：
Figure 814914DEST_PATH_IMAGE004
其中diff代表离散点的差分，end代表路径的最后一个点的索引；
然后对所有点的曲率进行求和，即：
Figure 397205DEST_PATH_IMAGE005
在可能的优选实施方式中，步骤S3中的回归路径长度表示移动机器人回归到给定轨迹需要行驶的距离，其中回归路径长度L求解如下式：
Figure 246737DEST_PATH_IMAGE006
。
为了实现上述目的，根据本发明的另一方面，还提供了一种路径规划装置，其包括：存储单元，处理单元，数据采集单元，数据发送单元，其中所述存储单元中存有如上述实施例中任一所述的移动机器人平滑回归给定轨迹的路径规划方法的步骤，以供处理器单元调取后执行包括：控制数据采集单元接收移动机器人的位姿与给定轨迹信息，并向处理器单元传输，以供生成移动机器人距离给定轨迹最近点的索引，后经遍历索引，根据位姿信息及遍历到的给定轨迹的点进行五次多项式路径的生成，并对生成的路径的最大曲率进行判断，当回归路径的最大曲率小于1/Rmin时，将路径加入到可行解集中，之后处理器单元基于以路径的曲率和最小，及回归路径长度最短为目标，进行多目标优化决策，以解出最优的回归路径，并经数据发送单元向移动机器人传输。
通过本发明提供的该移动机器人平滑回归给定轨迹的路径规划方法及装置，设计了一种移动机器人的回归策略，从而确保了移动机器人能够较快的回归到给定的轨迹，同时回归路径曲率连续，并且回归路径和给定轨迹在相交处也能保证斜率和曲率连续。此外在相应的方案中还对回归路径的最大曲率进行了限制，避免移动机器人在回归时有原地转向行为。
附图说明
构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1为本发明的方法步骤流程的示意图；
图2为移动机器人到给定轨迹上的点的距离所描述为的凸函数示意图；
图3为HFRI算法中向左拓宽搜索区间的示意图；
图4为HFRI算法中向右拓宽搜索区间的示意图；
图5为HFRI算法中将区间向MinCostIndex处收缩的示意图；
图6为示例1中通过五次多项式得到的最优回归路径的示意图。
具体实施方式
下面对本发明的具体实施方式进行详细地说明。以下示例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。
为了使本领域的技术人员更好的理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，在本领域普通技术人员没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的保护范围。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“S1”、”S2”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含。
请参阅图1至图6所示，本发明针对按照给定轨迹行驶的移动机器人在给定轨迹外时如何最优的回归到给定轨迹的问题，设计了一种回归策略，从而确保仓储车能够较快的回归到给定的轨迹，同时回归路径曲率连续，并且回归路径和给定轨迹在相交处也能保证斜率和曲率连续。此外还对回归路径的最大曲率进行了限制，避免无人仓储车在回归时有原地转向行为。
理论上由于对于任意一条给定的离散轨迹点，都可以通过一定方式转换为参数表达形式，即x = x(t), y = y(t)。因为给定轨迹的所有的点都是已知的，所以给定轨迹上每一t所代表的位置x(t) , x’(t) , x’’(t) , y(t) , y’(t) , y’’(t)都是已知的。当移动机器人在给定轨迹外时，必须在最快回到给定轨迹和以最光滑的轨迹回归到给定轨迹之间做出一定的平衡。
由此发明人提出了以五次多项式的参数方程形式生成回归路径的概念。以对于给定的移动机器人的起始位姿（x, y, th），将给定轨迹上点的位置、一阶导和二阶导作为参数方程求解的已知条件，则可以保证回归路径与给定轨迹在连接处曲率连续。
然后，通过迭代算法得到仓储车距离给定路径最近点的索引。最后从最小距离的索引开始沿着路径终点的方向进行遍历，每一次生成一条五次多项式作为回归路径，当回归路径的最大曲率小于仓储车最小转弯半径1/Rmin时，将该路径加入到可行解集，当可行解集的解数量达到一定或者遍历到给定轨迹终点时，选择一条回归路径长度最短的回归路径，就可得到最优回归路径。
具体来说，如图1所示的策略流程，本发明设计的移动机器人平滑回归给定轨迹的路径规划方法，主要由三个环节组成：
环结（1）
（1）首先路径规划装置接收移动机器人的位姿与给定轨迹信息，以生成移动机器人距离给定轨迹最近点的索引。
对于任意给定移动机器人车位姿与给定轨迹，移动机器人到给定轨迹的各点的距离总是大于移动机器人到给定轨迹最近点的距离，在不考虑非正常轨迹的情况下，移动机器人到给定轨迹上的点的距离大致可以描述为一个凸函数，如图2所示。
而对于一个具有凸性质的问题求解其最小值，并不需要通过遍历穷举，通过一个收敛速度快的迭代算法可以通过很少的迭代次数找出其最优解。为此本实施例中，发明人设计了一种具有启发式的快速随机迭代算法，本发明将其称为（Heuristic fast randomiteration启发式快速随机迭代）HFRI算法。
例如该HFRI算法中，定义了在任意一个有限区间，在起始区间内随机生成n个点，如本实施例中示例为n=4，实际中该n可取大于2的任意值个点，而后将自变量索引最小的点的索引值赋给Left，自变量索引最大的点的索引值赋给Right，通过比较四个随机点的代价值的大小，找出代价值最小的索引MinCostIndex，根据MinCostIndex所在的位置对区间进行收缩。
需要说明的是，每次区间的收缩拓展，都是相对于本次生成的(Left, Right)组成的区间进行的。
由此在整个迭代过程中MinCostIndex会出现三种情况：
a、MinCostIndex = Left，如图3所示，箭头表示区间拓展方向，则认为求解问题的最优解在Left的左侧，则需要向左拓宽搜索区间：
LeftLimit = max (1, Left - ((Right-Left) - mod (Right-Left, Step))/Step)
其中Step表示区间收缩或者拓宽的步长，如Step=2则表示收缩或者拓宽的区间长度占Right-Left区间长度的一半；mod表示一个取余，确保索引为整数；max函数是一个取最大值的操作，防止LeftLimit小于1。
b、MinCostIndex = Right, 如图4所示，则认为求解问题的最优解在Right的右侧，则需要向右拓宽搜索区间：
RightLimit = min (Size, Right + ((Right-Left)- mod(Right-Left,Step))/Step)
同理，Size表示该求解问题定义域索引的最大值，防止RightLimit超出Size。
c、MinCostIndex在索引Right与Left之间，如图5所示，则认为求解问题的最优解在索引Left和Right之间，此时需要将区间向MinCostIndex处收缩：
LeftLimit = Left + ((MinCostIndex - Left) – mod (MinCostIndex - Left,Step))/Step
RightLimit = Right - ((Right - MinCostIndex) – mod (Right -MinCostIndex, Step))/Step
通过上述迭代过程，当整个迭代区间长度小于一定值时，如当区间索引或者区间距离足够小的话，各个索引代表的值相差很小甚至细微时，理论上就没有继续迭代的必要了，则认为该值已经逼近最优解了。
环结（2）
（2）路径规划装置从环结（1）的索引开始遍历，根据位姿信息及遍历到的给定轨迹的点以五次多项式参数方程形式生成路径。
具体来说，首先将给定轨迹化为参数的表达形式，因为接收的给定轨迹是一系列的离散点（x（i）,y(i)）,将轨迹的每一个索引i进行归一化，并转变为对应的t，从而轨迹可以表示为（x(t), y(t)）的集合。因为给定轨迹上的所有的点都是已知的，所以给定轨迹上每一t所代表的位置x(t) , x’(t) , x’’(t) , y(t) , y’(t) , y’’(t)都是已知的。
之后根据已知的信息进行五次多项式参数方程形式的路径生成。对于一条路径，如果通过五次多项式的参数方程可以表达为：
Figure 205465DEST_PATH_IMAGE001
其一阶导数与二阶导数的表达式为：
Figure 453913DEST_PATH_IMAGE002
当移动机器人的位姿（x0, y0, th0）作为t = 0时已知状态，遍历到的给定轨迹点(x(i), y(i))作为t = 1时的已知状态,并记为（x1, y1）则：
Figure 675947DEST_PATH_IMAGE003
其中t=1时的点（x1, y1）都是表示给定轨迹的上的点，所以其对应的一阶导数与二阶导数都是已知的，并且认为在起始点移动机器人为静止状态，也就是起始点的路径曲率为零，为了减少未知变量的个数，可以令x’’ (t=0) = 0，y’’ (t=0) = 0。综上只有在起始点x’(t=0)和y’(t=0)未知，但是该点的切线方向是已知的，所以引入一个松弛变量x’(t=0) = a, y’(t=0) = a*tan(th0)，通过给定一个a的区间，如本实施例中优选为：0-10，从而控制可选择的曲线形式的数量，以此弹性控制最优解的精度或用于根据所需计算效率进行调整。
然后以路径曲率和最小为目标可以找出每次五次多项式拟合最佳a值，从而即可得到一条回归路径。
其中路径的曲率和表示回归路径的平滑度，即移动机器人沿路径行驶时的舵角调整的频率与大小。其中曲率计算的方式通过隐式方程曲率计算公式进行计算，对于任意求解出的路径序列（x[1:end]，y[1:end]），首先通过离散点的分别计算出x和y的一阶导x’(t)、y’(t)和二阶导数x’’ (t)、y’’ (t)；
x’(t) = diff(x)/diff(t)，y’(t) = diff(y)/diff(t)
x”(t) = diff(x’(t))/diff(t)，y”(t) = diff(y’(t))/diff(t)
然后通过隐式方程的曲率计算公式计算曲率：
Figure 822894DEST_PATH_IMAGE007
其中diff代表离散点的差分，end代表路径的最后一个点的索引；
然后对所有点的曲率进行求和，即：
Figure 506685DEST_PATH_IMAGE008
这里为求和到end-2是因为每进行一次求导后得到的数据点比原来的数据点少一个，通常对于实际问题两个路径点不会影响结果。因此本实施方式下取最后两个路径点的曲率与倒数第三个点的曲率相同。
具体的参数方程系数矩阵求解方式如下：
Figure 422689DEST_PATH_IMAGE009
环结（3）
（3）基于路径的最大曲率以及回归路径长度进行多目标优化决策，找出一条最优的回归路径。
具体来说，路径的最大曲率表示移动机器人的极限转弯能力，当最大曲率过大时，移动机器人必须通过原地转向才能按照该路径行驶。在求解回归路径时，应该尽量避免原地转向行为。
如路径规划装置可对环结（2）中生成的路径的最大曲率进行判断，当回归路径的最大曲率小于1/Rmin时，将路径加入到可行解集中；
而回归路径长度，表示移动机器人回归到给定轨迹需要行驶的距离，为了使移动机器人尽快回到给定轨迹应该使路径长度尽可能的小。其中路径长度L求解如下式：
Figure 64892DEST_PATH_IMAGE010
其中在多目标优化决策过程中，首先判断环节（2）生成的回归路径的最大曲率是否符合无人仓储车的最小转弯半径的限制，如果满足则将该路径加入可行解集，这样可以确保可已经集里的每条路径都可以满足无人仓储车的动力学约束。循环执行环节（2），对每次生成的回归路径进行判断，如设定直到可行解集的解的数量为1-100或者已经遍历到给定轨迹终点。而后最终在可行解集中，选择一条路径最短的回归路径，作为移动机器人回到给定轨迹的最优回归路径即可。
由此，通过上述三个环节本发明设计的最优回归策略已经求出最优的回归路径。简而言之，环节（1）可以确保生成的回归路径方向与给定轨迹的行驶方向相同；环节（2）在最优求解松弛变量a时，是以路径曲率和为评价指标的，所以可以确保回归路径平滑；环节（3）通过设置可行解集，可以确保每条路径都是符合无人仓储车的动力学约束，同时回归路径最小。
示例1
本发明针对按照给定轨迹行驶的无人仓储车，当小车在给定轨迹外时，提出了一种回归策略。通过环节（1）、环节（2）、环节（3）可以求出最优的回归路径。因为在实际问题中对于任意给定轨迹都是以离散点的形式进行接收，所以给定轨迹是通过何种方式进行生成的对本发明没有影响，即本发明的回归策略具有一般性。
例：选择给定轨迹的起始点位姿Start（1，1，0）与终点位姿End（10，10，0.7），通过五阶贝塞尔曲线生成一条轨迹作为给定轨迹。通过离散路径的求导方式求解出给定轨迹每一点的一阶导与二阶导。在生成的给定轨迹外取carPose（3，5，0）作为小车的位姿，并且认为在起始点小车为静止状态，也就是起始点的路径曲率为零，为了减少未知变量的个数，可以令x’’ (t=0) = 0，y’’ (t=0) = 0。其中t=1时的点（x1, y1）都是表示给定轨迹的贝塞尔路径上的点，所以其对应的一阶导数与二阶导数都是已知的，综上只有在起始点x’(t=0)和y’(t=0)未知，但是该点的切线方向是已知的，所以引入松弛变量a，本发明a的取值范围为[0,10]。在如上已知条件下，可以知道：
Figure 902398DEST_PATH_IMAGE011
下面根据上述已知信息给出最优回归路径的计算机计算过程：
[B5C_x, B5C_y] = getB5CPath[Start, End]; %通过五阶贝塞尔曲线得到给定轨迹
[minDistancePoint] = getMinDistancePoint [B5C_x, B5C_y, carPose]; %求解初始位姿与给定轨迹的最小距离的索引
[X, Y] = getPathInformation[B5C_x, B5C_y, carPose]; %通过给定轨迹与初始位姿求解X，Y矩阵
for index = minDistancePoint:1:lenPath %从最小距离的索引进行遍历生成五次多项式曲线
for a = 0.1 : 0.5 : 10 %为松弛变量a设置遍历范围
[x, y] = getPloynomialPath[X[index], Y[index], a];
[cur] = getPathCur[x, y]; %得到当前路径的曲率
[best_a] = updateBestA[cur]; %以路径曲率和最小更新最优的a值
[x, y] = getPloynomialPath[X[index], Y[index], best_a]; %在当前索引下以最优的a值生成五次多项式曲线
[cur] = getPathCur[x, y];
[pathLen] = getPathLen[x, y]; %得到当前路径的路径长度
if max(abs(cur)) > klimit %判断当前路径的曲率绝对值的最大值是大于限制曲率，
continue;
else
PathLen[end+1] = pathLen; %将路径长度，当前索引和当前最优a值加入可行集
BestIndex[end+1] = index;
bestA[end+1] = best_a;
[ReturnPoint] = getReturnPoint [PathLen, BestIndex]; %得到最优的回归点
ReturnPath = getPloynomialPath[X[ReturnPoint], Y[ReturnPoint], bestA[ReturnPoint]]; %通过五次多项式得到最优的回归路径
return ReturnPath;
具体实现如图6所示，其中minDistancePoint是根据环节（1）给出的迭代方式求解出carPose距离给定轨迹最近的点索引，ReturnPoint是通过环节（2）和环节（3）计算出的最优回归点的索引，ReturnPath代表的是通过五次多项式得到的最优回归路径。
本发明另一方面与该移动机器人平滑回归给定轨迹的路径规划方法所对应的，还提供了一种路径规划装置，其包括：存储单元，处理单元，数据采集单元，数据发送单元，其中所述存储单元中存有上述实施例中任一所述的移动机器人平滑回归给定轨迹的路径规划方法的步骤，以供处理器单元调取后执行包括：控制数据采集单元接收移动机器人的位姿与给定轨迹信息，并向处理器单元传输，以供生成移动机器人距离给定轨迹最近点的索引，后经遍历索引，根据位姿信息及遍历到的给定轨迹的点进行五次多项式路径的生成，并对生成的路径的最大曲率进行判断，当回归路径的最大曲率小于1/Rmin时，将路径加入到可行解集中，之后处理器单元基于以路径的曲率和最小，及回归路径长度最短为目标，进行多目标优化决策，以解出最优的回归路径，并经数据发送单元向移动机器人传输。
综上所述，本发明提供的该移动机器人平滑回归给定轨迹的路径规划方法及装置，通过五次多项式生成回归路径，克服了给定轨迹与回归路径在连接点处曲率不连续的情况，有利于移动机器人平滑的回归到给定轨迹。其次通过找到移动机器人距离给定轨迹最小距离的索引，并在该索引到给定轨迹终点索引的区间内寻找最优回归路径，从而确保回归路径的行驶方向与给定轨迹的行驶方向一致，再而通过设置松弛变量和可行解集的方式找到最优的回归路径，保证了回归路径的曲率和最小以及路径长度最短，同时满足最大曲率限制。
以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
本领域技术人员可以理解，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
此外实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得单片机、芯片或处理器（processor）执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，RandomAccess Memory）、磁碟或者光盘等各种可以存储程序代码的介质。
此外，本发明实施例的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施例的思想，其同样应当视为本发明实施例所公开的内容。

Claims (7)

1.一种移动机器人平滑回归给定轨迹的路径规划方法，其特征在于步骤包括：
S1 路径规划装置接收移动机器人的位姿与给定轨迹信息，以生成移动机器人距离给定轨迹最近点的索引；
S2 路径规划装置从步骤S1的索引开始遍历，根据位姿信息及遍历到的给定轨迹的点以五次多项式参数方程形式生成路径；
S3路径规划装置对步骤S2生成的路径的最大曲率进行判断，当回归路径的最大曲率小于1/Rmin时，将路径加入到可行解集中；
S4 路径规划装置计算步骤S3中各可行解集的回归路径长度，以选择最短的回归路径作为最优回归路径。
2.根据权利要求1所述的移动机器人平滑回归给定轨迹的路径规划方法，其特征在于，步骤S1中采用HFRI算法生成移动机器人距离给定轨迹最近点的索引，其步骤包括：
S11 在任意一个有限区间，在起始区间内随机生成n个点，且n大于2，将自变量索引最小的点的索引值赋给Left，自变量索引最大的点的索引值赋给Right，通过比较n个随机点的代价值的大小，找出代价值最小的索引MinCostIndex，根据MinCostIndex所在的位置对区间进行收缩；
S12 循环执行步骤S11，直至整个迭代区间长度小于预设值，以生成移动机器人距离给定轨迹最近点的索引。
3.根据权利要求2所述的移动机器人平滑回归给定轨迹的路径规划方法，其特征在于，步骤S11中根据MinCostIndex所在的位置对区间进行收缩的步骤包括：
判断若MinCostIndex = Left，则认为求解问题的最优解在Left的左侧，则需要向左拓宽搜索区间：LeftLimit = max (1, Left - ((Right-Left) - mod (Right-Left,Step))/Step)，其中Step表示区间收缩或者拓宽的步长，mod表示一个取余；max函数是一个取最大值的操作；
判断若MinCostIndex = Right，则认为求解问题的最优解在Right的右侧，则需要向右拓宽搜索区间：RightLimit = min (Size, Right + ((Right-Left)- mod(Right-Left,Step))/Step)，其中Size表示该求解问题定义域索引的最大值；
判断若MinCostIndex在索引Right与Left之间，则认为求解问题的最优解在索引Left和Right之间，此时需要将区间向MinCostIndex处收缩：
LeftLimit = Left + ((MinCostIndex - Left) – mod (MinCostIndex - Left,Step))/Step；
RightLimit = Right - ((Right - MinCostIndex) – mod (Right - MinCostIndex,Step))/Step。
4.根据权利要求1所述的移动机器人平滑回归给定轨迹的路径规划方法，其特征在于，步骤S2中根据位姿信息及遍历到的给定轨迹的点进行五次多项式路径的生成的步骤包括：
S21将给定轨迹点的集合（x(i), y(i)）中的每一个索引i进行归一化，并转变为对应的t，以化为（x(t), y(t)）的集合；
S22 根据S21获取的给定轨迹点的集合，以五次多项式参数方程形式生成路径：
Figure 271166DEST_PATH_IMAGE001
其一阶导数与二阶导数的表达式为：
Figure 494206DEST_PATH_IMAGE002
当移动机器人的位姿（x0, y0, th0）作为t = 0时已知状态，遍历到的给定轨迹点(x(i), y(i))作为t = 1时的已知状态,并记为（x1, y1）则：
Figure 58043DEST_PATH_IMAGE003
S23 引入松弛变量x’(t=0) = a, y’(t=0) = a*tan(th0)，通过给定的a值区间，以路径曲率和最小为目标，找出每次五次多项式拟合最佳a值的路径。
5.根据权利要求1所述的移动机器人平滑回归给定轨迹的路径规划方法，其特征在于，步骤S23中路径曲率和表示回归路径的平滑度，其中曲率计算的方式通过隐式方程曲率计算公式进行计算，对于步骤S2求解出的路径序列（x[1:end]，y[1:end]），首先通过离散点的分别计算出x和y的一阶导x’(t)、y’(t)和二阶导数x’ ’ (t)、y’ ’ (t)；
x’(t) = diff(x)/diff(t)，y’(t) = diff(y)/diff(t)
x”(t) = diff(x’(t))/diff(t)，y”(t) = diff(y’(t))/diff(t)
然后通过隐式方程的曲率计算公式计算曲率：
Figure 635042DEST_PATH_IMAGE004
其中diff代表离散点的差分，end代表路径的最后一个点的索引；
然后对所有点的曲率进行求和，即：
Figure 676947DEST_PATH_IMAGE005
。
6.根据权利要求1所述的移动机器人平滑回归给定轨迹的路径规划方法，其特征在于，步骤S3中的回归路径长度表示移动机器人回归到给定轨迹需要行驶的距离，其中回归路径长度L求解如下式：
Figure 567543DEST_PATH_IMAGE006
。
7.一种路径规划装置，其特征在于包括：存储单元，处理单元，数据采集单元，数据发送单元，其中所述存储单元中存有如权利要求1至6中任一所述的移动机器人平滑回归给定轨迹的路径规划方法的步骤，以供处理器单元调取后执行包括：控制数据采集单元接收移动机器人的位姿与给定轨迹信息，并向处理器单元传输，以供生成移动机器人距离给定轨迹最近点的索引，后经遍历索引，根据位姿信息及遍历到的给定轨迹的点进行五次多项式路径的生成，并对生成的路径的最大曲率进行判断，当回归路径的最大曲率小于1/Rmin时，将路径加入到可行解集中，之后处理器单元基于以路径的曲率和最小，及回归路径长度最短为目标，进行多目标优化决策，以解出最优的回归路径，并经数据发送单元向移动机器人传输。
CN202210083604.1A 2022-01-25 2022-01-25 一种移动机器人平滑回归给定轨迹的路径规划方法及装置 Active CN114115299B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210083604.1A CN114115299B (zh) 2022-01-25 2022-01-25 一种移动机器人平滑回归给定轨迹的路径规划方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210083604.1A CN114115299B (zh) 2022-01-25 2022-01-25 一种移动机器人平滑回归给定轨迹的路径规划方法及装置

Publications (2)

Publication Number Publication Date
CN114115299A true CN114115299A (zh) 2022-03-01
CN114115299B CN114115299B (zh) 2022-04-22

Family

ID=80360896

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210083604.1A Active CN114115299B (zh) 2022-01-25 2022-01-25 一种移动机器人平滑回归给定轨迹的路径规划方法及装置

Country Status (1)

Country Link
CN (1) CN114115299B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115127576A (zh) * 2022-09-02 2022-09-30 青岛慧拓智能机器有限公司 路径规划方法、装置、芯片、终端、电子设备及存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH10141891A (ja) * 1996-11-07 1998-05-29 Mitsubishi Heavy Ind Ltd 飛しょう経路の設定方法
US20190086925A1 (en) * 2017-09-18 2019-03-21 Baidu Usa Llc Path optimization based on constrained smoothing spline for autonomous driving vehicles
CN109521761A (zh) * 2017-09-18 2019-03-26 百度（美国）有限责任公司 用于自动驾驶车辆的基于约束平滑样条的速度优化
CN109885891A (zh) * 2019-01-24 2019-06-14 中国科学院合肥物质科学研究院 一种智能车gpu并行加速轨迹规划方法
CN110928290A (zh) * 2019-03-06 2020-03-27 纽劢科技(上海)有限公司 五次曲线路径规划方法及车道中心线维护方法
CN112799384A (zh) * 2019-10-24 2021-05-14 比亚迪股份有限公司 车辆及其行驶轨迹规划方法和规划装置
CN113031592A (zh) * 2021-02-25 2021-06-25 杭州国辰机器人科技有限公司 一种基于五阶贝塞尔曲线的机器人路径平滑方法及系统
CN113386795A (zh) * 2021-07-05 2021-09-14 西安电子科技大学芜湖研究院 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH10141891A (ja) * 1996-11-07 1998-05-29 Mitsubishi Heavy Ind Ltd 飛しょう経路の設定方法
US20190086925A1 (en) * 2017-09-18 2019-03-21 Baidu Usa Llc Path optimization based on constrained smoothing spline for autonomous driving vehicles
CN109521761A (zh) * 2017-09-18 2019-03-26 百度（美国）有限责任公司 用于自动驾驶车辆的基于约束平滑样条的速度优化
CN109521763A (zh) * 2017-09-18 2019-03-26 百度（美国）有限责任公司 用于自动驾驶车辆的基于约束平滑样条的路径优化
CN109885891A (zh) * 2019-01-24 2019-06-14 中国科学院合肥物质科学研究院 一种智能车gpu并行加速轨迹规划方法
CN110928290A (zh) * 2019-03-06 2020-03-27 纽劢科技(上海)有限公司 五次曲线路径规划方法及车道中心线维护方法
CN112799384A (zh) * 2019-10-24 2021-05-14 比亚迪股份有限公司 车辆及其行驶轨迹规划方法和规划装置
CN113031592A (zh) * 2021-02-25 2021-06-25 杭州国辰机器人科技有限公司 一种基于五阶贝塞尔曲线的机器人路径平滑方法及系统
CN113386795A (zh) * 2021-07-05 2021-09-14 西安电子科技大学芜湖研究院 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115127576A (zh) * 2022-09-02 2022-09-30 青岛慧拓智能机器有限公司 路径规划方法、装置、芯片、终端、电子设备及存储介质
CN115127576B (zh) * 2022-09-02 2022-12-13 青岛慧拓智能机器有限公司 路径规划方法、装置、芯片、终端、电子设备及存储介质

Also Published As

Publication number Publication date
CN114115299B (zh) 2022-04-22

Similar Documents

Publication Publication Date Title
Švancara et al. Online multi-agent pathfinding
JP2017016645A (ja) 半自律車両、及び、半自律車両を制御する方法
CN114115299B (zh) 一种移动机器人平滑回归给定轨迹的路径规划方法及装置
US20200086486A1 (en) Method and apparatus for collision-free motion planning of a manipulator
CN109564429B (zh) 多个可运动机器的路径的协调
US20230152809A1 (en) Method for Determining Termination of Obstacle-crossing, and Method for Controlling Obstacle-crossing, Chip, and Robot
Oliveira et al. Combining lattice-based planning and path optimization in autonomous heavy duty vehicle applications
CN110162029A (zh) 一种基于规划路径的运动控制方法及装置、机器人
JPH07129238A (ja) 障害物回避経路生成方式
CN113050621B (zh) 轨迹规划方法、装置、电子设备和存储介质
CN115079702B (zh) 一种混合道路场景下的智能车辆规划方法和系统
JP7312728B2 (ja) 車両を制御する方法、装置、機器及び記憶媒体
CN111998858B (zh) 一种基于改进a*算法的无人机航路规划方法
Filip et al. Trajectory control for autonomous planetary rovers
Atagoziev et al. Lane change scheduling for connected and autonomous vehicles
Zhe et al. Path planning based on ADFA* algorithm for quadruped robot
CN110849385B (zh) 基于双层启发搜索共轭梯度下降的轨迹规划方法及系统
Khaksar et al. A fuzzy-tabu real time controller for sampling-based motion planning in unknown environment
Dang Autonomous mobile robot path planning based on enhanced A* algorithm integrating with time elastic band
CN108475060A (zh) 位置检测装置、控制装置和移动体
CN115933630A (zh) 一种基于强化学习的复合避障控制方法及装置
WO2022124938A1 (ru) Способ планирования движения робота и мобильный робот
CN111323036B (zh) 原料场智能寻优路径方法、系统、电子设备和存储介质
JP2014115924A (ja) 経路修正装置
CN114460933A (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