四旋翼 PID + 轨迹跟踪 + 定位仿真方案

一、整体仿真架构

main.m
 ├─ 1. 参数初始化（质量、惯量、重力、采样时间）
 ├─ 2. 轨迹生成（八字形 / 圆形 / 多项式）
 ├─ 3. 状态定义（x,y,z,φ,θ,ψ, vx,vy,vz, wx,wy,wz）
 ├─ 4. 主循环（for k = 1:N）
 │    ├─ 4.1 传感器模拟（GPS/视觉定位 + 噪声）
 │    ├─ 4.2 位置环 PID → φd, θd, T
 │    ├─ 4.3 姿态环 PID → 期望角速度
 │    ├─ 4.4 角速度环 PID → τx,τy,τz
 │    ├─ 4.5 电机混控
 │    ├─ 4.6 六自由度动力学更新
 │    └─ 4.7 记录数据
 └─ 5. 绘图（轨迹、误差、姿态、控制量）

---

二、四旋翼动力学

1、状态变量

X = [x y z];        % 位置
V = [vx vy vz];     % 速度
Euler = [phi theta psi];  % 欧拉角
W = [wx wy wz];     % 机体角速度

2、动力学方程（牛顿–欧拉）

% 位置动力学
dxdt = vx;
dvdt = R * [0; 0; T] / m - [0; 0; g];

% 姿态动力学
omega_dot = J \ (tau - cross(W, J*W));

其中旋转矩阵 R 为：

function R = RotZYX(phi, theta, psi)
    R = [
        cos(psi)*cos(theta), ...
        cos(psi)*sin(theta)*sin(phi)-sin(psi)*cos(phi), ...
        cos(psi)*sin(theta)*cos(phi)+sin(psi)*sin(phi);
        
        sin(psi)*cos(theta), ...
        sin(psi)*sin(theta)*sin(phi)+cos(psi)*cos(phi), ...
        sin(psi)*sin(theta)*cos(phi)-cos(psi)*sin(phi);
        
        -sin(theta), ...
        cos(theta)*sin(phi), ...
        cos(theta)*cos(phi)
    ];
end

---

三、串级 PID（MATLAB 实现）

位置环 PID（外环）

function [T, phi_d, theta_d] = positionPID(xd, x, vd, v, dt)

Kp_xy = 2.0;  Kd_xy = 1.2;
Kp_z  = 3.0;  Kd_z  = 2.0;

ax = Kp_xy*(xd(1)-x(1)) + Kd_xy*(vd(1)-v(1));
ay = Kp_xy*(xd(2)-x(2)) + Kd_xy*(vd(2)-v(2));
az = Kp_z *(xd(3)-x(3)) + Kd_z *(vd(3)-v(3));

g = 9.81;
T = m*(g + az);

phi_d   = -ay/g;
theta_d = ax/g;
end

---

姿态 & 角速度 PID（内环）

function tau = attitudePID(euler_d, euler, W_d, W, dt)

Kp_att = [8 8 5];
Kd_att = [0.8 0.8 0.5];

e_att = euler_d - euler;
e_rate = W_d - W;

tau = Kp_att .* e_att + Kd_att .* e_rate;
end

---

四、轨迹生成

1、八字形轨迹（最常用）

t = linspace(0,10,1000);
A = 5; B = 3;

xd = A*sin(0.5*t);
yd = B*sin(t);
zd = 2 + 0.2*sin(0.3*t);

vx_d = gradient(xd,t);
vy_d = gradient(yd,t);
vz_d = gradient(zd,t);

优点：非线性、转弯明显、PID压力足

---

2、圆形轨迹（适合验证稳定性）

R = 5;
xd = R*cos(0.5*t);
yd = R*sin(0.5*t);
zd = ones(size(t))*2;

参考代码 四旋翼控制PID，轨迹定位 www.youwenfan.com/contentcnv/103618.html

五、轨迹定位（含噪声，仿真真实系统）

GPS / 视觉定位模型

function pos_meas = fakeGPS(pos_true, sigma)

sigma_pos = [0.2; 0.2; 0.1]; % 米
noise = sigma_pos .* randn(3,1);
pos_meas = pos_true + noise;
end

在控制循环中：

x_est = fakeGPS(X, sigma);

PID永远用估计值，不用真实值

---

六、电机混控

function motor = mixer(T, tau)

kT = 1e-5; kM = 1e-6; L = 0.25;

motor(1) = T/(4*kT) - tau(3)/(4*kM) - tau(2)/(2*L);
motor(2) = T/(4*kT) + tau(3)/(4*kM) + tau(1)/(2*L);
motor(3) = T/(4*kT) - tau(3)/(4*kM) + tau(2)/(2*L);
motor(4) = T/(4*kT) + tau(3)/(4*kM) - tau(1)/(2*L);

motor = sqrt(max(motor,0));
end

---

七、主仿真循环

dt = 0.01;
N = length(t);

for k = 1:N
    % 定位
    X_est = fakeGPS(X, sigma);

    % 位置PID
    [T, phi_d, theta_d] = positionPID(...
        [xd(k); yd(k); zd(k)], X_est, ...
        [vx_d(k); vy_d(k); vz_d(k)], V, dt);

    % 姿态PID
    tau = attitudePID([phi_d; theta_d; 0], Euler, zeros(3,1), W, dt);

    % 动力学
    [X, V, Euler, W] = quadrotorDynamics(X, V, Euler, W, T, tau, dt);
end