PX4的无人机飞控应用开发.doc

PX4/PixHawk无人机飞控应用开发1、PX4/Pixhawk飞控软件架构简介PX4是目前最流行的开源飞控板之一。PX4的软件系统实际上就是一个firmware，其核心OS为NuttX实时ARM系统。其固件同时附带了一系列工具集、系统驱动/模块与外围软件接口层，所有这些软件（包括用户自定义的飞控软件）随OS内核一起，统一编译为固件形式，然后上传到飞控板中，从而实现对飞控板的软件配置。PX4配套的软件架构主要分为4层。理解其软件架构是开发用户自定义飞控应用软件的基础。a) API层：这个好理解。b) 框架层：包含了操作基础飞行控制的默认程序集（节点）c) 系统库：包含了所有的系统库和基本交通控制的函数d) OS内核：提供硬件驱动程序、网络、UAVCAN和故障安全系统上述是个面向PX4系统实现者的相对具体的软件架构。实际上还有另外一种面向PX4自定义飞控应用开发者的高层软件架构描述，相对抽象，但更简单，就是整个PX4的软件从整体上分为2层：a) PX4 flight stack：一系列自治无人机自动控制算法的集合b) PX4 Middleware：一系列针对无人机控制器、传感器等物理设备的驱动及底层通信、调度等机制的集合PX4软件架构中，最有意思的一点在于整个架构的抽象性（多态性）。即，为了最大限度保障飞控算法代码的重用性，其将飞控逻辑与具体的底层控制器指令实现进行了解耦合。一套高层飞控算法（如autopilot、GeoFence等）在不做显著修改的情况下，能够适用于固定翼、直升机、多旋翼等多种机型的控制场合，这时候就体现出PX4飞控的威力来了：在用户程序写好之后，如果需要替换无人机机架的话，仅需简单的修改一下机架配置参数即可，高层的用户自定义飞控应用几乎无需修改。理解上述初衷至关重要。有很多搞自动化出身、没太多软件经验的朋友倾向于直接使用底层控制协议来控制飞控板，但实际上PX4架构已经在更高的抽象层面上提供了更好的选择，无论是代码维护成本、开发效率、硬件兼容性都能显著高于前者。很多支持前者方式的开发者的理由主要在于高层封装机制效率较低，而飞控板性能不够，容易给飞控板造成较大的处理负载，但实际从个人感觉上来看，遵循PX4的软件架构模式反倒更容易实现较高处理性能，不容易产生控制拥塞，提升无人机侧系统的并发处理效率。2、PX4/Pixhawk飞行控制协议与逻辑Mavlink是目前最常见的无人机飞控协议之一。PX4对Mavlink协议提供了良好的原生支持。该协议既可以用于地面站(GCS)对无人机(UAV)的控制，也可用于UAV对GCS的信息反馈。其飞控场景一般是这样的：a) 手工飞控：GCS - (MavLink) - UAVb) 信息采集：GCS - (Mavlink) (MavLink) - UAV也就是说，如果你想实现地面站控制飞行，那么由你的地面站使用Mavlink协议，通过射频信道(或 wifi etc.)给无人机发送控制指令就可以了。如果你想实现无人机自主飞行，那么就由你自己写的应用（运行在无人机系统上）使用Mavlink协议给无人机发送本地的控制指令就可以了。然而，为实现飞控架构的灵活性，避免对底层实现细节的依赖，在PX4中，并不鼓励开发者在自定义飞控程序中直接使用Mavlink，而是鼓励开发者使用一种名为uORB(Micro Object Request Broker，微对象请求代理)的消息机制。其实uORB在概念上等同于posix里面的命名管道(named pipe)，它本质上是一种进程间通信机制。由于PX4实际使用的是NuttX实时ARM系统，因此uORB实际上相当于是多个进程（驱动级模块）打开同一个设备文件，多个进程（驱动级模块）通过此文件节点进行数据交互和共享。在uORB机制中，交换的消息被称之为topic，一个topic仅包含一种message类型（即数据结构）。每个进程（或驱动模块）均可“订阅”或“发布”多个topic，一个topic可以存在多个发布者，而且一个订阅者可也订阅多个topic。而正因为有了uORB机制的存在，上述飞控场景变成了：a) 手工飞控：GCS - (MavLink) - (uORB topic) - UAVb) 信息采集：GCS - (Mavlink) - (uORB topic) (uORB topic) - (MavLink) - UAV有了以上背景基础，便可以自写飞控逻辑了，仅需在PX4源码中，添加一个自定义module，然后使用uORB订阅相关信息（如传感器消息等），并发布相关控制信息（如飞行模式控制消息等）即可。具体的uORB API、uORB消息定义可参考PX4文档与源码，所有控制命令都在firmware代码的msg里面，不再敷述。最后值得一提的是，在PX4系统中，还提供了一个名为mavlink的专用module，源码在firmware的src/modules/mavlink中，这货与linux的控制台命令工具集相当相似，其既可以作为ntt控制台下的命令使用，又可作为系统模块加载后台运行。其所实现的功能包括：1）uORB消息解析，将uORB消息实际翻译为具体的Mavlink底层指令，或反之。2）通过serial/射频通信接口获取或发送Mavlink消息，既考虑到了用户自写程序的开发模式，也适用于类似linux的脚本工具链开发模式，使用起来很灵活，有兴趣的可以看看。PX4飞控中利用EKF估计姿态角代码详解PX4飞控中主要用EKF算法来估计飞行器三轴姿态角，具体c文件在px4Firmwaresrcmodulesattitude_estimator_ekfcodegen目录下 具体原理 程序详解 下一步1.具体原理EKF算法原理不再多讲，具体可参见上一篇blog/lizilpl/article/details/45289541.这篇讲EKF算法执行过程，需要以下几个关键式子： 飞行器状态矩阵：这里表示三轴角速度，表示三轴角加速度，表示加速度在机体坐标系三轴分量，表示磁力计在机体坐标系三轴分量。 测量矩阵分别由三轴陀螺仪，加速度计，磁力计测得。 状态转移矩阵：飞行器下一时刻状态预测矩阵如下：其中W项均为高斯噪声，为飞行器在姿态发生变化后，坐标系余旋变换矩阵，对该函数在处求一阶偏导，可得到状态转移矩阵：此时可得到飞行器状态的先验估计： 利用测量值修正先验估计：这里测量矩阵H与状态矩阵X为线性关系，故无需求偏导。卡尔曼增益：状态后验估计：方差后验估计：2.程序详解整个EKF的代码挺长的，大部分是矩阵运算，而且使用嵌套for循环来执行的，所以读起来比较费劲，但是要是移植到自己工程上的话必然离不开这一步，所以花了一个下午把各个细节理清楚，顺便记录分享。/* Include files */#include rt_nonfinite.h#include attitudeKalmanfilter.h#include rdivide.h#include norm.h#include cross.h#include eye.h#include mrdivide.h/* 输入参数：updateVect3：用来记录陀螺仪，加速度计，磁力计传感器数值是否有效 z9 ：测量矩阵 x_aposteriori_k12：上一时刻状态后验估计矩阵，用来预测当前状态 P_aposteriori_k144：上一时刻后验估计方差 eulerAngles3 ：输出欧拉角 Rot_matrix9 ：输出余弦矩阵 x_aposteriori12 ：输出状态后验估计矩阵 P_aposteriori144 ：输出方差后验估计矩阵*/void attitudeKalmanfilter(const uint8_T updateVect3,real32_T dt, const real32_T z9, const real32_T x_aposteriori_k12, const real32_T P_aposteriori_k144, const real32_T q12, real32_T r9, real32_T eulerAngles3, real32_T Rot_matrix9,real32_T x_aposteriori12, real32_T P_aposteriori144)/*以下这一堆变量用到的时候再解释*/ real32_T wak3; real32_T O9; real_T dv09; real32_T a9; int32_T i; real32_T b_a9; real32_T x_n_b3; real32_T b_x_aposteriori_k3; real32_T z_n_b3; real32_T c_a3; real32_T d_a3; int32_T i0; real32_T x_apriori12; real_T dv1144; real32_T A_lin144; static const int8_T iv036 = 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 ; real32_T b_A_lin144; real32_T b_q144; real32_T c_A_lin144; real32_T d_A_lin144; real32_T e_A_lin144; int32_T i1; real32_T P_apriori144; real32_T b_P_apriori108; static const int8_T iv1108 = 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, 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 ; real32_T K_k108; real32_T fv081; static const int8_T iv2108 = 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, 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 ; real32_T b_r81; real32_T fv181; real32_T f0; real32_T c_P_apriori36= 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 fv236; static const int8_T iv436 = 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 ; real32_T c_r9; real32_T b_K_k36; real32_T d_P_apriori72; static const int8_T iv572 = 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 const int8_T iv672 = 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, 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 ; real32_T b_z6; static const int8_T iv772 = 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, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 ; static const int8_T iv872 = 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 fv36; real32_T c_z6; /*开始计算*/ /*wak为当前状态三轴角加速度*/ wak0 = x_aposteriori_k3; wak1 = x_aposteriori_k4; wak2 = x_aposteriori_k5; 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162/* 欧拉角旋转矩阵O=0wzwywz0wxwywx0这里的O矩阵相当于A矩阵中的的转置矩阵！*/ O0 = 0.0F; O1 = -x_aposteriori_k2; O2 = x_aposteriori_k1; O3 = x_aposteriori_k2; O4 = 0.0F; O5 = -x_aposteriori_k0; O6 = -x_aposteriori_k1; O7 = x_aposteriori_k0; O8 = 0.0F; /* 预测转过一个小角度之后的重力向量三轴投影 */ /* a = 1, -delta_z, delta_y; * delta_z, 1 , -delta_x; * -delta_y, delta_x, 1 ; */ eye(dv0); /dv0矩阵单位化 for (i = 0; i 9; i+) ai = (real32_T)dv0i + Oi * dt; /* 预测转过一个小角度之后的磁力向量三轴投影 */ eye(dv0); for (i = 0; i 9; i+) b_ai = (real32_T)dv0i + Oi * dt; 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28/*a=1zyz1xyx1其实就是这个大家都很眼熟的的余弦矩阵的转置, 用来更新机体转过一个角度之后的重力和磁力三轴投影，只不过两次计算间隔时间很短，变化角度很小，因此忽略高阶小量之后就变成了这个样子。这里还少一个时间系数dt，下面会补上。cosycoszcosysinzsiny(sinxsinycosz)+(cosxsinz)(sinxsinysinz)+(cosxcosz)sinxcosy(cosxsinycosz)+(sinxsinz)(cosxsinysinz)+(sinxcosz)cosxcosy*/ x_n_b0 = x_aposteriori_k0; /角速度 x_n_b1 = x_aposteriori_k1; x_n_b2 = x_aposteriori_k2; b_x_aposteriori_k0 = x_aposteriori_k6; / 加速度 b_x_aposteriori_k1 = x_aposteriori_k7; b_x_aposteriori_k2 = x_aposteriori_k8; z_n_b0 = x_aposteriori_k9; /磁力计 z_n_b1 = x_aposteriori_k10; z_n_b2 = x_aposteriori_k11; 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 + 3 * i0 * z_n_bi0; x_apriorii = x_n_bi + dt * waki; 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; /得到状态先验估计 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34/*根据上述矩阵运算，可以得到：c_a13=axayaza33从而：kra,kt31=c_a13Td_a13=mxmymza33从而：krm,kt31=d_a13T其中axayaz和mxmymz分别对应ra,k和rm,k的转置；得到状态先验估计:Xk+1121=x_apriori112T=x_n_b+wakdtwakc_ad_aT*/* 开始计算A矩阵*/ b_eye(dv1); for (i = 0; i 12; i+) for (i0 = 0; i0 3; i0+) A_lini0 + 12 * i = (real32_T)iv0i0 + 3 * i; /*1 2 3列*/ for (i0 = 0; i0 3; i0+) A_lin(i0 + 12 * i) + 3 = 0.0F; /*3 4 5列*/ /*6 7 8 列*/ A_lin6 = 0.0F; A_lin7 = x_aposteriori_k8; A_lin8 = -x_aposteriori_k7; A_lin18 = -x_aposteriori_k8; A_lin19 = 0.0F; A_lin20 = x_aposteriori_k6; A_lin30 = x_aposteriori_k7; A_lin31 = -x_aposteriori_k6; 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+) 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 + 9) + 6 = 0.0F; /*6 7 8 列*/ /*9 10 11 列*/ A_lin9 = 0.0F; A_lin10 = x_aposteriori_k11; A_lin11 = -x_aposteriori_k10; A_lin21 = -x_aposteriori_k11; A_lin22 = 0.0F; A_lin23 = x_aposteriori_k9; A_lin33 = x_aposteriori_k7; A_lin34 = -x_aposteriori_k9; A_lin35 = 0.0F; for (i = 0; i 3; i+) for (i0 = 0; i0 3; i0+) A_lin(i0 + 12 * (i + 3) + 9 = 0.0F; for (i = 0; i 3; i+) for (i0 = 0; i0 3; i0+) A_lin(i0 + 12 * (i + 6) + 9 = 0.0F; for (i = 0; i 3; i+) for (i0 = 0; i0 3; i0+) A_lin(i0 + 12 * (i + 9) + 9 = Oi0 + 3 * i; /*9 10 11 列*/ 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68/*根据上述矩阵运算，可以得到A_lin矩阵：A_lin1212=0I000000A10O0A200O其中各元素都是3*3的矩阵；I为单位矩阵，其中A1=0azayaz0axayax0=ra,Tk同样A2=0mzmymz0mxmymx0=rm,Tk*/ for (i = 0; i 12; i+) for (i0 = 0; i0 12; i0+) b_A_lini0 + 12 * i = (real32_T)dv1i0 + 12 * i + A_lini0 + 12 * i *dt; /最终A_link,k的逆矩阵 1 2 3 4 5得到：Alin,Tk=b_A_lin1212=I0000I0000I0000I+0I000000A10O0A200Odt/*开始根据计算过程方差*/过程噪声方差b_q1212=q00000q10000q20000q3其中各元素都是3*3的矩阵； b_q0 = q0; b_q12 = 0.0F; b_q24 = 0.0F; b_q36 = 0.0F; b_q48 = 0.0F; b_q60 = 0.0F; b_q72 = 0.0F; b_q84 = 0.0F; b_q96 = 0.0F; b_q108 = 0.0F; b_q120 = 0.0F; b_q132 = 0.0F; b_q1 = 0.0F; b_q13 = q0; b_q25 = 0.0F; b_q37 = 0.0F; b_q49 = 0.0F; b_q61 = 0.0F; b_q73 = 0.0F; b_q85 = 0.0F; b_q97 = 0.0F; b_q109 = 0.0F; b_q121 = 0.0F; b_q133 = 0.0F; b_q2 = 0.0F; b_q14 = 0.0F; b_q26 = q0; b_q38 = 0.0F; b_q50 = 0.0F; b_q62 = 0.0F; b_q74 = 0.0F; b_q86 = 0.0F; b_q98 = 0.0F; b_q110 = 0.0F; b_q122 = 0.0F; b_q134 = 0.0F; b_q3 = 0.0F; b_q15 = 0.0F; b_q27 = 0.0F; b_q39 = q1; b_q51 = 0.0F; b_q63 = 0.0F; b_q75 = 0.0F; b_q87 = 0.0F; b_q99 = 0.0F; b_q111 = 0.0F; b_q123 = 0.0F; b_q135 = 0.0F; b_q4 = 0.0F; b_q16 = 0.0F; b_q28 = 0.0F; b_q40 = 0.0F; b_q52 = q1; b_q64 = 0.0F; b_q76 = 0.0F; b_q88 = 0.0F; b_q100 = 0.0F; b_q112 = 0.0F; b_q124 = 0.0F; b_q136 = 0.0F; b_q5 = 0.0F; b_q17 = 0.0F; b_q29 = 0.0F; b_q41 = 0.0F; b_q53 = 0.0F; b_q65 = q1; b_q77 = 0.0F; b_q89 = 0.0F; b_q101 = 0.0F; b_q113 = 0.0F; b_q125 = 0.0F; b_q137 = 0.0F; b_q6 = 0.0F; b_q18 = 0.0F; b_q30 = 0.0F; b_q42 = 0.0F; b_q54 = 0.0F; b_q66 = 0.0F; b_q78 = q2; b_q90 = 0.0F; b_q102 = 0.0F; b_q114 = 0.0F; b_q126 = 0.0F; b_q138 = 0.0F; b_q7 = 0.0F; b_q19 = 0.0F; b_q31 = 0.0F; b_q43 = 0.0F; b_q55 = 0.0F; b_q67 = 0.0F; b_q79 = 0.0F; b_q91 = q2; b_q103 = 0.0F; b_q115 = 0.0F; b_q127 = 0.0F; b_q139 = 0.0F; b_q8 = 0.0F; b_q20 = 0.0F; b_q32 = 0.0F; b_q44 = 0.0F; b_q56 = 0.0F; b_q68 = 0.0F; b_q80 = 0.0F; b_q92 = 0.0F; b_q104 = q2; b_q116 = 0.0F; b_q128 = 0.0F; b_q140 = 0.0F; b_q9 = 0.0F; b_q21 = 0.0F; b_q33 = 0.0F; b_q45 = 0.0F; b_q57 = 0.0F; b_q69 = 0.0F; b_q81 = 0.0F; b_q93 = 0.0F; b_q105 = 0.0F; b_q117 = q3; b_q129 = 0.0F; b_q141 = 0.0F; b_q10 = 0.0F; b_q22 = 0.0F; b_q34 = 0.0F; b_q46 = 0.0F; b_q58 = 0.0F; b_q70 = 0.0F; b_q82 = 0.0F; b_q94 = 0.0F; b_q106 = 0.0F; b_q118 = 0.0F; b_q130 = q3; b_q142 = 0.0F; b_q11 = 0.0F; b_q23 = 0.0F; b_q35 = 0.0F; b_q47 = 0.0F; b_q59 = 0.0F; b_q71 = 0.0F; b_q83 = 0.0F; b_q95 = 0.0F; b_q107 = 0.0F; b_q119 = 0.0F; b_q131 = 0.0F; b_q143 = q3; for (i = 0; i 12; i+) for (i0 = 0; i0 12; i0+) A_lini + 12 * i0 = 0.0F; for (i1 = 0; i1 12; i1+) A_lini + 12 * i0 += b_A_lini + 12 * i1 * P_aposteriori_ki1 + 12 *i0; c_A_