使用ego-planner进行无人机控制时的数据流（自用）

啵啵啵啵哲

已于 2024-04-13 13:33:44 修改

阅读量761

点赞数 15

文章标签：无人机

于 2024-04-13 13:26:38 首次发布

本文链接：https://blog.csdn.net/xuzhengzhe/article/details/137712895

版权

本文将详细介绍如何使用ego-planner向无人机发送控制指令，实现自动控制的过程。我们将探讨ego-planner的运行方式以及控制指令的传输过程，涉及到ROS节点、MAVROS协议以及无人机的控制器等方面。

在实现自动控制时，控制指令的传输是至关重要的一环。本文将详细介绍ego-planner如何向无人机发送控制指令，从而实现自动控制的过程。

无人机控制流程：

运行run_ctrl.launch实现自动起飞和悬停。
可通过遥控器或机载电脑进行手动或自动控制。

本文主要讲解自动控制中的控制指令传输过程。
控制指令传输过程：
在ego-planner中，控制指令的传输是通过px4ctrl_node.cpp实现的，该节点利用MAVROS将控制指令发送给无人机。（realflight_modules/px4ctrl/src/px4ctrl_node.cpp）

ros::Subscriber cmd_sub = nh.subscribe<quadrotor_msgs::PositionCommand>(
    "cmd",
    100,
    boost::bind(&Command_Data_t::feed, &fsm.cmd_data, _1),
    ros::VoidConstPtr(),
    ros::TransportHints().tcpNoDelay());

fsm.ctrl_FCU_pub = nh.advertise<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 10);

在控制有限状态机中，当状态为CMD_CTRL时，通过get_cmd_des()函数获取控制指令的期望状态。

case CMD_CTRL:
{
    if (!rc_data.is_hover_mode || !odom_is_received(now_time))
    {
        state = MANUAL_CTRL;
        toggle_offboard_mode(false);

        ROS_WARN("[px4ctrl] From CMD_CTRL(L3) to MANUAL_CTRL(L1)!");
    }
    else if (!rc_data.is_command_mode || !cmd_is_received(now_time))
    {
        state = AUTO_HOVER;
        set_hov_with_odom();
        des = get_hover_des();
        ROS_INFO("[px4ctrl] From CMD_CTRL(L3) to AUTO_HOVER(L2)!");
    }
    else
    {
        des = get_cmd_des();
    }
    ...
    break;
}

控制指令获取过程：
在get_cmd_des()函数中，获取控制指令的期望状态des，包括位置、速度、加速度、加加速度、偏航角、偏航角速度。

Desired_State_t PX4CtrlFSM::get_cmd_des()
{
    Desired_State_t des;
    des.p = cmd_data.p;
    des.v = cmd_data.v;
    des.a = cmd_data.a;
    des.j = cmd_data.j;
    des.yaw = cmd_data.yaw;
    des.yaw_rate = cmd_data.yaw_rate;

    return des;
}

设定期望值des后，控制器负责根据获取的控制指令更新无人机的状态，其中包括推力模型的估计、控制指令的计算以及控制命令的发布。

// STEP2: estimate thrust model
	if (state == AUTO_HOVER || state == CMD_CTRL)
	{
		// controller.estimateThrustModel(imu_data.a, bat_data.volt, param);
		controller.estimateThrustModel(imu_data.a,param);

	}

	// STEP3: solve and update new control commands
	if (rotor_low_speed_during_land) // used at the start of auto takeoff
	{
		motors_idling(imu_data, u);
	}
	else
	{
		debug_msg = controller.calculateControl(des, odom_data, imu_data, u);
		debug_msg.header.stamp = now_time;
		debug_pub.publish(debug_msg);
	}

	// STEP4: publish control commands to mavros
	if (param.use_bodyrate_ctrl)
	{
		publish_bodyrate_ctrl(u, now_time);
	}
	else
	{
		publish_attitude_ctrl(u, now_time);
	}

cmd_data 通过px4ctrl中subscriber接收得到（见上面subscriber中定义）。

可以看到STEP4有两种情况：bodyrate_ctrl和attitude_ctrl。看一下函数定义：

void PX4CtrlFSM::publish_bodyrate_ctrl(const Controller_Output_t &u, const ros::Time &stamp)
{
	mavros_msgs::AttitudeTarget msg;

	msg.header.stamp = stamp;
	msg.header.frame_id = std::string("FCU");

	msg.type_mask = mavros_msgs::AttitudeTarget::IGNORE_ATTITUDE;

	msg.body_rate.x = u.bodyrates.x();
	msg.body_rate.y = u.bodyrates.y();
	msg.body_rate.z = u.bodyrates.z();

	msg.thrust = u.thrust;

	ctrl_FCU_pub.publish(msg);
}

void PX4CtrlFSM::publish_attitude_ctrl(const Controller_Output_t &u, const ros::Time &stamp)
{
	mavros_msgs::AttitudeTarget msg;

	msg.header.stamp = stamp;
	msg.header.frame_id = std::string("FCU");

	msg.type_mask = mavros_msgs::AttitudeTarget::IGNORE_ROLL_RATE |
					mavros_msgs::AttitudeTarget::IGNORE_PITCH_RATE |
					mavros_msgs::AttitudeTarget::IGNORE_YAW_RATE;

	msg.orientation.x = u.q.x();
	msg.orientation.y = u.q.y();
	msg.orientation.z = u.q.z();
	msg.orientation.w = u.q.w();

	msg.thrust = u.thrust;

	ctrl_FCU_pub.publish(msg);
}

这两个函数都是用来发布飞行器的控制指令的，但是它们控制的方式略有不同。
publish_bodyrate_ctrl 函数发布的是基于体轴角速度（Body Rate）的控制指令。这种控制方式直接指定飞行器绕体轴的角速度，也就是说，控制器会直接告诉飞行器应该以多快的速度绕着自身的 X、Y、Z 轴旋转。

publish_attitude_ctrl 函数发布的是基于姿态角（Attitude）的控制指令。这种控制方式指定的是飞行器的期望姿态，也就是说，控制器会告诉飞行器应该朝向哪个方向。该函数忽略了角速度，只指定了期望的姿态。飞行器会根据姿态控制算法自行调整引擎输出以达到期望的姿态。这种控制方式适用于需要控制飞行器朝向的情况，比如指定飞行器俯仰、横滚和偏航的角度。

查找参数设置，可以在realflight_modules/px4ctrl/config/strl_param_fpv.yaml中看到

use_bodyrate_ctrl: false

这说明在该控制器中实际采用的是姿态控制。

在realflight_modules/px4ctrl/launch/run_ctrl.launch中，进行了话题重映射

remap from="~cmd" to="/position_cmd" />

这说明控制器实际接收话题名称为/position_cmd 接下来我们寻找ego-planner中发布该指令的位置。

在/src/planner/plan_manage/src/traj_server.cpp中可以看到控制指令（位置控制）发布者的定义

pos_cmd_pub = nh.advertise<quadrotor_msgs::PositionCommand>("/position_cmd", 50);

顺藤摸瓜找到其发布消息的函数

void cmdCallback(const ros::TimerEvent &e)
{
  /* no publishing before receive traj_ */
  if (!receive_traj_)
    return;

  ros::Time time_now = ros::Time::now();
  double t_cur = (time_now - start_time_).toSec();

  Eigen::Vector3d pos(Eigen::Vector3d::Zero()), vel(Eigen::Vector3d::Zero()), acc(Eigen::Vector3d::Zero()), pos_f;
  std::pair<double, double> yaw_yawdot(0, 0);

  static ros::Time time_last = ros::Time::now();
  if (t_cur < traj_duration_ && t_cur >= 0.0)
  {
    pos = traj_[0].evaluateDeBoorT(t_cur);
    vel = traj_[1].evaluateDeBoorT(t_cur);
    acc = traj_[2].evaluateDeBoorT(t_cur);

    /*** calculate yaw ***/
    yaw_yawdot = calculate_yaw(t_cur, pos, time_now, time_last);
    /*** calculate yaw ***/

    double tf = min(traj_duration_, t_cur + 2.0);
    pos_f = traj_[0].evaluateDeBoorT(tf);
  }
  else if (t_cur >= traj_duration_)
  {
    /* hover when finish traj_ */
    pos = traj_[0].evaluateDeBoorT(traj_duration_);
    vel.setZero();
    acc.setZero();

    yaw_yawdot.first = last_yaw_;
    yaw_yawdot.second = 0;

    pos_f = pos;
    return;
  }
  else
  {
    cout << "[Traj server]: invalid time." << endl;
  }
  time_last = time_now;

  cmd.header.stamp = time_now;
  cmd.header.frame_id = "world";
  cmd.trajectory_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY;
  cmd.trajectory_id = traj_id_;

  cmd.position.x = pos(0);
  cmd.position.y = pos(1);
  cmd.position.z = pos(2);

  cmd.velocity.x = vel(0);
  cmd.velocity.y = vel(1);
  cmd.velocity.z = vel(2);

  cmd.acceleration.x = acc(0);
  cmd.acceleration.y = acc(1);
  cmd.acceleration.z = acc(2);

  cmd.yaw = yaw_yawdot.first;
  cmd.yaw_dot = yaw_yawdot.second;

  last_yaw_ = cmd.yaw;

  pos_cmd_pub.publish(cmd);
}

该函数为控制频率100Hz执行（traj_server.cpp, line 243）

ros::Timer cmd_timer = nh.createTimer(ros::Duration(0.01), cmdCallback);

查看该函数内容，可以发现cmd所需的位置、速度、加速度均通过轨迹直接计算，计算函数为evaluateDeBoorT。偏航角yaw、其变化率yawdot通过另一个函数calculate_yaw计算。

calculate_yaw函数定义（traj_server.cpp, line 71）为：

std::pair<double, double> calculate_yaw(double t_cur, Eigen::Vector3d &pos, ros::Time &time_now, ros::Time &time_last)
{
  constexpr double PI = 3.1415926;
  constexpr double YAW_DOT_MAX_PER_SEC = PI;
  // constexpr double YAW_DOT_DOT_MAX_PER_SEC = PI;
  std::pair<double, double> yaw_yawdot(0, 0);
  double yaw = 0;
  double yawdot = 0;

  Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? traj_[0].evaluateDeBoorT(t_cur + time_forward_) - pos : traj_[0].evaluateDeBoorT(traj_duration_) - pos;
  double yaw_temp = dir.norm() > 0.1 ? atan2(dir(1), dir(0)) : last_yaw_;
  double max_yaw_change = YAW_DOT_MAX_PER_SEC * (time_now - time_last).toSec();
  if (yaw_temp - last_yaw_ > PI)
  {
    if (yaw_temp - last_yaw_ - 2 * PI < -max_yaw_change)
    {
      yaw = last_yaw_ - max_yaw_change;
      if (yaw < -PI)
        yaw += 2 * PI;

      yawdot = -YAW_DOT_MAX_PER_SEC;
    }
    else
    {
      yaw = yaw_temp;
      if (yaw - last_yaw_ > PI)
        yawdot = -YAW_DOT_MAX_PER_SEC;
      else
        yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
    }
  }
  else if (yaw_temp - last_yaw_ < -PI)
  {
    if (yaw_temp - last_yaw_ + 2 * PI > max_yaw_change)
    {
      yaw = last_yaw_ + max_yaw_change;
      if (yaw > PI)
        yaw -= 2 * PI;

      yawdot = YAW_DOT_MAX_PER_SEC;
    }
    else
    {
      yaw = yaw_temp;
      if (yaw - last_yaw_ < -PI)
        yawdot = YAW_DOT_MAX_PER_SEC;
      else
        yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
    }
  }
  else
  {
    if (yaw_temp - last_yaw_ < -max_yaw_change)
    {
      yaw = last_yaw_ - max_yaw_change;
      if (yaw < -PI)
        yaw += 2 * PI;

      yawdot = -YAW_DOT_MAX_PER_SEC;
    }
    else if (yaw_temp - last_yaw_ > max_yaw_change)
    {
      yaw = last_yaw_ + max_yaw_change;
      if (yaw > PI)
        yaw -= 2 * PI;

      yawdot = YAW_DOT_MAX_PER_SEC;
    }
    else
    {
      yaw = yaw_temp;
      if (yaw - last_yaw_ > PI)
        yawdot = -YAW_DOT_MAX_PER_SEC;
      else if (yaw - last_yaw_ < -PI)
        yawdot = YAW_DOT_MAX_PER_SEC;
      else
        yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
    }
  }

  if (fabs(yaw - last_yaw_) <= max_yaw_change)
    yaw = 0.5 * last_yaw_ + 0.5 * yaw; // nieve LPF
  yawdot = 0.5 * last_yaw_dot_ + 0.5 * yawdot;
  last_yaw_ = yaw;
  last_yaw_dot_ = yawdot;

  yaw_yawdot.first = yaw;
  yaw_yawdot.second = yawdot;

  return yaw_yawdot;
}

该段代码是一个用于计算航向角（yaw）及其变化率（yaw rate, 即yaw的一阶导数）的函数。函数接收当前时间t_cur、当前位置pos、当前时间time_now及上一次计算时间time_last作为输入，返回一对双精度浮点数，分别代表当前的航向角和航向角变化率。

函数逻辑解析如下：

1. 初始化常量与变量:

YAW_DOT_MAX_PER_SEC：航向角变化率的最大值，限制航向角的变化速度。
yaw_yawdot：存储计算结果的pair，初始为(0, 0)。
yaw和yawdot：分别用于存储计算出的航向角和航向角变化率。

2. 计算目标方向:

使用De Boor算法（用于计算B样条形式的样条曲线）计算给定时间点的目标位置，然后计算从当前位置pos到目标位置的方向向量dir。
$\text{DeBoor}(t\_cur +t\_forward )− pos , \text{ if }t\_cur +t\_forward ≤t\_duration \\ dir = \text{DeBoor}(t\_duration )− pos, \text{ otherwise } $

3. 计算临时航向角:

yaw_temp：基于dir向量，使用atan2函数计算得到的临时航向角，当dir的模长大于0.1时有效，否则使用上一次的航向角last_yaw_。

4. 计算航向角变化限制:

max_yaw_change：根据YAW_DOT_MAX_PER_SEC和时间差(time_now - time_last)计算的最大航向角变化量。
$Δyaw max =YAW\_DOT\_MAX\_PER\_SEC×Δt$

5. 调整航向角:

通过比较yaw_temp与last_yaw_的差值，考虑边界情况（如航向角跨越±π），并限制航向角的变化量不超过max_yaw_change，从而计算出新的yaw和yawdot。

6. 平滑处理:

对计算出的yaw和yawdot进行简单的低通滤波处理，以平滑航向角及其变化率。
$yaw=0.5×last\_yaw\_+0.5×yaw\\ yawdot=0.5×last\_yaw\_dot\_+0.5×yawdot\\ yawdot=0.5×last\_yaw\_dot\_+0.5×yawdot$

7. 更新并返回结果:

更新last_yaw_和last_yaw_dot_为当前计算结果。将计算结果存入yaw_yawdot并返回。

啵啵啵啵哲

关注

15
点赞
踩
22

收藏

觉得还不错? 一键收藏
0
评论
使用ego-planner进行无人机控制时的数据流（自用）

本文将详细介绍如何使用ego-planner向无人机发送控制指令，实现自动控制的过程。我们将探讨ego-planner的运行方式以及控制指令的传输过程，涉及到ROS节点、MAVROS协议以及无人机的控制器等方面。在实现自动控制时，控制指令的传输是至关重要的一环。本文将详细介绍ego-planner如何向无人机发送控制指令，从而实现自动控制的过程。本文主要讲解自动控制中的控制指令传输过程。控制指令传输过程：在ego-planner中，控制指令的传输是通过。
复制链接

扫一扫