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

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。