网站的tdk指的是什么意思,企业所得税率2022最新,各种网站,网站锚点本文将详细介绍如何使用ego-planner向无人机发送控制指令#xff0c;实现自动控制的过程。我们将探讨ego-planner的运行方式以及控制指令的传输过程#xff0c;涉及到ROS节点、MAVROS协议以及无人机的控制器等方面。
在实现自动控制时#xff0c;控制指令的传输是至关重要的…本文将详细介绍如何使用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.subscribequadrotor_msgs::PositionCommand(cmd,100,boost::bind(Command_Data_t::feed, fsm.cmd_data, _1),ros::VoidConstPtr(),ros::TransportHints().tcpNoDelay());fsm.ctrl_FCU_pub nh.advertisemavros_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 modelif (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 commandsif (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 mavrosif (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.advertisequadrotor_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::pairdouble, 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::pairdouble, 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::pairdouble, 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;elseyawdot (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;elseyawdot (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;elseyawdot (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 LPFyawdot 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。 d i r DeBoor ( t _ c u r t _ f o r w a r d ) − p o s , if t _ c u r t _ f o r w a r d ≤ t _ d u r a t i o n d i r DeBoor ( t _ d u r a t i o n ) − p o s , otherwise dir \text{DeBoor}(t\_cur t\_forward )− pos , \text{ if }t\_cur t\_forward ≤t\_duration \\ dir \text{DeBoor}(t\_duration )− pos, \text{ otherwise } dirDeBoor(t_curt_forward)−pos, if t_curt_forward≤t_durationdirDeBoor(t_duration)−pos, otherwise
3. 计算临时航向角:
yaw_temp基于dir向量使用atan2函数计算得到的临时航向角当dir的模长大于0.1时有效否则使用上一次的航向角last_yaw_。
4. 计算航向角变化限制:
max_yaw_change根据YAW_DOT_MAX_PER_SEC和时间差(time_now - time_last)计算的最大航向角变化量。 Δ y a w m a x Y A W _ D O T _ M A X _ P E R _ S E C × Δ t Δyaw max YAW\_DOT\_MAX\_PER\_SEC×Δt ΔyawmaxYAW_DOT_MAX_PER_SEC×Δt
5. 调整航向角:
通过比较yaw_temp与last_yaw_的差值考虑边界情况如航向角跨越±π并限制航向角的变化量不超过max_yaw_change从而计算出新的yaw和yawdot。
6. 平滑处理:
对计算出的yaw和yawdot进行简单的低通滤波处理以平滑航向角及其变化率。 y a w 0.5 × l a s t _ y a w _ 0.5 × y a w y a w d o t 0.5 × l a s t _ y a w _ d o t _ 0.5 × y a w d o t y a w d o t 0.5 × l a s t _ y a w _ d o t _ 0.5 × y a w d o t yaw0.5×last\_yaw\_0.5×yaw\\ yawdot0.5×last\_yaw\_dot\_0.5×yawdot\\ yawdot0.5×last\_yaw\_dot\_0.5×yawdot yaw0.5×last_yaw_0.5×yawyawdot0.5×last_yaw_dot_0.5×yawdotyawdot0.5×last_yaw_dot_0.5×yawdot
7. 更新并返回结果:
更新last_yaw_和last_yaw_dot_为当前计算结果。将计算结果存入yaw_yawdot并返回。
