[发明专利]轨迹优化方法、装置、设备和存储介质

权利要求书

1.一种轨迹优化方法，其特征在于，所述方法包括：

基于目标优化轨迹上多个目标优化轨迹点的第一坐标，得到所述目标优化轨迹的优化项，其中，所述优化项包括所述目标优化轨迹的平滑项、轨迹长度项以及偏移项中的至少一个；

基于所述优化项和所述多个目标优化轨迹点的第一坐标，构建以所述多个目标优化轨迹点为优化目标的代价函数；

将所述代价函数满足预设要求时各所述目标优化轨迹点的第二坐标，确定为各所述目标优化轨迹点的优化后轨迹点坐标。

2.根据权利要求1所述的方法，其特征在于，所述基于所述优化项和所述多个目标优化轨迹点的第一坐标，构建以所述多个目标优化轨迹点为优化目标的代价函数，包括：

确定各所述优化项分别对应的第一系数矩阵和第二系数矩阵，其中，各所述优化项对应的所述第一系数矩阵由各所述优化项中二次项的系数构成，各所述优化项对应的所述第二系数矩阵由各所述优化项中一次项的系数构成；

获取由所述多个目标优化轨迹点的第一坐标所构成的目标优化矩阵；

基于所述目标优化矩阵以及各所述优化项分别对应的所述第一系数矩阵和所述第二系数矩阵，构建所述代价函数。

3.根据权利要求2所述的方法，其特征在于，所述基于所述目标优化矩阵以及各所述优化项分别对应的所述第一系数矩阵和所述第二系数矩阵，构建所述代价函数，包括：

对各所述优化项分别对应的所述第一系数矩阵进行加权处理，得到第三系数矩阵，以及对各所述优化项分别对应的所述第二系数矩阵进行加权处理，得到第四系数矩阵；

基于所述第三系数矩阵、所述第四系数矩阵以及所述目标优化矩阵，构建所述代价函数。

4.根据权利要求3所述的方法，其特征在于，所述基于所述第三系数矩阵、所述第四系数矩阵以及所述目标优化矩阵，构建所述代价函数，包括：

获取由所述目标优化矩阵对应的转置矩阵、所述第三系数矩阵以及所述目标优化矩阵的乘积所构成的第一乘积项，并获取由所述第四系数矩阵的转置矩阵以及所述目标优化矩阵的乘积所构成的第二乘积项；

将所述第一乘积项与所述第二乘积项之和，作为所述代价函数。

5.根据权利要求1所述的方法，其特征在于，所述平滑项的获取步骤包括：

对于各所述目标优化轨迹点，基于所述目标优化轨迹点的第一坐标，以及所述目标优化轨迹上与所述目标优化轨迹点前后相邻的两个目标优化轨迹点的第一坐标，得到所述目标优化轨迹点对应的子平滑项；

将各所述目标优化轨迹点对应的子平滑项的累加和，作为所述平滑项。

6.根据权利要求1所述的方法，其特征在于，所述轨迹长度项的获取步骤包括：

对于各所述目标优化轨迹点，基于所述目标优化轨迹点的第一坐标，以及所述目标优化轨迹上与所述目标优化轨迹点相邻的目标优化轨迹点的第一坐标，得到所述目标优化轨迹点对应的子长度项；

将各所述目标优化轨迹点对应的子长度项的累加和，作为所述轨迹长度项。

7.根据权利要求1所述的方法，其特征在于，所述偏移项的获取步骤包括：

对于各所述目标优化轨迹点，基于所述目标优化轨迹点的第一坐标，以及原始规划轨迹上与所述目标优化轨迹点对应的原始规划轨迹点的坐标，得到所述目标优化轨迹点对应的子偏移项；

将各所述目标优化轨迹点对应的子偏移项的累加和，作为所述偏移项。

8.根据权利要求1所述的方法，其特征在于，所述预设要求包括所述代价函数为最小值。