基于Arduino Nano+MPU6050的4舵机PID稳定平台控制与调参问询

1. PID输出至4路舵机的分配逻辑 #

4路舵机的核心分配原则是基于安装位置的角度修正叠加，前提是舵机采用常见的四角布局（前左FL、前右FR、后左BL、后右BR）。PID输出分为rollOutput（横滚修正量）和pitchOutput（俯仰修正量）两个独立分量，每个舵机的最终角度调整是这两个分量的组合，本质是通过舵机的协同动作抵消平台的倾斜。

2. 横滚/俯仰修正量到舵机的映射方法 #

映射逻辑需结合倾斜方向与舵机位置的对应关系，以下是通用映射公式（需根据实际机械安装微调符号）：

• 前左舵机：中位角度 + pitchOutput + rollOutput
• 前右舵机：中位角度 + pitchOutput - rollOutput
• 后左舵机：中位角度 - pitchOutput + rollOutput
• 后右舵机：中位角度 - pitchOutput - rollOutput

代码实现示例 #

#include <Servo.h>
#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;
Servo servoFL, servoFR, servoBL, servoBR;

// 舵机中位角度（根据实际安装调整，通常为90°）
const int SERVO_MID = 90;
// PID初始参数（需实际整定）
float Kp_roll = 2.0, Ki_roll = 0.1, Kd_roll = 0.5;
float Kp_pitch = 2.0, Ki_pitch = 0.1, Kd_pitch = 0.5;

// PID控制变量
float roll_set = 0, pitch_set = 0;
float roll_err, last_roll_err, roll_int, roll_out;
float pitch_err, last_pitch_err, pitch_int, pitch_out;

void setup() {
  Wire.begin();
  mpu.initialize();
  // 水平状态下校准传感器
  mpu.calibrateGyro();
  mpu.calibrateAccel();

  // 绑定舵机引脚
  servoFL.attach(2);
  servoFR.attach(3);
  servoBL.attach(4);
  servoBR.attach(5);

  // 初始化舵机到中位
  servoFL.write(SERVO_MID);
  servoFR.write(SERVO_MID);
  servoBL.write(SERVO_MID);
  servoBR.write(SERVO_MID);
}

void loop() {
  // 读取MPU6050数据并计算姿态角
  VectorInt16 accel = mpu.getAcceleration();
  float roll = atan2(accel.y, accel.z) * 180.0 / PI;
  float pitch = atan2(-accel.x, sqrt(accel.y*accel.y + accel.z*accel.z)) * 180.0 / PI;

  // 计算Roll PID输出
  roll_err = roll_set - roll;
  roll_int += roll_err * 0.01; // 0.01为采样周期（100Hz）
  roll_int = constrain(roll_int, -10, 10); // 积分限幅防止饱和
  roll_out = Kp_roll * roll_err + Ki_roll * roll_int + Kd_roll * (roll_err - last_roll_err);
  last_roll_err = roll_err;

  // 计算Pitch PID输出
  pitch_err = pitch_set - pitch;
  pitch_int += pitch_err * 0.01;
  pitch_int = constrain(pitch_int, -10, 10);
  pitch_out = Kp_pitch * pitch_err + Ki_pitch * pitch_int + Kd_pitch * (pitch_err - last_pitch_err);
  last_pitch_err = pitch_err;

  // 映射至各舵机角度并限幅
  int angleFL = constrain(SERVO_MID + pitch_out + roll_out, 0, 180);
  int angleFR = constrain(SERVO_MID + pitch_out - roll_out, 0, 180);
  int angleBL = constrain(SERVO_MID - pitch_out + roll_out, 0, 180);
  int angleBR = constrain(SERVO_MID - pitch_out - roll_out, 0, 180);

  // 更新舵机位置
  servoFL.write(angleFL);
  servoFR.write(angleFR);
  servoBL.write(angleBL);
  servoBR.write(angleBR);

  delay(10); // 稳定100Hz采样频率
}

3. 多舵机稳定系统的PID参数整定策略 #

采用单轴独立整定法，避免横滚和俯仰参数互相干扰，步骤如下：

1. 初始化参数：将Ki、Kd设为0，仅保留Kp。
2. 整定Kp：从1开始逐步增大Kp，直到平台能快速响应倾斜但出现轻微振荡，然后将当前值减半，作为最终Kp。
3. 整定Kd：从0.2开始逐步增大Kd，直到振荡完全消失，平台响应速度满足需求（Kd过大易导致舵机抖动）。
4. 整定Ki：若平台静止后仍存在稳态倾斜，缓慢增大Ki（每次加0.05），直到倾斜消除，注意Ki过大易引发超调。
5. 联合微调：单轴参数稳定后，再同时调整横滚和俯仰参数，根据平台整体稳定状态做小幅度修正。

关键注意事项 #

• 传感器必须在水平状态下校准，否则初始姿态基准错误会导致PID失效。
• 舵机需单独供电（避免与Arduino共用电源），防止电流波动影响系统稳定性。
• 每次仅调整一个参数，调整后观察至少10秒再进行下一次修改，确保效果可复现。

内容的提问来源于stack exchange，提问作者Atasya Rahma