如何从一系列点生成轨迹路径

Question

我需要一些关于如何生成路径规划算法的建议，当提供一个起始姿势（x，y，z）和方向（x，y，z，偏航）、一系列中间点（可选）和一个结束姿势和方向时，该算法生成一个最优路径。我已经研究了可能的方法，但是许多不同的方法似乎实现了对象厌恶，这在本例中不是必需的。该算法将用python实现，并用于控制6自由度机器人的运动。我最初考虑使用拉格朗日算法来计算通过每个指定点的多项式方程，但是使用这种方法的误差似乎很大。有人有什么建议吗？你知道吗