一、開篇

? ? ? ? 終于到ardupilot源代碼的姿態(tài)解算了，有了前期關(guān)于mahony姿態(tài)解算算法的基礎(chǔ)以后，理解源代碼的姿態(tài)解算算法就快多了，所有的東西都在腦海中初步有了一個(gè)框架；首先要做什么，然后再做什么，再然后捏~~~反正容易上手的。

? ? ? ? 2016.04.04日晚，別人都在嗨，而我卻在實(shí)驗(yàn)室苦逼的工作著，今晚最大的收獲就是發(fā)現(xiàn)了“新大陸”-----“北航可靠飛行控制研究組”，其喜悅之情絕不亞于哥倫布發(fā)現(xiàn)新大陸。他們才是專業(yè)的啊，看看他們畢業(yè)生的去向，不是研究所就是出國深造，好吧，人家才是專業(yè)搞科研的，不食人間煙火，那么問題來了：DJI的員工都是在哪招來的呢？！哎，我是俗人一個(gè)，經(jīng)濟(jì)基礎(chǔ)決定上層建筑，好好學(xué)習(xí)才能掙大錢。最近他們開設(shè)了一門課程《多旋翼飛行器設(shè)計(jì)與控制》，課程體系安排的非常好，現(xiàn)在更新到第四講了（聽北航一個(gè)博士說只有PPT沒有視頻，感謝Mallin的幫助，成功打入內(nèi)部），PPT也足夠了，相當(dāng)上檔次啊，課程到2016.06.30結(jié)束，正好可以把無人機(jī)的整個(gè)架構(gòu)理解完，找工作去~~~

? ? ? ? 在下面的基礎(chǔ)知識(shí)部分先分享一部分“北航”的研究成果，特別是氣動(dòng)方面的，以前一點(diǎn)概念都沒有，只看著超跑的流線型非常炫酷，不知其原因，特此記錄，大家共勉。

三、實(shí)驗(yàn)平臺(tái)

Software?Version：PX4Firmware

Hardware Version：pixhawk

IDE：eclipse Juno （Windows）

四、基礎(chǔ)知識(shí)（均來自北航可靠飛行控制研究組）

1、無人機(jī)飛行的氣動(dòng)模型與分析

? ? ? ? 1）多旋翼前飛情形：在下圖中，因?yàn)槁菪龢娜嵝?#xff0c;誘導(dǎo)的來流會(huì)產(chǎn)生阻力。

? ? ? ? 如果多旋翼重心在槳盤平面下方，那么阻力形成的力矩會(huì)促使多旋翼俯仰角轉(zhuǎn)向0度方向。

? ? ? ? 如果多旋翼重心在槳盤平面上方，那么阻力形成的力矩會(huì)促使多旋翼俯仰角朝發(fā)散方向發(fā)展，直至翻轉(zhuǎn)。因此，當(dāng)多旋翼前飛時(shí)，重心在槳盤平面的下方會(huì)使前飛運(yùn)動(dòng)穩(wěn)定。

? ? ? ? 2）多旋翼風(fēng)干擾情形：在下圖中，當(dāng)陣風(fēng)吹來，因?yàn)槁菪龢娜嵝?#xff0c;誘導(dǎo)的來流會(huì)在產(chǎn)生阻力。

? ? ? ? 如果多旋翼重心在下，那么阻力形成的力矩會(huì)促使多旋翼俯仰角朝發(fā)散的方向發(fā)展，直至翻轉(zhuǎn)。

? ? ? ? 如果多旋翼重心在上，那么阻力形成的力矩會(huì)促使多旋翼俯仰超0度方向發(fā)展。因此，當(dāng)多旋翼受到外界風(fēng)干擾時(shí)，重心在槳盤平面的上方可以抑制擾動(dòng)。

? ? ? ? 3）綜上所述：無論重心在槳盤平面的上方或下方都不能使多旋翼穩(wěn)定。因此需要通過反饋控制將多旋翼平衡。然而，如果重心在槳盤平面很靠上的位置，會(huì)使多旋翼某個(gè)運(yùn)動(dòng)模態(tài)很不穩(wěn)定。因此，實(shí)際中建議將重心配置在飛行器槳盤周圍，可以稍微靠下。這樣控制器控制起來更容易些。

2、氣動(dòng)布局

? ? ? ? 對(duì)外形進(jìn)行設(shè)計(jì)主要是為了降低飛行時(shí)的阻力。按其產(chǎn)生的原因不同可分為

（1）摩擦阻力

（2）壓差阻力

（3）誘導(dǎo)阻力

（4）干擾阻力

? ? ? ? 要減少這些阻力，需要妥善考慮和安排各部件之間的相對(duì)位置關(guān)系，部件連接處盡量圓滑過渡，減少漩渦產(chǎn)生。

? ? ? ? 因此它與物體的迎風(fēng)面積有很大關(guān)系，迎風(fēng)面積越大，壓差阻力也越大。物體的形狀也對(duì)壓差阻力影響很大。如上圖所示的三個(gè)物體，圓盤的壓差阻力最大，球體次之，而流線體的最小，就壓差阻力而言可以是平板壓差阻力的1/20。

? ? ? ? 設(shè)計(jì)建議：（法拉利、保馳捷等超跑的流線型車就是很好的榜樣，寶馬 Z4也可以，奔馳Smart就太low了，差點(diǎn)忘記特斯拉了）

（1）需要考慮多旋翼前飛時(shí)的傾角，減少最大迎風(fēng)面積。

（2）并設(shè)計(jì)流線型機(jī)身。

（3）考慮和安排各部件之間的相對(duì)位置關(guān)系，部件連接處盡量圓滑過渡，飛機(jī)表面也要盡量光滑。

（4）通過CFD仿真（Computational Fluid Dynamics：計(jì)算流體動(dòng)力學(xué)）計(jì)算阻力系數(shù)，不斷優(yōu)化。

? ? ? ? 昨天見到了零度的四旋翼，外形設(shè)計(jì)的就是好，可惜只靠外形還是干不過DJI的，加油吧，零度。

五、正文（源代碼的姿態(tài)解算算法）

1、進(jìn)程入口：voidAttitudeEstimatorQ::task_main()

? ? ? ? 1）訂閱所需要的topics，注意sensor_combined，傳感器數(shù)據(jù)都是靠它來的。

? ? ? ? 在訂閱時(shí)使用ORB_ID(sensor_combined)獲取ID號(hào)，該ID號(hào)代表了從topic_name到topicmetadata structure name之間的轉(zhuǎn)換橋梁。在task_main()的初始部分使用uORB模型的orb_subscribe()函數(shù)獲取在sensors.cpp中通過orb_advertise()函數(shù)廣播的傳感器信息（sensor_combined）。由此說明在使用之前需要通過orb_advertise()函數(shù)之后才能在需要其數(shù)據(jù)的地方使用orb_subscribe()獲取。如果沒有使用，訂閱者可以訂閱，但是接收不到有效數(shù)據(jù)。

? ? ? ? 關(guān)于uOBR模型的不再贅述，詳細(xì)介紹參看：http://blog.csdn.net/freeape/article/details/46880637

和http://www.pixhawk.com/start?id=zh%2Fdev%2Fshared_object_communication&go

? ? ? ? px4_poll(fds,1, 1000)：配置阻塞時(shí)間，1ms讀取一次sensor_combined的數(shù)據(jù)。

? ? ? ? 必須有了orb_advertise()函數(shù)和orb_subscribe()函數(shù)（通過它獲取orb_copy()函數(shù)中的參數(shù)handle）之后才可以使用orb_copy(ORB_ID(sensor_combined),_sensors_sub, &sensors)函數(shù)獲取sensors的實(shí)際有效數(shù)據(jù)。

? ? ? ? 首先是讀取gyro的數(shù)據(jù)：（有個(gè)特別的地方就是把陀螺儀的值先積分然后再微分，這個(gè)其實(shí)就是求平均_2016.05.27補(bǔ)充）

[plain] view plain copy

float?gyro[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];??

????????}??

}??

? ? ? ? 然后以同樣的方法讀取accel和mag的數(shù)據(jù)。

[plain] view plain copy

Void?DataValidatorGroup::put(unsigned?index,?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);//goto??

????????????????break;??

????????????????}??

????????????next?=?next->sibling();//下一組數(shù)據(jù)??

????????????i++;??

????????????}??

}??

? ? ? ? 最終還是goto到put函數(shù)。

[plain] view plain copy

Void?DataValidator::put(uint64_t?timestamp,?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++)?{//_dimensions=3??

????????if?(_time_last?==?0)?{//時(shí)間變量已經(jīng)初始化為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;??

????????????}??

????????}??

????????//?XXX?replace?with?better?filter,?make?it?auto-tune?to?update?rate??

????????_lp[i]?=?_lp[i]?*?0.5f?+?val[i]?*?0.5f;??

????????_value[i]?=?val[i];??

????}??

????_time_last?=?timestamp;??

}??

? ? ? ? 詳細(xì)介紹使用方法：主要是將三類參數(shù)分別建立相應(yīng)的 DataValidatorGroup類來對(duì)數(shù)據(jù)進(jìn)行處理。

? ? ? ? DataValidatorGroup類： _voter_gyro、_voter_accel、_voter_mag

???????? 調(diào)用方法：

???????? 1）首先gyro、accel、mag每次讀取數(shù)據(jù)都是三組三組的讀取

???????? 2）先將每組的數(shù)據(jù)（例如gyro將三個(gè)維度的的傳感器數(shù)據(jù)put入（如_voter_gyro.put(...))）DataValidatorGroup中，并goto到DataValidator::put函數(shù)

???????? 3）在DataValidator函數(shù)中計(jì)算數(shù)據(jù)的誤差、平均值、并進(jìn)行濾波。

??? 濾波入口的put函數(shù)：

???????? val=傳感器讀取的數(shù)據(jù)

???????? _lp=濾波器的系數(shù)（lowpass value）

???????? 初始化：由上圖可知當(dāng)?shù)谝淮巫x到傳感器數(shù)據(jù)時(shí)_mean和_M2置0，_lp=val；

???????? lp_val= val - _lp

???????? delta_val= lp_val - _mean

???????? _mean= (平均值）每次數(shù)據(jù)讀取時(shí)，每次val和_lp的差值之和的平均值 _mean[i] += delta_val / _event_count

???????? _M2= （均方根值）delta_val * (lp_val - _mean)的和

???????? _rms= 均方根值sqrtf(_M2[i] / (_event_count - 1))

???????? 優(yōu)化濾波器系數(shù)：_lp[i]= _lp[i] * 0.5f + val[i] * 0.5f

???????? _value= val ：get_best()函數(shù)的最后調(diào)用該結(jié)果(通過比較三組數(shù)據(jù)的confidence大小決定是否選取）。

??? 濾波器的confidence函數(shù)（信任度函數(shù)，貌似模糊控制理論有個(gè)隸屬函數(shù)，應(yīng)該類似的功能）：返回值是對(duì)上N次測(cè)量的驗(yàn)證的信任程度，其值在0到1之間，越大越好。返回值是返回上N次測(cè)量的誤差診斷，用于get_best函數(shù)選擇最優(yōu)值，選擇的方法如下：

?Switch if:

???????? 1)the confidence is higher and priority is equal or higher

???????? 2)the confidence is no less than 1% different and the priority is higher

2、根據(jù)_voter_gyro、_voter_accel、_voter_mag三個(gè)參數(shù)的failover_count函數(shù)判斷是否存在數(shù)據(jù)獲取失誤問題，并通過mavlink協(xié)議顯示錯(cuò)誤原因。

3、根據(jù)_voter_gyro、_voter_accel、_voter_mag三個(gè)參數(shù)的get_vibration_factor函數(shù)判斷是否有震動(dòng)現(xiàn)象，返回值是float型的RSM值，其代表振動(dòng)的幅度大小。

[plain] view plain copy

Float?DataValidatorGroup::get_vibration_factor(uint64_t?timestamp)??

{??

????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;//返回DataValidatorGroup中的三組中的最大的振動(dòng)值??

}??

? ? ? ? 將rms變量（原來put函數(shù)中的_rms）傳回主函數(shù)中和_vibration_warning_threshold進(jìn)行判斷。

4、通過uORB模型獲取vision和mocap的數(shù)據(jù)（視覺和mocap）

[plain] view plain copy

//?Update?vision?and?motion?capture?heading??

????????bool?vision_updated?=?false;??

????????orb_check(_vision_sub,?&vision_updated);??

????????bool?mocap_updated?=?false;??

????????orb_check(_mocap_sub,?&mocap_updated);??

????????if?(vision_updated)?{??

????????????orb_copy(ORB_ID(vision_position_estimate),?_vision_sub,?&_vision);??

????????????math::Quaternion?q(_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::Quaternion?q(_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;??

????????}</span><span?style="font-size:14px;">??

5、位置加速度獲取（position，注意最后在修正時(shí)會(huì)用到該處的_pos_acc）（484）

? ? ? ? 首先檢測(cè)是否配置了自動(dòng)磁偏角獲取，如果配置了則用當(dāng)前的經(jīng)緯度（longitude and latitude）通過get_mag_declination(_gpos.lat,_gpos.lon)函數(shù)獲取當(dāng)前位置的磁偏角（magnetic declination），實(shí)現(xiàn)過程就是一系列的算算算，自己跟蹤源碼看吧，用地面站校準(zhǔn)羅盤的應(yīng)該比較熟悉這個(gè)。然后就是獲取機(jī)體的速度，通過速度計(jì)算機(jī)體的加速度。

[plain] view plain copy

bool?gpos_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?*/??

????????????????_mag_decl?=?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)?{??

????????????????????float?vel_dt?=?(_gpos.timestamp?-?_vel_prev_t)?/?1000000.0f;??

????????????????????/*?calculate?acceleration?in?body?frame?：速度之差除時(shí)間*/??

????????????????????_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;??

????????}??

6、update函數(shù)（528行）

? ? ? ? 接下來就是528行的updata函數(shù)了，非常重要，這個(gè)函數(shù)主要用于對(duì)四元數(shù)向量_q進(jìn)行初始化賦值或者更新。

? ? ? ? --------------------------------------------------------------------------------------------------------------------------

? ? ? ? 首先判斷是否是第一次進(jìn)入該函數(shù)，第一次進(jìn)入該函數(shù)先進(jìn)入init函數(shù)初始化，源碼如下。

[plain] view plain copy

bool?AttitudeEstimatorQ::init()??

{??

????//?Rotation?matrix?can?be?easily?constructed?from?acceleration?and?mag?field?vectors??

????//?'k'?is?Earth?Z?axis?(Down)?unit?vector?in?body?frame??

????Vector<3>?k?=?-_accel;??

????k.normalize();??

????//?'i'?is?Earth?X?axis?(North)?unit?vector?in?body?frame,?orthogonal?with?'k'??

????Vector<3>?i?=?(_mag?-?k?*?(_mag?*?k));??

????i.normalize();??

????//?'j'?is?Earth?Y?axis?(East)?unit?vector?in?body?frame,?orthogonal?with?'k'?and?'i'??

????Vector<3>?j?=?k?%?i;??

????//?Fill?rotation?matrix??

????Matrix<3,?3>?R;??

????R.set_row(0,?i);??

????R.set_row(1,?j);??

????R.set_row(2,?k);??

????//?Convert?to?quaternion??

????_q.from_dcm(R);??

????_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;??

}??

? ? ? ? 初始化方法：

? ? ? ? k= -_accel 然后歸一化k，k為加速度傳感器測(cè)量到加速度方向向量，由于第一次測(cè)量數(shù)據(jù)時(shí)無人機(jī)一般為平穩(wěn)狀態(tài)無運(yùn)動(dòng)狀態(tài)，所以可以直接將測(cè)到的加速度視為重力加速度g，以此作為dcm旋轉(zhuǎn)矩陣的第三行k（這個(gè)介紹過了）。

? ? ? ? i= （_mag - k * (_mag * k)) _mag向量指向正北方，k*(_mag*k) 正交correction值，對(duì)于最終的四元數(shù)歸一化以后的范數(shù)可以在正負(fù)5%以內(nèi)；感覺不如《DCM IMU:Theory》中提出的理論“強(qiáng)制正交化”修正的好，Renormalization算法在ardupilot的上層應(yīng)用AP_AHRS_DCM中使用到了。

? ? ? ? j= k%i ：外積、叉乘。關(guān)于上面的Vector<3>k = -_accel，Vector<3>相當(dāng)于一個(gè)類型（int）定義一個(gè)變量k，然后把-_accel負(fù)值給k，在定義_accel時(shí)也是使用Vector<3>，屬于同一種類型的，主要就是為了考慮其實(shí)例化過程（類似函數(shù)重載）。

? ? ? ? 然后以模板的形式使用“Matrix<3, 3> R”建立3x3矩陣R，通過set_row()函數(shù)賦值。

[plain] view plain copy

/**??

?*?set?row?from?vector??

?*/??

void?set_row(unsigned?int?row,?const?Vector<N>?v)?{??

????for?(unsigned?i?=?0;?i?<?N;?i++)?{??

????????data[row][i]?=?v.data[i];??

????}??

}??

? ? ? ? 第一行賦值i 向量指向北，第二行賦值j 向量指向東，第三行賦值k向量指向DOWN。然后就是把DCM轉(zhuǎn)換為四元數(shù)_q （使用from_dcm），并歸一化四元數(shù)，一定要保持歸一化的思想。來一個(gè)比較牛掰的求范數(shù)的倒數(shù)的快速算法（mahony的算法實(shí)現(xiàn)用到的）：

[plain] view plain copy

float?invSqrt(float?x)?{??

????float?halfx?=?0.5f?*?x;??

????float?y?=?x;??

????long?i?=?*(long*)&y;??

????i?=?0x5f3759df?-?(i>>1);??

????y?=?*(float*)&i;??

????y?=?y?*?(1.5f?-?(halfx?*?y?*?y));??

????return?y;??

}??

? ? ? ? 具體為什么這么實(shí)現(xiàn)的還是看wiki連接吧：https://en.wikipedia.org/wiki/Fast_inverse_square_root

? ? ? ? 其中DCM轉(zhuǎn)四元數(shù)的算法如下，tr>0時(shí)比較好理解，別的情況被簡寫的看不透了。后續(xù)給出原始代碼便于理解。

[plain] view plain copy

tr?=?dcm.data[0][0]?+?dcm.data[1][1]?+?dcm.data[2][2]??

????如果?tr>0??

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 單位化結(jié)束初始化。PS：另外根據(jù)DCM求取四元數(shù)的算法是參考MartinJohn Baker的，就是上述的原始版，該算法在AP_Math/quaternion.cpp中，鏈接：

http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm

? ? ? ? 源碼如下。

[plain] view plain copy

void?Quaternion::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;??

????}??

}??

? ? ? ? ---------------------------------------------------------------------------------------------------------------------------------? ? ? ? ??

? ? ? ? 如果不是第一次進(jìn)入該函數(shù)，則判斷是使用什么mode做修正的，比如vision、mocap、acc和mag（DJI精靈4用的視覺壁障應(yīng)該就是這個(gè)vision），Hdg就是heading。

[plain] view plain copy

if?(_ext_hdg_mode?>?0?&&?_ext_hdg_good)?{??

????????if?(_ext_hdg_mode?==?1)?{??

????????????//?Vision?heading?correction??

????????????//?Project?heading?to?global?frame?and?extract?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?and?extract?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;??

????????}??

????}??

? ? ? ? _ext_hdg_mode== 1、2時(shí)都是利用vision數(shù)據(jù)和mocap數(shù)據(jù)對(duì)gyro數(shù)據(jù)進(jìn)行修正，下面的global frame就是所謂的earthframe。

? ? ? ? ?---------------------------------------------------------------------------------------------------------------------------------

? ? ? ? _ext_hdg_mode== 0利用磁力計(jì)修正。

[plain] view plain copy

if?(_ext_hdg_mode?==?0??||?!_ext_hdg_good)?{??

????????//?Magnetometer?correction??

????????//?Project?mag?field?vector?to?global?frame?and?extract?XY?component??

????????Vector<3>?mag_earth?=?_q.conjugate(_mag);?//b系到n系??

????????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;?//n系到b系??

????}??

? ? ? ? _w_mag為mag的權(quán)重。PS：發(fā)現(xiàn)一個(gè)規(guī)律，在不太理解C++的情況下看代碼的算法過程中經(jīng)常性的不知道某行代碼是做什么的，在哪定義的，比如這個(gè)Vector<3>，前面已經(jīng)介紹過它了，只要有它這樣的做前綴的，后面的變量就是類似int定義一個(gè)變量一樣，幾乎都在PX4Firmware/src/lib/mathlib/math/Quaternion.hpp中，進(jìn)行實(shí)例展開的。磁力計(jì)數(shù)據(jù)為_mag。mag_earth= _q.conjugate(_mag)，這行代碼的含義為先將歸一化的_q的旋轉(zhuǎn)矩陣R（b系轉(zhuǎn)n系）乘以_mag向量（以自身機(jī)體坐標(biāo)系為視角看向北方的向量表示），得到n系（NED坐標(biāo)系）下的磁力計(jì)向量。如下就是conjugate函數(shù)，其過程就是實(shí)現(xiàn)從b系到n系的轉(zhuǎn)換，使用左乘。

[plain] view plain copy

/**??

?????*?conjugation//b系到n系??

?????*/??

????Vector<3>?conjugate(const?Vector<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)??

????????);??

????}??

????mag_err?=?_wrap_pi(atan2f(mag_earth(1),?mag_earth(0))?-?_mag_decl);??

? ? ? ? 只考慮Vector<3> mag_earth中的前兩維的數(shù)據(jù)mag_earth(1)和mag_earth(0)（即x、y，忽略z軸上的偏移），通過arctan（mag_earth(1),mag_earth(0)）得到的角度和前面根據(jù)經(jīng)緯度獲取的磁偏角做差值得到糾偏誤差角度mag_err ，_wrap_pi函數(shù)是用于限定結(jié)果-pi到pi的函數(shù)，源碼如下。

[plain] view plain copy

__EXPORT?float?_wrap_pi(float?bearing)??

{??

????/*?value?is?inf?or?NaN?*/??

????if?(!isfinite(bearing))?{??

????????return?bearing;??

????}??

????int?c?=?0;??

//大于pi則與2pi做差取補(bǔ)??

????while?(bearing?>=?M_PI_F)?{//M_PI_F==3.14159265358979323846f??

????????bearing?-=?M_TWOPI_F;//M_TWOPI_F==2*M_PI_F??

????????if?(c++?>?3)?{??

????????????return?NAN;//NaN==not?a?number??

????????}??

????}??

????c?=?0;??

//小于-pi則與2pi做和取補(bǔ)??

????while?(bearing?<?-M_PI_F)?{??

????????bearing?+=?M_TWOPI_F;??

????????if?(c++?>?3)?{??

????????????return?NAN;??

????????}??

????}??

????return?bearing;??

}??

類似的約束函數(shù)都在src/lib/geo/geo.c中，比如：??

__EXPORT?float?_wrap_180(float?bearing);??

__EXPORT?float?_wrap_360(float?bearing);??

__EXPORT?float?_wrap_pi(float?bearing);??

__EXPORT?float?_wrap_2pi(float?bearing);??

? ? ? ? ?corr +=_q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;//n系到b系

? ? ? ? ?計(jì)算corr值等于單位化的旋轉(zhuǎn)矩陣R（b系轉(zhuǎn)n系）的轉(zhuǎn)置（可以理解為 R（n系轉(zhuǎn)b系））乘以（0,0，-mag_err），相當(dāng)于機(jī)體坐標(biāo)系繞地理坐標(biāo)系N軸（Z軸）轉(zhuǎn)動(dòng)arctan（mag_earth(1), mag_earth(0)）度。

? ? ? ? ?關(guān)于磁還需要更深入的了解，看相關(guān)論文吧，一大推~~~

? ? ? ? ?------------------------------------------------------------------------------------------------------------------------------

? ? ? ? 加速度計(jì)修正

? ? ? ? 首先就是把歸一化的n系重力加速度通過旋轉(zhuǎn)矩陣R左乘旋轉(zhuǎn)到b系，即k為歸一化的旋轉(zhuǎn)矩陣R（b-e）的第三行，代碼如下。

[plain] view plain copy

//?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));//?n系到b系??

????//?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))??

????);??

? ? ? ? 上面這段代碼是不是很熟悉，還記得mahony算法中的的計(jì)算過程么？！都是一樣的，這里只是換種方式（C++）表達(dá)，不在贅述。

? ? ? ? 下面這個(gè)比較重要：

? ? ? ??corr += (k %(_accel - _pos_acc).normalized()) * _w_accel。

? ? ? ? {k%（_accel“加速度計(jì)的測(cè)量值”-位移加速度）的單位化）<約等于重力加速度g>}*權(quán)重。

? ? ? ? 關(guān)于這個(gè)“_accel-_pos_acc”的含義：

? ? ? ? 總的受到合力的方向（_accel）減去機(jī)體加速度方向（_pos_acc）得到g的方向，即總加速度（加速度獲取）減去機(jī)體運(yùn)動(dòng)加速度（第五部分）獲取重力加速度，然后姿態(tài)矩陣的不是行就是列來與純重力加速度來做叉積，算出誤差。因?yàn)檫\(yùn)動(dòng)加速度是有害的干擾，必須減掉。算法的理論基礎(chǔ)是[0,0,1]與姿態(tài)矩陣相乘。該差值獲取的重力加速度的方向是導(dǎo)航坐標(biāo)系下的z軸，加上運(yùn)動(dòng)加速度之后，總加速度的方向就不是與導(dǎo)航坐標(biāo)系的天或地平行了，所以要消除這個(gè)誤差，即“_accel-_pos_acc”。然后叉乘z軸向量得到誤差，進(jìn)行校準(zhǔn) 。

? ? ? ? 關(guān)于%運(yùn)算符在vector.hpp中介紹了其原型：

[plain] view plain copy

Vector<3>?operator?%(const?Vector<3>?&v)?const?{??

????????return?Vector<3>(??

???????????????????data[1]?*?v.data[2]?-?data[2]?*?v.data[1],??

???????????????????data[2]?*?v.data[0]?-?data[0]?*?v.data[2],??

???????????????????data[0]?*?v.data[1]?-?data[1]?*?v.data[0]??

???????????????);??

????}??

};??

? ? ? ?“ %”其實(shí)就是求向量叉積，叉積公式如下。

? ? ? ? ---------------------------------------------------------------------------------------------------------------------------------

? ? ? ? _gyro_bias+= corr * (_w_gyro_bias * dt);PI控制器中的I（積分）效果。然后對(duì)_gyro_bias做約束處理。

[plain] view plain copy

for?(int?i?=?0;?i?<?3;?i++)?{??

????????_gyro_bias(i)?=?math::constrain(_gyro_bias(i),?-_bias_max,?_bias_max);??

????}??

? ? ? ? 對(duì)偏移量進(jìn)行約束處理，源碼寫的相當(dāng)好啊，簡單易懂，其函數(shù)原型是：

[plain] view plain copy

uint64_t?__EXPORT?constrain(uint64_t?val,?uint64_t?min,?uint64_t?max)??

{??

??????return?(val?<?min)???min?:?((val?>?max)???max?:?val);??

}??

? ? ? ? ---------------------------------------------------------------------------------------------------------------------------------

? ? ? ? 最后就是使用修正的數(shù)據(jù)更新四元數(shù)，并把_rates和_gyro_bias置零便于下次調(diào)用時(shí)使用。

[plain] view plain copy

_rates?=?_gyro?+?_gyro_bias;?//得到經(jīng)過修正后的角速度??

????//?Feed?forward?gyro??

????corr?+=?_rates;//PI控制器的體現(xiàn)??

????//?Apply?correction?to?state??

????_q?+=?_q.derivative(corr)?*?dt;//736??

????//?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;??

? ? ? ? 上面736行的_q += _q.derivative(corr) * dt非常重要，又用到了微分方程離散化的思想。以前講過DCM矩陣更新過程中也是用到了該思想。先看看代碼，有點(diǎn)怪，怪就怪在derivative（衍生物）這個(gè)名字上，平時(shí)一大推的論文和期刊上面都是用的omga *Q 的形式，而這里的代碼實(shí)現(xiàn)確是用的Q * omga的形式，所以構(gòu)造的4*4矩陣的每一列的符號(hào)就不一樣了。

[plain] view plain copy

/***?derivative*/??

????const?Quaternion?derivative(const?Vector<3>?&w)?{??

????????float?dataQ[]?=?{??

????????????data[0],?-data[1],?-data[2],?-data[3],??

????????????data[1],??data[0],?-data[3],??data[2],??

????????????data[2],??data[3],??data[0],?-data[1],??

????????????data[3],?-data[2],??data[1],??data[0]??

????????};??

????????Matrix<4,?4>?Q(dataQ);??

????????Vector<4>?v(0.0f,?w.data[0],?w.data[1],?w.data[2]);??

????????return?Q?*?v?*?0.5f;??

????}??

? ? ? ? 這一部分理論基礎(chǔ)在《捷聯(lián)慣性導(dǎo)航技術(shù)》中有詳細(xì)介紹，關(guān)于DCM隨時(shí)間傳遞的推導(dǎo)過程、四元數(shù)隨時(shí)間傳遞的推導(dǎo)以及DCM、歐拉角、四元數(shù)之間的相互關(guān)系都有詳細(xì)的介紹。

? ? ? ? 總結(jié)一下：corr包含磁力計(jì)修正、加速度計(jì)修正、（vision、mocap修正）、gyro中測(cè)量到的角速度偏轉(zhuǎn)量，且因?yàn)閏orr為update函數(shù)中定義的變量，所以每次進(jìn)入update函數(shù)中時(shí)會(huì)刷新corr變量的數(shù)據(jù)； _rate也會(huì)刷新其中的數(shù)據(jù)，含義為三個(gè)姿態(tài)角的角速度（修正后）； _q為外部定義的變量，在這個(gè)函數(shù)中只會(huì)+=不會(huì)重新賦值，如果計(jì)算出現(xiàn)錯(cuò)誤會(huì)返回上一次計(jì)算出的_q。

? ? ? ? ---------------------------------------------------------------------------------------------------------------------------------

7、將_q轉(zhuǎn)換成歐拉角euler并發(fā)布（532）

? ? ? ? 終于從updata函數(shù)出來了，它還是相當(dāng)重要的，主要就是它了，需要深入的理解透徹，下面就是些小角色了。Updata函出來就是直接用一更新的四元數(shù)（_q）求出對(duì)于的歐拉角，以便在控制過程中實(shí)現(xiàn)完美的控制，控制還是需要用直接明了的歐拉角。上源碼：

[plain] view plain copy

Vector<3>?euler?=?_q.to_euler();??

????????struct?vehicle_attitude_s?att?=?{};??

????????att.timestamp?=?sensors.timestamp;??

????????att.roll?=?euler(0);//獲取的歐拉角賦值給roll、pitch、yaw??

????????att.pitch?=?euler(1);??

????????att.yaw?=?euler(2);??

??

????????att.rollspeed?=?_rates(0);?//獲取roll、pitch、yaw得旋轉(zhuǎn)速度??

????????att.pitchspeed?=?_rates(1);??

????????att.yawspeed?=?_rates(2);??

????????for?(int?i?=?0;?i?<?3;?i++)?{??

????????????att.g_comp[i]?=?_accel(i)?-?_pos_acc(i);???

//獲取導(dǎo)航坐標(biāo)系的重力加速度，前面已介紹過??

????????}??

? ? ? ? 比較重要的就是如何由四元數(shù)獲取歐拉角：_q.to_euler()

[plain] view plain copy

/**??

?????*?create?Euler?angles?vector?from?the?quaternion??

?????*/??

????Vector<3>?to_euler()?const?{??

????????return?Vector<3>(??

????????????atan2f(2.0f?*?(data[0]?*?data[1]?+?data[2]?*?data[3]),?1.0f?-?2.0f?*?(data[1]?*?data[1]?+?data[2]?*?data[2])),??

????????????asinf(2.0f?*?(data[0]?*?data[2]?-?data[3]?*?data[1])),??

????????????atan2f(2.0f?*?(data[0]?*?data[3]?+?data[1]?*?data[2]),?1.0f?-?2.0f?*?(data[2]?*?data[2]?+?data[3]?*?data[3]))??

????????);??

????}??

? ? ? ? 下面的就比較好理解了，不在贅述。

[plain] view plain copy

if?(_att_pub?==?nullptr)?{??

????????????_att_pub?=?orb_advertise(ORB_ID(vehicle_attitude),?&att);??

????????}?else?{??

????????????orb_publish(ORB_ID(vehicle_attitude),?_att_pub,?&att);??

????????}??

????????struct?control_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));??

????????/*?Publish?to?control?state?topic?*/??

????????if?(_ctrl_state_pub?==?nullptr)?{??

????????????_ctrl_state_pub?=?orb_advertise(ORB_ID(control_state),?&ctrl_state);??

????????}?else?{??

????????????orb_publish(ORB_ID(control_state),?_ctrl_state_pub,?&ctrl_state);??

????????}??

六、總結(jié)
? ? ? ? 通過上面的分析對(duì)ardupilot源代碼中的姿態(tài)解算算法有了深一步的了解，還有一部分就是關(guān)于mag的，還需要看一些論文，磁是最不容易處理的，極易受到外部干擾。還有就是加速度計(jì)的數(shù)據(jù)，對(duì)震動(dòng)比較敏感，所以在無人機(jī)裝機(jī)時(shí)需要在pixhawk下面安裝減震板，做減震處理。
? ? ? ? 基本的姿態(tài)解算和飛行控制靠陀螺儀和加速計(jì)等肯定可以實(shí)現(xiàn)，代碼也比較好寫，可以基于mahony的那套算法，硬件部分就是選定芯片，確定通信接口；然后移植一套免費(fèi)的OS即可。但是難得就是飛行時(shí)的穩(wěn)定性和對(duì)震動(dòng)和噪聲的處理問題，這些都是最細(xì)節(jié)最重要的部分，也是最難的部分。
? ? ? ? 接下來就是關(guān)于姿態(tài)控制的了，至少一個(gè)多禮拜的時(shí)間才能搞明白吧；att_estimate、pos_estimate和att_control、pos_control這四個(gè)部分全部搞定需要下很大的功夫，慢慢搞吧，看論文吧，站在別人的肩膀上，加油~~~ ? ??
? ? ? ? 感謝Mr、青龍等群友的幫助~~~

版權(quán)聲明：. https://blog.csdn.net/qq_21842557/article/details/51058206

總結(jié)

以上是生活随笔為你收集整理的Pixhawk之姿态解算篇（3）_源码姿态解算算法分析的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。