用c++写frenet绕障算法

Frenet 绕障算法是一种基于车辆轨迹曲率和横向偏差的路径规划方法，可以使车辆在避开障碍物的同时保持良好的稳定性和舒适性。下面是一个简单的 C++ 实现示例：

#include <iostream>
#include <cmath>
#include <vector>
using namespace std;

struct Point {
    double x;
    double y;
};

struct State {
    double x;
    double y;
    double theta;
    double kappa;
    double s;
};

struct Obstacle {
    double x;
    double y;
    double radius;
};

double distance(const Point& p1, const Point& p2) {
    double dx = p1.x - p2.x;
    double dy = p1.y - p2.y;
    return sqrt(dx * dx + dy * dy);
}

vector<State> frenet_planner(const vector<Point>& waypoints,
                             const Point& start,
                             const double max_speed,
                             const double max_accel,
                             const double max_curvature,
                             const vector<Obstacle>& obstacles) {
    // 参数设置
    const double dt = 0.1;
    const double max_delta = 0.5;
    const double max_delta_dot = 0.5;

    // 路径规划
    vector<State> states;
    State state = {start.x, start.y, 0.0, 0.0, 0.0};
    states.push_back(state);
    for (int i = 1; i < waypoints.size(); i++) {
        Point p1 = waypoints[i - 1];
        Point p2 = waypoints[i];
        double dist = distance(p1, p2);
        double theta = atan2(p2.y - p1.y, p2.x - p1.x);
        for (double s = 0.0; s < dist; s += max_speed * dt) {
            double kappa = 0.0;
            double delta = 0.0;
            double delta_dot = 0.0;
            for (double d = -max_delta; d <= max_delta; d += 0.1) {
                double k = d / (1.0 + d * state.kappa);
                if (fabs(k) > max_curvature) {
                    continue;
                }
                double theta_d = atan2(k, 1.0);
                double delta_dot_d = k / max_speed;
                for (double dd = -max_delta_dot; dd <= max_delta_dot; dd += 0.1) {
                    double delta_dot_new = delta_dot_d + dd;
                    if (fabs(delta_dot_new) > max_curvature / max_speed) {
                        continue;
                    }
                    double kappa_new = state.kappa + delta_dot_new * max_accel;
                    if (fabs(kappa_new) > max_curvature) {
                        continue;
                    }
                    double x_new = state.x + max_speed * cos(theta + theta_d) * dt;
                    double y_new = state.y + max_speed * sin(theta + theta_d) * dt;
                    State state_new = {x_new, y_new, theta + theta_d, kappa_new, state.s + max_speed * dt};
                    bool collision = false;
                    for (int j = 0; j < obstacles.size(); j++) {
                        double d = distance({obstacles[j].x, obstacles[j].y}, {x_new, y_new});
                        if (d < obstacles[j].radius) {
                            collision = true;
                            break;
                        }
                    }
                    if (collision) {
                        continue;
                    }
                    if (states.empty() || distance({state_new.x, state_new.y}, {states.back().x, states.back().y}) > max_speed * dt / 2.0) {
                        states.push_back(state_new);
                    }
                }
            }
            state = states.back();
        }
    }
    return states;
}

int main() {
    // 地图参数
    vector<Point> waypoints = {{0.0, 0.0}, {10.0, 0.0}, {10.0, 10.0}, {0.0, 10.0}, {0.0, 20.0}, {10.0, 20.0}};
    Point start = {0.0, 0.0};
    double max_speed = 5.0;
    double max_accel = 2.0;
    double max_curvature = 0.1;
    vector<Obstacle> obstacles = {{5.0, 5.0, 2.0}, {7.0, 15.0, 1.0}};

    // 路径规划
    vector<State> states = frenet_planner(waypoints, start, max_speed, max_accel, max_curvature, obstacles);

    // 输出结果
    for (int i = 0; i < states.size(); i++) {
        cout << "x = " << states[i].x << ", y = " << states[i].y << ", theta = " << states[i].theta << ", kappa = " << states[i].kappa << ", s = " << states[i].s << endl;
    }

    return 0;
}

在这个示例中，我们定义了三个结构体：`Point` 表示一个二维点坐标，`State` 表示车辆在路径上的状态，包括车辆当前位置 `(x, y)`、车辆朝向角度 `theta`、车辆曲率 `kappa` 和车辆行驶距离 `s`。`Obstacle` 表示一个障碍物，包括障碍物圆心坐标 `(x, y)` 和障碍物半径 `radius`。

`distance` 函数用于计算两个二维点之间的距离。

`frenet_planner` 函数是核心函数，用于实现 Frenet 绕障算法。该函数接收一组路标点 `waypoints`，起点坐标 `start`，最大速度 `max_speed`、最大加速度 `max_accel`、最大曲率 `max_curvature` 和障碍物信息 `obstacles`。函数返回一组车辆状态 `states`，表示车辆在路径上的运动状态。

在 `frenet_planner` 函数中，我们首先定义了一些常量和参数，然后对于每一对相邻的路标点，我们计算出路标点之间的距离和方向角度，并在该路段上按照最大速度进行等间距离的分段，然后对于每一段路程，我们计算出车辆可能的曲率和转向角度，并根据当前车辆状态计算出车辆新的状态。在计算新状态时，我们需要考虑车辆的运动学约束和障碍物的影响，如果新状态与上一个状态距离过近，则忽略该状态。

最后，在 `main` 函数中，我们定义了一组路标点和障碍物，并调用 `frenet_planner` 函数进行路径规划，并输出结果。