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

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) -> UAV
b) 信息采集：GCS <- (Mavlink) <- UAV
c) 自治飞控：User App -> (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) -> UAV
b) 信息采集：GCS <- (Mavlink) <- (uORB topic) <- UAV
c) 自治飞控：User App -> (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文件在px4\Firmware\src\modules\attitude_estimator_ekf\codegen\目录下
· 具体原理
· 程序详解
· 下一步

1.具体原理
EKF算法原理不再多讲，具体可参见上一篇blog 
这篇讲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"

/*
'输入参数：updateVect[3]：用来记录陀螺仪，加速度计，磁力计传感器数值是否有效
z[9] ：测量矩阵
x_aposteriori_k[12]：上一时刻状态后验估计矩阵，用来预测当前状态
P_aposteriori_k[144]：上一时刻后验估计方差
eulerAngles[3] ：输出欧拉角
Rot_matrix[9] ：输出余弦矩阵
x_aposteriori[12] ：输出状态后验估计矩阵
P_aposteriori[144] ：输出方差后验估计矩阵'
*/
void attitudeKalmanfilter(
const uint8_T updateVect[3],
real32_T dt,
const real32_T z[9],
const real32_T x_aposteriori_k[12],
const real32_T P_aposteriori_k[144],
const real32_T q[12],
real32_T r[9],
real32_T eulerAngles[3],
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 a[9];
int32_T i;
real32_T b_a[9];
real32_T x_n_b[3];
real32_T b_x_aposteriori_k[3];
real32_T z_n_b[3];
real32_T c_a[3];
real32_T d_a[3];
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 };

real32_T b_A_lin[144];
real32_T b_q[144];
real32_T c_A_lin[144];
real32_T d_A_lin[144];
real32_T e_A_lin[144]
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, 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_k[108];
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, 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_r[81];
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,
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0 };

real32_T c_r[9];
real32_T b_K_k[36];
real32_T d_P_apriori[72];
static const int8_T iv5[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_k[72];
static const int8_T iv6[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_T b_z[6];
static const int8_T iv7[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 };

static const int8_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_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
/* 
根据上述矩阵运算，可以得到： 

c_a[1∗3]=[axayaz]∗a[3∗3]
从而：
ω˜kra,kΔt[3∗1]=c_a[1∗3]T

d_a[1∗3]=[mxmymz]∗a[3∗3]
从而：
ω˜krm,kΔt[3∗1]=d_a[1∗3]T
其中
[axayaz]和[mxmymz]分别对应ra,k和rm,k的转置；
得到状态先验估计: 

Xk+1[12∗1]=x_apriori[1∗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_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 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_lin[6] = 0.0F;
A_lin[7] = x_aposteriori_k[8];
A_lin[8] = -x_aposteriori_k[7];
A_lin[18] = -x_aposteriori_k[8];
A_lin[19] = 0.0F;
A_lin[20] = x_aposteriori_k[6];
A_lin[30] = x_aposteriori_k[7];
A_lin[31] = -x_aposteriori_k[6];
A_lin[32] = 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] = O[i0 + 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_lin[9] = 0.0F;
A_lin[10] = x_aposteriori_k[11];
A_lin[11] = -x_aposteriori_k[10];
A_lin[21] = -x_aposteriori_k[11];
A_lin[22] = 0.0F;
A_lin[23] = x_aposteriori_k[9];
A_lin[33] = x_aposteriori_k[7];
A_lin[34] = -x_aposteriori_k[9];
A_lin[35] = 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] = O[i0 + 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_lin[12∗12]=⎡⎣⎢⎢⎢0I000000A10O0A200O⎤⎦⎥⎥⎥
其中各元素都是3*3的矩阵；I为单位矩阵，其中 

A1=⎡⎣⎢0−azayaz0−ax−ayax0⎤⎦⎥=−r˜a,Tk
同样
A2=⎡⎣⎢0−mzmymz0−mx−mymx0⎤⎦⎥=−r˜m,Tk
*/
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] *dt;
}
} //最终A_link,k的逆矩阵
· 1
· 2
· 3
· 4
· 5
得到： 

Alin,Tk=b_A_lin[12∗12]=⎡⎣⎢⎢⎢I0000I0000I0000I⎤⎦⎥⎥⎥+⎡⎣⎢⎢⎢0I000000A10O0A200O⎤⎦⎥⎥⎥∗dt
/* 
开始根据计算过程方差 
*/ 

过程噪声方差b_q[12∗12]=⎡⎣⎢⎢⎢⎢q00000q10000q20000q3⎤⎦⎥⎥⎥⎥
其中各元素都是3*3的矩阵；
b_q[0] = q[0];
b_q[12] = 0.0F;
b_q[24] = 0.0F;
b_q[36] = 0.0F;
b_q[48] = 0.0F;
b_q[60] = 0.0F;
b_q[72] = 0.0F;
b_q[84] = 0.0F;
b_q[96] = 0.0F;
b_q[108] = 0.0F;
b_q[120] = 0.0F;