PX4代码解析：姿态期望生成

诗筱涵

已于 2025-02-19 15:13:13 修改

阅读量142

点赞数

分类专栏：无人机-pixhawk-PX4 文章标签： px4

于 2025-02-19 00:03:58 首次发布

原文链接：https://mp.weixin.qq.com/s/weGCgpAWEgilw9KPh5OZ9Q

版权

无人机-pixhawk-PX4 专栏收录该内容

176 篇文章

订阅专栏

转载自 PX4代码解析：姿态期望生成位置控制输出\x0a姿态期望生成https://mp.weixin.qq.com/s/weGCgpAWEgilw9KPh5OZ9Q

如有侵权可以私信联系删除

PX4代码解析：姿态期望生成

原创无人机系统技术无人机系统技术 2019年09月14日 20:00

引言

关于PX4源码中控制器的设计部分我们前几讲已经对位置控制、姿态控制等核心函数进行了详细分析，逐行解释。今天我们就将剩下的最后一部分内容：位置控制器输出到姿态控制器输入的转换部分做一个解读。

转换部分的函数就一个：

vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp)在Firmware\src\modules\mc_pos_control\Utility\ControlMath.cpp文件中。

我们先贴出此函数的源码：

vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp){  vehicle_attitude_setpoint_s att_sp = {};  att_sp.yaw_body = yaw_sp;
  // desired body_z axis = -normalize(thrust_vector)  Vector3f body_x, body_y, body_z;
  if (thr_sp.length() > 0.00001f) {    body_z = -thr_sp.normalized();
  } else {    // no thrust, set Z axis to safe value    body_z = Vector3f(0.f, 0.f, 1.f);  }
  // vector of desired yaw direction in XY plane, rotated by PI/2  Vector3f y_C(-sinf(att_sp.yaw_body), cosf(att_sp.yaw_body), 0.0f);
  if (fabsf(body_z(2)) > 0.000001f) {    // desired body_x axis, orthogonal to body_z    body_x = y_C % body_z;
    // keep nose to front while inverted upside down    if (body_z(2) < 0.0f) {      body_x = -body_x;    }
    body_x.normalize();
  } else {    // desired thrust is in XY plane, set X downside to construct correct matrix,    // but yaw component will not be used actually    body_x.zero();    body_x(2) = 1.0f;  }
  // desired body_y axis  body_y = body_z % body_x;
  Dcmf R_sp;
  // fill rotation matrix  for (int i = 0; i < 3; i++) {    R_sp(i, 0) = body_x(i);    R_sp(i, 1) = body_y(i);    R_sp(i, 2) = body_z(i);  }
  //copy quaternion setpoint to attitude setpoint topic  Quatf q_sp = R_sp;  q_sp.copyTo(att_sp.q_d);  att_sp.q_d_valid = true;
  // calculate euler angles, for logging only, must not be used for control  Eulerf euler = R_sp;  att_sp.roll_body = euler(0);  att_sp.pitch_body = euler(1);  att_sp.thrust_body[2] = -thr_sp.length();
  return att_sp;}

代码解读

//函数开始：

vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp){

//定义姿态期望，赋值期望偏航角

  vehicle_attitude_setpoint_s att_sp = {};  att_sp.yaw_body = yaw_sp;

//定义期望机体轴系三个轴的单位向量在机载NED系下的表示

  // desired body_z axis = -normalize(thrust_vector)  Vector3f body_x, body_y, body_z;

//根据期望升力的方向给期望机体轴系的Z轴单位向量赋值

  if (thr_sp.length() > 0.00001f) {    body_z = -thr_sp.normalized();
  } else {

//如果期望升力向量模为0时，归一化会出现奇异，赋值成(0,0,1)

    // no thrust, set Z axis to safe value    body_z = Vector3f(0.f, 0.f, 1.f);  }

//机体轴系X轴单位向量在NE平面的投影绕D轴旋转PI/2角度后的向量

  // vector of desired yaw direction in XY plane, rotated by PI/2  Vector3f y_C(-sinf(att_sp.yaw_body), cosf(att_sp.yaw_body), 0.0f);

//期望机体轴系的X轴与Z轴以及上一行计算出来的向量垂直，因此通过叉乘计算可以得到期望机体轴系的X轴单位向量

  if (fabsf(body_z(2)) > 0.000001f) {    // desired body_x axis, orthogonal to body_z    body_x = y_C % body_z;

//期望机体轴系的X轴在计算时与Z轴的方向是相关的，因为R3r的第三个元素的正负代表飞行器是否需要翻过来，如果飞行器需要翻过来，那么此时要保持机头方向还是向前需要加负号。计算后再做归一化处理。

    // keep nose to front while inverted upside down    if (body_z(2) < 0.0f) {      body_x = -body_x;    }
    body_x.normalize();

//如果期望机体轴系Z轴位于NE平面内，则将X轴赋值为(0,0,1)，即机头垂直向下,此时，期望偏航角就不起作用了。

  } else {    // desired thrust is in XY plane, set X downside to construct correct matrix,    // but yaw component will not be used actually    body_x.zero();    body_x(2) = 1.0f;  }

//根据期望机体轴系三轴互相垂直的关系计算Y轴单位向量的值

  // desired body_y axis  body_y = body_z % body_x;

//根据定义给期望旋转矩阵赋值

  Dcmf R_sp;
  // fill rotation matrix  for (int i = 0; i < 3; i++) {    R_sp(i, 0) = body_x(i);    R_sp(i, 1) = body_y(i);    R_sp(i, 2) = body_z(i);  }

//期望旋转矩阵转换成期望四元数，并给用于发布消息的期望四元数话题赋值

  //copy quaternion setpoint to attitude setpoint topic  Quatf q_sp = R_sp;  q_sp.copyTo(att_sp.q_d);  att_sp.q_d_valid = true;

//将旋转矩阵期望转换成欧拉角以及油门期望，用于日志记录，便于分析

  // calculate euler angles, for logging only, must not be used for control  Eulerf euler = R_sp;  att_sp.roll_body = euler(0);  att_sp.pitch_body = euler(1);  att_sp.thrust_body[2] = -thr_sp.length();
  return att_sp;}

总结

本篇内容对PX4中位置控制器的输出转换成期望姿态的过程进行了详细解读，代码进行了逐行分析，关于算法公式的推导，大家可以参考之前的文章：无人机控制器设计（二）：位置控制器设计。