探索四旋翼飞行器仿真：从公式推导到代码实现

RZcKUXIhvPR

105人浏览 · 2026-04-02 16:30:00

RZcKUXIhvPR · 2026-04-02 16:30:00 发布

四旋翼、四轴飞行器仿真及公式推导：四旋翼定高控制、定高姿态控制、自由落体仿真，动力学模型线性化、位置环轨迹跟踪控制、姿态环轨迹跟踪控制，多点任务控制、圆弧轨迹、直线轨迹、定高轨迹、风阻力模型，状态观测器设计

在飞行器领域，四旋翼飞行器凭借其灵活的机动性和相对简单的结构，成为了研究和应用的热门对象。今天，咱就来深入探讨四旋翼飞行器仿真过程中的各种关键环节，从理论公式推导到实际代码实现。

四旋翼飞行器的定高控制

定高姿态控制

定高姿态控制的核心在于通过调整飞行器的姿态，来维持其高度的稳定。假设我们已经有了飞行器的动力学模型，这个模型可以用一系列的微分方程来描述。例如，高度 $h$ 与旋翼产生的升力 $F$ 之间的关系可以简化为：

$m\ddot{h} = F - mg$

这里 $m$ 是飞行器的质量，$g$ 是重力加速度。要实现定高，我们就需要让 $\ddot{h} = 0$，也就是 $F = mg$。在代码中，我们可以通过PID控制器来实现这一目标：

import numpy as np

class PID:
    def __init__(self, kp, ki, kd):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.prev_error = 0
        self.integral = 0

    def update(self, setpoint, process_variable):
        error = setpoint - process_variable
        p_term = self.kp * error
        self.integral += error
        i_term = self.ki * error
        derivative = error - self.prev_error
        d_term = self.kd * derivative
        self.prev_error = error
        return p_term + i_term + d_term


# 初始化PID参数
kp = 1.0
ki = 0.1
kd = 0.01
pid = PID(kp, ki, kd)

# 模拟高度控制
current_height = 0
target_height = 10
time_step = 0.01
for _ in range(1000):
    control_signal = pid.update(target_height, current_height)
    # 根据控制信号更新高度，这里简化假设升力与控制信号成正比
    current_height += control_signal * time_step
    print(f"Current height: {current_height}")

在这段代码中，PID类通过不断计算当前高度与目标高度的误差，然后利用比例、积分和微分三个项来调整控制信号，从而使飞行器尽可能接近目标高度。

自由落体仿真

自由落体仿真是检验定高控制算法的一个重要场景。在自由落体状态下，飞行器不受升力控制，仅受重力作用。根据运动学公式：

$h = h0 + v0t - \frac{1}{2}gt^2$

这里 $h0$ 是初始高度，$v0$ 是初始速度。在代码实现中：

import matplotlib.pyplot as plt

# 自由落体参数
h0 = 100
v0 = 0
g = 9.81
t = np.arange(0, 5, 0.01)
h = h0 + v0 * t - 0.5 * g * t**2

plt.plot(t, h)
plt.xlabel('Time (s)')
plt.ylabel('Height (m)')
plt.title('Free Fall Simulation')
plt.show()

这段代码简单地根据自由落体公式计算不同时间点的高度，并通过matplotlib库绘制出高度随时间变化的曲线。

动力学模型线性化

四旋翼飞行器的动力学模型本质上是非线性的，但为了便于控制设计，我们常常对其进行线性化处理。假设飞行器的姿态角为 $\phi, \theta, \psi$（分别为滚转角、俯仰角和偏航角），通过小角度假设（即 $\sin\phi \approx \phi$, $\sin\theta \approx \theta$, $\cos\phi \approx 1$, $\cos\theta \approx 1$），我们可以将原本复杂的非线性动力学方程简化为线性形式。

例如，在姿态动力学方程中，对于滚转运动：

$Ix\ddot{\phi} = L(F2 - F_4)$

这里 $Ix$ 是飞行器绕 $x$ 轴的转动惯量，$F2$ 和 $F_4$ 分别是对应旋翼产生的升力，$L$ 是旋翼到飞行器中心的距离。线性化后，我们可以更容易地设计基于线性系统理论的控制器。

位置环与姿态环轨迹跟踪控制

位置环轨迹跟踪控制

位置环的目标是让飞行器跟踪给定的位置轨迹，比如直线轨迹、圆弧轨迹或定高轨迹。以直线轨迹为例，假设目标轨迹为 $x = x0 + vt$，$y = y0$，$z = z_0$。我们可以在代码中这样实现：

import numpy as np

# 定义目标直线轨迹参数
x0 = 0
y0 = 0
z0 = 10
v = 1
time = np.arange(0, 10, 0.01)
target_x = x0 + v * time
target_y = np.zeros_like(time) + y0
target_z = np.zeros_like(time) + z0

# 这里假设已经有飞行器当前位置计算函数
def get_current_position():
    # 实际应用中这里会根据传感器或模型计算位置
    return np.random.rand(3)

current_x, current_y, current_z = get_current_position()
for i in range(len(time)):
    # 计算位置误差
    error_x = target_x[i] - current_x
    error_y = target_y[i] - current_y
    error_z = target_z[i] - current_z
    # 这里可以添加基于误差的控制算法来更新飞行器位置
    current_x += 0.1 * error_x
    current_y += 0.1 * error_y
    current_z += 0.1 * error_z
    print(f"Current position: ({current_x}, {current_y}, {current_z})")

这段代码首先定义了一个直线轨迹，然后模拟获取飞行器当前位置，通过计算位置误差并根据简单的控制算法来更新飞行器位置，以尝试跟踪目标轨迹。

姿态环轨迹跟踪控制

姿态环主要负责调整飞行器的姿态以实现位置环所需的运动。姿态的控制与旋翼产生的力矩密切相关。例如，要产生滚转运动，我们需要调整相应旋翼的转速来产生不同的升力差，进而产生滚转力矩。

多点任务控制与轨迹规划

多点任务控制

在实际应用中，四旋翼飞行器可能需要执行多点任务，比如从点A飞到点B，再到点C等。这就需要合理的轨迹规划算法。一种简单的方法是通过线性插值来连接各个点。假设我们有点 $A(x1, y1, z1)$ 和点 $B(x2, y2, z2)$，在时间 $t$ 内从A飞到B，则位置可以表示为：

$x(t) = x1 + \frac{x2 - x_1}{t}t$

$y(t) = y1 + \frac{y2 - y_1}{t}t$

$z(t) = z1 + \frac{z2 - z_1}{t}t$

圆弧轨迹与直线轨迹

圆弧轨迹的生成相对复杂一些，需要用到三角函数来描述位置的变化。例如，以圆心在 $(xc, yc)$，半径为 $R$ 的圆弧为例，在 $xy$ 平面内的轨迹可以表示为：

$x = x_c + R\cos(\omega t)$

$y = y_c + R\sin(\omega t)$

$z = z_0$

这里 $\omega$ 是角速度。在代码实现中，我们可以通过离散化时间步来模拟飞行器沿着圆弧轨迹飞行。

风阻力模型

风阻力对四旋翼飞行器的飞行性能有显著影响。一般来说，风阻力可以用以下公式近似：

$Fd = \frac{1}{2}\rho v^2CdA$

这里 $\rho$ 是空气密度，$v$ 是飞行器与空气的相对速度，$C_d$ 是阻力系数，$A$ 是飞行器的迎风面积。在动力学模型中加入风阻力后，运动方程会变得更加复杂，但也更接近实际情况。在代码中，我们可以根据实时的风速和飞行器速度来计算风阻力，并将其作为一个干扰力加入到动力学模拟中。

状态观测器设计

状态观测器用于估计飞行器的状态，比如位置、速度和姿态等。因为实际飞行中，传感器测量可能存在噪声和误差，状态观测器可以通过融合多种传感器信息或基于模型预测来更准确地估计飞行器状态。常见的状态观测器如卡尔曼滤波器，它通过预测和更新两个步骤来不断优化状态估计。

import numpy as np

# 假设状态转移矩阵
A = np.array([[1, 0.01, 0],
              [0, 1, 0],
              [0, 0, 1]])
# 假设观测矩阵
H = np.array([[1, 0, 0],
              [0, 0, 1]])
# 过程噪声协方差
Q = np.array([[0.001, 0, 0],
              [0, 0.001, 0],
              [0, 0, 0.001]])
# 观测噪声协方差
R = np.array([[0.1, 0],
              [0, 0.1]])

# 初始化状态和协方差
x_hat = np.array([[0], [0], [0]])
P = np.eye(3)

def kalman_filter(z):
    global x_hat, P
    # 预测步骤
    x_hat_minus = A @ x_hat
    P_minus = A @ P @ A.T + Q
    # 更新步骤
    K = P_minus @ H.T @ np.linalg.inv(H @ P_minus @ H.T + R)
    x_hat = x_hat_minus + K @ (z - H @ x_hat_minus)
    P = (np.eye(3) - K @ H) @ P_minus
    return x_hat


# 模拟观测数据
observations = np.array([[1.1, 2.2],
                         [1.3, 2.4],
                         [1.5, 2.6]])
for z in observations:
    z = z.reshape(-1, 1)
    estimated_state = kalman_filter(z)
    print(f"Estimated state: {estimated_state.flatten()}")

这段代码展示了一个简单的卡尔曼滤波器实现，通过不断处理观测数据来估计飞行器的状态。

通过以上对四旋翼飞行器仿真各个方面的探讨，我们从理论公式一步步走到代码实现，希望能为大家在这个有趣的领域提供一些深入的见解和实践思路。