✅ 博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。
✅ 具体问题可以私信或扫描文章底部二维码。
（1）基于智能优化的多层次路径规划算法设计
无人驾驶汽车的路径规划问题本质上是在复杂约束条件下寻找从起始点到目标点的最优或近优路径的组合优化问题。传统的路径规划算法在面对大规模、高复杂度的道路环境时往往存在搜索效率低下、计算时间冗长以及容易陷入局部最优解等关键问题。针对这些技术挑战，本研究提出了一种融合多种智能优化策略的分层路径规划架构，该架构通过将全局路径规划与局部路径优化相结合，实现了计算效率与路径质量的平衡统一。
在全局路径规划层面，采用改进的混合蚁群算法作为核心搜索引擎。该算法的创新之处在于对传统蚁群算法的三个关键要素进行了系统性优化改进。首先是信息素初始化策略的智能化设计，通过深度分析道路网络的拓扑结构特征和障碍物分布密度信息，建立了基于节点重要度和连通性的自适应信息素初始化机制。该机制能够根据不同道路段的交通流量特性、道路等级以及历史通行数据来合理分配初始信息素浓度，使得算法在搜索初期就能够体现出对优质路径的偏好性指导，从而显著减少无效搜索的计算开销。
其次是启发式信息的多维度融合优化。传统蚁群算法通常仅考虑距离因素作为启发信息，这种单一维度的评价机制难以全面反映复杂交通环境下的路径优劣性。本研究设计的多维度启发信息融合机制综合考虑了距离代价、交通拥堵程度、道路安全等级、天气影响因子以及实时路况变化等多个关键要素。通过引入动态权重分配策略，系统能够根据当前交通状况和任务需求自动调整各维度因素的重要性权重，确保启发信息能够准确反映当前环境下的最优路径选择倾向。
信息素更新机制的自适应优化是该算法的第三个重要改进点。针对传统固定挥发系数容易导致算法收敛过早或收敛过慢的问题，设计了基于种群多样性和收敛程度的动态调节策略。该策略通过实时监测当前迭代中解的分布情况和质量改进程度，智能调整信息素的挥发速度和增强强度。当检测到种群多样性下降或出现停滞现象时，系统会适当增加信息素挥发系数并引入扰动机制，促使算法跳出局部最优区域继续探索更优解。
为了进一步提升算法的搜索效率和解质量，还集成了基于遗传算法思想的种群进化策略。该策略定期对蚁群中的个体进行适应度评估和优胜劣汰操作，同时引入交叉变异机制来增强种群的多样性。通过这种生物进化启发的优化手段，算法能够在保持收敛速度的同时有效避免早熟收敛问题，显著提高了找到全局最优解的概率。
在局部路径优化层面，设计了基于动态窗口方法的实时避障机制。该机制能够在车辆行驶过程中实时感知周围环境变化，对全局规划路径进行局部调整和优化。通过建立多目标优化模型，系统能够在保证行驶安全的前提下，综合考虑路径长度、行驶时间、燃油消耗以及乘坐舒适度等多个优化目标，实现帕累托最优解的快速求解。
算法的并行化实现是提升计算效率的重要技术手段。通过分析蚁群算法的计算特征，设计了基于多线程并行计算的加速策略。将搜索空间进行合理划分，不同线程负责不同区域的路径搜索任务，通过共享内存机制实现信息素矩阵的同步更新。这种并行化设计不仅充分利用了现代多核处理器的计算资源，还通过减少计算时间提升了算法的实时性能，为无人驾驶汽车的实际应用奠定了技术基础。
（2）多约束条件下的路径平滑优化技术
路径平滑优化是连接全局路径规划与轨迹跟踪控制的重要桥梁，其主要目的是将离散的路径节点转换为连续可导的平滑轨迹，以满足车辆动力学约束和乘坐舒适性要求。传统的路径平滑方法往往存在计算复杂度高、约束处理能力有限以及平滑效果不理想等问题。本研究提出了一种基于参数化曲线的多层次路径平滑优化框架，该框架通过集成多种数学工具和优化策略，实现了高质量平滑轨迹的快速生成。
参数化曲线的选择与设计是路径平滑的核心技术环节。经过深入的理论分析和实验对比，本研究选择了五次B样条曲线作为基础的参数化表示工具。五次B样条曲线具有良好的局部控制性、连续性以及数值稳定性，能够保证生成轨迹在位置、速度和加速度层面的连续性，满足车辆动力学控制的基本要求。通过合理设置控制点分布和节点向量参数，可以实现对轨迹形状的精确控制，同时保持较低的计算复杂度。
多约束条件的处理是路径平滑优化面临的重要挑战。车辆在实际行驶过程中需要满足多种物理约束和安全约束，包括最大转向角限制、最大加速度限制、最大速度限制、道路边界约束以及障碍物避让约束等。本研究建立了基于约束优化理论的数学模型，将各类约束条件统一表示为数学不等式或等式约束。通过引入拉格朗日乘数法和序列二次规划算法，实现了多约束条件下的高效求解。
几何连续性保证是路径平滑的基本要求。为了确保生成的轨迹在几何上具有良好的连续性和可驾驶性，设计了基于曲率连续性的约束条件。通过控制轨迹上各点的曲率变化率，避免出现急剧的转向变化，从而提升行驶的平稳性和安全性。同时建立了基于曲率半径的安全性评估机制，确保轨迹上所有点的曲率半径都大于车辆的最小转弯半径，防止出现车辆无法跟踪的不可行轨迹。
动态约束的考虑是区别于静态路径规划的重要特征。车辆在行驶过程中的动态特性直接影响其轨迹跟踪能力和行驶安全性。本研究建立了考虑车辆动力学特性的轨迹优化模型，将车辆的质量、转动惯量、轮胎特性以及悬架系统参数等物理特性纳入优化过程。通过建立车辆动力学方程和轮胎力学模型，确保生成的轨迹在动力学层面具有可行性和可跟踪性。
速度规划的集成优化是提升轨迹质量的重要手段。传统的路径平滑往往只关注几何轨迹的优化，忽略了速度分布对行驶效果的重要影响。本研究提出了几何轨迹与速度规划的联合优化策略，通过建立速度-曲率约束关系，确保车辆在不同曲率段的行驶速度符合安全要求。同时考虑了加速度舒适性约束，通过限制纵向和横向加速度的大小和变化率，提升乘坐的舒适性体验。
实时性优化是算法工程化应用的关键要求。针对无人驾驶汽车对实时性的严格要求，设计了基于分段优化的快速求解策略。将长距离轨迹分解为多个短段进行独立优化，通过并行计算和流水线处理提升计算效率。同时建立了基于预计算的轨迹库管理机制，对常见的道路模式和转弯场景预先计算优化轨迹模板，在实际应用中通过参数调整和局部优化快速生成满足要求的平滑轨迹。
鲁棒性设计是确保算法可靠性的重要保障。考虑到实际应用中可能存在的传感器噪声、定位误差以及环境变化等不确定因素，设计了基于鲁棒优化理论的轨迹生成算法。通过引入不确定性集合和鲁棒性约束，确保生成的轨迹在一定范围的扰动下仍能保持良好的性能。同时建立了轨迹质量评估指标体系，包括平滑度指标、安全性指标以及舒适性指标等，为轨迹优化提供定量的评价标准。
（3）自适应模型预测控制的轨迹跟踪策略
轨迹跟踪控制是无人驾驶汽车将规划轨迹转化为实际车辆运动的关键技术环节，其控制性能直接决定了车辆的行驶精度、安全性和舒适性。传统的轨迹跟踪控制方法往往存在适应性差、鲁棒性不足以及实时性有限等问题，难以满足复杂交通环境下的高精度控制需求。本研究提出了一种基于自适应模型预测控制的智能轨迹跟踪策略，该策略通过集成先进的控制理论和人工智能技术，实现了高精度、强鲁棒性的轨迹跟踪控制效果。
车辆动力学建模是轨迹跟踪控制的理论基础。为了准确描述车辆在不同工况下的运动特性，建立了多层次的车辆动力学模型体系。在高速工况下采用基于自行车模型的简化动力学描述，该模型能够有效捕捉车辆的主要运动特征同时保持较低的计算复杂度。在低速和复杂机动工况下采用基于四轮模型的精确动力学描述，该模型考虑了前后轴载荷转移、轮胎非线性特性以及悬架系统影响等因素，能够提供更加准确的车辆行为预测。
轮胎力学模型的准确性是车辆动力学建模的关键因素。本研究采用了改进的魔术轮胎模型来描述轮胎的纵向和横向力特性。该模型通过引入温度、载荷以及路面摩擦系数等环境因素的影响，能够更加准确地预测轮胎在不同工况下的力学响应。同时建立了基于实时数据的轮胎参数在线辨识机制，通过分析车辆的运动状态和传感器数据，实时更新轮胎模型参数，提升模型的适应性和预测精度。
模型预测控制器的设计采用了多目标优化的框架结构。控制目标函数综合考虑了轨迹跟踪精度、控制能耗、执行器约束以及乘坐舒适性等多个优化目标。通过建立加权多目标优化模型，系统能够根据不同的行驶场景和任务需求自动调整各目标的重要性权重。在高精度要求的场景下增加跟踪精度的权重，在节能驾驶模式下增加能耗优化的权重，在舒适驾驶模式下增加平滑性的权重。
预测模型的自适应机制是提升控制性能的重要创新点。传统的模型预测控制往往基于固定的车辆模型参数，难以适应车辆特性的变化和不同的行驶环境。本研究设计了基于递推最小二乘法的在线参数估计算法，能够实时识别车辆的关键动力学参数，包括质量、转动惯量、轮胎刚度以及空气阻力系数等。通过将参数估计结果反馈到预测模型中，实现了模型的自适应更新，显著提升了控制系统的鲁棒性和适应性。
约束处理是模型预测控制实现的技术难点。车辆轨迹跟踪控制需要同时满足多种物理约束和安全约束，包括转向角约束、转向角速度约束、纵向加速度约束、横向加速度约束以及轮胎附着力约束等。本研究采用了基于内点法的约束优化求解算法，通过将不等式约束转化为障碍函数的形式，实现了约束条件的有效处理。同时引入了软约束的概念，对于某些可以适度违反的约束条件，通过在目标函数中添加惩罚项的方式进行处理，提升了算法的数值稳定性。
滚动优化策略的设计是平衡控制性能与计算效率的关键技术。预测时域和控制时域的选择直接影响控制效果和计算负担。本研究提出了基于车辆状态和环境复杂度的自适应时域调整策略。在直线行驶等简单场景下采用较短的预测时域以提升计算效率，在转弯、避障等复杂场景下采用较长的预测时域以提升控制精度。同时建立了基于计算资源监测的动态调整机制，根据处理器的负载情况实时调整优化算法的计算精度和迭代次数。
扰动观测与补偿是提升控制鲁棒性的重要手段。实际行驶环境中存在多种不确定性因素，包括路面不平、侧风干扰、载荷变化以及传感器噪声等。本研究设计了基于扩张状态观测器的扰动估计算法，能够实时估计作用在车辆上的未知扰动力和力矩。通过将扰动估计结果作为前馈补偿信号引入控制系统，有效抑制了外界扰动对轨迹跟踪精度的影响。
容错控制机制的集成是确保系统安全性的重要保障。针对执行器故障、传感器失效等潜在故障模式，设计了基于冗余配置和降级控制的容错策略。当检测到转向执行器故障时，系统能够自动切换到基于差速制动的轨迹跟踪模式；当检测到定位传感器故障时，系统能够基于航迹推算和视觉里程计提供备用的状态估计信息。通过多层次的容错设计，确保了控制系统在故障条件下仍能维持基本的轨迹跟踪能力。
import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import minimize
from scipy.interpolate import BSpline, splev, splrep
import rospy
from geometry_msgs.msg import PoseStamped, Twist
from nav_msgs.msg import Path, OccupancyGrid
from std_msgs.msg import Float64MultiArray
import threading
import time
class ImprovedAntColonyOptimizer:
"""改进蚁群算法路径规划器"""
def __init__(self, grid_map, start, goal, max_iter=100, ant_count=50):
self.grid_map = grid_map
self.start = start
self.goal = goal
self.max_iter = max_iter
self.ant_count = ant_count
self.rows, self.cols = grid_map.shape
# 算法参数
self.alpha = 1.0 # 信息素重要程度
self.beta = 2.0 # 启发信息重要程度
self.rho = 0.1 # 信息素挥发系数
self.Q = 100 # 信息素增强系数
# 初始化信息素矩阵
self.pheromone = self._initialize_pheromone()
self.heuristic = self._calculate_heuristic()
self.best_path = None
self.best_length = float('inf')
self.convergence_history = []
def _initialize_pheromone(self):
"""智能信息素初始化"""
pheromone = np.ones((self.rows, self.cols)) * 0.01
# 基于距离的信息素初始化
for i in range(self.rows):
for j in range(self.cols):
if self.grid_map[i][j] == 0: # 可通行区域
dist_to_goal = np.sqrt((i - self.goal[0])**2 + (j - self.goal[1])**2)
dist_to_start = np.sqrt((i - self.start[0])**2 + (j - self.start[1])**2)
total_dist = dist_to_goal + dist_to_start
# 距离越近，初始信息素越高
pheromone[i][j] = 0.1 + 0.9 * np.exp(-total_dist / 50.0)
return pheromone
def _calculate_heuristic(self):
"""计算多维度启发信息"""
heuristic = np.zeros((self.rows, self.cols))
for i in range(self.rows):
for j in range(self.cols):
if self.grid_map[i][j] == 0:
# 距离启发信息
dist_heuristic = 1.0 / (1.0 + np.sqrt((i - self.goal[0])**2 + (j - self.goal[1])**2))
# 障碍物密度启发信息
obstacle_density = self._calculate_local_obstacle_density(i, j)
density_heuristic = np.exp(-obstacle_density * 2.0)
# 路径方向启发信息
direction_heuristic = self._calculate_direction_heuristic(i, j)
# 综合启发信息
heuristic[i][j] = 0.5 * dist_heuristic + 0.3 * density_heuristic + 0.2 * direction_heuristic
return heuristic
def _calculate_local_obstacle_density(self, x, y, radius=3):
"""计算局部障碍物密度"""
obstacle_count = 0
total_count = 0
for dx in range(-radius, radius + 1):
for dy in range(-radius, radius + 1):
nx, ny = x + dx, y + dy
if 0 <= nx < self.rows and 0 <= ny < self.cols:
total_count += 1
if self.grid_map[nx][ny] == 1:
obstacle_count += 1
return obstacle_count / total_count if total_count > 0 else 0
def _calculate_direction_heuristic(self, x, y):
"""计算方向启发信息"""
goal_direction = np.arctan2(self.goal[1] - y, self.goal[0] - x)
return 0.5 * (1 + np.cos(goal_direction))
def _get_neighbors(self, pos):
"""获取邻近可行节点"""
x, y = pos
neighbors = []
# 8方向移动
directions = [(-1,-1), (-1,0), (-1,1), (0,-1), (0,1), (1,-1), (1,0), (1,1)]
for dx, dy in directions:
nx, ny = x + dx, y + dy
if (0 <= nx < self.rows and 0 <= ny < self.cols and
self.grid_map[nx][ny] == 0):
# 计算移动代价
cost = np.sqrt(dx**2 + dy**2)
neighbors.append(((nx, ny), cost))
return neighbors
def _ant_search(self):
"""单只蚂蚁路径搜索"""
current_pos = self.start
path = [current_pos]
visited = set([current_pos])
total_cost = 0
while current_pos != self.goal:
neighbors = self._get_neighbors(current_pos)
if not neighbors:
return None, float('inf') # 无路径
# 计算选择概率
probabilities = []
for (next_pos, cost) in neighbors:
if next_pos not in visited:
x, y = next_pos
pheromone_val = self.pheromone[x][y]
heuristic_val = self.heuristic[x][y]
prob = (pheromone_val ** self.alpha) * (heuristic_val ** self.beta)
probabilities.append(prob)
else:
probabilities.append(0)
if sum(probabilities) == 0:
return None, float('inf') # 陷入死胡同
# 轮盘赌选择
probabilities = np.array(probabilities)
probabilities = probabilities / sum(probabilities)
choice_idx = np.random.choice(len(neighbors), p=probabilities)
next_pos, cost = neighbors[choice_idx]
current_pos = next_pos
path.append(current_pos)
visited.add(current_pos)
total_cost += cost
# 防止路径过长
if len(path) > self.rows * self.cols:
return None, float('inf')
return path, total_cost
def _update_pheromone(self, ant_paths):
"""自适应信息素更新"""
# 信息素挥发
self.pheromone *= (1 - self.rho)
# 计算当前代最优解质量
current_best = min([cost for _, cost in ant_paths if cost != float('inf')])
# 动态调整挥发系数
if current_best < self.best_length:
self.rho = max(0.05, self.rho * 0.95) # 减少挥发，加强好路径
else:
self.rho = min(0.3, self.rho * 1.05) # 增加挥发，增加探索
# 精英策略信息素更新
for path, cost in ant_paths:
if path and cost != float('inf'):
pheromone_increment = self.Q / cost
for i in range(len(path) - 1):
x1, y1 = path[i]
x2, y2 = path[i + 1]
# 对路径上的点增加信息素
self.pheromone[x1][y1] += pheromone_increment
self.pheromone[x2][y2] += pheromone_increment
# 限制信息素范围
self.pheromone = np.clip(self.pheromone, 0.001, 10.0)
def optimize(self):
"""执行蚁群优化"""
for iteration in range(self.max_iter):
ant_paths = []
# 多线程并行蚂蚁搜索
def ant_work(ant_id, results):
path, cost = self._ant_search()
results[ant_id] = (path, cost)
results = {}
threads = []
for ant_id in range(self.ant_count):
thread = threading.Thread(target=ant_work, args=(ant_id, results))
threads.append(thread)
thread.start()
for thread in threads:
thread.join()
ant_paths = list(results.values())
# 更新最优解
for path, cost in ant_paths:
if cost < self.best_length:
self.best_length = cost
self.best_path = path
# 更新信息素
self._update_pheromone(ant_paths)
# 记录收敛历史
self.convergence_history.append(self.best_length)
# 早期停止条件
if iteration > 20 and len(set(self.convergence_history[-10:])) == 1:
print(f"算法在第{iteration}代收敛")
break
return self.best_path, self.best_length
class PathSmoother:
"""基于B样条曲线的路径平滑器"""
def __init__(self, degree=3):
self.degree = degree
def smooth_path(self, path_points, num_points=100):
"""使用B样条曲线平滑路径"""
if len(path_points) < 4:
return path_points
# 转换为numpy数组
points = np.array(path_points)
# 计算累积弧长参数
distances = np.sqrt(np.sum(np.diff(points, axis=0)**2, axis=1))
t = np.concatenate(([0], np.cumsum(distances)))
t = t / t[-1] # 归一化到[0,1]
# 拟合B样条曲线
tck_x, _ = splrep(t, points[:, 0], s=0, k=min(3, len(points)-1))
tck_y, _ = splrep(t, points[:, 1], s=0, k=min(3, len(points)-1))
# 生成平滑路径点
t_smooth = np.linspace(0, 1, num_points)
x_smooth = splev(t_smooth, tck_x)
y_smooth = splev(t_smooth, tck_y)
smooth_path = list(zip(x_smooth, y_smooth))
return self._optimize_curvature(smooth_path)
def _optimize_curvature(self, path_points, max_curvature=0.1):
"""优化路径曲率"""
optimized_path = []
for i in range(len(path_points)):
if i == 0 or i == len(path_points) - 1:
optimized_path.append(path_points[i])
continue
# 计算局部曲率
p1 = np.array(path_points[i-1])
p2 = np.array(path_points[i])
p3 = np.array(path_points[i+1])
# 计算向量
v1 = p2 - p1
v2 = p3 - p2
# 计算曲率
cross_product = np.cross(v1, v2)
curvature = abs(cross_product) / (np.linalg.norm(v1) * np.linalg.norm(v2) + 1e-6)
if curvature > max_curvature:
# 调整点位置以减小曲率
adjustment = 0.3 * (p1 + p3) / 2 + 0.7 * p2
optimized_path.append(tuple(adjustment))
else:
optimized_path.append(path_points[i])
return optimized_path
class ModelPredictiveController:
"""模型预测控制器"""
def __init__(self, vehicle_params, prediction_horizon=10, control_horizon=3):
self.vehicle_params = vehicle_params
self.N = prediction_horizon # 预测时域
self.M = control_horizon # 控制时域
self.dt = 0.1 # 采样时间
# 状态和控制约束
self.max_steering = np.pi / 6 # 最大转向角
self.max_velocity = 20.0 # 最大速度
self.max_acceleration = 3.0 # 最大加速度
# 权重矩阵
self.Q = np.diag([10, 10, 1, 5]) # 状态权重 [x, y, theta, v]
self.R = np.diag([1, 1]) # 控制权重 [delta, a]
self.Qf = 2 * self.Q # 终端权重
def vehicle_model(self, state, control):
"""车辆运动学模型"""
x, y, theta, v = state
delta, a = control
# 自行车模型
L = self.vehicle_params['wheelbase'] # 轴距
# 状态导数
x_dot = v * np.cos(theta)
y_dot = v * np.sin(theta)
theta_dot = v * np.tan(delta) / L
v_dot = a
# 欧拉积分
x_next = x + x_dot * self.dt
y_next = y + y_dot * self.dt
theta_next = theta + theta_dot * self.dt
v_next = v + v_dot * self.dt
return np.array([x_next, y_next, theta_next, v_next])
def linearize_model(self, state, control):
"""模型线性化"""
x, y, theta, v = state
delta, a = control
L = self.vehicle_params['wheelbase']
# 状态矩阵A
A = np.array([
[1, 0, -v * np.sin(theta) * self.dt, np.cos(theta) * self.dt],
[0, 1, v * np.cos(theta) * self.dt, np.sin(theta) * self.dt],
[0, 0, 1, np.tan(delta) / L * self.dt],
[0, 0, 0, 1]
])
# 控制矩阵B
B = np.array([
[0, 0],
[0, 0],
[v / (L * np.cos(delta)**2) * self.dt, 0],
[0, self.dt]
])
return A, B
def solve_mpc(self, current_state, reference_trajectory):
"""求解MPC优化问题"""
# 优化变量：[u0, u1, ..., u_{M-1}]
n_vars = 2 * self.M # 每个控制输入2维
def objective(u_seq):
u_seq = u_seq.reshape((self.M, 2))
cost = 0
state = current_state.copy()
for k in range(self.N):
# 确定当前控制输入
if k < self.M:
control = u_seq[k]
else:
control = u_seq[-1] # 保持最后一个控制输入
# 预测下一状态
state = self.vehicle_model(state, control)
# 参考轨迹
if k < len(reference_trajectory):
ref_state = reference_trajectory[k]
else:
ref_state = reference_trajectory[-1]
# 状态误差
state_error = state - ref_state
# 计算代价
if k == self.N - 1: # 终端代价
cost += state_error.T @ self.Qf @ state_error
else: # 阶段代价
cost += state_error.T @ self.Q @ state_error
# 控制代价
if k < self.M:
cost += control.T @ self.R @ control
return cost
# 约束条件
constraints = []
# 控制输入约束
bounds = []
for k in range(self.M):
bounds.extend([(-self.max_steering, self.max_steering), # 转向角
(-self.max_acceleration, self.max_acceleration)]) # 加速度
# 初始解
u0 = np.zeros(n_vars)
# 求解优化问题
result = minimize(objective, u0, method='SLSQP', bounds=bounds,
constraints=constraints, options={'maxiter': 100})
if result.success:
optimal_controls = result.x.reshape((self.M, 2))
return optimal_controls[0] # 返回第一个控制输入
else:
print("MPC优化失败，使用默认控制")
return np.array([0.0, 0.0])
class AutonomousVehicleSystem:
"""无人驾驶汽车系统主类"""
def __init__(self):
# 初始化ROS节点
rospy.init_node('autonomous_vehicle_system', anonymous=True)
# 车辆参数
self.vehicle_params = {
'wheelbase': 2.8,
'width': 1.8,
'length': 4.5,
'max_steering_angle': np.pi / 6
}
# 初始化各模块
self.path_planner = None
self.path_smoother = PathSmoother()
self.controller = ModelPredictiveController(self.vehicle_params)
# ROS发布者和订阅者
self.path_pub = rospy.Publisher('/planned_path', Path, queue_size=1)
self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
self.goal_sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.goal_callback)
# 系统状态
self.current_state = np.array([0.0, 0.0, 0.0, 0.0]) # [x, y, theta, v]
self.reference_path = []
self.grid_map = None
rospy.loginfo("无人驾驶汽车系统初始化完成")
def map_callback(self, msg):
"""地图回调函数"""
# 转换占据栅格地图为二维数组
width = msg.info.width
height = msg.info.height
self.grid_map = np.array(msg.data).reshape((height, width))
# 转换为二值地图：0-可通行，1-障碍物
self.grid_map[self.grid_map == -1] = 0 # 未知区域视为可通行
self.grid_map[self.grid_map > 50] = 1 # 障碍物
self.grid_map[self.grid_map <= 50] = 0 # 可通行区域
rospy.loginfo("地图数据已更新")
def goal_callback(self, msg):
"""目标点回调函数"""
if self.grid_map is None:
rospy.logwarn("地图未加载，无法进行路径规划")
return
# 提取目标位置
goal_x = int(msg.pose.position.x)
goal_y = int(msg.pose.position.y)
start_x = int(self.current_state[0])
start_y = int(self.current_state[1])
# 执行路径规划
self.plan_and_execute_path((start_x, start_y), (goal_x, goal_y))
def plan_and_execute_path(self, start, goal):
"""规划和执行路径"""
# 路径规划
rospy.loginfo(f"开始路径规划：从{start}到{goal}")
self.path_planner = ImprovedAntColonyOptimizer(
self.grid_map, start, goal, max_iter=50, ant_count=30
)
best_path, path_length = self.path_planner.optimize()
if best_path is None:
rospy.logerr("路径规划失败")
return
rospy.loginfo(f"路径规划成功，路径长度：{path_length:.2f}")
# 路径平滑
smooth_path = self.path_smoother.smooth_path(best_path, num_points=50)
self.reference_path = smooth_path
# 发布路径
self.publish_path(smooth_path)
# 执行轨迹跟踪
self.execute_trajectory_tracking()
def publish_path(self, path_points):
"""发布规划路径"""
path_msg = Path()
path_msg.header.frame_id = "map"
path_msg.header.stamp = rospy.Time.now()
for point in path_points:
pose = PoseStamped()
pose.header = path_msg.header
pose.pose.position.x = point[0]
pose.pose.position.y = point[1]
pose.pose.position.z = 0.0
path_msg.poses.append(pose)
self.path_pub.publish(path_msg)
def execute_trajectory_tracking(self):
"""执行轨迹跟踪控制"""
if not self.reference_path:
return
rate = rospy.Rate(10) # 10Hz控制频率
path_index = 0
while not rospy.is_shutdown() and path_index < len(self.reference_path):
# 构建参考轨迹
horizon_path = []
for i in range(self.controller.N):
idx = min(path_index + i, len(self.reference_path) - 1)
ref_point = self.reference_path[idx]
# 构建参考状态 [x, y, theta, v]
if i == 0:
ref_theta = self.current_state[2]
ref_v = 5.0 # 期望速度
else:
prev_point = self.reference_path[max(0, idx-1)]
ref_theta = np.arctan2(ref_point[1] - prev_point[1],
ref_point[0] - prev_point[0])
ref_v = 5.0
horizon_path.append([ref_point[0], ref_point[1], ref_theta, ref_v])
# MPC控制
control_input = self.controller.solve_mpc(self.current_state, horizon_path)
# 发布控制命令
cmd_msg = Twist()
cmd_msg.linear.x = self.current_state[3] + control_input[1] * 0.1
cmd_msg.angular.z = control_input[0]
self.cmd_pub.publish(cmd_msg)
# 更新车辆状态（仿真）
self.current_state = self.controller.vehicle_model(
self.current_state, control_input
)
# 检查是否到达当前路径点
current_pos = self.current_state[:2]
target_pos = np.array(self.reference_path[path_index])
if np.linalg.norm(current_pos - target_pos) < 1.0:
path_index += 1
rate.sleep()
rospy.loginfo("轨迹跟踪完成")
def run(self):
"""运行主循环"""
rospy.loginfo("无人驾驶汽车系统开始运行")
rospy.spin()
if __name__ == '__main__':
try:
vehicle_system = AutonomousVehicleSystem()
vehicle_system.run()
except rospy.ROSInterruptException:
rospy.loginfo("系统退出")
如有问题，可以直接沟通
👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇
扫一扫
4万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
