从零学习自动驾驶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，通过计算轨迹各点间差值平方和来评估轨迹长度。

在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规划算法。