基于ESP32四旋翼无人机如何根据PID算法进行电机的PWM duty补偿

1.计算模型

电机位置和补偿输出的关系，需要求解的是C1,C2,C3,C4的电机输出补偿值。分别对应M1,M2,M3,M4四个电机。

2.推导过程（可以跳过不看，直接到第三步）

3. 四个电机的PID补偿输出的算法如下：

C1到C4的值是根据上面的公式C=A(−1)P，计算出来的。P是pc,rc,yc三个姿态角的力矩。A是转移矩阵。C1到C4是四个电机的PWM duty补偿值。
void dandelion_pid_update(const dandelion_pose_projection* pose,const dandelion_pose_projection* poseLast,const dandelion_pose_projection* target)
{
/* Pitch */
PIDController_PitchRoll2Pwm_Update(&mDandelion.pid.pitch_pose, target->pitch , pose->pitch);

/* Roll */
PIDController_PitchRoll2Pwm_Update(&mDandelion.pid.roll_pose, target->roll, pose->roll);

/* Yaw */
PIDController_YawRate2Pwm_Update(&mDandelion.pid.yaw_rate, target->yawGyro, pose->yawGyro);

mDandelion.pid.compensate[0] = 0;
mDandelion.pid.compensate[1] = 0;
mDandelion.pid.compensate[2] = 0;
mDandelion.pid.compensate[3] = 0;

// pitch
mDandelion.pid.compensate[0] += mDandelion.pid.pitch_pose.out * mDandelion.pid.config_f[DANDELION_PID_CFG_PITCH_F] / 2.0f;
mDandelion.pid.compensate[1] -= mDandelion.pid.pitch_pose.out * mDandelion.pid.config_f[DANDELION_PID_CFG_PITCH_F] / 2.0f;
mDandelion.pid.compensate[2] -= mDandelion.pid.pitch_pose.out * mDandelion.pid.config_f[DANDELION_PID_CFG_PITCH_F] / 2.0f;
mDandelion.pid.compensate[3] += mDandelion.pid.pitch_pose.out * mDandelion.pid.config_f[DANDELION_PID_CFG_PITCH_F] / 2.0f;

// roll
mDandelion.pid.compensate[0] += mDandelion.pid.roll_pose.out * mDandelion.pid.config_f[DANDELION_PID_CFG_ROLL_F] / 2.0f;
mDandelion.pid.compensate[1] += mDandelion.pid.roll_pose.out * mDandelion.pid.config_f[DANDELION_PID_CFG_ROLL_F] / 2.0f;
mDandelion.pid.compensate[2] -= mDandelion.pid.roll_pose.out * mDandelion.pid.config_f[DANDELION_PID_CFG_ROLL_F] / 2.0f;
mDandelion.pid.compensate[3] -= mDandelion.pid.roll_pose.out * mDandelion.pid.config_f[DANDELION_PID_CFG_ROLL_F] / 2.0f;

// rotate
mDandelion.pid.compensate[0] -= mDandelion.pid.yaw_rate.out * mDandelion.pid.config_f[DANDELION_PID_CFG_YAW_F] / 2.0f;
mDandelion.pid.compensate[1] += mDandelion.pid.yaw_rate.out * mDandelion.pid.config_f[DANDELION_PID_CFG_YAW_F] / 2.0f;
mDandelion.pid.compensate[2] -= mDandelion.pid.yaw_rate.out * mDandelion.pid.config_f[DANDELION_PID_CFG_YAW_F] / 2.0f;
mDandelion.pid.compensate[3] += mDandelion.pid.yaw_rate.out * mDandelion.pid.config_f[DANDELION_PID_CFG_YAW_F] / 2.0f;


memcpy(&mDandelion.dmp.poseLast, pose, sizeof(dandelion_pose_projection));

}

4.电机PWM duty输出

最终电机PWM duty输出需要加入遥控器传递过来的参数油门参数。
在dandelion_pwm_update函数中将遥控器的油门大小和PID duty结果相加，计算出最终的电机控制的PWM duty参数。它来自于下面两参数：
gCompensate和mDandelion.pid.compensate。将两个参数相加得到pid.output（也就是PWM duty）

void dandelion_pwm_update(float gCompensate)
{

/* Compensate Pose */

float fpCompensate = 1.0; // limit compensate output 
if(1)
{
	float cmax = mDandelion.pid.compensate[0];
	float cmin = mDandelion.pid.compensate[0];
	for(int j=0;j<4;j++)
	{
		if(mDandelion.pid.compensate[j]>cmax) cmax = mDandelion.pid.compensate[j];
		if(mDandelion.pid.compensate[j]<cmin) cmin = mDandelion.pid.compensate[j];
	}
	
	if(cmax*cmin<0) 
	{
		// Max Value Limit
		if((gCompensate+cmax)>0.95)
		{
			fpCompensate = fpCompensate * (0.95-gCompensate)/(cmax);
		}

		// Min Value Limit
		if((gCompensate+cmin*fpCompensate)<0.05)
		{
			fpCompensate = fpCompensate * (0.05-gCompensate)/(cmin);
		}
	}
}

/* ---------------------------------------------------------------- */




/* Update Register */

mDandelion.pid.output[0] = gCompensate + mDandelion.pid.compensate[0] * fpCompensate;
mDandelion.pid.output[1] = gCompensate + mDandelion.pid.compensate[1] * fpCompensate;
mDandelion.pid.output[2] = gCompensate + mDandelion.pid.compensate[2] * fpCompensate;
mDandelion.pid.output[3] = gCompensate + mDandelion.pid.compensate[3] * fpCompensate;

bsp_motor_drive(mDandelion.pid.output);
//bsp_motor_drive(NULL);
return 0;

}
其中bsp_motor_drive(mDandelion.pid.output)，该函数最终调用PWM控制函数，调整PWM波形控制电机转速。
void bsp_motor_drive(float* duty) //控制四个马达功率
{
if(duty==NULL)
{
// stop all
for(int i=0;i<4;i++)
{
ledc_set_duty(_pwm_ch_config[i].speed_mode,_pwm_ch_config[i].channel,0); //设置占空比
ledc_update_duty(_pwm_ch_config[i].speed_mode, _pwm_ch_config[i].channel);//使新配置生效

}
}
else
{
for(int i=0;i<4;i++)
{
if(duty[i]>=0&&duty[i]<=1)
{
ledc_set_duty(_pwm_ch_config[i].speed_mode,_pwm_ch_config[i].channel,duty[i] * BSP_MOTOR_OVERFLOW);
ledc_update_duty(_pwm_ch_config[i].speed_mode, _pwm_ch_config[i].channel);
}
else if(duty[i]>=1)
{
ledc_set_duty(_pwm_ch_config[i].speed_mode,_pwm_ch_config[i].channel,BSP_MOTOR_OVERFLOW);
ledc_update_duty(_pwm_ch_config[i].speed_mode, _pwm_ch_config[i].channel);
}
else
{
ledc_set_duty(_pwm_ch_config[i].speed_mode,_pwm_ch_config[i].channel,0);
ledc_update_duty(_pwm_ch_config[i].speed_mode, _pwm_ch_config[i].channel);
}

}
}
}