第一问: import numpy as np import matplotlib.pyplot as plt from deap import base, creator, tools, algorithms # 定义D-H参数 a = [0, 300, 1200, 300, 0, 0] alpha = [0, -np.pi/2, 0, -np.pi/2, -np.pi/2, -np.pi/2] d = [600, 0, 0, 1200, 0, 0] theta_init = [0, -np.pi/2, 0, np.pi, -np.pi/2, 0] # 目标点位置 target_position = np.array([1500, 1200, 200]) # 计算齐次变换矩阵 def dh_transform(a, alpha, d, theta): return np.array([ [np.cos(theta), -np.sin(theta) * np.cos(alpha), np.sin(theta) * np.sin(alpha), a * np.cos(theta)], [np.sin(theta), np.cos(theta) * np.cos(alpha), -np.cos(theta) * np.sin(alpha), a * np.sin(theta)], [0, np.sin(alpha), np.cos(alpha), d], [0, 0, 0, 1] ]) # 计算末端位置 def forward_kinematics(joint_angles): T = np.eye(4) for i in range(6): T_i = dh_transform(a[i], alpha[i], d[i], joint_angles[i]) T = np.dot(T, T_i) return T[:3, 3] # 计算末端误差 def fitness_function(joint_angles): end_position = forward_kinematics(joint_angles) error = np.linalg.norm(end_position - target_position) return (error,) # 遗传算法参数 creator.create("FitnessMin", base.Fitness, weights=(-1.0,)) creator.create("Individual", list, fitness=creator.FitnessMin) toolbox = base.Toolbox() toolbox.register("attr_float", np.random.uniform, -np.pi, np.pi) toolbox.register("individual", tools.initRepeat, creator.Individual, toolbox.attr_float, n=6) toolbox.register("population", tools.initRepeat, list, toolbox.individual) toolbox.register("evaluate", fitness_function) toolbox.register("mate", tools.cxBlend, alpha=0.5) # 交叉概率参数alpha toolbox.register("mutate", tools.mutGaussian, mu=0, sigma=0.1, indpb=0.2) # 变异概率参数mu, sigma, indpb toolbox.register("select", tools.selTournament, tournsize=3) # 锦标赛选择参数tournsize # 参数设置 population = toolbox.population(n=50) # 种群大小 NGEN = 50 # 代数 CXPB, MUTPB = 0.5, 0.2 # 交叉概率和变异概率 # 遗传算法主循环 for gen in range(NGEN): # 评估当前种群中所有个体的适应度 invalid_ind = [ind for ind in population if not ind.fitness.valid] fitnesses = map(toolbox.evaluate, invalid_ind) for ind, fit in zip(invalid_ind, fitnesses): ind.fitness.values = fit # 选择下一代个体 offspring = toolbox.select(population, len(population)) offspring = list(map(toolbox.clone, offspring)) # 进行交叉和变异操作 for child1, child2 in zip(offspring[::2], offspring[1::2]): if np.random.rand() < CXPB: toolbox.mate(child1, child2) del child1.fitness.values del child2.fitness.values for mutant in offspring: if np.random.rand() < MUTPB: toolbox.mutate(mutant) del mutant.fitness.values # 评估新生成个体的适应度 invalid_ind = [ind for ind in offspring if not ind.fitness.valid] fitnesses = map(toolbox.evaluate, invalid_ind) for ind, fit in zip(invalid_ind, fitnesses): ind.fitness.values = fit population[:] = offspring # 评估每一代的最佳个体 best_individual = tools.selBest(population, 1)[0] print(f"Generation {gen}: Best fitness: {best_individual.fitness.values[0]}") # 找到最佳个体 best_individual = tools.selBest(population, 1)[0] # 检查并打印最佳个体及其适应度 print("Best individual:", best_individual) print("Best fitness:", best_individual.fitness.values if best_individual.fitness.values else "No fitness values") # 可视化结果 end_position = forward_kinematics(best_individual) plt.scatter(target_position[0], target_position[1], c='r', label='Target Position') plt.scatter(end_position[0], end_position[1], c='b', label='End Position') plt.xlabel('X Position (mm)') plt.ylabel('Y Position (mm)') plt.legend() plt.title('Target vs End Position') plt.grid(True) plt.show()
import numpy as np import matplotlib.pyplot as plt from deap import base, creator, tools, algorithms # 定义D-H参数 a = [0, 300, 1200, 300, 0, 0] alpha = [0, -np.pi/2, 0, -np.pi/2, -np.pi/2, -np.pi/2] d = [600, 0, 0, 1200, 0, 0] theta_init = [0, -np.pi/2, 0, np.pi, -np.pi/2, 0] # 目标点位置 target_position = np.array([1500, 1200, 200]) # 计算齐次变换矩阵 def dh_transform(a, alpha, d, theta): return np.array([ [np.cos(theta), -np.sin(theta) * np.cos(alpha), np.sin(theta) * np.sin(alpha), a * np.cos(theta)], [np.sin(theta), np.cos(theta) * np.cos(alpha), -np.cos(theta) * np.sin(alpha), a * np.sin(theta)], [0, np.sin(alpha), np.cos(alpha), d], [0, 0, 0, 1] ]) # 计算末端位置 def forward_kinematics(joint_angles): T = np.eye(4) for i in range(6): T_i = dh_transform(a[i], alpha[i], d[i], joint_angles[i]) T = np.dot(T, T_i) return T[:3, 3] # 计算末端误差 def fitness_function(joint_angles): end_position = forward_kinematics(joint_angles) error = np.linalg.norm(end_position - target_position) return (error,) # 遗传算法参数 creator.create("FitnessMin", base.Fitness, weights=(-1.0,)) creator.create("Individual", list, fitness=creator.FitnessMin) toolbox = base.Toolbox() toolbox.register("attr_float", np.random.uniform, -np.pi, np.pi) toolbox.register("individual", tools.initRepeat, creator.Individual, toolbox.attr_float, n=6) toolbox.register("population", tools.initRepeat, list, toolbox.individual) toolbox.register("evaluate", fitness_function) toolbox.register("mate", tools.cxBlend, alpha=0.5) # 交叉概率参数alpha toolbox.register("mutate", tools.mutGaussian, mu=0, sigma=0.1, indpb=0.2) # 变异概率参数mu, sigma, indpb toolbox.register("select", tools.selTournament, tournsize=3) # 锦标赛选择参数tourn