无人机避障——（运动规划部分）深蓝学院动力学kinodynamic A* 3D算法理论解读（附C++代码）

开源代码链接：GitHub - Perishell/motion-planning

效果展示：

ROS 节点展示全局规划和轨迹生成部分：

Kinodynamic A*代码主体：

int KinoAstar::search(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel,
         Eigen::Vector3d end_pt, Eigen::Vector3d end_vel,
         std::vector<Eigen::Vector3d>& path)
{
  // 记录算法启动时间，计算加速度分辨率倒数，初始化最优时间为无穷大
  ros::Time start_time = ros::Time::now();
  double inv_acc_res_ = 1.0 / acc_res_;
  double optimal_time = inf;
  // 从节点池中分配起点节点，设置位置、速度、索引、实际代价g_cost（起点到当前节点的累积代价）和综合代价f_cost（g_cost + 启发式代价）
  KinoAstarNodePtr start_node = path_node_pool_[use_node_num_];
  start_node->position = start_pt;
  start_node->velocity = start_vel;
  start_node->index = posToIndex(start_pt);
  start_node->g_cost = 0.0;
  start_node->f_cost = lambda_heu_ * getHeuristicCost(start_pt, start_vel, end_pt, end_vel, optimal_time);
  use_node_num_++;
  // 将起点加入优先队列open_list_（按f_cost排序），并标记为"待扩展"
  open_list_.push(start_node);
  expanded_list_.insert(start_node->index, start_node);
  start_node->node_state = IN_OPEN_LIST_;

  std::vector<Eigen::Vector3d> path_nodes_list;

  while (!open_list_.empty())
  {
    // 循环处理开放列表，取出当前最优节点，移入关闭列表close_list_，标记为"已扩展"
    KinoAstarNodePtr current_node = open_list_.top();
    open_list_.pop();
    close_list_.insert(current_node->index, current_node);
    current_node->node_state = IN_CLOSE_LIST_;
    current_node->duration = sample_tau_;
    // debug
    // std::cout << "current_node: " << current_node->position.transpose() << std::endl;

    // check if near goal
    // 若当前节点接近终点，尝试直接生成无碰撞的最优轨迹（如多项式插值或动力学可行路径）
    if ((current_node->position - end_pt).norm() < goal_tolerance_)
    {
      double tmp_cost = lambda_heu_ * getHeuristicCost(current_node->position, current_node->velocity, end_pt, end_vel, optimal_time);
      bool shot_path_found = computeShotTraj(current_node->position, current_node->velocity, end_pt, end_vel, optimal_time);
      // std::cout << "optimal_time: " << optimal_time << std::endl;
      if (shot_path_found)
      {
        ros::Time end_time = ros::Time::now();
        std::cout << "kinodynamic path found, time cost: " << (end_time - start_time).toSec() << std::endl;
        std::cout << "use_node_num: " << use_node_num_ << std::endl;
        std::cout << "total_cost_J: " << current_node->g_cost + tmp_cost << std::endl;
        current_node->duration = optimal_time;
        std::vector<KinoAstarNodePtr> path_pool = retrievePath(current_node, path_nodes_list);
        path_nodes_list.push_back(end_pt);
        visPathNodes(path_nodes_list);
        switch (collision_check_type_)
        {
          case 1: // grid map check
          {
            samplePath(path_pool, path);
            break;
          }
          case 2: // local cloud check
          {
            sampleEllipsoid(path_pool, path, rot_list);
            visEllipsoid(path, rot_list);
            break;
          }  
        }
        return REACH_END;
      }
      else if (current_node->parent != NULL)
      {
        // std::cout << "near end!" << std::endl;
      }
      else
      {
        std::cout << "no shot path found, end searching" << std::endl;
        return NO_PATH_FOUND;   
      }
      // else continue;  
    }

    // expand current node
    // (2r+1)^3 sample
    for (double ax = -max_accel_; ax <= max_accel_ + 1e-3; ax += inv_acc_res_ * max_accel_)
      for (double ay = -max_accel_; ay <= max_accel_ + 1e-3; ay += inv_acc_res_ * max_accel_)
        for (double az = -max_accel_; az <= max_accel_ + 1e-3; az += inv_acc_res_ * max_accel_)
        {
          Eigen::Vector3d ut;
          ut << ax, ay, az;
          Eigen::Matrix<double, 6, 1> x0;
          x0.head(3) = current_node->position;
          x0.tail(3) = current_node->velocity;
          Eigen::Matrix<double, 6, 1> xt;

          int segment_num = std::floor(sample_tau_ / step_size_);
          bool flag = false;
          bool collision_flag = false;
          for (int i = 0; i <= segment_num; i++)
          {
            double t = i * step_size_;
            StateTransit(x0, xt, ut, t);
            Eigen::Vector3d tmp_pos = xt.head(3);
            // check collision and if out of map
            if (grid_map_->isInMap(tmp_pos) == false)
            {
              flag = true;
              break;
            }
            // check collision
            switch (collision_check_type_)
            {
              case 1: // grid map check
                if (grid_map_->getInflateOccupancy(tmp_pos) == 1)
                {
                  collision_flag = true;
                  break;
                }
              
              case 2: // local cloud check
                if (isCollisionFree(tmp_pos, ut) == false)
                {
                  collision_flag = true;
                  break;
                }
            }
            if (collision_flag)
            {
              flag = true;
              break;
            }
            // check velocity limit
            if (xt.tail(3)(0) < -max_vel_ || xt.tail(3)(0) > max_vel_ 
                || xt.tail(3)(1) < -max_vel_ || xt.tail(3)(1) > max_vel_ 
                || xt.tail(3)(2) < -max_vel_ || xt.tail(3)(2) > max_vel_)
              {
                flag = true;
                break;
              }
          }
          if (flag) continue;

          StateTransit(x0, xt, ut, sample_tau_);
          // std::cout << "xt: " << xt.head(3).transpose() << std::endl;
          // std::cout << "index: " << posToIndex(xt.head(3)).transpose() << std::endl;
          // check if in close_list_
          if (close_list_.find(posToIndex(xt.head(3))) != NULL)
          {
            continue;
          }
          // check if in expanded_list_
          else if (expanded_list_.find(posToIndex(xt.head(3))) == NULL)
          {
            KinoAstarNodePtr pro_node = path_node_pool_[use_node_num_];
            pro_node->position = xt.head(3);
            pro_node->velocity = xt.tail(3);
            pro_node->index = posToIndex(xt.head(3));
            pro_node->g_cost = current_node->g_cost + (ut.dot(ut) + rou_) * sample_tau_;
            pro_node->f_cost = pro_node->g_cost + lambda_heu_ * getHeuristicCost(pro_node->position, pro_node->velocity, end_pt, end_vel, optimal_time);
            pro_node->parent = current_node;
            pro_node->input = ut;
            pro_node->duration = sample_tau_;
            pro_node->node_state = IN_OPEN_LIST_;
            use_node_num_++;
            open_list_.push(pro_node);
            expanded_list_.insert(pro_node->index, pro_node);

            // check if use_node_num reach the max_node_num
            if (use_node_num_ >= allocated_node_num_)
            {
              std::cout << "reach max node num, end searching" << std::endl;
              return NO_PATH_FOUND;
            }
          }
          else
          {
            // pruning, if in same grid, check if g_cost is smaller and update
            double tmp_g_cost = current_node->g_cost + (ut.dot(ut) + rou_) * sample_tau_;
            KinoAstarNodePtr old_node = expanded_list_.find(posToIndex(xt.head(3)));
            if (tmp_g_cost < old_node->g_cost)
            {
              old_node->position = xt.head(3);
              old_node->velocity = xt.tail(3);
              // old_node->index = posToIndex(xt.head(3));
              old_node->g_cost = tmp_g_cost;
              old_node->f_cost = old_node->g_cost + lambda_heu_ * getHeuristicCost(old_node->position, old_node->velocity, end_pt, end_vel, optimal_time);
              old_node->parent = current_node;
              old_node->input = ut;
            }
          }
        }

  }

  std::cout << "open_list is empty, end searching, no path found" << std::endl;
  return NO_PATH_FOUND;
}

初始化阶段：

ros::Time start_time = ros::Time::now();
double inv_acc_res_ = 1.0 / acc_res_;
double optimal_time = inf;

记录算法启动时间，计算加速度分辨率倒数，初始化最优时间为无穷大。

起点节点初始化：

KinoAstarNodePtr start_node = path_node_pool_[use_node_num_];
start_node->position = start_pt;
start_node->velocity = start_vel;
start_node->index = posToIndex(start_pt);
start_node->g_cost = 0.0;
start_node->f_cost = lambda_heu_ * getHeuristicCost(...);
use_node_num_++;

从节点池中分配起点节点，设置位置、速度、索引、实际代价g_cost（起点到当前节点的累积代价）和综合代价f_cost（g_cost + 启发式代价）。

开放列表与扩展列表管理​：

open_list_.push(start_node);
expanded_list_.insert(start_node->index, start_node);
start_node->node_state = IN_OPEN_LIST_;

将起点加入优先队列open_list_（按f_cost排序），并标记为"待扩展"。

主搜索循环​:

while (!open_list_.empty()) {
KinoAstarNodePtr current_node = open_list_.top();
open_list_.pop();
close_list_.insert(current_node->index, current_node);
current_node->node_state = IN_CLOSE_LIST_;

循环处理开放列表，取出当前最优节点，移入关闭列表close_list_，标记为"已扩展"。

终点检查与轨迹优化​：

if ((current_node->position - end_pt).norm() < goal_tolerance_) {
    bool shot_path_found = computeShotTraj(...);
    if (shot_path_found) {
        samplePath(path_pool, path);
        return REACH_END;
    }
}

若当前节点接近终点，尝试直接生成无碰撞的最优轨迹（如多项式插值或动力学可行路径）。

samplePath：

主体部分：

void KinoAstar::samplePath(std::vector<KinoAstarNodePtr> path_pool, std::vector<Eigen::Vector3d>& path)
{
  // 当路径池中有多个节点时（非单节点情况）
  if (path_pool.size() != 1)
  {
    // 遍历路径池中相邻的节点对
    for (int i = 0; i < path_pool.size() - 1; i++)
    {
      KinoAstarNodePtr curr_node = path_pool[i];   // 当前路径节点
      KinoAstarNodePtr next_node = path_pool[i + 1]; // 下一路径节点
      double curr_t = 0.0;  // 当前时间戳
      Eigen::Matrix<double, 6, 1> x0, xt; // 状态向量（6维：位置+速度）
      x0 << curr_node->position, curr_node->velocity; // 初始化当前状态

      // 根据节点持续时间分割时间段（基于固定步长step_size_）
      int segment_num = std::floor(curr_node->duration / step_size_);
      // 逐时间段生成路径点
      for (int j = 0; j < segment_num; j++)
      {
        curr_t = j * step_size_;  // 计算当前时间
        StateTransit(x0, xt, next_node->input, curr_t); // 状态转移计算（可能涉及运动学方程）[1,5](@ref)
        path.push_back(xt.head(3));  // 将位置信息(x,y,z)加入路径集合
      }
      // 处理最后一个时间段（可能不完整的步长）
      StateTransit(x0, xt, next_node->input, curr_node->duration);
      // 验证最终状态与目标节点的位置一致性
      if ((xt.head(3) - next_node->position).norm() > 1e-2)
      {
        std::cerr << "error in sample!" << std::endl; // 超阈值报错
      }
    }

    // 处理路径池中最后一个节点的轨迹
    KinoAstarNodePtr last_node = path_pool.back();
    double td = last_node->duration;  // 获取总持续时间
    Eigen::Matrix<double, 4, 1> t_vector; // 时间多项式向量（t^0, t^1, t^2, t^3）

    // 分割时间段生成路径点
    int segment_num = std::floor(td / step_size_);
    double curr_t = 0.0;
    for (int j = 0; j <= segment_num; j++)
    {
      curr_t = j * step_size_;  // 当前时间戳
      for (int i = 0; i < 4; i++) {
        t_vector(i) = pow(curr_t, i); // 构建时间多项式项（如t³, t², t, 1）
      }
      Eigen::Vector3d shot_pos = shot_coef_ * t_vector; // 通过多项式系数生成路径点（轨迹插值）[5](@ref)
      path.push_back(shot_pos); // 将插值点加入路径
    }
  }
  else  // 当路径池只有单个节点时的处理逻辑
  {
    KinoAstarNodePtr last_node = path_pool.back();
    double td = last_node->duration;  // 总持续时间
    Eigen::Matrix<double, 4, 1> t_vector; // 同上时间多项式向量

    int segment_num = std::floor(td / step_size_);
    double curr_t = 0.0;
    for (int j = 0; j <= segment_num; j++) {
      curr_t = j * step_size_;
      for (int i = 0; i < 4; i++) {
        t_vector(i) = pow(curr_t, i);
      }
      Eigen::Vector3d shot_pos = shot_coef_ * t_vector; // 多项式轨迹生成
      path.push_back(shot_pos);
    }
  }
}

该函数主要用于：

1. ​路径采样​ ：根据路径池（path_pool）中的节点，通过时间分片（step_size_）生成连续路径点。
2. ​状态转移​ ：在多节点情况下，通过StateTransit函数计算相邻节点间的运动状态（可能包含速度/加速度模型）
3. ​轨迹插值​ ：在单节点或最终阶段，利用四次多项式系数（shot_coef_）生成平滑轨迹，适用于机器人/自动驾驶的运动规划
4. ​错误校验​：检查采样结果与目标节点的位置偏差，确保路径精度。

getHeuristicCost：

主体部分：

double KinoAstar::getHeuristicCost(Eigen::Vector3d x1, Eigen::Vector3d v1,
                        Eigen::Vector3d x2, Eigen::Vector3d v2,
                        double &optimal_time)
{
  Eigen::Vector3d dp = x2 - x1;
  double optimal_cost = inf;

  double a = -36 * dp.dot(dp);
  double b = 24 * dp.dot(v1 + v2);
  double c = -4 * (v1.dot(v1) + v1.dot(v2) + v2.dot(v2));
  double d = 0;
  double e = rou_;

  std::vector<double> dts = quartic(e, d, c, b, a);
  double T_bar = ((x1 - x2).lpNorm<Eigen::Infinity>() / max_vel_);
  for (int i = 0; i < dts.size(); i++)
  {
    double t = dts[i];
    double tmp_cost = a / (-3 * t * t * t) + b / (-2 * t * t) + c / (-1 * t) + e * t;
    if (tmp_cost < optimal_cost && t > T_bar && tmp_cost > 0)
    {
      optimal_cost = tmp_cost;
      optimal_time = t;
    }
  }
  return tie_breaker_ * optimal_cost;

}

KinoAstar::getHeuristicCost 是运动规划中结合动力学约束的启发式函数，其核心目标是​​通过最优控制理论计算从当前状态到目标状态的最小控制能量代价​​，用于引导A*算法的搜索方向并加速收敛。以下是逐层解析：

函数参数与目标​：

• 输入参数​ ：
  • x1/v1：起点位置和速度
  • x2/v2：终点位置和速度
  • optimal_time（输出）：最优时间估计
• ​输出​：启发式代价（预估最小控制能量）

数学原理：最优控制问题建模​：

​该函数基于​​庞特里亚金极小值原理​​（Pontryagin's Minimum Principle），求解两点边界最优控制问题（OBVP）。目标是最小化控制能量（如加加速度的积分），同时满足位置和速度的终点约束。

其中：

• j(t) 是加加速度（控制输入）
• ρ 是时间权重（对应代码中的 rou_）

通过状态方程和协态方程推导，最终得到关于时间 T 的​​四次多项式方程​​，其根对应可能的极值点。

代码实现步骤​：

1. 构造四次方程系数​​：

   • 计算位置差 dp = x2 - x1
   • 根据几何关系和控制能量积分，生成系数 a, b, c, d, e（对应四次方程 aT^4+bT^3+cT^2+d^T+e=0）

2. ​​求解方程根​​：

   • 调用 quartic(e, d, c, b, a) 解四次方程，得到候选时间解 dts。

3. ​​筛选最优时间与代价​​：

   • 计算时间下限 T_bar（确保速度不超过最大值 max_vel_）
   • 遍历所有候选时间 t，计算对应的能量代价 tmp_cost，筛选满足 t>T_bar 的最小代价 optimal_cost 及对应时间 optimal_time。

4. ​​返回加权代价​​：

   • 最终代价乘以 tie_breaker_（通常略大于1的系数，避免路径重复扩展）

computeShotTraj：

主体部分：

bool KinoAstar::computeShotTraj(Eigen::Vector3d x1, Eigen::Vector3d v1,
                            Eigen::Vector3d x2, Eigen::Vector3d v2,
                            double optimal_time)
{
  double td = optimal_time;
  Eigen::Vector3d dp = x2 - x1;
  Eigen::Vector3d dv = v2 - v1;
  shot_coef_.col(0) = x1;
  shot_coef_.col(1) = v1;
  shot_coef_.col(2) = 0.5 * (6 / (td * td) * (dp - v1 * td) - 2 * dv / td);
  shot_coef_.col(3) = 1.0 / 6.0 * (-12 / (td * td *td) * (dp - v1 * td) + 6 * dv / (td * td));

  Eigen::Matrix<double, 4, 4> Transit_v;
  Transit_v << 0, 0, 0, 0, 
             1, 0, 0, 0,
             0, 2, 0, 0, 
             0, 0, 3, 0;
  Eigen::Matrix<double, 4, 4> Transit_a;
  Transit_a << 0, 0, 0, 0, 
             1, 0, 0, 0,
             0, 2, 0, 0, 
             0, 0, 0, 0;
  vel_coef_ = shot_coef_ * Transit_v;
  acc_coef_ = vel_coef_ * Transit_a;

  Eigen::Matrix<double, 4, 1> t_vector;

  int segment_num = std::floor(td / step_size_);
  double curr_t = 0.0;
  for (int j = 0; j <= segment_num; j++)
  {
    curr_t = j * step_size_;
    for (int i = 0; i < 4; i++)
    {
      t_vector(i) = pow(curr_t, i);
    }

    Eigen::Vector3d shot_pos = shot_coef_ * t_vector;
    Eigen::Vector3d shot_vel = vel_coef_ * t_vector;
    Eigen::Vector3d shot_acc = acc_coef_ * t_vector;
    // check collision
    if (grid_map_->getInflateOccupancy(shot_pos))
    {
      return false;
    }
    // only check collision, vel and acc limit by limit T_
    // // check velocity limit
    // if (shot_vel(0) > max_vel_ || shot_vel(0) < -max_vel_ || 
    //     shot_vel(1) > max_vel_ || shot_vel(1) < -max_vel_ || 
    //     shot_vel(2) > max_vel_ || shot_vel(2) < -max_vel_)
    // {
    //   return false;
    // }
    // // check acceleration limit
    // if (shot_acc(0) > max_accel_ || shot_acc(0) < -max_accel_ || 
    //     shot_acc(1) > max_accel_ || shot_acc(1) < -max_accel_ || 
    //     shot_acc(2) > max_accel_ || shot_acc(2) < -max_accel_)
    // {
    //   return false;
    // }
  }
  return true;
}

定义了一个函数computeShotTraj，属于类KinoAstar：

bool KinoAstar::computeShotTraj(Eigen::Vector3d x1, Eigen::Vector3d v1,
                            Eigen::Vector3d x2, Eigen::Vector3d v2,
                            double optimal_time)

该函数用于计算一步到位的航迹，参数包括初始位置x1、初始速度v1、目标位置x2、目标速度v2，以及最优时间optimal_time，返回值为布尔类型。

double td = optimal_time;

将optimal_time赋值给变量td，表示轨迹的时间长度。

Eigen::Vector3d dp = x2 - x1;
Eigen::Vector3d dv = v2 - v1;

计算位置差和速度差。

shot_coef_.col(0) = x1;
shot_coef_.col(1) = v1;
shot_coef_.col(2) = 0.5 * (6 / (td * td) * (dp - v1 * td) - 2 * dv / td);
shot_coef_.col(3) = 1.0 / 6.0 * (-12 / (td * td *td) * (dp - v1 * td) + 6 * dv / (td * td));

将初始位置x1和初始速度v1赋值给shot_coef_的第一列和第二列，shot_coef_是一个用于存储三次多项式系数的矩阵。计算三次多项式的第二项系数，涉及位置差dp、初始速度v1、速度差dv，以及时间td的计算。

Eigen::Matrix<double, 4, 4> Transit_v;
  Transit_v << 0, 0, 0, 0, 
             1, 0, 0, 0,
             0, 2, 0, 0, 
             0, 0, 3, 0;
  Eigen::Matrix<double, 4, 4> Transit_a;
  Transit_a << 0, 0, 0, 0, 
             1, 0, 0, 0,
             0, 2, 0, 0, 
             0, 0, 0, 0;

定义一个4x4的矩阵Transit_v，用于将位置系数转换为速度系数。矩阵的结构是根据速度的导数特性定义的。定义一个4x4的矩阵Transit_a，用于将速度系数转换为加速度系数。矩阵的结构是根据加速度的导数特性定义的。

vel_coef_ = shot_coef_ * Transit_v;
acc_coef_ = vel_coef_ * Transit_a;

通过shot_coef_和Transit_v的矩阵乘法，计算出速度系数矩阵vel_coef_。 通过vel_coef_和Transit_a的矩阵乘法，计算出加速度系数矩阵acc_coef_。

Eigen::Matrix<double, 4, 1> t_vector;
int segment_num = std::floor(td / step_size_);
double curr_t = 0.0;
for (int j = 0; j <= segment_num; j++)
  {
    curr_t = j * step_size_;
    for (int i = 0; i < 4; i++)
    {
      t_vector(i) = pow(curr_t, i);
    }

    Eigen::Vector3d shot_pos = shot_coef_ * t_vector;
    Eigen::Vector3d shot_vel = vel_coef_ * t_vector;
    Eigen::Vector3d shot_acc = acc_coef_ * t_vector;
    // check collision
    if (grid_map_->getInflateOccupancy(shot_pos))
    {
      return false;
    }

定义一个4x1的矩阵t_vector，用于存储时间的幂次计算。计算时间分割数segment_num，即时间td除以步长step_size_并向下取整。循环遍历时间分割点，从0到segment_num。计算当前时间curr_t，即j乘以步长step_size_。通过shot_coef_和t_vector的矩阵乘法，计算出当前位置shot_pos。通过vel_coef_和t_vector的矩阵乘法，计算出当前速度shot_vel。通过acc_coef_和t_vector的矩阵乘法，计算出当前加速度shot_acc。检查当前位置shot_pos是否与网格地图中的膨胀障碍物发生碰撞。如果发生碰撞，返回false，表示轨迹不可行。

retrievePath：

主体部分：

std::vector<KinoAstarNodePtr> KinoAstar::retrievePath(KinoAstarNodePtr end_node, std::vector<Eigen::Vector3d>& path_nodes_list)
{
  KinoAstarNodePtr current_node = end_node;
  std::vector<KinoAstarNodePtr> path_nodes;

  while (current_node->parent != NULL)
  {
    path_nodes.push_back(current_node);
    path_nodes_list.push_back(current_node->position);
    current_node = current_node->parent;
  }
  path_nodes.push_back(current_node);
  path_nodes_list.push_back(current_node->position);
  std::reverse(path_nodes.begin(), path_nodes.end());
  std::reverse(path_nodes_list.begin(), path_nodes_list.end());
  return path_nodes;
}

std::vector<KinoAstarNodePtr> path_pool = retrievePath(current_node, path_nodes_list);

这段代码定义了一个函数 retrievePath，属于类 KinoAstar，用于从终点节点回溯并提取出路径上的所有节点以及它们的位置，形成完整的路径。

1. 路径回溯 ：从终点节点开始，通过每个节点的parent指针逐级向上访问，直到找到起点（即parent为NULL的节点）。
2. 路径构建 ：在回溯过程中，将每个节点及其位置分别添加到path_nodes和path_nodes_list中。
3. 顺序调整 ：由于回溯得到的路径是从终点到起点的顺序，使用std::reverse将两个向量反转，最终得到从起点到终点的正确路径顺序。

节点扩展与状态转移：

for (double ax = -max_accel_; ...) {
    Eigen::Vector3d ut(ax, ay, az);
    StateTransit(x0, xt, ut, sample_tau_);
    // 碰撞检测与速度限制检查
}

功能​​：遍历所有可能的加速度控制输入（三维离散化），生成子节点状态

• 关键点​ ：
  • ​状态转移方程​ ：StateTransit根据动力学模型（如匀加速运动学）计算下一时刻状态
  • ​碰撞检测​ ：分段检查轨迹段是否在自由空间内（grid_map_或点云检测）

StateTransit：

主体部分：

void KinoAstar::StateTransit(Eigen::Matrix<double, 6, 1> &x0, 
                            Eigen::Matrix<double, 6, 1> &xt,
                            Eigen::Vector3d ut, 
                            double t)
{
  // 初始化6x6状态转移矩阵e_At为单位矩阵（初始状态下时间t=0时的状态转移）
  Eigen::Matrix<double, 6, 6> e_At = Eigen::Matrix<double, 6, 6>::Identity();
  
  // 前三行的位置-速度关系处理：位置 += 速度 * 时间t（例如x = x0 + vx*t）
  for (int i = 0; i < 3; i++) {
    e_At(i, 3 + i) = t;  // 例如e_At矩阵第0行第3列设为t，对应x方向的位置增量
  }
  
  // 初始化6x3的积分矩阵（用于控制输入ut对状态的影响）
  Eigen::Matrix<double, 6, 3> Integral = Eigen::Matrix<double, 6, 3>::Zero();
  
  // 填充积分矩阵：
  // 前三行对角线元素为0.5*t²（加速度对位置的影响，如Δx = 0.5*a*t²）
  // 后三行对角线元素为t（加速度对速度的影响，如Δv = a*t）
  for (int i = 0; i < 6; i++) {
    if (i < 3) 
      Integral(i, i) = 0.5 * t * t;  // 位置积分项
    else 
      Integral(i, i - 3) = t;         // 速度积分项
  }

  // 状态转移方程：xt = e_At * x0 + Integral * ut
  // 其中：
  // - e_At * x0 表示初始状态的线性传播（位置+速度）
  // - Integral * ut 表示控制输入（如加速度）对状态的影响
  xt = e_At * x0 + Integral * ut;
}

功能总结：

1. ​​状态转移模型​ ​

   基于线性运动学模型，通过矩阵运算实现状态预测。假设系统为二阶模型（包含位置和速度），控制输入ut可能对应加速度

2. ​​矩阵作用​​

   • ​e_At​：6x6矩阵，描述位置和速度随时间变化的线性关系（如匀速运动模型）。
   • ​Integral​：6x3矩阵，计算控制输入对状态的积分贡献（如匀加速运动中的位移和速度变化）

3. ​​物理意义​ ​

   假设系统状态为[x, y, z, vx, vy, vz]，控制输入为加速度[ax, ay, az]，则：

   • 位置更新：x_new = x0 + vx*t + 0.5*ax*t²
   • 速度更新：vx_new = vx + ax*t