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

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

《PX4的无人机飞控应用开发.docx》由会员分享，可在线阅读，更多相关《PX4的无人机飞控应用开发.docx（75页珍藏版）》请在冰豆网上搜索。

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）->UAV

b）信息采集：

GCS<-（Mavlink）<-UAV

c）自治飞控：

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）->UAV

b）信息采集：

GCS<-（Mavlink）<-（uORBtopic）<-UAV

c）自治飞控：

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*/

#include"rt_nonfinite.h"

#include"attitudeKalmanfilter.h"

#include"rdivide.h"

#include"norm.h"

#include"cross.h"

#include"eye.h"

#include"mrdivide.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,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_Tb_z[6];

staticconstint8_Tiv7[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,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};

staticconstint8_Tiv8[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_Tfv3[6];

real32_Tc_z[6];

/*开始计算*/

/*'wak[]为当前状态三轴角加速度'*/

wak[0]=x_aposteriori_k[3];

wak[1]=x_aposteriori_k[4];

wak[2]=x_aposteriori_k[5];

∙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=⎡⎣⎢0wzwy−wz0wxwy−wx0⎤⎦⎥

这里的O矩阵相当于A矩阵中的

的转置矩阵！

*/

O[0]=0.0F;

O[1]=-x_aposteriori_k[2];

O[2]=x_aposteriori_k[1];

O[3]=x_aposteriori_k[2];

O[4]=0.0F;

O[5]=-x_aposteriori_k[0];

O[6]=-x_aposteriori_k[1];

O[7]=x_aposteriori_k[0];

O[8]=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++）{

a[i]=（real32_T）dv0[i]+O[i]*dt;

}

/*预测转过一个小角度之后的磁力向量三轴投影*/

eye（dv0）;

for（i=0;i<9;i++）{

b_a[i]=（real32_T）dv0[i]+O[i]*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=⎡⎣⎢1Δz−Δy−Δz1ΔxΔy−Δx1⎤⎦⎥

其实就是这个大家都很眼熟的的余弦矩阵的转置,用来更新机体转过一个角度之后的重力和磁力三轴投影，只不过两次计算间隔时间很短，变化角度很小，因此忽略高阶小量之后就变成了这个样子。

这里还少一个时间系数dt，下面会补上。

⎡⎣⎢cosy∗cosz−cosy∗sinzsiny（sinx∗siny∗cosz）+（cosx∗sinz）−（sinx∗siny∗sinz）+（cosx∗cosz）−sinx∗cosy−（cosx∗siny∗cosz）+（sinx∗sinz）（cosx∗siny∗sinz）+（sinx∗cosz）cosx∗cosy⎤⎦⎥

*/

x_n_b[0]=x_aposteriori_k[0];//角速度

x_n_b[1]=x_aposteriori_k[1];

x_n_b[2]=x_aposteriori_k[2];

b_x_aposteriori_k[0]=x_aposteriori_k[6];//加速度

b_x_aposteriori_k[1]=x_aposteriori_k[7];

b_x_aposteriori_k[2]=x_aposteriori_k[8];

z_n_b[0]=x_aposteriori_k[9];//磁力计

z_n_b[1]=x_aposteriori_k[10];

z_n_b[2]=x_aposteriori_k[11];

for（i=0;i<3;i++）{

c_a[i]=0.0F;

for（i0=0;i0<3;i0++）{

c_a[i]+=a[i+3*i0]*b_x_aposteriori_k[i0];

}

d_a[i]=0.0F;

for（i0=0;i0<3;i0++）{

d_a[i]+=b_a[i+3*i0]*z_n_b[i0];

}

x_apriori[i]=x_n_b[i]+dt*wak[i];

}

for（i=0;i<3;i++）{

x_apriori[i+3]=wak[i];

}

for（i=0;i<3;i++）{

x_apriori[i+6]=c_a[i];

}

for（i=0;i<3;i++）{

x_apriori[i+9]=d_a[i];

}//得到状态先验估计

∙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

/* 

根据上述矩阵运算，可