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

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

效果展示:

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

Kinodynamic A*代码主体:

cpp 复制代码
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;
}

初始化阶段:

cpp 复制代码
ros::Time start_time = ros::Time::now();
double inv_acc_res_ = 1.0 / acc_res_;
double optimal_time = inf;

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

起点节点初始化:

cpp 复制代码
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_costg_cost + 启发式代价)。

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

cpp 复制代码
open_list_.push(start_node);
expanded_list_.insert(start_node->index, start_node);
start_node->node_state = IN_OPEN_LIST_;

将起点加入优先队列open_list_(按f_cost排序),并标记为"待扩展"。

主搜索循环​:

cpp 复制代码
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_,标记为"已扩展"。

终点检查与轨迹优化​:

cpp 复制代码
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:

主体部分:

cpp 复制代码
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:

主体部分:

cpp 复制代码
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:

主体部分:

cpp 复制代码
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:

cpp 复制代码
bool KinoAstar::computeShotTraj(Eigen::Vector3d x1, Eigen::Vector3d v1,
                            Eigen::Vector3d x2, Eigen::Vector3d v2,
                            double optimal_time)

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

cpp 复制代码
double td = optimal_time;

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

cpp 复制代码
Eigen::Vector3d dp = x2 - x1;
Eigen::Vector3d dv = v2 - v1;

计算位置差和速度差。

cpp 复制代码
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的计算。

cpp 复制代码
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,用于将速度系数转换为加速度系数。矩阵的结构是根据加速度的导数特性定义的。

cpp 复制代码
vel_coef_ = shot_coef_ * Transit_v;
acc_coef_ = vel_coef_ * Transit_a;

通过shot_coef_Transit_v的矩阵乘法,计算出速度系数矩阵vel_coef_。 通过vel_coef_Transit_a的矩阵乘法,计算出加速度系数矩阵acc_coef_

cpp 复制代码
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:

主体部分:

cpp 复制代码
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;
}
cpp 复制代码
std::vector<KinoAstarNodePtr> path_pool = retrievePath(current_node, path_nodes_list);

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

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

节点扩展与状态转移:

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

功能​​:遍历所有可能的加速度控制输入(三维离散化),生成子节点状态

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

StateTransit:

主体部分:

cpp 复制代码
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
相关推荐
总有刁民想爱朕ha1 天前
YOLO目标检测:一种用于无人机的新型轻量级目标检测网络
yolo·目标检测·无人机
GIS数据转换器1 天前
2025无人机在农业生态中的应用实践
大数据·网络·人工智能·安全·无人机
无线图像传输研究探索1 天前
5G无人机用单兵图传设备 5G单兵图传 无线图传 无人机图传
5g·无人机·5g单兵图传·单兵图传·无人机图传
蜀中廖化1 天前
高压输电线背景下无人机检测输电线和周围树木以及计算两者之间的距离
无人机·双目摄像头测距·输电线检测·树木检测
应用市场1 天前
傅里叶变换在无人机振动分析中的应用详解
无人机
云卓SKYDROID1 天前
无人机RTK信号增强技术要点
无人机·遥控器·中继器·高科技·云卓科技
GIS数据转换器2 天前
带高度多边形,生成3D建筑模型,支持多种颜色或纹理的OBJ、GLTF、3DTiles格式
数据库·人工智能·机器学习·3d·重构·无人机
天青色等烟雨..4 天前
AI+Python驱动的无人机生态三维建模与碳储/生物量/LULC估算全流程实战技术
人工智能·python·无人机
青岛前景互联信息技术有限公司4 天前
前景互联应急救援指挥平台接入大疆机场3无人机
物联网·无人机·智慧城市
GIS 数据栈4 天前
SegGIS 无人机 · 遥感影像识别软件V2(重大更新)
无人机