无人机控制算法

课程内容及答疑：
https://www.shenlanxueyuan.com/course/385
手动 or 自动？控制输入是？
空中机器人的发展

Scale/cost down

Agility/autonomy up
 感知-规划-控制：闭环系统
传感

建图 定位

⚫ 估计 ⚫ 感知
规划 ➢ 低延迟 ➢ 三维 & 稠密感知
➢ 高精度 & 一致性 ➢ 地图维护 & 作用于规划

控制 ⚫ 规划 ⚫ 控制
➢ 应对复杂 & 未知环境 ➢ 高机动运动
➢ 安全 & 动力学可行性 ➢ 光滑轨迹跟踪
➢ 有限的传感器 & 计算能力

https://github.com/HKUST-Aerial-Robotics/Teach-Repeat-Replan/tree/experiment/onboard_computer/controller/n3ctrl
控制
控制

6
定位+控制
定位+建图+规划+控制
定位+建图+规划+控制+跟踪

9
定位+建图+规划+控制+跟踪

10
定位+建图+规划+控制+集群

11
简简单单的悬停

12
高速高机动控制

13
四旋翼飞行器动力学

14
 四旋翼飞行器的动力学
• 电机模型: 𝜔ሶ 𝑖 = 𝑘𝑚 (𝜔𝑖𝑑𝑒𝑠 − 𝜔𝑖 )
• 单个电机产生的推力: 𝐹𝑖 = 𝑘𝐹 𝜔𝑖2
• 单个电机产生的力矩: 𝑀𝑖 = 𝑘𝑀 𝜔𝑖2

𝒛𝑏

𝒛𝑎
𝒚𝑏

𝒙𝑏
𝒚𝑎

𝒙𝑎
15
 四旋翼飞行器的动力学

• Z-X-Y 欧拉角: 𝑹𝑎𝑏 = 𝑹𝑧 𝜓 ⋅ 𝑹𝑥 𝜙 ⋅ 𝑹𝑦 𝜃

• 围绕机体系三个轴的旋转顺序

𝒛𝑏

𝒛𝑎
𝒚𝑏

𝒙𝑏
𝒚𝑎

𝒙𝑎
16
 四旋翼飞行器的动力学

𝑐𝜓𝑐𝜃 − 𝑠𝜙𝑠𝜓𝑠𝜃 −𝑐𝜙𝑠𝜓 𝑐𝜓𝑠𝜃 + 𝑐𝜃𝑠𝜙𝑠𝜓

• 𝑹𝑎𝑏 = 𝑐𝜃𝑠𝜓 + 𝑐𝜓𝑠𝜙𝑠𝜃 𝑐𝜙𝑐𝜓 𝑠𝜓𝑠𝜃 − 𝑐𝜓𝑐𝜃𝑠𝜙
−𝑐𝜙𝑠𝜃 𝑠𝜙 𝑐𝜙𝑐𝜃

𝜔𝑥 𝑐𝜃 0 −𝑐𝜙𝑠𝜃 𝜙ሶ
• 𝜔𝑦 = 0 1 𝑠𝜙 𝜃ሶ
𝜔𝑧 𝑠𝜃 0 𝑐𝜙𝑐𝜃 𝜓ሶ
𝒛𝑏
机体系下，机体旋
转的即时角速度 𝒛𝑎
𝒚𝑏

𝒙𝑏
𝒚𝑎

𝒙𝑎
17
 牛顿-欧拉方程
0 0
• 牛顿方程: 𝑚𝒑𝑎ሷ = 0 + 𝑹𝑎𝑏 0
−𝑚𝑔 𝐹1 + 𝐹2 + 𝐹3 + 𝐹4
𝐹3 , 𝑀3
𝐹4 , 𝑀4

𝑙 𝒛𝑏

𝒛𝑎 𝐹2 , 𝑀2
𝒚𝑏

𝒙𝑏
𝒚𝑎

𝒙𝑎 𝐹1 , 𝑀1
18
 牛顿欧拉方程
𝜔𝑥ሶ 𝜔𝑥 𝜔𝑥 𝑙(𝐹2 − 𝐹4 )
• 欧拉方程: 𝑰𝑏 ⋅ 𝜔𝑦ሶ + 𝜔𝑦 × 𝑰𝑏 ⋅ 𝜔𝑦 = 𝑙 𝐹3 − 𝐹1
𝜔𝑧ሶ 𝜔𝑧 𝜔𝑧 𝑀1 − 𝑀2 + 𝑀3 − 𝑀4
𝐹3 , 𝑀3
𝐹4 , 𝑀4

𝑙 𝒛𝑏

𝒛𝑎 𝐹2 , 𝑀2
𝒚𝑏

𝒙𝑏
𝒚𝑎

𝒙𝑎 𝐹1 , 𝑀1
19
 四旋翼动力学
• 电机模型: 𝜔ሶ 𝑖 = 𝑘𝑚 (𝜔𝑖𝑑𝑒𝑠 − 𝜔𝑖 )
• 单个电机的推力: 𝐹𝑖 = 𝑘𝐹 𝜔𝑖2
• 单个电机的转矩: 𝑀𝑖 = 𝑘𝑀 𝜔𝑖2
𝜔𝑥ሶ 𝜔𝑥 𝜔𝑥 𝑙(𝐹2 − 𝐹4 )
• 欧拉方程: 𝑰𝑏 ⋅ 𝜔𝑦ሶ + 𝜔𝑦 × 𝑰𝑏 ⋅ 𝜔𝑦 = 𝑙 𝐹3 − 𝐹1
𝜔𝑧ሶ 𝜔𝑧 𝜔𝑧 𝑀1 − 𝑀2 + 𝑀3 − 𝑀4
𝜔𝑥 𝑐𝜃 0 −𝑐𝜙𝑠𝜃 𝜙ሶ
• 即时角速度与欧拉角的导数之间的转换: 𝜔𝑦 = 0 1 𝑠𝜙 𝜃ሶ
𝜔𝑧 𝑠𝜃 0 𝑐𝜙𝑐𝜃 𝜓ሶ
𝑐𝜓𝑐𝜃 − 𝑠𝜙𝑠𝜓𝑠𝜃 −𝑐𝜙𝑠𝜓 𝑐𝜓𝑠𝜃 + 𝑐𝜃𝑠𝜙𝑠𝜓
• 旋转矩阵: 𝑹𝑎𝑏 = 𝑐𝜃𝑠𝜓 + 𝑐𝜓𝑠𝜙𝑠𝜃 𝑐𝜙𝑐𝜓 𝑠𝜓𝑠𝜃 − 𝑐𝜓𝑐𝜃𝑠𝜙
−𝑐𝜙𝑠𝜃 𝑠𝜙 𝑐𝜙𝑐𝜃

0 0
• 牛顿方程: 𝑚𝒑𝑎ሷ = 0 + 𝑹𝑎𝑏 0
−𝑚𝑔 𝐹1 + 𝐹2 + 𝐹3 + 𝐹4 20
 平面旋翼

1
− sin 𝜙 0
𝑦ሷ 0 𝑚
1 𝐹
• 𝑧ሷ = −𝑔 + 𝑚
cos 𝜙 0
𝑀
𝜙ሷ 0 1
0 𝐹 𝒛𝑏
𝐼𝑥𝑥 𝒚𝑏

𝑀
𝒛𝑎 𝜙

𝒚𝑎

21
控制系统设计

22
 控制系统设计

• 控制系统介绍
• 线性时不变(LTI)系统
• 简单的一阶系统
• 简单的二阶系统
• 控制器设计
• 增益调节
• 基于模型的控制

23
 控制系统介绍

要求机器人 如果机器人
沿着一条直 的运动受到
线运动或者 扰动
尽可能靠近
这条直线运
动
机器人就
会偏离目
标

❖ 因此，基于机器人的实际运动和目标，我们需要一个控制器来实时控制它的运动

24
控制系统介绍

25
 控制系统介绍

• 开环控制
• 以预先确定的方式移动机器人
• 举例：闭着眼睛走路
• 闭环（反馈）控制
• 根据输出(例如机器人的位置)来调整机器人运动的输入(例如方向，也可
能是速度)
• 也称之为反馈控制，因为控制决策是根据输出的反馈做出的
• 举例：睁着眼睛走路
• 使用闭环控制来使系统稳定

26
 控制系统介绍

• 控制的目标之一是使被控对象稳定，并尽可能精确且快
速地跟踪给定的参考输入信号

r: 参考输入
控制器 被控对象 u: 控制器的输入
d: 扰动
y: 被控对象输出
+ 传感器 n: 通信噪声

• 控制器只是一个计算单元，它计算“最优的”或“期望的”
输入给到被控对象

“反馈是一种通过将系统过去表现的结果作用于该系统来控制这个系统的方法”
27
 控制系统介绍

举例：洗澡

前向通路
期望温度𝑇𝑑 角度𝜃 输出温度𝑇𝑜𝑢𝑡
+
神经系统 手 水阀
- 控制器 执行器 装置

“测量”得到的温度
皮肤
反馈回路
传感器
28
 控制系统介绍

• 上升时间 Tr（Rise time）:

• 响应值从10% 到 90%的用时
• 稳态误差（Steady-state error）
• 超调量（Overshoot）
• 峰值超过最终值的百分比

• 校正时间 Ts（Settling time）

• 让响应值与其最终值的偏差在1%以内的用时
• 一个好的控制系统应该是上升时间短、校正时间短、超调量小且稳
态误差小的

29
 控制系统介绍

• 举例：洗澡水温度控制（开关控制）
• 如果水温低于期望值，增加加热器功率
• 如果水温高于期望值，降低加热器功率
• 简单
• 过渡不平滑
水温

加热器功率

30
 简单一阶系统的控制
• 问题
状态，输入
𝑥, 𝑢 ∈ 𝑹
对象的运动学模型
𝑥ሶ = 𝑢
想要状态 𝑥 去跟随期望轨迹 𝑥 𝑑𝑒𝑠 (𝑡)
• 方法
定义误差, 𝑒 𝑡 = 𝑥 𝑑𝑒𝑠 𝑡 − 𝑥(𝑡)
想要误差 𝑒 𝑡 指数收敛到 0
• 策略
找到能够满足下面要求的输入 𝑢
𝑒ሶ + 𝐾𝑝 𝑒 = 0 𝐾𝑝 > 0
𝑢 𝑡 = 𝑥ሶ 𝑑𝑒𝑠 𝑡 + 𝐾𝑝 𝑒(𝑡)

31
前馈 比例
 简单二阶系统的控制
• 问题
状态，输入
𝑥, 𝑢 ∈ 𝑹
对象的运动学模型
𝑥ሷ = 𝑢
想要状态 𝑥 去跟随期望轨迹 𝑥 𝑑𝑒𝑠 (𝑡)
• 方法
定义误差, 𝑒 𝑡 = 𝑥 𝑑𝑒𝑠 𝑡 − 𝑥(𝑡)
想要误差 𝑒 𝑡 指数收敛到 0
• 策略
找到能够满足下面要求的输入 𝑢
𝑒ሷ + 𝐾𝑑 𝑒ሶ + 𝐾𝑝 𝑒 = 0 𝐾𝑑 , 𝐾𝑝 > 0
𝑢 𝑡 = 𝑥ሷ 𝑑𝑒𝑠 𝑡 + 𝐾𝑑 𝑒ሶ 𝑡 + 𝐾𝑝 𝑒(𝑡)

32
前馈 微分 比例
 PD控制和PID控制
• PD 控制
𝑢 𝑡 = 𝑥ሷ 𝑑𝑒𝑠 𝑡 + 𝐾𝑑 𝑒ሶ 𝑡 + 𝐾𝑝 𝑒(𝑡)
比例控制的作用类似于弹簧响应
微分控制的作用类似于粘性阻尼响应
微分增益过大会导致系统过阻尼，收敛速度慢

• PID 控制
𝑡
𝑢 𝑡 = 𝑥ሷ 𝑑𝑒𝑠 𝑡 + 𝐾𝑑 𝑒ሶ 𝑡 + 𝐾𝑝 𝑒 𝑡 + 𝐾𝐼 න 𝑒 𝜏 𝑑𝜏
0

积分
PID 控制产生了一个三阶的闭环系统
积分控制使得稳态误差趋近于0
33
 增益调节

稳定 临界稳定 不稳定
(收敛) (振荡) (发散)

34
人工整定

35
 人工整定

参数变化 𝐾𝑝 ↑ 𝐾𝑑 ↑ 𝐾𝑖 ↑

Rise Time（上升时间） 减少 - 减少

Overshoot（超调量） 增加 减少 增加

Setting Time（校正时间） - 减少 增加

Steady-State Error（稳态误差） 减少 - 消除

36
 人工整定
PD 控制器 调高𝐾𝑝 (超调变大)

调低𝐾𝑝 (慢响应) 调高𝐾𝑑 (过阻尼)

37
 Ziegler-Nichols 整定法

• 调节增益时的启示（建议）
1. 设置 𝐾𝑖 = 𝐾𝑑 = 0
2. 增大 𝐾𝑝 直到输出发生震荡，此时𝐾𝑝 记为最大增益𝐾𝑢
3. 记录增益为𝐾𝑢 时的振荡周期 𝑇𝑢
4. 根据下表来设置增益参数:

控制器 𝐾𝑝 𝐾𝑑 𝐾𝑖
P 0.5𝐾𝑢 - -
PD 0.8𝐾𝑢 𝐾𝑝 𝑇𝑢 /8 -

PID 0.6𝐾𝑢 𝐾𝑝 𝑇𝑢 /8 2𝐾𝑝 /𝑇𝑢

38
 基于模型的控制
• 一个普通的二阶模型
𝑚𝑥ሷ 𝑡 + 𝑏𝑥(𝑡)
ሶ + 𝑘𝑥(𝑡) = 𝑢(t)

• PID 或 PD 控制方案的缺点
𝑢 𝑡 = 𝑥ሷ 𝑑𝑒𝑠 𝑡 + 𝐾𝑑 𝑒ሶ 𝑡 + 𝐾𝑝 𝑒(𝑡)
• 表现效果取决于模型
• 需要调节增益参数来获得最佳的效果

• 基于模型的控制律 基于模型的（估计值）

ෝ 𝑥ሷ 𝑑𝑒𝑠 𝑡 + 𝐾𝑑 𝑒ሶ 𝑡 + 𝐾𝑝 𝑒(𝑡)) + 𝑏෠ 𝑥(𝑡)

𝑢 𝑡 = 𝑚( ሶ ෠
+ 𝑘𝑥(𝑡)

• 基于模型的部分 伺服：前馈+ PD反馈

• 抵消了系统的动力学
• 特定于模型
• 基于伺服的部分
• 使用带有前馈的PID或PD控制器来将执行误差消除到0
39
• 独立于系统模型
 基于模型的控制

• 基于模型的控制方案
基于模型的（估计值）

ෝ 𝑥ሷ 𝑑𝑒𝑠 𝑡 + 𝐾𝑑 𝑒ሶ 𝑡 + 𝐾𝑝 𝑒(𝑡)) + 𝑏෠ 𝑥(𝑡)

𝑢 𝑡 = 𝑚( ሶ ෠
+ 𝑘𝑥(𝑡)

• 优势 伺服：前馈+ PD反馈
• 将控制分解成两部分
• 依赖于模型的部分(取决于模型的精确程度)
• 独立于模型的部分(伺服控制，增益与模型无关)

• 劣势
• 基于对模型参数的估计
• 理想表现
𝑒ሷ + 𝐾𝑑 𝑒ሶ + 𝐾𝑝 𝑒 = 0
• 实际表现
𝑚 ෠
(𝑏 − 𝑏) ෠
(𝑘 − 𝑘)
𝑒ሷ + 𝐾𝑑 𝑒ሶ + 𝐾𝑝 𝑒 = − 1 𝑥ሷ + 𝑥ሶ + 𝑥
𝑚
ෝ 𝑚
ෝ 𝑚
ෝ 40
 基于模型的控制
• 表现
ෝ 𝑥ሷ 𝑑𝑒𝑠 𝑡 + 𝐾𝑑 𝑒ሶ 𝑡 + 𝐾𝑝 𝑒(𝑡)) + 𝑏෠ 𝑥(𝑡)
𝑢 𝑡 = 𝑚( ሶ ෠
+ 𝑘𝑥(𝑡)

精确模型 不精确模型，模型参数误差10%

41
四旋翼控制

42
 应用于四旋翼
z

𝑢1 = 推力
𝑢2 = 力矩

43
 平面四旋翼模型
z

𝑢1 = 推力
𝑢2 = 力矩

1
− 𝑠𝑖𝑛ϕ 0
෍ 𝐹𝑦 = − 𝑢1 𝑠𝑖𝑛 𝜙 = 𝑚𝑦ሷ 𝑚
𝑦ሷ 0 1 𝑢1
𝑧ሷ = −𝑔 + 𝑐𝑜𝑠ϕ 0 𝑢2
෍ 𝐹𝑧 = − 𝑚𝑔 + 𝑢1 𝑐𝑜𝑠 𝜙 = 𝑚𝑧ሷ ϕሷ 𝑚
0 1
0
𝑀 = 𝑢2 = 𝐼𝑥𝑥 ϕሷ 𝐼𝑥𝑥
44
 线性化的动力学模型

• 非线性动力学模型
𝑢
𝑦ሷ = − 𝑚1 𝑠𝑖𝑛(ϕ)
𝑢1
𝑧ሷ = −𝑔 + 𝑐𝑜𝑠(ϕ)
𝑚
𝑢
ϕሷ = 2𝐼𝑥𝑥

• 平衡悬停态
𝑦0 , 𝑧0 , ϕ0 = 0, 𝑢1,0 = 𝑚𝑔, 𝑢2,0 = 0

• 线性化的动力学模型
𝑦ሷ = −𝑔ϕ 级联的二阶系统
𝑢1
𝑧ሷ = −𝑔 + 𝑚
𝑢
ϕሷ = 2 一个简单的二阶系统
𝐼𝑥𝑥

45
 控制回路结构

𝑢1
𝑦𝑑𝑒𝑠 , 𝑧𝑑𝑒𝑠
𝑦ሶ 𝑑𝑒𝑠 , 𝑧ሶ𝑑𝑒𝑠
𝑦ሷ 𝑑𝑒𝑠 , 𝑧ሷ𝑑𝑒𝑠
位置 ϕ𝑐 , ϕሶ 𝑐 姿态 𝑢2
控制器 控制器

𝑦, 𝑧, 𝑦,ሶ 𝑧ሶ ϕ, ϕሶ

46
 控制方程组
• Z轴方向位置控制
PD控制: 𝑧ሷ𝑐 = 𝑧ሷ 𝑑𝑒𝑠 + 𝐾𝑑,𝑧 (𝑧ሶ 𝑑𝑒𝑠 − 𝑧)ሶ + 𝐾𝑝,𝑧 (𝑧 𝑑𝑒𝑠 − 𝑧)
𝑢1
模型: 𝑧ሷ = −𝑔 +
𝑚

𝑢1 = 𝑚(𝑔 + 𝑧ሷ 𝑑𝑒𝑠 + 𝐾𝑑,𝑧 (𝑧ሶ 𝑑𝑒𝑠 − 𝑧)ሶ + 𝐾𝑝,𝑧 (𝑧 𝑑𝑒𝑠 − 𝑧))

𝑧𝑑𝑒𝑠 𝑢1
𝑧ሶ𝑑𝑒𝑠
𝑧ሷ𝑑𝑒𝑠
Z 轴位置
控制器

𝑧, 𝑧ሶ 47
 线性化的动力学模型
• Y轴方向的位置控制
PD: 𝑦ሷ𝑐 = 𝑦ሷ 𝑑𝑒𝑠 + 𝐾𝑑,𝑦 (𝑦ሶ 𝑑𝑒𝑠 − 𝑦)
ሶ + 𝐾𝑝,𝑦 (𝑦 𝑑𝑒𝑠 − 𝑦)
模型: 𝑦ሷ = −𝑔ϕ
1
ϕ𝑐 = − (𝑦ሷ 𝑑𝑒𝑠 + 𝐾𝑑,𝑦 (𝑦ሶ 𝑑𝑒𝑠 − 𝑦)
ሶ + 𝐾𝑝,𝑦 (𝑦 𝑑𝑒𝑠 − 𝑦))
𝑔

• 姿态控制
PD: ϕሷ 𝑐 = 𝐾𝑑,ϕ (ϕሶ𝑐 − ϕ)
ሶ + 𝐾𝑝,ϕ (ϕ𝑐 − ϕ)
𝑢2
模型: ϕሷ =
𝐼𝑥𝑥

𝑢2 = 𝐼𝑥𝑥 (𝐾𝑑,ϕ (ϕሶ𝑐 − ϕ)

ሶ + 𝐾𝑝,ϕ (ϕ𝑐 − ϕ))

y方向位置 ϕ𝑐 , ϕሶ 𝑐 姿态 𝑢2
控制器 控制器
𝑦𝑑𝑒𝑠
𝑦ሶ 𝑑𝑒𝑠
𝑦ሷ 𝑑𝑒𝑠 𝑦, 𝑦ሶ ϕ, ϕሶ 48
 轨迹跟踪

给定期望状态 𝑥 𝑑𝑒𝑠 ,𝑥ሶ 𝑑𝑒𝑠 ,𝑥ሷ 𝑑𝑒𝑠

𝑥 𝑑𝑒𝑠 − x

𝑒𝑝 = 𝑥 𝑑𝑒𝑠 − x
𝑒𝑣 = 𝑥ሶ 𝑑𝑒𝑠 − 𝑥ሶ
𝑥ሷ 𝑐 = 𝑥ሷ 𝑑𝑒𝑠 + 𝐾𝑑 𝑒𝑣 + 𝐾𝑝 𝑒𝑝
49
 3-D 四旋翼
𝒑𝑑𝑒𝑠 𝑢1
𝒑ሶ 𝑑𝑒𝑠
𝒑ሷ 𝑑𝑒𝑠
位置 ϕ𝑐 , 𝜃𝑐 , ψ𝑐 , 姿态 𝒖2 𝒛𝑏
轨迹规划 𝒚𝑏
控制器 控制器 𝒙𝑏

𝒑, 𝒑ሶ ϕ, 𝜔𝑥 , 𝜃, 𝜔𝑦 , ψ, 𝜔𝑧

• 非线性动力学
0 0
牛顿方程: 𝑚𝒑ሷ = 0 +𝑹 0
−𝑚𝑔 𝐹1 + 𝐹2 + 𝐹3 + 𝐹4 𝑢1

𝜔𝑥ሶ 𝜔𝑥 𝜔𝑥 𝑙(𝐹2 − 𝐹4 )
欧拉方程: 𝑰 ⋅ 𝜔𝑦ሶ + 𝜔𝑦 × 𝑰 ⋅ 𝜔𝑦 = 𝑙 𝐹3 − 𝐹1
𝜔𝑧ሶ 𝜔𝑧 𝜔𝑧 𝑀1 − 𝑀2 + 𝑀3 − 𝑀4 𝒖2
50
 机体坐标系下角速度

回顾𝜔𝑏 = 𝑅𝑇 𝑅ሶ
ZXY轴的欧拉角： 𝜓, 𝜙, 𝜃

𝑝 𝑐𝜃 0 −𝑐𝜙𝑠𝜃 𝜙ሶ Roll角速度
𝜔𝑏 = 𝑞 = 0 1 𝑠𝜙 𝜃ሶ Pitch角速度
𝑟 𝑠𝜃 0 𝑐𝜙𝑐𝜃 𝜓ሶ Yaw角速度

机体坐标系 世界坐标系

51
 3-D 旋翼

• 线性化 平衡悬停态 (ϕ0 ~0, 𝜃0 ~0, 𝑢1,0 ~𝑚𝑔)

0 0 𝒑ሷ 1 = 𝑥ሷ = 𝑔(𝜃𝑐𝑜𝑠ψ + ϕ𝑠𝑖𝑛ψ)
牛顿方程 𝑚𝒑ሷ = 0 +𝑹 0 𝒑ሷ 2 = 𝑦ሷ = 𝑔(𝜃𝑠𝑖𝑛ψ − ϕ𝑐𝑜𝑠ψ)
𝑢
−𝑚𝑔 𝐹1 + 𝐹2 + 𝐹3 + 𝐹4 𝒑ሷ 3 = 𝑧ሷ = −𝑔 + 1
𝑚
𝑐𝜓𝑐𝜃 − 𝑠𝜙𝑠𝜓𝑠𝜃 −𝑐𝜙𝑠𝜓 𝑐𝜓𝑠𝜃 + 𝑐𝜃𝑠𝜙𝑠𝜓
𝑹 = 𝑐𝜃𝑠𝜓 + 𝑐𝜓𝑠𝜙𝑠𝜃 𝑐𝜙𝑐𝜓 𝑠𝜓𝑠𝜃 − 𝑐𝜓𝑐𝜃𝑠𝜙
−𝑐𝜙𝑠𝜃 𝑠𝜙 𝑐𝜙𝑐𝜃

欧拉角微分 𝜔𝑥 𝑐𝜃 0 −𝑐𝜙𝑠𝜃 𝜙ሶ 𝜔𝑥 𝜙ሶ
𝜔𝑦 = 0 1 𝑠𝜙 𝜃ሶ 𝜔𝑦 = 𝜃ሶ
𝜔𝑧 𝑠𝜃 0 𝑐𝜙𝑐𝜃 𝜓ሶ 𝜔𝑧 𝜓ሶ

ϕሷ 𝜔𝑥 𝜔𝑥 𝑙(𝐹2 − 𝐹4 )
欧拉方程: 𝑰 ⋅ 𝜃ሷ + 𝜔𝑦 × 𝑰 ⋅ 𝜔𝑦 = 𝑙 𝐹3 − 𝐹1
ψሷ 𝜔𝑧 𝜔𝑧 𝑀1 − 𝑀2 + 𝑀3 − 𝑀4
52
 𝒑𝑑𝑒𝑠 𝑢1
𝒑ሶ 𝑑𝑒𝑠
𝒑ሷ 𝑑𝑒𝑠 位置 ϕ𝑐 , 𝜃𝑐 , ψ𝑐 , 姿态 𝒖2 𝒛𝑏
轨迹规划 𝒚𝑏
控制器 控制器 𝒙𝑏

𝒑, 𝒑ሶ ϕ, 𝜔𝑥 , 𝜃, 𝜔𝑦 , ψ, 𝜔𝑧
• 位置控制
PID: 𝒑ሷ 𝑖,𝑐 = 𝒑ሷ𝑖 𝑑𝑒𝑠 + 𝐾𝑑,𝑖 𝒑ሶ𝑖 𝑑𝑒𝑠 − 𝒑ሶ 𝑖 + 𝐾𝑝,𝑖 𝒑𝑖 𝑑𝑒𝑠 − 𝒑𝑖
模型: 𝑢1 = 𝑚(𝑔 + 𝒑ሷ 3,𝑐 ) (牛顿方程)
1 1
ϕ𝑐 = (𝒑ሷ 1,𝑐 𝑠𝑖𝑛ψ − 𝒑ሷ 2,𝑐 cosψ) θ𝑐 = (𝒑ሷ 1,𝑐 𝑐𝑜𝑠ψ + 𝒑ሷ 2,𝑐 𝑠𝑖𝑛ψ)
𝑔 𝑔
注意：这些是当前测量的yaw，不是期望的yaw
• 姿态控制

ϕሷ 𝑐 𝐾𝑝,ϕ ϕ𝑐 − ϕ + 𝐾𝑑,ϕ (ϕሶ 𝑐 − ϕ)ሶ

PID: 𝜃ሷ𝑐 = 𝐾𝑝,𝜃 𝜃𝑐 − 𝜃 + 𝐾𝑑,ϕ (𝜃ሶ𝑐 − 𝜃)
ሶ
ψሷ 𝑐 𝐾𝑝,ψ ψ𝑐 − ψ + 𝐾𝑑,ψ (ψሶ 𝑐 − ψ) ሶ

ϕሷ 𝑐 𝜔𝑥 𝜔𝑥
模型: 𝒖2 = 𝑰 ⋅ 𝜃ሷ𝑐 + 𝜔𝑦 × 𝑰 ⋅ 𝜔𝑦 (欧拉方程)
𝜔𝑧 𝜔𝑧 53
ψሷ 𝑐
四旋翼先进控制

54
SE(3)控制

55
 微分平坦

四旋翼的状态和输入可以写成四个挑选出来的输
出变量及其导数的代数函数

• 能够自动生成轨迹
• 在空间中任何具有平坦输出（输出具有合理的有界
导数）的任意平滑轨迹都可以被旋翼跟踪
• 一种输出选择:
𝝈 = 𝑥, 𝑦, 𝑧, 𝜓 𝑇

• 在空间中有平坦输出的轨迹:
𝝈 𝑡 = 𝑇0 , 𝑇𝑀 → ℝ3 × 𝑆𝑂 2

Minimum snap trajectory generation and control for quadrotors 56

 微分平坦
• 旋翼状态
‐ 位置，姿态，线速度，角速度：
𝐗 = [𝑥, 𝑦, 𝑧, 𝜙, 𝜃, 𝜓, 𝑥,ሶ 𝑦,ሶ 𝑧,ሶ 𝜔𝑥 , 𝜔𝑦 , 𝜔𝑧 ]𝑇
机体系下，机体旋转的
即时角速度
‐ 运动方程:
𝑚𝒑ሷ = −𝑚𝑔𝐳𝑊 + 𝑢1 𝐳𝐵 .

𝜔𝑥 𝑢2
𝝎𝐵 = 𝜔𝑦 , 𝝎𝐵ሶ = 𝑰−1 −𝝎𝐵 × 𝑰 ⋅ 𝝎𝐵 + 𝑢3
𝜔𝑧 𝑢4

• 位置，速度，加速度都是平坦输出（𝝈 = 𝑥, 𝑦, 𝑧, 𝜓 𝑇 ）的
简单导数
57
 微分平坦
• 姿态角
➢ 旋翼姿态:

𝐗 = [𝑥, 𝑦, 𝑧, 𝜙, 𝜃, 𝜓, 𝑥,ሶ 𝑦,ሶ 𝑧,ሶ 𝜔𝑥 , 𝜔𝑦 , 𝜔𝑧 ]𝑇

➢ 从运动方程中可得:
𝐭 𝑇
𝐳𝐵 = , 𝐭 = 𝝈ሷ 1 , 𝝈ሷ 2 , 𝝈ሷ 3 + 𝑔
𝐭
➢ 定义yaw的向量(Z-X-Y 欧拉角):
𝐱 𝐶 = 𝑐𝑜𝑠𝝈4 , 𝑠𝑖𝑛𝝈4 , 0 𝑇

➢ 姿态角可以按如下公式获取
𝐳𝐵 × 𝐱 𝐶
𝐲𝐵 = , 𝐱 𝐵 = 𝐲𝐵 × 𝐳𝐵 𝑹𝐵 = 𝐱 𝐵 𝐲𝐵 𝐳𝐵
𝐳𝐵 × 𝐱 𝐶

58
 微分平坦
• 角速度
➢ 旋翼姿态:
𝐗 = [𝑥, 𝑦, 𝑧, 𝜙, 𝜃, 𝜓, 𝑥,ሶ 𝑦,ሶ 𝑧,ሶ 𝜔𝑥 , 𝜔𝑦 , 𝜔𝑧 ]𝑇
➢ 对运动方程求导
𝑚𝒑ሷ = −𝑚𝑔𝐳𝑊 + 𝑢1 𝐳𝐵 . 𝑚𝒂ሶ = 𝑢ሶ 1 𝐳𝐵 + 𝝎𝐵𝑊 × 𝑢1 𝐳𝐵
世界系下，机体旋转的
➢ 旋翼只有机体z轴方向有推力: 即时角速度

𝑢ሶ 1 = 𝐳𝐵 ∙ 𝑚𝒂ሶ

59
 微分平坦
• 角速度
➢ 旋翼姿态:

𝐗 = [𝑥, 𝑦, 𝑧, 𝜙, 𝜃, 𝜓, 𝑥,ሶ 𝑦,ሶ 𝑧,ሶ 𝜔𝑥 , 𝜔𝑦 , 𝜔𝑧 ]𝑇

➢ 可推:
𝑚
𝐡𝜔 = 𝝎𝐵𝑊 × 𝐳𝐵 = 𝒂ሶ − 𝐳𝐵 ∙ 𝒂ሶ 𝐳𝐵 .
𝑢1
𝑚
这是将 𝑢 𝒂ሶ 投影到𝑥𝐵 − 𝑦𝐵 平面
1

➢ 由于
𝝎𝐵𝑊 = 𝜔𝑥 𝐱 𝐵 + 𝜔𝑦 𝐲𝐵 + 𝜔𝑧 𝐳𝐵
➢ 沿着𝒙𝐵 和𝒚𝐵 方向的角速度:
𝜔𝑥 = −𝐡𝜔 ∙ 𝐲𝐵 , 𝜔𝑦 = 𝐡𝜔 ∙ 𝐱 𝐵
➢ 由于ωBW = ωBC + ωCW ，ωBC 没有 zB 分量：
ሶ 𝑊 ∙ 𝐳𝐵
𝜔𝑧 = 𝝎𝐵𝑊 ∙ 𝐳𝐵 = 𝝎𝐶𝑊 ∙ 𝐳𝐵 = 𝜓𝐳
60
 微分平坦
• 总结
➢ 旋翼姿态:

𝐗 = [𝑥, 𝑦, 𝑧, 𝜙, 𝜃, 𝜓, 𝑥,ሶ 𝑦,ሶ 𝑧,ሶ 𝜔𝑥 , 𝜔𝑦 , 𝜔𝑧 ]𝑇

➢ 平坦的输出:
𝝈 = 𝑥, 𝑦, 𝑧, 𝜓 𝑇

➢ 位置，速度，加速度:
平坦输出的导数
➢ 姿态：
𝐱 𝐶 = 𝑐𝑜𝑠𝜎4 , 𝑠𝑖𝑛𝜎4 , 0 𝑇 𝑹𝐵 = 𝐱 𝐵 𝐲𝐵 𝐳𝐵
➢ 角速度：
ሶ 𝑤 ∙ 𝐳𝐵
𝜔𝑥 = −𝐡𝜔 ∙ 𝐲𝐵 , 𝜔𝑦 = 𝐡𝜔 ∙ 𝐱 𝐵 , 𝜔𝑧 = 𝜓𝐳

61
 SE(3)控制

要求跟踪一条期望轨迹

𝝈𝑻 𝑡 = 𝑟𝑻 𝑡 𝑇 , 𝜓𝑻 𝑡 𝑇

其中， 𝑟是位置， 𝜓是yaw角

62
 SE(3)控制

➢ 定义位置和速度误差:
𝑒𝑝 = 𝑟 − 𝑟𝑻 , 𝑒𝑣 = 𝑟ሶ − 𝑟𝑻ሶ
➢ 计算期望的推力:
𝐹𝑑𝑒𝑠 = −𝐾𝑝 𝑒𝑝 − 𝐾𝑣 𝑒𝑣 + 𝑚𝑔𝑧𝑾 + 𝑚𝑟𝑻ሷ
➢ 将计算得到的推力投影到当前实际的机体坐标系的z轴作为输入:
𝑢1 = 𝐹𝑑𝑒𝑠 ∙ 𝑧𝐵

63
 SE(3)控制
➢ 为了得到剩下的三个输入，需要计算旋转误差
➢ 计算期望的𝑧𝐵 ：
𝐹𝑑𝑒𝑠
𝑧B,𝑑𝑒𝑠 =
𝐹𝑑𝑒𝑠
➢ 根据前面介绍的微分平坦可以计算𝑥B,𝑑𝑒𝑠 和𝑦B,𝑑𝑒𝑠 :
𝑇
𝑥𝐶,𝑑𝑒𝑥 = 𝑐𝑜𝑠(𝜓𝑻 ), 𝑠𝑖𝑛(𝜓𝑻 ), 0

𝑧𝐵,𝑑𝑒𝑠 × 𝑥𝐶,𝑑𝑒𝑠
𝑦𝐵,𝑑𝑒𝑠 =
𝑧𝐵,𝑑𝑒𝑠 × 𝑥𝐶,𝑑𝑒𝑠

𝑥𝐵,𝑑𝑒𝑠 = 𝑦𝐵,𝑑𝑒𝑠 × 𝑧𝐵,𝑑𝑒𝑠

𝑅𝐵,𝑑𝑒𝑠 = 𝑥𝐵,𝑑𝑒𝑠 𝑦𝐵,𝑑𝑒𝑠 𝑧𝐵,𝑑𝑒𝑠

64
 SE(3)控制
➢ 定义姿态误差
1 𝑇 𝑊 𝑊 T ∨
𝑒𝑅 = 𝑅𝑑𝑒𝑠 𝑅𝐵 − 𝑅𝐵 𝑅𝑑𝑒𝑠
2
∨表示从so(3)到ℝ3 的vee 映射
➢ 定义角速度误差为机体坐标系下实际角速度和期望角速度的差：
𝐵
𝑒𝜔 = 𝐵
𝜔𝐵𝑊 − 𝜔𝐵𝑊,𝑇

➢ 剩下的三个输入可以计算如下：
𝑢2 , 𝑢3 , 𝑢4 𝑇 = −𝐾𝑅 𝑒𝑅 − 𝐾𝜔 𝑒𝜔

65
MPC控制

66
• 平衡跟踪进度和执行进度.
• 走廊约束保证安全.
• 终端速度约束保证可行.
• QP方法让问题在机载电脑上求解时间少于5ms.

67
67
 CMPCC
◆ 目标函数
𝑇
状态: 𝐱 (𝑘)
= 𝑥, 𝑣𝑥 , 𝑎𝑥 , 𝑦, 𝑣𝑦 , 𝑎𝑦 , 𝑧, 𝑣𝑧 , 𝑎𝑧 , 𝜃, 𝑣𝜃 , 𝑎𝜃 𝐬. 𝐭. 𝐱 (𝑘+1) = 𝐀𝐝 𝐱 (𝑘) + 𝐁𝐝 𝐮(𝑘) , 𝑘 = 1,2,3, … , 𝑁 − 1
𝑇 𝐱 𝑙 ≤ 𝐱 𝑘 ≤ 𝐱 𝑢 , 𝑘 = 1,2,3, … , 𝑁 − 1
输入: 𝐮(𝑘) = 𝑗𝑥 , 𝑗𝑦 , 𝑗𝑧 , 𝑗𝜃
𝐮𝑙 ≤ 𝐮𝑘 ≤ 𝐮𝑢 , 𝑘 = 1,2,3, … , 𝑁 − 1
𝑁
2
𝐂𝑘 ⋅ 𝒙𝑘 , 𝑦 𝑘 , 𝑧 𝑘 𝑇 ≤ 𝐛𝑘 , 𝑘 = 1,2,3, … , 𝑁 − 1
𝑘 𝑘 (𝑘)
𝐽 = min ෍ { ෍ (𝜇 − 𝑝𝜇 𝜃 − 𝑞 ⋅ 𝑣𝜃 } 𝑁
𝑣𝜇 ≤ 𝑣𝑡𝜇 , 𝜇 = 𝑥, 𝑦, 𝑧
𝐱,𝐮
𝑘=1 𝜇=𝑥,𝑦,𝑧

𝑠 0
优化的局部CMPCC 轨迹
全局轨迹:
𝑠 1
𝑃 = 𝑝𝜇 𝜃 , 𝜇 ∈ 𝑥, 𝑦, 𝑧
2
4 𝑠 𝑝(𝜃 (0) )
𝑠 3
𝑠
𝑝(𝜃 (1) )
𝑝(𝜃 (4) )
𝑝(𝜃 (3) ) 𝑝(𝜃 (2) )

68
 CMPCC
◆ 目标函数
𝑇
状态: 𝐱 (𝑘)
= 𝑥, 𝑣𝑥 , 𝑎𝑥 , 𝑦, 𝑣𝑦 , 𝑎𝑦 , 𝑧, 𝑣𝑧 , 𝑎𝑧 , 𝜃, 𝑣𝜃 , 𝑎𝜃 𝐬. 𝐭. 𝐱 (𝑘+1) = 𝐀𝐝 𝐱 (𝑘) + 𝐁𝐝 𝐮(𝑘) , 𝑘 = 1,2,3, … , 𝑁 − 1
𝑇 𝐱 𝑙 ≤ 𝐱 𝑘 ≤ 𝐱 𝑢 , 𝑘 = 1,2,3, … , 𝑁 − 1
输入: 𝐮(𝑘) = 𝑗𝑥 , 𝑗𝑦 , 𝑗𝑧 , 𝑗𝜃
𝐮𝑙 ≤ 𝐮𝑘 ≤ 𝐮𝑢 , 𝑘 = 1,2,3, … , 𝑁 − 1
𝑁
2
𝐂𝑘 ⋅ 𝒙𝑘 , 𝑦 𝑘 , 𝑧 𝑘 𝑇 ≤ 𝐛𝑘 , 𝑘 = 1,2,3, … , 𝑁 − 1
𝑘 𝑘 (𝑘)
𝐽 = min ෍ { ෍ (𝜇 − 𝑝𝜇 𝜃 − 𝑞 ⋅ 𝑣𝜃 } 𝑁
𝑣𝜇 ≤ 𝑣𝑡𝜇 , 𝜇 = 𝑥, 𝑦, 𝑧
𝐱,𝐮
𝑘=1 𝜇=𝑥,𝑦,𝑧

0
𝑠 优化的局部CMPCC 轨迹
全局轨迹:
𝑠 1
𝑃 = 𝑝𝜇 𝜃 , 𝜇 ∈ 𝑥, 𝑦, 𝑧
2
4 𝑠 (0)
𝑠 3 𝑣𝜃
𝑠
(4)
𝑣𝜃 (1)
𝑣𝜃
(3)
𝑣𝜃 (2)
𝑣𝜃
69
 CMPCC
◆ 安全约束
给定一个凸多面体形状的安全飞行走廊，我 𝐬. 𝐭. 𝐱 (𝑘+1) = 𝐀𝐝 𝐱 (𝑘) + 𝐁𝐝 𝐮(𝑘) , 𝑘 = 1,2,3, … , 𝑁 − 1
们在每个状态 𝑠 (𝑘) 生成一个多边形管道来表
𝐱 𝑙 ≤ 𝐱 𝑘 ≤ 𝐱 𝑢 , 𝑘 = 1,2,3, … , 𝑁 − 1
示优化过程中的安全空间
这些约束是线性的
𝐮𝑙 ≤ 𝐮𝑘 ≤ 𝐮𝑢 , 𝑘 = 1,2,3, … , 𝑁 − 1
𝐂𝑘 ⋅ 𝒙𝑘 , 𝑦 𝑘 , 𝑧 𝑘 𝑇 ≤ 𝐛𝑘 , 𝑘 = 1,2,3, … , 𝑁 − 1
𝑁
𝑣𝜇 ≤ 𝑣𝑡𝜇 , 𝜇 = 𝑥, 𝑦, 𝑧

凸多面体安全 凸多边形管道
飞行走廊 0
𝑠

𝑠 1

2
4 𝑠
𝑠 3
𝑠 ሶ (0) )
𝑝(𝜃
ሶ (4) )
𝑝(𝜃 Ξ
ሶ (3) )
𝑝(𝜃 ሶ (1) )
𝑝(𝜃
ሶ (2) )
𝑝(𝜃
70
 CMPCC
◆ 安全约束

每个时刻的状态𝑠 (𝑘) 都对应一个 𝐬. 𝐭. 𝐱 (𝑘+1) = 𝐀𝐝 𝐱 (𝑘) + 𝐁𝐝 𝐮(𝑘) , 𝑘 = 1,2,3, … , 𝑁 − 1

单独的多边形管道 𝐱 𝑙 ≤ 𝐱 𝑘 ≤ 𝐱 𝑢 , 𝑘 = 1,2,3, … , 𝑁 − 1
𝐮𝑙 ≤ 𝐮𝑘 ≤ 𝐮𝑢 , 𝑘 = 1,2,3, … , 𝑁 − 1
𝐂𝑘 ⋅ 𝒙𝑘 , 𝑦 𝑘 , 𝑧 𝑘 𝑇 ≤ 𝐛𝑘 , 𝑘 = 1,2,3, … , 𝑁 − 1
𝑁
𝑣𝜇 ≤ 𝑣𝑡𝜇 , 𝜇 = 𝑥, 𝑦, 𝑧

凸多边形安全
飞行走廊 𝑠 0

𝑠 1

2
4 𝑠
𝑠 3
𝑠 ሶ (0) )
𝑝(𝜃
ሶ (4) )
𝑝(𝜃 Ξ (0)
ሶ (1) ) Ξ (1)
ሶ (3) )
𝑝(𝜃 (2)
𝑝(𝜃
Ξ (4) 𝑝(𝜃
Ξ (3)
ሶ )
Ξ (2) 71
 CMPCC
◆ 安全约束

凸多面体飞行走廊和产生的多边形管道Ω

Ω Ω

𝑣𝑝 𝑣𝑝
全局轨迹
Φ
72
 CMPCC
◆ 终端速度约束

限制终端速度可确保轨迹可行性， 𝐬. 𝐭. 𝐱 (𝑘+1) = 𝐀𝐝 𝐱 (𝑘) + 𝐁𝐝 𝐮(𝑘) , 𝑘 = 1,2,3, … , 𝑁 − 1

同时大大缩短规划的时间。 𝐱 𝑙 ≤ 𝐱 𝑘 ≤ 𝐱 𝑢 , 𝑘 = 1,2,3, … , 𝑁 − 1
𝐮𝑙 ≤ 𝐮𝑘 ≤ 𝐮𝑢 , 𝑘 = 1,2,3, … , 𝑁 − 1
𝐂𝑘 ⋅ 𝒙𝑘 , 𝑦 𝑘 , 𝑧 𝑘 𝑇 ≤ 𝐛𝑘 , 𝑘 = 1,2,3, … , 𝑁 − 1
𝑁
𝑣𝜇 ≤ 𝑣𝑡𝜇 , 𝜇 = 𝑥, 𝑦, 𝑧

(𝑁)
𝑣𝜇

𝑣𝑡𝜇

73
 Robust and Aggressive MPCC Flight Control
◆ 接触干扰

全局参考轨迹
CMPCC 轨迹
飞行走廊
多边形管道

干扰发生!

74
所有视频未经加速处理
 全局参考轨迹
CMPCC 轨迹
飞行走廊
多边形管道

干扰发生!

所有视频未经加速处理
 ◆ 抗风实验
撞在障碍物上

鼓风机 抵抗了风力的
影响
谢谢观看