从零学习自动驾驶Lattice规划算法(下
从零学习自动驾驶Lattice规划算法(下) 轨迹采样 轨迹评估 碰撞检测 包含matlab代码实现和cpp代码实现，方便对照学习。 cpp代码用vs2019编译 依赖qt5.15做可视化 更新: 1 优化绘图 2 增加轨迹预测模块 3 增加从mat文件加载场景的功能，方便场景自定义
在自动驾驶领域，Lattice规划算法是实现智能路径规划的重要技术。今天我们接着上一篇，深入探讨轨迹采样、轨迹评估以及碰撞检测，并且为大家呈现Matlab和C++的代码实现，方便大家对照学习。
轨迹采样
轨迹采样是为了在车辆可行驶的空间中生成一系列可能的轨迹。在Matlab中，我们可以这样实现简单的轨迹采样：
% 轨迹采样参数设置 num_samples = 100; step_size = 0.1; t = 0:step_size:10; % 初始化轨迹矩阵 trajectories = zeros(length(t), num_samples); for i = 1:num_samples % 简单的多项式轨迹采样 a = rand; b = rand; c = rand; trajectories(:, i) = a * t.^2 + b * t + c; end这里我们先设置了采样的数量numsamples和步长stepsize，然后初始化了一个轨迹矩阵trajectories。通过循环，每次生成随机的多项式系数a、b、c，从而得到不同的轨迹。
在C++中，借助Qt框架实现轨迹采样。假设我们已经有了一个TrajectorySampler类：
#include <QVector> #include <cmath> #include <random> class TrajectorySampler { public: TrajectorySampler(int numSamples, double stepSize, double endTime) : numSamples(numSamples), stepSize(stepSize), endTime(endTime) {} QVector<QVector<double>> sampleTrajectories() { QVector<QVector<double>> trajectories; std::random_device rd; std::mt19937 gen(rd()); std::uniform_real_distribution<> dis(0.0, 1.0); for (int i = 0; i < numSamples; ++i) { double a = dis(gen); double b = dis(gen); double c = dis(gen); QVector<double> trajectory; for (double t = 0; t <= endTime; t += stepSize) { double pos = a * std::pow(t, 2) + b * t + c; trajectory.append(pos); } trajectories.append(trajectory); } return trajectories; } private: int numSamples; double stepSize; double endTime; };在C++代码中，我们通过std::random_device和std::mt19937实现随机数生成，同样生成一系列多项式轨迹。
轨迹评估
轨迹评估用于判断生成的轨迹是否满足自动驾驶的各种要求，比如安全性、舒适性等。下面是Matlab中的简单轨迹评估示例：
% 假设已经有了采样得到的轨迹trajectories % 定义一个简单的评估函数，这里只考虑轨迹长度 evaluateTrajectory = @(traj) sum(diff(traj).^2); evaluation_results = zeros(1, num_samples); for i = 1:num_samples evaluation_results(i) = evaluateTrajectory(trajectories(:, i)); end这里我们定义了一个简单的评估函数evaluateTrajectory，通过计算轨迹各点间差值平方和来评估轨迹长度。
从零学习自动驾驶Lattice规划算法(下) 轨迹采样 轨迹评估 碰撞检测 包含matlab代码实现和cpp代码实现，方便对照学习。 cpp代码用vs2019编译 依赖qt5.15做可视化 更新: 1 优化绘图 2 增加轨迹预测模块 3 增加从mat文件加载场景的功能，方便场景自定义
在C++中，我们在TrajectoryEvaluator类中实现类似功能：
#include <QVector> #include <cmath> class TrajectoryEvaluator { public: static double evaluateTrajectory(const QVector<double>& traj) { double result = 0.0; for (int i = 1; i < traj.size(); ++i) { result += std::pow(traj[i] - traj[i - 1], 2); } return result; } };碰撞检测
碰撞检测是自动驾驶规划中至关重要的环节，确保车辆行驶安全。以下是Matlab的碰撞检测示例：
% 假设车辆位置和尺寸 vehicle_x = 5; vehicle_y = 5; vehicle_width = 2; vehicle_length = 4; % 假设障碍物位置和尺寸 obstacle_x = 10; obstacle_y = 10; obstacle_width = 3; obstacle_length = 5; % 碰撞检测 is_collision = (abs(vehicle_x - obstacle_x) < (vehicle_width + obstacle_width) / 2) &&... (abs(vehicle_y - obstacle_y) < (vehicle_length + obstacle_length) / 2);这段代码通过比较车辆和障碍物的位置及尺寸来判断是否发生碰撞。
在C++中，实现一个简单的碰撞检测函数：
#include <cmath> bool collisionDetection(double vehicleX, double vehicleY, double vehicleWidth, double vehicleLength, double obstacleX, double obstacleY, double obstacleWidth, double obstacleLength) { return (std::abs(vehicleX - obstacleX) < (vehicleWidth + obstacleWidth) / 2) && (std::abs(vehicleY - obstacleY) < (vehicleLength + obstacleLength) / 2); }更新内容
1. 优化绘图
在Matlab中，我们可以通过设置图形属性来优化绘图，比如改变线条颜色、标记样式等：
figure; for i = 1:num_samples plot(t, trajectories(:, i), 'Color', rand(1, 3), 'LineWidth', 1.5, 'Marker', 'o'); hold on; end xlabel('Time'); ylabel('Position'); title('Sampled Trajectories'); grid on;在C++中，借助Qt的绘图功能优化绘图。假设我们有一个TrajectoryPlotter类：
#include <QPainter> #include <QColor> class TrajectoryPlotter { public: void plotTrajectories(const QVector<QVector<double>>& trajectories, const QVector<double>& t) { QPixmap pixmap(800, 600); pixmap.fill(Qt::white); QPainter painter(&pixmap); painter.setPen(Qt::black); painter.drawText(10, 20, "Sampled Trajectories"); painter.drawLine(50, 50, 50, 550); painter.drawLine(50, 550, 750, 550); for (const auto& traj : trajectories) { QColor color = QColor::fromRgbF(qrand() / (double)RAND_MAX, qrand() / (double)RAND_MAX, qrand() / (double)RAND_MAX); painter.setPen(color); for (int i = 1; i < traj.size(); ++i) { int x1 = static_cast<int>(50 + t[i - 1] * 700 / t.back()); int y1 = static_cast<int>(550 - traj[i - 1] * 500 / 10); int x2 = static_cast<int>(50 + t[i] * 700 / t.back()); int y2 = static_cast<int>(550 - traj[i] * 500 / 10); painter.drawLine(x1, y1, x2, y2); } } pixmap.save("trajectories.png"); } };2. 增加轨迹预测模块
在Matlab中，可以通过扩展现有的轨迹采样和评估部分来实现简单的轨迹预测：
% 假设已经有评估后的轨迹evaluation_results % 选择评估结果最好的轨迹作为预测轨迹 [~, best_index] = min(evaluation_results); predicted_trajectory = trajectories(:, best_index);在C++中，在TrajectoryPredictor类中实现：
#include <QVector> #include <limits> class TrajectoryPredictor { public: QVector<double> predictTrajectory(const QVector<QVector<double>>& trajectories, const QVector<double>& evaluationResults) { double minResult = std::numeric_limits<double>::max(); int bestIndex = 0; for (int i = 0; i < evaluationResults.size(); ++i) { if (evaluationResults[i] < minResult) { minResult = evaluationResults[i]; bestIndex = i; } } return trajectories[bestIndex]; } };3. 增加从mat文件加载场景的功能，方便场景自定义
在Matlab中，可以使用load函数加载mat文件：
load('scene.mat'); % scene.mat文件中假设包含车辆和障碍物的相关参数 vehicle_x = scene.vehicle_x; vehicle_y = scene.vehicle_y; % 其他参数同理加载在C++中，需要借助第三方库来读取mat文件，比如matio库：
#include <matio.h> #include <iostream> int main() { mat_t* matfp = Mat_Open("scene.mat", MAT_ACC_RDONLY); if (matfp == NULL) { std::cerr << "Error opening mat file" << std::endl; return 1; } matvar_t* matvar = Mat_VarRead(matfp, "vehicle_x"); if (matvar!= NULL) { double vehicleX = *(double*)matvar->data; std::cout << "Vehicle X: " << vehicleX << std::endl; Mat_VarFree(matvar); } Mat_Close(matfp); return 0; }希望通过以上Matlab和C++代码实现以及相关功能更新，能帮助大家更好地理解和学习自动驾驶Lattice规划算法。