PX4的无人机飞控应用开发

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)PX4flightstack：一系列自治无人机自动控制算法的集合b)PX4Middleware：一系列针对无人机控制器、传感器等物理设备的驱动及底层通信、调度等机制的集合PX4软件架构中，最有意思的一点在于整个架构的抽象性（多态性）。即，为了最大限度保障飞控算法代码的重用性，其将飞控逻辑与具体的底层控制器指令实现进行了解耦合。一套高层飞控算法（如autopilot、GeoFence等）在不做显著修改的情况下，能够适用于固定翼、直升机、多旋翼等多种机型的控制场合，这时候就体现出PX4飞控的威力来了：在用户程序写好之后，如果需要替换无人机机架的话，仅需简单的修改一下机架配置参数即可，高层的用户自定义飞控应用几乎无需修改。理解上述初衷至关重要。有很多搞自动化出身、没太多软件经验的朋友倾向于直接使用底层控制协议来控制飞控板，但实际上PX4架构已经在更高的抽象层面上提供了更好的选择，无论是代码维护成本、开发效率、硬件兼容性都能显著高于前者。很多支持前者方式的开发者的理由主要在于高层封装机制效率较低，而飞控板性能不够，容易给飞控板造成较大的处理负载，但实际从个人感觉上来看，遵循PX4的软件架构模式反倒更容易实现较高处理性能，不容易产生控制拥塞，提升无人机侧系统的并发处理效率。2、PX4/Pixhawk飞行控制协议与逻辑Mavlink是目前最常见的无人机飞控协议之一。PX4对Mavlink协议提供了良好的原生支持。该协议既可以用于地面站(GCS)对无人机(UAV)的控制，也可用于UAV对GCS的信息反馈。其飞控场景一般是这样的：a)手工飞控：GCS-(MavLink)-UAVb)信息采集：GCS-(Mavlink)-UAVc)自治飞控：UserApp-(MavLink)-UAV也就是说，如果你想实现地面站控制飞行，那么由你的地面站使用Mavlink协议，通过射频信道(或wifietc.)给无人机发送控制指令就可以了。如果你想实现无人机自主飞行，那么就由你自己写的应用（运行在无人机系统上）使用Mavlink协议给无人机发送本地的控制指令就可以了。然而，为实现飞控架构的灵活性，避免对底层实现细节的依赖，在PX4中，并不鼓励开发者在自定义飞控程序中直接使用Mavlink，而是鼓励开发者使用一种名为uORB((MicroObjectRequestBroker，微对象请求代理)的消息机制。其实uORB在概念上等同于posix里面的命名管道(namedpipe)，它本质上是一种进程间通信机制。由于PX4实际使用的是NuttX实时ARM系统，因此uORB实际上相当于是多个进程（驱动级模块）打开同一个设备文件，多个进程（驱动级模块）通过此文件节点进行数据交互和共享。在uORB机制中，交换的消息被称之为topic，一个topic仅包含一种message类型（即数据结构）。每个进程（或驱动模块）均可“订阅”或“发布”多个topic，一个topic可以存在多个发布者，而且一个订阅者可也订阅多个topic。而正因为有了uORB机制的存在，上述飞控场景变成了：a)手工飞控：GCS-(MavLink)-(uORBtopic)-UAVb)信息采集：GCS-(Mavlink)-(uORBtopic)-UAVc)自治飞控：UserApp-(uORBtopic)-(MavLink)-UAV有了以上背景基础，便可以自写飞控逻辑了，仅需在PX4源码中，添加一个自定义module，然后使用uORB订阅相关信息（如传感器消息等），并发布相关控制信息（如飞行模式控制消息等）即可。具体的uORBAPI、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文件在px4\Firmware\src\modules\attitude_estimator_ekf\codegen\目录下具体原理程序详解下一步1.具体原理EKF算法原理不再多讲，具体可参见上一篇blog这篇讲EKF算法执行过程，需要以下几个关键式子：飞行器状态矩阵：这里表示三轴角速度，表示三轴角加速度，表示加速度在机体坐标系三轴分量，，表示磁力计在机体坐标系三轴分量。测量矩阵分别由三轴陀螺仪，加速度计，磁力计测得。状态转移矩阵：飞行器下一时刻状态预测矩阵如下：其中W项均为高斯噪声，为飞行器在姿态发生变化后，坐标系余旋变换矩阵，对该函数在处求一阶偏导，可得到状态转移矩阵：此时可得到飞行器状态的先验估计：利用测量值修正先验估计：这里测量矩阵H与状态矩阵X为线性关系，故无需求偏导。卡尔曼增益：状态后验估计：方差后验估计：2.程序详解整个EKF的代码挺长的，大部分是矩阵运算，而且使用嵌套for循环来执行的，所以读起来比较费劲，但是要是移植到自己工程上的话必然离不开这一步，所以花了一个下午把各个细节理清楚，顺便记录分享。/*Includefiles*/#includert_nonfinite.h#includeattitudeKalmanfilter.h#includerdivide.h#includenorm.h#includecross.h#includeeye.h#includemrdivide.h/*'输入参数：updateVect[3]：用来记录陀螺仪，加速度计，磁力计传感器数值是否有效z[9]：测量矩阵x_aposteriori_k[12]：上一时刻状态后验估计矩阵，用来预测当前状态P_aposteriori_k[144]：上一时刻后验估计方差eulerAngles[3]：输出欧拉角Rot_matrix[9]：输出余弦矩阵x_aposteriori[12]：输出状态后验估计矩阵P_aposteriori[144]：输出方差后验估计矩阵'*/voidattitudeKalmanfilter(constuint8_TupdateVect[3],real32_Tdt,constreal32_Tz[9],constreal32_Tx_aposteriori_k[12],constreal32_TP_aposteriori_k[144],constreal32_Tq[12],real32_Tr[9],real32_TeulerAngles[3],real32_TRot_matrix[9],real32_Tx_aposteriori[12],real32_TP_aposteriori[144]){/*以下这一堆变量用到的时候再解释*/real32_Twak[3];real32_TO[9];real_Tdv0[9];real32_Ta[9];int32_Ti;real32_Tb_a[9];real32_Tx_n_b[3];real32_Tb_x_aposteriori_k[3];real32_Tz_n_b[3];real32_Tc_a[3];real32_Td_a[3];int32_Ti0;real32_Tx_apriori[12];real_Tdv1[144];real32_TA_lin[144];staticconstint8_Tiv0[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};real32_Tb_A_lin[144];real32_Tb_q[144];real32_Tc_A_lin[144];real32_Td_A_lin[144];real32_Te_A_lin[144];int32_Ti1;real32_TP_apriori[144];real32_Tb_P_apriori[108];staticconstint8_Tiv1[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,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_TK_k[108];real32_Tfv0[81];staticconstint8_Tiv2[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,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_Tb_r[81];real32_Tfv1[81];real32_Tf0;real32_Tc_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_Tfv2[36];staticconstint8_Tiv4[36]={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_Tc_r[9];real32_Tb_K_k[36];real32_Td_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_Tc_K_k[72];staticconstint8_Tiv6[72]={1,0,0,0,0,0,0,1,0,0,