一、前言
上一節(jié)介紹了PX4姿態(tài)估計調(diào)用函數(shù)的流程，這一節(jié)分享一下我對PX4姿態(tài)解算的解讀.首先，要理解PX4姿態(tài)解算的程序，要先從傳感器的特性入手，這里主要介紹的傳感器有加速度計，磁力計，陀螺儀.
二、傳感器特性
1.加速度計
pixhawk上使用的為三軸加速度計，主要用于測量x,y,z三軸的加速度值，常用的傳感器例如mpu6000與mpu9250，在進(jìn)行PX4二次開發(fā)時，我們并不需要編寫加速度計相關(guān)驅(qū)動程序，其代碼已經(jīng)在PX4_Firmware/src/drivers/imu下進(jìn)行了實現(xiàn)，感興趣可以自己查閱.這里我們只需了解加速度計的原理與特性.
如圖所示，該圖為加速度計簡易模型，由彈簧與質(zhì)量塊構(gòu)成，當(dāng)外界有加速度時，質(zhì)量塊位置會發(fā)生變化，從而使得電容兩端距離改變，流經(jīng)電容的電流也會發(fā)生變化，通過測量電流大小，就可以得到質(zhì)量塊移動距離，從而得到加速度大小。理想狀態(tài)下，當(dāng)沒有外界作用時，質(zhì)量塊會處于零點位置，但由于工藝，使用時間長短等各種因素，質(zhì)量塊可能會處于非零點位置，即所謂零偏，因此，在飛無人機之前往往需要進(jìn)行校準(zhǔn)。
此外，加速度計校準(zhǔn)還涉及到另一個參數(shù)，這個參數(shù)是標(biāo)度因數(shù)，在這里可以理解為一個比例系數(shù)，測量值乘以這個比例系數(shù)后得到實際值。
從加速度原理可以看出，在測量加速度時質(zhì)量塊需要不斷移動，移動需要時間，因此，在高頻情況下加速度計值不一定準(zhǔn)確，低頻率特性比較好.
2.陀螺儀
pixhawk上另一個比較重要的傳感器就是陀螺儀，陀螺儀用于測量x,y,z三軸角速度，其基本的原理是動量守恒。當(dāng)外部系統(tǒng)發(fā)生旋轉(zhuǎn)時，內(nèi)部轉(zhuǎn)動裝置會保持恒定的速度與方向旋轉(zhuǎn)，測量這兩個系統(tǒng)的差就可以得到當(dāng)前系統(tǒng)角速度。為了測量x,y,z三軸角速度，通常采用萬向節(jié)構(gòu)成轉(zhuǎn)動裝置
以其中一個方向為例，當(dāng)陀螺儀測量裝置隨著轉(zhuǎn)動軸轉(zhuǎn)動時，在半徑方向上會存在力，使得質(zhì)量塊在半徑方向進(jìn)行周期往復(fù)運動，從而得到旋轉(zhuǎn)角速度.但由于存在零點漂移，陀螺儀在低頻特性較差，高頻特性較好.
3.磁力計
磁力計主要用于測量當(dāng)前磁場強度。為了能夠測量地磁方向，通常將地磁分為水平與垂直方向，水平方向可以近似表示地磁方向。但地球的磁軸與地軸有著夾角，一般稱為磁偏角，磁偏角在不同地理位置不同，因此在無人機航向計算時，需要gps獲得當(dāng)前經(jīng)緯度，然后查表得到當(dāng)前位置磁偏角，對航向進(jìn)行修正（后續(xù)代碼中會看到）.同樣的磁力計校正也涉及到偏移與標(biāo)度因數(shù).
三、姿態(tài)解算算法
1.為什么采用四元數(shù)計算
在介紹姿態(tài)解算算法前，先來談?wù)勛藨B(tài)的表示方式，常用的姿態(tài)表示方法有：四元數(shù)，歐拉角，方向余弦這幾種方式，并且這幾種方式是可以 互相轉(zhuǎn)換，歐拉角雖然計算簡單，但是存在退化現(xiàn)象；方向余弦有9個參數(shù)，導(dǎo)致其計算量過大，實時性不好；因此，PX4源碼中采用四元數(shù)來表示姿態(tài)。
2.姿態(tài)解算的坐標(biāo)系
在無人機的坐標(biāo)變換過程中，一般會涉及到以下四個坐標(biāo)系
1.傳感器坐標(biāo)系
傳感器本身具有自己的測量坐標(biāo)系，在安裝過程中會存在安裝誤差，而傳感器讀數(shù)是在傳感器坐標(biāo)系下測得，為了能讓無人機使用，需要將其轉(zhuǎn)換到機體坐標(biāo)系。但加速度計，磁力計，陀螺儀這些傳感器安裝時一般與無人機機體中心位置與方向都重合，所以可以粗略認(rèn)為傳感器坐標(biāo)系與無人機坐標(biāo)系重合
2.機體坐標(biāo)系
一般以無人機幾何中心為中心，以右手定則建立的三維直角坐標(biāo)系，x軸位于無人機機體平面，以無人機前進(jìn)方向為正方向；y軸在機體平面且垂直x軸，z軸垂直機體平面，以向下為正.
3.本地坐標(biāo)系（local）
為了確定無人機相對于地面的速度與位置，需要引入本地坐標(biāo)系（仿真中常用的地面坐標(biāo)系）。一般情況下，本地坐標(biāo)系是以無人機起點為坐標(biāo)中心，在水平面上正北方向為x軸，正東為y軸方向，z軸垂直地面向下，這也是所謂的北東地（NED）坐標(biāo)系
4.全局坐標(biāo)系（global）
前面提到，在不同經(jīng)緯度下，地軸與磁軸之間的磁偏角是不一樣的，為了修正無人機航向，還需要引入GPS測得的地球經(jīng)緯度.
3.Mahony濾波算法
下面來講解算法，如圖為mahony濾波的流程圖，取自文獻(xiàn)[1]
先來解釋一下上圖：
1.
a
,
m
a,m
a,m與
ω
\omega
ω分別是加速度計測得的加速度，磁力計測得的磁場強度以及陀螺儀測得的角速度。其實這里就引出了一個問題，為什么需要加速度計與磁力計，光靠陀螺儀不就可以得到無人機姿態(tài)嗎？
確實，光靠陀螺儀是可以得到無人機姿態(tài)的，通過對得到的角速度積分就得到了姿態(tài)，但靠積分得到的姿態(tài)會存在積分誤差，這個光靠陀螺儀是無法解決的，因此引入了加速度計與磁力計來解決陀螺儀積分誤差。
2.四元數(shù)微分方程為
Q
˙
=
1
2
?
ω
n
b
b
\dot{Q} =\frac{1}{2} \otimes \omega _{nb}^
Q˙?=21??ωnbb?,式中
Q
Q
Q為姿態(tài)四元數(shù)，
ω
n
b
b
\omega _{nb}^
ωnbb?為無人機機體坐標(biāo)系b相對于北東地（NED）坐標(biāo)系n的角速度，這個式子是姿態(tài)解算的核心
3.由加速度計與磁力計得來的姿態(tài)同樣存在誤差，因此需要PI控制器來對誤差進(jìn)行修正，PI控制器的輸入是由加速度計與磁力計算出的姿態(tài)與最終通過四元數(shù)微分方程計算出的實際姿態(tài)的差值，輸出角速度修正值，補償?shù)酵勇輧x，抵消陀螺儀誤差
4.這個過程在無人機運行過程中循環(huán)計算，不斷進(jìn)行姿態(tài)解算
四、姿態(tài)解算算法代碼講解
在PX4代碼解析(5)中我已經(jīng)介紹了代碼運行流程，本節(jié)只針對姿態(tài)解算算法重點部分進(jìn)行講解，我將這部分代碼分為以下幾個部分：
1.AttitudeEstimatorQ對象建立以及相關(guān)數(shù)據(jù)獲取
2.四元數(shù)q的初始化
3.姿態(tài)解算
1.AttitudeEstimatorQ對象的構(gòu)造函數(shù)
先來看看.AttitudeEstimatorQ對象的構(gòu)造函數(shù)
//在對象的構(gòu)造函數(shù)中，主要是對對象成員變量清0
AttitudeEstimatorQ::AttitudeEstimatorQ() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl)
{
_vel_prev.zero();
_pos_acc.zero();
//gyro是陀螺儀數(shù)據(jù)，是一個1*3或者3*1向量
_gyro.zero();
//accel表示的是加速度計數(shù)據(jù)
_accel.zero();
//mag是磁力計數(shù)據(jù)
_mag.zero();
//vision是視覺數(shù)據(jù)，mocap是動作捕捉的數(shù)據(jù)，暫時用不到
_vision_hdg.zero();
_mocap_hdg.zero();
//四元數(shù)q清0
_q.zero();
_rates.zero();
_gyro_bias.zero();
//將參數(shù)文件中參數(shù)拷貝到當(dāng)前進(jìn)程使用，這些參數(shù)就是你在地面站里可以修改的那些，比如加速度計的偏移等數(shù)據(jù)
update_parameters(true);
}
2.Run函數(shù)
在執(zhí)行完構(gòu)造函數(shù)后，該進(jìn)程會執(zhí)行run函數(shù)
//該函數(shù)主要是讀取各傳感器數(shù)據(jù)，并分配給相應(yīng)變量，為后續(xù)姿態(tài)解算做前置工作
void
AttitudeEstimatorQ::Run()
{
//第一個if主要用于判斷傳感器那邊數(shù)據(jù)準(zhǔn)備好沒有
if (should_exit()) {
_sensors_sub.unregisterCallback();
exit_and_cleanup();
return;
}
//定義了一個sensor_combine的結(jié)構(gòu)體，用于接收數(shù)據(jù)
sensor_combined_s sensors;
//將數(shù)據(jù)拷貝到sensors
if (_sensors_sub.update(&sensors)) {
update_parameters();//默認(rèn)參數(shù)為force
//檢查數(shù)據(jù)是否為最新陀螺儀
// Feed validator with recent sensor data
if (sensors.timestamp > 0) {
_gyro(0) = sensors.gyro_rad[0];
_gyro(1) = sensors.gyro_rad[1];
_gyro(2) = sensors.gyro_rad[2];
}
//更新加速度計數(shù)據(jù)
if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
_accel(0) = sensors.accelerometer_m_s2[0];
_accel(1) = sensors.accelerometer_m_s2[1];
_accel(2) = sensors.accelerometer_m_s2[2];
if (_accel.length() < 0.01f) {
PX4_ERR("degenerate accel!");
return;
}
}
// U更新磁力計數(shù)據(jù)
if (_magnetometer_sub.updated()) {
vehicle_magnetometer_s magnetometer;
if (_magnetometer_sub.copy(&magnetometer)) {
_mag(0) = magnetometer.magnetometer_ga[0];
_mag(1) = magnetometer.magnetometer_ga[1];
_mag(2) = magnetometer.magnetometer_ga[2];
//如果磁力計數(shù)據(jù)太小，則返回報錯
if (_mag.length() < 0.01f) {
PX4_ERR("degenerate mag!");
return;
}
}
}
//數(shù)據(jù)更新完成標(biāo)志位
_data_good = true;
// Update vision and motion capture heading
//是否有視覺或者動作捕捉，false不使用
_ext_hdg_good = false;
if (_vision_odom_sub.updated()) {
vehicle_odometry_s vision;
if (_vision_odom_sub.copy(&vision)) {
// validation check for vision attitude data
bool vision_att_valid = PX4_ISFINITE(vision.q[0])
&& (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE],
fmaxf(vision.pose_covariance[vision.COVARIANCE_MATRIX_PITCH_VARIANCE],
vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
if (vision_att_valid) {
Dcmf Rvis = Quatf(vision.q);
Vector3f 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.transpose() * v;
// vision external heading usage (ATT_EXT_HDG_M 1)
if (_param_att_ext_hdg_m.get() == 1) {
// Check for timeouts on data
_ext_hdg_good = vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000);
}
}
}
}
//動捕
if (_mocap_odom_sub.updated()) {
vehicle_odometry_s mocap;
if (_mocap_odom_sub.copy(&mocap)) {
// validation check for mocap attitude data
bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
&& (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE],
fmaxf(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_PITCH_VARIANCE],
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
if (mocap_att_valid) {
Dcmf Rmoc = Quatf(mocap.q);
Vector3f 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.transpose() * v;
// Motion Capture external heading usage (ATT_EXT_HDG_M 2)
if (_param_att_ext_hdg_m.get() == 2) {
// Check for timeouts on data
_ext_hdg_good = mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000);
}
}
}
}
//上面的代碼是讀取視覺與動捕數(shù)據(jù)，沒有這些傳感器的用不到
//如果gps數(shù)據(jù)更新
if (_gps_sub.updated()) {
vehicle_gps_position_s gps;//定義結(jié)構(gòu)體，該結(jié)構(gòu)體在msg文件夾中
if (_gps_sub.copy(&gps)) {
//如果gps數(shù)據(jù)獲取成功并且精度不錯，則使用gps數(shù)據(jù)查閱磁偏角
if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
//通過gps得到的經(jīng)緯度查表，得到磁偏角，去修正磁力計數(shù)據(jù)
update_mag_declination(math::radians(get_mag_declination(gps.lat, gps.lon)));
}
}
}
//更新在NED坐標(biāo)系的位置
if (_local_position_sub.updated()) {
vehicle_local_position_s lpos;
if (_local_position_sub.copy(&lpos)) {
//是否進(jìn)行運動加速度計算，加速度計數(shù)據(jù)比較好
if (_param_att_acc_comp.get() && (hrt_elapsed_time(&lpos.timestamp) < 20_ms)
&& lpos.v_xy_valid && lpos.v_z_valid && (lpos.eph < 5.0f) && _inited) {
/* position data is actual */
const Vector3f vel(lpos.vx, lpos.vy, lpos.vz);
//更新速度
/* velocity updated */
if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) {
//將時間由us轉(zhuǎn)為s
float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f;
/* calculate acceleration in body frame *///補償加速度計
_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
}
_vel_prev_t = lpos.timestamp;
_vel_prev = vel;
} else {//數(shù)據(jù)過期，重置加速度
/* position data is outdated, reset acceleration */
_pos_acc.zero();
_vel_prev.zero();
_vel_prev_t = 0;
}
}
}
//以上是獲取數(shù)據(jù)，對數(shù)據(jù)簡單處理
//上一次迭代時間
/* time from previous iteration */
hrt_abstime now = hrt_absolute_time();//高精度定時器，得到當(dāng)前時間
//得到用于計算積分的步長時間
const float dt = math::constrain((now - _last_time) / 1e6f, _dt_min, _dt_max);
//更新last_time
_last_time = now;
if (update(dt)) {//姿態(tài)解算
vehicle_attitude_s att = {};//
att.timestamp = sensors.timestamp;
_q.copyTo(att.q);
/* the instance count is not used here */
_att_pub.publish(att);//姿態(tài)發(fā)布
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
if (_param_est_group.get() == 3) {
// In this case the estimator_q is running without LPE and needs
// to publish ekf2_timestamps because SITL lockstep requires it.
ekf2_timestamps_s ekf2_timestamps;
ekf2_timestamps.timestamp = now;
ekf2_timestamps.airspeed_timestamp_rel = 0;
ekf2_timestamps.distance_sensor_timestamp_rel = 0;
ekf2_timestamps.optical_flow_timestamp_rel = 0;
ekf2_timestamps.vehicle_air_data_timestamp_rel = 0;
ekf2_timestamps.vehicle_magnetometer_timestamp_rel = 0;
ekf2_timestamps.visual_odometry_timestamp_rel = 0;
_ekf2_timestamps_pub.publish(ekf2_timestamps);
}
#endif
}
}
}
代碼中，我對其進(jìn)行了注釋，下面有幾個需要強調(diào)的點
1.sensor_combined_s是什么？
他是一個結(jié)構(gòu)體，這個結(jié)構(gòu)體是通過定義的消息自動生成，可以在msg文件中查看，在msg文件中找到sensor_combine，文件內(nèi)容如下：
# Sensor readings in SI-unit form.
# These fields are scaled and offset-compensated where possible and do not
# change with board revisions and sensor updates.
uint64 timestamp # time since system start (microseconds)
int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid
//陀螺儀數(shù)據(jù)
# gyro timstamp is equal to the timestamp of the message
float32[3] gyro_rad # average angular rate measured in the XYZ body frame in rad/s over the last gyro sampling period
uint32 gyro_integral_dt # gyro measurement sampling period in us
//加速度計數(shù)據(jù)
int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
float32[3] accelerometer_m_s2 # average value acceleration measured in the XYZ body frame in m/s/s over the last accelerometer sampling period
uint32 accelerometer_integral_dt # accelerometer measurement sampling period in us
uint8 CLIPPING_X = 1
uint8 CLIPPING_Y = 2
uint8 CLIPPING_Z = 4
uint8 accelerometer_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the sampling period
可以看到，在結(jié)構(gòu)體sensor_combine_s中，存放了陀螺儀與加速度計的數(shù)據(jù)，所以run函數(shù)一直到_data_good = true;代碼之前的工作就是更新陀螺儀，加速度計，磁力計.當(dāng)_data_good=true，說明數(shù)據(jù)準(zhǔn)備完成
2._pos_acc是運動加速度，運動加速度與加速度計測量的值不同，加速度計的值=運動加速度+重力加速度；在這里，代碼利用兩次速度之差除以時間得到運動加速度，這個運動加速度在后續(xù)姿態(tài)解算有用
3.update(dt)函數(shù)就是姿態(tài)解算的核心代碼，下面對這部分代碼進(jìn)行解讀
3.update姿態(tài)解算函數(shù)
我先把姿態(tài)解算的代碼貼上來
bool
AttitudeEstimatorQ::update(float dt)
{
//判斷是否存在四元數(shù)初值
if (!_inited) {
//如果數(shù)據(jù)沒有準(zhǔn)備好，就是加速度計，磁力計這些數(shù)據(jù)沒有更新，則不進(jìn)行下面操作
if (!_data_good) {
return false;
}
//執(zhí)行四元數(shù)初始化函數(shù)
return init_attq();//進(jìn)行初始姿態(tài)獲取
}
1.從前面的原理可知，姿態(tài)解算的核心公式是四元數(shù)的微分方程，而為了實現(xiàn)微分方程的計算，我們需要一個四元數(shù)初值，四元數(shù)初值是通過init_attq（）函數(shù)產(chǎn)生，代碼如下：
bool
AttitudeEstimatorQ::init_attq()
{
// Rotation matrix can be easily constructed from acceleration and mag field vectors
//初始方向余弦矩陣可以由加速度計與磁力計獲得
// 'k' is Earth Z axis (Down) unit vector in body frame
//以加速度計測得向量反方向作為z軸，因為加速度計測量坐標(biāo)系z軸向上，與NED坐標(biāo)系的z軸方向相反
Vector3f k = -_accel;
//進(jìn)行歸一化，k向量除以它的二范數(shù)，目的是為了構(gòu)成余弦矩陣
k.normalize();
// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
//_mag*k可以看做
Vector3f i = (_mag - k * (_mag * k));//以磁力計作為x軸，保證x與z軸正交
i.normalize();
// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
向量叉乘得到y(tǒng)軸
Vector3f j = k % i;//向量叉乘得到y(tǒng)軸
// Fill rotation matrix
//填充旋轉(zhuǎn)矩陣
Dcmf R;//旋轉(zhuǎn)矩陣
R.row(0) = i;
R.row(1) = j;
R.row(2) = k;
// Convert to quaternion
//轉(zhuǎn)換為四元數(shù)，PX4中重載了賦值號
_q = R;
// Compensate for magnetic declination
//補償磁力計偏移
Quatf decl_rotation = Eulerf(0.0f, 0.0f, _mag_decl);
//四元數(shù)更新并重新初始化
_q = _q * decl_rotation;
_q.normalize();
//判斷更新是否成功
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;
}
這里面需要提醒的是這句代碼
Vector3f i = (_mag - k * (_mag * k))
代碼中_mag與k都為向量，且向量k在經(jīng)歷歸一化后為單位向量，因此_mag*k可以看做_mag在k向量上的投影長度，然后乘以單位向量k就變成了與k同向，長度為|_mag|*cos
θ
\theta
θ的向量，再使用_mag減去該向量就得到垂直于k的向量i，具體可見下圖
這里就完成了第一段講解，接著向下看文章來源：http://www.zghlxwxcb.cn/news/detail-621893.html
//還是update函數(shù)
//記錄上一次四元數(shù)值
Quatf q_last = _q;
// Angular rate of correction
Vector3f corr;//
float spinRate = _gyro.length();
//_ext_hdg_good==false，不使用動作捕捉與視覺
if (_param_att_ext_hdg_m.get() > 0 && _ext_hdg_good) {
if (_param_att_ext_hdg_m.get() == 1) {
// Vision heading correction
// Project heading to global frame and extract XY component
Vector3f 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(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get();
}
if (_param_att_ext_hdg_m.get() == 2) {
// Mocap heading correction
// Project heading to global frame and extract XY component
Vector3f 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(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get();
}
}
//使用磁力計
if (_param_att_ext_hdg_m.get() == 0 || !_ext_hdg_good) {
// Magnetometer correction
//我們使用磁力計的目的是得到無人機的偏航角，因此我們需要將磁力計的值投影到NED坐標(biāo)系的XY平面，
//磁力計得到數(shù)據(jù)是位于機體坐標(biāo)系，需要把其轉(zhuǎn)換到NED坐標(biāo)系后投影
// Project mag field vector to global frame and extract XY component
//從機體坐標(biāo)系轉(zhuǎn)到NED系
Vector3f mag_earth = _q.conjugate(_mag);
//_mag_decl就是剛剛查表得到的磁偏角，這里是對磁力計補償，得到磁偏角誤差
//wrap_pi（）函數(shù)是個限幅函數(shù)，保證磁力計誤差在-pi到pi（因為我們拿磁力計去算偏航，偏航范圍是這個）
float mag_err = wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
//以下四句代碼，個人理解是在對磁力計進(jìn)行標(biāo)度因數(shù)的校正
float gainMult = 1.0f;//標(biāo)度因數(shù)
const float fifty_dps = 0.873f;
if (spinRate > fifty_dps) {
gainMult = math::min(spinRate / fifty_dps, 10.0f);
}
//轉(zhuǎn)換到機體坐標(biāo)系，conjugate_inversed（）是將NED坐標(biāo)系向量轉(zhuǎn)為機體坐標(biāo)系，_param_att_w_mag為磁力計濾波系數(shù)
//磁力計標(biāo)度因子
// Project magnetometer correction to body frame
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;
}
//四元數(shù)歸一化
_q.normalize();
//加速度計的修正
//首先，實際加速度 =加速度計的值-重力加速度
//下面的k向量是重力加速度歸一化后從NED系轉(zhuǎn)到機體坐標(biāo)系后的表示
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
// Vector3f k = _q.conjugate_inversed(Vector3f(0.0f, 0.0f, 1.0f));
// Optimized version with dropped zeros
Vector3f 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))
);
// If we are not using acceleration compensation based on GPS velocity,
// fuse accel data only if its norm is close to 1 g (reduces drift).
//對數(shù)據(jù)進(jìn)行限幅處理
const float accel_norm_sq = _accel.norm_squared();
const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
if (_param_att_acc_comp.get() || ((accel_norm_sq > lower_accel_limit * lower_accel_limit) &&
(accel_norm_sq < upper_accel_limit * upper_accel_limit))) {
//這句代碼是重點，我來解釋一下：_accel這是加速度計的值，_pos_acc是通過兩次速度之差計算出的運動加速度，都在機體坐標(biāo)系下
//那么，_accel-_pos_acc就是機體坐標(biāo)系下的重力加速度，然后再歸一化。這里%代表向量叉乘，在傳感器都是理想情況下，k向量與
//(_accel - _pos_acc).normalized()向量的叉乘應(yīng)該為0，但由于存在傳感器誤差，這一項不為0，于是就得到了相應(yīng)的加速度計誤差.
//_param_att_w_acc.get（）為互補濾波中加速度計權(quán)重
corr += (k % (_accel - _pos_acc).normalized()) * _param_att_w_acc.get();
}
//陀螺儀偏移估計
//_param_att_w_gyro_bias.get()陀螺儀偏移的初始值
// Gyro bias estimation
if (spinRate < 0.175f) {
_gyro_bias += corr * (_param_att_w_gyro_bias.get() * dt);
//對陀螺儀偏移每一項的限幅
for (int i = 0; i < 3; i++) {
_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
}
}
//經(jīng)過修正后的角速度
_rates = _gyro + _gyro_bias;
//得到角速度的校正值
// Feed forward gyro
corr += _rates;
// Apply correction to state
//四元數(shù)微分方程求解
_q += _q.derivative1(corr) * dt;
// Normalize quaternion
//四元數(shù)歸一化
_q.normalize();
//如果四元數(shù)數(shù)據(jù)出現(xiàn)異常，恢復(fù)到最近一次的正常的狀態(tài)，并將漂移和角速率置為零．
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;
}
五、參考文獻(xiàn)及博客
[1]儲開斌, 趙爽, 馮成濤. 基于Mahony-EKF的無人機姿態(tài)解算算法[J]. 電子測量與儀器學(xué)報, 2020, 32(12):7.
[2]Valenti, Roberto G , Dryanovski, et al. Sensors, Vol. 15, Pages 19302-19330: Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs. 2015.
[3]米剛, 田增山, 金悅,等. 基于MIMU和磁力計的姿態(tài)更新算法研究[J]. 傳感技術(shù)學(xué)報, 2015, 28(1):6.
[4]姿態(tài)估計（互補濾波）文章來源地址http://www.zghlxwxcb.cn/news/detail-621893.html
到了這里，關(guān)于PX4代碼解析(6)的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
