推荐课程：
张飞四旋翼飞行器视频套件，76小时吃透四轴算法
http://t.elecfans.com/topic/40.html?elecfans_trackid=bbs_post
一.无人机的分类
按飞行平台构型分类：无人机可分为固定翼无人机，旋翼无人机，无人飞艇，伞翼无人机，扑翼无人机等.
图1 无人机平台构型
多轴飞行器multirotor是一种具有三个以上旋翼轴的特殊的直升机。旋翼的总距固定而不像一般直升机那样可变。通过改变不同旋翼相对转速可以改变单轴推进力的大小，从而控制飞行器的运行轨迹.
图2 多轴飞行器
图3 各类变模态平台
二.无人机的系统架构
图4 无人机系统架构
三.飞控系统简介
导航飞控系统之导航子系统功能：向无人机提供位置，速度，飞行姿态，引导无人
机沿指定航线安全，准时，准确的飞行。
获得必要的导航要素：高度，速度，姿态，航向
给出定位信息：经度，纬度，相对位移
引导飞机沿规定计划飞行
接收控制站的命令并执行
配合其它系统完成各种任务
1.飞控系统功能:
导航飞控之飞控子系统功能：完成起飞，空中飞行，执行任务，返航等整个飞行过
程的核习系统，对无人机实现全权控制与管理，是无人机的大脑。
无人机姿态稳定与控制
与导航子系统协调完成航迹控制
起飞与返航控制
无人机飞行管理
无人机任务管理与控制
应急控制
2.飞控系统--传感器:
飞控系统常用的传感器包括：
角速率传感器陀螺仪
图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 姿态控制
| 
电子发烧友论坛
3
/6 
工商网监
湘ICP备2023018690号
5643
淘帖
0
