粒子群与 3 - 5 - 3 多项式时间最优轨迹规划代码复现之旅

粒子群轨迹规划，3-5-3多项式时间最优轨迹规划，复现文章代码

在机器人运动规划等领域，轨迹规划是一个关键环节。今天咱们来聊聊粒子群轨迹规划以及 3 - 5 - 3 多项式时间最优轨迹规划，并复现相关文章代码。

粒子群轨迹规划

粒子群算法（PSO）灵感来源于鸟群觅食行为。想象一群鸟随机在空间中寻找食物，每只鸟知道自己当前位置和曾经到过的最好位置（个体极值），同时也知道整个鸟群目前找到的最好位置（全局极值）。每只鸟根据这两个信息来调整自己的飞行方向和速度，从而最终找到食物。

在轨迹规划中，粒子群算法可用于优化轨迹的一些参数，使得生成的轨迹满足特定要求，比如最短时间、最小能量等。

粒子群轨迹规划，3-5-3多项式时间最优轨迹规划，复现文章代码

以下是一个简单的粒子群算法 Python 代码示例：

import numpy as np # 定义适应度函数，这里简单假设为粒子位置的平方和 def fitness_function(position): return np.sum(position ** 2) # 粒子群算法实现 def pso(num_particles, num_dimensions, max_iterations, c1, c2, w): # 初始化粒子位置和速度 positions = np.random.rand(num_particles, num_dimensions) velocities = np.random.rand(num_particles, num_dimensions) pbest_positions = positions.copy() pbest_fitness = np.array([fitness_function(p) for p in positions]) gbest_index = np.argmin(pbest_fitness) gbest_position = pbest_positions[gbest_index] gbest_fitness = pbest_fitness[gbest_index] for i in range(max_iterations): r1 = np.random.rand(num_particles, num_dimensions) r2 = np.random.rand(num_particles, num_dimensions) # 更新速度 velocities = w * velocities + c1 * r1 * (pbest_positions - positions) + c2 * r2 * ( gbest_position - positions) # 更新位置 positions = positions + velocities fitness_values = np.array([fitness_function(p) for p in positions]) improved_indices = fitness_values < pbest_fitness pbest_positions[improved_indices] = positions[improved_indices] pbest_fitness[improved_indices] = fitness_values[improved_indices] current_best_index = np.argmin(pbest_fitness) if pbest_fitness[current_best_index] < gbest_fitness: gbest_position = pbest_positions[current_best_index] gbest_fitness = pbest_fitness[current_best_index] return gbest_position, gbest_fitness

这段代码首先定义了一个简单的适应度函数，用于评估粒子的 “好坏”。然后在pso函数中，初始化粒子的位置和速度，接着在每次迭代中，根据粒子群算法的公式更新速度和位置，并不断更新个体极值和全局极值。

3 - 5 - 3 多项式时间最优轨迹规划

3 - 5 - 3 多项式轨迹规划常用于机器人关节运动轨迹规划。它通过使用 3 次、5 次和 3 次多项式来分别描述轨迹的起始段、中间段和结束段，以保证轨迹的平滑性和满足一些边界条件，比如起始和结束位置、速度、加速度为零等，从而实现时间最优。

假设我们要规划一个机器人关节从起始位置qstart到目标位置qend的轨迹，以下是一个简化的 3 - 5 - 3 多项式轨迹规划的 Python 代码：

import numpy as np import matplotlib.pyplot as plt # 3 - 5 - 3 多项式轨迹规划函数 def cubic_polynomial(q_start, q_end, t, T): a0 = q_start a1 = 0 a2 = 3 * (q_end - q_start) / T ** 2 a3 = -2 * (q_end - q_start) / T ** 3 return a0 + a1 * t + a2 * t ** 2 + a3 * t ** 3 def quintic_polynomial(q_start, q_end, t, T): a0 = q_start a1 = 0 a2 = 0 a3 = 10 * (q_end - q_start) / T ** 3 a4 = -15 * (q_end - q_start) / T ** 4 a5 = 6 * (q_end - q_start) / T ** 5 return a0 + a1 * t + a2 * t ** 2 + a3 * t ** 3 + a4 * t ** 4 + a5 * t ** 5 def three_five_three_trajectory(q_start, q_end, T1, T2, T3): t1 = np.linspace(0, T1, 100) t2 = np.linspace(0, T2, 100) t3 = np.linspace(0, T3, 100) q1 = cubic_polynomial(q_start, q_start, t1, T1) q2 = quintic_polynomial(q_start, q_end, t2, T2) q3 = cubic_polynomial(q_end, q_end, t3, T3) q = np.concatenate((q1, q2, q3)) t_total = np.concatenate((t1, t1[-1] + t2, t1[-1] + t2[-1] + t3)) return t_total, q # 示例调用 q_start = 0 q_end = 1 T1 = 1 T2 = 2 T3 = 1 t, q = three_five_three_trajectory(q_start, q_end, T1, T2, T3) plt.plot(t, q) plt.xlabel('Time') plt.ylabel('Position') plt.title('3 - 5 - 3 Polynomial Trajectory') plt.show()

在这段代码中，cubicpolynomial函数定义了 3 次多项式，quinticpolynomial定义了 5 次多项式。threefivethree_trajectory函数则将三段多项式组合起来生成完整轨迹，并使用matplotlib进行可视化。

通过粒子群算法和 3 - 5 - 3 多项式轨迹规划的结合，我们可以在满足一些复杂约束条件下，更高效地规划出机器人的最优运动轨迹。希望今天分享的代码复现和分析能帮助大家更好地理解这两种轨迹规划方法。