[发明专利]一种路径的轨迹修正方法、装置、设备和介质

权利要求书

1.一种路径的轨迹修正方法，其特征在于，所述方法包括：

获取输入的待修正路径的初始路径点的坐标集合，进行B样条拟合得到所述待修正路径的每个控制点的位置；

针对所述每个控制点，根据该控制点以及该控制点之前的相邻两个控制点，确定该控制点与上一控制点的连线朝向的第一朝向角度值、以及上一控制点与再上一控制点的连线朝向的第二朝向角度值，并确定所述第二朝向角度值与所述第一朝向角度值的角度差值，和/或根据该控制点的位置以及预先保存的欧几里得符号距离场ESDF地图，确定该控制点对应的最近障碍物的距离，根据该控制点对应的角度差值和/或距离，进行角度优化和/或障碍物距离优化确定该控制点的更新后的位置；

根据所述每个控制点的更新后的位置，进行B样条距离采样得到所述待修正路径轨迹修改后的每个路径点的位置。

2.根据权利要求1所述的方法，其特征在于，所述根据该控制点的位置以及预先保存的欧几里得符号距离场ESDF地图，确定该控制点对应的最近障碍物的距离包括：

根据该控制点的位置、预先保存的欧几里得符号距离场ESDF地图，确定该控制点的位置在所述ESDF地图中对应的第一目标体素，将所述第一目标体素对应保存的距离确定为该控制点对应的最近障碍物的距离；或，

根据该控制点的位置以及欧几里得符号距离场ESDF地图，在所述ESDF地图中确定该控制点的位置对应的第一目标体素，根据预先保存的自动导向车AGV模型、以及设定数量的每个预设球体模型，将所述AGV模型的运动中心的位置放置于所述ESDF地图的所述第一目标体素处，得到所述AGV模型上所述每个预设球体模型的每个球心位置对应的每个第二目标体素，根据所述每个第二目标体素对应保存的距离，确定最小距离为该控制点对应的最近障碍物的距离；或，

根据该控制点的位置以及欧几里得符号距离场ESDF地图，在所述ESDF地图中确定该控制点的位置对应的第一目标体素，根据预先保存的自动导向车AGV模型、以及所述AGV模型的每个预设表面点，将所述AGV模型的运动中心的位置放置于所述第一目标体素处，得到所述AGV模型的所述每个预设表面点对应的每个第三目标体素点，根据所述每个第三目标体素对应保存的距离，确定最小距离为该控制点对应的最近障碍物的距离。

3.根据权利要求1所述的方法，其特征在于，所述根据该控制点对应的角度差值，进行角度优化定该控制点的更新后的位置包括：

若所述角度差值大于预设角度阈值，则确定所述角度差值与预设角度阈值的差值，将所述差值的平方值与预设权重值的乘积值确定为该控制点的目标代价值，将所述目标代价值与预设标准梯度向量的乘积向量确定为该控制点的目标梯度向量；若所述角度差值不大于所述预设角度阈值，则将预设代价值确定为该控制点的目标代价值，将预设梯度向量确定为该控制点的目标梯度向量；

将所述目标代价值和所述目标梯度向量输入预先保存的优化器，得到输出的该控制点进行角度优化后的位置，根据该控制点的角度优化后的位置，重复进行角度优化直到优化次数满足优化终止条件后确定该控制点的更新后的位置。

4.根据权利要求1所述的方法，其特征在于，所述根据该控制点对应的距离，进行障碍物距离优化确定该控制点的更新后的位置包括：

若所述距离大于预设距离阈值，则确定所述距离与预设距离阈值的差值，将所述差值的平方值与预设权重值的乘积值确定为该控制点的目标代价值，将所述目标代价值与预设标准梯度向量的乘积向量确定为该控制点的目标梯度向量，若所述距离不大于所述预设距离阈值，则将预设代价值确定为该控制点的目标代价值，将预设梯度向量确定为该控制点的目标梯度向量；

将所述目标代价值和所述目标梯度向量输入预先保存的优化器，得到输出的该控制点进行障碍物距离优化后的位置，根据该控制点的障碍物距离优化后的位置，重复进行障碍物距离优化直到优化次数满足优化终止条件后确定该控制点的更新后的位置。