无人机系列之飞控算法

一.无人机的分类

按飞行平台构型分类：无人机可分为固定翼无人机，旋翼无人机，无人飞艇，伞翼无人机，扑翼无人机等.

图1 无人机平台构型

多轴飞行器multirotor是一种具有三个以上旋翼轴的特殊的直升机。旋翼的总距固定而不像一般直升机那样可变。通过改变不同旋翼相对转速可以改变单轴推进力的大小，从而控制飞行器的运行轨迹.

图2 多轴飞行器

图3 各类变模态平台

二.无人机的系统架构

图4 无人机系统架构

三.飞控系统简介

导航飞控系统之导航子系统功能：向无人机提供位置，速度，飞行姿态，引导无人
机沿指定航线安全，准时，准确的飞行。
 获得必要的导航要素：高度，速度，姿态，航向
 给出定位信息：经度，纬度，相对位移
 引导飞机沿规定计划飞行
 接收控制站的命令并执行
 配合其它系统完成各种任务

1.飞控系统功能:

导航飞控之飞控子系统功能：完成起飞，空中飞行，执行任务，返航等整个飞行过
程的核习系统，对无人机实现全权控制与管理，是无人机的大脑。
 无人机姿态稳定与控制
 与导航子系统协调完成航迹控制
 起飞与返航控制
 无人机飞行管理
 无人机任务管理与控制
 应急控制

2.飞控系统--传感器:

 飞控系统常用的传感器包括：
 角速率传感器陀螺仪

图5 陀螺仪

 加速度传感器

图5 加速计

 气压计和超声波

图5 声纳与气压二合一

 GPS

图6 GPS示意图

 光流

从二维图像序列中检测物体运动、提取运动参数并且分析物体运动的相关规律
 光流是空间运动物体在观测成像平面上的像素运动的“瞬时速度”
 用于飞行器的动态定位和辅助惯性导航
 Lucas Kanade算法
这个算法是最常见，最流行的。它计算两帧在时间t到t + δt之间每个像素点位置的移动

图7 光流算法示意图

 地磁传感器

图8 磁力计

四.飞控系统的关键算法

1.关键算法流程框图

图9 关键算法流程框图

2.姿态解算

（1）init函数初始化，建立3x3矩阵R。
（2）磁力计修正，得到误差corr：先计算得到误差角度mag_er，再用_wrap_pi函数做约束，再计算corr值，相当于机体坐标系绕地理坐标系N轴（Z轴）转动arctan（mag_earth(1), mag_earth(0)）度。
（3）加速度计修正更新误差corr：将陀螺仪计算得到的矩阵第三行（即重力加速度部分）转换到b系，再将加速度测得重力加速度（_accel - 机体加速度）的数据归一化（本身属于b系），将这两个的值进行叉乘即测得误差。具体过程：归一化的n系重力加速度通过旋转矩阵R左乘旋转到b系，即k为归一化的旋转矩阵R（b-e）的第三行，总的受到合力的方向（_accel）减去机体加速度方向（_pos_acc）得到g的方向，即总加速度（加速度获取）减去机体运动加速度（第五部分）获取重力加速度，然后姿态矩阵的不是行就是列来与纯重力加速度来做叉积，算出误差。因为运动加速度是有害的干扰，必须减掉。算法的理论基础是[0,0,1]与姿态矩阵相乘。该差值获取的重力加速度的方向是导航坐标系下的z轴，加上运动加速度之后，总加速度的方向就不是与导航坐标系的天或地平行了，所以要消除这个误差，即“_accel-_pos_acc”。然后叉乘z轴向量得到误差，进行校准 。
（4）对误差corr进行PI控制器中的I（积分），得到_gyro_bias，再对_gyro_bias做约束处理。
（5）使用修正的数据更新四元数，并把_rates和_gyro_bias置零便于下次调用时使用。

图10 姿态解算

3.姿态控制

3.1.姿态角度控制

【1】计算误差值e_R：
（1）获取目标姿态target，并构建目标姿态旋转矩阵。
（2）通过控制四元数获取当前状态的旋转矩阵DCM。
（3）取两个矩阵中的Z轴向量，即YAW-axis。
（4）计算roll，pitch误差，得到误差值e_R：通过R_z%R_sp_z叉乘当前姿态的z轴和目标姿态的z轴的误差大小（即需要旋转的角度）并旋转到b系（即先对齐Z轴）。
（5）计算yaw的权重.
（6）构造e_R_cp反对称矩阵，通过罗德里格公式旋转得到roll，pitch旋转后的矩阵R_rp。
（7）计算yaw的误差，进一步更新误差值e_R：roll_pitch旋转后的矩阵的x轴和目标姿态的x轴的误差，乘上yaw的权重。
【2】计算e_R_d：
（1）目标姿态旋转矩阵获取四元数。
（2）对四元数的虚部取出赋值给e_R_d，再对其进行归一化处理。
（3）对四元数的实部取出。
（4）计算e_R_d：通过虚部与实部的一系列计算得来。
【3】计算direct_w
【4】进一步更新误差值e_R：通过e_R *  (1 - direct_w) + e_R_d * direct_w。
【5】得到_rates_sp角速度变量：对e_R进行p控制，再进行约束

3.2.姿态角速度控制

【1】获取当前角速度值rates：通过_ctrl_state数据结构（当前姿态信息）把需要的有效数据赋值给rates。
【2】获取目前角速度值_rates_sp。
【3】计算得到角速度差rates_err。
【4】对角速度差rates_err进行PD控制，还需要加一个前馈。
【5】发布控制量_att_control。

图11 姿态控制