第九章 PX4-pixhawk-姿態(tài)估計(jì)解析?
這一章節(jié)我們對(duì)姿態(tài)估計(jì)進(jìn)行解析，這一章節(jié)涉及到算法，主要涉及到的還是DCM(方向余弦)算法。首先我們還從啟動(dòng)文件開(kāi)始進(jìn)行講解。?
我們找到rc.mc_apps中，我們可以找到SYS_MC_EST_GROUP的參數(shù)為0，所以我們對(duì)attitude_estimator_q.cpp進(jìn)行講解? 。?
回到attitude_estimator_q.cpp文件中，這里提一下這個(gè)文件應(yīng)該知道是怎么編譯的吧，在cmake文件中找到的。找到入口函數(shù)attitude_estimator_q_main中。我們可以知道參數(shù)只有start，這個(gè)怎么解析的不用說(shuō)了吧，前幾章節(jié)都說(shuō)到過(guò)了。 ?
這里面沒(méi)有什么好說(shuō)的，new這個(gè)就不用說(shuō)了吧，我們找到start函數(shù) ?這里就創(chuàng)建了任務(wù)task_main_trampoline? 再找到task_main函數(shù)。?
首先還是先訂閱相關(guān)數(shù)據(jù)，包括傳感器、視覺(jué)、空速計(jì)、參數(shù)、定位信息并且更新參數(shù) ?，這里的更新各個(gè)參數(shù)的函數(shù)就不用多說(shuō)了吧，主要是濾波權(quán)重、偏移量和模式選擇等等。?
接著到wihile中，首先還是poll函數(shù)來(lái)防止阻塞，這個(gè)用法直接去度一下poll就有說(shuō)明。首先進(jìn)行參數(shù)更新檢測(cè)? ，這個(gè)沒(méi)什么好說(shuō)的，如果不懂那我也沒(méi)辦法了哦。然后就是更新傳感器數(shù)據(jù)? ，這個(gè)更新里面有加速度計(jì)和加速度計(jì)的更新，而且這里還對(duì)它們進(jìn)行了一個(gè)二階低通濾波，這種濾波器比較普遍也比較容易，有興趣的同學(xué)去研究一下，不懂得郵件我也行，如果大家有更高明的濾波算法也可以分享一下，我也可以代勞來(lái)編寫一下。接著對(duì)磁力計(jì)更新。接下來(lái)的幾個(gè)更新我們不做細(xì)去說(shuō)明。視覺(jué)更新 動(dòng)作捕捉更新 ?空速計(jì)數(shù)據(jù)更新? GPS數(shù)據(jù)更新 ?這里的GPS的更新數(shù)據(jù)主要還是計(jì)算磁偏角用來(lái)修正電子羅盤用的，這個(gè)的計(jì)算主要是利用經(jīng)緯度通過(guò)查表發(fā)進(jìn)行查找磁偏角。接著就是利用從GPS中獲取的NED坐標(biāo)系的速度來(lái)計(jì)算加速度 ?這里有個(gè)利用四元素來(lái)進(jìn)行坐標(biāo)系的轉(zhuǎn)換_q.conjugate_inversed這個(gè)就是將數(shù)據(jù)轉(zhuǎn)換成機(jī)體坐標(biāo)系中，至于這個(gè)函數(shù)的如何去解析，我們待會(huì)去一起解析。?
然后就是進(jìn)入到最重要的函數(shù)update，這個(gè)函數(shù)就涉及到姿態(tài)解算了。我們進(jìn)入到這個(gè)函數(shù)中 ?。函數(shù)開(kāi)始會(huì)對(duì)是否初始化進(jìn)行判斷，如果是第一次運(yùn)行就會(huì)運(yùn)行init函數(shù)，運(yùn)行一次后就不會(huì)運(yùn)行了。這里的初始化主要是構(gòu)建旋轉(zhuǎn)矩陣，然后通過(guò)旋轉(zhuǎn)矩陣轉(zhuǎn)換為四元素。這里主要是運(yùn)用了較多的矢量運(yùn)算和旋轉(zhuǎn)矩陣的一些知識(shí)。?
首先要做的是加速率的偏移調(diào)整，這里我們不用視覺(jué)和動(dòng)作捕捉，所以不對(duì)它們進(jìn)行講解，這些主要是對(duì)航向角進(jìn)行修正。接下來(lái)是利用電子羅盤數(shù)據(jù)進(jìn)行航向修正，這里的到的修正量是yaw。?
?
這里講一下_q.conjugate函數(shù)這里是坐標(biāo)系的轉(zhuǎn)換也就是b系轉(zhuǎn)R系也就是機(jī)體坐標(biāo)系轉(zhuǎn)大地坐標(biāo)系，這里的_q是四元素，首先我們看看這個(gè)系的轉(zhuǎn)換公式（四元素法）? ，這里利用的是C++中的vector這里我們?nèi)タ纯碿onjugate函數(shù)? 這里的理解一下其實(shí)就date數(shù)據(jù)就是_q.conjugate中的_q賦過(guò)來(lái)的，而v.date就是conjugate后面的數(shù)據(jù)這個(gè)我不知道講清楚了沒(méi)，我們看到conjugate后面的參數(shù)是_mag，其實(shí)這個(gè)參數(shù)也是有mag(0)、mag(1)、mag(2)所以這個(gè)就不難理解了吧！然后就是利用轉(zhuǎn)換后的X、Y進(jìn)行轉(zhuǎn)換得到mag_err,最后通過(guò)conjugate_inversed函數(shù)再次轉(zhuǎn)換到機(jī)體坐標(biāo)，轉(zhuǎn)換公式貼出來(lái)自己去看。?
?
然后進(jìn)行四元素的歸一化處理，這個(gè)normalize函數(shù)不難理解吧，平方求根后作商。接著利用accl進(jìn)行陀螺儀數(shù)據(jù)修正這個(gè)貌似不難，跟inint立面差不多。然后進(jìn)行PI調(diào)節(jié) 這個(gè)理論性很強(qiáng)哦。主要是利用PI調(diào)節(jié)對(duì)姿態(tài)進(jìn)行估計(jì) ，大家去看看姿態(tài)解算的論文就會(huì)明白的。同樣最后對(duì)四元素進(jìn)行歸一化處理并且對(duì)四元素進(jìn)行數(shù)據(jù)判別。那么這個(gè)update就基本講完了。?
我們回到att_estimator_q_main中，接著就是利用四元素轉(zhuǎn)換為歐拉角。 ?這個(gè)的理論公式如下 ?自己對(duì)照看吧！然后對(duì)這些數(shù)據(jù)進(jìn)行賦值到att結(jié)構(gòu)體中接著利用四元素轉(zhuǎn)換為方向余弦矩陣中最后賦值到date中。然后就是發(fā)布att的數(shù)據(jù) ?同時(shí)也將數(shù)據(jù)賦值到控制狀態(tài)中再進(jìn)行發(fā)布數(shù)據(jù)。說(shuō)到這里姿態(tài)估計(jì)這章節(jié)就講完了。又不懂的還是那句話大家互相交流可以郵件給我或直接qq給我都行。

總結(jié)

以上是生活随笔為你收集整理的第九章 PX4-pixhawk-姿态估计解析的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

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