✅ 博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。
✅ 具体问题可以私信或扫描文章底部二维码。
仓储物流、灾难救援、智慧农业等复杂动态场景中，移动机器人的自主导航能力是其执行任务的基础，而路径规划是导航的核心环节。传统规划算法如A*、Dijkstra在已知静态地图中能保证找到最短路径，但在面对高维连续空间、复杂障碍物形态、动态不确定性以及多约束（如平滑度、能耗、安全性）时，往往显得力不从心，存在规划速度慢、路径不够平滑、易陷入局部死区等问题。智能优化算法以其对问题模型依赖小、全局搜索能力强、易于融合多种约束等优势，为复杂环境下的路径规划提供了新的突破口。然而，直接将标准智能算法用于路径规划，常面临路径编码表示低效、约束处理生硬、收敛速度与解质量难以兼顾等挑战。本文从环境建模、路径表示、算法改进三个层面进行系统性创新，核心方案包括：
（1）提出一种基于B样条曲线的可变节点路径编码与自适应可行域采样策略。路径不再被表示为离散网格点的序列，而是用一条参数化的B样条曲线来刻画。机器人的路径由一组控制点唯一确定。这种表示具有内在的平滑性（取决于样条阶数），能直接满足机器人运动学对曲率连续的要求。我们将智能算法（如粒子群算法）中每个个体的位置，编码为这些控制点的坐标。关键创新在于控制点的数量并非固定，而是根据环境复杂度自适应确定：在空旷区域用较少的控制点描述长直线段，在障碍物密集区域则插入更多的控制点以提供足够的灵活性来绕行。同时，在初始化种群和更新位置时，我们引入可行域引导采样：利用障碍物地图信息，快速生成一个粗略的“通道”或“走廊”，新的控制点被倾向于放置在这些通道内，极大地减少了无效搜索，提升了初始种群质量和迭代效率。对于动态环境，B样条的控制点可以关联时间参数，从而将时空联合规划问题也纳入同一框架。
（2）设计一种结合斥力场引导与多目标分解的复合适应度函数，以平衡路径长度、安全性与平滑度。适应度函数是引导算法搜索的指挥棒。我们设计了一个多目标的复合函数。首要目标是路径长度，由B样条曲线的弧长近似计算。安全性目标通过模拟人工势场法中的斥力场实现：计算路径上采样点到最近障碍物的距离，若小于安全阈值，则施加一个与该距离成反比的惩罚项，该惩罚项在距离趋近于零时急剧增大，确保路径远离障碍物。平滑度目标则通过计算B样条控制点形成的多边形的转角变化或曲线的曲率积分来度量。为了解决多目标权重人工设定困难的问题，我们引入分解的思想：将多目标优化问题转化为一系列单目标子问题。例如，使用切比雪夫分解法，为每个子问题设定一个权重向量方向，每个个体（路径）在所有子问题上都有一个表现值。算法同时优化这个种群，最终逼近一组帕累托最优路径（即短路径、安全路径、平滑路径的折衷集合），为决策者提供多种选择。
（3）提出一种分层混合优化框架，集成改进蚁狮优化器的全局探索与序列二次规划的局部精细化。单一的智能优化算法在后期局部开发能力可能不足。我们设计了一个两阶段分层框架。第一阶段，采用一种深度改进的蚁狮优化算法进行全局路径搜索。针对标准ALO易早熟的缺点，我们引入了对立学习初始化以增强种群多样性；在蚂蚁随机游走阶段，不仅受当前最优蚁狮（精英）吸引，还引入一个动态的“干扰因子”，模拟粒子群中个体历史最优和群体社会信息的影响，避免过早收敛；同时，蚁狮的陷阱大小（即吸引范围）根据迭代次数和周围蚂蚁的分布自适应调整，平衡探索与开发。该阶段旨在快速找到一个位于全局最优区域附近的、可行的粗路径（由较少控制点定义的B样条）。第二阶段，以前一阶段得到的最优路径控制点作为初始猜测，启动一个序列二次规划局部优化器。在此阶段，我们将路径长度、安全性（转化为不等式约束：路径点与障碍物距离 >= 阈值）、平滑度（控制点二阶差分约束）等目标精确建模为非线性优化问题，利用SQP强大的局部收敛能力，对控制点坐标进行微调，得到一条严格满足约束、且在所有局部改进方向上都达到最优的高质量路径。仿真与实物实验表明，该混合框架在复杂迷宫、动态办公室等多种仿真环境中，相比标准PSO、ALO及传统A*算法，在规划成功率、路径质量（长度缩短10%-20%）、路径平滑度以及规划时间稳定性方面均有显著优势。
import numpy as np
import matplotlib.pyplot as plt
from scipy import interpolate
from scipy.spatial.distance import cdist
class BSplinePath:
def __init__(self, degree=3):
self.degree = degree
self.ctrl_points = None
self.knots = None
self.spline = None
def set_ctrl_points(self, points):
self.ctrl_points = np.array(points)
n_ctrl = len(self.ctrl_points)
self.knots = np.clip(np.linspace(0, 1, n_ctrl + self.degree + 1), 0, 1)
self.knots[:self.degree] = 0
self.knots[-self.degree:] = 1
self.spline = interpolate.BSpline(self.knots, self.ctrl_points, self.degree)
def evaluate(self, t):
return self.spline(t)
def get_path_length(self, num_samples=100):
t_samples = np.linspace(0, 1, num_samples)
points = self.evaluate(t_samples)
diffs = np.diff(points, axis=0)
return np.sum(np.sqrt(np.sum(diffs**2, axis=1)))
def calculate_smoothness(self):
if len(self.ctrl_points) < 3:
return 0
vectors = np.diff(self.ctrl_points, axis=0)
angles = []
for i in range(len(vectors)-1):
v1 = vectors[i]
v2 = vectors[i+1]
norm1 = np.linalg.norm(v1)
norm2 = np.linalg.norm(v2)
if norm1 > 1e-6 and norm2 > 1e-6:
dot = np.dot(v1, v2) / (norm1 * norm2)
dot = np.clip(dot, -1.0, 1.0)
angles.append(np.arccos(dot))
return np.sum(np.array(angles)**2) if angles else 0
class Environment:
def __init__(self, width, height):
self.width = width
self.height = height
self.obstacles = []
def add_obstacle(self, center, radius):
self.obstacles.append((np.array(center), radius))
def distance_to_obstacles(self, point):
min_dist = float('inf')
for center, radius in self.obstacles:
dist = np.linalg.norm(point - center) - radius
min_dist = min(min_dist, dist)
return min_dist
def is_collision_free(self, path_points, safety_margin=0.2):
for point in path_points:
if self.distance_to_obstacles(point) < safety_margin:
return False
return True
def get_feasible_guidance(self, start, goal, num_points=5):
guidance_points = [start]
for i in range(1, num_points-1):
alpha = i / (num_points-1)
direct_point = start + alpha * (goal - start)
perturb = np.random.randn(2) * 0.3 * self.width
candidate = direct_point + perturb
candidate = np.clip(candidate, [0,0], [self.width, self.height])
guidance_points.append(candidate)
guidance_points.append(goal)
return np.array(guidance_points)
class ImprovedALO_SQP:
def __init__(self, env, start, goal, num_ctrl=8, pop_size=30, max_iter=100):
self.env = env
self.start = np.array(start)
self.goal = np.array(goal)
self.num_ctrl = num_ctrl
self.pop_size = pop_size
self.max_iter = max_iter
self.dim = num_ctrl * 2
self.ants_pos = None
self.ants_fitness = None
self.antlions_pos = None
self.antlions_fitness = None
self.elite_pos = None
self.elite_fitness = float('inf')
self.bounds = np.array([[0, env.width], [0, env.height]] * num_ctrl)
def initialize_population(self):
guidance = self.env.get_feasible_guidance(self.start, self.goal, self.num_ctrl)
self.antlions_pos = np.zeros((self.pop_size, self.dim))
for i in range(self.pop_size):
base = guidance.flatten()
noise = np.random.randn(self.dim) * 0.5
individual = base + noise
individual = np.clip(individual.reshape(-1,2), self.bounds[0], self.bounds[1]).flatten()
self.antlions_pos[i] = individual
self.ants_pos = self.antlions_pos.copy()
self.evaluate_all()
self.update_elite()
def decode_to_path(self, position):
ctrl_points = position.reshape(-1, 2)
ctrl_with_ends = np.vstack([self.start, ctrl_points, self.goal])
return ctrl_with_ends
def fitness_function(self, position):
ctrl_with_ends = self.decode_to_path(position)
path_obj = BSplinePath()
path_obj.set_ctrl_points(ctrl_with_ends)
sample_t = np.linspace(0, 1, 50)
path_points = path_obj.evaluate(sample_t)
length_cost = path_obj.get_path_length()
smooth_cost = path_obj.calculate_smoothness() * 0.1
safety_cost = 0
for point in path_points:
dist = self.env.distance_to_obstacles(point)
if dist < 0.3:
safety_cost += (0.3 - dist) * 10
if dist < 0.1:
safety_cost += 50
if not self.env.is_collision_free(path_points, 0.1):
safety_cost += 1000
total_cost = length_cost + smooth_cost + safety_cost
return total_cost
def evaluate_all(self):
self.ants_fitness = np.array([self.fitness_function(pos) for pos in.ants_pos])
self.antlions_fitness = np.array([self.fitness_function(pos) for pos in self.antlions_pos])
def update_elite(self):
all_fitness = np.concatenate([self.ants_fitness, self.antlions_fitness])
all_positions = np.vstack([self.ants_pos, self.antlions_pos])
best_idx = np.argmin(all_fitness)
if all_fitness[best_idx] < self.elite_fitness:
self.elite_fitness = all_fitness[best_idx]
self.elite_pos = all_positions[best_idx].copy()
def roulette_wheel(self, fitness):
max_fit = np.max(fitness)
weights = max_fit - fitness
if np.sum(weights) == 0:
return np.random.randint(len(fitness))
probs = weights / np.sum(weights)
return np.random.choice(len(fitness), p=probs)
def random_walk_around_antlion(self, antlion_pos, iter_ratio):
I = 1 if iter_ratio > 0.5 else 1 + 100 * iter_ratio
random_walk = np.cumsum(2*(np.random.rand(self.dim) > 0.5) - 1)
min_walk = np.min(random_walk)
max_walk = np.max(random_walk)
normalized_walk = (random_walk - min_walk) / (max_walk - min_walk + 1e-12)
lb = antlion_pos / I
ub = antlion_pos * I
lb = np.clip(lb, self.bounds[:,0], self.bounds[:,1])
ub = np.clip(ub, self.bounds[:,0], self.bounds[:,1])
new_pos = lb + (ub - lb) * normalized_walk
return new_pos
def run_alo_phase(self):
self.initialize_population()
for t in range(self.max_iter):
iter_ratio = t / self.max_iter
for i in range(self.pop_size):
selected_idx = self.roulette_wheel(self.antlions_fitness)
antlion_pos = self.antlions_pos[selected_idx]
rw_pos = self.random_walk_around_antlion(antlion_pos, iter_ratio)
if iter_ratio > 0.3:
social_influence = 0.5 * (self.elite_pos - self.ants_pos[i]) + 0.3 * (antlion_pos - self.ants_pos[i])
rw_pos = 0.7 * rw_pos + 0.3 * social_influence
self.ants_pos[i] = np.clip(rw_pos.reshape(-1,2), self.bounds[0], self.bounds[1]).flatten()
self.ants_fitness[i] = self.fitness_function(self.ants_pos[i])
if self.ants_fitness[i] < self.antlions_fitness[selected_idx]:
self.antlions_pos[selected_idx] = self.ants_pos[i].copy()
self.antlions_fitness[selected_idx] = self.ants_fitness[i]
self.update_elite()
return self.elite_pos, self.elite_fitness
def local_sqp_refine(self, initial_guess):
from scipy.optimize import minimize, Bounds, NonlinearConstraint
ctrl_points = initial_guess.reshape(-1, 2)
x0 = ctrl_points.flatten()
bounds = Bounds(np.tile(self.bounds[:,0], self.num_ctrl), np.tile(self.bounds[:,1], self.num_ctrl))
def objective(x):
return self.fitness_function(x)
def safety_constraint(x):
ctrl_with_ends = self.decode_to_path(x)
path_obj = BSplinePath()
path_obj.set_ctrl_points(ctrl_with_ends)
sample_t = np.linspace(0, 1, 30)
path_points = path_obj.evaluate(sample_t)
min_dists = [self.env.distance_to_obstacles(p) for p in path_points]
return np.array(min_dists) - 0.15
nonlin_con = NonlinearConstraint(safety_constraint, 0, np.inf)
res = minimize(objective, x0, method='SLSQP', bounds=bounds, constraints=[nonlin_con], options={'maxiter': 50, 'ftol': 1e-4})
return res.x, res.fun
def optimize(self):
print("Running Improved ALO Global Phase...")
best_global_pos, best_global_fit = self.run_alo_phase()
print(f"Global best fitness after ALO: {best_global_fit:.4f}")
print("Running SQP Local Refinement Phase...")
best_refined_pos, best_refined_fit = self.local_sqp_refine(best_global_pos)
print(f"Refined best fitness after SQP: {best_refined_fit:.4f}")
return self.decode_to_path(best_refined_pos), best_refined_fit
如有问题，可以直接沟通
👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇
扫一扫
2393
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
