http://www.uavrs.com
Ardupilot 飞控架构分析
作者:敬文义
时间:2019/12/05
开源代码路径:
Ardupilot:https://github.com/ArduPilot/ardupilot
1
http://www.uavrs.com
1、姿态解算
IMU 原始数据 iir 滤波
姿态更新代码流程
2
http://www.uavrs.com
DCM 方向余弦矩阵更新姿态原理
DCMDraft2.pdf
fastRotations.pdf RollPitchDriftCompensation.pdf(理论在这 3 篇文章中)
代码中对应的公式在以上 3 篇文章中
3
http://www.uavrs.com
EKF 姿态解算
4
http://www.uavrs.com
2、导航控制
悬停模式下
5
http://www.uavrs.com
自动模式下
代码思路:
1) 、求出航点 A 到航点 B 的轨道的单位向量
2) 、计算约束条件,单位时间在单位向量上能够提供的最大加速度、最大速度、最大行驶
距离、刹车距离
3) 、把飞机当前位置 P 点的位置矢量投影到 AB 轨道单位向量上,求出轨道上现在对于的
位置 M 点,求出 MP 矢量;
4) 、PN 是最大行驶距离,MN*MN = PN*PN - PM*PM, MN 是下一个时刻行驶的最大距离;
5) 、下一个时刻轨道上期望行驶的最大距离 AN = AM + MN;
6) 、飞机的位置可能不在 AB 轨道上,同样 AB 的速度也可能不在 AB 轨道上,把飞机的速
度投影到 AB 轨道上,这里分两种情况,一种投影的速度与 AB 同向,另一种投影的速度与
AB 反向;
7) 、如果反向:限制轨道运行速度为 = 0;
8) 、如果同向:限制轨道速度 + = 2*a*t;(这里乘 2 因为 xy, z 都可以提供 a 这么大的加速
度,最大就可以提供 2 倍)
9) 、通过求出的限制轨道速度在时间上积分求出期望在轨道上运行的距离;
10) 、从轨道距离转到空间距离,加上期望改变的高度,计算出下一时刻期望的位置;
11) 、通过可接受半径判断是否到达 B 点。
6
http://www.uavrs.com
7
http://www.uavrs.com
8