[发明专利]一种移动机器人平滑回归给定轨迹的路径规划方法及装置

权利要求书

1.一种移动机器人平滑回归给定轨迹的路径规划方法，其特征在于步骤包括：

S1 路径规划装置接收移动机器人的位姿与给定轨迹信息，采用启发式快速随机迭代算法以生成移动机器人距离给定轨迹最近点的索引，其骤包括：

S11 在任意一个有限区间，在起始区间内随机生成n个点，且n大于2，将自变量索引最小的点的索引值赋给Left，自变量索引最大的点的索引值赋给Right，通过比较n个随机点的代价值的大小，找出代价值最小的索引MinCostIndex，根据MinCostIndex所在的位置对区间进行收缩；

S12 循环执行步骤S11，直至整个迭代区间长度小于预设值，以生成移动机器人距离给定轨迹最近点的索引；

S2 路径规划装置从步骤S1的索引开始遍历，根据位姿信息及遍历到的给定轨迹的点以五次多项式参数方程形式生成路径；

S3路径规划装置对步骤S2生成的路径的最大曲率进行判断，当回归路径的最大曲率小于1/Rmin时，Rmin为最小曲率半径，将路径加入到可行解集中；

S4 路径规划装置计算步骤S3中各可行解集的回归路径长度，以选择最短的回归路径作为最优回归路径。

2.根据权利要求1所述的移动机器人平滑回归给定轨迹的路径规划方法，其特征在于，步骤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。

3.根据权利要求1所述的移动机器人平滑回归给定轨迹的路径规划方法，其特征在于，步骤S2中根据位姿信息及遍历到的给定轨迹的点进行五次多项式路径的生成的步骤包括：

S21将给定轨迹点的集合（x(i), y(i)）中的每一个索引i进行归一化，并转变为对应的t，以化为（x(t), y(t)）的集合；

S22 根据S21获取的给定轨迹点的集合，以五次多项式参数方程形式生成路径：

其一阶导数与二阶导数的表达式为：

当移动机器人的位姿（x0, y0, th0）作为t = 0时已知状态，遍历到的给定轨迹点(x(i), y(i))作为t = 1时的已知状态,并记为（x1, y1）则：

S23 引入松弛变量x^’(t=0) = a, y^’(t=0) = a*tan(th₀)，通过给定的a值区间，以路径曲率和最小为目标，找出每次五次多项式拟合最佳a值的路径。