PID和ADRC的四旋翼轨迹跟踪控制 #四旋翼模型 #姿态控制 #高度控制 #姿态动力学 #ADRC四旋翼飞行器的轨迹跟踪控制一直是无人机领域的热门话题。今天咱们就来聊聊PID和ADRC这两种控制方法在四旋翼轨迹跟踪中的应用。首先我们得了解一下四旋翼的基本模型。四旋翼模型四旋翼飞行器的运动可以分为姿态控制和高度控制两部分。姿态控制主要涉及到飞行器的滚转、俯仰和偏航而高度控制则是让飞行器在垂直方向上保持稳定。class Quadrotor: def __init__(self, mass, gravity, Ixx, Iyy, Izz): self.mass mass self.gravity gravity self.Ixx Ixx self.Iyy Iyy self.Izz Izz def dynamics(self, state, control_input): x, y, z, vx, vy, vz, phi, theta, psi, p, q, r state # 控制输入包括四个电机的推力 u1, u2, u3, u4 control_input # 姿态动力学方程 phi_dot p q * np.sin(phi) * np.tan(theta) r * np.cos(phi) * np.tan(theta) theta_dot q * np.cos(phi) - r * np.sin(phi) psi_dot q * np.sin(phi) / np.cos(theta) r * np.cos(phi) / np.cos(theta) # 角速度变化率 p_dot (self.Iyy - self.Izz) * q * r / self.Ixx u1 / self.Ixx q_dot (self.Izz - self.Ixx) * p * r / self.Iyy u2 / self.Iyy r_dot (self.Ixx - self.Iyy) * p * q / self.Izz u3 / self.Izz # 位置和速度变化率 x_dot vx y_dot vy z_dot vz vx_dot (np.cos(phi) * np.sin(theta) * np.cos(psi) np.sin(phi) * np.sin(psi)) * u4 / self.mass vy_dot (np.cos(phi) * np.sin(theta) * np.sin(psi) - np.sin(phi) * np.cos(psi)) * u4 / self.mass vz_dot (np.cos(phi) * np.cos(theta)) * u4 / self.mass - self.gravity return [x_dot, y_dot, z_dot, vx_dot, vy_dot, vz_dot, phi_dot, theta_dot, psi_dot, p_dot, q_dot, r_dot]这段代码定义了一个四旋翼飞行器的动力学模型。通过这个模型我们可以模拟飞行器在不同控制输入下的运动状态。PID控制PID控制是最经典的控制方法之一它通过比例P、积分I和微分D三个环节来调节系统输出。在四旋翼控制中PID控制器通常用于姿态和高度控制。class PIDController: def __init__(self, kp, ki, kd): self.kp kp self.ki ki self.kd kd self.prev_error 0 self.integral 0 def control(self, setpoint, current_value): error setpoint - current_value self.integral error derivative error - self.prev_error self.prev_error error return self.kp * error self.ki * self.integral self.kd * derivative这个PID控制器实现简单但在实际应用中参数的调节是一个比较繁琐的过程。如果参数设置不当可能会导致系统振荡或响应过慢。ADRC控制ADRCActive Disturbance Rejection Control是一种更先进的控制方法它通过估计和补偿系统中的扰动来提高控制性能。ADRC的核心思想是将系统中的不确定性和外部扰动视为一个总扰动并通过扩张状态观测器ESO来估计这个扰动。class ADRCController: def __init__(self, beta1, beta2, beta3, kp, kd): self.beta1 beta1 self.beta2 beta2 self.beta3 beta3 self.kp kp self.kd kd self.z1 0 self.z2 0 self.z3 0 def eso(self, y, u): e y - self.z1 self.z1 self.z2 self.beta1 * e self.z2 self.z3 self.beta2 * e u self.z3 self.beta3 * e return self.z3 def control(self, setpoint, current_value): y current_value u 0 # 初始控制输入 disturbance self.eso(y, u) error setpoint - y u self.kp * error - self.kd * self.z2 - disturbance return uADRC控制器通过ESO来估计系统中的扰动并在控制律中进行补偿。这种方法对于系统中的不确定性和外部扰动具有较强的鲁棒性。对比与应用在实际应用中PID控制器简单易实现但需要仔细调节参数。而ADRC控制器虽然复杂一些但在面对系统不确定性和外部扰动时表现更为出色。# 假设我们有一个四旋翼飞行器 quadrotor Quadrotor(mass1.0, gravity9.81, Ixx0.1, Iyy0.1, Izz0.1) # PID控制器 pid_controller PIDController(kp1.0, ki0.1, kd0.01) # ADRC控制器 adrc_controller ADRCController(beta11.0, beta20.1, beta30.01, kp1.0, kd0.01) # 模拟控制过程 state [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] setpoint 1.0 for _ in range(100): control_input_pid pid_controller.control(setpoint, state[2]) # 高度控制 control_input_adrc adrc_controller.control(setpoint, state[2]) state quadrotor.dynamics(state, [control_input_pid, 0, 0, 0])在这个简单的模拟中我们可以看到PID和ADRC控制器在四旋翼高度控制中的应用。ADRC控制器在面对系统扰动时表现更为稳定。总结PID和ADRC各有优缺点选择哪种控制方法取决于具体的应用场景和需求。对于简单的系统PID可能已经足够而对于复杂、不确定的系统ADRC则能提供更好的控制效果。无论哪种方法理解其背后的原理和实现细节都是关键。希望这篇文章能帮助你在四旋翼轨迹跟踪控制中找到适合自己的方法。PID和ADRC的四旋翼轨迹跟踪控制 #四旋翼模型 #姿态控制 #高度控制 #姿态动力学 #ADRC
PID和ADRC的四旋翼轨迹跟踪控制 #四旋翼模型 #姿态控制 #高度控制 #姿态动力学
PID和ADRC的四旋翼轨迹跟踪控制 #四旋翼模型 #姿态控制 #高度控制 #姿态动力学 #ADRC四旋翼飞行器的轨迹跟踪控制一直是无人机领域的热门话题。今天咱们就来聊聊PID和ADRC这两种控制方法在四旋翼轨迹跟踪中的应用。首先我们得了解一下四旋翼的基本模型。四旋翼模型四旋翼飞行器的运动可以分为姿态控制和高度控制两部分。姿态控制主要涉及到飞行器的滚转、俯仰和偏航而高度控制则是让飞行器在垂直方向上保持稳定。class Quadrotor: def __init__(self, mass, gravity, Ixx, Iyy, Izz): self.mass mass self.gravity gravity self.Ixx Ixx self.Iyy Iyy self.Izz Izz def dynamics(self, state, control_input): x, y, z, vx, vy, vz, phi, theta, psi, p, q, r state # 控制输入包括四个电机的推力 u1, u2, u3, u4 control_input # 姿态动力学方程 phi_dot p q * np.sin(phi) * np.tan(theta) r * np.cos(phi) * np.tan(theta) theta_dot q * np.cos(phi) - r * np.sin(phi) psi_dot q * np.sin(phi) / np.cos(theta) r * np.cos(phi) / np.cos(theta) # 角速度变化率 p_dot (self.Iyy - self.Izz) * q * r / self.Ixx u1 / self.Ixx q_dot (self.Izz - self.Ixx) * p * r / self.Iyy u2 / self.Iyy r_dot (self.Ixx - self.Iyy) * p * q / self.Izz u3 / self.Izz # 位置和速度变化率 x_dot vx y_dot vy z_dot vz vx_dot (np.cos(phi) * np.sin(theta) * np.cos(psi) np.sin(phi) * np.sin(psi)) * u4 / self.mass vy_dot (np.cos(phi) * np.sin(theta) * np.sin(psi) - np.sin(phi) * np.cos(psi)) * u4 / self.mass vz_dot (np.cos(phi) * np.cos(theta)) * u4 / self.mass - self.gravity return [x_dot, y_dot, z_dot, vx_dot, vy_dot, vz_dot, phi_dot, theta_dot, psi_dot, p_dot, q_dot, r_dot]这段代码定义了一个四旋翼飞行器的动力学模型。通过这个模型我们可以模拟飞行器在不同控制输入下的运动状态。PID控制PID控制是最经典的控制方法之一它通过比例P、积分I和微分D三个环节来调节系统输出。在四旋翼控制中PID控制器通常用于姿态和高度控制。class PIDController: def __init__(self, kp, ki, kd): self.kp kp self.ki ki self.kd kd self.prev_error 0 self.integral 0 def control(self, setpoint, current_value): error setpoint - current_value self.integral error derivative error - self.prev_error self.prev_error error return self.kp * error self.ki * self.integral self.kd * derivative这个PID控制器实现简单但在实际应用中参数的调节是一个比较繁琐的过程。如果参数设置不当可能会导致系统振荡或响应过慢。ADRC控制ADRCActive Disturbance Rejection Control是一种更先进的控制方法它通过估计和补偿系统中的扰动来提高控制性能。ADRC的核心思想是将系统中的不确定性和外部扰动视为一个总扰动并通过扩张状态观测器ESO来估计这个扰动。class ADRCController: def __init__(self, beta1, beta2, beta3, kp, kd): self.beta1 beta1 self.beta2 beta2 self.beta3 beta3 self.kp kp self.kd kd self.z1 0 self.z2 0 self.z3 0 def eso(self, y, u): e y - self.z1 self.z1 self.z2 self.beta1 * e self.z2 self.z3 self.beta2 * e u self.z3 self.beta3 * e return self.z3 def control(self, setpoint, current_value): y current_value u 0 # 初始控制输入 disturbance self.eso(y, u) error setpoint - y u self.kp * error - self.kd * self.z2 - disturbance return uADRC控制器通过ESO来估计系统中的扰动并在控制律中进行补偿。这种方法对于系统中的不确定性和外部扰动具有较强的鲁棒性。对比与应用在实际应用中PID控制器简单易实现但需要仔细调节参数。而ADRC控制器虽然复杂一些但在面对系统不确定性和外部扰动时表现更为出色。# 假设我们有一个四旋翼飞行器 quadrotor Quadrotor(mass1.0, gravity9.81, Ixx0.1, Iyy0.1, Izz0.1) # PID控制器 pid_controller PIDController(kp1.0, ki0.1, kd0.01) # ADRC控制器 adrc_controller ADRCController(beta11.0, beta20.1, beta30.01, kp1.0, kd0.01) # 模拟控制过程 state [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] setpoint 1.0 for _ in range(100): control_input_pid pid_controller.control(setpoint, state[2]) # 高度控制 control_input_adrc adrc_controller.control(setpoint, state[2]) state quadrotor.dynamics(state, [control_input_pid, 0, 0, 0])在这个简单的模拟中我们可以看到PID和ADRC控制器在四旋翼高度控制中的应用。ADRC控制器在面对系统扰动时表现更为稳定。总结PID和ADRC各有优缺点选择哪种控制方法取决于具体的应用场景和需求。对于简单的系统PID可能已经足够而对于复杂、不确定的系统ADRC则能提供更好的控制效果。无论哪种方法理解其背后的原理和实现细节都是关键。希望这篇文章能帮助你在四旋翼轨迹跟踪控制中找到适合自己的方法。PID和ADRC的四旋翼轨迹跟踪控制 #四旋翼模型 #姿态控制 #高度控制 #姿态动力学 #ADRC
