teb局部路径规划 c语言实现，输出线速度角速度

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

// 位姿结构体
typedef struct
{
    double x;     // x坐标 (米)
    double y;     // y坐标 (米)
    double theta; // 朝向角 (弧度)
    double t;     // 时间戳 (秒)
} PoseSE2;

// 速度命令结构体
typedef struct
{
    double v;     // 线速度 (米/秒)
    double omega; // 角速度 (弧度/秒)
} VelocityCommand;

// 轨迹结构体
typedef struct
{
    PoseSE2 *poses;     // 位姿数组
    double *time_diffs; // 时间间隔数组
    int size;           // 当前点数
    int capacity;       // 数组容量
} TebTrajectory;

// 初始化轨迹
void teb_trajectory_init(TebTrajectory *traj, int capacity)
{
    traj->poses = (PoseSE2 *)malloc(capacity * sizeof(PoseSE2));
    traj->time_diffs = (double *)malloc((capacity - 1) * sizeof(double));
    traj->size = 0;
    traj->capacity = capacity;
}

// 添加位姿点
void teb_trajectory_add_pose(TebTrajectory *traj, PoseSE2 pose)
{
    if (traj->size >= traj->capacity)
    {
        fprintf(stderr, "Trajectory capacity exceeded\n");
        return;
    }

    traj->poses[traj->size] = pose;
    if (traj->size > 0)
    {
        traj->time_diffs[traj->size - 1] = pose.t - traj->poses[traj->size - 1].t;
    }
    traj->size++;
}

// 释放轨迹内存
void teb_trajectory_free(TebTrajectory *traj)
{
    free(traj->poses);
    free(traj->time_diffs);
    traj->size = 0;
    traj->capacity = 0;
}

// 计算两点之间的线速度和角速度
VelocityCommand calculate_velocity(const PoseSE2 *pose1, const PoseSE2 *pose2)
{
    VelocityCommand cmd = {0.0, 0.0};
    double dt = pose2->t - pose1->t;

    if (dt <= 0.0)
    {
        return cmd; // 返回零速度
    }

    // 计算位移
    double dx = pose2->x - pose1->x;
    double dy = pose2->y - pose1->y;

    // 线速度 (米/秒)
    cmd.v = sqrt(dx * dx + dy * dy) / dt;

    // 角速度 (弧度/秒)
    double delta_theta = atan2(dy, dx) - pose1->theta;
    // 归一化角度差到 [-π, π]
    while (delta_theta > M_PI)
        delta_theta -= 2 * M_PI;
    while (delta_theta < -M_PI)
        delta_theta += 2 * M_PI;
    cmd.omega = delta_theta / dt;

    return cmd;
}

// 从轨迹生成速度剖面
VelocityCommand *generate_velocity_profile(const TebTrajectory *traj, int *cmd_count)
{
    if (traj->size < 2)
    {
        *cmd_count = 0;
        return NULL;
    }

    *cmd_count = traj->size - 1;
    VelocityCommand *commands = (VelocityCommand *)malloc((*cmd_count) * sizeof(VelocityCommand));

    for (int i = 0; i < *cmd_count; i++)
    {
        commands[i] = calculate_velocity(&traj->poses[i], &traj->poses[i + 1]);
    }

    return commands;
}

// 简单的轨迹优化 (仅考虑障碍物避让)
void optimize_trajectory(TebTrajectory *traj, const double *obstacles, int obstacle_count)
{
    const double max_v = 0.5;     // 最大线速度 (m/s)
    const double max_omega = 1.0; // 最大角速度 (rad/s)
    const double learning_rate = 0.01;
    const int iterations = 50;

    for (int iter = 0; iter < iterations; iter++)
    {
        for (int i = 1; i < traj->size - 1; i++)
        { // 不优化起点和终点
            // 计算障碍物梯度
            double grad_x = 0.0, grad_y = 0.0;
            for (int j = 0; j < obstacle_count; j++)
            {
                double dx = traj->poses[i].x - obstacles[j * 2];
                double dy = traj->poses[i].y - obstacles[j * 2 + 1];
                double dist_sq = dx * dx + dy * dy;
                double dist = sqrt(dist_sq);

                if (dist < 0.5)
                { // 安全距离0.5米
                    grad_x += dx / dist * (0.5 - dist);
                    grad_y += dy / dist * (0.5 - dist);
                }
            }

            // 更新位姿
            traj->poses[i].x -= learning_rate * grad_x;
            traj->poses[i].y -= learning_rate * grad_y;

            // 限制速度
            VelocityCommand cmd = calculate_velocity(&traj->poses[i - 1], &traj->poses[i]);
            if (cmd.v > max_v || fabs(cmd.omega) > max_omega)
            {
                // 如果速度过大，调整时间间隔
                double scale = fmax(cmd.v / max_v, fabs(cmd.omega) / max_omega);
                traj->time_diffs[i - 1] *= scale;
            }
        }
    }
}

int main()
{
    // 1. 初始化轨迹
    TebTrajectory traj;
    teb_trajectory_init(&traj, 10);

    // 添加初始轨迹点 (直线运动)
    for (int i = 0; i < 5; i++)
    {
        PoseSE2 pose = {i * 0.5, 0.0, 0.0, i * 0.5};
        teb_trajectory_add_pose(&traj, pose);
    }

    // 2. 定义障碍物 (x,y 数组)
    double obstacles[] = {1.2, 0.3, 2.5, -0.2};
    int obstacle_count = sizeof(obstacles) / (2 * sizeof(double));

    // 3. 优化轨迹
    optimize_trajectory(&traj, obstacles, obstacle_count);

    // 4. 生成速度命令
    int cmd_count;
    VelocityCommand *commands = generate_velocity_profile(&traj, &cmd_count);

    // 5. 打印结果
    printf("Time\tV (m/s)\tOmega (rad/s)\n");
    double current_time = 0.0;
    for (int i = 0; i < cmd_count; i++)
    {
        printf("%.2f\t%.2f\t%.2f\n",
               current_time, commands[i].v, commands[i].omega);
        current_time += traj.time_diffs[i];
    }

    // 6. 释放内存
    free(commands);
    teb_trajectory_free(&traj);

    return 0;
}