pixhawk 姿态与控制部分的记录
Posted 虾米一代
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了pixhawk 姿态与控制部分的记录相关的知识，希望对你有一定的参考价值。
此篇是把之前看到的资料总结整理一遍，大部分是搬砖，也加入了自己的一点思考，写的过程中晕了好多次，先大体记录下来，肯定有错误，日后再改正吧。
关于pixhawk程序执行流程，依然还没有实际运行调试过程序，只是逻辑上理清先做什么再做什么，以防实际调试程序时脑袋晕掉，所以还只是纸上谈兵。把整个程序流程脑袋中跑一遍、来龙去脉搞清楚点后再实际调试的。
经过前面这些博客分析，基本把程序分析的障碍扫除了，但是理论基础十分薄弱（还没来得及细细看来）。
姿态解算
这部分需要的理论知识较多，现在只浅浅的理解了一部分，先记下来，以免以后搞混了乱了。主要感谢牛叔的讲解！
（1）Mahony算法解析：
传感器自身属性，陀螺仪短时期动态响应可以利用、长时期会温漂，加速度机长时间可以利用短时期振动太厉害不能利用。因此就形成了互补融合滤波。网上一个很好的比喻，让飞行器保持一个姿态相当于是开一艘船到某个目的地，陀螺仪相当于是掌舵手，自己可以短时期推断船的航向，但是没有指向，一段时间后就会走偏了，所以需要一个观望者，指明方向，告诉他开的方向对不对，进行校正，而这个角色在飞行器中就是由加速度计来担任。所以mahony的算法核心就是：掌舵者一直问观望者我开的对不对，然后校正。
以上是感性认识，以下是理论算法上解释：
首先需要明确几个概念：飞行器姿态有几种表达方式，比如欧拉角（pitch、roll、yaw）、四元数矩阵（q1q2q3q4）。而飞行器自身传感器测量的数据都是相对于飞行器坐标而言的，也就是body坐标系；实际上我们需要飞行器保持什么姿态、去哪个地方都是相对于地理坐标系而言的，也就是r坐标系，为了达到控制目的，所以需要一个从b坐标系到r坐标系的旋转矩阵，为了校正磁偏也需要从r坐标系到b坐标系的旋转矩阵。不同的旋转顺序，旋转矩阵是不同的，因为相同的两矩阵相乘哪个在前哪个在后乘出来结果是不同的，对应于坐标系就是对齐x轴、y轴、z轴的先后顺序了，比如b坐标系旋转到r坐标系，旋转顺序是x-y-z，那么旋转回来到b坐标系就是z-y-x，所以不同的旋转顺序，对应的旋转矩阵是不同的！
PI校正的解释（这个也是mahony的核心部分）：
这个就是总体的流程
说到底这个姿态解算的过程就是，先在b坐标系下用传感器进行测量，并且用四元数表达出来，其中用向量叉积求得2个向量之间的误差，加速度计的向量矩阵与陀螺仪的向量矩阵做叉积以按时间来分配权重，重新得到一个新的姿态也就是新的四元数矩阵，转换成欧拉角输出给后面的控制。哎，理论知识太欠缺了，只能理解到这一步。
这部分的代码就是
void MahonyIMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
float recipNorm;
float halfvx, halfvy, halfvz;
float halfex, halfey, halfez;
float qa, qb, qc;
gx*=0.0174;
gy*=0.0174;
gz*=0.0174;
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Estimated direction of gravity and vector perpendicular to magnetic flux
halfvx = q1 * q3 - q0 * q2;
halfvy = q0 * q1 + q2 * q3;
halfvz = q0 * q0 - 0.5f + q3 * q3;
// Error is sum of cross product between estimated and measured direction of gravity
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);
// Compute and apply integral feedback if enabled
integralFBx += twoKi * halfex * dt; // integral error scaled by Ki
integralFBy += twoKi * halfey * dt;
integralFBz += twoKi * halfez * dt;
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
// Integrate rate of change of quaternion
gx *= 0.0125f;//(0.5f * dt); // pre-multiply common factors
gy *= 0.0125f;//(0.5f * dt);
gz *= 0.0125f;//(0.5f * dt); //0.00125f
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
Mahony最后的那个公式解释：
这张图的意思是：假如左边[x y z]’是地理坐标下的姿态，右边[x0 y0 z0]’是机体坐标下的姿态，中间通过旋转矩阵得以转换，而欧拉角和四元数是旋转矩阵不同的表达方式，相同的姿态，两个坐标系对应都是相同的，所以四元数和欧拉角之间有个对应关系(当然也和旋转顺序有关)，这个关系就是mahony算法最后的那个公式
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Yaw = atan2(2*q1*q2 - 2*q0*q3, 2*q0*q0 + 2*q1*q1 - 1) * 57.3;//-360;
更加详细的解释可以查看“四元数解算姿态完全解析及资料汇总”
截下关键步骤
之前的都没有加入磁力计，因为磁力计和加速度计不一样，它在三维空间中并不和某个轴重合，而受到的影响也是不一样的，所以需要不同的处理方法，也就是需要从机体坐标系旋转到地理坐标系干个什么事，在旋转回机体坐标系进行误差补偿。先这么理解吧，笔者也不大懂。
（3）pixhawk姿态解算流程
以下规定黑色为最外层（控制主程序），红色为第二层（主程序的注释），蓝色为第三层（主程序注释程序的注释）
程序入口Ardupilot/modules/PX4Firmware/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
void AttitudeEstimatorQ::task_main()
#ifdef __PX4_POSIX
perf_counter_t_perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay"));
perf_counter_t_perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay"));
perf_counter_t_perf_mag(perf_alloc_once(PC_ELAPSED, "sim_mag_delay"));
#endif
_sensors_sub= orb_subscribe(ORB_ID(sensor_combined));
_vision_sub= orb_subscribe(ORB_ID(vision_position_estimate));
_mocap_sub= orb_subscribe(ORB_ID(att_pos_mocap));
_airspeed_sub= orb_subscribe(ORB_ID(airspeed));
_params_sub= orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub= orb_subscribe(ORB_ID(vehicle_global_position));
订阅各种数据
update_parameters(true);
初始化各种参数
hrt_abstimelast_time = 0;
px4_pollfd_struct_tfds[1] = ;
fds[0].fd= _sensors_sub;
fds[0].events= POLLIN;
while(!_task_should_exit)
intret = px4_poll(fds, 1, 1000);
if(ret < 0)
//Poll error, sleep and try again
usleep(10000);
PX4_WARN("QPOLL ERROR");
continue;
else if (ret == 0)
//Poll timeout, do nothing
PX4_WARN("QPOLL TIMEOUT");
continue;
update_parameters(false);
//Update sensors
sensor_combined_ssensors;
intbest_gyro = 0;
intbest_accel = 0;
intbest_mag = 0;
if(!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors))
//Feed validator with recent sensor data获取传感器数据
Ardupilot/modules/PX4Firmware/src/modules/uORB/topics/sensor_combined.h
#ifdef __cplusplus
struct __EXPORTsensor_combined_s
#else
struct sensor_combined_s
#endif
uint64_t timestamp;
uint64_t gyro_timestamp[3];
int16_t gyro_raw[9];
float gyro_rad_s[9];
uint32_t gyro_priority[3];
float gyro_integral_rad[9];
uint64_t gyro_integral_dt[3];
uint32_t gyro_errcount[3];
float gyro_temp[3];
int16_t accelerometer_raw[9];
float accelerometer_m_s2[9];
float accelerometer_integral_m_s[9];
uint64_t accelerometer_integral_dt[3];
int16_t accelerometer_mode[3];
float accelerometer_range_m_s2[3];
uint64_t accelerometer_timestamp[3];
uint32_t accelerometer_priority[3];
uint32_t accelerometer_errcount[3];
float accelerometer_temp[3];
int16_t magnetometer_raw[9];
float magnetometer_ga[9];
int16_t magnetometer_mode[3];
float magnetometer_range_ga[3];
float magnetometer_cuttoff_freq_hz[3];
uint64_t magnetometer_timestamp[3];
uint32_t magnetometer_priority[3];
uint32_t magnetometer_errcount[3];
float magnetometer_temp[3];
float baro_pres_mbar[3];
float baro_alt_meter[3];
float baro_temp_celcius[3];
uint64_t baro_timestamp[3];
uint32_t baro_priority[3];
uint32_t baro_errcount[3];
float adc_voltage_v[10];
uint16_t adc_mapping[10];
float mcu_temp_celcius;
float differential_pressure_pa[3];
uint64_t differential_pressure_timestamp[3];
float differential_pressure_filtered_pa[3];
uint32_t differential_pressure_priority[3];
uint32_t differential_pressure_errcount[3];
#ifdef __cplusplus
static const int32_t MAGNETOMETER_MODE_NORMAL = 0;
static const int32_t MAGNETOMETER_MODE_POSITIVE_BIAS = 1;
static const int32_t MAGNETOMETER_MODE_NEGATIVE_BIAS = 2;
static const uint32_t SENSOR_PRIO_MIN = 0;
static const uint32_t SENSOR_PRIO_VERY_LOW = 25;
static const uint32_t SENSOR_PRIO_LOW = 50;
static const uint32_t SENSOR_PRIO_DEFAULT = 75;
static const uint32_t SENSOR_PRIO_HIGH = 100;
static const uint32_t SENSOR_PRIO_VERY_HIGH = 125;
static const uint32_t SENSOR_PRIO_MAX = 255;
#endif
; for(unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) /sizeof(sensors.gyro_timestamp[0])); i++)
/*ignore empty fields */
if(sensors.gyro_timestamp[i] > 0)
floatgyro[3];
for(unsigned j = 0; j < 3; j++)
if(sensors.gyro_integral_dt[i] > 0)
gyro[j]= (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] /1e6);
else
/*fall back to angular rate */
gyro[j]= sensors.gyro_rad_s[i * 3 + j];
_voter_gyro.put(i,sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i],sensors.gyro_priority[i]);
gyro、accel、mag每次读取数据都是三组三组的读取
Ardupilot/modules/PX4Firmware/src/lib/ecl/validation/data_validator_group.cpp
void
DataValidatorGroup::put(unsignedindex, uint64_t timestamp, float val[3], uint64_t error_count, int priority)
DataValidator *next = _first;
unsigned i = 0;
while (next != nullptr)
if (i == index)
next->put(timestamp, val, error_count,priority);
break;
next = next->sibling();
i++;
Ardupilot/modules/PX4Firmware/src/lib/ecl/validation/data_validator.cpp
void
DataValidator::put(uint64_ttimestamp, float val, uint64_t error_count_in, int priority_in)
float data[3];
data[0] = val;
data[1] = 0.0f;
data[2] = 0.0f;
put(timestamp, data, error_count_in, priority_in);
void
DataValidator::put(uint64_ttimestamp, float val[3], uint64_t error_count_in, int priority_in)
_event_count++;
if (error_count_in > _error_count)
_error_density += (error_count_in - _error_count);
else if (_error_density > 0)
_error_density--;
_error_count = error_count_in;
_priority = priority_in;
for (unsigned i = 0; i < _dimensions; i++)
if (_time_last == 0)
_mean[i] = 0;
_lp[i] = val[i];
_M2[i] = 0;
else
float lp_val = val[i] - _lp[i];
float delta_val = lp_val - _mean[i];
_mean[i] += delta_val / _event_count;
_M2[i] += delta_val * (lp_val -_mean[i]);
_rms[i] = sqrtf(_M2[i] / (_event_count -1));
if (fabsf(_value[i] - val[i]) <0.000001f)
_value_equal_count++;
else
_value_equal_count = 0;
_vibe[i] = _vibe[i] * 0.99f + 0.01f * fabsf(val[i]- _lp[i]);
// XXX replace with better filter, make itauto-tune to update rate
_lp[i] = _lp[i] * 0.99f + 0.01f * val[i];
_value[i] = val[i];
_time_last = timestamp;
先将每组的数据（例如gyro）将三个维度的的传感器数据put入（如_voter_gyro.put(...))）DataValidatorGroup中，并goto到DataValidator::put函数
在DataValidator函数中计算数据的误差、平均值、并进行滤波。
滤波入口的put函数：
val=传感器读取的数据
_lp=滤波器的系数（lowpass value）
初始化：由上图可知当第一次读到传感器数据时_mean和_M2置0，_lp=val；
lp_val= val - _lp
delta_val= lp_val - _mean
_mean= (平均值）每次数据读取时，每次val和_lp的差值之和的平均值 _mean[i] += delta_val / _event_count
_M2= （均方根值）delta_val * (lp_val - _mean)的和
_rms= 均方根值sqrtf(_M2[i] / (_event_count - 1))
优化滤波器系数：_lp[i]= _lp[i] * 0.5f + val[i] * 0.5f
/*ignore empty fields */
if(sensors.accelerometer_timestamp[i] > 0)
_voter_accel.put(i,sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
sensors.accelerometer_errcount[i],sensors.accelerometer_priority[i]);
/*ignore empty fields */
if(sensors.magnetometer_timestamp[i] > 0)
_voter_mag.put(i,sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
sensors.magnetometer_errcount[i],sensors.magnetometer_priority[i]);
//Get best measurement values
hrt_abstimecurr_time = hrt_absolute_time();
_gyro.set(_voter_gyro.get_best(curr_time,&best_gyro));
_accel.set(_voter_accel.get_best(curr_time,&best_accel));
_mag.set(_voter_mag.get_best(curr_time,&best_mag));Ardupilot/modules/PX4Firmware/src/lib/ecl/validation/data_validator_group.cpp
float*
DataValidatorGroup::get_best(uint64_ttimestamp, int *index)
DataValidator *next = _first;
// XXX This should eventually also include voting
int pre_check_best = _curr_best;
float pre_check_confidence = 1.0f;
int pre_check_prio = -1;
float max_confidence = -1.0f;
int max_priority = -1000;
int max_index = -1;
DataValidator *best = nullptr;
unsigned i = 0;
while (next != nullptr)
float confidence = next->confidence(timestamp);
if (static_cast<int>(i) == pre_check_best)
pre_check_prio = next->priority();
pre_check_confidence = confidence;
Ardupilot/modules/PX4Firmware/src/lib/ecl/validation/data_validator.cpp
float
DataValidator::confidence(uint64_ttimestamp)
float ret = 1.0f;
/* check if we have any data */
if (_time_last == 0)
_error_mask |=ERROR_FLAG_NO_DATA;
ret = 0.0f;
/* timed out - that's it */
else if (timestamp - _time_last >_timeout_interval)
_error_mask |=ERROR_FLAG_TIMEOUT;
ret = 0.0f;
/* we got the exact same sensor value Ntimes in a row */
else if (_value_equal_count >VALUE_EQUAL_COUNT_MAX)
_error_mask |=ERROR_FLAG_STALE_DATA;
ret = 0.0f;
/* check error count limit */
else if (_error_count >NORETURN_ERRCOUNT)
_error_mask |=ERROR_FLAG_HIGH_ERRCOUNT;
ret = 0.0f;
/* cap error density counter at windowsize */
else if (_error_density >ERROR_DENSITY_WINDOW)
_error_mask |=ERROR_FLAG_HIGH_ERRDENSITY;
_error_density =ERROR_DENSITY_WINDOW;
/* no error */
else
_error_mask = ERROR_FLAG_NO_ERROR;
/* no critical errors */
if (ret > 0.0f)
/* return local error densityfor last N measurements */
ret = 1.0f - (_error_density/ ERROR_DENSITY_WINDOW);
return ret;
滤波器的confidence函数（信任度函数，貌似模糊控制理论有个隶属函数，应该类似的功能）：返回值是对上N次测量的验证的信任程度，其值在0到1之间，越大越好。返回值是返回上N次测量的误差诊断，用于get_best函数选择最优值
/*
* Switchif:
* 1) theconfidence is higher and priority is equal or higher
* 2) theconfidence is no less than 1% different and the priority is higher
*/
if (((max_confidence < MIN_REGULAR_CONFIDENCE)&& (confidence >= MIN_REGULAR_CONFIDENCE)) ||
(confidence > max_confidence&& (next->priority() >= max_priority)) ||
(fabsf(confidence - max_confidence) <0.01f && (next->priority() > max_priority))
)
max_index = i;
max_confidence = confidence;
max_priority = next->priority();
best = next;
next = next->sibling();
i++;
/* the current best sensor is not matching the previous bestsensor */
if (max_index != _curr_best)
bool true_failsafe = true;
/* check whether the switch was a failsafe orpreferring a higher priority sensor */
if (pre_check_prio != -1 && pre_check_prio< max_priority &&
fabsf(pre_check_confidence -max_confidence) < 0.1f)
/* this is not a failover */
true_failsafe = false;
/* reset error flags, this is likely ahotplug sensor coming online late */
best->reset_state();
/* if we're no initialized, initialize thebookkeeping but do not count a failsafe */
if (_curr_best < 0)
_prev_best = max_index;
else
/* we were initialized before, this is areal failsafe */
_prev_best = pre_check_best;
if (true_failsafe)
_toggle_count++;
/* if this is the first time,log when we failed */
if (_first_failover_time == 0)
_first_failover_time= timestamp;
/* for all cases we want to keep a record of thebest index */
_curr_best = max_index;
*index = max_index;
return (best) ? best->value() : nullptr;
if(_accel.length() < 0.01f)
warnx("WARNING:degenerate accel!");
continue;
if(_mag.length() < 0.01f)
warnx("WARNING:degenerate mag!");
continue;
_data_good= true;
if(!_failsafe)
uint32_tflags = DataValidator::ERROR_FLAG_NO_ERROR;
#ifdef __PX4_POSIX
perf_end(_perf_accel);
perf_end(_perf_mpu);
perf_end(_perf_mag);
#endif
if(_voter_gyro.failover_count() > 0)
_failsafe= true;
flags= _voter_gyro.failover_state();
mavlink_and_console_log_emergency(&_mavlink_log_pub,"Gyro #%i failure :%s%s%s%s%s!",
_voter_gyro.failover_index(),
((flags &DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags &DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" :""),
((flags &DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags &DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" :""),
((flags &DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" :""));
if(_voter_accel.failover_count() > 0)
_failsafe= true;
flags= _voter_accel.failover_state();
mavlink_and_console_log_emergency(&_mavlink_log_pub,"Accel #%i failure :%s%s%s%s%s!",
_voter_accel.failover_index(),
((flags &DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags &DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" :""),
((flags &DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT)? " High error count" : ""),
((flags &DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" :""));
if(_voter_mag.failover_count() > 0)
_failsafe= true;
flags= _voter_mag.failover_state();
mavlink_and_console_log_emergency(&_mavlink_log_pub,"Mag #%i failure :%s%s%s%s%s!",
_voter_mag.failover_index(),
((flags &DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA)? " Stale data" : ""),
((flags &DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags &DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" :""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY)? " High error density" : ""));_voter_gyro、_voter_accel、_voter_mag三个参数的failover_count函数判断是否存在数据获取失误问题，并通过mavlink协议显示错误原因
if(_failsafe)
mavlink_and_console_log_emergency(&_mavlink_log_pub,"SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
if(!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time)> _vibration_warning_threshold ||
_voter_accel.get_vibration_factor(curr_time) >_vibration_warning_threshold ||
_voter_mag.get_vibration_factor(curr_time) >_vibration_warning_threshold)) Ardupilot/modules/PX4Firmware/src/lib/ecl/validation/data_validator_group.cpp
float
DataValidatorGroup::get_vibration_factor(uint64_ttimestamp)
DataValidator *next = _first;
float vibe = 0.0f;
/* find the best RMS value of a non-timed out sensor */
while (next != nullptr)
if (next->confidence(timestamp) > 0.5f)
float* rms = next->rms();
for (unsigned j = 0; j < 3; j++)
if (rms[j] > vibe)
vibe= rms[j];
next = next->sibling();
return vibe;//将rms变量（原来put函数中的_rms）传回主函数中和_vibration_warning_threshold进行判断。
根据_voter_gyro、_voter_accel、_voter_mag三个参数的get_vibration_factor函数判断是否有震动现象，返回值是float型的RSM值，其代表振动的幅度大小。
if(_vibration_warning_timestamp == 0)
_vibration_warning_timestamp= curr_time;
else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000)
_vibration_warning= true;
mavlink_and_console_log_critical(&_mavlink_log_pub,"HIGH VIBRATION! g: %d a: %d m: %d",
(int)(100 *_voter_gyro.get_vibration_factor(curr_time)),
(int)(100 *_voter_accel.get_vibration_factor(curr_time)),
(int)(100 *_voter_mag.get_vibration_factor(curr_time)));
else
_vibration_warning_timestamp= 0;
//Update vision and motion capture heading
boolvision_updated = false;
orb_check(_vision_sub,&vision_updated);
boolmocap_updated = false;
orb_check(_mocap_sub,&mocap_updated);
if(vision_updated)
orb_copy(ORB_ID(vision_position_estimate),_vision_sub, &_vision);
math::Quaternionq(_vision.q);
math::Matrix<3,3> Rvis = q.to_dcm();
math::Vector<3>v(1.0f, 0.0f, 0.4f);
//Rvis is Rwr (robot respect to world) while v is respect to world.
//Hence Rvis must be transposed having (Rwr)' * Vw
//Rrw * Vw = vn. This way we have consistency
_vision_hdg= Rvis.transposed() * v;
if(mocap_updated)
orb_copy(ORB_ID(att_pos_mocap),_mocap_sub, &_mocap);
math::Quaternionq(_mocap.q);
math::Matrix<3,3> Rmoc = q.to_dcm();
math::Vector<3>v(1.0f, 0.0f, 0.4f);
//Rmoc is Rwr (robot respect to world) while v is respect to world.
//Hence Rmoc must be transposed having (Rwr)' * Vw
//Rrw * Vw = vn. This way we have consistency
_mocap_hdg= Rmoc.transposed() * v;
通过uORB模型获取vision和mocap的数据（视觉和mocap）
//Update airspeed
boolairspeed_updated = false;
orb_check(_airspeed_sub,&airspeed_updated);
if(airspeed_updated)
orb_copy(ORB_ID(airspeed),_airspeed_sub, &_airspeed);
//Check for timeouts on data
if(_ext_hdg_mode == 1)
_ext_hdg_good= _vision.timestamp_boot > 0 &&(hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
else if (_ext_hdg_mode == 2)
_ext_hdg_good= _mocap.timestamp_boot > 0 &&(hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
boolgpos_updated;
orb_check(_global_pos_sub,&gpos_updated);
if(gpos_updated)
orb_copy(ORB_ID(vehicle_global_position),_global_pos_sub, &_gpos);
if(_mag_decl_auto && _gpos.eph < 20.0f &&hrt_elapsed_time(&_gpos.timestamp) < 1000000)
/*set magnetic declination automatically */
update_mag_declination(math::radians(get_mag_declination(_gpos.lat,_gpos.lon)));
if(_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() <_gpos.timestamp + 20000 && _gpos.eph < 5.0f && _inited)
/*position data is actual */
if(gpos_updated)
Vector<3>vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);
/*velocity updated */
if(_vel_prev_t != 0 && _gpos.timestamp != _vel_prev_t)
floatvel_dt = (_gpos.timestamp - _vel_prev_t) / 1000000.0f;
/*calculate acceleration in body frame */
_pos_acc= _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
_vel_prev_t= _gpos.timestamp;
_vel_prev= vel;
else
/*position data is outdated, reset acceleration */
_pos_acc.zero();
_vel_prev.zero();
_vel_prev_t= 0;
位置加速度获取（position，注意最后在修正时会用到该处的_pos_acc）
/*time from previous iteration */
hrt_abstimenow = hrt_absolute_time();
floatdt = (last_time > 0) ? ((now -last_time) / 1000000.0f) : 0.00001f;
last_time= now;
if(dt > _dt_max)
dt= _dt_max;
if(!update(dt))
continue;
Ardupilot/modules/PX4Firmware/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
boolAttitudeEstimatorQ::update(float dt)
if (!_inited)
if (!_data_good)
return false;
return init();
首先判断是否是第一次进入该函数，第一次进入该函数先进入init函数初始化，源码如下
bool AttitudeEstimatorQ::init()
// Rotation matrix can be easilyconstructed from acceleration and mag field vectors
// 'k' is Earth Z axis (Down) unitvector in body frame
Vector<3> k = -_accel;
k.normalize();初始化方法：k为加速度传感器测量到加速度方向向量，由于第一次测量数据时无人机一般为平稳状态无运动状态，所以可以直接将测到的加速度视为重力加速度g，以此作为dcm旋转矩阵的第三行k
// 'i' is Earth X axis (North) unitvector in body frame, orthogonal with 'k'
Vector<3> i = (_mag - k * (_mag *k));
i.normalize();向量指向正北方，k*(_mag*k)正交correction值，对于最终的四元数归一化以后的范数可以在正负5%以内；感觉不如《DCM IMU:Theory》中提出的理论“强制正交化”修正的好，Renormalization算法在ardupilot的上层应用AP_AHRS_DCM中使用到了
// 'j' is Earth Y axis (East) unitvector in body frame, orthogonal with 'k' and 'i'
Vector<3> j = k % i;外积、叉乘。关于上面的Vector<3>k =-_accel，Vector<3>相当于一个类型（int）定义一个变量k，然后把-_accel负值给k，在定义_accel时也是使用Vector<3>，属于同一种类型的，主要就是为了考虑其实例化过程（类似函数重载）。
// Fill rotation matrix
Matrix<3, 3> R;
R.set_row(0, i);
R.set_row(1, j);
R.set_row(2, k);然后以模板的形式使用“Matrix<3, 3> R”建立3x3矩阵R，通过set_row()函数赋值
ardupilot/modules/PX4Firmware/lib/mathlib/math/Matrix.hpp
/**
* set row from vector
*/
void set_row(unsigned int row, constVector<N> v)
for (unsigned i = 0; i <N; i++)
data[row][i] =v.data[i];
第一行赋值i向量指向北，第二行赋值j向量指向东，第三行赋值k向量指向DOWN
// Convert to quaternion
_q.from_dcm(R);ardupilot/modules/PX4Firmware/lib/mathlib/math/Quaternion.hpp
/**
* set quaternion to rotation by DCM
* Reference: Shoemake, Quaternions,http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
*/
void from_dcm(const Matrix<3, 3>&dcm)
float tr = dcm.data[0][0] +dcm.data[1][1] + dcm.data[2][2];
if (tr > 0.0f)
float s = sqrtf(tr +1.0f);
data[0] = s * 0.5f;
s = 0.5f / s;
data[1] =(dcm.data[2][1] - dcm.data[1][2]) * s;
data[2] =(dcm.data[0][2] - dcm.data[2][0]) * s;
data[3] = (dcm.data[1][0]- dcm.data[0][1]) * s;
其他情况去看代码吧，不好解释。然后_q单位化结束初始化。PS：另外根据DCM求取四元数的算法是参考MartinJohnBaker的，就是上述的原始版，该算法在AP_Math/quaternion.cpp中，链接：
http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.h
源码如下
Ardupilot/libraries/AP_Math/quaternion.cpp PS:求四元数参考的代码DCM求取四元数的算法
// return therotation matrix equivalent for this quaternion
// Thanks to MartinJohn Baker
//http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
voidQuaternion::from_rotation_matrix(const Matrix3f &m)
const float &m00 = m.a.x;
const float &m11 = m.b.y;
const float &m22 = m.c.z;
const float &m10 = m.b.x;
const float &m01 = m.a.y;
const float &m20 = m.c.x;
const float &m02 = m.a.z;
const float &m21 = m.c.y;
const float &m12 = m.b.z;
float &qw = q1;
float &qx = q2;
float &qy = q3;
float &qz = q4;
float tr = m00 + m11 + m22;
if (tr > 0)
float S = sqrtf(tr+1) * 2;
qw = 0.25f * S;
qx = (m21 - m12) / S;
qy = (m02 - m20) / S;
qz = (m10 - m01) / S;
else if ((m00 > m11) && (m00> m22))
float S = sqrtf(1.0f + m00 - m11 - m22)* 2;
qw = (m21 - m12) / S;
qx = 0.25f * S;
qy = (m01 + m10) / S;
qz = (m02 + m20) / S;
else if (m11 > m22)
float S = sqrtf(1.0f + m11 - m00 - m22)* 2;
qw = (m02 - m20) / S;
qx = (m01 + m10) / S;
qy = 0.25f * S;
qz = (m12 + m21) / S;
else
float S = sqrtf(1.0f + m22 - m00 - m11)* 2;
qw = (m10 - m01) / S;
qx = (m02 + m20) / S;
qy = (m12 + m21) / S;
qz = 0.25f * S;
else
/* Find maximumdiagonal element in dcm
* store index indcm_i */
int dcm_i = 0;
for (int i = 1; i< 3; i++)
if(dcm.data[i][i] > dcm.data[dcm_i][dcm_i])
dcm_i= i;
int dcm_j = (dcm_i +1) % 3;
int dcm_k = (dcm_i +2) % 3;
float s =sqrtf((dcm.data[dcm_i][dcm_i] - dcm.data[dcm_j][dcm_j] -
dcm.data[dcm_k][dcm_k])+ 1.0f);
data[dcm_i + 1] = s* 0.5f;
s= 0.5f / s;
data[dcm_j + 1] =(dcm.data[dcm_i][dcm_j] + dcm.data[dcm_j][dcm_i]) * s;
data[dcm_k + 1] =(dcm.data[dcm_k][dcm_i] + dcm.data[dcm_i][dcm_k]) * s;
data[0] =(dcm.data[dcm_k][dcm_j] - dcm.data[dcm_j][dcm_k]) * s;
// Compensate for magnetic declination
Quaternion decl_rotation;
decl_rotation.from_yaw(_mag_decl);
_q = decl_rotation * _q;
_q.normalize();DCM转换为四元数_q（使用from_dcm），并归一化四元数，一定要保持归一化的思想
if (PX4_ISFINITE(_q(0)) &&PX4_ISFINITE(_q(1)) &&
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
_q.length() > 0.95f && _q.length() < 1.05f)
_inited = true;
else
_inited = false;
return _inited;
Quaternion q_last = _q;
// Angular rate of correction
Vector<3> corr;
if (_ext_hdg_mode > 0 && _ext_hdg_good)
if (_ext_hdg_mode == 1)
// Vision heading correction
// Project heading to global frame andextract XY component
Vector<3> vision_hdg_earth =_q.conjugate(_vision_hdg);
float vision_hdg_err =_wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
// Project correction to body frame
corr +=_q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) *_w_ext_hdg;
if (_ext_hdg_mode == 2)
// Mocap heading correction
// Project heading to global frame andextract XY component
Vector<3> mocap_hdg_earth =_q.conjugate(_mocap_hdg);
float mocap_hdg_err =_wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
// Project correction to body frame
corr +=_q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) *_w_ext_hdg;
如果不是第一次进入该函数，则判断是使用什么mode做修正的，比如vision、mocap、acc和mag（DJI精灵4用的视觉壁障应该就是这个vision），Hdg就是heading。
_ext_hdg_mode==1、2时都是利用vision数据和mocap数据对gyro数据进行修正
if (_ext_hdg_mode == 0 || !_ext_hdg_good)
// Magnetometer correction
// Project mag field vector to global frame andextract XY component
Vector<3> mag_earth = _q.conjugate(_mag);
float mag_err = _wrap_pi(atan2f(mag_earth(1),mag_earth(0)) - _mag_decl);
// Project magnetometer correction to body frame
corr +=_q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
利用磁力计修正, _w_mag为mag的权重
mag_earth=_q.conjugate(_mag)，这行代码的含义为先将归一化的_q的旋转矩阵R（b系转n系）乘以_mag向量（以自身机体坐标系为视角看向北方的向量表示），得到n系（NED坐标系）下的磁力计向量
如下就是conjugate函数，其过程就是实现从b系到n系的转换，使用左乘
ardupilot/modules/PX4Firmware/src/lib/mathlib/math/Quaternion.hpp
/**
* conjugation //b系到n系
*/
Vector<3> conjugate(constVector<3> &v) const
float q0q0 = data[0] *data[0];
float q1q1 = data[1] *data[1];
float q2q2 = data[2] *data[2];
float q3q3 = data[3] *data[3];
return Vector<3>(
v.data[0] *(q0q0 + q1q1 - q2q2 - q3q3) +
v.data[1] *2.0f * (data[1] * data[2] - data[0] * data[3]) +
v.data[2] *2.0f * (data[0] * data[2] + data[1] * data[3]),
v.data[0] *2.0f * (data[1] * data[2] + data[0] * data[3]) +
v.data[1] *(q0q0 - q1q1 + q2q2 - q3q3) +
v.data[2] *2.0f * (data[2] * data[3] - data[0] * data[1]),
v.data[0] *2.0f * (data[1] * data[3] - data[0] * data[2]) +
v.data[1] *2.0f * (data[0] * data[1] + data[2] * data[3]) +
v.data[2] *(q0q0 - q1q1 - q2q2 + q3q3)
);
_q.normalize();
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
// Vector<3> k =_q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
// Optimized version with dropped zeros
Vector<3> k(
2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) +_q(3) * _q(3))
);
corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
// Gyro bias estimation
_gyro_bias += corr * (_w_gyro_bias * dt);
for (int i = 0; i < 3; i++)
_gyro_bias(i) = math::constrain(_gyro_bias(i),-_bias_max, _bias_max);
_rates = _gyro + _gyro_bias;
// Feed forward gyro
corr += _rates;
// Apply correction to state
_q += _q.derivative(corr) * dt;
// Normalize quaternion
_q.normalize();
if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1))&&
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3))))
// Reset quaternion to last good state
_q = q_last;
_rates.zero();
_gyro_bias.zero();
return false;
return true;
Vector<3>euler = _q.to_euler();
structvehicle_attitude_s att = ;
att.timestamp= sensors.timestamp;
att.roll= euler(0);
att.pitch= euler(1);
att.yaw= euler(2);
att.rollspeed= _rates(0);
att.pitchspeed= _rates(1);
att.yawspeed= _rates(2);
for(int i = 0; i < 3; i++)
att.g_comp[i]= _accel(i) - _pos_acc(i);
/*copy offsets */
memcpy(&att.rate_offsets,_gyro_bias.data, sizeof(att.rate_offsets));
Matrix<3,3> R = _q.to_dcm();
/*copy rotation matrix */
memcpy(&att.R[0],R.data, sizeof(att.R));
att.R_valid= true;
memcpy(&att.q[0],_q.data, sizeof(att.q));
att.q_valid= true;
att.rate_vibration= _voter_gyro.get_vibration_factor(hrt_absolute_time());
att.accel_vibration= _voter_accel.get_vibration_factor(hrt_absolute_time());
att.mag_vibration= _voter_mag.get_vibration_factor(hrt_absolute_time());
/*the instance count is not used here */
intatt_inst;
orb_publish_auto(ORB_ID(vehicle_attitude),&_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
structcontrol_state_s ctrl_state = ;
ctrl_state.timestamp= sensors.timestamp;
/*attitude quaternions for control state */
ctrl_state.q[0]= _q(0);
ctrl_state.q[1]= _q(1);
ctrl_state.q[2]= _q(2);
ctrl_state.q[3]= _q(3);
/*attitude rates for control state */
ctrl_state.roll_rate= _lp_roll_rate.apply(_rates(0));
ctrl_state.pitch_rate= _lp_pitch_rate.apply(_rates(1));
ctrl_state.yaw_rate= _lp_yaw_rate.apply(_rates(2));
/*Airspeed - take airspeed measurement directly here as no wind is estimated */
if(PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time()- _airspeed.timestamp < 1e6
&& _airspeed.timestamp > 0)
ctrl_state.airspeed= _airspeed.indicated_airspeed_m_s;
ctrl_state.airspeed_valid= true;
以上是关于pixhawk 姿态与控制部分的记录的主要内容，如果未能解决你的问题，请参考以下文章