pixhawk之uav控制理论
一、开篇
“您有无人机么？
没有。
那赶紧去某宝买一套。”
昨天开会开到接近下午一点钟，收获相当大，原本不太清楚的ardupilot框架现在也大致熟悉了，接下来主要就是结合源码了解其控制过程了，整体控制台过于复杂，还需要慢慢的研究。
但是，本篇博客还是不会太涉及那么多关于源代码的东西的，特别是关于通过代码实现控制理论的（其实我现在也不太懂~~~），下面还有很多的基本知识需要阐述，在阐述以后会先给出一部分的关于代码框架的东西，不是很深入，请结合源码再看后面的东西。
如果看到这里还对UAV、pixhawk不太了解的，请先阅读前一篇关于pixhawk的博客（地址：Pixhawk之前期准备）以及结合这个介绍的相当详细的网站：Pixhawk飞行控制器概览 。
下面会给出几个比较重要的框架图，希望各位看客熟记！！！
三、实验平台
Software Version：ArduCopter（Ver_3.3）
Hardware Version：pixhawk
IDE：eclipse Juno （Windows）
四、基本知识介绍
下面的5个部分您都了解么？
1）名词解释
惯性测量单元IMU(InertialMeasurementUnit)
姿态航向参考系统AHRS(Attitudeand Heading Reference System)
地磁角速度重力MARG(Magnetic,Angular Rate, and Gravity)
微机电系统MEMS(MicroElectrical Mechanical Systems)
自由度维数DOF(Dimension OfFreedom)
无人驾驶飞行器UAV(UnmannedAerial Vehicle)
扩展卡尔曼滤波EKF(ExtendedKalman Filter)
无损卡尔曼滤波UKF(UnscentedKalman Filter)
惯性导航系统INS(InertialNavigation System)
全球导航卫星系统GNSS(GlobalNavigation Satellite System)
天文导航系统CNS(CelestialNavigation System)
可垂直起降VTOL(VerticalTake-off and Landing)
2）坐标系介绍有两个基本坐标系：“地理”坐标系（Earth Frame）和“载体”坐标系(Body Frame)。”地理”坐标系指的就是地球上的“东北天（ENU）”坐标系，而“载体”坐标系值的就是四轴自己的坐标系。当我们在实际控制当中，我们关心的显然是载体坐标系相对于地理坐标系之间的变化，所以我们通常使用的旋转矩阵是把“地理”坐标系转到“载体”坐标系的矩阵，两者之间的转换关系自行百度吧，讲的很详细。转化的方法就是坐标系的转换，目前有三种方式：四元数（q0123）、欧拉角（yaw（Z轴）、pitch（Y轴）、roll（X轴）属于其中一种旋转顺序Z-Y-Xà航空次序欧拉角）、方向余弦矩阵（9个系数）。其中使用四元数运算比较快，但是它没有实际的物理含义，纯数学推到。
3）姿态数据
姿态的数据来源有5个：重力、地磁、陀螺仪、加速度计、电子罗盘。其中前两个来自“地理”坐标系，后三个来自“载体”坐标系。。在“地理”坐标系中，重力的值始终是（0，0，1g），地磁的值始终是（0，1，x）。这些值就是由放置在四轴上的传感器测量出来的。在单位时间内的位移被定义为速度，速度有线速度和角速度之分，分别对应两种传感器测量这两种不同的速度：线速度传感器（加速度计）、角速度传感器（陀螺仪）。
4）导航的基本原则
导航的基本原则就是保证两个基本坐标系的正确转化，没有误差。只有实现了这个原则，载体才可以在自己的坐标系中完成一系列动作而被转换到地理坐标系中看起来是正确的。为了达到这个目标，需要对两个坐标系进行实时的标定和修正。因为坐标系有三个轴，偏航yaw修正由电子罗盘（基于载体）、地磁（基于地理）对比修正误差补偿得到。俯仰pitch和横滚roll上的修正由加速度计（基于载体）、重力（基于地理）对比修正误差得到。在完成了基本原则的基础之后，即保证两个坐标系的正确转化后，利用基于载体上的陀螺仪进行积分运算，得到基于载体坐标系的姿态数据，经过一系列PID控制，给出控制量，完成基于载体坐标系上的稳定控制后，反应到地理坐标系上的稳定控制，从而达到我们观察到的定高、偏航、翻滚、倾仰等动作。下一篇博客会给出具体的PID回路控制框图，这篇博客就不添加了。
加速度计在地球上测量的是重力加速度，如果载体沿着z轴旋转，加速度计是无法感知他的运动的；类似的，电子罗盘测量的是地球上的磁场方向，如果载体沿着y轴旋转，电子罗盘同样也是无法感知他的运动的。综上所述，加速度计和电子罗盘只能得到2维的角度关系，通过某种方式的融合，可以得到正确的三维姿态信息。
对于上述论述可以看出，导航姿态从理论上讲只用陀螺仪是可以完成任务的。但是由于陀螺仪在积分过程中会产生误差累计，加上白噪声、温度偏差等会造成导航姿态的解算随着时间的流逝而逐渐增加。所以就需要用加速度计在水平面对重力进行比对和补偿，用来修正陀螺仪的误差。但是对于竖直轴上的旋转，加速度计是无能为力的，此时用的是电子罗盘。也可以测量出水平面内的地磁方向用来修正陀螺仪的水平误差。通过这两个器件的修正补偿，使得陀螺仪更加稳定、可靠的工作。
5）AHRS和IMU的差异AHRS由加速度计、磁场计、陀螺仪构成，AHRS的真正参考来自于地球的重力场和地球的磁场，它的静态精度取决于对磁场的测量精度和对重力的测量精度，而陀螺仪决定了他的动态性能。在这种前提下，说明AHRS离开了地球这种有重力和磁场环境的时候是没法正常工作的。而且特别注意，磁场和重力场越正交，航姿测量效果越好；也就是说如果磁场和重力场平行了，比如在地磁南北极。这里的磁场是向下的，即和重量场方向相同了。这个时候航线交是没法测出的，这是航姿系统的缺陷所在；在高纬度的地方航线角误差会越来越大。
IMU（Inertial measurement unit）学名惯性测量单元，大学的理论力学告诉我们，所有的运动都可以分解为一个直线运动和一个旋转运动，故这个惯性测量单元就是测量这两种运动，直线运动通过加速度计可以测量，旋转运动则通过陀螺。假设IMU的陀螺和加速度计的测量是没有任何误差的，那么通过陀螺则可以精确的测量物体的姿态。通过加速度计可以二次积分得出位移，实现完整的6DOF，也就是说你带着一台这种理论型的IMU在宇宙任何位置运动。我们都可以知道它当前的姿态和相对位移，这将不局限于任何场。
从上面的描述何以看出。实际上AHRS比IMU还多一个磁场传感器，而为什么AHRS的级别却低于IMU而需要依赖于重力场和磁场呢？这是由传感器器件架构所决定的。AHRS的传感器通常是成本低廉的mems传感器。这种传感器的陀螺仪和加速度计的噪声相对来说很大。以平面陀螺为例：用ADI的陀螺仪进行积分一分钟会漂移2度左右，这种前提下如果没有磁场和重力场来修正三轴陀螺的话。那么基本上3分钟以后物体的实际姿态和测量输出姿态就完全变样了，所以在这种低价陀螺仪和加速度计的架构下必须运用场向量来进行修正，而IMU实际上也是这样的。因为我们知道没有绝对精确的传感器，只有相对精确的传感器，IMU的陀螺仪用的是光纤陀螺或者机械陀螺，这种陀螺的成本很高。精度相对mems陀螺也很高，精度高不代表准确，IMU的姿态精度参数通常是一小时飘多少度。
而用加速度计积分做位置的话。AHRS是不现实的（1分钟就能飘出几十米，而且是成二次方的速度递增）。AHRS通常要结合GPS和气压计做位置，IMU积分做位置的是一天多少海里。这样的一个参数数量级。也许在海上还能用的到，这就是AHRS和IMU在我的理解里的一个差异。
五、源码框架介绍先来一发高清好图，一直再找它，终于找到了，正所谓“众里寻它千百度，蓦然回首那图却在灯火阑珊处”。希望大家能从该图大致的理解这个该死的pixhawk代码框架，下图就不做解释了，肯定都能看的懂。
上面的图记下了么？记下了接着往下看~~~
1）阅读下面内容时请结合源码阅读，便于理解。
The basic structure of ArduPilot is broken up into 5 main parts：
(1)vehicle directories
(2) AP_HAL
(3) libraries
(4) tools directories
(5) external support code
(1)Vehicle Directories
The vehicle directories are the top level directories that define the firmware for each vehicle type. Currently there are 4 vehicle types – Plane、 Copter、APMrover2 and AntennaTracker。
Along with the *.cpp files, each vehicle directory contains a make.inc file which lists library dependencies. The Makefiles read this to create the -I and -L flags for the build.
(2) AP_HAL
The AP_HAL layer (Hardware Abstraction Layer) is how we make ArduPilot portable to lots of different platforms。 There is a top level AP_HAL in libraries/AP_HAL that defines the interface that the rest of the code has to specific board features, then there is a AP_HAL_XXX subdirectory for each board type, for example AP_HAL_AVR for AVR based boards, AP_HAL_PX4 for PX4 boards and AP_HAL_Linux for Linux based boards。
(3) libraries
(4) Tools directories
The tools directories are miscellaneous support directories. For examples, tools/autotest provides the autotest infrastructure behind the autotest.diydrones.com site and tools/Replay provides our log replay utility.
(5) External support code
On some platforms we need external support code to provide additional features or board support. Currently the external trees are:
PX4NuttX – the core NuttX RTOS used on PX4 boards
PX4Firmware – the base PX4 middleware and drivers used on PX4 boards
uavcan – the uavcan CANBUS implementation used in ArduPilot
mavlink – the mavlink protocol and code generator
（1）核心库
AP_AHRS：采用DCM（方向余弦矩阵方法）或EKF（扩展卡尔曼滤波方法）预估飞行器姿态。
AP_Common：所有执行文件（sketch格式，arduino IDE的文件）和其他库都需要的基础核心库。
AP_Math：包含了许多数学函数，特别对于矢量运算。
AC_PID：PID控制器库。
AP_InertialNav：扩展带有gps和气压计数据的惯性导航库。
AC_AttitudeControl：姿态控制相关库。
AP_WPNav：航点相关的导航库。
AP_Motors：多旋翼和传统直升机混合的电机库。
RC_Channel：更多的关于从APM_RC的PWM输入/输出数据转换到内部通用单位的库，比如角度。
AP_HAL，AP_HAL_AVR，AP_HAL_PX4:硬件抽象层库，提供给其他高级控制代码一致的接口，而不必担心底层不同的硬件。AP_HAL_PX4：GPIO、I2C、UART、RCinput/output、scheduler、semaphores、storage。
（2）传感器相关库
AP_InertialSensor：读取陀螺仪和加速度计数据，并向主程序执行标准程序和提供标准单位数据（deg/s，m/s）。
AP_RangerFinder：声呐和红外测距传感器的交互库
AP_Baro：气压计相关库
AP_GPS：GPS相关库
AP_Compass：三轴罗盘相关库
AP_OpticalFlow：光流传感器相关库
（3）其他库
AP_Mount，AP_Camera, AP_Relay：相机安装控制库，相机快门控制库
AP_Mission： 从eeprom（电可擦只读存储器）存储/读取飞行指令相关库
AP_Buffer：惯性导航时所用到的一个简单的堆栈（FIFO，先进先出）缓冲区
AP_AccelCal、AP_Declination、AP_RCMapper、AP_RPM、AP_RSSI
AP_ADC：Analog to Digital
APM_Control： pitch/roll/yaw controller
DataFlash：flash memory
GCS_Console/GCS_MAVLink：地面站通信、飞行日志
3）关于主控MCU STM32F4的选择和协处理器STM32F1
在源代码中，大部分代码都是运行在主控MCU STM32F4芯片上，并且是通过直接配置寄存器来实现相应的功能，代码位于“/ardupilot/modules/PX4Firmware/Build/px4fmu-v2_APM.build/nuttx-export/arch/chip”中。
4）关于pixhawk使用的OS：NuttX
在modules /PX4NuttX/nuttx/sched文件中有os_start.c定义文件，内部进行了一系列的关于操作系统的初始化。在os_start()函数里面进行了如下初始化。
以上并非必须初始化，可以有选择性的初始化。Ifdef/ifndef….
重点了解一下关于nuttx中的modules /PX4NuttX/nuttx/mm。
5）姿态控制的软件流程
简单的软件流程官方给出了大致的介绍，该部分详见官方介绍：Code Overview
六、再深入一点
如下顺序不分先后，都是平时自己整理的，等以后对ardupilot的整体框架了解的比较透彻以后再回头修改吧。
1）关于飞行模式
2）关于参数的使用
3）关于一些报警和灯显、日志
4）关于校准和失控保护
5）关于RC输入和输出（接收机）
6）关于机型选择和电机控制
代码赏析：如下事例摘自motors.cpp中的一段关于电机解锁和上锁的源码，这段代码试飞过无人机的肯定一看就懂~~~但是，如果没有感性的是飞过无人机，那么。。。。。。。您自己理解吧
- // arm_motors_check - checks for pilot input to arm or disarm the copter
- // called at 10hz
- void Copter::arm_motors_check()
- {
- static int16_t arming_counter;
- // ensure throttle is down首先判断油门是否为最小
- if (channel_throttle->control_in > 0)
- {
- arming_counter = 0;
- return;
- }
- //油门最小则检测yaw的行程量
- int16_t tmp = channel_yaw->control_in;
- // full right 解锁
- if (tmp > 4000)
- {
- // increase the arming counter to a maximum of 1 beyond the auto trim counter
- if( arming_counter <= AUTO_TRIM_DELAY )
- {
- arming_counter++;
- }
- // arm the motors and configure for flight
- if (arming_counter == ARM_DELAY && !motors.armed())
- {
- // reset arming counter if arming fail
- if (!init_arm_motors(false))
- {
- arming_counter = 0;
- }
- }
- // arm the motors and configure for flight
- if (arming_counter == AUTO_TRIM_DELAY && motors.armed() && control_mode == STABILIZE)
- {
- auto_trim_counter = 250;
- // ensure auto-disarm doesn’t trigger immediately
- auto_disarm_begin = millis();
- }
- // full left 上锁
- }
- else if (tmp < -4000)
- {
- if (!mode_has_manual_throttle(control_mode) && !ap.land_complete) {
- arming_counter = 0;
- return;
- }
- // increase the counter to a maximum of 1 beyond the disarm delay
- if( arming_counter <= DISARM_DELAY )
- {
- arming_counter++;
- }
- // disarm the motors
- if (arming_counter == DISARM_DELAY && motors.armed())
- {
- init_disarm_motors();
- }
- // Yaw is centered so reset arming counter
- }
- Else
- {
- arming_counter = 0;
- }
- }
// arm_motors_check - checks for pilot input to arm or disarm the copter
// called at 10hz
void Copter::arm_motors_check()
{
static int16_t arming_counter;
// ensure throttle is down首先判断油门是否为最小
if (channel_throttle->control_in > 0)
{
arming_counter = 0;
return;
}
//油门最小则检测yaw的行程量
int16_t tmp = channel_yaw->control_in;
// full right 解锁
if (tmp > 4000)
{
// increase the arming counter to a maximum of 1 beyond the auto trim counter
if( arming_counter <= AUTO_TRIM_DELAY )
{
arming_counter++;
}
// arm the motors and configure for flight
if (arming_counter == ARM_DELAY && !motors.armed())
{
// reset arming counter if arming fail
if (!init_arm_motors(false))
{
arming_counter = 0;
}
}
// arm the motors and configure for flight
if (arming_counter == AUTO_TRIM_DELAY && motors.armed() && control_mode == STABILIZE)
{
auto_trim_counter = 250;
// ensure auto-disarm doesn't trigger immediately
auto_disarm_begin = millis();
}
// full left 上锁
}
else if (tmp < -4000)
{
if (!mode_has_manual_throttle(control_mode) && !ap.land_complete) {
arming_counter = 0;
return;
}
// increase the counter to a maximum of 1 beyond the disarm delay
if( arming_counter <= DISARM_DELAY )
{
arming_counter++;
}
// disarm the motors
if (arming_counter == DISARM_DELAY && motors.armed())
{
init_disarm_motors();
}
// Yaw is centered so reset arming counter
}
Else
{
arming_counter = 0;
}
}
七、总结
阅读源码是在eclipse中直接看的，没有用SI（据说很高级，但是用不习惯）。通过上述介绍，对ardupilot这套代码有了初步的理解，初期接触这套代码的第一感觉就是“What’s the fucking code!”，经过一段时间的不懈努力，终于有了些头绪，苍天不负有心人。
接下来的博客内容是关于这套ardupilot代码如何使用操作系统的、Where is the fucking function main?、进程问题，还有就是验证控制回路，PID的使用，EKF等等，应该会分开写吧，这么多东西~~~
整整写了一晚上啊，写博客真不是一件容易的事情，写的有点乱，没头没尾的，希望各位看客喜欢。有错误的请帮忙指出来，大家一起进步~~~~