四旋翼无人机控制教程

导读

本教程从数学基础出发，逐步构建四旋翼无人机的完整控制体系。各章节解决的核心问题及相互关系如下：

章节	解决的核心问题	与其他章节的关系
一、坐标系与旋转基础	如何描述无人机在三维空间中的姿态？	数学预备，后续所有章节的基础
二、动力学建模	力和力矩如何驱动无人机运动？运动方程是什么？	依赖一（旋转矩阵），为所有控制器设计提供被控对象模型
三、微分平坦性	能否只规划 $[x,y,z,\psi]$ 的轨迹就推出全部状态和输入？	依赖二（牛顿方程），是连接"轨迹规划"与"控制"的桥梁
四、PID 控制基础	控制器参数如何影响系统响应？	控制理论入门，后续所有控制方法的起点
五、基于模型的控制	如何利用模型信息改善控制效果？（2D）	依赖二（线性化模型）+ 四（PID），引入前馈+反馈结构
六、三维轨迹跟踪	如何将 2D 控制推广到完整的 3D 飞行？	五的三维扩展，增加了偏航角耦合处理
七、SE(3) 控制	大角度机动时如何避免欧拉角的万向节锁？	依赖三（姿态矩阵构建），是六的非线性替代方案
八、MPC 控制	如何在有约束条件下同时做局部规划和控制？	融合规划与控制，依赖二（预测模型）

阅读建议：

快速入门（理解基本控制）：一 → 二 → 四 → 五
完整学习：按顺序一 → 八
专题阅读：三（微分平坦性）是理解轨迹规划教程的前置知识

控制教程（8 个代码块）：

一、坐标系与旋转基础 — 3D 旋转可视化：无人机"+"字形绕 Z/Y/X 轴旋转

二、动力学建模 — 给定 4 个电机转速，计算推力/力矩，可视化推力方向

三、微分平坦性 — 螺旋轨迹 → 由平坦输出推导姿态角和推力

四、PID 控制 — 二阶系统 Kp/Kd/Ki 对阶跃响应的影响对比

五、基于模型控制 — 2D 无人机 Y-Z 平面前馈+PD 控制仿真

六、三维轨迹跟踪 — 3D 八字形轨迹跟踪，期望 vs 实际对比

七、SE(3) 控制 — 大角度翻转：欧拉角控制 vs SE(3) 控制误差对比

八、MPC 控制 — 1D 高度控制 MPC，展示预测区间和约束处理

一、坐标系与旋转基础

在研究四旋翼无人机之前，我们需要先理解坐标系变换和旋转矩阵，这是描述无人机姿态的数学基础。

1.1 二维坐标旋转的几何推导

考虑平面上一个点 $A(x, y)$，当坐标系旋转角度 $\theta$ 后，该点在新坐标系下的坐标为 $A'(x', y')$。

方式一：几何投影法

设原坐标系为 $OXY$，新坐标系为 $OX'Y'$（逆时针旋转 $\theta$）。通过几何投影关系：

$$ x' = OB + BC = OD\cos\theta + AD\sin\theta = x\cos\theta + y\sin\theta $$

$$ y' = AE - CE = AP\cos\theta - OD\sin\theta = y\cos\theta - x\sin\theta $$

写成矩阵形式：

$$ \begin{bmatrix} x' \ y' \end{bmatrix} = \begin{bmatrix} \cos\theta & \sin\theta \ -\sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} x \ y \end{bmatrix} $$

方式二：极坐标法

设点 $A$ 到原点距离为 $r$，与 $X$ 轴夹角为 $\alpha$。旋转后：

$$ x' = r\cos(\alpha + \theta) = r\cos\alpha\cos\theta - r\sin\alpha\sin\theta = x\cos\theta - y\sin\theta $$

$$ y' = r\sin(\alpha + \theta) = r\sin\alpha\cos\theta + r\cos\alpha\sin\theta = y\cos\theta + x\sin\theta $$

写成矩阵形式：

$$ \begin{bmatrix} x' \ y' \end{bmatrix} = \begin{bmatrix} \cos\theta & -\sin\theta \ \sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} x \ y \end{bmatrix} $$

个人理解：方式二是点绕原点逆时针旋转 $\theta$ 角，所以旋转矩阵与方式一（坐标轴旋转）互为转置。

1.2 两种旋转模式的区别

模式一：坐标轴旋转（点不动，坐标系旋转）

当原坐标系不动，坐标轴旋转时，求点在新坐标系下的坐标：

$$ \begin{bmatrix} x' \ y' \end{bmatrix} = \begin{bmatrix} \cos\theta & \sin\theta \ -\sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} x \ y \end{bmatrix} $$

模式二：点旋转（坐标系不动，点旋转）

$$ \begin{bmatrix} x' \ y' \end{bmatrix} = \begin{bmatrix} \cos\theta & -\sin\theta \ \sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} x \ y \end{bmatrix} $$

个人理解：换个角度理解——求点在新坐标系下的坐标，等价于把坐标轴顺时针旋转 $\theta$。可以想象自己站在坐标轴上跟着转，周围的点相对自己的位置自然就变了。

个人理解：两种模式的旋转矩阵互为转置。模式一可理解为"坐标系主动旋转"，模式二为"点的主动旋转"。在无人机中，我们通常使用模式一——已知机体旋转了多少角度，求同一个点在新坐标系下的表示。

1.3 三维旋转矩阵

将二维推广到三维，分别绕三个轴旋转：

绕 $Z$ 轴旋转（Yaw，偏航角 $\psi$）：

$$ R(\psi) = \begin{bmatrix} \cos\psi & -\sin\psi & 0 \ \sin\psi & \cos\psi & 0 \ 0 & 0 & 1 \end{bmatrix} $$

绕 $Y$ 轴旋转（Pitch，俯仰角 $\theta$）：

$$ R(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \ 0 & 1 & 0 \ -\sin\theta & 0 & \cos\theta \end{bmatrix} $$

绕 $X$ 轴旋转（Roll，滚转角 $\phi$）：

$$ R(\phi) = \begin{bmatrix} 1 & 0 & 0 \ 0 & \cos\phi & -\sin\phi \ 0 & \sin\phi & \cos\phi \end{bmatrix} $$

1.4 无人机坐标系变换

个人理解：无人机的场景属于"坐标轴旋转"——机体坐标系相对于世界坐标系发生了旋转，我们需要把机体系下的向量转换到世界系下表达。

那么从自身坐标轴旋转的意义就是 $R_c = R_o^T \cdot R_r$。

如果已知 $R_o$ 对 $R_c$， $R_c$ 为解坐标轴倒推可得 $R_o$。但此时一定是对于世界坐标的旋转。

个人理解：在无人机应用中，旋转矩阵 $R$ 通常表示从机体坐标系（Body Frame）到世界坐标系（World Frame）的变换，即 $\mathbf{p}_W = R \cdot \mathbf{p}_B$。其逆变换（世界→机体）为 $R^T$，因为旋转矩阵是正交矩阵， $R^{-1} = R^T$。

Python 验证

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

plt.rcParams['font.sans-serif'] = ['SimHei']  # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False    # 解决负号显示为方框的问题

# ========== 可视化无人机"+"十字形绕 Z/Y/X 轴旋转 ==========

# --- 构造无人机"+"形状（4 个臂端 + 中心） ---
L = 1.0  # 臂长
drone = np.array([
    [ L, 0, 0],  # 右臂端
    [-L, 0, 0],  # 左臂端
    [ 0, L, 0],  # 前臂端
    [ 0,-L, 0],  # 后臂端
    [ 0, 0, 0],  # 中心
]).T  # 3×5 矩阵，每列一个点

# --- 三维旋转矩阵 ---
def Rz(psi):
    """绕 Z 轴旋转（偏航角 ψ）"""
    c, s = np.cos(psi), np.sin(psi)
    return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])

def Ry(theta):
    """绕 Y 轴旋转（俯仰角 θ）"""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])

def Rx(phi):
    """绕 X 轴旋转（滚转角 φ）"""
    c, s = np.cos(phi), np.sin(phi)
    return np.array([[1, 0, 0], [0, c, -s], [0, s, c]])

angle = np.radians(45)  # 旋转 45°

# --- 绘制 4 个子图：原始 + 3 种旋转 ---
titles = ['原始姿态', f'绕 Z 轴旋转 {int(np.degrees(angle))}°（偏航）',
          f'绕 Y 轴旋转 {int(np.degrees(angle))}°（俯仰）',
          f'绕 X 轴旋转 {int(np.degrees(angle))}°（滚转）']
rotations = [np.eye(3), Rz(angle), Ry(angle), Rx(angle)]

fig = plt.figure(figsize=(14, 10))
for i, (R, title) in enumerate(zip(rotations, titles)):
    ax = fig.add_subplot(2, 2, i + 1, projection='3d')
    pts = R @ drone  # 旋转后的点坐标

    # 画两条臂（十字形）
    ax.plot([pts[0,0], pts[0,1]], [pts[1,0], pts[1,1]],
            [pts[2,0], pts[2,1]], 'b-o', lw=3, label='臂 1')
    ax.plot([pts[0,2], pts[0,3]], [pts[1,2], pts[1,3]],
            [pts[2,2], pts[2,3]], 'r-o', lw=3, label='臂 2')
    # 标注中心
    ax.scatter(*pts[:, 4], color='k', s=80, zorder=5)

    # 画旋转后的机体坐标轴
    axis_len = 0.6
    for j, (color, lbl) in enumerate(zip(['r','g','b'], ['Xb','Yb','Zb'])):
        axis_vec = R @ (axis_len * np.eye(3)[:, j])
        ax.quiver(0, 0, 0, *axis_vec, color=color, arrow_length_ratio=0.15)

    ax.set_xlim(-1.5, 1.5); ax.set_ylim(-1.5, 1.5); ax.set_zlim(-1.5, 1.5)
    ax.set_xlabel('X'); ax.set_ylabel('Y'); ax.set_zlabel('Z')
    ax.set_title(title)

plt.suptitle('无人机"+"十字形的三维旋转演示', fontsize=14)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/control1.png', dpi=150, bbox_inches='tight')
plt.show()

运行结果：

二、四旋翼飞行器动力学建模

2.1 电机模型

四旋翼飞行器的动力来源于四个电机，其模型如下：

电机转速模型（一阶惯性环节）：

$$ \dot{\omega}_i = k_m(\omega_i^{des} - \omega_i) $$

个人理解：电机转速模型由厂家提供，内部已做好闭环控制，我们在建模时无需关心其内部实现，直接使用即可。系数 $k_m$ 可通过实测标定。

单个电机产生的推力：

$$ F_i = k_F \omega_i^2 \quad $$

单个电机产生的反扭矩：

$$ M_i = k_M \omega_i^2 $$

其中 $k_m$ 为电机响应常数， $k_F$ 为推力系数， $k_M$ 为力矩系数。

个人理解：推力和力矩与转速的平方成正比，这是气动力学的基本结论。实际系统中 $k_F$、 $k_M$ 通过台架实验标定得到。电机模型为一阶惯性环节， $k_m$ 越大电机响应越快；在控制器设计中通常假设电机响应足够快，可忽略该动态。

2.2 Z-X-Y 欧拉角与旋转矩阵

采用 Z-X-Y 顺序的欧拉角表示旋转（先偏航 $\psi$，再滚转 $\phi$，最后俯仰 $\theta$）：

$$ R_{ab} = R_z(\psi) \cdot R_x(\phi) \cdot R_y(\theta) $$

展开得（令 $c = \cos$， $s = \sin$）：

$$ R_{ab} = \begin{bmatrix} c\psi c\theta - s\phi s\psi s\theta & -c\phi s\psi & c\psi s\theta + c\theta s\phi s\psi \ c\theta s\psi + c\psi s\phi s\theta & c\phi c\psi & s\psi s\theta - c\psi c\theta s\phi \ -c\phi s\theta & s\phi & c\phi c\theta \end{bmatrix} \quad $$

2.3 角速度与欧拉角导数的关系

机体角速度 $[\omega_x, \omega_y, \omega_z]^T$ 与欧拉角导数 $[\dot{\phi}, \dot{\theta}, \dot{\psi}]^T$ 的关系为：

$$ \begin{bmatrix} \omega_x \ \omega_y \ \omega_z \end{bmatrix} = \begin{bmatrix} c\theta & 0 & -c\phi s\theta \ 0 & 1 & s\phi \ s\theta & 0 & c\phi c\theta \end{bmatrix} \begin{bmatrix} \dot{\phi} \ \dot{\theta} \ \dot{\psi} \end{bmatrix} \quad $$

个人理解：这个转换矩阵在 $\phi = \pm 90°$ 时出现奇异性（Gimbal Lock），这也是欧拉角表示法的固有缺陷。对于小角度运动（如悬停附近），该矩阵近似为单位阵，即角速度近似等于欧拉角导数。

2.4 牛顿方程（平动动力学）

个人理解：牛顿-欧拉方程在惯性坐标系下建立，描述无人机在合力和合力矩作用下的运动规律。

牛顿方程描述质心的平动：

$$ m\ddot{\mathbf{p}} = \begin{bmatrix} 0 \ 0 \ -mg \end{bmatrix} + R \begin{bmatrix} 0 \ 0 \ F_1 + F_2 + F_3 + F_4 \end{bmatrix} \quad $$

个人理解：等号左边 $m\ddot{\mathbf{p}}$ 是质心加速度乘质量；右边第一项是重力（竖直向下），第二项是旋转矩阵 $R$ 将机体 $z$ 轴方向的总推力转换到世界坐标系（注意推力方向始终沿机体 $z$ 轴正方向）。此处忽略了空气阻力。

其中 $u_1 = F_1 + F_2 + F_3 + F_4$ 为总推力。展开各分量可得：

$$ m\ddot{x} = u_1(c\psi s\theta c\phi + s\psi s\phi) $$

$$ m\ddot{y} = u_1(s\psi s\theta c\phi - c\psi s\phi) $$

$$ m\ddot{z} = -mg + u_1 c\phi c\theta $$

2.5 欧拉方程（转动动力学）

$$ I \begin{bmatrix} \dot{\omega}_x \ \dot{\omega}_y \ \dot{\omega}_z \end{bmatrix} + \begin{bmatrix} \omega_x \ \omega_y \ \omega_z \end{bmatrix} \times I \begin{bmatrix} \omega_x \ \omega_y \ \omega_z \end{bmatrix} = \begin{bmatrix} l(F_2 - F_4) \ l(F_3 - F_1) \ M_1 - M_2 + M_3 - M_4 \end{bmatrix} \quad $$

右侧分别定义为控制力矩输入：

$$ u_2 = l(F_2 - F_4), \quad u_3 = l(F_3 - F_1), \quad u_4 = M_1 - M_2 + M_3 - M_4 $$

个人理解：欧拉方程在机体坐标系下建立，因为转动惯量矩阵 $I$ 在机体系下是常数对角阵，避免了随姿态变化的复杂性。

个人理解： $\omega \times I\omega$ 项是科里奥利力矩 / 陀螺效应项，在悬停附近（角速度小时）可忽略，这为后续线性化提供了基础。 $u_2, u_3$ 通过差分推力产生滚转/俯仰力矩， $u_4$ 通过差分反扭矩产生偏航力矩。

2.6 动力学模型总结

编号	名称	描述
①	电机模型	$F_i = k_F \omega_i^2$
②	欧拉方程	转动动力学
③	角速度转换	欧拉角速度与欧拉角的导数之间的转换
④	旋转矩阵	Z-X-Y 欧拉角表示
⑤	牛顿方程	平动动力学

2.7 平面状态方程示例

以平面（Y-Z平面）运动为例，状态向量 $\mathbf{x} = [y, z, \phi, \dot{y}, \dot{z}, \dot{\phi}]^T$，输入 $\mathbf{u} = [u_1, u_2]^T$：

$$ \begin{bmatrix} \ddot{y} \ \ddot{z} \ \ddot{\phi} \end{bmatrix} = \begin{bmatrix} -\frac{u_1}{m}\sin\phi \ \frac{u_1}{m}\cos\phi - g \ \frac{u_2}{I_{xx}} \end{bmatrix} $$

个人理解：这就是二维情况下的非线性状态方程。可以看到 $y$ 方向的加速度依赖于 $\sin\phi$（需要先倾斜才能水平移动）， $z$ 方向加速度依赖于 $\cos\phi$（推力在竖直方向的分量）， $\phi$ 由力矩 $u_2$ 直接控制。这体现了四旋翼"欠驱动"的特性——只有4个输入却要控制6个自由度。

Python 验证

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
plt.rcParams['font.sans-serif'] = ['SimHei']  # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False    # 解决负号显示为方框的问题

# ========== 四旋翼电机模型：推力/力矩计算与可视化 ==========

# --- 电机参数 ---
kF = 6.11e-8   # 推力系数 (N/(rad/s)^2)
kM = 1.5e-9    # 力矩系数 (N·m/(rad/s)^2)
L  = 0.175     # 臂长 (m)

# --- 四个电机转速 (rad/s)，模拟非对称飞行 ---
omega = np.array([4000, 4200, 3800, 4100])  # ω1, ω2, ω3, ω4

# --- 计算各电机推力和反扭矩 ---
F = kF * omega**2     # 各电机推力
M = kM * omega**2     # 各电机反扭矩

# --- 计算总推力和控制力矩 ---
u1 = np.sum(F)                       # 总推力
u2 = L * (F[1] - F[3])              # 滚转力矩
u3 = L * (F[2] - F[0])              # 俯仰力矩
u4 = M[0] - M[1] + M[2] - M[3]     # 偏航力矩

print(f"各电机推力: F = {F} N")
print(f"总推力 u1 = {u1:.4f} N")
print(f"滚转力矩 u2 = {u2:.6f} N·m")
print(f"俯仰力矩 u3 = {u3:.6f} N·m")
print(f"偏航力矩 u4 = {u4:.8f} N·m")

# --- 可视化：俯视图（电机位置 + 推力）和 3D 推力箭头 ---
fig = plt.figure(figsize=(14, 5))

# 子图 1：俯视图，四个电机位置和推力大小
ax1 = fig.add_subplot(1, 2, 1)
motor_pos = np.array([[0, L], [-L, 0], [0, -L], [L, 0]])  # 电机 1前 2左 3后 4右
colors = ['#e74c3c', '#3498db', '#2ecc71', '#f39c12']
for i in range(4):
    circle = plt.Circle(motor_pos[i], 0.04, color=colors[i], zorder=5)
    ax1.add_patch(circle)
    ax1.annotate(f'M{i+1}\nω={omega[i]}\nF={F[i]:.3f}N',
                 motor_pos[i], textcoords="offset points",
                 xytext=(15, 10), fontsize=8, color=colors[i])
# 画机体十字
ax1.plot([motor_pos[1,0], motor_pos[3,0]],
         [motor_pos[1,1], motor_pos[3,1]], 'k-', lw=2)
ax1.plot([motor_pos[0,0], motor_pos[2,0]],
         [motor_pos[0,1], motor_pos[2,1]], 'k-', lw=2)
ax1.set_xlim(-0.35, 0.35); ax1.set_ylim(-0.35, 0.35)
ax1.set_aspect('equal'); ax1.grid(True, alpha=0.3)
ax1.set_title('俯视图：电机分布与推力'); ax1.set_xlabel('Y (m)'); ax1.set_ylabel('X (m)')

# 子图 2：3D 视图，显示推力箭头
ax2 = fig.add_subplot(1, 2, 2, projection='3d')
for i in range(4):
    x, y = motor_pos[i]
    # 各电机推力箭头（沿 Z 轴向上）
    scale = F[i] / np.max(F) * 0.3  # 归一化显示
    ax2.quiver(x, y, 0, 0, 0, scale, color=colors[i],
               arrow_length_ratio=0.2, lw=2, label=f'M{i+1}: {F[i]:.3f}N')
    ax2.scatter(x, y, 0, color=colors[i], s=60)
# 总推力箭头（从中心出发）
ax2.quiver(0, 0, 0, 0, 0, 0.4, color='k', arrow_length_ratio=0.15,
           lw=3, label=f'总推力 u1={u1:.3f}N')
ax2.set_xlabel('Y'); ax2.set_ylabel('X'); ax2.set_zlabel('Z（推力方向）')
ax2.set_title('3D 视图：推力矢量'); ax2.legend(fontsize=7, loc='upper left')

plt.suptitle('四旋翼电机模型可视化', fontsize=14)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/control2.png', dpi=150, bbox_inches='tight')
plt.show()

运行结果：

三、微分平坦性

3.1 微分平坦的定义与意义

个人理解：微分平坦性是连接轨迹规划和控制的桥梁——有了微分平坦性，规划只需要设计平坦输出的轨迹，所有状态和输入都能自动推出。结合 SE(3) 控制，可以实现非常精确的位姿跟踪。

微分平坦（Differential Flatness）是指：如果系统的所有状态和输入都可以由一组**平坦输出（Flat Output）**及其有限次微分来表示，那么这个系统就是微分平坦的。

个人理解：四旋翼有 12 维状态和 4 维输入，但平坦输出只有 4 维（ $x, y, z, \psi$）。微分平坦的意义在于：如果系统的所有状态和输入都可以由一组"平坦输出"及其有限阶导数表示，那么高维的控制问题就可以简化为对这 4 个平坦输出的轨迹设计问题。

3.2 平坦输出的选择

对于四旋翼无人机，选择平坦输出为：

$$ \sigma = [x, y, z, \psi]^T $$

它定义了一个足够宽的轨迹集合：

$$ G(t) = [T_i, T_f] \rightarrow \mathbb{R}^3 \times SO(3) $$

个人理解：平坦输出 $\sigma$ 定义了 $SE(3)$ 上的轨迹——即同时包含位置（ $\mathbb{R}^3$）和姿态（ $SO(3)$）的完整刚体运动轨迹。世界坐标系到机体坐标系的变换可以完全由 $\sigma$ 及其导数确定。

完整状态向量为：

$$ X = [x, y, z, \dot{x}, \dot{y}, \dot{z}, \phi, \theta, \psi, \omega_x, \omega_y, \omega_z]^T $$

个人理解：选定 $\sigma = [x, y, z]^T$， $\dot{\sigma} = [\dot{x}, \dot{y}, \dot{z}]^T$，可以解析得到 roll、pitch、yaw 和角速度。

3.3 从平坦输出推导全部状态

微分平坦的核心价值在于：只要给定 $\sigma(t) = [x(t), y(t), z(t), \psi(t)]$ 及其各阶导数，就能推出系统的全部 12 个状态和 4 个输入。推导路径如下：

已知量	推出量	所需导数阶数
$\sigma$	位置 $x, y, z$	0 阶
$\dot{\sigma}$	速度 $\dot{x}, \dot{y}, \dot{z}$	1 阶
$\ddot{\sigma}$	总推力 $u_1$；姿态角 $\phi, \theta$	2 阶
$\dddot{\sigma}$	角速度 $p, q$	3 阶
$\sigma^{(4)}$	角加速度 $\dot{p}, \dot{q}$；力矩 $u_2, u_3$	4 阶（snap）

个人理解：这就是为什么轨迹规划中要最小化 snap（四阶导数）——snap 直接关联到控制力矩的大小。如果 snap 过大，意味着需要很大的控制力矩，电机可能饱和。

3.4 姿态角的推导

从牛顿方程出发：

$$ m\ddot{\mathbf{p}} = -mg\hat{z}_W + u_1 \hat{z}_B $$

推导总推力 $u_1$

$$ u_1 \hat{z}_B = m\ddot{\mathbf{r}} + mg\hat{z}_W $$

个人理解：总推力 $u_1$ 等于合力的模： $u_1 = m\sqrt{\ddot{x}^2 + \ddot{y}^2 + (\ddot{z}+g)^2}$。物理含义是：总推力必须同时克服重力并提供所需的加速度。

由此得到机体 $z$ 轴方向：

$$ \hat{z}_B = \frac{m[\ddot{x}, \ddot{y}, \ddot{z}+g]^T}{|m[\ddot{x}, \ddot{y}, \ddot{z}+g]^T|} = \mathbf{t} $$

其中 $\mathbf{t}$ 为单位推力方向向量。

构建期望旋转矩阵

通过中间变量 $\hat{x}_C$ 方向（由偏航角 $\psi$ 确定）：

$$ \hat{x}_C = [\cos\psi, \sin\psi, 0]^T $$

$$ \hat{y}_B = \frac{\hat{z}_B \times \hat{x}_C}{|\hat{z}_B \times \hat{x}_C|} $$

个人理解：构建旋转矩阵的思路——先由推力方向确定 $\hat{z}_B$，再由偏航角确定辅助方向 $\hat{x}_C$。 $\hat{y}_B$ 通过叉乘 $\hat{z}_B \times \hat{x}_C$ 得到，它垂直于 $\hat{z}_B$ 和 $\hat{x}_C$ 构成的平面。最后 $\hat{x}_B = \hat{y}_B \times \hat{z}_B$ 补全右手系。

$$ \hat{x}_B = \hat{y}_B \times \hat{z}_B $$

$$ R_{ab} = [\hat{x}_B, \hat{y}_B, \hat{z}_B] $$

提取欧拉角

从旋转矩阵中提取各欧拉角：

$$ \psi = \arctan\frac{r_{21}}{r_{11}}, \quad \theta = \arcsin(-r_{31}), \quad \phi = \arcsin\frac{r_{32}}{r_{33}} $$

个人理解：因此 $\phi, \theta$ 完全由平坦输出 $[x, y, z, \psi]$ 的二阶导数（加速度）决定，无需更高阶导数。

3.5 角速度 p, q, r 的推导

个人理解：接下来推导角速度 $p, q, r$，需要对牛顿方程再求一次时间导数（从加速度到 jerk）。

对牛顿方程求时间导数（jerk）：

$$ m\dddot{\mathbf{p}} = \frac{d}{dt}(-mg\hat{z}_W) + \frac{d}{dt}(u_1\hat{z}_B) $$

个人理解: $\hat{z}_W$ 是世界坐标系的固定单位向量，不随时间变化，所以 $\frac{d}{dt}(-mg\hat{z}_W) = 0$。

由于 $\hat{z}_W$ 是常向量，其导数为零，因此：

$$ m\dddot{\mathbf{p}} = \dot{u}_1 \hat{z}_B + u_1 \dot{\hat{z}}_B $$

个人理解：对 $u_1\hat{z}_B$ 求导时用乘积法则： $\dot{u}_1$ 是推力大小的变化率， $\dot{\hat{z}}_B$ 是推力方向的变化率。根据刚体运动学，方向向量的变化率可以用角速度的叉乘表示: $\dot{\hat{z}}B = \omega{BW} \times \hat{z}_B$。

利用运动学关系 $\dot{\hat{z}}B = \omega{BW} \times \hat{z}_B$ ：

$$ m\dddot{\mathbf{p}} = \dot{u}_1 \hat{z}_B + \omega_{BW} \times u_1 \hat{z}_B $$

消去 $\hat{z}_B$ 方向的分量（两边点乘 $\hat{z}_B$ 后相减）：

$$ m\dddot{\mathbf{p}} - (\hat{z}_B \cdot m\dddot{\mathbf{p}})\hat{z}_B = \omega_{BW} \times u_1\hat{z}_B $$

定义辅助向量：

$$ h_W = \frac{\omega_{BW} \times \hat{z}_B}{u_1} = \frac{m}{u_1}\left(\dddot{\mathbf{p}} - (\hat{z}_B \cdot \dddot{\mathbf{p}})\hat{z}_B\right) $$

个人理解： $h_W$ 是一个辅助向量，由已知的 jerk 和推力计算得到，它包含了 $\omega_{BW}$ 在垂直于 $\hat{z}_B$ 平面内的分量信息，可以从中提取出 $p$ 和 $q$。

提取各分量 p, q, r

角速度在机体系下分解：

$$ \omega_{BW} = p\hat{x}_B + q\hat{y}_B + r\hat{z}_B $$

个人理解：注意 $\omega_{BW} = p\hat{x}_B + q\hat{y}_B + r\hat{z}_B$ 是矢量分解，利用 $\hat{x}_B, \hat{y}_B, \hat{z}_B$ 的正交性可以通过点乘分别提取各分量。

我们知道 $\hat{x}_B \cdot \hat{y}_B = |\hat{x}_B||\hat{y}_B|\cos 90° = 0$，而 $\hat{x}_B$ 和 $\hat{y}_B$ 正交坐标， $\cos\alpha = 0$。

利用 $h_W = \omega_{BW} \times \hat{z}_B / u_1$ 与各轴单位向量点乘：

$$ p = -h_W \cdot \hat{y}_B $$

$$ q = h_W \cdot \hat{x}_B $$

对于 $r$ 分量—— $\Gamma$ 是角速度 $\omega_{BW}$ 在 $\hat{z}_B$ 方向上的分量：

$$ r = \omega_{BW} \cdot \hat{z}_B = p(\hat{x}_B \cdot \hat{z}_B) + q(\hat{y}_B \cdot \hat{z}_B) + r(\hat{z}_B \cdot \hat{z}_B) $$

个人理解：由正交性 $\hat{x}_B \perp \hat{z}_B$、 $\hat{y}_B \perp \hat{z}_B$，所以 $\hat{z}_B$ 方向的角速度分量 $r$ 无法从 $h_W$（垂直于 $\hat{z}_B$ 的量）中获得，需要通过偏航角速率 $\dot{\psi}$ 单独计算。

$$ r = \omega_{BW} \cdot \hat{z}_B = (\omega_{BL} + \omega_{CW}) \cdot \hat{z}_B = \omega_{CW} \cdot \hat{z}_B = \dot{\psi} \hat{z}_W \cdot \hat{z}_B $$

个人理解： $r$ 分量直接与偏航角速率 $\dot{\psi}$ 相关，这体现了偏航角是独立的平坦输出之一。 $p, q$ 需要 jerk（三阶导数）才能算出，而 $r$ 只需要 $\dot{\psi}$——这也是偏航轴解耦的体现。

Python 验证

import numpy as np
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei']  # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False    # 解决负号显示为方框的问题
# ========== 微分平坦性验证：从圆形轨迹推导姿态角和推力 ==========

# --- 物理参数 ---
m = 0.5    # 无人机质量 (kg)
g = 9.81   # 重力加速度 (m/s^2)

# --- 定义圆形轨迹（平坦输出 σ = [x, y, z, ψ]） ---
T_total = 10.0              # 总时间 (s)
dt = 0.01
t = np.arange(0, T_total, dt)
omega_c = 2 * np.pi / T_total  # 圆轨迹角频率

# 位置轨迹：水平圆 + 缓慢上升 + 偏航跟随速度方向
radius = 2.0
x = radius * np.cos(omega_c * t)
y = radius * np.sin(omega_c * t)
z = 0.5 * t                                # 螺旋上升
psi = np.arctan2(np.gradient(y, dt), np.gradient(x, dt))  # 偏航角跟随速度方向

# --- 计算各阶导数 ---
ddx = np.gradient(np.gradient(x, dt), dt)  # x 的二阶导数（加速度）
ddy = np.gradient(np.gradient(y, dt), dt)  # y 的二阶导数
ddz = np.gradient(np.gradient(z, dt), dt)  # z 的二阶导数

# --- 由微分平坦性推导总推力 u1 ---
u1 = m * np.sqrt(ddx**2 + ddy**2 + (ddz + g)**2)

# --- 由加速度推导姿态角 φ (roll) 和 θ (pitch) ---
# 机体 z 轴方向（单位推力方向）
tx = ddx / (ddz + g + 1e-10)
ty = ddy / (ddz + g + 1e-10)

# 通过旋转矩阵关系推导欧拉角
phi   = np.arcsin(np.clip(tx * np.sin(psi) - ty * np.cos(psi), -1, 1))  # 滚转角
theta = np.arctan2(tx * np.cos(psi) + ty * np.sin(psi),
                   1 + ddz / g)  # 俯仰角

# --- 绘制结果 ---
fig = plt.figure(figsize=(15, 10))

# 子图 1：三维轨迹
ax1 = fig.add_subplot(2, 2, 1, projection='3d')
ax1.plot(x, y, z, 'b-', lw=1.5)
# 每隔一段标注无人机位置和朝向
step = len(t) // 8
for i in range(0, len(t), step):
    ax1.scatter(x[i], y[i], z[i], c='r', s=40, zorder=5)
    dx_arrow = 0.3 * np.cos(psi[i])
    dy_arrow = 0.3 * np.sin(psi[i])
    ax1.quiver(x[i], y[i], z[i], dx_arrow, dy_arrow, 0,
               color='r', arrow_length_ratio=0.3)
ax1.set_xlabel('X (m)'); ax1.set_ylabel('Y (m)'); ax1.set_zlabel('Z (m)')
ax1.set_title('螺旋上升轨迹 σ(t)')

# 子图 2：总推力 u1
ax2 = fig.add_subplot(2, 2, 2)
ax2.plot(t, u1, 'g-', lw=1.5)
ax2.axhline(y=m*g, color='k', ls='--', alpha=0.5, label=f'悬停推力 mg={m*g:.2f}N')
ax2.set_xlabel('时间 (s)'); ax2.set_ylabel('u₁ (N)')
ax2.set_title('总推力 u₁（由二阶导数推出）'); ax2.legend(); ax2.grid(True, alpha=0.3)

# 子图 3：滚转角 φ
ax3 = fig.add_subplot(2, 2, 3)
ax3.plot(t, np.degrees(phi), 'r-', lw=1.5, label='φ (roll)')
ax3.set_xlabel('时间 (s)'); ax3.set_ylabel('角度 (°)')
ax3.set_title('滚转角 φ（由 σ̈ 推出）'); ax3.legend(); ax3.grid(True, alpha=0.3)

# 子图 4：俯仰角 θ
ax4 = fig.add_subplot(2, 2, 4)
ax4.plot(t, np.degrees(theta), 'b-', lw=1.5, label='θ (pitch)')
ax4.set_xlabel('时间 (s)'); ax4.set_ylabel('角度 (°)')
ax4.set_title('俯仰角 θ（由 σ̈ 推出）'); ax4.legend(); ax4.grid(True, alpha=0.3)

plt.suptitle('微分平坦性验证：从轨迹 [x,y,z,ψ] 推导姿态与推力', fontsize=14)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/control3.png', dpi=150, bbox_inches='tight')
plt.show()

运行结果：

四、PID 控制基础

4.1 PID 参数对系统响应的影响

PID 控制器的三个参数 $K_p$、 $K_d$、 $K_i$ 分别影响系统的不同响应特征：

参数	Rise Time（上升时间）	Overshoot（超调量）	Settling Time（调节时间）	Steady-state Error（稳态误差）
$K_p \uparrow$	减少	增加	减少	减少
$K_d \uparrow$	—	减少	减少	—
$K_i \uparrow$	增加	增加	增加	—

4.2 阶跃响应特征量

阶跃响应的关键指标：

Rise Time（上升时间）：响应从初始值上升到稳态值的时间
Overshoot（超调量）：响应超过稳态值的最大偏差
Settling Time（调节时间）：响应进入并保持在稳态值 $\pm 1%$（或 $\pm 5%$）范围内的时间
Steady-state Error（稳态误差）：稳态时响应值与期望值之间的差异

个人理解：Settling Time 常用的标准有两种：进入 $\pm 5%$ 范围或 $\pm 1%$ 范围所需的时间。工程中更常用 $\pm 2%$ 标准。

个人理解： $K_p$ 减少上升时间但增加超调，$K_d$ 增加阻尼减少超调，$K_i$ 消除稳态误差但可能引起振荡。在四旋翼控制中，合理调节这三个参数至关重要——通常先调 $K_p$ 保证响应速度，再调 $K_d$ 抑制超调，最后适量加入 $K_i$ 消除稳态误差。

Python 验证

import numpy as np
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei']  # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False    # 解决负号显示为方框的问题
# ========== PID 参数调节对二阶系统阶跃响应的影响 ==========
# 被控对象：无人机高度方向简化模型  m * z̈ = u - mg
# 等效二阶系统：ë + (Kd/m)*ė + (Kp/m)*e = -(Ki/m)*∫e dt

def simulate_pid(Kp, Kd, Ki, dt=0.001, T=5.0):
    """仿真 PID 控制的二阶系统阶跃响应"""
    steps = int(T / dt)
    z = 0.0       # 当前位置（高度）
    dz = 0.0      # 当前速度
    z_des = 1.0   # 期望高度（阶跃输入）
    m = 0.5       # 无人机质量
    g = 9.81      # 重力加速度
    integral = 0.0  # 积分项

    z_hist = np.zeros(steps)
    for i in range(steps):
        e = z_des - z           # 位置误差
        de = -dz                # 速度误差（期望速度为 0）
        integral += e * dt      # 误差积分

        # PID 控制律：u = mg + Kp*e + Kd*ė + Ki*∫e
        u = m * g + Kp * e + Kd * de + Ki * integral

        # 动力学：m * z̈ = u - mg
        ddz = (u - m * g) / m
        dz += ddz * dt
        z += dz * dt
        z_hist[i] = z
    return z_hist

dt = 0.001; T = 5.0
t = np.arange(0, T, dt)

# --- 子图 1：改变 Kp（固定 Kd=2, Ki=0） ---
fig, axes = plt.subplots(1, 3, figsize=(16, 4.5))

ax = axes[0]
for Kp in [1, 5, 15, 30]:
    z_hist = simulate_pid(Kp=Kp, Kd=2.0, Ki=0.0, dt=dt, T=T)
    ax.plot(t, z_hist, label=f'Kp={Kp}')
ax.axhline(y=1.0, color='k', ls='--', alpha=0.4, label='目标')
ax.set_title('改变 Kp（Kd=2, Ki=0）')
ax.set_xlabel('时间 (s)'); ax.set_ylabel('高度 z (m)')
ax.legend(fontsize=8); ax.grid(True, alpha=0.3)
ax.set_ylim(-0.1, 1.8)

# --- 子图 2：改变 Kd（固定 Kp=15, Ki=0） ---
ax = axes[1]
for Kd in [0, 1, 3, 8]:
    z_hist = simulate_pid(Kp=15.0, Kd=Kd, Ki=0.0, dt=dt, T=T)
    ax.plot(t, z_hist, label=f'Kd={Kd}')
ax.axhline(y=1.0, color='k', ls='--', alpha=0.4, label='目标')
ax.set_title('改变 Kd（Kp=15, Ki=0）')
ax.set_xlabel('时间 (s)'); ax.set_ylabel('高度 z (m)')
ax.legend(fontsize=8); ax.grid(True, alpha=0.3)
ax.set_ylim(-0.1, 1.8)

# --- 子图 3：改变 Ki（固定 Kp=10, Kd=3） ---
ax = axes[2]
for Ki in [0, 2, 8, 20]:
    z_hist = simulate_pid(Kp=10.0, Kd=3.0, Ki=Ki, dt=dt, T=T)
    ax.plot(t, z_hist, label=f'Ki={Ki}')
ax.axhline(y=1.0, color='k', ls='--', alpha=0.4, label='目标')
ax.set_title('改变 Ki（Kp=10, Kd=3）')
ax.set_xlabel('时间 (s)'); ax.set_ylabel('高度 z (m)')
ax.legend(fontsize=8); ax.grid(True, alpha=0.3)
ax.set_ylim(-0.1, 1.8)

plt.suptitle('PID 参数对无人机高度阶跃响应的影响', fontsize=14)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/control4.png', dpi=150, bbox_inches='tight')
plt.show()

运行结果：

五、基于模型的控制

5.1 二阶系统模型

一个常见的二阶系统模型：

$$ m\ddot{x}(t) + b\dot{x}(t) + kx(t) = u(t) $$

其中 $m$ 为质量， $b$ 为阻尼系数， $k$ 为弹簧刚度， $u(t)$ 为控制输入。

5.2 PD 控制

PD 控制律：

$$ u(t) = \ddot{x}^{des}(t) + k_d \dot{e}(t) + k_p e(t) $$

其中 $e(t) = x^{des}(t) - x(t)$ 为跟踪误差。

个人理解：纯 PD 控制没有利用模型信息——它不可能完全补偿系统动态，加上传感器噪声和模型误差，跟踪误差一定会存在。这正是引入基于模型的前馈控制的动机。

5.3 基于模型的控制律（前馈 + PD 反馈）

$$ u(t) = \hat{m}(\ddot{x}^{des}(t) + k_d\dot{e}(t) + k_pe(t)) + \hat{b}\dot{x}(t) + \hat{k}x(t) $$

个人理解：该控制律的结构 = 前馈项（ $\hat{m}\ddot{x}^{des} + \hat{b}\dot{x} + \hat{k}x$，利用模型抵消已知动态） + PD 反馈项（ $k_d\dot{e} + k_pe$，修正建模误差和扰动）。

个人理解：与纯 PD 控制相比，基于模型的控制律多了 $\hat{b}\dot{x}$ 和 $\hat{k}x$ 两项前馈补偿。前馈项利用模型知识主动抵消已知动态，PD 反馈负责修正建模误差。 $\hat{m}, \hat{b}, \hat{k}$ 为模型估计值——估计越准，反馈项的负担越轻。

5.4 基于模型的控制步骤

获得目标系统的动态
特定子模型

基于伺服的控制：

使用带有微分的 PID 或 PD 控制器来将误差收敛到 0
独立子系统模型

个人理解：PID 和 LQR 都需要线性化模型作为前提。将非线性系统在工作点（如悬停点）线性化后，再对线性化后的子系统分别设计控制器。

5.5 控制框图（2D 情况）

ydes, zdes    ┌──────────┐     ┌──────┐     ┌──────────┐   u₂   ┌─────┐
ẏdes, żdes ──>│ 位置控制  │────>│ φ_c  │────>│ 姿态控制  │──────>│ UAV │
              └──────────┘     └──────┘     └──────────┘       └─────┘
                   ↑               u₁                             │
                   └──── y, z, ẏ, ż ──────── φ, φ̇ ──────────────┘

5.6 平衡条件与线性化

平衡条件（悬停状态）：

$$ y_0 = 0, \quad z_0 = 0, \quad \dot{\phi}_0 = 0, \quad u_{1,0} = mg, \quad u_{2,0} = 0 $$

线性化动力学模型（在悬停点附近）：

$$ \ddot{y} = -g\phi $$

$$ \ddot{z} = -g + \frac{u_1}{m} $$

$$ \ddot{\phi} = \frac{u_2}{I_{xx}} $$

个人理解：线性化后 $y$ 和 $\phi$ 直接耦合（ $\ddot{y} = -g\phi$）， $z$ 由 $u_1$ 独立控制， $\phi$ 由 $u_2$ 独立控制。这样原来的 6 维非线性系统变成了两个独立的线性子系统：(1) Z 轴高度控制，(2) Y-$\phi$ 耦合的位置-姿态控制。

5.7 Z 轴方向位置控制

PD 控制律：

$$ \ddot{z}_c = \ddot{z}^{des} + k_{d,z}(\dot{z}^{des} - \dot{z}) + k_{p,z}(z^{des} - z) $$

代入模型 $\ddot{z} = -g + \frac{u_1}{m}$：

$$ \boxed{u_1 = m\left(g + \ddot{z}^{des} + k_{d,z}(\dot{z}^{des} - \dot{z}) + k_{p,z}(z^{des} - z)\right)} $$

5.8 Y 轴方向位置控制

外环——PD 位置控制律：

$$ \ddot{y}_c = \ddot{y}^{des} + k_{d,y}(\dot{y}^{des} - \dot{y}) + k_{p,y}(y^{des} - y) $$

由模型 $\ddot{y} = -g\phi$，得到期望滚转角（外环输出）：

$$ \phi_c = -\frac{1}{g}\left(\ddot{y}^{des} + k_{d,y}(\dot{y}^{des} - \dot{y}) + k_{p,y}(y^{des} - y)\right) $$

内环——PD 姿态控制：

$$ \ddot{\phi}_c = k_{d,\phi}(\dot{\phi}_c - \dot{\phi}) + k_{p,\phi}(\phi_c - \phi) $$

$$ \boxed{u_2 = I_{xx}\left(k_{d,\phi}(\dot{\phi}_c - \dot{\phi}) + k_{p,\phi}(\phi_c - \phi)\right)} $$

个人理解：Y 轴位置控制是一个典型的级联（cascade）控制结构——外环位置控制器输出期望滚转角 $\phi_c$，内环姿态控制器跟踪该期望角度。内环频率需高于外环（通常 5~10 倍），以保证时间尺度分离。

Python 验证

import numpy as np
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei']  # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False    # 解决负号显示为方框的问题
# ========== 2D 无人机（Y-Z 平面）前馈 + PD 控制仿真 ==========

# --- 物理参数 ---
m = 0.5          # 质量 (kg)
g = 9.81         # 重力加速度 (m/s^2)
Ixx = 0.002      # 绕 x 轴转动惯量 (kg·m^2)
dt = 0.002       # 仿真步长 (s)
T_sim = 6.0      # 总仿真时间
N = int(T_sim / dt)

# --- PD 增益 ---
kp_z, kd_z = 15.0, 8.0       # Z 轴位置 PD
kp_y, kd_y = 8.0, 6.0        # Y 轴位置 PD
kp_phi, kd_phi = 150.0, 20.0 # 姿态角 PD

# --- 期望轨迹：悬停 2s 后移动到目标位置 ---
y_des = np.where(np.arange(N) * dt < 2.0, 0.0, 2.0)
z_des = np.where(np.arange(N) * dt < 2.0, 1.0, 2.5)

# --- 状态初始化 [y, z, phi, dy, dz, dphi] ---
state = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
hist_y, hist_z, hist_u1, hist_u2, hist_phi = [], [], [], [], []

for i in range(N):
    y, z, phi, dy, dz, dphi = state

    # 外环：Z 轴 PD + 重力前馈
    ddz_c = kp_z * (z_des[i] - z) + kd_z * (0 - dz)
    u1 = m * (g + ddz_c) / (np.cos(phi) + 1e-6)  # 前馈：mg/cos(φ)
    u1 = np.clip(u1, 0, 3 * m * g)  # 推力限幅

    # 外环：Y 轴 PD → 期望滚转角
    ddy_c = kp_y * (y_des[i] - y) + kd_y * (0 - dy)
    phi_c = -ddy_c / g  # 由线性化关系 ÿ = -g·φ
    phi_c = np.clip(phi_c, -0.5, 0.5)  # 限制期望角度

    # 内环：姿态 PD
    u2 = Ixx * (kp_phi * (phi_c - phi) + kd_phi * (0 - dphi))

    # 非线性动力学更新
    ddy = -(u1 / m) * np.sin(phi)
    ddz = (u1 / m) * np.cos(phi) - g
    ddphi = u2 / Ixx

    state += np.array([dy, dz, dphi, ddy, ddz, ddphi]) * dt

    hist_y.append(y); hist_z.append(z)
    hist_u1.append(u1); hist_u2.append(u2); hist_phi.append(phi)

t_arr = np.arange(N) * dt

# --- 绘图：2 行 2 列 ---
fig, axes = plt.subplots(2, 2, figsize=(13, 9))

# Y-Z 轨迹
axes[0, 0].plot(hist_y, hist_z, 'b-', lw=1.5, label='实际轨迹')
axes[0, 0].plot(y_des[0], z_des[0], 'go', ms=10, label='起始目标')
axes[0, 0].plot(y_des[-1], z_des[-1], 'r*', ms=14, label='终点目标')
axes[0, 0].set_xlabel('Y (m)'); axes[0, 0].set_ylabel('Z (m)')
axes[0, 0].set_title('Y-Z 平面飞行轨迹'); axes[0, 0].legend(); axes[0, 0].grid(True, alpha=0.3)

# 高度与水平位置响应
axes[0, 1].plot(t_arr, hist_z, 'b-', label='z 实际')
axes[0, 1].plot(t_arr, z_des, 'b--', alpha=0.5, label='z 期望')
axes[0, 1].plot(t_arr, hist_y, 'r-', label='y 实际')
axes[0, 1].plot(t_arr, y_des, 'r--', alpha=0.5, label='y 期望')
axes[0, 1].set_xlabel('时间 (s)'); axes[0, 1].set_ylabel('位置 (m)')
axes[0, 1].set_title('位置跟踪响应'); axes[0, 1].legend(); axes[0, 1].grid(True, alpha=0.3)

# 控制输入
axes[1, 0].plot(t_arr, hist_u1, 'g-', label='u₁ (总推力)')
axes[1, 0].axhline(y=m * g, color='k', ls='--', alpha=0.4, label=f'悬停推力 mg={m*g:.2f}')
axes[1, 0].set_xlabel('时间 (s)'); axes[1, 0].set_ylabel('推力 (N)')
axes[1, 0].set_title('总推力 u₁'); axes[1, 0].legend(); axes[1, 0].grid(True, alpha=0.3)

# 滚转角
axes[1, 1].plot(t_arr, np.degrees(hist_phi), 'm-', label='φ (滚转角)')
axes[1, 1].set_xlabel('时间 (s)'); axes[1, 1].set_ylabel('角度 (°)')
axes[1, 1].set_title('滚转角 φ 响应'); axes[1, 1].legend(); axes[1, 1].grid(True, alpha=0.3)

plt.suptitle('第五章：2D 无人机前馈 + PD 控制仿真（悬停→移动）', fontsize=14)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/control5.png', dpi=150, bbox_inches='tight')
plt.show()

运行结果：

六、三维轨迹跟踪控制

6.1 轨迹跟踪基本框架

给定期望轨迹 $\mathbf{x}^{des}(t)$，$\dot{\mathbf{x}}^{des}(t)$，$\ddot{\mathbf{x}}^{des}(t)$，在时间 $t \in [t_0, T]$ 内跟踪。

定义误差：

$$ e_p = \mathbf{x}^{des} - \mathbf{x}, \quad e_v = \dot{\mathbf{x}}^{des} - \dot{\mathbf{x}} $$

控制律：

$$ \ddot{\mathbf{x}} = \ddot{\mathbf{x}}^{des} + k_d e_v + k_p e_p $$

6.2 3D 控制框图

p_des  ┌────────────┐   ┌──────────┐        ┌──────────┐  u₂,u₃,u₄  ┌─────┐
ṗ_des ─>│ 轨迹跟踪    │──>│ 位置控制  │──u₁──>│ 姿态控制  │──────────>│ UAV │
       └────────────┘   └──────────┘  φc,θc  └──────────┘           └─────┘
            ↑                                                          │
            └─── p, ṗ ──────────────── φ,θ,ψ, ωx,ωy,ωz ─────────────┘

6.3 机体坐标系下的动力学方程

牛顿方程：

$$ m\ddot{\mathbf{p}} = \begin{bmatrix} 0 \ 0 \ -mg \end{bmatrix} + R \begin{bmatrix} 0 \ 0 \ F_1 + F_2 + F_3 + F_4 \end{bmatrix} $$

欧拉方程：

$$ I\begin{bmatrix} \dot{\omega}_x \ \dot{\omega}_y \ \dot{\omega}_z \end{bmatrix} + \begin{bmatrix} \omega_x \ \omega_y \ \omega_z \end{bmatrix} \times I\begin{bmatrix} \omega_x \ \omega_y \ \omega_z \end{bmatrix} = \begin{bmatrix} l(F_2 - F_4) \ l(F_3 - F_1) \ M_1 - M_2 + M_3 - M_4 \end{bmatrix} $$

角速度回收： $\omega_b = R^T \dot{R}$

6.4 ZXY 旋转欧拉角展开

$$ \omega_b = \begin{bmatrix} p \ q \ r \end{bmatrix} = \begin{bmatrix} c\theta & 0 & -c\phi s\theta \ 0 & 1 & s\phi \ s\theta & 0 & c\phi c\theta \end{bmatrix} \begin{bmatrix} \dot{\phi} \ \dot{\theta} \ \dot{\psi} \end{bmatrix} $$

其中 $p$ = Roll 角速度， $q$ = Pitch 角速度， $r$ = Yaw 角速度。

个人理解：该角速度转换矩阵将欧拉角导数（定义在各中间旋转坐标系中）映射为机体坐标系下的角速度分量。

6.5 欧拉角微分方程

欧拉角微分的逆关系（求解 $\dot{\phi}, \dot{\theta}, \dot{\psi}$）：

$$ \begin{bmatrix} \dot{\phi} \ \dot{\theta} \ \dot{\psi} \end{bmatrix} = \begin{bmatrix} c\theta & 0 & -c\phi s\theta \ 0 & 1 & s\phi \ s\theta & 0 & c\phi c\theta \end{bmatrix}^{-1} \begin{bmatrix} \omega_x \ \omega_y \ \omega_z \end{bmatrix} $$

6.6 悬停点线性化

在悬停点附近（ $\psi_0 = 0$， $\theta_0 = 0$， $u_{1,0} = mg$）线性化：

$$ \ddot{p}_x = \ddot{x} = g(\theta\cos\psi + \phi\sin\psi) $$

$$ \ddot{p}_y = \ddot{y} = g(\theta\sin\psi - \phi\cos\psi) $$

$$ \ddot{p}_z = \ddot{z} = -g + \frac{u_1}{m} $$

个人理解：注意 $\psi$ 并不为零时（即偏航角不为零）， $x$ 和 $y$ 方向的加速度都同时依赖于 $\phi$ 和 $\theta$，这就是偏航角带来的耦合。当 $\psi = 0$ 时简化为 $\ddot{x} = g\theta$, $\ddot{y} = -g\phi$，与第五章的 2D 情况一致。

6.7 位置控制器（PID）

$$ \ddot{p}_{i,c} = \ddot{p}_i^{des} + k_{d,i}(\dot{p}_i^{des} - \dot{p}_i) + k_{p,i}(p_i^{des} - p_i) $$

总推力：

$$ u_1 = m(g + \ddot{p}_{z,c}) $$

期望姿态角（考虑偏航角 $\psi$）：

$$ \phi_c = \frac{1}{g}(\ddot{p}_{x,c}\sin\psi - \ddot{p}_{y,c}\cos\psi) $$

$$ \theta_c = \frac{1}{g}(\ddot{p}_{x,c}\cos\psi + \ddot{p}_{y,c}\sin\psi) $$

个人理解：位置控制器只输出期望的 $\phi_c, \theta_c$ 和推力 $u_1$，偏航角 $\psi$ 由单独的偏航控制器跟踪期望值 $\psi^{des}$，不参与位置外环。

6.8 姿态控制器（PID）

$$ \begin{bmatrix} \ddot{\phi}_c \ \ddot{\theta}_c \ \ddot{\psi}_c \end{bmatrix} = \begin{bmatrix} k_{p,\phi}(\phi_c - \phi) + k_{d,\phi}(\dot{\phi}_c - \dot{\phi}) \ k_{p,\theta}(\theta_c - \theta) + k_{d,\theta}(\dot{\theta}_c - \dot{\theta}) \ k_{p,\psi}(\psi_c - \psi) + k_{d,\psi}(\dot{\psi}_c - \dot{\psi}) \end{bmatrix} $$

控制力矩：

$$ \mathbf{u}_\alpha = I \begin{bmatrix} \ddot{\phi}_c \ \ddot{\theta}_c \ \ddot{\psi}_c \end{bmatrix} + \begin{bmatrix} \omega_x \ \omega_y \ \omega_z \end{bmatrix} \times I \begin{bmatrix} \omega_x \ \omega_y \ \omega_z \end{bmatrix} $$

个人理解：力矩控制律中的第二项 $\omega \times I\omega$ 是前馈补偿项，用于抵消陀螺效应。在悬停附近可以忽略，但在快速机动时必须加入。

Python 验证

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
plt.rcParams['font.sans-serif'] = ['SimHei']  # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False    # 解决负号显示为方框的问题
# ========== 3D "8"字轨迹跟踪仿真 ==========

# --- 物理参数 ---
m = 0.5; g = 9.81; dt = 0.005; T_sim = 12.0
N = int(T_sim / dt)
t = np.arange(N) * dt
Kp = np.diag([6.0, 6.0, 10.0])    # 位置 PD 增益
Kd = np.diag([4.0, 4.0, 6.0])
kp_att, kd_att = 80.0, 15.0       # 姿态 PD 增益
Ixx = Iyy = 0.002; Izz = 0.004

# --- 期望 "8" 字轨迹 ---
omega_t = 2 * np.pi / T_sim
x_des = 2.0 * np.sin(omega_t * t)
y_des = 1.5 * np.sin(2 * omega_t * t)
z_des = 1.0 + 0.3 * np.sin(omega_t * t)

# 期望速度、加速度（解析求导）
dx_des = 2.0 * omega_t * np.cos(omega_t * t)
dy_des = 1.5 * 2 * omega_t * np.cos(2 * omega_t * t)
dz_des = 0.3 * omega_t * np.cos(omega_t * t)
ddx_des = -2.0 * omega_t**2 * np.sin(omega_t * t)
ddy_des = -1.5 * (2*omega_t)**2 * np.sin(2 * omega_t * t)
ddz_des = -0.3 * omega_t**2 * np.sin(omega_t * t)

# --- 状态初始化 ---
pos = np.array([0.0, 0.0, 0.0])   # 位置
vel = np.array([0.0, 0.0, 0.0])   # 速度
phi, theta, psi = 0.0, 0.0, 0.0   # 欧拉角
dphi, dtheta, dpsi = 0.0, 0.0, 0.0

hist_pos = np.zeros((N, 3))
for i in range(N):
    p_des = np.array([x_des[i], y_des[i], z_des[i]])
    v_des = np.array([dx_des[i], dy_des[i], dz_des[i]])
    a_des = np.array([ddx_des[i], ddy_des[i], ddz_des[i]])

    # 位置 PD 控制 → 期望加速度
    ep = p_des - pos; ev = v_des - vel
    acc_cmd = a_des + Kp @ ep + Kd @ ev

    # 总推力（前馈 + PD）
    u1 = m * np.sqrt(acc_cmd[0]**2 + acc_cmd[1]**2 + (acc_cmd[2] + g)**2)
    u1 = np.clip(u1, 0.1, 4 * m * g)

    # 期望姿态角（考虑 ψ≈0 简化）
    phi_c = np.clip((acc_cmd[0]*np.sin(psi) - acc_cmd[1]*np.cos(psi)) / g, -0.5, 0.5)
    theta_c = np.clip((acc_cmd[0]*np.cos(psi) + acc_cmd[1]*np.sin(psi)) / g, -0.5, 0.5)

    # 姿态 PD 控制
    u2 = Ixx * (kp_att * (phi_c - phi) + kd_att * (0 - dphi))
    u3 = Iyy * (kp_att * (theta_c - theta) + kd_att * (0 - dtheta))

    # 简化 3D 动力学更新
    cphi, sphi = np.cos(phi), np.sin(phi)
    cth, sth = np.cos(theta), np.sin(theta)
    cpsi, spsi = np.cos(psi), np.sin(psi)
    ax = (u1/m) * (cpsi*sth*cphi + spsi*sphi)
    ay = (u1/m) * (spsi*sth*cphi - cpsi*sphi)
    az = (u1/m) * cphi*cth - g

    vel += np.array([ax, ay, az]) * dt
    pos += vel * dt
    dphi += (u2 / Ixx) * dt; phi += dphi * dt
    dtheta += (u3 / Iyy) * dt; theta += dtheta * dt

    hist_pos[i] = pos

# --- 绘图：3D 轨迹对比 ---
fig = plt.figure(figsize=(12, 8))
ax3d = fig.add_subplot(1, 1, 1, projection='3d')
ax3d.plot(x_des, y_des, z_des, 'b--', lw=1.5, alpha=0.6, label='期望轨迹（"8"字）')
ax3d.plot(hist_pos[:, 0], hist_pos[:, 1], hist_pos[:, 2],
          'r-', lw=1.2, label='实际轨迹')

# 标注起点和终点
ax3d.scatter(*hist_pos[0], c='g', s=100, marker='o', zorder=5, label='起点')
ax3d.scatter(*hist_pos[-1], c='k', s=100, marker='*', zorder=5, label='终点')

# 每隔一段绘制位置标记
step = N // 10
for j in range(0, N, step):
    ax3d.plot([x_des[j], hist_pos[j, 0]], [y_des[j], hist_pos[j, 1]],
              [z_des[j], hist_pos[j, 2]], 'k-', alpha=0.2, lw=0.8)

ax3d.set_xlabel('X (m)'); ax3d.set_ylabel('Y (m)'); ax3d.set_zlabel('Z (m)')
ax3d.set_title('第六章：3D "8"字轨迹跟踪（PD 控制）', fontsize=14)
ax3d.legend(fontsize=10)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/control6.png', dpi=150, bbox_inches='tight')
plt.show()

运行结果：

七、SE(3) 控制

个人理解：当欧拉角线性化带来的近似误差不可接受时（例如大角度机动），就需要使用 SE(3) 控制——它直接在旋转矩阵上定义误差，不做任何线性化近似，因此不损失精度。

7.1 SE(3) 群上的跟踪控制

SE(3) 控制直接在特殊欧几里得群 $SE(3)$ 上定义误差，避免了欧拉角的万向节锁（Gimbal Lock）问题。

状态表示：

$$ G(t) = \begin{bmatrix} R(t) & \mathbf{p}(t) \ 0 & 1 \end{bmatrix} \in SE(3) $$

其中 $R \in SO(3)$ 为旋转矩阵， $\mathbf{p} \in \mathbb{R}^3$ 为位置。

7.2 误差定义

位置误差：

$$ e_p = \mathbf{p} - \mathbf{p}^{des} $$

速度误差：

$$ e_v = \dot{\mathbf{p}} - \dot{\mathbf{p}}^{des} $$

7.3 期望力计算

$$ \mathbf{f}_{des} = -(-k_p e_p - k_d e_v + mg\hat{z}_W + m\ddot{\mathbf{x}}^{des}) $$

个人理解：将期望力投影到机体 $z$ 轴上得到总推力标量 $u_1$，剩余的力方向信息用于构建期望姿态。这样位置控制和姿态控制仍然是解耦的：位置环输出期望力，姿态环跟踪期望姿态。

总推力（投影到机体 $z$ 轴）：

$$ u_1 = \mathbf{f}_{des} \cdot \hat{z}_B $$

7.4 期望姿态矩阵构建

从期望力方向构建期望旋转矩阵：

$$ \hat{z}_{B,des} = \frac{\mathbf{f}_{des}}{|\mathbf{f}_{des}|} $$

个人理解：从期望力方向构建完整的旋转矩阵 $R_{des}$。注意：在 snap 较大的激进机动中，期望力方向可能接近水平，此时 $\hat{z}_{B,des}$ 接近与 $\hat{x}_C$ 平行，叉乘会退化，需要做奇异性保护。

根据期望偏航角 $\psi^{des}$ 构建完整旋转矩阵：

$$ \hat{x}_C = [\cos(\psi^{des}), \sin(\psi^{des}), 0]^T $$

$$ \hat{y}_{B,des} = \frac{\hat{z}_{B,des} \times \hat{x}_C}{|\hat{z}_{B,des} \times \hat{x}_C|} $$

$$ \hat{x}_{B,des} = \hat{y}_{B,des} \times \hat{z}_{B,des} $$

$$ R_{des} = [\hat{x}_{B,des}, \hat{y}_{B,des}, \hat{z}_{B,des}] $$

7.5 姿态误差与角速度误差

姿态误差（使用 Vee 映射 $\vee$，从 $\mathfrak{so}(3)$ 到 $\mathbb{R}^3$）：

$$ e_R = \frac{1}{2}(R_{des}^T R - R^T R_{des})^\vee $$

角速度误差：

$$ e_\omega = \omega_{BW} - R^T R_{des} \omega_{BW,T} $$

7.6 控制力矩

$$ \boxed{[u_2, u_3, u_4]^T = -k_R e_R - k_\omega e_\omega} $$

个人理解：SE(3) 控制的优势在于：(1) 避免万向节锁，适用于大角度机动；(2) 姿态误差定义在 $SO(3)$ 上，具有全局收敛性。代价是实现复杂度较高，需要直接操作旋转矩阵。其构建期望姿态矩阵的方法与第三章微分平坦中的推导完全一致。

Python 验证

import numpy as np
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False
# ========== 对比：欧拉角控制 vs SE(3) 控制在大角度翻转中的表现 ==========

def vee(M):
    """反对称矩阵 → 向量"""
    return np.array([M[2,1], M[0,2], M[1,0]])

def Ry(a):
    """绕 Y 轴旋转矩阵"""
    c, s = np.cos(a), np.sin(a)
    return np.array([[c,0,s],[0,1,0],[-s,0,c]])

# --- 仿真参数（使用单位惯量简化，聚焦控制差异） ---
dt = 0.005; T_sim = 6.0; N = int(T_sim / dt)
t = np.arange(N) * dt
I_mat = np.diag([1.0, 1.0, 1.0])  # 单位惯量

# 目标：绕 Y 轴翻转 170°（接近 pitch ±90° 万向节锁区域）
flip_angle = np.deg2rad(170)
R_des = Ry(flip_angle)

# --- 方法 1：欧拉角 PD 控制 ---
euler = np.zeros(3)   # [phi, theta, psi]
deuler = np.zeros(3)
kp_e, kd_e = 4.0, 3.0

# 从 R_des 提取目标欧拉角（ZYX 顺序）
theta_des = np.arctan2(-R_des[2,0], np.sqrt(R_des[0,0]**2 + R_des[1,0]**2))
phi_des = np.arctan2(R_des[2,1], R_des[2,2])
psi_des = np.arctan2(R_des[1,0], R_des[0,0])
euler_des = np.array([phi_des, theta_des, psi_des])

err_euler = np.zeros(N)
for i in range(N):
    e_ang = euler_des - euler
    err_euler[i] = np.linalg.norm(e_ang)
    tau = kp_e * e_ang + kd_e * (-deuler)
    deuler += tau * dt
    euler += deuler * dt

# --- 方法 2：SE(3) 控制 ---
R_cur = np.eye(3)
omega = np.zeros(3)
kR, kw = 4.0, 3.0
err_se3 = np.zeros(N)
for i in range(N):
    eR_mat = R_des.T @ R_cur - R_cur.T @ R_des
    eR = 0.5 * vee(eR_mat)
    err_se3[i] = np.linalg.norm(eR)
    tau_se3 = -kR * eR - kw * omega
    omega += tau_se3 * dt
    # Rodrigues 公式更新旋转矩阵
    wx = omega * dt
    ang = np.linalg.norm(wx)
    if ang > 1e-12:
        K = np.array([[0,-wx[2],wx[1]],[wx[2],0,-wx[0]],[-wx[1],wx[0],0]]) / ang
        dR = np.eye(3) + np.sin(ang)*K + (1-np.cos(ang))*(K@K)
    else:
        dR = np.eye(3)
    R_cur = R_cur @ dR

# --- 绘图对比 ---
fig, axes = plt.subplots(1, 2, figsize=(14, 5))
axes[0].plot(t, err_euler, 'r-', lw=1.5, label='欧拉角 PD 误差')
axes[0].plot(t, err_se3, 'b-', lw=1.5, label='SE(3) 姿态误差')
axes[0].set_xlabel('时间 (s)'); axes[0].set_ylabel('姿态误差范数')
axes[0].set_title('姿态误差对比（目标：绕 Y 轴翻转 170°）')
axes[0].legend(); axes[0].grid(True, alpha=0.3)

axes[1].semilogy(t, np.clip(err_euler, 1e-10, None), 'r-', lw=1.5, label='欧拉角 PD')
axes[1].semilogy(t, np.clip(err_se3, 1e-10, None), 'b-', lw=1.5, label='SE(3)')
axes[1].set_xlabel('时间 (s)'); axes[1].set_ylabel('姿态误差（对数）')
axes[1].set_title('对数尺度对比'); axes[1].legend(); axes[1].grid(True, alpha=0.3)

plt.suptitle('第七章：欧拉角 vs SE(3) 控制 — 大角度翻转机动', fontsize=14)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/control7.png', dpi=150, bbox_inches='tight')
plt.show()

运行结果：

八、MPC 控制

8.1 Planning 与 Control 的关系

个人理解：Planning 就是外环的 Control，Control 就是内环的 Planning。

个人理解：Planning 基于模型预测系统对输入的响应，提前规划动作序列；Control 则负责在实际执行中弥补模型与现实的 mismatch（建模误差、扰动等）。两者并非对立——MPC 将 Planning 和 Control 融合在一起，在滑动时间窗口内同时做规划和控制。

个人理解：MPC 同时包含了轨迹规划和控制——它需要在预测区间内有完整的轨迹。如果只需要跟踪已有的 P.V.A.（位置、速度、加速度）参考轨迹，可以直接交给 SE(3) 控制器，无需 MPC。MPC 的价值在于能处理非线性约束和在线重规划，但代价是求解复杂度高。

8.2 CMPC 状态空间模型

状态向量：

$$ \mathbf{x}^{(k)} = [x, y, V_x, a_x, V_y, a_y, z, V_z, \theta_{des}, \theta, V_\phi, \theta_\phi]^T $$

输入向量：

$$ \mathbf{u}^{(k)} = [j_x, j_y, j_z, \ddot{\alpha}]^T $$

8.3 优化问题

$$ \min J = \sum_{k=1}^{N} \left(|\mathbf{x}^{(k)} - \mathbf{x}_{ref}^{(k)}|^2 + q \cdot V_\alpha^{(k)}\right) $$

约束条件：

$$ \text{s.t.} \quad \mathbf{x}^{(k+1)} = A\mathbf{x}^{(k)} + B\mathbf{u}^{(k)}, \quad k = 1, 2, \ldots, N $$

$$ \mathbf{x}_l \leq \mathbf{x}^{(k)} \leq \mathbf{x}_u, \quad k = 1, 2, \ldots, N-1 $$

$$ \mathbf{u}_l \leq \mathbf{u}^{(k)} \leq \mathbf{u}_u, \quad k = 1, 2, \ldots, N-1 $$

$$ C^k: [x^k, y^k, z^k]^T \in \delta^k, \quad k = 1, 2, \ldots, N-1 $$

个人理解：约束含义——状态约束限制了速度上下限（如 $|V_x^{(k)}| \leq V_{max}$），输入约束限制了 jerk 范围， $C^k$ 约束要求每一步的位置 $[x^k, y^k, z^k]$ 在安全区域 $\delta^k$ 内（即碰撞规避约束）。

个人理解：CMPC 中的线性化模型将状态间的动态关系用离散时间递推表达（$\mathbf{x}^{(k+1)} = A\mathbf{x}^{(k)} + B\mathbf{u}^{(k)}$），包括位置-速度-加速度的积分关系和姿态动态。轨迹跟踪目标和安全约束都在 CMPC 的统一优化框架中处理。

个人理解：MPC 的核心思想是在每个时刻求解一个有限时域的优化问题，只执行第一步控制输入，然后滚动重复（Receding Horizon）。其优势在于可以显式处理约束（速度限制、位置约束、碰撞约束等），但计算量较大，需要在线求解优化问题。MPC 模糊了控制与规划的边界——它在每个控制周期内同时做局部轨迹规划和控制。

Python 验证

import numpy as np
import matplotlib.pyplot as plt

# ========== 简化 1D MPC 高度控制（滚动优化 + 推力约束） ==========
plt.rcParams['font.sans-serif'] = ['SimHei']  # Windows 自带黑体
plt.rcParams['axes.unicode_minus'] = False    # 解决负号显示为方框的问题
# --- 系统参数 ---
m = 0.5; g = 9.81; dt_ctrl = 0.05  # 控制周期 50ms
N_horizon = 20                       # 预测步长
T_sim = 6.0; N_sim = int(T_sim / dt_ctrl)
z_target = 2.0                       # 目标高度

# 推力约束（物理限制）
a_min = -g          # 最小加速度（自由落体，推力=0）
a_max = 2.0 * g     # 最大加速度（推力上限）

# MPC 代价权重
Q_z, Q_v, R_a = 10.0, 5.0, 0.1  # 位置、速度、控制代价

def solve_mpc(z0, v0, z_ref, N_h):
    """
    求解 1D MPC：手动构造二次规划并用 numpy 求解
    状态：[z, v]，输入：a（加速度），离散模型：z+=v*dt, v+=a*dt
    """
    # 构造预测矩阵：X = Sx*x0 + Su*U
    Sx = np.zeros((2 * N_h, 2))  # 状态传播矩阵
    Su = np.zeros((2 * N_h, N_h))  # 输入影响矩阵

    A = np.array([[1, dt_ctrl], [0, 1]])
    B = np.array([[0.5 * dt_ctrl**2], [dt_ctrl]])

    # 逐步构造预测矩阵
    A_pow = np.eye(2)
    for k in range(N_h):
        A_pow = A_pow @ A
        Sx[2*k:2*k+2, :] = A_pow
        for j in range(k + 1):
            A_mid = np.linalg.matrix_power(A, k - j)
            Su[2*k:2*k+2, j] = (A_mid @ B).flatten()

    # 代价矩阵 Q_bar, R_bar
    Q_bar = np.zeros((2 * N_h, 2 * N_h))
    for k in range(N_h):
        Q_bar[2*k, 2*k] = Q_z      # 位置权重
        Q_bar[2*k+1, 2*k+1] = Q_v  # 速度权重
    R_bar = R_a * np.eye(N_h)

    # 参考轨迹向量
    X_ref = np.zeros(2 * N_h)
    for k in range(N_h):
        X_ref[2*k] = z_ref
        X_ref[2*k+1] = 0.0  # 期望速度为 0

    x0 = np.array([z0, v0])

    # 无约束最优解：U* = (Su'QSu + R)^{-1} Su'Q(Xref - Sx*x0)
    H = Su.T @ Q_bar @ Su + R_bar
    f = Su.T @ Q_bar @ (X_ref - Sx @ x0)
    U_star = np.linalg.solve(H, f)

    # 施加约束（简单裁剪）
    U_star = np.clip(U_star, a_min, a_max)
    return U_star

# --- 仿真主循环 ---
z, v = 0.0, 0.0  # 初始状态：地面静止
hist_z, hist_v, hist_a = [], [], []
hist_pred_z = []  # 记录每步的预测轨迹（用于可视化）

for i in range(N_sim):
    U_opt = solve_mpc(z, v, z_target, N_horizon)
    a_cmd = U_opt[0]  # 只执行第一步（滚动优化）

    # 记录预测轨迹
    if i % 20 == 0:
        pred_z = [z]
        pz, pv = z, v
        for k in range(N_horizon):
            ak = np.clip(U_opt[k], a_min, a_max)
            pv += ak * dt_ctrl
            pz += pv * dt_ctrl
            pred_z.append(pz)
        hist_pred_z.append((i * dt_ctrl, pred_z))

    # 状态更新
    v += a_cmd * dt_ctrl
    z += v * dt_ctrl
    hist_z.append(z); hist_v.append(v); hist_a.append(a_cmd)

t_arr = np.arange(N_sim) * dt_ctrl

# --- 绘图 ---
fig, axes = plt.subplots(2, 2, figsize=(14, 9))

# 高度响应 + 预测轨迹
ax = axes[0, 0]
ax.plot(t_arr, hist_z, 'b-', lw=2, label='实际高度')
ax.axhline(y=z_target, color='r', ls='--', lw=1.5, label=f'目标 z={z_target}m')
for (t0, pred) in hist_pred_z:
    t_pred = t0 + np.arange(len(pred)) * dt_ctrl
    ax.plot(t_pred, pred, 'g-', alpha=0.3, lw=0.8)
ax.plot([], [], 'g-', alpha=0.5, label='预测轨迹（滚动窗口）')
ax.set_xlabel('时间 (s)'); ax.set_ylabel('高度 (m)')
ax.set_title('MPC 高度控制响应'); ax.legend(); ax.grid(True, alpha=0.3)

# 速度
ax = axes[0, 1]
ax.plot(t_arr, hist_v, 'c-', lw=1.5)
ax.set_xlabel('时间 (s)'); ax.set_ylabel('速度 (m/s)')
ax.set_title('垂直速度'); ax.grid(True, alpha=0.3)

# 控制输入（加速度/推力）
ax = axes[1, 0]
ax.plot(t_arr, hist_a, 'm-', lw=1.5, label='MPC 输出 a')
ax.axhline(y=a_max, color='r', ls=':', label=f'最大加速度 {a_max:.1f}')
ax.axhline(y=a_min, color='b', ls=':', label=f'最小加速度 {a_min:.1f}')
ax.axhline(y=0, color='k', ls='-', alpha=0.2)
ax.set_xlabel('时间 (s)'); ax.set_ylabel('加速度 (m/s²)')
ax.set_title('控制输入（含约束裁剪）'); ax.legend(fontsize=8); ax.grid(True, alpha=0.3)

# 对应推力
ax = axes[1, 1]
thrust = [m * (a + g) for a in hist_a]
ax.plot(t_arr, thrust, 'g-', lw=1.5, label='推力 u₁ = m(a+g)')
ax.axhline(y=m * g, color='k', ls='--', alpha=0.4, label=f'悬停推力 {m*g:.2f}N')
ax.axhline(y=m * (a_max + g), color='r', ls=':', alpha=0.6, label='推力上限')
ax.axhline(y=0, color='b', ls=':', alpha=0.6, label='推力下限 (0N)')
ax.set_xlabel('时间 (s)'); ax.set_ylabel('推力 (N)')
ax.set_title('推力约束可视化'); ax.legend(fontsize=8); ax.grid(True, alpha=0.3)

plt.suptitle('第八章：1D MPC 高度控制 — 滚动优化 + 推力约束', fontsize=14)
plt.tight_layout()
import os; os.makedirs('assets', exist_ok=True)
plt.savefig('assets/control8.png', dpi=150, bbox_inches='tight')
plt.show()

运行结果：

附录：控制部分符号表

符号	含义
$m$	无人机质量
$g$	重力加速度
$I$	转动惯量矩阵
$I_{xx}$	绕 $x$ 轴转动惯量
$R$	旋转矩阵（机体→世界）
$\phi, \theta, \psi$	滚转角、俯仰角、偏航角
$\omega_x, \omega_y, \omega_z$	机体角速度分量
$p, q, r$	Roll/Pitch/Yaw 角速度
$F_i$	第 $i$ 个电机推力
$M_i$	第 $i$ 个电机反扭矩
$u_1$	总推力
$u_2, u_3, u_4$	控制力矩
$l$	电机到质心的距离
$k_F, k_M$	推力/力矩系数
$k_p, k_d, k_i$	PID 增益
$\hat{z}_B, \hat{z}_W$	机体/世界坐标系 Z 轴单位向量
$e_p, e_v$	位置/速度跟踪误差
$e_R, e_\omega$	姿态/角速度误差（SE(3)）
$SE(3)$	特殊欧几里得群
$SO(3)$	特殊正交群
$\sigma$	平坦输出 $[x, y, z, \psi]^T$

Uh oh!

FilesExpand file tree

无人机控制教程.md

Latest commit

History

无人机控制教程.md

File metadata and controls

四旋翼无人机控制教程

导读

一、坐标系与旋转基础

1.1 二维坐标旋转的几何推导

方式一：几何投影法

方式二：极坐标法

1.2 两种旋转模式的区别

1.3 三维旋转矩阵

1.4 无人机坐标系变换

Python 验证

二、四旋翼飞行器动力学建模

2.1 电机模型

2.2 Z-X-Y 欧拉角与旋转矩阵

2.3 角速度与欧拉角导数的关系

2.4 牛顿方程（平动动力学）

2.5 欧拉方程（转动动力学）

2.6 动力学模型总结

2.7 平面状态方程示例

Python 验证

三、微分平坦性

3.1 微分平坦的定义与意义

3.2 平坦输出的选择

3.3 从平坦输出推导全部状态

3.4 姿态角的推导

推导总推力 $u_1$

构建期望旋转矩阵

提取欧拉角

3.5 角速度 p, q, r 的推导

提取各分量 p, q, r

Python 验证

四、PID 控制基础

4.1 PID 参数对系统响应的影响

4.2 阶跃响应特征量

Python 验证

五、基于模型的控制

5.1 二阶系统模型

5.2 PD 控制

5.3 基于模型的控制律（前馈 + PD 反馈）

5.4 基于模型的控制步骤

5.5 控制框图（2D 情况）

5.6 平衡条件与线性化

5.7 Z 轴方向位置控制

5.8 Y 轴方向位置控制

Python 验证

六、三维轨迹跟踪控制

6.1 轨迹跟踪基本框架

6.2 3D 控制框图

6.3 机体坐标系下的动力学方程

6.4 ZXY 旋转欧拉角展开

6.5 欧拉角微分方程

6.6 悬停点线性化

6.7 位置控制器（PID）

6.8 姿态控制器（PID）

Python 验证

七、SE(3) 控制

7.1 SE(3) 群上的跟踪控制

7.2 误差定义

7.3 期望力计算

7.4 期望姿态矩阵构建

7.5 姿态误差与角速度误差

7.6 控制力矩

Python 验证

八、MPC 控制

8.1 Planning 与 Control 的关系

8.2 CMPC 状态空间模型

8.3 优化问题

Python 验证

附录：控制部分符号表