PX4的無人機飛控應(yīng)用開發(fā)

1、.PX4/PixHawk無人機飛控應(yīng)用開發(fā)1、 PX4/Pixhawk飛控軟件架構(gòu)簡介PX4 是目前最流行的開源飛控板之一。 PX4 的軟件系統(tǒng)實際上就是一個firmware ，其核心 OS為 NuttX實時 ARM 系統(tǒng)。 其固件同時附帶了一系列工具集、系統(tǒng)驅(qū)動 / 模塊與外圍軟件接口層，所有這些軟件（包括用戶自定義的飛控軟件）隨 OS 內(nèi)核一起 ，統(tǒng)一編譯為固件形式，然后上傳到飛控板中，從而實現(xiàn)對飛控板的軟件配置。PX4 配套的軟件架構(gòu)主要分為4 層 。 理解其軟件架構(gòu)是開發(fā)用戶自定義飛控應(yīng)用軟件的基礎(chǔ)。a) API 層：這個好理解 。b) 框架層 ：包含了操作基礎(chǔ)飛行控制的默認(rèn)程序集（節(jié)

2、點 ）c) 系統(tǒng)庫 ：包含了所有的系統(tǒng)庫和基本交通控制的函數(shù)d) OS 內(nèi)核 ：提供硬件驅(qū)動程序 、網(wǎng)絡(luò) 、 UAVCAN 和故障安全系統(tǒng)上述是個面向PX4 系統(tǒng)實現(xiàn)者的相對具體的軟件架構(gòu)。實際上還有另外一種面向PX4 自定義飛控應(yīng)用開發(fā)者的高層軟件架構(gòu)描述，相對抽象 ，但更簡單 ，就是整個PX4 的軟件從整體上分為2 層：a) PX4 flight stack ：一系列自治無人機自動控制算法的集合b) PX4 Middleware：一系列針對無人機控制器、傳感器等物理設(shè)備的驅(qū)動及底層通信、調(diào)度等機制的集合.專業(yè)專注.PX4 軟件架構(gòu)中 ，最有意思的一點在于整個架構(gòu)的抽象性（多態(tài)性 ）。 即，

3、為了最大限度保障飛控算法代碼的重用性，其將飛控邏輯與具體的底層控制器指令實現(xiàn)進(jìn)行了解耦合。一套高層飛控算法 （如 autopilot、 GeoFence 等）在不做顯著修改的情況下，能夠適用于固定翼 、直升機、多旋翼等多種機型的控制場合，這時候就體現(xiàn)出PX4 飛控的威力來了 ：在用戶程序?qū)懞弥?，如果需要替換無人機機架的話，僅需簡單的修改一下機架配置參數(shù)即可，高層的用戶自定義飛控應(yīng)用幾乎無需修改。理解上述初衷至關(guān)重要。有很多搞自動化出身、沒太多軟件經(jīng)驗的朋友傾向于直接使用底層控制協(xié)議來控制飛控板，但實際上PX4 架構(gòu)已經(jīng)在更高的抽象層面上提供了更好的選擇，無論是代碼維護(hù)成本、開發(fā)效率 、硬件兼

4、容性都能顯著高于前者。很多支持前者方式的開發(fā)者的理由主要在于高層封裝機制效率較低，而飛控板性能不夠， 容易給飛控板造成較大的處理負(fù)載，但實際從個人感覺上來看，遵循 PX4 的軟件架構(gòu)模式反倒更容易實現(xiàn)較高處理性能，不容易產(chǎn)生控制擁塞 ，提升無人機側(cè)系統(tǒng)的并發(fā)處理效率。2、 PX4/Pixhawk飛行控制協(xié)議與邏輯Mavlink是目前最常見的無人機飛控協(xié)議之一。 PX4 對 Mavlink協(xié)議提供了良好的原生支持。該協(xié)議既可以用于地面站(GCS)對無人機 (UAV) 的控制 ，也可用于 UAV 對 GCS 的信息反饋 。 其飛控場景一般是這樣的：a) 手工飛控 ： GCS -> (MavL

5、ink) -> UAV.專業(yè)專注.b) 信息采集 ：GCS <- (Mavlink) <- UAVc) 自治飛控 ： User App -> (MavLink) -> UAV也就是說 ，如果你想實現(xiàn)地面站控制飛行，那么由你的地面站使用Mavlink協(xié)議 ， 通過射頻信道(或 wifi etc.) 給無人機發(fā)送控制指令就可以了。如果你想實現(xiàn)無人機自主飛行，那么就由你自己寫的應(yīng)用 （運行在無人機系統(tǒng)上）使用Mavlink協(xié)議給無人機發(fā)送本地的控制指令就可以了。然而，為實現(xiàn)飛控架構(gòu)的靈活性，避免對底層實現(xiàn)細(xì)節(jié)的依賴，在 PX4 中，并不鼓勵開發(fā)者在自定義飛控程序中直接使

6、用Mavlink ，而是鼓勵開發(fā)者使用一種名為uORB(Micro ObjectRequest Broker，微對象請求代理)的消息機制 。 其實 uORB 在概念上等同于posix 里面的命名管道 (named pipe)，它本質(zhì)上是一種進(jìn)程間通信機制。由于 PX4 實際使用的是NuttX實時 ARM系統(tǒng)，因此 uORB 實際上相當(dāng)于是多個進(jìn)程（驅(qū)動級模塊）打開同一個設(shè)備文件，多個進(jìn)程（驅(qū)動級模塊）通過此文件節(jié)點進(jìn)行數(shù)據(jù)交互和共享。在 uORB 機制中 ，交換的消息被稱之為topic ，一個 topic 僅包含一種message 類型 （即數(shù)據(jù)結(jié)構(gòu)）。 每個進(jìn)程 （或驅(qū)動模塊 ）均可 “訂閱

7、 ”或 “發(fā)布 ”多個 topic ，一個 topic 可以存在多個發(fā)布者，而且一個訂閱者可也訂閱多個topic 。 而正因為有了uORB 機制的存在，上述飛控場景變成了：a) 手工飛控 ： GCS -> (MavLink) -> (uORB topic) -> UAVb) 信息采集 ：GCS <- (Mavlink) <- (uORB topic) <- UAVc) 自治飛控 ： User App -> (uORB topic) -> (MavLink) -> UAV.專業(yè)專注.有了以上背景基礎(chǔ)，便可以自寫飛控邏輯了，僅需在PX4 源碼中

8、 ，添加一個自定義module ，然后使用uORB 訂閱相關(guān)信息（如傳感器消息等）， 并發(fā)布相關(guān)控制信息（如飛行模式控制消息等）即可 。 具體的 uORB API 、 uORB 消息定義可參考PX4 文檔與源碼，所有控制命令都在firmware代碼的 msg 里面 ，不再敷述 。最后值得一提的是，在 PX4 系統(tǒng)中 ，還提供了一個名為mavlink的專用module ，源碼在firmware的 src/modules/mavlink中，這貨與 linux 的控制臺命令工具集相當(dāng)相似，其既可以作為 ntt 控制臺下的命令使用，又可作為系統(tǒng)模塊加載后臺運行。其所實現(xiàn)的功能包括： 1）uORB 消息

9、解析 ，將 uORB 消息實際翻譯為具體的Mavlink底層指令 ，或反之 。 2 ）通過 serial/射頻通信接口獲取或發(fā)送Mavlink消息 ，既考慮到了用戶自寫程序的開發(fā)模式，也適用于類似linux 的腳本工具鏈開發(fā)模式，使用起來很靈活，有興趣的可以看看。PX4 飛控中利用EKF 估計姿態(tài)角代碼詳解PX4 飛控中主要用EKF 算法來估計飛行器三軸姿態(tài)角，具體 c 文件在目錄下具體原理程序詳解下一步.專業(yè)專注.1.具體原理EKF 算法原理不再多講，具體可參見上一篇.這篇講 EKF 算法執(zhí)行過程，需要以下幾個關(guān)鍵式子：飛行器狀態(tài)矩陣：這里表示三軸角速度，表示三軸角加速度，表示加速度在機體坐

10、標(biāo)系三軸分量，表示磁力計在機體坐標(biāo)系三軸分量。測量矩陣分別由三軸陀螺儀，加速度計 ，磁力計測得 。狀態(tài)轉(zhuǎn)移矩陣 ：飛行器下一時刻狀態(tài)預(yù)測矩陣如下：.專業(yè)專注.其中 W 項均為高斯噪聲，為飛行器在姿態(tài)發(fā)生變化后，坐標(biāo)系余旋變換矩陣，對該函數(shù)在處求一階偏導(dǎo)，可得到狀態(tài)轉(zhuǎn)移矩陣：此時可得到飛行器狀態(tài)的先驗估計：利用測量值修正先驗估計：這里測量矩陣H 與狀態(tài)矩陣X 為線性關(guān)系 ，故無需求偏導(dǎo) ?？柭鲆?：狀態(tài)后驗估計 ：方差后驗估計 ：.專業(yè)專注.2.程序詳解整個 EKF的代碼挺長的 ，大部分是矩陣運算，而且使用嵌套for 循環(huán)來執(zhí)行的 ，所以讀起來比較費勁 ，但是要是移植到自己工程上的話必然離不

11、開這一步，所以花了一個下午把各個細(xì)節(jié)理清楚 ，順便記錄分享。/* Includefiles */#include"rt_nonfinite.h"#include"attitudeKalmanfilter.h"#include"rdivide.h"#include"norm.h"#include"cross.h"#include"eye.h"#include"mrdivide.h"/*'輸入?yún)?shù) ：updateVect3 ：用來記錄陀螺儀 ，加速度計

12、 ，磁力計傳感器數(shù)值是否有效z9：測量矩陣.專業(yè)專注.x_aposteriori_k12 ：上一時刻狀態(tài)后驗估計矩陣，用來預(yù)測當(dāng)前狀態(tài)P_aposteriori_k144 ：上一時刻后驗估計方差eulerAngles3：輸出歐拉角Rot_matrix9：輸出余弦矩陣x_aposteriori12：輸出狀態(tài)后驗估計矩陣P_aposteriori144：輸出方差后驗估計矩陣'*/void attitudeKalmanfilter(const uint8_T updateVect 3,real32_T dt,const real32_T z9,const real32_T x_aposter

13、iori_k 12,const real32_T P_aposteriori_k 144,const real32_T q12,real32_T r9,real32_T eulerAngles 3,.專業(yè)專注.real32_T Rot_matrix 9,real32_T x_aposteriori 12,real32_T P_aposteriori 144)/* 以下這一堆變量用到的時候再解釋*/real32_T wak 3;real32_T O 9;real_T dv0 9;real32_T a9;int32_T i;real32_T b_a9;real32_T x_n_b3;real32_

14、T b_x_aposteriori_k 3;real32_T z_n_b3;real32_T c_a3;real32_T d_a3;.專業(yè)專注. . .int32_T i0;real32_T x_apriori 12;real_T dv1 144;real32_T A_lin 144 ;static const int8_T iv0 36 =0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 ;.專業(yè)專注.real32_T b_A_lin 144;real32_T b_q 144;real32_T c_

15、A_lin144;real32_T d_A_lin 144;real32_T e_A_lin144;int32_T i1;real32_T P_apriori 144;real32_T b_P_apriori 108;static const int8_T iv1 108 = 1, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0

16、,0,0,0,0,0,1,0,0,.專業(yè)專注. . .0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,1;real32_T K_k108;real32_T fv0 81;static const int8_T iv2 108 = 1, 0,0, 0, 0, 0, 0, 0, 0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,

17、0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,.專業(yè)專注.0,0,0,0,0,0,0,0,1;real32_T b_r81;real32_T fv1 81;real32_T f0;real32_T c_P_apriori 36=1,0,0,0,0,0,0,0,0,0,0,0, 0, 1, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0,0,1,0,0,0,0,0,0,0,0,0;real32_T fv2 36;static const int8_T iv4 36 = 1, 0,0,0,1,0,0,0,1,0,0,0,0,0,0,.專業(yè)專注.0

18、, 0, 0,0, 0, 0,0, 0, 0,0,0,0;real32_T c_r9;real32_T b_K_k36;real32_T d_P_apriori 72;staticconstint8_Tiv5 72=1,0,0,0,0,0, 0,0, 0, 0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0;real32_T c_K_k72;static constint8_T iv6 72 = 1, 0, 0, 0, 0, 0,.專業(yè)專注.real32_T b_z6;static cons

19、tint8_T iv7 72= 1,0,0,0,0,0,0,0,0,0,0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,0, 1, 0, 0, 0, 0,0, 0, 1, 0, 0, 0,0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,0, 0, 0, 1, 0, 0,0, 0, 0, 0, 1, 0,0, 0, 0, 0, 0, 1,0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,0,0,0,0,0,0;0,0,0,.專業(yè)專注. . .0,0,0,0,0,0,0,0

20、,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1;static constint8_T iv8 72= 1,0,0,0,0,0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,0, 0, 0, 1, 0, 0,0,0,0,0,0,1;real32_T fv3 6;real32_T c_z6;.專業(yè)專注./* 開始計算 */*'wak 為當(dāng)前狀態(tài)三軸角加速度'*/wak0 = x_aposteriori_k 3;wak1 = x_apos

21、teriori_k 4;wak2 = x_aposteriori_k 5;. .1234567891011121314.專業(yè)專注.15161718192021222324252627282930313233343536.專業(yè)專注.37383940414243444546474849505152535455565758.專業(yè)專注.59606162636465666768697071727374757677787980.專業(yè)專注.81828384858687888990919293949596979899100101102.專業(yè)專注.10310410510610710810911011111211

22、3114115116117118119120121122123124.專業(yè)專注.125126127128129130131132133134135136137138139140141142143144145146.專業(yè)專注.147148149150151152153154155156157158159160161162/*歐拉角旋轉(zhuǎn)矩陣O= ? ? ? 0w zwy - wz0w xwy- wx0? ? ?這里的 O 矩陣相當(dāng)于A 矩陣中的的轉(zhuǎn)置矩陣 ！.專業(yè)專注.*/O0=0.0F;O1= -x_aposteriori_k 2;O2=x_aposteriori_k 1;O3=x_aposte

23、riori_k 2;O4=0.0F;O5= -x_aposteriori_k 0;O6= -x_aposteriori_k 1;O7=x_aposteriori_k 0;O8=0.0F;/* 預(yù)測轉(zhuǎn)過一個小角度之后的重力向量三軸投影*/* a = 1,-delta_z, delta_y;*delta_z,1, -delta_x;*-delta_y,delta_x,1' */.專業(yè)專注.eye(dv0);/dv0矩陣單位化for (i = 0; i < 9; i+)ai = (real32_T)dv0i+ Oi * dt;/* 預(yù)測轉(zhuǎn)過一個小角度之后的磁力向量三軸投影*/eye(d

24、v0);for (i = 0; i < 9; i+)b_ai = (real32_T)dv0i+ Oi* dt;12345.專業(yè)專注.6789101112131415161718192021222324252627.專業(yè)專注.28/*a= ? ? ? 1z-y-z1x y-x1? ? ?其實就是這個大家都很眼熟的的余弦矩陣的轉(zhuǎn)置, 用來更新機體轉(zhuǎn)過一個角度之后的重力和磁力三軸投影，只不過兩次計算間隔時間很短，變化角度很小 ，因此忽略高階小量之后就變成了這個樣子。這里還少一個時間系數(shù)dt ，下面會補上 。? ? ? cosy?cosz- cosy?sinzsiny(sinx ?siny ?

25、 cosz)+(cosx ?sinz)- (si nx?siny ?sinz)+(cosx ?cosz)- sinx ?cosy- (cosx?siny ?cosz)+(sinx ?sinz)(cosx ?siny?sinz)+(sinx ?cosz)cosx?cosy? ? ?*/x_n_b0=x_aposteriori_k 0;/ 角速度x_n_b1=x_aposteriori_k 1;x_n_b2=x_aposteriori_k 2;b_x_aposteriori_k 0 = x_aposteriori_k 6;/加速度b_x_aposteriori_k 1 = x_aposterior

26、i_k 7;b_x_aposteriori_k 2 = x_aposteriori_k 8;.專業(yè)專注.z_n_b0 = x_aposteriori_k 9;/ 磁力計z_n_b1 = x_aposteriori_k 10;z_n_b2 = x_aposteriori_k 11;for (i = 0; i < 3; i+)c_ai = 0.0F;for (i0 = 0; i0 < 3; i0+)c_ai +=ai + 3 * i0 * b_x_aposteriori_ki0;d_ai = 0.0F;for (i0 = 0; i0 < 3; i0+)d_ai +=b_ai +

27、3 * i0 * z_n_bi0;x_apriorii= x_n_bi + dt * waki;.專業(yè)專注.for (i = 0; i < 3; i+)x_apriorii+ 3 = waki;for (i = 0; i < 3; i+)x_apriorii+ 6 = c_ai;for (i = 0; i < 3; i+)x_apriorii+ 9 = d_ai; / 得到狀態(tài)先驗估計12345.專業(yè)專注.6789101112131415161718192021222324252627.專業(yè)專注.28293031323334/*根據(jù)上述矩陣運算，可以得到 ：c_a1?3=a

28、xayaz?a3?3從而 ：?kra,k t3?1=c_a1 ?3Td_a1?3=m xmy mz?a3?3從而 ：?krm ,kt3?1=d_a1 ?3T其中axayaz和 m xmy mz分別對應(yīng) ra,k 和 rm ,k 的轉(zhuǎn)置 ；.專業(yè)專注.得到狀態(tài)先驗估計:Xk + 112 ?1=x_apriori1 ?12 T=x_n_b+wak ?dtwakc_ad_a T*/* '開始計算 A 矩陣 '*/b_eye(dv1);for (i = 0; i < 12; i+)for (i0 = 0; i0 < 3; i0+)A_lini0 + 12 * i = (re

29、al32_T)iv0i0 + 3 * i; /*1 2 3列*/for (i0 = 0; i0 < 3; i0+)A_lin(i0+ 12 * i) + 3 = 0.0F;/*34 5 列*/*6 7 8 列*/.專業(yè)專注. . .A_lin6 = 0.0F;A_lin7=x_aposteriori_k 8;A_lin8 = -x_aposteriori_k 7;A_lin18=-x_aposteriori_k 8;A_lin19=0.0F;A_lin20=x_aposteriori_k 6;A_lin30=x_aposteriori_k 7;A_lin31=-x_aposteriori_k 6;A_lin32=0.0F;for (i=0;i<3;i+)for(i0=0;i0<3;i0+)A_lin(i0+12* (i+ 3)+6=0.0F;for (i = 0; i < 3; i+).專業(yè)專注.for (i0 = 0; i0 < 3; i0+)A_lin(i0+ 12 * (i + 6) + 6 = Oi0 + 3 * i;for(i=0; i<3;i+)for(i0=0;i0 <3;i0+)A_lin(i0+12*(i+