PX4多旋翼姿态控制程序分析mc_att_control
2024-04-21 15:54:26
PX4多旋翼姿态控制程序分析mc_att_control
1. 程序流程图
原图大小太大，无法上传（在我主页下载里面找）
2.PX4主程序梳理：建议对照上图
PX4程序的基本流程如下：
int MulticopterAttitudeControl::task_spawn(int argc, char *argv[])
{bool vtol = false;// 判断是否是VTOL飞行器if (argc > 1) {if (strcmp(argv[1], "vtol") == 0) {vtol = true;}// 否则创建的任务将是普通的多旋翼飞行器}MulticopterAttitudeControl *instance = new MulticopterAttitudeControl(vtol);// 实例初始化if (instance) {_object.store(instance);_task_id = task_id_is_work_queue;if (instance->init()) {return PX4_OK;}} else {PX4_ERR("alloc failed");}// 创建任务失败或初始化失败时，清除实例并释放任务所占用的内存delete instance;_object.store(nullptr);_task_id = -1;return PX4_ERROR;
}
MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) :ModuleParams(nullptr),WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),// 根据vtol来决定发布主题_vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),_vtol(vtol)
{// 多旋翼飞行器为VTOL类型if (_vtol) {int32_t vt_type = -1;// 查找参数名为VT_TYPE的参数，将参数值赋值给变量vt_typeif (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {// 将vt_type的值转换为vtol_type枚举类型，并与TAILSITTER进行比较// 如果相等，则说明多旋翼飞行器为垂尾式VTOL_vtol_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);}}// 参数更新parameters_updated();
}
bool MulticopterAttitudeControl::init()
{// 注册了一个 姿态信息订阅器的回调函数if (!_vehicle_attitude_sub.registerCallback()) {PX4_ERR("vehicle_attitude callback registration failed!");return false;}return true;
}
3.Run()函数解析
// 检查是否需要退出程序
if (should_exit()) {_vehicle_attitude_sub.unregisterCallback();// 取消姿态信息订阅器的回调函数exit_and_cleanup();// 执行清理函数return;}// 启动计时perf_begin(_loop_perf);// 检查参数是否发生变化if (_parameter_update_sub.updated()) {// clear updateparameter_update_s param_update;_parameter_update_sub.copy(¶m_update);// 如果有变化就更新参数updateParams();//通知其他的函数参数已经被更新。这个函数通常在初始化时调用一次，之后会在循环中不断检查参数是否有变化，并且及时更新参数parameters_updated();}
parameter_update_s结构体包含了各种参数更新相关的信息:
struct __EXPORT parameter_update_s {#else
struct parameter_update_s {#endifuint64_t timestamp; // 更新时的时间戳uint32_t instance; // 参数的实例编号uint32_t get_count; // 获取参数的次数uint32_t set_count; // 设置参数的次数uint32_t find_count; // 查找参数的次数uint32_t export_count; // 导出参数的次数uint16_t active; // 参数是否激活的标志uint16_t changed; // 参数是否发生变化的标志uint16_t custom_default;// 是否使用自定义的默认值的标志uint8_t _padding0[6]; // required for logger#ifdef __cplusplus#endif
};
// run controller on attitude updates
vehicle_attitude_s v_att;
// 存储飞行器姿态数据 结构体
struct vehicle_attitude_s {#endifuint64_t timestamp; // 时间戳uint64_t timestamp_sample; // 采样时间戳 float q[4]; // 飞行器当前的四元数（即姿态）float delta_q_reset[4]; // 飞行器重置后的姿态变化量（四元数表示）uint8_t quat_reset_counter; // 飞行器姿态重置次数uint8_t _padding0[7]; // required for logger
#ifdef __cplusplus
#endif
};
// 这是一个存储飞行器姿态期望数据的结构体
struct vehicle_attitude_setpoint_s {#endifuint64_t timestamp; // 姿态期望数据的时间戳float roll_body; // 期望的机身坐标系下的横滚角float pitch_body;float yaw_body;float yaw_sp_move_rate;float q_d[4]; // 期望的四元数姿态：期望的旋转方向和角度大小float thrust_body[3]; // 期望的机身坐标系下的推力向量bool roll_reset_integral; // 表示是否需要重置横滚角的积分项bool pitch_reset_integral;bool yaw_reset_integral;bool fw_control_yaw; // 表示是否使用飞翼模式下的偏航控制uint8_t apply_flaps; // 当前的襟翼状态，可能的取值为 FLAPS_OFF（襟翼关闭）、FLAPS_LAND（着陆襟翼）、FLAPS_TAKEOFF（起飞襟翼）uint8_t _padding0[7]; // required for logger
#ifdef __cplusplusstatic constexpr uint8_t FLAPS_OFF = 0;static constexpr uint8_t FLAPS_LAND = 1;static constexpr uint8_t FLAPS_TAKEOFF = 2;
#endif
};
// 获取最新的飞行器姿态数据
if (_vehicle_attitude_sub.update(&v_att)) {// 检查是否有新的姿态设定值更新if (_vehicle_attitude_setpoint_sub.updated()) {// 如果有新的设定值,则从_vehicle_attitude_setpoint_sub订阅的话题中获取最新的设定值vehicle_attitude_setpoint_s vehicle_attitude_setpoint;_vehicle_attitude_setpoint_sub.update(&vehicle_attitude_setpoint);// 姿态设定:四元数（期望的旋转方向和角度大小），期望的偏航速率_attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate);// 将设定值中的机体坐标系推力矢量（thrust_body）转换成向量（Vector3f）_thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body);}// 检查飞机姿态是否进行了重置if (_quat_reset_counter != v_att.quat_reset_counter) {const Quatf delta_q_reset(v_att.delta_q_reset);// 如果重置，就将重置带来的航向变化量添加到期望的航向角度_man_yaw_sp += Eulerf(delta_q_reset).psi();// 转换为欧拉角_attitude_control.adaptAttitudeSetpoint(delta_q_reset);_quat_reset_counter = v_att.quat_reset_counter;}// 计算时间步长dtconst float dt = math::constrain(((v_att.timestamp - _last_run) * 1e-6f), 0.0002f, 0.02f);_last_run = v_att.timestamp;/* check for updates in other topics */_manual_control_setpoint_sub.update(&_manual_control_setpoint); // 获取手动控制指令_v_control_mode_sub.update(&_v_control_mode);// 获取飞行模式// 检查否有新的更新if (_vehicle_land_detected_sub.updated()) {vehicle_land_detected_s vehicle_land_detected;// 是否成功从主题中复制了最新的消息if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {_landed = vehicle_land_detected.landed; //提取出是否着陆的信息}}
struct manual_control_setpoint_s {uint64_t timestamp;uint64_t timestamp_sample;float x; // 遥控 油门输入，用于控制飞机的前进速度float y; // 横滚输入，用于控制飞机的左右滚转角度float z; // 俯仰输入，用于控制飞机的上下俯仰角度float r; // 偏航输入，用于控制飞机绕垂直轴旋转的角度float flaps;// 襟翼输入，用于改变飞机的升力和阻力，控制飞机的下降速率和着陆方式等float aux1; // 额外辅助输入，用于控制飞机的其他功能，例如开启/关闭飞机的自动巡航功能等float aux2;float aux3;float aux4;float aux5;float aux6;uint8_t data_source;// 手动遥控信号的来源。可以是手动遥控器本身，也可以是来自其他设备的信号。uint8_t _padding0[3]; // required for logger
};
// 检测无人机状态是否有新的更新
if (_vehicle_status_sub.updated()) {vehicle_status_s vehicle_status;// // 是否成功从主题中复制了最新的消息if (_vehicle_status_sub.copy(&vehicle_status)) {// 记录当前无人机是否为旋翼机型_vehicle_type_rotary_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);// 记录当前无人机是否为 VTOL（垂直起降固定翼）_vtol = vehicle_status.is_vtol;// 记录当前无人机是否处于转换模式（即从一种飞行模式切换到另一种飞行模式的过程中）_vtol_in_transition_mode = vehicle_status.in_transition_mode;}
}
vehicle_status_s用于存储无人机的状态信息
struct vehicle_status_s {#endifuint64_t timestamp; // 系统时间戳uint64_t nav_state_timestamp; // 最后一次导航状态的时间戳uint64_t failsafe_timestamp; // 最后一次进入失控保护状态的时间戳uint64_t armed_time; // 解锁时间uint64_t takeoff_time; // 起飞时间uint32_t onboard_control_sensors_present; // 当前连接的传感器掩码uint32_t onboard_control_sensors_enabled; // 当前启用的传感器掩码uint32_t onboard_control_sensors_health; // 当前连接的传感器状态掩码。uint8_t nav_state; // 当前导航状态uint8_t arming_state; // 当前解锁状态uint8_t hil_state; // 硬件在环仿真(Hardware-in-the-Loop Simulation)状态bool failsafe; // 是否处于失控保护状态。uint8_t system_type; // 类型(例如：固定翼、多旋翼、无人船)uint8_t system_id; // 系统IDuint8_t component_id; // 组件IDuint8_t vehicle_type; // 类型(例如：固定翼、多旋翼、无人船)bool is_vtol; // 是否为垂直起降固定翼bool is_vtol_tailsitter; // 是否为尾垂式VTOLbool vtol_fw_permanent_stab; // VTOL前飞行时是否使用永久稳定器bool in_transition_mode; // 载具是否处于转换模式bool in_transition_to_fw; // 载具是否从垂直起降模式转换到了固定翼模式bool rc_signal_lost; // 是否失去了遥控信号uint8_t rc_input_mode; // 当前遥控输入模式bool data_link_lost; // 是否失去了数据链路信号uint8_t data_link_lost_counter; // 数据链路丢失的次数bool high_latency_data_link_lost;// 是否失去了高延迟数据链路信号bool engine_failure; // 是否发生了引擎故障bool mission_failure; // 是否发生了任务失败bool geofence_violated; // 是否越界uint8_t failure_detector_status; // 故障检测器状态uint8_t latest_arming_reason; uint8_t latest_disarming_reason;uint8_t _padding0[4]; // required for logger
}
bool attitude_setpoint_generated = false;// 当前无人机正处于悬停模式 ( 当前飞行器类型为旋翼机 && 当前飞行器不处于转换模式)const bool is_hovering = (_vehicle_type_rotary_wing && !_vtol_in_transition_mode);// 当前飞行器是否为尾翼式垂直起降飞行器并且处于转换模式const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode);// 是否启用了姿态控制 并且 当前无人机正处于悬停模式或者当前飞行器是否为尾翼式垂直起降飞行器并且处于转换模式bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);// 是否需要执行姿态控制if (run_att_ctrl) {const Quatf q{v_att.q};// 判断当前控制模式为手动（Manual）或者稳定（Stabilized）模式，且没有开启高度、速度、位置控制if (_v_control_mode.flag_control_manual_enabled &&!_v_control_mode.flag_control_altitude_enabled &&!_v_control_mode.flag_control_velocity_enabled &&!_v_control_mode.flag_control_position_enabled) {// 根据手柄输入和当前姿态 生成姿态设定点generate_attitude_setpoint(q, dt, _reset_yaw_sp);// 表示已经生成了姿态设定点attitude_setpoint_generated = true;} else { //否则将手柄输入滤波器的状态重置为0_man_x_input_filter.reset(0.f);_man_y_input_filter.reset(0.f);}// update()函数返回一个计算出的期望角速度向量rates_sp// 参数q是当前飞行器的姿态四元数。通过将当前飞行器的姿态四元数和期望姿态四元数（在之前的代码中计算）一起传递给姿态控制器，它计算出旋转向量（旋转速度），并将其转换为期望角速度向量，即返回的rates_spVector3f rates_sp = _attitude_control.update(q);// 发布无人机期望的姿态速率（角速度）和推力信息vehicle_rates_setpoint_s v_rates_sp{};v_rates_sp.roll = rates_sp(0);v_rates_sp.pitch = rates_sp(1);v_rates_sp.yaw = rates_sp(2);_thrust_setpoint_body.copyTo(v_rates_sp.thrust_body);v_rates_sp.timestamp = hrt_absolute_time();_v_rates_sp_pub.publish(v_rates_sp);}// reset yaw setpoint during transitions, tailsitter.cpp generates// attitude setpoint for the transition// 用于在姿态控制中 generate_attitude_setpoint（） 指示是否需要重置偏航设定点// 当attitude_setpoint_generated为false（还未生成新的姿态设定点），或者飞机降落（即_landed为true），或者垂直起降机处于转换模式（即_vtol和_vtol_in_transition_mode都为true）时，就将_reset_yaw_sp设置为true。_reset_yaw_sp = !attitude_setpoint_generated || _landed || (_vtol && _vtol_in_transition_mode);}// 结束计时器perf_end(_loop_perf);
}
4.计算期望姿态角
该函数将一个四元数表示的当前姿态和一个时间步长作为输入，并根据遥控器输入和其它参数生成新的姿态设定点。**函数的主要任务是将遥控器输入的横滚和俯仰信号映射到机体坐标系中的横滚和俯仰角，并将遥控器输入的偏航信号转化为偏航速度设定值。**同时，它还会对姿态角进行限制，以避免出现机体不稳定的情况。
void MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, bool reset_yaw_sp)
{vehicle_attitude_setpoint_s attitude_setpoint{};/*当前偏航角计算-----------------------------------------------------------------------------------------------------------------------*/const float yaw = Eulerf(q).psi();/* reset yaw setpoint to current position if needed */if (reset_yaw_sp) {_man_yaw_sp = yaw; // 将遥控器输入的偏航信号设为当前位置} else if (math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) > 0.05f /* 检查手动控制输入信号的高度 是否大于 0.05f */|| _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) { // 或飞行器是否处于 roll_pitch_yaw 模式（在这种模式下，飞行器的横滚、俯仰和偏航姿态受到手动遥控器输入的控制，这种模式适用于需要更精细控制的飞行，例如飞行器需要保持特定的朝向进行视觉或传感器任务时。）const float yaw_rate = math::radians(_param_mpc_man_y_max.get());attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate; // 手动控制输入的偏航量 * 偏航速率限制 = 偏航角速度设定值_man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt);// 乘以 dt 并加上当前的 偏航角，最后使用 wrap_pi 函数将结果限制在 [-π, π] }/** Input mapping for roll & pitch setpoints* ----------------------------------------* We control the following 2 angles:* - tilt angle, given by sqrt(x*x + y*y)* - the direction of the maximum tilt in the XY-plane, which also defines the direction of the motion** This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick* points to, and changes of the stick input are linear.*/_man_x_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());_man_y_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());// 控制的倾斜角度最大值_man_x_input_filter.update(_manual_control_setpoint.x * _man_tilt_max);_man_y_input_filter.update(_manual_control_setpoint.y * _man_tilt_max);const float x = _man_x_input_filter.getState();const float y = _man_y_input_filter.getState();// we want to fly towards the direction of (x, y), so we use a perpendicular axis angle vector in the XY-plane// 使用更新后的摇杆信号 x 和 y，创建一个二维向量 v，表示无人机在 XY 平面上的倾斜方向。// 为了使无人机朝向这个方向，我们使用 v 的法向量作为无人机的滚转和俯仰轴Vector2f v = Vector2f(y, -x);// 计算向量 v 的模来获取倾斜角度float v_norm = v.norm(); // the norm of v defines the tilt angleif (v_norm > _man_tilt_max) { // limit to the configured maximum tilt anglev *= _man_tilt_max / v_norm;}Quatf q_sp_rpy = AxisAnglef(v(0), v(1), 0.f); // 计算出一个旋转角度Eulerf euler_sp = q_sp_rpy; // 转化为欧拉角// v的x和y坐标分别对应飞机在滚转和俯仰轴上的倾斜角度// euler_sp(2)对应的是旋转后的姿态中无人机的偏航角；然后将yaw的值设为_man_yaw_sp加上旋转后的姿态中的偏航角// 这样，就得到了完整的飞行姿态控制指令。attitude_setpoint.roll_body = euler_sp(0);attitude_setpoint.pitch_body = euler_sp(1);// The axis angle can change the yaw as well (noticeable at higher tilt angles).// This is the formula by how much the yaw changes:// let a := tilt angle, b := atan(y/x) (direction of maximum tilt)// yaw = atan(-2 * sin(b) * cos(b) * sin^2(a/2) / (1 - 2 * cos^2(b) * sin^2(a/2))).attitude_setpoint.yaw_body = _man_yaw_sp + euler_sp(2); /* modify roll/pitch only if we're a VTOL */// 这段代码是用于处理 VTOL（垂直起降）机型的姿态设定if (_vtol) {// Construct attitude setpoint rotation matrix. Modify the setpoints for roll// and pitch such that they reflect the user's intention even if a large yaw error// (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix// from the pure euler angle setpoints will lead to unexpected attitude behaviour from// the user's view as the euler angle sequence uses the yaw setpoint and not the current// heading of the vehicle.// However there's also a coupling effect that causes oscillations for fast roll/pitch changes// at higher tilt angles, so we want to avoid using this on multicopters.// The effect of that can be seen with:// - roll/pitch into one direction, keep it fixed (at high angle)// - apply a fast yaw rotation// - look at the roll and pitch angles: they should stay pretty much the same as when not yawing// calculate our current yaw errorfloat yaw_error = wrap_pi(attitude_setpoint.yaw_body - yaw);// compute the vector obtained by rotating a z unit vector by the rotation// given by the roll and pitch commands of the user// zB表示初始向量，即垂直于机身的向量(0,0,1)Vector3f zB = {0.0f, 0.0f, 1.0f};// 使用欧拉角构造一个方向余弦矩阵（DCM），该矩阵描述了一个旋转，将一个向量从机体坐标系（body frame）旋转到一个参考坐标系（reference frame）Dcmf R_sp_roll_pitch = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, 0.0f);// 将一个表示沿着z轴方向的 z_roll_pitch_sp，// 表示这个单位向量在机体坐标系下的方向。// 将Dcmf类型的 R_sp_roll_pitch旋转矩阵和Vector3f 类型的zB向量相乘，实际上是将zB向量沿着roll和pitc轴旋转，得到了一个新向量 z_roll_pitch_sp// 这里可以把 R_sp_roll_pitch 看成是从机体坐标系到惯性坐标系的旋转矩阵，zB 表示沿着机体坐标系 z 轴的方向，// z_roll_pitch_sp 则是表示沿着机体坐标系 roll 和 pitch 方向的单位向量Vector3f z_roll_pitch_sp = R_sp_roll_pitch * zB;// transform the vector into a new frame which is rotated around the z axis// by the current yaw error. this vector defines the desired tilt when we look// into the direction of the desired heading// 将旋转矩阵 R_sp_roll_pitch 绕着z轴 旋转一个角度 yaw_error ，得到新的向量 z_roll_pitch_sp。// 由于欧拉角和旋转矩阵是等价的，可以通过欧拉角构建旋转矩阵Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error);// 将沿着机体坐标系 roll 和 pitch 方向的单位向量 z_roll_pitch_sp 乘以旋转矩阵 R_yaw_correction// 就得到在当前偏航误差下的新向量，这个新向量表示期望的俯仰和滚转角度。z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;// use the formula z_roll_pitch_sp = R_tilt * [0;0;1]// R_tilt is computed from_euler; only true if cos(roll) not equal zero// -> valid if roll is not +-pi/2;attitude_setpoint.roll_body = -asinf(z_roll_pitch_sp(1));attitude_setpoint.pitch_body = atan2f(z_roll_pitch_sp(0), z_roll_pitch_sp(2));}/* copy quaternion setpoint to attitude setpoint topic */// 将欧拉角转换为四元数。使用欧拉角设置飞行器的期望角度// 四元数可以更方便地表示飞行器的姿态，因为它们不容易受到万向锁问题的影响，并且在执行姿态控制时具有更好的数学性质Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body);q_sp.copyTo(attitude_setpoint.q_d);// 设置了期望姿态的体轴系的油门// 对手动控制器的纵轴输入值进行了处理，使其在0到1之间，并进行了缩放和反向操作（将输入值反向，以便使油门随着手动控制器的上升而增加）。// 最后将油门值设置为一个在0到-1之间的值，其中负数表示将油门放置在飞机体轴系下的“下方”，即向地面施加一个向上的力。attitude_setpoint.thrust_body[2] = -throttle_curve(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f));attitude_setpoint.timestamp = hrt_absolute_time();//发布_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
}
5.姿态控制
matrix::Vector3f AttitudeControl::update(const Quatf &q) const
{/*先进行roll-pitch控制-------------------------------------------------------------------------------------------------------------------*/// 将当前期望的姿态四元数存储在 qd 中Quatf qd = _attitude_setpoint_q; // 当前飞行器的姿态 q 和 期望姿态 qd// calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch（优先考虑俯仰和滚转方向，而忽略偏航方向）const Vector3f e_z = q.dcm_z(); // 计算当前姿态（q）的Z轴单位向量，也就是当前飞行器的朝向向量。该向量通常被视为飞行器的“前方”。const Vector3f e_z_d = qd.dcm_z(); // 计算期望姿态（qd）的Z轴向量Quatf qd_red(e_z, e_z_d); // 表示从当前姿态到期望姿态的旋转
// 由于目标姿态的 $\hat{z}$ 向量（期望的推力方向）与当前姿态的 $\hat{z}$ 向量不同，所以需要旋转当前姿态使得 $\hat{z}$ 与期望的 $\hat{z}$ 方向对齐，然后在这个基础上对 roll 和 pitch 进行控制。这样做可以确保在控制 roll 和 pitch 的同时保持飞机在期望的推力方向上运动。if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {// 期望姿态和当前姿态之差比较小，直接将期望姿态转化为当前姿态qd_red = qd;} else {// 较大情况：将期望的姿态旋转到当前的机体坐标系中，变成了基于当前姿态的期望姿态，这样就可以通过控制 roll 和 pitch 实现对机体在推力方向上的运动控制了qd_red *= q;}/*再进行Yaw控制--------------------------------------------------------------------------------------------------------------------------*//* 计算当前的姿态与期望姿态的误差，并规范化误差四元数 */// 计算 将 当前姿态转换到期望姿态 需要 旋转的四元数（从当前姿态到期望姿态的旋转的差）Quatf q_mix = qd_red.inversed() * qd; // inversed（）用于计算四元数的逆// 由于上述计算可能会出现镜像或近似镜像的情况，所以需要规范化误差四元数（即使存在数值误差也能保证旋转的正确性）q_mix.canonicalize(); // canonicalize（）用于将四元数规范化// catch numerical problems with the domain of acosf and asinf// 由于 q_mix 可能存在数值问题，超出了 acosf 和 asinf 的定义域，所以这里使用了 constrain 函数进行限制，使其在合理的范围内q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);// 计算期望姿态/* cosf(_yaw_w * acosf(q_mix(0)))计算需要施加到期望姿态的旋转四元数中的标量部分，以确保正确的旋转方向* 1、q_mix(0)表示从期望姿态到当前姿态的旋转四元数中的标量部分，即旋转角度的余弦值* 2、acosf(q_mix(0))表示我们要计算的从期望姿态到当前姿态的旋转角度（弧度）：所以对q_mix(0)反余弦即可* 3、_yaw_w * acosf(q_mix(0))表示需要绕z轴施加的旋转角度，其中_yaw_w是yaw轴控制增益*//* sinf(_yaw_w * asinf(q_mix(3)))计算需要施加到期望姿态四元数的Z轴的sine值，以确保正确的旋转方向* 1、q_mix(3)表示从期望姿态到当前姿态的旋转四元数中Z轴的旋转角度的sin值* 2、反正弦* 3、同理也是需要绕z轴施加的旋转角度*//*综上所述，这行代码的作用是通过将从期望姿态到当前姿态的旋转与需要绕z轴施加的旋转组合在一起，得到一个期望姿态，其中包括滚转和俯仰控制，并绕z轴旋转Yaw角度，以实现期望的飞行姿态。*/ qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));// 四元数 qe ：需要施加的姿态控制误差。const Quatf qe = q.inversed() * qd; // 当前姿态的逆 * 期望姿态// 三维向量 eq：当前姿态与期望姿态之间的误差const Vector3f eq = 2.f * qe.canonical().imag(); // 2 * qe的虚部（旋转向量）/* note1：这里使用了canonical()函数，是因为qe是四元数类型，它可以表示任意的三维旋转，但四元数也有多个等价表示，因此使用canonical()函数可以使它变为一个具有唯一性的表示。note2：在四元数和欧拉角之间进行转换时，欧拉角的转换公式中包含一个因子2，因此这里需要将差向量eq乘以2。具体来说，将差向量的每个分量乘以2后，可以得到欧拉角（roll, pitch, yaw）的变化量（d_roll, d_pitch, d_yaw）*///三维向量 rate_setpoint： 期望的三个角速度分量matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain); // eq（当前姿态与期望姿态之间的误差）点乘 _proportional_gain（比例控制器的增益矩阵）// Feed forward the yaw setpoint rate.// yawspeed_setpoint is the feed forward commanded rotation around the world z-axis,// but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).// Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)// and multiply it by the yaw setpoint rate (yawspeed_setpoint).// This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame// such that it can be added to the rates setpoint.// _yawspeed_setpoint 前馈偏航设定点速率，q.inversed().dcm_z()是当前机体坐标系（即机体坐标系相对于惯性坐标系的旋转）的 z 轴单位向量在惯性坐标系中的表示// q.inversed().dcm_z() * _yawspeed_setpoint 期望绕 z 轴的角速度if (is_finite(_yawspeed_setpoint)) {rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint;}// limit ratesfor (int i = 0; i < 3; i++) {rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));}return rate_setpoint;
}PX4多旋翼姿态控制程序分析mc_att_control相关推荐
- PX4多旋翼位置控制程序分析 mc_pos_control
PX4多旋翼位置控制程序分析 mc_pos_control 1. 概述 在本文中,我们将分析PX4程序的基本流程和关键模块. 2. PX4程序流程图 原图大小太大,无法上传(在我主页下载里面找) 3. ...
- 【控制】四旋翼无人机姿态角分析
搞起来,从建模到控制,再到仿真 Link: [控制]<多无人机协同控制技术>周伟老师-第3章-面向协同控制的无人机单机控制 文章目录 1. 坐标系 2. 角度介绍 航向角 yaw ange ...
- 【PX4-AutoPilot教程-1】PX4源码文件目录架构分析
PX4源码文件目录架构分析 PX4源代码的结构复杂,这是源代码的总目录结构(以v1.13.0为例): Firmware ├─boards ├─build ├─cmake ├─Documentation ...
- 【UAV】从单个螺旋桨到四旋翼无人机运动学分析
文章目录 1 单个螺旋桨受力分析 2 坐标变化 3 四个螺旋桨对无人机的影响 3.1 旋翼对位置的影响 3.2 旋翼对姿态角影响 4 机体坐标下的输出量 5 位置变量转换到世界坐标系 6 简化分析 1 ...
- 四旋翼姿态解算——梯度下降法理论推导
转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...
- PX4代码解析：振动分析
本篇文章首发于公众号:无人机系统技术.更多无人机技术相关文章请关注此公众号 一.前言 前面的文章主要都是一些理论知识为主,很多读者朋友看了之后可能会有点枯燥,里面很多公式看起来也比较晦涩,今天起给大家 ...
- PX4从放弃到精通（五）：PX4中的姿态表示方法及转换关系
文章目录 前言 坐标系 无人机的姿态描述 转换关系 总结 前言 一个人可以走的更快,一群人才能走的更远,交流学习加qq:2096723956 更多保姆级PX4+ROS学习视频:https://b23. ...
- 四旋翼姿态解算——互补滤波算法及理论推导
转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...
- 四旋翼姿态解算——基础理论及推导
转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...
最新文章
- Unirest 轻量级的HTTP开发库
- STM32时钟配置方法详解
- [置顶] 电信系统方案 电信Boss系统
- js中获取事件对象的方法小结
- mysql 1045 登录失败
- 代码示例：使用redis计数来控制单位时间内对某接口的访问量
- jaxb注解使用_使用JAXB时
- 原来公司需要这样的你
- SQL注入：6、SQLMAP的使用
- windows证书地址
- PHP微信多级三级分佣系统,微信三级分销系统微信三级分销系统
- 集体智慧编程(5)——优化
- Flash Player的终章——赠予它的挽歌
- 宝塔面板安装和使用教程(详细)
- 互联网产品的前世今生
- 学习笔记(01):通俗易懂的Bootstrap视频课程（适合初学者的教程）-图标、下拉菜单、按钮组...
- 遭遇盗取网络游戏帐号木马等
- web端接收读卡器卡片信息
- 对计算机知识的掌握,计算机操作基本知识
- 计算机组成原理 主存储器1