四旋翼无人机PID模型，飞行器本体模型，位置控制，姿态控制，控制分配和电机控制。

四旋翼飞控就像给无人机装了个自动导航系统。想象你手里拿着遥控器，想让飞机悬停在空中某个点，这时候位置控制开始干活了。但想让飞机听话得先知道它现在啥姿势——这就是姿态控制的任务。不过别急，咱们先看看这铁鸟怎么动起来的。

本体模型这玩意儿用牛顿定律就能说清楚：

class Drone:
    def __init__(self):
        self.mass = 1.0  # 千克
        self.Ix = 0.1    # 转动惯量
        self.Iy = 0.1
        self.Iz = 0.2
        self.g = 9.81

    def update(self, thrusts, dt):
        tau_x = (thrusts[1] - thrusts[3]) * 0.1
        tau_y = (thrusts[2] - thrusts[0]) * 0.1
        tau_z = (thrusts[0] + thrusts[2] - thrusts[1] - thrusts[3]) * 0.05
        
        # 姿态动力学
        angular_accel = [
            tau_x / self.Ix,
            tau_y / self.Iy,
            tau_z / self.Iz
        ]
        return angular_accel

这里有个坑：四个电机的安装位置距离中心轴的距离会影响力矩计算，代码里0.1那个系数就是杆臂长度。别小看这数字，实测时要是飞机总往奇怪方向飘，八成是这参数没调准。

PID控制器这货是飞控的灵魂，但别被公式吓到：

class PID {
public:
    PID(float kp, float ki, float kd) : 
        kp(kp), ki(ki), kd(kd),
        prev_error(0), integral(0) {}
        
    float compute(float error, float dt) {
        integral += error * dt;
        float derivative = (error - prev_error) / dt;
        prev_error = error;
        return kp*error + ki*integral + kd*derivative;
    }
    
private:
    float kp, ki, kd;
    float prev_error, integral;
};

注意积分项是个双刃剑——能消除静差但也会导致积分饱和。有次测试我把ki调太大，飞机直接在空中跳起甩葱舞。后来加了积分限幅才老实。

四旋翼无人机PID模型，飞行器本体模型，位置控制，姿态控制，控制分配和电机控制。

当你想让飞机飞到（x,y,z）坐标时，位置控制和姿态控制得配合演戏。位置环输出的是期望的姿态角，姿态环再转化成电机转速。比如高度控制：

def altitude_control(target_z, current_z, dt):
    error_z = target_z - current_z
    # 先来点P控制
    thrust = pid_z.compute(error_z, dt)
    # 重力补偿
    return (thrust + drone.mass * drone.g) / 4  # 均分给四个电机

这里有个骚操作：直接加重力补偿相当于开环前馈，比纯PID响应快得多。实测悬停时油门值能稳定在±5%以内波动。

控制分配最像调酒——把姿态指令兑成四个电机的鸡尾酒。核心是这个混控矩阵：

| 1  1  1  1 |   motor1   |   | thrust  |
| 1 -1 -1  1 | * motor2   = | roll    |
| 1  1 -1 -1 |   motor3     | pitch   |
| 1 -1  1 -1 |   motor4     | yaw     |

用numpy解方程超方便：

def mix_commands(roll_cmd, pitch_cmd, yaw_cmd, thrust):
    mix_matrix = np.array([
        [1, 1, 1, 1],
        [1, -1, -1, 1],
        [1, 1, -1, -1],
        [1, -1, 1, -1]
    ])
    inputs = np.linalg.pinv(mix_matrix) @ [thrust, roll_cmd, pitch_cmd, yaw_cmd]
    return np.clip(inputs, 0, 100)  # 限制电机转速范围

注意伪逆矩阵的使用，比直接求逆更稳当。有一次手滑写错矩阵系数，飞机直接变身陀螺满地打转，差点把实验室桌子掀了。

最后到电机控制环节，PWM信号转化看着简单：

void set_motor_speed(uint8_t pin, float cmd) {
    cmd = constrain(cmd, 0, 1000);  // 限制指令范围
    analogWrite(pin, map(cmd, 0, 100, 1000, 2000));  // 转成PWM脉宽
}

但有个魔鬼细节：电调校准。有次没校准直接上电，四个电机转速差出200多转，飞机离地瞬间就表演了180度空翻。后来学乖了，每次上电先发最小PWM信号，听到"哔哔"两声再动油门。

调参时记得带个结实点的罩子——别问我怎么知道的。当P值太大导致电机疯狂振荡时，那个螺旋桨能把矿泉水瓶削成雪花。现在我的工位上还留着被桨叶打穿的马克杯，当作调参路上的勋章。