无人机集群固定阵型轨迹生成模型

# 假设已获得领航机轨迹数据 leader_traj: list of (x, y, theta) 对
# formation_params 包含阵型类型和相关间隔参数

def compute_rotation_matrix(theta):
    return [[cos(theta), -sin(theta)],
            [sin(theta),  cos(theta)]]

def formation_offset(i, formation_params, branch='center'):
    """
    根据阵型参数计算第 i 架 UAV 的局部偏移。
    branch 参数用于楔形阵型：'left' 或 'right' 表示左右分支，
    'center' 可用于线型或单边阵型。
    """
    if formation_params['type'] == 'line':
        # 示例：所有 UAV 沿一侧，间隔 d
        d = formation_params['d']
        return [0, i * d]
    elif formation_params['type'] == 'wedge':
        d_x = formation_params['d_x']
        d_y = formation_params['d_y']
        if branch == 'left':
            return [i * d_x, i * d_y]
        elif branch == 'right':
            return [i * d_x, -i * d_y]
    # 可扩展其他阵型
    return [0, 0]

def smooth_trajectory(traj):
    # 根据需求对轨迹进行平滑处理
    # 例如使用样条曲线拟合
    return traj  # 此处为占位函数

# 主函数：生成所有 UAV 的期望轨迹
def generate_formation_trajectories(leader_traj, formation_params, num_followers):
    formation_trajs = {i: [] for i in range(1, num_followers+1)}
    
    for (x_L, y_L, theta) in leader_traj:
        # 计算旋转矩阵
        R = compute_rotation_matrix(theta)
        
        # 遍历所有 UAV
        for i in range(1, num_followers+1):
            # 根据阵型类型确定局部偏移，此处以线型为例，
            # 若为楔形阵型，则需要区分左右分支
            Delta = formation_offset(i, formation_params, branch='center')
            
            # 全局坐标转换：P_follower = P_leader + R * Delta
            # 这里假设 R 与 Delta 均为列表或数组，进行矩阵向量乘法
            P_follower = [
                x_L + R[0][0] * Delta[0] + R[0][1] * Delta[1],
                y_L + R[1][0] * Delta[0] + R[1][1] * Delta[1]
            ]
            
            formation_trajs[i].append(P_follower)
    
    # 对每个 UAV 的轨迹进行平滑处理
    for i in formation_trajs:
        formation_trajs[i] = smooth_trajectory(formation_trajs[i])
    
    return formation_trajs

# 示例调用
leader_traj = [
    # (x, y, theta) 序列，实际数据由传感器或规划模块提供
    (0, 0, 0),
    (1, 0, 0),
    (2, 0, 0),
    # ... 更多数据
]
formation_params = {'type': 'line', 'd': 5.0}  # 线型阵型，每架 UAV 间隔 5 单位
num_followers = 3

formation_trajectories = generate_formation_trajectories(leader_traj, formation_params, num_followers)

#include <iostream>
#include <vector>
#include <cmath>
#include <string>

// -------------------------- 数据结构定义 --------------------------

/// 用于表示平面位置点
struct Point {
    double x;
    double y;
};

/// 表示无人机的位姿（位置和航向角）
struct Pose {
    double x;
    double y;
    double theta; // 航向角（弧度制）
};

/// 枚举不同的阵型类型
enum class FormationType {
    LINE,   // 线型阵型
    WEDGE   // 楔形阵型
};

/// 阵型参数定义
struct FormationParameters {
    FormationType type;
    // 线型阵型参数：两架 UAV 间的固定间隔（沿局部 y 轴方向）
    double d;

    // 楔形阵型参数：沿航向方向间隔 d_x 及横向间隔 d_y
    double d_x;
    double d_y;
};

// -------------------------- FormationGenerator 类 --------------------------

/**
 * @brief 生成跟随 UAV 的期望轨迹
 *
 * 利用领航机轨迹和阵型参数，在每个时刻计算每架跟随 UAV 的全局期望位置。
 */
class FormationGenerator {
public:
    /**
     * @brief 构造函数
     * @param leaderTrajectory 领航机轨迹（每个时刻的位姿）
     * @param formationParams 阵型参数
     * @param numFollowers 跟随 UAV 数量
     */
    FormationGenerator(const std::vector<Pose>& leaderTrajectory,
                       const FormationParameters& formationParams,
                       int numFollowers)
        : leaderTrajectory_(leaderTrajectory),
          formationParams_(formationParams),
          numFollowers_(numFollowers) {}

    /**
     * @brief 生成所有跟随 UAV 的轨迹
     * @return 每个 UAV 的轨迹，外层 vector 下标对应 UAV 序号（从 0 开始），内层 vector 为轨迹点序列
     */
    std::vector<std::vector<Point>> generate();

private:
    std::vector<Pose> leaderTrajectory_;
    FormationParameters formationParams_;
    int numFollowers_;

    /**
     * @brief 对二维向量 (dx, dy) 根据角度 theta 进行旋转
     * @param dx 原向量 x 分量
     * @param dy 原向量 y 分量
     * @param theta 旋转角度（弧度制）
     * @return 旋转后的向量
     */
    Point rotatePoint(double dx, double dy, double theta) {
        Point rotated;
        rotated.x = dx * std::cos(theta) - dy * std::sin(theta);
        rotated.y = dx * std::sin(theta) + dy * std::cos(theta);
        return rotated;
    }

    /**
     * @brief 根据阵型参数计算局部偏移
     *
     * 对于线型阵型，所有 UAV 沿固定侧边（这里假设在局部 y 正方向）排列；
     * 对于楔形阵型，则根据左右分支分别计算偏移。
     *
     * @param followerIndex 当前 UAV 的编号（从 1 开始，线型阵型时直接使用；楔形阵型时用来计算排名）
     * @param branch 对于楔形阵型，“left”表示左侧分支，“right”表示右侧分支；线型阵型时可以忽略此参数
     * @return 局部坐标系中的偏移向量
     */
    Point computeLocalOffset(int followerIndex, const std::string& branch = "center") {
        Point offset{0.0, 0.0};
        if (formationParams_.type == FormationType::LINE) {
            // 线型阵型：假设所有 UAV 沿局部 y 方向正侧边排列
            offset.x = 0.0;
            offset.y = followerIndex * formationParams_.d;
        } else if (formationParams_.type == FormationType::WEDGE) {
            // 楔形阵型：分别计算左右分支的局部偏移
            if (branch == "left") {
                offset.x = followerIndex * formationParams_.d_x;
                offset.y = followerIndex * formationParams_.d_y;
            } else if (branch == "right") {
                offset.x = followerIndex * formationParams_.d_x;
                offset.y = -followerIndex * formationParams_.d_y;
            }
        }
        return offset;
    }

    /**
     * @brief 对生成的轨迹进行平滑处理
     *
     * 目前为占位函数，可根据需要替换为样条、Bézier 曲线等算法
     *
     * @param traj 原始轨迹
     * @return 平滑后的轨迹
     */
    std::vector<Point> smoothTrajectory(const std::vector<Point>& traj) {
        // 此处直接返回原始轨迹，可根据实际需求实现平滑算法
        return traj;
    }
};

// -------------------------- FormationGenerator 方法实现 --------------------------

std::vector<std::vector<Point>> FormationGenerator::generate() {
    // 初始化每架跟随 UAV 的轨迹（编号从 0 到 numFollowers_-1）
    std::vector<std::vector<Point>> formationTrajectories(numFollowers_);

    // 对领航机轨迹中的每个时刻，计算所有跟随 UAV 的期望位置
    for (const auto& leaderPose : leaderTrajectory_) {
        // 对于每架 UAV
        for (int i = 0; i < numFollowers_; ++i) {
            Point localOffset;
            if (formationParams_.type == FormationType::WEDGE) {
                // 对于楔形阵型，常采用左右对称的方式
                // 例如：将 UAV 分为左右两支，按照索引交替分配
                // 此处计算时，用 (i/2 + 1) 作为该分支中的排名
                int rank = i / 2 + 1;
                std::string branch = (i % 2 == 0) ? "left" : "right";
                localOffset = computeLocalOffset(rank, branch);
            } else {
                // 对于线型阵型，直接使用 UAV 编号（从 1 开始）
                localOffset = computeLocalOffset(i + 1, "center");
            }

            // 将局部偏移转换到全局坐标
            Point rotated = rotatePoint(localOffset.x, localOffset.y, leaderPose.theta);
            Point globalPos;
            globalPos.x = leaderPose.x + rotated.x;
            globalPos.y = leaderPose.y + rotated.y;

            formationTrajectories[i].push_back(globalPos);
        }
    }

    // 对每架 UAV 的轨迹进行平滑处理（如有需要）
    for (auto& traj : formationTrajectories) {
        traj = smoothTrajectory(traj);
    }

    return formationTrajectories;
}

// -------------------------- 示例主函数 --------------------------

int main() {
    // 示例：构造领航机轨迹数据（假设每个时刻的位姿信息）
    std::vector<Pose> leaderTrajectory = {
        {0.0, 0.0, 0.0},
        {10.0, 0.0, 0.0},
        {20.0, 0.0, 0.0},
        {30.0, 0.0, M_PI / 6},  // 转弯示例，航向变为 30 度
        {40.0, 5.0, M_PI / 4}
        // ... 可根据实际情况扩展数据
    };

    // 示例1：线型阵型，间隔 5.0 个单位
    FormationParameters lineParams;
    lineParams.type = FormationType::LINE;
    lineParams.d = 5.0;

    // 例如，3 架跟随 UAV
    int numFollowers = 3;
    FormationGenerator lineFormationGenerator(leaderTrajectory, lineParams, numFollowers);
    auto lineTrajectories = lineFormationGenerator.generate();

    std::cout << "线型阵型生成的跟随 UAV 轨迹：" << std::endl;
    for (int i = 0; i < numFollowers; ++i) {
        std::cout << "UAV " << i + 1 << " 轨迹:" << std::endl;
        for (const auto& pt : lineTrajectories[i]) {
            std::cout << "(" << pt.x << ", " << pt.y << ") ";
        }
        std::cout << std::endl;
    }

    // 示例2：楔形阵型，设置 d_x = 3.0, d_y = 2.0
    FormationParameters wedgeParams;
    wedgeParams.type = FormationType::WEDGE;
    wedgeParams.d_x = 3.0;
    wedgeParams.d_y = 2.0;

    FormationGenerator wedgeFormationGenerator(leaderTrajectory, wedgeParams, numFollowers);
    auto wedgeTrajectories = wedgeFormationGenerator.generate();

    std::cout << "\n楔形阵型生成的跟随 UAV 轨迹：" << std::endl;
    for (int i = 0; i < numFollowers; ++i) {
        std::cout << "UAV " << i + 1 << " 轨迹:" << std::endl;
        for (const auto& pt : wedgeTrajectories[i]) {
            std::cout << "(" << pt.x << ", " << pt.y << ") ";
        }
        std::cout << std::endl;
    }

    return 0;
}

// formation_control.hpp
#pragma once

#include <vector>
#include <map>
#include <cmath>
#include <memory>
#include <Eigen/Dense>

namespace uav {

// 基础数据结构
struct Point2D {
    double x;
    double y;
    
    Point2D(double x = 0, double y = 0) : x(x), y(y) {}
    
    Point2D operator+(const Point2D& other) const {
        return Point2D(x + other.x, y + other.y);
    }
};

struct UAVState {
    Point2D position;
    double heading;  // theta
    double velocity;
    
    UAVState(const Point2D& pos = Point2D(), 
             double h = 0, 
             double v = 0) 
        : position(pos), heading(h), velocity(v) {}
};

// 阵型类型枚举
enum class FormationType {
    LINE,
    WEDGE,
    // 可扩展其他阵型
};

// 阵型参数基类
class FormationParams {
public:
    virtual ~FormationParams() = default;
};

// 线型阵型参数
class LineFormationParams : public FormationParams {
public:
    double spacing;  // UAV间距
    
    explicit LineFormationParams(double d) : spacing(d) {}
};

// 楔形阵型参数
class WedgeFormationParams : public FormationParams {
public:
    double dx;  // 纵向间距
    double dy;  // 横向间距
    
    WedgeFormationParams(double dx, double dy) 
        : dx(dx), dy(dy) {}
};

// 旋转矩阵计算类
class RotationMatrix {
public:
    static Eigen::Matrix2d compute(double theta) {
        Eigen::Matrix2d R;
        R << std::cos(theta), -std::sin(theta),
             std::sin(theta),  std::cos(theta);
        return R;
    }
};

// 局部偏移计算类
class FormationOffset {
public:
    static Point2D compute(int uavIndex, 
                         const FormationType& type,
                         const std::shared_ptr<FormationParams>& params,
                         const std::string& branch = "center") {
        switch (type) {
            case FormationType::LINE:
                return computeLineOffset(uavIndex, 
                    std::static_pointer_cast<LineFormationParams>(params));
            case FormationType::WEDGE:
                return computeWedgeOffset(uavIndex, 
                    std::static_pointer_cast<WedgeFormationParams>(params),
                    branch);
            default:
                return Point2D(0, 0);
        }
    }

private:
    static Point2D computeLineOffset(
        int uavIndex, 
        const std::shared_ptr<LineFormationParams>& params) {
        return Point2D(0, uavIndex * params->spacing);
    }
    
    static Point2D computeWedgeOffset(
        int uavIndex,
        const std::shared_ptr<WedgeFormationParams>& params,
        const std::string& branch) {
        if (branch == "left") {
            return Point2D(uavIndex * params->dx, uavIndex * params->dy);
        } else if (branch == "right") {
            return Point2D(uavIndex * params->dx, -uavIndex * params->dy);
        }
        return Point2D(0, 0);
    }
};

// 轨迹平滑器接口
class TrajectorySmoothing {
public:
    virtual std::vector<UAVState> smooth(
        const std::vector<UAVState>& rawTrajectory) = 0;
    virtual ~TrajectorySmoothing() = default;
};

// 简单的轨迹平滑器实现（可扩展）
class SimpleSmoothing : public TrajectorySmoothing {
public:
    std::vector<UAVState> smooth(
        const std::vector<UAVState>& rawTrajectory) override {
        // 这里可以实现具体的平滑算法，如样条插值
        return rawTrajectory;  // 当前仅返回原轨迹
    }
};

// 队形控制器主类
class FormationController {
public:
    FormationController(
        FormationType type,
        std::shared_ptr<FormationParams> params,
        std::shared_ptr<TrajectorySmoothing> smoother)
        : formationType(type)
        , formationParams(params)
        , trajectorySmoother(smoother) {}

    std::map<int, std::vector<UAVState>> generateFormationTrajectories(
        const std::vector<UAVState>& leaderTrajectory,
        int numFollowers) {
        
        std::map<int, std::vector<UAVState>> formationTrajs;
        
        // 为每个跟随者初始化轨迹容器
        for (int i = 1; i <= numFollowers; ++i) {
            formationTrajs[i] = std::vector<UAVState>();
        }
        
        // 计算每个时间点的队形位置
        for (const auto& leaderState : leaderTrajectory) {
            Eigen::Matrix2d R = RotationMatrix::compute(leaderState.heading);
            
            for (int i = 1; i <= numFollowers; ++i) {
                // 计算局部偏移
                Point2D localOffset = FormationOffset::compute(
                    i, formationType, formationParams);
                    
                // 转换到全局坐标
                Eigen::Vector2d localOffsetVec(localOffset.x, localOffset.y);
                Eigen::Vector2d globalOffset = R * localOffsetVec;
                
                // 计算跟随者位置
                Point2D followerPos(
                    leaderState.position.x + globalOffset(0),
                    leaderState.position.y + globalOffset(1)
                );
                
                // 创建跟随者状态并添加到轨迹中
                UAVState followerState(
                    followerPos,
                    leaderState.heading,
                    leaderState.velocity
                );
                formationTrajs[i].push_back(followerState);
            }
        }
        
        // 对所有轨迹进行平滑处理
        for (auto& [uavId, trajectory] : formationTrajs) {
            trajectory = trajectorySmoother->smooth(trajectory);
        }
        
        return formationTrajs;
    }

private:
    FormationType formationType;
    std::shared_ptr<FormationParams> formationParams;
    std::shared_ptr<TrajectorySmoothing> trajectorySmoother;
};

} // namespace uav

using namespace uav;

// 创建线型阵型参数
auto lineParams = std::make_shared<LineFormationParams>(5.0);  // 5.0米间距

// 创建轨迹平滑器
auto smoother = std::make_shared<SimpleSmoothing>();

// 创建队形控制器
FormationController controller(
    FormationType::LINE,
    lineParams,
    smoother
);

// 准备领航机轨迹
std::vector<UAVState> leaderTrajectory = {
    UAVState(Point2D(0, 0), 0, 20),  // 位置(0,0)，航向0，速度20
    UAVState(Point2D(10, 0), M_PI/4, 20),
    // ... 更多轨迹点
};

// 生成编队轨迹
int numFollowers = 3;
auto formationTrajectories = controller.generateFormationTrajectories(
    leaderTrajectory,
    numFollowers
);

#include <iostream>
#include <vector>
#include <cmath>

// -------------------- 数据结构定义 --------------------

// UAV 状态（位置、航向角、速度）
struct UAVState {
    double x;
    double y;
    double theta; // 航向角（弧度）
    double v;     // 速度（假设保持恒定或可控制）
};

// 期望轨迹点（含时间标签）
struct TrajectoryPoint {
    double t;     // 时间
    double x;
    double y;
    double theta; // 期望航向角（可选）
};

// -------------------- PID 控制器 --------------------

class PIDController {
public:
    PIDController(double kp, double ki, double kd)
        : kp_(kp), ki_(ki), kd_(kd), prevError_(0.0), integral_(0.0) {}

    // 输入误差，输出控制命令（例如：航向角速率或角度修正）
    double compute(double error, double dt) {
        integral_ += error * dt;
        double derivative = (error - prevError_) / dt;
        prevError_ = error;
        return kp_ * error + ki_ * integral_ + kd_ * derivative;
    }

private:
    double kp_, ki_, kd_;
    double prevError_;
    double integral_;
};

// -------------------- 辅助函数 --------------------

// 将角度差归一化到 [-pi, pi]
double normalizeAngle(double angle) {
    while (angle > M_PI)
        angle -= 2 * M_PI;
    while (angle < -M_PI)
        angle += 2 * M_PI;
    return angle;
}

// 根据当前 UAV 状态和期望轨迹点，计算航向角误差
double computeHeadingError(const UAVState& current, const TrajectoryPoint& desired) {
    // 计算从当前点指向参考点的角度
    double desiredTheta = std::atan2(desired.y - current.y, desired.x - current.x);
    double error = normalizeAngle(desiredTheta - current.theta);
    return error;
}

// 简单的 UAV 状态更新函数（基于 kinematic 模型）
UAVState updateState(const UAVState& current, double controlCommand, double dt) {
    UAVState next = current;
    // 假设 controlCommand 为航向角速率控制输入
    next.theta += controlCommand * dt;
    next.theta = normalizeAngle(next.theta);
    next.x += current.v * std::cos(next.theta) * dt;
    next.y += current.v * std::sin(next.theta) * dt;
    return next;
}

// -------------------- 主仿真程序 --------------------

int main() {
    // 1. 初始化仿真参数
    double simulationTime = 10.0; // 总仿真时间（秒）
    double dt = 0.1;              // 时间步长（秒）
    double currentTime = 0.0;

    // 2. 初始化 UAV 状态
    UAVState uav = {0.0, 0.0, 0.0, 5.0}; // 初始位置 (0,0), 航向 0, 速度 5

    // 3. 定义参考轨迹（生成的轨迹）——示例数据
    std::vector<TrajectoryPoint> refTrajectory = {
        {0.0, 0.0, 0.0, 0.0},
        {2.0, 2.0, 2.0, 0.0},
        {4.0, 4.0, 4.0, 0.0},
        {6.0, 6.0, 6.0, 0.0},
        {8.0, 8.0, 8.0, 0.0},
        {10.0, 10.0, 10.0, 0.0}
    };

    // 4. 初始化 PID 控制器参数（调试时根据需要调整）
    PIDController pid(1.0, 0.0, 0.1);

    // 5. 仿真主循环（按步进运行）
    while (currentTime <= simulationTime) {
        // 5.1 根据当前时间获取参考轨迹点
        // 此处简单选取离当前时间最近的点（可改为线性插值）
        TrajectoryPoint desired = refTrajectory.back();
        for (const auto& point : refTrajectory) {
            if (point.t >= currentTime) {
                desired = point;
                break;
            }
        }

        // 5.2 计算跟踪误差（这里以航向角误差为例）
        double headingError = computeHeadingError(uav, desired);

        // 5.3 控制器计算，输出控制命令（例如航向角速率）
        double controlCommand = pid.compute(headingError, dt);

        // 5.4 更新 UAV 状态（状态更新函数可以根据 UAV 动力学进行扩展）
        uav = updateState(uav, controlCommand, dt);

        // 5.5 输出当前状态与参考信息，便于观察跟踪效果
        std::cout << "Time: " << currentTime
                  << " | UAV: (" << uav.x << ", " << uav.y << ", " << uav.theta << ")"
                  << " | Ref: (" << desired.x << ", " << desired.y << ")"
                  << " | Heading Error: " << headingError
                  << std::endl;

        // 5.6 更新时间
        currentTime += dt;
    }

    return 0;
}

// trajectory_tracking.hpp
#pragma once

#include <vector>
#include <cmath>
#include <Eigen/Dense>
#include "formation_control.hpp"  // 引用之前的编队控制代码

namespace uav {

// UAV动力学模型参数
struct UAVDynamicsParams {
    double max_velocity;        // 最大速度
    double min_velocity;        // 最小速度
    double max_acceleration;    // 最大加速度
    double max_yaw_rate;       // 最大偏航角速率
    double control_frequency;   // 控制频率 (Hz)
    
    UAVDynamicsParams(
        double maxV = 30.0,
        double minV = 15.0,
        double maxA = 5.0,
        double maxYaw = 0.5,
        double freq = 50.0
    ) : max_velocity(maxV),
        min_velocity(minV),
        max_acceleration(maxA),
        max_yaw_rate(maxYaw),
        control_frequency(freq) {}
};

// 轨迹跟踪控制器
class TrajectoryTracker {
public:
    TrajectoryTracker(const UAVDynamicsParams& params)
        : dynamics_params(params)
        , dt(1.0 / params.control_frequency)
        , current_index(0) {
        // PID控制器增益
        pid_gains = {
            2.0,  // Kp for heading
            0.1,  // Ki for heading
            0.5,  // Kd for heading
            1.0,  // Kp for velocity
            0.1,  // Ki for velocity
            0.2   // Kd for velocity
        };
    }

    // 设置目标轨迹
    void setTrajectory(const std::vector<UAVState>& trajectory) {
        reference_trajectory = trajectory;
        current_index = 0;
        resetPIDErrors();
    }

    // 更新UAV状态（单步仿真）
    UAVState updateState(const UAVState& current_state) {
        if (current_index >= reference_trajectory.size()) {
            return current_state;  // 轨迹已完成
        }

        // 获取参考状态
        const UAVState& reference = reference_trajectory[current_index];
        
        // 计算误差
        double dx = reference.position.x - current_state.position.x;
        double dy = reference.position.y - current_state.position.y;
        double distance_error = std::sqrt(dx * dx + dy * dy);
        
        // 计算期望航向角
        double desired_heading = std::atan2(dy, dx);
        double heading_error = normalizeAngle(desired_heading - current_state.heading);
        
        // 更新PID误差
        updatePIDErrors(heading_error, distance_error);
        
        // 计算控制输入
        double yaw_rate = computeHeadingControl();
        double velocity = computeVelocityControl(distance_error);
        
        // 应用动力学约束
        yaw_rate = std::clamp(yaw_rate, 
            -dynamics_params.max_yaw_rate,
            dynamics_params.max_yaw_rate);
        velocity = std::clamp(velocity,
            dynamics_params.min_velocity,
            dynamics_params.max_velocity);
        
        // 更新状态
        UAVState new_state;
        new_state.heading = normalizeAngle(
            current_state.heading + yaw_rate * dt);
        new_state.velocity = velocity;
        new_state.position.x = current_state.position.x + 
            velocity * std::cos(new_state.heading) * dt;
        new_state.position.y = current_state.position.y + 
            velocity * std::sin(new_state.heading) * dt;
        
        // 检查是否需要切换到下一个轨迹点
        if (distance_error < 1.0) {  // 1.0米阈值
            current_index++;
        }
        
        return new_state;
    }

private:
    UAVDynamicsParams dynamics_params;
    std::vector<UAVState> reference_trajectory;
    std::vector<double> pid_gains;  // [Kp_h, Ki_h, Kd_h, Kp_v, Ki_v, Kd_v]
    
    double dt;
    size_t current_index;
    
    // PID控制器状态
    struct {
        double heading_error_sum = 0;
        double heading_error_prev = 0;
        double velocity_error_sum = 0;
        double velocity_error_prev = 0;
    } pid_state;

    void resetPIDErrors() {
        pid_state = {};
    }

    void updatePIDErrors(double heading_error, double distance_error) {
        pid_state.heading_error_sum += heading_error * dt;
        pid_state.velocity_error_sum += distance_error * dt;
    }

    double computeHeadingControl() {
        double p_term = pid_gains[0] * normalizeAngle(
            reference_trajectory[current_index].heading - 
            pid_state.heading_error_prev);
        double i_term = pid_gains[1] * pid_state.heading_error_sum;
        double d_term = pid_gains[2] * (normalizeAngle(
            reference_trajectory[current_index].heading - 
            pid_state.heading_error_prev) / dt);
        
        return p_term + i_term + d_term;
    }

    double computeVelocityControl(double distance_error) {
        double p_term = pid_gains[3] * distance_error;
        double i_term = pid_gains[4] * pid_state.velocity_error_sum;
        double d_term = pid_gains[5] * 
            (distance_error - pid_state.velocity_error_prev) / dt;
        
        return reference_trajectory[current_index].velocity + 
            p_term + i_term + d_term;
    }

    // 将角度归一化到[-π, π]范围
    static double normalizeAngle(double angle) {
        while (angle > M_PI) angle -= 2 * M_PI;
        while (angle < -M_PI) angle += 2 * M_PI;
        return angle;
    }
};

// 仿真管理器
class SimulationManager {
public:
    SimulationManager(
        const UAVDynamicsParams& params,
        const std::vector<UAVState>& leader_trajectory,
        int num_followers)
        : dynamics_params(params)
        , num_uavs(num_followers + 1)  // 包括领航机
    {
        // 创建编队控制器
        auto formation_params = std::make_shared<LineFormationParams>(5.0);
        auto smoother = std::make_shared<SimpleSmoothing>();
        formation_controller = std::make_unique<FormationController>(
            FormationType::LINE,
            formation_params,
            smoother
        );

        // 生成编队轨迹
        auto formation_trajectories = 
            formation_controller->generateFormationTrajectories(
                leader_trajectory,
                num_followers
            );

        // 为每个UAV创建轨迹跟踪器
        trackers.push_back(std::make_unique<TrajectoryTracker>(params));
        trackers[0]->setTrajectory(leader_trajectory);

        for (int i = 1; i <= num_followers; ++i) {
            trackers.push_back(std::make_unique<TrajectoryTracker>(params));
            trackers[i]->setTrajectory(formation_trajectories[i]);
        }

        // 初始化UAV状态
        current_states.resize(num_uavs);
        for (int i = 0; i < num_uavs; ++i) {
            if (i == 0) {
                current_states[i] = leader_trajectory[0];
            } else {
                current_states[i] = formation_trajectories[i][0];
            }
        }
    }

    // 单步仿真
    void stepSimulation() {
        for (int i = 0; i < num_uavs; ++i) {
            current_states[i] = trackers[i]->updateState(current_states[i]);
        }
    }

    // 获取当前所有UAV状态
    const std::vector<UAVState>& getCurrentStates() const {
        return current_states;
    }

private:
    UAVDynamicsParams dynamics_params;
    int num_uavs;
    std::unique_ptr<FormationController> formation_controller;
    std::vector<std::unique_ptr<TrajectoryTracker>> trackers;
    std::vector<UAVState> current_states;
};

} // namespace uav

using namespace uav;

// 创建动力学参数
UAVDynamicsParams params(
    30.0,  // 最大速度 (m/s)
    15.0,  // 最小速度 (m/s)
    5.0,   // 最大加速度 (m/s^2)
    0.5,   // 最大偏航角速率 (rad/s)
    50.0   // 控制频率 (Hz)
);

// 创建领航机轨迹
std::vector<UAVState> leader_trajectory = {
    UAVState(Point2D(0, 0), 0, 20),
    UAVState(Point2D(100, 0), 0, 20),
    UAVState(Point2D(200, 50), M_PI/4, 20),
    // ... 更多轨迹点
};

// 创建仿真管理器
SimulationManager sim(params, leader_trajectory, 3);  // 3个跟随者

// 仿真主循环
double sim_time = 0.0;
double sim_duration = 100.0;  // 仿真100秒
while (sim_time < sim_duration) {
    // 更新仿真
    sim.stepSimulation();
    
    // 获取当前状态
    const auto& states = sim.getCurrentStates();
    
    // 可以在这里添加可视化或数据记录代码
    for (const auto& state : states) {
        std::cout << "Position: (" << state.position.x << ", " 
                  << state.position.y << ")" << std::endl;
    }
    
    sim_time += 1.0 / params.control_frequency;
}

#include <iostream>
#include <vector>
#include <cmath>

// 定义二维点结构体
struct Point {
    double x;
    double y;
};

// 定义 UAV 状态结构体
struct UAVState {
    double x;
    double y;
    double theta; // 航向角（弧度）
};

// 计算两点之间欧氏距离
double distance(const Point& p1, const Point& p2) {
    return std::sqrt((p1.x - p2.x)*(p1.x - p2.x) + (p1.y - p2.y)*(p1.y - p2.y));
}

// 选择参考点函数：从轨迹中选择一个既满足前瞻距离又在前方的点
Point selectReferencePoint(const UAVState& current, const std::vector<Point>& trajectory, double lookaheadDistance) {
    // UAV 当前位置
    Point currentPos{ current.x, current.y };
    // 计算 UAV 的前向单位向量
    Point forward{ std::cos(current.theta), std::sin(current.theta) };
    
    // 遍历轨迹点，选择第一个满足条件的点
    for (const auto& candidate : trajectory) {
        // 计算候选点相对于当前的位置差
        Point diff { candidate.x - currentPos.x, candidate.y - currentPos.y };
        double d = distance(candidate, currentPos);
        // 计算点积，判断候选点是否在前方
        double dot = forward.x * diff.x + forward.y * diff.y;
        
        if (d >= lookaheadDistance && dot > 0) {
            // 找到合适的参考点
            return candidate;
        }
    }
    // 如果没有找到合适的点，返回轨迹末尾点（或根据实际需求进行其他处理）
    return trajectory.back();
}

// 示例主函数：演示在每个时间步如何选择参考点
int main() {
    // 假设已生成的平滑过渡轨迹（全局坐标），可以通过规划模块获得
    std::vector<Point> trajectory = {
        {10, 5}, {20, 10}, {30, 15}, {40, 20}, {50, 25}
    };
    
    // 仿真参数
    double lookaheadDistance = 10.0; // 前瞻距离
    
    // 模拟 UAV 初始状态
    UAVState uav = {0, 0, 0}; // 初始位置 (0,0)，航向 0 弧度（正向 x 轴）
    
    // 仿真循环（这里只做简单模拟）
    for (int step = 0; step < 10; ++step) {
        // 假设 UAV 状态由控制器更新，这里做简单线性前进模拟
        uav.x += 5; // 每步前进 5 单位
        // 例如保持航向不变，实际可通过控制器调整
        
        // 选择参考点
        Point refPoint = selectReferencePoint(uav, trajectory, lookaheadDistance);
        
        // 输出当前状态和参考点信息
        std::cout << "Step " << step << " | UAV Position: (" << uav.x << ", " << uav.y << ")"
                  << " | Reference Point: (" << refPoint.x << ", " << refPoint.y << ")\n";
        
        // 此处调用控制器：计算误差、生成控制命令，并更新 UAV 状态
        // ...
    }
    
    return 0;
}

#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>

// ---------------------- 数据结构定义 ----------------------

/// 表示二维点
struct Point {
    double x;
    double y;
};

/// 表示 UAV 状态（位置及航向角）
struct UAVState {
    double x;
    double y;
    double theta; // 航向角（弧度制）
};

// ---------------------- 辅助函数 ----------------------

/// 计算两点之间的欧氏距离
double distance(const Point& p1, const Point& p2) {
    return std::sqrt((p1.x - p2.x) * (p1.x - p2.x) +
                     (p1.y - p2.y) * (p1.y - p2.y));
}

// ---------------------- 轨迹平滑算法 ----------------------
/*
    使用简单的移动平均滤波器对原始轨迹进行平滑处理。  
    参数 windowSize 为滤波窗口大小（建议取奇数），窗口内所有点的均值作为当前平滑后的点。
*/
std::vector<Point> smoothTrajectory(const std::vector<Point>& trajectory, int windowSize = 5) {
    std::vector<Point> smoothed;
    int n = trajectory.size();
    if (n == 0) return smoothed;
    smoothed.resize(n);
    int halfWindow = windowSize / 2;
    for (int i = 0; i < n; ++i) {
        double sumX = 0.0, sumY = 0.0;
        int count = 0;
        // 计算窗口内点的均值
        int start = std::max(0, i - halfWindow);
        int end   = std::min(n - 1, i + halfWindow);
        for (int j = start; j <= end; j++) {
            sumX += trajectory[j].x;
            sumY += trajectory[j].y;
            count++;
        }
        smoothed[i].x = sumX / count;
        smoothed[i].y = sumY / count;
    }
    return smoothed;
}

// ---------------------- 前瞻参考点选择函数 ----------------------
/*
    从给定轨迹中选择一个满足前瞻距离且位于 UAV 当前航向前的点作为参考点。
    条件：候选点与 UAV 之间的距离大于 lookaheadDistance，
          且候选点相对于 UAV 当前位置在前进方向上（点积大于零）。
*/
Point selectReferencePoint(const UAVState& current, const std::vector<Point>& trajectory, double lookaheadDistance) {
    // UAV 当前的位置
    Point currentPos { current.x, current.y };
    // 计算 UAV 前进方向（单位向量）
    Point forward { std::cos(current.theta), std::sin(current.theta) };
    
    // 遍历轨迹点，选择第一个满足条件的候选点
    for (const auto& candidate : trajectory) {
        Point diff { candidate.x - currentPos.x, candidate.y - currentPos.y };
        double d = distance(candidate, currentPos);
        double dot = forward.x * diff.x + forward.y * diff.y;
        
        if (d >= lookaheadDistance && dot > 0) {
            return candidate;
        }
    }
    // 若未找到合适点，则返回轨迹最后一个点（或根据实际情况进行其他处理）
    return trajectory.back();
}

// ---------------------- 主函数 ----------------------
int main() {
    // 原始轨迹（例如从横排到楔形阵型转换过程中的规划轨迹，单位为全局坐标）
    std::vector<Point> rawTrajectory = {
        {10, 5}, {20, 10}, {30, 15}, {40, 20}, {50, 25},
        {60, 30}, {70, 35}, {80, 40}, {90, 45}, {100, 50}
    };

    // 对原始轨迹进行平滑处理（窗口大小可根据实际需要调整）
    std::vector<Point> trajectory = smoothTrajectory(rawTrajectory, 3);
    
    // 仿真中设定的前瞻距离
    double lookaheadDistance = 10.0;
    
    // 初始化 UAV 状态：初始位置 (0, 0)，航向 0 弧度（正向 x 轴）
    UAVState uav {0, 0, 0};
    
    // 简单仿真循环：模拟 UAV 沿 x 方向前进，并选择合适的参考点
    for (int step = 0; step < 10; ++step) {
        // 模拟 UAV 前进：每个时间步沿 x 轴前进 5 个单位（实际系统中由控制器更新状态）
        uav.x += 5;
        
        // 选择当前时刻的参考点
        Point refPoint = selectReferencePoint(uav, trajectory, lookaheadDistance);
        
        // 输出当前 UAV 状态和选取的参考点
        std::cout << "Step " << step << " | UAV Position: (" << uav.x << ", " << uav.y << ")"
                  << " | Reference Point: (" << refPoint.x << ", " << refPoint.y << ")\n";
        
        // 此处可调用跟踪控制器，将 refPoint 作为目标进行误差计算和控制指令生成
    }
    
    return 0;
}