使用三次或五次多项式生成约束路径

我需要编写一个程序来使用三次/五次多项式生成路径。

我编写了以下代码来生成 3D 空间的路径。它绘制一条路径（使用三次多项式），并对起点、目标点、初始速度和目标速度进行约束。

import numpy as np
import matplotlib.pyplot as plt


def cubic_trajectory(x0, xf, v0, vf, T):
    # Calculate coefficients for cubic polynomial
    a0 = x0
    a1 = v0
    a2 = (3 * (xf - x0) - (2 * v0 + vf) * T) / T ** 2
    a3 = (2 * (x0 - xf) + (v0 + vf) * T) / T ** 3
    return a0, a1, a2, a3


def generate_trajectory(a0, a1, a2, a3, T, num_points=100):
    t = np.linspace(0, T, num_points)
    x_t = a0 + a1 * t + a2 * t ** 2 + a3 * t ** 3
    return t, x_t


def plot_trajectories_3d(trajectories):
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')

    for traj in trajectories:
        t, x_t, y_t, z_t = traj
        ax.plot(x_t, y_t, z_t,
                label=f'Trajectory from ({x_t[0]}, {y_t[0]}, {z_t[0]}) to ({x_t[-1]}, {y_t[-1]}, {z_t[-1]})')

    ax.set_xlabel('X Position')
    ax.set_ylabel('Y Position')
    ax.set_zlabel('Z Position')
    ax.set_title('3D Cubic Polynomial Trajectories')
    ax.legend()
    plt.show()


# Define multiple start and end conditions for 3D
conditions_3d = [
    {'x0': (-0.5, 0.0, 0.5), 'xf': (0.5, 0.0, 1.0), 'v0': (0, 0, 0), 'vf': (0, 10, 0), 'T': 5},
    #{'x0': (5, 5, 5), 'xf': (15, 15, 15), 'v0': (1, 1, 1), 'vf': (-1, -1, -1), 'T': 6},
    #{'x0': (-3, -3, -3), 'xf': (7, 7, 7), 'v0': (0, 0, 0), 'vf': (2, 0, 0), 'T': 4},
]

trajectories_3d = []
for cond in conditions_3d:
    a0_x, a1_x, a2_x, a3_x = cubic_trajectory(cond['x0'][0], cond['xf'][0], cond['v0'][0], cond['vf'][0], cond['T'])
    a0_y, a1_y, a2_y, a3_y = cubic_trajectory(cond['x0'][1], cond['xf'][1], cond['v0'][1], cond['vf'][1], cond['T'])
    a0_z, a1_z, a2_z, a3_z = cubic_trajectory(cond['x0'][2], cond['xf'][2], cond['v0'][2], cond['vf'][2], cond['T'])

    t, x_t = generate_trajectory(a0_x, a1_x, a2_x, a3_x, cond['T'])
    _, y_t = generate_trajectory(a0_y, a1_y, a2_y, a3_y, cond['T'])
    _, z_t = generate_trajectory(a0_z, a1_z, a2_z, a3_z, cond['T'])

    trajectories_3d.append((t, x_t, y_t, z_t))

plot_trajectories_3d(trajectories_3d)

现在我需要扩展此代码，以便生成的路径位于特定区域内。例如，在下图中的蓝色区域中。

问题：

• 如何向三次或五次多项式添加约束，以便生成的线位于某个区域内。例如，在下面的蓝色区域中：

例如，如果我生成一条从 (3,4) 到 (8,5) 的线，则输出线应如下图所示的红线（意味着不穿过白色部分）：

谢谢

不幸的是，不能保证使用三次或五次多项式生成完全位于给定区域内的路径，同时仍然满足起点、终点、初始速度和最终速度的约束条件。这是因为多项式具有固定的数学形式，并且对于更复杂形状的区域（例如提供的示例），多项式可能无法准确地跟随区域边界。

以下是可以考虑的替代方法：

1. 使用其他路径规划算法：

• 基于采样的路径规划： 诸如快速探索随机树 (RRT) 或概率路线图 (PRM) 之类的算法可以在复杂环境中找到可行的路径。这些算法通过在约束空间中采样随机点并连接这些点以形成路径来工作。
• 人工势场 (APF)： 此方法将目标建模为具有吸引力的“势场”，并将障碍物建模为具有排斥力的“势场”。然后，通过遵循势场的梯度，可以找到从起点到终点的路径。
• A* 搜索算法： 这是一种图搜索算法，可以找到从起点到终点的最短路径，同时也要考虑成本函数，该成本函数可以根据想要生成的路径留在区域内的程度来设计。

2. 对多项式路径进行优化：

• 可以生成初始三次或五次多项式路径，然后使用优化技术将其调整为在所需区域内。
• 此方法涉及定义一个衡量路径违反约束条件程度的成本函数。
• 然后，可以使用诸如梯度下降或遗传算法之类的优化算法来调整多项式系数，以最小化成本函数。

3. 使用样条曲线：

• 样条曲线是分段多项式函数，可用于创建更灵活和平滑的路径。
• 通过使用多个低阶多项式，样条曲线可以更好地逼近复杂形状。
• 可以定义控制点以约束样条曲线并确保其位于所需区域内。

对于提供的区域示例，基于采样的路径规划算法或使用样条曲线可能是合适的选择。

实施这些方法需要更高级的算法和编程技术。有许多可用的库和工具可以帮助进行路径规划，例如：

• OMPL (Open Motion Planning Library)： 一个提供各种基于采样的路径规划算法的 C++ 库。
• ROS (Robot Operating System)： 一个广泛用于机器人应用程序的框架，其中包括用于路径规划的工具和库。
• Python Robotics： 一个 Python 代码集合，其中包括用于路径规划和机器人技术中其他常见任务的算法。

请记住，选择的最佳方法将取决于的特定需求和约束条件。