PX4的无人机飞控应用开发.docx
- 文档编号:7719483
- 上传时间:2023-01-26
- 格式:DOCX
- 页数:75
- 大小:104.89KB
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
/*
根据上述矩阵运算
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- PX4 无人机 应用 开发
![提示](https://static.bdocx.com/images/bang_tan.gif)