Implicit-SDF-Planner代码详解（2）

Implicit-SDF-Planner代码详解（2）

第三部分:轨迹优化

在获得了一条初始路径后,下一步就是在此基础上生成一条光滑、安全、动力学可行的轨迹,这就是轨迹优化要解决的问题。

让我们回到plan_manager.cpp,看看PlannerManager的generateTraj函数:

void PlannerManager::generateTraj(const vector<Vector3d> &path)
{
  // 从A*路径中均匀采样得到控制点
  vector<Vector3d> Q;
  vector<Vector3d> acc_list;
  vector<Matrix3d> rotatelist;
  
  for (int ind = index_gap; ind < path_size - 1; ind += index_gap)
  {
    Q.push_back(path[ind]);
    // ...
  }
  
  // 设置优化的障碍物点云
  minco_traj_optimizer->parallel_points.clear();
  for (const auto &pair : minco_traj_optimizer->pcsmap_manager->aabb_points)
  {
    minco_traj_optimizer->parallel_points.push_back(pair.second); 
  }
  
  // 调用OriTraj生成初始轨迹
  bool mid_ret;
  Eigen::VectorXd opt_x;
  mid_ret = ori_traj_generator->getOriTraj(initState, finalState, Q, ts, acc_list, rotatelist, N, recent_traj, opt_x);
  
  // 将初始轨迹传递给swept volume manager
  sv_manager->updateTraj(recent_traj);
  
  if (mid_ret)
  {
    // 调用TrajOptimizer进行轨迹优化
    bool ret = minco_traj_optimizer->optimize_traj_lmbm(initState, finalState, opt_x, N, recent_traj);
    // ...
    
    if (ret)
    {
      // 更新swept volume,检查是否碰撞
      sv_manager->updateTraj(recent_traj);
      if (sv_manager->isTrajCollide())
      {
        ROS_WARN("traj collide.");
      }
    }
  }
}

这个函数主要做了以下几件事:

1. 从A*路径中均匀采样,得到一些控制点Q。
2. 从地图中提取障碍物点云,传递给轨迹优化器。
3. 调用OriTraj的getOriTraj函数,生成一条初始轨迹。
4. 将初始轨迹传递给swept volume manager,用于碰撞检测。
5. 调用TrajOptimizer的optimize_traj_lmbm函数,在初始轨迹的基础上进行优化。
6. 将最终轨迹传递给swept volume manager,检查是否碰撞。

让我们首先看一下OriTraj是如何生成初始轨迹的:

bool OriTraj::getOriTraj(const Eigen::Matrix3d initS,
                         const Eigen::Matrix3d finalS,
                         const std::vector<Eigen::Vector3d> &Q,
                         Eigen::VectorXd T,
                         std::vector<Eigen::Vector3d> acc_list,
                         std::vector<Eigen::Matrix3d> rot_list,
                         const int N,
                         Trajectory<TRAJ_ORDER> &traj,
                         Eigen::VectorXd& opt_x)
{
  // 设置boundary condition
  initState  = initS;
  finalState = finalS;
  minco.setConditions(initState, finalState, pieceN);
  
  // 设置优化变量
  Eigen::VectorXd x(temporalDim + spatialDim);
  Eigen::Map<Eigen::VectorXd> tau(x.data(), temporalDim);
  Eigen::Map<Eigen::VectorXd> xi(x.data() + temporalDim, spatialDim);
  backwardT(T, tau);
  
  for (int i = 0; i < N - 1; ++i)
  {
    xi.segment(3 * i, 3) = Q[i];
    ref_points.col(i) = Q[i];
    accelerations.col(i) = acc_list[i];
    tk += T(i + 1);
  }

  // 设置Lbfgs参数
  lbfgs::lbfgs_parameter_t lbfgs_params;
  // ...
  
  // 开始优化
  int ret = lbfgs::lbfgs_optimize(x,
                                  final_cost,
                                  &costFunction,
                                  nullptr,
                                  &earlyExit,
                                  this,
                                  lbfgs_params);
                                                    
  if (ret >= 0)
  {
    forwardT(tau, times); 
    forwardP(xi, points);
    minco.setConditions(initState, finalState, pieceN);
    minco.setParameters(points, times);
    minco.getTrajectory(traj);
    opt_x = x;
    return true;
  }
  else
  {
    // ...
    return false;
  }                                                                    
}

这个函数的主要步骤如下:

1. 设置boundary condition,即起点状态和终点状态。
2. 设置优化变量,包括时间变量tau和空间变量xi。
3. 设置Lbfgs优化器的参数。
4. 调用Lbfgs优化器,开始优化。优化目标由costFunction定义。
5. 如果优化成功,提取优化后的时间和控制点,生成轨迹。

其中,优化变量的排列顺序为[tau, xi],tau为时间变量,xi为控制点变量。它们与轨迹之间的关系如下:

• 轨迹由n段quintic polynomial组成,每段的时间间隔为tau[i]。
• 轨迹的第i段上的第j个控制点为xi[3*i+j]。

backwardT函数将时间T转化为tau:

void OriTraj::backwardT(Eigen::VectorXd &T, Eigen::Ref<Eigen::VectorXd> tau)
{
  tau(0) = T(0);
  for (int i = 1; i < T.size(); ++i)
  {
    tau(i) = T(i) - T(i - 1);
  }
}

forwardT函数将tau转化为时间T:

void OriTraj::forwardT(const Eigen::Ref<const Eigen::VectorXd> &tau, Eigen::VectorXd &T)
{
  T.resize(tau.size());
  T(0) = tau(0);
  for (int i = 1; i < tau.size(); ++i)
  {
    T(i) = T(i - 1) + tau(i);
  }
}

forwardP函数将优化后的控制点xi转化为点序列points:

void OriTraj::forwardP(const Eigen::Ref<const Eigen::VectorXd>& xi, Eigen::Ref<Eigen::Matrix3Xd> points)
{
  int N = points.cols();
  for (int i = 0; i < N - 1; ++i)
  {
    points.col(i) = xi.segment(3 * i, 3);
  }
}

最后,通过minco库的setConditions、setParameters和getTrajectory函数,生成最终的轨迹。

接下来,让我们看一下costFunction,看看优化的目标是什么:

double OriTraj::costFunction(void *ptr,
                             const double *x,
                             double *grad,
                             const int n)
{
  OriTraj* ptr_traj = reinterpret_cast<OriTraj*>(ptr);
  const int N = ptr_traj->pieceN;
  const Eigen::Matrix3d& head = ptr_traj->initState;
  const Eigen::Matrix3d& tail = ptr_traj->finalState;
  
  // 提取优化变量
  Eigen::Map<const Eigen::VectorXd> tau(x, N);
  Eigen::Map<const Eigen::VectorXd> xi(x + N, 3 * (N - 1));
  Eigen::Map<const Eigen::MatrixXd> points(xi.data(), 3, N - 1);
  
  // smooth cost
  double cost_smooth = 0.0;
  Eigen::Vector3d zero(0,0,0);
  Eigen::Matrix3Xd q = ptr_traj->points;
  Eigen::Matrix3Xd ref = ptr_traj->ref_points;
  Eigen::Matrix3Xd acc = ptr_traj->accelerations;
  
  Eigen::VectorXd T_ori(N); // Time of original position
  Eigen::VectorXd T(N);
  ptr_traj->forwardT(tau,T);
  ptr_traj->forwardT(ptr_traj->times, T_ori);
  int grad_flag = grad ? 1 : 0;
  Eigen::VectorXd gdT(3*N), gdX(3*N);
  
  Eigen::MatrixXd Q_3D = Eigen::MatrixXd::Zero(6, N + 1);
  Q_3D.block(0, 1, 3, N - 1) = points;
  Q_3D.block(0, 0, 3, 1) = ptr_traj->initState.col(0);
  Q_3D.block(0, N, 3, 1) = ptr_traj->finalState.col(0);
  
  Eigen::VectorXd vT(N);
  for (int i = 0; i < N; ++i) 
  {
    vT(i) = T(i + 1) - T(i);
  }
  
  double  ts = 0;
  double  t_ori = 0;
  Eigen::VectorXd Thre = ptr_traj->ref_cost_threshold;
  int M = 20;
  
  for (int i = 0; i < N; i++)
  {
    ts = ts + tau(i);
    t_ori = t_ori + ptr_traj->times(i);
  }
  
  // 加速度
  cost_smooth = ptr_traj->minco.calRibbonCost_fixdt(Q_3D, vT, grad_flag, gdT, gdX, ptr_traj->Tk_range, ptr_traj->minco_lowerBound_range, ptr_traj->minco_upperBound_range, ptr_traj->minco_maxvel_range);
  
  // attitde cost
  Eigen::Matrix3Xd att_err, att_vec;
  att_err.resize(3,N-1);
  att_vec.resize(3,N-1);
  Eigen::Vector3d att_temp;
  double cost_att = 0.0;
  
  for (int i = 0 ; i < N - 1; i++)
  {
    att_temp = acc.col(i);
    if (att_temp.norm() < 1e-4)
    {
      att_temp = Eigen::Vector3d(0,0,1);
    }
    att_temp = att_temp / att_temp.norm();
    Eigen::Matrix3d R_temp = ptr_traj->att_constraints[i];
    Eigen::Vector3d R_tempy = R_temp.col(1);
    Eigen::Vector3d err = R_tempy - att_temp;
    cost_att += err.squaredNorm();
    att_err.col(i) = err;
    att_vec.col(i) = att_temp;
  }
  
  // 引用轨迹代价
  Eigen::MatrixXd ref_diff = points - ref;
  double ref_cost = 0.0;
  
  for (int i = 0; i < 3; i++)
  {
    for (int j = 0; j < N-1; j++)
    {
      double diff = ref_diff(i,j) / Thre(i);
      ref_cost += std::min(1.0, fabs(diff));  
    }
  }
  ref_cost = ref_cost / (3.0*(N-1)) * 100.0;
  
  // 总代价
  double final_cost = 0.0;
  final_cost = cost_smooth * 1.0 +
               cost_att    * 0.1 +
               ref_cost    * 1.0;
               
  // 梯度
  Eigen::VectorXd grad_tmp;
  if (grad_flag == 1)
  {
    Eigen::MatrixXd dxi = gdX.head(3 * (N - 1)); // 3 * (N - 1) * 1
    Eigen::MatrixXd dtau = gdT.head(N); // N * 1
    
    Eigen::MatrixXd grad_smooth_xi   =  dxi  * 1.0; // ref_smooth * A   
    Eigen::MatrixXd grad_smooth_tau  =  dtau * 1.0; // ref_smooth * A   
    Eigen::MatrixXd grad_ref_xi  =  -ref_diff / (3.0 *(N-1)) * 100.0;
    Eigen::MatrixXd grad_att_xi = Eigen::MatrixXd::Zero(3*(N-1),1);
    
    for (int i = 0; i < 3; i++)
    {
      grad_ref_xi.row(i) = grad_ref_xi.row(i).cwiseProduct(Thre.transpose().row(i));
    }

    for (int i = 0; i < N-1; i++)
    {
      Eigen::Vector3d x_next = points.col(i);
      Eigen::Vector3d a_vec = acc.col(i);
      if (a_vec.norm() < 1e-4)
      {
        a_vec = Eigen::Vector3d(0,0,1);
      }
      a_vec = a_vec / a_vec.norm();

      Eigen::Matrix3d R = ptr_traj->att_constraints[i];
      Eigen::Vector3d jac_vec = hat(R.col(1)) * a_vec;
      grad_att_xi.block(3*i,0,3,1) = (2 * R.col(1).dot(a_vec) * hat(a_vec) - hat(R.col(1))) * jac_vec;
    }

    grad_tmp = grad_smooth_xi * 1.0 + 
               grad_att_xi    * 0.01 +
               grad_ref_xi    * 1.0;
               
    Eigen::Map<Eigen::VectorXd> grad_xi(grad, 3 * (N - 1));
    grad_xi = grad_tmp;

    // gradient_tau
    grad_tmp = grad_smooth_tau * 1.0;
    Eigen::Map<Eigen::VectorXd> grad_tau(grad + 3 * (N - 1), N);
    grad_tau = grad_tmp;
  }
  
  iter++;
  return final_cost;
}

这个代价函数包含了几个部分:

1. 光滑代价(cost_smooth):表示轨迹的加速度变化率,通过minco库的calRibbonCost_fixdt函数计算。
2. 姿态代价(cost_att):表示轨迹的姿态与参考姿态之间的差异,通过计算姿态向量与加速度向量之间的误差得到。
3. 参考轨迹代价(ref_cost):表示轨迹与参考轨迹之间的差异,通过计算控制点与参考点之间的距离得到。
4. 总代价(final_cost):将上述三个代价加权求和得到。

代价函数还会计算各个代价对优化变量的梯度,存储在grad中。梯度的计算主要有两个部分:

1. 对控制点xi的梯度:包括光滑代价的梯度、姿态代价的梯度和参考轨迹代价的梯度。
2. 对时间变量tau的梯度:主要是光滑代价的梯度。

其中几个关键的数学计算:

1. 李括号hat(v):将向量v转化为反对称矩阵,即hat(v)=[0,-v3,v2;v3,0,-v1;-v2,v1,0]。
2. 姿态向量的雅可比矩阵:设R为旋转矩阵,a为加速度向量,则有hat(R*a)a=[hat®,hat(a)][vec®;a]。这里用到了克罗内克积的性质。

优化得到的轨迹会传递给swept volume manager进行碰撞检测,相关的代码在sw_manager.cpp中:

bool SweptVolumeManager::isTrajCollide() 
{
  for(int i = 0; i < parallel_points.size() ; i++)
  {    
    Eigen::RowVector3d grad;
    double f_tStar_with_time=getSDFofSweptVolume<true>(parallel_points[i], tstar_with_time, grad, false);

    if (f_tStar_with_time < 0)
    {
      return true;
    }
  }
  return false;
}

这个函数遍历所有的障碍物点,对每个点计算其到swept volume的距离。如果距离小于0,说明存在碰撞,函数返回true。

距离的计算通过getSDFofSweptVolume函数实现:

template <bool get_time>
double SweptVolumeManager::getSDFofSweptVolume(const Eigen::RowVector3d &point, double &t, Eigen::RowVector3d &g, bool forward)
{
  double pos_occup=1.0;
  double pos_occup_cur=1.0;
  
  double min_dist = 10000.0;
  double min_t, cur_t;
  
  cur_t = 0.0;
  while(cur_t < traj_T_max + 1e-3) // 遍历轨迹的每个时刻
  {
    pos_occup_cur = getSDFofRobot(point, cur_t, g, forward); // 计算point到当前时刻机器人的距离
    if (get_time && min_dist > pos_occup_cur && pos_occup_cur > SweptVolumeManager::resolution) 
    // 更新最小距离和对应时刻
    {
      min_dist = pos_occup_cur;
      min_t = cur_t;
    }
    if(pos_occup_cur < pos_occup)
      pos_occup = pos_occup_cur;
      
    cur_t += resolution * 2.5;
  }
  
  if (get_time)
  {
    t = min_t;
  }
  
  return pos_occup;
}

这个函数通过遍历轨迹的每个时刻,计算point到当前时刻机器人的距离,并更新最小距离和对应时刻。其中单个时刻的距离计算通过getSDFofRobot函数实现:

double SweptVolumeManager::getSDFofRobot(const Eigen::RowVector3d& point, double t, Eigen::RowVector3d& grad, bool forward)
{
  Eigen::RowVector3d pos_r = traj->getPos(t);
  double yaw = traj->getYaw(t);
  double perimeter = yaw / current_robot_shape->res_z + current_robot_shape->zero_pose_i;
  int _i = floor(perimeter);
  
  double weight = perimeter - _i;
  int _j = floor((point(2) - pos_r(2)) / current_robot_shape->res_z) + current_robot_shape->zero_pose_j;
  int _k = floor((point(0) - pos_r(0)) / grid_resolution) + current_robot_shape->kernel_size / 2;
  int _l = floor((point(1) - pos_r(1)) / grid_resolution) + current_robot_shape->kernel_size / 2;
  
  _i = std::max(std::min(_i, current_robot_shape->size_i - 1), 0);
  _j = std::max(std::min(_j, current_robot_shape->size_j - 1), 0);
  _k = std::max(std::min(_k, current_robot_shape->kernel_size - 1), 0);
  _l = std::max(std::min(_l, current_robot_shape->kernel_size - 1), 0);
  
  double d1 = current_robot_shape->byte_shape_kernels[_i * current_robot_shape->size_j + _j].getSDF(_k,_l);
  double d2 = current_robot_shape->byte_shape_kernels[(_i+1) * current_robot_shape->size_j + _j].getSDF(_k,_l);
  double dists = d1 * (1 - weight) + d2 * weight;
  
  if(dists < current_robot_shape->kernel_backup)
    return dists;
    
  return current_robot_shape->kernel_backup;
}

这个函数首先根据时刻t计算机器人的位置pos_r和偏航角yaw,然后根据point和pos_r的相对位置,在机器人形状的字节地图(byte_shape_kernels)中查询距离值。这里使用了双线性插值来处理非整数索引的情况。

根据代码,这套系统主要调用了两个优化器:LMBM(用于后端优化)和LBFGS(用于中端优化)。让我们分别看一下。

1. 后端优化(back_end_optimizer.cpp)

在BackEndOptimizer::optimize_traj_lmbm函数中,调用了LMBM优化器:

int ret = lmbm::lmbm_optimize(total_opt_variable_num,
                              x_variable,
                              &final_cost,
                              costFunctionLmbm,
                              this,
                              earlyexitLmbm,
                              &param);

这里的lmbm::lmbm_optimize就是LMBM优化器的接口。它需要提供优化变量的个数(total_opt_variable_num)、优化变量的初始值(x_variable)、代价函数(costFunctionLmbm)、提前终止条件(earlyexitLmbm)等参数。

LMBM优化器在每次迭代时,会调用costFunctionLmbm计算代价函数的值和梯度,直到满足earlyexitLmbm定义的终止条件。

2. 中端优化(mid_end.cpp)

在OriTraj::getOriTraj函数中,调用了LBFGS优化器:

int ret = lbfgs::lbfgs_optimize(x,
                                final_cost,
                                &costFunction,
                                nullptr,
                                &earlyExit,
                                this,
                                lbfgs_params);

这里的lbfgs::lbfgs_optimize就是LBFGS优化器的接口。它需要提供优化变量的初始值(x)、代价函数(costFunction)、提前终止条件(earlyExit)、优化参数(lbfgs_params)等。

与LMBM类似,LBFGS优化器在每次迭代时,会调用costFunction计算代价函数的值和梯度,直到满足earlyExit定义的终止条件。

这两个优化器分别来自第三方库lmbm和lbfgs。它们的具体实现没有包含在提供的代码中,但我们可以从优化器的接口和调用方式中了解其工作原理。

LMBM和LBFGS都属于准牛顿法,通过近似Hessian矩阵来加速收敛。不同的是,LMBM还引入了信赖域的概念,在每次迭代时限制变量的变化量,以保证收敛性。

在这套系统中,LMBM用于后端的轨迹优化,目标是生成一条避障、光滑、动力学可行的轨迹。而LBFGS用于中端的初始轨迹生成,目标是在起点和终点之间快速生成一条可行解,为后端优化提供良好的初值。

这种分层优化的架构,既保证了求解的质量,又兼顾了计算效率。不同层级的优化器可以根据具体任务的特点和要求,灵活地进行选择和配置。

至此,我们详细分析了轨迹优化的整个流程,包括初始轨迹的生成、基于梯度的数值优化求解以及最终轨迹的碰撞检测。下面,让我们进入最后一个环节:轨迹的跟踪控制。

第四部分:轨迹跟踪控制

轨迹跟踪控制的主要目的是让无人机准确地执行规划出的轨迹。这个过程涉及到以下几个关键模块:traj_server、so3_control和quadrotor_simulator。

traj_server负责将优化后的轨迹发布出去:

void polyTrajCallback(traj_utils::PolyTrajPtr msg)
{
  if (msg->order != 5) // 检查轨迹阶数
  {ROS_ERROR("[traj_server] Only support trajectory order equals 5 now!");
    return;
  }
  
  // 将轨迹消息转化为minco格式
  int piece_nums = msg->duration.size();
  std::vector<double> dura(piece_nums);
  std::vector<Trajectory<5>::CoefficientMat> cMats(piece_nums);
  
  for (int i = 0; i < piece_nums; ++i)
  {
    int i6 = i * 6;
    cMats[i].row(0) << msg->coef_x[i6 + 0], msg->coef_x[i6 + 1], msg->coef_x[i6 + 2],
        msg->coef_x[i6 + 3], msg->coef_x[i6 + 4], msg->coef_x[i6 + 5];
    cMats[i].row(1) << msg->coef_y[i6 + 0], msg->coef_y[i6 + 1], msg->coef_y[i6 + 2],
        msg->coef_y[i6 + 3], msg->coef_y[i6 + 4], msg->coef_y[i6 + 5];
    cMats[i].row(2) << msg->coef_z[i6 + 0], msg->coef_z[i6 + 1], msg->coef_z[i6 + 2],
        msg->coef_z[i6 + 3], msg->coef_z[i6 + 4], msg->coef_z[i6 + 5];

    dura[i] = msg->duration[i];
  }

  jerk_traj.reset(new Trajectory<5>(dura, cMats));
  
  // 记录一些信息
  start_time_ = msg->start_time;
  traj_duration_ = jerk_traj->getTotalDuration();
  traj_id_ = msg->traj_id;

  receive_traj_ = true;
}

这个回调函数首先检查轨迹的阶数是否为5,然后将轨迹的系数和时间信息转化为minco库的格式。最后,它更新了一些状态变量,表示收到了新的轨迹。

轨迹的发布通过定时器回调函数实现:

void cmdCallback(const ros::TimerEvent &e)
{
  // 检查系统状态
  if (!receive_traj_)
    return;
    
  ros::Time time_now = ros::Time::now();
  double t_cur = (time_now - start_time_).toSec();

  // 如果当前时刻在轨迹时间范围内  
  if (t_cur < traj_duration_ && t_cur >= 0.0)
  {
    // 计算当前时刻的控制指令
    Eigen::Vector3d pos = jerk_traj->getPos(t_cur);
    Eigen::Vector3d vel = jerk_traj->getVel(t_cur);
    Eigen::Vector3d acc = jerk_traj->getAcc(t_cur);
    Eigen::Vector3d jer = jerk_traj->getJer(t_cur);
    
    // 计算偏航角和偏航角速度
    std::pair<double, double> yaw_yawdot = calculate_yaw(t_cur, pos, (time_now - time_last).toSec());

    // 发布控制指令
    publish_cmd(pos, vel, acc, jer, yaw_yawdot.first, yaw_yawdot.second);
  }
}

这个函数首先检查系统状态,确保已经收到轨迹。然后,它计算当前时刻t_cur,如果t_cur在轨迹的时间范围内,就通过minco轨迹对象的getPos、getVel等函数,计算当前时刻的位置、速度、加速度和加加速度。接着,它调用calculate_yaw函数计算偏航角和偏航角速度。最后,通过publish_cmd函数将控制指令发布出去。

calculate_yaw函数的实现如下:

std::pair<double, double> calculate_yaw(double t_cur, Eigen::Vector3d &pos, double dt)
{
  // 根据当前位置和下一时刻位置计算偏航角
  Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_
                            ? jerk_traj->getPos(t_cur + time_forward_) - pos
                            : jerk_traj->getPos(traj_duration_) - pos;
  double yaw_temp = dir.norm() > 0.1
                        ? atan2(dir(1), dir(0))
                        : last_yaw_;

  // 计算偏航角速度                        
  double yawdot = 0;
  double d_yaw = yaw_temp - last_yaw_;
  if (d_yaw >= M_PI)
    d_yaw -= 2 * M_PI;
  if (d_yaw <= -M_PI)
    d_yaw += 2 * M_PI;
  yawdot = d_yaw / dt;
  
  // 更新last_yaw_
  last_yaw_ = yaw_temp;
  
  return std::make_pair(yaw_temp, yawdot);
}

这个函数首先根据当前位置和下一时刻位置计算偏航角,然后根据偏航角的变化量计算偏航角速度。需要注意的是,由于偏航角的周期性,在计算变化量时需要进行特殊处理。

publish_cmd函数负责将控制指令封装成ROS消息并发布出去:

void publish_cmd(Vector3d p, Vector3d v, Vector3d a, Vector3d j, double y, double yd)
{
  cmd.header.stamp = ros::Time::now();
  cmd.header.frame_id = "map";
  cmd.trajectory_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY;
  cmd.trajectory_id = traj_id_;

  cmd.position.x = p(0);
  cmd.position.y = p(1);
  cmd.position.z = p(2);
  cmd.velocity.x = v(0);
  cmd.velocity.y = v(1);
  cmd.velocity.z = v(2);
  cmd.acceleration.x = a(0);
  cmd.acceleration.y = a(1);
  cmd.acceleration.z = a(2);
  cmd.jerk.x = j(0);
  cmd.jerk.y = j(1);
  cmd.jerk.z = j(2);
  cmd.yaw = y;
  cmd.yaw_dot = yd;
  
  pos_cmd_pub.publish(cmd);
}

这里使用了ROS的PositionCommand消息类型,将位置、速度、加速度、加加速度、偏航角和偏航角速度打包发布。

接下来,让我们看一下位置控制器so3_control是如何处理这些控制指令的:

void SO3ControlNodelet::position_cmd_callback(
  const quadrotor_msgs::PositionCommand::ConstPtr& cmd)
{
  // 提取控制指令
  des_pos_ = Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z);
  des_vel_ = Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
  des_acc_ = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y,
                             cmd->acceleration.z);
  des_yaw_ = cmd->yaw;
  des_yaw_dot_ = cmd->yaw_dot;
  
  // 调用SO3Control的计算函数
  publishSO3Command();
}

这个回调函数首先提取控制指令中的目标位置、速度、加速度、偏航角和偏航角速度,然后调用publishSO3Command函数进行控制计算:

void SO3ControlNodelet::publishSO3Command(void)
{
  // 计算控制力和姿态
  controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_yaw_,
                               des_yaw_dot_, kx_, kv_);

  const Eigen::Vector3d&    force       = controller_.getComputedForce();
  const Eigen::Quaterniond& orientation = controller_.getComputedOrientation();

  // 发布SO3Command消息  
  quadrotor_msgs::SO3Command::Ptr so3_command(
    new quadrotor_msgs::SO3Command);
  so3_command->header.stamp    = ros::Time::now();
  so3_command->header.frame_id = frame_id_;
  so3_command->force.x         = force(0);
  so3_command->force.y         = force(1);
  so3_command->force.z         = force(2);
  so3_command->orientation.x   = orientation.x();
  so3_command->orientation.y   = orientation.y();
  so3_command->orientation.z   = orientation.z();
  so3_command->orientation.w   = orientation.w();
  // ...
   
  so3_command_pub_.publish(so3_command);
}

这个函数主要做了两件事:

1. 调用SO3Control的calculateControl函数,根据当前状态和目标状态计算控制力和姿态。
2. 将控制力和姿态封装成SO3Command消息,发布给下一层的控制器(如PX4)。

让我们进一步看一下calculateControl函数的实现:

void SO3Control::calculateControl(const Eigen::Vector3d& des_pos,
                                  const Eigen::Vector3d& des_vel,
                                  const Eigen::Vector3d& des_acc,
                                  const double des_yaw, const double des_yaw_dot,
                                  const Eigen::Vector3d& kx,
                                  const Eigen::Vector3d& kv)
{
  // 位置误差
  Eigen::Vector3d e_pos = des_pos - pos_;
  
  // 速度误差  
  Eigen::Vector3d e_vel = des_vel - vel_;
  
  // 加速度误差
  Eigen::Vector3d e_acc = des_acc - acc_;
  
  // 计算控制力
  force_ = mass_ * g_ * Eigen::Vector3d(0, 0, 1);
  force_ += kx.asDiagonal() * e_pos;
  force_ += kv.asDiagonal() * e_vel;      
  force_ += mass_ * des_acc;
  
  // 限制控制力的倾斜角
  double          theta = M_PI / 2;
  double          c     = cos(theta);
  Eigen::Vector3d f;
  f = force_ - mass_ * g_ * Eigen::Vector3d(0, 0, 1);
  if (force_.dot(Eigen::Vector3d(0, 0, 1)) < force_.norm() * c)
  {
    double A         = c * c * f.squaredNorm() - f(2) * f(2);
    double B         = 2 * (c * c - 1) * f(2) * mass_ * g_;
    double C         = (c * c - 1) * mass_ * mass_ * g_ * g_;
    double s         = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
    force_ = s * f + mass_ * g_ * Eigen::Vector3d(0, 0, 1);
  }
  
  // 计算期望姿态
  Eigen::Vector3d b1c, b2c, b3c;
  Eigen::Vector3d b1d(cos(des_yaw), sin(des_yaw), 0);

  b3c = force_.normalized();
  b2c = b3c.cross(b1d).normalized();
  b1c = b2c.cross(b3c).normalized();

  Eigen::Matrix3d R;
  R << b1c, b2c, b3c;

  orientation_ = Eigen::Quaterniond(R);
}

这个函数的主要步骤如下:

1. 计算位置误差、速度误差和加速度误差。
2. 根据PID控制律,计算控制力force_。其中kx和kv是位置环和速度环的增益。
3. 限制控制力的倾斜角,确保无人机不会过度倾斜。这里使用了一个二次规划问题求解修正后的force_。
4. 根据期望偏航角des_yaw和force_,计算期望姿态orientation_。具体来说,先根据偏航角计算期望的x轴方向b1d,再根据force_计算期望的z轴方向b3c,最后利用叉乘计算出y轴方向b2c。由b1c,b2c,b3c构成的旋转矩阵即为期望姿态。

可以看到,SO3Control的核心是一个位置-速度-加速度的PID控制器,再加上一个偏航角控制器。它的输出是期望力和期望姿态,这就需要下层控制器(如PX4)将其转化为具体的电机控制信号。

最后,让我们简单看一下quadrotor_simulator是如何模拟无人机运动的:

void Quadrotor::step(double dt)
{
  // 获取当前状态
  State cur_state = getState();
  
  // 计算电机转速
  Control control = getControl(*this, command);
  
  // 设置外部扰动
  setExternalForce(disturbance.f);
  setExternalMoment(disturbance.m);
  
  // 积分计算下一时刻状态
  ode_step(cur_state, control.rpm, dt);
}

这个函数主要做了以下事情:

1. 获取当前状态cur_state。
2. 根据当前状态和控制指令command,计算电机转速control。
3. 设置外部扰动force和moment。
4. 调用ode_step函数,根据电机转速和当前状态,积分计算下一时刻的状态。

其中ode_step函数封装了四阶龙格库塔积分器:

void Quadrotor::ode_step(State& s, const Vector4d& rpm, double dt)
{
  // 将状态向量压平
  VectorXd x0 = state_to_vec(s);
  
  // 定义积分函数
  auto func = [this, &rpm](const VectorXd& x, VectorXd& dxdt, const double t) {
    State s = vec_to_state(x);
    vec_to_state_dot(dxdt, x, rpm);
  };
  
  // 执行四阶龙格库塔积分
  VectorXd x1 = rk4(func, x0, 0, dt);

  // 将向量解压为状态  
  s = vec_to_state(x1);
  
  // 限制状态变量
  limit_state(s);
}

这里使用了C++11的lambda函数定义了系统的微分方程,然后调用rk4函数执行四阶龙格库塔积分。积分结果会更新状态s。

其中vec_to_state和state_to_vec函数负责状态向量和State结构体之间的转换:

VectorXd Quadrotor::state_to_vec(const State& s) const
{
  VectorXd x(13);
  x.segment(0, 3)  = s.p;
  x.segment(3, 3)  = s.v;
  x.segment(6, 4)  = s.q.coeffs();
  x.segment(10, 3) = s.w;
  return x; 
}

Quadrotor::State Quadrotor::vec_to_state(const VectorXd& x) const
{  
  State s;
  s.p = x.segment(0, 3);
  s.v = x.segment(3, 3);
  s.q = Quaterniond(x.segment(6, 4));
  s.w = x.segment(10, 3);
  
  // 四元数单位化
  s.q.normalize();
  
  return s;
}

vec_to_state_dot函数定义了系统的微分方程,即四旋翼动力学方程:

void Quadrotor::vec_to_state_dot(VectorXd& dxdt, const VectorXd& x, const Vector4d& rpm) const
{
  State s = vec_to_state(x);
  
  // 四旋翼动力学参数
  double m = mass_;
  Matrix3d J = J_.asDiagonal(); 
  double g = g_;
  double kr = kr_;
  double kt = kt_;
  double l = l_;
  
  // 计算合力和合力矩
  Vector3d F(0, 0, kt_ * rpm.sum());
  Vector3d M(
    l * kt * (rpm(1) - rpm(3)),
    l * kt * (rpm(2) - rpm(0)), 
    kr * (rpm(0) - rpm(1) + rpm(2) - rpm(3))
  );
  
  // 欧拉运动方程
  Vector3d p_dot = s.v;
  Vector3d v_dot = F / m - Vector3d(0, 0, g);
  Quaterniond q_dot = s.q * Quaterniond(0, s.w(0), s.w(1), s.w(2)) * 0.5;
  Vector3d w_dot = J.inverse() * (M - s.w.cross(J * s.w));
  
  // 将结果压平为向量
  dxdt.segment(0, 3)  = p_dot;
  dxdt.segment(3, 3)  = v_dot; 
  dxdt.segment(6, 4)  = q_dot.coeffs();
  dxdt.segment(10, 3) = w_dot;
}

这里的核心是欧拉运动方程,描述了四旋翼在合力F和合力矩M作用下的运动。其中rpm是四个电机的转速,kt和kr是拉力系数和扭矩系数,l是机臂长度。

至此,我们详细分析了从轨迹生成到无人机运动控制的整个流程。总结一下,主要分为以下几个步骤:

1. 在点云地图中搜索一条无碰撞的路径(A*)。
2. 根据路径生成一条光滑、安全、动态可行的轨迹(TrajectoryOptimizer)。
3. 将轨迹发布给位置控制器(traj_server)。
4. 位置控制器根据轨迹和当前状态计算控制指令,发布给底层控制器(so3_control)。
5. 底层控制器根据控制指令计算电机控制信号,驱动无人机运动(未给出)。
6. 无人机的运动可以用四旋翼动力学模型描述(quadrotor_simulator)。

可以看到,这是一个完整的自主飞行系统,涉及到路径搜索、轨迹优化、控制分配、状态估计等多个模块。每个模块都有其对应的算法和数学原理,共同保证了无人机的安全和高效飞行。

当然,受限于篇幅,这里只能对一些关键算法进行介绍和分析。